i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-11 14:00:20 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
e4873e770a1e303c997f4c477fc92f8e67c9592e
rtabmap/corelib/src/EpipolarGeometry.cpp
T

665 lines
21 KiB
C++
Raw Blame History

/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/EpipolarGeometry.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UMath.h"
#include <opencv2/core/core.hpp>
#include <opencv2/core/core_c.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <iostream>
namespace rtabmap
{
/////////////////////////
// HypVerificatorEpipolarGeo
/////////////////////////
EpipolarGeometry::EpipolarGeometry(const ParametersMap & parameters) :
_matchCountMinAccepted(Parameters::defaultVhEpMatchCountMin()),
_ransacParam1(Parameters::defaultVhEpRansacParam1()),
_ransacParam2(Parameters::defaultVhEpRansacParam2())
{
this->parseParameters(parameters);
}
EpipolarGeometry::~EpipolarGeometry() {
}
void EpipolarGeometry::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kVhEpMatchCountMin(), _matchCountMinAccepted);
Parameters::parse(parameters, Parameters::kVhEpRansacParam1(), _ransacParam1);
Parameters::parse(parameters, Parameters::kVhEpRansacParam2(), _ransacParam2);
}
bool EpipolarGeometry::check(const Signature * ssA, const Signature * ssB)
{
if(ssA == 0 || ssB == 0)
{
return false;
}
ULOGGER_DEBUG("id(%d,%d)", ssA->id(), ssB->id());
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
findPairsUnique(ssA->getWords(), ssB->getWords(), pairs);
if((int)pairs.size()<_matchCountMinAccepted)
{
return false;
}
std::vector<uchar> status;
cv::Mat f = findFFromWords(pairs, status, _ransacParam1, _ransacParam2);
int inliers = uSum(status);
if(inliers < _matchCountMinAccepted)
{
ULOGGER_DEBUG("Epipolar constraint failed A : not enough inliers (%d/%d), min is %d", inliers, pairs.size(), _matchCountMinAccepted);
return false;
}
else
{
UDEBUG("inliers = %d/%d", inliers, pairs.size());
return true;
}
}
//STATIC STUFF
//Epipolar geometry
void EpipolarGeometry::findEpipolesFromF(const cv::Mat & fundamentalMatrix, cv::Vec3d & e1, cv::Vec3d & e2)
{
if(fundamentalMatrix.rows != 3 || fundamentalMatrix.cols != 3)
{
ULOGGER_ERROR("The matrix is not the good size...");
return;
}
if(fundamentalMatrix.type() != CV_64FC1)
{
ULOGGER_ERROR("The matrix is not the good type...");
return;
}
cv::SVD svd(fundamentalMatrix);
cv::Mat u = svd.u;
cv::Mat v = svd.vt;
cv::Mat w = svd.w;
// v is for image 1
// u is for image 2
e1[0] = v.at<double>(0,2);// /v->data.db[2*3+2];
e1[1] = v.at<double>(1,2);// /v->data.db[2*3+2];
e1[2] = v.at<double>(2,2);// /v->data.db[2*3+2];
e2[0] = u.at<double>(0,2);// /u->data.db[2*3+2];
e2[1] = u.at<double>(1,2);// /u->data.db[2*3+2];
e2[2] = u.at<double>(2,2);// /u->data.db[2*3+2];
}
int inFrontOfBothCameras(const cv::Mat & x, const cv::Mat & xp, const cv::Mat & R, const cv::Mat & T)
{
// P0 matrix 3X4
cv::Mat p0 = cv::Mat::zeros(3, 4, CV_64FC1);
p0.at<double>(0,0) = 1;
p0.at<double>(1,1) = 1;
p0.at<double>(2,2) = 1;
cv::Mat p = cv::Mat::zeros(3, 4, CV_64FC1);
p.at<double>(0,0) = R.at<double>(0,0);
p.at<double>(0,1) = R.at<double>(0,1);
p.at<double>(0,2) = R.at<double>(0,2);
p.at<double>(1,0) = R.at<double>(1,0);
p.at<double>(1,1) = R.at<double>(1,1);
p.at<double>(1,2) = R.at<double>(1,2);
p.at<double>(2,0) = R.at<double>(2,0);
p.at<double>(2,1) = R.at<double>(2,1);
p.at<double>(2,2) = R.at<double>(2,2);
p.at<double>(0,3) = T.at<double>(0,0);
p.at<double>(1,3) = T.at<double>(1,0);
p.at<double>(2,3) = T.at<double>(2,0);
cv::Mat pts4D;
//std::vector<double> reprojErrors;
//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
//EpipolarGeometry::triangulatePoints(x, xp, p0, p, cloud, reprojErrors);
cv::triangulatePoints(p0, p, x, xp, pts4D);
//http://en.wikipedia.org/wiki/Essential_matrix#3D_points_from_corresponding_image_points
int nValid = 0;
for(int i=0; i<x.cols; ++i)
{
// the five to ignore when all points are super close to the camera
if(pts4D.at<double>(2,i)/pts4D.at<double>(3,i) > 5)
//if(cloud->at(i).z > 5)
{
++nValid;
}
}
UDEBUG("nValid=%d/%d", nValid, x.cols);
return nValid;
}
//Assuming P0 = [eye(3) zeros(3,1)]
// x1 and x2 are 2D points
// return camera matrix P (3x4) matrix
//http://www.robots.ox.ac.uk/~vgg/hzbook/hzbook2/HZepipolar.pdf
cv::Mat EpipolarGeometry::findPFromE(const cv::Mat & E,
const cv::Mat & x,
const cv::Mat & xp)
{
UDEBUG("begin");
UASSERT(E.rows == 3 && E.cols == 3);
UASSERT(E.type() == CV_64FC1);
UASSERT(x.rows == 2 && x.cols>0 && x.type() == CV_64FC1);
UASSERT(xp.rows == 2 && xp.cols>0 && x.type() == CV_64FC1);
// skew matrix 3X3
cv::Mat w = cv::Mat::zeros( 3, 3, CV_64FC1);
w.at<double>(0,1) = -1;
w.at<double>(1,0) = 1;
w.at<double>(2,2) = 1;
//std::cout << "W=" << w << std::endl;
cv::Mat e = E;
cv::SVD svd(e,cv::SVD::MODIFY_A);
cv::Mat u = svd.u;
cv::Mat vt = svd.vt;
cv::Mat s = svd.w;
//std::cout << "u=" << u << std::endl;
//std::cout << "vt=" << vt << std::endl;
//std::cout << "s=" << s << std::endl;
// E = u*diag(1,1,0)*vt
cv::Mat diag = cv::Mat::eye(3,3,CV_64FC1);
diag.at<double>(2,2) = 0;
e = u*diag*vt;
svd(e,cv::SVD::MODIFY_A);
u = svd.u;
vt = svd.vt;
s = svd.w;
cv::Mat r = u*w*vt;
if(cv::determinant(r)+1.0 < 1e-09) {
//according to http://en.wikipedia.org/wiki/Essential_matrix#Showing_that_it_is_valid
UDEBUG("det(R) == -1 [%f]: flip E's sign", cv::determinant(r));
e = -E;
svd(e,cv::SVD::MODIFY_A);
u = svd.u;
vt = svd.vt;
s = svd.w;
}
cv::Mat wt = w.t();
// INFO: There 4 cases of P, only one have all the points in
// front of the two cameras (positive z).
cv::Mat r1 = u*w*vt;
cv::Mat r2 = u*wt*vt;
cv::Mat t1 = u.col(2);
cv::Mat t2 = u.col(2)*-1;
int max = 0;
int maxIndex = 1;
int maxTmp;
cv::Mat R=r1,T=t1;
// Case 1 : P = [U*W*V' t];
max = inFrontOfBothCameras(x, xp, r1, t1);
// Case 2 : P = [U*W*V' -t];
maxTmp = inFrontOfBothCameras(x, xp, r1, t2);
if(maxTmp > max)
{
maxIndex = 2;
max = maxTmp;
R=r1,T=t2;
}
// Case 3 : P = [U*W'*V' t];
maxTmp = inFrontOfBothCameras(x, xp, r2, t1);
if(maxTmp > max)
{
maxIndex = 3;
max = maxTmp;
R=r2,T=t1;
}
// Case 4 : P = [U*W'*V' -t];
maxTmp = inFrontOfBothCameras(x, xp, r2, t2);
if(maxTmp > max)
{
maxIndex = 4;
max = maxTmp;
R=r2,T=t2;
}
if(max > 0)
{
UDEBUG("Case %d", maxIndex);
// P matrix 3x4
cv::Mat p = cv::Mat::zeros(3, 4, CV_64FC1);
p.at<double>(0,0) = R.at<double>(0,0);
p.at<double>(0,1) = R.at<double>(0,1);
p.at<double>(0,2) = R.at<double>(0,2);
p.at<double>(1,0) = R.at<double>(1,0);
p.at<double>(1,1) = R.at<double>(1,1);
p.at<double>(1,2) = R.at<double>(1,2);
p.at<double>(2,0) = R.at<double>(2,0);
p.at<double>(2,1) = R.at<double>(2,1);
p.at<double>(2,2) = R.at<double>(2,2);
p.at<double>(0,3) = T.at<double>(0);
p.at<double>(1,3) = T.at<double>(1);
p.at<double>(2,3) = T.at<double>(2);
return p;
}
return cv::Mat();
}
cv::Mat EpipolarGeometry::findFFromWords(
const std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs, // id, kpt1, kpt2
std::vector<uchar> & status,
double ransacReprojThreshold,
double ransacConfidence)
{
status = std::vector<uchar>(pairs.size(), 0);
//Convert Keypoints to a structure that OpenCV understands
//3 dimensions (Homogeneous vectors)
cv::Mat points1(1, pairs.size(), CV_32FC2);
cv::Mat points2(1, pairs.size(), CV_32FC2);
float * points1data = points1.ptr<float>(0);
float * points2data = points2.ptr<float>(0);
// Fill the points here ...
int i=0;
for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::const_iterator iter = pairs.begin();
iter != pairs.end();
++iter )
{
points1data[i*2] = (*iter).second.first.pt.x;
points1data[i*2+1] = (*iter).second.first.pt.y;
points2data[i*2] = (*iter).second.second.pt.x;
points2data[i*2+1] = (*iter).second.second.pt.y;
++i;
}
UTimer timer;
timer.start();
// Find the fundamental matrix
cv::Mat fundamentalMatrix = cv::findFundamentalMat(
points1,
points2,
status,
cv::FM_RANSAC,
ransacReprojThreshold,
ransacConfidence);
ULOGGER_DEBUG("Find fundamental matrix (OpenCV) time = %fs", timer.ticks());
// Fundamental matrix is valid ?
bool fundMatFound = false;
UASSERT(fundamentalMatrix.type() == CV_64FC1);
if(fundamentalMatrix.cols==3 && fundamentalMatrix.rows==3 &&
(fundamentalMatrix.at<double>(0,0) != 0.0 ||
fundamentalMatrix.at<double>(0,1) != 0.0 ||
fundamentalMatrix.at<double>(0,2) != 0.0 ||
fundamentalMatrix.at<double>(1,0) != 0.0 ||
fundamentalMatrix.at<double>(1,1) != 0.0 ||
fundamentalMatrix.at<double>(1,2) != 0.0 ||
fundamentalMatrix.at<double>(2,0) != 0.0 ||
fundamentalMatrix.at<double>(2,1) != 0.0 ||
fundamentalMatrix.at<double>(2,2) != 0.0) )
{
fundMatFound = true;
}
ULOGGER_DEBUG("fm_count=%d...", fundMatFound);
if(fundMatFound)
{
// Show the fundamental matrix
UDEBUG(
"F = [%f %f %f;%f %f %f;%f %f %f]",
fundamentalMatrix.ptr<double>(0)[0],
fundamentalMatrix.ptr<double>(0)[1],
fundamentalMatrix.ptr<double>(0)[2],
fundamentalMatrix.ptr<double>(0)[3],
fundamentalMatrix.ptr<double>(0)[4],
fundamentalMatrix.ptr<double>(0)[5],
fundamentalMatrix.ptr<double>(0)[6],
fundamentalMatrix.ptr<double>(0)[7],
fundamentalMatrix.ptr<double>(0)[8]);
}
return fundamentalMatrix;
}
void EpipolarGeometry::findRTFromP(
const cv::Mat & p,
cv::Mat & r,
cv::Mat & t)
{
UASSERT(p.cols == 4 && p.rows == 3);
r = cv::Mat(p, cv::Range(0,3), cv::Range(0,3));
//r = -r.inv();
//t = r*p.col(3);
t = p.col(3);
}
cv::Mat EpipolarGeometry::findFFromCalibratedStereoCameras(double fx, double fy, double cx, double cy, double Tx, double Ty)
{
cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
double Bx = Tx/-fx;
double By = Ty/-fy;
cv::Mat tx = (cv::Mat_<double>(3,3) <<
0, 0, By,
0, 0, -Bx,
-By, Bx, 0);
cv::Mat K = (cv::Mat_<double>(3,3) <<
fx, 0, cx,
0, fy, cy,
0, 0, 1);
cv::Mat E = tx*R;
return K.inv().t()*E*K.inv();
}
/**
* if a=[1 2 3 4 6], b=[1 2 4 5 6], results= [(1,1) (2,2) (4,4) (6,6)]
* realPairsCount = 4
*/
int EpipolarGeometry::findPairs(
const std::map<int, cv::KeyPoint> & wordsA,
const std::map<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
bool ignoreInvalidIds)
{
int realPairsCount = 0;
pairs.clear();
for(std::map<int, cv::KeyPoint>::const_iterator i=wordsA.begin(); i!=wordsA.end(); ++i)
{
if(!ignoreInvalidIds || (ignoreInvalidIds && i->first>=0))
{
std::map<int, cv::KeyPoint>::const_iterator ptB = wordsB.find(i->first);
if(ptB != wordsB.end())
{
pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(i->first, std::pair<cv::KeyPoint, cv::KeyPoint>(i->second, ptB->second)));
++realPairsCount;
}
}
}
return realPairsCount;
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
* realPairsCount = 5
*/
int EpipolarGeometry::findPairs(const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
bool ignoreInvalidIds)
{
const std::list<int> & ids = uUniqueKeys(wordsA);
std::multimap<int, cv::KeyPoint>::const_iterator iterA;
std::multimap<int, cv::KeyPoint>::const_iterator iterB;
pairs.clear();
int realPairsCount = 0;
for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
{
if(!ignoreInvalidIds || (ignoreInvalidIds && *i >= 0))
{
iterA = wordsA.find(*i);
iterB = wordsB.find(*i);
while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
{
pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>((*iterA).second, (*iterB).second)));
++iterA;
++iterB;
++realPairsCount;
}
}
}
return realPairsCount;
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
* realPairsCount = 5
*/
int EpipolarGeometry::findPairsUnique(
const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
bool ignoreInvalidIds)
{
const std::list<int> & ids = uUniqueKeys(wordsA);
int realPairsCount = 0;
pairs.clear();
for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
{
if(!ignoreInvalidIds || (ignoreInvalidIds && *i>=0))
{
std::list<cv::KeyPoint> ptsA = uValues(wordsA, *i);
std::list<cv::KeyPoint> ptsB = uValues(wordsB, *i);
if(ptsA.size() == 1 && ptsB.size() == 1)
{
pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>(ptsA.front(), ptsB.front())));
++realPairsCount;
}
else if(ptsA.size()>1 && ptsB.size()>1)
{
// just update the count
realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
}
}
}
return realPairsCount;
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (1,1b) (2,2) (4,4) (6a,6a) (6a,6b) (6b,6a) (6b,6b)]
* realPairsCount = 5
*/
int EpipolarGeometry::findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
bool ignoreInvalidIds)
{
UTimer timer;
timer.start();
const std::list<int> & ids = uUniqueKeys(wordsA);
pairs.clear();
int realPairsCount = 0;;
for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{
if(!ignoreInvalidIds || (ignoreInvalidIds && *iter>=0))
{
std::list<cv::KeyPoint> ptsA = uValues(wordsA, *iter);
std::list<cv::KeyPoint> ptsB = uValues(wordsB, *iter);
realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
for(std::list<cv::KeyPoint>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
{
for(std::list<cv::KeyPoint>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
{
pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*iter, std::pair<cv::KeyPoint, cv::KeyPoint>(*jter, *kter)));
}
}
}
}
ULOGGER_DEBUG("time = %f", timer.ticks());
return realPairsCount;
}
/**
source = SfM toy library: https://github.com/royshil/SfM-Toy-Library
From "Triangulation", Hartley, R.I. and Sturm, P., Computer vision and image understanding, 1997
return 1x3 double
*/
cv::Mat EpipolarGeometry::linearLSTriangulation(
cv::Point3d u, //homogenous image point (u,v,1)
cv::Matx34d P, //camera 1 matrix 3x4 double
cv::Point3d u1, //homogenous image point in 2nd camera
cv::Matx34d P1 //camera 2 matrix 3x4 double
)
{
//build matrix A for homogenous equation system Ax = 0
//assume X = (x,y,z,1), for Linear-LS method
//which turns it into a AX = B system, where A is 4x3, X is 3x1 and B is 4x1
cv::Mat A = (cv::Mat_<double>(4,3) <<
u.x*P(2,0)-P(0,0), u.x*P(2,1)-P(0,1), u.x*P(2,2)-P(0,2),
u.y*P(2,0)-P(1,0), u.y*P(2,1)-P(1,1), u.y*P(2,2)-P(1,2),
u1.x*P1(2,0)-P1(0,0), u1.x*P1(2,1)-P1(0,1), u1.x*P1(2,2)-P1(0,2),
u1.y*P1(2,0)-P1(1,0), u1.y*P1(2,1)-P1(1,1), u1.y*P1(2,2)-P1(1,2)
);
cv::Mat B = (cv::Mat_<double>(4,1) <<
-(u.x*P(2,3) -P(0,3)),
-(u.y*P(2,3) -P(1,3)),
-(u1.x*P1(2,3) -P1(0,3)),
-(u1.y*P1(2,3) -P1(1,3)));
cv::Mat X;
solve(A,B,X,cv::DECOMP_SVD);
return X; // return 1x3 double
}
/**
source = SfM toy library: https://github.com/royshil/SfM-Toy-Library
From "Triangulation", Hartley, R.I. and Sturm, P., Computer vision and image understanding, 1997
return 4x1 double
*/
cv::Mat EpipolarGeometry::iterativeLinearLSTriangulation(
cv::Point3d u, //homogenous image point (u,v,1)
const cv::Matx34d & P, //camera 1 matrix 3x4 double
cv::Point3d u1, //homogenous image point in 2nd camera
const cv::Matx34d & P1) //camera 2 matrix 3x4 double
{
double wi = 1, wi1 = 1;
double EPSILON = 0.0001;
cv::Mat X(4,1,CV_64FC1);
cv::Mat X_ = linearLSTriangulation(u,P,u1,P1);
X.at<double>(0) = X_.at<double>(0);
X.at<double>(1) = X_.at<double>(1);
X.at<double>(2) = X_.at<double>(2);
X.at<double>(3) = 1.0;
for (int i=0; i<10; i++) //Hartley suggests 10 iterations at most
{
//recalculate weights
double p2x = cv::Mat(cv::Mat(P).row(2)*X).at<double>(0);
double p2x1 = cv::Mat(cv::Mat(P1).row(2)*X).at<double>(0);
//breaking point
if(fabs(wi - p2x) <= EPSILON && fabs(wi1 - p2x1) <= EPSILON) break;
wi = p2x;
wi1 = p2x1;
//reweight equations and solve
cv::Mat A = (cv::Mat_<double>(4,3) <<
(u.x*P(2,0)-P(0,0))/wi, (u.x*P(2,1)-P(0,1))/wi, (u.x*P(2,2)-P(0,2))/wi,
(u.y*P(2,0)-P(1,0))/wi, (u.y*P(2,1)-P(1,1))/wi, (u.y*P(2,2)-P(1,2))/wi,
(u1.x*P1(2,0)-P1(0,0))/wi1, (u1.x*P1(2,1)-P1(0,1))/wi1, (u1.x*P1(2,2)-P1(0,2))/wi1,
(u1.y*P1(2,0)-P1(1,0))/wi1, (u1.y*P1(2,1)-P1(1,1))/wi1, (u1.y*P1(2,2)-P1(1,2))/wi1);
cv::Mat B = (cv::Mat_<double>(4,1) <<
-(u.x*P(2,3) -P(0,3))/wi,
-(u.y*P(2,3) -P(1,3))/wi,
-(u1.x*P1(2,3) -P1(0,3))/wi1,
-(u1.y*P1(2,3) -P1(1,3))/wi1);
solve(A,B,X_,cv::DECOMP_SVD);
X.at<double>(0) = X_.at<double>(0);
X.at<double>(1) = X_.at<double>(1);
X.at<double>(2) = X_.at<double>(2);
X.at<double>(3) = 1.0;
}
return X; // return 4x1 double
}
/**
source = SfM toy library: https://github.com/royshil/SfM-Toy-Library
*/
//Triagulate points
double EpipolarGeometry::triangulatePoints(
const cv::Mat& pt_set, //2xN double
const cv::Mat& pt_set1, //2xN double
const cv::Mat& P, // 3x4 double
const cv::Mat& P1, // 3x4 double
pcl::PointCloud<pcl::PointXYZ>::Ptr & pointcloud,
std::vector<double> & reproj_errors)
{
pointcloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
unsigned int pts_size = pt_set.cols;
pointcloud->resize(pts_size);
reproj_errors.resize(pts_size);
for(unsigned int i=0; i<pts_size; i++)
{
cv::Point3d u(pt_set.at<double>(0,i),pt_set.at<double>(1,i),1.0);
cv::Point3d u1(pt_set1.at<double>(0,i),pt_set1.at<double>(1,i),1.0);
cv::Mat X = iterativeLinearLSTriangulation(u,P,u1,P1);
cv::Mat x_proj = P * X; //reproject
x_proj = x_proj / x_proj.at<double>(2);
cv::Point3d xPt_img_(x_proj.at<double>(0), x_proj.at<double>(1), 1.0);
double reprj_err = norm(xPt_img_ - u);
reproj_errors[i] = reprj_err;
pointcloud->at(i) = pcl::PointXYZ(X.at<double>(0),X.at<double>(1),X.at<double>(2));
}
return cv::mean(reproj_errors)[0]; // mean reproj error
}
} // namespace rtabmap
Reference in New Issue View Git Blame Copy Permalink