区块链技术博客
www.b2bchain.cn

VINS-Mono阅读笔记一

这篇文章主要介绍了VINS-Mono阅读笔记一的讲解,通过具体代码实例进行19564 讲解,并且分析了VINS-Mono阅读笔记一的详细步骤与相关技巧,需要的朋友可以参考下https://www.b2bchain.cn/?p=19564

本文实例讲述了2、树莓派设置连接WiFi,开启VNC等等的讲解。分享给大家供大家参考文章查询地址https://www.b2bchain.cn/7039.html。具体如下:

VINS-Mono阅读笔记一
标签: vslam IMU

目录

    • 1. 概要
    • 2. camera_model
    • 3. feature_model
      • 3.1 代码流程图
      • 3.2 各关键代码段分析
        • 3.2.1 img_callback()
        • 3.2.2 readImage()
        • 3.2.3 rejectWithF()
        • 3.2.4 undistortedPoints()
    • 4. vins_estimator
      • 4.1 姿态解算方法回顾
        • 4.1.1 2D-2D姿态解算-对极几何约束
        • 4.1.2 三角化深度求解
        • 4.1.3 3D-2D姿态解算-PNP
        • 4.1.4 3D-3D姿态解算-ICP

1. 概要


本阅读笔记将围绕vins-mono的ros功能包展开介绍,第二部分介绍camera_model的相机球体模型,抽出其中的鱼眼相机模型进行详细分析;eature_model模块介绍前端的特征点检测算法及光流跟踪模块;vis_estimator介绍算法的后端主体部分,涉及相机、IMU外参数标定,状态初始化,数据对齐,非线性优化等内容;pose_graph介绍图优化相关的内容。

2. camera_model


vins-mono支持的相机格式为等距投影模型的鱼眼相机模型(与opencv的鱼眼模型一致),针孔相机模型及全景相机模型,下面将比较vins鱼眼相机模型中的代码与opencv中的鱼眼相机模型代码,并作详细分析。 VINS-Mono阅读笔记一 图2.1显示了等距模型的鱼眼相机成像模型,

PcP_c

为相机坐标系下的点,成像平面A垂直于光轴Z且与球体相切。绿点

PundistortedP_{undistorted}

PcP_c

归一化后的3D点,前两维坐标即为畸变矫正归一化后的像素坐标点。黄点为入射光线与单位球体的交点,其与球体上顶点构成的弧线向成像平面等距投影后的蓝点

PdistortedP_{distorted}

即为畸变图像归一化后的像素坐标点。下面代码为vins中的等距鱼眼模型代码:

    template<typename T>   // 畸变系数拟合     T EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta)     {         // k1 = 1         return theta +                k2 * theta * theta * theta +                k3 * theta * theta * theta * theta * theta +                k4 * theta * theta * theta * theta * theta * theta * theta +                k5 * theta * theta * theta * theta * theta * theta * theta * theta * theta;     }          // 将2D畸变原图坐标转为投影平面的3D坐标(矫正过程)     void EquidistantCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const     {         // Lift points to normalised plane         Eigen::Vector2d p_u;         p_u << m_inv_K11 * p(0) + m_inv_K13,                m_inv_K22 * p(1) + m_inv_K23;              // Obtain a projective ray         double theta, phi;         backprojectSymmetric(p_u, theta, phi);              P(0) = sin(theta) * cos(phi);         P(1) = sin(theta) * sin(phi);         P(2) = cos(theta);     }          // 将射线上的3D点投影到成像平面(投影过程)     void EquidistantCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const     {         double theta = acos(P(2) / P.norm());         double phi = atan2(P(1), P(0));              Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), mParameters.k5(), theta) * Eigen::Vector2d(cos(phi), sin(phi));              // Apply generalised projection matrix         p << mParameters.mu() * p_u(0) + mParameters.u0(),              mParameters.mv() * p_u(1) + mParameters.v0();     } 

下图为OPENCV对鱼眼相机模型的描述,采用同vins相同的等距投影模型,其中

xx^`

yy^`

