Increased database version to 0.10.7: added scan_max_range column. Create2DMap: updated unknown cells filling depending on max laser scan range. Added util3d::frustumFiltering() to filter points in a specified frustum.

CMakeLists.txt

@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 10)
-SET(RTABMAP_PATCH_VERSION 6)
+SET(RTABMAP_PATCH_VERSION 7)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

corelib/include/rtabmap/core/CameraModel.h

@@ -96,6 +96,7 @@ public:
 	void setLocalTransform(const Transform & transform) {localTransform_ = transform;}
 	const Transform & localTransform() const {return localTransform_;}
 
+	void setImageSize(const cv::Size & size) {imageSize_ = size;}
 	const cv::Size & imageSize() const {return imageSize_;}
 	int imageWidth() const {return imageSize_.width;}
 	int imageHeight() const {return imageSize_.height;}
@@ -105,6 +106,9 @@ public:
 
 	void scale(double scale);
 
+	double horizontalFOV() const; // in degrees
+	double verticalFOV() const;   // in degrees
+
 	// For depth images, your should use cv::INTER_NEAREST
 	cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const;
 	cv::Mat rectifyDepth(const cv::Mat & raw) const;

corelib/include/rtabmap/core/SensorData.h

@@ -75,6 +75,7 @@ public:
 	SensorData(
 			const cv::Mat & laserScan,
 			int laserScanMaxPts,
+			float laserScanMaxRange,
 			const cv::Mat & rgb,
 			const cv::Mat & depth,
 			const CameraModel & cameraModel,
@@ -95,6 +96,7 @@ public:
 	SensorData(
 			const cv::Mat & laserScan,
 			int laserScanMaxPts,
+			float laserScanMaxRange,
 			const cv::Mat & rgb,
 			const cv::Mat & depth,
 			const std::vector<CameraModel> & cameraModels,
@@ -115,6 +117,7 @@ public:
 	SensorData(
 			const cv::Mat & laserScan,
 			int laserScanMaxPts,
+			float laserScanMaxRange,
 			const cv::Mat & left,
 			const cv::Mat & right,
 			const StereoCameraModel & cameraModel,
@@ -147,6 +150,7 @@ public:
 	double stamp() const {return _stamp;}
 	void setStamp(double stamp) {_stamp = stamp;}
 	int laserScanMaxPts() const {return _laserScanMaxPts;}
+	float laserScanMaxRange() const {return _laserScanMaxRange;}
 
 	const cv::Mat & imageCompressed() const {return _imageCompressed;}
 	const cv::Mat & depthOrRightCompressed() const {return _depthOrRightCompressed;}
@@ -157,7 +161,7 @@ public:
 	const cv::Mat & laserScanRaw() const {return _laserScanRaw;}
 	void setImageRaw(const cv::Mat & imageRaw) {_imageRaw = imageRaw;}
 	void setDepthOrRightRaw(const cv::Mat & depthOrImageRaw) {_depthOrRightRaw =depthOrImageRaw;}
-	void setLaserScanRaw(const cv::Mat & laserScanRaw, int laserScanMaxPts) {_laserScanRaw =laserScanRaw;_laserScanMaxPts = laserScanMaxPts;}
+	void setLaserScanRaw(const cv::Mat & laserScanRaw, int maxPts, float maxRange) {_laserScanRaw =laserScanRaw;_laserScanMaxPts = maxPts;_laserScanMaxRange=maxRange;}
 	void setCameraModel(const CameraModel & model) {_cameraModels.clear(); _cameraModels.push_back(model);}
 	void setCameraModels(const std::vector<CameraModel> & models) {_cameraModels = models;}
 	void setStereoCameraModel(const StereoCameraModel & stereoCameraModel) {_stereoCameraModel = stereoCameraModel;}
@@ -190,6 +194,7 @@ private:
 	int _id;
 	double _stamp;
 	int _laserScanMaxPts;
+	float _laserScanMaxRange;
 
 	cv::Mat _imageCompressed;          // compressed image
 	cv::Mat _depthOrRightCompressed;   // compressed image

corelib/include/rtabmap/core/util3d_filtering.h

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define UTIL3D_FILTERING_H_
 
 #include <rtabmap/core/RtabmapExp.h>
+#include <rtabmap/core/Transform.h>
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -63,12 +64,31 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP passThrough(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const std::string & axis,
 		float min,
-		float max);
+		float max,
+		bool negative = false);
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP passThrough(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const std::string & axis,
 		float min,
-		float max);
+		float max,
+		bool negative = false);
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP frustumFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const Transform & cameraPose,
+		float horizontalFOV, // in degrees, xfov = atan((image_width/2)/fx)*2
+		float verticalFOV,   // in degrees, yfov = atan((image_height/2)/fy)*2
+		float nearClipPlaneDistance,
+		float farClipPlaneDistance,
+		bool negative = false);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP frustumFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const Transform & cameraPose,
+		float horizontalFOV, // in degrees, xfov = atan((image_width/2)/fx)*2
+		float verticalFOV,   // in degrees, yfov = atan((image_height/2)/fy)*2
+		float nearClipPlaneDistance,
+		float farClipPlaneDistance,
+		bool negative = false);
 
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP removeNaNFromPointCloud(

corelib/include/rtabmap/core/util3d_mapping.h

@@ -48,7 +48,8 @@ void RTABMAP_EXP occupancy2DFromLaserScan(
 		cv::Mat & ground,
 		cv::Mat & obstacles,
 		float cellSize,
-		bool unknownSpaceFilled = false);
+		bool unknownSpaceFilled = false,
+		float scanMaxRange = 0.0f); // would be set if unknownSpaceFilled=true
 
 cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps(
 		const std::map<int, Transform> & poses,
@@ -65,7 +66,8 @@ cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
 		bool unknownSpaceFilled,
 		float & xMin,
 		float & yMin,
-		float minMapSize = 0.0f);
+		float minMapSize = 0.0f,
+		float scanMaxRange = 0.0f); // would be set if unknownSpaceFilled=true
 
 void RTABMAP_EXP rayTrace(const cv::Point2i & start,
 		const cv::Point2i & end,

corelib/src/CameraModel.cpp

@@ -277,6 +277,24 @@ void CameraModel::scale(double scale)
 	P_.at<double>(1,2) *= scale;
 }
 
