SlamBook14讲-ICP

Eigen中用于特征值分解的几个类的介绍 官方文档里有这几个函数的对比第二列是这些方法对矩阵的要求,第三列是计算的速度,第四列是算法的可靠性与准确性。BDCSVD和JacobiSVD是计算奇异值的,如果维度小于16, 可以直接用JacobiSVD。SelfAdjointEigenSolver是计算实对称矩阵或者复数下的自伴矩阵的特征值与特征向量的。ComplexEigenSolver是计算复数矩阵的特征值与特征向量的。EigenSolver是一般矩阵(非对称矩阵,复数矩阵)的特征值与特征向量。 阅读详情

很久没有更新 有些懈怠 争取这个月把这本书啃完

ICP主要用在3d-3d点的相机位姿估计问题上 主要分为SVD与非线性优化两种方法

SVD:

整体思路是在获取了两张图片匹配点的,带有深度信息的点后,对其进行一定的处理,通过化简目标函数,最后通过求解一个矩阵的SVD来求的R和t。具体的推导在书中有 不再赘述。

在编写代码的时候我基本没有看示例代码,按照数学推导自己编写的代码,不是很难。

svd的计算用到了

Eigen:: JacobiSVD<Eigen::Matrix<double,3,3>>(W, Eigen::ComputeFullU|Eigen::ComputeFullV)

函数,模板参数是要分解的矩阵类型。后面的参数表示分别计算方针U和V

参数还有ComputeThinU,ComputeThinV,区别是矩阵维数是否为动态的:

Thin unitaries are only available if your matrix type has a Dynamic number of columns (for example MatrixXf).

通过svd.matrixU()和svd.matrixU()即可得到U和V。

这里需要注意如何将常用的Eigen库Eigen::Martix3d转化为OpenCV的图像格式cv::Mat,这里用的是R = ( cv::Mat_<double> ( 3,3 ) << R_ ( 0,0 ), R_ ( 0,1 ), R_ ( 0,2 ), R_ ( 1,0 ), R_ ( 1,1 ), R_ ( 1,2 ), R_ ( 2,0 ), R_ ( 2,1 ), R_ ( 2,2 ));

借助了cv::Mat_是Mat的子类,它可以直接通过重载<<运算符构造出匿名对象,很方便。Eigen库可以通过重载的()运算符直接访问具体的元素。

BA:

仍在是借助了图优化的内容,但是复杂了一些。重点是雅可比矩阵的计算。

先简单复习一下g2o的优化过程,我的其他文章已经写过了

首先需要定义线性方程求解器,然后定义矩阵块求解器,选择算法,然后创建优化对象。

typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,3>> Block;// Implementation of a solver operating on the blocks of the Hessian 矩阵块求解器
Block::LinearSolverType* LinearSovler = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); //线性方程求解器 //需要在cmake里链接g2o_csparse_extension库
Block* BlockSolverPtr = new Block(std::unique_ptr<Block::LinearSolverType> (LinearSovler)); //构造函数创建矩阵块求解器对象(指针)
g2o::OptimizationAlgorithmLevenberg* Algorithm = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<g2o::Solver>(BlockSolverPtr));//BlockSolver是g2o::Solver的子类 共有继承下来的
g2o::SparseOptimizer Optimizer;
Optimizer.setAlgorithm(Algorithm);

对于这个问题,目标函数是

 相对于李代数exp(\xi ^{\wedge} )最小化它的误差e(x),需要对李代数进行求导,也就是李代数的扰动模型。

后文再细说为什么是这个形式-(TP)^{\ominus }

这里的顶点用的是自带的g2o::VertexSE3Expmap,注意需要在CMakeLists中添加g2o_types_sba,因为VertexSE3Expmap是在这个库中定义的,需要进行链接。

    //添加顶点 
    g2o::VertexSE3Expmap* VertexPose = new  g2o::VertexSE3Expmap(); //自带类 se3类型 包含R和t
    VertexPose->setId(0);
    Eigen::Matrix3d R_;
    Eigen::Matrix3d _R = Eigen::Matrix3d::Identity() ;
    R_ << R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ), R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ), R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
    Eigen::Vector3d t_;
    t_ << t.at<double> (0,0), t.at<double> (1,0), t.at<double> (2,0);
    g2o::SE3Quat et(R_, t_);
    VertexPose->setEstimate(et);
    Optimizer.addVertex(VertexPose);

这个类在进行setEstimate时可要先将R和t转化为四元数的形式。

接下来是边的定义,需要自己重写一个

class Edge3d : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap>{//误差值的维度 类型 顶点类型 这里误差是p-exp(e)p' 三维向量
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;//宏函数 重写内存分配函数
    Edge3d(const Eigen::Vector3d& point) : _point(point){}; //传进来的是p'
    
