// Reconstruction function for API
void
reconstruct(const InputArrayOfArrays points2d, OutputArrayOfArrays projection_matrices, OutputArray points3d,
bool is_projective, bool has_outliers, bool is_sequence)
{
int nviews = points2d.total();
cv::Mat F;
// OpenCV data types
std::vector<cv::Mat> pts2d;
points2d.getMatVector(pts2d);
int depth = pts2d[0].depth();
// Projective reconstruction
if (is_projective)
{
// Two view reconstruction
if (nviews == 2)
{
// Get fundamental matrix
fundamental8Point(pts2d[0], pts2d[1], F, has_outliers);
// Get Projection matrices
cv::Mat P, Pp;
projectionsFromFundamental(F, P, Pp);
projection_matrices.create(2, 1, depth);
P.copyTo(projection_matrices.getMatRef(0));
Pp.copyTo(projection_matrices.getMatRef(1));
// Triangulate and find 3D points using inliers
triangulatePoints(points2d, projection_matrices, points3d);
}
}
// Affine reconstruction
else
{
// Two view reconstruction
if (nviews == 2)
{
}
else
{
}
}
}
void AAM::triangulateMeanPoints()
{
cout<<"in triangulation mean function"<<endl;
if (!this->meanShapePointsOrder.empty())
this->meanShapePointsOrder.clear();
for(int i=0; i<meanShape.cols/2; i++)
{
//pair<int, int> point=make_pair(meanShape.at<int>(0, 2*i), meanShape.at<int>(0, 2*i+1));
Point point(meanShape.at<int>(0, 2*i), meanShape.at<int>(0, 2*i+1));
this->meanShapePointsOrder[point]=i;
this->meanPoints.push_back(Point(meanShape.at<int>(0, 2*i), meanShape.at<int>(0, 2*i+1)));
}
cout<<"mean points: "<<this->meanPoints<<endl;
this->meanShapeTriangulation=triangulatePoints(this->meanPoints);
this->displayTriangulation(this->meanShapeTriangulation);
for (std::map<Point,unsigned int, comparePoint>::iterator it=this->meanShapePointsOrder.begin(); it!=this->meanShapePointsOrder.end(); ++it)
std::cout << it->first << " => " << it->second << '\n';
}
void reconstruct(Mat& K, Mat& R, Mat& T, vector<Point2f>& p1, vector<Point2f>& p2, Mat& structure)
{
Mat proj1(3, 4, CV_32FC1);
Mat proj2(3, 4, CV_32FC1);
proj1(Range(0, 3), Range(0, 3)) = Mat::eye(3, 3, CV_32FC1);
proj1.col(3) = Mat::zeros(3, 1, CV_32FC1);
R.convertTo(proj2(Range(0, 3), Range(0, 3)), CV_32FC1);
T.convertTo(proj2.col(3), CV_32FC1);
Mat fK;
K.convertTo(fK, CV_32FC1);
proj1 = fK*proj1;
proj2 = fK*proj2;
triangulatePoints(proj1, proj2, p1, p2, structure);
}
bool ReconstructionHandler::triangulatePointsBetweenViews(
int workingView,
int olderView,
std::vector<struct CloudPoint>& newTriangulated,
std::vector<int>& addToCloud,
int numViews)
{
LOG (Debug, "Triangulating image ", workingView, " and ", olderView);
cv::Matx34d P0 = sceneModel->poseMats[olderView];
cv::Matx34d P1 = sceneModel->poseMats[workingView];
// Get aligned matched 2D points from the two views
std::vector<cv::KeyPoint> pt_set1,pt_set2;
std::vector<cv::DMatch> matches = sceneModel->getMatches()[std::make_pair(olderView,workingView)];
for (unsigned int i=0; i<matches.size(); i++) {
pt_set1.push_back((sceneModel->getKeypoints())[olderView][matches[i].queryIdx]);
pt_set2.push_back((sceneModel->getKeypoints())[workingView][matches[i].trainIdx]);
}
// Adding more triangulated points to general cloud
// Why not 4 triangulations as in stereo initialization?
double reprojectionError = triangulatePoints(pt_set1, pt_set2, sceneModel->K, sceneModel->Kinv, sceneModel->distortionCoefficients, P0, P1, newTriangulated);
LOG(Info, "Triangulation reprojection error ", reprojectionError);
if (newTriangulated.size()==0){
LOG(Warn, "Zero points were triangulated");
return false;
}
std::vector<uchar> trig_status;
if(!testTriangulation(newTriangulated, P0, trig_status) ||
!testTriangulation(newTriangulated, P1, trig_status)) {
LOG(Warn, "Triangulation did not succeed");
return false;
}
if(reprojectionError > 50.0) {
LOG(Warn, "Reprojection error too high - triangulation failed.");
return false;
}
// Filter out outlier points with high reprojection
std::vector<double> reprojectionErrors;
for(int i=0;i<newTriangulated.size();i++) {
reprojectionErrors.push_back(newTriangulated[i].reprojection_error);
}
std::sort(reprojectionErrors.begin(),reprojectionErrors.end());
// Get the 80% precentile
double reprj_err_cutoff = reprojectionErrors[4 * reprojectionErrors.size() / 5] * 2.4; //threshold from Snavely07 4.2
std::vector<CloudPoint> newTriangulatedFiltered;
std::vector<cv::DMatch> newMatches;
for(int i=0;i<newTriangulated.size();i++) {
if(trig_status[i] == 0)
continue; //point was not in front of either camera
if(newTriangulated[i].reprojection_error > 16.0) {
continue; //reject point as it is inaccurate
}
if(newTriangulated[i].reprojection_error < 4.0 ||
newTriangulated[i].reprojection_error < reprj_err_cutoff)
{
newTriangulatedFiltered.push_back(newTriangulated[i]);
newMatches.push_back(matches[i]);
}
else
{
continue;
}
}
LOG(Info, " Filtered out ", (newTriangulated.size() - newTriangulatedFiltered.size()), " high-error points");
// Check if all points are filtered out
if(newTriangulatedFiltered.size() <= 0){
LOG(Warn, "All points have been filtered out.");
return false;
}
// Use filtered points now
newTriangulated.clear();
newTriangulated.insert(newTriangulated.begin(), newTriangulatedFiltered.begin(), newTriangulatedFiltered.end());
// Use filtered matches
matches = newMatches;
// Update the matches storage
sceneModel->getMatches()[std::make_pair(olderView,workingView)] = newMatches; //just to make sure, remove if unneccesary
sceneModel->getMatches()[std::make_pair(workingView,olderView)] = featureHandler->flipMatches(newMatches);
// Now determine which points should be added to the cloud based on which already exist
addToCloud.clear();
addToCloud.resize(newTriangulated.size(),1);
int foundOtherViewsCount = 0;
// Scan new triangulated points, if they were already triangulated before - strengthen cloud
// If not, mark them to be added
for (int j = 0; j<newTriangulated.size(); j++) {
newTriangulated[j].imgpt_for_img.resize(numViews,-1);
//matches[j] corresponds to new_triangulated[j]
//matches[j].queryIdx = point in <olderView>
//matches[j].trainIdx = point in <workingView>
newTriangulated[j].imgpt_for_img[olderView] = matches[j].queryIdx; //2D reference to <olderView>
newTriangulated[j].imgpt_for_img[workingView] = matches[j].trainIdx; //2D reference to <workingView>
bool foundInOtherView = false;
for (unsigned int view = 0; view < numViews; view++) {
if(view != olderView) {
// Look for points in <view> that match to points in <workingView>
std::vector<cv::DMatch> submatches = sceneModel->getMatches()[std::make_pair(view,workingView)];
for (unsigned int ii = 0; ii < submatches.size(); ii++) {
if (submatches[ii].trainIdx == matches[j].trainIdx && !foundInOtherView)
{
// Point was already found in <view> - strengthen it in the known cloud, if it exists there
for (unsigned int pt3d=0; pt3d<sceneModel->reconstructedPts.size(); pt3d++) {
if (sceneModel->reconstructedPts[pt3d].imgpt_for_img[view] == submatches[ii].queryIdx)
{
//sceneModel->reconstructedPts[pt3d] - a point that has 2d reference in <view>
{
// As this point has already been reconstructed in <view>, update 2D match references
sceneModel->reconstructedPts[pt3d].imgpt_for_img[workingView] = matches[j].trainIdx;
sceneModel->reconstructedPts[pt3d].imgpt_for_img[olderView] = matches[j].queryIdx;
foundInOtherView = true;
addToCloud[j] = 0;
}
}
}
}
}
}
}
{
if (foundInOtherView) {
// If point was found in any of the previous views
foundOtherViewsCount++;
} else {
addToCloud[j] = 1;
}
}
}
LOG(Debug, "Number of points found in other views: ", foundOtherViewsCount, "/", newTriangulated.size());
LOG(Debug, "Adding new points from triangulation: ", cv::countNonZero(addToCloud));
return true;
}
void EstimateCameraPose::calcCameraPoseFromE()
{
cv::SVD svd(E, CV_SVD_MODIFY_A);
cv::Mat svd_u = svd.u;
cv::Mat svd_vt = svd.vt;
cv::Mat svd_w = svd.w;
cv::Matx33d W(0, -1, 0, 1, 0, 0, 0, 0, 1);
cv::Matx33d W_t;
cv::transpose(W, W_t);
cv::Mat_<double> R = svd_u * cv::Mat(W) * svd_vt; //HZ 9.19
cv::Mat_<double> R2 = svd_u * cv::Mat(W_t) * svd_vt;
cv::Mat_<double> t = svd_u.col(2); //u3
cv::Mat_<double> t2 = -t;
if (!CheckCoherentRotation(R)||!CheckCoherentRotation(R2)) {
std::cout << "resulting rotation is not coherent\n";
return;
}
cv::Matx34d origin = cv::Matx34d(1, 0, 0, 0 ,0, 1, 0, 0, 0, 0, 1, 0);
M1 = intrinsic*origin;
//four possible camera matrices
M2.resize(4);
M2[0] = cv::Matx34d(R(0, 0), R(0, 1), R(0, 2), t(0),
R(1, 0), R(1, 1), R(1, 2), t(1),
R(2, 0), R(2, 1), R(2, 2), t(2));
M2[1] = cv::Matx34d(R(0, 0), R(0, 1), R(0, 2), t2(0),
R(1, 0), R(1, 1), R(1, 2), t2(1),
R(2, 0), R(2, 1), R(2, 2), t2(2));
M2[2] = cv::Matx34d(R2(0, 0), R2(0, 1), R2(0, 2), t2(0),
R2(1, 0), R2(1, 1), R2(1, 2), t2(1),
R2(2, 0), R2(2, 1), R2(2, 2), t2(2));
M2[3] = cv::Matx34d(R2(0, 0), R2(0, 1), R2(0, 2), t(0),
R2(1, 0), R2(1, 1), R2(1, 2), t(1),
R2(2, 0), R2(2, 1), R2(2, 2), t(2));
for (int i = 0; i < 4; i++)
{
M2[i] = intrinsic*M2[i];
}
//homogeneous 3D points
cv::Mat points4d;
//record the number of zeros that are negative
std::vector<int> falsezero={0,0,0,0};
points3d.resize(4);
for (int i = 0; i < M2.size(); i++)
{
triangulatePoints(M1, M2[i], matchedpoints1,
matchedpoints2, points4d);
for (int j = 0; j < points4d.cols; j++)
{
//convert homogeneous 3D points to 3D points
float w = points4d.at<float>(3, j);
float x = points4d.at<float>(0, j) / w;
float y = points4d.at<float>(1, j) / w;
float z = points4d.at<float>(2, j) / w;
cv::Point3d p(x, y, z);
if (z < 0)
{
falsezero[i]++;
}
points3d[i].push_back(p);
}
}
int i = 0;
int small = falsezero[0];
for (int j = 1; j < 4; j++)
{
if (falsezero[j] < small)
{
small = falsezero[j];
i = j;
}
}
truepoints3d = points3d[i];
switch (i)
{
case 0:
{
rotation = R;
translation = t;
break;
}
case 1:
{
rotation = R;
translation = t2;
break;
}
case 2:
{
rotation = R2;
translation = t2;
break;
}
case 3:
{
rotation = R2;
translation = t;
break;
}
default:
break;
}
}
// Reconstruction function for API
void
reconstruct(InputArrayOfArrays points2d, OutputArray Ps, OutputArray points3d, InputOutputArray K,
bool is_projective)
{
const int nviews = points2d.total();
CV_Assert( nviews >= 2 );
// OpenCV data types
std::vector<Mat> pts2d;
points2d.getMatVector(pts2d);
const int depth = pts2d[0].depth();
Matx33d Ka = K.getMat();
// Projective reconstruction
if (is_projective)
{
if ( nviews == 2 )
{
// Get Projection matrices
Matx33d F;
Matx34d P, Pp;
normalizedEightPointSolver(pts2d[0], pts2d[1], F);
projectionsFromFundamental(F, P, Pp);
Ps.create(2, 1, depth);
Mat(P).copyTo(Ps.getMatRef(0));
Mat(Pp).copyTo(Ps.getMatRef(1));
// Triangulate and find 3D points using inliers
triangulatePoints(points2d, Ps, points3d);
}
else
{
std::vector<Mat> Rs, Ts;
reconstruct(points2d, Rs, Ts, Ka, points3d, is_projective);
// From Rs and Ts, extract Ps
const int nviews = Rs.size();
Ps.create(nviews, 1, depth);
Matx34d P;
for (size_t i = 0; i < nviews; ++i)
{
projectionFromKRt(Ka, Rs[i], Vec3d(Ts[i]), P);
Mat(P).copyTo(Ps.getMatRef(i));
}
Mat(Ka).copyTo(K.getMat());
}
}
// Affine reconstruction
else
{
// TODO: implement me
}
}
int recoverPose( InputArray E, InputArray _points1, InputArray _points2, InputArray _cameraMatrix,
OutputArray _R, OutputArray _t, InputOutputArray _mask)
{
Mat points1, points2, cameraMatrix;
_points1.getMat().convertTo(points1, CV_64F);
_points2.getMat().convertTo(points2, CV_64F);
_cameraMatrix.getMat().convertTo(cameraMatrix, CV_64F);
int npoints = points1.checkVector(2);
CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints &&
points1.type() == points2.type());
CV_Assert(cameraMatrix.rows == 3 && cameraMatrix.cols == 3 && cameraMatrix.channels() == 1);
if (points1.channels() > 1)
{
points1 = points1.reshape(1, npoints);
points2 = points2.reshape(1, npoints);
}
double fx = cameraMatrix.at<double>(0,0);
double fy = cameraMatrix.at<double>(1,1);
double cx = cameraMatrix.at<double>(0,2);
double cy = cameraMatrix.at<double>(1,2);
points1.col(0) = (points1.col(0) - cx) / fx;
points2.col(0) = (points2.col(0) - cx) / fx;
points1.col(1) = (points1.col(1) - cy) / fy;
points2.col(1) = (points2.col(1) - cy) / fy;
points1 = points1.t();
points2 = points2.t();
Mat R1, R2, t;
decomposeEssentialMat(E, R1, R2, t);
Mat P0 = Mat::eye(3, 4, R1.type());
Mat P1(3, 4, R1.type()), P2(3, 4, R1.type()), P3(3, 4, R1.type()), P4(3, 4, R1.type());
P1(Range::all(), Range(0, 3)) = R1 * 1.0; P1.col(3) = t * 1.0;
P2(Range::all(), Range(0, 3)) = R2 * 1.0; P2.col(3) = t * 1.0;
P3(Range::all(), Range(0, 3)) = R1 * 1.0; P3.col(3) = -t * 1.0;
P4(Range::all(), Range(0, 3)) = R2 * 1.0; P4.col(3) = -t * 1.0;
// Do the cheirality check.
// Notice here a threshold dist is used to filter
// out far away points (i.e. infinite points) since
// there depth may vary between postive and negtive.
double dist = 50.0;
Mat Q;
triangulatePoints(P0, P1, points1, points2, Q);
Mat mask1 = Q.row(2).mul(Q.row(3)) > 0;
Q.row(0) /= Q.row(3);
Q.row(1) /= Q.row(3);
Q.row(2) /= Q.row(3);
Q.row(3) /= Q.row(3);
mask1 = (Q.row(2) < dist) & mask1;
Q = P1 * Q;
mask1 = (Q.row(2) > 0) & mask1;
mask1 = (Q.row(2) < dist) & mask1;
triangulatePoints(P0, P2, points1, points2, Q);
Mat mask2 = Q.row(2).mul(Q.row(3)) > 0;
Q.row(0) /= Q.row(3);
Q.row(1) /= Q.row(3);
Q.row(2) /= Q.row(3);
Q.row(3) /= Q.row(3);
mask2 = (Q.row(2) < dist) & mask2;
Q = P2 * Q;
mask2 = (Q.row(2) > 0) & mask2;
mask2 = (Q.row(2) < dist) & mask2;
triangulatePoints(P0, P3, points1, points2, Q);
Mat mask3 = Q.row(2).mul(Q.row(3)) > 0;
Q.row(0) /= Q.row(3);
Q.row(1) /= Q.row(3);
Q.row(2) /= Q.row(3);
Q.row(3) /= Q.row(3);
mask3 = (Q.row(2) < dist) & mask3;
Q = P3 * Q;
mask3 = (Q.row(2) > 0) & mask3;
mask3 = (Q.row(2) < dist) & mask3;
triangulatePoints(P0, P4, points1, points2, Q);
Mat mask4 = Q.row(2).mul(Q.row(3)) > 0;
Q.row(0) /= Q.row(3);
Q.row(1) /= Q.row(3);
Q.row(2) /= Q.row(3);
Q.row(3) /= Q.row(3);
mask4 = (Q.row(2) < dist) & mask4;
Q = P4 * Q;
mask4 = (Q.row(2) > 0) & mask4;
mask4 = (Q.row(2) < dist) & mask4;
mask1 = mask1.t();
mask2 = mask2.t();
mask3 = mask3.t();
mask4 = mask4.t();
// If _mask is given, then use it to filter outliers.
if (!_mask.empty())
{
Mat mask = _mask.getMat();
CV_Assert(mask.size() == mask1.size());
bitwise_and(mask, mask1, mask1);
bitwise_and(mask, mask2, mask2);
bitwise_and(mask, mask3, mask3);
bitwise_and(mask, mask4, mask4);
}
if (_mask.empty() && _mask.needed())
{
_mask.create(mask1.size(), CV_8U);
}
CV_Assert(_R.needed() && _t.needed());
_R.create(3, 3, R1.type());
_t.create(3, 1, t.type());
int good1 = countNonZero(mask1);
int good2 = countNonZero(mask2);
int good3 = countNonZero(mask3);
int good4 = countNonZero(mask4);
if (good1 >= good2 && good1 >= good3 && good1 >= good4)
{
R1.copyTo(_R);
t.copyTo(_t);
if (_mask.needed()) mask1.copyTo(_mask);
return good1;
}
else if (good2 >= good1 && good2 >= good3 && good2 >= good4)
{
R2.copyTo(_R);
t.copyTo(_t);
if (_mask.needed()) mask2.copyTo(_mask);
return good2;
}
else if (good3 >= good1 && good3 >= good2 && good3 >= good4)
{
t = -t;
R1.copyTo(_R);
t.copyTo(_t);
if (_mask.needed()) mask3.copyTo(_mask);
return good3;
}
else
{
t = -t;
R2.copyTo(_R);
t.copyTo(_t);
if (_mask.needed()) mask4.copyTo(_mask);
return good4;
}
}
