标签：总结 ceres const Eigen double 学习 SE3d Sophus

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,const,Eigen,double,学习,SE3d,Sophus
来源： https://blog.csdn.net/qq_35942419/article/details/118109693

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。