From 6a9af315cfaabb4d232fbfc2667383e78a38f345 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Wed, 13 Apr 2022 15:00:28 +0200 Subject: [PATCH 1/3] add check for within bounds --- ed_sensor_integration/include/ed/kinect/fitter.h | 4 ++-- ed_sensor_integration/src/kinect/fitter.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ed_sensor_integration/include/ed/kinect/fitter.h b/ed_sensor_integration/include/ed/kinect/fitter.h index e93cbac9..f53f0785 100644 --- a/ed_sensor_integration/include/ed/kinect/fitter.h +++ b/ed_sensor_integration/include/ed/kinect/fitter.h @@ -233,11 +233,11 @@ class Fitter /** * @brief checkExpectedBeamThroughEntity checks if the expected center beam passes through the entity. If - * not: something went wrong + * not: throw a fitter error * @param model_ranges * @param entity * @param sensor_pose_xya - * @param expected_center_beam + * @param expected_center_beam expected index of the beam through the center of the object. range: any int. indices outside bounds will also throw an error. */ void checkExpectedBeamThroughEntity(const std::vector &model_ranges, ed::EntityConstPtr entity, const geo::Pose3D &sensor_pose_xya, const int expected_center_beam) const; diff --git a/ed_sensor_integration/src/kinect/fitter.cpp b/ed_sensor_integration/src/kinect/fitter.cpp index 0f5dadf5..04d30228 100644 --- a/ed_sensor_integration/src/kinect/fitter.cpp +++ b/ed_sensor_integration/src/kinect/fitter.cpp @@ -463,7 +463,7 @@ void Fitter::checkExpectedBeamThroughEntity(const std::vector& model_ran std::vector expected_identifiers(nr_data_points_, 0); renderEntity(entity, sensor_pose_xya, 1, expected_ranges, expected_identifiers); - if (expected_identifiers[expected_center_beam] != 1) // expected center beam MUST contain the rendered model + if (expected_center_beam < 0 || expected_center_beam >= nr_data_points_ || expected_identifiers[expected_center_beam] != 1) // expected center beam MUST contain the rendered model throw FitterError("Expected beam does not go through entity"); } From 7b82885ee9d04ef93fcac1f2769129d2030ffbd1 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Wed, 13 Apr 2022 15:05:48 +0200 Subject: [PATCH 2/3] separate range check --- ed_sensor_integration/src/kinect/fitter.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ed_sensor_integration/src/kinect/fitter.cpp b/ed_sensor_integration/src/kinect/fitter.cpp index 04d30228..3aa6892d 100644 --- a/ed_sensor_integration/src/kinect/fitter.cpp +++ b/ed_sensor_integration/src/kinect/fitter.cpp @@ -462,8 +462,9 @@ void Fitter::checkExpectedBeamThroughEntity(const std::vector& model_ran expected_ranges = model_ranges; std::vector expected_identifiers(nr_data_points_, 0); renderEntity(entity, sensor_pose_xya, 1, expected_ranges, expected_identifiers); - - if (expected_center_beam < 0 || expected_center_beam >= nr_data_points_ || expected_identifiers[expected_center_beam] != 1) // expected center beam MUST contain the rendered model + if (expected_center_beam < 0 || expected_center_beam >= nr_data_points_) + throw FitterError("Expected beam outside of measurement range(" + std::to_string(nr_data_points_) + "), index: " + std::to_string(expected_center_beam)); + if (expected_identifiers[expected_center_beam] != 1) // expected center beam MUST contain the rendered model throw FitterError("Expected beam does not go through entity"); } From 7af2446bcb431e46c8ccb59c8e3c185bf44c5418 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 13 Apr 2022 17:15:29 +0200 Subject: [PATCH 3/3] [TEMP] Print when optimum is invalid --- ed_sensor_integration/src/kinect/fitter.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ed_sensor_integration/src/kinect/fitter.cpp b/ed_sensor_integration/src/kinect/fitter.cpp index 3aa6892d..d27ed327 100644 --- a/ed_sensor_integration/src/kinect/fitter.cpp +++ b/ed_sensor_integration/src/kinect/fitter.cpp @@ -377,6 +377,8 @@ std::unique_ptr Fitter::findOptimum(const EstimationInputData& input } if (valid_optimum) return current_optimum; + + ROS_ERROR_NAMED("fitter", "optimum is invalid"); std::unique_ptr invalid_optimum(new OptimalFit); return invalid_optimum; }