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 index 9df5c987a..733c7fb91 100644 --- 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 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 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 index 83f365747..28bf91b35 100644 --- 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 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(), image_coordinate_offset}; + return {index_rounded.cast(), + std::move(image_coordinate_offset)}; } inline std::array 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(); - 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 index d0c1651ba..7bdb02f5a 100644 --- 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 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 index 3748883f4..3f270671e 100644 --- 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 index 80219917d..4f37fc86d 100644 --- 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 index 99dfcc9ec..0230eab88 100644 --- 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; using ConstPtr = std::shared_ptr; - 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 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 index 5247fac85..555e63a83 100644 --- 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 index bd108c29f..9bf037c3d 100644 --- 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()), - Vector2D(config.elevation.min_angle, config.azimuth.min_angle)), + {config.elevation.min_angle, config.azimuth.min_angle}), config_(config.checkValid()) {} Eigen::Matrix 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 index e68ee12dd..ae8df498f 100644 --- 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()), - Vector2D(config.elevation.min_angle, config.azimuth.min_angle)), + {config.elevation.min_angle, config.azimuth.min_angle}), config_(config.checkValid()) {} Eigen::Matrix SphericalProjector::sensorAxisIsPeriodic() const {