From bb90ff048598c9683d89ad7cd86eec2500d1f7b4 Mon Sep 17 00:00:00 2001
From: blackcoder <tarek.taha@gmail.com>
Date: Thu, 3 Jun 2010 15:30:39 +1000
Subject: [PATCH] - A prototype cpp wrapper

---
 sm/CMakeLists.txt       |   2 +-
 sm/csmpp/CMakeLists.txt |   8 +-
 sm/csmpp/csmpp.cpp      |  61 ++++++++-
 sm/csmpp/csmpp.h        | 294 +++++++++++++++++++++++++++++++++++++++-
 4 files changed, 355 insertions(+), 10 deletions(-)

diff --git a/sm/CMakeLists.txt b/sm/CMakeLists.txt
index 19b9951..4f349ab 100644
--- a/sm/CMakeLists.txt
+++ b/sm/CMakeLists.txt
@@ -71,7 +71,7 @@ SUBDIRS(lib/json-c)
 SUBDIRS(lib/egsl)
 SUBDIRS(lib/gpc)
 SUBDIRS(csm)
-
+SUBDIRS(csmpp)
 
 
 
diff --git a/sm/csmpp/CMakeLists.txt b/sm/csmpp/CMakeLists.txt
index 9bbb3a8..67fe464 100644
--- a/sm/csmpp/CMakeLists.txt
+++ b/sm/csmpp/CMakeLists.txt
@@ -5,7 +5,7 @@ SET(csmpp_version 0.1)
 # Require we have pkgconfig installed
 find_package(PkgConfig REQUIRED)
 # Tell pkgconfig to look for CSM
-pkg_check_modules(CSM REQUIRED csm)
+#pkg_check_modules(CSM REQUIRED csm)
 
 # Look for CSM
 IF(${CSM_FOUND})
@@ -13,18 +13,16 @@ IF(${CSM_FOUND})
 	MESSAGE("CSM_LIBRARIES: ${CSM_LIBRARIES}")
 	MESSAGE("CSM_INCLUDE_DIRS: ${CSM_INCLUDE_DIRS}")
 	MESSAGE("CSM_LDFLAGS: " ${CSM_LDFLAGS})
-
-
 	INCLUDE_DIRECTORIES(${CSM_INCLUDE_DIRS}) # important! 
 	LINK_DIRECTORIES(${CSM_LIBRARY_DIRS})    # important! 
 ELSE(${CSM_FOUND})	
-	MESSAGE(FATAL_ERROR "CSM not found. Check that the environment variable PKG_CONFIG_PATH includes the path containing the file 'csm.pc'.")
+	#MESSAGE(FATAL_ERROR "CSM not found. Check that the environment variable PKG_CONFIG_PATH includes the path containing the file 'csm.pc'.")
 ENDIF(${CSM_FOUND})
  
 # Name of generated libraries
 SET(dynamic_lib csmpp)
 SET(static_lib csmpp-static)
-SET(csmpp_sources csmpp.cpp)
+SET(csmpp_sources csmpp.cpp csmpp.h)
 
 SET(csmpp_c_flags "")
 SET(csmpp_link_flags  ${CSM_LDFLAGS} )
diff --git a/sm/csmpp/csmpp.cpp b/sm/csmpp/csmpp.cpp
index 848d22c..019a623 100644
--- a/sm/csmpp/csmpp.cpp
+++ b/sm/csmpp/csmpp.cpp
@@ -1,6 +1,61 @@
 #include "csmpp.h"
 
