diff --git a/common/include/pcl/common/impl/transforms.hpp b/common/include/pcl/common/impl/transforms.hpp
index 22443fac664..fc9f4a09adf 100644
--- a/common/include/pcl/common/impl/transforms.hpp
+++ b/common/include/pcl/common/impl/transforms.hpp
@@ -1,7 +1,10 @@
 /*
  * Software License Agreement (BSD License)
  *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
  *  Copyright (c) 2010, Willow Garage, Inc.
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -34,11 +37,11 @@
  *
  */
 
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> void
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT, typename Scalar> void
 pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
                           pcl::PointCloud<PointT> &cloud_out,
-                          const Eigen::Affine3f &transform)
+                          const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
 {
   if (&cloud_in != &cloud_out)
   {
@@ -55,8 +58,12 @@ pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
   {
     // If the dataset is dense, simply transform it!
     for (size_t i = 0; i < cloud_out.points.size (); ++i)
-      cloud_out.points[i].getVector3fMap () = transform * 
-                                              cloud_in.points[i].getVector3fMap ();
+    {
+      //cloud_out.points[i].getVector3fMap () = transform * cloud_in.points[i].getVector3fMap ();
+      cloud_out[i].x = static_cast<float> (transform (0, 0) * cloud_in[i].x + transform (0, 1) * cloud_in[i].y + transform (0, 2) * cloud_in[i].z + transform (0, 3));
+      cloud_out[i].y = static_cast<float> (transform (1, 0) * cloud_in[i].x + transform (1, 1) * cloud_in[i].y + transform (1, 2) * cloud_in[i].z + transform (1, 3));
+      cloud_out[i].z = static_cast<float> (transform (2, 0) * cloud_in[i].x + transform (2, 1) * cloud_in[i].y + transform (2, 2) * cloud_in[i].z + transform (2, 3));
+    }
   }
   else
   {
@@ -68,24 +75,26 @@ pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
           !pcl_isfinite (cloud_in.points[i].y) || 
           !pcl_isfinite (cloud_in.points[i].z))
         continue;
-      cloud_out.points[i].getVector3fMap () = transform * 
-                                              cloud_in.points[i].getVector3fMap ();
+      //cloud_out.points[i].getVector3fMap () = transform * cloud_in.points[i].getVector3fMap ();
+      cloud_out[i].x = static_cast<float> (transform (0, 0) * cloud_in[i].x + transform (0, 1) * cloud_in[i].y + transform (0, 2) * cloud_in[i].z + transform (0, 3));
+      cloud_out[i].y = static_cast<float> (transform (1, 0) * cloud_in[i].x + transform (1, 1) * cloud_in[i].y + transform (1, 2) * cloud_in[i].z + transform (1, 3));
+      cloud_out[i].z = static_cast<float> (transform (2, 0) * cloud_in[i].x + transform (2, 1) * cloud_in[i].y + transform (2, 2) * cloud_in[i].z + transform (2, 3));
     }
   }
 }
 
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> void
+///////////////////////////////////////////////////////////////////////////////////////////
+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::Affine3f &transform)
+                          const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
 {
   size_t npts = indices.size ();
   // In order to transform the data, we need to remove NaNs
   cloud_out.is_dense = cloud_in.is_dense;
   cloud_out.header   = cloud_in.header;
-  cloud_out.width    = npts;
+  cloud_out.width    = static_cast<int> (npts);
   cloud_out.height   = 1;
   cloud_out.points.resize (npts);
 
@@ -95,8 +104,17 @@ 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]]; 
-      cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
+      //cloud_out.points[i] = cloud_in.points[indices[i]]; 
+      //cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
+      cloud_out[i].x = static_cast<float> (transform (0, 0) * cloud_in[indices[i]].x + 
+                                           transform (0, 1) * cloud_in[indices[i]].y + 
+                                           transform (0, 2) * cloud_in[indices[i]].z + transform (0, 3));
+      cloud_out[i].y = static_cast<float> (transform (1, 0) * cloud_in[indices[i]].x + 
+                                           transform (1, 1) * cloud_in[indices[i]].y + 
+                                           transform (1, 2) * cloud_in[indices[i]].z + transform (1, 3));
+      cloud_out[i].z = static_cast<float> (transform (2, 0) * cloud_in[indices[i]].x + 
+                                           transform (2, 1) * cloud_in[indices[i]].y + 
+                                           transform (2, 2) * cloud_in[indices[i]].z + transform (2, 3));
     }
   }
   else
@@ -109,17 +127,26 @@ pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
           !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 ();
+      //cloud_out.points[i] = cloud_in.points[indices[i]]; 
+      //cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
+      cloud_out[i].x = static_cast<float> (transform (0, 0) * cloud_in[indices[i]].x + 
+                                           transform (0, 1) * cloud_in[indices[i]].y + 
+                                           transform (0, 2) * cloud_in[indices[i]].z + transform (0, 3));
+      cloud_out[i].y = static_cast<float> (transform (1, 0) * cloud_in[indices[i]].x + 
+                                           transform (1, 1) * cloud_in[indices[i]].y + 
+                                           transform (1, 2) * cloud_in[indices[i]].z + transform (1, 3));
+      cloud_out[i].z = static_cast<float> (transform (2, 0) * cloud_in[indices[i]].x + 
+                                           transform (2, 1) * cloud_in[indices[i]].y + 
+                                           transform (2, 2) * cloud_in[indices[i]].z + transform (2, 3));
     }
   }
 }
 
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> void
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT, typename Scalar> void
 pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
                                      pcl::PointCloud<PointT> &cloud_out,
