particle_cuboid.h
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 
00037 #ifndef JSK_PCL_ROS_PARTICLE_CUBOID_H_
00038 #define JSK_PCL_ROS_PARTICLE_CUBOID_H_
00039 
00040 #include <pcl/point_cloud.h>
00041 #include "jsk_pcl_ros/geo_util.h"
00042 
00043 namespace pcl
00044 {
00045   namespace tracking
00046   {
00047     struct _ParticleCuboid
00048     {
00049       PCL_ADD_POINT4D;
00050       union
00051       {
00052         struct
00053         {
00054           float roll;
00055           float pitch;
00056           float yaw;
00057           float dx;
00058           float dy;
00059           float dz;
00060           float weight;
00061           
00062         };
00063         float data_c[8];
00064       };
00065       
00066       //jsk_pcl_ros::Polygon::Ptr plane;
00067       int plane_index;
00068     };
00069     
00070     struct EIGEN_ALIGN16 ParticleCuboid : public _ParticleCuboid
00071     {
00072 
00073       // Copy constructor
00074       inline ParticleCuboid(const ParticleCuboid& obj)
00075       {
00076         x = obj.x;
00077         y = obj.y;
00078         z = obj.z;
00079         roll = obj.roll;
00080         pitch = obj.pitch;
00081         yaw = obj.yaw;
00082         dx = obj.dx;
00083         dy = obj.dy;
00084         dz = obj.dz;
00085         weight = obj.weight;
00086         plane_index = obj.plane_index;
00087       }
00088       
00089       inline ParticleCuboid()
00090       {
00091         x = y = z = roll = pitch = yaw = dx = dy = dz = weight = 0.0;
00092         data[3] = 1.0f;
00093         plane_index = -1;
00094       }
00095       inline ParticleCuboid (float _x, float _y, float _z, float _roll, float _pitch, float _yaw)
00096       {
00097         x = _x; y = _y; z = _z;
00098         roll = _roll; pitch = _pitch; yaw = _yaw;
00099         dx = dy = dz = 0.0;
00100         data[3] = 1.0f;
00101       }
00102 
00103       inline Eigen::Affine3f toEigenMatrix() const
00104       {
00105         return getTransformation(x, y, z, roll, pitch, yaw);
00106       }
00107 
00108       inline void fromEigen(const Eigen::Affine3f& pose) 
00109       {
00110         Eigen::Vector3f pos(pose.translation());
00111         getEulerAngles(pose, roll, pitch, yaw);
00112         getVector3fMap() = pos;
00113       }
00114 
00115       inline void zero()
00116       {
00117         x = y = z = roll = pitch = yaw = dx = dy = dz = weight = 0.0;
00118       }
00119       
00120       inline float volume() const
00121       {
00122         return dx * dy * dz;
00123       }
00124 
00125       inline float area() const
00126       {
00127         return (dx * dy + dy * dz + dz * dx) * 2.0;
00128       }
00129 
00130       static pcl::tracking::ParticleCuboid
00131       toState (const Eigen::Affine3f &trans)
00132       {
00133         float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
00134         getTranslationAndEulerAngles (trans,
00135                                       trans_x, trans_y, trans_z,
00136                                       trans_roll, trans_pitch, trans_yaw);
00137         return pcl::tracking::ParticleCuboid (trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
00138       }
00139 
00140       // a[i]
00141       inline float operator [] (unsigned int i) const
00142       {
00143         switch (i)
00144         {
00145         case 0: return x;
00146         case 1: return y;
00147         case 2: return z;
00148         case 3: return roll;
00149         case 4: return pitch;
00150         case 5: return yaw;
00151         case 6: return dx;
00152         case 7: return dy;
00153         case 8: return dz;
00154         case 9: return weight;
00155         default: return 0.0;
00156         }
00157       }
00158       
00159       inline void sample(
00160         const std::vector<double>& mean, const std::vector<double>& cov)
00161       {
00162         // Do nothing because it is not used
00163       }
00164 
00165       inline static int stateDimension()
00166       {
00167         return 9;
00168       }
00169 
00170       inline jsk_recognition_msgs::BoundingBox toBoundingBox()
00171       {
00172         jsk_recognition_msgs::BoundingBox box;
00173         Eigen::Affine3f affine = toEigenMatrix();
00174         tf::poseEigenToMsg(affine, box.pose);
00175         box.dimensions.x = dx;
00176         box.dimensions.y = dy;
00177         box.dimensions.z = dz;
00178         return box;
00179       }
00180 
00181       inline std::vector<int> visibleFaceIndices(const Eigen::Vector3f local_view_point) const
00182       {
00183         std::vector<int> visible_faces;
00184         visible_faces.reserve(3);
00185         if (local_view_point[0] > 0) {
00186           visible_faces.push_back(0);
00187         }
00188         else if (local_view_point[0] < 0) {
00189           visible_faces.push_back(2);
00190         }
00191         if (local_view_point[1] > 0) {
00192           visible_faces.push_back(1);
00193         }
00194         else if (local_view_point[1] < 0) {
00195           visible_faces.push_back(3);
00196         }
00197         if (local_view_point[2] > 0) {
00198           visible_faces.push_back(4);
00199         }
00200         else if (local_view_point[2] < 0) {
00201           visible_faces.push_back(5);
00202         }
00203         return visible_faces;
00204       }
00205 
00206       inline double distanceNearestToPlaneWithOcclusion(
00207         const Eigen::Vector3f& v,
00208         const std::vector<int>& visible_faces,
00209         std::vector<jsk_pcl_ros::Polygon::Ptr>& faces) const
00210       {
00211         double min_distance = DBL_MAX;
00212         for (size_t i = 0; i < visible_faces.size(); i++) {
00213           jsk_pcl_ros::Polygon::Ptr face = faces[visible_faces[i]];
00214           double d;
00215           face->nearestPoint(v, d);
00216           if (min_distance > d) {
00217             min_distance = d;
00218           }
00219         }
00220         return min_distance;
00221       }
00222 
00223       
00224       // V should be on local coordinates.
00225       // TODO: update to take into boundary account
00226       inline double signedDistanceToPlane(
00227         const Eigen::Vector3f& v, const int plane_index) const
00228       {
00229         Eigen::Vector3f n;
00230         double d;
00231         switch (plane_index)
00232         {
00233         case 0: {
00234           n = Eigen::Vector3f::UnitX();
00235           d = - 0.5 * dx;
00236           break;
00237         }
00238         case 1: {
00239           n = Eigen::Vector3f::UnitY();
00240           d = - 0.5 * dy;
00241           break;
00242         }
00243         case 2: {
00244           n = - (Eigen::Vector3f::UnitX());
00245           d = - 0.5 * dx;
00246           break;
00247         }
00248         case 3: {
00249           n = - (Eigen::Vector3f::UnitY());
00250           d = - 0.5 * dy;
00251           break;
00252         }
00253         case 4: {
00254           n = Eigen::Vector3f::UnitZ();
00255           d = - 0.5 * dz;
00256           break;
00257         }
00258         case 5: {
00259           n = - (Eigen::Vector3f::UnitZ());
00260           d = - 0.5 * dz;
00261           break;
00262         }
00263 
00264         }
00265         return (n.dot(v) + d) / sqrt(n.dot(n) + d * d);
00266       }
00267 
00268       inline double distanceToPlane(
00269         const Eigen::Vector3f& v, const int plane_index) const
00270       {
00271         return std::abs(signedDistanceToPlane(v, plane_index));
00272       }
00273 
00274 
00275       
00276       inline int nearestPlaneIndex(const Eigen::Vector3f& local_v) const
00277       {
00278         const float x = local_v[0];
00279         const float y = local_v[1];
00280         const float z = local_v[2];
00281         const float abs_x = std::abs(x);
00282         const float abs_y = std::abs(y);
00283         const float abs_z = std::abs(z);
00284         if (x > 0 && abs_x >= abs_y && abs_x >= abs_z) {
00285           return 0;
00286         }
00287         else if (x < 0 && abs_x >= abs_y && abs_x >= abs_z) {
00288           return 2;
00289         }
00290         else if (y > 0 && abs_y >= abs_x && abs_y >= abs_z) {
00291           return 1;
00292         }
00293         else if (y < 0 && abs_y >= abs_x && abs_y >= abs_z) {
00294           return 3;
00295         }
00296         else if (z > 0 && abs_z >= abs_x && abs_z >= abs_y) {
00297           return 4;
00298         }
00299         else if (z < 0 && abs_z >= abs_x && abs_z >= abs_y) {
00300           return 5;
00301         }
00302       }
00303       
00304       inline jsk_pcl_ros::Cube::Ptr toCube() const
00305     {
00306       Eigen::Affine3f pose = toEigenMatrix();
00307       Eigen::Vector3f dimensions(dx, dy, dz);
00308       jsk_pcl_ros::Cube::Ptr cube(new jsk_pcl_ros::Cube(Eigen::Vector3f(pose.translation()),
00309                                                         Eigen::Quaternionf(pose.rotation()),
00310                                                         dimensions));
00311       return cube;
00312     }
00313 
00314 
00315       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00316     };
00317 
00318     inline ParticleCuboid operator* (const ParticleCuboid& p, const Eigen::Affine3f& transform)
00319     {
00320       Eigen::Affine3f particle_affine = p.toEigenMatrix();
00321       Eigen::Affine3f transformed_affine = transform * particle_affine;
00322       ParticleCuboid new_p;
00323       new_p.x = transformed_affine.translation()[0];
00324       new_p.y = transformed_affine.translation()[1];
00325       new_p.z = transformed_affine.translation()[2];
00326       getEulerAngles(transformed_affine, new_p.roll, new_p.pitch, new_p.yaw);
00327       new_p.dx = p.dx;
00328       new_p.dy = p.dy;
00329       new_p.dz = p.dz;
00330       return new_p;
00331     }
00332 
00333     // a + b
00334     inline pcl::tracking::ParticleCuboid operator+ (const ParticleCuboid& a, const ParticleCuboid& b)
00335     {
00336       pcl::tracking::ParticleCuboid newp;
00337       newp.x = a.x + b.x;
00338       newp.y = a.y + b.y;
00339       newp.z = a.z + b.z;
00340       newp.roll = a.roll + b.roll;
00341       newp.pitch = a.pitch + b.pitch;
00342       newp.yaw = a.yaw + b.yaw;
00343       newp.dx = a.dx + b.dx;
00344       newp.dy = a.dy + b.dy;
00345       newp.dz = a.dz + b.dz;
00346       return (newp);
00347     }
00348     
00349     inline pcl::tracking::ParticleCuboid operator * (const ParticleCuboid& p, double val)
00350     {
00351       pcl::tracking::ParticleCuboid newp;
00352       newp.x     = static_cast<float> (p.x * val);
00353       newp.y     = static_cast<float> (p.y * val);
00354       newp.z     = static_cast<float> (p.z * val);
00355       newp.roll  = static_cast<float> (p.roll * val);
00356       newp.pitch = static_cast<float> (p.pitch * val);
00357       newp.yaw   = static_cast<float> (p.yaw * val);
00358       newp.dx = static_cast<float> (p.dx * val);
00359       newp.dy = static_cast<float> (p.dy * val);
00360       newp.dz = static_cast<float> (p.dz * val);
00361       return (newp);
00362     }
00363     
00364     inline pcl::tracking::ParticleCuboid operator * (double val, const ParticleCuboid& p)
00365     {
00366       return p * val;
00367     }
00368     
00369     inline pcl::tracking::ParticleCuboid operator- (const ParticleCuboid& a, const ParticleCuboid& b)
00370     {
00371       return a + (-1.0) * b;
00372     }
00373     
00374   } // tracking
00375 
00376   
00377   template<>
00378   class DefaultPointRepresentation<pcl::tracking::ParticleCuboid>: public PointRepresentation<pcl::tracking::ParticleCuboid>
00379   {
00380   public:
00381     DefaultPointRepresentation()
00382     {
00383       nr_dimensions_ = 10;
00384     }
00385 
00386     virtual void
00387     copyToFloatArray (const pcl::tracking::ParticleCuboid &p, float * out) const
00388     {
00389       for (int i = 0; i < nr_dimensions_; ++i)
00390         out[i] = p[i];
00391     }
00392   };
00393   
00394 } // pcl
00395 
00396 // These registration is required to convert
00397 // pcl::PointCloud<pcl::tracking::ParticleCuboid> to PCLPointCloud2.
00398 // And pcl::fromROSMsg and pcl::toROSMsg depends on PCLPointCloud2
00399 // conversions.
00400 POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::tracking::_ParticleCuboid,
00401                                    (float, x, x)
00402                                    (float, y, y)
00403                                    (float, z, z)
00404                                    (float, roll, roll)
00405                                    (float, pitch, pitch)
00406                                    (float, yaw, yaw)
00407                                    (float, dx, dx)
00408                                    (float, dy, dy)
00409                                    (float, dz, dz)
00410                                    (float, weight, weight)
00411   )
00412 POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::tracking::ParticleCuboid, pcl::tracking::_ParticleCuboid)
00413 
00414 
00415 #endif


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:41:44