line.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2014, 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
39 #include <cfloat>
40 
41 namespace jsk_recognition_utils
42 {
43  Line::Line(const Eigen::Vector3f& direction, const Eigen::Vector3f& origin)
44  : direction_ (direction.normalized()), origin_(origin)
45  {
46 
47  }
48 
49  void Line::getDirection(Eigen::Vector3f& output) const
50  {
51  output = direction_;
52  }
53 
54  Eigen::Vector3f Line::getDirection() const
55  {
56  return direction_;
57  }
58 
59  void Line::getOrigin(Eigen::Vector3f& output) const
60  {
61  output = origin_;
62  }
63 
64  Eigen::Vector3f Line::getOrigin() const
65  {
66  return origin_;
67  }
68 
69  void Line::foot(const Eigen::Vector3f& point, Eigen::Vector3f& output) const
70  {
71  const double alpha = computeAlpha(point);
72  output = alpha * direction_ + origin_;
73  }
74 
76  const Eigen::Vector3f& from, Eigen::Vector3f& foot_point) const
77  {
78  foot(from, foot_point);
79  return (from - foot_point).norm();
80  }
81 
82  double Line::distanceToPoint(const Eigen::Vector3f& from) const
83  {
84  Eigen::Vector3f foot_point;
85  return distanceToPoint(from, foot_point);
86  }
87 
88  double Line::angle(const Line& other) const
89  {
90  double dot = fabs(direction_.dot(other.direction_));
91  if (dot > 1.0) {
92  return M_PI / 2.0;
93  }
94  else {
95  double theta = acos(dot);
96  if (theta > M_PI / 2.0) {
97  return M_PI / 2.0 - theta;
98  }
99  else {
100  return theta;
101  }
102  }
103  }
104 
105  bool Line::isParallel(const Line& other, double angle_threshold) const
106  {
107  return angle(other) < angle_threshold;
108  }
109 
110  bool Line::isPerpendicular(const Line& other, double angle_threshold) const
111  {
112  return (M_PI / 2.0 - angle(other)) < angle_threshold;
113  }
114 
115  bool Line::isSameDirection(const Line& other) const
116  {
117  return direction_.dot(other.direction_) > 0;
118  }
119 
121  {
122  Line::Ptr ret (new Line(-direction_, origin_));
123  return ret;
124  }
125 
126  Line::Ptr Line::midLine(const Line& other) const
127  {
128  Eigen::Vector3f new_directin = (direction_ + other.direction_).normalized();
129  Eigen::Vector3f new_origin;
130  other.foot(origin_, new_origin);
131  Line::Ptr ret (new Line(new_directin, (new_origin + origin_) / 2.0));
132  return ret;
133  }
134 
135  void Line::parallelLineNormal(const Line& other, Eigen::Vector3f& output)
136  const
137  {
138  Eigen::Vector3f foot_point;
139  other.foot(origin_, foot_point);
140  output = origin_ - foot_point;
141  }
142 
143  Line::Ptr Line::fromCoefficients(const std::vector<float>& coefficients)
144  {
145  Eigen::Vector3f p(coefficients[0],
146  coefficients[1],
147  coefficients[2]);
148  Eigen::Vector3f d(coefficients[3],
149  coefficients[4],
150  coefficients[5]);
151  Line::Ptr ret(new Line(d, p));
152  return ret;
153  }
154 
155  double Line::distance(const Line& other) const
156  {
157  Eigen::Vector3f v12 = (other.origin_ - origin_);
158  Eigen::Vector3f n = direction_.cross(other.direction_);
159  return fabs(n.dot(v12)) / n.norm();
160  }
161 
162  Line::Ptr Line::parallelLineOnAPoint(const Eigen::Vector3f& p) const
163  {
164  Line::Ptr ret (new Line(direction_, p));
165  return ret;
166  }
167 
168  double Line::computeAlpha(const Point& p) const
169  {
170  return p.dot(direction_) - origin_.dot(direction_);
171  }
172 
174  {
175  double min_alpha = DBL_MAX;
176  double max_alpha = - DBL_MAX;
177  Point min_alpha_point, max_alpha_point;
178  for (size_t i = 0; i < points.size(); i++) {
179  Point p = points[i];
180  double alpha = computeAlpha(p);
181  if (alpha > max_alpha) {
182  max_alpha_point = p;
183  max_alpha = alpha;
184  }
185  if (alpha < min_alpha) {
186  min_alpha_point = p;
187  min_alpha = alpha;
188  }
189  }
190  // ROS_INFO("min: %f", min_alpha);
191  // ROS_INFO("max: %f", max_alpha);
192  return boost::make_tuple<Point, Point>(min_alpha_point, max_alpha_point);
193  }
194 
195  void Line::print()
196  {
197  ROS_INFO("d: [%f, %f, %f], p: [%f, %f, %f]", direction_[0], direction_[1], direction_[2],
198  origin_[0], origin_[1], origin_[2]);
199  }
200 
201  void Line::point(double alpha, Eigen::Vector3f& output)
202  {
203  output = alpha * direction_ + origin_;
204  }
205 }
d
Definition: setup.py:9
virtual Line::Ptr flip()
Definition: line.cpp:120
virtual double distance(const Line &other) const
compute a distance to line.
Definition: line.cpp:155
virtual void foot(const Eigen::Vector3f &point, Eigen::Vector3f &output) const
compute a point which gives perpendicular projection.
Definition: line.cpp:69
virtual double angle(const Line &other) const
compute angle between a given line.
Definition: line.cpp:88
virtual double computeAlpha(const Point &p) const
Definition: line.cpp:168
virtual void print()
Print Line information.
Definition: line.cpp:195
virtual void point(double alpha, Eigen::Vector3f &ouptut)
Compute a point on normal from alpha parameter.
Definition: line.cpp:201
virtual PointPair findEndPoints(const Vertices &points) const
Extract end points from the points on the lines.
Definition: line.cpp:173
Eigen::Vector3f direction_
Definition: line.h:196
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
TFSIMD_FORCE_INLINE Vector3 normalized() const
virtual Ptr midLine(const Line &other) const
compute a middle line between given line.
Definition: line.cpp:126
#define ROS_INFO(...)
virtual Eigen::Vector3f getOrigin() const
get origin of the line.
Definition: line.cpp:64
virtual bool isPerpendicular(const Line &other, double angle_threshold=0.1) const
return true if given line is perpendicular. angle_threshold is error tolerance.
Definition: line.cpp:110
virtual double distanceToPoint(const Eigen::Vector3f &from) const
compute a distance to a point
Definition: line.cpp:82
static Ptr fromCoefficients(const std::vector< float > &coefficients)
Instantiate Line from array of float.
Definition: line.cpp:143
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices
Definition: types.h:48
virtual Ptr parallelLineOnAPoint(const Eigen::Vector3f &p) const
compute a line on a point, whose direction is same to the current line.
Definition: line.cpp:162
Line(const Eigen::Vector3f &direction, const Eigen::Vector3f &origin)
Construct a line from direction vector and a point on the line.
Definition: line.cpp:43
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
boost::tuple< Point, Point > PointPair
Definition: types.h:49
p
virtual bool isSameDirection(const Line &other) const
Definition: line.cpp:115
Eigen::Vector3f origin_
Definition: line.h:197
Eigen::Vector3f Point
Definition: types.h:45
virtual Eigen::Vector3f getDirection() const
get normalized direction vector of the line.
Definition: line.cpp:54
Class to represent 3-D straight line.
Definition: line.h:49
virtual bool isParallel(const Line &other, double angle_threshold=0.1) const
return true if given line is parallel. angle_threshold is error tolerance.
Definition: line.cpp:105
virtual void parallelLineNormal(const Line &other, Eigen::Vector3f &output) const
compute a perpendicular line of two lines from origin_
Definition: line.cpp:135


jsk_recognition_utils
Author(s):
autogenerated on Mon May 3 2021 03:03:03