Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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>
59
60////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
61template<typename PointT> void
62pcl::BoxClipper3D<PointT>::setTransformation (const Eigen::Affine3f& transformation)
63{
64 transformation_ = transformation;
65 //inverse_transformation_ = transformation_.inverse ();
66}
67
68////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
69template<typename PointT> void
70pcl::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////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
77template<typename PointT> pcl::Clipper3D<PointT>*
79{
80 return new BoxClipper3D<PointT> (transformation_);
81}
82
83////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
84template<typename PointT> void
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////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
110template<typename PointT> bool
112{
113 Eigen::Vector4f point_coordinates (transformation_.matrix ()
114 * point.getVector4fMap ());
115 return (point_coordinates.array ().abs () <= 1).all ();
116}
117
118////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
119/**
120 * @attention untested code
121 */
122template<typename PointT> bool
124{
125 /*
126 PointT pt1, pt2;
127 transformPoint (point1, pt1);
128 transformPoint (point2, pt2);
129
130 //
131 bool pt1InBox = (std::abs(pt1.x) <= 1.0 && std::abs (pt1.y) <= 1.0 && std::abs (pt1.z) <= 1.0);
132 bool pt2InBox = (std::abs(pt2.x) <= 1.0 && std::abs (pt2.y) <= 1.0 && std::abs (pt2.z) <= 1.0);
133
134 // one is outside the other one inside the box
135 //if (pt1InBox ^ pt2InBox)
136 if (pt1InBox && !pt2InBox)
137 {
138 PointT diff;
139 PointT lambda;
140 diff.getVector3fMap () = pt2.getVector3fMap () - pt1.getVector3fMap ();
141
142 if (diff.x > 0)
143 lambda.x = (1.0 - pt1.x) / diff.x;
144 else
145 lambda.x = (-1.0 - pt1.x) / diff.x;
146
147 if (diff.y > 0)
148 lambda.y = (1.0 - pt1.y) / diff.y;
149 else
150 lambda.y = (-1.0 - pt1.y) / diff.y;
151
152 if (diff.z > 0)
153 lambda.z = (1.0 - pt1.z) / diff.z;
154 else
155 lambda.z = (-1.0 - pt1.z) / diff.z;
156
157 pt2 = pt1 + std::min(std::min(lambda.x, lambda.y), lambda.z) * diff;
158
159 // inverse transformation
160 inverseTransformPoint (pt2, point2);
161 return true;
162 }
163 else if (!pt1InBox && pt2InBox)
164 {
165 return true;
166 }
167 */
168 throw std::logic_error ("Not implemented");
169 return false;
170}
171
172////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
173/**
174 * @attention untested code
175 */
176template<typename PointT> void
177pcl::BoxClipper3D<PointT>::clipPlanarPolygon3D (const std::vector<PointT, Eigen::aligned_allocator<PointT> >&, std::vector<PointT, Eigen::aligned_allocator<PointT> >& clipped_polygon) const
178{
179 // not implemented -> clip everything
180 clipped_polygon.clear ();
181 throw std::logic_error ("Not implemented");
182}
183
184////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
185/**
186 * @attention untested code
187 */
188template<typename PointT> void
189pcl::BoxClipper3D<PointT>::clipPlanarPolygon3D (std::vector<PointT, Eigen::aligned_allocator<PointT> >& polygon) const
190{
191 // not implemented -> clip everything
192 polygon.clear ();
193 throw std::logic_error ("Not implemented");
194}
195
196////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
197// /ToDo: write fast version using eigen map and single matrix vector multiplication, that uses advantages of eigens SSE operations.
198template<typename PointT> void
200{
201 clipped.clear ();
202 if (indices.empty ())
203 {
204 clipped.reserve (cloud_in.size ());
205 for (std::size_t pIdx = 0; pIdx < cloud_in.size (); ++pIdx)
206 if (clipPoint3D (cloud_in[pIdx]))
207 clipped.push_back (pIdx);
208 }
209 else
210 {
211 for (const auto &index : indices)
212 if (clipPoint3D (cloud_in[index]))
213 clipped.push_back (index);
214 }
215}
216#endif //PCL_FILTERS_IMPL_BOX_CLIPPER3D_HPP
Implementation of a box clipper in 3D. Actually it allows affine transformations, thus any parallelep...
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
~BoxClipper3D() noexcept
virtual destructor
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.
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.
std::size_t size() const
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
A point structure representing Euclidean xyz coordinates, and the RGB color.