i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added OpenNI2 and Freenect calibration #115

Browse Source
This commit is contained in:
matlabbe
2016-09-13 18:26:18 -04:00
parent e5977d6157
commit acb700bd27
14 changed files with 1248 additions and 753 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
corelib/src/util3d.cpp
View File

@@ -779,7 +779,10 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
if(tmp->size())
{
tmp = util3d::transformPointCloud(tmp, model.localTransform());
if(!model.localTransform().isNull() && !model.localTransform().isIdentity())
{
tmp = util3d::transformPointCloud(tmp, model.localTransform());
}
if(sensorData.cameraModels().size() > 1)
{
@@ -833,7 +836,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
if(cloud->size())
{
if(cloud->size())
if(cloud->size() && !sensorData.stereoCameraModel().left().localTransform().isNull() && !sensorData.stereoCameraModel().left().localTransform().isIdentity())
{
cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
}
@@ -862,8 +865,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
UDEBUG("");
UASSERT(int((sensorData.imageRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.imageRaw().cols);
UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols);
UASSERT(sensorData.imageRaw().cols % sensorData.depthRaw().cols == 0);
UASSERT(sensorData.imageRaw().rows % sensorData.depthRaw().rows == 0);
UASSERT_MSG(sensorData.imageRaw().cols % sensorData.depthRaw().cols == 0, uFormat("rgb=%d depth=%d", sensorData.imageRaw().cols, sensorData.depthRaw().cols).c_str());
UASSERT_MSG(sensorData.imageRaw().rows % sensorData.depthRaw().rows == 0, uFormat("rgb=%d depth=%d", sensorData.imageRaw().rows, sensorData.depthRaw().rows).c_str());
int subRGBWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size();
int subDepthWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
@@ -929,7 +932,10 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
if(tmp->size())
{
tmp = util3d::transformPointCloud(tmp, model.localTransform());
if(!model.localTransform().isNull() && !model.localTransform().isIdentity())
{
tmp = util3d::transformPointCloud(tmp, model.localTransform());
}
if(sensorData.cameraModels().size() > 1)
{
@@ -972,7 +978,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
validIndices,
stereoParameters);
if(cloud->size())
if(cloud->size() && !sensorData.stereoCameraModel().left().localTransform().isNull() && !sensorData.stereoCameraModel().left().localTransform().isIdentity())
{
cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
}