Skip to content
Snippets Groups Projects
Commit 1c35a4de authored by Idril-Tadzio Geer Cousté's avatar Idril-Tadzio Geer Cousté
Browse files

coldfix, csm has a hardcoded max distance of 100

parent d555f0a8
No related branches found
No related tags found
No related merge requests found
......@@ -17,8 +17,8 @@ public:
for(auto it : scan.ranges_raw_){
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;
if(scan_params.range_min_ <= it and it <= scan_params.range_max_ and 0 <= it and it <= 100){
laser_data->readings[i] = it;//*100/scan_params.range_max_;
laser_data->valid[i] = 1;
}else{
laser_data->readings[i] = NAN;
......@@ -158,9 +158,9 @@ icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, con
result.error = csm_output.error;
if (result.valid == 1) {
result.res_transf(0) = csm_output.x[0];
result.res_transf(1) = csm_output.x[1];
result.res_transf(2) = csm_output.x[2];
result.res_transf(0) = csm_output.x[0];//*_current_scan_params.range_max_/100;
result.res_transf(1) = csm_output.x[1];//*_current_scan_params.range_max_/100;
result.res_transf(2) = csm_output.x[2];//*_current_scan_params.range_max_/100;
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j)
......@@ -168,7 +168,7 @@ icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, con
// gsl_matrix_get(csm_output.cov_x_m, i, j); // NOT
// COMPILING
csm_output.cov_x_m
->data[i * csm_output.cov_x_m->tda + j]; // This does the same
->data[i * csm_output.cov_x_m->tda + j];//*_current_scan_params.range_max_/100*_current_scan_params.range_max_/100; // This does the same
} else {
std::cout << "ICP valid != 1, providing first guess transformation and "
"identity covariance\n";
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment