Added util3d::projectCloudToCamera() method (creating a registered depth image from the laser scan), used for "Generate depth from scan" option of CameraImages

CMakeLists.txt

@@ -419,6 +419,7 @@ ENDIF(NOT WIN32)
 IF(APPLE)
 MESSAGE(STATUS "  BUILD_AS_BUNDLE = ${BUILD_AS_BUNDLE}")
 ENDIF(APPLE)
+MESSAGE(STATUS "  CMAKE_CXX_FLAGS = ${CMAKE_CXX_FLAGS}")
 
 IF(OpenCV_FOUND)
   IF(OpenCV_VERSION_MAJOR EQUAL 2)

corelib/include/rtabmap/core/CameraInfo.h

@@ -55,6 +55,7 @@ public:
 	float timeMirroring;
 	float timeImageDecimation;
 	float timeScanFromDepth;
+	float timeDepthFromScan;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/CameraRGB.h

@@ -96,6 +96,13 @@ public:
 		}
 	}
 
+	void setDepthFromScan(bool enabled, bool fillHolesVertical = true, bool fillHolesFromBorder = false)
+	{
+		_depthFromScan = enabled;
+		_depthFromScanFillHolesVertical = fillHolesVertical;
+		_depthFromScanFillHolesFromBorder = fillHolesFromBorder;
+	}
+
 	void setGroundTruthPath(const std::string & filePath, int format = 0)
 	{
 		groundTruthPath_ = filePath;
@@ -134,6 +141,10 @@ private:
 	float _scanVoxelSize;
 	int _scanNormalsK;
 
+	bool _depthFromScan;
+	bool _depthFromScanFillHolesVertical;
+	bool _depthFromScanFillHolesFromBorder;
+
 	bool _filenamesAreTimestamps;
 	std::string timestampsPath_;
 

corelib/include/rtabmap/core/Parameters.h

@@ -397,7 +397,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Icp, VoxelSize,                 float, 0.025, "Uniform sampling voxel size (0=disabled).");
 	RTABMAP_PARAM(Icp, DownsamplingStep,          int, 1, 	    "Downsampling step size (1=no sampling). This is done before uniform sampling.");
 	RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05,  "Max distance for point correspondences.");
-	RTABMAP_PARAM(Icp, Iterations,                int, 30, 		"Max iterations.");
+	RTABMAP_PARAM(Icp, Iterations,                int, 10, 		"Max iterations.");
 	RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.3,   "Ratio of matching correspondences to accept the transform.");
 	RTABMAP_PARAM(Icp, PointToPlane,              bool, false, 	"Use point to plane ICP.");
 	RTABMAP_PARAM(Icp, PointToPlaneNormalNeighbors, int, 20,    "Number of neighbors to compute normals for point to plane.");

corelib/include/rtabmap/core/util2d.h

@@ -109,7 +109,7 @@ float RTABMAP_EXP getDepth(
 
 cv::Mat RTABMAP_EXP decimate(const cv::Mat & image, int d);
 
-// Registration Depth to RGB
+// Registration Depth to RGB (return registered depth image)
 cv::Mat RTABMAP_EXP registerDepth(
 		const cv::Mat & depth,
 		const cv::Mat & depthK,
@@ -117,7 +117,7 @@ cv::Mat RTABMAP_EXP registerDepth(
 		const rtabmap::Transform & transform);
 
 void RTABMAP_EXP fillRegisteredDepthHoles(
-		cv::Mat & depth,
+		cv::Mat & depthRegistered,
 		bool vertical,
 		bool horizontal,
 		bool fillDoubleHoles = false);

corelib/include/rtabmap/core/util3d.h

@@ -145,6 +145,26 @@ cv::Point3f RTABMAP_EXP projectDisparityTo3D(
 		const cv::Mat & disparity,
 		const StereoCameraModel & model);
 
+// Register point cloud to camera (return registered depth image)
+cv::Mat RTABMAP_EXP projectCloudToCamera(
+		const cv::Size & imageSize,
+		const cv::Mat & cameraMatrixK,  // /base_link -> /camera_link
+		const cv::Mat & laserScan,      // assuming points are already in /base_link coordinate
+		const rtabmap::Transform & cameraTransform);
+
+// Register point cloud to camera (return registered depth image)
+cv::Mat RTABMAP_EXP projectCloudToCamera(
+		const cv::Size & imageSize,
+		const cv::Mat & cameraMatrixK,                       // /base_link -> /camera_link
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr laserScan, // assuming points are already in /base_link coordinate
+		const rtabmap::Transform & cameraTransform);
+
+// Direction vertical (>=0), horizontal (<0)
+void RTABMAP_EXP fillProjectedCloudHoles(
+		cv::Mat & depthRegistered,
+		bool verticalDirection,
+		bool fillToBorder);
+
 bool RTABMAP_EXP isFinite(const cv::Point3f & pt);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(
@@ -199,6 +219,12 @@ cv::Mat RTABMAP_EXP loadScan(
 		float voxelSize = 0.0f,
 		int normalsK = 0);
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP loadCloud(
+		const std::string & path,
+		const Transform & transform = Transform::getIdentity(),
+		int downsampleStep = 1,
+		float voxelSize = 0.0f);
+
 } // namespace util3d
 } // namespace rtabmap
 

corelib/src/CameraRGB.cpp

@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_surface.h>
 
 #include <iostream>
 #include <cmath>
@@ -64,6 +65,7 @@ CameraImages::CameraImages() :
 		_scanDownsampleStep(1),
 		_scanVoxelSize(0.0f),
 		_scanNormalsK(0),
+		_depthFromScan(false),
 		_filenamesAreTimestamps(false),
 		_groundTruthFormat(0)
 	{}
@@ -85,6 +87,7 @@ CameraImages::CameraImages(const std::string & path,
 	_scanDownsampleStep(1),
 	_scanVoxelSize(0.0f),
 	_scanNormalsK(0),
+	_depthFromScan(false),
 	_filenamesAreTimestamps(false),
 	_groundTruthFormat(0)
 {
@@ -381,6 +384,7 @@ SensorData CameraImages::captureImage()
 	cv::Mat scan;
 	double stamp = UTimer::now();
 	Transform groundTruthPose;
+	cv::Mat depthFromScan;
 	UDEBUG("");
 	if(_dir->isValid())
 	{
@@ -392,6 +396,8 @@ SensorData CameraImages::captureImage()
 				_scanDir->update();
 			}
 		}
+		std::string imageFilePath;
+		std::string scanFilePath;
 		if(_startAt < 0)
 		{
 			const std::list<std::string> & fileNames = _dir->getFileNames();
@@ -400,8 +406,7 @@ SensorData CameraImages::captureImage()
 				if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0)
 				{
 					_lastFileName = *fileNames.rbegin();
-					std::string fullPath = _path + _lastFileName;
+					imageFilePath = _path + _lastFileName;
-					img = cv::imread(fullPath.c_str());
 				}
 			}
 			if(_scanDir)
@@ -412,109 +417,148 @@ SensorData CameraImages::captureImage()
 					if(_lastScanFileName.empty() || uStrNumCmp(_lastScanFileName,*scanFileNames.rbegin()) < 0)
 					{
 						_lastScanFileName = *scanFileNames.rbegin();
-						std::string fullPath = _scanPath + _lastScanFileName;
+						scanFilePath = _scanPath + _lastScanFileName;
-
-						scan = util3d::loadScan(fullPath, _scanLocalTransform, _scanDownsampleStep, _scanVoxelSize, _scanNormalsK);
 					}
 				}
 			}
 		}
 		else
 		{
-			if(stamps_.size())
-			{
-				stamp = stamps_.front();
-				stamps_.pop_front();
-				if(groundTruth_.size())
-				{
-					groundTruthPose = groundTruth_.front();
-					groundTruth_.pop_front();
-				}
-			}
-
 			std::string fileName;
-			std::string fullPath;
 			fileName = _dir->getNextFileName();
-			if(fileName.size())
+			if(!fileName.empty())
 			{
-				fullPath = _path + fileName;
+				imageFilePath = _path + fileName;
 				while(_count++ < _startAt && (fileName = _dir->getNextFileName()).size())
 				{
-					fullPath = _path + fileName;
+					imageFilePath = _path + fileName;
-				}
-				if(fileName.size())
-				{
-					ULOGGER_DEBUG("Loading image : %s", fullPath.c_str());
-
-#if CV_MAJOR_VERSION >2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
-					img = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED);
-#else
-					img = cv::imread(fullPath.c_str(), -1);
-#endif
-					UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d",
-							img.cols, img.rows, img.channels(), img.elemSize(), img.total());
-
-					if(_isDepth)
-					{
-						if(img.type() != CV_16UC1 && img.type() != CV_32FC1)
-						{
-							UERROR("Depth is on and the loaded image has not a format supported (file = \"%s\"). "
-									"Formats supported are 16 bits 1 channel and 32 bits 1 channel.",
-									fileName.c_str());
-							img = cv::Mat();
-						}
-
-						if(_depthScaleFactor > 1.0f)
-						{
-							img /= _depthScaleFactor;
-						}
-					}
-					else
-					{
-#if CV_MAJOR_VERSION < 3
-						// FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
-						if(img.depth() != CV_8U)
-						{
-							// The depth should be 8U
-							UWARN("Cannot read the image correctly, falling back to old OpenCV C interface...");
-							IplImage * i = cvLoadImage(fullPath.c_str());
-							img = cv::Mat(i, true);
-							cvReleaseImage(&i);
-						}
-#endif
-
-						if(img.channels()>3)
-						{
-							UWARN("Conversion from 4 channels to 3 channels (file=%s)", fullPath.c_str());
-							cv::Mat out;
-							cv::cvtColor(img, out, CV_BGRA2BGR);
-							img = out;
-						}
-					}
 				}
 			}
-
 			if(_scanDir)
 			{
 				fileName = _scanDir->getNextFileName();
-				if(fileName.size())
+				if(!fileName.empty())
 				{
-					fullPath = _scanPath + fileName;
+					scanFilePath = _scanPath + fileName;
 					while(++_countScan < _startAt && (fileName = _scanDir->getNextFileName()).size())
 					{
-						fullPath = _scanPath + fileName;
+						scanFilePath = _scanPath + fileName;
-					}
-					if(fileName.size())
-					{
-						scan = util3d::loadScan(fullPath, _scanLocalTransform, _scanDownsampleStep, _scanVoxelSize, _scanNormalsK);
 					}
 				}
 			}
 		}
 
