视觉SLAMch7 课后题

雨幕丶

已于 2022-12-20 22:20:05 修改

阅读量3.4k

点赞数 2

分类专栏：视觉SLAM 文章标签： slam 计算机视觉 opencv

于 2022-03-27 22:04:43 首次发布

本文链接：https://blog.csdn.net/qq_41451702/article/details/123739130

版权

视觉SLAM 专栏收录该内容

30 篇文章 19 订阅

订阅专栏

课后习题

1.除了ORB特征点，还有哪些特征点？说说SIFT与SURF的原理，并对比它们与ORB之间的优势。

仅提取关键点：Harris角点、FAST角点、GFTT(goodFeaturesToTrack)角点

既有关键点又有描述子：SIFT、SURF、ORB。

Harris角点

Harris角点检测是一种基于图像灰度的一阶导数矩阵检测方法。检测器的主要思想是局部自相似性/自相关性，即在某个局部窗口内图像块与在各个方向微小移动后的窗口内图像块的相似性。

GFTT角点

调用的是Shi-Tomasi方法检测角点，Shi-Tomas方法是对Harris角点的优化，实时性更强，并且能够避免聚簇现象。

cv::Ptr<cv::GFTTDetector> create( int maxCorners=1000, 
                                double qualityLevel=0.01,
                                double minDistance=1, 
                                int blockSize=3, 
                                bool useHarrisDetector=false, 
                                double k=0.04 );

maxCorners：检测到的最大角点数量；

qualityLevel：输出角点的质量等级，取值范围是 [ 0 , 1 ]；如果某个候选点的角点响应值小于（qualityLeve * 最大角点响应值），则该点会被抛弃，相当于判定某候选点为角点的阈值；

minDistance：两个角点间的最小距离，如果某两个角点间的距离小于minDistance，则会被认为是同一个角点；

mask：如果有该掩膜，则只计算掩膜内的角点；

blockSize：计算角点响应值的邻域大小，默认值为3；如果输入图像的分辨率比较大，可以选择比较大的blockSize；

useHarrisDector：布尔类型，如果为true则使用Harris角点检测；默认为false，使用shi-tomas角点检测算法；

k：只在使用Harris角点检测时才生效，也就是计算角点响应值时的系数k。

SIFT

SIFT大致分为三步：① 构建高斯差分金字塔 ② 提取关键点 ③ 计算描述子。

SIFT优缺点：

旋转不变性、尺度不变性、光照不变性，抗遮挡，对视角变化、仿射变换、噪声均保持一定稳定性。
独特性好，信息量丰富，适合海量特征库的匹配。
即使物体较少，也能够提取大量SIFT特征
扩招性，特征维度小，可以很方便的与其他的特征向量进行联合
对边缘光滑的目标无法准确提取特征
充分考虑了在图像变换过程中出现的光照、尺度、旋转等变化
计算量太大

SURF

为了加快SIFT算法的计算速度，减小SIFT算法的计算量，产生了SURF算法。

SURF算法的概念及步骤均建立在SIFT之上，但详细的流程略有不同。SURF算法大致分为三个步骤: ①特征点检测、②特征邻近描述、③描述子配对。

对比以上几种特征点方法：

提取关键点：

实时性：FAST > GFTT > Harris

鲁棒性：GFTT = Harris > FAST

避免聚簇现象：GFTT

关键点和描述子兼有：

计算速度：ORB > SURF > SIFT

旋转鲁棒性：SURF > ORB ~ SIFT

模糊鲁棒性：SURF > ORB ~ SIFT

尺度变换鲁棒性：SURF > SIFT > ORB( ORB本身并没有解决尺度不变性，而是在opencv实现中添加了图像金字塔 )

综上所述，如果对计算实时性要求非常高，可选用ORB算法；如果对实行性要求稍高，可以选择SURF；基本不用SIFT

2.设计程序调用OpenCV中其他种类特征点，统计提取1000个特征点所用时间。

OpenCV3中SIFT和SURF都被移动到了独立的库opencv_contrib 中

