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

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

2
corelib/include/rtabmap/core/OdometryInfo.h
View File

@@ -53,7 +53,7 @@ public:
bool lost;
int matches;
int inliers;
int icpInliersRatio;
float icpInliersRatio;
float variance;
int features;
int localMapSize;

1
corelib/include/rtabmap/core/Parameters.h
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.");

1
corelib/include/rtabmap/core/RegistrationIcp.h
View File

@@ -61,6 +61,7 @@ private:
int _downsamplingStep;
float _maxCorrespondenceDistance;
int _maxIterations;
float _epsilon;
float _correspondenceRatio;
bool _pointToPlane;
int _pointToPlaneNormalNeighbors;

1
corelib/include/rtabmap/core/util3d_registration.h
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(

25
corelib/src/Registration.cpp
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;
}

4
corelib/src/RegistrationIcp.cpp
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
}

2
corelib/src/toro3d/posegraph.hxx
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;
}
}

3
corelib/src/util3d_registration.cpp
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);

6
corelib/src/vertigo/g2o/edge_se2MaxMixture.cpp
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
*/

6
corelib/src/vertigo/g2o/edge_se2Switchable.cpp
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
*/

5
corelib/src/vertigo/g2o/edge_se3Switchable.cpp
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
*/

3
corelib/src/vertigo/g2o/types_g2o_robust.cpp
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
*/