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

OdomF2M: added support to laser scan

Browse Source
This commit is contained in:
matlabbe
2016-03-06 15:11:09 -05:00
parent eefd557ab4
commit 7a1cf84b08
22 changed files with 696 additions and 226 deletions
Download Patch File Download Diff File Expand all files Collapse all files

127
guilib/src/MainWindow.cpp
View File

@@ -500,6 +500,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", 0.0f);
_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", 0.0f);
_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", 0.0f);
_ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", 0.0f);
_ui->statsToolBox->updateStat("Odometry/Interval/ms", 0.0f);
_ui->statsToolBox->updateStat("Odometry/Speed/kph", 0.0f);
_ui->statsToolBox->updateStat("Odometry/Distance/m", 0.0f);
@@ -895,29 +896,54 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
}
}
// 2d cloud
if(!odom.data().laserScanRaw().empty() &&
_preferencesDialog->isScansShown(1))
if(_preferencesDialog->isScansShown(1))
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = util3d::laserScanToPointCloud(odom.data().laserScanRaw(), pose);
if(_preferencesDialog->getDownsamplingStepScan(1) > 0)
// scan local map
if(!odom.info().localScanMap.empty())
{
cloud = util3d::downsample(cloud, _preferencesDialog->getDownsamplingStepScan(1));
pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
cloud = util3d::laserScanToPointCloudNormal(odom.info().localScanMap);
if(!_ui->widget_cloudViewer->addCloud("scanMapOdom", cloud, _odometryCorrection, Qt::blue))
{
UERROR("Adding scanMapOdom to viewer failed!");
}
else
{
_ui->widget_cloudViewer->setCloudVisibility("scanMapOdom", true);
_ui->widget_cloudViewer->setCloudOpacity("scanMapOdom", _preferencesDialog->getScanOpacity(1));
_ui->widget_cloudViewer->setCloudPointSize("scanMapOdom", _preferencesDialog->getScanPointSize(1));
scanUpdated = true;
}
}
if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
// scan cloud
if(!odom.data().laserScanRaw().empty())
{
cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
}
if(!_ui->widget_cloudViewer->addCloud("scanOdom", cloud, _odometryCorrection))
{
UERROR("Adding scanOdom to viewer failed!");
}
_ui->widget_cloudViewer->setCloudVisibility("scanOdom", true);
_ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
cv::Mat scan = odom.data().laserScanRaw();
scanUpdated = true;
if(_preferencesDialog->getDownsamplingStepScan(1) > 0)
{
scan = util3d::downsample(scan, _preferencesDialog->getDownsamplingStepScan(1));
}
pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
cloud = util3d::laserScanToPointCloudNormal(scan, pose);
if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
{
cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
}
if(!_ui->widget_cloudViewer->addCloud("scanOdom", cloud, _odometryCorrection, Qt::magenta))
{
UERROR("Adding scanOdom to viewer failed!");
}
else
{
_ui->widget_cloudViewer->setCloudVisibility("scanOdom", true);
_ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
scanUpdated = true;
}
}
}
// 3d features
@@ -957,6 +983,10 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
{
_ui->widget_cloudViewer->setCloudVisibility("scanOdom", false);
}
if(!scanUpdated && _ui->widget_cloudViewer->getAddedClouds().contains("scanMapOdom"))
{
_ui->widget_cloudViewer->setCloudVisibility("scanMapOdom", false);
}
if(!featuresUpdated && _ui->widget_cloudViewer->getAddedClouds().contains("featuresOdom"))
{
_ui->widget_cloudViewer->setCloudVisibility("featuresOdom", false);
@@ -1092,42 +1122,16 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
}
//Process info
if(odom.info().inliers >= 0)
{
_ui->statsToolBox->updateStat("Odometry/Inliers/", (float)odom.data().id(), (float)odom.info().inliers);
}
if(odom.info().icpInliersRatio >= 0)
{
_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", (float)odom.data().id(), (float)odom.info().icpInliersRatio);
}
if(odom.info().matches >= 0)
{
_ui->statsToolBox->updateStat("Odometry/Matches/", (float)odom.data().id(), (float)odom.info().matches);
}
if(odom.info().variance >= 0)
{
_ui->statsToolBox->updateStat("Odometry/StdDev/", (float)odom.data().id(), sqrt((float)odom.info().variance));
}
if(odom.info().variance >= 0)
{
_ui->statsToolBox->updateStat("Odometry/Variance/", (float)odom.data().id(), (float)odom.info().variance);
}
if(odom.info().timeEstimation > 0)
{
_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", (float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f);
}
if(odom.info().timeParticleFiltering > 0)
{
_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", (float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f);
}
if(odom.info().features >=0)
{
_ui->statsToolBox->updateStat("Odometry/Features/", (float)odom.data().id(), (float)odom.info().features);
}
if(odom.info().localMapSize >=0)
{
_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", (float)odom.data().id(), (float)odom.info().localMapSize);
}
_ui->statsToolBox->updateStat("Odometry/Inliers/", (float)odom.data().id(), (float)odom.info().inliers);
_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", (float)odom.data().id(), (float)odom.info().icpInliersRatio);
_ui->statsToolBox->updateStat("Odometry/Matches/", (float)odom.data().id(), (float)odom.info().matches);
_ui->statsToolBox->updateStat("Odometry/StdDev/", (float)odom.data().id(), sqrt((float)odom.info().variance));
_ui->statsToolBox->updateStat("Odometry/Variance/", (float)odom.data().id(), (float)odom.info().variance);
_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", (float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f);
_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", (float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f);
_ui->statsToolBox->updateStat("Odometry/Features/", (float)odom.data().id(), (float)odom.info().features);
_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", (float)odom.data().id(), (float)odom.info().localMapSize);
_ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", (float)odom.data().id(), (float)odom.info().localScanMapSize);
_ui->statsToolBox->updateStat("Odometry/ID/", (float)odom.data().id(), (float)odom.data().id());
float x=0.0f,y,z, roll,pitch,yaw;
@@ -2075,6 +2079,21 @@ void MainWindow::updateMapCloud(
_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
}
}
if(viewerClouds.contains("scanMapOdom"))
{
if(!_preferencesDialog->isScansShown(1))
{
UDEBUG("");
_ui->widget_cloudViewer->setCloudVisibility("scanMapOdom", false);
}
else
{
UDEBUG("");
_ui->widget_cloudViewer->updateCloudPose("scanMapOdom", _odometryCorrection);
_ui->widget_cloudViewer->setCloudOpacity("scanMapOdom", _preferencesDialog->getScanOpacity(1));
_ui->widget_cloudViewer->setCloudPointSize("scanMapOdom", _preferencesDialog->getScanPointSize(1));
}
}
if(viewerClouds.contains("featuresOdom"))
{
if(!_preferencesDialog->isFeaturesShown(1))

3
guilib/src/PreferencesDialog.cpp
View File

@@ -732,11 +732,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str());
_ui->odom_dataBufferSize->setObjectName(Parameters::kOdomImageBufferSize().c_str());
_ui->odom_flow_keyframeThr->setObjectName(Parameters::kOdomKeyFrameThr().c_str());
_ui->odom_flow_scanKeyframeThr->setObjectName(Parameters::kOdomScanKeyFrameThr().c_str());
_ui->odom_flow_guessMotion->setObjectName(Parameters::kOdomGuessMotion().c_str());
//Odometry Frame to Map
_ui->odom_localHistory->setObjectName(Parameters::kOdomF2MMaxSize().c_str());
_ui->spinBox_odom_f2m_maxNewFeatures->setObjectName(Parameters::kOdomF2MMaxNewFeatures().c_str());
_ui->spinBox_odom_f2m_scanMaxSize->setObjectName(Parameters::kOdomF2MScanMaxSize().c_str());
_ui->doubleSpinBox_odom_f2m_scanRadius->setObjectName(Parameters::kOdomF2MScanSubstractRadius().c_str());
_ui->odom_fixedLocalMapPath->setObjectName(Parameters::kOdomF2MFixedMapPath().c_str());
connect(_ui->toolButton_odomBowFixedLocalMap, SIGNAL(clicked()), this, SLOT(changeOdomBowFixedLocalMapPath()));

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