安装参考：opencv_contrib

opencv_contrib配置

Ptr<FeatureDetector> detector_orb = ORB::create(1000);//检测1000个ORB点
Ptr<FeatureDetector> detector_sift= SIFT::create(1000);//检测1000个SIFT点
Ptr<FeatureDetector> detector_surf= xfeatures2d::SURF::create(400);//检测1000个SURF点

3.让特征点分布更均匀（避免聚簇）的方法

①GFTT

Ptr<FeatureDetector> detector = GFTTDetector::create(1000, 0.01, 50);

②ORB-SLAM2 用四叉树实现的特征点均匀分布算法

这部分代码可以参考：四叉树实现特征点均匀分布

4. FLANN为何能够快速处理匹配问题。除了FLANN还有哪些加速匹配的手段？

FLANN是快速最近邻搜索包（Fast Library for Approximate Nearest Neighbors）的简称。它是一个对大数据集和高维特征进行最近邻搜索的算法的集合，而且这些算法都已经被优化过了。在面对大数据集是它的效果要好于BFMatcher(暴力匹配)。

FLANN主要采用三种算法：k-d树、k-means树、层次聚类树。

k-d树是二叉树的升维形式，能够划分超平面来跳过一部分无必要的匹配。在点云最邻近搜索中也很常用。

其他两种树用得较少，不做讨论。

特征点加速匹配多数都是针对于SIFT的，常见的有主成分分析法，将SIFT描述子从128维降到36维，从而匹配速度增加3倍多；或者用GPU加速，可以使得匹配速度提高十多倍；再后来就是用FPGA加速，其匹配速度能提升10倍；再后来的VF-SIFT（very fast SIFT）算法，其核心思想是从SIFT特征中提取4个特征角，根据特征角区间的不同，避免了大量不必要的搜索，这样据说是普通搜索的1250倍。

5.其他PnP方法

①EPnP的思路是将空间中的任意3D点可以用4个不共面的3D点加权表示，然后通过n个3D点在相机平面的投影关系以及四个控制点的权重关系构建一个的矩阵，求这个矩阵的特征向量，就可以得到这个相机平面的坐标，再用POSIT正交投影变换算法得到相机位姿。EPnP的精度明显高于DLT，时间略微慢于DLT。
EPnP解法

② 迭代法实质是迭代求出重投影误差的最小解先用DLT直接线性变换求解，然后用LM算法进行优化，这个解显然不是正解，而且这个方法只能使用4个共面特征点才能求得。

③P3P法，它需要3对3D-2D的匹配点，然后通过三角形投影关系得到的方程组，然后将方程组进行变换得到两个二元二次方程进行求解。

④DLS（Direct Least-Squares）算法整体思路是首先对PnP非线性最小二乘建模，重新定义LS模型以便于参数降维然后构造Maculy矩阵进行求解，将PnP问题重构为无约束LSM问题然后优化求解。

⑤UPnP(Uncalibrated PnP)算法跟EPnP差不了太多，仅仅是多估计了焦距。因此，比较适合未标定场合。

下边第六、七题中的点和边大部分使用的是g2o已经封装好的，里面涉及到很多也比较复杂。

如果想自己定义顶点和类的话可以参考下面的文章，讲的还是很详细的。我的建议是先看这篇文章对比下和已经写过的PnP、ICP的区别。

视觉SLAM十四讲（第二版）第7讲习题解答 - 知乎 (zhihu.com)

6.在PnP中将第一个相机的观测考虑进来（注意看上面的介绍！！！）

原先不考虑第一个相机的位姿前提是设定第一个相机的坐标系为世界坐标系，而考虑第一个相机的观测时，则第一个相机的位姿节点是未知的，因此需将第一个相机的位姿作为新增的待优化节点。相当于在一次优化中分别对相机1和2做了一次PNP求解。因此最后输出的pose应该是两个相机的相对位姿。

从最后的结果看，加入第一个相机的观测与不加入几乎没有差异。

参考：