-                                     const Eigen::Affine3f &transform)
+                                     const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
 {
   if (&cloud_in != &cloud_out)
   {
@@ -137,12 +164,16 @@ pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
   {
     for (size_t i = 0; i < cloud_out.points.size (); ++i)
     {
-      cloud_out.points[i].getVector3fMap() = transform * 
-                                             cloud_in.points[i].getVector3fMap ();
+      //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
+      cloud_out[i].x = static_cast<float> (transform (0, 0) * cloud_in[i].x + transform (0, 1) * cloud_in[i].y + transform (0, 2) * cloud_in[i].z + transform (0, 3));
+      cloud_out[i].y = static_cast<float> (transform (1, 0) * cloud_in[i].x + transform (1, 1) * cloud_in[i].y + transform (1, 2) * cloud_in[i].z + transform (1, 3));
+      cloud_out[i].z = static_cast<float> (transform (2, 0) * cloud_in[i].x + transform (2, 1) * cloud_in[i].y + transform (2, 2) * cloud_in[i].z + transform (2, 3));
 
       // Rotate normals
-      cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * 
-                                                   cloud_in.points[i].getNormalVector3fMap ();
+      //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap ();
+      cloud_out[i].normal_x = static_cast<float> (transform (0, 0) * cloud_in[i].normal_x + transform (0, 1) * cloud_in[i].normal_y + transform (0, 2) * cloud_in[i].normal_z);
+      cloud_out[i].normal_y = static_cast<float> (transform (1, 0) * cloud_in[i].normal_x + transform (1, 1) * cloud_in[i].normal_y + transform (1, 2) * cloud_in[i].normal_z);
+      cloud_out[i].normal_z = static_cast<float> (transform (2, 0) * cloud_in[i].normal_x + transform (2, 1) * cloud_in[i].normal_y + transform (2, 2) * cloud_in[i].normal_z);
     }
   }
   // Dataset might contain NaNs and Infs, so check for them first.
@@ -154,88 +185,63 @@ pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
           !pcl_isfinite (cloud_in.points[i].y) || 
           !pcl_isfinite (cloud_in.points[i].z))
         continue;
-      cloud_out.points[i].getVector3fMap() = transform * 
-                                             cloud_in.points[i].getVector3fMap ();
 
-      // Rotate normals
-      cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * 
-                                                   cloud_in.points[i].getNormalVector3fMap ();
-    }
-  }
-}
+      //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
+      cloud_out[i].x = static_cast<float> (transform (0, 0) * cloud_in[i].x + transform (0, 1) * cloud_in[i].y + transform (0, 2) * cloud_in[i].z + transform (0, 3));
+      cloud_out[i].y = static_cast<float> (transform (1, 0) * cloud_in[i].x + transform (1, 1) * cloud_in[i].y + transform (1, 2) * cloud_in[i].z + transform (1, 3));
+      cloud_out[i].z = static_cast<float> (transform (2, 0) * cloud_in[i].x + transform (2, 1) * cloud_in[i].y + transform (2, 2) * cloud_in[i].z + transform (2, 3));
 
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> void
-pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
-                          pcl::PointCloud<PointT> &cloud_out,
-                          const Eigen::Matrix4f &transform)
-{
-  if (&cloud_in != &cloud_out)
-  {
-    // Note: could be replaced by cloud_out = cloud_in
-    cloud_out.header   = cloud_in.header;
-    cloud_out.width    = cloud_in.width;
-    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 ());
-  }
-
-  Eigen::Matrix3f rot   = transform.block<3, 3> (0, 0);
-  Eigen::Vector3f trans = transform.block<3, 1> (0, 3);
-  // If the data is dense, we don't need to check for NaN
-  if (cloud_in.is_dense)
-  {
-    for (size_t i = 0; i < cloud_out.points.size (); ++i)
-      cloud_out.points[i].getVector3fMap () = rot * 
-                                              cloud_in.points[i].getVector3fMap () + trans;
-  }
-  // Dataset might contain NaNs and Infs, so check for them first.
-  else
-  {
-    for (size_t i = 0; i < cloud_out.points.size (); ++i)
-    {
-      if (!pcl_isfinite (cloud_in.points[i].x) || 
-          !pcl_isfinite (cloud_in.points[i].y) || 
-          !pcl_isfinite (cloud_in.points[i].z))
-        continue;
-      cloud_out.points[i].getVector3fMap () = rot * 
-                                              cloud_in.points[i].getVector3fMap () + trans;
+      // Rotate normals
+      //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap ();
+      cloud_out[i].normal_x = static_cast<float> (transform (0, 0) * cloud_in[i].normal_x + transform (0, 1) * cloud_in[i].normal_y + transform (0, 2) * cloud_in[i].normal_z);
+      cloud_out[i].normal_y = static_cast<float> (transform (1, 0) * cloud_in[i].normal_x + transform (1, 1) * cloud_in[i].normal_y + transform (1, 2) * cloud_in[i].normal_z);
+      cloud_out[i].normal_z = static_cast<float> (transform (2, 0) * cloud_in[i].normal_x + transform (2, 1) * cloud_in[i].normal_y + transform (2, 2) * cloud_in[i].normal_z);
     }
   }
 }
 
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> void
+///////////////////////////////////////////////////////////////////////////////////////////
+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::Matrix4f &transform)
+                                     const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
 {
-  if (&cloud_in != &cloud_out)
-  {
-    // Note: could be replaced by cloud_out = cloud_in
-    cloud_out.header   = cloud_in.header;
-    cloud_out.width    = cloud_in.width;
-    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 ());
-  }
-
-  Eigen::Matrix3f rot   = transform.block<3, 3> (0, 0);
-  Eigen::Vector3f trans = transform.block<3, 1> (0, 3);
+  size_t npts = indices.size ();
+  // In order to transform the data, we need to remove NaNs
+  cloud_out.is_dense = cloud_in.is_dense;
+  cloud_out.header   = cloud_in.header;
+  cloud_out.width    = static_cast<int> (npts);
+  cloud_out.height   = 1;
+  cloud_out.points.resize (npts);
 
   // If the data is dense, we don't need to check for NaN
   if (cloud_in.is_dense)
   {
     for (size_t i = 0; i < cloud_out.points.size (); ++i)
     {
-      cloud_out.points[i].getVector3fMap () = rot * 
-                                              cloud_in.points[i].getVector3fMap () + trans;
+      //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
+      cloud_out[i].x = static_cast<float> (transform (0, 0) * cloud_in[indices[i]].x + 
+                                           transform (0, 1) * cloud_in[indices[i]].y + 
+                                           transform (0, 2) * cloud_in[indices[i]].z + transform (0, 3));
+      cloud_out[i].y = static_cast<float> (transform (1, 0) * cloud_in[indices[i]].x + 
+                                           transform (1, 1) * cloud_in[indices[i]].y + 
+                                           transform (1, 2) * cloud_in[indices[i]].z + transform (1, 3));
+      cloud_out[i].z = static_cast<float> (transform (2, 0) * cloud_in[indices[i]].x + 
+                                           transform (2, 1) * cloud_in[indices[i]].y + 
+                                           transform (2, 2) * cloud_in[indices[i]].z + transform (2, 3));
 
       // Rotate normals
-      cloud_out.points[i].getNormalVector3fMap() = rot * 
-                                                   cloud_in.points[i].getNormalVector3fMap ();
+      //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap ();
+      cloud_out[i].normal_x = static_cast<float> (transform (0, 0) * cloud_in[indices[i]].normal_x + 
+                                                  transform (0, 1) * cloud_in[indices[i]].normal_y + 
+                                                  transform (0, 2) * cloud_in[indices[i]].normal_z);
+      cloud_out[i].normal_y = static_cast<float> (transform (1, 0) * cloud_in[indices[i]].normal_x + 
+                                                  transform (1, 1) * cloud_in[indices[i]].normal_y + 
+                                                  transform (1, 2) * cloud_in[indices[i]].normal_z);
+      cloud_out[i].normal_z = static_cast<float> (transform (2, 0) * cloud_in[indices[i]].normal_x + 
+                                                  transform (2, 1) * cloud_in[indices[i]].normal_y + 
+                                                  transform (2, 2) * cloud_in[indices[i]].normal_z);
     }
   }
   // Dataset might contain NaNs and Infs, so check for them first.
