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

Added Icp/ReciprocalCorrespondences (new default true)

Browse Source
This commit is contained in:
matlabbe
2023-04-06 20:57:35 -07:00
parent 141f2d3334
commit 8b8a512cbb
8 changed files with 396 additions and 359 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/include/rtabmap/core/Parameters.h
View File

@@ -663,7 +663,8 @@ class RTABMAP_CORE_EXPORT Parameters
#else
RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05, "Max distance for point correspondences.");
#endif
RTABMAP_PARAM(Icp, Iterations, int, 30, "Max iterations.");
RTABMAP_PARAM(Icp, ReciprocalCorrespondences, bool, true, "To be a valid correspondence, the corresponding point in target cloud to point in source cloud should be both their closest closest correspondence.");
RTABMAP_PARAM(Icp, Iterations, int, 30, "Max iterations.");
RTABMAP_PARAM(Icp, Epsilon, float, 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.1, "Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(Icp, Force4DoF, bool, false, uFormat("Limit ICP to x, y, z and yaw DoF. Available if %s > 0.", kIcpStrategy().c_str()));

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

@@ -64,6 +64,7 @@ private:
float _rangeMin;
float _rangeMax;
float _maxCorrespondenceDistance;
bool _reciprocalCorrespondences;
int _maxIterations;
float _epsilon;
float _correspondenceRatio;

12
corelib/include/rtabmap/core/util3d_registration.h
View File

@@ -65,26 +65,30 @@ void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences(
double maxCorrespondenceDistance,
double maxCorrespondenceAngle, // <=0 means that we don't care about normal angle difference
double & variance,
int & correspondencesOut);
int & correspondencesOut,
bool reciprocal);
void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences(
const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudA,
const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudB,
double maxCorrespondenceDistance,
double maxCorrespondenceAngle, // <=0 means that we don't care about normal angle difference
double & variance,
int & correspondencesOut);
int & correspondencesOut,
bool reciprocal);
void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudA,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudB,
double maxCorrespondenceDistance,
double & variance,
int & correspondencesOut);
int & correspondencesOut,
bool reciprocal);
void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences(
const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudA,
const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudB,
double maxCorrespondenceDistance,
double & variance,
int & correspondencesOut);
int & correspondencesOut,
bool reciprocal);
Transform RTABMAP_CORE_EXPORT icp(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,

8
corelib/src/RegistrationIcp.cpp
View File

@@ -64,6 +64,7 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
_rangeMin(Parameters::defaultIcpRangeMin()),
_rangeMax(Parameters::defaultIcpRangeMax()),
_maxCorrespondenceDistance(Parameters::defaultIcpMaxCorrespondenceDistance()),
_reciprocalCorrespondences(Parameters::defaultIcpReciprocalCorrespondences()),
_maxIterations(Parameters::defaultIcpIterations()),
_epsilon(Parameters::defaultIcpEpsilon()),
_correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()),
@@ -109,6 +110,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kIcpRangeMin(), _rangeMin);
Parameters::parse(parameters, Parameters::kIcpRangeMax(), _rangeMax);
Parameters::parse(parameters, Parameters::kIcpMaxCorrespondenceDistance(), _maxCorrespondenceDistance);
Parameters::parse(parameters, Parameters::kIcpReciprocalCorrespondences(), _reciprocalCorrespondences);
Parameters::parse(parameters, Parameters::kIcpIterations(), _maxIterations);
Parameters::parse(parameters, Parameters::kIcpEpsilon(), _epsilon);
Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _correspondenceRatio);
@@ -649,7 +651,8 @@ Transform RegistrationIcp::computeTransformationImpl(
_maxCorrespondenceDistance,
_maxRotation,
variance,
correspondences);
correspondences,
_reciprocalCorrespondences);
}
}
////////////////////
@@ -840,7 +843,8 @@ Transform RegistrationIcp::computeTransformationImpl(
toCloud,
_maxCorrespondenceDistance,
variance,
correspondences);
correspondences,
_reciprocalCorrespondences);
}
} // END Registration PointToPLane to PointToPoint
UDEBUG("ICP (iterations=%d) time = %f s", _maxIterations, timer.ticks());

30
corelib/src/util3d_registration.cpp
View File

@@ -244,7 +244,8 @@ void computeVarianceAndCorrespondencesImpl(
double maxCorrespondenceDistance,
double maxCorrespondenceAngle,
double & variance,
int & correspondencesOut)
int & correspondencesOut,
bool reciprocal)
{
variance = 1;
correspondencesOut = 0;
@@ -255,7 +256,7 @@ void computeVarianceAndCorrespondencesImpl(
est->setInputTarget(target);
est->setInputSource(source);
pcl::Correspondences correspondences;
est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
if(correspondences.size())
{
@@ -305,9 +306,10 @@ void computeVarianceAndCorrespondences(
double maxCorrespondenceDistance,
double maxCorrespondenceAngle,
double & variance,
int & correspondencesOut)
int & correspondencesOut,
bool reciprocal)
{
computeVarianceAndCorrespondencesImpl<pcl::PointNormal>(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut);
computeVarianceAndCorrespondencesImpl<pcl::PointNormal>(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut, reciprocal);
}
void computeVarianceAndCorrespondences(
@@ -316,9 +318,10 @@ void computeVarianceAndCorrespondences(
double maxCorrespondenceDistance,
double maxCorrespondenceAngle,
double & variance,
int & correspondencesOut)
int & correspondencesOut,
bool reciprocal)
{
computeVarianceAndCorrespondencesImpl<pcl::PointXYZINormal>(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut);
computeVarianceAndCorrespondencesImpl<pcl::PointXYZINormal>(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut, reciprocal);
}
template<typename PointT>
@@ -327,7 +330,8 @@ void computeVarianceAndCorrespondencesImpl(
const typename pcl::PointCloud<PointT>::ConstPtr & cloudB,
double maxCorrespondenceDistance,
double & variance,
int & correspondencesOut)
int & correspondencesOut,
bool reciprocal)
{
variance = 1;
correspondencesOut = 0;
@@ -336,7 +340,7 @@ void computeVarianceAndCorrespondencesImpl(
est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB);
est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA);
pcl::Correspondences correspondences;
est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
if(correspondences.size()>=3)
{
@@ -360,9 +364,10 @@ void computeVarianceAndCorrespondences(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudB,
double maxCorrespondenceDistance,
double & variance,
int & correspondencesOut)
int & correspondencesOut,
bool reciprocal)
{
computeVarianceAndCorrespondencesImpl<pcl::PointXYZ>(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut);
computeVarianceAndCorrespondencesImpl<pcl::PointXYZ>(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut, reciprocal);
}
void computeVarianceAndCorrespondences(
@@ -370,9 +375,10 @@ void computeVarianceAndCorrespondences(
const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudB,
double maxCorrespondenceDistance,
double & variance,
int & correspondencesOut)
int & correspondencesOut,
bool reciprocal)
{
computeVarianceAndCorrespondencesImpl<pcl::PointXYZI>(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut);
computeVarianceAndCorrespondencesImpl<pcl::PointXYZI>(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut, reciprocal);
}
// return transform from source to target (All points must be finite!!!)