i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added ground_truth_frame_id to rtabmap node (to save ground truth in nodes)

Browse Source
This commit is contained in:
matlabbe
2016-01-12 16:07:15 -05:00
parent dc81f2446d
commit 352010cd2d
3 changed files with 48 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+46 -16
View File
@@ -80,6 +80,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
frameId_("base_link"),
mapFrameId_("map"),
odomFrameId_(""),
groundTruthFrameId_(""), // e.g., "world"
configPath_(""),
databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()),
waitForTransform_(true),
@@ -153,6 +154,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
pnh.param("frame_id", frameId_, frameId_);
pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF
pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
pnh.param("depth_cameras", depthCameras, depthCameras);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("stereo_approx_sync", stereoApproxSync, stereoApproxSync);
@@ -180,6 +182,11 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
{
odomFrameId_ = tfPrefix+"/"+odomFrameId_;
}
if(!groundTruthFrameId_.empty())
{
groundTruthFrameId_ = tfPrefix+"/"+groundTruthFrameId_;
}
// keep worldFrameId_ without prefix as it should be global
}
if(depthCameras <= 0 && subscribeDepth)
@@ -192,6 +199,10 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
{
ROS_INFO("rtabmap: odom_frame_id = %s", odomFrameId_.c_str());
}
if(!groundTruthFrameId_.empty())
{
ROS_INFO("rtabmap: ground_truth_frame_id = %s", groundTruthFrameId_.c_str());
}
ROS_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str());
ROS_INFO("rtabmap: queue_size = %d", queueSize);
ROS_INFO("rtabmap: tf_delay = %f", tfDelay);
@@ -880,15 +891,24 @@ void CoreWrapper::commonDepthCallback(
scan3dMsg.get() != 0?scan3dMsg->header.stamp:
depthMsgs[0]->header.stamp;
Transform groundTruthPose;
if(!groundTruthFrameId_.empty())
{
groundTruthPose = getTransform(groundTruthFrameId_, frameId_, stamp);
}
SensorData data(scan,
scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():genMaxScanPts,
scan2dMsg.get() != 0?scan2dMsg->range_max:(genScan_?genScanMaxDepth_:0.0f),
rgb,
depth,
cameraModels,
imageMsgs[0]->header.seq,
rtabmap_ros::timestampFromROS(stamp));
data.setGroundTruth(groundTruthPose);
process(stamp,
SensorData(scan,
scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():genMaxScanPts,
scan2dMsg.get() != 0?scan2dMsg->range_max:(genScan_?genScanMaxDepth_:0.0f),
rgb,
depth,
cameraModels,
imageMsgs[0]->header.seq,
rtabmap_ros::timestampFromROS(stamp)),
data,
lastPose_,
odomFrameId,
uIsFinite(rotVariance_) && rotVariance_>0?rotVariance_:1.0,
@@ -1019,15 +1039,25 @@ void CoreWrapper::commonStereoCallback(
ros::Time stamp = scan2dMsg.get() != 0?scan2dMsg->header.stamp:
scan3dMsg.get() != 0?scan3dMsg->header.stamp:
leftImageMsg->header.stamp;
Transform groundTruthPose;
if(!groundTruthFrameId_.empty())
{
groundTruthPose = getTransform(groundTruthFrameId_, frameId_, stamp);
}
SensorData data(scan,
scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():0,
scan2dMsg.get() != 0?scan2dMsg->range_max:0,
ptrLeftImage->image,
ptrRightImage->image,
stereoModel,
leftImageMsg->header.seq,
rtabmap_ros::timestampFromROS(stamp));
data.setGroundTruth(groundTruthPose);
process(stamp,
SensorData(scan,
scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():0,
scan2dMsg.get() != 0?scan2dMsg->range_max:0,
ptrLeftImage->image,
ptrRightImage->image,
stereoModel,
leftImageMsg->header.seq,
rtabmap_ros::timestampFromROS(stamp)),
data,
lastPose_,
odomFrameId,
uIsFinite(rotVariance_) && rotVariance_>0?rotVariance_:1.0,
src/CoreWrapper.h
+1
View File
@@ -266,6 +266,7 @@ private:
std::string frameId_;
std::string mapFrameId_;
std::string odomFrameId_;
std::string groundTruthFrameId_;
std::string configPath_;
std::string databasePath_;
bool waitForTransform_;
src/MapAssemblerNode.cpp
+1 -1
View File
@@ -89,7 +89,7 @@ public:
Signature tmpS = nodes_.at(poses.rbegin()->first);
SensorData tmpData = tmpS.sensorData();
tmpData.setId(-1);
uInsert(nodes_, std::make_pair(-1, Signature(-1, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), tmpS.getGroundTruthPose(), tmpData)));
uInsert(nodes_, std::make_pair(-1, Signature(-1, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), Transform(), tmpData)));
poses.insert(std::make_pair(-1, poses.rbegin()->second));
}