C/C++教程

ceres学习总结

本文主要是介绍ceres学习总结,对大家解决编程问题具有一定的参考价值,需要的程序猿们随着小编来一起学习吧!

ceres是用来求解优化问题的工具库。

使用时至少需要构建目标函数,优化变量。

如果自动求导困难,则需要给出雅克比矩阵。(一般是采用了第三方的库,比如eigen和sophus的一些运算)

如果优化变量不对加法封闭,则需要给出优化变量的更新方法。

==============================================================================

对于简单优化问题总体流程:(可以采用自动求导)

1、构建代价函数模型,描述如何计算残差

2、构建问题(要给一个初值)

3、配置求解器

4、结果输出

以两张图片的pnp优化为例:

//ceres pnp
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <chrono>
cv::Mat K = (cv::Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
cv::Point2d pixel2cam ( const cv::Point2d& p, const cv::Mat& K );
void find_feature_matches (
    const cv::Mat& img_1, const cv::Mat& img_2,
    std::vector<cv::KeyPoint>& keypoints_1,
    std::vector<cv::KeyPoint>& keypoints_2,
    std::vector<cv::DMatch>& matches );

struct cost_function  
{
    cost_function(cv::Point3f p1,cv::Point2f p2):pworld(p1),pimg(p2) {}
    template <typename T>
    bool operator()(const T* const ceres_r, const T* const ceres_t, T* residual) const
    { 

        T p_world[3];
        T p_cam[3];
        p_world[0] = T(pworld.x);
        p_world[1] = T(pworld.y);
        p_world[2] = T(pworld.z);
        ceres::AngleAxisRotatePoint(ceres_r,p_world,p_cam);
        p_cam[0] = p_cam[0] + ceres_t[0];
        p_cam[1] = p_cam[1] + ceres_t[1];
        p_cam[2] = p_cam[2] + ceres_t[2];
        // std::cout<< p_cam[0] << p_cam[1] << p_cam[2] <<std::endl;
        const T x = p_cam[0] / p_cam[2];
        const T y = p_cam[1] / p_cam[2];
        const T u = x * T(K.at<double>(0, 0)) + T(K.at<double>(0, 2));
        const T v = y * T(K.at<double>(1, 1)) + T(K.at<double>(1, 2));
        T u1 = T(pimg.x);
        T v1 = T(pimg.y);
        residual[0] = u - u1;
        residual[1] = v - v1;
        return true;
    }
private:
    cv::Point3f pworld;
    cv::Point2f pimg;
};


int main(int argc, char **argv) {

    //-- 读取图像
    cv::Mat img_1 = cv::imread("/home/smz/learning_ref/slambook2-master/ch7/1.png", CV_LOAD_IMAGE_COLOR);
    cv::Mat img_2 = cv::imread("/home/smz/learning_ref/slambook2-master/ch7/2.png", CV_LOAD_IMAGE_COLOR);
    assert(img_1.data && img_2.data && "Can not load images!");

    std::vector<cv::KeyPoint> keypoints_1, keypoints_2;
    std::vector<cv::DMatch> matches;
    find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
    std::cout << "一共找到了" << matches.size() << "组匹配点" << std::endl;

    // 建立3D点
    cv::Mat d1 = cv::imread("/home/smz/learning_ref/slambook2-master/ch7/1_depth.png", CV_LOAD_IMAGE_UNCHANGED);       // 深度图为16位无符号数,单通道图像
    std::vector<cv::Point3f> pts_3d;
    std::vector<cv::Point2f> pts_2d;
    for (cv::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;
        cv::Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
        pts_3d.push_back(cv::Point3f(p1.x * dd, p1.y * dd, dd));
        pts_2d.push_back(keypoints_2[m.trainIdx].pt);
    }

    std::cout << "3d-2d pairs: " << pts_3d.size() << std::endl;

    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    cv::Mat r, t;
    solvePnP(pts_3d, pts_2d, K, cv::Mat(), r, t, false); // 调用OpenCV 的 PnP 求解,可选择EPNP,DLS等方法
    cv::Mat R;
    cv::Rodrigues(r, R); // r为旋转向量形式,用Rodrigues公式转换为矩阵
    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
    std::cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << std::endl;

    std::cout << "R=" << std::endl << R << std::endl;
    std::cout << "t=" << std::endl << t << std::endl;
    
    double ceres_rot[3],ceres_tran[3],ceres_rho[6];
    ceres_rot[0] = 0;
    ceres_rot[1] = 1;
    ceres_rot[2] = 2;
    ceres_tran[0] = t.at<double>(0,0)+0.1;
    ceres_tran[1] = t.at<double>(1,0)+0.1;
    ceres_tran[2] = t.at<double>(2,0)+0.1;
    ceres::Problem problem;
    for(int i = 0; i  < pts_3d.size();i++)
    {
        problem.AddResidualBlock(new ceres::AutoDiffCostFunction<cost_function,2,3,3>(new cost_function(pts_3d[i],pts_2d[i])),nullptr,ceres_rot,ceres_tran);
    }

    ceres::Solver::Options option;
    option.linear_solver_type = ceres::DENSE_SCHUR;
    option.minimizer_progress_to_stdout=true;
    ceres::Solver::Summary summary;
    t1 = std::chrono::steady_clock::now();
    ceres::Solve(option,&problem,&summary);
    t2 = std::chrono::steady_clock::now();
    time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
    std::cout << summary.BriefReport() << std::endl;
    std::cout << "solve pnp by ceres cost time: " << time_used.count() << " seconds." << std::endl;
    cv::Mat cam_3d = ( cv::Mat_<double> ( 3,1 )<<ceres_rot[0],ceres_rot[1],ceres_rot[2]);
    cv::Mat cam_9d;
    cv::Rodrigues (cam_3d, cam_9d); 
    std::cout << "cam_9d:" << std::endl << cam_9d << std::endl;

    std::cout << "cam_t:" << ceres_tran[0] << "  " << ceres_tran[1] << "  " << ceres_tran[2] << std::endl;

    return 0;
}

void find_feature_matches(const cv::Mat &img_1, const cv::Mat &img_2,
                          std::vector<cv::KeyPoint> &keypoints_1,
                          std::vector<cv::KeyPoint> &keypoints_2,
                          std::vector<cv::DMatch> &matches) {
    //-- 初始化
    cv::Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();
    cv::Ptr<cv::DescriptorExtractor> descriptor = cv::ORB::create();
    cv::Ptr<cv::DescriptorMatcher> matcher = cv::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 距离
    std::vector<cv::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 <= cv::max(2 * min_dist, 30.0)) {
        matches.push_back(match[i]);
        }
    }
}

