- attempt to replace ICP with CSM, CSM is still giving weird output, very different from ICP.

sm/csm/laser_data.c

@@ -206,8 +206,8 @@ int ld_valid_fields(LDP ld)  {
 		return 0;
 	}
 	if(fabs(ld->max_theta - ld->theta[ld->nrays-1]) > 1e-8) {
-		sm_error("Min_theta (%f) should be theta[0] (%f)\n",
-			ld->max_theta, ld->theta[ld->nrays-1]);
+                sm_error("Max_theta (%f) should be theta[%d] (%f)\n",
+                        ld->max_theta,ld->nrays-1, ld->theta[ld->nrays-1]);
 		return 0;
 	}
 	/* Check that there are valid rays */

sm/csmpp/csmpp.cpp

@@ -20,11 +20,17 @@ CanonicalScanMatcher::~CanonicalScanMatcher()
 {
 }
 
-bool CanonicalScanMatcher::scanMatch(LDP refScan, LDP secondScan)
+sm_result CanonicalScanMatcher::scanMatch(LDP refScan, LDP laserScan)
 {
 
     params.setLaserRef(refScan);
-    params.setLaserSen(secondScan);
+    params.setLaserSen(laserScan);
+    double odometry[3];
+    double ominus_laser[3], temp[3];    
+    pose_diff_d(laserScan->odometry, refScan->odometry, odometry);
+    ominus_d(params.getParams()->laser, ominus_laser);
+    oplus_d(ominus_laser, odometry, temp);
+    oplus_d(temp, params.getParams()->laser, params.getParams()->first_guess);
 
     switch(matchingAlgorithm)
     {
@@ -39,10 +45,10 @@ bool CanonicalScanMatcher::scanMatch(LDP refScan, LDP secondScan)
         break;
     default:
         sm_error("Unknown algorithm to run: %d.\n",matchingAlgorithm);
-        return false;
+        return matchingResult;
     }
-
-    return true;
+    oplus_d(refScan->estimate, matchingResult.x, laserScan->estimate);
+    return matchingResult;
 }
 
 void CanonicalScanMatcher::setShowDebug(bool state)

sm/csmpp/csmpp.h

@@ -282,7 +282,7 @@ public:
     void setSMParameters(SMParameters parameters);
     void setShowDebug(bool);
     void setRecoverFromError(bool);
-    bool scanMatch(LDP refScan, LDP secondScan);
+    sm_result scanMatch(LDP refScan, LDP laserScan);
 private:
     int matchingAlgorithm;
     bool showDebug;

sm/drivers/playerstage/mricp/CMakeLists.txt

@@ -49,6 +49,8 @@ TARGET_LINK_LIBRARIES(mricp
   ${GTK_LIBRARIES}
   ${GDKPIXBUF_LIBRARIES}
   ${CSM_LIBRARY_DIRS}
+  /home/blackcoder/workspace/csm-git/sm/csmpp/libcsmpp.so
+  /home/blackcoder/workspace/csm-git/sm/libcsm.so
 )
 
 INCLUDE_DIRECTORIES(

sm/drivers/playerstage/mricp/stage_test/stage_mricp.cfg

@@ -17,8 +17,8 @@ driver
 
 driver
 (
-  name "mricp"
-  plugin "libmricp"
+  name "MRSM"
+  plugin "libmricp.so"
   provides ["position2d:1" "map:0"]
   requires ["position2d:0" "laser:0"]
   number_of_lasers 1