PTGs: small code refactoring; new method inverseMap_WS2TP()

jlblancoc · jlblancoc · commit 4d04ef50e3de · 2013-12-28T22:32:28.000+01:00
diff --git a/doc/doxygen-pages/changeLog_doc.h b/doc/doxygen-pages/changeLog_doc.h
@@ -69,6 +69,8 @@
 				- mrpt::slam::TMatchingExtraResults
 		- mrpt::slam::CObservationIMU now includes data fields for 3D magnetometers and altimeters. - [(commit)](http://code.google.com/p/mrpt/source/detail?r=3451)
 		- Method renamed mrpt::utils::CEnhancedMetaFile::selectVectorTextFont() to avoid shadowing mrpt::utils::CCanvas::selectTextFont()
+		- mrpt::reactivenav::CParameterizedTrajectoryGenerator: New method for inverse look-up of WS to TP space - [(commit)]()
+			- mrpt::reactivenav::CParameterizedTrajectoryGenerator::inverseMap_WS2TP()
 	- Deleted classes:
 		- mrpt::utils::CEvent, which was actually unimplemented (!)
 		- mrpt::hwdrivers::CInterfaceNI845x has been deleted. It didn't offer features enough to justify a class.
diff --git a/libs/reactivenav/include/mrpt/reactivenav/CPTG1.h b/libs/reactivenav/include/mrpt/reactivenav/CPTG1.h
@@ -48,8 +48,7 @@ namespace mrpt
 			 */
 			CPTG1(const TParameters<double> &params );
 
-			/** The lambda function. */
-			void lambdaFunction( float x, float y, int &out_k, float &out_d );
+			virtual bool inverseMap_WS2TP(float x, float y, int &out_k, float &out_d, float tolerance_dist = 0.10f) const;
 
 			/** Gets a short textual description of the PTG and its parameters. */
 			std::string getDescription() const;
diff --git a/libs/reactivenav/include/mrpt/reactivenav/CPTG2.h b/libs/reactivenav/include/mrpt/reactivenav/CPTG2.h
@@ -45,10 +45,6 @@ namespace mrpt
 			 */
 			CPTG2(const TParameters<double> &params );
 
-			/** The lambda function.
-			  */
-			void lambdaFunction( float x, float y, int &out_k, float &out_d );
-
 			/** Gets a short textual description of the PTG and its parameters.
 			*/
 			std::string getDescription() const;
diff --git a/libs/reactivenav/include/mrpt/reactivenav/CPTG3.h b/libs/reactivenav/include/mrpt/reactivenav/CPTG3.h
@@ -27,10 +27,6 @@ namespace mrpt
 			 */
 			CPTG3(const TParameters<double> &params );
 
-			/** The lambda function.
-			  */
-			void lambdaFunction( float x, float y, int &out_k, float &out_d );
-
 			/** Gets a short textual description of the PTG and its parameters.
 			*/
 			std::string getDescription() const;
diff --git a/libs/reactivenav/include/mrpt/reactivenav/CPTG4.h b/libs/reactivenav/include/mrpt/reactivenav/CPTG4.h
@@ -27,10 +27,6 @@ namespace mrpt
 			 */
 			CPTG4(const TParameters<double> &params );
 
-			/** The lambda function.
-			  */
-			void lambdaFunction( float x, float y, int &out_k, float &out_d );
-
 			/** Gets a short textual description of the PTG and its parameters.
 			*/
 			std::string getDescription() const;
diff --git a/libs/reactivenav/include/mrpt/reactivenav/CPTG5.h b/libs/reactivenav/include/mrpt/reactivenav/CPTG5.h
@@ -28,10 +28,6 @@ namespace mrpt
 			 */
 			CPTG5(const TParameters<double> &params );
 
-			/** The lambda function.
-			  */
-			void lambdaFunction( float x, float y, int &out_k, float &out_d );
-
 			/** Gets a short textual description of the PTG and its parameters.
 			*/
 			std::string getDescription() const;
diff --git a/libs/reactivenav/include/mrpt/reactivenav/CPTG6.h b/libs/reactivenav/include/mrpt/reactivenav/CPTG6.h
@@ -25,10 +25,6 @@ namespace mrpt
 			 */
 			CPTG6(const TParameters<double> &params );
 
-			/** The lambda function.
-			  */
-			void lambdaFunction( float x, float y, int &out_k, float &out_d );
-
 			/** Gets a short textual description of the PTG and its parameters.
 			*/
 			std::string getDescription() const;
diff --git a/libs/reactivenav/include/mrpt/reactivenav/CPTG7.h b/libs/reactivenav/include/mrpt/reactivenav/CPTG7.h
@@ -26,10 +26,6 @@ namespace mrpt
 			 */
 			CPTG7(const TParameters<double> &params );
 
-			/** The lambda function.
-			  */
-			void lambdaFunction( float x, float y, int &out_k, float &out_d );
-
 			/** Gets a short textual description of the PTG and its parameters.
 			*/
 			std::string getDescription() const;
diff --git a/libs/reactivenav/include/mrpt/reactivenav/CParameterizedTrajectoryGenerator.h b/libs/reactivenav/include/mrpt/reactivenav/CParameterizedTrajectoryGenerator.h
@@ -20,9 +20,12 @@ namespace mrpt
   {
 	  using namespace mrpt::utils;
 
-	/** This is the base class for any user defined PTG.
+	/** This is the base class for any user-defined PTG.
 	 *   The class factory interface in CParameterizedTrajectoryGenerator::CreatePTG.
 	 *
+	 * Papers: 
+	 *  - J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, "Extending Obstacle Avoidance Methods through Multiple Parameter-Space Transformations", Autonomous Robots, vol. 24, no. 1, 2008. http://ingmec.ual.es/~jlblanco/papers/blanco2008eoa_DRAFT.pdf
+	 *
 	 * Changes history:
 	 *		- 30/JUN/2004: Creation (JLBC)
 	 *		- 16/SEP/2004: Totally redesigned.
@@ -39,6 +42,8 @@ namespace mrpt
 		 *   - ref_distance: The maximum distance in PTGs
 		 *   - resolution: The cell size
 		 *   - v_max, w_max: Maximum robot speeds.
+		 *
+		 * See docs of derived classes for additional parameters:
 		 */
         CParameterizedTrajectoryGenerator(const TParameters<double> &params);
 
@@ -75,9 +80,22 @@ namespace mrpt
 				float			*out_max_acc_v = NULL,
 				float			*out_max_acc_w = NULL);
 
-        /** The "lambda" function, see paper for info. It takes the (a,d) pair that is closest to a given location.
-		 */
-        virtual void lambdaFunction( float x, float y, int &out_k, float &out_d );
+
+		/** Computes the closest (alpha,d) TP coordinates of the trajectory point closest to the Workspace (WS) Cartesian coordinates (x,y).
+		  * \param[in] x X coordinate of the query point.
+		  * \param[in] y Y coordinate of the query point.
+		  * \param[out] out_k Trajectory parameter index (discretized alpha value, 0-based index).
+		  * \param[out] out_d Trajectory distance, normalized such that D_max becomes 1.
+		  *
+		  * \return true if the distance between (x,y) and the actual trajectory point is below the given tolerance (in meters).
+		  * \note The default implementation in CParameterizedTrajectoryGenerator relies on a look-up-table. Derived classes may redefine this to closed-form expressions, when they exist.
+		  */
+		virtual bool inverseMap_WS2TP(float x, float y, int &out_k, float &out_d, float tolerance_dist = 0.10f) const;
+
+        /** The "lambda" function, see paper for info. It takes the (a,d) pair that is closest to a given location. */
+		MRPT_DEPRECATED_PRE("Use inverseMap_WS2TP() instead")
+        void lambdaFunction( float x, float y, int &out_k, float &out_d );
+		MRPT_DEPRECATED_POST("Use inverseMap_WS2TP() instead");
 
         /** Converts an "alpha" value (into the discrete set) into a feasible motion command.
 		 */
diff --git a/libs/reactivenav/src/CPTG1.cpp b/libs/reactivenav/src/CPTG1.cpp
@@ -50,13 +50,9 @@ bool CPTG1::PTG_IsIntoDomain( float x, float y )
 	return true;
 }
 
