DBReader: publish landmarks by default (also added option to ignore them)

corelib/include/rtabmap/core/DBReader.h

@@ -53,7 +53,8 @@ public:
 			 int startId = 0,
 			 int cameraIndex = -1,
 			 int stopId = 0,
-			 bool intermediateNodesIgnored = false);
+			 bool intermediateNodesIgnored = false,
+			 bool landmarksIgnored = false);
 	DBReader(const std::list<std::string> & databasePaths,
 			 float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
 			 bool odometryIgnored = false,
@@ -62,7 +63,8 @@ public:
 			 int startId = 0,
 			 int cameraIndex = -1,
 			 int stopId = 0,
-			 bool intermediateNodesIgnored = false);
+			 bool intermediateNodesIgnored = false,
+			 bool landmarksIgnored = false);
 	virtual ~DBReader();
 
 	virtual bool init(
@@ -88,6 +90,7 @@ private:
 	int _stopId;
 	int _cameraIndex;
 	bool _intermediateNodesIgnored;
+	bool _landmarksIgnored;
 
 	DBDriver * _dbDriver;
 	UTimer _timer;

corelib/include/rtabmap/core/Landmark.h

@@ -59,7 +59,7 @@ public:
         UASSERT_MSG(uIsFinite(covariance_.at<double>(4,4)) && covariance_.at<double>(4,4)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(4,4)).c_str());
         UASSERT_MSG(uIsFinite(covariance_.at<double>(5,5)) && covariance_.at<double>(5,5)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(5,5)).c_str());
     }
-    RTABMAP_DEPRECATED(Landmark(const int & id, const Transform & pose, const cv::Mat & covariance), "Use constructor with size instead.");
+    RTABMAP_DEPRECATED(Landmark(const int & id, const Transform & pose, const cv::Mat & covariance), "Use constructor with size=0 instead.");
 
 	virtual ~Landmark() {}
 

corelib/src/DBReader.cpp

@@ -50,7 +50,8 @@ DBReader::DBReader(const std::string & databasePath,
 				   int startId,
 				   int cameraIndex,
 				   int stopId,
-				   bool intermediateNodesIgnored) :
+				   bool intermediateNodesIgnored,
+				   bool landmarksIgnored) :
 	Camera(frameRate),
 	_paths(uSplit(databasePath, ';')),
 	_odometryIgnored(odometryIgnored),
@@ -60,6 +61,7 @@ DBReader::DBReader(const std::string & databasePath,
 	_stopId(stopId),
 	_cameraIndex(cameraIndex),
 	_intermediateNodesIgnored(intermediateNodesIgnored),
+	_landmarksIgnored(landmarksIgnored),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousMapId(-1),
@@ -81,7 +83,8 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 				   int startId,
 				   int cameraIndex,
 				   int stopId,
-				   bool intermediateNodesIgnored) :
+				   bool intermediateNodesIgnored,
+				   bool landmarksIgnored) :
 	Camera(frameRate),
    _paths(databasePaths),
 	_odometryIgnored(odometryIgnored),
@@ -91,6 +94,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	_stopId(stopId),
 	_cameraIndex(cameraIndex),
 	_intermediateNodesIgnored(intermediateNodesIgnored),
+	_landmarksIgnored(landmarksIgnored),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousMapId(-1),
@@ -402,6 +406,22 @@ SensorData DBReader::getNextData(CameraInfo * info)
 				gravityTransform = gravityLinks.begin()->second.transform();
 			}
 
+			Landmarks landmarks;
+			if(!_landmarksIgnored)
+			{
+				std::multimap<int, Link> landmarkLinks;
+				_dbDriver->loadLinks(*_currentId, landmarkLinks, Link::kLandmark);
+				for(std::multimap<int, Link>::iterator iter=landmarkLinks.begin(); iter!=landmarkLinks.end(); ++iter)
+				{
+					 cv::Mat landmarkSize = iter->second.uncompressUserDataConst();
+					landmarks.insert(std::make_pair(-iter->first,
+							Landmark(-iter->first,
+									!landmarkSize.empty() && landmarkSize.type() == CV_32FC1 && landmarkSize.total()==1?landmarkSize.at<float>(0,0):0.0f,
+									iter->second.transform(),
+									iter->second.infMatrix().inv())));
+				}
+			}
+
 			cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
 			if(!_odometryIgnored)
 			{
@@ -523,6 +543,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
 						cv::Vec3d(), cv::Mat(),
 						Transform::getIdentity())); // we assume that gravity links are already transformed in base_link
 			}
