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

Fixed build with latest rtabmap master

Browse Source
This commit is contained in:
matlabbe
2022-07-20 16:19:51 -04:00
parent b81009b9f6
commit f32d80c82c
2 changed files with 32 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+31 -26
View File
@@ -232,14 +232,14 @@ void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_ros::msg::RGBDImag
msg.rgb_camera_info.header = header; msg.rgb_camera_info.header = header;
localTransform = data.cameraModels().front().localTransform(); localTransform = data.cameraModels().front().localTransform();
} }
else else if(data.stereoCameraModels().size() == 1)
{ {
//stereo //stereo
rtabmap_ros::cameraModelToROS(data.stereoCameraModel().left(), msg.rgb_camera_info); rtabmap_ros::cameraModelToROS(data.stereoCameraModels()[0].left(), msg.rgb_camera_info);
rtabmap_ros::cameraModelToROS(data.stereoCameraModel().right(), msg.depth_camera_info); rtabmap_ros::cameraModelToROS(data.stereoCameraModels()[0].right(), msg.depth_camera_info);
msg.rgb_camera_info.header = header; msg.rgb_camera_info.header = header;
msg.depth_camera_info.header = header; msg.depth_camera_info.header = header;
localTransform = data.stereoCameraModel().localTransform(); localTransform = data.stereoCameraModels()[0].localTransform();
} }
if(!data.imageRaw().empty()) if(!data.imageRaw().empty())
@@ -1264,17 +1264,17 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::msg::NodeD
transformToGeometryMsg(signature.sensorData().cameraModels()[i].localTransform(), msg.local_transform[i]); transformToGeometryMsg(signature.sensorData().cameraModels()[i].localTransform(), msg.local_transform[i]);
} }
} }
else if(signature.sensorData().stereoCameraModel().isValidForProjection()) else if(signature.sensorData().stereoCameraModels().size()==1)
{ {
msg.fx.push_back(signature.sensorData().stereoCameraModel().left().fx()); msg.fx.push_back(signature.sensorData().stereoCameraModels()[0].left().fx());
msg.fy.push_back(signature.sensorData().stereoCameraModel().left().fy()); msg.fy.push_back(signature.sensorData().stereoCameraModels()[0].left().fy());
msg.cx.push_back(signature.sensorData().stereoCameraModel().left().cx()); msg.cx.push_back(signature.sensorData().stereoCameraModels()[0].left().cx());
msg.cy.push_back(signature.sensorData().stereoCameraModel().left().cy()); msg.cy.push_back(signature.sensorData().stereoCameraModels()[0].left().cy());
msg.width.push_back(signature.sensorData().stereoCameraModel().left().imageWidth()); msg.width.push_back(signature.sensorData().stereoCameraModels()[0].left().imageWidth());
msg.height.push_back(signature.sensorData().stereoCameraModel().left().imageHeight()); msg.height.push_back(signature.sensorData().stereoCameraModels()[0].left().imageHeight());
msg.baseline = signature.sensorData().stereoCameraModel().baseline(); msg.baseline = signature.sensorData().stereoCameraModels()[0].baseline();
msg.local_transform.resize(1); msg.local_transform.resize(1);
transformToGeometryMsg(signature.sensorData().stereoCameraModel().left().localTransform(), msg.local_transform[0]); transformToGeometryMsg(signature.sensorData().stereoCameraModels()[0].left().localTransform(), msg.local_transform[0]);
} }
//Features stuff... //Features stuff...
@@ -1477,7 +1477,9 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::msg::OdomInfo & msg, bo
UASSERT(msg.local_bundle_models.size() == msg.local_bundle_poses.size()); UASSERT(msg.local_bundle_models.size() == msg.local_bundle_poses.size());
for(size_t i=0; i<msg.local_bundle_ids.size(); ++i) for(size_t i=0; i<msg.local_bundle_ids.size(); ++i)
{ {
info.localBundleModels.insert(std::make_pair(msg.local_bundle_ids[i], cameraModelFromROS(msg.local_bundle_models[i], transformFromGeometryMsg(msg.local_bundle_model_transforms[i])))); std::vector<rtabmap::CameraModel> models;
models.push_back(cameraModelFromROS(msg.local_bundle_models[i], transformFromGeometryMsg(msg.local_bundle_model_transforms[i])));
info.localBundleModels.insert(std::make_pair(msg.local_bundle_ids[i], models));
info.localBundlePoses.insert(std::make_pair(msg.local_bundle_ids[i], transformFromPoseMsg(msg.local_bundle_poses[i]))); info.localBundlePoses.insert(std::make_pair(msg.local_bundle_ids[i], transformFromPoseMsg(msg.local_bundle_poses[i])));
} }
info.keyFrameAdded = msg.key_frame_added; info.keyFrameAdded = msg.key_frame_added;
@@ -1548,21 +1550,24 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::msg::OdomInf
msg.local_bundle_constraints = info.localBundleConstraints; msg.local_bundle_constraints = info.localBundleConstraints;
msg.local_bundle_time = info.localBundleTime; msg.local_bundle_time = info.localBundleTime;
UASSERT(info.localBundleModels.size() == info.localBundlePoses.size()); UASSERT(info.localBundleModels.size() == info.localBundlePoses.size());
for(std::map<int, rtabmap::CameraModel>::const_iterator iter=info.localBundleModels.begin(); for(std::map<int, std::vector<rtabmap::CameraModel> >::const_iterator iter=info.localBundleModels.begin();
iter!=info.localBundleModels.end(); iter!=info.localBundleModels.end();
++iter) ++iter)
{ {
msg.local_bundle_ids.push_back(iter->first); if(iter->second.size())
sensor_msgs::msg::CameraInfo camInfo; {
cameraModelToROS(iter->second, camInfo); msg.local_bundle_ids.push_back(iter->first);
msg.local_bundle_models.push_back(camInfo); sensor_msgs::msg::CameraInfo camInfo;
geometry_msgs::msg::Transform localT; cameraModelToROS(iter->second[0], camInfo);
transformToGeometryMsg(iter->second.localTransform(), localT); msg.local_bundle_models.push_back(camInfo);
msg.local_bundle_model_transforms.push_back(localT); geometry_msgs::msg::Transform localT;
UASSERT(info.localBundlePoses.find(iter->first)!=info.localBundlePoses.end()); transformToGeometryMsg(iter->second[0].localTransform(), localT);
geometry_msgs::msg::Pose pose; msg.local_bundle_model_transforms.push_back(localT);
transformToPoseMsg(info.localBundlePoses.at(iter->first), pose); UASSERT(info.localBundlePoses.find(iter->first)!=info.localBundlePoses.end());
msg.local_bundle_poses.push_back(pose); geometry_msgs::msg::Pose pose;
transformToPoseMsg(info.localBundlePoses.at(iter->first), pose);
msg.local_bundle_poses.push_back(pose);
}
} }
msg.key_frame_added = info.keyFrameAdded; msg.key_frame_added = info.keyFrameAdded;
msg.time_estimation = info.timeEstimation; msg.time_estimation = info.timeEstimation;
src/rviz/MapCloudDisplay.cpp
+1 -1
View File
@@ -280,7 +280,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::msg::MapData& map)
if((fromDepth && if((fromDepth &&
!s.sensorData().imageCompressed().empty() && !s.sensorData().imageCompressed().empty() &&
!s.sensorData().depthOrRightCompressed().empty() && !s.sensorData().depthOrRightCompressed().empty() &&
(s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValidForProjection())) || (s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModels().size())) ||
(!fromDepth && !s.sensorData().laserScanCompressed().isEmpty())) (!fromDepth && !s.sensorData().laserScanCompressed().isEmpty()))
{ {
cv::Mat image, depth; cv::Mat image, depth;