一.仅优化位姿
构造类和代价函数：
// 代价函数的计算模型 struct PnPCeres { PnPCeres ( Point2f uv,Point3f xyz ) : _uv(uv),_xyz(xyz) {} // 残差的计算 template <typename T> bool operator() ( const T* const camera, // 位姿参数，有6维 T* residual ) const // 残差 { T p[3]; T point[3]; point[0]=T(_xyz.x); point[1]=T(_xyz.y); point[2]=T(_xyz.z); AngleAxisRotatePoint(camera, point, p);//计算RP p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5]; T xp = p[0]/p[2]; T yp = p[1]/p[2];//xp,yp是归一化坐标，深度为p[2] T u_= xp*K.at<double>(0,0)+K.at<double>(0,2); T v_= yp*K.at<double>(1,1)+K.at<double>(1,2); residual[0] = T(_uv.x)-u_; residual[1] = T(_uv.y)-v_; return true; } static ceres::CostFunction* Create(const Point2f uv,const Point3f xyz) { return (new ceres::AutoDiffCostFunction<PnPCeres, 2, 6>( new PnPCeres(uv,xyz))); } const Point2f _uv; const Point3f _xyz; };其中，形参uv是坐标点对，xyz是路标点，由于仅仅优化位姿，我们假设路标点确定，像素点对由特征匹配得到，路标为世界坐标，也即第一帧相机坐标，AngleAxisRotatePoint在头文件rotation.h中，它根据相机位姿（旋转向量和平移向量表示，构成的6维数组，不对内参焦距进行优化，不考虑相机畸变），路标点（三维数组），计算得到RP，结合平移向量得到相机坐标，进而得到投影。
位姿初值：
double camera[6]={0,1,2,0,0,0};最小二乘问题的构建：
ceres::Problem problem; for (int i = 0; i < pts_2d.size(); ++i) { ceres::CostFunction* cost_function = PnPCeres::Create(pts_2d[i],pts_3d[i]); problem.AddResidualBlock(cost_function, NULL /* squared loss */, camera); }配置求解器与结构输出：
ceres::Solver::Options options; options.linear_solver_type = ceres::DENSE_SCHUR; options.minimizer_progress_to_stdout = true; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); std::cout << summary.FullReport() << "\n"; Mat R_vec = (Mat_<double>(3,1) << camera[0],camera[1],camera[2]);//数组转cv向量 Mat R_cvest; Rodrigues(R_vec,R_cvest);//罗德里格斯公式，旋转向量转旋转矩阵 cout<<"R_cvest="<<R_cvest<<endl; Eigen::Matrix3d R_est; cv2eigen(R_cvest,R_est);//cv矩阵转eigen矩阵 cout<<"R_est="<<R_est<<endl; Eigen::Vector3d t_est(camera[3],camera[4],camera[5]); cout<<"t_est="<<t_est<<endl; Eigen::Isometry3d T(R_est);//构造变换矩阵与输出 T.pretranslate(t_est); cout<<T.matrix()<<endl; return 0;优化结果：
/home/luoyongheng/study_slam/ch06/ceres_curve_fitting/cmake-build-debug/ICP_G2O 1.png 2.png 1_depth.png 2_depth.png [ INFO:0] Initialize OpenCL runtime... -- Max dist : 95.000000 -- Min dist : 7.000000 一共找到了81组匹配点 3d-2d pairs: 77 R= [0.9979193252225089, -0.05138618904650331, 0.03894200717386666; 0.05033852907733834, 0.9983556574295412, 0.02742286944793203; -0.04028712992734059, -0.02540552801469367, 0.9988651091656532] t= [-0.1255867099750398; -0.007363525258777434; 0.06099926588678889] r= [-0.02643561464539138; 0.03964668696558821; 0.05090359687960295] iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time 0 2.094503e+07 0.00e+00 6.40e+07 0.00e+00 0.00e+00 1.00e+04 0 6.15e-05 1.67e-04 1 6.438043e+06 1.45e+07 1.20e+07 1.36e+00 6.95e-01 1.06e+04 0 9.67e-05 2.87e-04 2 1.803107e+06 4.63e+06 3.13e+06 2.09e+00 7.22e-01 1.17e+04 0 5.50e-05 3.54e-04 3 4.214465e+07 -4.03e+07 0.00e+00 5.78e+00 -2.24e+01 5.83e+03 0 2.89e-05 3.91e-04 4 4.299904e+07 -4.12e+07 0.00e+00 5.77e+00 -2.29e+01 1.46e+03 0 2.55e-05 4.24e-04 5 4.893730e+07 -4.71e+07 0.00e+00 5.72e+00 -2.62e+01 1.82e+02 0 2.63e-05 4.57e-04 6 9.790949e+11 -9.79e+11 0.00e+00 5.26e+00 -5.45e+05 1.14e+01 0 2.52e-05 4.89e-04 7 5.266609e+05 1.28e+06 2.20e+06 2.00e+00 7.80e-01 1.38e+01 0 4.76e-05 5.44e-04 8 6.707009e+04 4.60e+05 1.24e+06 8.40e-01 9.55e-01 4.14e+01 0 5.07e-05 6.03e-04 9 5.987313e+03 6.11e+04 6.69e+04 1.97e-01 9.98e-01 1.24e+02 0 5.13e-05 6.63e-04 10 7.995258e+02 5.19e+03 1.66e+04 1.61e-01 9.87e-01 3.73e+02 0 6.17e-05 7.33e-04 11 1.761029e+02 6.23e+02 2.50e+03 7.18e-02 9.98e-01 1.12e+03 0 4.62e-05 7.87e-04 12 1.598476e+02 1.63e+01 1.46e+02 1.33e-02 1.00e+00 3.35e+03 0 4.59e-05 8.41e-04 13 1.597795e+02 6.81e-02 2.73e+00 9.24e-04 1.00e+00 1.01e+04 0 4.56e-05 8.94e-04 Solver Summary (v 2.0.0-eigen-(3.2.0)-lapack-suitesparse-(4.2.1)-cxsparse-(3.1.2)-eigensparse-openmp-no_tbb) Original Reduced Parameter blocks 1 1 Parameters 6 6 Residual blocks 77 77 Residuals 154 154 Minimizer TRUST_REGION Dense linear algebra library EIGEN Trust region strategy LEVENBERG_MARQUARDT Given Used Linear solver DENSE_SCHUR DENSE_SCHUR Threads 1 1 Linear solver ordering AUTOMATIC 1 Schur structure 2,6,0 2,d,d Cost: Initial 2.094503e+07 Final 1.597795e+02 Change 2.094487e+07 Minimizer iterations 14 Successful steps 10 Unsuccessful steps 4 Time (in seconds): Preprocessor 0.000105 Residual only evaluation 0.000122 (14) Jacobian & residual evaluation 0.000231 (10) Linear solver 0.000239 (14) Minimizer 0.000827 Postprocessor 0.000003 Total 0.000935 Termination: CONVERGENCE (Function tolerance reached. |cost_change|/cost: 2.977157e-07 <= 1.000000e-06) R_cvest=[0.9979190885523205, -0.05138264298263175, 0.03895274962085987; 0.0503343985300836, 0.9983556548774093, 0.02743054317569779; -0.04029815166382588, -0.02541279942112845, 0.9988644795957361] R_est= 0.997919 -0.0513826 0.0389527 0.0503344 0.998356 0.0274305 -0.0402982 -0.0254128 0.998864 t_est=-0.125604 -0.00737588 0.0609989 0.997919 -0.0513826 0.0389527 -0.125604 0.0503344 0.998356 0.0274305 -0.00737588 -0.0402982 -0.0254128 0.998864 0.0609989 0 0 0 1完整代码：
#include <iostream> #include <opencv2/core/core.hpp> #include <ceres/ceres.h> #include <opencv2/features2d/features2d.hpp> #include <opencv2/highgui/highgui.hpp> #include <opencv2/calib3d/calib3d.hpp> #include <opencv2/core/eigen.hpp> #include <Eigen/Core> #include <Eigen/Geometry> #include <chrono> #include "rotation.h" using namespace std; using namespace cv; Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 ); // 代价函数的计算模型 struct PnPCeres { PnPCeres ( Point2f uv,Point3f xyz ) : _uv(uv),_xyz(xyz) {} // 残差的计算 template <typename T> bool operator() ( const T* const camera, // 位姿参数，有6维 T* residual ) const // 残差 { T p[3]; T point[3]; point[0]=T(_xyz.x); point[1]=T(_xyz.y); point[2]=T(_xyz.z); AngleAxisRotatePoint(camera, point, p);//计算RP p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5]; T xp = p[0]/p[2]; T yp = p[1]/p[2];//xp,yp是归一化坐标，深度为p[2] T u_= xp*K.at<double>(0,0)+K.at<double>(0,2); T v_= yp*K.at<double>(1,1)+K.at<double>(1,2); residual[0] = T(_uv.x)-u_; residual[1] = T(_uv.y)-v_; return true; } static ceres::CostFunction* Create(const Point2f uv,const Point3f xyz) { return (new ceres::AutoDiffCostFunction<PnPCeres, 2, 6>( new PnPCeres(uv,xyz))); } const Point2f _uv; const Point3f _xyz; }; void find_feature_matches ( const Mat& img_1, const Mat& img_2, std::vector<KeyPoint>& keypoints_1, std::vector<KeyPoint>& keypoints_2, std::vector< DMatch >& matches ); // 像素坐标转相机归一化坐标 Point2d pixel2cam ( const Point2d& p, const Mat& K ); int main ( int argc, char** argv ) { double camera[6]={0,1,2,0,0,0}; if (argc != 5) { cout << "usage: pose_estimation_3d2d img1 img2 depth1 depth2" << endl; return 1; } //-- 读取图像 Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR); Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR); vector<KeyPoint> keypoints_1, keypoints_2; vector<DMatch> matches; find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches); cout << "一共找到了" << matches.size() << "组匹配点" << endl; // 建立3D点 Mat d1 = imread(argv[3], CV_LOAD_IMAGE_UNCHANGED); // 深度图为16位无符号数，单通道图像 Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1); vector<Point3f> pts_3d; vector<Point2f> pts_2d; for (DMatch m:matches) { ushort d = d1.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); } // double pt[3*pts_3d.size()]; // int i=0; // for(auto p:pts_3d) // { // pt[i]=p.x; // pt[++i]=p.y; // pt[++i]=p.z; // ++i; // } cout << "3d-2d pairs: " << pts_3d.size() << endl; 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公式转换为矩阵 cout << "R=" << endl << R << endl; cout << "t=" << endl << t << endl; cout << "r=" << endl << r << endl; ceres::Problem problem; for (int i = 0; i < pts_2d.size(); ++i) { ceres::CostFunction* cost_function = PnPCeres::Create(pts_2d[i],pts_3d[i]); problem.AddResidualBlock(cost_function, NULL /* squared loss */, camera); } ceres::Solver::Options options; options.linear_solver_type = ceres::DENSE_SCHUR; options.minimizer_progress_to_stdout = true; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); std::cout << summary.FullReport() << "\n"; Mat R_vec = (Mat_<double>(3,1) << camera[0],camera[1],camera[2]);//数组转cv向量 Mat R_cvest; Rodrigues(R_vec,R_cvest);//罗德里格斯公式，旋转向量转旋转矩阵 cout<<"R_cvest="<<R_cvest<<endl; Eigen::Matrix3d R_est; cv2eigen(R_cvest,R_est);//cv矩阵转eigen矩阵 cout<<"R_est="<<R_est<<endl; Eigen::Vector3d t_est(camera[3],camera[4],camera[5]); cout<<"t_est="<<t_est<<endl; Eigen::Isometry3d T(R_est);//构造变换矩阵与输出 T.pretranslate(t_est); cout<<T.matrix()<<endl; return 0; } void find_feature_matches ( const Mat& img_1, const Mat& img_2, std::vector<KeyPoint>& keypoints_1, std::vector<KeyPoint>& keypoints_2, std::vector< DMatch >& matches ) { //-- 初始化 Mat descriptors_1, descriptors_2; // used in OpenCV3 Ptr<FeatureDetector> detector = ORB::create(); Ptr<DescriptorExtractor> descriptor = ORB::create(); // use this if you are in OpenCV2 // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" ); // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" ); Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming"); //-- 第一步:检测 Oriented FAST 角点位置 detector->detect ( img_1,keypoints_1 ); detector->detect ( img_2,keypoints_2 ); //-- 第二步:根据角点位置计算 BRIEF 描述子 descriptor->compute ( img_1, keypoints_1, descriptors_1 ); descriptor->compute ( img_2, keypoints_2, descriptors_2 ); //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离 vector<DMatch> match; // BFMatcher matcher ( NORM_HAMMING ); matcher->match ( descriptors_1, descriptors_2, match ); //-- 第四步:匹配点对筛选 double min_dist=10000, max_dist=0; //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离 for ( int i = 0; i < descriptors_1.rows; i++ ) { double dist = match[i].distance; if ( dist < min_dist ) min_dist = dist; if ( dist > max_dist ) max_dist = dist; } printf ( "-- Max dist : %f \n", max_dist ); printf ( "-- Min dist : %f \n", min_dist ); //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限. for ( int i = 0; i < descriptors_1.rows; i++ ) { if ( match[i].distance <= max ( 2*min_dist, 30.0 ) ) { matches.push_back ( match[i] ); } } } Point2d pixel2cam ( const Point2d& p, const Mat& K ) { return Point2d ( ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ), ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 ) ); }二.添加路标点优化，位姿和路标结果相差太大，预计数据量不够多，约束不足？？？哪位知道，请赐教！
