LK光流特征追踪计算PNP位姿
角点提取及光流追踪
std::vector<cv::Point2f> prepoint, nextpoint;//前一帧角点，当前帧角点
std::vector<float> err;
std::vector<uchar> state;//追踪成功标志 追踪成功=1 失败=0
cv::goodFeaturesToTrack(imlast, prepoint, 1000, 0.01, 8, cv::Mat(), 3, true, 0.04);//提取上一帧中的角点存在prepoint中
cv::cornerSubPix(imlast, prepoint, cv::Size(10, 10), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));//亚像素提取
cv::calcOpticalFlowPyrLK(imlast, imRGB, prepoint, nextpoint, state, err, cv::Size(22, 22), 5, cv::TermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.01));//根据prepointLK光流追踪 得到结果存放在nextpoint
/*** 根据光流追踪的结果保存成功追踪的角点*/
std::vector<cv::Point2f> match_pre,match_cur;//保存匹配成功的点
int limit_edge_corner=5;//搜索边界
int sta_label=0;
for(int i=0;i<state.size();i++)
{
int x1 = prepoint[i].x, y1 = prepoint[i].y;
int x2 = nextpoint[i].x, y2 = nextpoint[i].y;
if ((x1 < limit_edge_corner || x1 >= imRGB.cols - limit_edge_corner || x2 < limit_edge_corner || x2 >= imRGB.cols - limit_edge_corner||
y1 < limit_edge_corner || y1 >= imRGB.rows - limit_edge_corner || y2 < limit_edge_corner || y2 >= imRGB.rows - limit_edge_corner))
//如果点的坐标在图像边界框内 舍弃掉
{
state[i] = 0;
continue;
}
sta_label=(int)segMask.ptr<uchar>((int)nextpoint[i].y)[(int)nextpoint[i].x];//根据实例分割 判断点是否在先验动态Mask内
//if(state[i]&&sta_label==0) {
if(state[i]) {//追踪成功则保存到match数组内
match_pre.push_back(prepoint[i]);
match_cur.push_back(nextpoint[i]);
}
}
将提取的角点由2D坐标恢复为3D坐标
///pnp计算位姿
int N=match_cur.size();
// construct input
std::vector<cv::Point2f> cur_2d(N);//当前帧的2D像素坐标
std::vector<cv::Point3f> pre_3d(N);//前一帧的2D像素坐标
std::vector<cv::Point3f> cur_3d(N);//世界坐标系下的3D点
for(int i=0;i<N;i++) {
float z1 = imDlast.at<float>(match_pre[i].y, match_pre[i].x);//前一帧的深度
float z2 = imdepth.at<float>(match_cur[i].y, match_cur[i].x);//当前帧的深度
if (z1 > 0 && z2 > 0) {//深度都为有效值 >=0
float x = (match_pre[i].x - mCurrentFrame.cx) * z1 * mCurrentFrame.invfx;//将像素坐标转化为非归一化上一帧的相机坐标系
float y = (match_pre[i].y - mCurrentFrame.cy) * z1 * mCurrentFrame.invfy;
//恢复为上一帧中的相机坐标系坐标
cv::Point3f tmp_3d;
cv::Mat x3Dp = (cv::Mat_<float>(3, 1) << x, y, z1);
///这里得到的相机坐标为上一帧的相机坐标系下观测到的3D点坐标
const cv::Mat Rlw = mLastFrame.mTcw.rowRange(0,3).colRange(0,3);
const cv::Mat Rwl = Rlw.t();
const cv::Mat tlw = mLastFrame.mTcw.rowRange(0,3).col(3);
const cv::Mat twl = -Rlw.t()*tlw;
//这里相当于求出了相机到世界的变换Twc，从而把上一帧中的相机坐标系3D点转换成了世界坐标系下的3D点 如果继续用TP 得到的是上一帧的3D在上一帧中的坐标系的点
//x3Dp=Rwl*x3Dp+twl;
x3Dp = mLastFrame.mRwc * x3Dp + mLastFrame.mOw;//这两行的转换是相同的
tmp_3d.x = x3Dp.at<float>(0);
tmp_3d.y = x3Dp.at<float>(1);
tmp_3d.z = x3Dp.at<float>(2);
pre_3d[i] = tmp_3d;
cur_2d[i].x = match_cur[i].x;
cur_2d[i].y = match_cur[i].y;
}
}
根据匹配好的3D点和2D点进行PNP位姿求解
// output
cv::Mat Rvec(3, 1, CV_64FC1);//旋转向量
cv::Mat Tvec(3, 1, CV_64FC1);//平移向量
cv::Mat d(3, 3, CV_64FC1);//旋转矩阵
cv::Mat inliers;//内点数
//Ransac优化
int iter_num = 500;//迭代次数
double reprojectionError = 0.4, confidence = 0.98; // 0.5 0.3 PNP中RANSAC判断内点的重投影误差阈值 小于0.4视为内点
cv::Mat camera_mat(3, 3, CV_64FC1);//内参矩阵
cv::Mat distCoeffs = cv::Mat::zeros(1, 4, CV_64FC1);
camera_mat.at<double>(0, 0) = mCurrentFrame.mK.at<float>(0,0);
camera_mat.at<double>(1, 1) = mCurrentFrame.mK.at<float>(1,1);
camera_mat.at<double>(0, 2) = mCurrentFrame.mK.at<float>(0,2);
camera_mat.at<double>(1, 2) = mCurrentFrame.mK.at<float>(1,2);
camera_mat.at<double>(2, 2) = 1.0;
cv::solvePnPRansac(pre_3d, cur_2d, camera_mat, distCoeffs, Rvec, Tvec, false,
iter_num, reprojectionError, confidence, inliers, cv::SOLVEPNP_P3P); // AP3P EPNP P3P ITERATIVE DLS
cv::Rodrigues(Rvec, d);//罗德里格斯公式转换成旋转矩阵
重投影误差计算
cv::Mat Mod = cv::Mat::eye(4,4,CV_32F);//初始估计模型 欧式变换矩阵T 包含旋转矩阵和平移向量
// assign the result to current pose
Mod.at<float>(0,0) = d.at<double>(0,0); Mod.at<float>(0,1) = d.at<double>(0,1); Mod.at<float>(0,2) = d.at<double>(0,2); Mod.at<float>(0,3) = Tvec.at<double>(0,0);
Mod.at<float>(1,0) = d.at<double>(1,0); Mod.at<float>(1,1) = d.at<double>(1,1); Mod.at<float>(1,2) = d.at<double>(1,2); Mod.at<float>(1,3) = Tvec.at<double>(1,0);
Mod.at<float>(2,0) = d.at<double>(2,0); Mod.at<float>(2,1) = d.at<double>(2,1); Mod.at<float>(2,2) = d.at<double>(2,2); Mod.at<float>(2,3) = Tvec.at<double>(2,0);
// calculate the re-projection error
std::vector<int> MM_inlier;//自己手动计算的内点数
cv::Mat MotionModel;//ORB中的匀速运动模型
if (mVelocity.empty())
MotionModel = cv::Mat::eye(4,4,CV_32F)*mLastFrame.mTcw;
else
MotionModel = mVelocity*mLastFrame.mTcw;
for (int i = 0; i < pre_3d.size(); ++i) {
if (pre_3d[i].z > 0&&(cur_2d[i].x!=0&&cur_2d[i].y!=0)) {//遍历每个匹配的有效的3D-2D点 深度大于0 且 当前像素点不为0
const cv::Mat x3D = (cv::Mat_<float>(3, 1) << pre_3d[i].x, pre_3d[i].y, pre_3d[i].z);//point3f转换为cv::Mat
const cv::Mat x3D_c =MotionModel.rowRange(0, 3).colRange(0, 3) * x3D + MotionModel.rowRange(0, 3).col(3);
//R*x+t 从世界坐标系转换为当前帧的相机坐标系
cout<<x3D_c<<endl;
cout<<"u="<<cur_2d[i].x<<"v="<<cur_2d[i].y<<endl;
if(x3D_c.at<float>(2)<0)
continue;
const float xc = x3D_c.at<float>(0);
const float yc = x3D_c.at<float>(1);
const float invzc = 1.0 / x3D_c.at<float>(2);
const float u = mCurrentFrame.fx * xc * invzc + mCurrentFrame.cx;//相机坐标系转化为图像像素坐标
const float v = mCurrentFrame.fy * yc * invzc + mCurrentFrame.cy;
const float u_ = cur_2d[i].x - u;
const float v_ = cur_2d[i].y - v;
const float Rpe = std::sqrt(u_ * u_ + v_ * v_);
mCurrentFrame.mvMatch.push_back(cur_2d[i]);
mCurrentFrame.mvRpe.push_back(Rpe);
if (Rpe < reprojectionError) {
MM_inlier.push_back(i);
}
}
}
cv::Mat output;
cout<<"PNPinliers:"<<inliers.rows<<endl<<"MM_inliers:"<<MM_inlier.size()<<endl;
调试输出是
PNPinliers:221
MM_inliers:5
利用cv::projector函数计算重投影误差也得不到对应的模型内点数目。平均重投影误差大小在5左右。
也就是PNP内部计算的重投影误差小于0.4的点是221个
我用PNP位姿手动计算重投影误差小于0.4的点是5个
问题不知道出在哪里
77
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
