2023年12月3日发(作者：)
视觉SLAM十四讲第九章0.4代码注释文中的主要代码引用自高翔博士的《视觉slam十四讲》，注释部分转载自链接1. run_// -------------- test the visual odometry -------------#include
//先读取rgb文件的数量 cout<<"read total "<
这里的Affine3d类型用来调整viz中的部件的位姿 cv::Affine3d M ( cv::Affine3d::Mat3 ( on_matrix() ( 0,0 ), on_matrix() ( 0,1 ), on_matrix() ( 0,2 ), on_matrix() ( 1,0 ), on_matrix() ( 1,1 ), on_matrix() ( 1,2 ), on_matrix() ( 2,0 ), on_matrix() ( 2,1 ), on_matrix() ( 2,2 ) ), cv::Affine3d::Vec3 ( ation() ( 0,0 ), ation() ( 1,0 ), ation() ( 2,0 ) ) ); //在图像上圈出特征点 Mat img_show = (); //遍历地图上的所有的地图点 for ( auto& pt:vo->map_->map_points_ ) { myslam::MapPoint::Ptr p = ; Vector2d pixel = pFrame->camera_->world2pixel ( p->pos_, pFrame->T_c_w_ ); //画圆 cv::circle ( img_show, cv::Point2f ( pixel ( 0,0 ),pixel ( 1,0 ) ), 5, cv::Scalar ( 0,255,0 ), 2 ); } cv::imshow ( "image", img_show ); cv::waitKey ( 1 ); getPose ( "Camera", M ); ce ( 1, false ); ce ( 1, false ); cout<
//建立一个保存描述子的map矩阵，保存匹配需要地图点的描述子， //因为描述子是一个行向量 Mat desp_map; // select the candidates in map
//candidate的生命周期只限于这个函数，暂时存放在当前帧视野以内的mappoint vector
检测这个点（世界坐标系中）是否在当前帧的视野之内
if ( curr_->isInFrame(p->pos_) ) { // add to candidate
p->visible_times_++; _back( p ); //Mat也可以利用push_back来插入一行，和vector类似 desp__back( p->descriptor_ ); } } //采用新的匹配方法FlannBasedMatcher(最近邻近似匹配)，而不是暴力匹配BFMatcher； //这步匹配中，第一个参数由原来的参考帧，变成了上面定义的desp_map地图，进行匹配。 //也就是当前帧直接和地图进行匹配 matcher_flann_.match ( desp_map, descriptors_curr_, matches ); // select the best matches //找出容器matches中最小的元素的指针或迭代器 //distance变量返回两个描述子之间的距离 //这里用到了lambda表达式，也就是这里面的第三个参数 //可以隐藏返回值的类型，会根据return自动确定，这里又加上了 float min_dis = std::min_element ( (), (), [] ( const cv::DMatch& m1, const cv::DMatch& m2 )->bool { return ce < ce; } )->distance; //保存已经匹配的MapPoint（MapPoint的指针类型） match_3dpts_.clear(); //保存已经匹配的2d关键点的索引 match_2dkp_index_.clear(); for ( cv::DMatch& m : matches ) { //如果描述子之间的距离小于一个值（30和min_dis*match_ratio_中较大的），则表示匹配成功 if ( ce < max
后面的参数跟ransac算法有关。倒是都有默认值 * useExtrinsicGuess，迭代初始值是否定为提供的rvec和tvec值，这里没有提供，所以用false * iterationsCount，迭代次数，ransac算法所必须的迭代次数 * reprojectionError，重投影误差。ransac算法迭代时也必须要规定的误差阀值，来确定是否为内点。 * confidence，置信度。ransac算法每次用于更新迭代次数的参数。一般固定选为0.995 * inliers，内点输出承接数组
*/ cv::solvePnPRansac ( pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers ); //局内点数量 num_inliers_ = ; //输出符合模型的数据的个数 cout<<"pnp inliers: "<
//这个参数用于计算匹配率 match_3dpts_[index]->matched_times_++; } lizeOptimization(); ze ( 10 ); //经过BA优化之后的TCW T_c_w_estimated_ = SE3 ( pose->estimate().rotation(), pose->estimate().translation());
//为啥要.matrix(),？？如果不加就变成了2*3的了，第一行表示旋转向量，第二行表示平移向量 //cout<<"T_c_w_estimated_: "<
continue; //在第一帧的时候ref=curr=pframe，其中pframe包含了相机参数 //一开始的Tcw会自动变成单位矩阵 Vector3d p_world = ref_->camera_->pixel2world ( Vector2d ( keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y ),
curr_->T_c_w_, d );
//好象是参考帧的相机中心到当前的关键点对应的三维点的位移 Vector3d n = p_world - ref_->getCamCenter(); //归一化，单位化 ize();
//createMapPoint函数返回托管Mappoint类的对象的指针 //这个用于将第一帧的点坐标，描述子，frame等信息传给MapPoint类，保存在这里 // curr_.get()，智能指针的get()
返回curr托管的Frame类的指针 MapPoint::Ptr map_point = MapPoint::createMapPoint( p_world, n, descriptors_curr_.row(i).clone(), curr_.get() ); //将包含信息的map_point插入容器中 //在insertMapPoint这里会输出关键帧的个数 map_->insertMapPoint( map_point ); } } //map有两个功能，管理地图中的点，和关键帧 //如果不是第一帧直接执行这里 map_->insertKeyFrame ( curr_ ); ref_ = curr_;}//新增函数，增加地图中的点。局部地图类似于slidewindow一样，随时的增删地图中的点，来跟随运动//后面如果发现地图中的点的数量不够，会调用它void VisualOdometry::addMapPoints(){ // add the new map points into map //创建一个bool型的数组matched，大小为当前keypoints数组大小，值全为false vector
//首先这个match_2dkp_index_是新来的当前帧跟地图匹配时，好的匹配到的关键点在keypoins数组中的索引 //在这里将已经匹配的keypoint索引置为true，因为之后增加的肯定是之前没匹配的 for ( int index:match_2dkp_index_ ) matched[index] = true;
//遍历当前keypoints数组，然后将深度大于0的关键点都加入地图中 for ( int i=0; i continue; //如果没有continue的话，说明这个点在地图中没有找到匹配，认定为新的点， //下一步就是找到depth数据，构造3D点，然后构造地图点，添加进地图即可。 double d = ref_->findDepth ( keypoints_curr_[i] ); if ( d<0 ) continue; Vector3d p_world = ref_->camera_->pixel2world ( Vector3d p_world = ref_->camera_->pixel2world ( Vector2d ( keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y ), curr_->T_c_w_, d); Vector3d n = p_world - ref_->getCamCenter(); ize(); MapPoint::Ptr map_point = MapPoint::createMapPoint( p_world, n, descriptors_curr_.row(i).clone(), curr_.get()); //将地图中原来没有的点添加进地图 map_->insertMapPoint( map_point ); }}//新增函数：优化地图。主要是为了维护地图的规模。删除一些地图点，在点过少时增加地图点等操作。void VisualOdometry::optimizeMap(){ // remove the hardly seen and no visible points //利用auto自动为变量选取类型，在for循环的时候适合使用auto for ( auto iter = map_->map_points_.begin(); iter != map_->map_points_.end(); ) { //iter->second表示map容器中元素的值vlaue，由于值为mappoint类指针，因此还可以指向类的成员变量pos_ //判断世界坐标系的点是否在当前good_画面的视野中 //如果点在当前帧都不可见了，说明跑的比较远了，删掉 if ( !curr_->isInFrame(iter->second->pos_) ) { //删除掉iter指向的这个元素，返回下一个元素的迭代器 iter = map_->map_points_.erase(iter); continue; } //定义匹配率，用匹配次数/可见次数，匹配率过低说明经常见但是没有几次匹配。 //应该是一些比较难识别的点，也就是出来的描述子比价奇葩。所以删掉 float match_ratio = float(iter->second->matched_times_)/iter->second->visible_times_; if ( match_ratio < map_point_erase_ratio_ ) { iter = map_->map_points_.erase(iter); continue; } //得到当前帧和地图点之间的夹角，角度过大，删除 double angle = getViewAngle( curr_, iter->second ); if ( angle > M_PI/6. ) { iter = map_->map_points_.erase(iter); continue; } //继续，可以根据一些其他条件自己添加要删除点的情况 if ( iter->second->good_ == false ) { // TODO try triangulate this map point } iter++; } //下面说一些增加点的情况，首先当前帧去地图中匹配时，点少于100个了， // 一般情况是运动幅度过大了，跟之前的帧没多少交集了，所以增加一下。 if ( match_2dkp_index_.size()<100 ) addMapPoints(); //如果点过多了，多于1000个，适当增加释放率，让地图处于释放点的趋势。 if ( map_->map_points_.size() > 1000 ) { // TODO map is too large, remove some one map_point_erase_ratio_ += 0.05; } //如果没有多于1000个，保持释放率在0.1，维持地图的体量为平衡态 //如果没有多于1000个，保持释放率在0.1，维持地图的体量为平衡态 else map_point_erase_ratio_ = 0.1; cout<<"map points after optimized: "<
发布评论