polyline.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2017, 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  PolyLine::PolyLine(const std::vector < Eigen::Vector3f > &points) : Line(points[points.size()-1] - points[0], points[0])
44  {
45  int n = points.size();
46  segments.resize(n-1);
47  for (int i = 0; i < n-1; i++) {
48  Segment::Ptr ln(new Segment(points[i], points[i+1]));
49  segments[i] = ln;
50  }
51  }
52 
53  Segment::Ptr PolyLine::at(int index) const
54  {
55  return segments.at(index);
56  }
57 
58  double PolyLine::distanceWithInfo(const Eigen::Vector3f& from,
59  Eigen::Vector3f& foot_point,
60  double& distance_to_goal,
61  int& foot_index,
62  double& foot_alpha) const
63  {
64  double min_len = DBL_MAX;
65  Eigen::Vector3f point;
66  double from_start_to_foot = 0;
67  double distance_from_start = 0;
68 
69  for(int i = 0; i < segments.size(); i++) {
70  double to_goal;
71  double dist = segments[i]->distanceWithInfo(from, point, to_goal);
72  if (dist < min_len) {
73  min_len = dist;
74  foot_point = point;
75  foot_index = i;
76  foot_alpha = (segments[i]->length() - to_goal);
77  from_start_to_foot = distance_from_start + foot_alpha;
78  }
79  distance_from_start += segments[i]->length();
80  }
81  distance_to_goal = distance_from_start - from_start_to_foot;
82  return min_len;
83  }
84 
85  double PolyLine::distance(const Eigen::Vector3f& from) const
86  {
87  double gl, alp;
88  int idx;
89  Eigen::Vector3f p;
90  distanceWithInfo(from, p, gl, idx, alp);
91  }
92 
93  double PolyLine::distance(const Eigen::Vector3f& from,
94  Eigen::Vector3f& foot_point) const
95  {
96  double gl, alp;
97  int idx;
98  distanceWithInfo(from, foot_point, gl, idx, alp);
99  }
100 
101  void PolyLine::getDirection(int index, Eigen::Vector3f& output) const
102  {
103  segments[index]->getDirection(output);
104  }
105  Eigen::Vector3f PolyLine::getDirection(int index) const
106  {
107  Eigen::Vector3f dir;
108  getDirection(index, dir);
109  return dir;
110  }
111 
112  double PolyLine::length() const
113  {
114  double distance_from_start = 0;
115  for(int i = 0; i < segments.size(); i++) {
116  distance_from_start += segments[i]->length();
117  }
118  return distance_from_start;
119  }
120 
122  {
123  PolyLine::Ptr ret;
124  return ret;
125  }
126 
127  void PolyLine::toMarker(visualization_msgs::Marker& marker) const
128  {
129  marker.type = visualization_msgs::Marker::LINE_STRIP;
130 
131  marker.scale.x = 0.02; // line width
132  marker.color.a = 1.0;
133  marker.color.r = 0.0;
134  marker.color.g = 1.0;
135  marker.color.b = 1.0;
136 
137  marker.pose.position.x = 0;
138  marker.pose.position.y = 0;
139  marker.pose.position.z = 0;
140  marker.pose.orientation.x = 0;
141  marker.pose.orientation.y = 0;
142  marker.pose.orientation.z = 0;
143  marker.pose.orientation.w = 1;
144 
145  marker.points.resize(0);
146  for(int i = 0; i < segments.size(); i++) {
147  Eigen::Vector3f p;
148  segments[i]->getOrigin(p);
149  geometry_msgs::Point pt;
150  pt.x = p[0];
151  pt.y = p[1];
152  pt.z = p[2];
153  marker.points.push_back(pt);
154  }
155  {
156  Eigen::Vector3f p;
157  segments[segments.size() - 1]->getEnd(p);
158  geometry_msgs::Point pt;
159  pt.x = p[0];
160  pt.y = p[1];
161  pt.z = p[2];
162  marker.points.push_back(pt);
163  }
164  }
165 
166  std::ostream& operator<<(std::ostream& os, const PolyLine& pl)
167  {
168  os << "[" << pl.origin_[0];
169  os << ", " << pl.origin_[1];
170  os << ", " << pl.origin_[2] << "]";
171 
172  for (int i = 0; i < pl.segments.size(); i++) {
173  Eigen::Vector3f p;
174  pl.segments[i]->getEnd(p);
175  os << " -- [" << p[0];
176  os << ", " << p[1];
177  os << ", " << p[2] << "]";
178  }
179  return os;
180  }
181 }
friend std::ostream & operator<<(std::ostream &os, const PolyLine &pl)
Definition: polyline.cpp:166
virtual double distanceWithInfo(const Eigen::Vector3f &from, Eigen::Vector3f &foot_point, double &distance_to_goal, int &foot_index, double &foot_alpha) const
compute a distance to a point, get various information
Definition: polyline.cpp:58
std::vector< Segment::Ptr > segments
Definition: polyline.h:118
virtual PolyLine::Ptr flipPolyLine() const
Definition: polyline.cpp:121
void toMarker(visualization_msgs::Marker &marker) const
Definition: polyline.cpp:127
PolyLine(const std::vector< Eigen::Vector3f > &points)
Construct a polyline from points. The polyline consists of lines which starts with p[i] and ends with...
Definition: polyline.cpp:43
virtual void point(double alpha, Eigen::Vector3f &ouptut)
Compute a point on normal from alpha parameter.
Definition: line.cpp:201
Class to represent 3-D polyline (not closed).
Definition: polyline.h:51
Class to represent 3-D straight line which has finite length.
Definition: segment.h:50
virtual double length() const
get total length of the polyline
Definition: polyline.cpp:112
virtual Segment::Ptr at(int index) const
Definition: polyline.cpp:53
virtual double distance(const Eigen::Vector3f &point, Eigen::Vector3f &foot_point) const
compute a distance to a point
Definition: polyline.cpp:93
p
Eigen::Vector3f origin_
Definition: line.h:197
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


jsk_recognition_utils
Author(s):
autogenerated on Tue Nov 24 2020 04:06:11