diff --git a/src/icp.cpp b/src/icp.cpp index aa43c13911d56725e6bd1c40d75c73104e62830e..8c68af7039771ebc5ba70b91f31d015dd5efba6e 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -67,8 +67,8 @@ icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, con sm_params csm_input{}; sm_result csm_output{}; - csm_input.min_reading = max(_current_scan_params.range_min_, _ref_scan_params.range_min_); - csm_input.max_reading = min(_current_scan_params.range_max_, _ref_scan_params.range_max_); + csm_input.min_reading = fmax(_current_scan_params.range_min_, _ref_scan_params.range_min_); + csm_input.max_reading = fmin(_current_scan_params.range_max_, _ref_scan_params.range_max_); csm_input.sigma = _ref_scan_params.range_std_dev_; csm_input.laser_ref = laser_scan_ref.laser_data;