test_plane_intersection.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the copyright holder(s) nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: test_plane_intersection.cpp 5686 2012-05-11 20:59:00Z gioia $
00035  */
00036 
00037 #include <gtest/gtest.h>
00038 #include <pcl/common/common.h>
00039 #include <pcl/common/intersections.h>
00040 #include <pcl/pcl_tests.h>
00041 
00042 
00043 using namespace pcl;
00044 
00046 TEST (PCL, lineWithLineIntersection)
00047 {
00048   Eigen::VectorXf line_a(6);
00049   Eigen::VectorXf line_b(6);
00050 
00051   //case 1
00052   line_a[0] = 0.01f;
00053   line_a[1] = 0.02f;
00054   line_a[2] = 0.03f;
00055   line_a[3] = 0.4f;
00056   line_a[4] = 0.5f;
00057   line_a[5] = 0.6f;
00058 
00059   line_b[0] = 0.1f;
00060   line_b[1] = 0.2f;
00061   line_b[2] = 0.3f;
00062   line_b[3] = 0.04f;
00063   line_b[4] = 0.05f;
00064   line_b[5] = 0.06f;
00065 
00066   Eigen::Vector4f p1, p2;
00067   lineToLineSegment (line_a, line_b, p1, p2);
00068 
00069   Eigen::Vector4f point_case_1;
00070   bool result_case_1 = lineWithLineIntersection(line_a, line_b, point_case_1);
00071 
00072   double sqr_dist_case_1 = (p1 - p2).squaredNorm ();
00073 
00074   double default_sqr_eps = 1e-4;
00075   EXPECT_GT(sqr_dist_case_1, default_sqr_eps);
00076   Eigen::Vector4f zero(0.0f, 0.0f, 0.0f, 0.0f);
00077 
00078   EXPECT_EQ(point_case_1[0], zero[0]);
00079   EXPECT_EQ(point_case_1[1], zero[1]);
00080   EXPECT_EQ(point_case_1[2], zero[2]);
00081   EXPECT_EQ(point_case_1[3], zero[3]);
00082 
00083   EXPECT_FALSE(result_case_1);
00084   
00085   pcl::ModelCoefficients line_a_mod;
00086   pcl::ModelCoefficients line_b_mod;
00087 
00088   std::vector<float> values_a_case_1;
00089   values_a_case_1.push_back(line_a[0]);
00090   values_a_case_1.push_back(line_a[1]);
00091   values_a_case_1.push_back(line_a[2]);
00092   values_a_case_1.push_back(line_a[3]);
00093   values_a_case_1.push_back(line_a[4]);
00094   values_a_case_1.push_back(line_a[5]);
00095 
00096   std::vector<float> values_b_case_1;
00097   values_b_case_1.push_back(line_b[0]);
00098   values_b_case_1.push_back(line_b[1]);
00099   values_b_case_1.push_back(line_b[2]);
00100   values_b_case_1.push_back(line_b[3]);
00101   values_b_case_1.push_back(line_b[4]);
00102   values_b_case_1.push_back(line_b[5]);
00103 
00104   line_a_mod.values = values_a_case_1;
00105   line_b_mod.values = values_b_case_1;
00106 
00107   Eigen::Vector4f point_mod_1;
00108   EXPECT_FALSE(lineWithLineIntersection(line_a_mod, line_b_mod, point_mod_1));
00109   EXPECT_EQ(result_case_1, lineWithLineIntersection(line_a_mod, line_b_mod, point_mod_1));
00110 
00111   EXPECT_EQ(point_mod_1[0], zero[0]);
00112   EXPECT_EQ(point_mod_1[1], zero[1]);
00113   EXPECT_EQ(point_mod_1[2], zero[2]);
00114   EXPECT_EQ(point_mod_1[3], zero[3]);
00115 
00116   //case 2
00117   line_a[0] = 0.00100f;
00118   line_a[1] = 0.00200f;
00119   line_a[2] = 0.00300f;
00120   line_a[3] = 0.00400f;
00121   line_a[4] = 0.00500f;
00122   line_a[5] = 0.00600f;
00123 
00124   line_b[0] = 0.00157f;
00125   line_b[1] = 0.00233f;
00126   line_b[2] = 0.00378f;
00127   line_b[3] = 0.00495f;
00128   line_b[4] = 0.00565f;
00129   line_b[5] = 0.00666f;
00130 
00131   lineToLineSegment (line_a, line_b, p1, p2);
00132 
00133   Eigen::Vector4f point_case_2;
00134   double sqr_eps_case_2 = 1e-1;
00135   bool result_case_2 = lineWithLineIntersection(line_a, line_b, point_case_2, sqr_eps_case_2);
00136 
00137   double sqr_dist_case_2 = (p1 - p2).squaredNorm ();
00138   EXPECT_LT(sqr_dist_case_2, sqr_eps_case_2);
00139 
00140   EXPECT_EQ(point_case_2[0], p1[0]);
00141   EXPECT_EQ(point_case_2[1], p1[1]);
00142   EXPECT_EQ(point_case_2[2], p1[2]);
00143   EXPECT_EQ(point_case_2[3], p1[3]);
00144 
00145   EXPECT_TRUE(result_case_2);
00146 
00147   pcl::ModelCoefficients line_a_mod_2;
00148   pcl::ModelCoefficients line_b_mod_2;
00149 
00150   std::vector<float> values_a_case_2;
00151   values_a_case_2.push_back(0.1000f);
00152   values_a_case_2.push_back(0.2000f);
00153   values_a_case_2.push_back(0.3000f);
00154   values_a_case_2.push_back(0.4000f);
00155   values_a_case_2.push_back(0.5000f);
00156   values_a_case_2.push_back(0.6000f);
00157 
00158   std::vector<float> values_b_case_2;
00159   values_b_case_2.push_back(0.1001f);
00160   values_b_case_2.push_back(0.2001f);
00161   values_b_case_2.push_back(0.3001f);
00162   values_b_case_2.push_back(0.4001f);
00163   values_b_case_2.push_back(0.5001f);
00164   values_b_case_2.push_back(0.6001f);
00165 
00166   line_a_mod_2.values = values_a_case_2;
00167   line_b_mod_2.values = values_b_case_2;
00168 
00169   Eigen::Vector4f point_mod_2;
00170   Eigen::Vector4f point_mod_case_2;
00171   double sqr_mod_case_2 = 1e-1;
00172 
00173   Eigen::VectorXf coeff1 = Eigen::VectorXf::Map (&line_a_mod_2.values[0], line_a_mod_2.values.size ());
00174   Eigen::VectorXf coeff2 = Eigen::VectorXf::Map (&line_b_mod_2.values[0], line_b_mod_2.values.size ());
00175 
00176   Eigen::Vector4f p1_mod, p2_mod;
00177   lineToLineSegment (coeff1, coeff2, p1_mod, p2_mod);
00178   double sqr_mod_act_2 = (p1_mod - p2_mod).squaredNorm ();
00179 
00180   EXPECT_LT(sqr_mod_act_2, sqr_mod_case_2);
00181   EXPECT_EQ(lineWithLineIntersection (coeff1, coeff2, point_mod_case_2, sqr_mod_case_2),
00182                         lineWithLineIntersection(line_a_mod_2, line_b_mod_2, point_mod_2, sqr_mod_case_2));
00183   EXPECT_TRUE(lineWithLineIntersection(line_a_mod_2, line_b_mod_2, point_mod_2, sqr_mod_case_2));
00184 
00185   EXPECT_EQ(point_mod_2[0], point_mod_case_2[0]);
00186   EXPECT_EQ(point_mod_2[1], point_mod_case_2[1]);
00187   EXPECT_EQ(point_mod_2[2], point_mod_case_2[2]);
00188   EXPECT_EQ(point_mod_2[3], point_mod_case_2[3]);
00189 
00190 }
00191 
00193 TEST (PCL, planeWithPlaneIntersection)
00194 {
00195   //Testing parallel planes
00196   const int k = 2;
00197   float x = 1.0f;
00198   float y = 2.0f;
00199   float z = 3.0f;
00200   float w = 4.0f;
00201   Eigen::Vector4f plane_a;
00202   plane_a.x() = x;
00203   plane_a.y() = y;
00204   plane_a.z() = z;
00205   plane_a.w() = w;
00206 
00207   EXPECT_EQ(1.0f, plane_a.x());
00208   EXPECT_EQ(2.0f, plane_a.y());
00209   EXPECT_EQ(3.0f, plane_a.z());
00210   EXPECT_EQ(4.0f, plane_a.w());
00211 
00212   Eigen::Vector4f plane_b;
00213   plane_b.x() = x;
00214   plane_b.y() = y;
00215   plane_b.z() = z;
00216   plane_b.w() = w + k;
00217 
00218   EXPECT_EQ(1.0f, plane_b.x());
00219   EXPECT_EQ(2.0f, plane_b.y());
00220   EXPECT_EQ(3.0f, plane_b.z());
00221   EXPECT_EQ(6.0f, plane_b.w());
00222 
00223   Eigen::VectorXf line;
00224 
00225   EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 45));
00226   std::cout << std::endl;
00227   EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 90));
00228   std::cout << std::endl;
00229   EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 180));
00230   std::cout << std::endl;
00231   EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 360));
00232 
00233   plane_b.x() = k * x;
00234   plane_b.y() = k * y;
00235   plane_b.z() = k * z;
00236   plane_b.w() = k * w;
00237 
00238   EXPECT_EQ(2.0f, plane_b.x());
00239   EXPECT_EQ(4.0f, plane_b.y());
00240   EXPECT_EQ(6.0f, plane_b.z());
00241   EXPECT_EQ(8.0f, plane_b.w());
00242 
00243   std::cout << std::endl;
00244   EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 45));
00245   std::cout << std::endl;
00246   EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 90));
00247   std::cout << std::endl;
00248   EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 180));
00249   std::cout << std::endl;
00250   EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 360));
00251 
00252   //overlapping planes
00253   plane_b.w() = w;
00254   EXPECT_EQ(4.0f, plane_b.w());
00255 
00256   std::cout << std::endl;
00257   EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 45));
00258   std::cout << std::endl;
00259   EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 90));
00260   std::cout << std::endl;
00261   EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 180));
00262   std::cout << std::endl;
00263   EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 360));
00264 
00265   //orthogonal planes
00266   plane_a.x() = 2.0f;
00267   plane_a.y() = 1.0f;
00268   plane_a.z() = -5.0f;
00269   plane_a.w() = 6.0f;
00270   EXPECT_EQ(2.0f, plane_a.x());
00271   EXPECT_EQ(1.0f, plane_a.y());
00272   EXPECT_EQ(-5.0f, plane_a.z());
00273   EXPECT_EQ(6.0f, plane_a.w());
00274 
00275   plane_b.x() = 2.0f;
00276   plane_b.y() = 1.0f;
00277   plane_b.z() = 1.0f;
00278   plane_b.w() = 7.0f;
00279 
00280   EXPECT_EQ(2.0f, plane_b.x());
00281   EXPECT_EQ(1.0f, plane_b.y());
00282   EXPECT_EQ(1.0f, plane_b.z());
00283   EXPECT_EQ(7.0f, plane_b.w());
00284 
00285   std::cout << std::endl;
00286   EXPECT_TRUE(planeWithPlaneIntersection(plane_a, plane_b, line, 0));
00287 
00288   //general planes
00289   plane_a.x() = 1.555f;
00290   plane_a.y() = 0.894f;
00291   plane_a.z() = 1.234f;
00292   plane_a.w() = 3.567f;
00293 
00294   plane_b.x() = 0.743f;
00295   plane_b.y() = 1.890f;
00296   plane_b.z() = 6.789f;
00297   plane_b.w() = 5.432f;
00298 
00299   std::cout << std::endl;
00300   EXPECT_TRUE(planeWithPlaneIntersection(plane_a, plane_b, line, 0));
00301 
00302 }
00303 
00304 //* ---[ */
00305 int
00306 main (int argc, char** argv)
00307 {
00308   testing::InitGoogleTest (&argc, argv);
00309   return (RUN_ALL_TESTS ());
00310 }
00311 /* ]--- */
00312 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:35:30