+			data.setLandmarks(landmarks);
 
 			UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, Grid=%d, UserData=%d, GlobalPose=%d, GPS=%d, IMU=%d",
 					data.laserScanRaw().isEmpty()?0:1,

guilib/src/PreferencesDialog.cpp

@@ -666,6 +666,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->source_checkBox_ignoreOdometry, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_ignoreGoalDelay, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_ignoreGoals, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->source_checkBox_ignoreLandmarks, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_spinBox_databaseStartId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_spinBox_databaseStopId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_useDbStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1901,6 +1902,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->source_checkBox_ignoreOdometry->setChecked(false);
 		_ui->source_checkBox_ignoreGoalDelay->setChecked(true);
 		_ui->source_checkBox_ignoreGoals->setChecked(true);
+		_ui->source_checkBox_ignoreLandmarks->setChecked(false);
 		_ui->source_spinBox_databaseStartId->setValue(0);
 		_ui->source_spinBox_databaseStopId->setValue(0);
 		_ui->source_spinBox_database_cameraIndex->setValue(-1);
@@ -2578,6 +2580,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->source_checkBox_ignoreOdometry->setChecked(settings.value("ignoreOdometry", _ui->source_checkBox_ignoreOdometry->isChecked()).toBool());
 	_ui->source_checkBox_ignoreGoalDelay->setChecked(settings.value("ignoreGoalDelay", _ui->source_checkBox_ignoreGoalDelay->isChecked()).toBool());
 	_ui->source_checkBox_ignoreGoals->setChecked(settings.value("ignoreGoals", _ui->source_checkBox_ignoreGoals->isChecked()).toBool());
+	_ui->source_checkBox_ignoreLandmarks->setChecked(settings.value("ignoreLandmarks", _ui->source_checkBox_ignoreLandmarks->isChecked()).toBool());
 	_ui->source_spinBox_databaseStartId->setValue(settings.value("startId", _ui->source_spinBox_databaseStartId->value()).toInt());
 	_ui->source_spinBox_databaseStopId->setValue(settings.value("stopId", _ui->source_spinBox_databaseStopId->value()).toInt());
 	_ui->source_spinBox_database_cameraIndex->setValue(settings.value("cameraIndex", _ui->source_spinBox_database_cameraIndex->value()).toInt());
@@ -3088,6 +3091,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("ignoreOdometry",    _ui->source_checkBox_ignoreOdometry->isChecked());
 	settings.setValue("ignoreGoalDelay",   _ui->source_checkBox_ignoreGoalDelay->isChecked());
 	settings.setValue("ignoreGoals",       _ui->source_checkBox_ignoreGoals->isChecked());
+	settings.setValue("ignoreLandmarks", _ui->source_checkBox_ignoreLandmarks->isChecked());
 	settings.setValue("startId",          _ui->source_spinBox_databaseStartId->value());
 	settings.setValue("stopId",          _ui->source_spinBox_databaseStopId->value());
 	settings.setValue("cameraIndex",       _ui->source_spinBox_database_cameraIndex->value());
