使用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,319.5f,525.0f,239.5f,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 并且没有提供默认为 flags 的 SOLVEPNP_ITERATIVE。这已经告诉您您的错误不是由 (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess)（因为 useExtrinsicGuess 设置为 false）而是由 (npoints >= 4) 引起的。

打开相应的 source-code file on Github 或在您的源代码文件夹中，您实际上会看到 npoints 被定义为

int npoints = std::max(opoints.checkVector(3,opoints.checkVector(3,CV_64F));

现在我们必须弄清楚 checkVector 的作用：See e.g. here 它检查矩阵的通道和深度并返回 -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 和/或 pts2d 实际上少于 3 个条目，这对于独特的解决方案来说太少了。这意味着在之前步骤中提供的图像之间发现的特征匹配不足！。再次检查您的输入文件，并可能尝试使用不同的文件。