-/*---------------------------------------------------------------
-                lambdaFunction
-  ---------------------------------------------------------------*/
-void CPTG1::lambdaFunction( float x, float y, int &k_out, float &d_out )
+bool CPTG1::inverseMap_WS2TP(float x, float y, int &k_out, float &d_out, float tolerance_dist) const
 {
-	MRPT_TODO("Update API to return a bool for approximate results!")
-
+	bool is_exact = true;
 	if (y!=0)
 	{
 		double R = (x*x+y*y)/(2*y);
@@ -83,10 +79,9 @@ void CPTG1::lambdaFunction( float x, float y, int &k_out, float &d_out )
 		// Distance thru arc:
 		d_out = (float)(theta * (fabs(R)+turningRadiusReference));
 
-		bool is_approx = false;
 		if (std::abs(R)<Rmin)
 		{
-			is_approx=true;
+			is_exact=false;
 			R=Rmin*mrpt::utils::sign(R);
 		}
 		
@@ -101,11 +96,13 @@ void CPTG1::lambdaFunction( float x, float y, int &k_out, float &d_out )
 		{
 			k_out = alpha2index(0);
 			d_out = x;
+			is_exact=true;
 		}
 		else
 		{
 			k_out = alpha2index((float)M_PI);
 			d_out = 1e+3;
+			is_exact=false;
 		}
 	}
 
@@ -114,4 +111,6 @@ void CPTG1::lambdaFunction( float x, float y, int &k_out, float &d_out )
 
 	ASSERT_ABOVEEQ_(k_out,0)
 	ASSERT_BELOW_(k_out,this->m_alphaValuesCount)
+
+	return is_exact;
 }
