Exemplo n.º 1
0
template <typename PointT, typename Scalar> void
pcl::demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
                       const Eigen::Matrix<Scalar, 4, 1> &centroid,
                       Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
                       int npts)
{
  // Calculate the number of points if not given
  if (npts == 0)
  {
    while (cloud_iterator.isValid ())
    {
      ++npts;
      ++cloud_iterator;
    }
    cloud_iterator.reset ();
  }

  cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);        // keep the data aligned

  int i = 0;
  while (cloud_iterator.isValid ())
  {
    cloud_out (0, i) = cloud_iterator->x - centroid[0];
    cloud_out (1, i) = cloud_iterator->y - centroid[1];
    cloud_out (2, i) = cloud_iterator->z - centroid[2];
    ++i;
    ++cloud_iterator;
  }
}
Exemplo n.º 2
0
TEST (PCL, compute3DCentroidCloudIterator)
{
  pcl::PointIndices pindices;
  std::vector<int> indices;
  PointXYZ point;
  PointCloud<PointXYZ> cloud;
  Eigen::Vector4f centroid_f;

  for (point.x = -1; point.x < 2; point.x += 2)
  {
    for (point.y = -1; point.y < 2; point.y += 2)
    {
      for (point.z = -1; point.z < 2; point.z += 2)
      {
        cloud.push_back (point);
      }
    }
  }
  cloud.is_dense = true;

  indices.resize (4); // only positive y values
  indices [0] = 2;
  indices [1] = 3;
  indices [2] = 6;
  indices [3] = 7;

  // Test finite data
  {
    ConstCloudIterator<PointXYZ> it (cloud, indices);
  
    EXPECT_EQ (compute3DCentroid (it, centroid_f), 4);

    EXPECT_EQ (centroid_f[0], 0.0f);
    EXPECT_EQ (centroid_f[1], 1.0f);
    EXPECT_EQ (centroid_f[2], 0.0f);
    EXPECT_EQ (centroid_f[3], 1.0f);

    Eigen::Vector4d centroid_d;
    it.reset ();
    EXPECT_EQ (compute3DCentroid (it, centroid_d), 4);

    EXPECT_EQ (centroid_d[0], 0.0);
    EXPECT_EQ (centroid_d[1], 1.0);
    EXPECT_EQ (centroid_d[2], 0.0);
    EXPECT_EQ (centroid_d[3], 1.0);
  }

  // Test for non-finite data
  {
    point.getVector3fMap() << std::numeric_limits<float>::quiet_NaN (),
                              std::numeric_limits<float>::quiet_NaN (),
                              std::numeric_limits<float>::quiet_NaN ();
    cloud.push_back (point);
    cloud.is_dense = false;
    ConstCloudIterator<PointXYZ> it (cloud);

    EXPECT_EQ (8, compute3DCentroid (it, centroid_f));
    EXPECT_EQ_VECTORS (Eigen::Vector4f (0.f, 0.f, 0.f, 1.f), centroid_f);
  }
}
Exemplo n.º 3
0
template <typename PointT, typename Scalar> void
pcl::demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
                       const Eigen::Matrix<Scalar, 4, 1> &centroid,
                       pcl::PointCloud<PointT> &cloud_out,
                       int npts)
{
  // Calculate the number of points if not given
  if (npts == 0)
  {
    while (cloud_iterator.isValid ())
    {
      ++npts;
      ++cloud_iterator;
    }
    cloud_iterator.reset ();
  }

  int i = 0;
  cloud_out.resize (npts);
  // Subtract the centroid from cloud_in
  while (cloud_iterator.isValid ())
  {
    cloud_out[i].x = cloud_iterator->x - centroid[0];
    cloud_out[i].y = cloud_iterator->y - centroid[1];
    cloud_out[i].z = cloud_iterator->z - centroid[2];
    ++i;
    ++cloud_iterator;
  }
  cloud_out.width = cloud_out.size ();
  cloud_out.height = 1;
}
Exemplo n.º 4
0
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
    ConstCloudIterator<PointSource>& source_it,
    ConstCloudIterator<PointTarget>& target_it,
    Matrix4 &transformation_matrix) const
{
  // Convert to Eigen format
  const int npts = static_cast <int> (source_it.size ());

  Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_src (3, npts);
  Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_tgt (3, npts);

  for (int i = 0; i < npts; ++i)
  {
    cloud_src (0, i) = source_it->x;
    cloud_src (1, i) = source_it->y;
    cloud_src (2, i) = source_it->z;
    ++source_it;

    cloud_tgt (0, i) = target_it->x;
    cloud_tgt (1, i) = target_it->y;
    cloud_tgt (2, i) = target_it->z;
    ++target_it;
  }

  if (use_umeyama_)
  {
    // Call Umeyama directly from Eigen (PCL patched version until Eigen is released)
    transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false);
  }
  else
  {
    source_it.reset (); target_it.reset ();
    // <cloud_src,cloud_src> is the source dataset
    transformation_matrix.setIdentity ();

    Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
    // Estimate the centroids of source, target
    compute3DCentroid (source_it, centroid_src);
    compute3DCentroid (target_it, centroid_tgt);
    source_it.reset (); target_it.reset ();

    // Subtract the centroids from source, target
    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean;
    demeanPointCloud (source_it, centroid_src, cloud_src_demean);
    demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean);

    getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
  }
}
Exemplo n.º 5
0
template <typename PointT, typename Scalar> inline unsigned int
pcl::compute3DCentroid (ConstCloudIterator<PointT> &cloud_iterator,
                        Eigen::Matrix<Scalar, 4, 1> &centroid)
{
  // Initialize to 0
  centroid.setZero ();
  
  unsigned cp = 0;

  // For each point in the cloud
  // If the data is dense, we don't need to check for NaN
  while (cloud_iterator.isValid ())
  {
    // Check if the point is invalid
    if (!pcl_isfinite (cloud_iterator->x) ||
        !pcl_isfinite (cloud_iterator->y) ||
        !pcl_isfinite (cloud_iterator->z))
      continue;
    centroid[0] += cloud_iterator->x;
    centroid[1] += cloud_iterator->y;
    centroid[2] += cloud_iterator->z;
    ++cp;
    ++cloud_iterator;
  }
  centroid[3] = 0;
  centroid /= static_cast<Scalar> (cp);
  return (cp);
}
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationTranslation<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
    ConstCloudIterator<PointSource>& source_it,
    ConstCloudIterator<PointTarget>& target_it,
    Matrix4 &transformation_matrix) const
{
    source_it.reset (); target_it.reset ();
    // <cloud_src,cloud_src> is the source dataset
    transformation_matrix.setIdentity ();

    Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;    
    // Estimate the centroids of source, target
    compute3DCentroid (source_it, centroid_src);
    compute3DCentroid (target_it, centroid_tgt);
    source_it.reset (); target_it.reset (); 

    getTransformationFromCorrelation (centroid_src, centroid_tgt, transformation_matrix);

}
Exemplo n.º 7
0
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
    ConstCloudIterator<PointSource>& source_it,
    ConstCloudIterator<PointTarget>& target_it,
    Matrix4 &transformation_matrix) const
{
  source_it.reset (); target_it.reset ();

  Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
  // Estimate the centroids of source, target
  compute3DCentroid (source_it, centroid_src);
  compute3DCentroid (target_it, centroid_tgt);
  source_it.reset (); target_it.reset ();

  // ignore z component
  centroid_src[2] = 0.0f;
  centroid_tgt[2] = 0.0f;
  // Subtract the centroids from source, target
  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean;
  demeanPointCloud (source_it, centroid_src, cloud_src_demean);
  demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean);

  getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
}
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it,
                             ConstCloudIterator<PointTarget>& target_it,
                             typename std::vector<Scalar>::const_iterator& weights_it,
                             Matrix4 &transformation_matrix) const
{
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  typedef Eigen::Matrix<double, 6, 6> Matrix6d;

  Matrix6d ATA;
  Vector6d ATb;
  ATA.setZero ();
  ATb.setZero ();

  while (source_it.isValid () && target_it.isValid ())
  {
    if (!pcl_isfinite (source_it->x) ||
        !pcl_isfinite (source_it->y) ||
        !pcl_isfinite (source_it->z) ||
        !pcl_isfinite (target_it->x) ||
        !pcl_isfinite (target_it->y) ||
        !pcl_isfinite (target_it->z) ||
        !pcl_isfinite (target_it->normal_x) ||
        !pcl_isfinite (target_it->normal_y) ||
        !pcl_isfinite (target_it->normal_z))
    {
      ++ source_it;
      ++ target_it;
      ++ weights_it;
      continue;
    }

    const float & sx = source_it->x;
    const float & sy = source_it->y;
    const float & sz = source_it->z;
    const float & dx = target_it->x;
    const float & dy = target_it->y;
    const float & dz = target_it->z;
    const float & nx = target_it->normal[0] * (*weights_it);
    const float & ny = target_it->normal[1] * (*weights_it);
    const float & nz = target_it->normal[2] * (*weights_it);

    double a = nz*sy - ny*sz;
    double b = nx*sz - nz*sx;
    double c = ny*sx - nx*sy;

    //    0  1  2  3  4  5
    //    6  7  8  9 10 11
    //   12 13 14 15 16 17
    //   18 19 20 21 22 23
    //   24 25 26 27 28 29
    //   30 31 32 33 34 35

    ATA.coeffRef (0) += a * a;
    ATA.coeffRef (1) += a * b;
    ATA.coeffRef (2) += a * c;
    ATA.coeffRef (3) += a * nx;
    ATA.coeffRef (4) += a * ny;
    ATA.coeffRef (5) += a * nz;
    ATA.coeffRef (7) += b * b;
    ATA.coeffRef (8) += b * c;
    ATA.coeffRef (9) += b * nx;
    ATA.coeffRef (10) += b * ny;
    ATA.coeffRef (11) += b * nz;
    ATA.coeffRef (14) += c * c;
    ATA.coeffRef (15) += c * nx;
    ATA.coeffRef (16) += c * ny;
    ATA.coeffRef (17) += c * nz;
    ATA.coeffRef (21) += nx * nx;
    ATA.coeffRef (22) += nx * ny;
    ATA.coeffRef (23) += nx * nz;
    ATA.coeffRef (28) += ny * ny;
    ATA.coeffRef (29) += ny * nz;
    ATA.coeffRef (35) += nz * nz;

    double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
    ATb.coeffRef (0) += a * d;
    ATb.coeffRef (1) += b * d;
    ATb.coeffRef (2) += c * d;
    ATb.coeffRef (3) += nx * d;
    ATb.coeffRef (4) += ny * d;
    ATb.coeffRef (5) += nz * d;

    ++ source_it;
    ++ target_it;
    ++ weights_it;
  }

  ATA.coeffRef (6) = ATA.coeff (1);
  ATA.coeffRef (12) = ATA.coeff (2);
  ATA.coeffRef (13) = ATA.coeff (8);
  ATA.coeffRef (18) = ATA.coeff (3);
  ATA.coeffRef (19) = ATA.coeff (9);
  ATA.coeffRef (20) = ATA.coeff (15);
  ATA.coeffRef (24) = ATA.coeff (4);
  ATA.coeffRef (25) = ATA.coeff (10);
  ATA.coeffRef (26) = ATA.coeff (16);
  ATA.coeffRef (27) = ATA.coeff (22);
  ATA.coeffRef (30) = ATA.coeff (5);
  ATA.coeffRef (31) = ATA.coeff (11);
  ATA.coeffRef (32) = ATA.coeff (17);
  ATA.coeffRef (33) = ATA.coeff (23);
  ATA.coeffRef (34) = ATA.coeff (29);

  // Solve A*x = b
  Vector6d x = static_cast<Vector6d> (ATA.inverse () * ATb);

  // Construct the transformation matrix from x
  constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
}