2019-02-24 点云的曲面法向量估计

使用拟合方法求取局部点云法向量，可使用ＰCA方法，如：https://blog.csdn.net/eric_e/article/details/82190616。
或使用ＳＶＤ方法，如https://www.cnblogs.com/houkai/p/6656894.html。
最小二乘法的求取方法：

void FiltersDemo::VectorscomputeWithAreaplaneSVD(grid_map::GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix)
{
  // For each cell in requested area.
  int filter_cells = 0;
  ros::Time begin_time = ros::Time::now ();
  for (GridMapIterator iterator(map);!iterator.isPastEnd(); ++iterator) {
    // Check if this is an empty cell (hole in the map).
    
    if (!map.isValid(*iterator, inputLayer_)) continue;
    filter_cells ++;
    // Requested position (center) of circle in map.
    Position center;
    map.getPosition(*iterator, center);

    // Prepare data computation.
    const int maxNumberOfCells = pow(ceil(2 * estimationRadius_ / map.getResolution()), 2);
    Eigen::MatrixXd points(maxNumberOfCells, 3);

    // Gather surrounding data.
    size_t nPoints = 0;
    for (CircleIterator iterator(map, center, estimationRadius_); !iterator.isPastEnd(); ++iterator) {
      if (!map.isValid(*iterator, inputLayer_)) continue;
      Position3 point;
      map.getPosition3(inputLayer_, *iterator, point);
      points.row(nPoints) = point;
      nPoints++;
    }

    points.conservativeResize(nPoints, 3); // TODO Eigen version?
    Eigen::MatrixXd NN(nPoints, 4);
    NN.leftCols(3) = points;
    NN.rightCols(1).setOnes();
    // Compute Eigenvectors.
    
    if (nPoints < 4) continue;
  
    //std::cout << " **" << NN <<std::endl;
    JacobiSVD<Eigen::MatrixXd> svd(NN, ComputeThinU | ComputeThinV );
      
    
    Matrix4d V = svd.matrixV();
    //std::cout << "--" <<V << std::endl;
    
    Vector3 eigenvector;
    eigenvector.x() = V(0,3);
    eigenvector.y() = V(1,3);
    eigenvector.z() = V(2,3);
    
    if (eigenvector.dot(normalVectorPositiveAxis_) < 0.0) eigenvector = -eigenvector;
    map.at(outputLayersPrefix_ + "x", *iterator) = eigenvector.x();
    map.at(outputLayersPrefix_ + "y", *iterator) = eigenvector.y();
    map.at(outputLayersPrefix_ + "z", *iterator) = eigenvector.z();
      
  }
  std::cout << "filtercells_num:"<<filter_cells<<std::endl;
  double t1;
  t1 = (ros::Time::now () - begin_time).toSec ();
  std::cout << "method1 SVD filter time is:" << t1 << std::endl; 
}

void FiltersDemo::VectorscomputeWithAreaplaneSVDeigenvalue(grid_map::GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix)
{
  // For each cell in requested area.
  int filter_cells = 0;
  ros::Time begin_time = ros::Time::now ();
  for (GridMapIterator iterator(map);!iterator.isPastEnd(); ++iterator) {
    // Check if this is an empty cell (hole in the map).
    
    if (!map.isValid(*iterator, inputLayer_)) continue;
    filter_cells ++;
    // Requested position (center) of circle in map.
    Position center;
    map.getPosition(*iterator, center);

    // Prepare data computation.
    const int maxNumberOfCells = pow(ceil(2 * estimationRadius_ / map.getResolution()), 2);
    Eigen::MatrixXd points(maxNumberOfCells, 3);

    // Gather surrounding data.
    size_t nPoints = 0;
    for (CircleIterator iterator(map, center, estimationRadius_); !iterator.isPastEnd(); ++iterator) {
      if (!map.isValid(*iterator, inputLayer_)) continue;
      Position3 point;
      map.getPosition3(inputLayer_, *iterator, point);
      points.row(nPoints) = point;
      nPoints++;
    }

    points.conservativeResize(nPoints, 3); // TODO Eigen version?
    Eigen::MatrixXd NN(nPoints, 4);
    NN.leftCols(3) = points;
    NN.rightCols(1).setOnes();
    // Compute Eigenvectors.
    
    if (nPoints < 4) continue;
  
    //std::cout << " **" << NN <<std::endl;
    const Eigen::Matrix4d covarianceMatrix(NN.transpose()*NN);
    Vector4d eigenvalues = Vector4d::Ones();
    Eigen::Matrix4d eigenvectors = Eigen::Matrix4d::Identity();
    // Ensure that the matrix is suited for eigenvalues calculation.
    const Eigen::EigenSolver<Eigen::MatrixXd> solver(covarianceMatrix);
    eigenvalues = solver.eigenvalues().real();
    eigenvectors = solver.eigenvectors().real();
    
    // Keep the smallest Eigenvector as normal vector.
    int smallestId(0);
    double smallestValue(std::numeric_limits<double>::max());
    for (int j = 0; j < eigenvectors.cols(); j++) {
      if (eigenvalues(j) < smallestValue) {
        smallestId = j;
        smallestValue = eigenvalues(j);
      }
    }
    Vector4d eigenvector = eigenvectors.col(smallestId);
    
    if (eigenvector.z() < 0.0) eigenvector = -eigenvector;
    map.at(outputLayersPrefix_ + "x", *iterator) = eigenvector.x();
    map.at(outputLayersPrefix_ + "y", *iterator) = eigenvector.y();
    map.at(outputLayersPrefix_ + "z", *iterator) = eigenvector.z();
      
  }
  std::cout << "filtercells_num:"<<filter_cells<<std::endl;
  double t1;
  t1 = (ros::Time::now () - begin_time).toSec ();
  std::cout << "method1 PAC filter time is:" << t1 << std::endl; 
}

