7.去畸变、计算图像边界、特征点网格划分
- Frame 跳转 UndistortKeyPoints
- UndistortKeyPoints 结束 返回 Frame
- (疑问疑问疑问)Frame 跳转 ComputeImageBounds
- ComputeImageBounds 结束 返回 Frame
- Frame 跳转 AssignFeaturesToGrid
- AssignFeaturesToGrid 跳转 PosInGrid
- PosInGrid 结束 返回 AssignFeaturesToGrid
- AssignFeaturesToGrid 结束 返回 Frame
- Frame 结束 返回 TrackMonocular
- TrackMonocular 结束 返回 main
提取图片会有广角,需要对提取到的特征点进行去畸变操作
// Step 4 用OpenCV的矫正函数、内参对提取到的特征点进行矫正
UndistortKeyPoints();Frame 跳转 UndistortKeyPoints
void Frame::UndistortKeyPoints()
{
// Step 1 如果第一个畸变参数为0，不需要矫正。第一个畸变参数k1是最重要的，一般不为0，为0的话，说明畸变参数都是0
//变量mDistCoef中存储了opencv指定格式的去畸变参数，格式为：(k1,k2,p1,p2,k3)
if(mDistCoef.at<float>(0)==0.0)
{
mvKeysUn=mvKeys;
return;
}如果第一个畸变参数不为0再进行畸变矫正,首先用临时Mat存储特征点xy坐标
为了能够直接调用opencv的函数来去畸变，需要先将矩阵调整为2通道
处理完就可以调用cv函数undistortPoints了
去畸变完成后再恢复单通道
// Step 2 如果畸变参数不为0，用OpenCV函数进行畸变矫正
// Fill matrix with points
// N为提取的特征点数量，为满足OpenCV函数输入要求，将N个特征点保存在N*2的矩阵中
cv::Mat mat(N,2,CV_32F);
//遍历每个特征点，并将它们的坐标保存到矩阵中
for(int i=0; i<N; i++)
{
//然后将这个特征点的横纵坐标分别保存
mat.at<float>(i,0)=mvKeys[i].pt.x;
mat.at<float>(i,1)=mvKeys[i].pt.y;
}
// Undistort points
// 函数reshape(int cn,int rows=0) 其中cn为更改后的通道数，rows=0表示这个行将保持原来的参数不变
//为了能够直接调用opencv的函数来去畸变，需要先将矩阵调整为2通道（对应坐标x,y）
mat=mat.reshape(2);
cv::undistortPoints(
mat, //输入的特征点坐标
mat, //输出的校正后的特征点坐标覆盖原矩阵
mK, //相机的内参数矩阵
mDistCoef, //相机畸变参数矩阵
cv::Mat(), //一个空矩阵，对应为函数原型中的R
mK); //新内参数矩阵，对应为函数原型中的P
//调整回只有一个通道，回归我们正常的处理方式
mat=mat.reshape(1);去畸变完成后,将特征点赋值 去畸变坐标 后存储到新的容器mvKeysUn中
// Fill undistorted keypoint vector
// Step 存储校正后的特征点
mvKeysUn.resize(N);
//遍历每一个特征点
for(int i=0; i<N; i++)
{
//根据索引获取这个特征点
//注意之所以这样做而不是直接重新声明一个特征点对象的目的是，能够得到源特征点对象的其他属性
cv::KeyPoint kp = mvKeys[i];
//读取校正后的坐标并覆盖老坐标
kp.pt.x=mat.at<float>(i,0);
kp.pt.y=mat.at<float>(i,1);
mvKeysUn[i]=kp;
}
}UndistortKeyPoints 结束 返回 Frame
单目没有右图和深度,输入参数-1表示没有信息
初始化地图点,内容为空,备用
// Set no stereo information
// 由于单目相机无法直接获得立体信息，所以这里要给右图像对应点和深度赋值-1表示没有相关信息
mvuRight = vector<float>(N,-1);
mvDepth = vector<float>(N,-1);
// 初始化本帧的地图点
mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
// 记录地图点是否为外点，初始化均为外点false
mvbOutlier = vector<bool>(N,false);如果是第一帧,那么mbInitialComputations是true状态,进入if
计算去畸变图像边界
更新去畸变相机内参
// This is done only for the first Frame (or after a change in the calibration)
// Step 5 计算去畸变后图像边界，将特征点分配到网格中。这个过程一般是在第一帧或者是相机标定参数发生变化之后进行
if(mbInitialComputations)
{
// 计算去畸变后图像的边界
ComputeImageBounds(imGray);(疑问疑问疑问)Frame 跳转 ComputeImageBounds
如果第一个畸变参数不为0,则进行去畸变
图像四个顶点坐标赋值给mat矩阵:
然后对这四个顶点进行去畸变??
void Frame::ComputeImageBounds(const cv::Mat &imLeft)
{
// 如果畸变参数不为0，用OpenCV函数进行畸变矫正
if(mDistCoef.at<float>(0)!=0.0)
{
// 保存矫正前的图像四个边界点坐标： (0,0) (cols,0) (0,rows) (cols,rows)
cv::Mat mat(4,2,CV_32F);
mat.at<float>(0,0)=0.0; //左上
mat.at<float>(0,1)=0.0;
mat.at<float>(1,0)=imLeft.cols; //右上
mat.at<float>(1,1)=0.0;
mat.at<float>(2,0)=0.0; //左下
mat.at<float>(2,1)=imLeft.rows;
mat.at<float>(3,0)=imLeft.cols; //右下
mat.at<float>(3,1)=imLeft.rows;
// Undistort corners
// 和前面校正特征点一样的操作，将这几个边界点作为输入进行校正
mat=mat.reshape(2);
cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);
mat=mat.reshape(1)矫正后图像内凹，坐标也内凹，要在图形外侧加一个边框
//校正后的四个边界点已经不能够围成一个严格的矩形，因此在这个四边形的外侧加边框作为坐标的边界
mnMinX = min(mat.at<float>(0,0),mat.at<float>(2,0));//左上和左下横坐标最小的
mnMaxX = max(mat.at<float>(1,0),mat.at<float>(3,0));//右上和右下横坐标最大的
mnMinY = min(mat.at<float>(0,1),mat.at<float>(1,1));//左上和右上纵坐标最小的
mnMaxY = max(mat.at<float>(2,1),mat.at<float>(3,1));//左下和右下纵坐标最小的
}
else
{
// 如果畸变参数为0，就直接获得图像边界
mnMinX = 0.0f;
mnMaxX = imLeft.cols;
mnMinY = 0.0f;
mnMaxY = imLeft.rows;
}
}ComputeImageBounds 结束 返回 Frame
返回到Frame后继续计算可能用到的参数
变量含义：
- mfGridElementWidthInv = 每个图像栅格列数 ÷ 图像宽度 = 图像每行有多少栅格 的倒数
- mfGridElementHeightInv = 每个图像栅格行数 ÷ 图像高度 = 图像每列有多少栅格 的倒数
// 表示一个图像像素相当于多少个图像网格列（宽）
mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);
// 表示一个图像像素相当于多少个图像网格行（高）
mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);
//给类的静态成员变量复制
fx = K.at<float>(0,0);
fy = K.at<float>(1,1);
cx = K.at<float>(0,2);
cy = K.at<float>(1,2);
// 猜测是因为这种除法计算需要的时间略长，所以这里直接存储了这个中间计算结果
invfx = 1.0f/fx;
invfy = 1.0f/fy;
//特殊的初始化过程完成，标志复位
mbInitialComputations=false;
}
//计算 basline
mb = mbf/fx;
// 将特征点分配到图像网格中
AssignFeaturesToGrid();
}Frame 跳转 AssignFeaturesToGrid
该函数的目的是将特征点分配到图像网格中
- N是总共的特征点数
- FRAME_GRID_COLS和FRAME_GRID_ROWS 是 图像包含的栅格列数和行数。乘起来就是图像包含总栅格数。
遍历所有栅格，为每个栅格预留0.5N的空间。
void Frame::AssignFeaturesToGrid()
{
// Step 1 给存储特征点的网格数组 Frame::mGrid 预分配空间
// ? 这里0.5 是为什么？节省空间？
// FRAME_GRID_COLS = 64，FRAME_GRID_ROWS=48
int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS);
//开始对mGrid这个二维数组中的每一个vector元素遍历并预分配空间
for(unsigned int i=0; i<FRAME_GRID_COLS;i++)
for (unsigned int j=0; j<FRAME_GRID_ROWS;j++)
mGrid[i][j].reserve(nReserve);分配完空间后，下面用PosInGrid函数计算特征点所在的坐标，返回到nGridPosX, nGridPosY中
然后将特征点序号i放到对应坐标的栅格容器mGrid中
// Step 2 遍历每个特征点，将每个特征点在mvKeysUn中的索引值放到对应的网格mGrid中
for(int i=0;i<N;i++)
{
//从类的成员变量中获取已经去畸变后的特征点
const cv::KeyPoint &kp = mvKeysUn[i];
//存储某个特征点所在网格的网格坐标，nGridPosX范围：[0,FRAME_GRID_COLS], nGridPosY范围：[0,FRAME_GRID_ROWS]
int nGridPosX, nGridPosY;
// 计算某个特征点所在网格的网格坐标，如果找到特征点所在的网格坐标，记录在nGridPosX,nGridPosY里，返回true，没找到返回false
if(PosInGrid(kp,nGridPosX,nGridPosY))
//如果找到特征点所在网格坐标，将这个特征点的索引添加到对应网格的数组mGrid中
mGrid[nGridPosX][nGridPosY].push_back(i);
}
}AssignFeaturesToGrid 跳转 PosInGrid
posX posY表示特征点位于第几个栅格
前面提到过
mfGridElementWidthInv和mfGridElementHeightInv分别是每列和每行包含的栅格数量的倒数
判断如果栅格行列数不符合标准则返回false，符合标准返回true。
bool Frame::PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY)
{
// 计算特征点x,y坐标落在哪个网格内，网格坐标为posX，posY
// mfGridElementWidthInv=(FRAME_GRID_COLS)/(mnMaxX-mnMinX);
// mfGridElementHeightInv=(FRAME_GRID_ROWS)/(mnMaxY-mnMinY);
posX = round((kp.pt.x-mnMinX)*mfGridElementWidthInv);
posY = round((kp.pt.y-mnMinY)*mfGridElementHeightInv);
//Keypoint's coordinates are undistorted, which could cause to go out of the image
// 因为特征点进行了去畸变，而且前面计算是round取整，所以有可能得到的点落在图像网格坐标外面
// 如果网格坐标posX，posY超出了[0,FRAME_GRID_COLS] 和[0,FRAME_GRID_ROWS]，表示该特征点没有对应网格坐标，返回false
if(posX<0 || posX>=FRAME_GRID_COLS || posY<0 || posY>=FRAME_GRID_ROWS)
return false;
// 计算成功返回true
return true;
}PosInGrid 结束 返回 AssignFeaturesToGrid
AssignFeaturesToGrid 结束 返回 Frame
处理完特征点和畸变后，开启跟踪线程Track，返回当前位姿后Frame就结束了
// Step 3 ：跟踪
Track();
//返回当前帧的位姿
return mCurrentFrame.mTcw.clone();
}Frame 结束 返回 TrackMonocular
设置一些变量后退出
unique_lock<mutex> lock2(mMutexState);
mTrackingState = mpTracker->mState;
mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
return Tcw;
}TrackMonocular 结束 返回 main
#ifdef COMPILEDWITHC11
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#else
std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif
double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
vTimesTrack[ni]=ttrack;
// Wait to load the next frame
double T=0;
if(ni<nImages-1)
T = vTimestamps[ni+1]-tframe;
else if(ni>0)
T = tframe-vTimestamps[ni-1];
if(ttrack<T)
usleep((T-ttrack)*1e6);
}
// Stop all threads
SLAM.Shutdown();
// Tracking time statistics
sort(vTimesTrack.begin(),vTimesTrack.end());
float totaltime = 0;
for(int ni=0; ni<nImages; ni++)
{
totaltime+=vTimesTrack[ni];
}
cout << "-------" << endl << endl;
cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
cout << "mean tracking time: " << totaltime/nImages << endl;
// Save camera trajectory
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
return 0;
}单目结束。
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
