Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00038 template <typename PointT> void
00039 pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00040 pcl::PointCloud<PointT> &cloud_out,
00041 const Eigen::Affine3f &transform)
00042 {
00043 if (&cloud_in != &cloud_out)
00044 {
00045
00046 cloud_out.header = cloud_in.header;
00047 cloud_out.is_dense = cloud_in.is_dense;
00048 cloud_out.width = cloud_in.width;
00049 cloud_out.height = cloud_in.height;
00050 cloud_out.points.reserve (cloud_out.points.size ());
00051 cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
00052 }
00053
00054 if (cloud_in.is_dense)
00055 {
00056
00057 for (size_t i = 0; i < cloud_out.points.size (); ++i)
00058 cloud_out.points[i].getVector3fMap () = transform *
00059 cloud_in.points[i].getVector3fMap ();
00060 }
00061 else
00062 {
00063
00064
00065 for (size_t i = 0; i < cloud_out.points.size (); ++i)
00066 {
00067 if (!pcl_isfinite (cloud_in.points[i].x) ||
00068 !pcl_isfinite (cloud_in.points[i].y) ||
00069 !pcl_isfinite (cloud_in.points[i].z))
00070 continue;
00071 cloud_out.points[i].getVector3fMap () = transform *
00072 cloud_in.points[i].getVector3fMap ();
00073 }
00074 }
00075 }
00076
00078 template <typename PointT> void
00079 pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00080 const std::vector<int> &indices,
00081 pcl::PointCloud<PointT> &cloud_out,
00082 const Eigen::Affine3f &transform)
00083 {
00084 size_t npts = indices.size ();
00085
00086 cloud_out.is_dense = cloud_in.is_dense;
00087 cloud_out.header = cloud_in.header;
00088 cloud_out.width = npts;
00089 cloud_out.height = 1;
00090 cloud_out.points.resize (npts);
00091
00092 if (cloud_in.is_dense)
00093 {
00094
00095 for (size_t i = 0; i < npts; ++i)
00096 {
00097
00098 cloud_out.points[i] = cloud_in.points[indices[i]];
00099 cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
00100 }
00101 }
00102 else
00103 {
00104
00105
00106 for (size_t i = 0; i < npts; ++i)
00107 {
00108 if (!pcl_isfinite (cloud_in.points[indices[i]].x) ||
00109 !pcl_isfinite (cloud_in.points[indices[i]].y) ||
00110 !pcl_isfinite (cloud_in.points[indices[i]].z))
00111 continue;
00112 cloud_out.points[i] = cloud_in.points[indices[i]];
00113 cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
00114 }
00115 }
00116 }
00117
00119 template <typename PointT> void
00120 pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
00121 pcl::PointCloud<PointT> &cloud_out,
00122 const Eigen::Affine3f &transform)
00123 {
00124 if (&cloud_in != &cloud_out)
00125 {
00126
00127 cloud_out.header = cloud_in.header;
00128 cloud_out.width = cloud_in.width;
00129 cloud_out.height = cloud_in.height;
00130 cloud_out.is_dense = cloud_in.is_dense;
00131 cloud_out.points.reserve (cloud_out.points.size ());
00132 cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
00133 }
00134
00135
00136 if (cloud_in.is_dense)
00137 {
00138 for (size_t i = 0; i < cloud_out.points.size (); ++i)
00139 {
00140 cloud_out.points[i].getVector3fMap() = transform *
00141 cloud_in.points[i].getVector3fMap ();
00142
00143
00144 cloud_out.points[i].getNormalVector3fMap() = transform.rotation () *
00145 cloud_in.points[i].getNormalVector3fMap ();
00146 }
00147 }
00148
00149 else
00150 {
00151 for (size_t i = 0; i < cloud_out.points.size (); ++i)
00152 {
00153 if (!pcl_isfinite (cloud_in.points[i].x) ||
00154 !pcl_isfinite (cloud_in.points[i].y) ||
00155 !pcl_isfinite (cloud_in.points[i].z))
00156 continue;
00157 cloud_out.points[i].getVector3fMap() = transform *
00158 cloud_in.points[i].getVector3fMap ();
00159
00160
00161 cloud_out.points[i].getNormalVector3fMap() = transform.rotation () *
00162 cloud_in.points[i].getNormalVector3fMap ();
00163 }
00164 }
00165 }
00166
00168 template <typename PointT> void
00169 pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00170 pcl::PointCloud<PointT> &cloud_out,
00171 const Eigen::Matrix4f &transform)
00172 {
00173 if (&cloud_in != &cloud_out)
00174 {
00175
00176 cloud_out.header = cloud_in.header;
00177 cloud_out.width = cloud_in.width;
00178 cloud_out.height = cloud_in.height;
00179 cloud_out.is_dense = cloud_in.is_dense;
00180 cloud_out.points.reserve (cloud_out.points.size ());
00181 cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
00182 }
00183
00184 Eigen::Matrix3f rot = transform.block<3, 3> (0, 0);
00185 Eigen::Vector3f trans = transform.block<3, 1> (0, 3);
00186
00187 if (cloud_in.is_dense)
00188 {
00189 for (size_t i = 0; i < cloud_out.points.size (); ++i)
00190 cloud_out.points[i].getVector3fMap () = rot *
00191 cloud_in.points[i].getVector3fMap () + trans;
00192 }
00193
00194 else
00195 {
00196 for (size_t i = 0; i < cloud_out.points.size (); ++i)
00197 {
00198 if (!pcl_isfinite (cloud_in.points[i].x) ||
00199 !pcl_isfinite (cloud_in.points[i].y) ||
00200 !pcl_isfinite (cloud_in.points[i].z))
00201 continue;
00202 cloud_out.points[i].getVector3fMap () = rot *
00203 cloud_in.points[i].getVector3fMap () + trans;
00204 }
00205 }
00206 }
00207
00209 template <typename PointT> void
00210 pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
00211 pcl::PointCloud<PointT> &cloud_out,
00212 const Eigen::Matrix4f &transform)
00213 {
00214 if (&cloud_in != &cloud_out)
00215 {
00216
00217 cloud_out.header = cloud_in.header;
00218 cloud_out.width = cloud_in.width;
00219 cloud_out.height = cloud_in.height;
00220 cloud_out.is_dense = cloud_in.is_dense;
00221 cloud_out.points.reserve (cloud_out.points.size ());
00222 cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
00223 }
00224
00225 Eigen::Matrix3f rot = transform.block<3, 3> (0, 0);
00226 Eigen::Vector3f trans = transform.block<3, 1> (0, 3);
00227
00228
00229 if (cloud_in.is_dense)
00230 {
00231 for (size_t i = 0; i < cloud_out.points.size (); ++i)
00232 {
00233 cloud_out.points[i].getVector3fMap () = rot *
00234 cloud_in.points[i].getVector3fMap () + trans;
00235
00236
00237 cloud_out.points[i].getNormalVector3fMap() = rot *
00238 cloud_in.points[i].getNormalVector3fMap ();
00239 }
00240 }
00241
00242 else
00243 {
00244 for (size_t i = 0; i < cloud_out.points.size (); ++i)
00245 {
00246 if (!pcl_isfinite (cloud_in.points[i].x) ||
00247 !pcl_isfinite (cloud_in.points[i].y) ||
00248 !pcl_isfinite (cloud_in.points[i].z))
00249 continue;
00250 cloud_out.points[i].getVector3fMap () = rot *
00251 cloud_in.points[i].getVector3fMap () + trans;
00252
00253
00254 cloud_out.points[i].getNormalVector3fMap() = rot *
00255 cloud_in.points[i].getNormalVector3fMap ();
00256 }
00257 }
00258 }
00259
00261 template <typename PointT> inline void
00262 pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00263 pcl::PointCloud<PointT> &cloud_out,
00264 const Eigen::Vector3f &offset,
00265 const Eigen::Quaternionf &rotation)
00266 {
00267 Eigen::Translation3f translation (offset);
00268
00269 Eigen::Affine3f t;
00270 t = translation * rotation;
00271 transformPointCloud (cloud_in, cloud_out, t);
00272 }
00273
00275 template <typename PointT> inline void
00276 pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
00277 pcl::PointCloud<PointT> &cloud_out,
00278 const Eigen::Vector3f &offset,
00279 const Eigen::Quaternionf &rotation)
00280 {
00281 Eigen::Translation3f translation (offset);
00282
00283 Eigen::Affine3f t;
00284 t = translation * rotation;
00285 transformPointCloudWithNormals (cloud_in, cloud_out, t);
00286 }
00287
00289 template <typename PointT> inline PointT
00290 pcl::transformPoint (const PointT &point, const Eigen::Affine3f &transform)
00291 {
00292 PointT ret = point;
00293 ret.getVector3fMap () = transform * point.getVector3fMap ();
00294 return ret;
00295 }