CObservation3DRangeScan: add mask; more SSE2 optimized code

Author: jlblancoc

doc/doxygen-pages/changeLog_doc.h

@@ -18,13 +18,17 @@
 	- Changes in apps:
 		- [RawLogViewer](http://www.mrpt.org/list-of-mrpt-apps/rawlogviewer/): Now displays a textual and graphical representation of all observation timestamps, useful to quickly detect sensor "shortages" or temporary failures.
 	- Changes in libraries:
+		- \ref mrpt_obs_grp
+			- mrpt::obs::CObservation3DRangeScan:
+				- Now uses more SSE2 optimized code
+				- Now allows filtering depth images by minimum depth mask in mrpt::obs::CObservation3DRangeScan::project3DPointsFromDepthImageInto() and  mrpt::obs::CObservation3DRangeScan::convertTo2DScan()
 		- \ref mrpt_hwdrivers_grp
 			- mrpt::hwdrivers::CGenericSensor: external image format is now `png` by default instead of `jpg` to avoid losses.
 			- mrpt::hwdrivers::COpenNI2Generic:
 				- refactored to expose more methods and allow changing parameters via its constructor.
 				- Now supports reading from an IR, RGB and Depth channels independenty.
 	- Changes in build system:
-    - [Windows only] `DLL`s/`LIB`s now have the signature `lib-${name}${2-digits-version}${compiler-name}_{x32|x64}.{dll/lib}`, allowing several MRPT versions to coexist in the system PATH.
+	- [Windows only] `DLL`s/`LIB`s now have the signature `lib-${name}${2-digits-version}${compiler-name}_{x32|x64}.{dll/lib}`, allowing several MRPT versions to coexist in the system PATH.
 		- Debian package: depends on libopenni-dev
 	- BUG FIXES:
 		- Fix inconsistent state after calling mrpt::obs::CObservation3DRangeScan::swap()

libs/obs/include/mrpt/obs/CObservation3DRangeScan.h

@@ -31,7 +31,7 @@ namespace obs
 	namespace detail {
 		// Implemented in CObservation3DRangeScan_project3D_impl.h
 		template <class POINTMAP>
-		void project3DPointsFromDepthImageInto(CObservation3DRangeScan    & src_obs,POINTMAP                   & dest_pointcloud,const bool                   takeIntoAccountSensorPoseOnRobot,const mrpt::poses::CPose3D * robotPoseInTheWorld,const bool                   PROJ3D_USE_LUT);
+		void project3DPointsFromDepthImageInto(CObservation3DRangeScan & src_obs,POINTMAP & dest_pointcloud,const bool takeIntoAccountSensorPoseOnRobot,const mrpt::poses::CPose3D * robotPoseInTheWorld,const bool PROJ3D_USE_LUT, const mrpt::math::CMatrix * minRangeMask);
 	}
 
 	/** Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
@@ -172,6 +172,7 @@ namespace obs
 		  *  the points are transformed with \a sensorPose. Furthermore, if provided, those coordinates are transformed with \a robotPoseInTheWorld
 		  *
 		  * \param[in] PROJ3D_USE_LUT (Only when range_is_depth=true) Whether to use a Look-up-table (LUT) to speed up the conversion. It's thread safe in all situations <b>except</b> when you call this method from different threads <b>and</b> with different camera parameter matrices. In all other cases, it's a good idea to left it enabled.
+		  * \param[in] minRangeMask If provided, a matrix with the minimum range (in meters) allowed at each direction (row,col) for a range to be considered valid.
 		  * \tparam POINTMAP Supported maps are all those covered by mrpt::utils::PointCloudAdapter (mrpt::maps::CPointsMap and derived, mrpt::opengl::CPointCloudColoured, PCL point clouds,...)
 		  *
 		  * \note In MRPT < 0.9.5, this method always assumes that ranges were in Kinect-like format.
@@ -181,9 +182,11 @@ namespace obs
 			POINTMAP                   & dest_pointcloud,
 			const bool takeIntoAccountSensorPoseOnRobot,
 			const mrpt::poses::CPose3D *robotPoseInTheWorld=NULL,
-			const bool PROJ3D_USE_LUT=true)
+			const bool PROJ3D_USE_LUT=true,
+			const mrpt::math::CMatrix * minRangeMask = NULL
+			)
 		{
-			detail::project3DPointsFromDepthImageInto<POINTMAP>(*this,dest_pointcloud,takeIntoAccountSensorPoseOnRobot,robotPoseInTheWorld,PROJ3D_USE_LUT);
+			detail::project3DPointsFromDepthImageInto<POINTMAP>(*this,dest_pointcloud,takeIntoAccountSensorPoseOnRobot,robotPoseInTheWorld,PROJ3D_USE_LUT,minRangeMask);
 		}
 
 		/** This method is equivalent to \c project3DPointsFromDepthImageInto() storing the projected 3D points (without color, in local coordinates) in this same class.
@@ -218,6 +221,7 @@ namespace obs
 		  *  \param[in] angle_sup (Default=5deg) The upper half-FOV angle (in radians)
 		  *  \param[in] angle_sup (Default=5deg) The lower half-FOV angle (in radians)
 		  *  \param[in] oversampling_ratio (Default=1.2=120%) How many more laser scans rays to create (read above).
+		  *  \param[in] minRangeMask If provided, a matrix with the minimum range (in meters) allowed at each direction (row,col) for a range to be considered valid.
 		  *
 		  * \sa The example in http://www.mrpt.org/Example_Kinect_To_2D_laser_scan
 		  */
@@ -226,7 +230,9 @@ namespace obs
 			const std::string       & sensorLabel,
 			const double angle_sup = mrpt::utils::DEG2RAD(5),
 			const double angle_inf = mrpt::utils::DEG2RAD(5),
-			const double oversampling_ratio = 1.2 );
+			const double oversampling_ratio = 1.2,
+			const mrpt::math::CMatrix * minRangeMask = NULL
+			);
 
 
 		bool hasPoints3D; //!< true means the field points3D contains valid data.

