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

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

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

@@ -89,6 +89,7 @@ private:
float _linearUpdate; float _linearUpdate;
float _angularUpdate; float _angularUpdate;
int _resetCountdown; int _resetCountdown;
bool _force2D;
Transform _pose; Transform _pose;
int _resetCurrentCount; int _resetCurrentCount;

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

@@ -246,7 +246,7 @@ class RTABMAP_EXP Parameters
// RGB-D SLAM // RGB-D SLAM
RTABMAP_PARAM(RGBD, Enabled, bool, true, ""); 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, 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, 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)."); 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(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_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, 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 // 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."); 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, 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, 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, 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, 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, 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."); RTABMAP_PARAM(LccReextract, NNDR, float, 0.7, "NNDR: nearest neighbor distance ratio.");

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

@@ -150,7 +150,7 @@ private:
float _newMapOdomChangeDistance; float _newMapOdomChangeDistance;
int _globalLoopClosureIcpType; int _globalLoopClosureIcpType;
float _globalLoopClosureIcpMaxDistance; float _globalLoopClosureIcpMaxDistance;
int _scanMatchingSize; bool _poseScanMatching;
bool _localLoopClosureDetectionTime; bool _localLoopClosureDetectionTime;
bool _localLoopClosureDetectionSpace; bool _localLoopClosureDetectionSpace;
float _localDetectRadius; float _localDetectRadius;

4
corelib/src/Memory.cpp
View File

@@ -2036,6 +2036,7 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
hasConverged, hasConverged,
fitness); fitness);
//UWARN("saving ICP2D clouds!");
//pcl::io::savePCDFile("lccold.pcd", *oldCloud); //pcl::io::savePCDFile("lccold.pcd", *oldCloud);
//pcl::io::savePCDFile("lccnewguess.pcd", *newCloud); //pcl::io::savePCDFile("lccnewguess.pcd", *newCloud);
newCloud = util3d::transformPointCloud<pcl::PointXYZ>(newCloud, icpT); newCloud = util3d::transformPointCloud<pcl::PointXYZ>(newCloud, icpT);
@@ -2147,6 +2148,7 @@ Transform Memory::computeScanMatchingTransform(
newCloud = util3d::voxelize<pcl::PointXYZ>(newCloud, _icp2VoxelSize); newCloud = util3d::voxelize<pcl::PointXYZ>(newCloud, _icp2VoxelSize);
} }
//UWARN("local scan matching pcd saved!");
//pcl::io::savePCDFile("old.pcd", *assembledOldClouds); //pcl::io::savePCDFile("old.pcd", *assembledOldClouds);
//pcl::io::savePCDFile("new.pcd", *newCloud); //pcl::io::savePCDFile("new.pcd", *newCloud);
@@ -2183,7 +2185,7 @@ Transform Memory::computeScanMatchingTransform(
{ {
transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId); 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); //pcl::io::savePCDFile("newFinal.pcd", *newCloud);
} }
else else

30
corelib/src/Odometry.cpp
View File

