Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009 #include <grid_map_filters/NormalVectorsFilter.hpp>
00010
00011 #include <grid_map_core/grid_map_core.hpp>
00012 #include <pluginlib/class_list_macros.h>
00013
00014 #include <Eigen/Dense>
00015 #include <stdexcept>
00016
00017 using namespace filters;
00018
00019 namespace grid_map {
00020
00021 template<typename T>
00022 NormalVectorsFilter<T>::NormalVectorsFilter()
00023 : method_(Method::Raster),
00024 estimationRadius_(0.0)
00025 {
00026 }
00027
00028 template<typename T>
00029 NormalVectorsFilter<T>::~NormalVectorsFilter()
00030 {
00031 }
00032
00033 template<typename T>
00034 bool NormalVectorsFilter<T>::configure()
00035 {
00036 if (!FilterBase<T>::getParam(std::string("radius"), estimationRadius_)) {
00037 ROS_DEBUG("Normal vectors filter did not find parameter `radius`.");
00038 method_ = Method::Raster;
00039 } else {
00040 method_ = Method::Area;
00041 if (estimationRadius_ < 0.0) {
00042 ROS_ERROR("Normal vectors filter estimation radius must be greater than zero.");
00043 return false;
00044 }
00045 ROS_DEBUG("Normal vectors estimation radius = %f", estimationRadius_);
00046 }
00047
00048 std::string normalVectorPositiveAxis;
00049 if (!FilterBase<T>::getParam(std::string("normal_vector_positive_axis"), normalVectorPositiveAxis)) {
00050 ROS_ERROR("Normal vectors filter did not find parameter `normal_vector_positive_axis`.");
00051 return false;
00052 }
00053 if (normalVectorPositiveAxis == "z") {
00054 normalVectorPositiveAxis_ = Vector3::UnitZ();
00055 } else if (normalVectorPositiveAxis == "y") {
00056 normalVectorPositiveAxis_ = Vector3::UnitY();
00057 } else if (normalVectorPositiveAxis == "x") {
00058 normalVectorPositiveAxis_ = Vector3::UnitX();
00059 } else {
00060 ROS_ERROR("The normal vector positive axis '%s' is not valid.", normalVectorPositiveAxis.c_str());
00061 return false;
00062 }
00063
00064 if (!FilterBase < T > ::getParam(std::string("input_layer"), inputLayer_)) {
00065 ROS_ERROR("Normal vectors filter did not find parameter `input_layer`.");
00066 return false;
00067 }
00068 ROS_DEBUG("Normal vectors filter input layer is = %s.", inputLayer_.c_str());
00069
00070 if (!FilterBase < T > ::getParam(std::string("output_layers_prefix"), outputLayersPrefix_)) {
00071 ROS_ERROR("Normal vectors filter did not find parameter `output_layers_prefix`.");
00072 return false;
00073 }
00074 ROS_DEBUG("Normal vectors filter output_layer = %s.", outputLayersPrefix_.c_str());
00075
00076 return true;
00077 }
00078
00079 template<typename T>
00080 bool NormalVectorsFilter<T>::update(const T& mapIn, T& mapOut)
00081 {
00082 std::vector<std::string> normalVectorsLayers;
00083 normalVectorsLayers.push_back(outputLayersPrefix_ + "x");
00084 normalVectorsLayers.push_back(outputLayersPrefix_ + "y");
00085 normalVectorsLayers.push_back(outputLayersPrefix_ + "z");
00086
00087 mapOut = mapIn;
00088 for (const auto& layer : normalVectorsLayers) mapOut.add(layer);
00089 switch (method_) {
00090 case Method::Area:
00091 computeWithArea(mapOut, inputLayer_, outputLayersPrefix_);
00092 break;
00093 case Method::Raster:
00094 computeWithRaster(mapOut, inputLayer_, outputLayersPrefix_);
00095 break;
00096 }
00097
00098 return true;
00099 }
00100
00101 template<typename T>
00102 void NormalVectorsFilter<T>::computeWithArea(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix)
00103 {
00104
00105 for (GridMapIterator iterator(map);
00106 !iterator.isPastEnd(); ++iterator) {
00107
00108 if (!map.isValid(*iterator, inputLayer_)) continue;
00109
00110
00111 Position center;
00112 map.getPosition(*iterator, center);
00113
00114
00115 const int maxNumberOfCells = pow(ceil(2 * estimationRadius_ / map.getResolution()), 2);
00116 Eigen::MatrixXd points(3, maxNumberOfCells);
00117
00118
00119 size_t nPoints = 0;
00120 for (CircleIterator iterator(map, center, estimationRadius_); !iterator.isPastEnd(); ++iterator) {
00121 if (!map.isValid(*iterator, inputLayer_)) continue;
00122 Position3 point;
00123 map.getPosition3(inputLayer_, *iterator, point);
00124 points.col(nPoints) = point;
00125 nPoints++;
00126 }
00127 points.conservativeResize(3, nPoints);
00128
00129
00130 const Position3 mean = points.leftCols(nPoints).rowwise().sum() / nPoints;
00131 const Eigen::MatrixXd NN = points.leftCols(nPoints).colwise() - mean;
00132
00133 const Eigen::Matrix3d covarianceMatrix(NN * NN.transpose());
00134 Vector3 eigenvalues = Vector3::Ones();
00135 Eigen::Matrix3d eigenvectors = Eigen::Matrix3d::Identity();
00136
00137 if (covarianceMatrix.fullPivHouseholderQr().rank() >= 3) {
00138 const Eigen::EigenSolver<Eigen::MatrixXd> solver(covarianceMatrix);
00139 eigenvalues = solver.eigenvalues().real();
00140 eigenvectors = solver.eigenvectors().real();
00141 } else {
00142 ROS_DEBUG("Covariance matrix needed for eigen decomposition is degenerated. Expected cause: no noise in data (nPoints = %i)", (int) nPoints);
00143
00144 eigenvalues.z() = 0.0;
00145 }
00146
00147 int smallestId(0);
00148 double smallestValue(std::numeric_limits<double>::max());
00149 for (int j = 0; j < eigenvectors.cols(); j++) {
00150 if (eigenvalues(j) < smallestValue) {
00151 smallestId = j;
00152 smallestValue = eigenvalues(j);
00153 }
00154 }
00155 Vector3 eigenvector = eigenvectors.col(smallestId);
00156 if (eigenvector.dot(normalVectorPositiveAxis_) < 0.0) eigenvector = -eigenvector;
00157 map.at(outputLayersPrefix_ + "x", *iterator) = eigenvector.x();
00158 map.at(outputLayersPrefix_ + "y", *iterator) = eigenvector.y();
00159 map.at(outputLayersPrefix_ + "z", *iterator) = eigenvector.z();
00160 }
00161 }
00162
00163 template<typename T>
00164 void NormalVectorsFilter<T>::computeWithRaster(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix)
00165 {
00166 throw std::runtime_error("NormalVectorsFilter::computeWithRaster() is not yet implemented!");
00167
00168 }
00169
00170 }
00171
00172 PLUGINLIB_EXPORT_CLASS(grid_map::NormalVectorsFilter<grid_map::GridMap>, filters::FilterBase<grid_map::GridMap>)