@@ -243,76 +249,97 @@ pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
   {
     for (size_t i = 0; i < cloud_out.points.size (); ++i)
     {
-      if (!pcl_isfinite (cloud_in.points[i].x) || 
-          !pcl_isfinite (cloud_in.points[i].y) || 
-          !pcl_isfinite (cloud_in.points[i].z))
+      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].getVector3fMap () = rot * 
-                                              cloud_in.points[i].getVector3fMap () + trans;
+
+      //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
+      cloud_out[i].x = static_cast<float> (transform (0, 0) * cloud_in[indices[i]].x + 
+                                           transform (0, 1) * cloud_in[indices[i]].y + 
+                                           transform (0, 2) * cloud_in[indices[i]].z + transform (0, 3));
+      cloud_out[i].y = static_cast<float> (transform (1, 0) * cloud_in[indices[i]].x + 
+                                           transform (1, 1) * cloud_in[indices[i]].y + 
+                                           transform (1, 2) * cloud_in[indices[i]].z + transform (1, 3));
+      cloud_out[i].z = static_cast<float> (transform (2, 0) * cloud_in[indices[i]].x + 
+                                           transform (2, 1) * cloud_in[indices[i]].y + 
+                                           transform (2, 2) * cloud_in[indices[i]].z + transform (2, 3));
 
       // Rotate normals
-      cloud_out.points[i].getNormalVector3fMap() = rot * 
-                                                   cloud_in.points[i].getNormalVector3fMap ();
+      //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap ();
+      cloud_out[i].normal_x = static_cast<float> (transform (0, 0) * cloud_in[indices[i]].normal_x + 
+                                                  transform (0, 1) * cloud_in[indices[i]].normal_y + 
+                                                  transform (0, 2) * cloud_in[indices[i]].normal_z);
+      cloud_out[i].normal_y = static_cast<float> (transform (1, 0) * cloud_in[indices[i]].normal_x + 
+                                                  transform (1, 1) * cloud_in[indices[i]].normal_y + 
+                                                  transform (1, 2) * cloud_in[indices[i]].normal_z);
+      cloud_out[i].normal_z = static_cast<float> (transform (2, 0) * cloud_in[indices[i]].normal_x + 
+                                                  transform (2, 1) * cloud_in[indices[i]].normal_y + 
+                                                  transform (2, 2) * cloud_in[indices[i]].normal_z);
     }
   }
 }
 
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> inline void
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT, typename Scalar> inline void
 pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
                           pcl::PointCloud<PointT> &cloud_out,
-                          const Eigen::Vector3f &offset, 
-                          const Eigen::Quaternionf &rotation)
+                          const Eigen::Matrix<Scalar, 3, 1> &offset, 
+                          const Eigen::Quaternion<Scalar> &rotation)
 {
-  Eigen::Translation3f translation (offset);
+  Eigen::Translation<Scalar, 3> translation (offset);
   // Assemble an Eigen Transform
-  Eigen::Affine3f t;
-  t = translation * rotation;
+  Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
   transformPointCloud (cloud_in, cloud_out, t);
 }
 
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> inline void
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT, typename Scalar> inline void
 pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
                                      pcl::PointCloud<PointT> &cloud_out,
