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 the copyright holder(s) 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$
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/io.h>
00045 #include <pcl/common/eigen.h>
00046 #include <pcl/point_types.h>
00047 #include <pcl/point_cloud.h>
00048 
00049 #include <pcl/common/centroid.h>
00050 
00051 using namespace pcl;
00052 
00054 TEST (PCL, PointXYZRGB)
00055 {
00056   PointXYZRGB p;
00057 
00058   uint8_t r = 127, g = 64, b = 254;
00059   uint32_t rgb = (static_cast<uint32_t> (r) << 16 | 
00060                   static_cast<uint32_t> (g) << 8 | 
00061                   static_cast<uint32_t> (b));
00062   p.rgb = *reinterpret_cast<float*>(&rgb);
00063 
00064   rgb = *reinterpret_cast<int*>(&p.rgb);
00065   uint8_t rr = (rgb >> 16) & 0x0000ff;
00066   uint8_t gg = (rgb >> 8)  & 0x0000ff;
00067   uint8_t bb = (rgb)       & 0x0000ff;
00068 
00069   EXPECT_EQ (r, rr);
00070   EXPECT_EQ (g, gg);
00071   EXPECT_EQ (b, bb);
00072   EXPECT_EQ (rr, 127);
00073   EXPECT_EQ (gg, 64);
00074   EXPECT_EQ (bb, 254);
00075 
00076   p.r = 0; p.g = 127; p.b = 0;
00077   rgb = *reinterpret_cast<int*>(&p.rgb);
00078   rr = (rgb >> 16) & 0x0000ff;
00079   gg = (rgb >> 8)  & 0x0000ff;
00080   bb = (rgb)       & 0x0000ff;
00081 
00082   EXPECT_EQ (rr, 0);
00083   EXPECT_EQ (gg, 127);
00084   EXPECT_EQ (bb, 0);
00085 }
00086 
00088 TEST (PCL, PointXYZRGBNormal)
00089 {
00090   PointXYZRGBNormal p;
00091 
00092   uint8_t r = 127, g = 64, b = 254;
00093   uint32_t rgb = (static_cast<uint32_t> (r) << 16 | 
00094                   static_cast<uint32_t> (g) << 8 | 
00095                   static_cast<uint32_t> (b));
00096   p.rgb = *reinterpret_cast<float*>(&rgb);
00097 
00098   rgb = *reinterpret_cast<int*>(&p.rgb);
00099   uint8_t rr = (rgb >> 16) & 0x0000ff;
00100   uint8_t gg = (rgb >> 8)  & 0x0000ff;
00101   uint8_t bb = (rgb)       & 0x0000ff;
00102 
00103   EXPECT_EQ (r, rr);
00104   EXPECT_EQ (g, gg);
00105   EXPECT_EQ (b, bb);
00106   EXPECT_EQ (rr, 127);
00107   EXPECT_EQ (gg, 64);
00108   EXPECT_EQ (bb, 254);
00109 
00110   p.r = 0; p.g = 127; p.b = 0;
00111   rgb = *reinterpret_cast<int*>(&p.rgb);
00112   rr = (rgb >> 16) & 0x0000ff;
00113   gg = (rgb >> 8)  & 0x0000ff;
00114   bb = (rgb)       & 0x0000ff;
00115 
00116   EXPECT_EQ (rr, 0);
00117   EXPECT_EQ (gg, 127);
00118   EXPECT_EQ (bb, 0);
00119 }
00120 
00122 TEST(PCL, isFinite)
00123 {
00124   PointXYZ p;
00125   p.x = std::numeric_limits<float>::quiet_NaN ();
00126   EXPECT_EQ (isFinite (p), false);
00127   Normal n;
00128   n.normal_x = std::numeric_limits<float>::quiet_NaN ();
00129   EXPECT_EQ (isFinite (n), false);
00130 }
00131 
00133 TEST (PCL, Common)
00134 {
00135   PointXYZ p1, p2, p3;
00136   p1.x = 1; p1.y = p1.z = 0;
00137   p2.y = 1; p2.x = p2.z = 0;
00138   p3.z = 1; p3.x = p3.y = 0;
00139   double radius = getCircumcircleRadius (p1, p2, p3);
00140   EXPECT_NEAR (radius, 0.816497, 1e-4);
00141 
00142   Eigen::Vector4f pt (1,0,0,0), line_pt (0,0,0,0), line_dir (1,1,0,0);
00143   double point2line_disance = sqrt (sqrPointToLineDistance (pt, line_pt, line_dir));
00144   EXPECT_NEAR (point2line_disance, sqrt(2.0)/2, 1e-4);
00145 }
00146 
00148 TEST (PCL, Eigen)
00149 {
00150   Eigen::Matrix3f mat, vec;
00151   mat << 0.000536227f, -1.56178e-05f, -9.47391e-05f, -1.56178e-05f, 0.000297322f, -0.000148785f, -9.47391e-05f, -0.000148785f, 9.7827e-05f;
00152   Eigen::Vector3f val;
00153 
00154   eigen33 (mat, vec, val);
00155 
00156   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);
00157   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);
00158   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);
00159 
00160   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);
00161 
00162   Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eig (mat);
00163 
00164   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);
00165   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);
00166   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);
00167 
00168   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);
00169 
00170   Eigen::Vector3f eivals = mat.selfadjointView<Eigen::Lower>().eigenvalues ();
00171 
00172   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);
00173 
00174 }
00175 
00177 TEST (PCL, PointCloud)
00178 {
00179   PointCloud<PointXYZ> cloud;
00180   cloud.width = 640;
00181   cloud.height = 480;
00182 
00183   EXPECT_EQ (cloud.isOrganized (), true);
00184 
00185   cloud.height = 1;
00186   EXPECT_EQ (cloud.isOrganized (), false);
00187 
00188   cloud.width = 10;
00189   for (uint32_t i = 0; i < cloud.width*cloud.height; ++i)
00190   {
00191     float j = static_cast<float> (i);
00192     cloud.points.push_back (PointXYZ (3.0f * j + 0.0f, 3.0f * j + 1.0f, 3.0f * j + 2.0f));
00193   }
00194 
00195   Eigen::MatrixXf mat_xyz1 = cloud.getMatrixXfMap ();
00196   Eigen::MatrixXf mat_xyz = cloud.getMatrixXfMap (3, 4, 0);
00197 
00198   if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit)
00199   {
00200     EXPECT_EQ (mat_xyz1.cols (), 4);
00201     EXPECT_EQ (mat_xyz1.rows (), cloud.width);
00202     EXPECT_EQ (mat_xyz1 (0, 0), 0);
00203     EXPECT_EQ (mat_xyz1 (cloud.width - 1, 2), 3 * cloud.width - 1);   // = 29
00204 
00205     EXPECT_EQ (mat_xyz.cols (), 3);
00206     EXPECT_EQ (mat_xyz.rows (), cloud.width);
00207     EXPECT_EQ (mat_xyz (0, 0), 0);
00208     EXPECT_EQ (mat_xyz (cloud.width - 1, 2), 3 * cloud.width - 1);    // = 29
00209   }
00210   else
00211   {
00212     EXPECT_EQ (mat_xyz1.cols (), cloud.width);
00213     EXPECT_EQ (mat_xyz1.rows (), 4);
00214     EXPECT_EQ (mat_xyz1 (0, 0), 0);
00215     EXPECT_EQ (mat_xyz1 (2, cloud.width - 1), 3 * cloud.width - 1);   // = 29
00216 
00217     EXPECT_EQ (mat_xyz.cols (), cloud.width);
00218     EXPECT_EQ (mat_xyz.rows (), 3);
00219     EXPECT_EQ (mat_xyz (0, 0), 0);
00220     EXPECT_EQ (mat_xyz (2, cloud.width - 1), 3 * cloud.width - 1);    // = 29
00221   }
00222   
00223 #ifdef NDEBUG
00224   if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit)
00225   {
00226     Eigen::MatrixXf mat_yz = cloud.getMatrixXfMap (2, 4, 1);
00227     EXPECT_EQ (mat_yz.cols (), 2);
00228     EXPECT_EQ (mat_yz.rows (), cloud.width);
00229     EXPECT_EQ (mat_yz (0, 0), 1);
00230     EXPECT_EQ (mat_yz (cloud.width - 1, 1), 3 * cloud.width - 1);
00231     uint32_t j = 1;
00232     for (uint32_t i = 1; i < cloud.width*cloud.height; i+=4, j+=3)
00233     {
00234       Eigen::MatrixXf mat_yz = cloud.getMatrixXfMap (2, 4, i);
00235       EXPECT_EQ (mat_yz.cols (), 2);
00236       EXPECT_EQ (mat_yz.rows (), cloud.width);
00237       EXPECT_EQ (mat_yz (0, 0), j);
00238     }
00239   }
00240   else
00241   {
00242     Eigen::MatrixXf mat_yz = cloud.getMatrixXfMap (2, 4, 1);
00243     EXPECT_EQ (mat_yz.cols (), cloud.width);
00244     EXPECT_EQ (mat_yz.rows (), 2);
00245     EXPECT_EQ (mat_yz (0, 0), 1);
00246     EXPECT_EQ (mat_yz (1, cloud.width - 1), 3 * cloud.width - 1);
00247     uint32_t j = 1;
00248     for (uint32_t i = 1; i < cloud.width*cloud.height; i+=4, j+=3)
00249     {
00250       Eigen::MatrixXf mat_yz = cloud.getMatrixXfMap (2, 4, i);
00251       EXPECT_EQ (mat_yz.cols (), cloud.width);
00252       EXPECT_EQ (mat_yz.rows (), 2);
00253       EXPECT_EQ (mat_yz (0, 0), j);
00254     }
00255   }
00256 #endif
00257   cloud.clear ();
00258   EXPECT_EQ (cloud.width, 0);
00259   EXPECT_EQ (cloud.height, 0);
00260 
00261   cloud.width = 640;
00262   cloud.height = 480;
00263 
00264   cloud.insert (cloud.end (), PointXYZ (1, 1, 1));
00265   EXPECT_EQ (cloud.isOrganized (), false);
00266   EXPECT_EQ (cloud.width, 1);
00267 
00268   cloud.insert (cloud.end (), 5, PointXYZ (1, 1, 1));
00269   EXPECT_EQ (cloud.isOrganized (), false);
00270   EXPECT_EQ (cloud.width, 6);
00271 
00272   cloud.erase (cloud.end () - 1);
00273   EXPECT_EQ (cloud.isOrganized (), false);
00274   EXPECT_EQ (cloud.width, 5);
00275 
00276   cloud.erase (cloud.begin (), cloud.end ());
00277   EXPECT_EQ (cloud.isOrganized (), false);
00278   EXPECT_EQ (cloud.width, 0);
00279 }
00280 
00282 TEST (PCL, PointTypes)
00283 {
00284   EXPECT_EQ (sizeof (PointXYZ), 16);
00285   EXPECT_EQ (__alignof (PointXYZ), 16);
00286   EXPECT_EQ (sizeof (PointXYZI), 32);
00287   EXPECT_EQ (__alignof (PointXYZI), 16);
00288   EXPECT_EQ (sizeof (PointXYZRGB), 32);
00289   EXPECT_EQ (__alignof (PointXYZRGB), 16);
00290   EXPECT_EQ (sizeof (PointXYZRGBA), 32);
00291   EXPECT_EQ (__alignof (PointXYZRGBA), 16);
00292   EXPECT_EQ (sizeof (Normal), 32);
00293   EXPECT_EQ (__alignof (Normal), 16);
00294   EXPECT_EQ (sizeof (PointNormal), 48);
00295   EXPECT_EQ (__alignof (PointNormal), 16);
00296   EXPECT_EQ (sizeof (PointXYZRGBNormal), 48);
00297   EXPECT_EQ (__alignof (PointXYZRGBNormal), 16);
00298 }
00299 
00301 TEST (PCL, Intersections)
00302 {
00303   Eigen::VectorXf zline (6), yline (6);
00304   zline[0] = 0.543892f; zline[1] = -0.515623f; zline[2] = 1.321f;   zline[3] = 0.0266191f; zline[4] = 0.600215f;  zline[5] = -0.0387667f;
00305   yline[0] = 0.493479f; yline[1] = 0.169246f;  yline[2] = 1.22677f; yline[3] = 0.5992f;    yline[4] = 0.0505085f; yline[5] = 0.405749f;
00306 
00307   Eigen::Vector4f pt;
00308   EXPECT_EQ ((pcl::lineWithLineIntersection (zline, yline, pt)), true);
00309   EXPECT_NEAR (pt[0], 0.574544, 1e-3);
00310   EXPECT_NEAR (pt[1], 0.175526, 1e-3);
00311   EXPECT_NEAR (pt[2], 1.27636,  1e-3);
00312   EXPECT_EQ (pt[3], 0);
00313 
00314   zline << 0.545203f, -0.514419f, 1.31967f, 0.0243372f, 0.597946f, -0.0413579f;
00315   yline << 0.492706f,  0.164196f, 1.23192f, 0.598704f,  0.0442014f, 0.411328f;
00316   EXPECT_EQ ((pcl::lineWithLineIntersection (zline, yline, pt)), false);
00317   //intersection: [ 3.06416e+08    15.2237     3.06416e+08       4.04468e-34 ]
00318 }
00319 
00321 TEST (PCL, compute3DCentroidFloat)
00322 {
00323   pcl::PointIndices pindices;
00324   std::vector<int> indices;
00325   PointXYZ point;
00326   PointCloud<PointXYZ> cloud;
00327   Eigen::Vector4f centroid;
00328 
00329   // test empty cloud which is dense
00330   cloud.is_dense = true;
00331   EXPECT_EQ (compute3DCentroid (cloud, centroid), 0);
00332 
00333   // test empty cloud non_dense
00334   cloud.is_dense = false;
00335   EXPECT_EQ (compute3DCentroid (cloud, centroid), 0);
00336 
00337   // test non-empty cloud non_dense
00338   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00339   cloud.push_back (point);
00340   EXPECT_EQ (compute3DCentroid (cloud, centroid), 0);
00341 
00342   // test non-empty cloud non_dense
00343   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00344   cloud.push_back (point);
00345   indices.push_back (1);
00346   EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 0);
00347 
00348   cloud.clear ();
00349   indices.clear ();
00350   for (point.x = -1; point.x < 2; point.x += 2)
00351   {
00352     for (point.y = -1; point.y < 2; point.y += 2)
00353     {
00354       for (point.z = -1; point.z < 2; point.z += 2)
00355       {
00356         cloud.push_back (point);
00357       }
00358     }
00359   }
00360   cloud.is_dense = true;
00361 
00362   // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1)
00363   centroid [0] = -100;
00364   centroid [1] = -200;
00365   centroid [2] = -300;
00366 
00367   EXPECT_EQ (compute3DCentroid (cloud, centroid), 8);
00368   EXPECT_EQ (centroid [0], 0);
00369   EXPECT_EQ (centroid [1], 0);
00370   EXPECT_EQ (centroid [2], 0);
00371 
00372   centroid [0] = -100;
00373   centroid [1] = -200;
00374   centroid [2] = -300;
00375   indices.resize (4); // only positive y values
00376   indices [0] = 2;
00377   indices [1] = 3;
00378   indices [2] = 6;
00379   indices [3] = 7;
00380   EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4);
00381 
00382   EXPECT_EQ (centroid [0], 0.0);
00383   EXPECT_EQ (centroid [1], 1.0);
00384   EXPECT_EQ (centroid [2], 0.0);
00385 
00386   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00387   cloud.push_back (point);
00388   cloud.is_dense = false;
00389 
00390   centroid [0] = -100;
00391   centroid [1] = -200;
00392   centroid [2] = -300;
00393   EXPECT_EQ (compute3DCentroid (cloud, centroid), 8);
00394 
00395   EXPECT_EQ (centroid [0], 0);
00396   EXPECT_EQ (centroid [1], 0);
00397   EXPECT_EQ (centroid [2], 0);
00398 
00399   centroid [0] = -100;
00400   centroid [1] = -200;
00401   centroid [2] = -300;
00402   indices [0] = 2;
00403   indices [1] = 3;
00404   indices [2] = 6;
00405   indices [3] = 7;
00406   indices.push_back (8); // add the NaN
00407   EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4);
00408 
00409   EXPECT_EQ (centroid [0], 0.0);
00410   EXPECT_EQ (centroid [1], 1.0);
00411   EXPECT_EQ (centroid [2], 0.0);
00412 
00413   pindices.indices = indices;
00414   EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4);
00415 
00416   EXPECT_EQ (centroid [0], 0.0);
00417   EXPECT_EQ (centroid [1], 1.0);
00418   EXPECT_EQ (centroid [2], 0.0);
00419 }
00420 
00422 TEST (PCL, compute3DCentroidDouble)
00423 {
00424   pcl::PointIndices pindices;
00425   std::vector<int> indices;
00426   PointXYZ point;
00427   PointCloud<PointXYZ> cloud;
00428   Eigen::Vector4d centroid;
00429 
00430   // test empty cloud which is dense
00431   cloud.is_dense = true;
00432   EXPECT_EQ (compute3DCentroid (cloud, centroid), 0);
00433 
00434   // test empty cloud non_dense
00435   cloud.is_dense = false;
00436   EXPECT_EQ (compute3DCentroid (cloud, centroid), 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   EXPECT_EQ (compute3DCentroid (cloud, centroid), 0);
00442 
00443   // test non-empty cloud non_dense
00444   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00445   cloud.push_back (point);
00446   indices.push_back (1);
00447   EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 0);
00448 
00449   cloud.clear ();
00450   indices.clear ();
00451   for (point.x = -1; point.x < 2; point.x += 2)
00452   {
00453     for (point.y = -1; point.y < 2; point.y += 2)
00454     {
00455       for (point.z = -1; point.z < 2; point.z += 2)
00456       {
00457         cloud.push_back (point);
00458       }
00459     }
00460   }
00461   cloud.is_dense = true;
00462 
00463   // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1)
00464   centroid [0] = -100;
00465   centroid [1] = -200;
00466   centroid [2] = -300;
00467 
00468   EXPECT_EQ (compute3DCentroid (cloud, centroid), 8);
00469   EXPECT_EQ (centroid [0], 0);
00470   EXPECT_EQ (centroid [1], 0);
00471   EXPECT_EQ (centroid [2], 0);
00472 
00473   centroid [0] = -100;
00474   centroid [1] = -200;
00475   centroid [2] = -300;
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   EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4);
00482 
00483   EXPECT_EQ (centroid [0], 0.0);
00484   EXPECT_EQ (centroid [1], 1.0);
00485   EXPECT_EQ (centroid [2], 0.0);
00486 
00487   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00488   cloud.push_back (point);
00489   cloud.is_dense = false;
00490 
00491   centroid [0] = -100;
00492   centroid [1] = -200;
00493   centroid [2] = -300;
00494   EXPECT_EQ (compute3DCentroid (cloud, centroid), 8);
00495 
00496   EXPECT_EQ (centroid [0], 0);
00497   EXPECT_EQ (centroid [1], 0);
00498   EXPECT_EQ (centroid [2], 0);
00499 
00500   centroid [0] = -100;
00501   centroid [1] = -200;
00502   centroid [2] = -300;
00503   indices [0] = 2;
00504   indices [1] = 3;
00505   indices [2] = 6;
00506   indices [3] = 7;
00507   indices.push_back (8); // add the NaN
00508   EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4);
00509 
00510   EXPECT_EQ (centroid [0], 0.0);
00511   EXPECT_EQ (centroid [1], 1.0);
00512   EXPECT_EQ (centroid [2], 0.0);
00513   
00514   pindices.indices = indices;
00515   EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4);
00516 
00517   EXPECT_EQ (centroid [0], 0.0);
00518   EXPECT_EQ (centroid [1], 1.0);
00519   EXPECT_EQ (centroid [2], 0.0);
00520 }
00521 
00523 TEST (PCL, compute3DCentroidCloudIterator)
00524 {
00525   pcl::PointIndices pindices;
00526   std::vector<int> indices;
00527   PointXYZ point;
00528   PointCloud<PointXYZ> cloud;
00529   Eigen::Vector4f centroid_f;
00530 
00531   for (point.x = -1; point.x < 2; point.x += 2)
00532   {
00533     for (point.y = -1; point.y < 2; point.y += 2)
00534     {
00535       for (point.z = -1; point.z < 2; point.z += 2)
00536       {
00537         cloud.push_back (point);
00538       }
00539     }
00540   }
00541   cloud.is_dense = true;
00542 
00543   indices.resize (4); // only positive y values
00544   indices [0] = 2;
00545   indices [1] = 3;
00546   indices [2] = 6;
00547   indices [3] = 7;
00548 
00549   ConstCloudIterator<PointXYZ> it (cloud, indices);
00550   
00551   EXPECT_EQ (compute3DCentroid (it, centroid_f), 4);
00552 
00553   EXPECT_EQ (centroid_f[0], 0.0f);
00554   EXPECT_EQ (centroid_f[1], 1.0f);
00555   EXPECT_EQ (centroid_f[2], 0.0f);
00556   
00557   Eigen::Vector4d centroid_d;
00558   it.reset ();
00559   EXPECT_EQ (compute3DCentroid (it, centroid_d), 4);
00560 
00561   EXPECT_EQ (centroid_d[0], 0.0);
00562   EXPECT_EQ (centroid_d[1], 1.0);
00563   EXPECT_EQ (centroid_d[2], 0.0);
00564 }
00565 
00566 
00568 TEST (PCL, computeCovarianceMatrix)
00569 {
00570   PointCloud<PointXYZ> cloud;
00571   PointXYZ point;
00572   std::vector <int> indices;
00573   Eigen::Vector4f centroid;
00574   Eigen::Matrix3f covariance_matrix;
00575 
00576   centroid [0] = 0;
00577   centroid [1] = 0;
00578   centroid [2] = 0;
00579 
00580   // test empty cloud which is dense
00581   cloud.is_dense = true;
00582   EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 0);
00583 
00584   // test empty cloud non_dense
00585   cloud.is_dense = false;
00586   EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 0);
00587 
00588   // test non-empty cloud non_dense
00589   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00590   cloud.push_back (point);
00591   EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 0);
00592 
00593   // test non-empty cloud non_dense
00594   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00595   cloud.push_back (point);
00596   indices.push_back (1);
00597   EXPECT_EQ (computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix), 0);
00598 
00599   cloud.clear ();
00600   indices.clear ();
00601   for (point.x = -1; point.x < 2; point.x += 2)
00602   {
00603     for (point.y = -1; point.y < 2; point.y += 2)
00604     {
00605       for (point.z = -1; point.z < 2; point.z += 2)
00606       {
00607         cloud.push_back (point);
00608       }
00609     }
00610   }
00611   cloud.is_dense = true;
00612 
00613   // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1)
00614 
00615   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00616   centroid [0] = 0;
00617   centroid [1] = 0;
00618   centroid [2] = 0;
00619 
00620   EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 8);
00621   EXPECT_EQ (covariance_matrix (0, 0), 8);
00622   EXPECT_EQ (covariance_matrix (0, 1), 0);
00623   EXPECT_EQ (covariance_matrix (0, 2), 0);
00624   EXPECT_EQ (covariance_matrix (1, 0), 0);
00625   EXPECT_EQ (covariance_matrix (1, 1), 8);
00626   EXPECT_EQ (covariance_matrix (1, 2), 0);
00627   EXPECT_EQ (covariance_matrix (2, 0), 0);
00628   EXPECT_EQ (covariance_matrix (2, 1), 0);
00629   EXPECT_EQ (covariance_matrix (2, 2), 8);
00630 
00631   indices.resize (4); // only positive y values
00632   indices [0] = 2;
00633   indices [1] = 3;
00634   indices [2] = 6;
00635   indices [3] = 7;
00636   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00637   centroid [1] = 1;
00638 
00639   EXPECT_EQ (computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix), 4);
00640   EXPECT_EQ (covariance_matrix (0, 0), 4);
00641   EXPECT_EQ (covariance_matrix (0, 1), 0);
00642   EXPECT_EQ (covariance_matrix (0, 2), 0);
00643   EXPECT_EQ (covariance_matrix (1, 0), 0);
00644   EXPECT_EQ (covariance_matrix (1, 1), 0);
00645   EXPECT_EQ (covariance_matrix (1, 2), 0);
00646   EXPECT_EQ (covariance_matrix (2, 0), 0);
00647   EXPECT_EQ (covariance_matrix (2, 1), 0);
00648   EXPECT_EQ (covariance_matrix (2, 2), 4);
00649 
00650   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00651   cloud.push_back (point);
00652   cloud.is_dense = false;
00653   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00654   centroid [1] = 0;
00655 
00656   EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 8);
00657   EXPECT_EQ (covariance_matrix (0, 0), 8);
00658   EXPECT_EQ (covariance_matrix (0, 1), 0);
00659   EXPECT_EQ (covariance_matrix (0, 2), 0);
00660   EXPECT_EQ (covariance_matrix (1, 0), 0);
00661   EXPECT_EQ (covariance_matrix (1, 1), 8);
00662   EXPECT_EQ (covariance_matrix (1, 2), 0);
00663   EXPECT_EQ (covariance_matrix (2, 0), 0);
00664   EXPECT_EQ (covariance_matrix (2, 1), 0);
00665   EXPECT_EQ (covariance_matrix (2, 2), 8);
00666 
00667   indices.push_back (8); // add the NaN
00668   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00669   centroid [1] = 1;
00670 
00671   EXPECT_EQ (computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix), 4);
00672   EXPECT_EQ (covariance_matrix (0, 0), 4);
00673   EXPECT_EQ (covariance_matrix (0, 1), 0);
00674   EXPECT_EQ (covariance_matrix (0, 2), 0);
00675   EXPECT_EQ (covariance_matrix (1, 0), 0);
00676   EXPECT_EQ (covariance_matrix (1, 1), 0);
00677   EXPECT_EQ (covariance_matrix (1, 2), 0);
00678   EXPECT_EQ (covariance_matrix (2, 0), 0);
00679   EXPECT_EQ (covariance_matrix (2, 1), 0);
00680   EXPECT_EQ (covariance_matrix (2, 2), 4);
00681 }
00682 
00684 TEST (PCL, computeCovarianceMatrixNormalized)
00685 {
00686   PointCloud<PointXYZ> cloud;
00687   PointXYZ point;
00688   std::vector <int> indices;
00689   Eigen::Vector4f centroid;
00690   Eigen::Matrix3f covariance_matrix;
00691 
00692   centroid [0] = 0;
00693   centroid [1] = 0;
00694   centroid [2] = 0;
00695 
00696   // test empty cloud which is dense
00697   cloud.is_dense = true;
00698   EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 0);
00699 
00700   // test empty cloud non_dense
00701   cloud.is_dense = false;
00702   EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 0);
00703 
00704   // test non-empty cloud non_dense
00705   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00706   cloud.push_back (point);
00707   EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 0);
00708 
00709   // test non-empty cloud non_dense
00710   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00711   cloud.push_back (point);
00712   indices.push_back (1);
00713   EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, indices, centroid, covariance_matrix), 0);
00714 
00715   cloud.clear ();
00716   indices.clear ();
00717   for (point.x = -1; point.x < 2; point.x += 2)
00718   {
00719     for (point.y = -1; point.y < 2; point.y += 2)
00720     {
00721       for (point.z = -1; point.z < 2; point.z += 2)
00722       {
00723         cloud.push_back (point);
00724       }
00725     }
00726   }
00727   cloud.is_dense = true;
00728 
00729   // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1)
00730 
00731   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00732   centroid [0] = 0;
00733   centroid [1] = 0;
00734   centroid [2] = 0;
00735 
00736   EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 8);
00737 
00738   EXPECT_EQ (covariance_matrix (0, 0), 1);
00739   EXPECT_EQ (covariance_matrix (0, 1), 0);
00740   EXPECT_EQ (covariance_matrix (0, 2), 0);
00741   EXPECT_EQ (covariance_matrix (1, 0), 0);
00742   EXPECT_EQ (covariance_matrix (1, 1), 1);
00743   EXPECT_EQ (covariance_matrix (1, 2), 0);
00744   EXPECT_EQ (covariance_matrix (2, 0), 0);
00745   EXPECT_EQ (covariance_matrix (2, 1), 0);
00746   EXPECT_EQ (covariance_matrix (2, 2), 1);
00747 
00748   indices.resize (4); // only positive y values
00749   indices [0] = 2;
00750   indices [1] = 3;
00751   indices [2] = 6;
00752   indices [3] = 7;
00753   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00754   centroid [1] = 1;
00755 
00756   EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, indices, centroid, covariance_matrix), 4);
00757 
00758   EXPECT_EQ (covariance_matrix (0, 0), 1);
00759   EXPECT_EQ (covariance_matrix (0, 1), 0);
00760   EXPECT_EQ (covariance_matrix (0, 2), 0);
00761   EXPECT_EQ (covariance_matrix (1, 0), 0);
00762   EXPECT_EQ (covariance_matrix (1, 1), 0);
00763   EXPECT_EQ (covariance_matrix (1, 2), 0);
00764   EXPECT_EQ (covariance_matrix (2, 0), 0);
00765   EXPECT_EQ (covariance_matrix (2, 1), 0);
00766   EXPECT_EQ (covariance_matrix (2, 2), 1);
00767 
00768   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00769   cloud.push_back (point);
00770   cloud.is_dense = false;
00771   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00772   centroid [1] = 0;
00773 
00774   EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 8);
00775   EXPECT_EQ (covariance_matrix (0, 0), 1);
00776   EXPECT_EQ (covariance_matrix (0, 1), 0);
00777   EXPECT_EQ (covariance_matrix (0, 2), 0);
00778   EXPECT_EQ (covariance_matrix (1, 0), 0);
00779   EXPECT_EQ (covariance_matrix (1, 1), 1);
00780   EXPECT_EQ (covariance_matrix (1, 2), 0);
00781   EXPECT_EQ (covariance_matrix (2, 0), 0);
00782   EXPECT_EQ (covariance_matrix (2, 1), 0);
00783   EXPECT_EQ (covariance_matrix (2, 2), 1);
00784 
00785   indices.push_back (8); // add the NaN
00786   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00787   centroid [1] = 1;
00788 
00789   EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, indices, centroid, covariance_matrix), 4);
00790   EXPECT_EQ (covariance_matrix (0, 0), 1);
00791   EXPECT_EQ (covariance_matrix (0, 1), 0);
00792   EXPECT_EQ (covariance_matrix (0, 2), 0);
00793   EXPECT_EQ (covariance_matrix (1, 0), 0);
00794   EXPECT_EQ (covariance_matrix (1, 1), 0);
00795   EXPECT_EQ (covariance_matrix (1, 2), 0);
00796   EXPECT_EQ (covariance_matrix (2, 0), 0);
00797   EXPECT_EQ (covariance_matrix (2, 1), 0);
00798   EXPECT_EQ (covariance_matrix (2, 2), 1);
00799 }
00800 
00802 TEST (PCL, computeDemeanedCovariance)
00803 {
00804   PointCloud<PointXYZ> cloud;
00805   PointXYZ point;
00806   std::vector <int> indices;
00807   Eigen::Matrix3f covariance_matrix;
00808 
00809   // test empty cloud which is dense
00810   cloud.is_dense = true;
00811   EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 0);
00812 
00813   // test empty cloud non_dense
00814   cloud.is_dense = false;
00815   EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 0);
00816 
00817   // test non-empty cloud non_dense
00818   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00819   cloud.push_back (point);
00820   EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 0);
00821 
00822   // test non-empty cloud non_dense
00823   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00824   cloud.push_back (point);
00825   indices.push_back (1);
00826   EXPECT_EQ (computeCovarianceMatrix (cloud, indices, covariance_matrix), 0);
00827 
00828   cloud.clear ();
00829   indices.clear ();
00830 
00831   for (point.x = -1; point.x < 2; point.x += 2)
00832   {
00833     for (point.y = -1; point.y < 2; point.y += 2)
00834     {
00835       for (point.z = -1; point.z < 2; point.z += 2)
00836       {
00837         cloud.push_back (point);
00838       }
00839     }
00840   }
00841   cloud.is_dense = true;
00842 
00843   // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1)
00844 
00845   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00846 
00847   EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 8);
00848   EXPECT_EQ (covariance_matrix (0, 0), 1);
00849   EXPECT_EQ (covariance_matrix (0, 1), 0);
00850   EXPECT_EQ (covariance_matrix (0, 2), 0);
00851   EXPECT_EQ (covariance_matrix (1, 0), 0);
00852   EXPECT_EQ (covariance_matrix (1, 1), 1);
00853   EXPECT_EQ (covariance_matrix (1, 2), 0);
00854   EXPECT_EQ (covariance_matrix (2, 0), 0);
00855   EXPECT_EQ (covariance_matrix (2, 1), 0);
00856   EXPECT_EQ (covariance_matrix (2, 2), 1);
00857 
00858   indices.resize (4); // only positive y values
00859   indices [0] = 2;
00860   indices [1] = 3;
00861   indices [2] = 6;
00862   indices [3] = 7;
00863   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00864 
00865   EXPECT_EQ (computeCovarianceMatrix (cloud, indices, covariance_matrix), 4);
00866   EXPECT_EQ (covariance_matrix (0, 0), 1);
00867   EXPECT_EQ (covariance_matrix (0, 1), 0);
00868   EXPECT_EQ (covariance_matrix (0, 2), 0);
00869   EXPECT_EQ (covariance_matrix (1, 0), 0);
00870   EXPECT_EQ (covariance_matrix (1, 1), 1);
00871   EXPECT_EQ (covariance_matrix (1, 2), 0);
00872   EXPECT_EQ (covariance_matrix (2, 0), 0);
00873   EXPECT_EQ (covariance_matrix (2, 1), 0);
00874   EXPECT_EQ (covariance_matrix (2, 2), 1);
00875 
00876   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00877   cloud.push_back (point);
00878   cloud.is_dense = false;
00879   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00880 
00881   EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 8);
00882   EXPECT_EQ (covariance_matrix (0, 0), 1);
00883   EXPECT_EQ (covariance_matrix (0, 1), 0);
00884   EXPECT_EQ (covariance_matrix (0, 2), 0);
00885   EXPECT_EQ (covariance_matrix (1, 0), 0);
00886   EXPECT_EQ (covariance_matrix (1, 1), 1);
00887   EXPECT_EQ (covariance_matrix (1, 2), 0);
00888   EXPECT_EQ (covariance_matrix (2, 0), 0);
00889   EXPECT_EQ (covariance_matrix (2, 1), 0);
00890   EXPECT_EQ (covariance_matrix (2, 2), 1);
00891 
00892   indices.push_back (8); // add the NaN
00893   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00894 
00895   EXPECT_EQ (computeCovarianceMatrix (cloud, indices, covariance_matrix), 4);
00896   EXPECT_EQ (covariance_matrix (0, 0), 1);
00897   EXPECT_EQ (covariance_matrix (0, 1), 0);
00898   EXPECT_EQ (covariance_matrix (0, 2), 0);
00899   EXPECT_EQ (covariance_matrix (1, 0), 0);
00900   EXPECT_EQ (covariance_matrix (1, 1), 1);
00901   EXPECT_EQ (covariance_matrix (1, 2), 0);
00902   EXPECT_EQ (covariance_matrix (2, 0), 0);
00903   EXPECT_EQ (covariance_matrix (2, 1), 0);
00904   EXPECT_EQ (covariance_matrix (2, 2), 1);
00905 }
00906 
00908 TEST (PCL, computeMeanAndCovariance)
00909 {
00910   PointCloud<PointXYZ> cloud;
00911   PointXYZ point;
00912   std::vector <int> indices;
00913   Eigen::Matrix3f covariance_matrix;
00914   Eigen::Vector4f centroid;
00915 
00916   // test empty cloud which is dense
00917   cloud.is_dense = true;
00918   EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 0);
00919 
00920   // test empty cloud non_dense
00921   cloud.is_dense = false;
00922   EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 0);
00923 
00924   // test non-empty cloud non_dense
00925   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00926   cloud.push_back (point);
00927   EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 0);
00928 
00929   // test non-empty cloud non_dense
00930   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00931   cloud.push_back (point);
00932   indices.push_back (1);
00933   EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, centroid), 0);
00934 
00935   cloud.clear ();
00936   indices.clear ();
00937 
00938   for (point.x = -1; point.x < 2; point.x += 2)
00939   {
00940     for (point.y = -1; point.y < 2; point.y += 2)
00941     {
00942       for (point.z = -1; point.z < 2; point.z += 2)
00943       {
00944         cloud.push_back (point);
00945       }
00946     }
00947   }
00948   cloud.is_dense = true;
00949 
00950   // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1)
00951 
00952   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00953   centroid [0] = -100;
00954   centroid [1] = -101;
00955   centroid [2] = -102;
00956   EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 8);
00957 
00958   EXPECT_EQ (centroid [0], 0);
00959   EXPECT_EQ (centroid [1], 0);
00960   EXPECT_EQ (centroid [2], 0);
00961   EXPECT_EQ (covariance_matrix (0, 0), 1);
00962   EXPECT_EQ (covariance_matrix (0, 1), 0);
00963   EXPECT_EQ (covariance_matrix (0, 2), 0);
00964   EXPECT_EQ (covariance_matrix (1, 0), 0);
00965   EXPECT_EQ (covariance_matrix (1, 1), 1);
00966   EXPECT_EQ (covariance_matrix (1, 2), 0);
00967   EXPECT_EQ (covariance_matrix (2, 0), 0);
00968   EXPECT_EQ (covariance_matrix (2, 1), 0);
00969   EXPECT_EQ (covariance_matrix (2, 2), 1);
00970 
00971   indices.resize (4); // only positive y values
00972   indices [0] = 2;
00973   indices [1] = 3;
00974   indices [2] = 6;
00975   indices [3] = 7;
00976   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00977   centroid [0] = -100;
00978   centroid [1] = -101;
00979   centroid [2] = -102;
00980 
00981   EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, centroid), 4);
00982   EXPECT_EQ (centroid [0], 0);
00983   EXPECT_EQ (centroid [1], 1);
00984   EXPECT_EQ (centroid [2], 0);
00985   EXPECT_EQ (covariance_matrix (0, 0), 1);
00986   EXPECT_EQ (covariance_matrix (0, 1), 0);
00987   EXPECT_EQ (covariance_matrix (0, 2), 0);
00988   EXPECT_EQ (covariance_matrix (1, 0), 0);
00989   EXPECT_EQ (covariance_matrix (1, 1), 0);
00990   EXPECT_EQ (covariance_matrix (1, 2), 0);
00991   EXPECT_EQ (covariance_matrix (2, 0), 0);
00992   EXPECT_EQ (covariance_matrix (2, 1), 0);
00993   EXPECT_EQ (covariance_matrix (2, 2), 1);
00994 
00995   point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00996   cloud.push_back (point);
00997   cloud.is_dense = false;
00998   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
00999   centroid [0] = -100;
01000   centroid [1] = -101;
01001   centroid [2] = -102;
01002 
01003   EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 8);
01004   EXPECT_EQ (centroid [0], 0);
01005   EXPECT_EQ (centroid [1], 0);
01006   EXPECT_EQ (centroid [2], 0);
01007   EXPECT_EQ (covariance_matrix (0, 0), 1);
01008   EXPECT_EQ (covariance_matrix (0, 1), 0);
01009   EXPECT_EQ (covariance_matrix (0, 2), 0);
01010   EXPECT_EQ (covariance_matrix (1, 0), 0);
01011   EXPECT_EQ (covariance_matrix (1, 1), 1);
01012   EXPECT_EQ (covariance_matrix (1, 2), 0);
01013   EXPECT_EQ (covariance_matrix (2, 0), 0);
01014   EXPECT_EQ (covariance_matrix (2, 1), 0);
01015   EXPECT_EQ (covariance_matrix (2, 2), 1);
01016 
01017   indices.push_back (8); // add the NaN
01018   covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122;
01019   centroid [0] = -100;
01020   centroid [1] = -101;
01021   centroid [2] = -102;
01022 
01023   EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, centroid), 4);
01024   EXPECT_EQ (centroid [0], 0);
01025   EXPECT_EQ (centroid [1], 1);
01026   EXPECT_EQ (centroid [2], 0);
01027   EXPECT_EQ (covariance_matrix (0, 0), 1);
01028   EXPECT_EQ (covariance_matrix (0, 1), 0);
01029   EXPECT_EQ (covariance_matrix (0, 2), 0);
01030   EXPECT_EQ (covariance_matrix (1, 0), 0);
01031   EXPECT_EQ (covariance_matrix (1, 1), 0);
01032   EXPECT_EQ (covariance_matrix (1, 2), 0);
01033   EXPECT_EQ (covariance_matrix (2, 0), 0);
01034   EXPECT_EQ (covariance_matrix (2, 1), 0);
01035   EXPECT_EQ (covariance_matrix (2, 2), 1);
01036 }
01037 
01039 TEST (PCL, CopyIfFieldExists)
01040 {
01041   PointXYZRGBNormal p;
01042 
01043   p.x = 1.0; p.y = 2;  p.z = 3.0;
01044   p.r = 127; p.g = 64; p.b = 254;
01045   p.normal_x = 1.0; p.normal_y = 0.0; p.normal_z = 0.0;
01046 
01047   typedef pcl::traits::fieldList<PointXYZRGBNormal>::type FieldList;
01048   bool is_x = false, is_y = false, is_z = false, is_rgb = false, 
01049        is_normal_x = false, is_normal_y = false, is_normal_z = false;
01050 
01051   float x_val, y_val, z_val, normal_x_val, normal_y_val, normal_z_val, rgb_val;
01052   x_val = y_val = z_val = std::numeric_limits<float>::quiet_NaN ();
01053   normal_x_val = normal_y_val = normal_z_val = std::numeric_limits<float>::quiet_NaN ();
01054   rgb_val = std::numeric_limits<float>::quiet_NaN ();
01055 
01056   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "x", is_x, x_val));
01057   EXPECT_EQ (is_x, true);
01058   EXPECT_EQ (x_val, 1.0);
01059   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "y", is_y, y_val));
01060   EXPECT_EQ (is_y, true);
01061   EXPECT_EQ (y_val, 2.0);
01062   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "z", is_z, z_val));
01063   EXPECT_EQ (is_z, true);
01064   EXPECT_EQ (z_val, 3.0);
01065   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "rgb", is_rgb, rgb_val));
01066   EXPECT_EQ (is_rgb, true);
01067   int rgb = *reinterpret_cast<int*>(&rgb_val);
01068   EXPECT_EQ (rgb, 8339710);      // alpha is 0
01069   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "normal_x", is_normal_x, normal_x_val));
01070   EXPECT_EQ (is_normal_x, true);
01071   EXPECT_EQ (normal_x_val, 1.0);
01072   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "normal_y", is_normal_y, normal_y_val));
01073   EXPECT_EQ (is_normal_y, true);
01074   EXPECT_EQ (normal_y_val, 0.0);
01075   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "normal_z", is_normal_z, normal_z_val));
01076   EXPECT_EQ (is_normal_z, true);
01077   EXPECT_EQ (normal_z_val, 0.0);
01078   
01079   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "x", x_val));
01080   EXPECT_EQ (x_val, 1.0);
01081 
01082   float xx_val = -1.0;
01083   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "xx", xx_val));
01084   EXPECT_EQ (xx_val, -1.0);
01085   bool is_xx = true;
01086   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "xx", is_xx, xx_val));
01087   EXPECT_EQ (is_xx, false);
01088 }
01089 
01091 TEST (PCL, SetIfFieldExists)
01092 {
01093   PointXYZRGBNormal p;
01094 
01095   p.x = p.y = p.z = 0.0;
01096   p.r = p.g = p.b = 0;
01097   p.normal_x = p.normal_y = p.normal_z = 0.0;
01098 
01099   typedef pcl::traits::fieldList<PointXYZRGBNormal>::type FieldList;
01100   pcl::for_each_type<FieldList> (SetIfFieldExists<PointXYZRGBNormal, float> (p, "x", 1.0));
01101   EXPECT_EQ (p.x, 1.0);
01102   pcl::for_each_type<FieldList> (SetIfFieldExists<PointXYZRGBNormal, float> (p, "y", 2.0));
01103   EXPECT_EQ (p.y, 2.0);
01104   pcl::for_each_type<FieldList> (SetIfFieldExists<PointXYZRGBNormal, float> (p, "z", 3.0));
01105   EXPECT_EQ (p.z, 3.0);
01106   pcl::for_each_type<FieldList> (SetIfFieldExists<PointXYZRGBNormal, float> (p, "normal_x", 1.0));
01107   EXPECT_EQ (p.normal_x, 1.0);
01108   pcl::for_each_type<FieldList> (SetIfFieldExists<PointXYZRGBNormal, float> (p, "normal_y", 0.0));
01109   EXPECT_EQ (p.normal_y, 0.0);
01110   pcl::for_each_type<FieldList> (SetIfFieldExists<PointXYZRGBNormal, float> (p, "normal_z", 0.0));
01111   EXPECT_EQ (p.normal_z, 0.0);
01112 
01113 //  pcl::PointXY p1;
01114 //  pcl::for_each_type<pcl::traits::fieldList<pcl::PointXY>::type> (pcl::SetIfFieldExists<pcl::PointXY, float> (p1, "intensity", 3.0));
01115 //
01116 //  pcl::PFHSignature125 p2;
01117 //  pcl::for_each_type<pcl::traits::fieldList<pcl::PFHSignature125>::type> (pcl::SetIfFieldExists<pcl::PFHSignature125, float*> (p2, "intensity", 3.0));
01118 }
01119 
01121 TEST (PCL, IsSamePointType)
01122 {
01123   bool status = isSamePointType<PointXYZ, PointXYZ> ();
01124   EXPECT_TRUE (status);
01125   status = isSamePointType<PointXYZ, PointXY> ();
01126   EXPECT_FALSE (status);
01127   status = isSamePointType<PointXY, PointXYZ> ();
01128   EXPECT_FALSE (status);
01129   status = isSamePointType<PointNormal, PointNormal> ();
01130   EXPECT_TRUE (status);
01131   status = isSamePointType<PointNormal, PointXYZRGBNormal> ();
01132   EXPECT_FALSE (status);
01133   status = isSamePointType<PointXYZRGB, PointXYZRGB> ();
01134   EXPECT_TRUE (status);
01135   
01136   // Even though it's the "same" type, rgb != rgba
01137   status = isSamePointType<PointXYZRGB, PointXYZRGBA> ();
01138   EXPECT_FALSE (status);
01139   status = isSamePointType<PointXYZRGBA, PointXYZRGB> ();
01140   EXPECT_FALSE (status);
01141 }
01142 
01143 /* ---[ */
01144 int
01145 main (int argc, char** argv)
01146 {
01147   testing::InitGoogleTest (&argc, argv);
01148   return (RUN_ALL_TESTS ());
01149 }
01150 /* ]--- */


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:34:27