为上述代码中的p_u向量,即:归一化的畸变平面像素坐标

PdistortedP_{distorted}

。其计算公式利用图2.1中黄色、绿色三角形相似计算而得,同vins中EquidistantCamera::spaceToPlane函数利用

ϕphi

计算一致。
  VINS-Mono阅读笔记一

3. feature_model


feature_model功能包入口函数为feature_tracker_node.cpp中的main函数,其代码如下,主函数在img_callback回调中将检测到的特征角点发布出去。
  该功能包订阅了IMAGE_TOPIC(/cam0/image_raw),并创建了三个发布的话题:
  pub_img–发布feature topic
  pub_match–发布feature_img topic
  pub_restart–发布restart topic

3.1 代码流程图

VINS-Mono阅读笔记一            图3.1 feature_model节点代码流程图

3.2 各关键代码段分析

3.2.1 img_callback()

下面就img_callback回调函数中的关键代码详细分析:

   //若当前帧时间与前一帧时间差<1s或时间错位则重新开始第一帧     if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time)     {         ROS_WARN("image discontinue! reset the feature tracker!");         first_image_flag = true;          last_image_time = 0;         pub_count = 1;         std_msgs::Bool restart_flag;         restart_flag.data = true;         pub_restart.publish(restart_flag);         return;     } 

vins并非将每一帧检测到的特征点都发送,而是控制以不大于10Hz的频率

    // 节点发布频率控制 < 10Hz     if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)     {         PUB_THIS_FRAME = true;         // reset the frequency control, FREQ = 10         // 控制特征点的发布频率<10Hz,否则发布flag置零                 if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ)         {             first_image_time = img_msg->header.stamp.toSec();             pub_count = 0;         }     }     else         PUB_THIS_FRAME = false; 

消息节点发布的内容

        sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);         sensor_msgs::ChannelFloat32 id_of_point;   // 特征点ID         sensor_msgs::ChannelFloat32 u_of_point;    // 特征点坐标u         sensor_msgs::ChannelFloat32 v_of_point;     // 特征点坐标v         sensor_msgs::ChannelFloat32 velocity_x_of_point;  // 光流速度x         sensor_msgs::ChannelFloat32 velocity_y_of_point;  // 光流速度y          feature_points->header = img_msg->header;  // 图像头部,含时间戳         feature_points->header.frame_id = "world";          vector<set<int>> hash_ids(NUM_OF_CAM);         for (int i = 0; i < NUM_OF_CAM; i++)         {             auto &un_pts = trackerData[i].cur_un_pts;// 畸变矫正归一化2D点             auto &cur_pts = trackerData[i].cur_pts; // 原图畸变2D点             auto &ids = trackerData[i].ids;         // 特征点ID号标识             auto &pts_velocity = trackerData[i].pts_velocity; // 前后帧特征点光流速度             for (unsigned int j = 0; j < ids.size(); j++)             {                 if (trackerData[i].track_cnt[j] > 1)                 {                     int p_id = ids[j];                     hash_ids[i].insert(p_id);                     geometry_msgs::Point32 p;                     p.x = un_pts[j].x;                     p.y = un_pts[j].y;                     p.z = 1;                      feature_points->points.push_back(p);                     id_of_point.values.push_back(p_id * NUM_OF_CAM + i);                     u_of_point.values.push_back(cur_pts[j].x);                     v_of_point.values.push_back(cur_pts[j].y);                     velocity_x_of_point.values.push_back(pts_velocity[j].x);                     velocity_y_of_point.values.push_back(pts_velocity[j].y);                 }             }         }         feature_points->channels.push_back(id_of_point);         feature_points->channels.push_back(u_of_point);         feature_points->channels.push_back(v_of_point);         feature_points->channels.push_back(velocity_x_of_point);         feature_points->channels.push_back(velocity_y_of_point); 

3.2.2 readImage()

readImage函数代码分析如下,

