i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +08:00
Code Issues Packages Projects Releases Wiki Activity

Implemented OptimizerG2O::optimimzeBA(). Updated PostProcessingDialog (g2o sba option). Fixed words descriptors not filled in rtabmap::getMap3D(). CameraModelD(): return 5 null coeff distorsions if not set

Browse Source
This commit is contained in:
matlabbe
2016-03-28 18:19:17 -04:00
parent 18759e7197
commit 8030d89634
16 changed files with 556 additions and 293 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
corelib/src/Memory.cpp
View File

@@ -2914,7 +2914,8 @@ SensorData Memory::getNodeData(int nodeId, bool uncompressedData, bool keepLoade
void Memory::getNodeWords(int nodeId,
std::multimap<int, cv::KeyPoint> & words,
std::multimap<int, cv::Point3f> & words3)
std::multimap<int, cv::Point3f> & words3,
std::multimap<int, cv::Mat> & wordsDescriptors)
{
UDEBUG("nodeId=%d", nodeId);
Signature * s = this->_getSignature(nodeId);
@@ -2922,6 +2923,7 @@ void Memory::getNodeWords(int nodeId,
{
words = s->getWords();
words3 = s->getWords3();
wordsDescriptors = s->getWordsDescriptors();
}
else if(_dbDriver)
{
@@ -2935,6 +2937,7 @@ void Memory::getNodeWords(int nodeId,
{
words = signatures.front()->getWords();
words3 = signatures.front()->getWords3();
wordsDescriptors = signatures.front()->getWordsDescriptors();
if(loadedFromTrash.size())
{
//put back

74
corelib/src/Optimizer.cpp
View File

@@ -31,6 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/Optimizer.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/RegistrationVis.h>
#include <set>
#include <queue>
@@ -269,4 +271,76 @@ void Optimizer::getConnectedGraph(
}
}
void Optimizer::computeBACorrespondences(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures,
std::map<int, cv::Point3f> & points3DMap,
std::map<int, std::map<int, cv::Point2f> > & wordReferences) // <ID words, IDs frames + keypoint>
{
int wordCount = 0;
int edgeWithWordsAdded = 0;
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
Link link = iter->second;
if(link.to() < link.from())
{
link = link.inverse();
}
if(uContains(signatures, link.from()) &&
uContains(signatures, link.to()) &&
uContains(poses, link.from()))
{
Signature sFrom = signatures.at(link.from());
Signature sTo = signatures.at(link.to());
if(sFrom.getWords().size() &&
sTo.getWords().size() &&
sFrom.getWords3().size())
{
ParametersMap regParam;
regParam.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
regParam.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
regParam.insert(ParametersPair(Parameters::kVisMinInliers(), "5"));
regParam.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
RegistrationVis reg(regParam);
//sFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
//sTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
RegistrationInfo info;
Transform t = reg.computeTransformationMod(sFrom, sTo, Transform(), &info);
//Transform t = reg.computeTransformationMod(sFrom, sTo, iter->second.transform(), &info);
UDEBUG("%d->%d, inliers=%d",sFrom.id(), sTo.id(), (int)info.inliersIDs.size());
if(!t.isNull())
{
Transform pose = poses.at(sFrom.id());
for(unsigned int i=0; i<info.inliersIDs.size(); ++i)
{
cv::Point3f p = sFrom.getWords3().lower_bound(info.inliersIDs[i])->second;
if(p.x > 0.0f) // make sure the point is valid
{
int wordId = ++wordCount;
p = util3d::transformPoint(p, pose);
points3DMap.insert(std::make_pair(wordId, p));
wordReferences.insert(std::make_pair(wordId, std::map<int, cv::Point2f>()));
wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(info.inliersIDs[i])->second.pt));
wordReferences.at(wordId).insert(std::make_pair(sTo.id(), sTo.getWords().lower_bound(info.inliersIDs[i])->second.pt));
}
}
++edgeWithWordsAdded;
}
else
{
UWARN("Not enough inliers (%d) between %d and %d", info.inliersIDs.size(), sFrom.id(), sTo.id());
}
}
}
}
UDEBUG("Added %d words (edges with words=%d/%d)", wordCount, edgeWithWordsAdded, links.size());
}
} /* namespace rtabmap */

94
corelib/src/OptimizerCVSBA.cpp
View File

@@ -69,7 +69,7 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
params.iterations = this->iterations();
params.minError = this->epsilon();
params.fixedIntrinsics = 5;
params.fixedDistortion = 5;
params.fixedDistortion = 5; // updated below
params.verbose=ULogger::level() <= ULogger::kInfo;
sba.setParams(params);
@@ -110,7 +110,15 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
frameIdToIndex.insert(std::make_pair(iter->first, oi));
cameraMatrix[oi] = model.K();
distCoeffs[oi] = model.D();
if(model.D().cols != 5)
{
distCoeffs[oi] = cv::Mat::zeros(1, 5, CV_64FC1);
UWARN("Camera model %d: Distortion coefficients are not 5, setting all them to 0 (assuming no distortion)", iter->first);
}
else
{
distCoeffs[oi] = model.D();
}
Transform t = (iter->second * model.localTransform()).inverse();
@@ -138,88 +146,28 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
distCoeffs.resize(oi);
std::map<int, cv::Point3f> points3DMap;
std::multimap<int, std::pair<int, cv::Point2f> > wordReferences; // <ID words, IDs frames + keypoint>
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
Link link = iter->second;
if(link.to() < link.from())
{
link = link.inverse();
}
if(uContains(signatures, link.from()) &&
uContains(signatures, link.to()) &&
uContains(frames, link.from()))
{
const Signature & sFrom = signatures.at(link.from());
const Signature & sTo = signatures.at(link.to());
std::map<int, std::map<int, cv::Point2f> > wordReferences; // <ID words, IDs frames + keypoint>
computeBACorrespondences(frames, links, signatures, points3DMap, wordReferences);
std::vector<int> inliers;
Transform t = util3d::estimateMotion3DTo3D(
uMultimapToMapUnique(sFrom.getWords3()),
uMultimapToMapUnique(sTo.getWords3()),
minInliers_,
inlierDistance_,
100,
10,
0,
0,
&inliers);
if(!t.isNull())
{
Transform pose = frames.at(sFrom.id());
for(unsigned int i=0; i<inliers.size(); ++i)
{
cv::Point3f p = util3d::transformPoint(sFrom.getWords3().lower_bound(inliers[i])->second, pose);
std::map<int, cv::Point3f>::iterator jter = points3DMap.find(inliers[i]);
if(jter == points3DMap.end())
{
points3DMap.insert(std::make_pair(inliers[i], p));
wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(inliers[i])->second.pt)));
wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sTo.id(), sTo.getWords().lower_bound(inliers[i])->second.pt)));
}
else
{
float dist = uNorm(p.x - jter->second.x, p.y - jter->second.y, p.z - jter->second.z);
if(dist <= inlierDistance_)
{
// in case of loop closure links
wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(inliers[i])->second.pt)));
wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sTo.id(), sTo.getWords().lower_bound(inliers[i])->second.pt)));
}
}
}
}
else
{
UWARN("Not enough inliers (%d) between %d and %d", inliers.size(), sFrom.id(), sTo.id());
}
}
}
std::list<int> wordReferencesKeys = uUniqueKeys(wordReferences);
UDEBUG("points=%d frames=%d", (int)wordReferencesKeys.size(), (int)frames.size());
std::vector<cv::Point3f> points(wordReferencesKeys.size()); //npoints
UDEBUG("points=%d frames=%d", (int)wordReferences.size(), (int)frames.size());
std::vector<cv::Point3f> points(wordReferences.size()); //npoints
std::vector<std::vector<cv::Point2f> > imagePoints(frames.size()); //nframes -> npoints
std::vector<std::vector<int> > visibility(frames.size()); //nframes -> npoints
for(unsigned int i=0; i<frames.size(); ++i)
{
imagePoints[i].resize(wordReferencesKeys.size(), cv::Point2f(std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN()));
visibility[i].resize(wordReferencesKeys.size(), 0);
imagePoints[i].resize(wordReferences.size(), cv::Point2f(std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN()));
visibility[i].resize(wordReferences.size(), 0);
}
int i=0;
for(std::list<int>::iterator iter = wordReferencesKeys.begin(); iter!=wordReferencesKeys.end(); ++iter)
for(std::map<int, std::map<int, cv::Point2f> >::iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
{
points[i] = points3DMap.at(*iter);
points[i] = points3DMap.at(iter->first);
std::multimap<int, std::pair<int, cv::Point2f> >::iterator jter = wordReferences.lower_bound(*iter);
while(jter->first == *iter && jter != wordReferences.end())
for(std::map<int, cv::Point2f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
{
imagePoints[frameIdToIndex.at(jter->second.first)][i] = jter->second.second;
visibility[frameIdToIndex.at(jter->second.first)][i] = 1;
++jter;
imagePoints[frameIdToIndex.at(jter->first)][i] = jter->second;
visibility[frameIdToIndex.at(jter->first)][i] = 1;
}
++i;
}

314
corelib/src/OptimizerG2O.cpp
View File

@@ -33,6 +33,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <set>
#include <rtabmap/core/OptimizerG2O.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d_motion_estimation.h>
#ifdef RTABMAP_G2O
#include "g2o/config.h"
@@ -42,6 +44,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "g2o/core/optimization_algorithm_factory.h"
#include "g2o/core/optimization_algorithm_gauss_newton.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/linear_solver.h"
#include "g2o/types/sba/types_sba.h"
#include "g2o/core/robust_kernel_impl.h"
#ifdef G2O_HAVE_CSPARSE
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#endif
@@ -152,12 +157,10 @@ std::map<int, Transform> OptimizerG2O::optimize(
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
int solverApproach = 0;
int optimizationApproach = 1;
SlamBlockSolver * blockSolver = 0;
if(solverApproach == 2)
if(solver_ == 2)
{
#ifdef G2O_HAVE_CHOLMOD
//chmold
@@ -166,7 +169,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
blockSolver = new SlamBlockSolver(linearSolver);
#endif
}
else if(solverApproach == 0)
else if(solver_ == 0)
{
#ifdef G2O_HAVE_CSPARSE
//csparse
@@ -183,7 +186,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
blockSolver = new SlamBlockSolver(linearSolver);
}
if(optimizationApproach == 1)
if(optimizer_ == 1)
{
optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(blockSolver));
}
@@ -538,6 +541,307 @@ std::map<int, Transform> OptimizerG2O::optimize(
return optimizedPoses;
}
std::map<int, Transform> OptimizerG2O::optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures)
{
std::map<int, Transform> optimizedPoses;
#ifdef RTABMAP_G2O
UDEBUG("Optimizing graph...");
optimizedPoses.clear();
if(links.size()>=1 && poses.size()>=2 && iterations() > 0)
{
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
g2o::BlockSolver_6_3::LinearSolverType * linearSolver = 0;
bool robustKernel = true;
if(solver_ == 2)
{
#ifdef G2O_HAVE_CHOLMOD
//chmold
linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
#endif
}
else if(solver_ == 0)
{
#ifdef G2O_HAVE_CSPARSE
//csparse
linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>();
#endif
}
if(linearSolver == 0)
{
//pcg
linearSolver = new g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType>();
}
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
if(optimizer_ == 1)
{
optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(solver_ptr));
}
else
{
optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(solver_ptr));
}
std::map<int, Transform> frames = poses;
UDEBUG("fill poses to g2o...");
std::map<int, CameraModel> models;
for(std::map<int, Transform>::iterator iter=frames.begin(); iter!=frames.end(); )
{
// Get camera model
CameraModel model;
if(uContains(signatures, iter->first))
{
if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValidForProjection())
{
model = signatures.at(iter->first).sensorData().cameraModels()[0];
}
else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValidForProjection())
{
model = signatures.at(iter->first).sensorData().stereoCameraModel().left();
}
else
{
UERROR("Missing calibration for node %d", iter->first);
}
}
else
{
UERROR("Did not find node %d in cache", iter->first);
}
if(model.isValidForProjection())
{
models.insert(std::make_pair(iter->first, model));
Transform camPose = iter->second * model.localTransform();
//iter->second = (iter->second * model.localTransform()).inverse();
UDEBUG("%d t=%s", iter->first, camPose.prettyPrint().c_str());
// Add node's pose
UASSERT(!camPose.isNull());
g2o::VertexCam * vCam = new g2o::VertexCam();
Eigen::Affine3d a = camPose.toEigen3d();
g2o::SBACam cam(Eigen::Quaterniond(a.rotation()), a.translation());
cam.setKcam(model.fx(), model.fy(), model.cx(), model.cy(), 0);
vCam->setEstimate(cam);
if(iter->first == rootId)
{
vCam->setFixed(true);
}
vCam->setId(iter->first);
std::cout << cam << std::endl;
UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert vertex %d!?", iter->first).c_str());
++iter;
}
else
{
frames.erase(iter++);
}
}
UDEBUG("fill edges to g2o and associate each 3D point to all frames observing it...");
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
Link link = iter->second;
if(link.to() < link.from())
{
link = link.inverse();
}
if(uContains(signatures, link.from()) &&
uContains(signatures, link.to()) &&
uContains(frames, link.from()) &&
uContains(frames, link.to()))
{
// add edge
int id1 = iter->first;
int id2 = iter->second.to();
UASSERT(!iter->second.transform().isNull());
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
}
// between cameras, not base_link
Transform camLink = models.at(id1).localTransform().inverse()*iter->second.transform()*models.at(id2).localTransform();
//Transform t = iter->second.transform();
UDEBUG("added edge %d=%s -> %d=%s",
id1,
iter->second.transform().prettyPrint().c_str(),
id2,
camLink.prettyPrint().c_str());
Eigen::Affine3d a = camLink.toEigen3d();
g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setMeasurement(g2o::SE3Quat(a.rotation(), a.translation()));
e->setInformation(information);
if (!optimizer.addEdge(e))
{
delete e;
UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
}
}
}
std::map<int, cv::Point3f> points3DMap;
std::map<int, std::map<int, cv::Point2f> > wordReferences; // <ID words, IDs frames + keypoint>
this->computeBACorrespondences(frames, links, signatures, points3DMap, wordReferences);
UDEBUG("fill 3D points to g2o...");
int stepVertexId = frames.rbegin()->first+1;
for(std::map<int, std::map<int, cv::Point2f> >::iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
{
const cv::Point3f & pt3d = points3DMap.at(iter->first);
g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();
vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
vpt3d->setId(stepVertexId + iter->first);
vpt3d->setMarginalized(true);
optimizer.addVertex(vpt3d);
// set observations
for(std::map<int, cv::Point2f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
{
int camId = jter->first;
const cv::Point2f & pt = jter->second;
Eigen::Matrix<double,2,1> obs;
obs << pt.x, pt.y;
UDEBUG("Added observation pt=%d to cam=%d (%f,%f)", vpt3d->id(), camId, pt.x, pt.y);
g2o::EdgeProjectP2MC* e = new g2o::EdgeProjectP2MC();
e->setVertex(0, vpt3d);
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(camId)));
e->setMeasurement(obs);
e->setInformation(Eigen::Matrix2d::Identity());
if(robustKernel)
{
e->setRobustKernel(new g2o::RobustKernelHuber);
}
optimizer.addEdge(e);
}
}
UDEBUG("Initial optimization...");
optimizer.initializeOptimization();
UASSERT(optimizer.verifyInformationMatrices());
UINFO("g2o optimizing begin (max iterations=%d, robustKernel=%d)", iterations(), robustKernel?1:0);
int it = 0;
UTimer timer;
double lastError = 0.0;
if(this->epsilon() > 0.0)
{
for(int i=0; i<iterations(); ++i)
{
it += optimizer.optimize(1);
// early stop condition
optimizer.computeActiveErrors();
double chi2 = optimizer.activeRobustChi2();
UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
if(i>0 && (optimizer.activeRobustChi2() > 1000000000000.0 || !uIsFinite(optimizer.activeRobustChi2())))
{
UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!");
return optimizedPoses;
}
double errorDelta = lastError - chi2;
if(i>0 && errorDelta < this->epsilon())
{
if(errorDelta < 0)
{
UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
}
else
{
UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
break;
}
}
else if(i==0 && chi2 < this->epsilon())
{
UINFO("Stop optimizing, error is already under epsilon (%f < %f)", chi2, this->epsilon());
break;
}
lastError = chi2;
}
}
else
{
it = optimizer.optimize(iterations());
optimizer.computeActiveErrors();
UDEBUG("%d nodes, %d edges, chi2: %f", (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.activeRobustChi2());
}
UINFO("g2o optimizing end (%d iterations done, error=%f, time = %f s)", it, optimizer.activeRobustChi2(), timer.ticks());
if(optimizer.activeRobustChi2() > 1000000000000.0)
{
UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!");
return optimizedPoses;
}
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(iter->first);
if(v)
{
Transform t = Transform::fromEigen3d(v->estimate());
UDEBUG("%d t=%s", iter->first, t.prettyPrint().c_str());
// remove model local transform
t *= models.at(iter->first).localTransform().inverse();
optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
}
else
{
UERROR("Vertex %d not found!?", iter->first);
}
}
}
else if(poses.size() == 1 || iterations() <= 0)
{
optimizedPoses = poses;
}
else
{
UWARN("This method should be called at least with 1 pose!");
}
UDEBUG("Optimizing graph...end!");
#else
UERROR("Not built with G2O support!");
#endif
return optimizedPoses;
}
bool OptimizerG2O::saveGraph(
const std::string & fileName,
const std::map<int, Transform> & poses,

4
corelib/src/Rtabmap.cpp
View File

@@ -3160,7 +3160,8 @@ void Rtabmap::get3DMap(
data.setId(*iter);
std::multimap<int, cv::KeyPoint> words;
std::multimap<int, cv::Point3f> words3;
_memory->getNodeWords(*iter, words, words3);
std::multimap<int, cv::Mat> wordsDescriptors;
_memory->getNodeWords(*iter, words, words3, wordsDescriptors);
signatures.insert(std::make_pair(*iter,
Signature(*iter,
mapId,
@@ -3172,6 +3173,7 @@ void Rtabmap::get3DMap(
data)));
signatures.at(*iter).setWords(words);
signatures.at(*iter).setWords3(words3);
signatures.at(*iter).setWordsDescriptors(wordsDescriptors);
}
}
else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1))