@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<y>-605</y>
<width>681</width>
<height>2010</height>
</rect>
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>11</number>
<number>14</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -7271,6 +7271,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</item>
<item>
<layout class="QGridLayout" name="gridLayout_27" columnstretch="0,1">
<item row="6" column="0">
<widget class="QCheckBox" name="odom_flow_guessMotion">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QDoubleSpinBox" name="odom_flow_keyframeThr">
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.500000000000000</double>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_233">
<property name="text">
@@ -7311,7 +7331,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="8" column="0">
<item row="9" column="0">
<widget class="QSpinBox" name="odom_dataBufferSize">
<property name="maximum">
<number>999999</number>
@@ -7348,7 +7368,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</item>
</widget>
</item>
<item row="9" column="0">
<item row="10" column="0">
<widget class="QPushButton" name="pushButton_testOdometry">
<property name="text">
<string>Test selected odometry</string>
@@ -7368,7 +7388,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="8" column="1">
<item row="9" column="1">
<widget class="QLabel" name="label_232">
<property name="text">
<string>Data buffer size (0 means inf).</string>
@@ -7436,17 +7456,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QCheckBox" name="odom_flow_guessMotion">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_196">
<property name="text">
<string>Create a new keyframe when the number of inliers drops under this threshold. Setting value to 0 means that a keyframe is created for each processed frame.</string>
<string>[Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting value to 0 means that a keyframe is created for each processed frame.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -7456,8 +7469,21 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QDoubleSpinBox" name="odom_flow_keyframeThr">
<item row="8" column="1">
<widget class="QLabel" name="label_246">
<property name="text">
<string>[Geometry] Create a new keyframe when the number of inliers drops under this threshold. Setting value to 0 means that a keyframe is created for each processed frame.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QDoubleSpinBox" name="odom_flow_scanKeyframeThr">
<property name="maximum">
<double>1.000000000000000</double>
</property>
@@ -7499,10 +7525,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</item>
<item>
<layout class="QGridLayout" name="gridLayout_29" columnstretch="0,0,1">
<item row="0" column="2">
<widget class="QLabel" name="label_190">
<item row="2" column="2">
<widget class="QLabel" name="label_195">
<property name="text">
<string>Maximum map size: If &gt; 0 (example 5000), the odometry will maintain a local map of X maximum words. This will decrease odometry drifting when the camera is not moving.</string>
<string>[Geometry] Maximum scan map size is defined by this factor times the maximum size of a single scan. For example, if the laser scans have 1000 values, then the maximum local map size will be 2000 if the factor is 2.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -7512,33 +7538,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="2" column="0">
<item row="4" column="0">
<widget class="QLineEdit" name="odom_fixedLocalMapPath"/>
</item>
<item row="0" column="0">
<widget class="QSpinBox" name="odom_localHistory">
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>0</number>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QToolButton" name="toolButton_odomBowFixedLocalMap">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item row="2" column="2">
<item row="4" column="2">
<widget class="QLabel" name="label_239">
<property name="text">
<string>Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP pose estimation is activated.</string>
@@ -7551,10 +7554,46 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_190">
<property name="text">
<string>[Visual] Maximum map size: If &gt; 0 (example 5000), the odometry will maintain a local map of X maximum features. This will decrease odometry drifting when the camera is not moving.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QSpinBox" name="odom_localHistory">
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>999999999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>0</number>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QToolButton" name="toolButton_odomBowFixedLocalMap">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="label_194">
<property name="text">
<string>Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.</string>
<string>[Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -7580,6 +7619,39 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="3" column="2">
<widget class="QLabel" name="label_215">
<property name="text">
<string>[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the laser scans.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_odom_f2m_scanRadius">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="value">
<double>0.025000000000000</double>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="spinBox_odom_f2m_scanMaxSize">
<property name="maximum">
<number>999999999</number>
</property>
</widget>
</item>
</layout>
</item>
<item>