plane.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 #define BOOST_PARAMETER_MAX_ARITY 7
37 
41 
42 namespace jsk_recognition_utils
43 {
44  Plane::Plane(const std::vector<float>& coefficients)
45  {
46  normal_ = Eigen::Vector3f(coefficients[0], coefficients[1], coefficients[2]);
47  d_ = coefficients[3] / normal_.norm();
48  normal_.normalize();
50  }
51 
52  Plane::Plane(const boost::array<float, 4>& coefficients)
53  {
54  normal_ = Eigen::Vector3f(coefficients[0], coefficients[1], coefficients[2]);
55  d_ = coefficients[3] / normal_.norm();
56  normal_.normalize();
58  }
59 
60  Plane::Plane(Eigen::Vector3f normal, double d) :
61  normal_(normal.normalized()), d_(d / normal.norm())
62  {
64  }
65 
66  Plane::Plane(Eigen::Vector3f normal, Eigen::Vector3f p) :
67  normal_(normal.normalized()), d_(- normal.dot(p) / normal.norm())
68  {
70  }
71 
72 
74  {
75 
76  }
77 
78  Eigen::Vector3f Plane::getPointOnPlane()
79  {
80  Eigen::Vector3f x = normal_ / (normal_.norm() * normal_.norm()) * (- d_);
81  return x;
82  }
83 
85  {
86  return Plane(- normal_, - d_);
87  }
88 
90  {
91  Eigen::Vector3f p = getPointOnPlane();
92  Eigen::Vector3f n = getNormal();
93 
94  if (p.dot(n) < 0) {
95  return Plane::Ptr (new Plane(normal_, d_));
96  }
97  else {
98  return Plane::Ptr (new Plane(- normal_, - d_));
99  }
100  }
101 
102  bool Plane::isSameDirection(const Plane& another)
103  {
104  return isSameDirection(another.normal_);
105  }
106 
107  bool Plane::isSameDirection(const Eigen::Vector3f& another_normal)
108  {
109  return normal_.dot(another_normal) > 0;
110  }
111 
112  double Plane::signedDistanceToPoint(const Eigen::Vector3f p)
113  {
114  return (normal_.dot(p) + d_);
115  }
116 
117  double Plane::signedDistanceToPoint(const Eigen::Vector4f p)
118  {
119  return signedDistanceToPoint(Eigen::Vector3f(p[0], p[1], p[2]));
120  }
121 
122  double Plane::distanceToPoint(const Eigen::Vector4f p)
123  {
124  return fabs(signedDistanceToPoint(p));
125  }
126 
127  double Plane::distanceToPoint(const Eigen::Vector3f p)
128  {
129  return fabs(signedDistanceToPoint(p));
130  }
131 
132  double Plane::distance(const Plane& another)
133  {
134  return fabs(fabs(d_) - fabs(another.d_));
135  }
136 
137  double Plane::angle(const Eigen::Vector3f& vector)
138  {
139  double dot = normal_.dot(vector);
140  if (dot > 1.0) {
141  dot = 1.0;
142  }
143  else if (dot < -1.0) {
144  dot = -1.0;
145  }
146  double theta = acos(dot);
147  if (theta > M_PI / 2.0) {
148  return M_PI - theta;
149  }
150 
151  return acos(dot);
152  }
153 
154  double Plane::angle(const Plane& another)
155  {
156  double dot = normal_.dot(another.normal_);
157  if (dot > 1.0) {
158  dot = 1.0;
159  }
160  else if (dot < -1.0) {
161  dot = -1.0;
162  }
163  double theta = acos(dot);
164  if (theta > M_PI / 2.0) {
165  return M_PI - theta;
166  }
167 
168  return acos(dot);
169  }
170 
171  void Plane::project(const Eigen::Vector3f& p, Eigen::Vector3f& output)
172  {
173  // double alpha = - p.dot(normal_);
174  // output = p + alpha * normal_;
175  double alpha = p.dot(normal_) + d_;
176  //double alpha = p.dot(normal_) - d_;
177  output = p - alpha * normal_;
178  }
179 
180  void Plane::project(const Eigen::Vector3d& p, Eigen::Vector3d& output)
181  {
182  Eigen::Vector3f output_f;
183  project(Eigen::Vector3f(p[0], p[1], p[2]), output_f);
184  pointFromVectorToVector<Eigen::Vector3f, Eigen::Vector3d>(output_f, output);
185  }
186 
187  void Plane::project(const Eigen::Vector3d& p, Eigen::Vector3f& output)
188  {
189  project(Eigen::Vector3f(p[0], p[1], p[2]), output);
190  }
191 
192  void Plane::project(const Eigen::Vector3f& p, Eigen::Vector3d& output)
193  {
194  Eigen::Vector3f output_f;
195  project(p, output);
196  pointFromVectorToVector<Eigen::Vector3f, Eigen::Vector3d>(output_f, output);
197  }
198 
199  void Plane::project(const Eigen::Affine3d& pose, Eigen::Affine3d& output)
200  {
201  Eigen::Affine3f pose_f, output_f;
202  convertEigenAffine3(pose, pose_f);
203  project(pose_f, output_f);
204  convertEigenAffine3(output_f, output);
205  }
206 
207  void Plane::project(const Eigen::Affine3f& pose, Eigen::Affine3f& output)
208  {
209  Eigen::Vector3f p(pose.translation());
210  Eigen::Vector3f output_p;
211  project(p, output_p);
212  Eigen::Quaternionf rot;
213  rot.setFromTwoVectors(pose.rotation() * Eigen::Vector3f::UnitZ(),
214  coordinates().rotation() * Eigen::Vector3f::UnitZ());
215  output = Eigen::Affine3f::Identity() * Eigen::Translation3f(output_p) * rot;
216  }
217 
218  Plane Plane::transform(const Eigen::Affine3f& transform)
219  {
220  Eigen::Affine3d transform_d;
221  convertEigenAffine3(transform, transform_d);
222  return this->transform(transform_d);
223  }
224 
225  Plane Plane::transform(const Eigen::Affine3d& transform)
226  {
227  Eigen::Vector4d n;
228  n[0] = normal_[0];
229  n[1] = normal_[1];
230  n[2] = normal_[2];
231  n[3] = d_;
232  Eigen::Matrix4d m = transform.matrix();
233  Eigen::Vector4d n_d = m.transpose() * n;
234  //Eigen::Vector4d n_dd = n_d.normalized();
235  Eigen::Vector4d n_dd = n_d / sqrt(n_d[0] * n_d[0] + n_d[1] * n_d[1] + n_d[2] * n_d[2]);
236  return Plane(Eigen::Vector3f(n_dd[0], n_dd[1], n_dd[2]), n_dd[3]);
237  }
238 
239  std::vector<float> Plane::toCoefficients()
240  {
241  std::vector<float> ret;
242  toCoefficients(ret);
243  return ret;
244  }
245 
246  void Plane::toCoefficients(std::vector<float>& output)
247  {
248  output.push_back(normal_[0]);
249  output.push_back(normal_[1]);
250  output.push_back(normal_[2]);
251  output.push_back(d_);
252  }
253 
254  Eigen::Vector3f Plane::getNormal()
255  {
256  return normal_;
257  }
258 
259  double Plane::getD()
260  {
261  return d_;
262  }
263 
265  {
266  Eigen::Quaternionf rot;
267  rot.setFromTwoVectors(Eigen::Vector3f::UnitZ(), normal_);
268  double c = normal_[2];
269  double z = 0.0;
270  // ax + by + cz + d = 0
271  // z = - d / c (when x = y = 0)
272  if (c == 0.0) { // its not good
273  z = 0.0;
274  }
275  else {
276  z = - d_ / c;
277  }
279  = Eigen::Affine3f::Identity() * Eigen::Translation3f(0, 0, z) * rot;
280  }
281 
282  Eigen::Affine3f Plane::coordinates()
283  {
284  return plane_coordinates_;
285  }
286 
287 }
d
Definition: setup.py:9
Eigen::Affine3f plane_coordinates_
Definition: plane.h:85
virtual Eigen::Vector3f getPointOnPlane()
Definition: plane.cpp:78
virtual bool isSameDirection(const Plane &another)
Definition: plane.cpp:102
virtual Plane::Ptr faceToOrigin()
Definition: plane.cpp:89
virtual double angle(const Plane &another)
Definition: plane.cpp:154
virtual Plane transform(const Eigen::Affine3d &transform)
Definition: plane.cpp:225
virtual Eigen::Affine3f coordinates()
Definition: plane.cpp:282
virtual void initializeCoordinates()
Definition: plane.cpp:264
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
Plane(const std::vector< float > &coefficients)
Definition: plane.cpp:44
TFSIMD_FORCE_INLINE Vector3 normalized() const
virtual double getD()
Definition: plane.cpp:259
virtual std::vector< float > toCoefficients()
Definition: plane.cpp:239
boost::shared_ptr< Plane > Ptr
Definition: plane.h:50
virtual Plane flip()
Definition: plane.cpp:84
void convertEigenAffine3(const Eigen::Affine3d &from, Eigen::Affine3f &to)
x
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
virtual void project(const Eigen::Vector3f &p, Eigen::Vector3f &output)
Definition: plane.cpp:171
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
virtual double signedDistanceToPoint(const Eigen::Vector4f p)
Definition: plane.cpp:117
p
virtual double distance(const Plane &another)
Definition: plane.cpp:132
Eigen::Vector3f normal_
Definition: plane.h:83
virtual Eigen::Vector3f getNormal()
Definition: plane.cpp:254
virtual double distanceToPoint(const Eigen::Vector4f p)
Definition: plane.cpp:122
z


jsk_recognition_utils
Author(s):
autogenerated on Wed May 27 2020 03:59:48