cv::Point2d pixel2cam(const cv::Point2d &p, const cv::Mat &K) {
  return cv::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)
    );
}


基本上可以简述为:

struct cost_function  //构建代价函数
{
    cost_function(cv::Point3f p1,cv::Point2f p2):pworld(p1),pimg(p2) {}//将非优化变量输入
    template <typename T> //定义模板函数
    bool operator()(const T* const ceres_r, const T* const ceres_t, T* residual) const 
    { 
        //给出非优化变量和优化变量应该如何计算得到最后的结果(残差),其中优化变量应该是以数组形式输入,残差也需要表示为数组格式
        T p_world[3];
        T p_cam[3];
        p_world[0] = T(pworld.x);
        p_world[1] = T(pworld.y);
        p_world[2] = T(pworld.z);
        ceres::AngleAxisRotatePoint(ceres_r,p_world,p_cam);
        p_cam[0] = p_cam[0] + ceres_t[0];
        p_cam[1] = p_cam[1] + ceres_t[1];
        p_cam[2] = p_cam[2] + ceres_t[2];
        // std::cout<< p_cam[0] << p_cam[1] << p_cam[2] <<std::endl;
        const T x = p_cam[0] / p_cam[2];
        const T y = p_cam[1] / p_cam[2];
        const T u = x * T(K.at<double>(0, 0)) + T(K.at<double>(0, 2));
        const T v = y * T(K.at<double>(1, 1)) + T(K.at<double>(1, 2));
        T u1 = T(pimg.x);
        T v1 = T(pimg.y);
        residual[0] = u - u1;
        residual[1] = v - v1;
        return true;
    }
private:
    cv::Point3f pworld;
    cv::Point2f pimg;
};
    ceres::Problem problem; //构建优化问题
    for(int i = 0; i  < pts_3d.size();i++)
    {
        problem.AddResidualBlock(new ceres::AutoDiffCostFunction<cost_function,2,3,3>(new cost_function(pts_3d[i],pts_2d[i])),nullptr,ceres_rot,ceres_tran);//给出每个问题的误差
    }

==============================================================================

复杂一点的需要给参数更新方式和雅克比矩阵

