文章目录
- 0 前情回顾
-
- 1 sync_process
- inputImage
- 2、trackImage
- 2.1 图像处理
- 2.2 hasPrediction
- 2.3 if(SHOW_TRACK)
- 2.4 setMask
- 2.5 goodFeaturesToTrack
- 2.3.1 undistortedPts
- 2.3.2 ptsVelocity
- 2.4 如果是双目
- 2.6 制作featureFrame
- 3 processMeasurements
- 3.1 对imu的处理
- 3.1.1 判断IMU数据是否可用
- 3.1.2 获得accVector和gyrVector
- 3.1.3 初始化IMU的姿态
- 3.1.4 处理IMU数据,运行processIMU
- 3.2 对图像的处理
0 前情回顾
上一个博客讲到了主程序rosNodeTest.cpp。在程序最后,会进入sync_process线程进行处理。本篇博客接着进行讲解。
本次工作
我首先一步步的把代码全部注释了,十分的详细,对于C++和OpenCV的一些操作也进行了详细的注释,对于刚入门的同学应该还是有帮助的。之后我将代码开源,并写了相应的博客进行讲解。
开源程序:
https://github.com/kuankuan-yue/VINS-FUSION-leanrning.git
相应博客:
VINS-FUSION代码超详细注释(VIO部分)/VIO入门(1)
VINS-FUSION代码超详细注释(VIO部分)/VIO入门(2)
VINS-FUSION代码超详细注释(VIO部分)/VIO入门(3)
VINS-FUSION代码超详细注释(VIO部分)/VIO入门(4)
1 sync_process
本程序的作用,判断是否双目,双目的话判断时间是否同步,之后讲图像image
(单目),或者image0
和image1
通过inputImage
输入到estimator
中。
inputImage
void Estimator::inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1)
首先设置参数,并开启processMeasurements线程
setParameter();
然后追踪图像上的特征。trackImage
,之后会进行详解,其中得到了featureFrame
if(_img1.empty())
featureFrame = featureTracker.trackImage(t, _img);
else
featureFrame = featureTracker.trackImage(t, _img, _img1);
然后,getTrackImage
对特征到跟踪的图像进行一些处理。并把追踪的图片imgTrack
发布出去.
if (SHOW_TRACK)
{
cv::Mat imgTrack = featureTracker.getTrackImage();
pubTrackImage(imgTrack, t);
}
然后,填充featureBuf
最后执行processMeasurements
,之后会进行详细讲解
2、trackImage
得到featureFrame
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> FeatureTracker::trackImage(double _cur_time, const cv::Mat &_img, const cv::Mat &_img1)
2.1 图像处理
可以添加图像处理的部分,比如直方图均衡等等方法。
{
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
clahe->apply(cur_img, cur_img);
if(!rightImg.empty())
clahe->apply(rightImg, rightImg);
}
2.2 hasPrediction
会对上一阵的点进行预测。但是具体是什么作用还不是很清楚
2.3 if(SHOW_TRACK)
drawTrack 画出追踪情况,就是在图像上的特征点位置出画圈圈,如果是双目的话就连线。
void FeatureTracker::drawTrack(const cv::Mat &imLeft, const cv::Mat &imRight,
vector<int> &curLeftIds,
vector<cv::Point2f> &curLeftPts,
vector<cv::Point2f> &curRightPts,
map<int, cv::Point2f> &prevLeftPtsMap)
2.4 setMask
在已跟踪到角点的位置上,将mask对应位置上设为0,
意为在cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
进行操作时在该点不再重复进行角点检测,这样可以使角点分布更加均匀
具体详情见开源的注释代码。
void FeatureTracker::setMask()
2.5 goodFeaturesToTrack
如果当前图像的特征点cur_pts数目小于规定的最大特征点数目MAX_CNT,则进行提取。
提取使用的cv::goodFeaturesToTrack。将点保存到n_pts
cv::goodFeaturesToTrack(cur_img, n_pts, MAX_CNT - cur_pts.size(), 0.01, MIN_DIST, mask);
之后将n_pts
保存到cur_pts
之中
2.3.1 undistortedPts
将像素座标系下的座标,转换为归一化相机座标系下的座标 即un_pts为归一化相机座标系下的座标。
vector<cv::Point2f> FeatureTracker::undistortedPts(vector<cv::Point2f> &pts, camodocal::CameraPtr cam)
cam->liftProjective(a, b);
这个函数是对鱼眼相机模型的标定及去畸变过程
void
PinholeCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const
2.3.2 ptsVelocity
计算当前帧相对于前一帧 特征点沿x,y方向的像素移动速度
vector<cv::Point2f> FeatureTracker::ptsVelocity(vector<int> &ids, vector<cv::Point2f> &pts,
map<int, cv::Point2f> &cur_id_pts, map<int, cv::Point2f> &prev_id_pts)
2.4 如果是双目
如果是双目相机,那么在右目上追踪左目的特征点。使用的函数是calcOpticalFlowPyrLK
cv::calcOpticalFlowPyrLK(cur_img, rightImg, cur_pts, cur_right_pts, status, err, cv::Size(21, 21), 3);
if(FLOW_BACK)
如果这个打开,就想前边的左右目图像的位置换一下,在进行一次特征跟踪,目的是反向跟踪,得到左右目都匹配到的点
cv::calcOpticalFlowPyrLK(rightImg, cur_img, cur_right_pts, reverseLeftPts, statusRightLeft, err, cv::Size(21, 21), 3);
之后undistortedPts
ptsVelocity
2.6 制作featureFrame
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
其中,camera_id = 0
为左目上的点,camera_id = 1
,为右目上的点。
3 processMeasurements
这是处理全部量测的线程,IMU的预积分,特征点的处理等等都在这里进行.
3.1 对imu的处理
3.1.1 判断IMU数据是否可用
if ((!USE_IMU || IMUAvailable(feature.first + td)))
其中
bool Estimator::IMUAvailable(double t)
3.1.2 获得accVector和gyrVector
对imu的时间进行判断,讲队列里的imu数据放入到accVector和gyrVector中,
bool Estimator::getIMUInterval(double t0, double t1, vector<pair<double, Eigen::Vector3d>> &accVector,
vector<pair<double, Eigen::Vector3d>> &gyrVector)
3.1.3 初始化IMU的姿态
initFirstIMUPose
,其实很简单,就是求一个姿态角,然后把航向角设为0
void Estimator::initFirstIMUPose(vector<pair<double, Eigen::Vector3d>> &accVector)
3.1.4 处理IMU数据,运行processIMU
void Estimator::processIMU(double t, double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
其中frame_count
是值窗内的第几帧图像
下边是新建一个预积分项目u
pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
预积分
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
其中的push_back
void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
{
dt_buf.push_back(dt);
acc_buf.push_back(acc);
gyr_buf.push_back(gyr);
propagate(dt, acc, gyr);
}
其中的propagate
void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
其中的midPointIntegration
.这里边就涉及到了IMU的传播方针和协方差矩阵.雅克比矩阵等等.哪里不懂可以VIO的理论知识.
【泡泡读者来稿】VINS 论文推导及代码解析(一)
【泡泡读者来稿】VINS 论文推导及代码解析(二)
【泡泡读者来稿】VINS 论文推导及代码解析(三)
【泡泡读者来稿】VINS 论文推导及代码解析(四)
void midPointIntegration(double _dt,
const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)
之后计算对应绝对坐标系下的位置等
int j = frame_count;
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
Vs[j] += dt * un_acc;
3.2 对图像的处理
请见下一博客
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)