Permalink
Browse files

finalizing multi view

  • Loading branch information...
1 parent 9d24c1a commit 1cc8aaa5f4027a8fd206bd9e019c3fff758cd199 @royshil committed Apr 29, 2012
Showing with 115 additions and 94 deletions.
  1. +1 −1 Common.h
  2. +80 −71 FindCameraMatrices.cpp
  3. +5 −1 FindCameraMatrices.h
  4. +29 −21 MultiCameraPnP.h
View
2 Common.h
@@ -14,7 +14,7 @@
struct CloudPoint {
cv::Point3d pt;
- std::vector<int> idx_in_imgpts_for_img;
+ std::vector<int> imgpt_for_img;
};
void KeyPointsToPoints(const std::vector<cv::KeyPoint>& kps, std::vector<cv::Point2f>& ps);
View
151 FindCameraMatrices.cpp
@@ -36,6 +36,85 @@ bool CheckCoherentRotation(cv::Mat_<double>& R) {
return true;
}
+Mat GetFundamentalMat(const vector<KeyPoint>& imgpts1,
+ const vector<KeyPoint>& imgpts2,
+ vector<KeyPoint>& imgpts1_good,
+ vector<KeyPoint>& imgpts2_good,
+ vector<DMatch>& matches) {
+ //Try to eliminate keypoints based on the fundamental matrix
+ //(although this is not the proper way to do this)
+ vector<uchar> status(imgpts1.size());
+
+#ifdef __SFM__DEBUG__
+ std::vector< DMatch > good_matches_;
+ std::vector<KeyPoint> keypoints_1, keypoints_2;
+#endif
+ // undistortPoints(imgpts1, imgpts1, cam_matrix, distortion_coeff);
+ // undistortPoints(imgpts2, imgpts2, cam_matrix, distortion_coeff);
+ //
+ imgpts1_good.clear(); imgpts2_good.clear();
+
+ vector<KeyPoint> imgpts1_tmp;
+ vector<KeyPoint> imgpts2_tmp;
+ if (matches.size() <= 0) {
+ imgpts1_tmp = imgpts1;
+ imgpts2_tmp = imgpts2;
+ } else {
+ GetAlignedPointsFromMatch(imgpts1, imgpts2, matches, imgpts1_tmp, imgpts2_tmp);
+ // for (unsigned int i=0; i<matches.size(); i++) {
+ // imgpts1_tmp.push_back(imgpts1[matches[i].queryIdx]);
+ // imgpts2_tmp.push_back(imgpts2[matches[i].trainIdx]);
+ // }
+ }
+
+ Mat F;
+ {
+ vector<Point2f> pts1,pts2;
+ KeyPointsToPoints(imgpts1_tmp, pts1);
+ KeyPointsToPoints(imgpts2_tmp, pts2);
+#ifdef __SFM__DEBUG__
+ cout << "pts1 " << pts1.size() << " (orig pts " << imgpts1_good.size() << ")" << endl;
+ cout << "pts2 " << pts2.size() << " (orig pts " << imgpts2_good.size() << ")" << endl;
+#endif
+ F = findFundamentalMat(pts1, pts2, FM_RANSAC, 3.0, 0.99, status);
+ }
+
+ vector<DMatch> new_matches;
+ cout << "keeping " << countNonZero(status) << " / " << status.size() << endl;
+ for (unsigned int i=0; i<status.size(); i++) {
+ if (status[i])
+ {
+ imgpts1_good.push_back(imgpts1_tmp[i]);
+ imgpts2_good.push_back(imgpts2_tmp[i]);
+ new_matches.push_back(DMatch(matches[i].queryIdx,matches[i].trainIdx,1.0));
+#ifdef __SFM__DEBUG__
+ good_matches_.push_back(DMatch(imgpts1_good.size()-1,imgpts1_good.size()-1,1.0));
+ keypoints_1.push_back(imgpts1_tmp[i]);
+ keypoints_2.push_back(imgpts2_tmp[i]);
+#endif
+ }
+ }
+
+ cout << matches.size() << " matches before, " << new_matches.size() << " new matches\n";
+ matches = new_matches; //keep only those points who survived the fundamental matrix
+
+ //-- Draw only "good" matches
+#ifdef __SFM__DEBUG__
+ {
+ Mat img_matches;
+ drawMatches( img_1, keypoints_1, img_2, keypoints_2,
+ good_matches_, img_matches, Scalar::all(-1), Scalar::all(-1),
+ vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
+ //-- Show detected matches
+ imshow( "Good Matches", img_matches );
+ waitKey(0);
+ destroyWindow("Good Matches");
+ }
+#endif
+
+ return F;
+}
+
void FindCameraMatrices(const Mat& K,
const Mat& Kinv,
const vector<KeyPoint>& imgpts1,
@@ -56,80 +135,10 @@ void FindCameraMatrices(const Mat& K,
{
cout << "Find camera matrices...";
double t = getTickCount();
- //Try to eliminate keypoints based on the fundamental matrix
- //(although this is not the proper way to do this)
- vector<uchar> status(imgpts1.size());
-
-#ifdef __SFM__DEBUG__
- std::vector< DMatch > good_matches_;
- std::vector<KeyPoint> keypoints_1, keypoints_2;
-#endif
- // undistortPoints(imgpts1, imgpts1, cam_matrix, distortion_coeff);
- // undistortPoints(imgpts2, imgpts2, cam_matrix, distortion_coeff);
- //
- imgpts1_good.clear(); imgpts2_good.clear();
-
- vector<KeyPoint> imgpts1_tmp;
- vector<KeyPoint> imgpts2_tmp;
- if (matches.size() <= 0) {
- imgpts1_tmp = imgpts1;
- imgpts2_tmp = imgpts2;
- } else {
- GetAlignedPointsFromMatch(imgpts1, imgpts2, matches, imgpts1_tmp, imgpts2_tmp);
-// for (unsigned int i=0; i<matches.size(); i++) {
-// imgpts1_tmp.push_back(imgpts1[matches[i].queryIdx]);
-// imgpts2_tmp.push_back(imgpts2[matches[i].trainIdx]);
-// }
- }
-
- Mat F;
- {
- vector<Point2f> pts1,pts2;
- KeyPointsToPoints(imgpts1_tmp, pts1);
- KeyPointsToPoints(imgpts2_tmp, pts2);
-#ifdef __SFM__DEBUG__
- cout << "pts1 " << pts1.size() << " (orig pts " << imgpts1_good.size() << ")" << endl;
- cout << "pts2 " << pts2.size() << " (orig pts " << imgpts2_good.size() << ")" << endl;
-#endif
- F = findFundamentalMat(pts1, pts2, FM_RANSAC, 3.0, 0.99, status);
- }
-
- vector<DMatch> new_matches;
- cout << "keeping " << countNonZero(status) << " / " << status.size() << endl;
- for (unsigned int i=0; i<status.size(); i++) {
- if (status[i])
- {
- imgpts1_good.push_back(imgpts1_tmp[i]);
- imgpts2_good.push_back(imgpts2_tmp[i]);
- new_matches.push_back(DMatch(matches[i].queryIdx,matches[i].trainIdx,1.0));
-#ifdef __SFM__DEBUG__
- good_matches_.push_back(DMatch(imgpts1_good.size()-1,imgpts1_good.size()-1,1.0));
- keypoints_1.push_back(imgpts1_tmp[i]);
- keypoints_2.push_back(imgpts2_tmp[i]);
-#endif
- }
- }
-
- cout << matches.size() << " matches before, " << new_matches.size() << " new matches\n";
- matches = new_matches; //keep only those points who survived the fundamental matrix
-
- //-- Draw only "good" matches
-#ifdef __SFM__DEBUG__
- {
- Mat img_matches;
- drawMatches( img_1, keypoints_1, img_2, keypoints_2,
- good_matches_, img_matches, Scalar::all(-1), Scalar::all(-1),
- vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
- //-- Show detected matches
- imshow( "Good Matches", img_matches );
- waitKey(0);
- destroyWindow("Good Matches");
- }
-#endif
+ Mat F = GetFundamentalMat(imgpts1,imgpts2,imgpts1_good,imgpts2_good,matches);
//Essential matrix: compute then extract cameras [R|t]
-
Mat_<double> E = K.t() * F * K; //according to HZ (9.12)
//decompose E to P' , HZ (9.19)
//if(false)
View
6 FindCameraMatrices.h
@@ -15,7 +15,11 @@
#undef __SFM__DEBUG__
-void KeyPointsToPoints(const std::vector<cv::KeyPoint>& kps, std::vector<cv::Point2f>& ps);
+cv::Mat GetFundamentalMat(const std::vector<cv::KeyPoint>& imgpts1,
+ const std::vector<cv::KeyPoint>& imgpts2,
+ std::vector<cv::KeyPoint>& imgpts1_good,
+ std::vector<cv::KeyPoint>& imgpts2_good,
+ std::vector<cv::DMatch>& matches);
void FindCameraMatrices(const cv::Mat& K,
const cv::Mat& Kinv,
View
50 MultiCameraPnP.h
@@ -10,6 +10,8 @@
#include "MultiCameraDistance.h"
+#include "Visualization.h"
+
#include "Common.h"
class MultiCameraPnP : public MultiCameraDistance {
@@ -79,10 +81,10 @@ class MultiCameraPnP : public MultiCameraDistance {
std::cout << "pt_set1.size() " << pt_set1.size() << " pt_set2.size() " << pt_set2.size() << " matches.size() " << matches.size() << std::endl;
for (unsigned int i=0; i<pcloud.size(); i++) {
- pcloud[i].idx_in_imgpts_for_img = std::vector<int>(imgs.size(),-1);
+ pcloud[i].imgpt_for_img = std::vector<int>(imgs.size(),-1);
//matches[i] corresponds to pointcloud[i]
- pcloud[i].idx_in_imgpts_for_img[0] = matches[i].queryIdx;
- pcloud[i].idx_in_imgpts_for_img[1] = matches[i].trainIdx;
+ pcloud[i].imgpt_for_img[0] = matches[i].queryIdx;
+ pcloud[i].imgpt_for_img[1] = matches[i].trainIdx;
}
}
std::cout << "triangulation reproj error " << reproj_error << std::endl;
@@ -98,20 +100,14 @@ class MultiCameraPnP : public MultiCameraDistance {
//loop images to incrementally recover more cameras
for (unsigned int i=2; i < imgs.size(); i++) {
+ std::cout << "=========================" << imgs_names[0] << " -> " << imgs_names[i] << "=============================\n";
- //Update the match between i and 0
- FindCameraMatrices(K, Kinv,
- imgpts[0],
+ //Update the match between i and 0 using the Fundamental matrix
+ GetFundamentalMat( imgpts[0],
imgpts[i],
imgpts_good[0],
imgpts_good[i],
- P,
- Pmats[std::make_pair(0,i)],
- matches_matrix[std::make_pair(0,i)],
- tmp_pcloud
-#ifdef __SFM__DEBUG__
- ,imgs[0],imgs[i]
-#endif
+ matches_matrix[std::make_pair(0,i)]
);
//check for matches between i'th frame and 0'th frame (and thus the current cloud)
@@ -121,7 +117,7 @@ class MultiCameraPnP : public MultiCameraDistance {
for (unsigned int pt_img0=0; pt_img0<matches.size(); pt_img0++) {
int matches_img0_queryIdx = matches[pt_img0].queryIdx;
for (unsigned int pcldp=0; pcldp<pcloud.size(); pcldp++) {
- if (matches_img0_queryIdx == pcloud[pcldp].idx_in_imgpts_for_img[0]) {
+ if (matches_img0_queryIdx == pcloud[pcldp].imgpt_for_img[0]) {
//point in cloud
ppcloud.push_back(pcloud[pcldp].pt);
//point in image i
@@ -133,7 +129,7 @@ class MultiCameraPnP : public MultiCameraDistance {
}
- cv::solvePnPRansac(ppcloud, imgPoints, K, distcoeff, rvec, t, true);
+ cv::solvePnPRansac(ppcloud, imgPoints, K, distcoeff, rvec, t, false);
// cv::solvePnP(ppcloud, imgPoints, K, distcoeff, rvec, t, false, CV_EPNP);
Rodrigues(rvec, R);
@@ -157,17 +153,29 @@ class MultiCameraPnP : public MultiCameraDistance {
std::cout << "before triangulation: " << start_i << " after " << pcloud.size() << std::endl;
for (unsigned int j = 0; j<matches.size(); j++) {
- pcloud[start_i + j].idx_in_imgpts_for_img = std::vector<int>(imgs.size(),-1);
+ pcloud[start_i + j].imgpt_for_img = std::vector<int>(imgs.size(),-1);
//matches[i] corresponds to pointcloud[i]
- pcloud[start_i + j].idx_in_imgpts_for_img[0] = matches[j].queryIdx;
- pcloud[start_i + j].idx_in_imgpts_for_img[i] = matches[j].trainIdx;
+ pcloud[start_i + j].imgpt_for_img[0] = matches[j].queryIdx;
+ pcloud[start_i + j].imgpt_for_img[i] = matches[j].trainIdx;
}
-
}
for (unsigned int i=0; i<pcloud.size(); i++) {
- pointcloud.push_back(pcloud[i]);
- pointCloudRGB.push_back(imgs_orig[0].at<cv::Vec3b>(imgpts[0][pcloud[i].idx_in_imgpts_for_img[0]].pt));
+ if (pcloud[i].imgpt_for_img[1] >= 0) {
+// pointCloudRGB.push_back(cv::Vec3b(255,0,0));
+// pointcloud.push_back(pcloud[i]);
+// pointCloudRGB.push_back(imgs_orig[0].at<cv::Vec3b>(imgpts[0][pcloud[i].imgpt_for_img[0]].pt));
+ }
+ if (pcloud[i].imgpt_for_img[2] >= 0) {
+// pointcloud.push_back(pcloud[i]);
+// pointCloudRGB.push_back(imgs_orig[0].at<cv::Vec3b>(imgpts[0][pcloud[i].imgpt_for_img[0]].pt));
+// pointCloudRGB.push_back(cv::Vec3b(0,255,0));
+ }
+ if (pcloud[i].imgpt_for_img[3] >= 0) {
+ pointcloud.push_back(pcloud[i]);
+ pointCloudRGB.push_back(imgs_orig[0].at<cv::Vec3b>(imgpts[0][pcloud[i].imgpt_for_img[0]].pt));
+// pointCloudRGB.push_back(cv::Vec3b(0,0,255));
+ }
}
}

0 comments on commit 1cc8aaa

Please sign in to comment.