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

Added groundTruthPose field to NodeData msg

Browse Source
This commit is contained in:
matlabbe
2016-01-12 12:39:23 -05:00
parent e877c543ac
commit 8dacddb110
4 changed files with 10 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+1 -1
View File
@@ -1269,7 +1269,7 @@ void CoreWrapper::process(
std::map<int, rtabmap::Signature> tmpSignature;
SensorData tmpData = data;
tmpData.setId(-1);
tmpSignature.insert(std::make_pair(-1, Signature(-1, -1, 0, data.stamp(), "", odom, tmpData)));
tmpSignature.insert(std::make_pair(-1, Signature(-1, -1, 0, data.stamp(), "", odom, Transform(), tmpData)));
filteredPoses.insert(std::make_pair(-1, rtabmap_.getMapCorrection()*odom));
// Update maps
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(), tmpData)));
uInsert(nodes_, std::make_pair(-1, Signature(-1, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), tmpS.getGroundTruthPose(), tmpData)));
poses.insert(std::make_pair(-1, poses.rbegin()->second));
}
src/MsgConversion.cpp
+5 -1
View File
@@ -486,6 +486,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
msg.stamp,
msg.label,
transformFromPoseMsg(msg.pose),
transformFromPoseMsg(msg.groundTruthPose),
stereoModel.isValid()?
rtabmap::SensorData(
compressedMatFromBytes(msg.laserScan),
@@ -520,6 +521,7 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
msg.stamp = signature.getStamp();
msg.label = signature.getLabel();
transformToPoseMsg(signature.getPose(), msg.pose);
transformToPoseMsg(signature.getGroundTruthPose(), msg.groundTruthPose);
compressedMatToBytes(signature.sensorData().imageCompressed(), msg.image);
compressedMatToBytes(signature.sensorData().depthOrRightCompressed(), msg.depth);
compressedMatToBytes(signature.sensorData().laserScanCompressed(), msg.laserScan);
@@ -596,7 +598,8 @@ rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData & msg)
msg.weight,
msg.stamp,
msg.label,
transformFromPoseMsg(msg.pose));
transformFromPoseMsg(msg.pose),
transformFromPoseMsg(msg.groundTruthPose));
return s;
}
void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg)
@@ -608,6 +611,7 @@ void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
msg.stamp = signature.getStamp();
msg.label = signature.getLabel();
transformToPoseMsg(signature.getPose(), msg.pose);
transformToPoseMsg(signature.getGroundTruthPose(), msg.groundTruthPose);
}
rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)