Skip to content

Commit

Permalink
Merge pull request #805 from taketwo/fix-transform
Browse files Browse the repository at this point in the history 
Add "copy_all_fields" option to transformPointCloud***() functions
  • Loading branch information
@jspricke
jspricke committed Sep 11, 2014
2 parents 62ef7bd + e3d63c5 commit 8d6103b
Show file tree
Hide file tree
Showing 3 changed files with 224 additions and 64 deletions.
48 changes: 34 additions & 14 deletions common/include/pcl/common/impl/transforms.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,17 +41,20 @@
template <typename PointT, typename Scalar> void
pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
bool copy_all_fields)
{
if (&cloud_in != &cloud_out)
{
// Note: could be replaced by cloud_out = cloud_in
cloud_out.header = cloud_in.header;
cloud_out.is_dense = cloud_in.is_dense;
cloud_out.width = cloud_in.width;
cloud_out.height = cloud_in.height;
cloud_out.points.reserve (cloud_out.points.size ());
cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
cloud_out.points.reserve (cloud_in.points.size ());
if (copy_all_fields)
cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
else
cloud_out.points.resize (cloud_in.points.size ());
cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
}
Expand Down Expand Up @@ -92,7 +95,8 @@ template <typename PointT, typename Scalar> void
pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
const std::vector<int> &indices,
pcl::PointCloud<PointT> &cloud_out,
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
bool copy_all_fields)
{
size_t npts = indices.size ();
// In order to transform the data, we need to remove NaNs
Expand All @@ -110,7 +114,8 @@ pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
for (size_t i = 0; i < npts; ++i)
{
// Copy fields first, then transform xyz data
//cloud_out.points[i] = cloud_in.points[indices[i]];
if (copy_all_fields)
cloud_out.points[i] = cloud_in.points[indices[i]];
//cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
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));
Expand All @@ -124,11 +129,12 @@ pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
// otherwise we get errors during the multiplication (?)
for (size_t i = 0; i < npts; ++i)
{
if (copy_all_fields)
cloud_out.points[i] = cloud_in.points[indices[i]];
if (!pcl_isfinite (cloud_in.points[indices[i]].x) ||
!pcl_isfinite (cloud_in.points[indices[i]].y) ||
!pcl_isfinite (cloud_in.points[indices[i]].z))
continue;
//cloud_out.points[i] = cloud_in.points[indices[i]];
//cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
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));
Expand All @@ -142,7 +148,8 @@ pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
template <typename PointT, typename Scalar> void
pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
bool copy_all_fields)
{
if (&cloud_in != &cloud_out)
{
Expand All @@ -152,7 +159,10 @@ pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
cloud_out.height = cloud_in.height;
cloud_out.is_dense = cloud_in.is_dense;
cloud_out.points.reserve (cloud_out.points.size ());
cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
if (copy_all_fields)
cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
else
cloud_out.points.resize (cloud_in.points.size ());
cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
}
Expand Down Expand Up @@ -207,7 +217,8 @@ template <typename PointT, typename Scalar> void
pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
const std::vector<int> &indices,
pcl::PointCloud<PointT> &cloud_out,
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
bool copy_all_fields)
{
size_t npts = indices.size ();
// In order to transform the data, we need to remove NaNs
Expand All @@ -224,6 +235,9 @@ pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
{
for (size_t i = 0; i < cloud_out.points.size (); ++i)
{
// Copy fields first, then transform
if (copy_all_fields)
cloud_out.points[i] = cloud_in.points[indices[i]];
//cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
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));
Expand All @@ -243,6 +257,10 @@ pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
{
for (size_t i = 0; i < cloud_out.points.size (); ++i)
{
// Copy fields first, then transform
if (copy_all_fields)
cloud_out.points[i] = cloud_in.points[indices[i]];

if (!pcl_isfinite (cloud_in.points[indices[i]].x) ||
!pcl_isfinite (cloud_in.points[indices[i]].y) ||
!pcl_isfinite (cloud_in.points[indices[i]].z))
Expand All @@ -269,25 +287,27 @@ template <typename PointT, typename Scalar> inline void
pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
const Eigen::Matrix<Scalar, 3, 1> &offset,
const Eigen::Quaternion<Scalar> &rotation)
const Eigen::Quaternion<Scalar> &rotation,
bool copy_all_fields)
{
Eigen::Translation<Scalar, 3> translation (offset);
// Assemble an Eigen Transform
Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
transformPointCloud (cloud_in, cloud_out, t);
transformPointCloud (cloud_in, cloud_out, t, copy_all_fields);
}

///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename Scalar> inline void
pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
const Eigen::Matrix<Scalar, 3, 1> &offset,
const Eigen::Quaternion<Scalar> &rotation)
const Eigen::Quaternion<Scalar> &rotation,
bool copy_all_fields)
{
Eigen::Translation<Scalar, 3> translation (offset);
// Assemble an Eigen Transform
Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
transformPointCloudWithNormals (cloud_in, cloud_out, t);
transformPointCloudWithNormals (cloud_in, cloud_out, t, copy_all_fields);
}

///////////////////////////////////////////////////////////////////////////////////////////
Expand Down
Loading

0 comments on commit 8d6103b

Please sign in to comment.