particle_cuboid.h
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 
37 #ifndef JSK_PCL_ROS_PARTICLE_CUBOID_H_
38 #define JSK_PCL_ROS_PARTICLE_CUBOID_H_
39 
40 #include <pcl/point_cloud.h>
41 #include "jsk_pcl_ros/geo_util.h"
42 
43 namespace pcl
44 {
45  namespace tracking
46  {
48  {
50  union
51  {
52  struct
53  {
54  float roll;
55  float pitch;
56  float yaw;
57  float dx;
58  float dy;
59  float dz;
60  float weight;
61 
62  };
63  float data_c[8];
64  };
65 
66  //jsk_pcl_ros::Polygon::Ptr plane;
68  };
69 
71  {
72 
73  // Copy constructor
74  inline ParticleCuboid(const ParticleCuboid& obj)
75  {
76  x = obj.x;
77  y = obj.y;
78  z = obj.z;
79  roll = obj.roll;
80  pitch = obj.pitch;
81  yaw = obj.yaw;
82  dx = obj.dx;
83  dy = obj.dy;
84  dz = obj.dz;
85  weight = obj.weight;
87  }
88 
89  inline ParticleCuboid()
90  {
91  x = y = z = roll = pitch = yaw = dx = dy = dz = weight = 0.0;
92  data[3] = 1.0f;
93  plane_index = -1;
94  }
95  inline ParticleCuboid (float _x, float _y, float _z, float _roll, float _pitch, float _yaw)
96  {
97  x = _x; y = _y; z = _z;
98  roll = _roll; pitch = _pitch; yaw = _yaw;
99  dx = dy = dz = 0.0;
100  data[3] = 1.0f;
101  }
102 
103  inline Eigen::Affine3f toEigenMatrix() const
104  {
105  return getTransformation(x, y, z, roll, pitch, yaw);
106  }
107 
108  inline void fromEigen(const Eigen::Affine3f& pose)
109  {
110  Eigen::Vector3f pos(pose.translation());
111  getEulerAngles(pose, roll, pitch, yaw);
112  getVector3fMap() = pos;
113  }
114 
115  inline void zero()
116  {
117  x = y = z = roll = pitch = yaw = dx = dy = dz = weight = 0.0;
118  }
119 
120  inline float volume() const
121  {
122  return dx * dy * dz;
123  }
124 
125  inline float area() const
126  {
127  return (dx * dy + dy * dz + dz * dx) * 2.0;
128  }
129 
131  toState (const Eigen::Affine3f &trans)
132  {
133  float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
134  getTranslationAndEulerAngles (trans,
135  trans_x, trans_y, trans_z,
136  trans_roll, trans_pitch, trans_yaw);
137  return pcl::tracking::ParticleCuboid (trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
138  }
139 
140  // a[i]
141  inline float operator [] (unsigned int i) const
142  {
143  switch (i)
144  {
145  case 0: return x;
146  case 1: return y;
147  case 2: return z;
148  case 3: return roll;
149  case 4: return pitch;
150  case 5: return yaw;
151  case 6: return dx;
152  case 7: return dy;
153  case 8: return dz;
154  case 9: return weight;
155  default: return 0.0;
156  }
157  }
158 
159  inline void sample(
160  const std::vector<double>& mean, const std::vector<double>& cov)
161  {
162  // Do nothing because it is not used
163  }
164 
165  inline static int stateDimension()
166  {
167  return 9;
168  }
169 
170  inline jsk_recognition_msgs::BoundingBox toBoundingBox()
171  {
172  jsk_recognition_msgs::BoundingBox box;
173  Eigen::Affine3f affine = toEigenMatrix();
174  tf::poseEigenToMsg(affine, box.pose);
175  box.dimensions.x = dx;
176  box.dimensions.y = dy;
177  box.dimensions.z = dz;
178  return box;
179  }
180 
181  inline std::vector<int> visibleFaceIndices(const Eigen::Vector3f local_view_point) const
182  {
183  std::vector<int> visible_faces;
184  visible_faces.reserve(3);
185  if (local_view_point[0] > 0) {
186  visible_faces.push_back(0);
187  }
188  else if (local_view_point[0] < 0) {
189  visible_faces.push_back(2);
190  }
191  if (local_view_point[1] > 0) {
192  visible_faces.push_back(1);
193  }
194  else if (local_view_point[1] < 0) {
195  visible_faces.push_back(3);
196  }
197  if (local_view_point[2] > 0) {
198  visible_faces.push_back(4);
199  }
200  else if (local_view_point[2] < 0) {
201  visible_faces.push_back(5);
202  }
203  return visible_faces;
204  }
205 
207  const Eigen::Vector3f& v,
208  const std::vector<int>& visible_faces,
209  std::vector<jsk_pcl_ros::Polygon::Ptr>& faces) const
210  {
211  double min_distance = DBL_MAX;
212  for (size_t i = 0; i < visible_faces.size(); i++) {
213  jsk_pcl_ros::Polygon::Ptr face = faces[visible_faces[i]];
214  double d;
215  face->nearestPoint(v, d);
216  if (min_distance > d) {
217  min_distance = d;
218  }
219  }
220  return min_distance;
221  }
222 
223 
224  // V should be on local coordinates.
225  // TODO: update to take into boundary account
226  inline double signedDistanceToPlane(
227  const Eigen::Vector3f& v, const int plane_index) const
228  {
229  Eigen::Vector3f n;
230  double d;
231  switch (plane_index)
232  {
233  case 0: {
234  n = Eigen::Vector3f::UnitX();
235  d = - 0.5 * dx;
236  break;
237  }
238  case 1: {
239  n = Eigen::Vector3f::UnitY();
240  d = - 0.5 * dy;
241  break;
242  }
243  case 2: {
244  n = - (Eigen::Vector3f::UnitX());
245  d = - 0.5 * dx;
246  break;
247  }
248  case 3: {
249  n = - (Eigen::Vector3f::UnitY());
250  d = - 0.5 * dy;
251  break;
252  }
253  case 4: {
254  n = Eigen::Vector3f::UnitZ();
255  d = - 0.5 * dz;
256  break;
257  }
258  case 5: {
259  n = - (Eigen::Vector3f::UnitZ());
260  d = - 0.5 * dz;
261  break;
262  }
263 
264  }
265  return (n.dot(v) + d) / sqrt(n.dot(n) + d * d);
266  }
267 
268  inline double distanceToPlane(
269  const Eigen::Vector3f& v, const int plane_index) const
270  {
271  return std::abs(signedDistanceToPlane(v, plane_index));
272  }
273 
274 
275 
276  inline int nearestPlaneIndex(const Eigen::Vector3f& local_v) const
277  {
278  const float x = local_v[0];
279  const float y = local_v[1];
280  const float z = local_v[2];
281  const float abs_x = std::abs(x);
282  const float abs_y = std::abs(y);
283  const float abs_z = std::abs(z);
284  if (x > 0 && abs_x >= abs_y && abs_x >= abs_z) {
285  return 0;
286  }
287  else if (x < 0 && abs_x >= abs_y && abs_x >= abs_z) {
288  return 2;
289  }
290  else if (y > 0 && abs_y >= abs_x && abs_y >= abs_z) {
291  return 1;
292  }
293  else if (y < 0 && abs_y >= abs_x && abs_y >= abs_z) {
294  return 3;
295  }
296  else if (z > 0 && abs_z >= abs_x && abs_z >= abs_y) {
297  return 4;
298  }
299  else if (z < 0 && abs_z >= abs_x && abs_z >= abs_y) {
300  return 5;
301  }
302  }
303 
305  {
306  Eigen::Affine3f pose = toEigenMatrix();
307  Eigen::Vector3f dimensions(dx, dy, dz);
308  jsk_pcl_ros::Cube::Ptr cube(new jsk_pcl_ros::Cube(Eigen::Vector3f(pose.translation()),
309  Eigen::Quaternionf(pose.rotation()),
310  dimensions));
311  return cube;
312  }
313 
314 
316  };
317 
318  inline ParticleCuboid operator* (const ParticleCuboid& p, const Eigen::Affine3f& transform)
319  {
320  Eigen::Affine3f particle_affine = p.toEigenMatrix();
321  Eigen::Affine3f transformed_affine = transform * particle_affine;
322  ParticleCuboid new_p;
323  new_p.x = transformed_affine.translation()[0];
324  new_p.y = transformed_affine.translation()[1];
325  new_p.z = transformed_affine.translation()[2];
326  getEulerAngles(transformed_affine, new_p.roll, new_p.pitch, new_p.yaw);
327  new_p.dx = p.dx;
328  new_p.dy = p.dy;
329  new_p.dz = p.dz;
330  return new_p;
331  }
332 
333  // a + b
335  {
337  newp.x = a.x + b.x;
338  newp.y = a.y + b.y;
339  newp.z = a.z + b.z;
340  newp.roll = a.roll + b.roll;
341  newp.pitch = a.pitch + b.pitch;
342  newp.yaw = a.yaw + b.yaw;
343  newp.dx = a.dx + b.dx;
344  newp.dy = a.dy + b.dy;
345  newp.dz = a.dz + b.dz;
346  return (newp);
347  }
348 
350  {
352  newp.x = static_cast<float> (p.x * val);
353  newp.y = static_cast<float> (p.y * val);
354  newp.z = static_cast<float> (p.z * val);
355  newp.roll = static_cast<float> (p.roll * val);
356  newp.pitch = static_cast<float> (p.pitch * val);
357  newp.yaw = static_cast<float> (p.yaw * val);
358  newp.dx = static_cast<float> (p.dx * val);
359  newp.dy = static_cast<float> (p.dy * val);
360  newp.dz = static_cast<float> (p.dz * val);
361  return (newp);
362  }
363 
365  {
366  return p * val;
367  }
368 
370  {
371  return a + (-1.0) * b;
372  }
373 
374  } // tracking
375 
376 
377  template<>
378  class DefaultPointRepresentation<pcl::tracking::ParticleCuboid>: public PointRepresentation<pcl::tracking::ParticleCuboid>
379  {
380  public:
382  {
383  nr_dimensions_ = 10;
384  }
385 
386  virtual void
387  copyToFloatArray (const pcl::tracking::ParticleCuboid &p, float * out) const
388  {
389  for (int i = 0; i < nr_dimensions_; ++i)
390  out[i] = p[i];
391  }
392  };
393 
394 } // pcl
395 
396 // These registration is required to convert
397 // pcl::PointCloud<pcl::tracking::ParticleCuboid> to PCLPointCloud2.
398 // And pcl::fromROSMsg and pcl::toROSMsg depends on PCLPointCloud2
399 // conversions.
401  (float, x, x)
402  (float, y, y)
403  (float, z, z)
404  (float, roll, roll)
405  (float, pitch, pitch)
406  (float, yaw, yaw)
407  (float, dx, dx)
408  (float, dy, dy)
409  (float, dz, dz)
410  (float, weight, weight)
411  )
412 POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::tracking::ParticleCuboid, pcl::tracking::_ParticleCuboid)
413 
414 
415 #endif
d
struct pcl::PointXYZHSV EIGEN_ALIGN16
pcl::tracking::ParticleCuboid operator+(const ParticleCuboid &a, const ParticleCuboid &b)
pos
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
GLfloat n[6][3]
ParticleCuboid(const ParticleCuboid &obj)
boost::shared_ptr< Polygon > Ptr
Eigen::Affine3f toEigenMatrix() const
jsk_pcl_ros::Cube::Ptr toCube() const
pose
pcl::tracking::ParticleCuboid operator-(const ParticleCuboid &a, const ParticleCuboid &b)
ParticleCuboid(float _x, float _y, float _z, float _roll, float _pitch, float _yaw)
std::vector< int > visibleFaceIndices(const Eigen::Vector3f local_view_point) const
virtual void copyToFloatArray(const pcl::tracking::ParticleCuboid &p, float *out) const
jsk_recognition_msgs::BoundingBox toBoundingBox()
double signedDistanceToPlane(const Eigen::Vector3f &v, const int plane_index) const
double sqrt()
ParticleCuboid operator*(const ParticleCuboid &p, const Eigen::Affine3f &transform)
face(linetab, int mi, int m, int r, int n)
void fromEigen(const Eigen::Affine3f &pose)
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
int nearestPlaneIndex(const Eigen::Vector3f &local_v) const
double distanceToPlane(const Eigen::Vector3f &v, const int plane_index) const
double distanceNearestToPlaneWithOcclusion(const Eigen::Vector3f &v, const std::vector< int > &visible_faces, std::vector< jsk_pcl_ros::Polygon::Ptr > &faces) const
static pcl::tracking::ParticleCuboid toState(const Eigen::Affine3f &trans)
void getTransformation(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2, Eigen::Vector3d &pos, Eigen::Quaterniond &rot)
Cube cube(Eigen::Vector3f(1, 0, 0), Eigen::Quaternionf(0.108755, 0.088921, 0.108755, 0.984092), Eigen::Vector3f(0.3, 0.3, 0.3))
POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::PointRGB,(float, rgb, rgb))
boost::shared_ptr< Cube > Ptr


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:47