手搓视觉SLAM定位算法第一章默认读者对视觉slam和视觉定位有一定的基础没有的话可以参考《视觉十四讲》或者作者的另外一个专栏有一点基础就行文章目录手搓视觉SLAM定位算法第一章前言一、理论基础二、代码实现2.1 设置参数2.2 匹配2.3 求解2.4 发布三、实验结果总结前言本章节的目的是实现一个帧到帧的视觉定位算法跟ros结合因为还不涉及建图所以这里还不能叫做slam一、理论基础ROS就不介绍了核心三件套roscore平台subscriber订阅器publisher发布器这里因为是用KITTI数据集所以直接从磁盘去读那么subscriber就用不到了只有publisher另外依赖什么的都不需要有opencv3就行本人用的是ubuntu18opencv3.4.16#----------------------------------------------------------------------------------------------------------------------------------------因为是帧到帧所以实际上非常非常的简单流程就是三步匹配求解发布匹配在参考帧中找n个角点用LK光流在目标帧中确定这些点的位置从而获得两组匹配的点对求解匹配步骤的结果是两组匹配点对那么结合相机的内参就可以计算两帧图像的本质矩阵通过这个矩阵就可以反推位姿这样就获得了目标帧相对参考帧的运动变换发布获得了目标帧相对参考帧的运动变换后结合参考帧在世界坐标系中的位姿就可以得到目标帧的位姿发布目标帧的位姿然后以目标帧作为新的参考帧进行下一个循环直到没有新的图像二、代码实现理论上很简单代码实现也一样的简单拆开几步走2.1 设置参数读取的参数主要就是相机的内参其他都不是很重要这里也可以直接手动输入当然我习惯了还是用config配合launch来读ros_nh.paramstd::string(mono/camera_topic,camera_topic,/mono/track_img);// 追踪图像发布topicros_nh.paramstd::string(mono/path_topic,path_topic,/mono/camera_path);// 相机轨迹发布topicros_nh.paramstd::string(data/image_path,img_path, );// KITTI图像所在目录ros_nh.paramint(data/image_nums,img_nums,0);// 读取多少个图像ros_nh.paramfloat(mono/intrinsic/fx,fx,0);// 相机内参ros_nh.paramfloat(mono/intrinsic/fy,fy,0);ros_nh.paramfloat(mono/intrinsic/cx,cx,0);ros_nh.paramfloat(mono/intrinsic/cy,cy,0);if(img_path[-1]!/)img_path/;std::cout[Ros_parameter]: camera_topic camera_topicstd::endl;std::cout[Ros_parameter]: path_topic path_topicstd::endl;std::cout[Ros_parameter]: img_path img_pathstd::endl;std::cout[Ros_parameter]: img_nums img_numsstd::endl;std::cout[Ros_parameter]: intrinsic fx fy cx cy std::endl;2.2 匹配按理讲应该第一张图片提取角点后后面都不需要再提取直到点的数量太少了的时候再重新提取角点但是作者比较懒所以每个图片都提取了角点并跟踪cv::Mat cur_img;cur_imgcv::imread(img_queue[loop_count],cv::IMREAD_UNCHANGED);std::vectorcv::Point2fpoints_pre;cv::goodFeaturesToTrack(pre_img,points_pre,230,0.01,10);// 对参考帧提取角点std::vectorcv::Point2fpoints_cur;std::vectorucharstatus;std::vectorfloaterr;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::vectorcv::Point2fpre_pts;std::vectorcv::Point2fcur_pts;for(size_t i0;ipoints_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 Ecv::findEssentialMat(pre_pts,cur_pts,K,cv::RANSAC,0.999,1.0);// 求解本质矩阵std::coutessential_matrix is std::endlEstd::endl;cv::Mat R,t;intinlierscv::recoverPose(E,pre_pts,cur_pts,K,R,t);// 反向推导位姿2.4 发布如果是用Eigen的话这里还是比较简单的可惜不是为了方便测试我只用了opencv来做这个事情那么发布位姿就需要按照下面的步骤来cv::Mat Rtcv::Mat::eye(4,4,CV_64F);R.copyTo(Rt(cv::Rect(0,0,3,3)));t.copyTo(Rt(cv::Rect(3,0,1,3)));TT*Rt.inv();cv::Mat R_poseT(cv::Rect(0,0,3,3));cv::Mat t_poseT(cv::Rect(3,0,1,3));cv::Mat R_cv;cv::Rodrigues(R_pose,R_cv);// 位姿矩阵转旋转向量doubleanglecv::norm(R_cv);// 计算旋转角cv::Mat axisR_cv/angle;// 计算旋转轴geometry_msgs::PoseStamped pose_stamped;pose_stamped.header.stampros::Time::now();pose_stamped.header.frame_idworld;pose_stamped.pose.position.xt_pose.atdouble(0);pose_stamped.pose.position.yt_pose.atdouble(1);pose_stamped.pose.position.zt_pose.atdouble(2);pose_stamped.pose.orientation.xaxis.atdouble(0)*sin(angle/2);pose_stamped.pose.orientation.yaxis.atdouble(1)*sin(angle/2);pose_stamped.pose.orientation.zaxis.atdouble(2)*sin(angle/2);pose_stamped.pose.orientation.wcos(angle/2);path_msg.poses.push_back(pose_stamped);// 做成pose_stamped来发布pub_camera_path.publish(path_msg);// 别忘了发布图像sensor_msgs::ImagePtr cur_img_ptrcv_bridge::CvImage(std_msgs::Header(),rgb8,result).toImageMsg();cur_img_ptr-header.frame_idworld;cur_img_ptr-header.stampros::Time::now();pub_track_img.publish(cur_img_ptr);三、实验结果不知道怎么上传视频就换成gif放上来看看效果吧不得不感慨KITTI00这个数据集确实适合做SLAM特征丰富参数也好帧到帧的定位问题也很明显了基本没有鲁棒性有点干扰就不行而且不考虑局部多帧的关联在停车那里直接飞了这也是local BA和滑窗被引入的意义完整代码后面再说吧有人看我再放到github上开的坑有点多填不完了总结做了一个非常简单的帧到帧视觉定位算法用KITTI数据集验证从实践中一点一点感受视觉slam的发展 手搓视觉SLAM定位算法：第一章
手搓视觉SLAM定位算法第一章默认读者对视觉slam和视觉定位有一定的基础没有的话可以参考《视觉十四讲》或者作者的另外一个专栏有一点基础就行文章目录手搓视觉SLAM定位算法第一章前言一、理论基础二、代码实现2.1 设置参数2.2 匹配2.3 求解2.4 发布三、实验结果总结前言本章节的目的是实现一个帧到帧的视觉定位算法跟ros结合因为还不涉及建图所以这里还不能叫做slam一、理论基础ROS就不介绍了核心三件套roscore平台subscriber订阅器publisher发布器这里因为是用KITTI数据集所以直接从磁盘去读那么subscriber就用不到了只有publisher另外依赖什么的都不需要有opencv3就行本人用的是ubuntu18opencv3.4.16#----------------------------------------------------------------------------------------------------------------------------------------因为是帧到帧所以实际上非常非常的简单流程就是三步匹配求解发布匹配在参考帧中找n个角点用LK光流在目标帧中确定这些点的位置从而获得两组匹配的点对求解匹配步骤的结果是两组匹配点对那么结合相机的内参就可以计算两帧图像的本质矩阵通过这个矩阵就可以反推位姿这样就获得了目标帧相对参考帧的运动变换发布获得了目标帧相对参考帧的运动变换后结合参考帧在世界坐标系中的位姿就可以得到目标帧的位姿发布目标帧的位姿然后以目标帧作为新的参考帧进行下一个循环直到没有新的图像二、代码实现理论上很简单代码实现也一样的简单拆开几步走2.1 设置参数读取的参数主要就是相机的内参其他都不是很重要这里也可以直接手动输入当然我习惯了还是用config配合launch来读ros_nh.paramstd::string(mono/camera_topic,camera_topic,/mono/track_img);// 追踪图像发布topicros_nh.paramstd::string(mono/path_topic,path_topic,/mono/camera_path);// 相机轨迹发布topicros_nh.paramstd::string(data/image_path,img_path, );// KITTI图像所在目录ros_nh.paramint(data/image_nums,img_nums,0);// 读取多少个图像ros_nh.paramfloat(mono/intrinsic/fx,fx,0);// 相机内参ros_nh.paramfloat(mono/intrinsic/fy,fy,0);ros_nh.paramfloat(mono/intrinsic/cx,cx,0);ros_nh.paramfloat(mono/intrinsic/cy,cy,0);if(img_path[-1]!/)img_path/;std::cout[Ros_parameter]: camera_topic camera_topicstd::endl;std::cout[Ros_parameter]: path_topic path_topicstd::endl;std::cout[Ros_parameter]: img_path img_pathstd::endl;std::cout[Ros_parameter]: img_nums img_numsstd::endl;std::cout[Ros_parameter]: intrinsic fx fy cx cy std::endl;2.2 匹配按理讲应该第一张图片提取角点后后面都不需要再提取直到点的数量太少了的时候再重新提取角点但是作者比较懒所以每个图片都提取了角点并跟踪cv::Mat cur_img;cur_imgcv::imread(img_queue[loop_count],cv::IMREAD_UNCHANGED);std::vectorcv::Point2fpoints_pre;cv::goodFeaturesToTrack(pre_img,points_pre,230,0.01,10);// 对参考帧提取角点std::vectorcv::Point2fpoints_cur;std::vectorucharstatus;std::vectorfloaterr;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::vectorcv::Point2fpre_pts;std::vectorcv::Point2fcur_pts;for(size_t i0;ipoints_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 Ecv::findEssentialMat(pre_pts,cur_pts,K,cv::RANSAC,0.999,1.0);// 求解本质矩阵std::coutessential_matrix is std::endlEstd::endl;cv::Mat R,t;intinlierscv::recoverPose(E,pre_pts,cur_pts,K,R,t);// 反向推导位姿2.4 发布如果是用Eigen的话这里还是比较简单的可惜不是为了方便测试我只用了opencv来做这个事情那么发布位姿就需要按照下面的步骤来cv::Mat Rtcv::Mat::eye(4,4,CV_64F);R.copyTo(Rt(cv::Rect(0,0,3,3)));t.copyTo(Rt(cv::Rect(3,0,1,3)));TT*Rt.inv();cv::Mat R_poseT(cv::Rect(0,0,3,3));cv::Mat t_poseT(cv::Rect(3,0,1,3));cv::Mat R_cv;cv::Rodrigues(R_pose,R_cv);// 位姿矩阵转旋转向量doubleanglecv::norm(R_cv);// 计算旋转角cv::Mat axisR_cv/angle;// 计算旋转轴geometry_msgs::PoseStamped pose_stamped;pose_stamped.header.stampros::Time::now();pose_stamped.header.frame_idworld;pose_stamped.pose.position.xt_pose.atdouble(0);pose_stamped.pose.position.yt_pose.atdouble(1);pose_stamped.pose.position.zt_pose.atdouble(2);pose_stamped.pose.orientation.xaxis.atdouble(0)*sin(angle/2);pose_stamped.pose.orientation.yaxis.atdouble(1)*sin(angle/2);pose_stamped.pose.orientation.zaxis.atdouble(2)*sin(angle/2);pose_stamped.pose.orientation.wcos(angle/2);path_msg.poses.push_back(pose_stamped);// 做成pose_stamped来发布pub_camera_path.publish(path_msg);// 别忘了发布图像sensor_msgs::ImagePtr cur_img_ptrcv_bridge::CvImage(std_msgs::Header(),rgb8,result).toImageMsg();cur_img_ptr-header.frame_idworld;cur_img_ptr-header.stampros::Time::now();pub_track_img.publish(cur_img_ptr);三、实验结果不知道怎么上传视频就换成gif放上来看看效果吧不得不感慨KITTI00这个数据集确实适合做SLAM特征丰富参数也好帧到帧的定位问题也很明显了基本没有鲁棒性有点干扰就不行而且不考虑局部多帧的关联在停车那里直接飞了这也是local BA和滑窗被引入的意义完整代码后面再说吧有人看我再放到github上开的坑有点多填不完了总结做了一个非常简单的帧到帧视觉定位算法用KITTI数据集验证从实践中一点一点感受视觉slam的发展