Postpone image offset error norm root computation

libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_beam.h

@@ -85,6 +85,8 @@ class ContinuousBeam : public MeasurementModelBase {
   const Image<Vector2D>::ConstPtr beam_offset_image_;
 
   const FloatingPoint angle_threshold_ = 6.f * config_.angle_sigma;
+  const FloatingPoint angle_threshold_squared =
+      angle_threshold_ * angle_threshold_;
   const FloatingPoint range_threshold_front = 3.f * config_.range_sigma;
   const FloatingPoint range_threshold_back_ = 6.f * config_.range_sigma;
   // NOTE: The angle and upper range thresholds have a width of 6 sigmas because
@@ -93,9 +95,10 @@ class ContinuousBeam : public MeasurementModelBase {
   //       sigma.
 
   // Compute the measurement update for a single beam
-  FloatingPoint computeBeamUpdate(FloatingPoint cell_to_sensor_distance,
-                                  FloatingPoint cell_to_beam_image_error_norm,
-                                  FloatingPoint measured_distance) const;
+  FloatingPoint computeBeamUpdate(
+      FloatingPoint cell_to_sensor_distance,
+      FloatingPoint cell_to_beam_image_error_norm_squared,
+      FloatingPoint measured_distance) const;
 };
 }  // namespace wavemap
 

libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_ray.h

