Skip to content

Commit

Permalink
Avoid unnecessary copies
Browse files Browse the repository at this point in the history
  • Loading branch information
@victorreijgwart
victorreijgwart committed Sep 29, 2023
1 parent b9182a1 commit 4e0a172
Show file tree
Hide file tree
Showing 7 changed files with 32 additions and 27 deletions.
17 changes: 9 additions & 8 deletions libraries/wavemap/include/wavemap/integrator/projection_model/impl/ouster_projector_inl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,24 +33,25 @@ inline Point3D OusterProjector::sensorToCartesian(
}

inline FloatingPoint OusterProjector::imageOffsetToErrorNorm(
const Vector2D& linearization_point, Vector2D offset) const {
const Vector2D& 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]);
offset[1] *= cos_elevation_angle;
return offset.norm();
return std::sqrt(offset[0] * offset[0] +
(cos_elevation_angle * cos_elevation_angle) *
(offset[1] * offset[1]));
}

inline std::array<FloatingPoint, 4> OusterProjector::imageOffsetsToErrorNorms(
const Vector2D& linearization_point,
ProjectorBase::CellToBeamOffsetArray offsets) const {
const ProjectorBase::CellToBeamOffsetArray& offsets) const {
const FloatingPoint cos_elevation_angle = std::cos(linearization_point[0]);
for (int offset_idx = 0; offset_idx < 4; ++offset_idx) {
offsets[offset_idx][1] *= cos_elevation_angle;
}
std::array<FloatingPoint, 4> error_norms{};
for (int offset_idx = 0; offset_idx < 4; ++offset_idx) {
error_norms[offset_idx] = offsets[offset_idx].norm();
error_norms[offset_idx] =
std::sqrt((offsets[offset_idx][0] * offsets[offset_idx][0]) +
(cos_elevation_angle * cos_elevation_angle) *
(offsets[offset_idx][1] * offsets[offset_idx][1]));
}
return error_norms;
}
Expand Down
9 changes: 6 additions & 3 deletions ...s/wavemap/include/wavemap/integrator/projection_model/impl/pinhole_camera_projector_inl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,16 +30,19 @@ inline Point3D PinholeCameraProjector::sensorToCartesian(
}

inline FloatingPoint PinholeCameraProjector::imageOffsetToErrorNorm(
const Vector2D&, Vector2D offset) const {
const Vector2D&, const Vector2D& offset) const {
return offset.norm();
}

inline std::array<FloatingPoint, 4>
PinholeCameraProjector::imageOffsetsToErrorNorms(
const Vector2D&, ProjectorBase::CellToBeamOffsetArray offsets) const {
const Vector2D&,
const ProjectorBase::CellToBeamOffsetArray& offsets) const {
std::array<FloatingPoint, 4> error_norms{};
for (int offset_idx = 0; offset_idx < 4; ++offset_idx) {
error_norms[offset_idx] = offsets[offset_idx].norm();
error_norms[offset_idx] =
std::sqrt(offsets[offset_idx][0] * offsets[offset_idx][0] +
offsets[offset_idx][1] * offsets[offset_idx][1]);
}
return error_norms;
}
Expand Down
17 changes: 9 additions & 8 deletions libraries/wavemap/include/wavemap/integrator/projection_model/impl/spherical_projector_inl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,25 +21,26 @@ inline Point3D SphericalProjector::sensorToCartesian(
}

inline FloatingPoint SphericalProjector::imageOffsetToErrorNorm(
const Vector2D& linearization_point, Vector2D offset) const {
const Vector2D& 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]);
offset[1] *= cos_elevation_angle;
return offset.norm();
return std::sqrt(offset[0] * offset[0] +
(cos_elevation_angle * cos_elevation_angle) *
(offset[1] * offset[1]));
}

