mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-06 17:57:45 +08:00
Merged Audio branch to trunk
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@560 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
@@ -19,9 +19,13 @@
|
||||
|
||||
#include "rtabmap/core/EpipolarGeometry.h"
|
||||
#include "utilite/ULogger.h"
|
||||
#include "utilite/UTimer.h"
|
||||
#include "utilite/UStl.h"
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <opencv2/core/core_c.h>
|
||||
#include <opencv2/calib3d/calib3d.hpp>
|
||||
#include <iostream>
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
@@ -41,71 +45,375 @@ void findEpipolesFromF(const cv::Mat & fundamentalMatrix, cv::Vec3d & e1, cv::Ve
|
||||
return;
|
||||
}
|
||||
|
||||
CvMat * w = cvCreateMat(3, 3, CV_64FC1);
|
||||
CvMat * u = cvCreateMat(3, 3, CV_64FC1);
|
||||
CvMat * v = cvCreateMat(3, 3, CV_64FC1);
|
||||
|
||||
CvMat f = fundamentalMatrix;
|
||||
cvSVD(&f, w, u, v);
|
||||
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->data.db[0*3+2];// /v->data.db[2*3+2];
|
||||
e1[1] = v->data.db[1*3+2];// /v->data.db[2*3+2];
|
||||
e1[2] = v->data.db[2*3+2];// /v->data.db[2*3+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->data.db[0*3+2];// /u->data.db[2*3+2];
|
||||
e2[1] = u->data.db[1*3+2];// /u->data.db[2*3+2];
|
||||
e2[2] = u->data.db[2*3+2];// /u->data.db[2*3+2];
|
||||
|
||||
cvReleaseMat(&w);
|
||||
cvReleaseMat(&u);
|
||||
cvReleaseMat(&v);
|
||||
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];
|
||||
}
|
||||
|
||||
// P2 = [M | t] = [[e']_x * F | e']
|
||||
void findPFromF(const cv::Mat & fundamentalMatrix, cv::Mat & p2, cv::Vec3d e2)
|
||||
//Assuming P0 = [eye(3) zeros(3,1)]
|
||||
// x1 and x2 are 2D points
|
||||
// return camera matrix P (3x4) matrix
|
||||
cv::Mat findPFromF(const cv::Mat & fundamentalMatrix, const cv::Mat & x1, const cv::Mat & x2)
|
||||
{
|
||||
if(p2.rows != 3 || p2.cols != 4 || fundamentalMatrix.rows != 3 || fundamentalMatrix.cols != 3)
|
||||
|
||||
if(fundamentalMatrix.rows != 3 || fundamentalMatrix.cols != 3)
|
||||
{
|
||||
ULOGGER_ERROR("Matrices are not the good size... ");
|
||||
return;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
if(p2.type()!= CV_64FC1 || fundamentalMatrix.type() != CV_64FC1)
|
||||
if(fundamentalMatrix.type() != CV_64FC1)
|
||||
{
|
||||
ULOGGER_ERROR("Matrices are not the good type...");
|
||||
return;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
if(e2[0] == 0 && e2[1] == 0 && e2[2] == 0)
|
||||
// P matrix 3x4
|
||||
cv::Mat p = cv::Mat::zeros(3, 4, CV_64FC1);
|
||||
|
||||
// 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::SVD doesn't five same results as cvSVD ?!? cvSVD return same values as in MatLab
|
||||
/*cv::SVD svd(fundamentalMatrix);
|
||||
cv::Mat u = svd.u;
|
||||
cv::Mat v = svd.vt;
|
||||
cv::Mat s = svd.w;
|
||||
cv::Mat e = u.col(2);*/
|
||||
|
||||
CvMat F = fundamentalMatrix;
|
||||
cv::Mat u(3,3,CV_64F);
|
||||
cv::Mat v(3,3,CV_64F);
|
||||
cv::Mat s(3,3,CV_64F);
|
||||
CvMat U = u;
|
||||
CvMat S = s;
|
||||
CvMat V = v;
|
||||
cvSVD(&F, &S, &U, &V, CV_SVD_U_T|CV_SVD_V_T); // F = U D V^T
|
||||
u = u.t();
|
||||
//
|
||||
// INFO: may be required to multiply by -1 the last column of U
|
||||
// TODO: Is any way to detect when it is required to do that ? When
|
||||
// it is wrong, triangulated points have their Z value below 1 (between 0 and 1)...
|
||||
//
|
||||
/*u.at<double>(0,2) = -u.at<double>(0,2);
|
||||
u.at<double>(1,2) = -u.at<double>(1,2);
|
||||
u.at<double>(2,2) = -u.at<double>(2,2);*/
|
||||
v = v.t();
|
||||
cv::Mat e = u.col(2);
|
||||
|
||||
//std::cout << "u=" << u << std::endl;
|
||||
//std::cout << "v=" << v << std::endl;
|
||||
//std::cout << "s=" << s << std::endl;
|
||||
|
||||
// skew matrix 3X3
|
||||
cv::Mat skew = cv::Mat::zeros( 3, 3, CV_64FC1);
|
||||
skew.at<double>(0,1) = -1;
|
||||
skew.at<double>(1,0) = 1;
|
||||
skew.at<double>(2,2) = 1;
|
||||
|
||||
cv::Mat r;
|
||||
cv::Mat x4d;
|
||||
|
||||
cv::Mat x = x1.col(0); // just take one point
|
||||
cv::Mat xp = x2.col(0); // just take one point
|
||||
|
||||
// INFO: There 4 cases of P, only one have the points in
|
||||
// front of the two cameras (positive z).
|
||||
|
||||
// Case 1 : P = [U*W*V' e];
|
||||
r = u*skew*v.t();
|
||||
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) = e.at<double>(0,0);
|
||||
p.at<double>(1,3) = e.at<double>(1,0);
|
||||
p.at<double>(2,3) = e.at<double>(2,0);
|
||||
|
||||
cv::triangulatePoints(p0, p, x, xp, x4d);
|
||||
x4d.at<double>(0) = x4d.at<double>(0)/x4d.at<double>(3);
|
||||
x4d.at<double>(1) = x4d.at<double>(1)/x4d.at<double>(3);
|
||||
x4d.at<double>(2) = x4d.at<double>(2)/x4d.at<double>(3);
|
||||
x4d.at<double>(3) = x4d.at<double>(3)/x4d.at<double>(3);
|
||||
|
||||
cv::Mat xt1 = p0*x4d;
|
||||
cv::Mat xt2 = p*x4d;
|
||||
|
||||
if(xt1.at<double>(2,0) < 0 || xt2.at<double>(2,0) < 0)
|
||||
{
|
||||
cv::Vec3d e1;
|
||||
findEpipolesFromF(fundamentalMatrix, e1, e2);
|
||||
// Case 2 : P = [U*W*V' -e];
|
||||
p.at<double>(0,3) = -e.at<double>(0,0);
|
||||
p.at<double>(1,3) = -e.at<double>(1,0);
|
||||
p.at<double>(2,3) = -e.at<double>(2,0);
|
||||
cv::triangulatePoints(p0, p, x, xp, x4d);
|
||||
x4d.at<double>(0) = x4d.at<double>(0)/x4d.at<double>(3);
|
||||
x4d.at<double>(1) = x4d.at<double>(1)/x4d.at<double>(3);
|
||||
x4d.at<double>(2) = x4d.at<double>(2)/x4d.at<double>(3);
|
||||
x4d.at<double>(3) = x4d.at<double>(3)/x4d.at<double>(3);
|
||||
xt1 = p0*x4d;
|
||||
xt2 = p*x4d;
|
||||
if(xt1.at<double>(2,0) < 0 || xt2.at<double>(2,0) < 0)
|
||||
{
|
||||
// Case 3 : P = [U*W'*V' e];
|
||||
r = u*skew.t()*v.t();
|
||||
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) = e.at<double>(0,0);
|
||||
p.at<double>(1,3) = e.at<double>(1,0);
|
||||
p.at<double>(2,3) = e.at<double>(2,0);
|
||||
p.col(3) = e;
|
||||
cv::triangulatePoints(p0, p, x, xp, x4d);
|
||||
x4d.at<double>(0) = x4d.at<double>(0)/x4d.at<double>(3);
|
||||
x4d.at<double>(1) = x4d.at<double>(1)/x4d.at<double>(3);
|
||||
x4d.at<double>(2) = x4d.at<double>(2)/x4d.at<double>(3);
|
||||
x4d.at<double>(3) = x4d.at<double>(3)/x4d.at<double>(3);
|
||||
xt1 = p0*x4d;
|
||||
xt2 = p*x4d;
|
||||
if(xt1.at<double>(2,0) < 0 || xt2.at<double>(2,0) < 0)
|
||||
{
|
||||
// Case 4 : P = [U*W'*V' -e];
|
||||
p.at<double>(0,3) = -e.at<double>(0,0);
|
||||
p.at<double>(1,3) = -e.at<double>(1,0);
|
||||
p.at<double>(2,3) = -e.at<double>(2,0);
|
||||
cv::triangulatePoints(p0, p, x, xp, x4d);
|
||||
x4d.at<double>(0) = x4d.at<double>(0)/x4d.at<double>(3);
|
||||
x4d.at<double>(1) = x4d.at<double>(1)/x4d.at<double>(3);
|
||||
x4d.at<double>(2) = x4d.at<double>(2)/x4d.at<double>(3);
|
||||
x4d.at<double>(3) = x4d.at<double>(3)/x4d.at<double>(3);
|
||||
xt1 = p0*x4d;
|
||||
xt2 = p*x4d;
|
||||
UDEBUG("Case 4");
|
||||
}
|
||||
else
|
||||
{
|
||||
UDEBUG("Case 3");
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
UDEBUG("Case 2");
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
UDEBUG("Case 1");
|
||||
}
|
||||
return p;
|
||||
}
|
||||
|
||||
cv::Mat findFFromWords(
|
||||
const std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs, // id, kpt1, kpt2
|
||||
std::vector<uchar> & status,
|
||||
double ransacParam1,
|
||||
double ransacParam2)
|
||||
{
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
double e2_sd[3*3] = { 0., -e2[2], e2[1],
|
||||
e2[2], 0., -e2[0],
|
||||
-e2[1], e2[0], 0. };
|
||||
CvMat e2_smt = cvMat( 3, 3, CV_64FC1, e2_sd );
|
||||
cv::Mat e2_sm(&e2_smt); //;
|
||||
UTimer timer;
|
||||
timer.start();
|
||||
|
||||
cv::Mat m = e2_sm*fundamentalMatrix;
|
||||
// Find the fundamental matrix
|
||||
cv::Mat fundamentalMatrix = cv::findFundamentalMat(
|
||||
points1,
|
||||
points2,
|
||||
status,
|
||||
cv::FM_RANSAC,
|
||||
ransacParam1,
|
||||
ransacParam2);
|
||||
|
||||
p2.at<double>(0,0) = m.at<double>(0,0);
|
||||
p2.at<double>(0,1) = m.at<double>(0,1);
|
||||
p2.at<double>(0,2) = m.at<double>(0,2);
|
||||
p2.at<double>(1,0) = m.at<double>(1,0);
|
||||
p2.at<double>(1,1) = m.at<double>(1,1);
|
||||
p2.at<double>(1,2) = m.at<double>(1,2);
|
||||
p2.at<double>(2,0) = m.at<double>(2,0);
|
||||
p2.at<double>(2,1) = m.at<double>(2,1);
|
||||
p2.at<double>(2,2) = m.at<double>(2,2);
|
||||
ULOGGER_DEBUG("Find fundamental matrix (OpenCV) time = %fs", timer.ticks());
|
||||
|
||||
p2.at<double>(0,3) = e2[0];
|
||||
p2.at<double>(1,3) = e2[1];
|
||||
p2.at<double>(2,3) = e2[2];
|
||||
// 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 findRTFromP(
|
||||
const cv::Mat & p,
|
||||
cv::Mat & r,
|
||||
cv::Mat & t)
|
||||
{
|
||||
UASSERT(p.cols == 4 && p.rows == 3);
|
||||
UDEBUG("");
|
||||
r = cv::Mat(p, cv::Range(0,3), cv::Range(0,3));
|
||||
UDEBUG("");
|
||||
r = -r.inv();
|
||||
UDEBUG("r=%d %d, t=%d", r.cols, r.rows, p.col(3).rows);
|
||||
t = r*p.col(3);
|
||||
UDEBUG("");
|
||||
}
|
||||
|
||||
/**
|
||||
* 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 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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
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 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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
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 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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
} // namespace rtabmap
|
||||
|
||||
Reference in New Issue
Block a user