@@ -68,6 +68,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_linearUpdate(Parameters::defaultOdomLinearUpdate()), _linearUpdate(Parameters::defaultOdomLinearUpdate()),
_angularUpdate(Parameters::defaultOdomAngularUpdate()), _angularUpdate(Parameters::defaultOdomAngularUpdate()),
_resetCountdown(Parameters::defaultOdomResetCountdown()), _resetCountdown(Parameters::defaultOdomResetCountdown()),
_force2D(Parameters::defaultOdomForce2D()),
_pose(Transform::getIdentity()), _pose(Transform::getIdentity()),
_resetCurrentCount(0) _resetCurrentCount(0)
{ {
@@ -82,12 +83,27 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth); Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth);
Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), _maxFeatures); Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), _maxFeatures);
Parameters::parse(parameters, Parameters::kOdomRoiRatios(), _roiRatios); Parameters::parse(parameters, Parameters::kOdomRoiRatios(), _roiRatios);
Parameters::parse(parameters, Parameters::kOdomForce2D(), _force2D);
} }
void Odometry::reset(const Transform & initialPose) void Odometry::reset(const Transform & initialPose)
{ {
_resetCurrentCount = 0; _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) bool Odometry::isLargeEnoughTransform(const Transform & transform)
@@ -110,7 +126,17 @@ Transform Odometry::process(SensorData & data, int * quality, int * features, in
{ {
_resetCurrentCount = _resetCountdown; _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; return _pose;
} }
else if(_resetCurrentCount > 0) else if(_resetCurrentCount > 0)

134
corelib/src/Rtabmap.cpp
View File

@@ -91,7 +91,7 @@ Rtabmap::Rtabmap() :
_newMapOdomChangeDistance(Parameters::defaultRGBDNewMapOdomChangeDistance()), _newMapOdomChangeDistance(Parameters::defaultRGBDNewMapOdomChangeDistance()),
_globalLoopClosureIcpType(Parameters::defaultLccIcpType()), _globalLoopClosureIcpType(Parameters::defaultLccIcpType()),
_globalLoopClosureIcpMaxDistance(Parameters::defaultLccIcpMaxDistance()), _globalLoopClosureIcpMaxDistance(Parameters::defaultLccIcpMaxDistance()),
_scanMatchingSize(Parameters::defaultRGBDScanMatchingSize()), _poseScanMatching(Parameters::defaultRGBDPoseScanMatching()),
_localLoopClosureDetectionTime(Parameters::defaultRGBDLocalLoopDetectionTime()), _localLoopClosureDetectionTime(Parameters::defaultRGBDLocalLoopDetectionTime()),
_localLoopClosureDetectionSpace(Parameters::defaultRGBDLocalLoopDetectionSpace()), _localLoopClosureDetectionSpace(Parameters::defaultRGBDLocalLoopDetectionSpace()),
_localDetectRadius(Parameters::defaultRGBDLocalLoopDetectionRadius()), _localDetectRadius(Parameters::defaultRGBDLocalLoopDetectionRadius()),
@@ -350,7 +350,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rgbdLinearUpdate); Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rgbdLinearUpdate);
Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rgbdAngularUpdate); Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rgbdAngularUpdate);
Parameters::parse(parameters, Parameters::kRGBDNewMapOdomChangeDistance(), _newMapOdomChangeDistance); 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::kLccIcpMaxDistance(), _globalLoopClosureIcpMaxDistance);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionTime(), _localLoopClosureDetectionTime); Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionTime(), _localLoopClosureDetectionTime);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionSpace(), _localLoopClosureDetectionSpace); Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionSpace(), _localLoopClosureDetectionSpace);
@@ -856,54 +856,31 @@ bool Rtabmap::process(const SensorData & data)
//============================================================ //============================================================
// Scan matching // Scan matching
//============================================================ //============================================================
if(_scanMatchingSize>0 && if(_poseScanMatching &&
signature->getNeighbors().size() == 1 && signature->getNeighbors().size() == 1 &&
!signature->getDepth2DCompressed().empty() && !signature->getDepth2DCompressed().empty() &&
rehearsedId == 0) // don't do it if rehearsal happened rehearsedId == 0) // don't do it if rehearsal happened
{ {
UINFO("Odometry correction by scan matching (size=%d)...", _scanMatchingSize); UINFO("Odometry correction by scan matching");
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;
}
}
}
int oldId = signature->getNeighbors().begin()->first; 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); scanMatchingSuccess = true;
UASSERT(oldS != 0); UINFO("Scan matching: update neighbor link (%d->%d) from %s to %s",
std::string rejectedMsg; signature->id(),
Transform t = _memory->computeScanMatchingTransform(signature->id(), oldId, poses, &rejectedMsg); oldId,
if(!t.isNull()) signature->getNeighbors().at(oldId).prettyPrint().c_str(),
{ t.prettyPrint().c_str());
scanMatchingSuccess = true; _memory->updateNeighborLink(signature->id(), oldId, t);
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());
}
} }
else else
{ {
UERROR("Poses are empty?!?"); UWARN("Scan matching rejected: %s", rejectedMsg.c_str());
} }
} }
timeScanMatching = timer.ticks(); timeScanMatching = timer.ticks();
@@ -1386,43 +1363,50 @@ bool Rtabmap::process(const SensorData & data)
_localLoopClosureDetectionSpace && _localLoopClosureDetectionSpace &&
!signature->getDepth2DCompressed().empty()) !signature->getDepth2DCompressed().empty())
{ {
//============================================================ if(_toroIterations == 0)
// 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())
{ {
std::string rejectedMsg; UWARN("Cannot do local loop closure detection in space if graph optimization is disabled!");
Transform t = _memory->computeScanMatchingTransform(signature->id(), localSpaceNearestId, poses, &rejectedMsg); }
if(!t.isNull()) else
{ {
localSpaceClosureId = localSpaceNearestId; //============================================================
UINFO("Add local loop closure in SPACE (%d->%d) %s", // Scan matching LOCAL LOOP CLOSURE SPACE
signature->id(), //============================================================
localSpaceNearestId, // get all nodes in radius of the current node
t.prettyPrint().c_str()); std::map<int, Transform> poses;
_memory->addLoopClosureLink(localSpaceNearestId, signature->id(), t, false); localSpaceNearestId = 0;
poses = this->getOptimizedWMPosesInRadius(signature->id(), _localDetectMaxNeighbors, _localDetectRadius, _localDetectMaxDiffID, localSpaceNearestId);
// Old map -> new map, used for localization correction on loop closure // add current node to poses
const Signature * oldS = _memory->getSignature(localSpaceNearestId); UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end());
UASSERT(oldS != 0); poses.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
_mapTransform = oldS->getPose() * t.inverse() * signature->getPose().inverse();
} localSpaceDetectionPosesCount = (int)poses.size()-1;
else //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());
}
} }
} }
} }

