diff --git a/cmake_modules/FindG2O.cmake b/cmake_modules/FindG2O.cmake index a16cd204..60309bbf 100644 --- a/cmake_modules/FindG2O.cmake +++ b/cmake_modules/FindG2O.cmake @@ -66,6 +66,7 @@ ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER # G2O itself declared found if we found the core libraries and at least one solver SET(G2O_FOUND "NO") +FIND_LIBRARY(CHOLMOD_LIB cholmod) IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND AND CSPARSE_FOUND) SET(G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIR} ${CSPARSE_INCLUDE_DIR}) SET(G2O_LIBRARIES @@ -77,8 +78,8 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FO ${G2O_TYPES_SLAM2D} ${G2O_TYPES_SLAM3D} ${CSPARSE_LIBRARY} - cholmod) + ${CHOLMOD_LIB}) SET(G2O_FOUND "YES") ELSEIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR) MESSAGE(STATUS "g2o core libraries found but some solvers are missing. Make sure to install \"libsuitesparse-dev\" before building/installing g2o.") -ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR) +ENDIF() diff --git a/corelib/include/rtabmap/core/OdometryInfo.h b/corelib/include/rtabmap/core/OdometryInfo.h index 874bceec..4f90fa9e 100644 --- a/corelib/include/rtabmap/core/OdometryInfo.h +++ b/corelib/include/rtabmap/core/OdometryInfo.h @@ -53,7 +53,7 @@ public: bool lost; int matches; int inliers; - int icpInliersRatio; + float icpInliersRatio; float variance; int features; int localMapSize; diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index c56e0745..ccb017f1 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -398,6 +398,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Icp, DownsamplingStep, int, 1, "Downsampling step size (1=no sampling). This is done before uniform sampling."); RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05, "Max distance for point correspondences."); RTABMAP_PARAM(Icp, Iterations, int, 10, "Max iterations."); + RTABMAP_PARAM(Icp, Epsilon, float, 0.0, "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution."); RTABMAP_PARAM(Icp, CorrespondenceRatio, float, 0.3, "Ratio of matching correspondences to accept the transform."); RTABMAP_PARAM(Icp, PointToPlane, bool, false, "Use point to plane ICP."); RTABMAP_PARAM(Icp, PointToPlaneNormalNeighbors, int, 20, "Number of neighbors to compute normals for point to plane."); diff --git a/corelib/include/rtabmap/core/RegistrationIcp.h b/corelib/include/rtabmap/core/RegistrationIcp.h index 60c8c0df..e801972b 100644 --- a/corelib/include/rtabmap/core/RegistrationIcp.h +++ b/corelib/include/rtabmap/core/RegistrationIcp.h @@ -61,6 +61,7 @@ private: int _downsamplingStep; float _maxCorrespondenceDistance; int _maxIterations; + float _epsilon; float _correspondenceRatio; bool _pointToPlane; int _pointToPlaneNormalNeighbors; diff --git a/corelib/include/rtabmap/core/util3d_registration.h b/corelib/include/rtabmap/core/util3d_registration.h index ecce84cb..2f5c7d27 100644 --- a/corelib/include/rtabmap/core/util3d_registration.h +++ b/corelib/include/rtabmap/core/util3d_registration.h @@ -75,6 +75,7 @@ Transform RTABMAP_EXP icp( int maximumIterations, bool & hasConverged, pcl::PointCloud & cloud_source_registered, + double epsilon = 0, bool icp2D = false); Transform RTABMAP_EXP icpPointToPlane( diff --git a/corelib/src/Registration.cpp b/corelib/src/Registration.cpp index 7df00c0c..2ba01cf7 100644 --- a/corelib/src/Registration.cpp +++ b/corelib/src/Registration.cpp @@ -185,18 +185,8 @@ Transform Registration::computeTransformationMod( { info = *infoOut; } + Transform t = computeTransformationImpl(from, to, guess, info); - if(child_) - { - if(!t.isNull()) - { - t = child_->computeTransformationMod(from, to, force3DoF_?t.to3DoF():t, &info); - } - } - else if(!t.isNull() && force3DoF_) - { - t = t.to3DoF(); - } if(varianceFromInliersCount_) { @@ -211,11 +201,22 @@ Transform Registration::computeTransformationMod( info.variance = info.variance>0.0f?info.variance:0.0001f; // epsilon if exact transform } + if(child_) + { + if(!t.isNull()) + { + t = child_->computeTransformationMod(from, to, force3DoF_?t.to3DoF():t, &info); + } + } + else if(!t.isNull() && force3DoF_) + { + t = t.to3DoF(); + } + if(infoOut) { *infoOut = info; } - return t; } diff --git a/corelib/src/RegistrationIcp.cpp b/corelib/src/RegistrationIcp.cpp index 5a15a045..fc974c3f 100644 --- a/corelib/src/RegistrationIcp.cpp +++ b/corelib/src/RegistrationIcp.cpp @@ -48,6 +48,7 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration _downsamplingStep(Parameters::defaultIcpDownsamplingStep()), _maxCorrespondenceDistance(Parameters::defaultIcpMaxCorrespondenceDistance()), _maxIterations(Parameters::defaultIcpIterations()), + _epsilon(Parameters::defaultIcpEpsilon()), _correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()), _pointToPlane(Parameters::defaultIcpPointToPlane()), _pointToPlaneNormalNeighbors(Parameters::defaultIcpPointToPlaneNormalNeighbors()) @@ -65,6 +66,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kIcpDownsamplingStep(), _downsamplingStep); Parameters::parse(parameters, Parameters::kIcpMaxCorrespondenceDistance(), _maxCorrespondenceDistance); Parameters::parse(parameters, Parameters::kIcpIterations(), _maxIterations); + Parameters::parse(parameters, Parameters::kIcpEpsilon(), _epsilon); Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _correspondenceRatio); Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane); Parameters::parse(parameters, Parameters::kIcpPointToPlaneNormalNeighbors(), _pointToPlaneNormalNeighbors); @@ -73,6 +75,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters) UASSERT_MSG(_downsamplingStep >= 0, uFormat("value=%d", _downsamplingStep).c_str()); UASSERT_MSG(_maxCorrespondenceDistance > 0.0f, uFormat("value=%f", _maxCorrespondenceDistance).c_str()); UASSERT_MSG(_maxIterations > 0, uFormat("value=%d", _maxIterations).c_str()); + UASSERT(_epsilon >= 0.0f); UASSERT_MSG(_correspondenceRatio >=0.0f && _correspondenceRatio <=1.0f, uFormat("value=%f", _correspondenceRatio).c_str()); UASSERT_MSG(_pointToPlaneNormalNeighbors > 0, uFormat("value=%d", _pointToPlaneNormalNeighbors).c_str()); } @@ -219,6 +222,7 @@ Transform RegistrationIcp::computeTransformationImpl( _maxIterations, hasConverged, *fromCloudRegistered, + _epsilon, !this->force3DoF()); // icp2D } diff --git a/corelib/src/toro3d/posegraph.hxx b/corelib/src/toro3d/posegraph.hxx index 06489f5d..96d2cf68 100644 --- a/corelib/src/toro3d/posegraph.hxx +++ b/corelib/src/toro3d/posegraph.hxx @@ -671,7 +671,7 @@ bool TreePoseGraph::sanityCheck(){ const EdgeList& children=it->second->children; for (typename EdgeList::const_iterator lt=children.begin(); lt!=children.end(); lt++){ if ((*lt)->v1!=v){ - std::cerr << "wrong direction of the edges" << std::cerr; + std::cerr << "wrong direction of the edges" << std::endl; return false; } } diff --git a/corelib/src/util3d_registration.cpp b/corelib/src/util3d_registration.cpp index 6e110580..016c0531 100644 --- a/corelib/src/util3d_registration.cpp +++ b/corelib/src/util3d_registration.cpp @@ -294,6 +294,7 @@ Transform icp(const pcl::PointCloud::ConstPtr & cloud_source, int maximumIterations, bool & hasConverged, pcl::PointCloud & cloud_source_registered, + double epsilon, bool icp2D) { pcl::IterativeClosestPoint icp; @@ -313,7 +314,7 @@ Transform icp(const pcl::PointCloud::ConstPtr & cloud_source, // Set the maximum number of iterations (criterion 1) icp.setMaximumIterations (maximumIterations); // Set the transformation epsilon (criterion 2) - //icp.setTransformationEpsilon (1e-8); + icp.setTransformationEpsilon (epsilon); // Set the euclidean distance difference epsilon (criterion 3) //icp.setEuclideanFitnessEpsilon (1); //icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance); diff --git a/corelib/src/vertigo/g2o/edge_se2MaxMixture.cpp b/corelib/src/vertigo/g2o/edge_se2MaxMixture.cpp index 41407b83..be4a17c0 100644 --- a/corelib/src/vertigo/g2o/edge_se2MaxMixture.cpp +++ b/corelib/src/vertigo/g2o/edge_se2MaxMixture.cpp @@ -88,13 +88,13 @@ void EdgeSE2MaxMixture::computeError() } - +/* // ================================================ #ifdef G2O_HAVE_OPENGL EdgeSE2MaxMixtureDrawAction::EdgeSE2MaxMixtureDrawAction(): DrawAction(typeid(EdgeSE2MaxMixture).name()){} g2o::HyperGraphElementAction* EdgeSE2MaxMixtureDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element, - g2o::HyperGraphElementAction::Parameters* /*params_*/){ + g2o::HyperGraphElementAction::Parameters* ){ if (typeid(*element).name()!=_typeName) return 0; EdgeSE2MaxMixture* e = static_cast(element); @@ -117,6 +117,6 @@ void EdgeSE2MaxMixture::computeError() return this; } #endif - +*/ diff --git a/corelib/src/vertigo/g2o/edge_se2Switchable.cpp b/corelib/src/vertigo/g2o/edge_se2Switchable.cpp index 1e6deffd..710afdab 100644 --- a/corelib/src/vertigo/g2o/edge_se2Switchable.cpp +++ b/corelib/src/vertigo/g2o/edge_se2Switchable.cpp @@ -102,12 +102,12 @@ void EdgeSE2Switchable::computeError() _error = delta.toVector() * v3->estimate(); } - +/* #ifdef G2O_HAVE_OPENGL EdgeSE2SwitchableDrawAction::EdgeSE2SwitchableDrawAction(): DrawAction(typeid(EdgeSE2Switchable).name()){} g2o::HyperGraphElementAction* EdgeSE2SwitchableDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element, - g2o::HyperGraphElementAction::Parameters* /*params_*/){ + g2o::HyperGraphElementAction::Parameters* ){ if (typeid(*element).name()!=_typeName) return 0; EdgeSE2Switchable* e = static_cast(element); @@ -128,4 +128,4 @@ void EdgeSE2Switchable::computeError() return this; } #endif - +*/ diff --git a/corelib/src/vertigo/g2o/edge_se3Switchable.cpp b/corelib/src/vertigo/g2o/edge_se3Switchable.cpp index d5b8a4ed..88d9e81b 100644 --- a/corelib/src/vertigo/g2o/edge_se3Switchable.cpp +++ b/corelib/src/vertigo/g2o/edge_se3Switchable.cpp @@ -92,12 +92,12 @@ void EdgeSE3Switchable::computeError() _error = g2o::internal::toVectorMQT(delta) * v3->estimate(); } - +/* #ifdef G2O_HAVE_OPENGL EdgeSE3SwitchableDrawAction::EdgeSE3SwitchableDrawAction(): DrawAction(typeid(EdgeSE3Switchable).name()){} g2o::HyperGraphElementAction* EdgeSE3SwitchableDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element, - g2o::HyperGraphElementAction::Parameters* /*params_*/){ + g2o::HyperGraphElementAction::Parameters* ){ if (typeid(*element).name()!=_typeName) return 0; EdgeSE3Switchable* e = static_cast(element); @@ -118,3 +118,4 @@ void EdgeSE3Switchable::computeError() return this; } #endif +*/ diff --git a/corelib/src/vertigo/g2o/types_g2o_robust.cpp b/corelib/src/vertigo/g2o/types_g2o_robust.cpp index f1553f5f..5439d0c6 100644 --- a/corelib/src/vertigo/g2o/types_g2o_robust.cpp +++ b/corelib/src/vertigo/g2o/types_g2o_robust.cpp @@ -14,9 +14,10 @@ G2O_REGISTER_TYPE(EDGE_SE2_SWITCHABLE, EdgeSE2Switchable); G2O_REGISTER_TYPE(EDGE_SE3_SWITCHABLE, EdgeSE3Switchable); G2O_REGISTER_TYPE(VERTEX_SWITCH, VertexSwitchLinear); +/* #ifdef G2O_HAVE_OPENGL G2O_REGISTER_ACTION(EdgeSE2SwitchableDrawAction); //G2O_REGISTER_ACTION(EdgeSE2MaxMixtureDrawAction); G2O_REGISTER_ACTION(EdgeSE3SwitchableDrawAction); #endif - +*/ diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index fd7d29ad..0c3deff4 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -672,6 +672,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->loopClosure_icpDownsamplingStep->setObjectName(Parameters::kIcpDownsamplingStep().c_str()); _ui->loopClosure_icpMaxCorrespondenceDistance->setObjectName(Parameters::kIcpMaxCorrespondenceDistance().c_str()); _ui->loopClosure_icpIterations->setObjectName(Parameters::kIcpIterations().c_str()); + _ui->loopClosure_icpEpsilon->setObjectName(Parameters::kIcpEpsilon().c_str()); _ui->loopClosure_icpRatio->setObjectName(Parameters::kIcpCorrespondenceRatio().c_str()); _ui->loopClosure_icpPointToPlane->setObjectName(Parameters::kIcpPointToPlane().c_str()); _ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kIcpPointToPlaneNormalNeighbors().c_str()); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 76b4a6ff..19bca84c 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -880 + 0 676 1982 @@ -86,7 +86,7 @@ QFrame::Raised - 3 + 14 @@ -8923,7 +8923,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + @@ -8969,7 +8969,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + 0.000000000000000 @@ -8985,7 +8985,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + Ratio of matching correspondences to accept the transform. If the maximum laser scans is set, this is the minimum ratio of correspondences on laser scan maximum size to accept the transform. @@ -8998,7 +8998,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + Number of neighbors to compute normals for point to plane. Normals won't be recomputed if uniform sampling is disabled and that there are already normals in the laser scans. @@ -9011,7 +9011,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + 1 @@ -9165,7 +9165,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + Point to plane ICP. Only for ICP 3D. @@ -9178,6 +9178,41 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare + + + + Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + m + + + 4 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.000000000000000 + + +