Point Cloud Library (PCL)  1.14.1-dev
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 }
45 
46 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
47 template<typename PointT>
48 pcl::BoxClipper3D<PointT>::BoxClipper3D (const Eigen::Vector3f& rodrigues, const Eigen::Vector3f& translation, const Eigen::Vector3f& box_size)
49 {
50  setTransformation (rodrigues, translation, box_size);
51 }
52 
53 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
54 template<typename PointT>
56 = default;
57 
58 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
59 template<typename PointT> void
60 pcl::BoxClipper3D<PointT>::setTransformation (const Eigen::Affine3f& transformation)
61 {
62  transformation_ = transformation;
63 }
64 
65 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
66 template<typename PointT> void
67 pcl::BoxClipper3D<PointT>::setTransformation (const Eigen::Vector3f& rodrigues, const Eigen::Vector3f& translation, const Eigen::Vector3f& box_size)
68 {
69  transformation_ = (Eigen::Translation3f (translation) * Eigen::AngleAxisf(rodrigues.norm (), rodrigues.normalized ()) * Eigen::Scaling (0.5f * box_size)).inverse ();
70 }
71 
72 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
73 template<typename PointT> pcl::Clipper3D<PointT>*
75 {
76  return new BoxClipper3D<PointT> (transformation_);
77 }
78 
79 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
80 template<typename PointT> void
81 pcl::BoxClipper3D<PointT>::transformPoint (const PointT& pointIn, PointT& pointOut) const
82 {
83  const Eigen::Vector4f& point = pointIn.getVector4fMap ();
84  pointOut.getVector4fMap () = transformation_ * point;
85 
86  // homogeneous value might not be 1
87  if (point [3] != 1)
88  {
89  // homogeneous component might be uninitialized -> invalid
90  if (point [3] != 0)
91  {
92  pointOut.x += (1 - point [3]) * transformation_.data () [ 9];
93  pointOut.y += (1 - point [3]) * transformation_.data () [10];
94  pointOut.z += (1 - point [3]) * transformation_.data () [11];
95  }
96  else
97  {
98  pointOut.x += transformation_.data () [ 9];
99  pointOut.y += transformation_.data () [10];
100  pointOut.z += transformation_.data () [11];
101  }
102  }
103 }
104 
105 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
106 template<typename PointT> bool
108 {
109  Eigen::Vector4f point_coordinates (transformation_.matrix ()
110  * point.getVector4fMap ());
111  return (point_coordinates.array ().abs () <= 1).all ();
112 }
113 
114 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
115 /**
116  * @attention untested code
117  */
118 template<typename PointT> bool
120 {
121  /*
122  PointT pt1, pt2;
123  transformPoint (point1, pt1);
124  transformPoint (point2, pt2);
125 
126  //
127  bool pt1InBox = (std::abs(pt1.x) <= 1.0 && std::abs (pt1.y) <= 1.0 && std::abs (pt1.z) <= 1.0);
128  bool pt2InBox = (std::abs(pt2.x) <= 1.0 && std::abs (pt2.y) <= 1.0 && std::abs (pt2.z) <= 1.0);
129 
130  // one is outside the other one inside the box
131  //if (pt1InBox ^ pt2InBox)
132  if (pt1InBox && !pt2InBox)
133  {
134  PointT diff;
135  PointT lambda;
136  diff.getVector3fMap () = pt2.getVector3fMap () - pt1.getVector3fMap ();
137 
138  if (diff.x > 0)
139  lambda.x = (1.0 - pt1.x) / diff.x;
140  else
141  lambda.x = (-1.0 - pt1.x) / diff.x;
142 
143  if (diff.y > 0)
144  lambda.y = (1.0 - pt1.y) / diff.y;
145  else
146  lambda.y = (-1.0 - pt1.y) / diff.y;
147 
148  if (diff.z > 0)
149  lambda.z = (1.0 - pt1.z) / diff.z;
150  else
151  lambda.z = (-1.0 - pt1.z) / diff.z;
152 
153  pt2 = pt1 + std::min(std::min(lambda.x, lambda.y), lambda.z) * diff;
154 
155  // inverse transformation
156  inverseTransformPoint (pt2, point2);
157  return true;
158  }
159  else if (!pt1InBox && pt2InBox)
160  {
161  return true;
162  }
163  */
164  throw std::logic_error ("Not implemented");
165  return false;
166 }
167 
168 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
169 /**
170  * @attention untested code
171  */
172 template<typename PointT> void
173 pcl::BoxClipper3D<PointT>::clipPlanarPolygon3D (const std::vector<PointT, Eigen::aligned_allocator<PointT> >&, std::vector<PointT, Eigen::aligned_allocator<PointT> >& clipped_polygon) const
174 {
175  // not implemented -> clip everything
176  clipped_polygon.clear ();
177  throw std::logic_error ("Not implemented");
178 }
179 
180 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
181 /**
182  * @attention untested code
183  */
184 template<typename PointT> void
185 pcl::BoxClipper3D<PointT>::clipPlanarPolygon3D (std::vector<PointT, Eigen::aligned_allocator<PointT> >& polygon) const
186 {
187  // not implemented -> clip everything
188  polygon.clear ();
189  throw std::logic_error ("Not implemented");
190 }
191 
192 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
193 // /ToDo: write fast version using eigen map and single matrix vector multiplication, that uses advantages of eigens SSE operations.
194 template<typename PointT> void
196 {
197  clipped.clear ();
198  if (indices.empty ())
199  {
200  clipped.reserve (cloud_in.size ());
201  for (std::size_t pIdx = 0; pIdx < cloud_in.size (); ++pIdx)
202  if (clipPoint3D (cloud_in[pIdx]))
203  clipped.push_back (pIdx);
204  }
205  else
206  {
207  for (const auto &index : indices)
208  if (clipPoint3D (cloud_in[index]))
209  clipped.push_back (index);
210  }
211 }
212 #endif //PCL_FILTERS_IMPL_BOX_CLIPPER3D_HPP
Implementation of a box clipper in 3D. Actually it allows affine transformations, thus any parallelep...
Definition: box_clipper3D.h:55
void transformPoint(const PointT &pointIn, PointT &pointOut) const
Clipper3D< PointT > * clone() const override
polymorphic method to clone the underlying clipper with its parameters.
bool clipLineSegment3D(PointT &from, PointT &to) const override
bool clipPoint3D(const PointT &point) const override
interface to clip a single point
BoxClipper3D(const Eigen::Affine3f &transformation)
Constructor taking an affine transformation matrix, which allows also shearing of the clipping area.
void clipPlanarPolygon3D(std::vector< PointT, Eigen::aligned_allocator< PointT > > &polygon) const override
void setTransformation(const Eigen::Affine3f &transformation)
Set the affine transformation.
~BoxClipper3D() noexcept override
virtual destructor
void clipPointCloud3D(const pcl::PointCloud< PointT > &cloud_in, Indices &clipped, const Indices &indices=Indices()) const override
interface to clip a point cloud
Base class for 3D clipper objects.
Definition: clipper3D.h:55
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
std::size_t size() const
Definition: point_cloud.h:443
Definition: bfgs.h:10
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
A point structure representing Euclidean xyz coordinates, and the RGB color.