使用solvePnP时如何解决断言错误？
How to solve Assertion Error when using solvePnP?
我是 Visual Odometry 的新手，正在学习使用 PnP 求解 VO 的教程。但是，当我 运行 程序时，出现以下错误：
terminate called after throwing an instance of 'cv::Exception'
what(): OpenCV(4.3.0) /home/wctu/opencv-4.3.0/modules/calib3d/src/solvepnp.cpp:754: error: (-215:Assertion failed) ( (npoints >= 4) || (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess) ) && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) in function 'solvePnPGeneric'
我的代码如下：
string datas[2266];
string str1;
std::getline(file, str1);
datas[0] = str1;
for(int i = 1; !file.eof(); i++)
{
string str;
std::getline(file, str);
datas[i] = str;
if(str.empty()) break;
if(str.at(0) == '#') continue; /* comment */
cout << datas[i-1] << endl << datas[i] << endl;
Mat image, depth, image1, depth1;
string rgbFilename1 = datas[i-1].substr(timestampLength + 1, rgbPathLehgth );
string timestap1 = datas[i-1].substr(0, timestampLength);
string depthFilename1 = datas[i-1].substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth );
image1 = imread(dirname + rgbFilename1);
depth1 = imread(dirname + depthFilename1, -1);
string rgbFilename = str.substr(timestampLength + 1, rgbPathLehgth );
string timestap = str.substr(0, timestampLength);
string depthFilename = str.substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth );
image = imread(dirname + rgbFilename);
depth = imread(dirname + depthFilename, -1);
CV_Assert(!image.empty());
CV_Assert(!depth.empty());
CV_Assert(depth.type() == CV_16UC1);
cout << i << " " << rgbFilename << " " << depthFilename << endl;
std::vector<KeyPoint> keypoints_1, keypoints_2;
vector<DMatch> matches;
find_feature_matches(image1, image, keypoints_1, keypoints_2, matches);
cout << "一共找到了" << matches.size() << "组匹配点" << endl;
// // 建立3D点
//Mat d1 = imread(depth1, IMREAD_UNCHANGED); // 深度图为16位无符号数，单通道图像
Mat K = (Mat_<double>(3, 3) << 525.0f, 0, 319.5f, 0, 525.0f, 239.5f, 0, 0, 1);
vector<Point3f> pts_3d;
vector<Point2f> pts_2d;
for (DMatch m:matches) {
ushort d = depth1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
if (d == 0) // bad depth
continue;
float dd = d / 5000.0;
Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd));
pts_2d.push_back(keypoints_2[m.trainIdx].pt);
}
cout << pts_3d[0] << " " << pts_2d[0] << endl;
cout << "3d-2d pairs: " << pts_3d.size() << " " << pts_2d.size() << endl;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
Mat r, t;
solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
Mat R;
cv::Rodrigues(r, R); // r为旋转向量形式，用Rodrigues公式转换为矩阵
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << endl;
argv[1]是将rgb图像与深度相关联的文本文件，格式如下：
1311877977.445420 rgb/1311877977.445420.png 1311877977.431871 depth/1311877977.431871.png
我已经在网上搜索了解决方案并尝试了所有方法，但仍然是徒劳的。
非常感谢你们的帮助，提前致谢。
**更新：
出现异常的输入如下，只有三对：
[0.94783, -1.70307, 7.3738] [383.4, 121.828]
[0.170393, -0.170453, 1.3256] [379.817, 186.325]
[0.610124, -0.161545, 3.4604] [403.108, 223.949]
OpenCV 函数 cv::solvePnP 会在内部检查您提供的输入数据是否确实有意义 并且是否与文档（断言）匹配。在您的情况下，它没有这样做，因此会抛出一条错误消息：
terminate called after throwing an instance of 'cv::Exception'
what(): OpenCV(4.3.0) /home/wctu/opencv-4.3.0/modules/calib3d/src/solvepnp.cpp:754: error:
(-215:Assertion failed)
( (npoints >= 4) || (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess) ) &&
npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) in function 'solvePnPGeneric'
因此输入的尺寸不正确或您使用的文件不合适。错误是根据其输入参数给出的。因此，您将不得不寻找 cv::solvePnP.
的相应文档
bool cv::solvePnP(InputArray objectPoints,
InputArray imagePoints,
InputArray cameraMatrix,
InputArray distCoeffs,
OutputArray rvec,
OutputArray tvec,
bool useExtrinsicGuess = false,
int flags = SOLVEPNP_ITERATIVE
)
将您的输入参数与上面给出的参数进行比较，您会发现您将 useExtrinsicGuess 设置为 false 并且没有提供默认为 SOLVEPNP_ITERATIVE 的 flags。这已经告诉您您的错误不是由 (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess)（因为 useExtrinsicGuess 设置为 false）引起的，而是由 (npoints >= 4).
引起的
打开相应的 source-code file on Github 或在您的源代码文件夹中，您实际上会看到 npoints 被定义为
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
现在我们必须弄清楚 checkVector 做了什么： 它检查矩阵的通道和深度以及 returns -1 if the requirement is not satisfied. Otherwise, it returns the number of elements in the matrix. Note that an element may have multiple channels..
这意味着您的代码失败了，因为为两种数据类型提供的输入格式不正确，或者 npoints 小于 4。
如果您再次查看文档，它会告诉您 objectPoints 需要 Array of object points in the object coordinate space, Nx3 1-channel or 1xN/Nx1 3-channel, where N is the number of points. vector<Point3d> can be also passed here. 而 imagePoints 需要 Array of corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel, where N is the number of points.
您传递的输入 pts_3d 和 pts_2d 显然满足了这一点，因为它们分别是 std::vector<Point3f> 和 std::vector<Point3f>。这意味着唯一合乎逻辑的原因是 pts_3d and/or pts2d 实际上少于 3 个条目 这对于唯一解决方案来说太少了。这意味着在之前步骤中提供的图像之间发现 特征匹配不足！。再次检查您的输入文件并尝试使用不同的文件。
我是 Visual Odometry 的新手，正在学习使用 PnP 求解 VO 的教程。但是，当我 运行 程序时，出现以下错误：
terminate called after throwing an instance of 'cv::Exception'
what(): OpenCV(4.3.0) /home/wctu/opencv-4.3.0/modules/calib3d/src/solvepnp.cpp:754: error: (-215:Assertion failed) ( (npoints >= 4) || (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess) ) && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) in function 'solvePnPGeneric'
我的代码如下：
string datas[2266];
string str1;
std::getline(file, str1);
datas[0] = str1;
for(int i = 1; !file.eof(); i++)
{
string str;
std::getline(file, str);
datas[i] = str;
if(str.empty()) break;
if(str.at(0) == '#') continue; /* comment */
cout << datas[i-1] << endl << datas[i] << endl;
Mat image, depth, image1, depth1;
string rgbFilename1 = datas[i-1].substr(timestampLength + 1, rgbPathLehgth );
string timestap1 = datas[i-1].substr(0, timestampLength);
string depthFilename1 = datas[i-1].substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth );
image1 = imread(dirname + rgbFilename1);
depth1 = imread(dirname + depthFilename1, -1);
string rgbFilename = str.substr(timestampLength + 1, rgbPathLehgth );
string timestap = str.substr(0, timestampLength);
string depthFilename = str.substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth );
image = imread(dirname + rgbFilename);
depth = imread(dirname + depthFilename, -1);
CV_Assert(!image.empty());
CV_Assert(!depth.empty());
CV_Assert(depth.type() == CV_16UC1);
cout << i << " " << rgbFilename << " " << depthFilename << endl;
std::vector<KeyPoint> keypoints_1, keypoints_2;
vector<DMatch> matches;
find_feature_matches(image1, image, keypoints_1, keypoints_2, matches);
cout << "一共找到了" << matches.size() << "组匹配点" << endl;
// // 建立3D点
//Mat d1 = imread(depth1, IMREAD_UNCHANGED); // 深度图为16位无符号数，单通道图像
Mat K = (Mat_<double>(3, 3) << 525.0f, 0, 319.5f, 0, 525.0f, 239.5f, 0, 0, 1);
vector<Point3f> pts_3d;
vector<Point2f> pts_2d;
for (DMatch m:matches) {
ushort d = depth1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
if (d == 0) // bad depth
continue;
float dd = d / 5000.0;
Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd));
pts_2d.push_back(keypoints_2[m.trainIdx].pt);
}
cout << pts_3d[0] << " " << pts_2d[0] << endl;
cout << "3d-2d pairs: " << pts_3d.size() << " " << pts_2d.size() << endl;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
Mat r, t;
solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
Mat R;
cv::Rodrigues(r, R); // r为旋转向量形式，用Rodrigues公式转换为矩阵
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << endl;
argv[1]是将rgb图像与深度相关联的文本文件，格式如下：
1311877977.445420 rgb/1311877977.445420.png 1311877977.431871 depth/1311877977.431871.png
我已经在网上搜索了解决方案并尝试了所有方法，但仍然是徒劳的。
非常感谢你们的帮助，提前致谢。
**更新： 出现异常的输入如下，只有三对：
[0.94783, -1.70307, 7.3738] [383.4, 121.828]
[0.170393, -0.170453, 1.3256] [379.817, 186.325]
[0.610124, -0.161545, 3.4604] [403.108, 223.949]
OpenCV 函数 cv::solvePnP 会在内部检查您提供的输入数据是否确实有意义 并且是否与文档（断言）匹配。在您的情况下，它没有这样做，因此会抛出一条错误消息：
terminate called after throwing an instance of 'cv::Exception'
what(): OpenCV(4.3.0) /home/wctu/opencv-4.3.0/modules/calib3d/src/solvepnp.cpp:754: error:
(-215:Assertion failed)
( (npoints >= 4) || (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess) ) &&
npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) in function 'solvePnPGeneric'
因此输入的尺寸不正确或您使用的文件不合适。错误是根据其输入参数给出的。因此，您将不得不寻找 cv::solvePnP.
bool cv::solvePnP(InputArray objectPoints,
InputArray imagePoints,
InputArray cameraMatrix,
InputArray distCoeffs,
OutputArray rvec,
OutputArray tvec,
bool useExtrinsicGuess = false,
int flags = SOLVEPNP_ITERATIVE
)
将您的输入参数与上面给出的参数进行比较，您会发现您将 useExtrinsicGuess 设置为 false 并且没有提供默认为 SOLVEPNP_ITERATIVE 的 flags。这已经告诉您您的错误不是由 (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess)（因为 useExtrinsicGuess 设置为 false）引起的，而是由 (npoints >= 4).
打开相应的 source-code file on Github 或在您的源代码文件夹中，您实际上会看到 npoints 被定义为
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
现在我们必须弄清楚 checkVector 做了什么：-1 if the requirement is not satisfied. Otherwise, it returns the number of elements in the matrix. Note that an element may have multiple channels..
这意味着您的代码失败了，因为为两种数据类型提供的输入格式不正确，或者 npoints 小于 4。
如果您再次查看文档，它会告诉您 objectPoints 需要 Array of object points in the object coordinate space, Nx3 1-channel or 1xN/Nx1 3-channel, where N is the number of points. vector<Point3d> can be also passed here. 而 imagePoints 需要 Array of corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel, where N is the number of points.
您传递的输入 pts_3d 和 pts_2d 显然满足了这一点，因为它们分别是 std::vector<Point3f> 和 std::vector<Point3f>。这意味着唯一合乎逻辑的原因是 pts_3d and/or pts2d 实际上少于 3 个条目 这对于唯一解决方案来说太少了。这意味着在之前步骤中提供的图像之间发现 特征匹配不足！。再次检查您的输入文件并尝试使用不同的文件。