mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-08 12:30:20 +08:00
DBReader: publish landmarks by default (also added option to ignore them)
This commit is contained in:
matlabbe
@@ -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;
|
||||
|
||||
@@ -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() {}
|
||||
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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
|
||||
{
|
||||
|
||||
+120
-100
@@ -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">
|
||||
<widget class="QLabel" name="label_263">
|
||||
<item row="2" column="1">
|
||||
<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">
|
||||
<widget class="QCheckBox" name="source_checkBox_ignoreOdometry">
|
||||
<item row="7" 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="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">
|
||||
<widget class="QToolButton" name="source_database_toolButton_selectSource">
|
||||
<item row="1" column="0">
|
||||
<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">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Vertical</enum>
|
||||
<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="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>20</width>
|
||||
<height>0</height>
|
||||
</size>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</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">
|
||||
<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="1" column="1">
|
||||
<widget class="QLabel" name="label_72">
|
||||
<item row="7" column="1">
|
||||
<widget class="QLabel" name="label_529">
|
||||
<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">
|
||||
<widget class="QCheckBox" name="source_checkBox_ignoreGoalDelay">
|
||||
<property name="text">
|
||||
<string/>
|
||||
<item row="10" column="1">
|
||||
<spacer name="verticalSpacer_35">
|
||||
<property name="orientation">
|
||||
<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">
|
||||
<widget class="QSpinBox" name="source_spinBox_database_cameraIndex">
|
||||
<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">
|
||||
<item row="3" column="1">
|
||||
<widget class="QLabel" name="label_263">
|
||||
<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">
|
||||
<widget class="QCheckBox" name="source_checkBox_useDbStamps">
|
||||
<item row="5" column="1">
|
||||
<widget class="QLabel" name="label_546">
|
||||
<property name="text">
|
||||
<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>
|
||||
<string>Ignore landmarks.</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">
|
||||
<widget class="QLabel" name="label_529">
|
||||
<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">
|
||||
<item row="5" column="0">
|
||||
<widget class="QCheckBox" name="source_checkBox_ignoreLandmarks">
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
|
||||
Reference in New Issue
Block a user