cube.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 #define BOOST_PARAMETER_MAX_ARITY 7
36 
39 
40 namespace jsk_recognition_utils
41 {
42  Cube::Cube(const Eigen::Vector3f& pos, const Eigen::Quaternionf& rot):
43  pos_(pos), rot_(rot)
44  {
45  dimensions_.resize(3);
46  }
47 
48  Cube::Cube(const Eigen::Vector3f& pos, const Eigen::Quaternionf& rot,
49  const std::vector<double>& dimensions):
50  pos_(pos), rot_(rot), dimensions_(dimensions)
51  {
52 
53  }
54  Cube::Cube(const Eigen::Vector3f& pos, const Eigen::Quaternionf& rot,
55  const Eigen::Vector3f& dimensions):
56  pos_(pos), rot_(rot)
57  {
58  dimensions_.resize(3);
59  dimensions_[0] = dimensions[0];
60  dimensions_[1] = dimensions[1];
61  dimensions_[2] = dimensions[2];
62  }
63 
64  Cube::Cube(const Eigen::Vector3f& pos,
65  const Line& line_a, const Line& line_b, const Line& line_c)
66  {
67  double distance_a_b = line_a.distance(line_b);
68  double distance_a_c = line_a.distance(line_c);
69  double distance_b_c = line_b.distance(line_c);
70  Line::Ptr axis;
71  dimensions_.resize(3);
72  Eigen::Vector3f ex, ey, ez;
73  if (distance_a_b >= distance_a_c &&
74  distance_a_b >= distance_b_c) {
75  axis = line_a.midLine(line_b);
76  line_a.parallelLineNormal(line_c, ex);
77  line_c.parallelLineNormal(line_b, ey);
78 
79  }
80  else if (distance_a_c >= distance_a_b &&
81  distance_a_c >= distance_b_c) {
82  axis = line_a.midLine(line_c);
83  line_a.parallelLineNormal(line_b, ex);
84  line_b.parallelLineNormal(line_c, ey);
85  }
86  else {
87  // else if (distance_b_c >= distance_a_b &&
88  // distance_b_c >= distance_a_c) {
89  axis = line_b.midLine(line_c);
90  line_b.parallelLineNormal(line_a, ex);
91  line_a.parallelLineNormal(line_c, ey);
92  }
93  dimensions_[0] = ex.norm();
94  dimensions_[1] = ey.norm();
95  axis->getDirection(ez);
96  ez.normalize();
97  ex.normalize();
98  ey.normalize();
99  if (ex.cross(ey).dot(ez) < 0) {
100  ez = - ez;
101  }
102  rot_ = rotFrom3Axis(ex, ey, ez);
103  axis->foot(pos, pos_); // project
104  }
105 
106  Cube::Cube(const jsk_recognition_msgs::BoundingBox& box)
107  {
108  dimensions_.resize(3);
109  dimensions_[0] = box.dimensions.x;
110  dimensions_[1] = box.dimensions.y;
111  dimensions_[2] = box.dimensions.z;
112  Eigen::Affine3f pose;
113  tf::poseMsgToEigen(box.pose, pose);
114  pos_ = Eigen::Vector3f(pose.translation());
115  rot_ = Eigen::Quaternionf(pose.rotation()); // slow
116  }
117 
119  {
120 
121  }
122 
123  std::vector<Segment::Ptr> Cube::edges()
124  {
125  std::vector<Segment::Ptr> ret;
126  Eigen::Vector3f A = pos_
127  + rot_ * ((+ dimensions_[0] * Eigen::Vector3f::UnitX()) +
128  (- dimensions_[1] * Eigen::Vector3f::UnitY()) +
129  (+ dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0;
130  Eigen::Vector3f B = pos_
131  + rot_ * ((+ dimensions_[0] * Eigen::Vector3f::UnitX()) +
132  (+ dimensions_[1] * Eigen::Vector3f::UnitY()) +
133  (+ dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0;
134  Eigen::Vector3f C = pos_
135  + rot_ * ((- dimensions_[0] * Eigen::Vector3f::UnitX()) +
136  (+ dimensions_[1] * Eigen::Vector3f::UnitY()) +
137  (+ dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0;
138  Eigen::Vector3f D = pos_
139  + rot_ * ((- dimensions_[0] * Eigen::Vector3f::UnitX()) +
140  (- dimensions_[1] * Eigen::Vector3f::UnitY()) +
141  (+ dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0;
142  Eigen::Vector3f E = pos_
143  + rot_ * ((+ dimensions_[0] * Eigen::Vector3f::UnitX()) +
144  (- dimensions_[1] * Eigen::Vector3f::UnitY()) +
145  (- dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0;
146  Eigen::Vector3f F = pos_
147  + rot_ * ((+ dimensions_[0] * Eigen::Vector3f::UnitX()) +
148  (+ dimensions_[1] * Eigen::Vector3f::UnitY()) +
149  (- dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0;
150  Eigen::Vector3f G = pos_
151  + rot_ * ((- dimensions_[0] * Eigen::Vector3f::UnitX()) +
152  (+ dimensions_[1] * Eigen::Vector3f::UnitY()) +
153  (- dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0;
154  Eigen::Vector3f H = pos_
155  + rot_ * ((- dimensions_[0] * Eigen::Vector3f::UnitX()) +
156  (- dimensions_[1] * Eigen::Vector3f::UnitY()) +
157  (- dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0;
158 
159  ret.push_back(Segment::Ptr(new Segment(A, B)));
160  ret.push_back(Segment::Ptr(new Segment(B, C)));
161  ret.push_back(Segment::Ptr(new Segment(C, D)));
162  ret.push_back(Segment::Ptr(new Segment(D, A)));
163  ret.push_back(Segment::Ptr(new Segment(E, F)));
164  ret.push_back(Segment::Ptr(new Segment(F, G)));
165  ret.push_back(Segment::Ptr(new Segment(G, H)));
166  ret.push_back(Segment::Ptr(new Segment(H, E)));
167  ret.push_back(Segment::Ptr(new Segment(A, E)));
168  ret.push_back(Segment::Ptr(new Segment(B, F)));
169  ret.push_back(Segment::Ptr(new Segment(C, G)));
170  ret.push_back(Segment::Ptr(new Segment(D, H)));
171  return ret;
172  }
173 
175  {
176  std::vector<Segment::Ptr> candidate_edges = edges();
177  Vertices intersects;
178  for (size_t i = 0; i < candidate_edges.size(); i++) {
179  Segment::Ptr edge = candidate_edges[i];
180  Eigen::Vector3f p;
181  if (edge->intersect(plane, p)) {
182  intersects.push_back(p);
183  }
184  }
185  //ROS_INFO("%lu intersects", intersects.size());
186  // Compute convex hull
187  pcl::ConvexHull<pcl::PointXYZ> chull;
188  pcl::PointCloud<pcl::PointXYZ>::Ptr chull_input
189  = verticesToPointCloud<pcl::PointXYZ>(intersects);
190  pcl::PointCloud<pcl::PointXYZ>::Ptr chull_cloud
191  (new pcl::PointCloud<pcl::PointXYZ>);
192  chull.setDimension(2);
193  chull.setInputCloud(chull_input);
194  {
195  boost::mutex::scoped_lock lock(global_chull_mutex);
196  chull.reconstruct(*chull_cloud);
197  }
198 
199  return ConvexPolygon::Ptr(
200  new ConvexPolygon(pointCloudToVertices(*chull_cloud)));
201  }
202 
203  jsk_recognition_msgs::BoundingBox Cube::toROSMsg()
204  {
205  jsk_recognition_msgs::BoundingBox ret;
206  ret.pose.position.x = pos_[0];
207  ret.pose.position.y = pos_[1];
208  ret.pose.position.z = pos_[2];
209  ret.pose.orientation.x = rot_.x();
210  ret.pose.orientation.y = rot_.y();
211  ret.pose.orientation.z = rot_.z();
212  ret.pose.orientation.w = rot_.w();
213  ret.dimensions.x = dimensions_[0];
214  ret.dimensions.y = dimensions_[1];
215  ret.dimensions.z = dimensions_[2];
216  return ret;
217  }
218 
220  {
221  Vertices vs;
222  vs.push_back(buildVertex(0.5, 0.5, 0.5));
223  vs.push_back(buildVertex(-0.5, 0.5, 0.5));
224  vs.push_back(buildVertex(-0.5, -0.5, 0.5));
225  vs.push_back(buildVertex(0.5, -0.5, 0.5));
226  vs.push_back(buildVertex(0.5, 0.5, -0.5));
227  vs.push_back(buildVertex(-0.5, 0.5, -0.5));
228  vs.push_back(buildVertex(-0.5, -0.5, -0.5));
229  vs.push_back(buildVertex(0.5, -0.5, -0.5));
230  return vs;
231  }
232 
233  Vertices Cube::transformVertices(const Eigen::Affine3f& pose_offset)
234  {
235  Vertices vs = vertices();
236  Vertices transformed_vertices;
237  for (size_t i = 0; i < vs.size(); i++) {
238  transformed_vertices.push_back(pose_offset * vs[i]);
239  }
240  return transformed_vertices;
241  }
242 
243  Polygon::Ptr Cube::buildFace(const Eigen::Vector3f v0,
244  const Eigen::Vector3f v1,
245  const Eigen::Vector3f v2,
246  const Eigen::Vector3f v3)
247  {
248  Vertices vs;
249  vs.push_back(v0);
250  vs.push_back(v1);
251  vs.push_back(v2);
252  vs.push_back(v3);
253  Polygon::Ptr(new Polygon(vs));
254  }
255 
256  std::vector<Polygon::Ptr> Cube::faces()
257  {
258  std::vector<Polygon::Ptr> fs(6);
259  Vertices vs = vertices();
260  Eigen::Vector3f A = vs[0];
261  Eigen::Vector3f B = vs[1];
262  Eigen::Vector3f C = vs[2];
263  Eigen::Vector3f D = vs[3];
264  Eigen::Vector3f E = vs[4];
265  Eigen::Vector3f F = vs[5];
266  Eigen::Vector3f G = vs[6];
267  Eigen::Vector3f H = vs[7];
268  Vertices vs0, vs1, vs2, vs3, vs4, vs5, vs6;
269  vs0.push_back(A); vs0.push_back(E); vs0.push_back(F); vs0.push_back(B);
270  vs1.push_back(B); vs1.push_back(F); vs1.push_back(G); vs1.push_back(C);
271  vs2.push_back(C); vs2.push_back(G); vs2.push_back(H); vs2.push_back(D);
272  vs3.push_back(D); vs3.push_back(H); vs3.push_back(E); vs3.push_back(A);
273  vs4.push_back(A); vs4.push_back(B); vs4.push_back(C); vs4.push_back(D);
274  vs5.push_back(E); vs5.push_back(H); vs5.push_back(G); vs5.push_back(F);
275  fs[0].reset(new Polygon(vs0));
276  fs[1].reset(new Polygon(vs1));
277  fs[2].reset(new Polygon(vs2));
278  fs[3].reset(new Polygon(vs3));
279  fs[4].reset(new Polygon(vs4));
280  fs[5].reset(new Polygon(vs5));
281  return fs;
282  }
283 
284  Eigen::Vector3f Cube::buildVertex(double i, double j, double k)
285  {
286  Eigen::Vector3f local = (Eigen::Vector3f::UnitX() * i * dimensions_[0] +
287  Eigen::Vector3f::UnitY() * j * dimensions_[1] +
288  Eigen::Vector3f::UnitZ() * k * dimensions_[2]);
289  return Eigen::Translation3f(pos_) * rot_ * local;
290  }
291 
292  Eigen::Vector3f Cube::nearestPoint(const Eigen::Vector3f& p,
293  double& distance)
294  {
295  std::vector<Polygon::Ptr> current_faces = faces();
296  double min_distance = DBL_MAX;
297  Eigen::Vector3f min_point;
298  for (size_t i = 0; i < current_faces.size(); i++) {
299  Polygon::Ptr f = current_faces[i];
300  double d;
301  Eigen::Vector3f q = f->nearestPoint(p, d);
302  if (min_distance > d) {
303  min_distance = d;
304  min_point = q;
305  }
306  }
307  distance = min_distance;
308  return min_point;
309  }
310 }
d
Definition: setup.py:9
pose
Vertices vertices()
returns vertices as an array of Eigen::Vectro3f. The order of the vertices is: [1, 1, 1], [-1, 1, 1], [-1, -1, 1], [1, -1, 1], [1, 1, -1], [-1, 1, -1], [-1, -1, -1], [1, -1, -1].
Definition: cube.cpp:219
Eigen::Quaternionf rot_
Definition: cube.h:102
std::vector< Segment::Ptr > edges()
Definition: cube.cpp:123
virtual double distance(const Line &other) const
compute a distance to line.
Definition: line.cpp:155
Vertices transformVertices(const Eigen::Affine3f &pose_offset)
returns vertices transformed by pose_offset.
Definition: cube.cpp:233
f
boost::mutex global_chull_mutex
Definition: pcl_util.cpp:43
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
boost::shared_ptr< ConvexPolygon > Ptr
boost::shared_ptr< Polygon > Ptr
Definition: polygon.h:58
ConvexPolygon::Ptr intersectConvexPolygon(Plane &plane)
Definition: cube.cpp:174
Class to represent 3-D straight line which has finite length.
Definition: segment.h:50
q
virtual Polygon::Ptr buildFace(const Eigen::Vector3f v0, const Eigen::Vector3f v1, const Eigen::Vector3f v2, const Eigen::Vector3f v3)
A helper method to build polygon from 4 vertices.
Definition: cube.cpp:243
Cube(const Eigen::Vector3f &pos, const Eigen::Quaternionf &rot)
Definition: cube.cpp:42
Eigen::Quaternionf rotFrom3Axis(const Eigen::Vector3f &ex, const Eigen::Vector3f &ey, const Eigen::Vector3f &ez)
Definition: geo_util.cpp:55
virtual Ptr midLine(const Line &other) const
compute a middle line between given line.
Definition: line.cpp:126
std::vector< double > dimensions_
Definition: cube.h:103
virtual Eigen::Vector3f buildVertex(double i, double j, double k)
A helper method to build vertex from x-y-z relatiev coordinates.
Definition: cube.cpp:284
virtual Eigen::Vector3f nearestPoint(const Eigen::Vector3f &p, double &distance)
compute minimum distance from point p to cube surface.
Definition: cube.cpp:292
jsk_recognition_msgs::BoundingBox toROSMsg()
Definition: cube.cpp:203
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices
Definition: types.h:48
std::vector< Polygon::Ptr > faces()
returns all the 6 faces as Polygon::Ptr. TODO: is it should be ConvexPolygon?
Definition: cube.cpp:256
Vertices pointCloudToVertices(const pcl::PointCloud< PointT > &cloud)
Compute Vertices from PointCloud.
Definition: geo_util.h:122
p
Eigen::Vector3f pos_
Definition: cube.h:101
Class to represent 3-D straight line.
Definition: line.h:49
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 Wed May 27 2020 03:59:48