1.《视觉slam十四讲》第七讲课后习题_hello我是小菜鸡的博客-CSDN博客

2.视觉slam十四讲第七章课后习题6 - 灰色的石头 - 博客园 (cnblogs.com)

3.g2o图优化中的要点与难点

根据pose_estimation_3d2d.cpp的代码，对其中BundleAdjustment函数进行修改。

1.这部分代码关于顶点和边类的定义使用g2o已经封装好的VertexSE3Expmap、VertexPointXYZ、EdgeProjectXYZ2UV。

在g2o中，内置了三大类： VertexSE3Expmap，这个表示李代数的位姿； VertexPointXYZ，表示空间点的位置； EdgeProjectXYZ2UV，表示投影方程的边。前两个是顶点类型，后一个是边类型。

先看VertexSE3Expmap的源码

class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>
{
public:
//表示数据对齐，固定格式
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  VertexSE3Expmap();
//读写操作 按需自己定义
  bool read(std::istream& is);
 
  bool write(std::ostream& os) const;
//设置初始的值
  virtual void setToOriginImpl() {
    _estimate = SE3Quat();
  }
//更新方式，更新小量乘以估计值，左乘更新
  virtual void oplusImpl(const double* update_)  { //update_需要传入数组
    Eigen::Map<const Vector6d> update(update_);
    setEstimate(SE3Quat::exp(update)*estimate());
  }
};

顶点需要继承BaseVertex，这里顶点里面存储的是位姿，因此< >里面的6代表se3李代数的6个值，即维度为6（g2o是旋转在前，平移在后），而SE3Quat作为顶点数据类型，存储这个se3。

VertexPointXYZ：

class G2O_TYPES_SLAM3D_API VertexPointXYZ : public BaseVertex<3, Vector3>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW    
    VertexPointXYZ();
    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;
    virtual void setToOriginImpl() {
      _estimate.fill(0);
    }
 
    virtual void oplusImpl(const number_t* update)
    {
      Eigen::Map<const Vector3> v(update);
      _estimate += v;
    }
};

新版g2o库中不再存在原先的VertexSBAPointXYZ，改为VertexPointXYZ

同样，对于VertexSBAPointXYZ来说,这里不管是世界坐标系还是相机坐标系下的点，都是xyz三个值，因此< >中，第一个是3,第二个是对应的数据格式，即Vector3D。

边EdgeProjectXYZ2UV：是一个二元边，连接两个顶点

class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public  BaseBinaryEdge<2, 
Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    EdgeProjectXYZ2UV();
    
    bool read(std::istream& is);
    
    bool write(std::ostream& os) const;
    
    void computeError()  {  //计算误差
      const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
      const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
      const CameraParameters * cam
        = static_cast<const CameraParameters *>(parameter(0));
      Vector2D obs(_measurement);
      _error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
    }
    
    virtual void linearizeOplus(); //计算雅可比矩阵
    
    CameraParameters * _cam;
};

先看CameraParameters 类

class G2O_TYPES_SBA_API CameraParameters : public g2o::Parameter {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  CameraParameters();
  CameraParameters(number_t focal_length, const Vector2 &principle_point, number_t baseline);

  Vector2 cam_map(const Vector3 &trans_xyz) const;
  Vector3 stereocam_uvu_map(const Vector3 &trans_xyz) const;
  bool read(std::istream &is);
  bool write(std::ostream &os) const;

  number_t focal_length;  //焦距 fx
  Vector2 principle_point;//相机光心（cx,cy）
  number_t baseline;    
};

①第一句，<2, Vector2D, VertexPointXYZ, VertexSE3Expmap>这四个值中，2表示观测的值是2维的（说白了也就是像素坐标），因此类型为Vector2D；该边被绑定在两个类型的顶点上：VertexPointXYZ, VertexSE3Expmap。（意思就是说，这条边构建的重投影误差，对两个点进行优化，一个是对世界坐标系下的点VertexPointXYZ进行优化，一个是对位姿VertexSE3Expmap进行优化。）

