Various minor optimizations

ethz-asl · Sep 29, 2023 · 464fd9c · 464fd9c
1 parent 4e0a172
commit 464fd9c
Show file tree

Hide file tree

Showing 9 changed files with 28 additions and 21 deletions.
diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/ouster_projector_inl.h b/libraries/wavemap/include/wavemap/integrator/projection_model/impl/ouster_projector_inl.h
@@ -46,11 +46,13 @@ inline std::array<FloatingPoint, 4> OusterProjector::imageOffsetsToErrorNorms(
     const Vector2D& linearization_point,
     const ProjectorBase::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 * cos_elevation_angle) *
+                  cos_elevation_angle_sq *
                       (offsets[offset_idx][1] * offsets[offset_idx][1]));
   }
   return error_norms;

diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/projector_base_inl.h b/libraries/wavemap/include/wavemap/integrator/projection_model/impl/projector_base_inl.h
@@ -32,9 +32,10 @@ inline std::pair<Index2D, Vector2D> ProjectorBase::imageToNearestIndexAndOffset(
     const Vector2D& image_coordinates) const {
   const Vector2D index = imageToIndexReal(image_coordinates);
   const Vector2D index_rounded = index.array().round();
-  const Vector2D image_coordinate_offset =
+  Vector2D image_coordinate_offset =
       (index - index_rounded).cwiseProduct(index_to_image_scale_factor_);
-  return {index_rounded.cast<IndexElement>(), image_coordinate_offset};
+  return {index_rounded.cast<IndexElement>(),
+          std::move(image_coordinate_offset)};
 }
 
 inline std::array<Index2D, 4> ProjectorBase::imageToNearestIndices(
@@ -68,11 +69,13 @@ ProjectorBase::imageToNearestIndicesAndOffsets(
         neighbox_idx & 0b01 ? index_upper[0] : index_lower[0],
         neighbox_idx & 0b10 ? index_upper[1] : index_lower[1]};
     indices[neighbox_idx] = index_rounded.cast<IndexElement>();
-    offsets[neighbox_idx] =
-        (index - index_rounded).cwiseProduct(index_to_image_scale_factor_);
+    offsets[neighbox_idx][0] =
+        index_to_image_scale_factor_[0] * (index[0] - index_rounded[0]);
+    offsets[neighbox_idx][1] =
+        index_to_image_scale_factor_[1] * (index[1] - index_rounded[1]);
   }
 
-  return {indices, offsets};
+  return {std::move(indices), std::move(offsets)};
 }
 
 inline Vector2D ProjectorBase::indexToImage(const Index2D& index) const {

diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/spherical_projector_inl.h b/libraries/wavemap/include/wavemap/integrator/projection_model/impl/spherical_projector_inl.h
@@ -35,11 +35,13 @@ SphericalProjector::imageOffsetsToErrorNorms(
     const Vector2D& linearization_point,
     const ProjectorBase::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 * cos_elevation_angle) *
+                  cos_elevation_angle_sq *
                       (offsets[offset_idx][1] * offsets[offset_idx][1]));
   }
   return error_norms;

diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h b/libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h
@@ -51,8 +51,6 @@ class OusterProjector : public ProjectorBase {
 
   explicit OusterProjector(const Config& config);
 
-  IndexElement getNumRows() const final { return config_.elevation.num_cells; }
-  IndexElement getNumColumns() const final { return config_.azimuth.num_cells; }
   Vector2D getMinImageCoordinates() const final {
     return {config_.elevation.min_angle, config_.azimuth.min_angle};
   }

diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/pinhole_camera_projector.h b/libraries/wavemap/include/wavemap/integrator/projection_model/pinhole_camera_projector.h
@@ -46,11 +46,10 @@ class PinholeCameraProjector : public ProjectorBase {
   using Config = PinholeCameraProjectorConfig;
 
   explicit PinholeCameraProjector(const Config& config)
-      : ProjectorBase(Vector2D::Ones(), Vector2D::Zero()),
+      : ProjectorBase({config.width, config.height}, Vector2D::Ones(),
+                      Vector2D::Zero()),
         config_(config.checkValid()) {}
 
-  IndexElement getNumRows() const final { return config_.width; }
-  IndexElement getNumColumns() const final { return config_.height; }
   Vector2D getMinImageCoordinates() const final {
     return indexToImage(Index2D::Zero());
   }

diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h b/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h
@@ -28,14 +28,16 @@ class ProjectorBase {
   using Ptr = std::shared_ptr<ProjectorBase>;
   using ConstPtr = std::shared_ptr<const ProjectorBase>;
 
-  ProjectorBase(Vector2D index_to_image_scale_factor, Vector2D image_offset)
-      : index_to_image_scale_factor_(std::move(index_to_image_scale_factor)),
+  ProjectorBase(Index2D dimensions, Vector2D index_to_image_scale_factor,
+                Vector2D image_offset)
+      : dimensions_(std::move(dimensions)),
+        index_to_image_scale_factor_(std::move(index_to_image_scale_factor)),
         image_offset_(std::move(image_offset)) {}
   virtual ~ProjectorBase() = default;
 
-  virtual IndexElement getNumRows() const = 0;
-  virtual IndexElement getNumColumns() const = 0;
-  Index2D getDimensions() const { return {getNumRows(), getNumColumns()}; }
+  IndexElement getNumRows() const { return dimensions_.x(); }
+  IndexElement getNumColumns() const { return dimensions_.y(); }
+  Index2D getDimensions() const { return dimensions_; }
   virtual Vector2D getMinImageCoordinates() const = 0;
   virtual Vector2D getMaxImageCoordinates() const = 0;
   virtual Eigen::Matrix<bool, 3, 1> sensorAxisIsPeriodic() const = 0;
@@ -90,6 +92,7 @@ class ProjectorBase {
       const Point3D& t_W_C) const = 0;
 
  protected:
+  const Index2D dimensions_;
   const Vector2D index_to_image_scale_factor_;
   const Vector2D image_to_index_scale_factor_ =
       Vector2D::Ones().cwiseQuotient(index_to_image_scale_factor_);

diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h b/libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h
@@ -37,8 +37,6 @@ class SphericalProjector : public ProjectorBase {
 
   explicit SphericalProjector(const Config& config);
 
-  IndexElement getNumRows() const final { return config_.elevation.num_cells; }
-  IndexElement getNumColumns() const final { return config_.azimuth.num_cells; }
   Vector2D getMinImageCoordinates() const final {
     return {config_.elevation.min_angle, config_.azimuth.min_angle};
   }

diff --git a/libraries/wavemap/src/integrator/projection_model/ouster_projector.cc b/libraries/wavemap/src/integrator/projection_model/ouster_projector.cc
@@ -5,12 +5,13 @@
 namespace wavemap {
 OusterProjector::OusterProjector(const OusterProjector::Config& config)
     : ProjectorBase(
+          {config.elevation.num_cells, config.azimuth.num_cells},
           Vector2D(config.elevation.max_angle - config.elevation.min_angle,
                    config.azimuth.max_angle - config.azimuth.min_angle)
               .cwiseQuotient(Index2D(config.elevation.num_cells - 1,
                                      config.azimuth.num_cells - 1)
                                  .cast<FloatingPoint>()),
-          Vector2D(config.elevation.min_angle, config.azimuth.min_angle)),
+          {config.elevation.min_angle, config.azimuth.min_angle}),
       config_(config.checkValid()) {}
 
 Eigen::Matrix<bool, 3, 1> OusterProjector::sensorAxisIsPeriodic() const {

diff --git a/libraries/wavemap/src/integrator/projection_model/spherical_projector.cc b/libraries/wavemap/src/integrator/projection_model/spherical_projector.cc
@@ -5,12 +5,13 @@
 namespace wavemap {
 SphericalProjector::SphericalProjector(const SphericalProjector::Config& config)
     : ProjectorBase(
+          {config.elevation.num_cells, config.azimuth.num_cells},
           Vector2D(config.elevation.max_angle - config.elevation.min_angle,
                    config.azimuth.max_angle - config.azimuth.min_angle)
               .cwiseQuotient(Index2D(config.elevation.num_cells - 1,
                                      config.azimuth.num_cells - 1)
                                  .cast<FloatingPoint>()),
-          Vector2D(config.elevation.min_angle, config.azimuth.min_angle)),
+          {config.elevation.min_angle, config.azimuth.min_angle}),
       config_(config.checkValid()) {}
 
 Eigen::Matrix<bool, 3, 1> SphericalProjector::sensorAxisIsPeriodic() const {