void FiltersDemo::VectorscomputeWithAreaSVD(grid_map::GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix)
{
  // For each cell in requested area.
  int filter_cells = 0;
  ros::Time begin_time = ros::Time::now ();
  for (GridMapIterator iterator(map);!iterator.isPastEnd(); ++iterator) {
    // Check if this is an empty cell (hole in the map).
    
    if (!map.isValid(*iterator, inputLayer_)) continue;
    filter_cells ++;
    // Requested position (center) of circle in map.
    Position center;
    map.getPosition(*iterator, center);

    // Prepare data computation.
    const int maxNumberOfCells = pow(ceil(2 * estimationRadius_ / map.getResolution()), 2);
    Eigen::MatrixXd points(maxNumberOfCells, 3);

    // Gather surrounding data.
    size_t nPoints = 0;
    for (CircleIterator iterator(map, center, estimationRadius_); !iterator.isPastEnd(); ++iterator) {
      if (!map.isValid(*iterator, inputLayer_)) continue;
      Position3 point;
      map.getPosition3(inputLayer_, *iterator, point);
      if(center.x() != point.x() && center.y() != point.y())
      {
        points.row(nPoints) = point;
        nPoints++;
      }
    }

    points.conservativeResize(nPoints, 3); // TODO Eigen version?
    //points.bottomRows(1).setOnes();
    // Compute Eigenvectors.

    if (nPoints < 4) continue;
    double smooth_height = points.rightCols(1).sum() / nPoints;
    double height = map.at(inputLayer_, *iterator);

    Position3 center3d;
    center3d.x() = center.x();
    center3d.y() = center.y();
    center3d.z() = height;
    Eigen::MatrixXd NN = points.bottomRows(nPoints).rowwise() - center3d.transpose();
     
    //std::cout << " **" << points <<std::endl;
    //std::cout << smooth_height << std::endl;
    JacobiSVD<Eigen::MatrixXd> svd(NN, ComputeThinU | ComputeThinV );
      
    Matrix3d V = svd.matrixV();
    
    Vector3 eigenvector = V.rightCols(1);
    if (eigenvector.dot(normalVectorPositiveAxis_) < 0.0) eigenvector = -eigenvector;

    /*
    double roughness = fabs(height - smooth_height);
    
    double slope = acos(eigenvector.z());
    
    double traversability = 0.5 * (1.0 - (slope / 0.6)) + 0.5 * (1.0 - (roughness / 0.1));
    */
    map.at(outputLayersPrefix_ + "x", *iterator) = eigenvector.x();
    map.at(outputLayersPrefix_ + "y", *iterator) = eigenvector.y();
    map.at(outputLayersPrefix_ + "z", *iterator) = eigenvector.z();

    /*
    map.at("Roughness", *iterator) = roughness;
    map.at("slope", *iterator) = slope;
    map.at("traversability", *iterator) = traversability;
    */

    
  }

  std::cout << "filtercells_num:"<<filter_cells<<std::endl;
  double t1;
  t1 = (ros::Time::now () - begin_time).toSec ();
  std::cout << "method3 SVD filter time is:" << t1 << std::endl; 
}