    virtual void computeError(){    //虚函数 指针交给父类管理 写入目标函数
        const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap* >(_vertices[0]); //把顶点转换为g2o::VertexSE3Expmap类型 同时把内容传进来
        _error = _measurement - pose->estimate().map(_point);//pose->estimate().map(_point)就是把p'坐标进行坐标变换 measurement是p
    }

    //这里只需要误差项关于相机位姿(李代数)的导数 也就是Xi雅可比
    //https://blog.csdn.net/zhubaohua_bupt/article/details/74011005?ops_request_misc=&request_id=&biz_id=102&utm_term=%E7%9B%B8%E6%9C%BA%E4%BD%8D%E5%A7%BF%E4%BC%98%E5%8C%96%20%E9%9B%85%E5%8F%AF%E6%AF%94&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-0-74011005.142^v70^js_top,201^v4^add_ask&spm=1018.2226.3001.4187
    virtual void linearizeOplus(){  //线性化 利用一阶泰勒展开近似 需要定义雅可比矩阵j
        const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap* >(_vertices[0]); //把顶点转换为g2o::VertexSE3Expmap类型 同时把内容传进来
        Eigen::Vector3d PointTrans = pose->estimate().map(_point);
        
        double x = PointTrans[0];
        double y = PointTrans[1];
        double z = PointTrans[2];

        _jacobianOplusXi(0,0) = 0;
        _jacobianOplusXi(0,1) = -z;
        _jacobianOplusXi(0,2) = y;
        _jacobianOplusXi(0,3) = -1;
        _jacobianOplusXi(0,4) = 0;
        _jacobianOplusXi(0,5) = 0;

        _jacobianOplusXi(1,0) = z;
        _jacobianOplusXi(1,1) = 0;
        _jacobianOplusXi(1,2) = -x;
        _jacobianOplusXi(1,3) = 0;
        _jacobianOplusXi(1,4) = -1;
        _jacobianOplusXi(1,5) = 0;

        _jacobianOplusXi(2,0) = -y;
        _jacobianOplusXi(2,1) = x;
        _jacobianOplusXi(2,2) = 0;
        _jacobianOplusXi(2,3) = 0;
        _jacobianOplusXi(2,4) = 0;
        _jacobianOplusXi(2,5) = -1;
    }

    bool read( std::istream& in){}
    bool write( std::ostream& out )const{}
protected:
    Eigen::Vector3d _point;
};

EIGEN_MAKE_ALIGNED_OPERATOR_NEW是Eigen中的宏函数,用于进行内存分配的。

computeError就是目标函数measurement是观测值,后者是预测值。

static_cast是一种类型转换 把储存的顶点类型转换为我们需要的VertexSE3Expmap

pose->estimate()是我们要的估计值,pose->estimate().map()其实就是对某个点进行坐标变换,pose->estimate().map(_point)就是对第二张图的点,利用迭代出的相继位姿进行变换,也就是exp(\xi^{\wedge})P'

这里书中没有再给出雅可比矩阵的推导过程,具体可以参考这篇文章:

SLAM优化位姿时,误差函数的雅可比矩阵的推导。_zhubaohua_bupt的博客-CSDN博客_slam 雅可比矩阵

 根据文章的推导,可以看出原本的雅可比矩阵分为三部分(书中进行了合并,分为了两部分)

第一部分是误差关于投影点的导数,对应BaseUnaryEdge中的_jacobianOplusXj,这部分不涉及复杂的扰动模型,归结到底就是相机归一化平面的模型。

第二部分是误差关于相机位姿的导数,对应BaseUnaryEdge中的_jacobianOplusXi,这里用到了扰动模型

回想3d-2d最小化重投影误差的时候,由于相机位姿和观测点是一起考虑的,所以分别计算了两部分雅可比,最终是2*6的矩阵。

但在3d-3d的优化过程中,其实是不必考虑相机内参部分的,所以ICP也可以用于激光SLAM领域。

由于不需要考虑关于投影点的雅可比,所以只需要在写代码时重写_jacobianOplusXi就可以了,是一个3*6的矩阵。这里需要将前三列与后三列调换位置,主要是g2o的规则不太相同。

在边的类写完之后,只需要将之前匹配好的点导入至各个边中就可以了,主要分为

setid, setvertex, setInformation, setMeasurement几个步骤

最后一个 setMeasurement 就会把值保存在computeError中的_ measurement中,一切就说通了。

优化完成后,通过访问顶点VertexPose->estimate就可以得到优化后的位姿,但他仍在是四元数的形式,可以通过类中的提供的方法to_homogeneous_matrix()变成李代数的形式。

