void getObjectPointsAndImagePoints(const Mat& depth1,const Mat& depth2,const vector<KeyPoint>& keyPts1,const vector<KeyPoint>& keyPts2,const vector<DMatch>& matches,vector<Point2f>& imagePoints,vector<Point3f>& objectPoints,vector<Eigen::Vector2d>& imagePoints1,vector<Eigen::Vector2d>& imagePoints2,const CAMERA_INTRINSIC_PARAMETERS& C)
{
for(size_t i=0;i<matches.size();i++)
{
int m = matches[i].queryIdx;
int n = matches[i].trainIdx;
int x1 = keyPts1[m].pt.x;
int y1 = keyPts1[m].pt.y;
Point2f p = keyPts1[m].pt;
int x = keyPts2[n].pt.x;
int y = keyPts2[n].pt.y;
ushort d = ((ushort*) depth1.data)[y*depth1.cols+x];
if(d == 0) continue;
Eigen::Vector2d p1;
p1<<x1,y1;
Eigen::Vector2d p2;
p2<<x,y;
imagePoints1.push_back(p1);
imagePoints2.push_back(p2);
Point3f q(x1,y1,d);
imagePoints.push_back(p);
Point3f q1 = point2dTo3d(q,C);
objectPoints.push_back(q1);
//cout<<"abcd"<<q1<<endl;
}
}
RESULT_OF_PNP estimateMotion( FRAME& frame1, FRAME& frame2, CAMERA_INTRINSIC_PARAMETERS& camera, double good_match_threshold, int min_good_match){
// 匹配描述子
vector<cv::DMatch> matches;
cv::FlannBasedMatcher matcher;
matcher.match(frame1.desp,frame2.desp,matches);
//cout<<"Find total "<<matches.size()<<" matches."<<endl;
/*
// 可视化：显示匹配的特征
cv::Mat imgMatches;
cv::drawMatches(frame1.rgb,frame1.kp,frame2.rgb,frame2.kp,matches,imgMatches);
cv::imshow("matches",imgMatches);
cv::imwrite("../pic/matches.png",imgMatches);
cv::waitKey(0);
*/
// 筛选匹配，把距离太大的去掉
// 这里使用的准则是去掉大于四倍最小距离的匹配
vector<cv::DMatch> goodMatches;
double minDis = 9999;
for(size_t i=0;i<matches.size();i++){
if(matches[i].distance<minDis){
minDis = matches[i].distance;
}
}
for(size_t i=0;i<matches.size();i++){
if(matches[i].distance < good_match_threshold*minDis){
goodMatches.push_back(matches[i]);
}
}
/*
cv::drawMatches(frame1.rgb,frame1.kp,frame2.rgb,frame2.kp,goodMatches,imgMatches);
cv::imshow("good matches",imgMatches);
cv::imwrite("../pic/good_matches.png",imgMatches);
cv::waitKey(0);
*/
// 计算图像间的运动关系
// 关键函数：cv::solvePnPRansac()
// 为调用此函数准备必要的参数
// 第一个帧的三维点
vector<cv::Point3f> pts_obj;
// 第二个帧的图像点
vector<cv::Point2f> pts_img;
RESULT_OF_PNP result;
if(goodMatches.size() < min_good_match){
result.inliers = 0;
}else{
// 显示 good matches
cout<<"good matches = "<<goodMatches.size()<<endl;
for(size_t i=0;i<goodMatches.size();i++){
// query 是第一个, train 是第二个
cv::Point2f p = frame1.kp[goodMatches[i].queryIdx].pt;
// 获取d是要小心！x是向右的，y是向下的，所以y才是行，x是列！
unsigned short d = frame1.depth.ptr<unsigned short>(int(p.y))[int(p.x)];
if(d == 0){
continue;
}
pts_img.push_back(cv::Point2f(frame2.kp[goodMatches[i].trainIdx].pt));
// 将(u,v,d)转成(x,y,z)
cv::Point3f pt(p.x,p.y,d);
cv::Point3f pd = point2dTo3d(pt,camera);
pts_obj.push_back(pd);
}
if(pts_obj.size() == 0){
result.inliers = 0;
return result;
}
double camera_matrix_data[3][3] = {
{camera.fx,0,camera.cx},
{0,camera.fy,camera.cy},
{0,0,1}
};
// 构建相机矩阵
cv::Mat cameraMatrix(3,3,CV_64F,camera_matrix_data);
cv::Mat inliers;
// 求解pnp
cv::solvePnPRansac(pts_obj,pts_img,cameraMatrix,cv::Mat(),result.rvec,result.tvec,false,100,1.0,100,inliers);
result.inliers = inliers.rows;
}
/*
// 画出inliers匹配
vector<cv::DMatch> matchesShow;
for(size_t i=0;i<result.inliers;i++){
matchesShow.push_back(goodMatches[inliers.ptr<int>(i)[0]]);
}
cv::drawMatches(frame1.rgb,frame1.kp,frame2.rgb,frame2.kp,matchesShow,imgMatches);
cv::imshow("inlier matches",imgMatches);
cv::imwrite("../pic/inliers.png",imgMatches);
cv::waitKey(0);
*/
return result;
}
