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
00037
00038 #ifndef PCL_IMPL_POINT_TYPES_HPP_
00039 #define PCL_IMPL_POINT_TYPES_HPP_
00040
00041
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::BorderDescription) \
00064 (pcl::IntensityGradient) \
00065 (pcl::Histogram<2>) \
00066 (pcl::PointWithScale)
00067
00068
00069 #define PCL_XYZ_POINT_TYPES \
00070 (pcl::PointXYZ) \
00071 (pcl::PointXYZI) \
00072 (pcl::PointXYZRGBA) \
00073 (pcl::PointXYZRGB) \
00074 (pcl::InterestPoint) \
00075 (pcl::PointNormal) \
00076 (pcl::PointXYZRGBNormal) \
00077 (pcl::PointXYZINormal) \
00078 (pcl::PointWithRange) \
00079 (pcl::PointWithViewpoint) \
00080 (pcl::PointWithScale)
00081
00082
00083 #define PCL_NORMAL_POINT_TYPES \
00084 (pcl::Normal) \
00085 (pcl::PointNormal) \
00086 (pcl::PointXYZRGBNormal) \
00087 (pcl::PointXYZINormal)
00088
00089 namespace pcl
00090 {
00091
00092 #define PCL_ADD_POINT4D \
00093 union { \
00094 float data[4]; \
00095 struct { \
00096 float x; \
00097 float y; \
00098 float z; \
00099 }; \
00100 } EIGEN_ALIGN16; \
00101 inline Eigen::Map<Eigen::Vector3f> getVector3fMap () { return (Eigen::Vector3f::Map (data)); } \
00102 inline const Eigen::Map<const Eigen::Vector3f> getVector3fMap () const { return (Eigen::Vector3f::Map (data)); } \
00103 inline Eigen::Map<Eigen::Vector4f, Eigen::Aligned> getVector4fMap () { return (Eigen::Vector4f::MapAligned (data)); } \
00104 inline const Eigen::Map<const Eigen::Vector4f, Eigen::Aligned> getVector4fMap () const { return (Eigen::Vector4f::MapAligned (data)); } \
00105 inline Eigen::Map<Eigen::Array3f> getArray3fMap () { return (Eigen::Array3f::Map (data)); } \
00106 inline const Eigen::Map<const Eigen::Array3f> getArray3fMap () const { return (Eigen::Array3f::Map (data)); } \
00107 inline Eigen::Map<Eigen::Array4f, Eigen::Aligned> getArray4fMap () { return (Eigen::Array4f::MapAligned (data)); } \
00108 inline const Eigen::Map<const Eigen::Array4f, Eigen::Aligned> getArray4fMap () const { return (Eigen::Array4f::MapAligned (data)); }
00109
00110 #define PCL_ADD_NORMAL4D \
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 } EIGEN_ALIGN16; \
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;
00138 EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
00139 };
00140
00141
00142
00143
00144
00145
00146
00147 struct PointXYZ : public _PointXYZ
00148 {
00149 inline PointXYZ()
00150 {
00151 x = y = z = 0.0f;
00152 data[3] = 1.0f;
00153 }
00154 inline PointXYZ (float _x, float _y, float _z) { x = _x; y = _y; z = _z; data[3] = 1.0f; }
00155 };
00156
00157 inline std::ostream& operator << (std::ostream& os, const PointXYZ& p)
00158 {
00159 os << "(" << p.x << "," << p.y << "," << p.z << ")";
00160 return (os);
00161 }
00162
00163 struct PointXYZI
00164 {
00165 PCL_ADD_POINT4D;
00166 union
00167 {
00168 struct
00169 {
00170 float intensity;
00171 };
00172 float data_c[4];
00173 };
00174 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00175 } EIGEN_ALIGN16;
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
00183 struct PointXYZRGBA
00184 {
00185 PCL_ADD_POINT4D;
00186 union
00187 {
00188 struct
00189 {
00190 uint32_t rgba;
00191 };
00192 float data_c[4];
00193 };
00194 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00195 } EIGEN_ALIGN16;
00196 inline std::ostream& operator << (std::ostream& os, const PointXYZRGBA& p)
00197 {
00198 unsigned char* rgba_ptr = (unsigned char*)&p.rgba;
00199 os << "(" << p.x << "," << p.y << "," << p.z << " - " << (int)(*rgba_ptr) << "," << (int)(*(rgba_ptr+1)) << "," << (int)(*(rgba_ptr+2)) << "," <<(int)(*(rgba_ptr+3)) << ")";
00200 return (os);
00201 }
00202
00203
00204 struct PointXYZRGB
00205 {
00206 PCL_ADD_POINT4D;
00207 union
00208 {
00209 struct
00210 {
00211 float rgb;
00212 };
00213 float data_c[4];
00214 };
00215 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00216 } EIGEN_ALIGN16;
00217 inline std::ostream& operator << (std::ostream& os, const PointXYZRGB& p)
00218 {
00219 os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.rgb << ")";
00220 return (os);
00221 }
00222
00223
00224 struct PointXY
00225 {
00226 float x;
00227 float y;
00228 };
00229 inline std::ostream& operator << (std::ostream& os, const PointXY& p)
00230 {
00231 os << "(" << p.x << "," << p.y << ")";
00232 return (os);
00233 }
00234
00235
00236 struct InterestPoint
00237 {
00238 PCL_ADD_POINT4D;
00239 union
00240 {
00241 struct
00242 {
00243 float strength;
00244 };
00245 float data_c[4];
00246 };
00247 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00248 } EIGEN_ALIGN16;
00249 inline std::ostream& operator << (std::ostream& os, const InterestPoint& p)
00250 {
00251 os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.strength << ")";
00252 return (os);
00253 }
00254
00255
00256 struct Normal
00257 {
00258 PCL_ADD_NORMAL4D;
00259 union
00260 {
00261 struct
00262 {
00263 float curvature;
00264 };
00265 float data_c[4];
00266 };
00267 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00268 } EIGEN_ALIGN16;
00269 inline std::ostream& operator << (std::ostream& os, const Normal& p)
00270 {
00271 os << "(" << p.normal[0] << "," << p.normal[1] << "," << p.normal[2] << " - " << p.curvature << ")";
00272 return (os);
00273 }
00274
00275
00276 struct PointNormal
00277 {
00278 PCL_ADD_POINT4D;
00279 PCL_ADD_NORMAL4D;
00280 union
00281 {
00282 struct
00283 {
00284 float curvature;
00285 };
00286 float data_c[4];
00287 };
00288 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00289 } EIGEN_ALIGN16;
00290 inline std::ostream& operator << (std::ostream& os, const PointNormal& p)
00291 {
00292 os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.normal[0] << "," << p.normal[1] << "," << p.normal[2] << " - " << p.curvature << ")";
00293 return (os);
00294 }
00295
00296
00297 struct PointXYZRGBNormal
00298 {
00299 PCL_ADD_POINT4D;
00300 PCL_ADD_NORMAL4D;
00301 union
00302 {
00303 struct
00304 {
00305 float rgb;
00306 float curvature;
00307 };
00308 float data_c[4];
00309 };
00310 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00311 } EIGEN_ALIGN16;
00312 inline std::ostream& operator << (std::ostream& os, const PointXYZRGBNormal& p)
00313 {
00314 os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.rgb << " - " << p.normal[0] << "," << p.normal[1] << "," << p.normal[2] << " - " << p.curvature << ")";
00315 return (os);
00316 }
00317
00318 struct PointXYZINormal
00319 {
00320 PCL_ADD_POINT4D;
00321 PCL_ADD_NORMAL4D;
00322 union
00323 {
00324 struct
00325 {
00326 float intensity;
00327 float curvature;
00328 };
00329 float data_c[4];
00330 };
00331 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00332 } EIGEN_ALIGN16;
00333 inline std::ostream& operator << (std::ostream& os, const PointXYZINormal& p)
00334 {
00335 os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.intensity << " - " << p.normal[0] << "," << p.normal[1] << "," << p.normal[2] << " - " << p.curvature << ")";
00336 return (os);
00337 }
00338
00339 struct PointWithRange
00340 {
00341 PCL_ADD_POINT4D;
00342 union
00343 {
00344 struct
00345 {
00346 float range;
00347 };
00348 float data_c[4];
00349 };
00350 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00351 } EIGEN_ALIGN16;
00352 inline std::ostream& operator << (std::ostream& os, const PointWithRange& p)
00353 {
00354 os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.range << ")";
00355 return (os);
00356 }
00357
00358 struct _PointWithViewpoint
00359 {
00360 PCL_ADD_POINT4D;
00361 union
00362 {
00363 struct
00364 {
00365 float vp_x;
00366 float vp_y;
00367 float vp_z;
00368 };
00369 float data_c[4];
00370 };
00371 EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
00372 } EIGEN_ALIGN16;
00373
00374 struct PointWithViewpoint : public _PointWithViewpoint
00375 {
00376 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)
00377 {
00378 x=_x; y=_y; z=_z; data[3] = 1.0f;
00379 vp_x=_vp_x; vp_y=_vp_y; vp_z=_vp_z;
00380 }
00381 };
00382 inline std::ostream& operator << (std::ostream& os, const PointWithViewpoint& p)
00383 {
00384 os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.vp_x << "," << p.vp_y << "," << p.vp_z << ")";
00385 return (os);
00386 }
00387
00388 struct MomentInvariants
00389 {
00390 float j1, j2, j3;
00391 };
00392 inline std::ostream& operator << (std::ostream& os, const MomentInvariants& p)
00393 {
00394 os << "(" << p.j1 << "," << p.j2 << "," << p.j3 << ")";
00395 return (os);
00396 }
00397
00398 struct PrincipalRadiiRSD
00399 {
00400 float r_min, r_max;
00401 };
00402 inline std::ostream& operator << (std::ostream& os, const PrincipalRadiiRSD& p)
00403 {
00404 os << "(" << p.r_min << "," << p.r_max << ")";
00405 return (os);
00406 }
00407
00408 struct Boundary
00409 {
00410 uint8_t boundary_point;
00411 };
00412 inline std::ostream& operator << (std::ostream& os, const Boundary& p)
00413 {
00414 os << p.boundary_point;
00415 return (os);
00416 }
00417
00418 struct PrincipalCurvatures
00419 {
00420 union
00421 {
00422 float principal_curvature[3];
00423 struct
00424 {
00425 float principal_curvature_x;
00426 float principal_curvature_y;
00427 float principal_curvature_z;
00428 };
00429 };
00430 float pc1;
00431 float pc2;
00432 };
00433 inline std::ostream& operator << (std::ostream& os, const PrincipalCurvatures& p)
00434 {
00435 os << "(" << p.principal_curvature[0] << "," << p.principal_curvature[1] << "," << p.principal_curvature[2] << " - " << p.pc1 << "," << p.pc2 << ")";
00436 return (os);
00437 }
00438
00439 struct PFHSignature125
00440 {
00441 float histogram[125];
00442 };
00443 inline std::ostream& operator << (std::ostream& os, const PFHSignature125& p)
00444 {
00445 for (int i = 0; i < 125; ++i)
00446 os << (i == 0 ? "(" : "") << p.histogram[i] << (i < 124 ? ", " : ")");
00447 return (os);
00448 }
00449
00450 struct FPFHSignature33
00451 {
00452 float histogram[33];
00453 };
00454 inline std::ostream& operator << (std::ostream& os, const FPFHSignature33& p)
00455 {
00456 for (int i = 0; i < 33; ++i)
00457 os << (i == 0 ? "(" : "") << p.histogram[i] << (i < 32 ? ", " : ")");
00458 return (os);
00459 }
00460
00461 struct VFHSignature308
00462 {
00463 float histogram[308];
00464 };
00465 inline std::ostream& operator << (std::ostream& os, const VFHSignature308& p)
00466 {
00467 for (int i = 0; i < 308; ++i)
00468 os << (i == 0 ? "(" : "") << p.histogram[i] << (i < 307 ? ", " : ")");
00469 return (os);
00470 }
00471
00472 struct Narf36
00473 {
00474 float x, y, z, roll, pitch, yaw;
00475 float descriptor[36];
00476 };
00477 inline std::ostream& operator << (std::ostream& os, const Narf36& p)
00478 {
00479 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 - ";
00480 for (int i = 0; i < 36; ++i)
00481 os << (i == 0 ? "(" : "") << p.descriptor[i] << (i < 35 ? ", " : ")");
00482 return (os);
00483 }
00484
00485 struct BorderDescription
00486 {
00487 int x, y;
00488 BorderTraits traits;
00489
00490 };
00491
00492 inline std::ostream& operator << (std::ostream& os, const BorderDescription& p)
00493 {
00494 os << "(" << p.x << "," << p.y << ")";
00495 return (os);
00496 }
00497
00498 struct IntensityGradient
00499 {
00500 union
00501 {
00502 float gradient[3];
00503 struct
00504 {
00505 float gradient_x;
00506 float gradient_y;
00507 float gradient_z;
00508 };
00509 };
00510 };
00511 inline std::ostream& operator << (std::ostream& os, const IntensityGradient& p)
00512 {
00513 os << "(" << p.gradient[0] << "," << p.gradient[1] << "," << p.gradient[2] << ")";
00514 return (os);
00515 }
00516
00517 template <int N>
00518 struct Histogram
00519 {
00520 float histogram[N];
00521 };
00522 template <int N>
00523 inline std::ostream& operator << (std::ostream& os, const Histogram<N>& p)
00524 {
00525 for (int i = 0; i < N; ++i)
00526 os << (i == 0 ? "(" : "") << p.histogram[i] << (i < N-1 ? ", " : ")");
00527 return (os);
00528 }
00529
00530 struct PointWithScale
00531 {
00532 PCL_ADD_POINT4D;
00533 float scale;
00534 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00535 } EIGEN_ALIGN16;
00536 inline std::ostream& operator << (std::ostream& os, const PointWithScale& p)
00537 {
00538 os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.scale << ")";
00539 return (os);
00540 }
00541
00542 struct PointSurfel
00543 {
00544 PCL_ADD_POINT4D;
00545 PCL_ADD_NORMAL4D;
00546 union
00547 {
00548 struct
00549 {
00550 uint32_t rgba;
00551 float radius;
00552 float confidence;
00553 float curvature;
00554 };
00555 float data_c[4];
00556 };
00557 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00558 } EIGEN_ALIGN16;
00559 inline std::ostream& operator << (std::ostream& os, const PointSurfel& p)
00560 {
00561 unsigned char* rgba_ptr = (unsigned char*)&p.rgba;
00562 os <<
00563 "(" << p.x << "," << p.y << "," << p.z << " - " <<
00564 p.normal_x << "," << p.normal_y << "," << p.normal_z << " - " <<
00565 (int)(*rgba_ptr) << "," << (int)(*(rgba_ptr+1)) << "," << (int)(*(rgba_ptr+2)) << "," <<(int)(*(rgba_ptr+3)) << " - " <<
00566 p.radius << " - " << p.confidence << " - " << p.curvature << ")";
00567 return (os);
00568 }
00569
00570
00571 template <typename PointType1, typename PointType2>
00572 inline float squaredEuclideanDistance (const PointType1& p1, const PointType2& p2)
00573 {
00574 float diff_x = p2.x - p1.x, diff_y = p2.y - p1.y, diff_z = p2.z - p1.z;
00575 return (diff_x*diff_x + diff_y*diff_y + diff_z*diff_z);
00576 }
00577
00578 template <typename PointType1, typename PointType2>
00579 inline float euclideanDistance (const PointType1& p1, const PointType2& p2)
00580 {
00581 return (sqrtf (squaredEuclideanDistance (p1, p2)));
00582 }
00583
00584 template <typename PointType>
00585 inline bool hasValidXYZ (const PointType& p)
00586 {
00587 return (pcl_isfinite (p.x) && pcl_isfinite (p.y) && pcl_isfinite (p.z));
00588 }
00589
00590 }
00591
00592 #endif