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 3006 2011-10-31 23:25:06Z 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::PointXYZL)              \
00046   (pcl::PointXYZRGBA)           \
00047   (pcl::PointXYZRGB)            \
00048   (pcl::PointXYZRGBL)           \
00049   (pcl::PointXYZHSV)            \
00050   (pcl::PointXY)                \
00051   (pcl::InterestPoint)          \
00052   (pcl::Normal)                 \
00053   (pcl::PointNormal)            \
00054   (pcl::PointXYZRGBNormal)      \
00055   (pcl::PointXYZINormal)        \
00056   (pcl::PointWithRange)         \
00057   (pcl::PointWithViewpoint)     \
00058   (pcl::MomentInvariants)       \
00059   (pcl::PrincipalRadiiRSD)      \
00060   (pcl::Boundary)               \
00061   (pcl::PrincipalCurvatures)    \
00062   (pcl::PFHSignature125)        \
00063   (pcl::PFHRGBSignature250)     \
00064   (pcl::PPFSignature)           \
00065   (pcl::PPFRGBSignature)        \
00066   (pcl::NormalBasedSignature12) \
00067   (pcl::FPFHSignature33)        \
00068   (pcl::VFHSignature308)        \
00069   (pcl::Narf36)                 \
00070   (pcl::IntensityGradient)      \
00071   (pcl::PointWithScale)
00072 
00073 // Define all point types that include XYZ data
00074 #define PCL_XYZ_POINT_TYPES   \
00075   (pcl::PointXYZ)             \
00076   (pcl::PointXYZI)            \
00077   (pcl::PointXYZL)            \
00078   (pcl::PointXYZRGBA)         \
00079   (pcl::PointXYZRGB)          \
00080   (pcl::PointXYZRGBL)         \
00081   (pcl::PointXYZHSV)          \
00082   (pcl::InterestPoint)        \
00083   (pcl::PointNormal)          \
00084   (pcl::PointXYZRGBNormal)    \
00085   (pcl::PointXYZINormal)      \
00086   (pcl::PointWithRange)       \
00087   (pcl::PointWithViewpoint)   \
00088   (pcl::PointWithScale)
00089 
00090 // Define all point types with XYZ and label
00091 #define PCL_XYZL_POINT_TYPES  \
00092   (pcl::PointXYZL)            \
00093   (pcl::PointXYZRGBL)
00094 
00095 // Define all point types that include normal[3] data
00096 #define PCL_NORMAL_POINT_TYPES  \
00097   (pcl::Normal)                 \
00098   (pcl::PointNormal)            \
00099   (pcl::PointXYZRGBNormal)      \
00100   (pcl::PointXYZINormal)
00101 
00102 // Define all point types that represent features
00103 #define PCL_FEATURE_POINT_TYPES \
00104   (pcl::PFHSignature125)        \
00105   (pcl::PFHRGBSignature250)     \
00106   (pcl::PPFSignature)           \
00107   (pcl::PPFRGBSignature)        \
00108   (pcl::NormalBasedSignature12) \
00109   (pcl::FPFHSignature33)        \
00110   (pcl::VFHSignature308)        \
00111   (pcl::Narf36)
00112 
00113 namespace pcl
00114 {
00115 
00116 #define PCL_ADD_POINT4D \
00117   EIGEN_ALIGN16 \
00118   union { \
00119     float data[4]; \
00120     struct { \
00121       float x; \
00122       float y; \
00123       float z; \
00124     }; \
00125   }; \
00126   inline Eigen::Map<Eigen::Vector3f> getVector3fMap () { return (Eigen::Vector3f::Map (data)); } \
00127   inline const Eigen::Map<const Eigen::Vector3f> getVector3fMap () const { return (Eigen::Vector3f::Map (data)); } \
00128   inline Eigen::Map<Eigen::Vector4f, Eigen::Aligned> getVector4fMap () { return (Eigen::Vector4f::MapAligned (data)); } \
00129   inline const Eigen::Map<const Eigen::Vector4f, Eigen::Aligned> getVector4fMap () const { return (Eigen::Vector4f::MapAligned (data)); } \
00130   inline Eigen::Map<Eigen::Array3f> getArray3fMap () { return (Eigen::Array3f::Map (data)); } \
00131   inline const Eigen::Map<const Eigen::Array3f> getArray3fMap () const { return (Eigen::Array3f::Map (data)); } \
00132   inline Eigen::Map<Eigen::Array4f, Eigen::Aligned> getArray4fMap () { return (Eigen::Array4f::MapAligned (data)); } \
00133   inline const Eigen::Map<const Eigen::Array4f, Eigen::Aligned> getArray4fMap () const { return (Eigen::Array4f::MapAligned (data)); }
00134 
00135 #define PCL_ADD_NORMAL4D \
00136   EIGEN_ALIGN16 \
00137   union { \
00138     float data_n[4]; \
00139     float normal[3]; \
00140     struct { \
00141       float normal_x; \
00142       float normal_y; \
00143       float normal_z; \
00144     }; \
00145   }; \
00146   inline Eigen::Map<Eigen::Vector3f> getNormalVector3fMap () { return (Eigen::Vector3f::Map (data_n)); } \
00147   inline const Eigen::Map<const Eigen::Vector3f> getNormalVector3fMap () const { return (Eigen::Vector3f::Map (data_n)); } \
00148   inline Eigen::Map<Eigen::Vector4f, Eigen::Aligned> getNormalVector4fMap () { return (Eigen::Vector4f::MapAligned (data_n)); } \
00149   inline const Eigen::Map<const Eigen::Vector4f, Eigen::Aligned> getNormalVector4fMap () const { return (Eigen::Vector4f::MapAligned (data_n)); }
00150 
00151   typedef Eigen::Map<Eigen::Array3f> Array3fMap;
00152   typedef const Eigen::Map<const Eigen::Array3f> Array3fMapConst;
00153   typedef Eigen::Map<Eigen::Array4f, Eigen::Aligned> Array4fMap;
00154   typedef const Eigen::Map<const Eigen::Array4f, Eigen::Aligned> Array4fMapConst;
00155   typedef Eigen::Map<Eigen::Vector3f> Vector3fMap;
00156   typedef const Eigen::Map<const Eigen::Vector3f> Vector3fMapConst;
00157   typedef Eigen::Map<Eigen::Vector4f, Eigen::Aligned> Vector4fMap;
00158   typedef const Eigen::Map<const Eigen::Vector4f, Eigen::Aligned> Vector4fMapConst;
00159 
00160   struct _PointXYZ
00161   {
00162     PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00163     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
00164   };
00165 
00166   /*struct PointXYZ
00167    {
00168    ADD_4D_POINT_WITH_XYZ;  // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00169    //inline PointXYZ() {}
00170    //inline PointXYZ(float x, float y, float z) : x(x), y(y), z(z) {}
00171    };*/
00175   struct EIGEN_ALIGN16 PointXYZ : public _PointXYZ
00176   {
00177     inline PointXYZ()
00178     {
00179       x = y = z = 0.0f;
00180       data[3] = 1.0f;
00181     }
00182     inline PointXYZ (float _x, float _y, float _z)
00183     { x = _x; y = _y; z = _z; data[3] = 1.0f;}
00184     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00185   };
00186 
00187   inline std::ostream& operator << (std::ostream& os, const PointXYZ& p)
00188   {
00189     os << "(" << p.x << "," << p.y << "," << p.z << ")";
00190     return (os);
00191   }
00192 
00212   struct RGB
00213   {
00214     union
00215     {
00216       struct
00217       {
00218         uint8_t b;
00219         uint8_t g;
00220         uint8_t r;
00221         uint8_t a;
00222       };
00223       uint32_t rgba;
00224     };
00225   };
00226 
00230   struct EIGEN_ALIGN16 PointXYZI
00231   {
00232     PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00233     union
00234     {
00235       struct
00236       {
00237         float intensity;
00238       };
00239       float data_c[4];
00240     };
00241     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00242   };
00243   inline std::ostream& operator << (std::ostream& os, const PointXYZI& p)
00244   {
00245     os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.intensity << ")";
00246     return (os);
00247   }
00248 
00249   struct EIGEN_ALIGN16 PointXYZL
00250   {
00251     PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00252     union
00253     {
00254       struct
00255       {
00256         uint8_t label;
00257       };
00258       uint32_t data_l;
00259     };
00260     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00261   };
00262   inline std::ostream& operator << (std::ostream& os, const PointXYZL& p)
00263   {
00264     os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.label << ")";
00265     return (os);
00266   }
00267 
00288   struct EIGEN_ALIGN16 _PointXYZRGBA
00289   {
00290     PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00291     EIGEN_ALIGN16
00292     union
00293     {
00294       struct
00295       {
00296         uint8_t b;
00297         uint8_t g;
00298         uint8_t r;
00299         uint8_t _unused;
00300       };
00301       uint32_t rgba;
00302     };
00303 
00304     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00305   };
00306 
00307   struct PointXYZRGBA : public _PointXYZRGBA
00308   {
00309     inline Eigen::Vector3i getRGBVector3i () { return (Eigen::Vector3i (r, g, b)); }
00310     inline const Eigen::Vector3i getRGBVector3i () const { return (Eigen::Vector3i (r, g, b)); }
00311     inline Eigen::Vector4i getRGBVector4i () { return (Eigen::Vector4i (r, g, b, 0)); }
00312     inline const Eigen::Vector4i getRGBVector4i () const { return (Eigen::Vector4i (r, g, b, 0)); }
00313   };
00314 
00315   inline std::ostream& operator << (std::ostream& os, const PointXYZRGBA& p)
00316   {
00317     unsigned char* rgba_ptr = (unsigned char*)&p.rgba;
00318     os << "(" << p.x << "," << p.y << "," << p.z << " - " << (int)(*rgba_ptr) << "," << (int)(*(rgba_ptr+1)) << "," << (int)(*(rgba_ptr+2)) << "," <<(int)(*(rgba_ptr+3)) << ")";
00319     return (os);
00320   }
00321 
00322   struct EIGEN_ALIGN16 _PointXYZRGB
00323   {
00324     PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00325     union
00326     {
00327       union
00328       {
00329         struct
00330         {
00331           uint8_t b;
00332           uint8_t g;
00333           uint8_t r;
00334           uint8_t _unused;
00335         };
00336         float rgb;
00337       };
00338       uint32_t rgba;
00339     };
00340 
00341     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00342   };
00343 
00344   struct EIGEN_ALIGN16 _PointXYZRGBL
00345   {
00346     PCL_ADD_POINT4D; // Thi adds the members x,y,z which can also be accessed using the point (which is float[4])
00347     union
00348     {
00349       struct
00350       {
00351         uint8_t b;
00352         uint8_t g;
00353         uint8_t r;
00354         uint8_t label;
00355       };
00356       uint32_t rgba;
00357     };
00358     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00359   };
00360 
00392   struct EIGEN_ALIGN16 PointXYZRGB : public _PointXYZRGB
00393   {
00394     inline PointXYZRGB ()
00395     {
00396       _unused = 0;
00397     }
00398     inline PointXYZRGB (uint8_t _r, uint8_t _g, uint8_t _b)
00399     {
00400       r = _r;
00401       g = _g;
00402       b = _b;
00403       _unused = 0;
00404     }
00405 
00406     inline Eigen::Vector3i getRGBVector3i () { return (Eigen::Vector3i (r, g, b)); }
00407     inline const Eigen::Vector3i getRGBVector3i () const { return (Eigen::Vector3i (r, g, b)); }
00408     inline Eigen::Vector4i getRGBVector4i () { return (Eigen::Vector4i (r, g, b, 0)); }
00409     inline const Eigen::Vector4i getRGBVector4i () const { return (Eigen::Vector4i (r, g, b, 0)); }
00410 
00411     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00412   };
00413   inline std::ostream& operator << (std::ostream& os, const PointXYZRGB& p)
00414   {
00415     os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.r << "," << p.g << "," << p.b << ")";
00416     return (os);
00417   }
00418 
00419   struct EIGEN_ALIGN16 PointXYZRGBL : public _PointXYZRGBL
00420   {
00421     inline PointXYZRGBL ()
00422     {
00423       label = 255;
00424     }
00425     inline PointXYZRGBL (uint8_t _r, uint8_t _g, uint8_t _b, uint8_t _label)
00426     {
00427       r = _r;
00428       g = _g;
00429       b = _b;
00430       label = _label;
00431     }
00432     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00433   };
00434   inline std::ostream& operator << (std::ostream& os, const PointXYZRGBL& p)
00435   {
00436     os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.r << "," << p.g << "," << p.b << " - " << p.label << ")";
00437     return (os);
00438   }
00439 
00440   struct _PointXYZHSV
00441   {
00442     PCL_ADD_POINT4D;    // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00443     union
00444     {
00445       struct
00446       {
00447         float h;
00448         float s;
00449         float v;
00450       };
00451       float data_c[4];
00452     };
00453     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00454   } EIGEN_ALIGN16;
00455 
00456   struct EIGEN_ALIGN16 PointXYZHSV : public _PointXYZHSV
00457   {
00458     inline PointXYZHSV ()
00459     {
00460       data_c[3] = 0;
00461     }
00462     inline PointXYZHSV (float _h, float _v, float _s)
00463     {
00464       h = _h; v = _v; s = _s;
00465       data_c[3] = 0;
00466     }
00467     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00468   };
00469   inline std::ostream& operator << (std::ostream& os, const PointXYZHSV& p)
00470   {
00471     os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.h << " , " <<  p.s << " , " << p.v << ")";
00472     return (os);
00473   }
00474 
00475 
00479   struct PointXY
00480   {
00481     float x;
00482     float y;
00483   };
00484   inline std::ostream& operator << (std::ostream& os, const PointXY& p)
00485   {
00486     os << "(" << p.x << "," << p.y << ")";
00487     return (os);
00488   }
00489 
00493   struct EIGEN_ALIGN16 InterestPoint
00494   {
00495     PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00496     union
00497     {
00498       struct
00499       {
00500         float strength;
00501       };
00502       float data_c[4];
00503     };
00504     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00505   };
00506   inline std::ostream& operator << (std::ostream& os, const InterestPoint& p)
00507   {
00508     os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.strength << ")";
00509     return (os);
00510   }
00511 
00515   struct EIGEN_ALIGN16 Normal
00516   {
00517     PCL_ADD_NORMAL4D; // This adds the member normal[3] which can also be accessed using the point (which is float[4])
00518     union
00519     {
00520       struct
00521       {
00522         float curvature;
00523       };
00524       float data_c[4];
00525     };
00526     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00527   };
00528   inline std::ostream& operator << (std::ostream& os, const Normal& p)
00529   {
00530     os << "(" << p.normal[0] << "," << p.normal[1] << "," << p.normal[2] << " - " << p.curvature << ")";
00531     return (os);
00532   }
00533 
00537   struct EIGEN_ALIGN16 PointNormal
00538   {
00539     PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00540     PCL_ADD_NORMAL4D; // This adds the member normal[3] which can also be accessed using the point (which is float[4])
00541     union
00542     {
00543       struct
00544       {
00545         float curvature;
00546       };
00547       float data_c[4];
00548     };
00549     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00550   };
00551   inline std::ostream& operator << (std::ostream& os, const PointNormal& p)
00552   {
00553     os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.normal[0] << "," << p.normal[1] << "," << p.normal[2] << " - " << p.curvature << ")";
00554     return (os);
00555   }
00556 
00586   struct EIGEN_ALIGN16 _PointXYZRGBNormal
00587   {
00588     PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00589     PCL_ADD_NORMAL4D; // This adds the member normal[3] which can also be accessed using the point (which is float[4])
00590     union
00591     {
00592       struct
00593       {
00594         // RGB union
00595         union
00596         {
00597           struct
00598           {
00599             uint8_t b;
00600             uint8_t g;
00601             uint8_t r;
00602             uint8_t _unused;
00603           };
00604           float rgb;
00605           uint32_t rgba;
00606         };
00607         float curvature;
00608       };
00609       float data_c[4];
00610     };
00611     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00612   };
00613   struct PointXYZRGBNormal : public _PointXYZRGBNormal
00614   {
00615     inline PointXYZRGBNormal ()
00616     {
00617       _unused = 0;
00618       data[3] = 1.0f;
00619       data_n[3] = 0.0f;
00620     }
00621 
00622     inline Eigen::Vector3i getRGBVector3i () { return (Eigen::Vector3i (r, g, b)); }
00623     inline const Eigen::Vector3i getRGBVector3i () const { return (Eigen::Vector3i (r, g, b)); }
00624     inline Eigen::Vector4i getRGBVector4i () { return (Eigen::Vector4i (r, g, b, 0)); }
00625     inline const Eigen::Vector4i getRGBVector4i () const { return (Eigen::Vector4i (r, g, b, 0)); }
00626   };
00627   inline std::ostream& operator << (std::ostream& os, const PointXYZRGBNormal& p)
00628   {
00629     os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.rgb << " - " << p.normal[0] << "," << p.normal[1] << "," << p.normal[2] << " - " << p.r << ", " << p.g << ", " << p.b << " - " << p.curvature << ")";
00630     return (os);
00631   }
00632 
00636   struct EIGEN_ALIGN16 PointXYZINormal
00637   {
00638     PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00639     PCL_ADD_NORMAL4D; // This adds the member normal[3] which can also be accessed using the point (which is float[4])
00640     union
00641     {
00642       struct
00643       {
00644         float intensity;
00645         float curvature;
00646       };
00647       float data_c[4];
00648     };
00649     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00650   };
00651   inline std::ostream& operator << (std::ostream& os, const PointXYZINormal& p)
00652   {
00653     os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.intensity << " - " << p.normal[0] << "," << p.normal[1] << "," << p.normal[2] << " - " << p.curvature << ")";
00654     return (os);
00655   }
00656 
00660   struct EIGEN_ALIGN16 PointWithRange
00661   {
00662     PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00663     union
00664     {
00665       struct
00666       {
00667         float range;
00668       };
00669       float data_c[4];
00670     };
00671     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00672   };
00673   inline std::ostream& operator << (std::ostream& os, const PointWithRange& p)
00674   {
00675     os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.range << ")";
00676     return (os);
00677   }
00678 
00679   struct EIGEN_ALIGN16 _PointWithViewpoint
00680   {
00681     PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4])
00682     union
00683     {
00684       struct
00685       {
00686         float vp_x;
00687         float vp_y;
00688         float vp_z;
00689       };
00690       float data_c[4];
00691     };
00692     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
00693   };
00694 
00698   struct EIGEN_ALIGN16 PointWithViewpoint : public _PointWithViewpoint
00699   {
00700     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)
00701     {
00702       x=_x; y=_y; z=_z; data[3] = 1.0f;
00703       vp_x=_vp_x; vp_y=_vp_y; vp_z=_vp_z;
00704     }
00705   };
00706   inline std::ostream& operator << (std::ostream& os, const PointWithViewpoint& p)
00707   {
00708     os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.vp_x << "," << p.vp_y << "," << p.vp_z << ")";
00709     return (os);
00710   }
00711 
00715   struct MomentInvariants
00716   {
00717     float j1, j2, j3;
00718   };
00719   inline std::ostream& operator << (std::ostream& os, const MomentInvariants& p)
00720   {
00721     os << "(" << p.j1 << "," << p.j2 << "," << p.j3 << ")";
00722     return (os);
00723   }
00724 
00728   struct PrincipalRadiiRSD
00729   {
00730     float r_min, r_max;
00731   };
00732   inline std::ostream& operator << (std::ostream& os, const PrincipalRadiiRSD& p)
00733   {
00734     os << "(" << p.r_min << "," << p.r_max << ")";
00735     return (os);
00736   }
00737 
00741   struct Boundary
00742   {
00743     uint8_t boundary_point;
00744   };
00745   inline std::ostream& operator << (std::ostream& os, const Boundary& p)
00746   {
00747     os << p.boundary_point;
00748     return (os);
00749   }
00750 
00754   struct PrincipalCurvatures
00755   {
00756     union
00757     {
00758       float principal_curvature[3];
00759       struct
00760       {
00761         float principal_curvature_x;
00762         float principal_curvature_y;
00763         float principal_curvature_z;
00764       };
00765     };
00766     float pc1;
00767     float pc2;
00768   };
00769   inline std::ostream& operator << (std::ostream& os, const PrincipalCurvatures& p)
00770   {
00771     os << "(" << p.principal_curvature[0] << "," << p.principal_curvature[1] << "," << p.principal_curvature[2] << " - " << p.pc1 << "," << p.pc2 << ")";
00772     return (os);
00773   }
00774 
00778   struct PFHSignature125
00779   {
00780     float histogram[125];
00781   };
00782   inline std::ostream& operator << (std::ostream& os, const PFHSignature125& p)
00783   {
00784     for (int i = 0; i < 125; ++i)
00785     os << (i == 0 ? "(" : "") << p.histogram[i] << (i < 124 ? ", " : ")");
00786     return (os);
00787   }
00788 
00792   struct PFHRGBSignature250
00793   {
00794     float histogram[250];
00795   };
00796   inline std::ostream& operator << (std::ostream& os, const PFHRGBSignature250& p)
00797   {
00798     for (int i = 0; i < 250; ++i)
00799     os << (i == 0 ? "(" : "") << p.histogram[i] << (i < 249 ? ", " : ")");
00800     return (os);
00801   }
00802 
00806   struct PPFSignature
00807   {
00808     float f1, f2, f3, f4;
00809     float alpha_m;
00810   };
00811   inline std::ostream& operator << (std::ostream& os, const PPFSignature& p)
00812   {
00813     os << "(" << p.f1 << ", " << p.f2 << ", " << p.f3 << ", " << p.f4 << ", " << p.alpha_m << ")";
00814     return (os);
00815   }
00816 
00820    struct PPFRGBSignature
00821    {
00822      float f1, f2, f3, f4;
00823      float r_ratio, g_ratio, b_ratio;
00824      float alpha_m;
00825    };
00826    inline std::ostream& operator << (std::ostream& os, const PPFRGBSignature& p)
00827    {
00828      os << "(" << p.f1 << ", " << p.f2 << ", " << p.f3 << ", " << p.f4 << ", " <<
00829          p.r_ratio << ", " << p.g_ratio << ", " << p.b_ratio << ", " << p.alpha_m << ")";
00830      return (os);
00831    }
00832 
00837   struct NormalBasedSignature12
00838   {
00839     float values[12];
00840   };
00841   inline std::ostream& operator << (std::ostream& os, const NormalBasedSignature12& p)
00842   {
00843     for (int i = 0; i < 12; ++i)
00844     os << (i == 0 ? "(" : "") << p.values[i] << (i < 11 ? ", " : ")");
00845     return (os);
00846   }
00847 
00851   struct SHOT
00852   {
00853     std::vector<float> descriptor;
00854     float rf[9];
00855   };
00856 
00857   inline std::ostream& operator << (std::ostream& os, const SHOT& p)
00858   {
00859     for (int i = 0; i < 9; ++i)
00860     os << (i == 0 ? "(" : "") << p.rf[i] << (i < 8 ? ", " : ")");
00861     for (size_t i = 0; i < p.descriptor.size (); ++i)
00862     os << (i == 0 ? "(" : "") << p.descriptor[i] << (i < p.descriptor.size()-1 ? ", " : ")");
00863     return (os);
00864   }
00865 
00869   //struct _SHOT352
00870   //{
00871   //
00873   //  float descriptor[352];
00874   //  float rf[9];
00875   //  uint32_t size;
00876   //};
00877   //struct SHOT352 : public _SHOT352
00878   //{
00879   //  SHOT352 ()
00880   //  {
00881   //    size = 352;
00882   //  }
00883   //};
00884   //inline std::ostream& operator << (std::ostream& os, const SHOT352& p)
00885   //{
00886   //  for (int i = 0; i < 9; ++i)
00887   //    os << (i == 0 ? "(" : "") << p.rf[i] << (i < 8 ? ", " : ")");
00888   //  for (uint32_t i = 0; i < p.size; ++i)
00889   //    os << (i == 0 ? "(" : "") << p.descriptor[i] << (i < 351 ? ", " : ")");
00890   //  return (os);
00891   //}
00892   //
00893   //
00895   //  * \ingroup common
00896   //  */
00897   //struct _SHOT1344
00898   //{
00900   //  float descriptor[1344];
00901   //  float rf[9];
00902   //  uint32_t size;
00903   //};
00904   //struct SHOT1344 : public _SHOT1344
00905   //{
00906   //  SHOT1344 ()
00907   //  {
00908   //    size = 1344;
00909   //  }
00910   //};
00911   //inline std::ostream& operator << (std::ostream& os, const SHOT1344& p)
00912   //{
00913   //  for (int i = 0; i < 9; ++i)
00914   //    os << (i == 0 ? "(" : "") << p.rf[i] << (i < 8 ? ", " : ")");
00915   //  for (uint32_t i = 0; i < p.size; ++i)
00916   //    os << (i == 0 ? "(" : "") << p.descriptor[i] << (i < 1343 ? ", " : ")");
00917   //  return (os);
00918   //}
00919 
00923   struct FPFHSignature33
00924   {
00925     float histogram[33];
00926   };
00927   inline std::ostream& operator << (std::ostream& os, const FPFHSignature33& p)
00928   {
00929     for (int i = 0; i < 33; ++i)
00930     os << (i == 0 ? "(" : "") << p.histogram[i] << (i < 32 ? ", " : ")");
00931     return (os);
00932   }
00933 
00937   struct VFHSignature308
00938   {
00939     float histogram[308];
00940   };
00941   inline std::ostream& operator << (std::ostream& os, const VFHSignature308& p)
00942   {
00943     for (int i = 0; i < 308; ++i)
00944     os << (i == 0 ? "(" : "") << p.histogram[i] << (i < 307 ? ", " : ")");
00945     return (os);
00946   }
00947 
00951   struct Narf36
00952   {
00953     float x, y, z, roll, pitch, yaw;
00954     float descriptor[36];
00955   };
00956   inline std::ostream& operator << (std::ostream& os, const Narf36& p)
00957   {
00958     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 - ";
00959     for (int i = 0; i < 36; ++i)
00960     os << (i == 0 ? "(" : "") << p.descriptor[i] << (i < 35 ? ", " : ")");
00961     return (os);
00962   }
00963 
00967   struct BorderDescription
00968   {
00969     int x, y;
00970     BorderTraits traits;
00971     //std::vector<const BorderDescription*> neighbors;
00972   };
00973 
00974   inline std::ostream& operator << (std::ostream& os, const BorderDescription& p)
00975   {
00976     os << "(" << p.x << "," << p.y << ")";
00977     return (os);
00978   }
00979 
00983   struct IntensityGradient
00984   {
00985     union
00986     {
00987       float gradient[3];
00988       struct
00989       {
00990         float gradient_x;
00991         float gradient_y;
00992         float gradient_z;
00993       };
00994     };
00995   };
00996   inline std::ostream& operator << (std::ostream& os, const IntensityGradient& p)
00997   {
00998     os << "(" << p.gradient[0] << "," << p.gradient[1] << "," << p.gradient[2] << ")";
00999     return (os);
01000   }
01001 
01005   template <int N>
01006   struct Histogram
01007   {
01008     float histogram[N];
01009   };
01010   template <int N>
01011   inline std::ostream& operator << (std::ostream& os, const Histogram<N>& p)
01012   {
01013     for (int i = 0; i < N; ++i)
01014     os << (i == 0 ? "(" : "") << p.histogram[i] << (i < N-1 ? ", " : ")");
01015     return (os);
01016   }
01017 
01021   struct EIGEN_ALIGN16 PointWithScale
01022   {
01023     PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4])
01024     float scale;
01025     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
01026   };
01027   inline std::ostream& operator << (std::ostream& os, const PointWithScale& p)
01028   {
01029     os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.scale << ")";
01030     return (os);
01031   }
01032 
01036   struct EIGEN_ALIGN16 PointSurfel
01037   {
01038     PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4])
01039     PCL_ADD_NORMAL4D; // This adds the member normal[3] which can also be accessed using the point (which is float[4])
01040     union
01041     {
01042       struct
01043       {
01044         uint32_t rgba;
01045         float radius;
01046         float confidence;
01047         float curvature;
01048       };
01049       float data_c[4];
01050     };
01051     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
01052   };
01053   inline std::ostream& operator << (std::ostream& os, const PointSurfel& p)
01054   {
01055     unsigned char* rgba_ptr = (unsigned char*)&p.rgba;
01056     os <<
01057     "(" << p.x << "," << p.y << "," << p.z << " - " <<
01058     p.normal_x << "," << p.normal_y << "," << p.normal_z << " - " <<
01059     (int)(*rgba_ptr) << "," << (int)(*(rgba_ptr+1)) << "," << (int)(*(rgba_ptr+2)) << "," <<(int)(*(rgba_ptr+3)) << " - " <<
01060     p.radius << " - " << p.confidence << " - " << p.curvature << ")";
01061     return (os);
01062   }
01063 
01064   template <typename PointType1, typename PointType2> inline float
01065   squaredEuclideanDistance (const PointType1& p1, const PointType2& p2)
01066   {
01067     float diff_x = p2.x - p1.x, diff_y = p2.y - p1.y, diff_z = p2.z - p1.z;
01068     return (diff_x*diff_x + diff_y*diff_y + diff_z*diff_z);
01069   }
01070 
01071   template <typename PointType1, typename PointType2> inline float
01072   euclideanDistance (const PointType1& p1, const PointType2& p2)
01073   {
01074     return (sqrtf (squaredEuclideanDistance (p1, p2)));
01075   }
01076 
01077   template <typename PointType> inline bool
01078   hasValidXYZ (const PointType& p)
01079   {
01080     return (pcl_isfinite (p.x) && pcl_isfinite (p.y) && pcl_isfinite (p.z));
01081   }
01082 
01083 } // End namespace
01084 
01085 #endif