diff --git a/libs/reactivenav/src/CPTG2.cpp b/libs/reactivenav/src/CPTG2.cpp
@@ -55,10 +55,3 @@ bool CPTG2::PTG_IsIntoDomain( float x, float y )
 	return true;
 }
 
-/*---------------------------------------------------------------
-					lambdaFunction
-  ---------------------------------------------------------------*/
-void CPTG2::lambdaFunction( float x, float y, int &out_k, float &out_d )
-{
-	CParameterizedTrajectoryGenerator::lambdaFunction(x,y,out_k,out_d);
-}
diff --git a/libs/reactivenav/src/CPTG3.cpp b/libs/reactivenav/src/CPTG3.cpp
@@ -90,10 +90,3 @@ bool CPTG3::PTG_IsIntoDomain( float x, float y )
 	}
 }
 
-/*---------------------------------------------------------------
-					lambdaFunction
-  ---------------------------------------------------------------*/
-void CPTG3::lambdaFunction( float x, float y, int &out_k, float &out_d )
-{
-	CParameterizedTrajectoryGenerator::lambdaFunction(x,y,out_k,out_d);
-}
diff --git a/libs/reactivenav/src/CPTG4.cpp b/libs/reactivenav/src/CPTG4.cpp
@@ -80,10 +80,3 @@ bool CPTG4::PTG_IsIntoDomain( float x, float y )
 }
 
 
-/*---------------------------------------------------------------
-					lambdaFunction
-  ---------------------------------------------------------------*/
-void CPTG4::lambdaFunction( float x, float y, int &out_k, float &out_d )
-{
-	CParameterizedTrajectoryGenerator::lambdaFunction(x,y,out_k,out_d);
-}
diff --git a/libs/reactivenav/src/CPTG5.cpp b/libs/reactivenav/src/CPTG5.cpp
@@ -84,11 +84,3 @@ bool CPTG5::PTG_IsIntoDomain( float x, float y )
 	}
 }
 
-
-/*---------------------------------------------------------------
-					lambdaFunction
-  ---------------------------------------------------------------*/
-void CPTG5::lambdaFunction( float x, float y, int &out_k, float &out_d )
-{
-	CParameterizedTrajectoryGenerator::lambdaFunction(x,y,out_k,out_d);
-}
diff --git a/libs/reactivenav/src/CPTG6.cpp b/libs/reactivenav/src/CPTG6.cpp
@@ -56,11 +56,3 @@ bool CPTG6::PTG_IsIntoDomain( float x, float y )
 {
 	return true;
 }
-
-/*---------------------------------------------------------------
-					lambdaFunction
-  ---------------------------------------------------------------*/
-void CPTG6::lambdaFunction( float x, float y, int &out_k, float &out_d )
-{
-	CParameterizedTrajectoryGenerator::lambdaFunction(x,y,out_k,out_d);
-}
diff --git a/libs/reactivenav/src/CPTG7.cpp b/libs/reactivenav/src/CPTG7.cpp
@@ -52,10 +52,3 @@ bool CPTG7::PTG_IsIntoDomain( float x, float y )
 	return !( (fabs(y)>M_PI*V_MAX) || (x<M_PI*V_MAX) );
 }
 
-/*---------------------------------------------------------------
-					lambdaFunction
-  ---------------------------------------------------------------*/
-void CPTG7::lambdaFunction( float x, float y, int &out_k, float &out_d )
-{
-	CParameterizedTrajectoryGenerator::lambdaFunction(x,y,out_k,out_d);
-}
diff --git a/libs/reactivenav/src/CParameterizedTrajectoryGenerator.cpp b/libs/reactivenav/src/CParameterizedTrajectoryGenerator.cpp
@@ -624,10 +624,13 @@ bool CParameterizedTrajectoryGenerator::CColisionGrid::loadFromFile( CStream *f,
 	}
 }
 