-                                     const Eigen::Vector3f &offset, 
-                                     const Eigen::Quaternionf &rotation)
+                                     const Eigen::Matrix<Scalar, 3, 1> &offset, 
+                                     const Eigen::Quaternion<Scalar> &rotation)
 {
-  Eigen::Translation3f translation (offset);
+  Eigen::Translation<Scalar, 3> translation (offset);
   // Assemble an Eigen Transform
-  Eigen::Affine3f t;
-  t = translation * rotation;
+  Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
   transformPointCloudWithNormals (cloud_in, cloud_out, t);
 }
 
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> inline PointT
-pcl::transformPoint (const PointT &point, const Eigen::Affine3f &transform)
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT, typename Scalar> inline PointT
+pcl::transformPoint (const PointT &point, 
+                     const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
 {
   PointT ret = point;
-  ret.getVector3fMap () = transform * point.getVector3fMap ();
-  return ret;
+  //ret.getVector3fMap () = transform * point.getVector3fMap ();
+  ret.x = static_cast<float> (transform (0, 0) * point.x + transform (0, 1) * point.y + transform (0, 2) * point.z + transform (0, 3));
+  ret.y = static_cast<float> (transform (1, 0) * point.x + transform (1, 1) * point.y + transform (1, 2) * point.z + transform (1, 3));
+  ret.z = static_cast<float> (transform (2, 0) * point.x + transform (2, 1) * point.y + transform (2, 2) * point.z + transform (2, 3));
+
+  return (ret);
 }
 
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> double
-pcl::getPrincipalTransformation (const pcl::PointCloud<PointT> &cloud, Eigen::Affine3f &transform)
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT, typename Scalar> double
+pcl::getPrincipalTransformation (const pcl::PointCloud<PointT> &cloud, 
+                                 Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
 {
-  EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
-  Eigen::Vector4f centroid;
+  EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
+  Eigen::Matrix<Scalar, 4, 1> centroid;
   
   pcl::computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid);
 
-  EIGEN_ALIGN16 Eigen::Matrix3f eigen_vects;
-  Eigen::Vector3f eigen_vals;
+  EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> eigen_vects;
+  Eigen::Matrix<Scalar, 3, 1> eigen_vals;
   pcl::eigen33 (covariance_matrix, eigen_vects, eigen_vals);
 
   double rel1 = eigen_vals.coeff (0) / eigen_vals.coeff (1);
   double rel2 = eigen_vals.coeff (1) / eigen_vals.coeff (2);
   
-  transform.translation () = centroid.head<3> ();
+  transform.translation () = centroid.head (3);
   transform.linear () = eigen_vects;
   
-  return std::min (rel1, rel2);
+  return (std::min (rel1, rel2));
 }
 
diff --git a/common/include/pcl/common/transforms.h b/common/include/pcl/common/transforms.h
index 36fb3ff9b5b..fce3498d4f5 100644
--- a/common/include/pcl/common/transforms.h
+++ b/common/include/pcl/common/transforms.h
@@ -1,7 +1,10 @@
 /*
  * Software License Agreement (BSD License)
  *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
  *  Copyright (c) 2010, Willow Garage, Inc.
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -40,114 +43,512 @@
 #include <pcl/point_types.h>
 #include <pcl/common/centroid.h>
 #include <pcl/common/eigen.h>
+#include <pcl/PointIndices.h>
 
 namespace pcl
 {
   /** \brief Apply an affine transform defined by an Eigen Transform
-    * \param cloud_in the input point cloud
-    * \param cloud_out the resultant output point cloud
-    * \param transform an affine transformation (typically a rigid transformation)
+    * \param[in] cloud_in the input point cloud
+    * \param[out] cloud_out the resultant output point cloud
+    * \param[in] transform an affine transformation (typically a rigid transformation)
     * \note Can be used with cloud_in equal to cloud_out
     * \ingroup common
     */
+  template <typename PointT, typename Scalar> void 
+  transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
+                       pcl::PointCloud<PointT> &cloud_out, 
+                       const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform);
+
   template <typename PointT> void 
   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
                        pcl::PointCloud<PointT> &cloud_out, 
-                       const Eigen::Affine3f &transform);
+                       const Eigen::Affine3f &transform)
+  {
+    return (transformPointCloud<PointT, float> (cloud_in, cloud_out, transform));
+  }
+
+  template <typename PointT> void 
+  transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
+                       pcl::PointCloud<PointT> &cloud_out, 
+                       const Eigen::Affine3d &transform)
+  {
+    return (transformPointCloud<PointT, double> (cloud_in, cloud_out, transform));
+  }
 
   /** \brief Apply an affine transform defined by an Eigen Transform
-    * \param cloud_in the input point cloud
-    * \param indices the set of point indices to use from the input point cloud
-    * \param cloud_out the resultant output point cloud
-    * \param transform an affine transformation (typically a rigid transformation)
-    * \note Can be used with cloud_in equal to cloud_out
+    * \param[in] cloud_in the input point cloud
+    * \param[in] indices the set of point indices to use from the input point cloud
+    * \param[out] cloud_out the resultant output point cloud
+    * \param[in] transform an affine transformation (typically a rigid transformation)
     * \ingroup common
     */
+  template <typename PointT, typename Scalar> void 
+  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);
+
   template <typename PointT> void 
   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
                        const std::vector<int> &indices, 
                        pcl::PointCloud<PointT> &cloud_out, 