void FiltersDemo::VectorscomputeWithAreaeigenValue(grid_map::GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix)
{
  // For each cell in requested area.
  int filter_cells = 0;
  ros::Time begin_time = ros::Time::now ();
  for (GridMapIterator iterator(map);!iterator.isPastEnd(); ++iterator) {
    // Check if this is an empty cell (hole in the map).
    
    if (!map.isValid(*iterator, inputLayer_)) continue;
    filter_cells ++;
    // Requested position (center) of circle in map.
    Position center;
    map.getPosition(*iterator, center);

    // Prepare data computation.
    const int maxNumberOfCells = pow(ceil(2 * estimationRadius_ / map.getResolution()), 2);
    Eigen::MatrixXd points(maxNumberOfCells, 3);

    // Gather surrounding data.
    size_t nPoints = 0;
    for (CircleIterator iterator(map, center, estimationRadius_); !iterator.isPastEnd(); ++iterator) {
      if (!map.isValid(*iterator, inputLayer_)) continue;
      Position3 point;
      map.getPosition3(inputLayer_, *iterator, point);
      if(center.x() != point.x() && center.y() != point.y())
      {
        points.row(nPoints) = point;
        nPoints++;
      }
    }
    
    points.conservativeResize(nPoints, 3); // TODO Eigen version?
    //points.bottomRows(1).setOnes();
    // Compute Eigenvectors.

    if (nPoints < 4) continue;
    double smooth_height = points.rightCols(1).sum() / nPoints;
    double height = map.at(inputLayer_, *iterator);

    Position3 center3d;
    center3d.x() = center.x();
    center3d.y() = center.y();
    center3d.z() = height;
    Eigen::MatrixXd NN = points.bottomRows(nPoints).rowwise() - center3d.transpose();

    const Eigen::Matrix3d covarianceMatrix(NN.transpose()*NN);
    Vector3 eigenvalues = Vector3::Ones();
    Eigen::Matrix3d eigenvectors = Eigen::Matrix3d::Identity();
    // Ensure that the matrix is suited for eigenvalues calculation.
    const Eigen::EigenSolver<Eigen::MatrixXd> solver(covarianceMatrix);
    eigenvalues = solver.eigenvalues().real();
    eigenvectors = solver.eigenvectors().real();
    
    // Keep the smallest Eigenvector as normal vector.
    int smallestId(0);
    double smallestValue(std::numeric_limits<double>::max());
    for (int j = 0; j < eigenvectors.cols(); j++) {
      if (eigenvalues(j) < smallestValue) {
        smallestId = j;
        smallestValue = eigenvalues(j);
      }
    }
    Vector3 eigenvector = eigenvectors.col(smallestId);
    if (eigenvector.dot(normalVectorPositiveAxis_) < 0.0) eigenvector = -eigenvector;
    /*
    double height = map.at(inputLayer_, *iterator);
    double smooth_height = mean.z();
    double roughness = fabs(height - smooth_height);
    
    double slope = acos(eigenvector.z());
    
    double traversability = 0.5 * (1.0 - (slope / 0.6)) + 0.5 * (1.0 - (roughness / 0.1));
    */
    map.at(outputLayersPrefix_ + "x", *iterator) = eigenvector.x();
    map.at(outputLayersPrefix_ + "y", *iterator) = eigenvector.y();
    map.at(outputLayersPrefix_ + "z", *iterator) = eigenvector.z();

    /*
    map.at("Roughness", *iterator) = roughness;
    map.at("slope", *iterator) = slope;
    map.at("traversability", *iterator) = traversability;
    */
  }
  std::cout << "filtercells_num:"<<filter_cells<<std::endl;
  double t1;
  t1 = (ros::Time::now () - begin_time).toSec ();
  std::cout << "method3 PAC filter time is:" << t1 << std::endl; 
}