//  利用ceres实现位姿图优化,位姿图的输入是一个球,数据在文件sphere.g2o中 数据结构如下
/*  sphere.g2o  结构
数据:
VERTEX_SE3:QUAT 2499 1.0318 85.323 -76.0615 0.779315 0.0718127 -0.620437 -0.0506864 
EDGE_SE3:QUAT 0 1 -0.0187953 0.0328449 -0.125146 0.0634648 -0.000250128 0.00237634 0.997981 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 40000 0 0 40000 0 40000 
顶点:
数据中以VERTEX_SE3:QUAT开头的为顶点,结构为VERTEX_SE3:QUAT 节点id 平移 四元数
边:
以EDGE_SE3:QUAT 开头的为边,结构为 EDGE_SE3:QUAT 节点id1 节点id2 平移 四元数 信息矩阵上三角(6*6的信息矩阵,上三角为6*6/2+6/2=21)
*/
#include <iostream>
#include <fstream>

#include <Eigen/Core>

#include <sophus/so3.hpp>
#include <sophus/se3.hpp>
#include <ceres/ceres.h>

#include <pcl/point_types.h> 
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>  

typedef Eigen::Matrix<double,6,6> Matrix6d;
typedef Eigen::Matrix<double,6,1> Vector6d;

class mypoint 
{
public: 
    mypoint(int _id1, double x,double y,double z ,double q0 ,double q1 ,double q2 ,double q3){
        id1 = _id1;
        param[0] = x;
        param[1] = y;
        param[2] = z;
        param[3] = q0;
        param[4] = q1;
        param[5] = q2;
        param[6] = q3;
        con_param2se3();
    }
    int id1;
    double se3[6];
private:
    void con_param2se3();
    double param[7];
};

void mypoint::con_param2se3()
{
    Eigen::Quaterniond q(param[3], param[4], param[5], param[6]);
    q.normalize();
    Eigen::Matrix<double,6,1> se = Sophus::SE3d( q, Eigen::Vector3d(param[0], param[1], param[2]) ).log();
    for(int i = 0; i < 6; i++)
    {
        se3[i] = se(i,0);
    }
}

class SE3Parameterization : public ceres::LocalParameterization
{
public:
    SE3Parameterization() {}
    virtual ~SE3Parameterization() {}
    virtual bool Plus(const double* x,
                      const double* delta,
                      double* x_plus_delta) const
    {
        Eigen::Map<const Eigen::Matrix<double, 6, 1>> varse(x);
        Eigen::Map<const Eigen::Matrix<double, 6, 1>> delta_varse(delta);

        Sophus::SE3d T = Sophus::SE3d::exp(varse);
        Sophus::SE3d delta_T = Sophus::SE3d::exp(delta_varse);

        Vector6d x_plus_delta_varse = (delta_T * T).log();

        for(int i = 0; i < 6; ++i)
            x_plus_delta[i] = x_plus_delta_varse(i, 0);

        return true;
    }
    virtual bool ComputeJacobian(const double* x,
                                 double* jacobian) const
    {
        ceres::MatrixRef(jacobian, 6, 6) = ceres::Matrix::Identity(6, 6);
        return true;
    }
    virtual int GlobalSize() const { return 6; }
    virtual int LocalSize() const { return 6; }
};


class PoseGraphCostFunction : public ceres::SizedCostFunction<6,6,6>{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    ~PoseGraphCostFunction(){}
    PoseGraphCostFunction(Sophus::SE3d _se3, Matrix6d _covariance): edge_se3(_se3), convariance(_covariance){}

    virtual bool Evaluate(double const* const* parameters,
                          double *residuals,
                          double **jacobians) const{
        Sophus::SE3d pose_i = Sophus::SE3d::exp(Vector6d(parameters[0]));
        Sophus::SE3d pose_j = Sophus::SE3d::exp(Vector6d(parameters[1]));
        Matrix6d sqrt_info = Eigen::LLT<Matrix6d>(convariance.inverse()).matrixLLT().transpose();
        // Matrix6d sqrt_info = Eigen::LLT<Matrix6d>(convariance.inverse()).matrixLLT().transpose();
        Eigen::Map<Vector6d> residual(residuals);
        // residual = (edge_se3.inverse() * estimate).log();
        residual = (edge_se3.inverse() * pose_i.inverse() * pose_j).log();
        if(jacobians){
            Matrix6d Jr;
            Jr.block(0,0,3,3) = Sophus::SO3d::hat(Sophus::SE3d::exp(residual).so3().log());
            Jr.block(0,3,3,3) = Sophus::SO3d::hat(Sophus::SE3d::exp(residual).translation());
            Jr.block(3,0,3,3) = Eigen::Matrix3d::Zero(3,3);
            Jr.block(3,3,3,3) = Jr.block(0,0,3,3);
            Jr = Matrix6d::Identity() + 0.5 * Jr ;
            Matrix6d jacA = - Jr * pose_j.inverse().Adj();
            Matrix6d jacB = - jacA;
            if(jacobians[0])
            {
                Eigen::Map<Eigen::Matrix<double,6,6, Eigen::RowMajor>> jacobian_i(jacobians[0]);
                jacobian_i = sqrt_info * jacA;
            }
            if(jacobians[1])
            {
                Eigen::Map<Eigen::Matrix<double,6,6, Eigen::RowMajor>> jacobian_j(jacobians[1]);
                jacobian_j = sqrt_info * jacB;
            }
        }
        residual = sqrt_info * (edge_se3.inverse() * pose_i.inverse() * pose_j).log();
        return true;
    }
private:
    const Sophus::SE3d edge_se3;
    const Eigen::Matrix<double,6,6>  convariance;
};