-                       const Eigen::Affine3f &transform);
+                       const Eigen::Affine3f &transform)
+  {
+    return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform));
+  }
+
+  template <typename PointT> void 
+  transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
+                       const std::vector<int> &indices, 
+                       pcl::PointCloud<PointT> &cloud_out, 
+                       const Eigen::Affine3d &transform)
+  {
+    return (transformPointCloud<PointT, double> (cloud_in, indices, cloud_out, transform));
+  }
+
+  /** \brief Apply an affine transform defined by an Eigen Transform
+    * \param[in] cloud_in the input point cloud
+    * \param[in] indices the set of point indices to use from the input point cloud
+    * \param[out] cloud_out the resultant output point cloud
+    * \param[in] transform an affine transformation (typically a rigid transformation)
+    * \ingroup common
+    */
+  template <typename PointT, typename Scalar> void 
+  transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
+                       const pcl::PointIndices &indices, 
+                       pcl::PointCloud<PointT> &cloud_out, 
+                       const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
+  {
+    return (transformPointCloud<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform));
+  }
+
+  template <typename PointT> void 
+  transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
+                       const pcl::PointIndices &indices, 
+                       pcl::PointCloud<PointT> &cloud_out, 
+                       const Eigen::Affine3f &transform)
+  {
+    return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform));
+  }
+
+  template <typename PointT> void 
+  transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
+                       const pcl::PointIndices &indices, 
+                       pcl::PointCloud<PointT> &cloud_out, 
+                       const Eigen::Affine3d &transform)
+  {
+    return (transformPointCloud<PointT, double> (cloud_in, indices, cloud_out, transform));
+  }
 
   /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
-    * \param cloud_in the input point cloud
-    * \param cloud_out the resultant output point cloud
-    * \param transform an affine transformation (typically a rigid transformation)
+    * \param[in] cloud_in the input point cloud
+    * \param[out] cloud_out the resultant output point cloud
+    * \param[in] transform an affine transformation (typically a rigid transformation)
     * \note Can be used with cloud_in equal to cloud_out
     */
+  template <typename PointT, typename Scalar> void 
+  transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
+                                  pcl::PointCloud<PointT> &cloud_out, 
+                                  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform);
+
   template <typename PointT> void 
   transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
                                   pcl::PointCloud<PointT> &cloud_out, 
-                                  const Eigen::Affine3f &transform);
+                                  const Eigen::Affine3f &transform)
+  {
+    return (transformPointCloudWithNormals<PointT, float> (cloud_in, cloud_out, transform));
+  }
 
-  /** \brief Apply an affine transform defined by an Eigen Transform
-    * \param cloud_in the input point cloud
-    * \param cloud_out the resultant output point cloud
-    * \param transform an affine transformation (typically a rigid transformation)
+  template <typename PointT> void 
+  transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
+                                  pcl::PointCloud<PointT> &cloud_out, 
+                                  const Eigen::Affine3d &transform)
+  {
+    return (transformPointCloudWithNormals<PointT, double> (cloud_in, cloud_out, transform));
+  }
+
+  /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
+    * \param[in] cloud_in the input point cloud
+    * \param[in] indices the set of point indices to use from the input point cloud
+    * \param[out] cloud_out the resultant output point cloud
+    * \param[in] transform an affine transformation (typically a rigid transformation)
+    */
+  template <typename PointT, typename Scalar> void 
+  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);
+
+  template <typename PointT> void 
+  transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
+                                  const std::vector<int> &indices, 
+                                  pcl::PointCloud<PointT> &cloud_out, 
+                                  const Eigen::Affine3f &transform)
+  {
+    return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform));
+  }
+
+  template <typename PointT> void 
+  transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
+                                  const std::vector<int> &indices, 
+                                  pcl::PointCloud<PointT> &cloud_out, 
+                                  const Eigen::Affine3d &transform)
+  {
+    return (transformPointCloudWithNormals<PointT, double> (cloud_in, indices, cloud_out, transform));
+  }
+
+  /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
+    * \param[in] cloud_in the input point cloud
+    * \param[in] indices the set of point indices to use from the input point cloud
+    * \param[out] cloud_out the resultant output point cloud
+    * \param[in] transform an affine transformation (typically a rigid transformation)
+    */
+  template <typename PointT, typename Scalar> void 
+  transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
+                                  const pcl::PointIndices &indices, 
+                                  pcl::PointCloud<PointT> &cloud_out, 
+                                  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
+  {
+    return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform));
+  }
+
+
+  template <typename PointT> void 
+  transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
+                                  const pcl::PointIndices &indices, 
+                                  pcl::PointCloud<PointT> &cloud_out, 
+                                  const Eigen::Affine3f &transform)
+  {
+    return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform));
+  }
+
+  template <typename PointT> void 
+  transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
+                                  const pcl::PointIndices &indices, 
+                                  pcl::PointCloud<PointT> &cloud_out, 
+                                  const Eigen::Affine3d &transform)
+  {
+    return (transformPointCloudWithNormals<PointT, double> (cloud_in, indices, cloud_out, transform));
+  }
+
+  /** \brief Apply a rigid transform defined by a 4x4 matrix
+    * \param[in] cloud_in the input point cloud
+    * \param[out] cloud_out the resultant output point cloud
+    * \param[in] transform a rigid transformation 
     * \note Can be used with cloud_in equal to cloud_out
     * \ingroup common
     */
+  template <typename PointT, typename Scalar> void 
+  transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
+                       pcl::PointCloud<PointT> &cloud_out, 
+                       const Eigen::Matrix<Scalar, 4, 4> &transform)
+  {
+    Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform);
+    return (transformPointCloud<PointT, Scalar> (cloud_in, cloud_out, t));
+  }
+
   template <typename PointT> void 
   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
                        pcl::PointCloud<PointT> &cloud_out, 
