i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
matlabbe
2021-06-29 16:03:21 -04:00
parent 3da4bb9faa
commit 05d45872f3
5 changed files with 155 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
corelib/include/rtabmap/core/DBReader.h
View File

@@ -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;

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

@@ -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() {}

25
corelib/src/DBReader.cpp
View File

@@ -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,