41 template <
typename Po
intT,
typename Scalar>
void
44 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
46 if (&cloud_in != &cloud_out)
62 for (
size_t i = 0; i < cloud_out.
points.size (); ++i)
65 Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z);
66 cloud_out[i].x =
static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
67 cloud_out[i].y =
static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
68 cloud_out[i].z =
static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
75 for (
size_t i = 0; i < cloud_out.
points.size (); ++i)
77 if (!pcl_isfinite (cloud_in.
points[i].x) ||
78 !pcl_isfinite (cloud_in.
points[i].y) ||
79 !pcl_isfinite (cloud_in.
points[i].z))
82 Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z);
83 cloud_out[i].x =
static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
84 cloud_out[i].y =
static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
85 cloud_out[i].z =
static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
91 template <
typename Po
intT,
typename Scalar>
void
93 const std::vector<int> &indices,
95 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
97 size_t npts = indices.size ();
101 cloud_out.
width =
static_cast<int> (npts);
103 cloud_out.
points.resize (npts);
110 for (
size_t i = 0; i < npts; ++i)
115 Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
116 cloud_out[i].x =
static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
117 cloud_out[i].y =
static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
118 cloud_out[i].z =
static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
125 for (
size_t i = 0; i < npts; ++i)
127 if (!pcl_isfinite (cloud_in.
points[indices[i]].x) ||
128 !pcl_isfinite (cloud_in.
points[indices[i]].y) ||
129 !pcl_isfinite (cloud_in.
points[indices[i]].z))
133 Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
134 cloud_out[i].x =
static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
135 cloud_out[i].y =
static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
136 cloud_out[i].z =
static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
142 template <
typename Po
intT,
typename Scalar>
void
145 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
147 if (&cloud_in != &cloud_out)
163 for (
size_t i = 0; i < cloud_out.
points.size (); ++i)
166 Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z);
167 cloud_out[i].x =
static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
168 cloud_out[i].y =
static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
169 cloud_out[i].z =
static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
173 Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[i].normal_x, cloud_in[i].normal_y, cloud_in[i].normal_z);
174 cloud_out[i].normal_x =
static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
175 cloud_out[i].normal_y =
static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
176 cloud_out[i].normal_z =
static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
182 for (
size_t i = 0; i < cloud_out.
points.size (); ++i)
184 if (!pcl_isfinite (cloud_in.
points[i].x) ||
185 !pcl_isfinite (cloud_in.
points[i].y) ||
186 !pcl_isfinite (cloud_in.
points[i].z))
190 Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z);
191 cloud_out[i].x =
static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
192 cloud_out[i].y =
static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
193 cloud_out[i].z =
static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
197 Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[i].normal_x, cloud_in[i].normal_y, cloud_in[i].normal_z);
198 cloud_out[i].normal_x =
static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
199 cloud_out[i].normal_y =
static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
200 cloud_out[i].normal_z =
static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
206 template <
typename Po
intT,
typename Scalar>
void
208 const std::vector<int> &indices,
210 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
212 size_t npts = indices.size ();
216 cloud_out.
width =
static_cast<int> (npts);
218 cloud_out.
points.resize (npts);
225 for (
size_t i = 0; i < cloud_out.
points.size (); ++i)
228 Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
229 cloud_out[i].x =
static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
230 cloud_out[i].y =
static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
231 cloud_out[i].z =
static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
235 Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[indices[i]].normal_x, cloud_in[indices[i]].normal_y, cloud_in[indices[i]].normal_z);
236 cloud_out[i].normal_x =
static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
237 cloud_out[i].normal_y =
static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
238 cloud_out[i].normal_z =
static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
244 for (
size_t i = 0; i < cloud_out.
points.size (); ++i)
246 if (!pcl_isfinite (cloud_in.
points[indices[i]].x) ||
247 !pcl_isfinite (cloud_in.
points[indices[i]].y) ||
248 !pcl_isfinite (cloud_in.
points[indices[i]].z))
252 Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
253 cloud_out[i].x =
static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
254 cloud_out[i].y =
static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
255 cloud_out[i].z =
static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
259 Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[indices[i]].normal_x, cloud_in[indices[i]].normal_y, cloud_in[indices[i]].normal_z);
260 cloud_out[i].normal_x =
static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
261 cloud_out[i].normal_y =
static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
262 cloud_out[i].normal_z =
static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
268 template <
typename Po
intT,
typename Scalar>
inline void
271 const Eigen::Matrix<Scalar, 3, 1> &offset,
272 const Eigen::Quaternion<Scalar> &rotation)
274 Eigen::Translation<Scalar, 3> translation (offset);
276 Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
281 template <
typename Po
intT,
typename Scalar>
inline void
284 const Eigen::Matrix<Scalar, 3, 1> &offset,
285 const Eigen::Quaternion<Scalar> &rotation)
287 Eigen::Translation<Scalar, 3> translation (offset);
289 Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
294 template <
typename Po
intT,
typename Scalar>
inline PointT
296 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
300 ret.x =
static_cast<float> (transform (0, 0) * point.x + transform (0, 1) * point.y + transform (0, 2) * point.z + transform (0, 3));
301 ret.y =
static_cast<float> (transform (1, 0) * point.x + transform (1, 1) * point.y + transform (1, 2) * point.z + transform (1, 3));
302 ret.z =
static_cast<float> (transform (2, 0) * point.x + transform (2, 1) * point.y + transform (2, 2) * point.z + transform (2, 3));
308 template <
typename Po
intT,
typename Scalar>
double
310 Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
313 Eigen::Matrix<Scalar, 4, 1> centroid;
318 Eigen::Matrix<Scalar, 3, 1> eigen_vals;
319 pcl::eigen33 (covariance_matrix, eigen_vects, eigen_vals);
321 double rel1 = eigen_vals.coeff (0) / eigen_vals.coeff (1);
322 double rel2 = eigen_vals.coeff (1) / eigen_vals.coeff (2);
324 transform.translation () = centroid.head (3);
325 transform.linear () = eigen_vects;
327 return (std::min (rel1, rel2));
void transformPoint(const Eigen::Matrix< Scalar, 3, 1 > &point_in, Eigen::Matrix< Scalar, 3, 1 > &point_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transformation)
Transform a point using an affine matrix.
double getPrincipalTransformation(const pcl::PointCloud< PointT > &cloud, Eigen::Transform< Scalar, 3, Eigen::Affine > &transform)
Calculates the principal (PCA-based) alignment of the point cloud.
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
struct pcl::_PointXYZHSV EIGEN_ALIGN16
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
uint32_t width
The point cloud width (if organized as an image-structure).
void transformPointCloudWithNormals(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform)
Transform a point cloud and rotate its normals using an Eigen transform.
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform)
Apply an affine transform defined by an Eigen Transform.
pcl::PCLHeader header
The point cloud header.
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void transformPointCloudWithNormals(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform)
Transform a point cloud and rotate its normals using an Eigen transform.
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
A point structure representing Euclidean xyz coordinates, and the RGB color.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
uint32_t height
The point cloud height (if organized as an image-structure).