14 #include <Eigen/Dense> 34 ROS_ERROR(
"Curvature filter did not find parameter `input_layer`.");
37 ROS_DEBUG(
"Curvature filter input layer is = %s.", inputLayer_.c_str());
40 ROS_ERROR(
"Curvature filter did not find parameter `output_layer`.");
43 ROS_DEBUG(
"Curvature filter output_layer = %s.", outputLayer_.c_str());
51 if (!mapIn.isDefaultStartIndex())
throw std::runtime_error(
52 "CurvatureFilter cannot be used with grid maps that don't have a default buffer start index.");
55 mapOut.add(outputLayer_);
56 auto& input = mapOut[inputLayer_];
57 auto& curvature = mapOut[outputLayer_];
58 const float L2 = mapOut.getResolution() * mapOut.getResolution();
60 for (Eigen::Index j{0}; j < input.cols(); ++j) {
61 for (Eigen::Index i{0}; i < input.rows(); ++i) {
63 if (!std::isfinite(input(i, j)))
continue;
64 float D = ((input(i, j==0 ? j : j-1) + input(i, j==input.cols()-1 ? j : j + 1)) / 2.0 - input(i, j)) / L2;
65 float E = ((input(i==0 ? i : i-1, j) + input(i==input.rows()-1 ? i : i + 1, j)) / 2.0 - input(i, j)) / L2;
66 if (!std::isfinite(D)) D = 0.0;
67 if (!std::isfinite(E)) E = 0.0;
68 curvature(i, j) = -2.0 * (D + E);
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)