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;
}
}