+double CameraModel::horizontalFOV() const
+{
+	if(imageWidth() > 0 && fx() > 0.0)
+	{
+		return atan((double(imageWidth())/2.0)/fx())*2.0*180.0/CV_PI;
+	}
+	return 0.0;
+}
+
+double CameraModel::verticalFOV() const
+{
+	if(imageHeight() > 0 && fy() > 0.0)
+	{
+		return atan((double(imageHeight())/2.0)/fy())*2.0*180.0/CV_PI;
+	}
+	return 0.0;
+}
+
 cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const
 {
 	if(!mapX_.empty() && !mapY_.empty())

corelib/src/DBDriverSqlite3.cpp

@@ -457,7 +457,14 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
 
-		if(uStrNumCmp(_version, "0.10.1") >= 0)
+		if(uStrNumCmp(_version, "0.10.7") >= 0)
+		{
+			query << "SELECT image, depth, calibration, scan_max_pts, scan_max_range, scan, user_data "
+				  << "FROM Data "
+				  << "WHERE id = ?"
+				  <<";";
+		}
+		else if(uStrNumCmp(_version, "0.10.1") >= 0)
 		{
 			query << "SELECT image, depth, calibration, scan_max_pts, scan, user_data "
 				  << "FROM Data "
@@ -653,6 +660,12 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
 					laserScanMaxPts = sqlite3_column_int(ppStmt, index++);
 				}
 
+				float laserScanMaxRange = 0.0f;
+				if(uStrNumCmp(_version, "0.10.7") >= 0)
+				{
+					laserScanMaxRange = sqlite3_column_int(ppStmt, index++);
+				}
+
 				data = sqlite3_column_blob(ppStmt, index);
 				dataSize = sqlite3_column_bytes(ppStmt, index++);
 				//Create the laserScan
@@ -685,6 +698,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
 					(*iter)->sensorData() = SensorData(
 							scanCompressed,
 							laserScanMaxPts,
+							laserScanMaxRange,
 							imageCompressed,
 							depthOrRightCompressed,
 							models,
@@ -697,6 +711,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
 					(*iter)->sensorData() = SensorData(
 							scanCompressed,
 							laserScanMaxPts,
+							laserScanMaxRange,
 							imageCompressed,
 							depthOrRightCompressed,
 							stereoModel,
@@ -2360,7 +2375,11 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensor
 std::string DBDriverSqlite3::queryStepSensorData() const
 {
 	UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
-	if(uStrNumCmp(_version, "0.10.1") >= 0)
+	if(uStrNumCmp(_version, "0.10.7") >= 0)
+	{
+		return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan_max_range, scan, user_data) VALUES(?,?,?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.10.1") >= 0)
 	{
 		return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan, user_data) VALUES(?,?,?,?,?,?,?);";
 	}
@@ -2455,6 +2474,13 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
 	rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanMaxPts());
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
+	// scan_max_range
+	if(uStrNumCmp(_version, "0.10.7") >= 0)
+	{
+		rc = sqlite3_bind_double(ppStmt, index++, sensorData.laserScanMaxRange());
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+	}
+
 	// scan
 	if(!sensorData.laserScanCompressed().empty())
 	{

corelib/src/Memory.cpp

@@ -4302,6 +4302,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 				SensorData(
 						ctDepth2d.getCompressedData(),
 						data.laserScanMaxPts(),
+						data.laserScanMaxRange(),
 						ctImage.getCompressedData(),
 						ctDepth.getCompressedData(),
 						stereoCameraModel,
@@ -4311,6 +4312,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 				SensorData(
 						ctDepth2d.getCompressedData(),
 						data.laserScanMaxPts(),
+						data.laserScanMaxRange(),
 						ctImage.getCompressedData(),
 						ctDepth.getCompressedData(),
 						cameraModels,
@@ -4337,6 +4339,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 				SensorData(
 						ctDepth2d.getCompressedData(),
 						data.laserScanMaxPts(),
+						data.laserScanMaxRange(),
 						cv::Mat(),
 						cv::Mat(),
 						stereoCameraModel,
@@ -4346,6 +4349,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 				SensorData(
 						ctDepth2d.getCompressedData(),
 						data.laserScanMaxPts(),
+						data.laserScanMaxRange(),
 						cv::Mat(),
 						cv::Mat(),
 						cameraModels,
@@ -4359,7 +4363,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	{
 		s->sensorData().setImageRaw(image);
 		s->sensorData().setDepthOrRightRaw(depthOrRightImage);
-		s->sensorData().setLaserScanRaw(laserScan, data.laserScanMaxPts());
+		s->sensorData().setLaserScanRaw(laserScan, data.laserScanMaxPts(), data.laserScanMaxRange());
 		s->sensorData().setUserDataRaw(data.userDataRaw());
 	}
 

corelib/src/SensorData.cpp

@@ -38,7 +38,8 @@ namespace rtabmap
 SensorData::SensorData() :
 		_id(0),
 		_stamp(0.0),
-		_laserScanMaxPts(0)
+		_laserScanMaxPts(0),
+		_laserScanMaxRange(0.0f)
 {
 }
 
@@ -50,7 +51,8 @@ SensorData::SensorData(
 		const cv::Mat & userData) :
 		_id(id),
 		_stamp(stamp),
-		_laserScanMaxPts(0)
+		_laserScanMaxPts(0),
+		_laserScanMaxRange(0.0f)
 {
 	if(image.rows == 1)
 	{
@@ -84,6 +86,7 @@ SensorData::SensorData(
 		_id(id),
 		_stamp(stamp),
 		_laserScanMaxPts(0),
+		_laserScanMaxRange(0.0f),
 		_cameraModels(std::vector<CameraModel>(1, cameraModel))
 {
 	if(image.rows == 1)
@@ -119,6 +122,7 @@ SensorData::SensorData(
 		_id(id),
 		_stamp(stamp),
 		_laserScanMaxPts(0),
+		_laserScanMaxRange(0.0f),
 		_cameraModels(std::vector<CameraModel>(1, cameraModel))
 {
 	if(rgb.rows == 1)
@@ -159,6 +163,7 @@ SensorData::SensorData(
 SensorData::SensorData(
 		const cv::Mat & laserScan,
 		int laserScanMaxPts,
+		float laserScanMaxRange,
 		const cv::Mat & rgb,
 		const cv::Mat & depth,
 		const CameraModel & cameraModel,
@@ -168,6 +173,7 @@ SensorData::SensorData(
 		_id(id),
 		_stamp(stamp),
 		_laserScanMaxPts(laserScanMaxPts),
+		_laserScanMaxRange(laserScanMaxRange),
 		_cameraModels(std::vector<CameraModel>(1, cameraModel))
 {
 	if(rgb.rows == 1)
@@ -224,6 +230,7 @@ SensorData::SensorData(
 		_id(id),
 		_stamp(stamp),
 		_laserScanMaxPts(0),
+		_laserScanMaxRange(0.0f),
 		_cameraModels(cameraModels)
 {
 	if(rgb.rows == 1)
@@ -263,6 +270,7 @@ SensorData::SensorData(
 SensorData::SensorData(
 		const cv::Mat & laserScan,
 		int laserScanMaxPts,
+		float laserScanMaxRange,
 		const cv::Mat & rgb,
 		const cv::Mat & depth,
 		const std::vector<CameraModel> & cameraModels,
@@ -272,6 +280,7 @@ SensorData::SensorData(
 		_id(id),
 		_stamp(stamp),
 		_laserScanMaxPts(laserScanMaxPts),
+		_laserScanMaxRange(laserScanMaxRange),
 		_cameraModels(cameraModels)
 {
 	if(rgb.rows == 1)
@@ -328,6 +337,7 @@ SensorData::SensorData(
 		_id(id),
 		_stamp(stamp),
 		_laserScanMaxPts(0),
+		_laserScanMaxRange(0.0f),
 		_stereoCameraModel(cameraModel)
 {
 	if(left.rows == 1)
@@ -367,6 +377,7 @@ SensorData::SensorData(
 SensorData::SensorData(
 		const cv::Mat & laserScan,
 		int laserScanMaxPts,
+		float laserScanMaxRange,
 		const cv::Mat & left,
 		const cv::Mat & right,
 		const StereoCameraModel & cameraModel,
@@ -376,6 +387,7 @@ SensorData::SensorData(
 		_id(id),
 		_stamp(stamp),
 		_laserScanMaxPts(laserScanMaxPts),
+		_laserScanMaxRange(laserScanMaxRange),
 		_stereoCameraModel(cameraModel)
 {
 	if(left.rows == 1)

corelib/src/resources/DatabaseSchema.sql.in

@@ -31,6 +31,7 @@ CREATE TABLE Data (
 	calibration BLOB,         -- fx, fy, cx, cy [,baseline] local_transform
 	scan BLOB,                -- compressed data (Laser scan)
 	scan_max_pts INTEGER,     -- Laser scan max points
+	scan_max_range FLOAT,     -- Laser max range
 	user_data BLOB,           -- compressed data (User data)
 	time_enter DATE,
 	PRIMARY KEY (id)

corelib/src/util3d_filtering.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <pcl/filters/extract_indices.h>
 #include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/frustum_culling.h>
 #include <pcl/filters/random_sample.h>
 #include <pcl/filters/passthrough.h>
 
@@ -114,13 +115,15 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr passThrough(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const std::string & axis,
 		float min,
-		float max)
+		float max,
+		bool negative)
 {
 	UASSERT(max > min);
 	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::PassThrough<pcl::PointXYZ> filter;
+	filter.setNegative(negative);
 	filter.setFilterFieldName(axis);
 	filter.setFilterLimits(min, max);
 	filter.setInputCloud(cloud);
@@ -132,13 +135,15 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr passThrough(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const std::string & axis,
 		float min,
-		float max)
+		float max,
+		bool negative)
 {
 	UASSERT(max > min);
 	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
 
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
 	pcl::PassThrough<pcl::PointXYZRGB> filter;
+	filter.setNegative(negative);
 	filter.setFilterFieldName(axis);
 	filter.setFilterLimits(min, max);
 	filter.setInputCloud(cloud);
@@ -146,6 +151,62 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr passThrough(
 	return output;
 }
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr frustumFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const Transform & cameraPose,
+		float horizontalFOV, // in degrees
+		float verticalFOV,   // in degrees
+		float nearClipPlaneDistance,
+		float farClipPlaneDistance,
+		bool negative)
+{
+	UASSERT(horizontalFOV > 0.0f && verticalFOV > 0.0f);
+	UASSERT(farClipPlaneDistance > nearClipPlaneDistance);
+	UASSERT(!cameraPose.isNull());
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::FrustumCulling<pcl::PointXYZ> fc;
+	fc.setNegative(negative);
+	fc.setInputCloud (cloud);
+	fc.setVerticalFOV (verticalFOV);
+	fc.setHorizontalFOV (horizontalFOV);
+	fc.setNearPlaneDistance (nearClipPlaneDistance);
+	fc.setFarPlaneDistance (farClipPlaneDistance);
+
+	fc.setCameraPose (cameraPose.toEigen4f());
+	fc.filter (*output);
+
+	return output;
+}
+
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr frustumFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const Transform & cameraPose,
+		float horizontalFOV, // in degrees
+		float verticalFOV,   // in degrees
+		float nearClipPlaneDistance,
+		float farClipPlaneDistance,
+		bool negative)
+{
+	UASSERT(horizontalFOV > 0.0f && verticalFOV > 0.0f);
+	UASSERT(farClipPlaneDistance > nearClipPlaneDistance);
+	UASSERT(!cameraPose.isNull());
+
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
+	pcl::FrustumCulling<pcl::PointXYZRGB> fc;
+	fc.setNegative(negative);
+	fc.setInputCloud (cloud);
+	fc.setVerticalFOV (verticalFOV);
+	fc.setHorizontalFOV (horizontalFOV);
+	fc.setNearPlaneDistance (nearClipPlaneDistance);
+	fc.setFarPlaneDistance (farClipPlaneDistance);
+
+	fc.setCameraPose (cameraPose.toEigen4f());
+	fc.filter (*output);
+
+	return output;
+}
+
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr removeNaNFromPointCloud(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud)

corelib/src/util3d_mapping.cpp

@@ -50,7 +50,8 @@ void occupancy2DFromLaserScan(
 		cv::Mat & ground,
 		cv::Mat & obstacles,
 		float cellSize,
-		bool unknownSpaceFilled)
+		bool unknownSpaceFilled,
+		float scanMaxRange)
 {
 	if(scan.empty())
 	{
@@ -67,7 +68,7 @@ void occupancy2DFromLaserScan(
 	scans.insert(std::make_pair(1, obstaclesCloud));
 
 	float xMin, yMin;
-	cv::Mat map8S = create2DMap(poses, scans, cellSize, unknownSpaceFilled, xMin, yMin);
+	cv::Mat map8S = create2DMap(poses, scans, cellSize, unknownSpaceFilled, xMin, yMin, 0.0f, scanMaxRange);
 
 	// find ground cells
 	std::list<int> groundIndices;
@@ -388,6 +389,8 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
  * @param unknownSpaceFilled if false no fill, otherwise a virtual laser sweeps the unknown space from each pose (stopping on detected obstacle)
  * @param xMin
  * @param yMin
+ * @param minMapSize minimum map size in meters
+ * @param scanMaxRange laser scan maximum range, would be set if unknownSpaceFilled=true
  */
 cv::Mat create2DMap(const std::map<int, Transform> & poses,
 		const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
@@ -395,7 +398,8 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 		bool unknownSpaceFilled,
 		float & xMin,
 		float & yMin,
-		float minMapSize)
+		float minMapSize,
+		float scanMaxRange)
 {
 	UDEBUG("poses=%d, scans = %d", poses.size(), scans.size());
 	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > localScans;
@@ -429,7 +433,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 		pcl::getMinMax3D(minMax, min, max);
 
 		// Added X2 to make sure that all points are inside the map (when rounded to integer)
-		float marging = cellSize*10.0f;
+		float marging = cellSize*10.0f + (scanMaxRange>0.0f?scanMaxRange/cellSize:0.0f);
 		xMin = min.x-marging;
 		yMin = min.y-marging;
 		float xMax = max.x+marging;
@@ -440,7 +444,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 		UTimer timer;
 
 		map = cv::Mat::ones((yMax - yMin) / cellSize + 0.5f, (xMax - xMin) / cellSize + 0.5f, CV_8S)*-1;
-		std::vector<float> maxSquaredLength(localScans.size(), 0.0f);
+		std::vector<float> maxSquaredLength(localScans.size(), scanMaxRange*scanMaxRange);
 		int j=0;
 		for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
 		{
@@ -510,7 +514,10 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 						endRotatedVector.at<float>(1) = endRotated.at<float>(1) - origin.at<float>(1);
 						endRotatedVector.at<float>(2) = 0.0f;
 						//UWARN("endRotated = %f %f", endRotated.at<float>(0), endRotated.at<float>(1));
-						while(endRotatedVector.cross(endLastVector).at<float>(2) > 0.0f)
+						float CV_PI_2 = CV_PI/2.0f;
+						float normEndRotatedVector = cv::norm(endRotatedVector);
+						endLastVector = endLastVector / cv::norm(endLastVector);
+						while(acos((endRotatedVector/normEndRotatedVector).dot(endLastVector)) > CV_PI_2 || endRotatedVector.cross(endLastVector).at<float>(2) > 0.0f)
 						{
 							cv::Point2i end((endRotated.at<float>(0)-xMin)/cellSize + 0.5f, (endRotated.at<float>(1)-yMin)/cellSize + 0.5f);
 							//end must be inside the grid
@@ -519,7 +526,6 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 							end.y = end.y < 0?0:end.y;
 							end.y = end.y >= map.rows?map.rows-1:end.y;
 							rayTrace(start, end, map, true); // trace free space
-
 							// next point
 							endRotated = rotation*(endRotated - origin) + origin;
 							endRotatedVector.at<float>(0) = endRotated.at<float>(0) - origin.at<float>(0);

guilib/src/CloudViewer.cpp

@@ -33,7 +33,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/core/util3d.h>
 #include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/filters/frustum_culling.h>
 #include <QMenu>
 #include <QAction>
 #include <QtGui/QContextMenuEvent>

guilib/src/DatabaseViewer.cpp

@@ -848,6 +848,7 @@ void DatabaseViewer::exportDatabase()
 						sensorData = rtabmap::SensorData(
 							dialog.isDepth2dExported()?data.laserScanRaw():cv::Mat(),
 							dialog.isDepth2dExported()?data.laserScanMaxPts():0,
+							dialog.isDepth2dExported()?data.laserScanMaxRange():0,
 							dialog.isRgbExported()?data.imageRaw():cv::Mat(),
 							dialog.isDepthExported()?data.depthOrRightRaw():cv::Mat(),
 							data.cameraModels(),
@@ -860,6 +861,7 @@ void DatabaseViewer::exportDatabase()
 						sensorData = rtabmap::SensorData(
 							dialog.isDepth2dExported()?data.laserScanRaw():cv::Mat(),
 							dialog.isDepth2dExported()?data.laserScanMaxPts():0,
+							dialog.isDepth2dExported()?data.laserScanMaxRange():0,
 							dialog.isRgbExported()?data.imageRaw():cv::Mat(),
 							dialog.isDepthExported()?data.depthOrRightRaw():cv::Mat(),
 							data.stereoCameraModel(),