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

New parameters: max accepted ICP translation error, new map on large odometry error

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1126 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-02-19 19:00:38 +00:00
parent 5dbf6678ce
commit b44e01a3bb
8 changed files with 140 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -204,6 +204,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, ScanMatchingSize, int, 0, "Laser scan matching history for odometry correction (laser scans are required). Set to 0 to disable odometry correction.");
RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.0, "Min linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.0, "Min angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 1, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
RTABMAP_PARAM(RGBD, ToroIterations, int, 100, "TORO graph optimization iterations")
// Local loop closure detection
@@ -238,6 +239,7 @@ class RTABMAP_EXP Parameters
// Loop closure constraint
RTABMAP_PARAM(LccIcp, Type, int, 0, "0=No ICP, 1=ICP 3D, 2=ICP 2D");
RTABMAP_PARAM(LccIcp, MaxDistance, float, 0.2, "Maximum ICP correction distance accepted (m).");
RTABMAP_PARAM(LccBow, MinInliers, int, 20, "Minimum visual word correspondences to compute geometry transform.");
RTABMAP_PARAM(LccBow, InlierDistance, float, 0.01, "Maximum distance for visual word correspondences.");

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

@@ -141,7 +141,9 @@ private:
bool _rgbdSlamMode;
float _rgbdLinearUpdate;
float _rgbdAngularUpdate;
float _newMapOdomChangeDistance;
int _globalLoopClosureIcpType;
float _globalLoopClosureIcpMaxDistance;
int _scanMatchingSize;
bool _localLoopClosureDetectionTime;
bool _localLoopClosureDetectionSpace;

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

@@ -52,6 +52,9 @@ public:
Transform translation() const;
void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
void getTranslation(float & x, float & y, float & z) const;
float getNorm() const;
float getNormSquared() const;
std::string prettyPrint() const;
Transform operator*(const Transform & t) const;

57
corelib/src/Rtabmap.cpp
View File

@@ -79,7 +79,9 @@ Rtabmap::Rtabmap() :
_rgbdSlamMode(Parameters::defaultRGBDEnabled()),
_rgbdLinearUpdate(Parameters::defaultRGBDLinearUpdate()),
_rgbdAngularUpdate(Parameters::defaultRGBDAngularUpdate()),
_newMapOdomChangeDistance(Parameters::defaultRGBDNewMapOdomChangeDistance()),
_globalLoopClosureIcpType(Parameters::defaultLccIcpType()),
_globalLoopClosureIcpMaxDistance(Parameters::defaultLccIcpMaxDistance()),
_scanMatchingSize(Parameters::defaultRGBDScanMatchingSize()),
_localLoopClosureDetectionTime(Parameters::defaultRGBDLocalLoopDetectionTime()),
_localLoopClosureDetectionSpace(Parameters::defaultRGBDLocalLoopDetectionSpace()),
@@ -327,7 +329,9 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDEnabled(), _rgbdSlamMode);
Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rgbdLinearUpdate);
Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rgbdAngularUpdate);
Parameters::parse(parameters, Parameters::kRGBDNewMapOdomChangeDistance(), _newMapOdomChangeDistance);
Parameters::parse(parameters, Parameters::kRGBDScanMatchingSize(), _scanMatchingSize);
Parameters::parse(parameters, Parameters::kLccIcpMaxDistance(), _globalLoopClosureIcpMaxDistance);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionTime(), _localLoopClosureDetectionTime);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionSpace(), _localLoopClosureDetectionSpace);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionRadius(), _localDetectRadius);
@@ -739,11 +743,7 @@ bool Rtabmap::process(const Image & image)
// Detect if the odometry is reset. If yes, trigger a new map.
if(_memory->getLastWorkingSignature())
{
Transform lastPose = _memory->getLastWorkingSignature()->getPose(); // use raw odometry
Transform lastPoseToNewPose = lastPose.inverse() * image.pose();
float x,y,z, roll,pitch,yaw;
lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
// TODO Increment map id also if there is a big position change
const Transform & lastPose = _memory->getLastWorkingSignature()->getPose(); // use raw odometry
if(!lastPose.isIdentity() && image.pose().isIdentity())
{
int mapId = _memory->incrementMapId();
@@ -751,6 +751,23 @@ bool Rtabmap::process(const Image & image)
_optimizedPoses.clear();
_constraints.clear();
}
else
{
Transform lastPoseToNewPose = lastPose.inverse() * image.pose();
float x,y,z, roll,pitch,yaw;
lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
if(_newMapOdomChangeDistance > 0.0 && (x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance)
{
int mapId = _memory->incrementMapId();
UWARN("Odometry is reset (large odometry change detected > %f). A new map (%d) is created! Last pose = %s, new pose = %s",
_newMapOdomChangeDistance,
mapId,
lastPose.prettyPrint().c_str(),
image.pose().prettyPrint().c_str());
_optimizedPoses.clear();
_constraints.clear();
}
}
}
}
}
@@ -892,7 +909,20 @@ bool Rtabmap::process(const Image & image)
Transform transform = _memory->computeVisualTransform(*iter, signature->id());
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
{
transform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1);
Transform icpTransform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1);
float squaredNorm = (transform.inverse()*icpTransform).getNormSquared();
if(!icpTransform.isNull() &&
_globalLoopClosureIcpMaxDistance>0.0f &&
squaredNorm > _globalLoopClosureIcpMaxDistance*_globalLoopClosureIcpMaxDistance)
{
UWARN("Local loop closure rejected (%d->%d) (ICP correction too large %f > %f [squared norm])",
signature->id(),
*iter,
squaredNorm,
_globalLoopClosureIcpMaxDistance*_globalLoopClosureIcpMaxDistance);
icpTransform.setNull();
}
transform = icpTransform;
}
if(!transform.isNull())
{
@@ -1218,7 +1248,20 @@ bool Rtabmap::process(const Image & image)
transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id());
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
{
transform = _memory->computeIcpTransform(_lcHypothesisId, signature->id(), transform, _globalLoopClosureIcpType == 1);
Transform icpTransform = _memory->computeIcpTransform(_lcHypothesisId, signature->id(), transform, _globalLoopClosureIcpType == 1);
float squaredNorm = (transform.inverse()*icpTransform).getNormSquared();
if(!icpTransform.isNull() &&
_globalLoopClosureIcpMaxDistance>0.0f &&
squaredNorm > _globalLoopClosureIcpMaxDistance*_globalLoopClosureIcpMaxDistance)
{
UWARN("Global loop closure rejected (%d->%d) (ICP correction too large %f > %f [squared norm])",
signature->id(),
_lcHypothesisId,
squaredNorm,
_globalLoopClosureIcpMaxDistance*_globalLoopClosureIcpMaxDistance);
icpTransform.setNull();
}
transform = icpTransform;
}
rejectedHypothesis = transform.isNull();
if(rejectedHypothesis)

17
corelib/src/Transform.cpp
View File

@@ -143,6 +143,23 @@ void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, fl
pcl::getTranslationAndEulerAngles(util3d::transformToEigen3f(*this), x, y, z, roll, pitch, yaw);
}
void Transform::getTranslation(float & x, float & y, float & z) const
{
x = this->x();
y = this->y();
z = this->z();
}
float Transform::getNorm() const
{
return std::sqrt(this->getNorm());
}
float Transform::getNormSquared() const
{
return this->x()*this->x() + this->y()*this->y() + this->z()*this->z();
}
std::string Transform::prettyPrint() const
{
float x,y,z,roll,pitch,yaw;