14 #include <Eigen/Dense> 24 estimationRadius_(0.0)
37 ROS_DEBUG(
"Normal vectors filter did not find parameter `radius`.");
42 ROS_ERROR(
"Normal vectors filter estimation radius must be greater than zero.");
48 std::string normalVectorPositiveAxis;
50 ROS_ERROR(
"Normal vectors filter did not find parameter `normal_vector_positive_axis`.");
53 if (normalVectorPositiveAxis ==
"z") {
55 }
else if (normalVectorPositiveAxis ==
"y") {
57 }
else if (normalVectorPositiveAxis ==
"x") {
60 ROS_ERROR(
"The normal vector positive axis '%s' is not valid.", normalVectorPositiveAxis.c_str());
65 ROS_ERROR(
"Normal vectors filter did not find parameter `input_layer`.");
71 ROS_ERROR(
"Normal vectors filter did not find parameter `output_layers_prefix`.");
82 std::vector<std::string> normalVectorsLayers;
88 for (
const auto& layer : normalVectorsLayers) mapOut.add(layer);
116 Eigen::MatrixXd points(3, maxNumberOfCells);
124 points.col(nPoints) = point;
127 points.conservativeResize(3, nPoints);
130 const Position3 mean = points.leftCols(nPoints).rowwise().sum() / nPoints;
131 const Eigen::MatrixXd NN = points.leftCols(nPoints).colwise() - mean;
133 const Eigen::Matrix3d covarianceMatrix(NN * NN.transpose());
134 Vector3 eigenvalues = Vector3::Ones();
135 Eigen::Matrix3d eigenvectors = Eigen::Matrix3d::Identity();
137 if (covarianceMatrix.fullPivHouseholderQr().rank() >= 3) {
138 const Eigen::EigenSolver<Eigen::MatrixXd> solver(covarianceMatrix);
139 eigenvalues = solver.eigenvalues().real();
140 eigenvectors = solver.eigenvectors().real();
142 ROS_DEBUG(
"Covariance matrix needed for eigen decomposition is degenerated. Expected cause: no noise in data (nPoints = %i)", (
int) nPoints);
144 eigenvalues.z() = 0.0;
148 double smallestValue(std::numeric_limits<double>::max());
149 for (
int j = 0; j < eigenvectors.cols(); j++) {
150 if (eigenvalues(j) < smallestValue) {
152 smallestValue = eigenvalues(j);
155 Vector3 eigenvector = eigenvectors.col(smallestId);
166 throw std::runtime_error(
"NormalVectorsFilter::computeWithRaster() is not yet implemented!");
void computeWithRaster(GridMap &map, const std::string &inputLayer, const std::string &outputLayersPrefix)
std::string outputLayersPrefix_
Output layer name.
bool getPosition(const Index &index, Position &position) const
virtual bool update(const T &mapIn, T &mapOut)
std::string inputLayer_
Input layer name.
bool isValid(const Index &index) const
Eigen::Vector3d Position3
Eigen::Vector3d normalVectorPositiveAxis_
Normal vector positive axis.
double getResolution() const
virtual ~NormalVectorsFilter()
float & at(const std::string &layer, const Index &index)
double estimationRadius_
Radius of submap for normal vector estimation.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
bool getPosition3(const std::string &layer, const Index &index, Position3 &position) const
void computeWithArea(GridMap &map, const std::string &inputLayer, const std::string &outputLayersPrefix)