二元边定义的参数 <误差维度，误差类型，顶点一，顶点二>

注意：_vertices[0]和_vertices[1]，分别表示“ : public BaseBinaryEdge<2, Vector2D, VertexPointXYZ, VertexSE3Expmap>”中后面两个绑定点的类型。因此main函数中，这种边如果想绑定到节点上，即edge->setVertex(0,……)这里的“……”写的就是VertexPointXYZ类型；如果这种边想绑定到VertexSE3Expmap类型的节点上，edge->setVertex( )中第一个参数就是填1.

②computeError函数中，描述了error的计算过程，即观测的值，减去计算出的值。

Vector2D obs(_measurement); 
_error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));

//就拿这句话来说，obs是测量值，即某个点在像素坐标系下实实在在的值
//v2->estimate()，其实就是VertexPointXYZ类型的一个3D点坐标
//v1->estimate() 就是从顶点中获取的位姿
//把“v2->estimate()”传入v1->estimate().map()中，其实就是世界坐标系下的点经过外参，得到相机坐标系下的点
//即（v1->estimate().map(v2->estimate())
//之后，经过cam->cam_map（）变换，得到像素坐标系下的点，即计算值
//测量值减去计算值，构建一个误差

该函数中的camera是要求用户设置的，在配置SparseOptimizer的时候传入相机参数，如下代码块所示：这个误差其实就是重投影误差。重投影误差，需要对涉及到的两个顶点求其偏导数。也就是说，重投影误差需要对世界坐标系下的坐标点和位姿分别求偏导数。书187页已经给出。

③如果绑定了两个顶点，在linearizeOplus()中则需要写清楚误差分别对于两个顶点（空间点VertexPointXYZ和位姿VertexSE3Expmap）的偏导数。第一个为_jacobianOplusXi,第二个为_jacobianOplusXj，需要显示的写出来。如果该边只绑定了一个顶点，则只写_jacobianOplusXi就可以了。我们现在看看它的两个偏导数：

Matrix<double,2,3,Eigen::ColMajor> tmp;
tmp(0,0) = cam->focal_length;
tmp(0,1) = 0;
tmp(0,2) = -x/z*cam->focal_length;
tmp(1,0) = 0;
tmp(1,1) = cam->focal_length;
tmp(1,2) = -y/z*cam->focal_length;
//_jacobianOplusXi是误差关于世界坐标系下坐标点的偏导
//其中，-1./z * tmp是误差关于相机坐标系下坐标点的偏导，
//而T.rotation().toRotationMatrix()也就是R，是相机坐标系下坐标点关于世界坐标系下坐标点的偏导
//二者相乘，得到误差关于世界坐标系下坐标点的偏导
_jacobianOplusXi = -1./z * tmp * T.rotation().toRotationMatrix();
//_jacobianOplusXj是误差关于位姿扰动小量的偏导
//_jacobianOplusXj本质上是由两项相乘得到
//第一项，其实是误差关于相机坐标系下坐标点的偏导，也就是-1./z * tmp
//第二项，其实是相机坐标系下坐标点关于位姿扰动小量的偏导
//两项相乘，得到下面的jacobianOplusXj 
_jacobianOplusXj ( 0,0 ) =  x*y/z_2 *camera_->fx_;
_jacobianOplusXj ( 0,1 ) = - ( 1+ ( x*x/z_2 ) ) *camera_->fx_;
_jacobianOplusXj ( 0,2 ) = y/z * camera_->fx_;
_jacobianOplusXj ( 0,3 ) = -1./z * camera_->fx_;
_jacobianOplusXj ( 0,4 ) = 0;
_jacobianOplusXj ( 0,5 ) = x/z_2 * camera_->fx_;
_jacobianOplusXj ( 1,0 ) = ( 1+y*y/z_2 ) *camera_->fy_;
_jacobianOplusXj ( 1,1 ) = -x*y/z_2 *camera_->fy_;
_jacobianOplusXj ( 1,2 ) = -x/z *camera_->fy_;
_jacobianOplusXj ( 1,3 ) = 0;
_jacobianOplusXj ( 1,4 ) = -1./z *camera_->fy_;
_jacobianOplusXj ( 1,5 ) = y/z_2 *camera_->fy_;

