Merge dev-functional (#593)

Change function pointers to C++11 std::function<> wherever it makes sense.

cc: #66

libs/base/include/mrpt/math/CLevenbergMarquardt.h

@@ -14,6 +14,7 @@
 #include <mrpt/math/num_jacobian.h>
 #include <mrpt/utils/printf_vector.h>
 #include <mrpt/math/ops_containers.h>
+#include <functional>
 
 namespace mrpt
 {
@@ -55,15 +56,15 @@ class CLevenbergMarquardtTempl : public mrpt::utils::COutputLogger
 	 * root error, for the given "x". The functor code must set the size of this
 	 * vector.
 	  */
-	typedef void (*TFunctorEval)(
-		const VECTORTYPE& x, const USERPARAM& y, VECTORTYPE& out);
+	using TFunctorEval = std::function<void(
+		const VECTORTYPE& x, const USERPARAM& y, VECTORTYPE& out)>;
 
 	/** The type of an optional functor passed to \a execute to replace the
 	 * Euclidean addition "x_new = x_old + x_incr" by any other operation.
 	  */
-	typedef void (*TFunctorIncrement)(
+	using TFunctorIncrement = std::function<void(
 		VECTORTYPE& x_new, const VECTORTYPE& x_old, const VECTORTYPE& x_incr,
-		const USERPARAM& user_param);
+		const USERPARAM& user_param)>;
 
 	struct TResultInfo
 	{

libs/base/include/mrpt/math/jacobians.h

@@ -15,6 +15,7 @@
 #include <mrpt/poses/CPosePDF.h>
 #include <mrpt/poses/CPose3DQuatPDF.h>
 #include <mrpt/poses/CPose3DPDF.h>
+#include <functional>
 
 namespace mrpt
 {
@@ -142,7 +143,9 @@ template <class VECTORLIKE, class VECTORLIKE2, class VECTORLIKE3,
 		  class MATRIXLIKE, class USERPARAM>
 inline void jacob_numeric_estimate(
 	const VECTORLIKE& x,
-	void (*functor)(const VECTORLIKE& x, const USERPARAM& y, VECTORLIKE3& out),
+	std::function<
+		void(const VECTORLIKE& x, const USERPARAM& y, VECTORLIKE3& out)>
+		functor,
 	const VECTORLIKE2& increments, const USERPARAM& userParam,
 	MATRIXLIKE& out_Jacobian)
 {
@@ -151,9 +154,6 @@ inline void jacob_numeric_estimate(
 }
 
 }  // End of jacobians namespace
-
 }  // End of MATH namespace
-
 }  // End of namespace
-
 #endif

libs/base/include/mrpt/math/num_jacobian.h

@@ -9,6 +9,7 @@
 #pragma once
 
 #include <mrpt/utils/core_defs.h>
+#include <functional>
 
 namespace mrpt
 {
@@ -29,7 +30,8 @@ template <class VECTORLIKE, class VECTORLIKE2, class VECTORLIKE3,
 		  class MATRIXLIKE, class USERPARAM>
 void estimateJacobian(
 	const VECTORLIKE& x,
-	void (*functor)(const VECTORLIKE& x, const USERPARAM& y, VECTORLIKE3& out),
+	const std::function<void(
+		const VECTORLIKE& x, const USERPARAM& y, VECTORLIKE3& out)>& functor,
 	const VECTORLIKE2& increments, const USERPARAM& userParam,
 	MATRIXLIKE& out_Jacobian)
 {

libs/base/include/mrpt/math/ransac.h

@@ -12,6 +12,7 @@
 #include <mrpt/utils/COutputLogger.h>
 #include <mrpt/math/CMatrixD.h>
 #include <set>
+#include <functional>
 
 namespace mrpt
 {
@@ -33,24 +34,24 @@ class BASE_IMPEXP RANSAC_Template : public mrpt::utils::COutputLogger
 	RANSAC_Template() : mrpt::utils::COutputLogger("RANSAC_Template") {}
 	/** The type of the function passed to mrpt::math::ransac - See the
 	 * documentation for that method for more info. */
-	typedef void (*TRansacFitFunctor)(
+	using TRansacFitFunctor = std::function<void(
 		const CMatrixTemplateNumeric<NUMTYPE>& allData,
 		const mrpt::vector_size_t& useIndices,
-		std::vector<CMatrixTemplateNumeric<NUMTYPE>>& fitModels);
+		std::vector<CMatrixTemplateNumeric<NUMTYPE>>& fitModels)>;
 
 	/** The type of the function passed to mrpt::math::ransac  - See the
 	 * documentation for that method for more info. */
-	typedef void (*TRansacDistanceFunctor)(
+	using TRansacDistanceFunctor = std::function<void(
 		const CMatrixTemplateNumeric<NUMTYPE>& allData,
 		const std::vector<CMatrixTemplateNumeric<NUMTYPE>>& testModels,
 		const NUMTYPE distanceThreshold, unsigned int& out_bestModelIndex,
-		mrpt::vector_size_t& out_inlierIndices);
+		mrpt::vector_size_t& out_inlierIndices)>;
 
 	/** The type of the function passed to mrpt::math::ransac  - See the
 	 * documentation for that method for more info. */
-	typedef bool (*TRansacDegenerateFunctor)(
+	using TRansacDegenerateFunctor = std::function<bool(
 		const CMatrixTemplateNumeric<NUMTYPE>& allData,
-		const mrpt::vector_size_t& useIndices);
+		const mrpt::vector_size_t& useIndices)>;
 
 	/** An implementation of the RANSAC algorithm for robust fitting of models
 	 * to data.
@@ -66,8 +67,10 @@ class BASE_IMPEXP RANSAC_Template : public mrpt::utils::COutputLogger
 	 * COutputLogger settings.
 	  */
 	bool execute(
-		const CMatrixTemplateNumeric<NUMTYPE>& data, TRansacFitFunctor fit_func,
-		TRansacDistanceFunctor dist_func, TRansacDegenerateFunctor degen_func,
+		const CMatrixTemplateNumeric<NUMTYPE>& data,
+		const TRansacFitFunctor& fit_func,
+		const TRansacDistanceFunctor& dist_func,
+		const TRansacDegenerateFunctor& degen_func,
 		const double distanceThreshold,
 		const unsigned int minimumSizeSamplesToFit,
 		mrpt::vector_size_t& out_best_inliers,

libs/base/include/mrpt/math/transform_gaussian.h

@@ -17,6 +17,7 @@
 #include <mrpt/math/jacobians.h>
 #include <mrpt/math/data_utils.h>
 #include <mrpt/random.h>
+#include <functional>
 
 namespace mrpt
 {
@@ -165,7 +166,10 @@ void transform_gaussian_linear(
 				  VECTORLIKE1::RowsAtCompileTime>
 		H;
 	mrpt::math::jacobians::jacob_numeric_estimate(
-		x_mean, functor, x_increments, fixed_param, H);
+		x_mean, std::function<void(
+					const VECTORLIKE1& x, const USERPARAM& fixed_param,
+					VECTORLIKE3& y)>(functor),
+		x_increments, fixed_param, H);
 	H.multiply_HCHt(x_cov, y_cov);
 	MRPT_END
 }

libs/base/src/math/ransac.cpp

@@ -23,9 +23,10 @@ using namespace std;
  ---------------------------------------------------------------*/
 template <typename NUMTYPE>
 bool RANSAC_Template<NUMTYPE>::execute(
-	const CMatrixTemplateNumeric<NUMTYPE>& data, TRansacFitFunctor fit_func,
-	TRansacDistanceFunctor dist_func, TRansacDegenerateFunctor degen_func,
-	const double distanceThreshold, const unsigned int minimumSizeSamplesToFit,
+	const CMatrixTemplateNumeric<NUMTYPE>& data,
+	const TRansacFitFunctor& fit_func, const TRansacDistanceFunctor& dist_func,
+	const TRansacDegenerateFunctor& degen_func, const double distanceThreshold,
+	const unsigned int minimumSizeSamplesToFit,
 	mrpt::vector_size_t& out_best_inliers,
 	CMatrixTemplateNumeric<NUMTYPE>& out_best_model, const double p,
 	const size_t maxIter) const

libs/base/src/math/ransac_applications.cpp

@@ -138,8 +138,9 @@ void mrpt::math::ransac_detect_3D_planes(
 		math::RANSAC_Template<NUMTYPE> ransac;
 		ransac.setVerbosityLevel(mrpt::utils::LVL_INFO);
 		ransac.execute(
-			remainingPoints, ransac3Dplane_fit, ransac3Dplane_distance,
-			ransac3Dplane_degenerate, threshold,
+			remainingPoints, mrpt::math::ransac3Dplane_fit<NUMTYPE>,
+			mrpt::math::ransac3Dplane_distance<NUMTYPE>,
+			mrpt::math::ransac3Dplane_degenerate<NUMTYPE>, threshold,
 			3,  // Minimum set of points
 			this_best_inliers, this_best_model,
 			0.999  // Prob. of good result
@@ -306,8 +307,9 @@ void mrpt::math::ransac_detect_2D_lines(
 		math::RANSAC_Template<NUMTYPE> ransac;
 		ransac.setVerbosityLevel(mrpt::utils::LVL_INFO);
 		ransac.execute(
-			remainingPoints, ransac2Dline_fit, ransac2Dline_distance,
-			ransac2Dline_degenerate, threshold,
+			remainingPoints, ransac2Dline_fit<NUMTYPE>,
+			ransac2Dline_distance<NUMTYPE>, ransac2Dline_degenerate<NUMTYPE>,
+			threshold,
 			2,  // Minimum set of points
 			this_best_inliers, this_best_model,
 			0.99999  // Prob. of good result

libs/base/src/poses/CPose3DPDFGaussian_unittest.cpp

@@ -179,7 +179,10 @@ class Pose3DPDFGaussTests : public ::testing::Test
 			x_incrs.assign(1e-7);
 			CMatrixDouble numJacobs;
 			mrpt::math::jacobians::jacob_numeric_estimate(
-				x_mean, func_compose, x_incrs, DUMMY, numJacobs);
+				x_mean, std::function<void(
+							const CArrayDouble<12>& x, const double& dummy,
+							CArrayDouble<6>& Y)>(&func_compose),
+				x_incrs, DUMMY, numJacobs);
 
 			numJacobs.extractMatrix(0, 0, num_df_dx);
 			numJacobs.extractMatrix(0, 6, num_df_du);

libs/base/src/poses/CPose3DQuatPDFGaussian.cpp

@@ -220,7 +220,7 @@ void CPose3DQuatPDFGaussian::copyFrom(const CPose3DPDFGaussian& o)
 
 		static const double dummy = 0;
 		mrpt::math::transform_gaussian_unscented(
-			x_mean, o.cov, aux_poseypr2posequat, dummy, this->mean, this->cov);
+			x_mean, o.cov, &aux_poseypr2posequat, dummy, this->mean, this->cov);
 	}
 }
 

libs/base/src/poses/CPose3DQuatPDFGaussian_unittest.cpp

@@ -174,7 +174,10 @@ class Pose3DQuatPDFGaussTests : public ::testing::Test
 			x_incrs.assign(1e-7);
 			CMatrixDouble numJacobs;
 			mrpt::math::jacobians::jacob_numeric_estimate(
-				x_mean, func_compose, x_incrs, DUMMY, numJacobs);
+				x_mean, std::function<void(
+							const CArrayDouble<2 * 7>& x, const double& dummy,
+							CArrayDouble<7>& Y)>(&func_compose),
+				x_incrs, DUMMY, numJacobs);
 
 			numJacobs.extractMatrix(0, 0, num_df_dx);
 			numJacobs.extractMatrix(0, 7, num_df_du);

libs/base/src/poses/CPose3DQuat_unittest.cpp

@@ -164,7 +164,10 @@ class Pose3DQuatTests : public ::testing::Test
 			x_incrs.assign(1e-7);
 			CMatrixDouble numJacobs;
 			mrpt::math::jacobians::jacob_numeric_estimate(
-				x_mean, func_compose_point, x_incrs, DUMMY, numJacobs);
+				x_mean, std::function<void(
+							const CArrayDouble<7 + 3>& x, const double& dummy,
+							CArrayDouble<3>& Y)>(&func_compose_point),
+				x_incrs, DUMMY, numJacobs);
 
 			numJacobs.extractMatrix(0, 0, num_df_dpose);
 			numJacobs.extractMatrix(0, 7, num_df_dpoint);
@@ -288,7 +291,10 @@ class Pose3DQuatTests : public ::testing::Test
 			x_incrs.assign(1e-7);
 			CMatrixDouble numJacobs;
 			mrpt::math::jacobians::jacob_numeric_estimate(
-				x_mean, func_inv_compose_point, x_incrs, DUMMY, numJacobs);
+				x_mean, std::function<void(
+							const CArrayDouble<7 + 3>& x, const double& dummy,
+							CArrayDouble<3>& Y)>(&func_inv_compose_point),
+				x_incrs, DUMMY, numJacobs);
 
 			numJacobs.extractMatrix(0, 0, num_df_dpose);
 			numJacobs.extractMatrix(0, 7, num_df_dpoint);
@@ -493,7 +499,10 @@ class Pose3DQuatTests : public ::testing::Test
 			x_incrs.assign(1e-7);
 			CMatrixDouble numJacobs;
 			mrpt::math::jacobians::jacob_numeric_estimate(
-				x_mean, func_spherical_coords, x_incrs, DUMMY, numJacobs);
+				x_mean, std::function<void(
+							const CArrayDouble<7 + 3>& x, const double& dummy,
+							CArrayDouble<3>& Y)>(&func_spherical_coords),
+				x_incrs, DUMMY, numJacobs);
 
 			numJacobs.extractMatrix(0, 0, num_df_dpose);
 			numJacobs.extractMatrix(0, 7, num_df_dpoint);
@@ -551,7 +560,10 @@ class Pose3DQuatTests : public ::testing::Test
 			x_incrs.assign(1e-5);
 			CMatrixDouble numJacobs;
 			mrpt::math::jacobians::jacob_numeric_estimate(
-				x_mean, func_normalizeJacob, x_incrs, DUMMY, numJacobs);
+				x_mean, std::function<void(
+							const CArrayDouble<4>& x, const double& dummy,
+							CArrayDouble<4>& Y)>(&func_normalizeJacob),
+				x_incrs, DUMMY, numJacobs);
 
 			numJacobs.extractMatrix(0, 0, num_df_dpose);
 		}