3
guilib/src/PreferencesDialog.cpp
View File

@@ -416,7 +416,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->rgdb_linearUpdate->setObjectName(Parameters::kRGBDLinearUpdate().c_str()); _ui->rgdb_linearUpdate->setObjectName(Parameters::kRGBDLinearUpdate().c_str());
_ui->rgdb_angularUpdate->setObjectName(Parameters::kRGBDAngularUpdate().c_str()); _ui->rgdb_angularUpdate->setObjectName(Parameters::kRGBDAngularUpdate().c_str());
_ui->rgdb_newMapOdomChange->setObjectName(Parameters::kRGBDNewMapOdomChangeDistance().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_toroIterations->setObjectName(Parameters::kRGBDToroIterations().c_str());
_ui->globalDetection_optimizeFromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().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_maxDepth->setObjectName(Parameters::kOdomMaxDepth().c_str());
_ui->odom_minInliers->setObjectName(Parameters::kOdomMinInliers().c_str()); _ui->odom_minInliers->setObjectName(Parameters::kOdomMinInliers().c_str());
_ui->odom_refine_iterations->setObjectName(Parameters::kOdomRefineIterations().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()); _ui->lineEdit_odom_roi->setObjectName(Parameters::kOdomRoiRatios().c_str());
//Odometry BOW //Odometry BOW

31
guilib/src/ui/preferencesDialog.ui
View File

@@ -65,7 +65,7 @@
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>744</width> <width>744</width>
<height>932</height> <height>1161</height>
</rect> </rect>
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout_16"> <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum> <enum>QFrame::Raised</enum>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>23</number> <number>19</number>
</property> </property>
<widget class="QWidget" name="page_22"> <widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29"> <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"> <item row="3" column="1">
<widget class="QLabel" name="label_scanMatching"> <widget class="QLabel" name="label_scanMatching">
<property name="text"> <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>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>
@@ -4931,7 +4931,11 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</widget> </widget>
</item> </item>
<item row="3" column="0"> <item row="3" column="0">
<widget class="QSpinBox" name="odomScanHistory"/> <widget class="QCheckBox" name="odomScanHistory">
<property name="text">
<string/>
</property>
</widget>
</item> </item>
</layout> </layout>
</widget> </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"> <item row="0" column="0">
<widget class="QSpinBox" name="globalDetection_toroIterations"> <widget class="QSpinBox" name="globalDetection_toroIterations">
<property name="minimum"> <property name="minimum">
<number>1</number> <number>0</number>
</property> </property>
<property name="maximum"> <property name="maximum">
<number>10000</number> <number>10000</number>
@@ -6323,6 +6327,23 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property> </property>
</widget> </widget>
</item> </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> </layout>
</item> </item>
<item> <item>

5
tools/CameraRGBD/main.cpp
View File

@@ -120,6 +120,7 @@ int main(int argc, char * argv[])
cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window
cv::namedWindow("Depth", CV_WINDOW_AUTOSIZE); // create window cv::namedWindow("Depth", CV_WINDOW_AUTOSIZE); // create window
pcl::visualization::CloudViewer viewer("cloud"); 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()) while(!rgb.empty() && !viewer.wasStopped())
{ {
cv::Mat tmp; cv::Mat tmp;
@@ -128,7 +129,9 @@ int main(int argc, char * argv[])
cv::imshow("Video", rgb); // show frame cv::imshow("Video", rgb); // show frame
cv::imshow("Depth",tmp); 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 int c = cv::waitKey(10); // wait 10 ms or for key stroke
if(c == 27) if(c == 27)