+CanonicalScanMatcher::CanonicalScanMatcher(SMParameters parameters):
+        params(parameters),
+        showDebug(0),
+        recoverFromError(0),
+        matchingAlgorithm(ICP)
+{
 
-void test_function() {
-	
-}
\ No newline at end of file
+}
+
+CanonicalScanMatcher::CanonicalScanMatcher():
+        showDebug(0),
+        recoverFromError(0),
+        matchingAlgorithm(ICP)
+{
+}
+
+CanonicalScanMatcher::~CanonicalScanMatcher()
+{
+}
+
+bool CanonicalScanMatcher::scanMatch(LDP refScan, LDP secondScan)
+{
+
+    params.setLaserRef(refScan);
+    params.setLaserSen(secondScan);
+
+    switch(matchingAlgorithm)
+    {
+    case(ICP):
+        sm_icp(params.getParams(), &matchingResult);
+        break;
+    case(GPM):
+        sm_gpm(params.getParams(), &matchingResult);
+        break;
+    case(HSM):
+        sm_hsm(params.getParams(), &matchingResult);
+        break;
+    default:
+        sm_error("Unknown algorithm to run: %d.\n",matchingAlgorithm);
+        return false;
+    }
+
+    return true;
+}
+
+void CanonicalScanMatcher::setShowDebug(bool state)
+{
+    this->showDebug = state;
+}
+
+void CanonicalScanMatcher::setSMParameters(SMParameters parameters)
+{
+    this->params = parameters;
+}
+
+void CanonicalScanMatcher::setRecoverFromError(bool state)
+{
+    this->recoverFromError = state;
+}
diff --git a/sm/csmpp/csmpp.h b/sm/csmpp/csmpp.h
index 8fe5682..0c2b1ce 100644
--- a/sm/csmpp/csmpp.h
+++ b/sm/csmpp/csmpp.h
@@ -1,2 +1,294 @@
+#ifndef CSM_WRAPPER
+#define CSM_WRAPPER
 