重投影误差关于世界坐标系下的坐标点P求偏导，即是误差e先对相机坐标系下的点P'求偏导，再由P'对于P求偏导。（注意：由于重投影误差是观测值减去计算值，计算值在后，观测值在求导的时候看作是常量，因此“误差e先对相机坐标系下的点P'求偏导”说白了也就是-(u,v)对于P'求偏导。）重投影误差关于位姿扰动小量求偏导，也就是误差e先对相机坐标系下的点P'求偏导，然后P'再对扰动小量求偏导。前者刚刚说过，实际上是-(u,v)对于P'求偏导，后者在李群李代数一节已经推导过，是[I,-P'^]。不过写在雅克比矩阵_jacobianOplusXj中，需要对调前三列和后三列（因为在公式推导中，是平移在前，旋转在后；而在g2o中是旋转在前，平移在后）。

g2o图优化中的要点与难点

CMakeLists.txt

add_executable(homework7_6 homework7_6.cpp)
target_link_libraries(homework7_6 glog::glog 
                                  g2o_types_sba 
                                  g2o_types_slam3d 
                                  ${OpenCV_LIBS} 
                                  ${G2O_CORE_LIBRARY} 
                                  ${G2O_STUFF_LIBRARY}  
                                  fmt )

#
#g2o_types_sba     G2O_TYPES_SBA_API VertexSE3Expmap   G2O_TYPES_SBA_API EdgeProjectXYZ2UV
#g2o_types_slam3d  G2O_TYPES_SLAM3D_API VertexPointXYZ

小记：

第一个顶点表示要优化的相机位姿（T1和T2），同时也是我们最终要求的结果

第二个顶点表示要优化的空间点（第一帧的坐标）

对于顶点的操作有三个：setId() 设置顶点编号、 setEstimate()设置初始值（三个初始值分别为:g2o::SE3Quat()、g2o::SE3Quat(R，t) 、Eigen::Vector3d(p.x,p.y,p.z) ）、 addVertex()添加顶点

边就是两个相机的观测（也就是两帧的像素坐标）

对于边的操作有：setId()、 setVertex()设置要连接的顶点、 setMeasurement()设置观测值，也就是误差项中的 $u_i$ 、setInformation()设置协方差的逆，和误差维度有关，误差是二维的话括号在的参数是 Eigen::Matrix2d::Identity() 、addEdge() 添加边。

7.在ICP 程序中，将空间点也作为优化变量考虑进来

在将空间点作为优化变量考虑后，此时图优化的边为二元边，二元边的其中一个顶点为原来的李代数位姿，另一个顶点是在相机二坐标系下的空间点三维坐标；
二元边中误差的计算computeError()需要修改，测量值为相机一坐标系下的空间点三维坐标值，与之作差的是需要优化的位姿乘以相机二坐标系下的空间点三维坐标，即将其转化到世界坐标系下;
雅可比矩阵有两个，第一个是误差对相机二空间点的导数；第二个是误差对位姿的导数

参考：视觉slam十四讲第七章课后习题7 - 灰色的石头 - 博客园 (cnblogs.com)

分析:在ICP例程中，本书使用的是自定义的一个EdgeProjectXYZRGBDPoseOnly继承BaseUnaryEdge的边，这个类在linearizeOplus中写下了关于位姿节点的雅克比矩阵，里面也没有相机模型参数模型（没有涉及到相机内参），没有关于空间坐标点的雅克比矩阵。

通过误差函数，可以将空间点也作为优化变量（优化的是第二帧的空间点）。误差关于空间点的雅克比矩阵是 -R 。仿照g2o::EdgeProjectXYZ2UV这个类的写法。修改后的类：

