From f85169463ac8b0f006191aeebcb8705a3dc066dd Mon Sep 17 00:00:00 2001
From: PepMS <jmarti@iri.upc.edu>
Date: Thu, 30 May 2019 15:05:39 +0200
Subject: [PATCH] WIP
---
src/icp.cpp | 52 ++++++++++++++++++++++++++--------------------------
1 file changed, 26 insertions(+), 26 deletions(-)
diff --git a/src/icp.cpp b/src/icp.cpp
index 81e5d47..a7c938e 100644
--- a/src/icp.cpp
+++ b/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];
--
GitLab