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