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

OodmInfo msg: Added local bundle key frames

Browse Source
This commit is contained in:
matlabbe
2021-09-30 15:04:03 -04:00
parent a5fe8df170
commit 3288c56151
2 changed files with 35 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/OdomInfo.msg
+10
View File
@@ -28,6 +28,16 @@ int32 memoryUsage # MB
float32 gravityRollError float32 gravityRollError
float32 gravityPitchError float32 gravityPitchError
# Local bundle camera ids
int32[] localBundleIds
# Local bundle camera models
sensor_msgs/CameraInfo[] localBundleModels
geometry_msgs/Transform[] localBundleModelTransforms
# Local bundle camera poses
geometry_msgs/Pose[] localBundlePoses
geometry_msgs/Transform transform geometry_msgs/Transform transform
geometry_msgs/Transform transformFiltered geometry_msgs/Transform transformFiltered
geometry_msgs/Transform transformGroundTruth geometry_msgs/Transform transformGroundTruth
src/MsgConversion.cpp
+25
View File
@@ -1439,6 +1439,14 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg, bool ig
info.localBundleOutliers = msg.localBundleOutliers; info.localBundleOutliers = msg.localBundleOutliers;
info.localBundleConstraints = msg.localBundleConstraints; info.localBundleConstraints = msg.localBundleConstraints;
info.localBundleTime = msg.localBundleTime; info.localBundleTime = msg.localBundleTime;
UASSERT(msg.localBundleModels.size() == msg.localBundleIds.size());
UASSERT(msg.localBundleModels.size() == msg.localBundleModelTransforms.size());
UASSERT(msg.localBundleModels.size() == msg.localBundlePoses.size());
for(size_t i=0; i<msg.localBundleIds.size(); ++i)
{
info.localBundleModels.insert(std::make_pair(msg.localBundleIds[i], cameraModelFromROS(msg.localBundleModels[i], transformFromGeometryMsg(msg.localBundleModelTransforms[i]))));
info.localBundlePoses.insert(std::make_pair(msg.localBundleIds[i], transformFromPoseMsg(msg.localBundlePoses[i])));
}
info.keyFrameAdded = msg.keyFrameAdded; info.keyFrameAdded = msg.keyFrameAdded;
info.timeEstimation = msg.timeEstimation; info.timeEstimation = msg.timeEstimation;
info.timeParticleFiltering = msg.timeParticleFiltering; info.timeParticleFiltering = msg.timeParticleFiltering;
@@ -1506,6 +1514,23 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
msg.localBundleOutliers = info.localBundleOutliers; msg.localBundleOutliers = info.localBundleOutliers;
msg.localBundleConstraints = info.localBundleConstraints; msg.localBundleConstraints = info.localBundleConstraints;
msg.localBundleTime = info.localBundleTime; msg.localBundleTime = info.localBundleTime;
UASSERT(info.localBundleModels.size() == info.localBundlePoses.size());
for(std::map<int, rtabmap::CameraModel>::const_iterator iter=info.localBundleModels.begin();
iter!=info.localBundleModels.end();
++iter)
{
msg.localBundleIds.push_back(iter->first);
sensor_msgs::CameraInfo camInfo;
cameraModelToROS(iter->second, camInfo);
msg.localBundleModels.push_back(camInfo);
geometry_msgs::Transform localT;
transformToGeometryMsg(iter->second.localTransform(), localT);
msg.localBundleModelTransforms.push_back(localT);
UASSERT(info.localBundlePoses.find(iter->first)!=info.localBundlePoses.end());
geometry_msgs::Pose pose;
transformToPoseMsg(info.localBundlePoses.at(iter->first), pose);
msg.localBundlePoses.push_back(pose);
}
msg.keyFrameAdded = info.keyFrameAdded; msg.keyFrameAdded = info.keyFrameAdded;
msg.timeEstimation = info.timeEstimation; msg.timeEstimation = info.timeEstimation;
msg.timeParticleFiltering = info.timeParticleFiltering; msg.timeParticleFiltering = info.timeParticleFiltering;