Go to the documentation of this file.00001 namespace lslgeneric
00002 {
00003
00004 template< typename PointT>
00005 pcl::PointCloud<PointT> transformPointCloud(Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &Tr, const pcl::PointCloud<PointT> &pc)
00006 {
00007 Eigen::Transform<float,3,Eigen::Affine,Eigen::ColMajor> T = Tr.cast<float>();
00008 pcl::PointCloud<PointT> cloud;
00009 for(unsigned int pit=0; pit<pc.points.size(); ++pit)
00010 {
00011 PointT thisPoint = pc.points[pit];
00012 Eigen::Map<Eigen::Vector3f> pt((float*)&thisPoint,3);
00013 pt = T*pt;
00014 cloud.points.push_back(thisPoint);
00015 }
00016 cloud.width = pc.width;
00017 cloud.height = pc.height;
00018 return cloud;
00019 }
00020
00021 template< typename PointT>
00022 void transformPointCloudInPlace(Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &Tr, pcl::PointCloud<PointT> &pc)
00023 {
00024 Eigen::Transform<float,3,Eigen::Affine,Eigen::ColMajor> T = Tr.cast<float>();
00025 for(unsigned int pit=0; pit<pc.points.size(); ++pit)
00026 {
00027 Eigen::Map<Eigen::Vector3f> pt((float*)&pc.points[pit],3);
00028 pt = T*pt;
00029 }
00030 }
00031
00032 template< typename PointT>
00033 double geomDist(PointT p1, PointT p2)
00034 {
00035 Eigen::Vector3d v;
00036 v << p1.x-p2.x, p1.y-p2.y, p1.z-p2.z;
00037 return v.norm();
00038 }
00039
00040 }