libs/base/src/poses/CPose3D_unittest.cpp

@@ -319,7 +319,10 @@ class Pose3DTests : public ::testing::Test
 			x_incrs.assign(1e-7);
 			CMatrixDouble numJacobs;
 			mrpt::math::jacobians::jacob_numeric_estimate(
-				x_mean, func_compose_point, x_incrs, DUMMY, numJacobs);
+				x_mean, std::function<void(
+							const CArrayDouble<6 + 3>& x, const double& dummy,
+							CArrayDouble<3>& Y)>(&func_compose_point),
+				x_incrs, DUMMY, numJacobs);
 
 			numJacobs.extractMatrix(0, 0, num_df_dpose);
 			numJacobs.extractMatrix(0, 6, num_df_dpoint);
@@ -391,7 +394,10 @@ class Pose3DTests : public ::testing::Test
 			x_incrs.assign(1e-7);
 			CMatrixDouble numJacobs;
 			mrpt::math::jacobians::jacob_numeric_estimate(
-				x_mean, func_inv_compose_point, x_incrs, DUMMY, numJacobs);
+				x_mean, std::function<void(
+							const CArrayDouble<6 + 3>& x, const double& dummy,
+							CArrayDouble<3>& Y)>(&func_inv_compose_point),
+				x_incrs, DUMMY, numJacobs);
 
 			numJacobs.extractMatrix(0, 0, num_df_dpose);
 			numJacobs.extractMatrix(0, 6, num_df_dpoint);
