Point Cloud Library (PCL)
1.7.0
|
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 // particle definition 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 // a[i] 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 // a * k 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 // a + b 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 // a - b 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 //########################################################################33 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 // particle definition 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 // a[i] 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 // a * k 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 // a + b 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 // a - b 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 //########################################################################33 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 // particle definition 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 // a[i] 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 // a * k 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 // a + b 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 // a - b 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 //########################################################################33 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 // particle definition 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 // a[i] 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 // a * k 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 // a + b 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 // a - b 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 //########################################################################33 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 // particle definition 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 // a[i] 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 // a * k 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 // a + b 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 // a - b 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 //