void FiltersDemo::VectorscomputeWithArea(grid_map::GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix)
{
  // For each cell in requested area.
  int filter_cells = 0;
  ros::Time begin_time = ros::Time::now ();
  for (GridMapIterator iterator(map);!iterator.isPastEnd(); ++iterator) {
    // Check if this is an empty cell (hole in the map).
    
    if (!map.isValid(*iterator, inputLayer_)) continue;
    filter_cells ++;
    // Requested position (center) of circle in map.
    Position center;
    map.getPosition(*iterator, center);

    // Prepare data computation.
    const int maxNumberOfCells = pow(ceil(2 * estimationRadius_ / map.getResolution()), 2);
    Eigen::MatrixXd points(3, maxNumberOfCells);

    // Gather surrounding data.
    size_t nPoints = 0;
    for (CircleIterator iterator(map, center, estimationRadius_); !iterator.isPastEnd(); ++iterator) {
      if (!map.isValid(*iterator, inputLayer_)) continue;
      Position3 point;
      map.getPosition3(inputLayer_, *iterator, point);
      points.col(nPoints) = point;
      nPoints++;
    }
    if (nPoints < 4) continue;
    points.conservativeResize(3, nPoints); // TODO Eigen version?

    // Compute Eigenvectors.
    
    const Position3 mean = points.leftCols(nPoints).rowwise().sum() / nPoints;
    const Eigen::MatrixXd NN = points.leftCols(nPoints).colwise() - mean;
    const Eigen::Matrix3d covarianceMatrix(NN * NN.transpose());
    Vector3 eigenvalues = Vector3::Ones();
    Eigen::Matrix3d eigenvectors = Eigen::Matrix3d::Identity();
    // Ensure that the matrix is suited for eigenvalues calculation.
    const Eigen::EigenSolver<Eigen::MatrixXd> solver(covarianceMatrix);
    eigenvalues = solver.eigenvalues().real();
    eigenvectors = solver.eigenvectors().real();
    
    // Keep the smallest Eigenvector as normal vector.
    int smallestId(0);
    double smallestValue(std::numeric_limits<double>::max());
    for (int j = 0; j < eigenvectors.cols(); j++) {
      if (eigenvalues(j) < smallestValue) {
        smallestId = j;
        smallestValue = eigenvalues(j);
      }
    }
    Vector3 eigenvector = eigenvectors.col(smallestId);
    if (eigenvector.dot(normalVectorPositiveAxis_) < 0.0) eigenvector = -eigenvector;
    /*
    double height = map.at(inputLayer_, *iterator);
    double smooth_height = mean.z();
    double roughness = fabs(height - smooth_height);
    
    double slope = acos(eigenvector.z());
    
    double traversability = 0.5 * (1.0 - (slope / 0.6)) + 0.5 * (1.0 - (roughness / 0.1));
    */
    map.at(outputLayersPrefix_ + "x", *iterator) = eigenvector.x();
    map.at(outputLayersPrefix_ + "y", *iterator) = eigenvector.y();
    map.at(outputLayersPrefix_ + "z", *iterator) = eigenvector.z();

    /*
    map.at("Roughness", *iterator) = roughness;
    map.at("slope", *iterator) = slope;
    map.at("traversability", *iterator) = traversability;
    */
  }
  std::cout << "filtercells_num:"<<filter_cells<<std::endl;
  double t1;
  t1 = (ros::Time::now () - begin_time).toSec ();
  std::cout << "method2 PAC filter time is:" << t1 << std::endl; 
}

void FiltersDemo::VectorscomputeWithArea_SVD(grid_map::GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix)
{
  // For each cell in requested area.
  int filter_cells = 0;
  ros::Time begin_time = ros::Time::now ();
  for (GridMapIterator iterator(map);!iterator.isPastEnd(); ++iterator) {
    // Check if this is an empty cell (hole in the map).
    
    if (!map.isValid(*iterator, inputLayer_)) continue;
    filter_cells ++;
    // Requested position (center) of circle in map.
    Position center;
    map.getPosition(*iterator, center);

    // Prepare data computation.
    const int maxNumberOfCells = pow(ceil(2 * estimationRadius_ / map.getResolution()), 2);
    Eigen::MatrixXd points(3, maxNumberOfCells);

    // Gather surrounding data.
    size_t nPoints = 0;
    for (CircleIterator iterator(map, center, estimationRadius_); !iterator.isPastEnd(); ++iterator) {
      if (!map.isValid(*iterator, inputLayer_)) continue;
      Position3 point;
      map.getPosition3(inputLayer_, *iterator, point);
      points.col(nPoints) = point;
      nPoints++;
    }
    if (nPoints < 4) continue;
    points.conservativeResize(3, nPoints); // TODO Eigen version?
    // Compute Eigenvectors.
    
    const Position3 mean = points.leftCols(nPoints).rowwise().sum() / nPoints;
    const Eigen::MatrixXd NN = points.leftCols(nPoints).colwise() - mean;

    JacobiSVD<Eigen::MatrixXd> svd(NN.transpose(), ComputeThinU | ComputeThinV );
      
    Matrix3d V = svd.matrixV();
    Vector3 eigenvector = V.rightCols(1);

    if (eigenvector.dot(normalVectorPositiveAxis_) < 0.0) eigenvector = -eigenvector;
    
    double height = map.at(inputLayer_, *iterator);
    double smooth_height = mean.z();
    double roughness = fabs(height - smooth_height);
    
    double slope = acos(eigenvector.z());
    
    double traversability = 0.5 * (1.0 - (slope / 0.6)) + 0.5 * (1.0 - (roughness / 0.1));
    
    map.at(outputLayersPrefix_ + "x", *iterator) = eigenvector.x();
    map.at(outputLayersPrefix_ + "y", *iterator) = eigenvector.y();
    map.at(outputLayersPrefix_ + "z", *iterator) = eigenvector.z();

    
    map.at("Roughness", *iterator) = roughness;
    map.at("slope", *iterator) = slope;
    map.at("traversability", *iterator) = traversability;
    
  }
  std::cout << "filtercells_num:"<<filter_cells<<std::endl;
  double t1;
  t1 = (ros::Time::now () - begin_time).toSec ();
  std::cout << "method2 SVD filter time is:" << t1 << std::endl; 
}