@@ -474,7 +480,10 @@ class Pose3DTests : public ::testing::Test
 			CArrayDouble<6> x_incrs;
 			x_incrs.assign(1e-9);
 			mrpt::math::jacobians::jacob_numeric_estimate(
-				x_mean, func_compose_point_se3, x_incrs, P, num_df_dse3);
+				x_mean, std::function<void(
+							const CArrayDouble<6>& x, const CArrayDouble<3>& P,
+							CArrayDouble<3>& Y)>(&func_compose_point_se3),
+				x_incrs, P, num_df_dse3);
 		}
 
 		EXPECT_NEAR(0, (df_dse3 - num_df_dse3).array().abs().sum(), 3e-3)
@@ -508,7 +517,10 @@ class Pose3DTests : public ::testing::Test
 			CArrayDouble<6> x_incrs;
 			x_incrs.assign(1e-9);
 			mrpt::math::jacobians::jacob_numeric_estimate(
-				x_mean, func_invcompose_point_se3, x_incrs, P, num_df_dse3);
+				x_mean, std::function<void(
+							const CArrayDouble<6>& x, const CArrayDouble<3>& P,
+							CArrayDouble<3>& Y)>(&func_invcompose_point_se3),
+				x_incrs, P, num_df_dse3);
 		}
 
 		EXPECT_NEAR(0, (df_dse3 - num_df_dse3).array().abs().sum(), 3e-3)
