text: #include <gpg/local_frame.h>
  
  
  void LocalFrame::print()
  {
    std::cout << "sample: " << sample_.transpose() << std::endl;
    std::cout << "curvature_axis: " << curvature_axis_.transpose() << std::endl;
    std::cout << "normal: " << normal_.transpose() << std::endl;
    std::cout << "binormal: " << binormal_.transpose() << std::endl;
    std::cout << "-----------\n";
  }
  
  
  void LocalFrame::findAverageNormalAxis(const Eigen::MatrixXd &normals)
  {
    // 1. Calculate curvature axis.
    Eigen::Matrix3d M = normals * normals.transpose();
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(M); // M is adjoint (M is equal to its transpose)
    Eigen::Vector3d eigen_values = eigen_solver.eigenvalues().real();
    Eigen::Matrix3d eigen_vectors = eigen_solver.eigenvectors().real();
    int min_index;
    eigen_values.minCoeff(&min_index);
    curvature_axis_ = eigen_vectors.col(min_index);
  
    // 2. Calculate surface normal (corresponds to major principal curvature axis).
    int max_index;
    eigen_values.maxCoeff(&max_index);
    normal_ = eigen_vectors.col(max_index);
  
    // 3. Ensure that the new normal is pointing in the same direction as the existing normals.
    Eigen::Vector3d avg_normal = normals.rowwise().sum();
    avg_normal /= avg_normal.norm();
    if (avg_normal.transpose() * normal_ < 0)
    {
      normal_ *= -1.0;
    }
  
    // 4. Create binormal.
    binormal_ = curvature_axis_.cross(normal_);
  }
  
  
  void LocalFrame::plotAxes(void* viewer_void, int id) const
  {
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer
      = *static_cast<boost::shared_ptr<pcl::visualization::PCLVisualizer> *>(viewer_void);
  
    pcl::PointXYZ p, q, r;
    p.x = sample_(0);
    p.y = sample_(1);
    p.z = sample_(2);
    q.x = p.x + 0.02 * curvature_axis_(0);
    q.y = p.y + 0.02 * curvature_axis_(1);
    q.z = p.z + 0.02 * curvature_axis_(2);
    r.x = p.x + 0.02 * normal_(0);
    r.y = p.y + 0.02 * normal_(1);
    r.z = p.z + 0.02 * normal_(2);
    viewer->addLine<pcl::PointXYZ>(p, q, 0, 0, 255, "curvature_axis_" + boost::lexical_cast<std::string>(id));
    viewer->addLine<pcl::PointXYZ>(p, r, 255, 0, 0, "normal_axis_" + boost::lexical_cast<std::string>(id));
  }
  