Point Cloud Library (PCL) 1.13.0
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
40template<typename PointT>
41pcl::BoxClipper3D<PointT>::BoxClipper3D (const Eigen::Affine3f& transformation)
42: transformation_ (transformation)
43{
44 //inverse_transformation_ = transformation_.inverse ();
45}
46
47////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
48template<typename PointT>
49pcl::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////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
55template<typename PointT>
57= default;
58
59////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
60template<typename PointT> void
61pcl::BoxClipper3D<PointT>::setTransformation (const Eigen::Affine3f& transformation)
62{
63 transformation_ = transformation;
64 //inverse_transformation_ = transformation_.inverse ();
65}
66
67////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
68template<typename PointT> void
69pcl::BoxClipper3D<PointT>::setTransformation (const Eigen::Vector3f& rodrigues, const Eigen::Vector3f& translation, const Eigen::Vector3f& box_size)
70{
71 transformation_ = Eigen::Translation3f (translation) * Eigen::AngleAxisf(rodrigues.norm (), rodrigues.normalized ()) * Eigen::Scaling (box_size);
72 //inverse_transformation_ = transformation_.inverse ();
73}
74
75////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
76template<typename PointT> pcl::Clipper3D<PointT>*
78{
79 return new BoxClipper3D<PointT> (transformation_);
80}
81
82////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
83template<typename PointT> void
85{
86 const Eigen::Vector4f& point = pointIn.getVector4fMap ();
87 pointOut.getVector4fMap () = transformation_ * point;
88
89 // homogeneous value might not be 1
90 if (point [3] != 1)
91 {
92 // homogeneous component might be uninitialized -> invalid
93 if (point [3] != 0)
94 {
95 pointOut.x += (1 - point [3]) * transformation_.data () [ 9];
96 pointOut.y += (1 - point [3]) * transformation_.data () [10];
97 pointOut.z += (1 - point [3]) * transformation_.data () [11];
98 }
99 else
100 {
101 pointOut.x += transformation_.data () [ 9];
102 pointOut.y += transformation_.data () [10];
103 pointOut.z += transformation_.data () [11];
104 }
105 }
106}
107
108////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
109template<typename PointT> bool
111{
112 Eigen::Vector4f point_coordinates (transformation_.matrix ()
113 * point.getVector4fMap ());
114 return (point_coordinates.array ().abs () <= 1).all ();
115}
116
117////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
118/**
119 * @attention untested code
120 */
121template<typename PointT> bool
123{
124 /*
125 PointT pt1, pt2;
126 transformPoint (point1, pt1);
127 transformPoint (point2, pt2);
128
129 //
130 bool pt1InBox = (std::abs(pt1.x) <= 1.0 && std::abs (pt1.y) <= 1.0 && std::abs (pt1.z) <= 1.0);
131 bool pt2InBox = (std::abs(pt2.x) <= 1.0 && std::abs (pt2.y) <= 1.0 && std::abs (pt2.z) <= 1.0);
132
133 // one is outside the other one inside the box
134 //if (pt1InBox ^ pt2InBox)
135 if (pt1InBox && !pt2InBox)
136 {
137 PointT diff;
138 PointT lambda;
139 diff.getVector3fMap () = pt2.getVector3fMap () - pt1.getVector3fMap ();
140
141 if (diff.x > 0)
142 lambda.x = (1.0 - pt1.x) / diff.x;
143 else
144 lambda.x = (-1.0 - pt1.x) / diff.x;
145
146 if (diff.y > 0)
147 lambda.y = (1.0 - pt1.y) / diff.y;
148 else
149 lambda.y = (-1.0 - pt1.y) / diff.y;
150
151 if (diff.z > 0)
152 lambda.z = (1.0 - pt1.z) / diff.z;
153 else
154 lambda.z = (-1.0 - pt1.z) / diff.z;
155
156 pt2 = pt1 + std::min(std::min(lambda.x, lambda.y), lambda.z) * diff;
157
158 // inverse transformation
159 inverseTransformPoint (pt2, point2);
160 return true;
161 }
162 else if (!pt1InBox && pt2InBox)
163 {
164 return true;
165 }
166 */
167 throw std::logic_error ("Not implemented");
168 return false;
169}
170
171////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
172/**
173 * @attention untested code
174 */
175template<typename PointT> void
176pcl::BoxClipper3D<PointT>::clipPlanarPolygon3D (const std::vector<PointT, Eigen::aligned_allocator<PointT> >&, std::vector<PointT, Eigen::aligned_allocator<PointT> >& clipped_polygon) const
177{
178 // not implemented -> clip everything
179 clipped_polygon.clear ();
180 throw std::logic_error ("Not implemented");
181}
182
183////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
184/**
185 * @attention untested code
186 */
187template<typename PointT> void
188pcl::BoxClipper3D<PointT>::clipPlanarPolygon3D (std::vector<PointT, Eigen::aligned_allocator<PointT> >& polygon) const
189{
190 // not implemented -> clip everything
191 polygon.clear ();
192 throw std::logic_error ("Not implemented");
193}
194
195////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
196// /ToDo: write fast version using eigen map and single matrix vector multiplication, that uses advantages of eigens SSE operations.
197template<typename PointT> void
199{
200 clipped.clear ();
201 if (indices.empty ())
202 {
203 clipped.reserve (cloud_in.size ());
204 for (std::size_t pIdx = 0; pIdx < cloud_in.size (); ++pIdx)
205 if (clipPoint3D (cloud_in[pIdx]))
206 clipped.push_back (pIdx);
207 }
208 else
209 {
210 for (const auto &index : indices)
211 if (clipPoint3D (cloud_in[index]))
212 clipped.push_back (index);
213 }
214}
215#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:54
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.