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.

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,