-		if(!img.empty() && _model.isValid() && _rectifyImages)
+		if(stamps_.size())
 		{
-			img = _model.rectifyImage(img);
+			stamp = stamps_.front();
+			stamps_.pop_front();
+			if(groundTruth_.size())
+			{
+				groundTruthPose = groundTruth_.front();
+				groundTruth_.pop_front();
+			}
+		}
+
+		if(!imageFilePath.empty())
+		{
+			ULOGGER_DEBUG("Loading image : %s", imageFilePath.c_str());
+
+#if CV_MAJOR_VERSION >2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
+			img = cv::imread(imageFilePath.c_str(), cv::IMREAD_UNCHANGED);
+#else
+			img = cv::imread(imageFilePath.c_str(), -1);
+#endif
+			UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d",
+					img.cols, img.rows, img.channels(), img.elemSize(), img.total());
+
+			if(_isDepth)
+			{
+				if(img.type() != CV_16UC1 && img.type() != CV_32FC1)
+				{
+					UERROR("Depth is on and the loaded image has not a format supported (file = \"%s\"). "
+							"Formats supported are 16 bits 1 channel and 32 bits 1 channel.",
+							imageFilePath.c_str());
+					img = cv::Mat();
+				}
+
+				if(_depthScaleFactor > 1.0f)
+				{
+					img /= _depthScaleFactor;
+				}
+			}
+			else
+			{
+#if CV_MAJOR_VERSION < 3
+				// FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
+				if(img.depth() != CV_8U)
+				{
+					// The depth should be 8U
+					UWARN("Cannot read the image correctly, falling back to old OpenCV C interface...");
+					IplImage * i = cvLoadImage(fullPath.c_str());
+					img = cv::Mat(i, true);
+					cvReleaseImage(&i);
+				}
+#endif
+
+				if(img.channels()>3)
+				{
+					UWARN("Conversion from 4 channels to 3 channels (file=%s)", imageFilePath.c_str());
+					cv::Mat out;
+					cv::cvtColor(img, out, CV_BGRA2BGR);
+					img = out;
+				}
+			}
+
+			if(!img.empty() && _model.isValid() && _rectifyImages)
+			{
+				img = _model.rectifyImage(img);
+			}
+		}
+
+		if(!scanFilePath.empty())
+		{
+			// load without filtering
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::loadCloud(scanFilePath, _scanLocalTransform);
+			UDEBUG("Loaded scan=%d points", (int)cloud->size());
+			if(_depthFromScan && !img.empty())
+			{
+				UDEBUG("Computing depth from scan...");
+				if(!_model.isValid())
+				{
+					UWARN("Depth from laser scan: Camera model should be valid.");
+				}
+				else if(_isDepth)
+				{
+					UWARN("Depth from laser scan: Loading already a depth image.");
+				}
+				else
+				{
+					depthFromScan = util3d::projectCloudToCamera(img.size(), _model.K(), cloud, _model.localTransform());
+					util3d::fillProjectedCloudHoles(depthFromScan, _depthFromScanFillHolesVertical, _depthFromScanFillHolesFromBorder);
+				}
+			}
+			// filter the scan after registration
+			int previousSize = (int)cloud->size();
+			if(_scanDownsampleStep > 1 && cloud->size())
+			{
+				cloud = util3d::downsample(cloud, _scanDownsampleStep);
+				UDEBUG("Downsampling scan (step=%d): %d -> %d", _scanDownsampleStep, previousSize, (int)cloud->size());
+			}
+			previousSize = (int)cloud->size();
+			if(_scanVoxelSize > 0.0f && cloud->size())
+			{
+				cloud = util3d::voxelize(cloud, _scanVoxelSize);
+				UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d", _scanVoxelSize, previousSize, (int)cloud->size());
+			}
+			if(_scanNormalsK > 0 && cloud->size())
+			{
+				pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals = util3d::computeNormals(cloud, _scanNormalsK);
+				scan = util3d::laserScanFromPointCloud(*cloudNormals);
+			}
+			else
+			{
+				scan = util3d::laserScanFromPointCloud(*cloud);
+			}
 		}
 	}
 	else
@@ -522,7 +566,7 @@ SensorData CameraImages::captureImage()
 		UWARN("Directory is not set, camera must be initialized.");
 	}
 
-	SensorData data(scan, scan.empty()?0:_scanMaxPts, 0, _isDepth?cv::Mat():img, _isDepth?img:cv::Mat(), _model, this->getNextSeqID(), stamp);
+	SensorData data(scan, scan.empty()?0:_scanMaxPts, 0, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
 	data.setGroundTruth(groundTruthPose);
 	return data;
 }

corelib/src/util2d.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UStl.h>
+#include <rtabmap/core/util3d_transforms.h>
 #include <opencv2/calib3d/calib3d.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/video/tracking.hpp>
@@ -1077,7 +1078,7 @@ cv::Mat decimate(const cv::Mat & image, int decimation)
 	return out;
 }
 
