本文主要是介绍ROS-3DSLAM(8):视觉部分visual_estimator第二节:initial 2,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!
2021@SDUSC
2021年11月12日星期四——2021年11月13日星期六
一、背景简介:
结束了对于initial_alignment的初步阅读,下一步还要继续对于initial文件夹下的其他文件的分析,毕竟我们的最终目的还是对于函数esitimator中的的processImage()做分析。
这次的计划是针对Initial_ex_rotation和solve_5ph给出分析。
二、Initial_ex_rotation分析:
A、功能分析
函数的功能是求解从相机(Camera)坐标系到IMU坐标系的相对旋 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-mHOutEzL-1636789051650)(https://www.zhihu.com/equation?tex=R_%7Bbc%7D)],在该函数中用的ric表示的。
经过我的搜索,发现这一部分被称为SLAM的基础的算法,也算是著名的部分经典部分了。
这部分的原理介绍网上有很多,我在这里就不搬过来了,只是简单地给出自己的说明:
首先利用相机两帧的位姿计算出来一个旋转量R1,或利用imu预积分得到R2,经过计算得到R1‘,根据两者之间的差距计算得出最准确的RC。
B、代码分析
1、首先是头文件
开头的注释说明了代码的功能,在后面给出了五个函数的声明(不包含类的构造函数)
/* This class help you to calibrate extrinsic rotation between imu and camera when your totally don't konw the extrinsic parameter */
class InitialEXRotation
{
public:InitialEXRotation();bool CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result);
private:Matrix3d solveRelativeR(const vector<pair<Vector3d, Vector3d>> &corres);double testTriangulation(const vector<cv::Point2f> &l,const vector<cv::Point2f> &r,cv::Mat_<double> R, cv::Mat_<double> t);void decomposeE(cv::Mat E,cv::Mat_<double> &R1, cv::Mat_<double> &R2,cv::Mat_<double> &t1, cv::Mat_<double> &t2);int frame_count;vector< Matrix3d > Rc;vector< Matrix3d > Rimu;vector< Matrix3d > Rc_g;Matrix3d ric;
};
2、CalibrationExRotation,最重要的函数
#include "initial_ex_rotation.h"
//构造函数,初始化各个参数,MatrixXd::Identity(rows,cols) // eye(rows,cols) //单位矩阵
InitialEXRotation::InitialEXRotation(){frame_count = 0;Rc.push_back(Matrix3d::Identity());Rc_g.push_back(Matrix3d::Identity());Rimu.push_back(Matrix3d::Identity());ric = Matrix3d::Identity();
}
//先要弄清楚这些传进来的参数是什么, 一共有三个传进来的参数
//corres 两帧之间匹配的特征点 归一化坐标,通过对极约束?,构建本质矩阵E,分解得到两帧之间的旋转矩阵, solveRelativeR(corres) 函数实现,也就是开头提到的通过相机获得的旋转量。
//delta_q_imu 为相邻两时刻 IMU预积分的旋转量,四元数表示传入进来的 匹配的归一化坐标,IMU预计分旋转量,会先存在vector中,因为需要多对信息,构建超定方程矩阵A
//calib_ric_result 为待求的外参数
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{frame_count++;//利用下面的函数计算得到旋转量1Rc.push_back(solveRelativeR(corres));//将原来的四元数转化成了旋转矩阵,方便后面的计算。Rimu.push_back(delta_q_imu.toRotationMatrix());//每帧IMU相对于起始帧IMU的R,初始化为IMU预积分Rc_g.push_back(ric.inverse() * delta_q_imu * ric);//构建公式当中出现的矩阵Eigen::MatrixXd A(frame_count * 4, 4);A.setZero();int sum_ok = 0;for (int i = 1; i <= frame_count; i++){Quaterniond r1(Rc[i]);Quaterniond r2(Rc_g[i]);/* angularDistance就是计算两个坐标系之间相对旋转矩阵在做轴角变换后(u * theta)的角度theta, theta越小说明两个坐标系的姿态越接近,这个角度距离用于后面计算权重,这里计算权重就是为了降低外点的干扰,意思就是为了防止出现误差非常大的R_bk+1^bk和 R_ck+1^ck约束导致估计的结果偏差太大*/double angular_distance = 180 / M_PI * r1.angularDistance(r2);ROS_DEBUG("%d %f", i, angular_distance);//核函数 加权
这篇关于ROS-3DSLAM(8):视觉部分visual_estimator第二节:initial 2的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!