Пример #1
0
void mesh_core::Plane::leastSquaresGeneral(
      const EigenSTL::vector_Vector3d& points,
      Eigen::Vector3d* average)
{
  if (points.empty())
  {
    normal_ = Eigen::Vector3d(0,0,1);
    d_ = 0;
    if (average)
      *average = Eigen::Vector3d::Zero();
    return;
  }

  // find c, the average of the points
  Eigen::Vector3d c;
  c.setZero();

  EigenSTL::vector_Vector3d::const_iterator p = points.begin();
  EigenSTL::vector_Vector3d::const_iterator end = points.end();
  for ( ; p != end ; ++p)
    c += *p;

  c *= 1.0/double(points.size());

  // Find the matrix
  Eigen::Matrix3d m;
  m.setZero();

  p = points.begin();
  for ( ; p != end ; ++p)
  {
    Eigen::Vector3d cp = *p - c;
    m(0,0) += cp.x() * cp.x();
    m(1,0) += cp.x() * cp.y();
    m(2,0) += cp.x() * cp.z();
    m(1,1) += cp.y() * cp.y();
    m(2,1) += cp.y() * cp.z();
    m(2,2) += cp.z() * cp.z();
  }
  m(0,1) = m(1,0);
  m(0,2) = m(2,0);
  m(1,2) = m(2,1);

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(m);
  if (eigensolver.info() == Eigen::Success)
  {
    normal_ = eigensolver.eigenvectors().col(0);
    normal_.normalize();
  }
  else
  {
    normal_ = Eigen::Vector3d(0,0,1);
  }

  d_ = -c.dot(normal_);

  if (average)
    *average = c;
}
Пример #2
0
void mesh_core::generateAABB(
      const EigenSTL::vector_Vector3d& points,
      Eigen::Vector3d& min,
      Eigen::Vector3d& max)
{
  min = Eigen::Vector3d(std::numeric_limits<double>::max(),
                        std::numeric_limits<double>::max(),
                        std::numeric_limits<double>::max());
  max = Eigen::Vector3d(-std::numeric_limits<double>::max(),
                        -std::numeric_limits<double>::max(),
                        -std::numeric_limits<double>::max());

  EigenSTL::vector_Vector3d::const_iterator it = points.begin();
  EigenSTL::vector_Vector3d::const_iterator end = points.end();
  for ( ; it != end ; ++it)
  {
    min = min.array().min(it->array());
    max = max.array().max(it->array());
  }
}
Пример #3
0
void mesh_core::Plane::leastSquaresFast(
      const EigenSTL::vector_Vector3d& points,
      Eigen::Vector3d* average)
{
  Eigen::Matrix3d m;
  Eigen::Vector3d b;
  Eigen::Vector3d c;

  m.setZero();
  b.setZero();
  c.setZero();

  EigenSTL::vector_Vector3d::const_iterator p = points.begin();
  EigenSTL::vector_Vector3d::const_iterator end = points.end();
  for ( ; p != end ; ++p)
  {
    m(0,0) += p->x() * p->x();
    m(1,0) += p->x() * p->y();
    m(2,0) += p->x();
    m(1,1) += p->y() * p->y();
    m(2,1) += p->y();
    b(0) += p->x() * p->z();
    b(1) += p->y() * p->z();
    b(2) += p->z();
    c += *p;
  }
  m(0,1) = m(1,0);
  m(0,2) = m(2,0);
  m(1,2) = m(2,1);
  m(2,2) = double(points.size());
  c *= 1.0/double(points.size());

  normal_ = m.colPivHouseholderQr().solve(b);
  if (normal_.squaredNorm() > std::numeric_limits<double>::epsilon())
    normal_.normalize();

  d_ = -c.dot(normal_);

  if (average)
    *average = c;
}