From 268d516d1fd27450273a14217083273c9ee023a7 Mon Sep 17 00:00:00 2001
From: PepMS <jmarti@iri.upc.edu>
Date: Thu, 30 May 2019 09:58:45 +0200
Subject: [PATCH] Minor Changes
---
src/icp.cpp | 29 +++++++++++++++++++----------
1 file changed, 19 insertions(+), 10 deletions(-)
diff --git a/src/icp.cpp b/src/icp.cpp
index 4b4f0f4..81e5d47 100644
--- a/src/icp.cpp
+++ b/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;
}
--
GitLab