class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, g2o::VertexPointXYZ, g2o::VertexSE3Expmap>
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  EdgeProjectXYZRGBDPoseOnly() {}
  
  virtual void computeError()
  {
    const g2o::VertexPointXYZ * point = static_cast<const g2o::VertexPointXYZ*> (_vertices[0]);
    const g2o::VertexSE3Expmap * pose = static_cast<const g2o::VertexSE3Expmap*> (_vertices[1]);
   
    // measurement is p, point is p'
    //pose->estimate().map( _point );中用estimate（）估计一个值后，然后用映射函数 就是旋转加平移 将其_point映射到另一个相机坐标系下去
    //实际值 - 映射值 因为我们做的试验是3D-3D 所以这里把第一帧的3D坐标当做实际值，然后把第二帧坐标映射到第一帧坐标系中
    _error = _measurement - pose->estimate().map(point->estimate());
  }
  
  virtual void linearizeOplus() override final  //这里override 表示override覆盖基类的同名同参函数， final表示派生类的某个函数不能覆盖这个函数
  {
    g2o::VertexPointXYZ * point = static_cast<g2o::VertexPointXYZ * > (_vertices[0]);
    g2o::VertexSE3Expmap * pose = static_cast<g2o::VertexSE3Expmap * > (_vertices[1]);
    g2o::SE3Quat T (pose->estimate());
    
    Eigen::Vector3d xyz_trans = T.map(point->estimate());    //xyz_trans 是雅可比矩阵中的Pi
    double x = xyz_trans[0];    
    double y = xyz_trans[1];
    double z = xyz_trans[2];
    
    //误差对空间点求导的雅克比矩阵
    _jacobianOplusXi = -T.rotation().toRotationMatrix();
    
    //误差对位姿求导的雅可比矩阵
    _jacobianOplusXj(0,0) = 0;
    _jacobianOplusXj(0,1) = -z;
    _jacobianOplusXj(0,2) = y;
    _jacobianOplusXj(0,3) = -1;
    _jacobianOplusXj(0,4) = 0;
    _jacobianOplusXj(0,5) = 0;
 
    _jacobianOplusXj(1,0) = z;
    _jacobianOplusXj(1,1) = 0;
    _jacobianOplusXj(1,2) = -x;
    _jacobianOplusXj(1,3) = 0;
    _jacobianOplusXj(1,4) = -1;
    _jacobianOplusXj(1,5) = 0;
 
    _jacobianOplusXj(2,0) = -y;
    _jacobianOplusXj(2,1) = x;
    _jacobianOplusXj(2,2) = 0;
    _jacobianOplusXj(2,3) = 0;
    _jacobianOplusXj(2,4) = 0;
    _jacobianOplusXj(2,5) = -1;
  }
  
  bool read (istream& in) {}
  bool write (ostream& out) const {}
 
};

顶点类还是使用VertexSE3Expmap 和 VertexPointXYZ，这里只是把边类进行了重写

注意使用封装好的顶点类和边类的时候加头文件

#include <g2o/types/sba/types_six_dof_expmap.h>

总结：关于习题6和7的雅可比矩阵推导：

关于习题6和7的添加顶点和边操作：

PnP:

添加第一帧的顶点（位姿T）、添加第二帧的顶点（位姿T）、添加第一帧的顶点（空间点P）

添加第一帧的边（第一帧的相机观测）：新加入的第一个相机的观测是第一帧的像素坐标

添加第二帧的边（第二帧的相机观测）：第二帧的像素坐标

ICP: 第二帧变换到第一帧

添加第二帧的顶点（位姿T）

添加第二帧的顶点（第二帧的空间点）：这里仅优化第二帧的空间点（相机坐标系的点）

添加第二帧的边（第一帧的相机观测）：第一帧的世界系坐标

8.在特征点匹配过程中，不可避免地会遇到误匹配的情况。如果把误匹配的情况输入到PnP中或ICP中，会发生什么情况？有哪些能避免误匹配的方法？

目前书中用的是根据汉明距离的暴力匹配方法，然后根据经验参数（30或者是最小距离的两倍）对匹配子根据其距离进行筛选。

