Point Cloud Library (PCL)
1.7.0
|
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 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 00035 #ifndef PCL_FILTERS_IMPL_BOX_CLIPPER3D_HPP 00036 #define PCL_FILTERS_IMPL_BOX_CLIPPER3D_HPP 00037 00038 #include <pcl/filters/box_clipper3D.h> 00039 00040 template<typename PointT> 00041 pcl::BoxClipper3D<PointT>::BoxClipper3D (const Eigen::Affine3f& transformation) 00042 : transformation_ (transformation) 00043 { 00044 //inverse_transformation_ = transformation_.inverse (); 00045 } 00046 00047 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00048 template<typename PointT> 00049 pcl::BoxClipper3D<PointT>::BoxClipper3D (const Eigen::Vector3f& rodrigues, const Eigen::Vector3f& translation, const Eigen::Vector3f& box_size) 00050 { 00051 setTransformation (rodrigues, translation, box_size); 00052 } 00053 00054 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00055 template<typename PointT> 00056 pcl::BoxClipper3D<PointT>::~BoxClipper3D () throw () 00057 { 00058 } 00059 00060 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00061 template<typename PointT> void 00062 pcl::BoxClipper3D<PointT>::setTransformation (const Eigen::Affine3f& transformation) 00063 { 00064 transformation_ = transformation; 00065 //inverse_transformation_ = transformation_.inverse (); 00066 } 00067 00068 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00069 template<typename PointT> void 00070 pcl::BoxClipper3D<PointT>::setTransformation (const Eigen::Vector3f& rodrigues, const Eigen::Vector3f& translation, const Eigen::Vector3f& box_size) 00071 { 00072 transformation_ = Eigen::Translation3f (translation) * Eigen::AngleAxisf(rodrigues.norm (), rodrigues.normalized ()) * Eigen::Scaling (box_size); 00073 //inverse_transformation_ = transformation_.inverse (); 00074 } 00075 00076 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00077 template<typename PointT> pcl::Clipper3D<PointT>* 00078 pcl::BoxClipper3D<PointT>::clone () const 00079 { 00080 return new BoxClipper3D<PointT> (transformation_); 00081 } 00082 00083 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00084 template<typename PointT> void 00085 pcl::BoxClipper3D<PointT>::transformPoint (const PointT& pointIn, PointT& pointOut) const 00086 { 00087 const Eigen::Vector4f& point = pointIn.getVector4fMap (); 00088 pointOut.getVector4fMap () = transformation_ * point; 00089 00090 // homogeneous value might not be 1 00091 if (point [3] != 1) 00092 { 00093 // homogeneous component might be uninitialized -> invalid 00094 if (point [3] != 0) 00095 { 00096 pointOut.x += (1 - point [3]) * transformation_.data () [ 9]; 00097 pointOut.y += (1 - point [3]) * transformation_.data () [10]; 00098 pointOut.z += (1 - point [3]) * transformation_.data () [11]; 00099 } 00100 else 00101 { 00102 pointOut.x += transformation_.data () [ 9]; 00103 pointOut.y += transformation_.data () [10]; 00104 pointOut.z += transformation_.data () [11]; 00105 } 00106 } 00107 } 00108 00109 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00110 // ToDo use product on point.getVector3fMap () and transformatio_.col (i) to use the SSE advantages of eigen 00111 template<typename PointT> bool 00112 pcl::BoxClipper3D<PointT>::clipPoint3D (const PointT& point) const 00113 { 00114 return (fabs(transformation_.data () [ 0] * point.x + 00115 transformation_.data () [ 3] * point.y + 00116 transformation_.data () [ 6] * point.z + 00117 transformation_.data () [ 9]) <= 1 && 00118 fabs(transformation_.data () [ 1] * point.x + 00119 transformation_.data () [ 4] * point.y + 00120 transformation_.data () [ 7] * point.z + 00121 transformation_.data () [10]) <= 1 && 00122 fabs(transformation_.data () [ 2] * point.x + 00123 transformation_.data () [ 5] * point.y + 00124 transformation_.data () [ 8] * point.z + 00125 transformation_.data () [11]) <= 1 ); 00126 } 00127 00128 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00129 /** 00130 * @attention untested code 00131 */ 00132 template<typename PointT> bool 00133 pcl::BoxClipper3D<PointT>::clipLineSegment3D (PointT& point1, PointT& point2) const 00134 { 00135 /* 00136 PointT pt1, pt2; 00137 transformPoint (point1, pt1); 00138 transformPoint (point2, pt2); 00139 00140 // 00141 bool pt1InBox = (fabs(pt1.x) <= 1.0 && fabs (pt1.y) <= 1.0 && fabs (pt1.z) <= 1.0); 00142 bool pt2InBox = (fabs(pt2.x) <= 1.0 && fabs (pt2.y) <= 1.0 && fabs (pt2.z) <= 1.0); 00143 00144 // one is outside the other one inside the box 00145 //if (pt1InBox ^ pt2InBox) 00146 if (pt1InBox && !pt2InBox) 00147 { 00148 PointT diff; 00149 PointT lambda; 00150 diff.getVector3fMap () = pt2.getVector3fMap () - pt1.getVector3fMap (); 00151 00152 if (diff.x > 0) 00153 lambda.x = (1.0 - pt1.x) / diff.x; 00154 else 00155 lambda.x = (-1.0 - pt1.x) / diff.x; 00156 00157 if (diff.y > 0) 00158 lambda.y = (1.0 - pt1.y) / diff.y; 00159 else 00160 lambda.y = (-1.0 - pt1.y) / diff.y; 00161 00162 if (diff.z > 0) 00163 lambda.z = (1.0 - pt1.z) / diff.z; 00164 else 00165 lambda.z = (-1.0 - pt1.z) / diff.z; 00166 00167 pt2 = pt1 + std::min(std::min(lambda.x, lambda.y), lambda.z) * diff; 00168 00169 // inverse transformation 00170 inverseTransformPoint (pt2, point2); 00171 return true; 00172 } 00173 else if (!pt1InBox && pt2InBox) 00174 { 00175 return true; 00176 } 00177 */ 00178 return false; 00179 } 00180 00181 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00182 /** 00183 * @attention untested code 00184 */ 00185 template<typename PointT> void 00186 pcl::BoxClipper3D<PointT>::clipPlanarPolygon3D (const std::vector<PointT>& polygon, std::vector<PointT>& clipped_polygon) const 00187 { 00188 // not implemented -> clip everything 00189 clipped_polygon.clear (); 00190 } 00191 00192 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00193 /** 00194 * @attention untested code 00195 */ 00196 template<typename PointT> void 00197 pcl::BoxClipper3D<PointT>::clipPlanarPolygon3D (std::vector<PointT>& polygon) const 00198 { 00199 // not implemented -> clip everything 00200 polygon.clear (); 00201 } 00202 00203 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00204 // /ToDo: write fast version using eigen map and single matrix vector multiplication, that uses advantages of eigens SSE operations. 00205 template<typename PointT> void 00206 pcl::BoxClipper3D<PointT>::clipPointCloud3D (const pcl::PointCloud<PointT>& cloud_in, std::vector<int>& clipped, const std::vector<int>& indices) const 00207 { 00208 if (indices.empty ()) 00209 { 00210 clipped.reserve (cloud_in.size ()); 00211 for (register unsigned pIdx = 0; pIdx < cloud_in.size (); ++pIdx) 00212 if (clipPoint3D (cloud_in[pIdx])) 00213 clipped.push_back (pIdx); 00214 } 00215 else 00216 { 00217 for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt) 00218 if (clipPoint3D (cloud_in[*iIt])) 00219 clipped.push_back (*iIt); 00220 } 00221 } 00222 #endif //PCL_FILTERS_IMPL_BOX_CLIPPER3D_HPP