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

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!!!)