@@ -542,7 +554,10 @@ class Pose3DTests : public ::testing::Test
 		x_incrs.assign(1e-9);
 		CMatrixDouble numJacobs;
 		mrpt::math::jacobians::jacob_numeric_estimate(
-			x_mean, func_jacob_expe_e, x_incrs, dummy, numJacobs);
+			x_mean, std::function<void(
+						const CArrayDouble<6>& x, const double& dummy,
+						CArrayDouble<12>& Y)>(&func_jacob_expe_e),
+			x_incrs, dummy, numJacobs);
 
 		// Theoretical matrix:
 		// [ 0   -[e1]_x ]
@@ -601,7 +616,10 @@ class Pose3DTests : public ::testing::Test
 			CArrayDouble<12> x_incrs;
 			x_incrs.assign(1e-6);
 			mrpt::math::jacobians::jacob_numeric_estimate(
-				x_mean, func_jacob_LnT_T, x_incrs, dummy, numJacobs);
+				x_mean, std::function<void(
+							const CArrayDouble<12>& x, const double& dummy,
+							CArrayDouble<6>& Y)>(&func_jacob_LnT_T),
+				x_incrs, dummy, numJacobs);
 		}
 
 		EXPECT_NEAR((numJacobs - theor_jacob).array().abs().sum(), 0, 1e-3)