-                       const Eigen::Matrix4f &transform);
+                       const Eigen::Matrix4f &transform)
+  {
+    return (transformPointCloud<PointT, float> (cloud_in, cloud_out, transform));
+  }
+
+  template <typename PointT> void 
+  transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
+                       pcl::PointCloud<PointT> &cloud_out, 
+                       const Eigen::Matrix4d &transform)
+  {
+    return (transformPointCloud<PointT, double> (cloud_in, cloud_out, transform));
+  }
+
+  /** \brief Apply a rigid transform defined by a 4x4 matrix
+    * \param[in] cloud_in the input point cloud
+    * \param[in] indices the set of point indices to use from the input point cloud
+    * \param[out] cloud_out the resultant output point cloud
+    * \param[in] transform a rigid transformation 
+    * \ingroup common
+    */
+  template <typename PointT, typename Scalar> void 
+  transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
+                       const std::vector<int> &indices, 
+                       pcl::PointCloud<PointT> &cloud_out, 
+                       const Eigen::Matrix<Scalar, 4, 4> &transform)
+  {
+    Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform);
+    return (transformPointCloud<PointT, Scalar> (cloud_in, indices, cloud_out, t));
+  }
+
+  template <typename PointT> void 
+  transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
+                       const std::vector<int> &indices, 
+                       pcl::PointCloud<PointT> &cloud_out, 
+                       const Eigen::Matrix4f &transform)
+  {
+    return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform));
+  }
+
+  template <typename PointT> void 
+  transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
+                       const std::vector<int> &indices, 
+                       pcl::PointCloud<PointT> &cloud_out, 
+                       const Eigen::Matrix4d &transform)
+  {
+    return (transformPointCloud<PointT, double> (cloud_in, indices, cloud_out, transform));
+  }
+
+  /** \brief Apply a rigid transform defined by a 4x4 matrix
+    * \param[in] cloud_in the input point cloud
+    * \param[in] indices the set of point indices to use from the input point cloud
+    * \param[out] cloud_out the resultant output point cloud
+    * \param[in] transform a rigid transformation 
+    * \ingroup common
+    */
+  template <typename PointT, typename Scalar> void 
+  transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
+                       const pcl::PointIndices &indices, 
+                       pcl::PointCloud<PointT> &cloud_out, 
+                       const Eigen::Matrix<Scalar, 4, 4> &transform)
+  {
+    return (transformPointCloud<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform));
+  }
+
+  template <typename PointT> void 
+  transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
+                       const pcl::PointIndices &indices, 
+                       pcl::PointCloud<PointT> &cloud_out, 
+                       const Eigen::Matrix4f &transform)
+  {
+    return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform));
+  }
+
+  template <typename PointT> void 
+  transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
+                       const pcl::PointIndices &indices, 
+                       pcl::PointCloud<PointT> &cloud_out, 
+                       const Eigen::Matrix4d &transform)
+  {
+    return (transformPointCloud<PointT, double> (cloud_in, indices, cloud_out, transform));
+  }
+
+  /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
+    * \param[in] cloud_in the input point cloud
+    * \param[out] cloud_out the resultant output point cloud
+    * \param[in] transform an affine transformation (typically a rigid transformation)
+    * \note Can be used with cloud_in equal to cloud_out
+    * \ingroup common
+    */
+  template <typename PointT, typename Scalar> void 
+  transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
+                                  pcl::PointCloud<PointT> &cloud_out, 
+                                  const Eigen::Matrix<Scalar, 4, 4> &transform)
+  {
+    Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform);
+    return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, cloud_out, t));
+  }
+
+
+  template <typename PointT> void 
+  transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
+                                  pcl::PointCloud<PointT> &cloud_out, 
+                                  const Eigen::Matrix4f &transform)
+  {
+    return (transformPointCloudWithNormals<PointT, float> (cloud_in, cloud_out, transform));
+  }
+
+  template <typename PointT> void 
+  transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
+                                  pcl::PointCloud<PointT> &cloud_out, 
+                                  const Eigen::Matrix4d &transform)
+  {
+    return (transformPointCloudWithNormals<PointT, double> (cloud_in, cloud_out, transform));
+  }
+
+  /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
+    * \param[in] cloud_in the input point cloud
+    * \param[in] indices the set of point indices to use from the input point cloud
+    * \param[out] cloud_out the resultant output point cloud
+    * \param[in] transform an affine transformation (typically a rigid transformation)
+    * \note Can be used with cloud_in equal to cloud_out
+    * \ingroup common
+    */
+  template <typename PointT, typename Scalar> void 
+  transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
+                                  const std::vector<int> &indices, 
+                                  pcl::PointCloud<PointT> &cloud_out, 
+                                  const Eigen::Matrix<Scalar, 4, 4> &transform)
+  {
+    Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform);
+    return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices, cloud_out, t));
+  }
+
+
+  template <typename PointT> void 
+  transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
+                                  const std::vector<int> &indices, 
+                                  pcl::PointCloud<PointT> &cloud_out, 
+                                  const Eigen::Matrix4f &transform)
+  {
+    return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform));
+  }
+
+  template <typename PointT> void 
+  transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
+                                  const std::vector<int> &indices, 
+                                  pcl::PointCloud<PointT> &cloud_out, 
+                                  const Eigen::Matrix4d &transform)
+  {
+    return (transformPointCloudWithNormals<PointT, double> (cloud_in, indices, cloud_out, transform));
+  }
 
   /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
-    * \param cloud_in the input point cloud
-    * \param cloud_out the resultant output point cloud
-    * \param transform an affine transformation (typically a rigid transformation)
+    * \param[in] cloud_in the input point cloud
+    * \param[in] indices the set of point indices to use from the input point cloud
+    * \param[out] cloud_out the resultant output point cloud
+    * \param[in] transform an affine transformation (typically a rigid transformation)
     * \note Can be used with cloud_in equal to cloud_out
     * \ingroup common
     */
+  template <typename PointT, typename Scalar> void 
+  transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
+                                  const pcl::PointIndices &indices, 
+                                  pcl::PointCloud<PointT> &cloud_out, 
+                                  const Eigen::Matrix<Scalar, 4, 4> &transform)
+  {
+    Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform);
+    return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices, cloud_out, t));
+  }
+
+
+  template <typename PointT> void 
+  transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
+                                  const pcl::PointIndices &indices, 
+                                  pcl::PointCloud<PointT> &cloud_out, 
+                                  const Eigen::Matrix4f &transform)
+  {
+    return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform));
+  }
+
   template <typename PointT> void 
   transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