libs/obs/include/mrpt/obs/CObservation3DRangeScan_project3D_impl.h

@@ -15,16 +15,17 @@ namespace mrpt {
 namespace obs {
 namespace detail {
 	// Auxiliary functions which implement SSE-optimized proyection of 3D point cloud:
-	template <class POINTMAP> void do_project_3d_pointcloud(const int H,const int W,const float *kys,const float *kzs,const mrpt::math::CMatrix &rangeImage, mrpt::utils::PointCloudAdapter<POINTMAP> &pca, std::vector<uint16_t> &idxs_x, std::vector<uint16_t> &idxs_y);
-	template <class POINTMAP> void do_project_3d_pointcloud_SSE2(const int H,const int W,const float *kys,const float *kzs,const mrpt::math::CMatrix &rangeImage, mrpt::utils::PointCloudAdapter<POINTMAP> &pca, std::vector<uint16_t> &idxs_x, std::vector<uint16_t> &idxs_y);
+	template <class POINTMAP> void do_project_3d_pointcloud(const int H,const int W,const float *kys,const float *kzs,const mrpt::math::CMatrix &rangeImage, mrpt::utils::PointCloudAdapter<POINTMAP> &pca, std::vector<uint16_t> &idxs_x, std::vector<uint16_t> &idxs_y,const mrpt::math::CMatrix * minRangeMask);
+	template <class POINTMAP> void do_project_3d_pointcloud_SSE2(const int H,const int W,const float *kys,const float *kzs,const mrpt::math::CMatrix &rangeImage, mrpt::utils::PointCloudAdapter<POINTMAP> &pca, std::vector<uint16_t> &idxs_x, std::vector<uint16_t> &idxs_y,const mrpt::math::CMatrix * minRangeMask);
 
 	template <class POINTMAP>
 	void project3DPointsFromDepthImageInto(
 			CObservation3DRangeScan    & src_obs,
 			POINTMAP                   & dest_pointcloud,
 			const bool                   takeIntoAccountSensorPoseOnRobot,
 			const mrpt::poses::CPose3D * robotPoseInTheWorld,
-			const bool                   PROJ3D_USE_LUT)
+			const bool                   PROJ3D_USE_LUT,
+			const mrpt::math::CMatrix * minRangeMask)
 	{
 		using namespace mrpt::math;
 
@@ -37,6 +38,7 @@ namespace detail {
 		// ------------------------------------------------------------
 		const int W = src_obs.rangeImage.cols();
 		const int H = src_obs.rangeImage.rows();
+		ASSERT_(W!=0 && H!=0);
 		const size_t WH = W*H;
 
 		src_obs.resizePoints3DVectors(WH); // This is to make sure points3D_idxs_{x,y} have the expected sizes.
@@ -76,12 +78,16 @@ namespace detail {
 				float *kys = &src_obs.m_3dproj_lut.Kys[0];
 				float *kzs = &src_obs.m_3dproj_lut.Kzs[0];
 
+				if (minRangeMask) { // sanity check:
+					ASSERT_EQUAL_(minRangeMask->cols(), src_obs.rangeImage.cols());
+					ASSERT_EQUAL_(minRangeMask->rows(), src_obs.rangeImage.rows());
+				}
 	#if MRPT_HAS_SSE2
 				if ((W & 0x07)==0)
-				     do_project_3d_pointcloud_SSE2(H,W,kys,kzs,src_obs.rangeImage,pca, src_obs.points3D_idxs_x, src_obs.points3D_idxs_y );
-				else do_project_3d_pointcloud(H,W,kys,kzs,src_obs.rangeImage,pca, src_obs.points3D_idxs_x, src_obs.points3D_idxs_y );  // if image width is not 8*N, use standard method
+				     do_project_3d_pointcloud_SSE2(H,W,kys,kzs,src_obs.rangeImage,pca, src_obs.points3D_idxs_x, src_obs.points3D_idxs_y,minRangeMask );
+				else do_project_3d_pointcloud(H,W,kys,kzs,src_obs.rangeImage,pca, src_obs.points3D_idxs_x, src_obs.points3D_idxs_y,minRangeMask );  // if image width is not 8*N, use standard method
 	#else
-				do_project_3d_pointcloud(H,W,kys,kzs,src_obs.rangeImage,pca,src_obs.points3D_idxs_x, src_obs.points3D_idxs_y);
+				do_project_3d_pointcloud(H,W,kys,kzs,src_obs.rangeImage,pca,src_obs.points3D_idxs_x, src_obs.points3D_idxs_y,minRangeMask);
 	#endif
 			}
 			else
@@ -264,14 +270,15 @@ namespace detail {
 
 	// Auxiliary functions which implement proyection of 3D point clouds:
 	template <class POINTMAP>
-	inline void do_project_3d_pointcloud(const int H,const int W,const float *kys,const float *kzs,const mrpt::math::CMatrix &rangeImage, mrpt::utils::PointCloudAdapter<POINTMAP> &pca, std::vector<uint16_t> &idxs_x, std::vector<uint16_t> &idxs_y)
+	inline void do_project_3d_pointcloud(const int H,const int W,const float *kys,const float *kzs,const mrpt::math::CMatrix &rangeImage, mrpt::utils::PointCloudAdapter<POINTMAP> &pca, std::vector<uint16_t> &idxs_x, std::vector<uint16_t> &idxs_y,const mrpt::math::CMatrix * minRangeMask)
 	{
+		// Preconditions: minRangeMask() has the right size
 		size_t idx=0;
 		for (int r=0;r<H;r++)
 			for (int c=0;c<W;c++)
 			{
 				const float D = rangeImage.coeff(r,c);
-				if (D!=.0f) {
+				if (D!=.0f && (!minRangeMask || D>minRangeMask->coeff(r,c)) ) {
 					pca.setPointXYZ(idx, D /*x*/, *kys++ * D /*y*/, *kzs++ * D /*z*/);
 					idxs_x[idx]=c;
 					idxs_y[idx]=r;
@@ -283,36 +290,49 @@ namespace detail {
 
 	// Auxiliary functions which implement proyection of 3D point clouds:
 	template <class POINTMAP>
-	inline void do_project_3d_pointcloud_SSE2(const int H,const int W,const float *kys,const float *kzs,const mrpt::math::CMatrix &rangeImage, mrpt::utils::PointCloudAdapter<POINTMAP> &pca, std::vector<uint16_t> &idxs_x, std::vector<uint16_t> &idxs_y)
+	inline void do_project_3d_pointcloud_SSE2(const int H,const int W,const float *kys,const float *kzs,const mrpt::math::CMatrix &rangeImage, mrpt::utils::PointCloudAdapter<POINTMAP> &pca, std::vector<uint16_t> &idxs_x, std::vector<uint16_t> &idxs_y,const mrpt::math::CMatrix * minRangeMask)
 	{
 	#if MRPT_HAS_SSE2
+			// Preconditions: minRangeMask() has the right size
 			// Use optimized version:
 			const int W_4 = W >> 2;  // /=4 , since we process 4 values at a time.
 			size_t idx=0;
 			MRPT_ALIGN16 float xs[4],ys[4],zs[4];
+			const __m128 D_zeros = _mm_set_ps(.0f,.0f,.0f,.0f);
 			for (int r=0;r<H;r++)
 			{
 				const float *D_ptr = &rangeImage.coeffRef(r,0);  // Matrices are 16-aligned
+				const float *Dmin_ptr = !minRangeMask ? NULL : &minRangeMask->coeffRef(r,0);
 
 				for (int c=0;c<W_4;c++)
 				{
 					const __m128 D = _mm_load_ps(D_ptr);
+					__m128 valid_range_mask = _mm_cmpgt_ps(D, D_zeros);
 
-					const __m128 KY = _mm_load_ps(kys);
-					const __m128 KZ = _mm_load_ps(kzs);
-
-					_mm_storeu_ps(xs , D);
-					_mm_storeu_ps(ys , _mm_mul_ps(KY,D));
-					_mm_storeu_ps(zs , _mm_mul_ps(KZ,D));
-
-					for (int q=0;q<4;q++)
-						if (xs[q]!=.0f) {
-							pca.setPointXYZ(idx,xs[q],ys[q],zs[q]);
-							idxs_x[idx]=(c<<2)+q;
-							idxs_y[idx]=r;
-							++idx;
-						}
+					if (minRangeMask) {
+						const __m128 gt_mask = _mm_cmpgt_ps(D, _mm_load_ps(Dmin_ptr) );
+						valid_range_mask = _mm_and_ps(valid_range_mask, gt_mask );
+					}
+					const int valid_range_maski = _mm_movemask_epi8(_mm_castps_si128(valid_range_mask)); // 0x{f|0}{f|0}{f|0}{f|0}
+					if (valid_range_maski)  // Any of the 4 values is valid?
+					{
+						const __m128 KY = _mm_load_ps(kys);
+						const __m128 KZ = _mm_load_ps(kzs);
+
+						_mm_storeu_ps(xs , D);
+						_mm_storeu_ps(ys , _mm_mul_ps(KY,D));
+						_mm_storeu_ps(zs , _mm_mul_ps(KZ,D));
+
+						for (int q=0;q<4;q++)
+							if ((valid_range_maski & (1<<(q*4))) !=0) {
+								pca.setPointXYZ(idx,xs[q],ys[q],zs[q]);
+								idxs_x[idx]=(c<<2)+q;
+								idxs_y[idx]=r;
+								++idx;
+							}
+					}
 					D_ptr+=4;
+					if (Dmin_ptr) Dmin_ptr+=4;
 					kys+=4;
 					kzs+=4;
 				}

libs/obs/src/CObservation3DRangeScan.cpp

@@ -856,7 +856,8 @@ void CObservation3DRangeScan::convertTo2DScan(
 	const std::string &sensorLabel,
 	const double angle_sup,
 	const double angle_inf,
-	const double oversampling_ratio
+	const double oversampling_ratio,
+	const mrpt::math::CMatrix * minRangeMask
 	)
 {
 	out_scan2d.sensorLabel = sensorLabel;
@@ -871,6 +872,10 @@ void CObservation3DRangeScan::convertTo2DScan(
 
 	const size_t nCols = this->rangeImage.cols();
 	const size_t nRows = this->rangeImage.rows();
+	if (minRangeMask) { // sanity check:
+		ASSERT_EQUAL_(minRangeMask->cols(), rangeImage.cols());
+		ASSERT_EQUAL_(minRangeMask->rows(), rangeImage.rows());
+	}
 
 	// Compute the real horizontal FOV from the range camera intrinsic calib data:
 	// Note: this assumes the range image has been "undistorted", which is true for data
@@ -933,7 +938,7 @@ void CObservation3DRangeScan::convertTo2DScan(
 		for (size_t r=0;r<nRows;r++)
 		{
 			const float D = this->rangeImage.coeff(r,c);
-			if (D>0)
+			if (D>0 && (!minRangeMask || D>minRangeMask->coeff(r,c)))
 			{
 				const float this_point_tan = vert_ang_tan[r] * D;
 				if (this_point_tan>tan_min && this_point_tan<tan_max)