00001 #ifndef PCL_TRACKING_IMPL_TRACKING_H_
00002 #define PCL_TRACKING_IMPL_TRACKING_H_
00003
00004 #include <pcl/common/eigen.h>
00005 #include <ctime>
00006 #include <pcl/tracking/boost.h>
00007 #include <pcl/tracking/tracking.h>
00008
00009 namespace pcl
00010 {
00011 namespace tracking
00012 {
00013 struct _ParticleXYZRPY
00014 {
00015 PCL_ADD_POINT4D;
00016 union
00017 {
00018 struct
00019 {
00020 float roll;
00021 float pitch;
00022 float yaw;
00023 float weight;
00024 };
00025 float data_c[4];
00026 };
00027 };
00028
00029
00030 struct EIGEN_ALIGN16 ParticleXYZRPY : public _ParticleXYZRPY
00031 {
00032 inline ParticleXYZRPY ()
00033 {
00034 x = y = z = roll = pitch = yaw = 0.0;
00035 data[3] = 1.0f;
00036 }
00037
00038 inline ParticleXYZRPY (float _x, float _y, float _z)
00039 {
00040 x = _x; y = _y; z = _z;
00041 roll = pitch = yaw = 0.0;
00042 data[3] = 1.0f;
00043 }
00044
00045 inline ParticleXYZRPY (float _x, float _y, float _z, float _roll, float _pitch, float _yaw)
00046 {
00047 x = _x; y = _y; z = _z;
00048 roll = _roll; pitch = _pitch; yaw = _yaw;
00049 data[3] = 1.0f;
00050 }
00051
00052 inline static int
00053 stateDimension () { return 6; }
00054
00055 void
00056 sample (const std::vector<double>& mean, const std::vector<double>& cov)
00057 {
00058 x += static_cast<float> (sampleNormal (mean[0], cov[0]));
00059 y += static_cast<float> (sampleNormal (mean[1], cov[1]));
00060 z += static_cast<float> (sampleNormal (mean[2], cov[2]));
00061 roll += static_cast<float> (sampleNormal (mean[3], cov[3]));
00062 pitch += static_cast<float> (sampleNormal (mean[4], cov[4]));
00063 yaw += static_cast<float> (sampleNormal (mean[5], cov[5]));
00064 }
00065
00066 void
00067 zero ()
00068 {
00069 x = 0.0;
00070 y = 0.0;
00071 z = 0.0;
00072 roll = 0.0;
00073 pitch = 0.0;
00074 yaw = 0.0;
00075 }
00076
00077 inline Eigen::Affine3f
00078 toEigenMatrix () const
00079 {
00080 return getTransformation(x, y, z, roll, pitch, yaw);
00081 }
00082
00083 static pcl::tracking::ParticleXYZRPY
00084 toState (const Eigen::Affine3f &trans)
00085 {
00086 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
00087 getTranslationAndEulerAngles (trans,
00088 trans_x, trans_y, trans_z,
00089 trans_roll, trans_pitch, trans_yaw);
00090 return pcl::tracking::ParticleXYZRPY (trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
00091 }
00092
00093
00094 inline float operator [] (unsigned int i)
00095 {
00096 switch (i)
00097 {
00098 case 0: return x;
00099 case 1: return y;
00100 case 2: return z;
00101 case 3: return roll;
00102 case 4: return pitch;
00103 case 5: return yaw;
00104 default: return 0.0;
00105 }
00106 }
00107
00108 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00109 };
00110
00111 inline std::ostream& operator << (std::ostream& os, const ParticleXYZRPY& p)
00112 {
00113 os << "(" << p.x << "," << p.y << "," << p.z << ","
00114 << p.roll << "," << p.pitch << "," << p.yaw << ")";
00115 return (os);
00116 }
00117
00118
00119 inline pcl::tracking::ParticleXYZRPY operator * (const ParticleXYZRPY& p, double val)
00120 {
00121 pcl::tracking::ParticleXYZRPY newp;
00122 newp.x = static_cast<float> (p.x * val);
00123 newp.y = static_cast<float> (p.y * val);
00124 newp.z = static_cast<float> (p.z * val);
00125 newp.roll = static_cast<float> (p.roll * val);
00126 newp.pitch = static_cast<float> (p.pitch * val);
00127 newp.yaw = static_cast<float> (p.yaw * val);
00128 return (newp);
00129 }
00130
00131
00132 inline pcl::tracking::ParticleXYZRPY operator + (const ParticleXYZRPY& a, const ParticleXYZRPY& b)
00133 {
00134 pcl::tracking::ParticleXYZRPY newp;
00135 newp.x = a.x + b.x;
00136 newp.y = a.y + b.y;
00137 newp.z = a.z + b.z;
00138 newp.roll = a.roll + b.roll;
00139 newp.pitch = a.pitch + b.pitch;
00140 newp.yaw = a.yaw + b.yaw;
00141 return (newp);
00142 }
00143
00144
00145 inline pcl::tracking::ParticleXYZRPY operator - (const ParticleXYZRPY& a, const ParticleXYZRPY& b)
00146 {
00147 pcl::tracking::ParticleXYZRPY newp;
00148 newp.x = a.x - b.x;
00149 newp.y = a.y - b.y;
00150 newp.z = a.z - b.z;
00151 newp.roll = a.roll - b.roll;
00152 newp.pitch = a.pitch - b.pitch;
00153 newp.yaw = a.yaw - b.yaw;
00154 return (newp);
00155 }
00156
00157 }
00158 }
00159
00160
00161
00162
00163
00164 namespace pcl
00165 {
00166 namespace tracking
00167 {
00168 struct _ParticleXYZR
00169 {
00170 PCL_ADD_POINT4D;
00171 union
00172 {
00173 struct
00174 {
00175 float roll;
00176 float pitch;
00177 float yaw;
00178 float weight;
00179 };
00180 float data_c[4];
00181 };
00182 };
00183
00184
00185 struct EIGEN_ALIGN16 ParticleXYZR : public _ParticleXYZR
00186 {
00187 inline ParticleXYZR ()
00188 {
00189 x = y = z = roll = pitch = yaw = 0.0;
00190 data[3] = 1.0f;
00191 }
00192
00193 inline ParticleXYZR (float _x, float _y, float _z)
00194 {
00195 x = _x; y = _y; z = _z;
00196 roll = pitch = yaw = 0.0;
00197 data[3] = 1.0f;
00198 }
00199
00200 inline ParticleXYZR (float _x, float _y, float _z, float, float _pitch, float)
00201 {
00202 x = _x; y = _y; z = _z;
00203 roll = 0; pitch = _pitch; yaw = 0;
00204 data[3] = 1.0f;
00205 }
00206
00207 inline static int
00208 stateDimension () { return 6; }
00209
00210 void
00211 sample (const std::vector<double>& mean, const std::vector<double>& cov)
00212 {
00213 x += static_cast<float> (sampleNormal (mean[0], cov[0]));
00214 y += static_cast<float> (sampleNormal (mean[1], cov[1]));
00215 z += static_cast<float> (sampleNormal (mean[2], cov[2]));
00216 roll = 0;
00217 pitch += static_cast<float> (sampleNormal (mean[4], cov[4]));
00218 yaw = 0;
00219 }
00220
00221 void
00222 zero ()
00223 {
00224 x = 0.0;
00225 y = 0.0;
00226 z = 0.0;
00227 roll = 0.0;
00228 pitch = 0.0;
00229 yaw = 0.0;
00230 }
00231
00232 inline Eigen::Affine3f
00233 toEigenMatrix () const
00234 {
00235 return getTransformation(x, y, z, roll, pitch, yaw);
00236 }
00237
00238 static pcl::tracking::ParticleXYZR
00239 toState (const Eigen::Affine3f &trans)
00240 {
00241 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
00242 getTranslationAndEulerAngles (trans,
00243 trans_x, trans_y, trans_z,
00244 trans_roll, trans_pitch, trans_yaw);
00245 return (pcl::tracking::ParticleXYZR (trans_x, trans_y, trans_z, 0, trans_pitch, 0));
00246 }
00247
00248
00249 inline float operator [] (unsigned int i)
00250 {
00251 switch (i)
00252 {
00253 case 0: return x;
00254 case 1: return y;
00255 case 2: return z;
00256 case 3: return roll;
00257 case 4: return pitch;
00258 case 5: return yaw;
00259 default: return 0.0;
00260 }
00261 }
00262
00263 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00264 };
00265
00266 inline std::ostream& operator << (std::ostream& os, const ParticleXYZR& p)
00267 {
00268 os << "(" << p.x << "," << p.y << "," << p.z << ","
00269 << p.roll << "," << p.pitch << "," << p.yaw << ")";
00270 return (os);
00271 }
00272
00273
00274 inline pcl::tracking::ParticleXYZR operator * (const ParticleXYZR& p, double val)
00275 {
00276 pcl::tracking::ParticleXYZR newp;
00277 newp.x = static_cast<float> (p.x * val);
00278 newp.y = static_cast<float> (p.y * val);
00279 newp.z = static_cast<float> (p.z * val);
00280 newp.roll = static_cast<float> (p.roll * val);
00281 newp.pitch = static_cast<float> (p.pitch * val);
00282 newp.yaw = static_cast<float> (p.yaw * val);
00283 return (newp);
00284 }
00285
00286
00287 inline pcl::tracking::ParticleXYZR operator + (const ParticleXYZR& a, const ParticleXYZR& b)
00288 {
00289 pcl::tracking::ParticleXYZR newp;
00290 newp.x = a.x + b.x;
00291 newp.y = a.y + b.y;
00292 newp.z = a.z + b.z;
00293 newp.roll = 0;
00294 newp.pitch = a.pitch + b.pitch;
00295 newp.yaw = 0.0;
00296 return (newp);
00297 }
00298
00299
00300 inline pcl::tracking::ParticleXYZR operator - (const ParticleXYZR& a, const ParticleXYZR& b)
00301 {
00302 pcl::tracking::ParticleXYZR newp;
00303 newp.x = a.x - b.x;
00304 newp.y = a.y - b.y;
00305 newp.z = a.z - b.z;
00306 newp.roll = 0.0;
00307 newp.pitch = a.pitch - b.pitch;
00308 newp.yaw = 0.0;
00309 return (newp);
00310 }
00311
00312 }
00313 }
00314
00315
00316
00317
00318
00319 namespace pcl
00320 {
00321 namespace tracking
00322 {
00323 struct _ParticleXYRPY
00324 {
00325 PCL_ADD_POINT4D;
00326 union
00327 {
00328 struct
00329 {
00330 float roll;
00331 float pitch;
00332 float yaw;
00333 float weight;
00334 };
00335 float data_c[4];
00336 };
00337 };
00338
00339
00340 struct EIGEN_ALIGN16 ParticleXYRPY : public _ParticleXYRPY
00341 {
00342 inline ParticleXYRPY ()
00343 {
00344 x = y = z = roll = pitch = yaw = 0.0;
00345 data[3] = 1.0f;
00346 }
00347
00348 inline ParticleXYRPY (float _x, float, float _z)
00349 {
00350 x = _x; y = 0; z = _z;
00351 roll = pitch = yaw = 0.0;
00352 data[3] = 1.0f;
00353 }
00354
00355 inline ParticleXYRPY (float _x, float, float _z, float _roll, float _pitch, float _yaw)
00356 {
00357 x = _x; y = 0; z = _z;
00358 roll = _roll; pitch = _pitch; yaw = _yaw;
00359 data[3] = 1.0f;
00360 }
00361
00362 inline static int
00363 stateDimension () { return 6; }
00364
00365 void
00366 sample (const std::vector<double>& mean, const std::vector<double>& cov)
00367 {
00368 x += static_cast<float> (sampleNormal (mean[0], cov[0]));
00369 y = 0;
00370 z += static_cast<float> (sampleNormal (mean[2], cov[2]));
00371 roll += static_cast<float> (sampleNormal (mean[3], cov[3]));
00372 pitch += static_cast<float> (sampleNormal (mean[4], cov[4]));
00373 yaw += static_cast<float> (sampleNormal (mean[5], cov[5]));
00374 }
00375
00376 void
00377 zero ()
00378 {
00379 x = 0.0;
00380 y = 0.0;
00381 z = 0.0;
00382 roll = 0.0;
00383 pitch = 0.0;
00384 yaw = 0.0;
00385 }
00386
00387 inline Eigen::Affine3f
00388 toEigenMatrix () const
00389 {
00390 return getTransformation(x, y, z, roll, pitch, yaw);
00391 }
00392
00393 static pcl::tracking::ParticleXYRPY
00394 toState (const Eigen::Affine3f &trans)
00395 {
00396 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
00397 getTranslationAndEulerAngles (trans,
00398 trans_x, trans_y, trans_z,
00399 trans_roll, trans_pitch, trans_yaw);
00400 return (pcl::tracking::ParticleXYRPY (trans_x, 0, trans_z, trans_roll, trans_pitch, trans_yaw));
00401 }
00402
00403
00404 inline float operator [] (unsigned int i)
00405 {
00406 switch (i)
00407 {
00408 case 0: return x;
00409 case 1: return y;
00410 case 2: return z;
00411 case 3: return roll;
00412 case 4: return pitch;
00413 case 5: return yaw;
00414 default: return 0.0;
00415 }
00416 }
00417
00418 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00419 };
00420
00421 inline std::ostream& operator << (std::ostream& os, const ParticleXYRPY& p)
00422 {
00423 os << "(" << p.x << "," << p.y << "," << p.z << ","
00424 << p.roll << "," << p.pitch << "," << p.yaw << ")";
00425 return (os);
00426 }
00427
00428
00429 inline pcl::tracking::ParticleXYRPY operator * (const ParticleXYRPY& p, double val)
00430 {
00431 pcl::tracking::ParticleXYRPY newp;
00432 newp.x = static_cast<float> (p.x * val);
00433 newp.y = static_cast<float> (p.y * val);
00434 newp.z = static_cast<float> (p.z * val);
00435 newp.roll = static_cast<float> (p.roll * val);
00436 newp.pitch = static_cast<float> (p.pitch * val);
00437 newp.yaw = static_cast<float> (p.yaw * val);
00438 return (newp);
00439 }
00440
00441
00442 inline pcl::tracking::ParticleXYRPY operator + (const ParticleXYRPY& a, const ParticleXYRPY& b)
00443 {
00444 pcl::tracking::ParticleXYRPY newp;
00445 newp.x = a.x + b.x;
00446 newp.y = 0;
00447 newp.z = a.z + b.z;
00448 newp.roll = a.roll + b.roll;
00449 newp.pitch = a.pitch + b.pitch;
00450 newp.yaw = a.yaw + b.yaw;
00451 return (newp);
00452 }
00453
00454
00455 inline pcl::tracking::ParticleXYRPY operator - (const ParticleXYRPY& a, const ParticleXYRPY& b)
00456 {
00457 pcl::tracking::ParticleXYRPY newp;
00458 newp.x = a.x - b.x;
00459 newp.z = a.z - b.z;
00460 newp.y = 0;
00461 newp.roll = a.roll - b.roll;
00462 newp.pitch = a.pitch - b.pitch;
00463 newp.yaw = a.yaw - b.yaw;
00464 return (newp);
00465 }
00466
00467 }
00468 }
00469
00470
00471
00472 namespace pcl
00473 {
00474 namespace tracking
00475 {
00476 struct _ParticleXYRP
00477 {
00478 PCL_ADD_POINT4D;
00479 union
00480 {
00481 struct
00482 {
00483 float roll;
00484 float pitch;
00485 float yaw;
00486 float weight;
00487 };
00488 float data_c[4];
00489 };
00490 };
00491
00492
00493 struct EIGEN_ALIGN16 ParticleXYRP : public _ParticleXYRP
00494 {
00495 inline ParticleXYRP ()
00496 {
00497 x = y = z = roll = pitch = yaw = 0.0;
00498 data[3] = 1.0f;
00499 }
00500
00501 inline ParticleXYRP (float _x, float, float _z)
00502 {
00503 x = _x; y = 0; z = _z;
00504 roll = pitch = yaw = 0.0;
00505 data[3] = 1.0f;
00506 }
00507
00508 inline ParticleXYRP (float _x, float, float _z, float, float _pitch, float _yaw)
00509 {
00510 x = _x; y = 0; z = _z;
00511 roll = 0; pitch = _pitch; yaw = _yaw;
00512 data[3] = 1.0f;
00513 }
00514
00515 inline static int
00516 stateDimension () { return 6; }
00517
00518 void
00519 sample (const std::vector<double>& mean, const std::vector<double>& cov)
00520 {
00521 x += static_cast<float> (sampleNormal (mean[0], cov[0]));
00522 y = 0;
00523 z += static_cast<float> (sampleNormal (mean[2], cov[2]));
00524 roll = 0;
00525 pitch += static_cast<float> (sampleNormal (mean[4], cov[4]));
00526 yaw += static_cast<float> (sampleNormal (mean[5], cov[5]));
00527 }
00528
00529 void
00530 zero ()
00531 {
00532 x = 0.0;
00533 y = 0.0;
00534 z = 0.0;
00535 roll = 0.0;
00536 pitch = 0.0;
00537 yaw = 0.0;
00538 }
00539
00540 inline Eigen::Affine3f
00541 toEigenMatrix () const
00542 {
00543 return getTransformation(x, y, z, roll, pitch, yaw);
00544 }
00545
00546 static pcl::tracking::ParticleXYRP
00547 toState (const Eigen::Affine3f &trans)
00548 {
00549 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
00550 getTranslationAndEulerAngles (trans,
00551 trans_x, trans_y, trans_z,
00552 trans_roll, trans_pitch, trans_yaw);
00553 return (pcl::tracking::ParticleXYRP (trans_x, 0, trans_z, 0, trans_pitch, trans_yaw));
00554 }
00555
00556
00557 inline float operator [] (unsigned int i)
00558 {
00559 switch (i)
00560 {
00561 case 0: return x;
00562 case 1: return y;
00563 case 2: return z;
00564 case 3: return roll;
00565 case 4: return pitch;
00566 case 5: return yaw;
00567 default: return 0.0;
00568 }
00569 }
00570
00571 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00572 };
00573
00574 inline std::ostream& operator << (std::ostream& os, const ParticleXYRP& p)
00575 {
00576 os << "(" << p.x << "," << p.y << "," << p.z << ","
00577 << p.roll << "," << p.pitch << "," << p.yaw << ")";
00578 return (os);
00579 }
00580
00581
00582 inline pcl::tracking::ParticleXYRP operator * (const ParticleXYRP& p, double val)
00583 {
00584 pcl::tracking::ParticleXYRP newp;
00585 newp.x = static_cast<float> (p.x * val);
00586 newp.y = static_cast<float> (p.y * val);
00587 newp.z = static_cast<float> (p.z * val);
00588 newp.roll = static_cast<float> (p.roll * val);
00589 newp.pitch = static_cast<float> (p.pitch * val);
00590 newp.yaw = static_cast<float> (p.yaw * val);
00591 return (newp);
00592 }
00593
00594
00595 inline pcl::tracking::ParticleXYRP operator + (const ParticleXYRP& a, const ParticleXYRP& b)
00596 {
00597 pcl::tracking::ParticleXYRP newp;
00598 newp.x = a.x + b.x;
00599 newp.y = 0;
00600 newp.z = a.z + b.z;
00601 newp.roll = 0;
00602 newp.pitch = a.pitch + b.pitch;
00603 newp.yaw = a.yaw + b.yaw;
00604 return (newp);
00605 }
00606
00607
00608 inline pcl::tracking::ParticleXYRP operator - (const ParticleXYRP& a, const ParticleXYRP& b)
00609 {
00610 pcl::tracking::ParticleXYRP newp;
00611 newp.x = a.x - b.x;
00612 newp.z = a.z - b.z;
00613 newp.y = 0;
00614 newp.roll = 0.0;
00615 newp.pitch = a.pitch - b.pitch;
00616 newp.yaw = a.yaw - b.yaw;
00617 return (newp);
00618 }
00619
00620 }
00621 }
00622
00623
00624
00625 namespace pcl
00626 {
00627 namespace tracking
00628 {
00629 struct _ParticleXYR
00630 {
00631 PCL_ADD_POINT4D;
00632 union
00633 {
00634 struct
00635 {
00636 float roll;
00637 float pitch;
00638 float yaw;
00639 float weight;
00640 };
00641 float data_c[4];
00642 };
00643 };
00644
00645
00646 struct EIGEN_ALIGN16 ParticleXYR : public _ParticleXYR
00647 {
00648 inline ParticleXYR ()
00649 {
00650 x = y = z = roll = pitch = yaw = 0.0;
00651 data[3] = 1.0f;
00652 }
00653
00654 inline ParticleXYR (float _x, float, float _z)
00655 {
00656 x = _x; y = 0; z = _z;
00657 roll = pitch = yaw = 0.0;
00658 data[3] = 1.0f;
00659 }
00660
00661 inline ParticleXYR (float _x, float, float _z, float, float _pitch, float)
00662 {
00663 x = _x; y = 0; z = _z;
00664 roll = 0; pitch = _pitch; yaw = 0;
00665 data[3] = 1.0f;
00666 }
00667
00668 inline static int
00669 stateDimension () { return 6; }
00670
00671 void
00672 sample (const std::vector<double>& mean, const std::vector<double>& cov)
00673 {
00674 x += static_cast<float> (sampleNormal (mean[0], cov[0]));
00675 y = 0;
00676 z += static_cast<float> (sampleNormal (mean[2], cov[2]));
00677 roll = 0;
00678 pitch += static_cast<float> (sampleNormal (mean[4], cov[4]));
00679 yaw = 0;
00680 }
00681
00682 void
00683 zero ()
00684 {
00685 x = 0.0;
00686 y = 0.0;
00687 z = 0.0;
00688 roll = 0.0;
00689 pitch = 0.0;
00690 yaw = 0.0;
00691 }
00692
00693 inline Eigen::Affine3f
00694 toEigenMatrix () const
00695 {
00696 return getTransformation(x, y, z, roll, pitch, yaw);
00697 }
00698
00699 static pcl::tracking::ParticleXYR
00700 toState (const Eigen::Affine3f &trans)
00701 {
00702 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
00703 getTranslationAndEulerAngles (trans,
00704 trans_x, trans_y, trans_z,
00705 trans_roll, trans_pitch, trans_yaw);
00706 return (pcl::tracking::ParticleXYR (trans_x, 0, trans_z, 0, trans_pitch, 0));
00707 }
00708
00709
00710 inline float operator [] (unsigned int i)
00711 {
00712 switch (i)
00713 {
00714 case 0: return x;
00715 case 1: return y;
00716 case 2: return z;
00717 case 3: return roll;
00718 case 4: return pitch;
00719 case 5: return yaw;
00720 default: return 0.0;
00721 }
00722 }
00723
00724 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00725 };
00726
00727 inline std::ostream& operator << (std::ostream& os, const ParticleXYR& p)
00728 {
00729 os << "(" << p.x << "," << p.y << "," << p.z << ","
00730 << p.roll << "," << p.pitch << "," << p.yaw << ")";
00731 return (os);
00732 }
00733
00734
00735 inline pcl::tracking::ParticleXYR operator * (const ParticleXYR& p, double val)
00736 {
00737 pcl::tracking::ParticleXYR newp;
00738 newp.x = static_cast<float> (p.x * val);
00739 newp.y = static_cast<float> (p.y * val);
00740 newp.z = static_cast<float> (p.z * val);
00741 newp.roll = static_cast<float> (p.roll * val);
00742 newp.pitch = static_cast<float> (p.pitch * val);
00743 newp.yaw = static_cast<float> (p.yaw * val);
00744 return (newp);
00745 }
00746
00747
00748 inline pcl::tracking::ParticleXYR operator + (const ParticleXYR& a, const ParticleXYR& b)
00749 {
00750 pcl::tracking::ParticleXYR newp;
00751 newp.x = a.x + b.x;
00752 newp.y = 0;
00753 newp.z = a.z + b.z;
00754 newp.roll = 0;
00755 newp.pitch = a.pitch + b.pitch;
00756 newp.yaw = 0.0;
00757 return (newp);
00758 }
00759
00760
00761 inline pcl::tracking::ParticleXYR operator - (const ParticleXYR& a, const ParticleXYR& b)
00762 {
00763 pcl::tracking::ParticleXYR newp;
00764 newp.x = a.x - b.x;
00765 newp.z = a.z - b.z;
00766 newp.y = 0;
00767 newp.roll = 0.0;
00768 newp.pitch = a.pitch - b.pitch;
00769 newp.yaw = 0.0;
00770 return (newp);
00771 }
00772
00773 }
00774 }
00775
00776 #define PCL_STATE_POINT_TYPES \
00777 (pcl::tracking::ParticleXYR) \
00778 (pcl::tracking::ParticleXYZRPY) \
00779 (pcl::tracking::ParticleXYZR) \
00780 (pcl::tracking::ParticleXYRPY) \
00781 (pcl::tracking::ParticleXYRP)
00782
00783 #endif //