+                                  const pcl::PointIndices &indices, 
                                   pcl::PointCloud<PointT> &cloud_out, 
-                                  const Eigen::Matrix4f &transform);
+                                  const Eigen::Matrix4d &transform)
+  {
+    return (transformPointCloudWithNormals<PointT, double> (cloud_in, indices, cloud_out, transform));
+  }
 
   /** \brief Apply a rigid transform defined by a 3D offset and a quaternion
-    * \param cloud_in the input point cloud
-    * \param cloud_out the resultant output point cloud
-    * \param offset the translation component of the rigid transformation
-    * \param rotation the rotation component of the rigid transformation
+    * \param[in] cloud_in the input point cloud
+    * \param[out] cloud_out the resultant output point cloud
+    * \param[in] offset the translation component of the rigid transformation
+    * \param[in] rotation the rotation component of the rigid transformation
     * \ingroup common
     */
+  template <typename PointT, typename Scalar> inline void 
+  transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
+                       pcl::PointCloud<PointT> &cloud_out, 
+                       const Eigen::Matrix<Scalar, 3, 1> &offset, 
+                       const Eigen::Quaternion<Scalar> &rotation);
+
   template <typename PointT> inline void 
   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
                        pcl::PointCloud<PointT> &cloud_out, 
                        const Eigen::Vector3f &offset, 
-                       const Eigen::Quaternionf &rotation);
+                       const Eigen::Quaternionf &rotation)
+  {
+    return (transformPointCloud<PointT, float> (cloud_in, cloud_out, offset, rotation));
+  }
+
+  template <typename PointT> inline void 
+  transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
+                       pcl::PointCloud<PointT> &cloud_out, 
+                       const Eigen::Vector3d &offset, 
+                       const Eigen::Quaterniond &rotation)
+  {
+    return (transformPointCloud<PointT, double> (cloud_in, cloud_out, offset, rotation));
+  }
 
   /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
-    * \param cloud_in the input point cloud
-    * \param cloud_out the resultant output point cloud
-    * \param offset the translation component of the rigid transformation
-    * \param rotation the rotation component of the rigid transformation
+    * \param[in] cloud_in the input point cloud
+    * \param[out] cloud_out the resultant output point cloud
+    * \param[in] offset the translation component of the rigid transformation
+    * \param[in] rotation the rotation component of the rigid transformation
     * \ingroup common
     */
-  template <typename PointT> inline void 
+  template <typename PointT, typename Scalar> inline void 
+  transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
+                                  pcl::PointCloud<PointT> &cloud_out, 
+                                  const Eigen::Matrix<Scalar, 3, 1> &offset, 
+                                  const Eigen::Quaternion<Scalar> &rotation);
+
+  template <typename PointT> void 
   transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
                                   pcl::PointCloud<PointT> &cloud_out, 
                                   const Eigen::Vector3f &offset, 
-                                  const Eigen::Quaternionf &rotation);
+                                  const Eigen::Quaternionf &rotation)
+  {
+    return (transformPointCloudWithNormals<PointT, float> (cloud_in, cloud_out, offset, rotation));
+  }
+
+  template <typename PointT> void 
+  transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
+                                  pcl::PointCloud<PointT> &cloud_out, 
+                                  const Eigen::Vector3d &offset, 
+                                  const Eigen::Quaterniond &rotation)
+  {
+    return (transformPointCloudWithNormals<PointT, double> (cloud_in, cloud_out, offset, rotation));
+  }
+
   /** \brief Transform a point with members x,y,z
     * \param[in] point the point to transform
     * \param[out] transform the transformation to apply
     * \return the transformed point
     * \ingroup common
     */
-  template <typename PointT> inline PointT
-  transformPoint (const PointT &point, const Eigen::Affine3f &transform);
+  template <typename PointT, typename Scalar> inline PointT
+  transformPoint (const PointT &point, 
+                  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform);
   
-  /**
-   * \brief calculates the principal (PCA-based) alignment of the point cloud
-   * \param[in] cloud
-   * \param[out] transform
-   * \return the ratio lambda1/lambda2 or lambda2/lambda3, whatever is closer to 1.
-   * \note If the return value is close to one then the transformation might be not unique -> two principal directions have
-   * almost same variance (extend)
-   */
+  template <typename PointT> inline PointT
+  transformPoint (const PointT &point, 
+                  const Eigen::Affine3f &transform)
+  {
+    return (transformPoint<PointT, float> (point, transform));
+  }
+
+  template <typename PointT> inline PointT
+  transformPoint (const PointT &point, 
+                  const Eigen::Affine3d &transform)
+  {
+    return (transformPoint<PointT, double> (point, transform));
+  }
+
+  /** \brief Calculates the principal (PCA-based) alignment of the point cloud
+    * \param[in] cloud the input point cloud
+    * \param[out] transform the resultant transform
+    * \return the ratio lambda1/lambda2 or lambda2/lambda3, whatever is closer to 1.
+    * \note If the return value is close to one then the transformation might be not unique -> two principal directions have
+    * almost same variance (extend)
+    */
+  template <typename PointT, typename Scalar> inline double
+  getPrincipalTransformation (const pcl::PointCloud<PointT> &cloud, 
+                              Eigen::Transform<Scalar, 3, Eigen::Affine> &transform);
+
+  template <typename PointT> inline double
+  getPrincipalTransformation (const pcl::PointCloud<PointT> &cloud, 
+                              Eigen::Affine3f &transform)
+  {
+    return (getPrincipalTransformation<PointT, float> (cloud, transform));
+  }
+
   template <typename PointT> inline double
-  getPrincipalTransformation (const pcl::PointCloud<PointT> &cloud, Eigen::Affine3f &transform);
+  getPrincipalTransformation (const pcl::PointCloud<PointT> &cloud, 
+                              Eigen::Affine3d &transform)
+  {
+    return (getPrincipalTransformation<PointT, double> (cloud, transform));
+  }
 }
 
 #include <pcl/common/impl/transforms.hpp>