-/*---------------------------------------------------------------
-				lambdaFunction
-  ---------------------------------------------------------------*/
-void CParameterizedTrajectoryGenerator::lambdaFunction( float x, float y, int &k_out, float &d_out )
+// Deprecated:         
+void CParameterizedTrajectoryGenerator::lambdaFunction( float x, float y, int &out_k, float &out_d )
+{
+	this->inverseMap_WS2TP(x,y,out_k,out_d);
+}
+
+bool CParameterizedTrajectoryGenerator::inverseMap_WS2TP( float x, float y, int &k_out, float &d_out, float tolerance_dist ) const
 {
     ASSERTMSG_(m_alphaValuesCount>0, "Have you called simulateTrajectories() first?")
 
@@ -650,7 +653,7 @@ void CParameterizedTrajectoryGenerator::lambdaFunction( float x, float y, int &k
 	{
 		for (int cy=cy0-1;cy<=cy0+1;cy++)
 		{
-			TCellForLambdaFunction	*cell = m_lambdaFunctionOptimizer.cellByIndex(cx,cy);
+			const TCellForLambdaFunction	*cell = m_lambdaFunctionOptimizer.cellByIndex(cx,cy);
 			if (cell && !cell->isEmpty())
 			{
 				if (!at_least_one)
@@ -702,7 +705,7 @@ void CParameterizedTrajectoryGenerator::lambdaFunction( float x, float y, int &k
 	{
 		k_out = selected_k;
 		d_out = selected_d / refDistance;
-		return;
+		return (selected_dist <= square(tolerance_dist));
 	}
 
 	// If not found, compute an extrapolation:
@@ -730,6 +733,7 @@ void CParameterizedTrajectoryGenerator::lambdaFunction( float x, float y, int &k
 
 	k_out = selected_k;
 	d_out = selected_d / refDistance;
+	return false;
 }
 
 

Original file line number	Diff line number	Diff line change
`@@ -50,13 +50,9 @@ bool CPTG1::PTG_IsIntoDomain( float x, float y )`
`50`	`50`	`return true;`
`51`	`51`	`}`
`52`	`52`
`53`		`-/*---------------------------------------------------------------`
`54`		`- lambdaFunction`
`55`		`- ---------------------------------------------------------------*/`
`56`		`-void CPTG1::lambdaFunction( float x, float y, int &k_out, float &d_out )`
	`53`	`+bool CPTG1::inverseMap_WS2TP(float x, float y, int &k_out, float &d_out, float tolerance_dist) const`
`57`	`54`	`{`
`58`		`- MRPT_TODO("Update API to return a bool for approximate results!")`
`59`		`-`
	`55`	`+ bool is_exact = true;`
`60`	`56`	`if (y!=0)`
`61`	`57`	`{`
`62`	`58`	`double R = (xx+yy)/(2*y);`
`@@ -83,10 +79,9 @@ void CPTG1::lambdaFunction( float x, float y, int &k_out, float &d_out )`
`83`	`79`	`// Distance thru arc:`
`84`	`80`	`d_out = (float)(theta * (fabs(R)+turningRadiusReference));`
`85`	`81`
`86`		`- bool is_approx = false;`
`87`	`82`	`if (std::abs(R)<Rmin)`
`88`	`83`	`{`
`89`		`- is_approx=true;`
	`84`	`+ is_exact=false;`
`90`	`85`	`R=Rmin*mrpt::utils::sign(R);`
`91`	`86`	`}`
`92`	`87`
`@@ -101,11 +96,13 @@ void CPTG1::lambdaFunction( float x, float y, int &k_out, float &d_out )`
`101`	`96`	`{`
`102`	`97`	`k_out = alpha2index(0);`
`103`	`98`	`d_out = x;`
	`99`	`+ is_exact=true;`
`104`	`100`	`}`
`105`	`101`	`else`
`106`	`102`	`{`
`107`	`103`	`k_out = alpha2index((float)M_PI);`
`108`	`104`	`d_out = 1e+3;`
	`105`	`+ is_exact=false;`
`109`	`106`	`}`
`110`	`107`	`}`
`111`	`108`
`@@ -114,4 +111,6 @@ void CPTG1::lambdaFunction( float x, float y, int &k_out, float &d_out )`
`114`	`111`
`115`	`112`	`ASSERT_ABOVEEQ_(k_out,0)`
`116`	`113`	`ASSERT_BELOW_(k_out,this->m_alphaValuesCount)`
	`114`	`+`
	`115`	`+ return is_exact;`
`117`	`116`	`}`