void drawpclpoint(std::vector<mypoint> points)
{
    pcl::PointCloud<pcl::PointXYZRGB> ::Ptr pointCloud( new pcl::PointCloud<pcl::PointXYZRGB> ); 
    for(int i = 0; i < points.size(); i++ )
    {
        pcl::PointXYZRGB pointpcl;
        Eigen::Map<const Eigen::Matrix<double,6,1>> poseAVec6d( points[i].se3 );
        Sophus::SE3d poseSE3 = Sophus::SE3d::exp(poseAVec6d);        
        Eigen::Matrix<double,3,1> point_t = poseSE3.translation().transpose();
        pointpcl.x = point_t(0,0);
        pointpcl.y = point_t(1,0);
        pointpcl.z = point_t(2,0);
        pointpcl.b = 0;
        pointpcl.g = 0;
        pointpcl.r = 255;
        pointCloud->points.push_back( pointpcl );
    }
    // pointCloud->is_dense = true;
    pcl::visualization::CloudViewer viewer("Cloud Viewer");
    viewer.showCloud(pointCloud);
    while (!viewer.wasStopped()) 
    {
        
    }
}


int main(int argc, char** argv)
{
    const std::string file_path = "/home/smz/learning_ref/slambook2-master/ch10/sphere.g2o";
    std::ifstream g2oFile(file_path);
    if(!g2oFile)
    {
        std::cout << "file " << file_path << "doesnot exist" << std::endl;
        return -1; 
    }
    ceres::Problem problem;
    ceres::LocalParameterization *local_param = new SE3Parameterization();


    int pose_num = 0;
    int edge_num = 0;
    std::vector<mypoint> points;

    while(!g2oFile.eof())
    {
        std::string flag;
        g2oFile >> flag;
        if(flag[0] == 'V')
        {
            pose_num++;
            int id;
            double x,y,z,q0,q1,q2,q3;
            g2oFile >> id >> x >> y >> z >> q1 >> q2 >> q3 >> q0;
            mypoint p(id,x,y,z,q0,q1,q2,q3);
            points.push_back(p);
        }
        if(flag[0] == 'E')
        {
            edge_num++;
            int id1,id2;
            double x,y,z,q0,q1,q2,q3;
            g2oFile >> id1 >> id2 >> x >> y >> z >> q1 >> q2 >> q3 >> q0;
            Sophus::SE3d edge_m(Eigen::Quaterniond(q0, q1, q2, q3).normalized(),Eigen::Vector3d(x, y, z));
            Eigen::Matrix<double,6,6> information;
            for(int i = 0;i < 6; i++)
            {
                for(int j = i;j < 6; j++)
                {
                    g2oFile >> information(i,j);
                    if(j != i)
                        information(j, i) = information(i, j);
                }
            }
            ceres::LossFunction *loss = new ceres::HuberLoss(1.0);
            ceres::CostFunction *costfunc = new PoseGraphCostFunction(edge_m, information);
            problem.AddResidualBlock(costfunc, loss, points[id1].se3, points[id2].se3);
            problem.SetParameterization(points[id1].se3, local_param);
            problem.SetParameterization(points[id2].se3, local_param);
        }
    }
    g2oFile.close();


    drawpclpoint(points);

    
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
    options.max_linear_solver_iterations = 50;
    options.minimizer_progress_to_stdout = true;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.FullReport() << std::endl;

    std::ofstream txt("./result.g2o");
    for( int i=0; i < points.size(); i++ )
    {
        Eigen::Map<const Eigen::Matrix<double,6,1>> poseAVec6d( points[i].se3 );
        Sophus::SE3d poseSE3 = Sophus::SE3d::exp(poseAVec6d);
        Eigen::Quaterniond q = poseSE3.so3().unit_quaternion();

        txt << "VERTEX_SE3:QUAT" << ' ';
        txt << i << ' ';
        txt << poseSE3.translation().transpose() << ' ';
        txt << q.x() <<' '<< q.y()<< ' ' << q.z() <<' '<< q.w()<<' ' << std::endl;
    }
    g2oFile.open(file_path);
    while(!g2oFile.eof()){
        std::string s;
        std::getline(g2oFile, s);
        if(s[0] != 'E') continue;
        else txt << s << std::endl;
    }
    g2oFile.close();
    txt.close();

    drawpclpoint(points);

    return 0;
}