diff --git a/test/test_transforms.cpp b/test/test_transforms.cpp
index 010de0dcf74..9d80ec6d084 100644
--- a/test/test_transforms.cpp
+++ b/test/test_transforms.cpp
@@ -189,6 +189,80 @@ TEST (PCL, Transform)
   EXPECT_EQ (1, points2[3].z);
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////////
+TEST (PCL, Matrix4Affine3Transform)
+{
+  float rot_x = 2.8827f;
+  float rot_y = -0.31190f;
+  float rot_z = -0.93058f;
+  Eigen::Affine3f affine;
+  pcl::getTransformation (0, 0, 0, rot_x, rot_y, rot_z, affine);
+
+  EXPECT_FLOAT_EQ (affine (0, 0),  0.56854731f); EXPECT_FLOAT_EQ (affine (0, 1), -0.82217032f); EXPECT_FLOAT_EQ (affine (0, 2), -0.028107658f);
+  EXPECT_FLOAT_EQ (affine (1, 0), -0.76327348f); EXPECT_FLOAT_EQ (affine (1, 1), -0.51445758f); EXPECT_FLOAT_EQ (affine (1, 2), -0.39082864f);
+  EXPECT_FLOAT_EQ (affine (2, 0),  0.30686751f); EXPECT_FLOAT_EQ (affine (2, 1),  0.24365838f); EXPECT_FLOAT_EQ (affine (2, 2), -0.920034f);
+
+  // Approximative!!! Uses SVD internally! See http://eigen.tuxfamily.org/dox/Transform_8h_source.html
+  Eigen::Matrix3f rotation = affine.rotation ();
+
+  EXPECT_NEAR (rotation (0, 0),  0.56854731f, 1e-4); EXPECT_NEAR (rotation (0, 1), -0.82217032f, 1e-4); EXPECT_NEAR (rotation (0, 2), -0.028107658f, 1e-4);
+  EXPECT_NEAR (rotation (1, 0), -0.76327348f, 1e-4); EXPECT_NEAR (rotation (1, 1), -0.51445758f, 1e-4); EXPECT_NEAR (rotation (1, 2), -0.39082864f, 1e-4);
+  EXPECT_NEAR (rotation (2, 0),  0.30686751f, 1e-4); EXPECT_NEAR (rotation (2, 1),  0.24365838f, 1e-4); EXPECT_NEAR (rotation (2, 2), -0.920034f, 1e-4);
+
+  float trans_x, trans_y, trans_z;
+  pcl::getTransformation (0.1f, 0.2f, 0.3f, rot_x, rot_y, rot_z, affine);
+  pcl::getTranslationAndEulerAngles (affine, trans_x, trans_y, trans_z, rot_x, rot_y, rot_z);
+  EXPECT_FLOAT_EQ (trans_x, 0.1f);
+  EXPECT_FLOAT_EQ (trans_y, 0.2f);
+  EXPECT_FLOAT_EQ (trans_z, 0.3f);
+  EXPECT_FLOAT_EQ (rot_x, 2.8827f);
+  EXPECT_FLOAT_EQ (rot_y, -0.31190f);
+  EXPECT_FLOAT_EQ (rot_z, -0.93058f);
+
+  Eigen::Matrix4f transformation (Eigen::Matrix4f::Identity ());
+  transformation.block<3, 3> (0, 0) = affine.rotation ();
+  transformation.block<3, 1> (0, 3) = affine.translation ();
+
+  PointXYZ p (1.f, 2.f, 3.f);
+  Eigen::Vector3f v3 = p.getVector3fMap ();
+  Eigen::Vector4f v4 = p.getVector4fMap ();
+
+  Eigen::Vector3f v3t (affine * v3);
+  Eigen::Vector4f v4t (transformation * v4);
+
+  PointXYZ pt = pcl::transformPoint (p, affine);
+
+  EXPECT_FLOAT_EQ (pt.x, v3t.x ()); EXPECT_FLOAT_EQ (pt.x, v4t.x ());
+  EXPECT_FLOAT_EQ (pt.y, v3t.y ()); EXPECT_FLOAT_EQ (pt.y, v4t.y ());
+  EXPECT_FLOAT_EQ (pt.z, v3t.z ()); EXPECT_FLOAT_EQ (pt.z, v4t.z ());
+
+  PointCloud<PointXYZ> c, ct;
+  c.push_back (p);
+  pcl::transformPointCloud (c, ct, affine);
+  EXPECT_FLOAT_EQ (pt.x, ct[0].x); 
+  EXPECT_FLOAT_EQ (pt.y, ct[0].y); 
+  EXPECT_FLOAT_EQ (pt.z, ct[0].z); 
+
+  pcl::transformPointCloud (c, ct, transformation);
+  EXPECT_FLOAT_EQ (pt.x, ct[0].x); 
+  EXPECT_FLOAT_EQ (pt.y, ct[0].y); 
+  EXPECT_FLOAT_EQ (pt.z, ct[0].z); 
+
+  affine = transformation;
+
+  std::vector<int> indices (1); indices[0] = 0;
+
+  pcl::transformPointCloud (c, indices, ct, affine);
+  EXPECT_FLOAT_EQ (pt.x, ct[0].x); 
+  EXPECT_FLOAT_EQ (pt.y, ct[0].y); 
+  EXPECT_FLOAT_EQ (pt.z, ct[0].z); 
+
+  pcl::transformPointCloud (c, indices, ct, transformation);
+  EXPECT_FLOAT_EQ (pt.x, ct[0].x); 
+  EXPECT_FLOAT_EQ (pt.y, ct[0].y); 
+  EXPECT_FLOAT_EQ (pt.z, ct[0].z); 
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////
 TEST (PCL, commonTransform)
 {