最后附上计算结果:

********************************SVD********************************
[0.9984466389557263, 0.05557944836736908, 0.003903085598995937;
 -0.05512774034557463, 0.9956276426496312, -0.07540908059596663;
 -0.008077215015402317, 0.07507677477833613, 0.9971450428530838]
[0.0487365597401111;
 0.0487365597401111;
 0.0487365597401111]
iteration= 0     chi2= 0.500594  time= 4.0599e-05        cumTime= 4.0599e-05     edges= 65       schur= 0        lambda= 0.000596        levenbergIter= 1
iteration= 1     chi2= 0.500594  time= 1.0461e-05        cumTime= 5.106e-05      edges= 65       schur= 0        lambda= 0.000397        levenbergIter= 1
iteration= 2     chi2= 0.500594  time= 9.11e-06  cumTime= 6.017e-05      edges= 65       schur= 0        lambda= 0.000265        levenbergIter= 1
iteration= 3     chi2= 0.500594  time= 8.93e-06  cumTime= 6.91e-05       edges= 65       schur= 0        lambda= 0.000177        levenbergIter= 1
iteration= 4     chi2= 0.500594  time= 8.95e-06  cumTime= 7.805e-05      edges= 65       schur= 0        lambda= 0.000118        levenbergIter= 1
iteration= 5     chi2= 0.500594  time= 3.0891e-05        cumTime= 0.000108941    edges= 65       schur= 0        lambda= 4241247602600.487305 levenbergIter= 10
********************************BA********************************
[0.9984466389541541, 0.05557944839565528, 0.003903085598318479;
 -0.05512774037632337, 0.9956276426712256, -0.07540908028837792;
 -0.008077214999849563, 0.07507677447102423, 0.9971450428763476]
[0.04873655974105028;
 0.08215334050889304;
 -0.02880827951251929]

这里仍在是用SVD的结果作为BA的输入,我试着用单位阵作为R的初始迭代值,基本没有偏差,依然很准确。

最后附上源码:

#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/imgcodecs.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <g2o/core/block_solver.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/types/slam3d/vertex_pointxyz.h>
#include <g2o/types/sba/vertex_se3_expmap.h>
#include <g2o/core/base_unary_edge.h>

cv::Point2d pixel2cam ( const cv::Point2d& , const cv::Mat&  );
void PoseEstimationSVD(const std::vector<cv::Point3d>& p1, const std::vector<cv::Point3d>& p2, cv::Mat& R, cv::Mat& t);
void PoseEstimation3d3dBA(cv::Mat& , cv::Mat& , const std::vector<cv::Point3d>& , const std::vector<cv::Point3d>& );

class Edge3d : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap>{//误差值的维度 类型 顶点类型 这里误差是p-exp(e)p' 三维向量
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;//宏函数 重写内存分配函数
    Edge3d(const Eigen::Vector3d& point) : _point(point){}; //传进来的是p'
    
    virtual void computeError(){    //虚函数 指针交给父类管理 写入目标函数
        const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap* >(_vertices[0]); //把顶点转换为g2o::VertexSE3Expmap类型 同时把内容传进来
        _error = _measurement - pose->estimate().map(_point);//pose->estimate().map(_point)就是把p'坐标进行坐标变换 measurement是p
    }

    //这里只需要误差项关于相机位姿(李代数)的导数 也就是Xi雅可比
    //https://blog.csdn.net/zhubaohua_bupt/article/details/74011005?ops_request_misc=&request_id=&biz_id=102&utm_term=%E7%9B%B8%E6%9C%BA%E4%BD%8D%E5%A7%BF%E4%BC%98%E5%8C%96%20%E9%9B%85%E5%8F%AF%E6%AF%94&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-0-74011005.142^v70^js_top,201^v4^add_ask&spm=1018.2226.3001.4187
    virtual void linearizeOplus(){  //线性化 利用一阶泰勒展开近似 需要定义雅可比矩阵j
        const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap* >(_vertices[0]); //把顶点转换为g2o::VertexSE3Expmap类型 同时把内容传进来
        Eigen::Vector3d PointTrans = pose->estimate().map(_point);
        
        double x = PointTrans[0];
        double y = PointTrans[1];
        double z = PointTrans[2];

        _jacobianOplusXi(0,0) = 0;
        _jacobianOplusXi(0,1) = -z;
        _jacobianOplusXi(0,2) = y;
        _jacobianOplusXi(0,3) = -1;
        _jacobianOplusXi(0,4) = 0;
        _jacobianOplusXi(0,5) = 0;

        _jacobianOplusXi(1,0) = z;
        _jacobianOplusXi(1,1) = 0;
        _jacobianOplusXi(1,2) = -x;
        _jacobianOplusXi(1,3) = 0;
        _jacobianOplusXi(1,4) = -1;
        _jacobianOplusXi(1,5) = 0;

        _jacobianOplusXi(2,0) = -y;
        _jacobianOplusXi(2,1) = x;
        _jacobianOplusXi(2,2) = 0;
        _jacobianOplusXi(2,3) = 0;
        _jacobianOplusXi(2,4) = 0;
        _jacobianOplusXi(2,5) = -1;
    }