@@ -645,7 +663,10 @@ class Pose3DTests : public ::testing::Test
 			CArrayDouble<6> x_incrs;
 			x_incrs.assign(1e-6);
 			mrpt::math::jacobians::jacob_numeric_estimate(
-				x_mean, func_jacob_expe_D, x_incrs, p, numJacobs);
+				x_mean, std::function<void(
+							const CArrayDouble<6>& eps, const CPose3D& D,
+							CArrayDouble<12>& Y)>(&func_jacob_expe_D),
+				x_incrs, p, numJacobs);
 		}
 
 		EXPECT_NEAR((numJacobs - theor_jacob).array().abs().maxCoeff(), 0, 1e-3)
@@ -699,7 +720,11 @@ class Pose3DTests : public ::testing::Test
 			CArrayDouble<6> x_incrs;
 			x_incrs.assign(1e-6);
 			mrpt::math::jacobians::jacob_numeric_estimate(
-				x_mean, func_jacob_Aexpe_D, x_incrs, params, numJacobs);
+				x_mean, std::function<void(
+							const CArrayDouble<6>& eps,
+							const TParams_func_jacob_Aexpe_D& params,
+							CArrayDouble<12>& Y)>(&func_jacob_Aexpe_D),
+				x_incrs, params, numJacobs);
 		}
 
 		EXPECT_NEAR((numJacobs - theor_jacob).array().abs().maxCoeff(), 0, 1e-3)

libs/base/src/poses/SE_traits_unittest.cpp

@@ -101,7 +101,12 @@ class SE_traits_tests : public ::testing::Test
 			x_incrs.assign(1e-4);
 			CMatrixDouble numJacobs;
 			mrpt::math::jacobians::jacob_numeric_estimate(
-				x_mean, func_numeric, x_incrs, params, numJacobs);
+				x_mean,
+				std::function<void(
+					const CArrayDouble<2 * SE_TYPE::VECTOR_SIZE>& x,
+					const TParams& params,
+					CArrayDouble<SE_TYPE::VECTOR_SIZE>& Y)>(&func_numeric),
+				x_incrs, params, numJacobs);
 
 			numJacobs.extractMatrix(0, 0, num_J1);
 			numJacobs.extractMatrix(0, DIMS, num_J2);