8
corelib/src/util3d_features.cpp
View File

@@ -100,7 +100,7 @@ std::vector<cv::Point3f> generateKeypoints3DDepth(
cv::Point3f pt(bad_point, bad_point, bad_point);
if(pcl::isFinite(ptXYZ) &&
(minDepth <= 0.0f || ptXYZ.z >= minDepth) &&
(minDepth < 0.0f || ptXYZ.z > minDepth) &&
(maxDepth <= 0.0f || ptXYZ.z <= maxDepth))
{
pt = cv::Point3f(ptXYZ.x, ptXYZ.y, ptXYZ.z);
@@ -137,7 +137,7 @@ std::vector<cv::Point3f> generateKeypoints3DDisparity(
cv::Point3f pt(bad_point, bad_point, bad_point);
if(util3d::isFinite(tmpPt) &&
(minDepth <= 0.0f || tmpPt.z >= minDepth) &&
(minDepth < 0.0f || tmpPt.z > minDepth) &&
(maxDepth <= 0.0f || tmpPt.z <= maxDepth))
{
pt = tmpPt;
@@ -181,7 +181,7 @@ std::vector<cv::Point3f> generateKeypoints3DStereo(
model);
if(util3d::isFinite(tmpPt) &&
(minDepth <= 0.0f || tmpPt.z >= minDepth) &&
(minDepth < 0.0f || tmpPt.z > minDepth) &&
(maxDepth <= 0.0f || tmpPt.z <= maxDepth))
{
pt = tmpPt;
@@ -221,7 +221,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
std::map<int, cv::Point3f> words3D;
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
int pairsFound = EpipolarGeometry::findPairs(refWords, nextWords, pairs);
UDEBUG("pairsFound=%d", pairsFound);
UDEBUG("pairsFound=%d/%d", pairsFound, int(refWords.size()>nextWords.size()?refWords.size():nextWords.size()));
if(pairsFound > 8)
{
std::vector<unsigned char> status;