    bool read( std::istream& in){}
    bool write( std::ostream& out )const{}
protected:
    Eigen::Vector3d _point;
};


int main(){
    cv::Mat img1 = cv::imread("../1.png",1);
    cv::Mat img2 = cv::imread("../2.png",1);
    cv::Mat depth1 = cv::imread("../1_depth.png");
    cv::Mat depth2 = cv::imread("../2_depth.png");   

    std::vector<cv::KeyPoint> KeyPoint1,KeyPoint2;
    cv::Mat descriptors1,descriptors2;
    cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();//represented as vectors in a multidimensional space
    cv::Ptr<cv::DescriptorExtractor> descriptor = cv::ORB::create();//初始化描述子 descriptor为指针
    cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");

    detector->detect(img1,KeyPoint1);
    detector->detect(img2,KeyPoint2);

    descriptor->compute(img1,KeyPoint1,descriptors1);
    descriptor->compute(img2,KeyPoint2,descriptors2);

    std::vector<cv::DMatch> matches;
    matcher->match(descriptors1,descriptors2,matches);

    double min = 10000,max = 0;
    min = min_element( matches.begin(), matches.end(), [](const cv::DMatch& m1, const cv::DMatch& m2) {return m1.distance<m2.distance;} )->distance;
    max = max_element( matches.begin(), matches.end(), [](const cv::DMatch& m1, const cv::DMatch& m2) {return m1.distance<m2.distance;} )->distance;

    std::vector<cv::DMatch> GoodMatches;
    for(auto i = matches.begin(); i < matches.end(); i++){
        if(i->distance <= std::max( 2*min, 30.0 )){
            GoodMatches.push_back(*i);
        }
    }

    std::vector<cv::Point3d> depthpoint1;
    std::vector<cv::Point3d> depthpoint2;    
    cv::Mat K = (cv::Mat_<double>(3,3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);//相机内参 fx fy cx cy

    for(auto i:GoodMatches){
        unsigned short d1 = depth1.ptr<unsigned short>(int(KeyPoint1[i.queryIdx].pt.y))[int(KeyPoint1[i.queryIdx].pt.x)];
        unsigned short d2 = depth2.ptr<unsigned short>(int(KeyPoint2[i.trainIdx].pt.y))[int(KeyPoint2[i.trainIdx].pt.x)];
        if(d1 == 0 || d2 == 0) continue;
        cv::Point2d pixel1(KeyPoint1[i.queryIdx].pt.x,KeyPoint1[i.queryIdx].pt.y);
        cv::Point2d pixel2(KeyPoint2[i.trainIdx].pt.x,KeyPoint2[i.trainIdx].pt.y);
        cv::Point3d temp1(pixel2cam(pixel1,K).x*d1/5000.0, pixel2cam(pixel1,K).y*d1/5000.0, d1/5000.0);
        cv::Point3d temp2(pixel2cam(pixel2,K).x*d2/5000.0, pixel2cam(pixel2,K).y*d2/5000.0, d2/5000.0);
        depthpoint1.push_back(temp1);
        depthpoint2.push_back(temp2); 
    }

    //std::cout << GoodMatches.size() << std::endl;
    cv::Mat R,t;
    PoseEstimationSVD(depthpoint1,depthpoint2,R,t);
    std::cout << "********************************SVD********************************" << std::endl;
    std::cout << R << std::endl;
    std::cout << t << std::endl;
    PoseEstimation3d3dBA(R,t,depthpoint1, depthpoint2);
    std::cout << "********************************BA********************************" << std::endl;   
    std::cout << R << std::endl;
    std::cout << t << std::endl;
    return 0;
}

//像素平面到归一化平面变换 相差相机内参
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 )
           );
}