其中class SE3Parameterization : public ceres::LocalParameterization 构建了优化变量

class SE3Parameterization : public ceres::LocalParameterization
{
public:
    SE3Parameterization() {}
    virtual ~SE3Parameterization() {}
    virtual bool Plus(const double* x,
                      const double* delta,
                      double* x_plus_delta) const
    {
        Eigen::Map<const Eigen::Matrix<double, 6, 1>> varse(x);
        Eigen::Map<const Eigen::Matrix<double, 6, 1>> delta_varse(delta);

        Sophus::SE3d T = Sophus::SE3d::exp(varse);
        Sophus::SE3d delta_T = Sophus::SE3d::exp(delta_varse);

        Vector6d x_plus_delta_varse = (delta_T * T).log();

        for(int i = 0; i < 6; ++i)
            x_plus_delta[i] = x_plus_delta_varse(i, 0);

        return true;
    }
    virtual bool ComputeJacobian(const double* x,
                                 double* jacobian) const
    {
        ceres::MatrixRef(jacobian, 6, 6) = ceres::Matrix::Identity(6, 6);
        return true;
    }
    virtual int GlobalSize() const { return 6; }
    virtual int LocalSize() const { return 6; }
};

输入是变量,增量,变量+增量,用来构建更新方式,同时给出雅克比矩阵,最后给出变量维度和正切空间维度

class PoseGraphCostFunction : public ceres::SizedCostFunction<6,6,6>{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    ~PoseGraphCostFunction(){}
    PoseGraphCostFunction(Sophus::SE3d _se3, Matrix6d _covariance): edge_se3(_se3), convariance(_covariance){}

    virtual bool Evaluate(double const* const* parameters,
                          double *residuals,
                          double **jacobians) const{
        Sophus::SE3d pose_i = Sophus::SE3d::exp(Vector6d(parameters[0]));
        Sophus::SE3d pose_j = Sophus::SE3d::exp(Vector6d(parameters[1]));
        Matrix6d sqrt_info = Eigen::LLT<Matrix6d>(convariance.inverse()).matrixLLT().transpose();
        // Matrix6d sqrt_info = Eigen::LLT<Matrix6d>(convariance.inverse()).matrixLLT().transpose();
        Eigen::Map<Vector6d> residual(residuals);
        // residual = (edge_se3.inverse() * estimate).log();
        residual = (edge_se3.inverse() * pose_i.inverse() * pose_j).log();
        if(jacobians){
            Matrix6d Jr;
            Jr.block(0,0,3,3) = Sophus::SO3d::hat(Sophus::SE3d::exp(residual).so3().log());
            Jr.block(0,3,3,3) = Sophus::SO3d::hat(Sophus::SE3d::exp(residual).translation());
            Jr.block(3,0,3,3) = Eigen::Matrix3d::Zero(3,3);
            Jr.block(3,3,3,3) = Jr.block(0,0,3,3);
            Jr = Matrix6d::Identity() + 0.5 * Jr ;
            Matrix6d jacA = - Jr * pose_j.inverse().Adj();
            Matrix6d jacB = - jacA;
            if(jacobians[0])
            {
                Eigen::Map<Eigen::Matrix<double,6,6, Eigen::RowMajor>> jacobian_i(jacobians[0]);
                jacobian_i = sqrt_info * jacA;
            }
            if(jacobians[1])
            {
                Eigen::Map<Eigen::Matrix<double,6,6, Eigen::RowMajor>> jacobian_j(jacobians[1]);
                jacobian_j = sqrt_info * jacB;
            }
        }
        residual = sqrt_info * (edge_se3.inverse() * pose_i.inverse() * pose_j).log();
        return true;
    }
private:
    const Sophus::SE3d edge_se3;
    const Eigen::Matrix<double,6,6>  convariance;
};


注意:1、所有变量都是在parameters中,输入的时候需要将其分解分别保存

2、注意行主还是列主的矩阵

其他的就和自动求导的一样

 

 

这篇关于ceres学习总结的文章就介绍到这儿,希望我们推荐的文章对大家有所帮助,也希望大家多多支持为之网!