-// Registration Depth to RGB
+// Registration Depth to RGB (return registered depth image)
 cv::Mat registerDepth(
 		const cv::Mat & depth,
 		const cv::Mat & depthK,
@@ -1086,7 +1087,7 @@ cv::Mat registerDepth(
 {
 	UASSERT(!transform.isNull());
 	UASSERT(!depth.empty());
-	UASSERT(depth.type() == CV_16UC1); // mm
+	UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1); // mm or m
 	UASSERT(depthK.type() == CV_64FC1 && depthK.cols == 3 && depthK.cols == 3);
 	UASSERT(colorK.type() == CV_64FC1 && colorK.cols == 3 && colorK.cols == 3);
 
@@ -1105,12 +1106,13 @@ cv::Mat registerDepth(
 	P4[3] = 1;
 	cv::Mat registered = cv::Mat::zeros(depth.rows, depth.cols, depth.type());
 
+	bool depthInMM = depth.type() == CV_16UC1;
 	for(int y=0; y<depth.rows; ++y)
 	{
 		for(int x=0; x<depth.cols; ++x)
 		{
 			//filtering
-			float dz = float(depth.at<unsigned short>(y,x))*0.001f; // put in meter for projection
+			float dz = depthInMM?float(depth.at<unsigned short>(y,x))*0.001f:depth.at<float>(y,x); // put in meter for projection
 			if(dz>=0.0f)
 			{
 				// Project to 3D
@@ -1126,11 +1128,22 @@ cv::Mat registerDepth(
 
 				if(uIsInBounds(dx, 0, registered.cols) && uIsInBounds(dy, 0, registered.rows))
 				{
-					unsigned short z16 = z * 1000; //mm
+					if(depthInMM)
-					unsigned short &zReg = registered.at<unsigned short>(dy, dx);
-					if(zReg == 0 || z16 < zReg)
 					{
-						zReg = z16;
+						unsigned short z16 = z * 1000; //mm
+						unsigned short &zReg = registered.at<unsigned short>(dy, dx);
+						if(zReg == 0 || z16 < zReg)
+						{
+							zReg = z16;
+						}
+					}
+					else
+					{
+						float &zReg = registered.at<float>(dy, dx);
+						if(zReg == 0 || z < zReg)
+						{
+							zReg = z;
+						}
 					}
 				}
 			}

corelib/src/util3d.cpp

@@ -787,7 +787,7 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform)
 {
 	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC3);
-	bool nullTransform = transform.isNull();
+	bool nullTransform = transform.isNull() || transform.isIdentity();
 	Eigen::Affine3f transform3f = transform.toEigen3f();
 	for(unsigned int i=0; i<cloud.size(); ++i)
 	{
@@ -812,7 +812,7 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, co
 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform)
 {
 	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
-	bool nullTransform = transform.isNull();
+	bool nullTransform = transform.isNull() || transform.isIdentity();
 	Eigen::Affine3f transform3f = transform.toEigen3f();
 	for(unsigned int i=0; i<cloud.size(); ++i)
 	{
@@ -976,6 +976,201 @@ cv::Point3f projectDisparityTo3D(
 	return cv::Point3f(bad_point, bad_point, bad_point);
 }
 
+// Register point cloud to camera (return registered depth image)
+cv::Mat projectCloudToCamera(
+		const cv::Size & imageSize,
+		const cv::Mat & cameraMatrixK, // /base_link -> /camera_link
+		const cv::Mat & laserScan,     // assuming laser scan points are already in /base_link coordinate
+		const rtabmap::Transform & cameraTransform)
+{
+	UASSERT(!cameraTransform.isNull());
+	UASSERT(!laserScan.empty());
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6));
+	UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
+
+	float fx = cameraMatrixK.at<double>(0,0);
+	float fy = cameraMatrixK.at<double>(1,1);
+	float cx = cameraMatrixK.at<double>(0,2);
+	float cy = cameraMatrixK.at<double>(1,2);
+
+	cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
+	Transform t = cameraTransform.inverse();
+
+	int count = 0;
+	for(int i=0; i<laserScan.cols; ++i)
+	{
+		// Get 3D from laser scan
+		cv::Point3f ptScan;
+		if(laserScan.type() == CV_32FC2)
+		{
+			ptScan.x = laserScan.at<cv::Vec2f>(i)[0];
+			ptScan.y = laserScan.at<cv::Vec2f>(i)[1];
+			ptScan.z = 0;
+		}
+		else if(laserScan.type() == CV_32FC3)
+		{
+			ptScan.x = laserScan.at<cv::Vec3f>(i)[0];
+			ptScan.y = laserScan.at<cv::Vec3f>(i)[1];
+			ptScan.z = laserScan.at<cv::Vec3f>(i)[2];
+		}
+		else
+		{
+			ptScan.x = laserScan.at<cv::Vec6f>(i)[0];
+			ptScan.y = laserScan.at<cv::Vec6f>(i)[1];
+			ptScan.z = laserScan.at<cv::Vec6f>(i)[2];
+		}
+		ptScan = util3d::transformPoint(ptScan, t);
+
+		// re-project in camera frame
+		float z = ptScan.z;
+		float invZ = 1.0f/z;
+		int dx = (fx*ptScan.x)*invZ + cx;
+		int dy = (fy*ptScan.y)*invZ + cy;
+
+		if(z > 0.0f && uIsInBounds(dx, 0, registered.cols) && uIsInBounds(dy, 0, registered.rows))
+		{
+			++count;
+			float &zReg = registered.at<float>(dy, dx);
+			if(zReg == 0 || z < zReg)
+			{
+				zReg = z;
+			}
+		}
+	}
+	UDEBUG("Points in camera=%d/%d", count, laserScan.cols);
+
+	return registered;
+}
+
+cv::Mat projectCloudToCamera(
+		const cv::Size & imageSize,
+		const cv::Mat & cameraMatrixK, // /base_link -> /camera_link
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr laserScan,  // assuming points are already in /base_link coordinate
+		const rtabmap::Transform & cameraTransform)
+{
+	UASSERT(!cameraTransform.isNull());
+	UASSERT(!laserScan->empty());
+	UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
+
+	float fx = cameraMatrixK.at<double>(0,0);
+	float fy = cameraMatrixK.at<double>(1,1);
+	float cx = cameraMatrixK.at<double>(0,2);
+	float cy = cameraMatrixK.at<double>(1,2);
+
+	cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
+	Transform t = cameraTransform.inverse();
+
+	int count = 0;
+	for(int i=0; i<(int)laserScan->size(); ++i)
+	{
+		// Get 3D from laser scan
+		pcl::PointXYZ ptScan = laserScan->at(i);
+		ptScan = util3d::transformPoint(ptScan, t);
+
+		// re-project in camera frame
+		float z = ptScan.z;
+		float invZ = 1.0f/z;
+		int dx = (fx*ptScan.x)*invZ + cx;
+		int dy = (fy*ptScan.y)*invZ + cy;
+
+		if(z > 0.0f && uIsInBounds(dx, 0, registered.cols) && uIsInBounds(dy, 0, registered.rows))
+		{
+			++count;
+			float &zReg = registered.at<float>(dy, dx);
+			if(zReg == 0 || z < zReg)
+			{
+				zReg = z;
+			}
+		}
+	}
+	UDEBUG("Points in camera=%d/%d", count, (int)laserScan->size());
+
+	return registered;
+}
+
+void fillProjectedCloudHoles(cv::Mat & registeredDepth, bool verticalDirection, bool fillToBorder)
+{
+	UASSERT(registeredDepth.type() == CV_32FC1);
+	if(verticalDirection)
+	{
+		// vertical, for each column
+		for(int x=0; x<registeredDepth.cols; ++x)
+		{
+			float valueA = 0.0f;
+			int indexA = -1;
+			for(int y=0; y<registeredDepth.rows; ++y)
+			{
+				float v = registeredDepth.at<float>(y,x);
+				if(fillToBorder && y == registeredDepth.rows-1 && v<=0.0f && indexA>=0)
+				{
+					v = valueA;
+				}
+				if(v > 0.0f)
+				{
+					if(fillToBorder && indexA < 0)
+					{
+						indexA = 0;
+						valueA = v;
+					}
+					if(indexA >=0)
+					{
+						int range = y-indexA;
+						if(range > 1)
+						{
+							float slope = (v-valueA)/(range);
+							for(int k=1; k<range; ++k)
+							{
+								registeredDepth.at<float>(indexA+k,x) = valueA+slope*float(k);
+							}
+						}
+					}
+					valueA = v;
+					indexA = y;
+				}
+			}
+		}
+	}
+	else
+	{
+		// horizontal, for each row
+		for(int y=0; y<registeredDepth.rows; ++y)
+		{
+			float valueA = 0.0f;
+			int indexA = -1;
+			for(int x=0; x<registeredDepth.cols; ++x)
+			{
+				float v = registeredDepth.at<float>(y,x);
+				if(fillToBorder && x == registeredDepth.cols-1 && v<=0.0f && indexA>=0)
+				{
+					v = valueA;
+				}
+				if(v > 0.0f)
+				{
+					if(fillToBorder && indexA < 0)
+					{
+						indexA = 0;
+						valueA = v;
+					}
+					if(indexA >=0)
+					{
+						int range = x-indexA;
+						if(range > 1)
+						{
+							float slope = (v-valueA)/(range);
+							for(int k=1; k<range; ++k)
+							{
+								registeredDepth.at<float>(y,indexA+k) = valueA+slope*float(k);
+							}
+						}
+					}
+					valueA = v;
+					indexA = x;
+				}
+			}
+		}
+	}
+}
+
 bool isFinite(const cv::Point3f & pt)
 {
 	return uIsFinite(pt.x) && uIsFinite(pt.y) && uIsFinite(pt.z);
@@ -1114,7 +1309,28 @@ cv::Mat loadScan(
 		int normalsK)
 {
 	cv::Mat scan;
-	UDEBUG("Loading scan (step=%d, voxel=%f m, normalsK=%d) : %s", downsampleStep, voxelSize, normalsK, path.c_str());
+	UDEBUG("Loading scan (normalsK=%d) : %s", normalsK, path.c_str());
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = loadCloud(path, Transform::getIdentity(), downsampleStep, voxelSize);
+	if(normalsK > 0 && cloud->size())
+	{
+		pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals = util3d::computeNormals(cloud, normalsK);
+		scan = util3d::laserScanFromPointCloud(*cloudNormals, transform);
+	}
+	else
+	{
+		scan = util3d::laserScanFromPointCloud(*cloud, transform);
+	}
+	return scan;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr loadCloud(
+		const std::string & path,
+		const Transform & transform,
+		int downsampleStep,
+		float voxelSize)
+{
+	UASSERT(!transform.isNull());
+	UDEBUG("Loading cloud (step=%d, voxel=%f m) : %s", downsampleStep, voxelSize, path.c_str());
 	std::string fileName = UFile::getName(path);
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 	if(UFile::getExtension(fileName).compare("bin") == 0)
@@ -1141,16 +1357,11 @@ cv::Mat loadScan(
 		cloud = util3d::voxelize(cloud, voxelSize);
 		UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d", voxelSize, previousSize, (int)cloud->size());
 	}
-	if(normalsK > 0 && cloud->size())
+	if(transform.isIdentity())
 	{
-		pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals = util3d::computeNormals(cloud, normalsK);
+		return cloud;
-		scan = util3d::laserScanFromPointCloud(*cloudNormals, transform);
 	}
-	else
+	return util3d::transformPointCloud(cloud, transform);
-	{
-		scan = util3d::laserScanFromPointCloud(*cloud, transform);
-	}
-	return scan;
 }
 
 }

guilib/src/MainWindow.cpp

@@ -742,6 +742,7 @@ void MainWindow::processCameraInfo(const rtabmap::CameraInfo & info)
 	_ui->statsToolBox->updateStat("Camera/Time disparity/ms", (float)info.id, (float)info.timeDisparity*1000.0);
 	_ui->statsToolBox->updateStat("Camera/Time mirroring/ms", (float)info.id, (float)info.timeMirroring*1000.0);
 	_ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", (float)info.id, (float)info.timeScanFromDepth*1000.0);
+	_ui->statsToolBox->updateStat("Camera/Time depth from scan/ms", (float)info.id, (float)info.timeDepthFromScan*1000.0);
 }
 
 void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)

guilib/src/PreferencesDialog.cpp

@@ -405,6 +405,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->doubleSpinBox_cameraImages_scanVoxelSize, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_cameraImages_gt, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_cameraImages_gtFormat, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->groupBox_depthFromScan, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->checkBox_depthFromScan_vertical, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->checkBox_depthFromScan_fillBorders, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
 	connect(_ui->toolButton_cameraStereoImages_path_left, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesPathLeft()));
 	connect(_ui->toolButton_cameraStereoImages_path_right, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesPathRight()));
@@ -1215,6 +1218,10 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->setValue(4.0);
 		_ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(0.0f);
 		_ui->spinBox_cameraImages_scanNormalsK->setValue(0);
+
+		_ui->groupBox_depthFromScan->setChecked(false);
+		_ui->checkBox_depthFromScan_vertical->setChecked(true);
+		_ui->checkBox_depthFromScan_fillBorders->setChecked(false);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_rtabmap_basic0->objectName())
 	{
@@ -1537,6 +1544,12 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->spinBox_cameraImages_scanNormalsK->setValue(settings.value("normalsK", _ui->spinBox_cameraImages_scanNormalsK->value()).toInt());
 	settings.endGroup();//ScanFromDepth
 
+	settings.beginGroup("DepthFromScan");
+	_ui->groupBox_depthFromScan->setChecked(settings.value("depthFromScan", _ui->groupBox_depthFromScan->isChecked()).toBool());
+	_ui->checkBox_depthFromScan_vertical->setChecked(settings.value("depthFromScanVertical", _ui->checkBox_depthFromScan_vertical->isChecked()).toBool());
+	_ui->checkBox_depthFromScan_fillBorders->setChecked(settings.value("depthFromScanFillBorders", _ui->checkBox_depthFromScan_fillBorders->isChecked()).toBool());
+	settings.endGroup();
+
 	settings.beginGroup("Database");
 	_ui->source_database_lineEdit_path->setText(settings.value("path",_ui->source_database_lineEdit_path->text()).toString());
 	_ui->source_checkBox_ignoreOdometry->setChecked(settings.value("ignoreOdometry", _ui->source_checkBox_ignoreOdometry->isChecked()).toBool());
@@ -1894,6 +1907,12 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("normalsK", 			_ui->spinBox_cameraImages_scanNormalsK->value());
 	settings.endGroup();
 
+	settings.beginGroup("DepthFromScan");
+	settings.setValue("depthFromScan", _ui->groupBox_depthFromScan->isChecked());
+	settings.setValue("depthFromScanVertical", _ui->checkBox_depthFromScan_vertical->isChecked());
+	settings.setValue("depthFromScanFillBorders", _ui->checkBox_depthFromScan_fillBorders->isChecked());
+	settings.endGroup();
+
 	settings.beginGroup("Database");
 	settings.setValue("path", 			   _ui->source_database_lineEdit_path->text());
 	settings.setValue("ignoreOdometry",    _ui->source_checkBox_ignoreOdometry->isChecked());
@@ -3356,9 +3375,11 @@ void PreferencesDialog::updateSourceGrpVisibility()
 	_ui->groupBox_sourceImages_optional->setVisible(
 			(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD) ||
 			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo) ||
-			(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->comboBox_sourceType->currentIndex() == kSrcImages-kSrcRGB));
+			(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB));
 
 	_ui->groupBox_scan->setVisible(_ui->comboBox_sourceType->currentIndex() != 3);