void PoseEstimationSVD(const std::vector<cv::Point3d>& p1, const std::vector<cv::Point3d>& p2, cv::Mat& R, cv::Mat& t){
    //计算质心
    cv::Point3d weight1, weight2;
    auto j = p2.begin();
    int count = p1.size();
    for(auto i = p1.begin(); i < p1.end() ; i++ ,j++){
        weight1.x += i->x;
        weight1.y += i->y;
        weight1.z += i->z;
        weight2.x += j->x;
        weight2.y += j->y;
        weight2.z += j->z;
    }
    weight1.x = weight1.x / count;
    weight1.y = weight1.y / count;
    weight1.z = weight1.z / count;
    weight2.x = weight2.x / count;
    weight2.y = weight2.y / count;
    weight2.z = weight2.z / count;

    //计算去质心坐标
    std::vector<Eigen::Vector3d> q1, q2;
    j = p2.begin();
    for(auto i = p1.begin(); i < p1.end() ; i++ ,j++){
        Eigen::Vector3d temp1(i->x - weight1.x, i->y - weight1.y, i->z - weight1.z);
        Eigen::Vector3d temp2(j->x - weight2.x, j->y - weight2.y, j->z - weight2.z);

        q1.push_back(temp1);
        q2.push_back(temp2);
    }

    //计算W矩阵
    Eigen::Matrix<double,3,3> W = Eigen::Matrix3d::Zero();
    auto ii = q2.begin();
    for(auto i = q1.begin(); i < q1.end() ; i++ ,ii++){
        //W += Eigen::Vector3d(*i) * Eigen::Vector3d(*ii).transpose();
        W += (*i) * (ii->transpose());
    }

    //std::cout << "W" << W <<std::endl;
    //对M 进行SVD分解 得到UV
    Eigen:: JacobiSVD<Eigen::Matrix<double,3,3>> svd(W, Eigen::ComputeFullU|Eigen::ComputeFullV);
    auto U = svd.matrixU();
    auto V = svd.matrixV();

    //std::cout << "U" << U <<std::endl;
    //std::cout << "V" << V <<std::endl;

    Eigen::Matrix<double,3,3> R_ = U * V.transpose();
    R = ( cv::Mat_<double> ( 3,3 ) << R_ ( 0,0 ), R_ ( 0,1 ), R_ ( 0,2 ), R_ ( 1,0 ), R_ ( 1,1 ), R_ ( 1,2 ), R_ ( 2,0 ), R_ ( 2,1 ), R_ ( 2,2 ));
    Eigen::Vector3d t_ =  Eigen::Vector3d(weight1.x, weight1.y, weight1.z) - R_ * Eigen::Vector3d(weight2.x, weight2.y, weight2.z);
    t = (cv::Mat_<double> (3,1) << t_(0,0), t_(0,1), t_(0,2));
}

void PoseEstimation3d3dBA(cv::Mat& R, cv::Mat& t, const std::vector<cv::Point3d>& point1, const std::vector<cv::Point3d>& point2){

    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,3>> Block;// Implementation of a solver operating on the blocks of the Hessian 矩阵块求解器
    Block::LinearSolverType* LinearSovler = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); //线性方程求解器 //需要在cmake里链接g2o_csparse_extension库
    Block* BlockSolverPtr = new Block(std::unique_ptr<Block::LinearSolverType> (LinearSovler)); //构造函数创建矩阵块求解器对象(指针)
    g2o::OptimizationAlgorithmLevenberg* Algorithm = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<g2o::Solver>(BlockSolverPtr));//BlockSolver是g2o::Solver的子类 共有继承下来的
    g2o::SparseOptimizer Optimizer;
    Optimizer.setAlgorithm(Algorithm);

    //添加顶点 
    g2o::VertexSE3Expmap* VertexPose = new  g2o::VertexSE3Expmap(); //自带类 se3类型 包含R和t
    VertexPose->setId(0);
    Eigen::Matrix3d R_;
    Eigen::Matrix3d _R = Eigen::Matrix3d::Identity() ;
    R_ << R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ), R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ), R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
    Eigen::Vector3d t_;
    t_ << t.at<double> (0,0), t.at<double> (1,0), t.at<double> (2,0);
    g2o::SE3Quat et(_R, t_);
    VertexPose->setEstimate(et);
    Optimizer.addVertex(VertexPose);

    int index = 0;
    auto j = point2.begin();
    for(auto i = point1.begin(); i < point1.end(); i++, j++, index++){
        Edge3d* Edge = new Edge3d(Eigen::Vector3d(j->x, j->y, j->z));
        Edge->setId(index);
        Edge->setVertex(0, Optimizer.vertex(0));
        Edge->setInformation(Eigen::Matrix3d::Identity());
        Edge->setMeasurement(Eigen::Vector3d(i->x, i->y, i->z));
        Optimizer.addEdge(Edge);
    }

    Optimizer.setVerbose(1);
    Optimizer.initializeOptimization();
    Optimizer.optimize(10);

    Eigen::Matrix4d T_ = VertexPose->estimate().to_homogeneous_matrix();
    R = ( cv::Mat_<double> ( 3,3 ) << T_ ( 0,0 ), T_ ( 0,1 ), T_ ( 0,2 ), T_ ( 1,0 ), T_ ( 1,1 ), T_ ( 1,2 ), T_ ( 2,0 ), T_ ( 2,1 ), T_ ( 2,2 ));
    t = (cv::Mat_<double>(3,1) << T_(0,3), T_(1,3), T_(2,3));

}


