test_common.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2012, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: test_common.cpp 6126 2012-07-03 20:19:58Z aichim $
00037  *
00038  */
00039 
00040 #include <gtest/gtest.h>
00041 #include <pcl/common/common.h>
00042 #include <pcl/common/distances.h>
00043 #include <pcl/common/intersections.h>
00044 #include <pcl/common/eigen.h>
00045 #include <pcl/point_types.h>
00046 #include <pcl/point_cloud.h>
00047 
00048 #include <pcl/common/centroid.h>
00049 
00050 using namespace pcl;
00051 
00053 TEST (PCL, PointXYZRGB)
00054 {
00055   PointXYZRGB p;
00056 
00057   uint8_t r = 127, g = 64, b = 254;
00058   uint32_t rgb = (static_cast<uint32_t> (r) << 16 | 
00059                   static_cast<uint32_t> (g) << 8 | 
00060                   static_cast<uint32_t> (b));
00061   p.rgb = *reinterpret_cast<float*>(&rgb);
00062 
00063   rgb = *reinterpret_cast<int*>(&p.rgb);
00064   uint8_t rr = (rgb >> 16) & 0x0000ff;
00065   uint8_t gg = (rgb >> 8)  & 0x0000ff;
00066   uint8_t bb = (rgb)       & 0x0000ff;
00067 
00068   EXPECT_EQ (r, rr);
00069   EXPECT_EQ (g, gg);
00070   EXPECT_EQ (b, bb);
00071   EXPECT_EQ (rr, 127);
00072   EXPECT_EQ (gg, 64);
00073   EXPECT_EQ (bb, 254);
00074 
00075   p.r = 0; p.g = 127; p.b = 0;
00076   rgb = *reinterpret_cast<int*>(&p.rgb);
00077   rr = (rgb >> 16) & 0x0000ff;
00078   gg = (rgb >> 8)  & 0x0000ff;
00079   bb = (rgb)       & 0x0000ff;
00080 
00081   EXPECT_EQ (rr, 0);
00082   EXPECT_EQ (gg, 127);
00083   EXPECT_EQ (bb, 0);
00084 }
00085 
00087 TEST (PCL, PointXYZRGBNormal)
00088 {
00089   PointXYZRGBNormal p;
00090 
00091   uint8_t r = 127, g = 64, b = 254;
00092   uint32_t rgb = (static_cast<uint32_t> (r) << 16 | 
00093                   static_cast<uint32_t> (g) << 8 | 
00094                   static_cast<uint32_t> (b));
00095   p.rgb = *reinterpret_cast<float*>(&rgb);
00096 
00097   rgb = *reinterpret_cast<int*>(&p.rgb);
00098   uint8_t rr = (rgb >> 16) & 0x0000ff;
00099   uint8_t gg = (rgb >> 8)  & 0x0000ff;
00100   uint8_t bb = (rgb)       & 0x0000ff;
00101 
00102   EXPECT_EQ (r, rr);
00103   EXPECT_EQ (g, gg);
00104   EXPECT_EQ (b, bb);
00105   EXPECT_EQ (rr, 127);
00106   EXPECT_EQ (gg, 64);
00107   EXPECT_EQ (bb, 254);
00108 
00109   p.r = 0; p.g = 127; p.b = 0;
00110   rgb = *reinterpret_cast<int*>(&p.rgb);
00111   rr = (rgb >> 16) & 0x0000ff;
00112   gg = (rgb >> 8)  & 0x0000ff;
00113   bb = (rgb)       & 0x0000ff;
00114 
00115   EXPECT_EQ (rr, 0);
00116   EXPECT_EQ (gg, 127);
00117   EXPECT_EQ (bb, 0);
00118 }
00119 
00121 TEST(PCL, isFinite)
00122 {
00123   PointXYZ p;
00124   p.x = std::numeric_limits<float>::quiet_NaN ();
00125   EXPECT_EQ (isFinite (p), false);
00126   Normal n;
00127   n.normal_x = std::numeric_limits<float>::quiet_NaN ();
00128   EXPECT_EQ (isFinite (n), false);
00129 }
00130 
00132 TEST (PCL, Common)
00133 {
00134   PointXYZ p1, p2, p3;
00135   p1.x = 1; p1.y = p1.z = 0;
00136   p2.y = 1; p2.x = p2.z = 0;
00137   p3.z = 1; p3.x = p3.y = 0;
00138   double radius = getCircumcircleRadius (p1, p2, p3);
00139   EXPECT_NEAR (radius, 0.816497, 1e-4);
00140 
00141   Eigen::Vector4f pt (1,0,0,0), line_pt (0,0,0,0), line_dir (1,1,0,0);
00142   double point2line_disance = sqrt (sqrPointToLineDistance (pt, line_pt, line_dir));
00143   EXPECT_NEAR (point2line_disance, sqrt(2.0)/2, 1e-4);
00144 }
00145 
00147 TEST (PCL, Eigen)
00148 {
00149   Eigen::Matrix3f mat, vec;
00150   mat << 0.000536227f, -1.56178e-05f, -9.47391e-05f, -1.56178e-05f, 0.000297322f, -0.000148785f, -9.47391e-05f, -0.000148785f, 9.7827e-05f;
00151   Eigen::Vector3f val;
00152 
00153   eigen33 (mat, vec, val);
00154 
00155   EXPECT_NEAR (fabs (vec (0, 0)), 0.168841, 1e-4); EXPECT_NEAR (fabs (vec (0, 1)), 0.161623, 1e-4); EXPECT_NEAR (fabs (vec (0, 2)), 0.972302, 1e-4);
00156   EXPECT_NEAR (fabs (vec (1, 0)), 0.451632, 1e-4); EXPECT_NEAR (fabs (vec (1, 1)), 0.889498, 1e-4); EXPECT_NEAR (fabs (vec (1, 2)), 0.0694328, 1e-4);
00157   EXPECT_NEAR (fabs (vec (2, 0)), 0.876082, 1e-4); EXPECT_NEAR (fabs (vec (2, 1)), 0.4274,   1e-4); EXPECT_NEAR (fabs (vec (2, 2)), 0.223178, 1e-4);
00158 
00159   EXPECT_NEAR (val (0), 2.86806e-06, 1e-4); EXPECT_NEAR (val (1), 0.00037165, 1e-4); EXPECT_NEAR (val (2), 0.000556858, 1e-4);
00160 
00161   Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eig (mat);
00162 
00163   EXPECT_NEAR (eig.eigenvectors () (0, 0), -0.168841, 1e-4); EXPECT_NEAR (eig.eigenvectors () (0, 1),  0.161623, 1e-4); EXPECT_NEAR (eig.eigenvectors () (0, 2),  0.972302, 1e-4);
00164   EXPECT_NEAR (eig.eigenvectors () (1, 0), -0.451632, 1e-4); EXPECT_NEAR (eig.eigenvectors () (1, 1), -0.889498, 1e-4); EXPECT_NEAR (eig.eigenvectors () (1, 2),  0.0694328, 1e-4);
00165   EXPECT_NEAR (eig.eigenvectors () (2, 0), -0.876083, 1e-4); EXPECT_NEAR (eig.eigenvectors () (2, 1),  0.4274,   1e-4); EXPECT_NEAR (eig.eigenvectors () (2, 2), -0.223178, 1e-4);
00166 
00167   EXPECT_NEAR (eig.eigenvalues () (0), 2.86806e-06, 1e-4); EXPECT_NEAR (eig.eigenvalues () (1), 0.00037165, 1e-4); EXPECT_NEAR (eig.eigenvalues () (2), 0.000556858, 1e-4);
00168 
00169   Eigen::Vector3f eivals = mat.selfadjointView<Eigen::Lower>().eigenvalues ();
00170 
00171   EXPECT_NEAR (eivals (0), 2.86806e-06, 1e-4); EXPECT_NEAR (eivals (1), 0.00037165, 1e-4); EXPECT_NEAR (eivals (2), 0.000556858, 1e-4);
00172 
00173 }
00174 
00176 TEST (PCL, PointCloud)
00177 {
00178   PointCloud<PointXYZ> cloud;
00179   cloud.width = 640;
00180   cloud.height = 480;
00181 
00182   EXPECT_EQ (cloud.isOrganized (), true);
00183 
00184   cloud.height = 1;
00185   EXPECT_EQ (cloud.isOrganized (), false);
00186 
00187   cloud.width = 10;
00188   for (uint32_t i = 0; i < cloud.width*cloud.height; ++i)
00189   {
00190     float j = static_cast<float> (i);
00191     cloud.points.push_back (PointXYZ (3.0f * j + 0.0f, 3.0f * j + 1.0f, 3.0f * j + 2.0f));
00192   }
00193 
00194   Eigen::MatrixXf mat_xyz1 = cloud.getMatrixXfMap ();
00195   Eigen::MatrixXf mat_xyz = cloud.getMatrixXfMap (3, 4, 0);
00196 
00197   if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit)
00198   {
00199     EXPECT_EQ (mat_xyz1.cols (), 4);
00200     EXPECT_EQ (mat_xyz1.rows (), cloud.width);
00201     EXPECT_EQ (mat_xyz1 (0, 0), 0);
00202     EXPECT_EQ (mat_xyz1 (cloud.width - 1, 2), 3 * cloud.width - 1);   // = 29
00203 
00204     EXPECT_EQ (mat_xyz.cols (), 3);
00205     EXPECT_EQ (mat_xyz.rows (), cloud.width);
00206     EXPECT_EQ (mat_xyz (0, 0), 0);
00207     EXPECT_EQ (mat_xyz (cloud.width - 1, 2), 3 * cloud.width - 1);    // = 29
00208   }
00209   else
00210   {
00211     EXPECT_EQ (mat_xyz1.cols (), cloud.width);
00212     EXPECT_EQ (mat_xyz1.rows (), 4);
00213     EXPECT_EQ (mat_xyz1 (0, 0), 0);
00214     EXPECT_EQ (mat_xyz1 (2, cloud.width - 1), 3 * cloud.width - 1);   // = 29
00215 
00216     EXPECT_EQ (mat_xyz.cols (), cloud.width);
00217     EXPECT_EQ (mat_xyz.rows (), 3);
00218     EXPECT_EQ (mat_xyz (0, 0), 0);
00219     EXPECT_EQ (mat_xyz (2, cloud.width - 1), 3 * cloud.width - 1);    // = 29
00220   }
00221   
00222 #ifdef NDEBUG
00223   if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit)
00224   {
00225     Eigen::MatrixXf mat_yz = cloud.getMatrixXfMap (2, 4, 1);
00226     EXPECT_EQ (mat_yz.cols (), 2);
00227     EXPECT_EQ (mat_yz.rows (), cloud.width);
00228     EXPECT_EQ (mat_yz (0, 0), 1);
00229     EXPECT_EQ (mat_yz (cloud.width - 1, 1), 3 * cloud.width - 1);
00230     uint32_t j = 1;
00231     for (uint32_t i = 1; i < cloud.width*cloud.height; i+=4, j+=3)
00232     {
00233       Eigen::MatrixXf mat_yz = cloud.getMatrixXfMap (2, 4, i);
00234       EXPECT_EQ (mat_yz.cols (), 2);
00235       EXPECT_EQ (mat_yz.rows (), cloud.width);
00236       EXPECT_EQ (mat_yz (0, 0), j);
00237     }
00238   }
00239   else
00240   {
00241     Eigen::MatrixXf mat_yz = cloud.getMatrixXfMap (2, 4, 1);
00242     EXPECT_EQ (mat_yz.cols (), cloud.width);
00243     EXPECT_EQ (mat_yz.rows (), 2);
00244     EXPECT_EQ (mat_yz (0, 0), 1);
00245     EXPECT_EQ (mat_yz (1, cloud.width - 1), 3 * cloud.width - 1);
00246     uint32_t j = 1;
00247     for (uint32_t i = 1; i < cloud.width*cloud.height; i+=4, j+=3)
00248     {
00249       Eigen::MatrixXf mat_yz = cloud.getMatrixXfMap (2, 4, i);
00250       EXPECT_EQ (mat_yz.cols (), cloud.width);
00251       EXPECT_EQ (mat_yz.rows (), 2);
00252       EXPECT_EQ (mat_yz (0, 0), j);
00253     }
00254   }
00255 #endif
00256   cloud.clear ();
00257   EXPECT_EQ (cloud.width, 0);
00258   EXPECT_EQ (cloud.height, 0);
00259 
00260   cloud.width = 640;
00261   cloud.height = 480;
00262 
00263   cloud.insert (cloud.end (), PointXYZ (1, 1, 1));
00264   EXPECT_EQ (cloud.isOrganized (), false);
00265   EXPECT_EQ (cloud.width, 1);
00266 
00267   cloud.insert (cloud.end (), 5, PointXYZ (1, 1, 1));
00268   EXPECT_EQ (cloud.isOrganized (), false);
00269   EXPECT_EQ (cloud.width, 6);
00270 
00271   cloud.erase (cloud.end () - 1);
00272   EXPECT_EQ (cloud.isOrganized (), false);
00273   EXPECT_EQ (cloud.width, 5);
00274 
00275   cloud.erase (cloud.begin (), cloud.end ());
00276   EXPECT_EQ (cloud.isOrganized (), false);
00277   EXPECT_EQ (cloud.width, 0);
00278 }
00279 
00281 TEST (PCL, PointTypes)
00282 {
00283   EXPECT_EQ (sizeof (PointXYZ), 16);
00284   EXPECT_EQ (__alignof (PointXYZ), 16);
00285   EXPECT_EQ (sizeof (PointXYZI), 32);
00286   EXPECT_EQ (__alignof (PointXYZI), 16);
00287   EXPECT_EQ (sizeof (PointXYZRGB), 32);
00288   EXPECT_EQ (__alignof (PointXYZRGB), 16);
00289   EXPECT_EQ (sizeof (PointXYZRGBA), 32);
00290   EXPECT_EQ (__alignof (PointXYZRGBA), 16);
00291   EXPECT_EQ (sizeof (Normal), 32);
00292   EXPECT_EQ (__alignof (Normal), 16);
00293   EXPECT_EQ (sizeof (PointNormal), 48);
00294   EXPECT_EQ (__alignof (PointNormal), 16);
00295   EXPECT_EQ (sizeof (PointXYZRGBNormal), 48);
00296   EXPECT_EQ (__alignof (PointXYZRGBNormal), 16);
00297 }
00298 
00300 TEST (PCL, Intersections)
00301 {
00302   Eigen::VectorXf zline (6), yline (6);
00303   zline[0] = 0.543892f; zline[1] = -0.515623f; zline[2] = 1.321f;   zline[3] = 0.0266191f; zline[4] = 0.600215f;  zline[5] = -0.0387667f;
00304   yline[0] = 0.493479f; yline[1] = 0.169246f;  yline[2] = 1.22677f; yline[3] = 0.5992f;    yline[4] = 0.0505085f; yline[5] = 0.405749f;
00305 
00306   Eigen::Vector4f pt;
00307   EXPECT_EQ ((pcl::lineWithLineIntersection (zline, yline, pt)), true);
00308   EXPECT_NEAR (pt[0], 0.574544, 1e-3);
00309   EXPECT_NEAR (pt[1], 0.175526, 1e-3);
00310   EXPECT_NEAR (pt[2], 1.27636,  1e-3);
00311   EXPECT_EQ (pt[3], 0);
00312 
00313   zline << 0.545203f, -0.514419f, 1.31967f, 0.0243372f, 0.597946f, -0.0413579f;
00314   yline << 0.492706f,  0.164196f, 1.23192f, 0.598704f,  0.0442014f, 0.411328f;
00315   EXPECT_EQ ((pcl::lineWithLineIntersection (zline, yline, pt)), false);
00316   //intersection: [ 3.06416e+08    15.2237     3.06416e+08       4.04468e-34 ]
00317 }
00318 
00320 TEST (PCL, compute3DCentroid)
00321 {
00322   std::vector<int> indices;
00323   PointXYZ point;
00324   PointCloud<PointXYZ> cloud;
00325   Eigen::Vector4f centroid;
00326 
00327   // test empty cloud which is dense
00328   cloud.is_dense = true;
00329   EXPECT_EQ (compute3DCentroid (cloud, centroid), 0);
00330 
00331   // test empty cloud non_dense
00332   cloud.is_dense = false;
00333   EXPECT_EQ (compute3DCentroid (cloud, centroid), 0);
00334 
00335   // test non-empty cloud non_dense
00336   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00337   cloud.push_back (point);
00338   EXPECT_EQ (compute3DCentroid (cloud, centroid), 0);
00339 
00340   // test non-empty cloud non_dense
00341   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00342   cloud.push_back (point);
00343   indices.push_back (1);
00344   EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 0);
00345 
00346   cloud.clear ();
00347   indices.clear ();
00348   for (point.x = -1; point.x < 2; point.x += 2)
00349   {
00350     for (point.y = -1; point.y < 2; point.y += 2)
00351     {
00352       for (point.z = -1; point.z < 2; point.z += 2)
00353       {
00354         cloud.push_back (point);
00355       }
00356     }
00357   }
00358   cloud.is_dense = true;
00359 
00360   // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1)
00361   centroid [0] = -100;
00362   centroid [1] = -200;
00363   centroid [2] = -300;
00364 
00365   EXPECT_EQ (compute3DCentroid (cloud, centroid), 8);
00366   EXPECT_EQ (centroid [0], 0);
00367   EXPECT_EQ (centroid [1], 0);
00368   EXPECT_EQ (centroid [2], 0);
00369 
00370   centroid [0] = -100;
00371   centroid [1] = -200;
00372   centroid [2] = -300;
00373   indices.resize (4); // only positive y values
00374   indices [0] = 2;
00375   indices [1] = 3;
00376   indices [2] = 6;
00377   indices [3] = 7;
00378   EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4);
00379 
00380   EXPECT_EQ (centroid [0], 0.0);
00381   EXPECT_EQ (centroid [1], 1.0);
00382   EXPECT_EQ (centroid [2], 0.0);
00383 
00384   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00385   cloud.push_back (point);
00386   cloud.is_dense = false;
00387 
00388   centroid [0] = -100;
00389   centroid [1] = -200;
00390   centroid [2] = -300;
00391   EXPECT_EQ (compute3DCentroid (cloud, centroid), 8);
00392 
00393   EXPECT_EQ (centroid [0], 0);
00394   EXPECT_EQ (centroid [1], 0);
00395   EXPECT_EQ (centroid [2], 0);
00396 
00397   centroid [0] = -100;
00398   centroid [1] = -200;
00399   centroid [2] = -300;
00400   indices [0] = 2;
00401   indices [1] = 3;
00402   indices [2] = 6;
00403   indices [3] = 7;
00404   indices.push_back (8); // add the NaN
00405   EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4);
00406 
00407   EXPECT_EQ (centroid [0], 0.0);
00408   EXPECT_EQ (centroid [1], 1.0);
00409   EXPECT_EQ (centroid [2], 0.0);
00410 }
00411 
00413 TEST (PCL, computeCovarianceMatrix)
00414 {
00415   PointCloud<PointXYZ> cloud;
00416   PointXYZ point;
00417   std::vector <int> indices;
00418   Eigen::Vector4f centroid;
00419   Eigen::Matrix3f covariance_matrix;
00420 
00421   centroid [0] = 0;
00422   centroid [1] = 0;
00423   centroid [2] = 0;
00424 
00425   // test empty cloud which is dense
00426   cloud.is_dense = true;
00427   EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 0);
00428 
00429   // test empty cloud non_dense
00430   cloud.is_dense = false;
00431   EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 0);
00432 
00433   // test non-empty cloud non_dense
00434   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00435   cloud.push_back (point);
00436   EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 0);
00437 
00438   // test non-empty cloud non_dense
00439   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00440   cloud.push_back (point);
00441   indices.push_back (1);
00442   EXPECT_EQ (computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix), 0);
00443 
00444   cloud.clear ();
00445   indices.clear ();
00446   for (point.x = -1; point.x < 2; point.x += 2)
00447   {
00448     for (point.y = -1; point.y < 2; point.y += 2)
00449     {
00450       for (point.z = -1; point.z < 2; point.z += 2)
00451       {
00452         cloud.push_back (point);
00453       }
00454     }
00455   }
00456   cloud.is_dense = true;
00457 
00458   // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1)
00459 
00460   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00461   centroid [0] = 0;
00462   centroid [1] = 0;
00463   centroid [2] = 0;
00464 
00465   EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 8);
00466   EXPECT_EQ (covariance_matrix (0, 0), 8);
00467   EXPECT_EQ (covariance_matrix (0, 1), 0);
00468   EXPECT_EQ (covariance_matrix (0, 2), 0);
00469   EXPECT_EQ (covariance_matrix (1, 0), 0);
00470   EXPECT_EQ (covariance_matrix (1, 1), 8);
00471   EXPECT_EQ (covariance_matrix (1, 2), 0);
00472   EXPECT_EQ (covariance_matrix (2, 0), 0);
00473   EXPECT_EQ (covariance_matrix (2, 1), 0);
00474   EXPECT_EQ (covariance_matrix (2, 2), 8);
00475 
00476   indices.resize (4); // only positive y values
00477   indices [0] = 2;
00478   indices [1] = 3;
00479   indices [2] = 6;
00480   indices [3] = 7;
00481   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00482   centroid [1] = 1;
00483 
00484   EXPECT_EQ (computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix), 4);
00485   EXPECT_EQ (covariance_matrix (0, 0), 4);
00486   EXPECT_EQ (covariance_matrix (0, 1), 0);
00487   EXPECT_EQ (covariance_matrix (0, 2), 0);
00488   EXPECT_EQ (covariance_matrix (1, 0), 0);
00489   EXPECT_EQ (covariance_matrix (1, 1), 0);
00490   EXPECT_EQ (covariance_matrix (1, 2), 0);
00491   EXPECT_EQ (covariance_matrix (2, 0), 0);
00492   EXPECT_EQ (covariance_matrix (2, 1), 0);
00493   EXPECT_EQ (covariance_matrix (2, 2), 4);
00494 
00495   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00496   cloud.push_back (point);
00497   cloud.is_dense = false;
00498   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00499   centroid [1] = 0;
00500 
00501   EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 8);
00502   EXPECT_EQ (covariance_matrix (0, 0), 8);
00503   EXPECT_EQ (covariance_matrix (0, 1), 0);
00504   EXPECT_EQ (covariance_matrix (0, 2), 0);
00505   EXPECT_EQ (covariance_matrix (1, 0), 0);
00506   EXPECT_EQ (covariance_matrix (1, 1), 8);
00507   EXPECT_EQ (covariance_matrix (1, 2), 0);
00508   EXPECT_EQ (covariance_matrix (2, 0), 0);
00509   EXPECT_EQ (covariance_matrix (2, 1), 0);
00510   EXPECT_EQ (covariance_matrix (2, 2), 8);
00511 
00512   indices.push_back (8); // add the NaN
00513   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00514   centroid [1] = 1;
00515 
00516   EXPECT_EQ (computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix), 4);
00517   EXPECT_EQ (covariance_matrix (0, 0), 4);
00518   EXPECT_EQ (covariance_matrix (0, 1), 0);
00519   EXPECT_EQ (covariance_matrix (0, 2), 0);
00520   EXPECT_EQ (covariance_matrix (1, 0), 0);
00521   EXPECT_EQ (covariance_matrix (1, 1), 0);
00522   EXPECT_EQ (covariance_matrix (1, 2), 0);
00523   EXPECT_EQ (covariance_matrix (2, 0), 0);
00524   EXPECT_EQ (covariance_matrix (2, 1), 0);
00525   EXPECT_EQ (covariance_matrix (2, 2), 4);
00526 }
00527 
00529 TEST (PCL, computeCovarianceMatrixNormalized)
00530 {
00531   PointCloud<PointXYZ> cloud;
00532   PointXYZ point;
00533   std::vector <int> indices;
00534   Eigen::Vector4f centroid;
00535   Eigen::Matrix3f covariance_matrix;
00536 
00537   centroid [0] = 0;
00538   centroid [1] = 0;
00539   centroid [2] = 0;
00540 
00541   // test empty cloud which is dense
00542   cloud.is_dense = true;
00543   EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 0);
00544 
00545   // test empty cloud non_dense
00546   cloud.is_dense = false;
00547   EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 0);
00548 
00549   // test non-empty cloud non_dense
00550   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00551   cloud.push_back (point);
00552   EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 0);
00553 
00554   // test non-empty cloud non_dense
00555   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00556   cloud.push_back (point);
00557   indices.push_back (1);
00558   EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, indices, centroid, covariance_matrix), 0);
00559 
00560   cloud.clear ();
00561   indices.clear ();
00562   for (point.x = -1; point.x < 2; point.x += 2)
00563   {
00564     for (point.y = -1; point.y < 2; point.y += 2)
00565     {
00566       for (point.z = -1; point.z < 2; point.z += 2)
00567       {
00568         cloud.push_back (point);
00569       }
00570     }
00571   }
00572   cloud.is_dense = true;
00573 
00574   // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1)
00575 
00576   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00577   centroid [0] = 0;
00578   centroid [1] = 0;
00579   centroid [2] = 0;
00580 
00581   EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 8);
00582 
00583   EXPECT_EQ (covariance_matrix (0, 0), 1);
00584   EXPECT_EQ (covariance_matrix (0, 1), 0);
00585   EXPECT_EQ (covariance_matrix (0, 2), 0);
00586   EXPECT_EQ (covariance_matrix (1, 0), 0);
00587   EXPECT_EQ (covariance_matrix (1, 1), 1);
00588   EXPECT_EQ (covariance_matrix (1, 2), 0);
00589   EXPECT_EQ (covariance_matrix (2, 0), 0);
00590   EXPECT_EQ (covariance_matrix (2, 1), 0);
00591   EXPECT_EQ (covariance_matrix (2, 2), 1);
00592 
00593   indices.resize (4); // only positive y values
00594   indices [0] = 2;
00595   indices [1] = 3;
00596   indices [2] = 6;
00597   indices [3] = 7;
00598   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00599   centroid [1] = 1;
00600 
00601   EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, indices, centroid, covariance_matrix), 4);
00602 
00603   EXPECT_EQ (covariance_matrix (0, 0), 1);
00604   EXPECT_EQ (covariance_matrix (0, 1), 0);
00605   EXPECT_EQ (covariance_matrix (0, 2), 0);
00606   EXPECT_EQ (covariance_matrix (1, 0), 0);
00607   EXPECT_EQ (covariance_matrix (1, 1), 0);
00608   EXPECT_EQ (covariance_matrix (1, 2), 0);
00609   EXPECT_EQ (covariance_matrix (2, 0), 0);
00610   EXPECT_EQ (covariance_matrix (2, 1), 0);
00611   EXPECT_EQ (covariance_matrix (2, 2), 1);
00612 
00613   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00614   cloud.push_back (point);
00615   cloud.is_dense = false;
00616   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00617   centroid [1] = 0;
00618 
00619   EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 8);
00620   EXPECT_EQ (covariance_matrix (0, 0), 1);
00621   EXPECT_EQ (covariance_matrix (0, 1), 0);
00622   EXPECT_EQ (covariance_matrix (0, 2), 0);
00623   EXPECT_EQ (covariance_matrix (1, 0), 0);
00624   EXPECT_EQ (covariance_matrix (1, 1), 1);
00625   EXPECT_EQ (covariance_matrix (1, 2), 0);
00626   EXPECT_EQ (covariance_matrix (2, 0), 0);
00627   EXPECT_EQ (covariance_matrix (2, 1), 0);
00628   EXPECT_EQ (covariance_matrix (2, 2), 1);
00629 
00630   indices.push_back (8); // add the NaN
00631   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00632   centroid [1] = 1;
00633 
00634   EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, indices, centroid, covariance_matrix), 4);
00635   EXPECT_EQ (covariance_matrix (0, 0), 1);
00636   EXPECT_EQ (covariance_matrix (0, 1), 0);
00637   EXPECT_EQ (covariance_matrix (0, 2), 0);
00638   EXPECT_EQ (covariance_matrix (1, 0), 0);
00639   EXPECT_EQ (covariance_matrix (1, 1), 0);
00640   EXPECT_EQ (covariance_matrix (1, 2), 0);
00641   EXPECT_EQ (covariance_matrix (2, 0), 0);
00642   EXPECT_EQ (covariance_matrix (2, 1), 0);
00643   EXPECT_EQ (covariance_matrix (2, 2), 1);
00644 }
00645 
00647 TEST (PCL, computeDemeanedCovariance)
00648 {
00649   PointCloud<PointXYZ> cloud;
00650   PointXYZ point;
00651   std::vector <int> indices;
00652   Eigen::Matrix3f covariance_matrix;
00653 
00654   // test empty cloud which is dense
00655   cloud.is_dense = true;
00656   EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 0);
00657 
00658   // test empty cloud non_dense
00659   cloud.is_dense = false;
00660   EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 0);
00661 
00662   // test non-empty cloud non_dense
00663   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00664   cloud.push_back (point);
00665   EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 0);
00666 
00667   // test non-empty cloud non_dense
00668   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00669   cloud.push_back (point);
00670   indices.push_back (1);
00671   EXPECT_EQ (computeCovarianceMatrix (cloud, indices, covariance_matrix), 0);
00672 
00673   cloud.clear ();
00674   indices.clear ();
00675 
00676   for (point.x = -1; point.x < 2; point.x += 2)
00677   {
00678     for (point.y = -1; point.y < 2; point.y += 2)
00679     {
00680       for (point.z = -1; point.z < 2; point.z += 2)
00681       {
00682         cloud.push_back (point);
00683       }
00684     }
00685   }
00686   cloud.is_dense = true;
00687 
00688   // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1)
00689 
00690   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00691 
00692   EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 8);
00693   EXPECT_EQ (covariance_matrix (0, 0), 1);
00694   EXPECT_EQ (covariance_matrix (0, 1), 0);
00695   EXPECT_EQ (covariance_matrix (0, 2), 0);
00696   EXPECT_EQ (covariance_matrix (1, 0), 0);
00697   EXPECT_EQ (covariance_matrix (1, 1), 1);
00698   EXPECT_EQ (covariance_matrix (1, 2), 0);
00699   EXPECT_EQ (covariance_matrix (2, 0), 0);
00700   EXPECT_EQ (covariance_matrix (2, 1), 0);
00701   EXPECT_EQ (covariance_matrix (2, 2), 1);
00702 
00703   indices.resize (4); // only positive y values
00704   indices [0] = 2;
00705   indices [1] = 3;
00706   indices [2] = 6;
00707   indices [3] = 7;
00708   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00709 
00710   EXPECT_EQ (computeCovarianceMatrix (cloud, indices, covariance_matrix), 4);
00711   EXPECT_EQ (covariance_matrix (0, 0), 1);
00712   EXPECT_EQ (covariance_matrix (0, 1), 0);
00713   EXPECT_EQ (covariance_matrix (0, 2), 0);
00714   EXPECT_EQ (covariance_matrix (1, 0), 0);
00715   EXPECT_EQ (covariance_matrix (1, 1), 1);
00716   EXPECT_EQ (covariance_matrix (1, 2), 0);
00717   EXPECT_EQ (covariance_matrix (2, 0), 0);
00718   EXPECT_EQ (covariance_matrix (2, 1), 0);
00719   EXPECT_EQ (covariance_matrix (2, 2), 1);
00720 
00721   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00722   cloud.push_back (point);
00723   cloud.is_dense = false;
00724   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00725 
00726   EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 8);
00727   EXPECT_EQ (covariance_matrix (0, 0), 1);
00728   EXPECT_EQ (covariance_matrix (0, 1), 0);
00729   EXPECT_EQ (covariance_matrix (0, 2), 0);
00730   EXPECT_EQ (covariance_matrix (1, 0), 0);
00731   EXPECT_EQ (covariance_matrix (1, 1), 1);
00732   EXPECT_EQ (covariance_matrix (1, 2), 0);
00733   EXPECT_EQ (covariance_matrix (2, 0), 0);
00734   EXPECT_EQ (covariance_matrix (2, 1), 0);
00735   EXPECT_EQ (covariance_matrix (2, 2), 1);
00736 
00737   indices.push_back (8); // add the NaN
00738   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00739 
00740   EXPECT_EQ (computeCovarianceMatrix (cloud, indices, covariance_matrix), 4);
00741   EXPECT_EQ (covariance_matrix (0, 0), 1);
00742   EXPECT_EQ (covariance_matrix (0, 1), 0);
00743   EXPECT_EQ (covariance_matrix (0, 2), 0);
00744   EXPECT_EQ (covariance_matrix (1, 0), 0);
00745   EXPECT_EQ (covariance_matrix (1, 1), 1);
00746   EXPECT_EQ (covariance_matrix (1, 2), 0);
00747   EXPECT_EQ (covariance_matrix (2, 0), 0);
00748   EXPECT_EQ (covariance_matrix (2, 1), 0);
00749   EXPECT_EQ (covariance_matrix (2, 2), 1);
00750 }
00751 
00753 TEST (PCL, computeMeanAndCovariance)
00754 {
00755   PointCloud<PointXYZ> cloud;
00756   PointXYZ point;
00757   std::vector <int> indices;
00758   Eigen::Matrix3f covariance_matrix;
00759   Eigen::Vector4f centroid;
00760 
00761   // test empty cloud which is dense
00762   cloud.is_dense = true;
00763   EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 0);
00764 
00765   // test empty cloud non_dense
00766   cloud.is_dense = false;
00767   EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 0);
00768 
00769   // test non-empty cloud non_dense
00770   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00771   cloud.push_back (point);
00772   EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 0);
00773 
00774   // test non-empty cloud non_dense
00775   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00776   cloud.push_back (point);
00777   indices.push_back (1);
00778   EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, centroid), 0);
00779 
00780   cloud.clear ();
00781   indices.clear ();
00782 
00783   for (point.x = -1; point.x < 2; point.x += 2)
00784   {
00785     for (point.y = -1; point.y < 2; point.y += 2)
00786     {
00787       for (point.z = -1; point.z < 2; point.z += 2)
00788       {
00789         cloud.push_back (point);
00790       }
00791     }
00792   }
00793   cloud.is_dense = true;
00794 
00795   // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1)
00796 
00797   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00798   centroid [0] = -100;
00799   centroid [1] = -101;
00800   centroid [2] = -102;
00801   EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 8);
00802 
00803   EXPECT_EQ (centroid [0], 0);
00804   EXPECT_EQ (centroid [1], 0);
00805   EXPECT_EQ (centroid [2], 0);
00806   EXPECT_EQ (covariance_matrix (0, 0), 1);
00807   EXPECT_EQ (covariance_matrix (0, 1), 0);
00808   EXPECT_EQ (covariance_matrix (0, 2), 0);
00809   EXPECT_EQ (covariance_matrix (1, 0), 0);
00810   EXPECT_EQ (covariance_matrix (1, 1), 1);
00811   EXPECT_EQ (covariance_matrix (1, 2), 0);
00812   EXPECT_EQ (covariance_matrix (2, 0), 0);
00813   EXPECT_EQ (covariance_matrix (2, 1), 0);
00814   EXPECT_EQ (covariance_matrix (2, 2), 1);
00815 
00816   indices.resize (4); // only positive y values
00817   indices [0] = 2;
00818   indices [1] = 3;
00819   indices [2] = 6;
00820   indices [3] = 7;
00821   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00822   centroid [0] = -100;
00823   centroid [1] = -101;
00824   centroid [2] = -102;
00825 
00826   EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, centroid), 4);
00827   EXPECT_EQ (centroid [0], 0);
00828   EXPECT_EQ (centroid [1], 1);
00829   EXPECT_EQ (centroid [2], 0);
00830   EXPECT_EQ (covariance_matrix (0, 0), 1);
00831   EXPECT_EQ (covariance_matrix (0, 1), 0);
00832   EXPECT_EQ (covariance_matrix (0, 2), 0);
00833   EXPECT_EQ (covariance_matrix (1, 0), 0);
00834   EXPECT_EQ (covariance_matrix (1, 1), 0);
00835   EXPECT_EQ (covariance_matrix (1, 2), 0);
00836   EXPECT_EQ (covariance_matrix (2, 0), 0);
00837   EXPECT_EQ (covariance_matrix (2, 1), 0);
00838   EXPECT_EQ (covariance_matrix (2, 2), 1);
00839 
00840   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00841   cloud.push_back (point);
00842   cloud.is_dense = false;
00843   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00844   centroid [0] = -100;
00845   centroid [1] = -101;
00846   centroid [2] = -102;
00847 
00848   EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 8);
00849   EXPECT_EQ (centroid [0], 0);
00850   EXPECT_EQ (centroid [1], 0);
00851   EXPECT_EQ (centroid [2], 0);
00852   EXPECT_EQ (covariance_matrix (0, 0), 1);
00853   EXPECT_EQ (covariance_matrix (0, 1), 0);
00854   EXPECT_EQ (covariance_matrix (0, 2), 0);
00855   EXPECT_EQ (covariance_matrix (1, 0), 0);
00856   EXPECT_EQ (covariance_matrix (1, 1), 1);
00857   EXPECT_EQ (covariance_matrix (1, 2), 0);
00858   EXPECT_EQ (covariance_matrix (2, 0), 0);
00859   EXPECT_EQ (covariance_matrix (2, 1), 0);
00860   EXPECT_EQ (covariance_matrix (2, 2), 1);
00861 
00862   indices.push_back (8); // add the NaN
00863   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00864   centroid [0] = -100;
00865   centroid [1] = -101;
00866   centroid [2] = -102;
00867 
00868   EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, centroid), 4);
00869   EXPECT_EQ (centroid [0], 0);
00870   EXPECT_EQ (centroid [1], 1);
00871   EXPECT_EQ (centroid [2], 0);
00872   EXPECT_EQ (covariance_matrix (0, 0), 1);
00873   EXPECT_EQ (covariance_matrix (0, 1), 0);
00874   EXPECT_EQ (covariance_matrix (0, 2), 0);
00875   EXPECT_EQ (covariance_matrix (1, 0), 0);
00876   EXPECT_EQ (covariance_matrix (1, 1), 0);
00877   EXPECT_EQ (covariance_matrix (1, 2), 0);
00878   EXPECT_EQ (covariance_matrix (2, 0), 0);
00879   EXPECT_EQ (covariance_matrix (2, 1), 0);
00880   EXPECT_EQ (covariance_matrix (2, 2), 1);
00881 }
00882 
00884 TEST (PCL, CopyIfFieldExists)
00885 {
00886   PointXYZRGBNormal p;
00887 
00888   p.x = 1.0; p.y = 2;  p.z = 3.0;
00889   p.r = 127; p.g = 64; p.b = 254;
00890   p.normal_x = 1.0; p.normal_y = 0.0; p.normal_z = 0.0;
00891 
00892   typedef pcl::traits::fieldList<PointXYZRGBNormal>::type FieldList;
00893   bool is_x = false, is_y = false, is_z = false, is_rgb = false, 
00894        is_normal_x = false, is_normal_y = false, is_normal_z = false;
00895 
00896   float x_val, y_val, z_val, normal_x_val, normal_y_val, normal_z_val, rgb_val;
00897   x_val = y_val = z_val = std::numeric_limits<float>::quiet_NaN ();
00898   normal_x_val = normal_y_val = normal_z_val = std::numeric_limits<float>::quiet_NaN ();
00899   rgb_val = std::numeric_limits<float>::quiet_NaN ();
00900 
00901   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "x", is_x, x_val));
00902   EXPECT_EQ (is_x, true);
00903   EXPECT_EQ (x_val, 1.0);
00904   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "y", is_y, y_val));
00905   EXPECT_EQ (is_y, true);
00906   EXPECT_EQ (y_val, 2.0);
00907   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "z", is_z, z_val));
00908   EXPECT_EQ (is_z, true);
00909   EXPECT_EQ (z_val, 3.0);
00910   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "rgb", is_rgb, rgb_val));
00911   EXPECT_EQ (is_rgb, true);
00912   int rgb = *reinterpret_cast<int*>(&rgb_val);
00913   EXPECT_EQ (rgb, 8339710);
00914   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "normal_x", is_normal_x, normal_x_val));
00915   EXPECT_EQ (is_normal_x, true);
00916   EXPECT_EQ (normal_x_val, 1.0);
00917   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "normal_y", is_normal_y, normal_y_val));
00918   EXPECT_EQ (is_normal_y, true);
00919   EXPECT_EQ (normal_y_val, 0.0);
00920   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "normal_z", is_normal_z, normal_z_val));
00921   EXPECT_EQ (is_normal_z, true);
00922   EXPECT_EQ (normal_z_val, 0.0);
00923   
00924   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "x", x_val));
00925   EXPECT_EQ (x_val, 1.0);
00926 
00927   float xx_val = -1.0;
00928   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "xx", xx_val));
00929   EXPECT_EQ (xx_val, -1.0);
00930   bool is_xx = true;
00931   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "xx", is_xx, xx_val));
00932   EXPECT_EQ (is_xx, false);
00933 }
00934 
00936 TEST (PCL, SetIfFieldExists)
00937 {
00938   PointXYZRGBNormal p;
00939 
00940   p.x = p.y = p.z = 0.0;
00941   p.r = p.g = p.b = 0;
00942   p.normal_x = p.normal_y = p.normal_z = 0.0;
00943 
00944   typedef pcl::traits::fieldList<PointXYZRGBNormal>::type FieldList;
00945   pcl::for_each_type<FieldList> (SetIfFieldExists<PointXYZRGBNormal, float> (p, "x", 1.0));
00946   EXPECT_EQ (p.x, 1.0);
00947   pcl::for_each_type<FieldList> (SetIfFieldExists<PointXYZRGBNormal, float> (p, "y", 2.0));
00948   EXPECT_EQ (p.y, 2.0);
00949   pcl::for_each_type<FieldList> (SetIfFieldExists<PointXYZRGBNormal, float> (p, "z", 3.0));
00950   EXPECT_EQ (p.z, 3.0);
00951   pcl::for_each_type<FieldList> (SetIfFieldExists<PointXYZRGBNormal, float> (p, "normal_x", 1.0));
00952   EXPECT_EQ (p.normal_x, 1.0);
00953   pcl::for_each_type<FieldList> (SetIfFieldExists<PointXYZRGBNormal, float> (p, "normal_y", 0.0));
00954   EXPECT_EQ (p.normal_y, 0.0);
00955   pcl::for_each_type<FieldList> (SetIfFieldExists<PointXYZRGBNormal, float> (p, "normal_z", 0.0));
00956   EXPECT_EQ (p.normal_z, 0.0);
00957 
00958 //  pcl::PointXY p1;
00959 //  pcl::for_each_type<pcl::traits::fieldList<pcl::PointXY>::type> (pcl::SetIfFieldExists<pcl::PointXY, float> (p1, "intensity", 3.0));
00960 //
00961 //  pcl::PFHSignature125 p2;
00962 //  pcl::for_each_type<pcl::traits::fieldList<pcl::PFHSignature125>::type> (pcl::SetIfFieldExists<pcl::PFHSignature125, float*> (p2, "intensity", 3.0));
00963 }
00964 
00965 //* ---[ */
00966 int
00967 main (int argc, char** argv)
00968 {
00969   testing::InitGoogleTest (&argc, argv);
00970   return (RUN_ALL_TESTS ());
00971 }
00972 /* ]--- */


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:18:13