在ceres中实现PnP优化（仅优化位姿）

一.仅优化位姿

构造类和代价函数：

// 代价函数的计算模型
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 )
            );
}

二.添加路标点优化，位姿和路标结果相差太大，预计数据量不够多，约束不足？？？哪位知道，请赐教！

在ceres中实现PnP优化（仅优化位姿）

继续阅读

PCB基本设计流程详解PCB基本设计流程详解

布线工程师谈PCB设计！

电源完整性与地弹噪声的高速PCB仿真

变种LZ77数据无损压缩算法

推荐一些VB的学习交流网站

重构，开始第一个案例前言 1 重构，开始第一个案例

dos命令(转东转西)

Cocos、Lua游戏内存释放之我见

手机游戏优化技巧

Lua游戏内存泄漏检查

视觉传感器在无人驾驶领域取得技术突破

GNU科学函数库[参考手册][v0.1 Build 090129 Beta][GNU Scientific Library]

对于0-1分数规划的Dinkelbach算法的分析

Sql优化一：sql语句优化

SLAM主要步骤

Git学习笔记5 merge冲突时二选一