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

Added cameraModelToROS()

Browse Source
This commit is contained in:
matlabbe
2016-04-26 10:19:29 -04:00
parent 76cb4365dd
commit e67770cc31
2 changed files with 38 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/MsgConversion.h
+4
View File
@@ -95,6 +95,10 @@ void points3fToROS(const std::vector<cv::Point3f> & kpts, std::vector<rtabmap_ro
rtabmap::CameraModel cameraModelFromROS( rtabmap::CameraModel cameraModelFromROS(
const sensor_msgs::CameraInfo & camInfo, const sensor_msgs::CameraInfo & camInfo,
const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity()); const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity());
void cameraModelToROS(
const rtabmap::CameraModel & model,
sensor_msgs::CameraInfo & camInfo);
rtabmap::StereoCameraModel stereoCameraModelFromROS( rtabmap::StereoCameraModel stereoCameraModelFromROS(
const sensor_msgs::CameraInfo & leftCamInfo, const sensor_msgs::CameraInfo & leftCamInfo,
const sensor_msgs::CameraInfo & rightCamInfo, const sensor_msgs::CameraInfo & rightCamInfo,
src/MsgConversion.cpp
+34
View File
@@ -341,6 +341,40 @@ rtabmap::CameraModel cameraModelFromROS(
0.0, 0.0,
cv::Size(model.fullResolution().width, model.fullResolution().height)); cv::Size(model.fullResolution().width, model.fullResolution().height));
} }
void cameraModelToROS(
const rtabmap::CameraModel & model,
sensor_msgs::CameraInfo & camInfo)
{
UASSERT(model.isValidForRectification());
camInfo.D = std::vector<double>(model.D_raw().cols);
memcpy(camInfo.D.data(), model.D_raw().data, model.D_raw().cols*sizeof(double));
UASSERT(model.K_raw().total() == 9);
memcpy(camInfo.K.elems, model.K_raw().data, 9*sizeof(double));
UASSERT(model.R().total() == 9);
memcpy(camInfo.R.elems, model.R().data, 9*sizeof(double));
UASSERT(model.P().total() == 12);
memcpy(camInfo.P.elems, model.P().data, 12*sizeof(double));
if(camInfo.D.size() > 5)
{
camInfo.distortion_model = "rational_polynomial";
}
else
{
camInfo.distortion_model = "plumb_bob";
}
camInfo.binning_x = 1;
camInfo.binning_y = 1;
camInfo.roi.width = model.imageWidth();
camInfo.roi.height = model.imageHeight();
camInfo.width = model.imageWidth();
camInfo.height = model.imageHeight();
}
rtabmap::StereoCameraModel stereoCameraModelFromROS( rtabmap::StereoCameraModel stereoCameraModelFromROS(
const sensor_msgs::CameraInfo & leftCamInfo, const sensor_msgs::CameraInfo & leftCamInfo,
const sensor_msgs::CameraInfo & rightCamInfo, const sensor_msgs::CameraInfo & rightCamInfo,