segment.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 
40 namespace jsk_recognition_utils
41 {
42  Segment::Segment(const Eigen::Vector3f& from, const Eigen::Vector3f to):
43  Line(to - from, from), to_(to), length_((to - from).norm())
44  {
45  }
46 
47  double Segment::dividingRatio(const Eigen::Vector3f& point) const
48  {
49  if (to_[0] != origin_[0]) {
50  return (point[0] - origin_[0]) / (to_[0] - origin_[0]);
51  }
52  else if (to_[1] != origin_[1]) {
53  return (point[1] - origin_[1]) / (to_[1] - origin_[1]);
54  }
55  else {
56  return (point[2] - origin_[2]) / (to_[2] - origin_[2]);
57  }
58  }
59 
60  void Segment::foot(const Eigen::Vector3f& from, Eigen::Vector3f& output) const
61  {
62  Eigen::Vector3f foot_point;
63  Line::foot(from, foot_point);
64  double r = dividingRatio(foot_point);
65  if (r < 0.0) {
66  output = origin_;
67  }
68  else if (r > 1.0) {
69  output = to_;
70  }
71  else {
72  output = foot_point;
73  }
74  }
75 
76  double Segment::distance(const Eigen::Vector3f& point) const
77  {
78  Eigen::Vector3f foot_point;
79  return distance(point, foot_point);
80  }
81 
82  double Segment::distance(const Eigen::Vector3f& point,
83  Eigen::Vector3f& foot_point) const
84  {
85  foot(point, foot_point);
86  return (foot_point - point).norm();
87  }
88 
89  bool Segment::intersect(Plane& plane, Eigen::Vector3f& point) const
90  {
91  double x = - (plane.getNormal().dot(origin_) + plane.getD()) / (plane.getNormal().dot(direction_));
92  point = direction_ * x + origin_;
93  double r = dividingRatio(point);
94  return 0 <= r && r <= 1.0;
95  }
96 
97  void Segment::midpoint(Eigen::Vector3f& midpoint) const
98  {
99  midpoint = (origin_ + to_) * 0.5;
100  }
101 
102  std::ostream& operator<<(std::ostream& os, const Segment& seg)
103  {
104  os << "[" << seg.origin_[0] << ", " << seg.origin_[1] << ", " << seg.origin_[2] << "] -- "
105  << "[" << seg.to_[0] << ", " << seg.to_[1] << ", " << seg.to_[2] << "]";
106  }
107 
108  void Segment::getEnd(Eigen::Vector3f& output) const
109  {
110  output = to_;
111  }
112 
113  Eigen::Vector3f Segment::getEnd() const
114  {
115  return to_;
116  }
117 
118  double Segment::distanceWithInfo(const Eigen::Vector3f& from,
119  Eigen::Vector3f& foot_point,
120  double &distance_to_goal) const
121  {
122  const double alpha = computeAlpha(from);
123 
124  if (alpha >= 0 && alpha <= length_) {
125  // foot on the line
126  foot_point = alpha * direction_ + origin_;
127  distance_to_goal = length_ - alpha;
128  } else if (alpha < 0) {
129  // foot out of the line
130  foot_point = origin_;
131  distance_to_goal = length_;
132  } else {
133  foot_point = to_;
134  distance_to_goal = 0;
135  }
136  return (from - foot_point).norm();
137  }
138 
140  {
141  Segment::Ptr ret (new Segment(to_, origin_));
142  return ret;
143  }
144 
145  double Segment::length() const
146  {
147  return length_;
148  }
149 
150  void Segment::toMarker(visualization_msgs::Marker& marker) const
151  {
152  marker.type = visualization_msgs::Marker::ARROW;//
153 
154  geometry_msgs::Point st;
155  geometry_msgs::Point ed;
156  st.x = origin_[0];
157  st.y = origin_[1];
158  st.z = origin_[2];
159  ed.x = to_[0];
160  ed.y = to_[1];
161  ed.z = to_[2];
162 
163  marker.points.push_back(st);
164  marker.points.push_back(ed);
165 
166  marker.scale.x = 0.012;
167  marker.scale.y = 0.02;
168  marker.color.a = 1;
169  marker.color.r = 1;
170  marker.color.g = 1;
171  marker.color.b = 0;
172  }
173 
174  bool Segment::isCross (const Line &ln, double distance_threshold) const
175  {
176  Eigen::Vector3f ln_origin = ln.getOrigin();
177  Eigen::Vector3f ln_direction = ln.getDirection();
178  Eigen::Vector3f v12 = (ln_origin - origin_);
179  double n1n2 = ln_direction.dot(direction_);
180  if (fabs(n1n2) < 1e-20) { // parallel
181  return false;
182  }
183  double alp1 = (ln_direction.dot(v12) - (n1n2 * direction_.dot(v12))) / (1 - n1n2 * n1n2);
184  double alp2 = ((n1n2 * ln_direction.dot(v12)) - direction_.dot(v12)) / (1 - n1n2 * n1n2);
185 
186  if (// alp1 >= 0 && alp1 <= ln.length() &&
187  alp2 >= 0 && alp2 <= length_) {
188  Eigen::Vector3f p1 = alp1 * ln_direction + ln_origin;
189  Eigen::Vector3f p2 = alp2 * direction_ + origin_;
190  if ((p1 - p2).norm() < distance_threshold) {
191  return true;
192  } else {
193  return false;
194  }
195  }
196 
197  return false;
198  }
199 
200  bool Segment::isCross (const Segment &ln, double distance_threshold) const
201  {
202  Eigen::Vector3f ln_origin = ln.getOrigin();
203  Eigen::Vector3f ln_direction = ln.getDirection();
204  Eigen::Vector3f v12 = (ln_origin - origin_);
205  double n1n2 = ln_direction.dot(direction_);
206  if (fabs(n1n2) < 1e-20) { // parallel
207  return false;
208  }
209  double alp1 = (ln_direction.dot(v12) - (n1n2 * direction_.dot(v12))) / (1 - n1n2 * n1n2);
210  double alp2 = ((n1n2 * ln_direction.dot(v12)) - direction_.dot(v12)) / (1 - n1n2 * n1n2);
211 
212  if (alp1 >= 0 && alp1 <= ln.length() &&
213  alp2 >= 0 && alp2 <= length_) {
214  Eigen::Vector3f p1 = alp1 * ln_direction + ln_origin;
215  Eigen::Vector3f p2 = alp2 * direction_ + origin_;
216  if ((p1 - p2).norm() < distance_threshold) {
217  return true;
218  } else {
219  return false;
220  }
221  }
222 
223  return false;
224  }
225 }
virtual double distanceWithInfo(const Eigen::Vector3f &from, Eigen::Vector3f &foot_point, double &distance_to_goal) const
compute a distance to a point
Definition: segment.cpp:118
virtual double distance(const Eigen::Vector3f &point) const
Definition: segment.cpp:76
virtual void foot(const Eigen::Vector3f &point, Eigen::Vector3f &output) const
compute a point which gives perpendicular projection.
Definition: line.cpp:69
virtual double computeAlpha(const Point &p) const
Definition: line.cpp:168
virtual bool intersect(Plane &plane, Eigen::Vector3f &point) const
Definition: segment.cpp:89
virtual void getOrigin(Eigen::Vector3f &output) const
get origin of the line and assing it to output.
Definition: line.cpp:59
Segment(const Eigen::Vector3f &from, const Eigen::Vector3f to)
Construct a line from a start point and a goal point.
Definition: segment.cpp:42
virtual void point(double alpha, Eigen::Vector3f &ouptut)
Compute a point on normal from alpha parameter.
Definition: line.cpp:201
Eigen::Vector3f direction_
Definition: line.h:196
Class to represent 3-D straight line which has finite length.
Definition: segment.h:50
virtual void getDirection(Eigen::Vector3f &output) const
get normalized direction vector of the line and assign it to output.
Definition: line.cpp:49
void toMarker(visualization_msgs::Marker &marker) const
make marker message to display the finite line
Definition: segment.cpp:150
virtual Eigen::Vector3f getEnd() const
Definition: segment.cpp:113
r
virtual double getD()
Definition: plane.cpp:259
virtual void foot(const Eigen::Vector3f &point, Eigen::Vector3f &output) const
compute a point which gives perpendicular projection.
Definition: segment.cpp:60
virtual void midpoint(Eigen::Vector3f &midpoint) const
Definition: segment.cpp:97
virtual double dividingRatio(const Eigen::Vector3f &point) const
Definition: segment.cpp:47
virtual bool isCross(const Line &ln, double distance_threshold=1e-5) const
is crossing with another line
Definition: segment.cpp:174
friend std::ostream & operator<<(std::ostream &os, const Segment &seg)
Definition: segment.cpp:102
x
virtual Segment::Ptr flipSegment() const
return flipped line (line of opposite direction)
Definition: segment.cpp:139
virtual double length() const
return length of the line
Definition: segment.cpp:145
Eigen::Vector3f origin_
Definition: line.h:197
Class to represent 3-D straight line.
Definition: line.h:49
virtual Eigen::Vector3f getNormal()
Definition: plane.cpp:254


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