手搓视觉SLAM定位算法：第一章-育师

手搓视觉SLAM定位算法：第一章

默认读者对视觉slam和视觉定位有一定的基础
没有的话可以参考《视觉十四讲》或者作者的另外一个专栏
有一点基础就行

文章目录

手搓视觉SLAM定位算法：第一章
前言
一、理论基础
二、代码实现
- 2.1 设置参数
- 2.2 匹配
- 2.3 求解
- 2.4 发布
三、实验结果
总结

前言

本章节的目的是实现一个帧到帧的视觉定位算法，跟ros结合
因为还不涉及建图，所以这里还不能叫做slam

一、理论基础

ROS就不介绍了，核心三件套：roscore平台，subscriber订阅器，publisher发布器

这里因为是用KITTI数据集，所以直接从磁盘去读，那么subscriber就用不到了，只有publisher

另外依赖什么的都不需要，有opencv3就行，本人用的是ubuntu18+opencv3.4.16

#----------------------------------------------------------------------------------------------------------------------------------------

因为是帧到帧，所以实际上非常非常的简单，流程就是三步：匹配，求解，发布

匹配：
在参考帧中找n个角点，用LK光流在目标帧中确定这些点的位置，从而获得两组匹配的点对

求解
匹配步骤的结果是两组匹配点对，那么结合相机的内参就可以计算两帧图像的本质矩阵，通过这个矩阵就可以反推位姿
这样就获得了目标帧相对参考帧的运动变换

发布
获得了目标帧相对参考帧的运动变换后，结合参考帧在世界坐标系中的位姿，就可以得到目标帧的位姿
发布目标帧的位姿，然后以目标帧作为新的参考帧，进行下一个循环，直到没有新的图像

二、代码实现

理论上很简单，代码实现也一样的简单，拆开几步走

2.1 设置参数

读取的参数主要就是相机的内参，其他都不是很重要，这里也可以直接手动输入，当然我习惯了还是用config配合launch来读

ros_nh.param<std::string>("mono/camera_topic",camera_topic,"/mono/track_img");// 追踪图像发布topicros_nh.param<std::string>("mono/path_topic",path_topic,"/mono/camera_path");// 相机轨迹发布topicros_nh.param<std::string>("data/image_path",img_path," ");// KITTI图像所在目录ros_nh.param<int>("data/image_nums",img_nums,0);// 读取多少个图像ros_nh.param<float>("mono/intrinsic/fx",fx,0);// 相机内参ros_nh.param<float>("mono/intrinsic/fy",fy,0);ros_nh.param<float>("mono/intrinsic/cx",cx,0);ros_nh.param<float>("mono/intrinsic/cy",cy,0);if(img_path[-1]!='/')img_path+='/';std::cout<<"[Ros_parameter]: camera_topic "<<" ==> "<<camera_topic<<std::endl;std::cout<<"[Ros_parameter]: path_topic "<<" ==> "<<path_topic<<std::endl;std::cout<<"[Ros_parameter]: img_path "<<" ==> "<<img_path<<std::endl;std::cout<<"[Ros_parameter]: img_nums "<<" ==> "<<img_nums<<std::endl;std::cout<<"[Ros_parameter]: intrinsic "<<" ==> "<<fx<<" "<<fy<<" "<<cx<<" "<<cy<<" "<<std::endl;

2.2 匹配

按理讲应该第一张图片提取角点后，后面都不需要再提取，直到点的数量太少了的时候再重新提取角点

但是作者比较懒，所以每个图片都提取了角点，并跟踪

cv::Mat cur_img;cur_img=cv::imread(img_queue[loop_count],cv::IMREAD_UNCHANGED);std::vector<cv::Point2f>points_pre;cv::goodFeaturesToTrack(pre_img,points_pre,230,0.01,10);// 对参考帧提取角点std::vector<cv::Point2f>points_cur;std::vector<uchar>status;std::vector<float>err;cv::calcOpticalFlowPyrLK(// 在目标帧中用LK光流跟踪pre_img,cur_img,points_pre,points_cur,status,err,cv::Size(21,21),3);cv::Mat result;cv::cvtColor(cur_img,result,cv::COLOR_GRAY2BGR);std::vector<cv::Point2f>pre_pts;std::vector<cv::Point2f>cur_pts;for(size_t i=0;i<points_pre.size();i++){// 画图，顺便筛一下错误的匹配点if(status[i]){cv::circle(result,points_cur[i],3,cv::Scalar(0,255,0),-1);cv::line(result,points_pre[i],points_cur[i],cv::Scalar(255,0,0),2);pre_pts.push_back(points_pre[i]);cur_pts.push_back(points_cur[i]);}}// 这里有说道的，我们只做了reference to target的LK光流追踪，为了降低错误，可以反向再来一次// 也就是做一下target to reference的LK光流，结合两个方向的结果来筛选最后正确的匹配点对// 这个方法在VINS-Fusion里使用了，作者比较懒就不搞了

2.3 求解

2.2的输出结果就是pre_pts和cur_pts两个匹配的点对，假设我们相机的内参矩阵是K

那么就可以求解位姿了

cv::Mat E=cv::findEssentialMat(pre_pts,cur_pts,K,cv::RANSAC,0.999,1.0);// 求解本质矩阵std::cout<<"essential_matrix is "<<std::endl<<E<<std::endl;cv::Mat R,t;intinliers=cv::recoverPose(E,pre_pts,cur_pts,K,R,t);// 反向推导位姿

2.4 发布

如果是用Eigen的话，这里还是比较简单的，可惜不是，为了方便测试，我只用了opencv来做这个事情

那么发布位姿就需要按照下面的步骤来

cv::Mat Rt=cv::Mat::eye(4,4,CV_64F);R.copyTo(Rt(cv::Rect(0,0,3,3)));t.copyTo(Rt(cv::Rect(3,0,1,3)));T=T*Rt.inv();cv::Mat R_pose=T(cv::Rect(0,0,3,3));cv::Mat t_pose=T(cv::Rect(3,0,1,3));cv::Mat R_cv;cv::Rodrigues(R_pose,R_cv);// 位姿矩阵转旋转向量doubleangle=cv::norm(R_cv);// 计算旋转角cv::Mat axis=R_cv/angle;// 计算旋转轴geometry_msgs::PoseStamped pose_stamped;pose_stamped.header.stamp=ros::Time::now();pose_stamped.header.frame_id="world";pose_stamped.pose.position.x=t_pose.at<double>(0);pose_stamped.pose.position.y=t_pose.at<double>(1);pose_stamped.pose.position.z=t_pose.at<double>(2);pose_stamped.pose.orientation.x=axis.at<double>(0)*sin(angle/2);pose_stamped.pose.orientation.y=axis.at<double>(1)*sin(angle/2);pose_stamped.pose.orientation.z=axis.at<double>(2)*sin(angle/2);pose_stamped.pose.orientation.w=cos(angle/2);path_msg.poses.push_back(pose_stamped);// 做成pose_stamped来发布pub_camera_path.publish(path_msg);// 别忘了发布图像sensor_msgs::ImagePtr cur_img_ptr=cv_bridge::CvImage(std_msgs::Header(),"rgb8",result).toImageMsg();cur_img_ptr->header.frame_id="world";cur_img_ptr->header.stamp=ros::Time::now();pub_track_img.publish(cur_img_ptr);