point_types.hpp

Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: point_types.hpp 1385 2011-06-19 19:15:56Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_IMPL_POINT_TYPES_HPP_
00039 #define PCL_IMPL_POINT_TYPES_HPP_
00040 
00041 // Define all PCL point types
00042 #define PCL_POINT_TYPES       \
00043   (pcl::PointXYZ)             \
00044   (pcl::PointXYZI)            \
00045   (pcl::PointXYZRGBA)         \
00046   (pcl::PointXYZRGB)          \
00047   (pcl::PointXY)              \
00048   (pcl::InterestPoint)        \
00049   (pcl::Normal)               \
00050   (pcl::PointNormal)          \
00051   (pcl::PointXYZRGBNormal)    \
00052   (pcl::PointXYZINormal)      \
00053   (pcl::PointWithRange)       \
00054   (pcl::PointWithViewpoint)   \
00055   (pcl::MomentInvariants)     \
00056   (pcl::PrincipalRadiiRSD)    \
00057   (pcl::Boundary)             \
00058   (pcl::PrincipalCurvatures)  \
00059   (pcl::PFHSignature125)      \
00060   (pcl::FPFHSignature33)      \
00061   (pcl::VFHSignature308)      \
00062   (pcl::Narf36)               \
00063   (pcl::IntensityGradient)    \
00064   (pcl::PointWithScale)
00065 
00066 // Define all point types that include XYZ data
00067 #define PCL_XYZ_POINT_TYPES   \
00068   (pcl::PointXYZ)             \
00069   (pcl::PointXYZI)            \
00070   (pcl::PointXYZRGBA)         \
00071   (pcl::PointXYZRGB)          \
00072   (pcl::InterestPoint)        \
00073   (pcl::PointNormal)          \
00074   (pcl::PointXYZRGBNormal)    \
00075   (pcl::PointXYZINormal)      \
00076   (pcl::PointWithRange)       \
00077   (pcl::PointWithViewpoint)   \
00078   (pcl::PointWithScale)
00079 
00080 // Define all point types that include normal[3] data
00081 #define PCL_NORMAL_POINT_TYPES  \
00082   (pcl::Normal)                 \
00083   (pcl::PointNormal)            \
00084   (pcl::PointXYZRGBNormal)      \
00085   (pcl::PointXYZINormal)
00086 
00087 namespace pcl
00088 {
00089 
00090 #define PCL_ADD_POINT4D \
00091   EIGEN_ALIGN16 \
00092   union { \
00093     float data[4]; \
00094     struct { \
00095       float x; \
00096       float y; \
00097       float z; \
00098     }; \
00099   } ; \
00100   inline Eigen::Map<Eigen::Vector3f> getVector3fMap () { return (Eigen::Vector3f::Map (data)); } \
00101   inline const Eigen::Map<const Eigen::Vector3f> getVector3fMap () const { return (Eigen::Vector3f::Map (data)); } \
00102   inline Eigen::Map<Eigen::Vector4f, Eigen::Aligned> getVector4fMap () { return (Eigen::Vector4f::MapAligned (data)); } \
00103   inline const Eigen::Map<const Eigen::Vector4f, Eigen::Aligned> getVector4fMap () const { return (Eigen::Vector4f::MapAligned (data)); } \
00104   inline Eigen::Map<Eigen::Array3f> getArray3fMap () { return (Eigen::Array3f::Map (data)); } \
00105   inline const Eigen::Map<const Eigen::Array3f> getArray3fMap () const { return (Eigen::Array3f::Map (data)); } \
00106   inline Eigen::Map<Eigen::Array4f, Eigen::Aligned> getArray4fMap () { return (Eigen::Array4f::MapAligned (data)); } \
00107   inline const Eigen::Map<const Eigen::Array4f, Eigen::Aligned> getArray4fMap () const { return (Eigen::Array4f::MapAligned (data)); }
00108 
00109 #define PCL_ADD_NORMAL4D \
00110   EIGEN_ALIGN16 \
00111   union { \
00112     float data_n[4]; \
00113     float normal[3]; \
00114     struct { \
00115       float normal_x; \
00116       float normal_y; \
00117       float normal_z; \
00118     }; \
00119   }; \
00120   inline Eigen::Map<Eigen::Vector3f> getNormalVector3fMap () { return (Eigen::Vector3f::Map (data_n)); } \
00121   inline const Eigen::Map<const Eigen::Vector3f> getNormalVector3fMap () const { return (Eigen::Vector3f::Map (data_n)); } \
00122   inline Eigen::Map<Eigen::Vector4f, Eigen::Aligned> getNormalVector4fMap () { return (Eigen::Vector4f::MapAligned (data_n)); } \
00123   inline const Eigen::Map<const Eigen::Vector4f, Eigen::Aligned> getNormalVector4fMap () const { return (Eigen::Vector4f::MapAligned (data_n)); }
00124 
00125 typedef Eigen::Map<Eigen::Array3f> Array3fMap;
00126 typedef const Eigen::Map<const Eigen::Array3f> Array3fMapConst;
00127 typedef Eigen::Map<Eigen::Array4f, Eigen::Aligned> Array4fMap;
00128 typedef const Eigen::Map<const Eigen::Array4f, Eigen::Aligned> Array4fMapConst;
00129 typedef Eigen::Map<Eigen::Vector3f> Vector3fMap;
00130 typedef const Eigen::Map<const Eigen::Vector3f> Vector3fMapConst;
00131 typedef Eigen::Map<Eigen::Vector4f, Eigen::Aligned> Vector4fMap;
00132 typedef const Eigen::Map<const Eigen::Vector4f, Eigen::Aligned> Vector4fMapConst;
00133 
00134 
00135 struct _PointXYZ
00136 {
00137   PCL_ADD_POINT4D;  // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00138   EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
00139 };
00140 
00144 struct EIGEN_ALIGN16 PointXYZ : public _PointXYZ
00145 {
00146   inline PointXYZ()
00147   {
00148     x = y = z = 0.0f;
00149     data[3] = 1.0f;
00150   }
00151   inline PointXYZ (float _x, float _y, float _z) { x = _x; y = _y; z = _z; data[3] = 1.0f; }
00152 };
00153 
00154 inline std::ostream& operator << (std::ostream& os, const PointXYZ& p)
00155 {
00156   os << "(" << p.x << "," << p.y << "," << p.z << ")";
00157   return (os);
00158 }
00159 
00163 struct EIGEN_ALIGN16 PointXYZI 
00164 { 
00165   PCL_ADD_POINT4D;  // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00166   union
00167   {
00168     struct
00169     {
00170       float intensity; 
00171     };
00172     float data_c[4];
00173   };
00174   EIGEN_MAKE_ALIGNED_OPERATOR_NEW 
00175 }; 
00176 inline std::ostream& operator << (std::ostream& os, const PointXYZI& p)
00177 {
00178   os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.intensity << ")";
00179   return (os);
00180 }
00181 
00182 
00186 struct EIGEN_ALIGN16 PointXYZRGBA
00187 {
00188   PCL_ADD_POINT4D;  // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00189   union
00190   {
00191     struct
00192     {
00193       uint32_t rgba;
00194     };
00195     float data_c[4];
00196   };
00197   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00198 };
00199 inline std::ostream& operator << (std::ostream& os, const PointXYZRGBA& p)
00200 {
00201   unsigned char* rgba_ptr = (unsigned char*)&p.rgba;
00202   os << "(" << p.x << "," << p.y << "," << p.z << " - " << (int)(*rgba_ptr) << "," << (int)(*(rgba_ptr+1)) << "," << (int)(*(rgba_ptr+2)) << "," <<(int)(*(rgba_ptr+3)) << ")";
00203   return (os);
00204 }
00205 
00206 
00210 struct EIGEN_ALIGN16 PointXYZRGB
00211 {
00212   PCL_ADD_POINT4D;  // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00213   union
00214   {
00215     struct
00216     {
00217       float rgb;
00218     };
00219     float data_c[4];
00220   };
00221   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00222 };
00223 inline std::ostream& operator << (std::ostream& os, const PointXYZRGB& p)
00224 {
00225   os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.rgb << ")";
00226   return (os);
00227 }
00228 
00229 
00233 struct PointXY
00234 {
00235   float x;
00236   float y;
00237 };
00238 inline std::ostream& operator << (std::ostream& os, const PointXY& p)
00239 {
00240   os << "(" << p.x << "," << p.y << ")";
00241   return (os);
00242 }
00243 
00244 
00248 struct EIGEN_ALIGN16 InterestPoint
00249 {
00250   PCL_ADD_POINT4D;  // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00251   union
00252   {
00253     struct
00254     {
00255       float strength;
00256     };
00257     float data_c[4];
00258   };
00259   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00260 };
00261 inline std::ostream& operator << (std::ostream& os, const InterestPoint& p)
00262 {
00263   os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.strength << ")";
00264   return (os);
00265 }
00266 
00267 
00271 struct EIGEN_ALIGN16 Normal
00272 {
00273   PCL_ADD_NORMAL4D;  // This adds the member normal[3] which can also be accessed using the point (which is float[4])
00274   union
00275   {
00276     struct
00277     {
00278       float curvature;
00279     };
00280     float data_c[4];
00281   };
00282   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00283 };
00284 inline std::ostream& operator << (std::ostream& os, const Normal& p)
00285 {
00286   os << "(" << p.normal[0] << "," << p.normal[1] << "," << p.normal[2] << " - " << p.curvature << ")";
00287   return (os);
00288 }
00289 
00290 
00294 struct EIGEN_ALIGN16 PointNormal
00295 {
00296   PCL_ADD_POINT4D;    // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00297   PCL_ADD_NORMAL4D;   // This adds the member normal[3] which can also be accessed using the point (which is float[4])
00298   union
00299   {
00300     struct
00301     {
00302       float curvature;
00303     };
00304     float data_c[4];
00305   };
00306   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00307 };
00308 inline std::ostream& operator << (std::ostream& os, const PointNormal& p)
00309 {
00310   os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.normal[0] << "," << p.normal[1] << "," << p.normal[2] << " - " << p.curvature << ")";
00311   return (os);
00312 }
00313 
00314 
00318 struct EIGEN_ALIGN16 PointXYZRGBNormal
00319 {
00320   PCL_ADD_POINT4D;    // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00321   PCL_ADD_NORMAL4D;   // This adds the member normal[3] which can also be accessed using the point (which is float[4])
00322   union
00323   {
00324     struct
00325     {
00326       float rgb;
00327       float curvature;
00328     };
00329     float data_c[4];
00330   };
00331   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00332 };
00333 inline std::ostream& operator << (std::ostream& os, const PointXYZRGBNormal& p)
00334 {
00335   os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.rgb << " - " << p.normal[0] << "," << p.normal[1] << "," << p.normal[2] << " - " << p.curvature << ")";
00336   return (os);
00337 }
00338 
00342 struct EIGEN_ALIGN16 PointXYZINormal
00343 {
00344   PCL_ADD_POINT4D;    // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00345   PCL_ADD_NORMAL4D;   // This adds the member normal[3] which can also be accessed using the point (which is float[4])
00346   union
00347   {
00348     struct
00349     {
00350       float intensity;
00351       float curvature;
00352     };
00353     float data_c[4];
00354   };
00355   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00356 };
00357 inline std::ostream& operator << (std::ostream& os, const PointXYZINormal& p)
00358 {
00359   os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.intensity << " - " << p.normal[0] << "," << p.normal[1] << "," << p.normal[2] << " - " << p.curvature << ")";
00360   return (os);
00361 }
00362 
00366 struct EIGEN_ALIGN16 PointWithRange 
00367 {
00368   PCL_ADD_POINT4D;    // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00369   union
00370   {
00371     struct
00372     {
00373       float range;
00374     };
00375     float data_c[4];
00376   };
00377   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00378 };
00379 inline std::ostream& operator << (std::ostream& os, const PointWithRange& p)
00380 {
00381   os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.range << ")";
00382   return (os);
00383 }
00384 
00385 struct EIGEN_ALIGN16 _PointWithViewpoint 
00386 {
00387   PCL_ADD_POINT4D;    // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00388   union
00389   {
00390     struct
00391     {
00392       float vp_x;
00393       float vp_y;
00394       float vp_z;
00395     };
00396     float data_c[4];
00397   };
00398   EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
00399 };
00400 
00404 struct PointWithViewpoint : public _PointWithViewpoint
00405 {
00406   PointWithViewpoint(float _x=0.0f, float _y=0.0f, float _z=0.0f, float _vp_x=0.0f, float _vp_y=0.0f, float _vp_z=0.0f)
00407   {
00408     x=_x; y=_y; z=_z; data[3] = 1.0f;
00409     vp_x=_vp_x; vp_y=_vp_y; vp_z=_vp_z;
00410   }
00411 };
00412 inline std::ostream& operator << (std::ostream& os, const PointWithViewpoint& p)
00413 {
00414   os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.vp_x << "," << p.vp_y << "," << p.vp_z << ")";
00415   return (os);
00416 }
00417 
00421 struct MomentInvariants
00422 {
00423   float j1, j2, j3;
00424 };
00425 inline std::ostream& operator << (std::ostream& os, const MomentInvariants& p)
00426 {
00427   os << "(" << p.j1 << "," << p.j2 << "," << p.j3 << ")";
00428   return (os);
00429 }
00430 
00434 struct PrincipalRadiiRSD
00435 {
00436   float r_min, r_max;
00437 };
00438 inline std::ostream& operator << (std::ostream& os, const PrincipalRadiiRSD& p)
00439 {
00440   os << "(" << p.r_min << "," << p.r_max << ")";
00441   return (os);
00442 }
00443 
00447 struct Boundary
00448 {
00449   uint8_t boundary_point;
00450 };
00451 inline std::ostream& operator << (std::ostream& os, const Boundary& p)
00452 {
00453   os << p.boundary_point;
00454   return (os);
00455 }
00456 
00460 struct PrincipalCurvatures
00461 {
00462   union
00463   {
00464     float principal_curvature[3];
00465     struct
00466     {
00467       float principal_curvature_x;
00468       float principal_curvature_y;
00469       float principal_curvature_z;
00470     };
00471   };
00472   float pc1;
00473   float pc2;
00474 };
00475 inline std::ostream& operator << (std::ostream& os, const PrincipalCurvatures& p)
00476 {
00477   os << "(" << p.principal_curvature[0] << "," << p.principal_curvature[1] << "," << p.principal_curvature[2] << " - " << p.pc1 << "," << p.pc2 << ")";
00478   return (os);
00479 }
00480 
00484 struct PFHSignature125
00485 {
00486   float histogram[125];
00487 };
00488 inline std::ostream& operator << (std::ostream& os, const PFHSignature125& p)
00489 {
00490   for (int i = 0; i < 125; ++i) 
00491     os << (i == 0 ? "(" : "") << p.histogram[i] << (i < 124 ? ", " : ")");
00492   return (os);
00493 }
00494 
00498 struct FPFHSignature33
00499 {
00500   float histogram[33];
00501 };
00502 inline std::ostream& operator << (std::ostream& os, const FPFHSignature33& p)
00503 {
00504   for (int i = 0; i < 33; ++i) 
00505     os << (i == 0 ? "(" : "") << p.histogram[i] << (i < 32 ? ", " : ")");
00506   return (os);
00507 }
00508 
00512 struct VFHSignature308
00513 {
00514   float histogram[308];
00515 };
00516 inline std::ostream& operator << (std::ostream& os, const VFHSignature308& p)
00517 {
00518   for (int i = 0; i < 308; ++i) 
00519     os << (i == 0 ? "(" : "") << p.histogram[i] << (i < 307 ? ", " : ")");
00520   return (os);
00521 }
00522 
00526 struct Narf36
00527 {
00528   float x, y, z, roll, pitch, yaw;
00529   float descriptor[36];
00530 };
00531 inline std::ostream& operator << (std::ostream& os, const Narf36& p)
00532 {
00533   os << p.x<<","<<p.y<<","<<p.z<<" - "<<p.roll*360.0/M_PI<<"deg,"<<p.pitch*360.0/M_PI<<"deg,"<<p.yaw*360.0/M_PI<<"deg - ";
00534   for (int i = 0; i < 36; ++i) 
00535     os << (i == 0 ? "(" : "") << p.descriptor[i] << (i < 35 ? ", " : ")");
00536   return (os);
00537 }
00538 
00542 struct BorderDescription 
00543 {
00544   int x, y;
00545   BorderTraits traits;
00546   //std::vector<const BorderDescription*> neighbors;
00547 };
00548 
00549 inline std::ostream& operator << (std::ostream& os, const BorderDescription& p)
00550 {
00551   os << "(" << p.x << "," << p.y << ")";
00552   return (os);
00553 }
00554 
00558 struct IntensityGradient
00559 {
00560   union
00561   {
00562     float gradient[3];
00563     struct
00564     {
00565       float gradient_x;
00566       float gradient_y;
00567       float gradient_z;
00568     };
00569   };
00570 };
00571 inline std::ostream& operator << (std::ostream& os, const IntensityGradient& p)
00572 {
00573   os << "(" << p.gradient[0] << "," << p.gradient[1] << "," << p.gradient[2] << ")";
00574   return (os);
00575 }
00576 
00580 template <int N>
00581 struct Histogram
00582 {
00583   float histogram[N];
00584 };
00585 template <int N>
00586 inline std::ostream& operator << (std::ostream& os, const Histogram<N>& p)
00587 {
00588   for (int i = 0; i < N; ++i) 
00589     os << (i == 0 ? "(" : "") << p.histogram[i] << (i < N-1 ? ", " : ")");
00590   return (os);
00591 }
00592 
00596 struct EIGEN_ALIGN16 PointWithScale
00597 {
00598   PCL_ADD_POINT4D;    // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00599   float scale;
00600   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00601 } ;
00602 inline std::ostream& operator << (std::ostream& os, const PointWithScale& p)
00603 {
00604   os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.scale << ")";
00605   return (os);
00606 }
00607 
00611 struct EIGEN_ALIGN16 PointSurfel
00612 {
00613   PCL_ADD_POINT4D;    // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00614   PCL_ADD_NORMAL4D;   // This adds the member normal[3] which can also be accessed using the point (which is float[4])
00615   union
00616   {
00617     struct
00618     {
00619       uint32_t rgba;
00620       float radius;
00621       float confidence;
00622       float curvature;
00623     };
00624     float data_c[4];
00625   };
00626   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00627 };
00628 inline std::ostream& operator << (std::ostream& os, const PointSurfel& p)
00629 {
00630   unsigned char* rgba_ptr = (unsigned char*)&p.rgba;
00631   os << 
00632     "(" << p.x << "," << p.y << "," << p.z << " - " <<
00633     p.normal_x << "," << p.normal_y << "," << p.normal_z << " - " <<
00634     (int)(*rgba_ptr) << "," << (int)(*(rgba_ptr+1)) << "," << (int)(*(rgba_ptr+2)) << "," <<(int)(*(rgba_ptr+3)) << " - " <<
00635     p.radius << " - " << p.confidence << " - " << p.curvature << ")";
00636   return (os);
00637 }
00638 
00639 
00640 template <typename PointType1, typename PointType2> inline float 
00641 squaredEuclideanDistance (const PointType1& p1, const PointType2& p2)
00642 {
00643   float diff_x = p2.x - p1.x, diff_y = p2.y - p1.y, diff_z = p2.z - p1.z;
00644   return (diff_x*diff_x + diff_y*diff_y + diff_z*diff_z);
00645 }
00646 
00647 template <typename PointType1, typename PointType2> inline float 
00648 euclideanDistance (const PointType1& p1, const PointType2& p2)
00649 {
00650   return (sqrtf (squaredEuclideanDistance (p1, p2)));
00651 }
00652 
00653 template <typename PointType> inline bool 
00654 hasValidXYZ (const PointType& p)
00655 {
00656   return (pcl_isfinite (p.x) && pcl_isfinite (p.y) && pcl_isfinite (p.z));
00657 }
00658 
00659 }  // End namespace
00660 
00661 #endif