visibility_confidence.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) 2009-2012, Willow Garage, Inc.
00006  * Copyright (c) 2012-, Open Perception, Inc.
00007  *
00008  * All rights reserved.
00009  *
00010  * Redistribution and use in source and binary forms, with or without
00011  * modification, are permitted provided that the following conditions
00012  * are met:
00013  *
00014  *  * Redistributions of source code must retain the above copyright
00015  *    notice, this list of conditions and the following disclaimer.
00016  *  * Redistributions in binary form must reproduce the above
00017  *    copyright notice, this list of conditions and the following
00018  *    disclaimer in the documentation and/or other materials provided
00019  *    with the distribution.
00020  *  * Neither the name of the copyright holder(s) nor the names of its
00021  *    contributors may be used to endorse or promote products derived
00022  *    from this software without specific prior written permission.
00023  *
00024  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  * POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  * $Id$
00038  *
00039  */
00040 
00041 #include <pcl/apps/in_hand_scanner/visibility_confidence.h>
00042 
00043 pcl::ihs::Dome::Dome ()
00044 {
00045   vertices_.col ( 0) = Eigen::Vector4f (-0.000000119f,  0.000000000f, 1.000000000f, 0.f);
00046   vertices_.col ( 1) = Eigen::Vector4f ( 0.894427180f,  0.000000000f, 0.447213739f, 0.f);
00047   vertices_.col ( 2) = Eigen::Vector4f ( 0.276393145f,  0.850650907f, 0.447213650f, 0.f);
00048   vertices_.col ( 3) = Eigen::Vector4f (-0.723606884f,  0.525731146f, 0.447213531f, 0.f);
00049   vertices_.col ( 4) = Eigen::Vector4f (-0.723606884f, -0.525731146f, 0.447213531f, 0.f);
00050   vertices_.col ( 5) = Eigen::Vector4f ( 0.276393145f, -0.850650907f, 0.447213650f, 0.f);
00051   vertices_.col ( 6) = Eigen::Vector4f ( 0.343278527f,  0.000000000f, 0.939233720f, 0.f);
00052   vertices_.col ( 7) = Eigen::Vector4f ( 0.686557174f,  0.000000000f, 0.727075875f, 0.f);
00053   vertices_.col ( 8) = Eigen::Vector4f ( 0.792636156f,  0.326477438f, 0.514918089f, 0.f);
00054   vertices_.col ( 9) = Eigen::Vector4f ( 0.555436373f,  0.652954817f, 0.514918029f, 0.f);
00055   vertices_.col (10) = Eigen::Vector4f ( 0.106078848f,  0.326477438f, 0.939233720f, 0.f);
00056   vertices_.col (11) = Eigen::Vector4f ( 0.212157741f,  0.652954817f, 0.727075756f, 0.f);
00057   vertices_.col (12) = Eigen::Vector4f (-0.065560505f,  0.854728878f, 0.514917910f, 0.f);
00058   vertices_.col (13) = Eigen::Vector4f (-0.449357629f,  0.730025530f, 0.514917850f, 0.f);
00059   vertices_.col (14) = Eigen::Vector4f (-0.277718395f,  0.201774135f, 0.939233661f, 0.f);
00060   vertices_.col (15) = Eigen::Vector4f (-0.555436671f,  0.403548241f, 0.727075696f, 0.f);
00061   vertices_.col (16) = Eigen::Vector4f (-0.833154857f,  0.201774105f, 0.514917850f, 0.f);
00062   vertices_.col (17) = Eigen::Vector4f (-0.833154857f, -0.201774150f, 0.514917850f, 0.f);
00063   vertices_.col (18) = Eigen::Vector4f (-0.277718395f, -0.201774135f, 0.939233661f, 0.f);
00064   vertices_.col (19) = Eigen::Vector4f (-0.555436671f, -0.403548241f, 0.727075696f, 0.f);
00065   vertices_.col (20) = Eigen::Vector4f (-0.449357659f, -0.730025649f, 0.514917910f, 0.f);
00066   vertices_.col (21) = Eigen::Vector4f (-0.065560460f, -0.854728937f, 0.514917850f, 0.f);
00067   vertices_.col (22) = Eigen::Vector4f ( 0.106078848f, -0.326477438f, 0.939233720f, 0.f);
00068   vertices_.col (23) = Eigen::Vector4f ( 0.212157741f, -0.652954817f, 0.727075756f, 0.f);
00069   vertices_.col (24) = Eigen::Vector4f ( 0.555436373f, -0.652954757f, 0.514917970f, 0.f);
00070   vertices_.col (25) = Eigen::Vector4f ( 0.792636156f, -0.326477349f, 0.514918089f, 0.f);
00071   vertices_.col (26) = Eigen::Vector4f ( 0.491123378f,  0.356822133f, 0.794654608f, 0.f);
00072   vertices_.col (27) = Eigen::Vector4f (-0.187592626f,  0.577350259f, 0.794654429f, 0.f);
00073   vertices_.col (28) = Eigen::Vector4f (-0.607062101f, -0.000000016f, 0.794654369f, 0.f);
00074   vertices_.col (29) = Eigen::Vector4f (-0.187592626f, -0.577350378f, 0.794654489f, 0.f);
00075   vertices_.col (30) = Eigen::Vector4f ( 0.491123348f, -0.356822133f, 0.794654548f, 0.f);
00076 
00077   for (unsigned int i=0; i<vertices_.cols (); ++i)
00078   {
00079     vertices_.col (i).head <3> ().normalize ();
00080   }
00081 }
00082 
00084 
00085 pcl::ihs::Dome::Vertices
00086 pcl::ihs::Dome::getVertices () const
00087 {
00088   return (vertices_);
00089 }
00090 
00092 
00093 namespace pcl
00094 {
00095   namespace ihs
00096   {
00097     static const pcl::ihs::Dome dome;
00098   } // End namespace ihs
00099 } // End namespace pcl
00100 
00102 
00103 void
00104 pcl::ihs::addDirection (const Eigen::Vector4f& normal,
00105                         const Eigen::Vector4f& direction,
00106                         uint32_t&              directions)
00107 {
00108   // Find the rotation that aligns the normal with [0; 0; 1]
00109   const float dot = normal.z ();
00110 
00111   Eigen::Isometry3f R = Eigen::Isometry3f::Identity ();
00112 
00113   // No need to transform if the normal is already very close to [0; 0; 1] (also avoids numerical issues)
00114   // TODO: The threshold is hard coded for a frequency=3.
00115   //       It can be calculated with
00116   //       - max_z = maximum z value of the dome vertices (excluding [0; 0; 1])
00117   //       - thresh = cos (acos (max_z) / 2)
00118   //       - always round up!
00119   //       - with max_z = 0.939 -> thresh = 0.9847 ~ 0.985
00120   if (dot <= .985f)
00121   {
00122     const Eigen::Vector3f axis = Eigen::Vector3f (normal.y (), -normal.x (), 0.f).normalized ();
00123     R = Eigen::Isometry3f (Eigen::AngleAxisf (std::acos (dot), axis));
00124   }
00125 
00126   // Transform the direction into the dome coordinate system (which is aligned with the normal)
00127   Eigen::Vector4f aligned_direction = (R * direction);
00128   aligned_direction.head <3> ().normalize ();
00129 
00130   if (aligned_direction.z () < 0)
00131   {
00132     return;
00133   }
00134 
00135   // Find the closest viewing direction
00136   // NOTE: cos (0deg) = 1 = max
00137   //       acos (angle) = dot (a, b) / (norm (a) * norm (b)
00138   //       m_sphere_vertices are already normalized
00139   unsigned int index = 0;
00140   aligned_direction.transpose ().lazyProduct (pcl::ihs::dome.getVertices ()).maxCoeff (&index);
00141 
00142   // Set the observed direction bit at 'index'
00143   // http://stackoverflow.com/questions/47981/how-do-you-set-clear-and-toggle-a-single-bit-in-c/47990#47990
00144   directions |= (1 << index);
00145 }
00146 
00148 
00149 unsigned int
00150 pcl::ihs::countDirections (const uint32_t directions)
00151 {
00152   // http://stackoverflow.com/questions/109023/best-algorithm-to-count-the-number-of-set-bits-in-a-32-bit-integer/109025#109025
00153   unsigned int i = directions - ((directions >> 1) & 0x55555555);
00154   i = (i & 0x33333333) + ((i >> 2) & 0x33333333);
00155 
00156   return ((((i + (i >> 4)) & 0x0F0F0F0F) * 0x01010101) >> 24);
00157 }
00158 


ros_in_hand_scanner
Author(s):
autogenerated on Thu Jun 6 2019 20:39:38