Added util3d::laserScanFomrDepthImage() and some refactoring

corelib/include/rtabmap/core/util3d.h

@@ -117,6 +117,25 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		float voxelSize = 0.0f,
 		int samples = 0);
 
+pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
+					const cv::Mat & depthImage,
+					float fx,
+					float fy,
+					float cx,
+					float cy,
+					float maxDepth = 0,
+					const Transform & localTransform = Transform::getIdentity());
+
+cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F);
+cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U);
+
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan);
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cvMat2Cloud(
+		const cv::Mat & matrix,
+		const Transform & tranform = Transform::getIdentity());
+
 pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
 		const cv::Point2f & pt,
 		float disparity,

corelib/include/rtabmap/core/util3d_conversions.h

@@ -1,57 +0,0 @@
-/*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#ifndef UTIL3D_CONVERSIONS_H_
-#define UTIL3D_CONVERSIONS_H_
-
-#include <rtabmap/core/RtabmapExp.h>
-
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <opencv2/core/core.hpp>
-#include <rtabmap/core/Transform.h>
-
-namespace rtabmap
-{
-
-namespace util3d
-{
-
-cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F);
-cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U);
-
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan);
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cvMat2Cloud(
-		const cv::Mat & matrix,
-		const Transform & tranform = Transform::getIdentity());
-
-} // namespace util3d
-} // namespace rtabmap
-
-#endif /* UTIL3D_CONVERSIONS_H_ */

corelib/src/CMakeLists.txt

@@ -35,7 +35,6 @@ SET(SRC_FILES
 	util3d_surface.cpp
 	util3d_features.cpp
 	util3d_correspondences.cpp
-	util3d_conversions.cpp
 	
 	SensorData.cpp
 	Graph.cpp

corelib/src/Memory.cpp

@@ -41,7 +41,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "VisualWord.h"
 #include "rtabmap/core/Features2d.h"
 #include "DBDriverSqlite3.h"
-#include "rtabmap/core/util3d_conversions.h"
 #include "rtabmap/core/util3d_features.h"
 #include "rtabmap/core/util3d_filtering.h"
 #include "rtabmap/core/util3d_correspondences.h"

corelib/src/Odometry.cpp

@@ -95,12 +95,7 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 		UASSERT(!data.depthOrRightRaw().empty());
 	}
 
