Add extra check to checkExpectedBeamThroughEntity to fix segfault (#61)

Author: MatthijsBurgh

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<double> &model_ranges, ed::EntityConstPtr entity,
                                         const geo::Pose3D &sensor_pose_xya, const int expected_center_beam) const;

ed_sensor_integration/src/kinect/fitter.cpp

@@ -377,6 +377,8 @@ std::unique_ptr<OptimalFit> Fitter::findOptimum(const EstimationInputData& input
     }
     if (valid_optimum)
         return current_optimum;
+
+    ROS_ERROR_NAMED("fitter", "optimum is invalid");
     std::unique_ptr<OptimalFit> invalid_optimum(new OptimalFit);
     return invalid_optimum;
 }
@@ -462,7 +464,8 @@ void Fitter::checkExpectedBeamThroughEntity(const std::vector<double>& model_ran
     expected_ranges = model_ranges;
     std::vector<int> 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_)
+        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");
 }