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

cameraModelToROS: removed isValidForRectification() assert

Browse Source
This commit is contained in:
Mathieu Labbe
2016-06-15 10:28:04 -04:00
parent d80b685c20
commit 21eb5120cb
1 changed files with 27 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+27 -8
View File
@@ -345,19 +345,38 @@ 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.K_raw().empty() || model.K_raw().total() == 9);
if(model.K_raw().empty())
{
memset(camInfo.K.elems, 0.0, 9*sizeof(double));
}
else
{
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.R().empty() || model.R().total() == 9);
if(model.R().empty())
{
memset(camInfo.R.elems, 0.0, 9*sizeof(double));
}
else
{
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));
UASSERT(model.P().empty() || model.P().total() == 12);
if(model.P().empty())
{
memset(camInfo.P.elems, 0.0, 12*sizeof(double));
}
else
{
memcpy(camInfo.P.elems, model.P().data, 12*sizeof(double));
}
if(camInfo.D.size() > 5)
{