plane_clipper3D.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, 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 Willow Garage, Inc. 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 
00035 #ifndef PCL_FILTERS_IMPL_PLANE_CLIPPER3D_HPP
00036 #define PCL_FILTERS_IMPL_PLANE_CLIPPER3D_HPP
00037 
00038 #include <pcl/filters/plane_clipper3D.h>
00039 
00040 template<typename PointT>
00041 pcl::PlaneClipper3D<PointT>::PlaneClipper3D (const Eigen::Vector4f& plane_params)
00042 : plane_params_ (plane_params)
00043 {
00044 }
00045 
00046 template<typename PointT>
00047 pcl::PlaneClipper3D<PointT>::~PlaneClipper3D () throw ()
00048 {
00049 }
00050 
00051 template<typename PointT> void
00052 pcl::PlaneClipper3D<PointT>::setPlaneParameters (const Eigen::Vector4f& plane_params)
00053 {
00054   plane_params_ = plane_params;
00055 }
00056 
00057 template<typename PointT> const Eigen::Vector4f&
00058 pcl::PlaneClipper3D<PointT>::getPlaneParameters () const
00059 {
00060   return plane_params_;
00061 }
00062 
00063 template<typename PointT> pcl::Clipper3D<PointT>*
00064 pcl::PlaneClipper3D<PointT>::clone () const
00065 {
00066   return new PlaneClipper3D<PointT> (plane_params_);
00067 }
00068 
00069 template<typename PointT> float
00070 pcl::PlaneClipper3D<PointT>::getDistance (const PointT& point) const
00071 {
00072   return (plane_params_[0] * point.x + plane_params_[1] * point.y + plane_params_[2] * point.z + plane_params_[3]);
00073 }
00074 
00075 template<typename PointT> bool
00076 pcl::PlaneClipper3D<PointT>::clipPoint3D (const PointT& point) const
00077 {
00078   return ((plane_params_[0] * point.x + plane_params_[1] * point.y + plane_params_[2] * point.z ) >= -plane_params_[3]);
00079 }
00080 
00084 template<typename PointT> bool
00085 pcl::PlaneClipper3D<PointT>::clipLineSegment3D (PointT& point1, PointT& point2) const
00086 {
00087   float dist1 = getDistance (point1);
00088   float dist2 = getDistance (point2);
00089 
00090   if (dist1 * dist2 > 0) // both on same side of the plane -> nothing to clip
00091     return (dist1 > 0); // true if both are on positive side, thus visible
00092 
00093   float lambda = dist2 / (dist2 - dist1);
00094 
00095   // get the plane intersecion
00096   PointT intersection;
00097   intersection.x = (point1.x - point2.x) * lambda + point2.x;
00098   intersection.y = (point1.y - point2.y) * lambda + point2.y;
00099   intersection.z = (point1.z - point2.z) * lambda + point2.z;
00100 
00101   // point1 is visible, point2 not => point2 needs to be replaced by intersection
00102   if (dist1 >= 0)
00103     point2 = intersection;
00104   else
00105     point1 = intersection;
00106 
00107   return false;
00108 }
00109 
00113 template<typename PointT> void
00114 pcl::PlaneClipper3D<PointT>::clipPlanarPolygon3D (const std::vector<PointT>& polygon, std::vector<PointT>& clipped_polygon) const
00115 {
00116   clipped_polygon.clear ();
00117   clipped_polygon.reserve (polygon.size ());
00118 
00119   // test for degenerated polygons
00120   if (polygon.size () < 3)
00121   {
00122     if (polygon.size () == 1)
00123     {
00124       // point outside clipping area ?
00125       if (clipPoint3D (polygon [0]))
00126         clipped_polygon.push_back (polygon [0]);
00127     }
00128     else if (polygon.size () == 2)
00129     {
00130       clipped_polygon.push_back (polygon [0]);
00131       clipped_polygon.push_back (polygon [1]);
00132       if (!clipLineSegment3D (clipped_polygon [0], clipped_polygon [1]))
00133         clipped_polygon.clear ();
00134     }
00135     return;
00136   }
00137 
00138   float previous_distance = getDistance (polygon [0]);
00139 
00140   if (previous_distance > 0)
00141     clipped_polygon.push_back (polygon [0]);
00142 
00143   typename std::vector<PointT>::const_iterator prev_it = polygon.begin ();
00144 
00145   for (typename std::vector<PointT>::const_iterator pIt = prev_it + 1; pIt != polygon.end (); prev_it = pIt++)
00146   {
00147     // if we intersect plane
00148     float distance = getDistance (*pIt);
00149     if (distance * previous_distance < 0)
00150     {
00151       float lambda = distance / (distance - previous_distance);
00152 
00153       PointT intersection;
00154       intersection.x = (prev_it->x - pIt->x) * lambda + pIt->x;
00155       intersection.y = (prev_it->y - pIt->y) * lambda + pIt->y;
00156       intersection.z = (prev_it->z - pIt->z) * lambda + pIt->z;
00157 
00158       clipped_polygon.push_back (intersection);
00159     }
00160     if (distance > 0)
00161       clipped_polygon.push_back (*pIt);
00162 
00163     previous_distance = distance;
00164   }
00165 }
00166 
00167 template<typename PointT> void
00168 pcl::PlaneClipper3D<PointT>::clipPointCloud3D (const pcl::PointCloud<PointT>& cloud_in, std::vector<int>& clipped, const std::vector<int>& indices) const
00169 {
00170   if (indices.empty ())
00171   {
00172     clipped.reserve (cloud_in.size ());
00173     /*
00174 #if 0
00175     Eigen::MatrixXf points = cloud_in.getMatrixXfMap (4, sizeof (PointT) / sizeof (float), offsetof(PointT,x) / sizeof (float));
00176     Eigen::VectorXf distances = plane_params_.transpose () * points;
00177     for (register unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx)
00178     {
00179       if (distances (rIdx, 0) >= -plane_params_[3])
00180         clipped.push_back (rIdx);
00181     }
00182 #else
00183     Eigen::Matrix4Xf points (4, cloud_in.size ());
00184     for (register unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx)
00185     {
00186       points (0, rIdx) = cloud_in[rIdx].x;
00187       points (1, rIdx) = cloud_in[rIdx].y;
00188       points (2, rIdx) = cloud_in[rIdx].z;
00189       points (3, rIdx) = 1;
00190     }
00191     Eigen::VectorXf distances = plane_params_.transpose () * points;
00192     for (register unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx)
00193     {
00194       if (distances (rIdx, 0) >= 0)
00195         clipped.push_back (rIdx);
00196     }
00197 
00198 #endif
00199 
00200     //cout << "points   : " << points.rows () << " x " << points.cols () << " * " << plane_params_.transpose ().rows () << " x " << plane_params_.transpose ().cols () << endl;
00201 
00202     //cout << "distances: " << distances.rows () << " x " << distances.cols () << endl;
00203     /*/
00204     for (register unsigned pIdx = 0; pIdx < cloud_in.size (); ++pIdx)
00205       if (clipPoint3D (cloud_in[pIdx]))
00206         clipped.push_back (pIdx);
00207     //*/
00208   }
00209   else
00210   {
00211     for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
00212       if (clipPoint3D (cloud_in[*iIt]))
00213         clipped.push_back (*iIt);
00214   }
00215 }
00216 #endif //PCL_FILTERS_IMPL_PLANE_CLIPPER3D_HPP


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:16:31