inline std::array<FloatingPoint, 4>
SphericalProjector::imageOffsetsToErrorNorms(
const Vector2D& linearization_point,
ProjectorBase::CellToBeamOffsetArray offsets) const {
const ProjectorBase::CellToBeamOffsetArray& offsets) const {
const FloatingPoint cos_elevation_angle = std::cos(linearization_point[0]);
for (int offset_idx = 0; offset_idx < 4; ++offset_idx) {
offsets[offset_idx][1] *= cos_elevation_angle;
}
std::array<FloatingPoint, 4> error_norms{};
for (int offset_idx = 0; offset_idx < 4; ++offset_idx) {
error_norms[offset_idx] = offsets[offset_idx].norm();
error_norms[offset_idx] =
std::sqrt((offsets[offset_idx][0] * offsets[offset_idx][0]) +
(cos_elevation_angle * cos_elevation_angle) *
(offsets[offset_idx][1] * offsets[offset_idx][1]));
}
return error_norms;
}
Expand Down
4 changes: 2 additions & 2 deletions libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,10 +74,10 @@ class OusterProjector : public ProjectorBase {
{image_coordinates.x(), image_coordinates.y(), range});
}
FloatingPoint imageOffsetToErrorNorm(const Vector2D& linearization_point,
Vector2D offset) const final;
const Vector2D& offset) const final;
std::array<FloatingPoint, 4> imageOffsetsToErrorNorms(
const Vector2D& linearization_point,
CellToBeamOffsetArray offsets) const final;
const CellToBeamOffsetArray& offsets) const final;

// Projection from Cartesian space onto the sensor's image surface
Vector2D cartesianToImage(const Point3D& C_point) const final;
Expand Down
4 changes: 2 additions & 2 deletions libraries/wavemap/include/wavemap/integrator/projection_model/pinhole_camera_projector.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,10 +71,10 @@ class PinholeCameraProjector : public ProjectorBase {
Point3D sensorToCartesian(const Vector2D& image_coordinates,
FloatingPoint depth) const final;
FloatingPoint imageOffsetToErrorNorm(const Vector2D& /*linearization_point*/,
Vector2D offset) const final;
const Vector2D& offset) const final;
std::array<FloatingPoint, 4> imageOffsetsToErrorNorms(
const Vector2D& /*linearization_point*/,
CellToBeamOffsetArray offsets) const final;
const CellToBeamOffsetArray& offsets) const final;

// Projection from Cartesian space onto the sensor's image surface
Vector2D cartesianToImage(const Point3D& C_point) const final {
Expand Down
4 changes: 2 additions & 2 deletions libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,11 +73,11 @@ class ProjectorBase {
// angle between the two rays whose offset is given. For camera models,
// it corresponds to the reprojection error in pixels.
virtual FloatingPoint imageOffsetToErrorNorm(
const Vector2D& linearization_point, Vector2D offset) const = 0;
const Vector2D& linearization_point, const Vector2D& offset) const = 0;
using CellToBeamOffsetArray = std::array<Vector2D, 4>;
virtual std::array<FloatingPoint, 4> imageOffsetsToErrorNorms(
const Vector2D& linearization_point,
CellToBeamOffsetArray offsets) const = 0;
const CellToBeamOffsetArray& offsets) const = 0;

// Convenience functions combining multiple of the above methods
Index2D cartesianToNearestIndex(const Point3D& C_point) const {
Expand Down
4 changes: 2 additions & 2 deletions libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,10 +60,10 @@ class SphericalProjector : public ProjectorBase {
{image_coordinates.x(), image_coordinates.y(), range});
}
FloatingPoint imageOffsetToErrorNorm(const Vector2D& linearization_point,
Vector2D offset) const final;
const Vector2D& offset) const final;
std::array<FloatingPoint, 4> imageOffsetsToErrorNorms(
const Vector2D& linearization_point,
CellToBeamOffsetArray offsets) const final;
const CellToBeamOffsetArray& offsets) const final;

// Projection from Cartesian space onto the sensor's image surface
Vector2D cartesianToImage(const Point3D& C_point) const final;
Expand Down

0 comments on commit 4e0a172

Please sign in to comment.