SLAM ---- VINS 外点剔除

brief

迁移多年前的个人网站 文章

1. 外点剔除

1.1 前端外点剔除

vins-mono 中根据光流跟踪，得到匹配点对; vins-fusion 中可以设置反向光流，进一步剔除

然后使用F基础矩阵进行剔除外点：rejectWithF()

原理介绍： 1. 输入的图片是带畸变的（如Euroc 单目数据集，为针孔相机模型） 2. 则需要先通过将像素坐标系转到归一化坐标系下并去畸变（可以用 camodocal 模型 liftProjective 完成） 3. liftProjective 之后得到归一化坐标系下的 3D点，除以 z ，得到 [x，y，1]形式的归一化平面上的点，再通过参数转到归一化的像素坐标系下（可以认为是转到去了畸变的图像上），然后进行基础矩阵F的求解，其中会调用RANSAC 进行剔除外点。 （参数：这儿的参数就是 FOCAL_LENGTH，COL / 2.0，ROW / 2.0，归一化坐标值乘以焦距转到像素坐标系，由于图片中原点在图片左上角，于是还需要进行关于原点的平移） 4. 使用的是经典的8点法求解，与本质矩阵 E 求解一样

给人的错觉就是 本质矩阵求解，有点疑惑，后续再看看！！！

voidFeatureTracker::rejectWithF(){if(forw_pts.size()>=8)// 当前帧（追踪上）特征点数量足够多{ROS_DEBUG("FM ransac begins");TicToc t_f;// 1.遍历所有特征点，转化为归一化相机坐标系vector<cv::Point2f>un_cur_pts(cur_pts.size()),un_forw_pts(forw_pts.size());for(unsignedinti=0;i<cur_pts.size();i++)//遍历上一帧所有特征点{Eigen::Vector3d tmp_p;//对于PINHOLE（针孔相机）可将像素坐标转换到归一化平面并去畸变。根据不同的相机模型将二维坐标转换到三维坐标m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x,cur_pts[i].y),tmp_p);//转换为归一化像素坐标，上一帧和当前帧tmp_p.x()=FOCAL_LENGTH*tmp_p.x()/tmp_p.z()+COL/2.0;tmp_p.y()=FOCAL_LENGTH*tmp_p.y()/tmp_p.z()+ROW/2.0;un_cur_pts[i]=cv::Point2f(tmp_p.x(),tmp_p.y());m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x,forw_pts[i].y),tmp_p);tmp_p.x()=FOCAL_LENGTH*tmp_p.x()/tmp_p.z()+COL/2.0;tmp_p.y()=FOCAL_LENGTH*tmp_p.y()/tmp_p.z()+ROW/2.0;un_forw_pts[i]=cv::Point2f(tmp_p.x(),tmp_p.y());}vector<uchar>status;// 2. 调用cv::findFundamentalMat对un_cur_pts和un_forw_pts计算F矩阵，需要归一化相机系，z=1cv::findFundamentalMat(un_cur_pts,un_forw_pts,cv::FM_RANSAC,F_THRESHOLD,0.99,status);intsize_a=cur_pts.size();// 3. 根据status删除一些特征点reduceVector(prev_pts,status);reduceVector(cur_pts,status);reduceVector(forw_pts,status);reduceVector(cur_un_pts,status);reduceVector(ids,status);reduceVector(track_cnt,status);ROS_DEBUG("FM ransac: %d -> %lu: %f",size_a,forw_pts.size(),1.0*forw_pts.size()/size_a);ROS_DEBUG("FM ransac costs: %fms",t_f.toc());}}

1.2 后端外点剔除

通过计算重投影误差剔除，若大于阈值则是外点。原理简介： 设置的阈值为3个像素点,由于误差是在归一化坐标系下计算的，还需要乘以焦距，将误差值转换到 像素平面下，若大于3个像素点，则是outliner

doubleEstimator::reprojectionError(Matrix3d&Ri,Vector3d&Pi,Matrix3d&rici,Vector3d&tici,Matrix3d&Rj,Vector3d&Pj,Matrix3d&ricj,Vector3d&ticj,doubledepth,Vector3d&uvi,Vector3d&uvj){Vector3d pts_w=Ri*(rici*(depth*uvi)+tici)+Pi;Vector3d pts_cj=ricj.transpose()*(Rj.transpose()*(pts_w-Pj)-ticj);Vector2d residual=(pts_cj/pts_cj.z()).head<2>()-uvj.head<2>();doublerx=residual.x();doublery=residual.y();returnsqrt(rx*rx+ry*ry);}voidEstimator::outliersRejection(set<int>&removeIndex){//return;intfeature_index=-1;for(auto&it_per_id:f_manager.feature){doubleerr=0;interrCnt=0;it_per_id.used_num=it_per_id.feature_per_frame.size();if(it_per_id.used_num<4)continue;feature_index++;intimu_i=it_per_id.start_frame,imu_j=imu_i-1;Vector3d pts_i=it_per_id.feature_per_frame[0].point;doubledepth=it_per_id.estimated_depth;for(auto&it_per_frame:it_per_id.feature_per_frame){imu_j++;if(imu_i!=imu_j){Vector3d pts_j=it_per_frame.point;doubletmp_error=reprojectionError(Rs[imu_i],Ps[imu_i],ric[0],tic[0],Rs[imu_j],Ps[imu_j],ric[0],tic[0],depth,pts_i,pts_j);err+=tmp_error;errCnt++;//printf("tmp_error %f\n", FOCAL_LENGTH / 1.5 * tmp_error);}// need to rewrite projecton factor.........if(STEREO&&it_per_frame.is_stereo){Vector3d pts_j_right=it_per_frame.pointRight;if(imu_i!=imu_j){doubletmp_error=reprojectionError(Rs[imu_i],Ps[imu_i],ric[0],tic[0],Rs[imu_j],Ps[imu_j],ric[1],tic[1],depth,pts_i,pts_j_right);err+=tmp_error;errCnt++;//printf("tmp_error %f\n", FOCAL_LENGTH / 1.5 * tmp_error);}else{doubletmp_error=reprojectionError(Rs[imu_i],Ps[imu_i],ric[0],tic[0],Rs[imu_j],Ps[imu_j],ric[1],tic[1],depth,pts_i,pts_j_right);err+=tmp_error;errCnt++;//printf("tmp_error %f\n", FOCAL_LENGTH / 1.5 * tmp_error);}}}doubleave_err=err/errCnt;// 得到归一化坐标系下的重投影误差值if(ave_err*FOCAL_LENGTH>3)// 乘以焦距，讲误差值转换到 像素平面下，若大于3个像素点，则是outlinerremoveIndex.insert(it_per_id.feature_id);}}