i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Odometry: added force2D option

Browse Source 
Changed parameter RGBD/ScanMatchingSize to RGBD/PoseScanMatching

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@2050 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-11-19 23:38:15 +00:00
parent 89bcea3057
commit faf43cc82c
9 changed files with 127 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Odometry.h
+1
View File
@@ -89,6 +89,7 @@ private:
float _linearUpdate;
float _angularUpdate;
int _resetCountdown;
bool _force2D;
Transform _pose;
int _resetCurrentCount;
corelib/include/rtabmap/core/Parameters.h
+3 -2
View File
@@ -246,7 +246,7 @@ class RTABMAP_EXP Parameters
// RGB-D SLAM
RTABMAP_PARAM(RGBD, Enabled, bool, true, "");
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, PoseScanMatching, bool, false, "Laser scan matching for odometry pose correction (laser scans are required).");
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, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
@@ -274,6 +274,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
RTABMAP_PARAM_STR(Odom, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
RTABMAP_PARAM(Odom, FeaturesRatio, float, 0.0, "Minimum ratio of keypoints between the current image and the last image to compute odometry.");
RTABMAP_PARAM(Odom, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw).");
// Odometry Bag-of-words
RTABMAP_PARAM(OdomBow, LocalHistorySize, int, 1000, "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
@@ -298,7 +299,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(LccBow, InlierDistance, float, 0.02, "Maximum distance for visual word correspondences.");
RTABMAP_PARAM(LccBow, Iterations, int, 100, "Maximum iterations to compute the transform from visual words.");
RTABMAP_PARAM(LccBow, MaxDepth, float, 4.0, "Max depth of the words (0 means no limit).");
RTABMAP_PARAM(LccBow, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw).")
RTABMAP_PARAM(LccBow, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw).");
RTABMAP_PARAM(LccReextract, Activated, bool, false, "Activate re-extracting features on global loop closure.");
RTABMAP_PARAM(LccReextract, NNType, int, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4.");
RTABMAP_PARAM(LccReextract, NNDR, float, 0.7, "NNDR: nearest neighbor distance ratio.");
corelib/include/rtabmap/core/Rtabmap.h
+1 -1
View File
@@ -150,7 +150,7 @@ private:
float _newMapOdomChangeDistance;
int _globalLoopClosureIcpType;
float _globalLoopClosureIcpMaxDistance;
int _scanMatchingSize;
bool _poseScanMatching;
bool _localLoopClosureDetectionTime;
bool _localLoopClosureDetectionSpace;
float _localDetectRadius;
corelib/src/Memory.cpp
+3 -1
View File
@@ -2036,6 +2036,7 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
hasConverged,
fitness);
//UWARN("saving ICP2D clouds!");
//pcl::io::savePCDFile("lccold.pcd", *oldCloud);
//pcl::io::savePCDFile("lccnewguess.pcd", *newCloud);
newCloud = util3d::transformPointCloud<pcl::PointXYZ>(newCloud, icpT);
@@ -2147,6 +2148,7 @@ Transform Memory::computeScanMatchingTransform(
newCloud = util3d::voxelize<pcl::PointXYZ>(newCloud, _icp2VoxelSize);
}
//UWARN("local scan matching pcd saved!");
//pcl::io::savePCDFile("old.pcd", *assembledOldClouds);
//pcl::io::savePCDFile("new.pcd", *newCloud);
@@ -2183,7 +2185,7 @@ Transform Memory::computeScanMatchingTransform(
{
transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId);
//newCloud = util3d::cvMat2Cloud(util3d::uncompressData(newS->getDepth2D()), poses.at(oldId)*transform.inverse());
//newCloud = util3d::cvMat2Cloud(util3d::uncompressData(newS->getDepth2DCompressed()), poses.at(oldId)*transform.inverse());
//pcl::io::savePCDFile("newFinal.pcd", *newCloud);
}
else
corelib/src/Odometry.cpp
+28 -2
View File
@@ -68,6 +68,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_linearUpdate(Parameters::defaultOdomLinearUpdate()),
_angularUpdate(Parameters::defaultOdomAngularUpdate()),
_resetCountdown(Parameters::defaultOdomResetCountdown()),
_force2D(Parameters::defaultOdomForce2D()),
_pose(Transform::getIdentity()),
_resetCurrentCount(0)
{
@@ -82,12 +83,27 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth);
Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), _maxFeatures);
Parameters::parse(parameters, Parameters::kOdomRoiRatios(), _roiRatios);
Parameters::parse(parameters, Parameters::kOdomForce2D(), _force2D);
}
void Odometry::reset(const Transform & initialPose)
{
_resetCurrentCount = 0;
_pose = initialPose;
if(_force2D)
{
float x,y,z, roll,pitch,yaw;
initialPose.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
if(z != 0.0f || roll != 0.0f || yaw != 0.0f)
{
UWARN("Force2D=true and the initial pose contains z, roll or pitch values (%s). They are set to null.", initialPose.prettyPrint().c_str());
}
Transform pose(x, y, 0, 0, 0, yaw);
_pose = pose;
}
else
{
_pose = initialPose;
}
}
bool Odometry::isLargeEnoughTransform(const Transform & transform)
@@ -110,7 +126,17 @@ Transform Odometry::process(SensorData & data, int * quality, int * features, in
{
_resetCurrentCount = _resetCountdown;
_pose *= t;
if(_force2D)
{
float x,y,z, roll,pitch,yaw;
t.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
_pose *= Transform(x,y,0,0,0,yaw);
}
else
{
_pose *= t;
}
return _pose;
}
else if(_resetCurrentCount > 0)
corelib/src/Rtabmap.cpp
+59 -75
View File
@@ -91,7 +91,7 @@ Rtabmap::Rtabmap() :
_newMapOdomChangeDistance(Parameters::defaultRGBDNewMapOdomChangeDistance()),
_globalLoopClosureIcpType(Parameters::defaultLccIcpType()),
_globalLoopClosureIcpMaxDistance(Parameters::defaultLccIcpMaxDistance()),
_scanMatchingSize(Parameters::defaultRGBDScanMatchingSize()),
_poseScanMatching(Parameters::defaultRGBDPoseScanMatching()),
_localLoopClosureDetectionTime(Parameters::defaultRGBDLocalLoopDetectionTime()),
_localLoopClosureDetectionSpace(Parameters::defaultRGBDLocalLoopDetectionSpace()),
_localDetectRadius(Parameters::defaultRGBDLocalLoopDetectionRadius()),
@@ -350,7 +350,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
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::kRGBDPoseScanMatching(), _poseScanMatching);
Parameters::parse(parameters, Parameters::kLccIcpMaxDistance(), _globalLoopClosureIcpMaxDistance);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionTime(), _localLoopClosureDetectionTime);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionSpace(), _localLoopClosureDetectionSpace);
@@ -856,54 +856,31 @@ bool Rtabmap::process(const SensorData & data)
//============================================================
// Scan matching
//============================================================
if(_scanMatchingSize>0 &&
if(_poseScanMatching &&
signature->getNeighbors().size() == 1 &&
!signature->getDepth2DCompressed().empty() &&
rehearsedId == 0) // don't do it if rehearsal happened
{
UINFO("Odometry correction by scan matching (size=%d)...", _scanMatchingSize);
const std::set<int> & stm = _memory->getStMem();
std::map<int, Transform> poses;
int count = _scanMatchingSize;
for(std::set<int>::const_reverse_iterator iter = stm.rbegin(); iter!=stm.rend() && count>0; ++iter)
{
if(_memory->getSignature(*iter)->mapId() == signature->mapId())
{
std::map<int, Transform>::iterator jter = _optimizedPoses.find(*iter);
UASSERT_MSG(jter != _optimizedPoses.end(), uFormat("%d not found in optimized poses!", *iter).c_str());
poses.insert(*jter);
if(*iter != signature->id())
{
--count;
}
}
}
UINFO("Odometry correction by scan matching");
int oldId = signature->getNeighbors().begin()->first;
if(poses.size())
const Signature * oldS = _memory->getSignature(oldId);
UASSERT(oldS != 0);
std::string rejectedMsg;
Transform guess = signature->getNeighbors().begin()->second;
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg);
if(!t.isNull())
{
const Signature * oldS = _memory->getSignature(oldId);
UASSERT(oldS != 0);
std::string rejectedMsg;
Transform t = _memory->computeScanMatchingTransform(signature->id(), oldId, poses, &rejectedMsg);
if(!t.isNull())
{
scanMatchingSuccess = true;
UINFO("Scan matching: update neighbor link (%d->%d) from %s to %s",
signature->id(),
oldId,
signature->getNeighbors().at(oldId).prettyPrint().c_str(),
t.prettyPrint().c_str());
_memory->updateNeighborLink(signature->id(), oldId, t);
}
else
{
UWARN("Scan matching rejected: %s", rejectedMsg.c_str());
}
scanMatchingSuccess = true;
UINFO("Scan matching: update neighbor link (%d->%d) from %s to %s",
signature->id(),
oldId,
signature->getNeighbors().at(oldId).prettyPrint().c_str(),
t.prettyPrint().c_str());
_memory->updateNeighborLink(signature->id(), oldId, t);
}
else
{
UERROR("Poses are empty?!?");
UWARN("Scan matching rejected: %s", rejectedMsg.c_str());
}
}
timeScanMatching = timer.ticks();
@@ -1386,43 +1363,50 @@ bool Rtabmap::process(const SensorData & data)
_localLoopClosureDetectionSpace &&
!signature->getDepth2DCompressed().empty())
{
//============================================================
// Scan matching LOCAL LOOP CLOSURE SPACE
//============================================================
// get all nodes in radius of the current node
std::map<int, Transform> poses;
localSpaceNearestId = 0;
poses = this->getOptimizedWMPosesInRadius(signature->id(), _localDetectMaxNeighbors, _localDetectRadius, _localDetectMaxDiffID, localSpaceNearestId);
// add current node to poses
UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end());
poses.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
localSpaceDetectionPosesCount = (int)poses.size()-1;
//The nearest will be the reference for a loop closure transform
if(poses.size() &&
localSpaceNearestId &&
signature->getChildLoopClosureIds().find(localSpaceNearestId) == signature->getChildLoopClosureIds().end())
if(_toroIterations == 0)
{
std::string rejectedMsg;
Transform t = _memory->computeScanMatchingTransform(signature->id(), localSpaceNearestId, poses, &rejectedMsg);
if(!t.isNull())
{
localSpaceClosureId = localSpaceNearestId;
UINFO("Add local loop closure in SPACE (%d->%d) %s",
signature->id(),
localSpaceNearestId,
t.prettyPrint().c_str());
_memory->addLoopClosureLink(localSpaceNearestId, signature->id(), t, false);
UWARN("Cannot do local loop closure detection in space if graph optimization is disabled!");
}
else
{
//============================================================
// Scan matching LOCAL LOOP CLOSURE SPACE
//============================================================
// get all nodes in radius of the current node
std::map<int, Transform> poses;
localSpaceNearestId = 0;
poses = this->getOptimizedWMPosesInRadius(signature->id(), _localDetectMaxNeighbors, _localDetectRadius, _localDetectMaxDiffID, localSpaceNearestId);
// Old map -> new map, used for localization correction on loop closure
const Signature * oldS = _memory->getSignature(localSpaceNearestId);
UASSERT(oldS != 0);
_mapTransform = oldS->getPose() * t.inverse() * signature->getPose().inverse();
}
else
// add current node to poses
UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end());
poses.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
localSpaceDetectionPosesCount = (int)poses.size()-1;
//The nearest will be the reference for a loop closure transform
if(poses.size() &&
localSpaceNearestId &&
signature->getChildLoopClosureIds().find(localSpaceNearestId) == signature->getChildLoopClosureIds().end())
{
UINFO("Local loop closure (space) rejected: %s", rejectedMsg.c_str());
std::string rejectedMsg;
Transform t = _memory->computeScanMatchingTransform(signature->id(), localSpaceNearestId, poses, &rejectedMsg);
if(!t.isNull())
{
localSpaceClosureId = localSpaceNearestId;
UINFO("Add local loop closure in SPACE (%d->%d) %s",
signature->id(),
localSpaceNearestId,
t.prettyPrint().c_str());
_memory->addLoopClosureLink(localSpaceNearestId, signature->id(), t, false);
// Old map -> new map, used for localization correction on loop closure
//const Signature * oldS = _memory->getSignature(localSpaceNearestId);
//UASSERT(oldS != 0);
_mapTransform = poses.at(localSpaceNearestId) * t.inverse() * poses.at(signature->id()).inverse();
}
else
{
UINFO("Local loop closure (space) rejected: %s", rejectedMsg.c_str());
}
}
}
}
guilib/src/PreferencesDialog.cpp
+2 -1
View File
@@ -416,7 +416,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->rgdb_linearUpdate->setObjectName(Parameters::kRGBDLinearUpdate().c_str());
_ui->rgdb_angularUpdate->setObjectName(Parameters::kRGBDAngularUpdate().c_str());
_ui->rgdb_newMapOdomChange->setObjectName(Parameters::kRGBDNewMapOdomChangeDistance().c_str());
_ui->odomScanHistory->setObjectName(Parameters::kRGBDScanMatchingSize().c_str());
_ui->odomScanHistory->setObjectName(Parameters::kRGBDPoseScanMatching().c_str());
_ui->globalDetection_toroIterations->setObjectName(Parameters::kRGBDToroIterations().c_str());
_ui->globalDetection_optimizeFromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
@@ -470,6 +470,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->odom_maxDepth->setObjectName(Parameters::kOdomMaxDepth().c_str());
_ui->odom_minInliers->setObjectName(Parameters::kOdomMinInliers().c_str());
_ui->odom_refine_iterations->setObjectName(Parameters::kOdomRefineIterations().c_str());
_ui->odom_force2D->setObjectName(Parameters::kOdomForce2D().c_str());
_ui->lineEdit_odom_roi->setObjectName(Parameters::kOdomRoiRatios().c_str());
//Odometry BOW
guilib/src/ui/preferencesDialog.ui
+26 -5
View File
@@ -65,7 +65,7 @@
<x>0</x>
<y>0</y>
<width>744</width>
<height>932</height>
<height>1161</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>23</number>
<number>19</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -4923,7 +4923,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item row="3" column="1">
<widget class="QLabel" name="label_scanMatching">
<property name="text">
<string>Laser scan matching history size for odometry correction. Set to 0 to disable odometry correction. ICP 2D only is used here.</string>
<string>Laser scan matching for odometry pose correction. ICP 2D only is used here.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -4931,7 +4931,11 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</widget>
</item>
<item row="3" column="0">
<widget class="QSpinBox" name="odomScanHistory"/>
<widget class="QCheckBox" name="odomScanHistory">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</widget>
@@ -5067,7 +5071,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item row="0" column="0">
<widget class="QSpinBox" name="globalDetection_toroIterations">
<property name="minimum">
<number>1</number>
<number>0</number>
</property>
<property name="maximum">
<number>10000</number>
@@ -6323,6 +6327,23 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QCheckBox" name="odom_force2D">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_196">
<property name="text">
<string>Force 2D transform (3DoF: x,y and yaw).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
<item>
tools/CameraRGBD/main.cpp
+4 -1
View File
@@ -120,6 +120,7 @@ int main(int argc, char * argv[])
cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window
cv::namedWindow("Depth", CV_WINDOW_AUTOSIZE); // create window
pcl::visualization::CloudViewer viewer("cloud");
rtabmap::Transform opticalTransform(0,0,1,0, -1,0,0,0, 0,-1,0,0);
while(!rgb.empty() && !viewer.wasStopped())
{
cv::Mat tmp;
@@ -128,7 +129,9 @@ int main(int argc, char * argv[])
cv::imshow("Video", rgb); // show frame
cv::imshow("Depth",tmp);
viewer.showCloud(rtabmap::util3d::cloudFromDepthRGB(rgb, depth, cx, cy, fx, fy), "cloud");
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(rgb, depth, cx, cy, fx, fy);
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, opticalTransform);
viewer.showCloud(cloud, "cloud");
int c = cv::waitKey(10); // wait 10 ms or for key stroke
if(c == 27)