Point Cloud Library (PCL)  1.7.1
box_clipper3D.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder(s) nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #ifndef PCL_FILTERS_IMPL_BOX_CLIPPER3D_HPP
36 #define PCL_FILTERS_IMPL_BOX_CLIPPER3D_HPP
37 
38 #include <pcl/filters/box_clipper3D.h>
39 
40 template<typename PointT>
41 pcl::BoxClipper3D<PointT>::BoxClipper3D (const Eigen::Affine3f& transformation)
42 : transformation_ (transformation)
43 {
44  //inverse_transformation_ = transformation_.inverse ();
45 }
46 
47 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
48 template<typename PointT>
49 pcl::BoxClipper3D<PointT>::BoxClipper3D (const Eigen::Vector3f& rodrigues, const Eigen::Vector3f& translation, const Eigen::Vector3f& box_size)
50 {
51  setTransformation (rodrigues, translation, box_size);
52 }
53 
54 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
55 template<typename PointT>
57 {
58 }
59 
60 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
61 template<typename PointT> void
62 pcl::BoxClipper3D<PointT>::setTransformation (const Eigen::Affine3f& transformation)
63 {
64  transformation_ = transformation;
65  //inverse_transformation_ = transformation_.inverse ();
66 }
67 
68 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
69 template<typename PointT> void
70 pcl::BoxClipper3D<PointT>::setTransformation (const Eigen::Vector3f& rodrigues, const Eigen::Vector3f& translation, const Eigen::Vector3f& box_size)
71 {
72  transformation_ = Eigen::Translation3f (translation) * Eigen::AngleAxisf(rodrigues.norm (), rodrigues.normalized ()) * Eigen::Scaling (box_size);
73  //inverse_transformation_ = transformation_.inverse ();
74 }
75 
76 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
77 template<typename PointT> pcl::Clipper3D<PointT>*
79 {
80  return new BoxClipper3D<PointT> (transformation_);
81 }
82 
83 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
84 template<typename PointT> void
85 pcl::BoxClipper3D<PointT>::transformPoint (const PointT& pointIn, PointT& pointOut) const
86 {
87  const Eigen::Vector4f& point = pointIn.getVector4fMap ();
88  pointOut.getVector4fMap () = transformation_ * point;
89 
90  // homogeneous value might not be 1
91  if (point [3] != 1)
92  {
93  // homogeneous component might be uninitialized -> invalid
94  if (point [3] != 0)
95  {
96  pointOut.x += (1 - point [3]) * transformation_.data () [ 9];
97  pointOut.y += (1 - point [3]) * transformation_.data () [10];
98  pointOut.z += (1 - point [3]) * transformation_.data () [11];
99  }
100  else
101  {
102  pointOut.x += transformation_.data () [ 9];
103  pointOut.y += transformation_.data () [10];
104  pointOut.z += transformation_.data () [11];
105  }
106  }
107 }
108 
109 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
110 // ToDo use product on point.getVector3fMap () and transformatio_.col (i) to use the SSE advantages of eigen
111 template<typename PointT> bool
113 {
114  return (fabs(transformation_.data () [ 0] * point.x +
115  transformation_.data () [ 3] * point.y +
116  transformation_.data () [ 6] * point.z +
117  transformation_.data () [ 9]) <= 1 &&
118  fabs(transformation_.data () [ 1] * point.x +
119  transformation_.data () [ 4] * point.y +
120  transformation_.data () [ 7] * point.z +
121  transformation_.data () [10]) <= 1 &&
122  fabs(transformation_.data () [ 2] * point.x +
123  transformation_.data () [ 5] * point.y +
124  transformation_.data () [ 8] * point.z +
125  transformation_.data () [11]) <= 1 );
126 }
127 
128 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
129 /**
130  * @attention untested code
131  */
132 template<typename PointT> bool
134 {
135  /*
136  PointT pt1, pt2;
137  transformPoint (point1, pt1);
138  transformPoint (point2, pt2);
139 
140  //
141  bool pt1InBox = (fabs(pt1.x) <= 1.0 && fabs (pt1.y) <= 1.0 && fabs (pt1.z) <= 1.0);
142  bool pt2InBox = (fabs(pt2.x) <= 1.0 && fabs (pt2.y) <= 1.0 && fabs (pt2.z) <= 1.0);
143 
144  // one is outside the other one inside the box
145  //if (pt1InBox ^ pt2InBox)
146  if (pt1InBox && !pt2InBox)
147  {
148  PointT diff;
149  PointT lambda;
150  diff.getVector3fMap () = pt2.getVector3fMap () - pt1.getVector3fMap ();
151 
152  if (diff.x > 0)
153  lambda.x = (1.0 - pt1.x) / diff.x;
154  else
155  lambda.x = (-1.0 - pt1.x) / diff.x;
156 
157  if (diff.y > 0)
158  lambda.y = (1.0 - pt1.y) / diff.y;
159  else
160  lambda.y = (-1.0 - pt1.y) / diff.y;
161 
162  if (diff.z > 0)
163  lambda.z = (1.0 - pt1.z) / diff.z;
164  else
165  lambda.z = (-1.0 - pt1.z) / diff.z;
166 
167  pt2 = pt1 + std::min(std::min(lambda.x, lambda.y), lambda.z) * diff;
168 
169  // inverse transformation
170  inverseTransformPoint (pt2, point2);
171  return true;
172  }
173  else if (!pt1InBox && pt2InBox)
174  {
175  return true;
176  }
177  */
178  return false;
179 }
180 
181 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
182 /**
183  * @attention untested code
184  */
185 template<typename PointT> void
186 pcl::BoxClipper3D<PointT>::clipPlanarPolygon3D (const std::vector<PointT>& polygon, std::vector<PointT>& clipped_polygon) const
187 {
188  // not implemented -> clip everything
189  clipped_polygon.clear ();
190 }
191 
192 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
193 /**
194  * @attention untested code
195  */
196 template<typename PointT> void
197 pcl::BoxClipper3D<PointT>::clipPlanarPolygon3D (std::vector<PointT>& polygon) const
198 {
199  // not implemented -> clip everything
200  polygon.clear ();
201 }
202 
203 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
204 // /ToDo: write fast version using eigen map and single matrix vector multiplication, that uses advantages of eigens SSE operations.
205 template<typename PointT> void
206 pcl::BoxClipper3D<PointT>::clipPointCloud3D (const pcl::PointCloud<PointT>& cloud_in, std::vector<int>& clipped, const std::vector<int>& indices) const
207 {
208  if (indices.empty ())
209  {
210  clipped.reserve (cloud_in.size ());
211  for (register unsigned pIdx = 0; pIdx < cloud_in.size (); ++pIdx)
212  if (clipPoint3D (cloud_in[pIdx]))
213  clipped.push_back (pIdx);
214  }
215  else
216  {
217  for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
218  if (clipPoint3D (cloud_in[*iIt]))
219  clipped.push_back (*iIt);
220  }
221 }
222 #endif //PCL_FILTERS_IMPL_BOX_CLIPPER3D_HPP