Minor Changes

src/icp.cpp

@@ -28,13 +28,13 @@ public:
             ++i;
         }
 
-        laser_data->odometry[0] = 0.0;
-        laser_data->odometry[1] = 0.0;
-        laser_data->odometry[2] = 0.0;
-
-        laser_data->true_pose[0] = 0.0;
-        laser_data->true_pose[1] = 0.0;
-        laser_data->true_pose[2] = 0.0;
+        // laser_data->odometry[0] = 1/0.0;
+        // laser_data->odometry[1] = 1/0.0;
+        // laser_data->odometry[2] = 1/0.0;
+        //
+        // laser_data->true_pose[0] = 1/0.0;
+        // laser_data->true_pose[1] = 1/0.0;
+        // laser_data->true_pose[2] = 1/0.0;
     }
     ~LDWrapper(){
         ld_free(laser_data);
@@ -61,18 +61,27 @@ icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPar
     int num_rays = _last_ls.ranges_raw_.size();
     sm_params csm_input{};
     sm_result csm_output{};
-    csm_input.laser_ref = last.laser_data;
-    csm_input.laser_sens = origin.laser_data;
+
+    csm_input.laser_ref   = origin.laser_data;
+    csm_input.laser_sens  = last.laser_data;
+    csm_input.first_guess[0] = _last_transf(0);
+    csm_input.first_guess[1] = _last_transf(1);
+    csm_input.first_guess[2] = _last_transf(2);
+    csm_input.use_point_to_line_distance = true;
+
     //TODO: min_theta and max_theta should come from LaserScanParams
     last.laser_data->min_theta = last.laser_data->theta[0];
     last.laser_data->max_theta = last.laser_data->theta[num_rays-1];
     origin.laser_data->min_theta = origin.laser_data->theta[0];
     origin.laser_data->max_theta = origin.laser_data->theta[num_rays-1];
+
     sm_icp(&csm_input, &csm_output);
-    std::cout << "My solution " << csm_output.x[0] << "," << csm_output.x[1] << "," << csm_output.x[2] << std::endl;
+    std::cout << "Result: " << csm_output.valid << '\n';
+    // std::cout << "My solution " << csm_output.x[0] << "," << csm_output.x[1] << "," << csm_output.x[2] << std::endl;
     icp_output result{};
     result.res_transf(0) = csm_output.x[0];
     result.res_transf(1) = csm_output.x[1];
     result.res_transf(2) = csm_output.x[2];
+
     return result;
 }