SLAM ---- VINS 外点剔除brief1. 外点剔除1.1 前端外点剔除1.2 后端外点剔除brief迁移多年前的个人网站 文章1. 外点剔除1.1 前端外点剔除vins-mono 中根据光流跟踪得到匹配点对; vins-fusion 中可以设置反向光流进一步剔除然后使用F基础矩阵进行剔除外点rejectWithF()原理介绍 1. 输入的图片是带畸变的如Euroc 单目数据集为针孔相机模型 2. 则需要先通过将像素坐标系转到归一化坐标系下并去畸变可以用 camodocal 模型 liftProjective 完成 3. liftProjective 之后得到归一化坐标系下的 3D点除以 z 得到 [xy1]形式的归一化平面上的点再通过参数转到归一化的像素坐标系下可以认为是转到去了畸变的图像上然后进行基础矩阵F的求解其中会调用RANSAC 进行剔除外点。 参数这儿的参数就是 FOCAL_LENGTHCOL / 2.0ROW / 2.0归一化坐标值乘以焦距转到像素坐标系由于图片中原点在图片左上角于是还需要进行关于原点的平移 4. 使用的是经典的8点法求解与本质矩阵 E 求解一样给人的错觉就是 本质矩阵求解有点疑惑后续再看看voidFeatureTracker::rejectWithF(){if(forw_pts.size()8)// 当前帧追踪上特征点数量足够多{ROS_DEBUG(FM ransac begins);TicToc t_f;// 1.遍历所有特征点转化为归一化相机坐标系vectorcv::Point2fun_cur_pts(cur_pts.size()),un_forw_pts(forw_pts.size());for(unsignedinti0;icur_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());}vectorucharstatus;// 2. 调用cv::findFundamentalMat对un_cur_pts和un_forw_pts计算F矩阵需要归一化相机系z1cv::findFundamentalMat(un_cur_pts,un_forw_pts,cv::FM_RANSAC,F_THRESHOLD,0.99,status);intsize_acur_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个像素点则是outlinerdoubleEstimator::reprojectionError(Matrix3dRi,Vector3dPi,Matrix3drici,Vector3dtici,Matrix3dRj,Vector3dPj,Matrix3dricj,Vector3dticj,doubledepth,Vector3duvi,Vector3duvj){Vector3d pts_wRi*(rici*(depth*uvi)tici)Pi;Vector3d pts_cjricj.transpose()*(Rj.transpose()*(pts_w-Pj)-ticj);Vector2d residual(pts_cj/pts_cj.z()).head2()-uvj.head2();doublerxresidual.x();doubleryresidual.y();returnsqrt(rx*rxry*ry);}voidEstimator::outliersRejection(setintremoveIndex){//return;intfeature_index-1;for(autoit_per_id:f_manager.feature){doubleerr0;interrCnt0;it_per_id.used_numit_per_id.feature_per_frame.size();if(it_per_id.used_num4)continue;feature_index;intimu_iit_per_id.start_frame,imu_jimu_i-1;Vector3d pts_iit_per_id.feature_per_frame[0].point;doubledepthit_per_id.estimated_depth;for(autoit_per_frame:it_per_id.feature_per_frame){imu_j;if(imu_i!imu_j){Vector3d pts_jit_per_frame.point;doubletmp_errorreprojectionError(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);errtmp_error;errCnt;//printf(tmp_error %f\n, FOCAL_LENGTH / 1.5 * tmp_error);}// need to rewrite projecton factor.........if(STEREOit_per_frame.is_stereo){Vector3d pts_j_rightit_per_frame.pointRight;if(imu_i!imu_j){doubletmp_errorreprojectionError(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);errtmp_error;errCnt;//printf(tmp_error %f\n, FOCAL_LENGTH / 1.5 * tmp_error);}else{doubletmp_errorreprojectionError(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);errtmp_error;errCnt;//printf(tmp_error %f\n, FOCAL_LENGTH / 1.5 * tmp_error);}}}doubleave_errerr/errCnt;// 得到归一化坐标系下的重投影误差值if(ave_err*FOCAL_LENGTH3)// 乘以焦距讲误差值转换到 像素平面下若大于3个像素点则是outlinerremoveIndex.insert(it_per_id.feature_id);}}