@@ -77,7 +77,6 @@ class ContinuousRay : public MeasurementModelBase {
   // NOTE: The upper range thresholds has a width of 6 sigmas because the
   //       assumed 'ground truth' surface thickness is 3 sigma, and the range
   //       uncertainty extends the non-zero regions with another 3 sigma.
-
   FloatingPoint computeBeamUpdate(const SensorCoordinates& sensor_coordinates,
                                   const Index2D& image_index) const;
 

libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_beam_inl.h

@@ -48,13 +48,13 @@ inline FloatingPoint ContinuousBeam::computeUpdate(
           beam_offset_image_->at(image_index) - cell_offset;
 
       // Compute the image error norm
-      const FloatingPoint cell_to_beam_image_error_norm =
-          projection_model_->imageOffsetToErrorNorm(sensor_coordinates.image,
-                                                    cell_to_beam_offset);
+      const FloatingPoint cell_to_beam_image_error_norm_squared =
+          projection_model_->imageOffsetToErrorSquaredNorm(
+              sensor_coordinates.image, cell_to_beam_offset);
 
       // Compute the update
       return computeBeamUpdate(cell_to_sensor_distance,
-                               cell_to_beam_image_error_norm,
+                               cell_to_beam_image_error_norm_squared,
                                measured_distance);
     }
 
@@ -77,16 +77,16 @@ inline FloatingPoint ContinuousBeam::computeUpdate(
       }
 
       // Compute the image error norms
-      const auto cell_to_beam_image_error_norms =
-          projection_model_->imageOffsetsToErrorNorms(sensor_coordinates.image,
-                                                      cell_to_beam_offsets);
+      const auto cell_to_beam_image_error_norms_sq =
+          projection_model_->imageOffsetsToErrorSquaredNorms(
+              sensor_coordinates.image, cell_to_beam_offsets);
 
       // Compute the update
       FloatingPoint update = 0.f;
       for (int neighbor_idx = 0; neighbor_idx < 4; ++neighbor_idx) {
         update +=
             computeBeamUpdate(cell_to_sensor_distance,
-                              cell_to_beam_image_error_norms[neighbor_idx],
+                              cell_to_beam_image_error_norms_sq[neighbor_idx],
                               measured_distances[neighbor_idx]);
       }
       return update;
@@ -99,10 +99,10 @@ inline FloatingPoint ContinuousBeam::computeUpdate(
 
 inline FloatingPoint ContinuousBeam::computeBeamUpdate(
     FloatingPoint cell_to_sensor_distance,
-    FloatingPoint cell_to_beam_image_error_norm,
+    FloatingPoint cell_to_beam_image_error_norm_squared,
     FloatingPoint measured_distance) const {
   const bool fully_in_unknown_space =
-      angle_threshold_ < cell_to_beam_image_error_norm ||
+      angle_threshold_squared < cell_to_beam_image_error_norm_squared ||
       measured_distance + range_threshold_back_ < cell_to_sensor_distance;
   if (fully_in_unknown_space) {
     return 0.f;
@@ -120,7 +120,8 @@ inline FloatingPoint ContinuousBeam::computeBeamUpdate(
         0.5f * ApproximateGaussianDistribution::cumulative(f - 3.f) - 0.5f;
   }
 
-  const FloatingPoint g = cell_to_beam_image_error_norm / config_.angle_sigma;
+  const FloatingPoint g =
+      std::sqrt(cell_to_beam_image_error_norm_squared) / config_.angle_sigma;
   const FloatingPoint angle_contrib =
       ApproximateGaussianDistribution::cumulative(g + 3.f) -
       ApproximateGaussianDistribution::cumulative(g - 3.f);

libraries/wavemap/include/wavemap/integrator/projection_model/impl/ouster_projector_inl.h

@@ -32,28 +32,28 @@ inline Point3D OusterProjector::sensorToCartesian(
   return C_point;
 }
 
-inline FloatingPoint OusterProjector::imageOffsetToErrorNorm(
+inline FloatingPoint OusterProjector::imageOffsetToErrorSquaredNorm(
     const ImageCoordinates& linearization_point, const Vector2D& offset) const {
   // Scale the azimuth offset by the cosine of the elevation angle to account
   // for the change in density along the azimuth axis in function of elevation
   const FloatingPoint cos_elevation_angle = std::cos(linearization_point[0]);
-  return std::sqrt(offset[0] * offset[0] +
-                   (cos_elevation_angle * cos_elevation_angle) *
-                       (offset[1] * offset[1]));
+  return offset[0] * offset[0] +
+         (cos_elevation_angle * cos_elevation_angle) * (offset[1] * offset[1]);
 }
 
-inline std::array<FloatingPoint, 4> OusterProjector::imageOffsetsToErrorNorms(
+inline std::array<FloatingPoint, 4>
+OusterProjector::imageOffsetsToErrorSquaredNorms(
     const ImageCoordinates& linearization_point,
-    const ProjectorBase::CellToBeamOffsetArray& offsets) const {
+    const CellToBeamOffsetArray& offsets) const {
   const FloatingPoint cos_elevation_angle = std::cos(linearization_point[0]);
   const FloatingPoint cos_elevation_angle_sq =
       cos_elevation_angle * cos_elevation_angle;
   std::array<FloatingPoint, 4> error_norms{};
   for (int offset_idx = 0; offset_idx < 4; ++offset_idx) {
     error_norms[offset_idx] =
-        std::sqrt((offsets[offset_idx][0] * offsets[offset_idx][0]) +
-                  cos_elevation_angle_sq *
-                      (offsets[offset_idx][1] * offsets[offset_idx][1]));
+        (offsets[offset_idx][0] * offsets[offset_idx][0]) +
+        cos_elevation_angle_sq *
+            (offsets[offset_idx][1] * offsets[offset_idx][1]);
   }
   return error_norms;
 }

libraries/wavemap/include/wavemap/integrator/projection_model/impl/pinhole_camera_projector_inl.h

@@ -23,21 +23,20 @@ inline Point3D PinholeCameraProjector::sensorToCartesian(
   return C_point;
 }
 
-inline FloatingPoint PinholeCameraProjector::imageOffsetToErrorNorm(
+inline FloatingPoint PinholeCameraProjector::imageOffsetToErrorSquaredNorm(
     const ImageCoordinates& /*linearization_point*/,
     const Vector2D& offset) const {
-  return offset.norm();
+  return offset.squaredNorm();
 }
 
 inline std::array<FloatingPoint, 4>
-PinholeCameraProjector::imageOffsetsToErrorNorms(
+PinholeCameraProjector::imageOffsetsToErrorSquaredNorms(
     const ImageCoordinates& /*linearization_point*/,
-    const ProjectorBase::CellToBeamOffsetArray& offsets) const {
+    const CellToBeamOffsetArray& offsets) const {
   std::array<FloatingPoint, 4> error_norms{};
   for (int offset_idx = 0; offset_idx < 4; ++offset_idx) {
-    error_norms[offset_idx] =
-        std::sqrt(offsets[offset_idx][0] * offsets[offset_idx][0] +
-                  offsets[offset_idx][1] * offsets[offset_idx][1]);
+    error_norms[offset_idx] = offsets[offset_idx][0] * offsets[offset_idx][0] +
+                              offsets[offset_idx][1] * offsets[offset_idx][1];
   }
   return error_norms;
 }

libraries/wavemap/include/wavemap/integrator/projection_model/impl/projector_base_inl.h

@@ -85,6 +85,22 @@ inline ImageCoordinates ProjectorBase::indexToImage(
          image_offset_;
 }
 
+inline FloatingPoint ProjectorBase::imageOffsetToErrorNorm(
+    const ImageCoordinates& linearization_point, const Vector2D& offset) const {
+  return std::sqrt(imageOffsetToErrorSquaredNorm(linearization_point, offset));
+}
+
+inline std::array<FloatingPoint, 4> ProjectorBase::imageOffsetsToErrorNorms(
+    const ImageCoordinates& linearization_point,
+    const ProjectorBase::CellToBeamOffsetArray& offsets) const {
+  auto error_norms =
+      imageOffsetsToErrorSquaredNorms(linearization_point, offsets);
+  for (auto& error_norm : error_norms) {
+    error_norm = std::sqrt(error_norm);
+  }
+  return error_norms;
+}
+
 inline Vector2D ProjectorBase::imageToIndexReal(
     const ImageCoordinates& image_coordinates) const {
   return (image_coordinates - image_offset_)

libraries/wavemap/include/wavemap/integrator/projection_model/impl/spherical_projector_inl.h

@@ -22,29 +22,28 @@ inline Point3D SphericalProjector::sensorToCartesian(
   return range * bearing;
 }
 
-inline FloatingPoint SphericalProjector::imageOffsetToErrorNorm(
+inline FloatingPoint SphericalProjector::imageOffsetToErrorSquaredNorm(
     const ImageCoordinates& linearization_point, const Vector2D& offset) const {
   // Scale the azimuth offset by the cosine of the elevation angle to account
   // for the change in density along the azimuth axis in function of elevation
   const FloatingPoint cos_elevation_angle = std::cos(linearization_point[0]);
-  return std::sqrt(offset[0] * offset[0] +
-                   (cos_elevation_angle * cos_elevation_angle) *
-                       (offset[1] * offset[1]));
+  return offset[0] * offset[0] +
+         (cos_elevation_angle * cos_elevation_angle) * (offset[1] * offset[1]);
 }
 
 inline std::array<FloatingPoint, 4>
-SphericalProjector::imageOffsetsToErrorNorms(
+SphericalProjector::imageOffsetsToErrorSquaredNorms(
     const ImageCoordinates& linearization_point,
-    const ProjectorBase::CellToBeamOffsetArray& offsets) const {
+    const CellToBeamOffsetArray& offsets) const {
   const FloatingPoint cos_elevation_angle = std::cos(linearization_point[0]);
   const FloatingPoint cos_elevation_angle_sq =
       cos_elevation_angle * cos_elevation_angle;
   std::array<FloatingPoint, 4> error_norms{};
   for (int offset_idx = 0; offset_idx < 4; ++offset_idx) {
     error_norms[offset_idx] =
-        std::sqrt((offsets[offset_idx][0] * offsets[offset_idx][0]) +
-                  cos_elevation_angle_sq *
-                      (offsets[offset_idx][1] * offsets[offset_idx][1]));
+        (offsets[offset_idx][0] * offsets[offset_idx][0]) +
+        cos_elevation_angle_sq *
+            (offsets[offset_idx][1] * offsets[offset_idx][1]);
   }
   return error_norms;
 }

libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h

@@ -60,10 +60,10 @@ class OusterProjector : public ProjectorBase {
   // Coordinate transforms between Cartesian and sensor space
   SensorCoordinates cartesianToSensor(const Point3D& C_point) const final;
   Point3D sensorToCartesian(const SensorCoordinates& coordinates) const final;
-  FloatingPoint imageOffsetToErrorNorm(
+  FloatingPoint imageOffsetToErrorSquaredNorm(
       const ImageCoordinates& linearization_point,
       const Vector2D& offset) const final;
-  std::array<FloatingPoint, 4> imageOffsetsToErrorNorms(
+  std::array<FloatingPoint, 4> imageOffsetsToErrorSquaredNorms(
       const ImageCoordinates& linearization_point,
       const CellToBeamOffsetArray& offsets) const final;
 

libraries/wavemap/include/wavemap/integrator/projection_model/pinhole_camera_projector.h

@@ -58,10 +58,10 @@ class PinholeCameraProjector : public ProjectorBase {
   // Coordinate transforms between Cartesian and sensor space
   SensorCoordinates cartesianToSensor(const Point3D& C_point) const final;
   Point3D sensorToCartesian(const SensorCoordinates& coordinates) const final;
-  FloatingPoint imageOffsetToErrorNorm(
+  FloatingPoint imageOffsetToErrorSquaredNorm(
       const ImageCoordinates& /*linearization_point*/,
       const Vector2D& offset) const final;
-  std::array<FloatingPoint, 4> imageOffsetsToErrorNorms(
+  std::array<FloatingPoint, 4> imageOffsetsToErrorSquaredNorms(
       const ImageCoordinates& /*linearization_point*/,
       const CellToBeamOffsetArray& offsets) const final;
 

libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h

@@ -86,11 +86,17 @@ class ProjectorBase {
   // NOTE: For spherical models, this error norm corresponds to the relative
   //       angle between the two rays whose offset is given. For camera models,
   //       it corresponds to the reprojection error in pixels.
-  virtual FloatingPoint imageOffsetToErrorNorm(
+  FloatingPoint imageOffsetToErrorNorm(
+      const ImageCoordinates& linearization_point,
+      const Vector2D& offset) const;
+  virtual FloatingPoint imageOffsetToErrorSquaredNorm(
       const ImageCoordinates& linearization_point,
       const Vector2D& offset) const = 0;
   using CellToBeamOffsetArray = std::array<Vector2D, 4>;
-  virtual std::array<FloatingPoint, 4> imageOffsetsToErrorNorms(
+  std::array<FloatingPoint, 4> imageOffsetsToErrorNorms(
+      const ImageCoordinates& linearization_point,
+      const CellToBeamOffsetArray& offsets) const;
+  virtual std::array<FloatingPoint, 4> imageOffsetsToErrorSquaredNorms(
       const ImageCoordinates& linearization_point,
       const CellToBeamOffsetArray& offsets) const = 0;
 

libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h

@@ -46,10 +46,10 @@ class SphericalProjector : public ProjectorBase {
   // Coordinate transforms between Cartesian and sensor space
   SensorCoordinates cartesianToSensor(const Point3D& C_point) const final;
   Point3D sensorToCartesian(const SensorCoordinates& coordinates) const final;
-  FloatingPoint imageOffsetToErrorNorm(
+  FloatingPoint imageOffsetToErrorSquaredNorm(
       const ImageCoordinates& linearization_point,
       const Vector2D& offset) const final;
-  std::array<FloatingPoint, 4> imageOffsetsToErrorNorms(
+  std::array<FloatingPoint, 4> imageOffsetsToErrorSquaredNorms(
       const ImageCoordinates& linearization_point,
       const CellToBeamOffsetArray& offsets) const final;