如果误匹配情况输入到PnP或是ICP中，再加上迭代算法选择不正确，初值估计不准确，就很容易导致计算结果产生误差，更有甚者会让迭代过程不稳定，甚至报错。

目前比较流行的避免误匹配方法有：

①交叉匹配：在暴力匹配的基础上再匹配一次，如果两次结果一致，则认为是个特征点，如果不一致则滤掉

②KNN匹配：K邻近匹配，匹配时候选择K个与特征点相似的点，一般K是2，如果区别足够大，则选择最相似的点作为匹配点

③RANSAC：随机采样一致性算法

9.用SE3重写PnP和ICP（g2o）

参考：

SLAM从入门到放弃：SLAM十四讲第七章习题（9）_yanqs_whu的博客-CSDN博客

10.用Ceres实现PnP和ICP

ch6中总结过Ceres库求解最小二乘问题的流程，Ceres的求解过程包括代价函数的构建、最小二乘问题的构建以及最小二乘问题的求解。

①PnP:

ceres::AngleAxisRotatePoint(r,p_3d,p_Cam);
//r是旋转向量 通过Rodrigues()可以转换为旋转矩阵R

AngleAxisRotatePoint()函数计算在相机仅旋转的情况下，计算新坐标系下的坐标。

输出结果：

可以看出Ceres最后迭代的结果和g2o差不多，但是Ceres相交于g2o而言代码更加简洁，而且迭代步数更加少，可增补性强。同理，对于ICP的Ceres优化方法也是类似。

程序中遇到的类型转换

①Mat 或 Eigen 类型的R、t 转换为 Mat类型的 T

Mat R,t;
Mat T;
T = (Mat_<double>(4,4)<<
        R.at<double>(0,0),R.at<double>(0,1),R.at<double>(0,2),t.at<double>(0,0),
        R.at<double>(1,0),R.at<double>(1,1),R.at<double>(1,2),t.at<double>(0,1), 
        R.at<double>(2,0),R.at<double>(2,1),R.at<double>(2,2),t.at<double>(0,2),
        0,                0,                0,                1    );

Eigen::Matrix3d R ;
Eigen::Vector3d t ;
Mat T;
 T = (Mat_<double>(4,4)<<
      R(0,0), R(0,1), R(0,2), t(0), 
      R(1,0), R(1,1), R(1,2), t(1), 
      R(2,0), R(2,1), R(2,2), t(2), 
      0,      0,      0,      1    );

②Eigen类型的R、t 转换为Eigen类型的 T

Eigen::Matrix3d R ;
Eigen::Vector3d t ;
Eigen::Isometry3d T(R);
T.pretranslate(t);
cout<<T.matrix()<<endl;

③Mat类型转换为Eigen类型

Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
Eigen::Matrix3d K_eigen;
K_eigen<<K.at<double>(0,0),K.at<double>(0,1),K.at<double>(0,2),
         K.at<double>(1,0),K.at<double>(1,1),K.at<double>(1,2),
         K.at<double>(2,0),K.at<double>(2,1),K.at<double>(2,2);

④Eigen类型转换为 Mat 类型

Eigen::Matrix3d R ;
Mat R_mat;
R_mat = (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) );

雨幕丶

关注

2
点赞
踩
12

收藏

觉得还不错? 一键收藏
1
评论
视觉SLAMch7 课后题

课后习题1.除了ORB特征点，还有哪些特征点？说说SIFT与SURF的原理，并对比它们与ORB之间的优势。仅提取关键点：Harris角点、FAST角点、GFTT(goodFeaturesToTrack)角点既有关键点又有描述子：SIFT、SURF、ORB。Harris角点Harris角点检测是一种基于图像灰度的一阶导数矩阵检测方法。检测器的主要思想是局部自相似性/自相关性，即在某个局部窗口内图像块与在各个方向微小移动后的窗口内图像块的相似性。GFTT角点调用的是Shi-Toma
复制链接

扫一扫

专栏目录