图优化库g2o中的节点(Vertex)和边(Edge) 阅读详情

相关推荐

g2o--icp代码解析

概要个人理解Icp是一种location算法。我们先将全局的事物特征化,提取出特征点。在求解过程中,将观察的的图像,同样进行特征化。将全局点与当前特征点进行匹配,就可以求得观察者当前的位姿。Icp算法通常分为粗匹配和精细匹配两部分。粗匹配是将观察特征点移动到对应全局特征点的附近,而精细匹配这是将一个一个对应的特征点,使用最小二乘优化进行调整。在精细匹配的过程中,特征点对的选取也很重要,icp是一套迭代的算法,每次变换后都需要重新选取特征点对。

zhanglehes的专栏 755

【SLAM】G2OBA优化及ICP的学习实践

1.G2O 在之前的章节我已经讲过了G2O(General Graph Optimization)的相关原理以及应用,这次的BA优化问题呢,我们也可以转化为一个G2O来求解。 G2O(General Graph Optimization)———— 通用图优化。 G2O(General Graph Optimization)的核里带有种类多样的求解器,而它的顶点、边的类型也是多种多样。我们可以自己定义顶点和边。总的来说,如果一个优化问题能够表达成图(顶点与边),那么这个问题就可以用G2O(General G

heroybc的博客 1621

uniapp 记录运动轨迹 开发_智能手环穿戴设备APP开发解决方案

智能手环是一种穿戴式智能设备,是目前备受关注的科技产品。目前智能手环具有的功能有记录日常生活中的运动数据、移动轨迹,定位等,有些还可以进行睡眠质量的检测,并将这些数据传送至移动端如手机、平板等,可对数据进行分析,指导健康生活的作用。而智能手环开发方案就是将这些功能实现,将算法烧录到主控板上。智能手环都有哪些功能  1、记录运动数据  对于喜欢健身的用户来说,智能手环记录的运动数据功能...

weixin_39678493的博客 1766

SLAM14讲学习笔记(十一)g2o图优化中的要点与难点(前端VO中雅克比矩阵的定义)

使用g2o优化的前提是,需要对各种误差的理解足够充足。我将雅克比矩阵的详细解析,写在了这里:点击查看 在源码和自定义的类中,各种雅克比矩阵有的是写在源码里的,有的是需要自己修改和定义的。然而在实践中发现,雅克比矩阵的定义往往不一,容易使初学者摸不着头脑。因此,这篇文章我从代码角度出发,简单解析一下g2o定义时候的要点,和内置的雅克比矩阵的书写方式。 在g2o中,内置了三大类: VertexS...

微鉴道长的博客 6396

BMS应用软件开发 — 7 从特斯拉到问界:主流BMS硬件架构深度解析

本文深度解析了从特斯拉Model 3到问界M5的主流BMS硬件架构,探讨了BMS在电动汽车中的核心价值与应用软件开发的关键技术。通过对比分析两家厂商的硬件设计哲学、芯片选型策略及成本控制方法,揭示了BMS硬件系统如何影响电池性能与整车安全。文章特别分享了电压采样精度、EMC设计等实战经验,为BMS开发提供宝贵参考。

weixin_42554408的博客 464

保姆级教程:在Ubuntu 22.04上用VLLM+OpenWebUI部署DeepSeek-R1-14B(含Q8量化模型下载)

本文提供在Ubuntu 22.04上使用VLLM和OpenWebUI部署DeepSeek-R1-14B大语言模型的完整教程,包括环境配置、模型下载、VLLM参数调优及OpenWebUI集成。特别针对Q8量化模型优化部署流程,解决CUDA版本冲突等常见问题,适合开发者快速搭建本地AI对话系统。

weixin_26712075的博客 433

slambook-en完整指南:如何从零开始学习视觉SLAM的14个关键讲座

slambook-en是视觉SLAM领域的经典开源项目,提供了《视觉SLAM十四讲:从理论到实践》的英文版本,为全球开发者和学习者提供了系统学习视觉SLAM技术的完整资源。无论你是刚入门的新手还是有一定经验的开发者,这份完整指南都将帮助你循序渐进地掌握视觉SLAM的核心概念、关键技术和实践应用。 ## 为什么选择slambook-en学习视觉SLAM? 视觉SLAM(Simultaneous

gitblog_00965的博客 424

Unity 实战项目 ☀️| 只用一个脚本做一个 刮刮乐 案例,一不小心刮出来一个女朋友!【学习娱乐一下】

只用一个脚本做一个刮刮乐,一不小心刮出来一个女朋友!【学习娱乐一下!】

努力前行,总会成为自己心中的那道光 23万+

视觉SLAM十四讲——ch7实践(视觉里程计1)

视觉SLAM十四讲——ch7

琉璃轻纱的博客 1688

【ZYNQ】OV5640 图像采集与显示

OV5640 是豪威(OmniVision)半导体公司推出的高性能的 CMOS 数字图像传感器,支持最高 500 万像素图像采集,支持 RAW RGB、RGB565、YCbCr422 等图像格式输出,可应用于手机、平板等电子设备。本文主要介绍如何使用 Zynq 驱动 OV5640 摄像头进行图像采集,并通过 HDMI 实时显示。

洋洋Young的博客 1437

手机开发实战58——小区广播介绍2

业务 针对小区广播短消息的特点,可以提供一些基于目前短消息不能或不适合提供的服务: 1. 显示网络服务基站名称:用户可以随时了解手机登记在哪个基站下,特别是在边界交叉覆盖区域,用户可以清楚的知道通话建立在哪个基站下,在用户投诉和故障描述中也会有所帮助; 2. 提供用户问候信息:可以在覆盖机场、车站等地区的基站广播一些用户问候语或一些企业形象广告语; 3. 充分利用小区广播地

xjbclz的专栏 776

高翔博士slambook2 ch7 编译运行笔记

BA的目标是同时优化相机位姿和三维地图点,使重投影误差(真实观测到的像素位置和根据当前估计的相机位姿、三维点投影出来的位置之间的差)最小。重投影误差 = 观测像素 - 投影像素BA是视觉SLAM后端优化的核心内容。

weixin_43429094的博客 182

YOLOv13改进策略【特征提取层】| 引入注意力卷积模块RFAConv,关注感受野空间特征 助力yolov13精度提升

RFAConv:创新空间注意力和标准卷积运算感受野空间特征是针对卷积核设计的,根据卷积核大小动态生成。以3×3卷积核为例,它由非重叠滑动窗口组成,每个窗口代表一个感受野滑块。

Limiiiing的博客 365

slambook-en视觉里程计实践:如何构建稳定的前端跟踪系统

视觉SLAM(Simultaneous Localization and Mapping)技术让机器能够像人类一样感知和理解环境,而**视觉里程计**作为SLAM系统的核心前端,是实现精准定位和地图构建的关键。在slambook-en项目中,作者详细介绍了视觉里程计的理论基础和实践方法,本文将为您解析如何构建一个稳定的前端跟踪系统。 ## 🎯 视觉里程计:SLAM系统的"眼睛" 视觉里程计是

gitblog_00301的博客 259

【Linux0.11代码分析】之 系列文章链接汇总(全)

【Linux0.11代码分析】00 之 系列文章链接汇总(全)

|~~~热爱生活、努力学习的小伙汁~~~| 1454

Outlook Express 6 (OE6) for Win7/Win8/Win10

英文版 Outlook Express6.0, 兼容Win7/Win10/WinXP,解压就可用。

高翔博士slambook2 ch6 编译运行笔记

ch6 帮助我们理解如何通过优化求解最优位姿和地图点,其步骤概括为:构造误差函数、计算雅可比、迭代优化、使用Ceres、使用g2o等。ch6主要讲非线性优化,是SLAM后端优化的入门章节。

weixin_43429094的博客 200

复分析 阿尔福斯(Ahlfors) 第三版答案 英文

答案分为十章,有部分习题没有答案,全英文复分析阿尔福斯(Ahlfors) 第三版答案

视觉SLAM十四讲CH7课后习题6,7,8

6. 在PnP 优化中,将第一个相机的观测也考虑进来,程序应如何书写?最后结果会有何变化? 代码链接:HW-of-SLAMBOOK2/p6.cpp at main · Philipcjh/HW-of-SLAMBOOK2 · GitHub pose_estimation_3d2d1.cpp #include <iostream> #include <opencv2/core/core.hpp> #include <opencv2/features2d/features2.

生活如此美好,致敬每一个热爱“热爱”的人! 725

visio安装包

Microsoft Office Visio 是一款专业的制图软件。利用经过更新的形状、协作工具和数据链接图表,创建专业级图表以简化复杂的信息。

STM32F407中文手册

STM32f407中文手册 本参考手册面向应用开发人员,提供有关使用 STM32F405xx/07xx、STM32F415xx/17xx、 STM32F42xxx 和 STM32F43xxx 微控制器存储器与外设的完整信息。STM32F405xx/07xx、STM32F415xx/17xx、STM32F42xxx 和 STM32F43xxx 构成一个微 控制器系列,各产品具有不同的存储器大小、封装和外设。有关订购信息以及器件的机械与电气特性,请参见数据手册。有关 ARM Cortex™-M4F 内核的信息,请参见《Cortex™-M4F 技术参考手册》。

slambook-en学习路线图:从初学者到专家的10个关键步骤

想要掌握视觉SLAM技术但不知从何开始?🤔 视觉SLAM(同时定位与地图构建)是机器人、自动驾驶和增强现实领域的核心技术,而《14 Lectures on Visual SLAM: From Theory to Practice》这本书提供了从理论到实践的完整指南。本文将为你揭示从初学者到专家的10个关键学习步骤,帮助你系统掌握视觉SLAM的核心概念和技术实现。 ## 🎯 第一步:建立SLA

gitblog_00766的博客 256

关于g2o的使用笔记

文章目录前言一、g2o进行非线性优化二、g2o进行前端位姿优化三、g2o进行后端位姿优化总结 前言 g2o是比较流行的图优化库,在视觉slam中的应用非常广泛,关于如何利用g2o求解BA优化问题,在此作个笔记,以供后面的学习中方便查阅。 一、g2o进行非线性优化 以《视觉slam十四讲》中的求解最小二乘问题为例: 原函数: 二范数: 误差函数: 构建图模型:顶点为待优化变量,边为误差。图的模型大致为下图(有点丑。。。)。 因为我们需要求的量只有一个,即:vector3d(a,b,c),因此,在这个图中,顶

ldk3679的博客 1432

G2O入门

G2O优化方法使用流程: 1、构建模型的顶点: oplusImpl( const double* update ) ——》顶点的更新函数 2、构建模型的边:void computeError() ——》计算曲线模型误差,边的误差计算函数,取出边所连接的顶点的当前估计值,与该顶点的观测值进行比较 3、生...

qq_29230261的博客 759

SLAM十四讲之第七讲:3D-3D ICP求解

​ICP问题存在唯一解或无穷多解的情况。在唯一解的情况下,只要能找到极小值解,这个极小值就是全局最优值(因此不会遇到局部极小而非全局最小的情况)。这也意味着ICP求解可以任意选定初始值。这是已匹配点时求解ICP的一大好处。

qq_49561752的博客 580

传统ORB-SLam中位姿优化中雅克比矩阵讲解

由于之前的鱼眼orbslam只有单目部分,所以在优化时也只是用了单目位姿优化和三维坐标点优化,并没有将双目的优化添加进去,不知道是否对结果有影响; 这里添加双目的优化部分,主要是将添加雅克比矩阵; orbslam中的优化部分使用了g2o库,具体的详细讲解可以参考https://zhuanlan.zhihu.com/p/58521241 讲解的很详细 这里不加赘述,主要从单目的雅克比矩阵讲解,...

2764

Eigen库中参数ComputeFullU,ComputeThinU,ComputeFullV,ComputeThinV的应用

Eigen库中参数ComputeFullU,ComputeThinU,ComputeFullV,ComputeThinV的应用 对于 J=UΣVTJ=U \Sigma V^TJ=UΣVT在Eigen库中进行奇异值分解 : JacobiSVD<MatrixXd> svd(J, ComputeFullV| ComputeFullU); U = svd.matrixU(); ...

TensorME 6054

视觉SLAM前端——ICP

视觉SLAM前端里程计之ICP,即已知多个匹配好的3D/3D点对,来估计相机的运动

Heathens的博客 909

淦ORB-SLAM2源码 08--PNP(DLT,P3P,EPNP)和ICP(SVD)

PNP和ICPPNPDLTP3PEPNPBAICPSVD法BA PNP DLT P3P EPNP BA ICP SVD法 BA

qq_44047943的博客 1769

SLAM十四讲学习笔记5—— 前端:两图之间关系求解——ICP

SLAM十四讲学习笔记5—— 前端:两图之间关系求解——ICP分类改进版的ICPICP变体3D-3D ICP问题 分类 2D-2D 对极几何:只有两个单目图像 3D-2D PnP:帧和地图 3D-3D ICP(Iterative Closest Point,ICP) 改进版的ICP 视觉相机可以通过上述特征匹配的方法得到两副图上的点与点一一对应的关系好了。根据已配对好的两组3D点,使用ICP法...

qq_41620270的博客 420
上一篇: SlamBook14讲-EPNP与BundleAdjustment
下一篇: SlamBook14讲-LKFlow与DirectMethod(直接法)
就差一点点_
博客等级 码龄6年 8粉丝 8原创
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付:10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值
⌂ 换网址