bool FSolver::checkFundamentalMat(const cv::Mat &P1, const cv::Mat &P2, double reprojection_error, int min_points, double p, int max_its) const { cv::Mat F = findFundamentalMat(P1, P2, reprojection_error, min_points, NULL, false, p, max_its); return !F.empty(); }
/****************************************************************** * 函数功能:使用OpenCV自带的RANSAC寻找内点 * */ void findInliers(std::vector<cv::KeyPoint> &qKeypoints, std::vector<cv::KeyPoint> &objKeypoints, std::vector<cv::DMatch> &matches, const cv::Mat &srcColorImage, const cv::Mat &dstColorImage){ // 获取关键点坐标 std::vector<cv::Point2f> queryCoord; std::vector<cv::Point2f> objectCoord; for( unsigned int i = 0; i < matches.size(); i++){ queryCoord.push_back((qKeypoints[matches[i].queryIdx]).pt); objectCoord.push_back((objKeypoints[matches[i].trainIdx]).pt); } // 使用自定义的函数显示匹配点对 plotMatches(srcColorImage, dstColorImage, queryCoord, objectCoord); // 计算homography矩阵 cv::Mat mask; std::vector<cv::Point2f> queryInliers; std::vector<cv::Point2f> sceneInliers; cv::Mat H = findFundamentalMat(queryCoord, objectCoord, mask, CV_FM_RANSAC); //Mat H = findHomography( queryCoord, objectCoord, CV_RANSAC, 10, mask); int inliers_cnt = 0, outliers_cnt = 0; for (int j = 0; j < mask.rows; j++){ if (mask.at<uchar>(j) == 1){ queryInliers.push_back(queryCoord[j]); sceneInliers.push_back(objectCoord[j]); inliers_cnt++; }else { outliers_cnt++; } } //显示剔除误配点对后的匹配点对 plotMatches(srcColorImage, dstColorImage, queryInliers, sceneInliers); }
Mat Panorama::findFmat(Mat img1, Mat img2, vector<KeyPoint> keypoints1, vector<KeyPoint> keypoints2, vector<DMatch> good_matches){ cout << "Finding the Fundamental Matrix..." << endl << endl; vector<Point2f> points1, points2; for( int i = 0; i < good_matches.size(); i++ ) { points1.push_back( keypoints1[ good_matches[i].queryIdx ].pt ); points2.push_back( keypoints2[ good_matches[i].trainIdx ].pt ); } Mat fundamental_matrix = findFundamentalMat(Mat(points2), Mat(points1), FM_RANSAC, 3, 0.99); return fundamental_matrix; }
Mat GetFundamentalMat(const vector<TrackedPoint> & trackedPoints, vector<TrackedPoint>* inliers, double ransac_max_dist, double ransac_p) { //vector<TrackedPoint> trackedPoints; //vector<matf> H_out; //NormalizePoints2(trackedPoints_, trackedPoints, H_out); const int method = FM_RANSAC; //const int method = FM_LMEDS; const double param1 = ransac_max_dist; const double param2 = ransac_p; #ifdef OPENCV_2_1 matf pts1(trackedPoints.size(), 2), pts2(trackedPoints.size(), 2); for (size_t i = 0; i < trackedPoints.size(); ++i) { pts1(i, 0) = trackedPoints[i].x1; pts2(i, 1) = trackedPoints[i].y1; pts2(i, 0) = trackedPoints[i].x2; pts2(i, 1) = trackedPoints[i].y2; } CvMat pts1cv = pts1; CvMat pts2cv = pts2; matf mat(3, 3); CvMat matcv = mat; Mat_<uchar> status(1, trackedPoints.size()); CvMat statuscv = status; cvFindFundamentalMat(&pts1cv, &pts2cv, &matcv, method, param1, param2, &statuscv); #else Mat_<Vec2f> pts1(trackedPoints.size(), 1), pts2(trackedPoints.size(), 1); for (size_t i = 0; i < trackedPoints.size(); ++i) { pts1(i, 0) = Vec2f(trackedPoints[i].x1, trackedPoints[i].y1); pts2(i, 0) = Vec2f(trackedPoints[i].x2, trackedPoints[i].y2); } vector<unsigned char> status; Mat mat = findFundamentalMat(pts1, pts2, method, param1, param2, status); #endif if (inliers) { inliers->clear(); for (size_t i = 0; i < trackedPoints.size(); ++i) #ifdef OPENCV_2_1 if (status(0, i)) #else if (status[i]) #endif inliers->push_back(trackedPoints[i]); } //return H_out[1].inv().t() * (matf)mat * H_out[0].inv(); return mat; }
// Compute the fundamental matrix given a set of // paired keypoints list void RobustMatcher::computeFundamentalMat(const KeyPointVec &kpts1, const KeyPointVec &kpts2, cv::Mat &fundamentalMat, cv::Mat &inliers_mask) { std::vector<cv::Point2f> kpts1_pts, kpts2_pts; // put keypoints inside vector for (int i = 0; i < kpts1.size(); ++i) { kpts1_pts.push_back(kpts1[i].pt); kpts2_pts.push_back(kpts2[i].pt); } int method = cv::FM_RANSAC; double prob = 0.99; // compute opencv essential mat fundamentalMat = findFundamentalMat(kpts1_pts, kpts2_pts, method, ransac_thresh_, prob); }
void getFundamentalandRemoveOutliers(std::vector< cv::Point2d > &pts1, std::vector< cv::Point2d > &pts2, std::vector< cv::DMatch > &matches, cv::Mat &Fundamental) { std::vector< cv::Point2d > pts1_tmp = pts1; std::vector< cv::Point2d > pts2_tmp = pts2; std::vector< cv::DMatch > new_matches; std::vector<uchar> status(pts1.size()); //Debug // for(int i=0; i < pts1_tmp.size(); i++) // { // std::cout << "Point of image1, " << i << ": " << pts1_tmp[i]; // std::cout << "\t||\tPoint of image2, " << i << ": " << pts2_tmp[i] << '\n'; // } // ==================== Fundamental Matrix computation ==================== double minVal,maxVal; cv::minMaxIdx(pts1_tmp,&minVal,&maxVal); Fundamental = findFundamentalMat(pts1_tmp, pts2_tmp, cv::FM_RANSAC, 0.006 * maxVal, 0.99, status); //threshold from [Snavely07 4.1] // ==================== status vector store inliers as indexes of matches ==================== if ( cv::countNonZero(status) != 0 ) { pts1.clear(); pts2.clear(); for (int i=0; i<status.size(); i++) { if (status[i]) { pts1.push_back(pts1_tmp[i]); pts2.push_back(pts2_tmp[i]); new_matches.push_back(matches[i]); } } // Debug // std::cout << "F keeping " << cv::countNonZero(status) << " / " << status.size() << '\n'; // std::cout << matches.size() << " matches before, " << new_matches.size() << " new matches after Fundamental Matrix\n"; matches.clear(); matches = new_matches; //keep only those points who survived the fundamental matrix } return; }
/* * @function surf_feature_detect_RANSAC SURF特征提取及匹配,RANSAC错误点消除以及物体标记 * @return null * @method SURF feature detector * @method SURF descriptor * @method findFundamentalMat RANSAC错误去除 * @method findHomography 寻找透视变换矩阵 */ void surf_feature_detect_bruteforce_RANSAC_Homography(Mat SourceImg, Mat SceneImg, Mat imageMatches, char* string) { vector<KeyPoint> keyPoints1, keyPoints2; SurfFeatureDetector detector(400); detector.detect(SourceImg, keyPoints1); //标注原图特征点 detector.detect(SceneImg, keyPoints2); //标注场景图特征点 SurfDescriptorExtractor surfDesc; Mat SourceImgDescriptor, SceneImgDescriptor; surfDesc.compute(SourceImg, keyPoints1, SourceImgDescriptor); //描述原图surf特征点 surfDesc.compute(SceneImg, keyPoints2, SceneImgDescriptor); //描述场景图surf特征点 //计算两张图片的特征点匹配数 BruteForceMatcher<L2<float>>matcher; vector<DMatch> matches; matcher.match(SourceImgDescriptor, SceneImgDescriptor, matches); std::nth_element(matches.begin(), matches.begin() + 29 ,matches.end()); matches.erase(matches.begin() + 30, matches.end()); //FLANN匹配检测算法 //vector<DMatch> matches; //DescriptorMatcher *pMatcher = new FlannBasedMatcher; //pMatcher->match(SourceImgDescriptor, SceneImgDescriptor, matches); //delete pMatcher; //keyPoints1 图1提取的关键点 //keyPoints2 图2提取的关键点 //matches 关键点的匹配 int ptCount = (int)matches.size(); Mat p1(ptCount, 2, CV_32F); Mat p2(ptCount, 2, CV_32F); Point2f pt; for(int i = 0; i < ptCount; i++) { pt = keyPoints1[matches[i].queryIdx].pt; p1.at<float>(i, 0) = pt.x; p1.at<float>(i, 1) = pt.y; pt = keyPoints2[matches[i].trainIdx].pt; p2.at<float>(i, 0) = pt.x; p2.at<float>(i, 1) = pt.y; } Mat m_Fundamental; vector<uchar> m_RANSACStatus; m_Fundamental = findFundamentalMat(p1, p2, m_RANSACStatus, FM_RANSAC); int OutlinerCount = 0; for(int i = 0; i < ptCount; i++) { if(m_RANSACStatus[i] == 0) { OutlinerCount++; } } // 计算内点 vector<Point2f> m_LeftInlier; vector<Point2f> m_RightInlier; vector<DMatch> m_InlierMatches; // 上面三个变量用于保存内点和匹配关系 int InlinerCount = ptCount - OutlinerCount; m_InlierMatches.resize(InlinerCount); m_LeftInlier.resize(InlinerCount); m_RightInlier.resize(InlinerCount); InlinerCount = 0; for (int i=0; i<ptCount; i++) { if (m_RANSACStatus[i] != 0) { m_LeftInlier[InlinerCount].x = p1.at<float>(i, 0); m_LeftInlier[InlinerCount].y = p1.at<float>(i, 1); m_RightInlier[InlinerCount].x = p2.at<float>(i, 0); m_RightInlier[InlinerCount].y = p2.at<float>(i, 1); m_InlierMatches[InlinerCount].queryIdx = InlinerCount; m_InlierMatches[InlinerCount].trainIdx = InlinerCount; InlinerCount++; } } // 把内点转换为drawMatches可以使用的格式 vector<KeyPoint> key1(InlinerCount); vector<KeyPoint> key2(InlinerCount); KeyPoint::convert(m_LeftInlier, key1); KeyPoint::convert(m_RightInlier, key2); //显示计算F过后的内点匹配 drawMatches(SourceImg, key1, SceneImg, key2, m_InlierMatches, imageMatches); //drawKeypoints(SourceImg, key1, SceneImg, Scalar(255, 0, 0), DrawMatchesFlags::DRAW_RICH_KEYPOINTS); vector<Point2f> obj; vector<Point2f> scene; for(unsigned int i = 0; i < m_InlierMatches.size(); i++) { obj.push_back(key1[m_InlierMatches[i].queryIdx].pt); //查询图像,即目标图像的特征描述 scene.push_back(key2[m_InlierMatches[i].trainIdx].pt); //模版图像,即场景图像的特征描述 } //求解变换矩阵 //作用同getPerspectiveTransform函数,输入原始图像和变换之后图像中对应的4个点,然后建立起变换映射关系,即变换矩阵 //findHomography直接使用透视平面来找变换公式 Mat H = findHomography(obj, scene, CV_RANSAC); vector<Point2f> obj_corners(4); obj_corners[0] = cvPoint(0, 0); obj_corners[1] = cvPoint(SourceImg.cols, 0); obj_corners[2] = cvPoint(SourceImg.cols, SourceImg.rows); obj_corners[3] = cvPoint(0, SourceImg.rows); vector<Point2f> scene_corners(4); //透视变换,将图片投影到一个新的视平面 //根据以求得的变换矩阵 perspectiveTransform(obj_corners, scene_corners, H); line(imageMatches, scene_corners[0] + Point2f(SourceImg.cols, 0), scene_corners[1] + Point2f(SourceImg.cols, 0), Scalar(0, 0, 255), 4); line(imageMatches, scene_corners[1] + Point2f(SourceImg.cols, 0), scene_corners[2] + Point2f(SourceImg.cols, 0), Scalar(0, 0, 255), 4); line(imageMatches, scene_corners[2] + Point2f(SourceImg.cols, 0), scene_corners[3] + Point2f(SourceImg.cols, 0), Scalar(0, 0, 255), 4); line(imageMatches, scene_corners[3] + Point2f(SourceImg.cols, 0), scene_corners[0] + Point2f(SourceImg.cols, 0), Scalar(0, 0, 255), 4); imshow(string, imageMatches); imwrite("feature_detect.jpg", imageMatches); }
void matchKeypoints(const homography_calc::features& msg){ ROS_INFO("Received an internal message"); cv_bridge::CvImagePtr cv_ptr; cv_ptr = cv_bridge::toCvCopy(msg.keyframe_descriptors, "bgr8"); cv::Mat descriptors_keyframe(cv_ptr->image); cv_ptr = cv_bridge::toCvCopy(msg.motion_descriptors, "bgr8"); cv::Mat descriptors_moving(cv_ptr->image); std::vector<cv::Point2d> raw_kps_keyframe; for (int i = 0; i < msg.keyframe_pts.size(); i++){ cv::Point2d pt(msg.keyframe_pts[i].x, msg.keyframe_pts[i].y); raw_kps_keyframe.push_back(pt); } std::vector<cv::Point2d> raw_kps_moving; for (int i = 0; i < msg.motion_pts.size(); i++){ cv::Point2d pt(msg.motion_pts[i].x, msg.motion_pts[i].y); raw_kps_moving.push_back(pt); } /******************************************** Match the keypoints between the two images so a homography can be made. ********************************************/ cv::BFMatcher matcher; std::vector< std::vector< cv::DMatch > > doubleMatches; std::vector< cv::DMatch > matches; //#if 0 /** Option 1: Straight feature matcher. ***/ //matcher.match( descriptors_moving, descriptors_keyframe, matches ); /** Option 2: knn matcher, which matches the best two points and then finds the best of the two. It may be overkill. **/ matcher.knnMatch( descriptors_moving, descriptors_keyframe, doubleMatches, 2 ); // If the matches are within a certain distance (descriptor space), // then call them a good match. double feature_distance; keypointProcessor::findParamDouble(&feature_distance, "featureDistance", 2.0); for (int i = 0; i < doubleMatches.size(); i++) { if (doubleMatches[i][0].distance < feature_distance * doubleMatches[i][1].distance){ matches.push_back(doubleMatches[i][0]); } } /** End of Option 2 **/ double max_dist = 0; double min_dist = 100; // Regardless of option picked, calculate max and min distances (descriptor space) between keypoints for( int i = 0; i < descriptors_moving.rows; i++ ) { double dist = matches[i].distance; if( dist < min_dist ) min_dist = dist; if( dist > max_dist ) max_dist = dist; } // Store only "good" matches (i.e. whose distance is less than minDistMult*min_dist ) std::vector< cv::DMatch > good_matches; double minDistMult; keypointProcessor::findParamDouble(&minDistMult, "minDistMult", 2.0); for( int i = 0; i < descriptors_moving.rows; i++ ){ if( matches[i].distance < minDistMult * min_dist ) { good_matches.push_back( matches[i] ); } } /******************************************** End of keypoint matching ********************************************/ /******************************************** Copy the keypoints from the matches into their own vectors. This is necessary because I don't want to take out any detected keypoints from the keyframe match, since I use that repeatedly. *********************************************/ // Vectors that contain the matched keypoints. std::vector<cv::Point2d> matched_kps_moved; std::vector<cv::Point2d> matched_kps_keyframe; double max = 0.85; double min = 0.15; for( int i = 0; i < good_matches.size(); i++ ) { matched_kps_moved.push_back( raw_kps_moving[i] ); // Left frame matched_kps_keyframe.push_back( raw_kps_keyframe[i] ); } // OK, so now we have two vectors of matched keypoints. if (! (matched_kps_moved.size() < 4 || matched_kps_keyframe.size() < 4) ){ std::vector<uchar> status; double fMatP1, fMatP2; keypointProcessor::findParamDouble(&fMatP1, "fundMatP1", 2.0); keypointProcessor::findParamDouble(&fMatP2, "fundMatP2", 0.99); findFundamentalMat(matched_kps_moved, matched_kps_keyframe, CV_FM_RANSAC, fMatP1, fMatP2, status); // Erase any points from the matched vectors that don't fit the fundamental matrix // with RANSAC. for (int i = matched_kps_moved.size() - 1; i >= 0; i--){ if (status[i] == 0){ matched_kps_moved.erase(matched_kps_moved.begin() + i); matched_kps_keyframe.erase(matched_kps_keyframe.begin() + i); } } /****************************************** Draw the matches. Not necessary here... ******************************************/ /* std::vector<cv:: DMatch > drawn_matches; std::vector<cv::KeyPoint> k2_moving, k2_keyframe; for (int i = 0; i < matched_kps_moved.size() ; i++){ k2_moving.push_back(cv::KeyPoint(matched_kps_moved[i], 1.f) ); k2_keyframe.push_back(cv::KeyPoint(matched_kps_keyframe[i], 1.f) ); drawn_matches.push_back( cv::DMatch(i, i, 0) ); } cv::Mat img_matches; cv::drawMatches(image, k2_moving, keyframe, k2_keyframe, drawn_matches, img_matches, cv::Scalar::all(-1), cv::Scalar::all(-1), std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS ); cv::waitKey(1); */ cv::Mat moved_mat(matched_kps_moved); // TODO Look here (??) cv::Mat keyframe_mat(matched_kps_keyframe); if (matched_kps_moved.size() >= 4){ // Despite appearances, this isn't unnecessary. It's possible to remove points // via RANSAC and then not have enough to compute the homography (also >=4 points required) homography_calc::matchedPoints kpMsg; kpMsg.keyframe_pts.clear(); for (int i = 0; i < matched_kps_moved.size(); i++){ geometry_msgs::Point keyframePoint; keyframePoint.z = 0; keyframePoint.x = matched_kps_keyframe[i].x; keyframePoint.y = matched_kps_keyframe[i].y; kpMsg.keyframe_pts.push_back(keyframePoint); geometry_msgs::Point motionPoint; motionPoint.z = 0; motionPoint.x = matched_kps_moved[i].x; motionPoint.y = matched_kps_moved[i].y; kpMsg.motion_pts.push_back(motionPoint); } matchedPointsPub.publish(kpMsg); } // geometry_msgs/Point /************************************************** Compute the homography using OpenCV **************************************************/ /* if (matched_kps_moved.size() >= 4){ // Despite appearances, this isn't unnecessary. It's possible to remove points // via RANSAC and then not have enough to compute the homography (also >=4 points required) cv::Mat H = cv::findHomography( moved_mat, keyframe_mat, CV_RANSAC); }*/ } }
void keypointFind(const sensor_msgs::Image::ConstPtr& msg) { ROS_INFO("Received an image1236"); cv_bridge::CvImagePtr cv_ptr; cv_ptr = cv_bridge::toCvCopy(msg, "bgr8"); cv::Mat image(cv_ptr->image); //if (ros::param::has("SURF_hessian") ){ // ros::param::get("SURF_hessian", minHessian); // }else{ // ROS_WARN("SURF_hessian is not set."); //minHessian = 50; //} #if(SURF) cv::FastFeatureDetector detector( minHessian ); // Sift seems much less crowded than Surf. cv::SurfDescriptorExtractor extractor; // SURF descriptor calculation is much faster. // FAST gets the points very quickly. #else /* if (ros::param::has("SIFT_hessian") ){ ros::param::get("SIFT_hessian", minHessian); }else{ ROS_WARN("SIFT_hessian is not set."); minHessian = 400; } cv::SiftFeatureDetector detector( minHessian ); cv::SiftDescriptorExtractor extractor;*/ #endif // If the keyframe is empty, define it, empty the keyframe points buffer, and detect the key points. if (keyframe.empty()){ keyframe = image; raw_kps_keyframe.erase(raw_kps_keyframe.begin(), raw_kps_keyframe.end() ); detector.detect(keyframe, raw_kps_keyframe); extractor.compute(keyframe, raw_kps_keyframe, descriptors_keyframe); exit; // If we have a keyframe, we know the homography is identity (should be) } // If we don't have a brand new keyframe, compute the keypoints on the moving image, // then extract the feature descriptors to use in the matcher. std::vector<cv::KeyPoint> raw_kps_moving; // Optional rotation double rotation_angle; keypointProcessor::findParamDouble(&rotation_angle, "image_rotation_angle", 0.0); keypointProcessor::rotate(image, rotation_angle); detector.detect(image, raw_kps_moving); cv::Mat descriptors_moving; extractor.compute(image, raw_kps_moving, descriptors_moving); // ROS_INFO("Number of descriptors is %d", raw_kps_moving.size() ); std::cout << "Rows " << descriptors_moving.rows << " Cols " << descriptors_moving.cols << std::endl; homography_calc::features kpMsg; kpMsg.keyframe_pts.clear(); for (int i = 0; i < raw_kps_keyframe.size(); i++){ geometry_msgs::Point keyframePoint; keyframePoint.z = 0; keyframePoint.x = raw_kps_keyframe[i].pt.x;; keyframePoint.y = raw_kps_keyframe[i].pt.y;; kpMsg.keyframe_pts.push_back(keyframePoint); } // Not necessarily the same size, yet. for (int i = 0; i < raw_kps_moving.size(); i++){ geometry_msgs::Point motionPoint; motionPoint.z = 0; motionPoint.x = raw_kps_moving[i].pt.x; motionPoint.y = raw_kps_moving[i].pt.y; kpMsg.motion_pts.push_back(motionPoint); } cv_bridge::CvImage cvi_keyframe; cvi_keyframe.header = std_msgs::Header(); cvi_keyframe.encoding = "bgr8"; cvi_keyframe.image = descriptors_keyframe; sensor_msgs::Image im_keyframe; cvi_keyframe.toImageMsg(im_keyframe); kpMsg.keyframe_descriptors = im_keyframe; cv_bridge::CvImage cvi_moving; cvi_moving.header = std_msgs::Header(); cvi_moving.encoding = "bgr8"; cvi_moving.image = descriptors_moving; sensor_msgs::Image im_moving; cvi_moving.toImageMsg(im_moving); kpMsg.motion_descriptors = im_moving; kpMsg.timestamp = msg->header; // Pass through the timestamp of the image rawFeaturesPub.publish(kpMsg); #if 0 /******************************************** Match the keypoints between the two images so a homography can be made. ********************************************/ cv::BFMatcher matcher; std::vector< std::vector< cv::DMatch > > doubleMatches; std::vector< cv::DMatch > matches; //#if 0 /** Option 1: Straight feature matcher. ***/ //matcher.match( descriptors_moving, descriptors_keyframe, matches ); /** Option 2: knn matcher, which matches the best two points and then finds the best of the two. It may be overkill. **/ matcher.knnMatch( descriptors_moving, descriptors_keyframe, doubleMatches, 2 ); // If the matches are within a certain distance (descriptor space), // then call them a good match. double feature_distance; keypointProcessor::findParamDouble(&feature_distance, "featureDistance", 2.0); for (int i = 0; i < doubleMatches.size(); i++) { if (doubleMatches[i][0].distance < feature_distance * doubleMatches[i][1].distance){ matches.push_back(doubleMatches[i][0]); } } /** End of Option 2 **/ double max_dist = 0; double min_dist = 100; // Regardless of option picked, calculate max and min distances (descriptor space) between keypoints for( int i = 0; i < descriptors_moving.rows; i++ ) { double dist = matches[i].distance; if( dist < min_dist ) min_dist = dist; if( dist > max_dist ) max_dist = dist; } // Store only "good" matches (i.e. whose distance is less than minDistMult*min_dist ) std::vector< cv::DMatch > good_matches; double minDistMult; keypointProcessor::findParamDouble(&minDistMult, "minDistMult", 2.0); for( int i = 0; i < descriptors_moving.rows; i++ ){ if( matches[i].distance < minDistMult * min_dist ) { good_matches.push_back( matches[i] ); } } /******************************************** End of keypoint matching ********************************************/ /******************************************** Copy the keypoints from the matches into their own vectors. This is necessary because I don't want to take out any detected keypoints from the keyframe match, since I use that repeatedly. *********************************************/ // Vectors that contain the matched keypoints. std::vector<cv::Point2d> matched_kps_moved; std::vector<cv::Point2d> matched_kps_keyframe; double max = 0.85; double min = 0.15; for( int i = 0; i < good_matches.size(); i++ ) { matched_kps_moved.push_back( raw_kps_moving[ good_matches[i].queryIdx ].pt ); // Left frame matched_kps_keyframe.push_back( raw_kps_keyframe[ good_matches[i].trainIdx ].pt ); } // OK, so now we have two vectors of matched keypoints. if (! (matched_kps_moved.size() < 4 || matched_kps_keyframe.size() < 4) ){ std::vector<uchar> status; double fMatP1, fMatP2; keypointProcessor::findParamDouble(&fMatP1, "fundMatP1", 2.0); keypointProcessor::findParamDouble(&fMatP2, "fundMatP2", 0.99); findFundamentalMat(matched_kps_moved, matched_kps_keyframe, CV_FM_RANSAC, fMatP1, fMatP2, status); // Erase any points from the matched vectors that don't fit the fundamental matrix // with RANSAC. for (int i = matched_kps_moved.size() - 1; i >= 0; i--){ if (status[i] == 0){ matched_kps_moved.erase(matched_kps_moved.begin() + i); matched_kps_keyframe.erase(matched_kps_keyframe.begin() + i); } } /****************************************** Draw the matches. Not necessary here... ******************************************/ /* std::vector<cv:: DMatch > drawn_matches; std::vector<cv::KeyPoint> k2_moving, k2_keyframe; for (int i = 0; i < matched_kps_moved.size() ; i++){ k2_moving.push_back(cv::KeyPoint(matched_kps_moved[i], 1.f) ); k2_keyframe.push_back(cv::KeyPoint(matched_kps_keyframe[i], 1.f) ); drawn_matches.push_back( cv::DMatch(i, i, 0) ); } cv::Mat img_matches; cv::drawMatches(image, k2_moving, keyframe, k2_keyframe, drawn_matches, img_matches, cv::Scalar::all(-1), cv::Scalar::all(-1), std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS ); cv::waitKey(1); */ cv::Mat moved_mat(matched_kps_moved); // TODO Look here (??) cv::Mat keyframe_mat(matched_kps_keyframe); if (matched_kps_moved.size() >= 4){ // Despite appearances, this isn't unnecessary. It's possible to remove points // via RANSAC and then not have enough to compute the homography (also >=4 points required) homography_calc::featurePoints kpMsg; kpMsg.keyframe_pts.clear(); for (int i = 0; i < matched_kps_moved.size(); i++){ geometry_msgs::Point keyframePoint; keyframePoint.z = 0; keyframePoint.x = matched_kps_keyframe[i].x; keyframePoint.y = matched_kps_keyframe[i].y; kpMsg.keyframe_pts.push_back(keyframePoint); geometry_msgs::Point motionPoint; motionPoint.z = 0; motionPoint.x = matched_kps_moved[i].x; motionPoint.y = matched_kps_moved[i].y; kpMsg.motion_pts.push_back(motionPoint); } keypointsPub.publish(kpMsg); } // geometry_msgs/Point /************************************************** Compute the homography using OpenCV **************************************************/ /* if (matched_kps_moved.size() >= 4){ // Despite appearances, this isn't unnecessary. It's possible to remove points // via RANSAC and then not have enough to compute the homography (also >=4 points required) cv::Mat H = cv::findHomography( moved_mat, keyframe_mat, CV_RANSAC); }*/ } #endif }
void StereoCalib(const vector<string>& imagelist, Size boardSize, bool useCalibrated=true, bool showRectified=true) { if( imagelist.size() % 2 != 0 ) { cout << "Error: the image list contains odd (non-even) number of elements\n"; return; } printf("board size: %d x %d", boardSize.width, boardSize.height); bool displayCorners = true; const int maxScale = 2; const float squareSize = 1.f; // Set this to your actual square size // ARRAY AND VECTOR STORAGE: vector<vector<Point2f> > imagePoints[2]; vector<vector<Point3f> > objectPoints; Size imageSize; int i, j, k, nimages = (int)imagelist.size()/2; imagePoints[0].resize(nimages); imagePoints[1].resize(nimages); vector<string> goodImageList; for( i = j = 0; i < nimages; i++ ) { for( k = 0; k < 2; k++ ) { const string& filename = imagelist[i*2+k]; Mat img = imread(filename, 0); if(img.empty()) break; if( imageSize == Size() ) imageSize = img.size(); else if( img.size() != imageSize ) { cout << "The image " << filename << " has the size different from the first image size. Skipping the pair\n"; break; } bool found = false; vector<Point2f>& corners = imagePoints[k][j]; for( int scale = 1; scale <= maxScale; scale++ ) { Mat timg; if( scale == 1 ) timg = img; else resize(img, timg, Size(), scale, scale); found = findChessboardCorners(timg, boardSize, corners, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE); if( found ) { if( scale > 1 ) { Mat cornersMat(corners); cornersMat *= 1./scale; } break; } } if( displayCorners ) { cout << filename << endl; Mat cimg, cimg1; cvtColor(img, cimg, CV_GRAY2BGR); drawChessboardCorners(cimg, boardSize, corners, found); double sf = 640./MAX(img.rows, img.cols); resize(cimg, cimg1, Size(), sf, sf); imshow("corners", cimg1); char c = (char)waitKey(500); if( c == 27 || c == 'q' || c == 'Q' ) //Allow ESC to quit exit(-1); } else putchar('.'); if( !found ) break; cornerSubPix(img, corners, Size(11,11), Size(-1,-1), TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 30, 0.01)); } if( k == 2 ) { goodImageList.push_back(imagelist[i*2]); goodImageList.push_back(imagelist[i*2+1]); j++; } } cout << j << " pairs have been successfully detected.\n"; nimages = j; if( nimages < 2 ) { cout << "Error: too little pairs to run the calibration\n"; return; } imagePoints[0].resize(nimages); imagePoints[1].resize(nimages); objectPoints.resize(nimages); for( i = 0; i < nimages; i++ ) { for( j = 0; j < boardSize.height; j++ ) for( k = 0; k < boardSize.width; k++ ) objectPoints[i].push_back(Point3f(j*squareSize, k*squareSize, 0)); } cout << "Running stereo calibration ...\n"; Mat cameraMatrix[2], distCoeffs[2]; cameraMatrix[0] = Mat::eye(3, 3, CV_64F); cameraMatrix[1] = Mat::eye(3, 3, CV_64F); Mat R, T, E, F; double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1], cameraMatrix[0], distCoeffs[0], cameraMatrix[1], distCoeffs[1], imageSize, R, T, E, F, TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 100, 1e-5), CV_CALIB_FIX_ASPECT_RATIO + CV_CALIB_ZERO_TANGENT_DIST + //CV_CALIB_SAME_FOCAL_LENGTH + CV_CALIB_RATIONAL_MODEL + CV_CALIB_FIX_K3 + CV_CALIB_FIX_K4 + CV_CALIB_FIX_K5); cout << "done with RMS error=" << rms << endl; // CALIBRATION QUALITY CHECK // because the output fundamental matrix implicitly // includes all the output information, // we can check the quality of calibration using the // epipolar geometry constraint: m2^t*F*m1=0 double err = 0; int npoints = 0; vector<Vec3f> lines[2]; for( i = 0; i < nimages; i++ ) { int npt = (int)imagePoints[0][i].size(); Mat imgpt[2]; for( k = 0; k < 2; k++ ) { imgpt[k] = Mat(imagePoints[k][i]); undistortPoints(imgpt[k], imgpt[k], cameraMatrix[k], distCoeffs[k], Mat(), cameraMatrix[k]); computeCorrespondEpilines(imgpt[k], k+1, F, lines[k]); } for( j = 0; j < npt; j++ ) { double errij = fabs(imagePoints[0][i][j].x*lines[1][j][0] + imagePoints[0][i][j].y*lines[1][j][1] + lines[1][j][2]) + fabs(imagePoints[1][i][j].x*lines[0][j][0] + imagePoints[1][i][j].y*lines[0][j][1] + lines[0][j][2]); err += errij; } npoints += npt; } cout << "average reprojection err = " << err/npoints << endl; // save intrinsic parameters FileStorage fs("calib/intrinsics.yml", CV_STORAGE_WRITE); if( fs.isOpened() ) { fs << "M1" << cameraMatrix[0] << "D1" << distCoeffs[0] << "M2" << cameraMatrix[1] << "D2" << distCoeffs[1]; fs.release(); } else cout << "Error: can not save the intrinsic parameters\n"; Mat R1, R2, P1, P2, Q; Rect validRoi[2]; stereoRectify(cameraMatrix[0], distCoeffs[0], cameraMatrix[1], distCoeffs[1], imageSize, R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, 1, imageSize, &validRoi[0], &validRoi[1]); fs.open("calib/extrinsics.yml", CV_STORAGE_WRITE); if( fs.isOpened() ) { fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1" << P1 << "P2" << P2 << "Q" << Q; fs.release(); } else cout << "Error: can not save the intrinsic parameters\n"; // OpenCV can handle left-right // or up-down camera arrangements bool isVerticalStereo = fabs(P2.at<double>(1, 3)) > fabs(P2.at<double>(0, 3)); // COMPUTE AND DISPLAY RECTIFICATION if( !showRectified ) return; Mat rmap[2][2]; // IF BY CALIBRATED (BOUGUET'S METHOD) if( useCalibrated ) { // we already computed everything } // OR ELSE HARTLEY'S METHOD else // use intrinsic parameters of each camera, but // compute the rectification transformation directly // from the fundamental matrix { vector<Point2f> allimgpt[2]; for( k = 0; k < 2; k++ ) { for( i = 0; i < nimages; i++ ) std::copy(imagePoints[k][i].begin(), imagePoints[k][i].end(), back_inserter(allimgpt[k])); } F = findFundamentalMat(Mat(allimgpt[0]), Mat(allimgpt[1]), FM_8POINT, 0, 0); Mat H1, H2; stereoRectifyUncalibrated(Mat(allimgpt[0]), Mat(allimgpt[1]), F, imageSize, H1, H2, 3); R1 = cameraMatrix[0].inv()*H1*cameraMatrix[0]; R2 = cameraMatrix[1].inv()*H2*cameraMatrix[1]; P1 = cameraMatrix[0]; P2 = cameraMatrix[1]; } //Precompute maps for cv::remap() initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R1, P1, imageSize, CV_16SC2, rmap[0][0], rmap[0][1]); initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2, imageSize, CV_16SC2, rmap[1][0], rmap[1][1]); Mat canvas; double sf; int w, h; if( !isVerticalStereo ) { sf = 600./MAX(imageSize.width, imageSize.height); w = cvRound(imageSize.width*sf); h = cvRound(imageSize.height*sf); canvas.create(h, w*2, CV_8UC3); } else { sf = 300./MAX(imageSize.width, imageSize.height); w = cvRound(imageSize.width*sf); h = cvRound(imageSize.height*sf); canvas.create(h*2, w, CV_8UC3); } for( i = 0; i < nimages; i++ ) { for( k = 0; k < 2; k++ ) { Mat img = imread(goodImageList[i*2+k], 0), rimg, cimg; remap(img, rimg, rmap[k][0], rmap[k][1], CV_INTER_LINEAR); cvtColor(rimg, cimg, CV_GRAY2BGR); Mat canvasPart = !isVerticalStereo ? canvas(Rect(w*k, 0, w, h)) : canvas(Rect(0, h*k, w, h)); resize(cimg, canvasPart, canvasPart.size(), 0, 0, CV_INTER_AREA); if( useCalibrated ) { Rect vroi(cvRound(validRoi[k].x*sf), cvRound(validRoi[k].y*sf), cvRound(validRoi[k].width*sf), cvRound(validRoi[k].height*sf)); rectangle(canvasPart, vroi, Scalar(0,0,255), 3, 8); } } if( !isVerticalStereo ) for( j = 0; j < canvas.rows; j += 16 ) line(canvas, Point(0, j), Point(canvas.cols, j), Scalar(0, 255, 0), 1, 8); else for( j = 0; j < canvas.cols; j += 16 ) line(canvas, Point(j, 0), Point(j, canvas.rows), Scalar(0, 255, 0), 1, 8); imshow("rectified", canvas); char c = (char)waitKey(); if( c == 27 || c == 'q' || c == 'Q' ) break; } }