Skip to content

Commit

Permalink
finalizing multi view
Browse files Browse the repository at this point in the history
  • Loading branch information
@royshil
royshil committed Apr 29, 2012
1 parent 9d24c1a commit 1cc8aaa
Show file tree
Hide file tree
Showing 4 changed files with 115 additions and 94 deletions.
2 changes: 1 addition & 1 deletion Common.h
View file
Expand Up @@ -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);
Expand Down
151 changes: 80 additions & 71 deletions FindCameraMatrices.cpp
View file
Expand Up @@ -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,
Expand All @@ -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)
Expand Down
6 changes: 5 additions & 1 deletion FindCameraMatrices.h
View file
Expand Up @@ -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,
Expand Down
50 changes: 29 additions & 21 deletions MultiCameraPnP.h
View file
Expand Up @@ -10,6 +10,8 @@

#include "MultiCameraDistance.h"

#include "Visualization.h"

#include "Common.h"

class MultiCameraPnP : public MultiCameraDistance {
Expand Down Expand Up @@ -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;
Expand All @@ -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)
Expand All @@ -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
Expand All @@ -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);
Expand All @@ -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));
}
}

}
Expand Down

0 comments on commit 1cc8aaa

Please sign in to comment.