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

Updated camera_info conversion for odometry nodes (https://github.com/introlab/rtabmap/issues/64)

Browse Source
This commit is contained in:
matlabbe
2016-03-27 10:09:30 -04:00
parent 81096a3f15
commit 19225a66be
2 changed files with 7 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/RGBDOdometryNode.cpp
+2 -16
View File
@@ -189,14 +189,7 @@ public:
if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0) if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
{ {
image_geometry::PinholeCameraModel model; rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform);
model.fromCameraInfo(*cameraInfo);
rtabmap::CameraModel rtabmapModel(
model.fx(),
model.fy(),
model.cx(),
model.cy(),
localTransform);
cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8"); cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth); cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth);
@@ -321,14 +314,7 @@ public:
return; return;
} }
image_geometry::PinholeCameraModel model; cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*infoMsgs[i], localTransform));
model.fromCameraInfo(*infoMsgs[i]);
cameraModels.push_back(rtabmap::CameraModel(
model.fx(),
model.fy(),
model.cx(),
model.cy(),
localTransform));
} }
rtabmap::SensorData data( rtabmap::SensorData data(
src/StereoOdometryNode.cpp
+5 -14
View File
@@ -145,24 +145,15 @@ public:
int quality = -1; int quality = -1;
if(imageRectLeft->data.size() && imageRectRight->data.size()) if(imageRectLeft->data.size() && imageRectRight->data.size())
{ {
image_geometry::StereoCameraModel model; rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform);
model.fromCameraInfo(*cameraInfoLeft, *cameraInfoRight); if(stereoModel.baseline() <= 0)
if(model.baseline() <= 0)
{ {
ROS_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo " ROS_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
"setup where the Tx (or P(0,3)) is negative in the right camera info msg.", model.baseline()); "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
return; return;
} }
rtabmap::StereoCameraModel stereoModel( if(stereoModel.baseline() > 10.0)
model.left().fx(),
model.left().fy(),
model.left().cx(),
model.left().cy(),
model.baseline(),
localTransform);
if(model.baseline() > 10.0)
{ {
static bool shown = false; static bool shown = false;
if(!shown) if(!shown)
@@ -170,7 +161,7 @@ public:
ROS_WARN("Detected baseline (%f m) is quite large! Is your " ROS_WARN("Detected baseline (%f m) is quite large! Is your "
"right camera_info P(0,3) correctly set? Note that " "right camera_info P(0,3) correctly set? Note that "
"baseline=-P(0,3)/P(0,0). This warning is printed only once.", "baseline=-P(0,3)/P(0,0). This warning is printed only once.",
model.baseline()); stereoModel.baseline());
shown = true; shown = true;
} }
} }