Resolve "icp: develop matching tools"

Closes #23 (closed)

src/icp.cpp

@@ -9,16 +9,13 @@ public:
     {
         int num_rays = scan.ranges_raw_.size();
         laser_data = ld_alloc_new(num_rays);
-        laser_data->nrays = num_rays;
-        laser_data->min_theta = 0;
-        laser_data->max_theta = scan_params.angle_max_;
-        double delta_theta = (laser_data->max_theta - laser_data->min_theta)/num_rays;
+
         int i = 0;
         for(auto it : scan.ranges_raw_){
-            laser_data->theta[i] = laser_data->min_theta + i*delta_theta;
+            laser_data->theta[i] = scan_params.angle_min_ + i*scan_params.angle_step_;
+
             if(scan_params.range_min_ <= it and it <= scan_params.range_max_){
                 laser_data->readings[i] = it;
-                // std::cout << "Current Reading " << i << " " << it << std::endl;
                 laser_data->valid[i] = 1;
             }else{
                 laser_data->readings[i] = -1;
@@ -28,13 +25,16 @@ public:
             ++i;
         }
 
-        // 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;
+        laser_data->min_theta = laser_data->theta[0];
+        laser_data->max_theta = laser_data->theta[num_rays-1];
+
+        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;
     }
     ~LDWrapper(){
         ld_free(laser_data);
@@ -53,30 +53,30 @@ ICP::~ICP()
 
 icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanParams& params, Eigen::Vector3s &_last_transf)
 {
-    LDWrapper last = LDWrapper(_last_ls, params);
-    // last->odometry[0] = _last_transf(0);
-    // last->odometry[1] = _last_transf(1);
-    // last->odometry[2] = _last_transf(2);
+    sm_debug_write(true);
+
+    LDWrapper last   = LDWrapper(_last_ls, params);
     LDWrapper origin = LDWrapper(_origin_ls, params);
+
     int num_rays = _last_ls.ranges_raw_.size();
+
     sm_params csm_input{};
     sm_result csm_output{};
 
-    csm_input.laser_ref   = origin.laser_data;
-    csm_input.laser_sens  = last.laser_data;
+    csm_input.min_reading = params.range_min_;
+    csm_input.max_reading = params.range_max_;
+
+    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];
+    csm_input.use_point_to_line_distance = true;
 
     sm_icp(&csm_input, &csm_output);
-    std::cout << "Result: " << csm_output.valid << '\n';
+    // 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];