-void test_function();
\ No newline at end of file
+#include <iostream>
+#include <csm/csm_all.h>
+
+class SMParameters
+{
+public:
+    SMParameters()
+    {
+        // Default Values
+        this->smParameters.max_angular_correction_deg          = 90;
+        this->smParameters.max_linear_correction               = 2;
+        this->smParameters.max_iterations                      = 1000;
+        this->smParameters.epsilon_xy                          = 0.0001;
+        this->smParameters.epsilon_theta                       = 0.0001;
+        this->smParameters.max_correspondence_dist             = 2;
+        this->smParameters.sigma                               = 0.01;
+        this->smParameters.use_corr_tricks                     = 1;
+        this->smParameters.restart                             = 1;
+        this->smParameters.restart_threshold_mean_error        = 0.01;
+        this->smParameters.restart_dt                          = 0.01;
+        this->smParameters.restart_dtheta                      = 0.0261799;
+        this->smParameters.clustering_threshold                = 0.05;
+        this->smParameters.orientation_neighbourhood           = 3;
+        this->smParameters.use_point_to_line_distance          = 1;
+        this->smParameters.do_alpha_test                       = 0;
+        this->smParameters.do_alpha_test_thresholdDeg          = 20;
+        this->smParameters.outliers_maxPerc                    = 0.95;
+        this->smParameters.outliers_adaptive_order             = 0.7;
+        this->smParameters.outliers_adaptive_mult              = 2;
+        this->smParameters.do_visibility_test                  = 0;
+        this->smParameters.outliers_remove_doubles             = 1;
+        this->smParameters.do_compute_covariance               = 0;
+        this->smParameters.debug_verify_tricks                 = 0;
+        this->smParameters.gpm_theta_bin_size_deg              = 5;
+        this->smParameters.gpm_extend_range_deg                = 15;
+        this->smParameters.gpm_interval                        = 1;
+        this->smParameters.min_reading                         = 0;
+        this->smParameters.max_reading                         = 1000;
+        this->smParameters.use_ml_weights                      = 0;
+        this->smParameters.use_sigma_weights                   = 0;
+        this->smParameters.hsm.linear_cell_size                = 0.03;
+        this->smParameters.hsm.angular_cell_size_deg           = 1;
+        this->smParameters.hsm.num_angular_hypotheses          = 8;
+        this->smParameters.hsm.xc_directions_min_distance_deg  = 10;
+        this->smParameters.hsm.xc_ndirections                  = 3;
+        this->smParameters.hsm.angular_hyp_min_distance_deg    = 10;
+        this->smParameters.hsm.linear_xc_max_npeaks            = 5;
+        this->smParameters.hsm.linear_xc_peaks_min_distance    = 5;
+    }
+    ~SMParameters(){}
+    sm_params * getParams()
+    {
+        return & this->smParameters;
+    }
+
+    /*! Where to start */
+    void setfirstGuess(double firtGuess[3])
+    {
+        this->smParameters.first_guess[0]  = firtGuess[0];
+        this->smParameters.first_guess[1]  = firtGuess[1];
+        this->smParameters.first_guess[2]  = firtGuess[2];
+    }
+
+    void setLaserRef(LDP laserRef)
+    {
+        this->smParameters.laser_ref = laserRef;
+    }
+
+    void setLaserSen(LDP laserSen)
+    {
+        this->smParameters.laser_sens = laserSen;
+    }
+
+    /*! Maximum angular displacement between scans (deg)*/
+    void setMaxAngularCorrectionDeg(double maxAng)
+    {
+        this->smParameters.max_angular_correction_deg  = maxAng;
+    }
+    /*! Maximum translation between scans (m) */
+    void setMaxLinearCorrection(double maxLinear)
+    {
+        this->smParameters.max_linear_correction  = maxLinear ;
+    }
+    /*! When to stop */
+    void setMaxIterations(int maxIt)
+    {
+        this->smParameters.max_iterations  = maxIt;
+    }
+    /*! A threshold for stopping. */
+    void setEpsilonXY(double eps)
+    {
+        this->smParameters.epsilon_xy  = eps;
+    }
+    /*! A threshold for stopping. */
+    void setEpsilonTheta(double epsTheta)
+    {
+        this->smParameters.epsilon_theta  = epsTheta;
+    }
+    /*! Maximum distance for a correspondence to be valid */
+    void setMaxCorrespondenceDist(double maxCorrDist)
+    {
+        this->smParameters.max_correspondence_dist  = maxCorrDist ;
+    }
+    /*! Use smart tricks for finding correspondences. Only influences speed; not convergence. */
+    void setUseCorrTricks(bool useTricks)
+    {
+        this->smParameters.use_corr_tricks  = useTricks;
+    }
+    /*! Restart if error under threshold (0 or 1)*/
+    void setRestart(bool restart)
+    {
+        this->smParameters.restart  = restart;
+    }
+    /*! Threshold for restarting */
+    void setRestartThresholdMeanError(double restartME)
+    {
+        this->smParameters.restart_threshold_mean_error  = restartME;
+    }
+    /*! Displacement for restarting */
+    void setRestartDT(double restardt)
+    {
+        this->smParameters.restart_dt  = restardt;
+    }
+    /*! Displacement for restarting */
+    void setRestartDTheta(double restartdtTheta)
+    {
+        this->smParameters.restart_dtheta  = restartdtTheta;
+    }
+    /*
+        Functions concerning discarding correspondences.
+        THESE ARE MAGIC NUMBERS -- and they need to be tuned.
+     */
+    /*!
+        Percentage of correspondences to consider: if 0.9,
+        always discard the top 10% of correspondences with more error
+     */
+    void setOutliersMaxPerc(double maxPerc)
+    {
+        this->smParameters.outliers_maxPerc  = maxPerc;
+    }
+    /*!
+        Parameters describing a simple adaptive algorithm for discarding.
+            1) Order the errors.
+            2) Choose the percentile according to outliers_adaptive_order.
+               (if it is 0.7, get the 70% percentile)
+            3) Define an adaptive threshold multiplying outliers_adaptive_mult
+               with the value of the error at the chosen percentile.
+            4) Discard correspondences over the threshold.
+
+            This is useful to be conservative; yet remove the biggest errors.
+    */
+    void setOutliersAdaptiveOrder(double adaptiveOrder)  /* 0.7 */
+    {
+        this->smParameters.outliers_adaptive_order  = adaptiveOrder;
+    }
+    void setOutliersAdaptiveMult(double adaptiveMult) /* 2 */
+    {
+        this->smParameters.outliers_adaptive_mult  = adaptiveMult;
+    }
+    /*! Do not allow two different correspondences to share a point */
+    void setOutliersRemoveDoubles(bool removeDoubles)
+    {
+        this->smParameters.outliers_remove_doubles  = removeDoubles ;
+    }
+    /* Functions that compute and use point orientation for defining matches. */
+    /*! For now, a very simple max-distance clustering algorithm is used */
+    void setClusteringThreshold(double clusteringThresh)
+    {
+        this->smParameters.clustering_threshold  =  clusteringThresh;
+    }
+    /*! Number of neighbour rays used to estimate the orientation.*/
+    void setOrientationNeighbourhood(int orientationNeighbourhood)
+    {
+        this->smParameters.orientation_neighbourhood  = orientationNeighbourhood ;
+    }
+    /*! Discard correspondences based on the angles */
+    void setDoAlphaTest(bool doAlphaTest)
+    {
+        this->smParameters.do_alpha_test  = doAlphaTest ;
+    }
+    void setDoAlphaTestThresholdDeg(int alphaTestThresh)
+    {
+        this->smParameters.do_alpha_test_thresholdDeg  = alphaTestThresh;
+    }
+    void setDoVisibilityTest(bool doVisibility )
+    {
+        this->smParameters.do_visibility_test  = doVisibility ;
+    }
+    /*! If 1, use PlICP; if 0, use vanilla ICP. */
+    void setUsePoint2LineDistance(bool usePoint2Line)
+    {
+        this->smParameters.use_point_to_line_distance  = usePoint2Line ;
+    }
+    /*!
+        If 1, the field "true_alpha" is used to compute the incidence
+        beta, and the factor (1/cos^2(beta)) used to weight the impact
+        of each correspondence. This works fabolously if doing localization,
+        that is the first scan has no noise.
+        If "true_alpha" is not available, it uses "alpha".
+    */
+    void setUseMLWeights(bool useMLWeights)
+    {
+        this->smParameters.use_ml_weights  = useMLWeights ;
+    }
+    /*! If 1, the field "readings_sigma" is used to weight the correspondence by 1/sigma^2 */
+    void setUseSigmaWeights(bool useSigmaWeights)
+    {
+        this->smParameters.use_sigma_weights  = useSigmaWeights ;
+    }
+    /*!
+        Use the method in http://purl.org/censi/2006/icpcov to compute
+        the matching covariance.
+    */
+    void setDoComputeCovariance(bool computeCovariance)
+    {
+        this->smParameters.do_compute_covariance  = computeCovariance ;
+    }
+    /*! Checks that find_correspondences_tricks give the right answer */
+    void setDebugVerifyTricks(bool debugVerifyTricks)
+    {
+        this->smParameters.debug_verify_tricks  = debugVerifyTricks ;
+    }
+    /*! Pose of sensor with respect to robot: used for computing
+        the first estimate given the odometry. */
+    void setLaserPose(double laserPose[3])
+    {
+        this->smParameters.laser[0]  = laserPose[0] ;
+        this->smParameters.laser[1]  = laserPose[1] ;
+        this->smParameters.laser[2]  = laserPose[2] ;
+    }
+    /*! Noise in the scan */
+    void setSigma(double sigma)
+    {
+        this->smParameters.sigma  = sigma;
+    }
+    /*! mark as invalid ( = don't use ) rays outside of this interval */
+    void setMinReading(double minReading)
+    {
+        this->smParameters.min_reading  = minReading;
+    }
+    void setMaxReading(double maxReading)
+    {
+        this->smParameters.max_reading  = maxReading;
+    }
+    /*! Parameters specific to GPM (unfinished :-/ ) */
+    void setGpmThetaBinSizeDeg(double gpmBinSize)
+    {
+        this->smParameters.gpm_theta_bin_size_deg  = gpmBinSize ;
+    }
+    void setGpmExtendRangeDeg(double extendRange)
+    {
+        this->smParameters.gpm_extend_range_deg  = extendRange ;
+    }
+    void setGpmInterval(int interval)
+    {
+        this->smParameters.gpm_interval  =  interval;
+    }
+    /*! Parameter specific to HSM (unfinished :-/ ) */
+    void setHSM (struct hsm_params  hsmParams)
+    {
+        this->smParameters.hsm  = hsmParams;
+    }
+private:
+    sm_params smParameters;
+};
+
+class CanonicalScanMatcher
+{
+    enum MatchingAlgorithims
+    {
+        ICP,
+        GPM,
+        HSM
+    };
+public:
+    CanonicalScanMatcher();
+    CanonicalScanMatcher(SMParameters parameters);
+    ~CanonicalScanMatcher();
+    void setSMParameters(SMParameters parameters);
+    void setShowDebug(bool);
+    void setRecoverFromError(bool);
+    bool scanMatch(LDP refScan, LDP secondScan);
+private:
+    int matchingAlgorithm;
+    bool showDebug;
+    bool recoverFromError;
+    SMParameters params;
+    sm_result matchingResult;
+};
+
+#endif
-- 
GitLab