+
+	_ui->groupBox_depthFromScan->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB);
 }
 
 /*** GETTERS ***/
@@ -3879,7 +3900,9 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 		((CameraImages*)camera)->setDirRefreshed(_ui->source_images_refreshDir->isChecked());
 		((CameraImages*)camera)->setImagesRectified(_ui->checkBox_rgbImages_rectify->isChecked());
 
-		((CameraRGBDImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex());
+		((CameraRGBDImages*)camera)->setGroundTruthPath(
+				_ui->lineEdit_cameraImages_gt->text().toStdString(),
+				_ui->comboBox_cameraImages_gtFormat->currentIndex());
 		((CameraRGBDImages*)camera)->setScanPath(
 						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
 						_ui->spinBox_cameraImages_max_scan_pts->value(),
@@ -3887,7 +3910,13 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 						_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
 						_ui->spinBox_cameraImages_scanNormalsK->value(),
 						this->getLaserLocalTransform());
-		((CameraRGBDImages*)camera)->setTimestamps(_ui->checkBox_cameraImages_timestamps->isChecked(), _ui->lineEdit_cameraImages_timestamps->text().toStdString());
+		((CameraRGBDImages*)camera)->setDepthFromScan(
+				_ui->groupBox_depthFromScan->isChecked(),
+				_ui->checkBox_depthFromScan_vertical->isChecked(),
+				_ui->checkBox_depthFromScan_fillBorders->isChecked());
+		((CameraRGBDImages*)camera)->setTimestamps(
+				_ui->checkBox_cameraImages_timestamps->isChecked(),
+				_ui->lineEdit_cameraImages_timestamps->text().toStdString());
 	}
 	else if(driver == kSrcDatabase)
 	{
@@ -3903,11 +3932,12 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 	{
 		// don't set calibration folder if we want raw images
 		QString dir = this->getCameraInfoDir();
-		QString name = QFileInfo(_ui->lineEdit_calibrationFile->text()
+		QString calibrationFile = _ui->lineEdit_calibrationFile->text();
-				.remove("_left.yaml")
+		if(!(driver >= kSrcRGB && driver <= kSrcVideo))
-				.remove("_right.yaml")
+		{
-				.remove("_pose.yaml")
+			calibrationFile.remove("_left.yaml").remove("_right.yaml").remove("_pose.yaml");
-				.remove(".yaml")).baseName();
+		}
+		QString name = QFileInfo(calibrationFile.remove(".yaml")).baseName();
 		if(!_ui->lineEdit_calibrationFile->text().isEmpty())
 		{
 			QDir d = QFileInfo(_ui->lineEdit_calibrationFile->text()).dir();

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-880</y>
             <width>676</width>
             <height>1982</height>
            </rect>
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>14</number>
+                <number>3</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -1910,7 +1910,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                     <item>
                      <widget class="QStackedWidget" name="stackedWidget_src">
                       <property name="currentIndex">
-                       <number>0</number>
+                       <number>2</number>
                       </property>
                       <widget class="QWidget" name="page_41">
                        <layout class="QVBoxLayout" name="verticalLayout_64">
@@ -2973,7 +2973,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_image">
                              <property name="currentIndex">
-                              <number>2</number>
+                              <number>1</number>
                              </property>
                              <widget class="QWidget" name="page_7">
                               <layout class="QVBoxLayout" name="verticalLayout_30">
@@ -2999,7 +2999,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                  <property name="title">
                                   <string>Images Dataset</string>
                                  </property>
-                                 <layout class="QGridLayout" name="gridLayout_7" columnstretch="0,1">
+                                 <layout class="QGridLayout" name="gridLayout_7" columnstretch="0,0">
                                   <item row="0" column="1">
                                    <widget class="QLineEdit" name="source_images_lineEdit_path">
                                     <property name="readOnly">
@@ -3020,6 +3020,13 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
+                                  <item row="2" column="0">
+                                   <widget class="QCheckBox" name="source_images_refreshDir">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
+                                  </item>
                                   <item row="1" column="1">
                                    <widget class="QLabel" name="label_21">
                                     <property name="text">
@@ -3033,13 +3040,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="2" column="0">
-                                   <widget class="QCheckBox" name="source_images_refreshDir">
-                                    <property name="text">
-                                     <string/>
-                                    </property>
-                                   </widget>
-                                  </item>
                                   <item row="1" column="0">
                                    <widget class="QSpinBox" name="source_images_spinBox_startPos">
                                     <property name="minimum">
@@ -3325,189 +3325,251 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                       <property name="title">
                        <string>Directory of images (optional settings)</string>
                       </property>
-                      <layout class="QGridLayout" name="gridLayout_67" columnstretch="0,0,1">
+                      <layout class="QVBoxLayout" name="verticalLayout_93">
-                       <item row="0" column="1">
+                       <property name="margin">
-                        <widget class="QCheckBox" name="checkBox_cameraImages_timestamps">
+                        <number>0</number>
-                         <property name="text">
+                       </property>
-                          <string/>
+                       <item>
-                         </property>
+                        <layout class="QGridLayout" name="gridLayout_68" columnstretch="0,0,1">
-                        </widget>
+                         <item row="0" column="1">
-                       </item>
+                          <widget class="QCheckBox" name="checkBox_cameraImages_timestamps">
-                       <item row="0" column="2">
+                           <property name="text">
-                        <widget class="QLabel" name="label_255">
+                            <string/>
-                         <property name="text">
+                           </property>
-                          <string>Use file names as timestamps. Format is epoch time. Example: &quot;1305031102.175304.png&quot;</string>
+                          </widget>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="1" column="0">
-                        <widget class="QToolButton" name="toolButton_cameraImages_timestamps">
-                         <property name="text">
-                          <string>...</string>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="1" column="1">
-                        <widget class="QLineEdit" name="lineEdit_cameraImages_timestamps">
-                         <property name="text">
-                          <string/>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="1" column="2">
-                        <widget class="QLabel" name="label_251">
-                         <property name="text">
-                          <string>Timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if &quot;Use file names as timestamps&quot; above is checked. </string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="2" column="0">
-                        <widget class="QToolButton" name="toolButton_cameraImages_gt">
-                         <property name="text">
-                          <string>...</string>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="2" column="1">
-                        <widget class="QLineEdit" name="lineEdit_cameraImages_gt">
-                         <property name="text">
-                          <string/>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="2" column="2">
-                        <widget class="QLabel" name="label_288">
-                         <property name="text">
-                          <string>Ground truth file. Select the correct format below.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="3" column="1">
-                        <widget class="QComboBox" name="comboBox_cameraImages_gtFormat">
-                         <item>
-                          <property name="text">
-                           <string>Raw</string>
-                          </property>
                          </item>
-                         <item>
+                         <item row="0" column="2">
-                          <property name="text">
+                          <widget class="QLabel" name="label_255">
-                           <string>RGBD-SLAM</string>
+                           <property name="text">
-                          </property>
+                            <string>Use file names as timestamps. Format is epoch time. Example: &quot;1305031102.175304.png&quot;</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
                          </item>
-                         <item>
+                         <item row="1" column="0">
-                          <property name="text">
+                          <widget class="QToolButton" name="toolButton_cameraImages_timestamps">
-                           <string>KITTI</string>
+                           <property name="text">
-                          </property>
+                            <string>...</string>
+                           </property>
+                          </widget>
                          </item>
-                         <item>
+                         <item row="1" column="1">
-                          <property name="text">
+                          <widget class="QLineEdit" name="lineEdit_cameraImages_timestamps">
-                           <string>TORO</string>
+                           <property name="text">
-                          </property>
+                            <string/>
+                           </property>
+                          </widget>
                          </item>
-                        </widget>
+                         <item row="1" column="2">
+                          <widget class="QLabel" name="label_251">
+                           <property name="text">
+                            <string>Timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if &quot;Use file names as timestamps&quot; above is checked. </string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="2" column="0">
+                          <widget class="QToolButton" name="toolButton_cameraImages_gt">
+                           <property name="text">
+                            <string>...</string>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="2" column="1">
+                          <widget class="QLineEdit" name="lineEdit_cameraImages_gt">
+                           <property name="text">
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="2" column="2">
+                          <widget class="QLabel" name="label_288">
+                           <property name="text">
+                            <string>Ground truth file. Select the correct format below.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="3" column="1">
+                          <widget class="QComboBox" name="comboBox_cameraImages_gtFormat">
+                           <item>
+                            <property name="text">
+                             <string>Raw</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>RGBD-SLAM</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>KITTI</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>TORO</string>
+                            </property>
+                           </item>
+                          </widget>
+                         </item>
+                         <item row="3" column="2">
+                          <widget class="QLabel" name="label_289">
+                           <property name="text">
+                            <string>Ground truth format.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="4" column="0">
+                          <widget class="QToolButton" name="toolButton_cameraImages_path_scans">
+                           <property name="text">
+                            <string>...</string>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="4" column="1">
+                          <widget class="QLineEdit" name="lineEdit_cameraImages_path_scans">
+                           <property name="text">
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="4" column="2">
+                          <widget class="QLabel" name="label_293">
+                           <property name="text">
+                            <string>Path to directory containing optional laser scans (*.pcd, *.ply, *.bin [KITTI format]). The directory should have the same size has the images directory. </string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="5" column="1">
+                          <widget class="QLineEdit" name="lineEdit_cameraImages_laser_transform">
+                           <property name="toolTip">
+                            <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;KITTI: /base_link to /scan = -0.27 0 0.08 0 0 0&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                           </property>
+                           <property name="text">
+                            <string>0 0 0 0 0 0</string>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="5" column="2">
+                          <widget class="QLabel" name="label_294">
+                           <property name="text">
+                            <string>Local transform from /base_link to /scan_link. Mouse over the box to show formats.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="6" column="1">
+                          <widget class="QSpinBox" name="spinBox_cameraImages_max_scan_pts">
+                           <property name="toolTip">
+                            <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                           </property>
+                           <property name="maximum">
+                            <number>99999999</number>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="6" column="2">
+                          <widget class="QLabel" name="label_292">
+                           <property name="text">
+                            <string>Maximum laser scan points.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                        </layout>
                        </item>
-                       <item row="3" column="2">
+                       <item>
-                        <widget class="QLabel" name="label_289">
+                        <widget class="QGroupBox" name="groupBox_depthFromScan">
-                         <property name="text">
+                         <property name="title">
-                          <string>Ground truth format.</string>
+                          <string>Generate depth image from laser scan</string>
                          </property>
-                         <property name="wordWrap">
+                         <property name="checkable">
                           <bool>true</bool>
                          </property>
-                         <property name="textInteractionFlags">
+                         <property name="checked">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                          <bool>false</bool>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="4" column="0">
-                        <widget class="QToolButton" name="toolButton_cameraImages_path_scans">
-                         <property name="text">
-                          <string>...</string>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="4" column="1">
-                        <widget class="QLineEdit" name="lineEdit_cameraImages_path_scans">
-                         <property name="text">
-                          <string/>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="4" column="2">
-                        <widget class="QLabel" name="label_293">
-                         <property name="text">
-                          <string>Path to directory containing optional laser scans (*.pcd, *.ply, *.bin [KITTI format]). The directory should have the same size has the images directory. </string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="5" column="1">
-                        <widget class="QLineEdit" name="lineEdit_cameraImages_laser_transform">
-                         <property name="toolTip">
-                          <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;KITTI: /base_link to /scan = -0.27 0 0.08 0 0 0&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-                         </property>
-                         <property name="text">
-                          <string>0 0 0 0 0 0</string>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="5" column="2">
-                        <widget class="QLabel" name="label_294">
-                         <property name="text">
-                          <string>Local transform from /base_link to /scan_link. Mouse over the box to show formats.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="6" column="1">
-                        <widget class="QSpinBox" name="spinBox_cameraImages_max_scan_pts">
-                         <property name="toolTip">
-                          <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-                         </property>
-                         <property name="maximum">
-                          <number>99999999</number>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="6" column="2">
-                        <widget class="QLabel" name="label_292">
-                         <property name="text">
-                          <string>Maximum laser scan points.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                          </property>
+                         <layout class="QGridLayout" name="gridLayout_67" columnstretch="0,1">
+                          <item row="0" column="0">
+                           <widget class="QCheckBox" name="checkBox_depthFromScan_vertical">
+                            <property name="text">
+                             <string/>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="0" column="1">
+                           <widget class="QLabel" name="label_305">
+                            <property name="text">
+                             <string>Fill holes vertically, otherwise fill horizontally.</string>
+                            </property>
+                            <property name="wordWrap">
+                             <bool>true</bool>
+                            </property>
+                            <property name="textInteractionFlags">
+                             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="1" column="0">
+                           <widget class="QCheckBox" name="checkBox_depthFromScan_fillBorders">
+                            <property name="text">
+                             <string/>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="1" column="1">
+                           <widget class="QLabel" name="label_306">
+                            <property name="text">
+                             <string>Fill holes from the image border.</string>
+                            </property>
+                            <property name="wordWrap">
+                             <bool>true</bool>
+                            </property>
+                            <property name="textInteractionFlags">
+                             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                            </property>
+                           </widget>
+                          </item>
+                         </layout>
                         </widget>
                        </item>
                       </layout>