-	if(data.cameraModels().size() > 1)
+	if(!data.stereoCameraModel().isValid() &&
-	{
-		UERROR("Odometry doesn't support multi-camera yet.");
-		return Transform();
-	}
-	else if(!data.stereoCameraModel().isValid() &&
 	   (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValid()))
 	{
 		UERROR("Rectified images required! Calibrate your camera.");

corelib/src/Rtabmap.cpp

@@ -1412,6 +1412,9 @@ bool Rtabmap::process(
 						++iter)
 					{
 						if(immunizedLocally >= maxLocalLocationsImmunized)
+						{
+							// set 20 to avoid this warning when starting mapping
+							if(maxLocalLocationsImmunized > 20)
 							{
 								UWARN("Could not immunize the whole local path (%d) between "
 									  "%d and %d (max location immunized=%d). You may want "
@@ -1424,6 +1427,7 @@ bool Rtabmap::process(
 										_localImmunizationRatio,
 										maxLocalLocationsImmunized,
 										(int)_memory->getWorkingMem().size());
+							}
 							break;
 						}
 						else if(!_memory->isInSTM(iter->first))

corelib/src/util3d.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UMath.h>
 #include <pcl/io/pcd_io.h>
+#include <pcl/common/transforms.h>
 #include <opencv2/imgproc/imgproc.hpp>
 
 namespace rtabmap
@@ -723,6 +724,143 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 	return cloud;
 }
 
+pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
+					const cv::Mat & depthImage,
+					float fx,
+					float fy,
+					float cx,
+					float cy,
+					float maxDepth,
+					const Transform & localTransform)
+{
+	UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
+	UASSERT(!localTransform.isNull());
+
+	pcl::PointCloud<pcl::PointXYZ> scan;
+	int middle = depthImage.rows/2;
+	if(middle)
+	{
+		scan.resize(depthImage.cols);
+		int oi = 0;
+		for(int i=0; i<depthImage.cols; ++i)
+		{
+			pcl::PointXYZ pt = util3d::projectDepthTo3D(depthImage, i, middle, cx, cy, fx, fy, false);
+			if(pcl::isFinite(pt) && (maxDepth == 0 || pt.z < maxDepth))
+			{
+				if(!localTransform.isIdentity())
+				{
+					pt = util3d::transformPoint(pt, localTransform);
+				}
+				scan[oi++] = pt;
+			}
+		}
+		scan.resize(oi);
+	}
+	return scan;
+}
+
+
+cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
+{
+	UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
+	cv::Mat depth16U;
+	if(!depth32F.empty())
+	{
+		depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
+		for(int i=0; i<depth32F.rows; ++i)
+		{
+			for(int j=0; j<depth32F.cols; ++j)
+			{
+				float depth = (depth32F.at<float>(i,j)*1000.0f);
+				unsigned short depthMM = 0;
+				if(depth <= (float)USHRT_MAX)
+				{
+					depthMM = (unsigned short)depth;
+				}
+				depth16U.at<unsigned short>(i, j) = depthMM;
+			}
+		}
+	}
+	return depth16U;
+}
+
+cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
+{
+	UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
+	cv::Mat depth32F;
+	if(!depth16U.empty())
+	{
+		depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
+		for(int i=0; i<depth16U.rows; ++i)
+		{
+			for(int j=0; j<depth16U.cols; ++j)
+			{
+				float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
+				depth32F.at<float>(i, j) = depth;
+			}
+		}
+	}
+	return depth32F;
+}
+
+cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
+{
+	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
+	for(unsigned int i=0; i<cloud.size(); ++i)
+	{
+		laserScan.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
+		laserScan.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
+	}
+	return laserScan;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan)
+{
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2);
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
+	output->resize(laserScan.cols);
+	for(int i=0; i<laserScan.cols; ++i)
+	{
+		output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
+		output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
+	}
+	return output;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr cvMat2Cloud(
+		const cv::Mat & matrix,
+		const Transform & tranform)
+{
+	UASSERT(matrix.type() == CV_32FC2 || matrix.type() == CV_32FC3);
+	UASSERT(matrix.rows == 1);
+
+	Eigen::Affine3f t = tranform.toEigen3f();
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	cloud->resize(matrix.cols);
+	if(matrix.channels() == 2)
+	{
+		for(int i=0; i<matrix.cols; ++i)
+		{
+			cloud->at(i).x = matrix.at<cv::Vec2f>(0,i)[0];
+			cloud->at(i).y = matrix.at<cv::Vec2f>(0,i)[1];
+			cloud->at(i).z = 0.0f;
+			cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
+		}
+	}
+	else // channels=3
+	{
+		for(int i=0; i<matrix.cols; ++i)
+		{
+			cloud->at(i).x = matrix.at<cv::Vec3f>(0,i)[0];
+			cloud->at(i).y = matrix.at<cv::Vec3f>(0,i)[1];
+			cloud->at(i).z = matrix.at<cv::Vec3f>(0,i)[2];
+			cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
+		}
+	}
+	return cloud;
+}
+
 // inspired from ROS image_geometry/src/stereo_camera_model.cpp
 pcl::PointXYZ projectDisparityTo3D(
 		const cv::Point2f & pt,

corelib/src/util3d_conversions.cpp

@@ -1,142 +0,0 @@
-/*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#include "rtabmap/core/util3d_conversions.h"
-
-#include "rtabmap/utilite/ULogger.h"
-#include <pcl/common/transforms.h>
-
-namespace rtabmap
-{
-
-namespace util3d
-{
-
-cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
-{
-	UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
-	cv::Mat depth16U;
-	if(!depth32F.empty())
-	{
-		depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
-		for(int i=0; i<depth32F.rows; ++i)
-		{
-			for(int j=0; j<depth32F.cols; ++j)
-			{
-				float depth = (depth32F.at<float>(i,j)*1000.0f);
-				unsigned short depthMM = 0;
-				if(depth <= (float)USHRT_MAX)
-				{
-					depthMM = (unsigned short)depth;
-				}
-				depth16U.at<unsigned short>(i, j) = depthMM;
-			}
-		}
-	}
-	return depth16U;
-}
-
-cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
-{
-	UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
-	cv::Mat depth32F;
-	if(!depth16U.empty())
-	{
-		depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
-		for(int i=0; i<depth16U.rows; ++i)
-		{
-			for(int j=0; j<depth16U.cols; ++j)
-			{
-				float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
-				depth32F.at<float>(i, j) = depth;
-			}
-		}
-	}
-	return depth32F;
-}
-
-cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
-{
-	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
-	for(unsigned int i=0; i<cloud.size(); ++i)
-	{
-		laserScan.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
-		laserScan.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
-	}
-	return laserScan;
-}
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan)
-{
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2);
-
-	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
-	output->resize(laserScan.cols);
-	for(int i=0; i<laserScan.cols; ++i)
-	{
-		output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
-		output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
-	}
-	return output;
-}
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr cvMat2Cloud(
-		const cv::Mat & matrix,
-		const Transform & tranform)
-{
-	UASSERT(matrix.type() == CV_32FC2 || matrix.type() == CV_32FC3);
-	UASSERT(matrix.rows == 1);
-
-	Eigen::Affine3f t = tranform.toEigen3f();
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-	cloud->resize(matrix.cols);
-	if(matrix.channels() == 2)
-	{
-		for(int i=0; i<matrix.cols; ++i)
-		{
-			cloud->at(i).x = matrix.at<cv::Vec2f>(0,i)[0];
-			cloud->at(i).y = matrix.at<cv::Vec2f>(0,i)[1];
-			cloud->at(i).z = 0.0f;
-			cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
-		}
-	}
-	else // channels=3
-	{
-		for(int i=0; i<matrix.cols; ++i)
-		{
-			cloud->at(i).x = matrix.at<cv::Vec3f>(0,i)[0];
-			cloud->at(i).y = matrix.at<cv::Vec3f>(0,i)[1];
-			cloud->at(i).z = matrix.at<cv::Vec3f>(0,i)[2];
-			cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
-		}
-	}
-	return cloud;
-}
-
-}
-
-}

corelib/src/util3d_features.cpp

@@ -82,7 +82,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
 					cameraModels.at(cameraIndex).fy(),
 					true);
 
-			if(!cameraModels.at(cameraIndex).localTransform().isNull() &&
+			if(pcl::isFinite(pt) &&
+				!cameraModels.at(cameraIndex).localTransform().isNull() &&
 			    !cameraModels.at(cameraIndex).localTransform().isIdentity())
 			{
 				pt = util3d::transformPoint(pt, cameraModels.at(cameraIndex).localTransform());

corelib/src/util3d_mapping.cpp

@@ -27,7 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/util3d_mapping.h"
 
-#include <rtabmap/core/util3d_conversions.h>
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/core/util3d.h>

guilib/src/DatabaseViewer.cpp

@@ -49,7 +49,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/gui/KeypointItem.h"
 #include "rtabmap/gui/UCv2Qt.h"
 #include "rtabmap/core/util3d.h"
-#include "rtabmap/core/util3d_conversions.h"
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/core/util3d_filtering.h"
 #include "rtabmap/core/util3d_surface.h"

guilib/src/LoopClosureViewer.cpp

@@ -31,7 +31,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Memory.h"
 #include "rtabmap/core/util3d_filtering.h"
 #include "rtabmap/core/util3d_transforms.h"
-#include "rtabmap/core/util3d_conversions.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/Signature.h"
 #include "rtabmap/utilite/ULogger.h"

guilib/src/MainWindow.cpp

@@ -86,7 +86,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/core/util3d_filtering.h"
-#include "rtabmap/core/util3d_conversions.h"
 #include "rtabmap/core/util3d_mapping.h"
 #include "rtabmap/core/util3d_surface.h"
 #include "rtabmap/core/util3d_registration.h"

tools/CameraRGBD/main.cpp

@@ -27,7 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/CameraRGBD.h"
 #include "rtabmap/core/util3d.h"
-#include "rtabmap/core/util3d_conversions.h"
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UMath.h"