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

rtabmap: added "ground_truth_base_frame_id" parameter

Browse Source
This commit is contained in:
matlabbe
2016-08-11 14:58:49 -04:00
parent 0930c816ba
commit 3ae00d6ad9
2 changed files with 12 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/CoreWrapper.h
+1
View File
@@ -268,6 +268,7 @@ private:
std::string mapFrameId_; std::string mapFrameId_;
std::string odomFrameId_; std::string odomFrameId_;
std::string groundTruthFrameId_; std::string groundTruthFrameId_;
std::string groundTruthBaseFrameId_;
std::string configPath_; std::string configPath_;
std::string databasePath_; std::string databasePath_;
bool waitForTransform_; bool waitForTransform_;
src/CoreWrapper.cpp
+11 -3
View File
@@ -88,6 +88,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
mapFrameId_("map"), mapFrameId_("map"),
odomFrameId_(""), odomFrameId_(""),
groundTruthFrameId_(""), // e.g., "world" groundTruthFrameId_(""), // e.g., "world"
groundTruthBaseFrameId_(""), // e.g., "base_link_gt"
configPath_(""), configPath_(""),
databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()), databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()),
waitForTransform_(true), waitForTransform_(true),
@@ -179,6 +180,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
pnh.param("map_frame_id", mapFrameId_, mapFrameId_); pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF
pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_); pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
pnh.param("ground_truth_base_frame_id", groundTruthBaseFrameId_, frameId_);
pnh.param("depth_cameras", depthCameras, depthCameras); pnh.param("depth_cameras", depthCameras, depthCameras);
pnh.param("queue_size", queueSize, queueSize); pnh.param("queue_size", queueSize, queueSize);
if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync")) if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync"))
@@ -225,6 +227,10 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
{ {
groundTruthFrameId_ = tfPrefix+"/"+groundTruthFrameId_; groundTruthFrameId_ = tfPrefix+"/"+groundTruthFrameId_;
} }
if(!groundTruthBaseFrameId_.empty())
{
groundTruthBaseFrameId_ = tfPrefix+"/"+groundTruthBaseFrameId_;
}
// keep worldFrameId_ without prefix as it should be global // keep worldFrameId_ without prefix as it should be global
} }
@@ -240,7 +246,9 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
} }
if(!groundTruthFrameId_.empty()) if(!groundTruthFrameId_.empty())
{ {
ROS_INFO("rtabmap: ground_truth_frame_id = %s", groundTruthFrameId_.c_str()); ROS_INFO("rtabmap: ground_truth_frame_id = %s -> ground_truth_base_frame_id = %s",
groundTruthFrameId_.c_str(),
groundTruthBaseFrameId_.c_str());
} }
ROS_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str()); ROS_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str());
ROS_INFO("rtabmap: queue_size = %d", queueSize); ROS_INFO("rtabmap: queue_size = %d", queueSize);
@@ -1114,7 +1122,7 @@ void CoreWrapper::commonDepthCallback(
Transform groundTruthPose; Transform groundTruthPose;
if(!groundTruthFrameId_.empty()) if(!groundTruthFrameId_.empty())
{ {
groundTruthPose = getTransform(groundTruthFrameId_, frameId_, stamp); groundTruthPose = getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, stamp);
} }
SensorData data(scan, SensorData data(scan,
@@ -1340,7 +1348,7 @@ void CoreWrapper::commonStereoCallback(
Transform groundTruthPose; Transform groundTruthPose;
if(!groundTruthFrameId_.empty()) if(!groundTruthFrameId_.empty())
{ {
groundTruthPose = getTransform(groundTruthFrameId_, frameId_, stamp); groundTruthPose = getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, stamp);
} }
SensorData data(scan, SensorData data(scan,