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

MacOSX: (g2o) Fixed cholmod library not found. New ICP parameter: "Icp/Epsilon". Fixed null ICP_inliers_ratio with odometry statistics.

Browse Source
This commit is contained in:
matlabbe
2016-01-12 11:37:50 -05:00
parent 42e1590abe
commit 5136b38111
15 changed files with 82 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cmake_modules/FindG2O.cmake
+3 -2
View File
@@ -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()
corelib/include/rtabmap/core/OdometryInfo.h
+1 -1
View File
@@ -53,7 +53,7 @@ public:
bool lost;
int matches;
int inliers;
int icpInliersRatio;
float icpInliersRatio;
float variance;
int features;
int localMapSize;
corelib/include/rtabmap/core/Parameters.h
+1
View File
@@ -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.");
corelib/include/rtabmap/core/RegistrationIcp.h
+1
View File
@@ -61,6 +61,7 @@ private:
int _downsamplingStep;
float _maxCorrespondenceDistance;
int _maxIterations;
float _epsilon;
float _correspondenceRatio;
bool _pointToPlane;
int _pointToPlaneNormalNeighbors;
corelib/include/rtabmap/core/util3d_registration.h
+1
View File
@@ -75,6 +75,7 @@ Transform RTABMAP_EXP icp(
int maximumIterations,
bool & hasConverged,
pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
double epsilon = 0,
bool icp2D = false);
Transform RTABMAP_EXP icpPointToPlane(
corelib/src/Registration.cpp
+13 -12
View File
@@ -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;
}
corelib/src/RegistrationIcp.cpp
+4
View File
@@ -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
}
corelib/src/toro3d/posegraph.hxx
+1 -1
View File
@@ -671,7 +671,7 @@ bool TreePoseGraph<Ops>::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;
}
}
corelib/src/util3d_registration.cpp
+2 -1
View File
@@ -294,6 +294,7 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
int maximumIterations,
bool & hasConverged,
pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
double epsilon,
bool icp2D)
{
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
@@ -313,7 +314,7 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::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);
corelib/src/vertigo/g2o/edge_se2MaxMixture.cpp
+3 -3
View File
@@ -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<EdgeSE2MaxMixture*>(element);
@@ -117,6 +117,6 @@ void EdgeSE2MaxMixture::computeError()
return this;
}
#endif
*/
corelib/src/vertigo/g2o/edge_se2Switchable.cpp
+3 -3
View File
@@ -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<EdgeSE2Switchable*>(element);
@@ -128,4 +128,4 @@ void EdgeSE2Switchable::computeError()
return this;
}
#endif
*/
corelib/src/vertigo/g2o/edge_se3Switchable.cpp
+3 -2
View File
@@ -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<EdgeSE3Switchable*>(element);
@@ -118,3 +118,4 @@ void EdgeSE3Switchable::computeError()
return this;
}
#endif
*/
corelib/src/vertigo/g2o/types_g2o_robust.cpp
+2 -1
View File
@@ -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
*/
guilib/src/PreferencesDialog.cpp
+1
View File
@@ -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());
guilib/src/ui/preferencesDialog.ui
+43 -8
View File
@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-880</y>
<y>0</y>
<width>676</width>
<height>1982</height>
</rect>
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>3</number>
<number>14</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -8923,7 +8923,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</item>
<item>
<layout class="QGridLayout" name="gridLayout_48" columnstretch="0,1">
<item row="7" column="0">
<item row="8" column="0">
<widget class="QCheckBox" name="loopClosure_icpPointToPlane">
<property name="text">
<string/>
@@ -8969,7 +8969,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="6" column="0">
<item row="7" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpRatio">
<property name="minimum">
<double>0.000000000000000</double>
@@ -8985,7 +8985,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="6" column="1">
<item row="7" column="1">
<widget class="QLabel" name="label_133">
<property name="text">
<string>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.</string>
@@ -8998,7 +8998,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="8" column="1">
<item row="9" column="1">
<widget class="QLabel" name="label_212">
<property name="text">
<string>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.</string>
@@ -9011,7 +9011,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="8" column="0">
<item row="9" column="0">
<widget class="QSpinBox" name="loopClosure_icpPointToPlaneNormals">
<property name="minimum">
<number>1</number>
@@ -9165,7 +9165,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="7" column="1">
<item row="8" column="1">
<widget class="QLabel" name="label_144">
<property name="text">
<string>Point to plane ICP. Only for ICP 3D.</string>
@@ -9178,6 +9178,41 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_136">
<property name="text">
<string>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.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpEpsilon">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>4</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
</layout>