void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) {     cv::Mat img;     TicToc t_r;     cur_time = _cur_time;     // 亮度均衡开启开关     if (EQUALIZE)     {         cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));         TicToc t_c;         clahe->apply(_img, img);         ROS_DEBUG("CLAHE costs: %fms", t_c.toc());     }     else         img = _img;      // 第一帧时为空,否则forw_img为当前帧图像     if (forw_img.empty())     {         prev_img = cur_img = forw_img = img;     }     else     {         forw_img = img;     }      forw_pts.clear();      // 如果已经存在前帧光流特征     if (cur_pts.size() > 0)     {         TicToc t_o;         vector<uchar> status;         vector<float> err;         //光流跟踪         // cur_img/前一帧图像  forw_img/当前帧图像 status/ 1:跟踪成功 0:跟踪失败         cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);          // 遍历角点,过滤在图像外的点         for (int i = 0; i < int(forw_pts.size()); i++)             if (status[i] && !inBorder(forw_pts[i]))                 status[i] = 0;          // 根据status状态更新所有点及ID                  reduceVector(prev_pts, status);         reduceVector(cur_pts, status);         reduceVector(forw_pts, status);         reduceVector(ids, status);         reduceVector(cur_un_pts, status);         reduceVector(track_cnt, status);         ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());     }      // 将所有跟踪的角点跟踪次数累加     for (auto &n : track_cnt)         n++;      // 满足发布频率条件下     if (PUB_THIS_FRAME)     {         // ransac 过滤外点         rejectWithF();         ROS_DEBUG("set mask begins");         TicToc t_m;         // 为跟踪到的特征点添加模板,防止二次检测         setMask();         ROS_DEBUG("set mask costs %fms", t_m.toc());          ROS_DEBUG("detect feature begins");         TicToc t_t;         // 若跟踪角点< 150则重新检测其他区域的角点         int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());         if (n_max_cnt > 0)         {             if(mask.empty())                 cout << "mask is empty " << endl;             if (mask.type() != CV_8UC1)                 cout << "mask type wrong " << endl;             if (mask.size() != forw_img.size())                 cout << "wrong size " << endl;             cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);         }         else             n_pts.clear();         ROS_DEBUG("detect feature costs: %fms", t_t.toc());          ROS_DEBUG("add feature begins");         TicToc t_a;         addPoints();         ROS_DEBUG("selectFeature costs: %fms", t_a.toc());     }      // 将图片数据、角点存入前一帧     prev_img = cur_img;     prev_pts = cur_pts;     prev_un_pts = cur_un_pts;     cur_img = forw_img;     cur_pts = forw_pts;     // 畸变矫正,计算光流速度     undistortedPoints();     prev_time = cur_time; } 

3.2.3 rejectWithF()

rejectWithF()函数利用F矩阵和ransac过滤外点,ransac距离阈值为1像素;此外,所有的特征点畸变矫正到统一固定的图像尺寸,防止因为尺寸差异导致ransac距离阈值无统一的尺度。

void FeatureTracker::rejectWithF() {     if (forw_pts.size() >= 8)     {         ROS_DEBUG("FM ransac begins");         TicToc t_f;         vector<cv::Point2f> un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size());         for (unsigned int i = 0; i < cur_pts.size(); i++)         {             // 将畸变原点投影到投影平面(矫正),归一化后转化为 FOCAL_LENGTH =460 ,偏移量为固定值的图像上             Eigen::Vector3d tmp_p;             m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);             tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;             tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;             un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());              m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), tmp_p);             tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;             tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;             un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());         }          vector<uchar> status;         // F_THRESHOLD = 1 为ransac阈值         cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);         int size_a = cur_pts.size();         // 删除外点,更新角点信息                 reduceVector(prev_pts, status);         reduceVector(cur_pts, status);         reduceVector(forw_pts, status);         reduceVector(cur_un_pts, status);         reduceVector(ids, status);         reduceVector(track_cnt, status);         ROS_DEBUG("FM ransac: %d -> %lu: %f", size_a, forw_pts.size(), 1.0 * forw_pts.size() / size_a);         ROS_DEBUG("FM ransac costs: %fms", t_f.toc());     } } 

3.2.4 undistortedPoints()

将原图上的畸变点矫正为投影平面上的归一化点,同时计算前后帧间已跟踪的特征点之间的光流速度

void FeatureTracker::undistortedPoints() {     cur_un_pts.clear();     cur_un_pts_map.clear();     //cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());     for (unsigned int i = 0; i < cur_pts.size(); i++)     {         Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y);         Eigen::Vector3d b;         // 将原图畸变点投影到3D投影平面,畸变矫正归一化平面         m_camera->liftProjective(a, b);         cur_un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));         cur_un_pts_map.insert(make_pair(ids[i], cv::Point2f(b.x() / b.z(), b.y() / b.z())));         //printf("cur pts id %d %f %f", ids[i], cur_un_pts[i].x, cur_un_pts[i].y);     }     // caculate points velocity     if (!prev_un_pts_map.empty())     {         double dt = cur_time - prev_time;         pts_velocity.clear();         for (unsigned int i = 0; i < cur_un_pts.size(); i++)         {             if (ids[i] != -1)             {                 std::map<int, cv::Point2f>::iterator it;                 // 寻找id号为ids[i]的特征点,计算前后两帧的光流速度                 it = prev_un_pts_map.find(ids[i]);                 if (it != prev_un_pts_map.end())                 {                     double v_x = (cur_un_pts[i].x - it->second.x) / dt;                     double v_y = (cur_un_pts[i].y - it->second.y) / dt;                     pts_velocity.push_back(cv::Point2f(v_x, v_y));                 }                 else                     pts_velocity.push_back(cv::Point2f(0, 0));             }             else             {                 pts_velocity.push_back(cv::Point2f(0, 0));             }         }     }     else     {         for (unsigned int i = 0; i < cur_pts.size(); i++)         {             pts_velocity.push_back(cv::Point2f(0, 0));         }     }     prev_un_pts_map = cur_un_pts_map; } 

4. vins_estimator


4.1 姿态解算方法回顾

4.1.1 2D-2D姿态解算-对极几何约束

VINS-Mono阅读笔记一             图4.1 对极约束说明
  如上图,点p1和点p2是世界坐标系下的同一点P向两幅不同成像平面投影而成,O1与O2分别为两相机的中心。对极约束的物理含义表述为线段

O1p1overrightarrow{O_1p_1}

O2p2overrightarrow{O_2p_2}

与线段

O1O2overrightarrow{O_1O_2}

共面。其数学含义可用下式表示

(O1p1×O1O2)O2p2=0(overrightarrow{O_1p_1} times overrightarrow{O_1O_2}) overrightarrow{O_2p_2}=0

由针孔相机模型获得的对极几何约束为:

x2TtRx1=0x^T_2t^{land}Rx_1 = 0

对极几何约束推导如下

P1P_1

为相机

O1O_1

坐标系下的点,则由针孔相机模型可得:

s1p1=KP1   s2p2=K(R21P1+t21)s_1p_1 = KP_1   s_2p_2 = K(R_{21}P_1+t_{21})

左侧归一化带入右侧归一化坐标,取齐次坐标前两维:

x1=K1p1=P1  x2=K1p2x_1 = K^{-1}p_1=P_1    x_2 = K^{-1}p_2

x2=R21x1+t21x_2 = R_{21}x_1+t_{21}

两侧左乘矩阵

t21t_{21}^{land}

得(共线向量叉乘等于0向量):

t21x2=t21R21x1t_{21}^{land}x_2 = t_{21}^{land}R_{21}x_1

两侧对

x2x_2

内积(相互垂直向量点乘为0),即得对极几何约束:

x2TtRx1=0x^T_2t^{land}Rx_1 = 0


向量外积
两向量外积

a×b=[ijka1a2a3b1b2b3]=[0a3a2a30a1a2a10]b=aboverrightarrow{a}timesoverrightarrow{b}=begin{bmatrix} overrightarrow{i} & overrightarrow{j} & overrightarrow{k} \ a_1 & a_2 & a_3 \ b_1 & b_2 & b_3\ end{bmatrix}=begin{bmatrix} 0 & -a_3 & a_2 \ a_3 & 0 & -a_1 \ -a_2 & a_1 & 0\ end{bmatrix}b=a^{land}b

,其中,

aa^{land}

记作向量

aoverrightarrow{a}

的反对称矩阵

其中

x2x_2

x1x_1

分别为图4.1中p1与p2图像点畸变矫正后的归一化坐标点,将归一化公式带入可得等价表达式:

p2TKTtRK1p1=0p^T_2K^{-T}t^{land}RK^{-1}p_1 = 0

E=tRE=t^{land}R

为本质矩阵,

F=KTtRK1F=K^{-T}t^{land}RK^{-1}

为基础矩阵,则对极几何约束可表示为:

x2TEx1=p2TFp1=0x^T_2Ex_1 = p^T_2Fp_1 = 0

由两帧图像之间的多对匹配特征点可求解出

EE

FF

矩阵,最终可将其分解,得到两帧图像之间的姿态

RR

TT

,具体求解思路可参考高翔《视觉SLAM十四讲》。

4.1.2 三角化深度求解

三角化(三角定位)是利用两相机的相对姿态与对应的像素坐标求解其齐次坐标的深度

ss

,从而求解出2D点对应的3D坐标。由针孔相机模型得:

s2x2=s1R21x1+t21s_2x_2 = s_1R_{21}x_1+t_{21}

两侧同乘

x2x^{land}_2

可得:

s2x2x2=s1R21x1+t21=0s_2x^{land}_2x_2 = s_1R_{21}x_1+t_{21}=0

求解出

s1s_1

的零解,带入原式求解出

s2s_2

零解;由此可知同一3D点在各相机坐标系下的坐标。

但通常由于误差导致上式无零解存在,常见做法是构建超定方程组,求解其最小二乘解,具体证明如下:
令世界坐标系下的点

=[X,Y,Z,1]P=[X,Y,Z,1]

,两帧间的姿态矩阵

Tcw=[r1r2r3]=[RT]T_{cw}=begin{bmatrix} r_1 \ r_2 \r_3\ end{bmatrix}=[R|T]

x=[u,v,1]Tx=[u,v,1]^T

为归一化后的平面坐标,

λlambda

为其深度,则有:
                     

λx=TPlambda x=TP


                  

x×λx=x×TP=0x^{land} times lambda x=x^{land} times TP=0


展开上式,得:
          

x×TP=[01v10uvu0][r1r2r3]Px^{land} times TP=begin{bmatrix} 0 & -1 & v \ 1 & 0 & -u \ -v & u & 0\ end{bmatrix}begin{bmatrix} r_1 \ r_2 \r_3\ end{bmatrix}P


               

=[r2+vr3r1ur3vr1+ur2]P=0=begin{bmatrix} -r_2+vr_3 \ r_1-ur_3 \-vr_1+ur_2\ end{bmatrix}P=0

上式中第三行为可通过消元化简,最终一个点可列两个方程组:
                 

[r2+vr3r1ur3]P=0begin{bmatrix} -r_2+vr_3 \ r_1-ur_3 \ end{bmatrix}P=0

则两对以上的匹配点便可构建超静定方程组              
                 

[r2(1)+v(1)r3(1)r1(1)u(1)r3(1)r2(2)+v(2)r3(2)r1(2)u(2)r3(2)]P=0begin{bmatrix} -r^{(1)}_2+v^{(1)}r^{(1)}_3 \ r^{(1)}_1-u^{(1)}r^{(1)}_3 \-r^{(2)}_2+v^{(2)}r^{(2)}_3 \ r^{(2)}_1-u^{(2)}r^{(2)}_3 \vdots end{bmatrix}P=0


上述方程通过SVD可求得最小二乘解,将其归一为齐次坐标即可得世界坐标系下的3D点P
                 

P=[XYZ1]=[XYZ1]=[X0/X3X1/X3X2/X3X3/X3]P=begin{bmatrix} X \ Y \ Z \ 1 \ end{bmatrix}=begin{bmatrix} X \ Y \ Z \ 1 \ end{bmatrix}=begin{bmatrix} X_0/X_3 \ X_1/X_3 \ X_2/X_3 \ X_3/X_3 \ end{bmatrix}

上述算法在vins-mono中的实现代码如下:
`

void GlobalSFM::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1, 						         Vector2d &point0, Vector2d &point1, Vector3d &point_3d) { 	Matrix4d design_matrix = Matrix4d::Zero(); 	design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0); 	design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1); 	design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0); 	design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1); 	Vector4d triangulated_point; 	triangulated_point = 		      design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>(); 	point_3d(0) = triangulated_point(0) / triangulated_point(3); 	point_3d(1) = triangulated_point(1) / triangulated_point(3); 	point_3d(2) = triangulated_point(2) / triangulated_point(3); } 

4.1.3 3D-2D姿态解算-PNP

4.1.2介绍了由对极几何求解2D-2D的相邻帧间姿态解算问题。该方法通常是单目slam初始化的必经步骤,若初始化尺度不正确将导致后面较大的误差累计。当初始化成功后,便可将第一帧的相机坐标系设为世界坐标系,在已知世界坐标系的3D点下,可通过PNP(Perspective-N-Point)求解后续的相机姿态,该方法不需要较多的特征点,slam中常用的姿态解算方法。             VINS-Mono阅读笔记一              图4.2 P3P求解说明示意图

PNP算法求解相机姿态的步骤如下:

  • 根据相似定理,已知匹配点3D世界坐标及当前帧的图像坐标情况下,求解匹配点的相机坐标系3D坐标(具体细节参考《视觉slam十四讲》)
  • 利用ICP,已知世界坐标系的3D点和对应相机坐标系的3D点求解相机坐标系与世界坐标系间的姿态

4.1.4 3D-3D姿态解算-ICP

3D-3D的姿态估计是利用匹配点对间的姿态转换约束构建方程组,通过最小化误差求解出两者间的最优姿态矩阵。
  假设有一组匹配3D点对

P={p1,,pn}P=begin{Bmatrix}p_1,cdots,p_n end{Bmatrix}

P={p1,,pn}P^*=begin{Bmatrix}p^*_1,cdots,p^*_n end{Bmatrix}

,则两者将的坐标转换可通过下式实现:
                    

i,pi=Rpi+tforall i, p_i=Rp^*_i+t


求解ICP常用方法有两种(具体求解推导参考《视觉SLAM十四讲》):

  • SVD法
    首先构建如下表示的最小二乘问题,求解使误差平方和最小的
    R,TR,T


    min(R,t) J=12i=1npi(Rpi+t)22min_{(R,t)} J=frac 12 sum^{n}_{i=1}||p_i-(Rp^*_i+t)||^2_2

  • 非线性优化法
    利用李代数表达目标函数为:
    minξ J=12i=1npiexp(ξ)pi22min_xi J=frac 12sum^{n}_{i=1}||p_i-exp(xi^{land})p^*_i||^2_2

    利用李代数扰动模型计算单误差对

    δξdeltaxi

    的偏导数:

    eδξ=[I(Rp+t)0T0]frac{partial e}{partial deltaxi}=-begin{bmatrix} I & -(Rp^*+t)^{land} \ 0^T & 0^* \ end{bmatrix}

    利用LM算法不断迭代

    ξxi

    便可求解出最优

    ξxi


      需要注意的是,vslam中的点对是已经匹配好的,这个最小二乘问题实际具有解析解,即:ICP迭代一次后的结果和SVD的代数解十分接近。利用ICP的好处是在匹配点误差较大或误匹配时,能获得比SVD更加准确的结果。

未完…

本文转自互联网,侵权联系删除VINS-Mono阅读笔记一

赞(0) 打赏
部分文章转自网络,侵权联系删除b2bchain区块链学习技术社区 » VINS-Mono阅读笔记一
分享到: 更多 (0)

评论 抢沙发

  • 昵称 (必填)
  • 邮箱 (必填)
  • 网址

b2b链

联系我们联系我们