@@ -6274,7 +6278,8 @@ Camera * PreferencesDialog::createCamera(
 				_ui->source_spinBox_databaseStartId->value(),
 				_ui->source_spinBox_database_cameraIndex->value(),
 				_ui->source_spinBox_databaseStopId->value(),
-				!_ui->general_checkBox_createIntermediateNodes->isChecked());
+				!_ui->general_checkBox_createIntermediateNodes->isChecked(),
+				_ui->source_checkBox_ignoreLandmarks->isChecked());
 	}
 	else
 	{

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-551</y>
+            <y>-496</y>
             <width>686</width>
             <height>3905</height>
            </rect>
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>22</number>
+                <number>5</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -3109,7 +3109,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                     <item>
                      <widget class="QStackedWidget" name="stackedWidget_src">
                       <property name="currentIndex">
-                       <number>0</number>
+                       <number>3</number>
                       </property>
                       <widget class="QWidget" name="page_41">
                        <layout class="QVBoxLayout" name="verticalLayout_64">
@@ -6087,10 +6087,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <bool>false</bool>
                           </property>
                           <layout class="QGridLayout" name="gridLayout_9" columnstretch="0,0,0">
-                           <item row="3" column="1">
+                           <item row="2" column="1">
-                            <widget class="QLabel" name="label_263">
+                            <widget class="QLabel" name="label_90">
                              <property name="text">
-                              <string>Ignore goals saved in the database.</string>
+                              <string>Use database stamps as input rate.</string>
                              </property>
                              <property name="wordWrap">
                               <bool>true</bool>
@@ -6100,6 +6100,36 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </widget>
                            </item>
+                           <item row="8" column="1">
+                            <widget class="QLabel" name="label_315">
+                             <property name="text">
+                              <string>Camera index. If the database contains multi-camera data, you can choose which camera to use. -1 means that all cameras are streamed.</string>
+                             </property>
+                             <property name="wordWrap">
+                              <bool>true</bool>
+                             </property>
+                             <property name="textInteractionFlags">
+                              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                             </property>
+                            </widget>
+                           </item>
+                           <item row="6" column="0">
+                            <widget class="QSpinBox" name="source_spinBox_databaseStartId">
+                             <property name="minimum">
+                              <number>0</number>
+                             </property>
+                             <property name="maximum">
+                              <number>99999</number>
+                             </property>
+                            </widget>
+                           </item>
+                           <item row="9" column="0">
+                            <widget class="QCheckBox" name="source_checkBox_stereoToDepthDB">
+                             <property name="text">
+                              <string/>
+                             </property>
+                            </widget>
+                           </item>
                            <item row="3" column="0">
                             <widget class="QCheckBox" name="source_checkBox_ignoreGoals">
                              <property name="text">
@@ -6107,13 +6137,43 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </widget>
                            </item>
-                           <item row="1" column="0">
+                           <item row="7" column="0">
-                            <widget class="QCheckBox" name="source_checkBox_ignoreOdometry">
+                            <widget class="QSpinBox" name="source_spinBox_databaseStopId">
+                             <property name="minimum">
+                              <number>0</number>
+                             </property>
+                             <property name="maximum">
+                              <number>99999</number>
+                             </property>
+                            </widget>
+                           </item>
+                           <item row="1" column="1">
+                            <widget class="QLabel" name="label_72">
+                             <property name="text">
+                              <string>Ignore odometry saved in the database, so if RGB-D SLAM is activated, odometry will be recomputed.</string>
+                             </property>
+                             <property name="wordWrap">
+                              <bool>true</bool>
+                             </property>
+                             <property name="textInteractionFlags">
+                              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                             </property>
+                            </widget>
+                           </item>
+                           <item row="2" column="0">
+                            <widget class="QCheckBox" name="source_checkBox_useDbStamps">
                              <property name="text">
                               <string/>
                              </property>
                             </widget>
                            </item>
+                           <item row="0" column="0">
+                            <widget class="QToolButton" name="source_database_toolButton_selectSource">
+                             <property name="text">
+                              <string>...</string>
+                             </property>
+                            </widget>
+                           </item>
                            <item row="0" column="2">
                             <widget class="QToolButton" name="toolButton_dbViewer">
                              <property name="toolTip">
@@ -6128,10 +6188,27 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </widget>
                            </item>
-                           <item row="0" column="0">
+                           <item row="1" column="0">
-                            <widget class="QToolButton" name="source_database_toolButton_selectSource">
+                            <widget class="QCheckBox" name="source_checkBox_ignoreOdometry">
                              <property name="text">
-                              <string>...</string>
+                              <string/>
+                             </property>
+                            </widget>
+                           </item>
+                           <item row="4" column="0">
+                            <widget class="QCheckBox" name="source_checkBox_ignoreGoalDelay">
+                             <property name="text">
+                              <string/>
+                             </property>
+                            </widget>
+                           </item>
+                           <item row="8" column="0">
+                            <widget class="QSpinBox" name="source_spinBox_database_cameraIndex">
+                             <property name="minimum">
+                              <number>-1</number>
+                             </property>
+                             <property name="maximum">
+                              <number>9999</number>
                              </property>
                             </widget>
                            </item>
@@ -6139,19 +6216,19 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                             <widget class="QLineEdit" name="source_database_lineEdit_path"/>
                            </item>
                            <item row="9" column="1">
-                            <spacer name="verticalSpacer_35">
+                            <widget class="QLabel" name="label_557">
-                             <property name="orientation">
+                             <property name="text">
-                              <enum>Qt::Vertical</enum>
+                              <string>If the database contains stereo data, generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images. Dense disparity parameters can be found under StereoBM tab.</string>
                              </property>
-                             <property name="sizeHint" stdset="0">
+                             <property name="wordWrap">
-                              <size>
+                              <bool>true</bool>
-                               <width>20</width>
-                               <height>0</height>
-                              </size>
                              </property>
-                            </spacer>
+                             <property name="textInteractionFlags">
+                              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                             </property>
+                            </widget>
                            </item>
-                           <item row="5" column="1">
+                           <item row="6" column="1">
                             <widget class="QLabel" name="label_58">
                              <property name="text">
                               <string>Start position (node ID).</string>
@@ -6161,20 +6238,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </widget>
                            </item>
-                           <item row="5" column="0">
+                           <item row="7" column="1">
-                            <widget class="QSpinBox" name="source_spinBox_databaseStartId">
+                            <widget class="QLabel" name="label_529">
-                             <property name="minimum">
-                              <number>0</number>
-                             </property>
-                             <property name="maximum">
-                              <number>99999</number>
-                             </property>
-                            </widget>
-                           </item>
-                           <item row="1" column="1">
-                            <widget class="QLabel" name="label_72">
                              <property name="text">
-                              <string>Ignore odometry saved in the database, so if RGB-D SLAM is activated, odometry will be recomputed.</string>
+                              <string>Stop position (node ID) is the last node to process. If 0, all nodes after start position are published.</string>
                              </property>
                              <property name="wordWrap">
                               <bool>true</bool>
@@ -6197,27 +6264,23 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </widget>
                            </item>
-                           <item row="4" column="0">
+                           <item row="10" column="1">
-                            <widget class="QCheckBox" name="source_checkBox_ignoreGoalDelay">
+                            <spacer name="verticalSpacer_35">
-                             <property name="text">
+                             <property name="orientation">
-                              <string/>
+                              <enum>Qt::Vertical</enum>
                              </property>
-                            </widget>
+                             <property name="sizeHint" stdset="0">
+                              <size>
+                               <width>20</width>
+                               <height>0</height>
+                              </size>
+                             </property>
+                            </spacer>
                            </item>
-                           <item row="7" column="0">
+                           <item row="3" column="1">
-                            <widget class="QSpinBox" name="source_spinBox_database_cameraIndex">
+                            <widget class="QLabel" name="label_263">
-                             <property name="minimum">
-                              <number>-1</number>
-                             </property>
-                             <property name="maximum">
-                              <number>9999</number>
-                             </property>
-                            </widget>
-                           </item>
-                           <item row="2" column="1">
-                            <widget class="QLabel" name="label_90">
                              <property name="text">
-                              <string>Use database stamps as input rate.</string>
+                              <string>Ignore goals saved in the database.</string>
                              </property>
                              <property name="wordWrap">
                               <bool>true</bool>
@@ -6227,17 +6290,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </widget>
                            </item>
-                           <item row="2" column="0">
+                           <item row="5" column="1">
-                            <widget class="QCheckBox" name="source_checkBox_useDbStamps">
+                            <widget class="QLabel" name="label_546">
                              <property name="text">
-                              <string/>
+                              <string>Ignore landmarks.</string>
-                             </property>
-                            </widget>
-                           </item>
-                           <item row="7" column="1">
-                            <widget class="QLabel" name="label_315">
-                             <property name="text">
-                              <string>Camera index. If the database contains multi-camera data, you can choose which camera to use. -1 means that all cameras are streamed.</string>
                              </property>
                              <property name="wordWrap">
                               <bool>true</bool>
@@ -6247,44 +6303,8 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </widget>
                            </item>
-                           <item row="6" column="1">
+                           <item row="5" column="0">
-                            <widget class="QLabel" name="label_529">
+                            <widget class="QCheckBox" name="source_checkBox_ignoreLandmarks">
-                             <property name="text">
-                              <string>Stop position (node ID) is the last node to process. If 0, all nodes after start position are published.</string>
-                             </property>
-                             <property name="wordWrap">
-                              <bool>true</bool>
-                             </property>
-                             <property name="textInteractionFlags">
-                              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                             </property>
-                            </widget>
-                           </item>
-                           <item row="6" column="0">
-                            <widget class="QSpinBox" name="source_spinBox_databaseStopId">
-                             <property name="minimum">
-                              <number>0</number>
-                             </property>
-                             <property name="maximum">
-                              <number>99999</number>
-                             </property>
-                            </widget>
-                           </item>
-                           <item row="8" column="1">
-                            <widget class="QLabel" name="label_557">
-                             <property name="text">
-                              <string>If the database contains stereo data, generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images. Dense disparity parameters can be found under StereoBM tab.</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="QCheckBox" name="source_checkBox_stereoToDepthDB">
                              <property name="text">
                               <string/>
                              </property>