Skip to content

Commit

Permalink
Docstrings and detail namespace
Browse files Browse the repository at this point in the history
Signed-off-by: Fernando-Sanz <sanz.fernando172@gmail.com>
  • Loading branch information
Fernando-Sanz committed Jan 22, 2025
1 parent 4bfacf0 commit 66db8c0
Show file tree
Hide file tree
Showing 2 changed files with 24 additions and 2 deletions.
22 changes: 20 additions & 2 deletions beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,13 +32,31 @@

namespace beluga_ros {

namespace detail {

/// Create an ellipoid based on NDT cell data (eigenvalues and eigenvectors) contained in the received marker.
/**
* \param eigen_solver Precreated eigen solver of the NDT cell.
* \param cell NDT cell.
* \param marker Marker that will contain the message.
* \return A boolean set to false if the ellipsoid could not be created (because the covariances are
* non-diagonalizable).
*/
bool use_mean_covariance(
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 3, 3>>& eigen_solver,
beluga::NDTCell<3> cell,
beluga_ros::msg::Marker& marker);

/// Create a cube contained in the received marker.
/**
* \param position Center of the cube.
* \param size Size of the edges.
* \param marker Marker that will contain the message.
*/
void use_cell_size(const Eigen::Vector<int, 3>& position, double size, beluga_ros::msg::Marker& marker);

} // namespace detail

/// Assign an ellipsoid to each cell of a SparseValueGrid. A cube is used instead if the distribution of the
/// cell is not suitable for the rotation matrix creation.
/**
Expand Down Expand Up @@ -74,10 +92,10 @@ std::pair<beluga_ros::msg::MarkerArray, bool> assign_obstacle_map(
marker.ns = "obstacles";

// Try to create an ellipsoid with values of the cell
if (!use_mean_covariance(eigen_solver, cell, marker)) {
if (!beluga_ros::detail::use_mean_covariance(eigen_solver, cell, marker)) {
// Create a cube based on the resolution of the grid
cubes_generated = true;
use_cell_size(cell_center, grid.resolution(), marker);
beluga_ros::detail::use_cell_size(cell_center, grid.resolution(), marker);
}

// Add the marker
Expand Down
4 changes: 4 additions & 0 deletions beluga_ros/src/ndt_ellipsoid.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@ inline bool is_approx(RealScalar x, RealScalar y, RealScalar prec = Eigen::NumTr

} // namespace

namespace detail {

bool use_mean_covariance(
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 3, 3>>& eigen_solver,
const beluga::NDTCell<3> cell,
Expand Down Expand Up @@ -102,4 +104,6 @@ void use_cell_size(const Eigen::Vector<int, 3>& position, double size, beluga_ro
marker.color.a = 1.0F;
}

} // namespace detail

} // namespace beluga_ros

0 comments on commit 66db8c0

Please sign in to comment.