cob_3d_features: invariant_surface

Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright (c) 2011
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob_environment_perception_intern
00012  * ROS package name: cob_3d_mapping_features
00013  * Description:
00014  *
00015  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00016  *
00017  * Author: Joshua Hampp, email:joshua.hampp@ipa.fraunhofer.de
00018  *
00019  * Date of creation: 03/2014
00020  * ToDo:
00021  *
00022  *
00023  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00024  *
00025  * Redistribution and use in source and binary forms, with or without
00026  * modification, are permitted provided that the following conditions are met:
00027  *
00028  *     * Redistributions of source code must retain the above copyright
00029  *       notice, this list of conditions and the following disclaimer.
00030  *     * Redistributions in binary form must reproduce the above copyright
00031  *       notice, this list of conditions and the following disclaimer in the
00032  *       documentation and/or other materials provided with the distribution.
00033  *     * Neither the name of the Fraunhofer Institute for Manufacturing
00034  *       Engineering and Automation (IPA) nor the names of its
00035  *       contributors may be used to endorse or promote products derived from
00036  *       this software without specific prior written permission.
00037  *
00038  * This program is free software: you can redistribute it and/or modify
00039  * it under the terms of the GNU Lesser General Public License LGPL as
00040  * published by the Free Software Foundation, either version 3 of the
00041  * License, or (at your option) any later version.
00042  *
00043  * This program is distributed in the hope that it will be useful,
00044  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00045  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00046  * GNU Lesser General Public License LGPL for more details.
00047  *
00048  * You should have received a copy of the GNU Lesser General Public
00049  * License LGPL along with this program.
00050  * If not, see <http://www.gnu.org/licenses/>.
00051  *
00052  ****************************************************************/
00053 
00054 #pragma once
00055 
00056 #include <complex>
00057 #include <vector>
00058 #include <boost/shared_ptr.hpp>
00059 #include <Eigen/Geometry>
00060 #include <cob_3d_mapping_common/polypartition.h>
00061 #include <pcl/PolygonMesh.h>            //only used for debugging/test functions (perhaps put outside?)
00062 
00063 #include "ShapeSPH/Util/Signature.h"
00064 #include "ShapeSPH/Util/SphereSampler.h"
00065 #include "ShapeSPH/Util/lineqn.h"
00066 #include "ShapeSPH/Util/SphericalPolynomials.h"
00067 #include "invariant_surface_feature/triangle.hpp"
00068 
00069 namespace cob_3d_features
00070 {
00072   template<typename TSurface, typename Scalar=double, typename Real=float, typename TAffine=Eigen::Affine3f>
00073   class InvariantSurfaceFeature
00074   {
00075           typedef Sampler<Real, Scalar> S;
00076           
00077           const int num_radius_, num_angle_;
00078           S sr_;
00079           typename S::Samples samples_;
00080   public:
00081 
00082     typedef Eigen::Matrix<Scalar, 3, 1> TVector;
00083     typedef std::vector<TSurface> TSurfaceList;
00084     typedef boost::shared_ptr<TSurfaceList> PTSurfaceList;
00085     
00086     typedef std::vector<Signature< Real > > Result;
00087     typedef boost::shared_ptr<Result> PResult;
00088     typedef boost::shared_ptr<const Result> PResultConst;
00089     
00090     //typedef Eigen::Matrix<Scalar, num_angle_, num_angle_> FeatureAngle;
00091     //typedef Eigen::Matrix<std::complex<Scalar>, num_angle_, num_angle_> FeatureAngleComplex;
00092     //typedef FeatureAngle Feature[num_radius_];
00093     /* Feature {
00094         FeatureAngle vals[num_radius_];
00095 
00096         Scalar operator-(const Feature &o) const {
00097                 Scalar r=0;
00098                 for(int i=0; i<num_radius_; i++)
00099                         r += (vals[i]-o.vals[i]).cwiseAbs().sum();
00100                 return r;
00101         }
00102     };
00103     typedef struct {FeatureAngleComplex vals[num_radius_];} FeatureComplex;
00104 
00105     struct ResultVector {
00106       TVector v;
00107       std::vector<Feature> ft;
00108     };
00109     typedef std::vector<ResultVector> ResultVectorList;
00110     typedef boost::shared_ptr<ResultVectorList> PResultVectorList;
00111     typedef boost::shared_ptr<const ResultVectorList> PResultVectorListConst;*/
00112 
00113     typedef enum {
00114       INVARAINCE_X=1, INVARAINCE_Y=2, INVARAINCE_Z=4,
00115       INVARAINCE_INCLINATION=8, INVARAINCE_AZIMUTH=16,
00116       INVARAINCE_ALL=0x1f
00117     } EINVARAINCE;
00118 
00120     InvariantSurfaceFeature(const int num_radius, const int num_angle) :
00121           num_radius_(num_radius), num_angle_(num_angle),
00122           sr_((Real)num_radius, num_radius, num_angle),
00123       invariance_(INVARAINCE_ALL)
00124     {
00125                 sr_.getSamples(samples_);
00126                 //TODO: default radi
00127     }
00128 
00130     virtual ~InvariantSurfaceFeature() {}
00131 
00132         //resets the input (list of surfaces with holes) and computes initial fft
00133     void setInput(PTSurfaceList surfs);
00134     void compute();
00135 
00136     PResultConst getResult() const {return result_;}
00137 
00138     const TAffine &getTransformation() const {return transform_;}
00139     void setTransformation(const TAffine &t) {transform_=t;}
00140 
00141     const EINVARAINCE &getInvarianceSettings() const {return invariance_;}
00142     void setInvarianceSettings(const EINVARAINCE &t) {invariance_=t;}
00143 
00144     const std::vector<float> &getRadii() const {return radii_;}
00145     void addRadius(const float r) {     //insert sorted!
00146                 std::vector<float>::iterator it = radii_.begin();
00147                 while(it!=radii_.end() && *it>r) ++it;
00148                 radii_.insert(it, r);
00149         }
00150 
00151     /*const int getNumRadius() const {return num_radius_;}
00152     void setNumRadius(const int t) {num_radius_=t;}
00153     const int getNumAngle() const {return num_angle_;}
00154     void setNumAngle(const int t) {num_angle_=t;}*/
00155 
00156 
00157     /* UNIT TESTS: available in invariant_surface_feature_unit_tests.hpp */
00158     bool test_singleTriangle(const int num) const;
00159         pcl::PolygonMesh::Ptr test_subsampling_of_Map(const int num, const Scalar r2);
00160         void test_addOffset(const Scalar off_x, const Scalar off_y, const Scalar off_z);
00161         void test_rotate(const Scalar angle);
00162         
00163         /* DEBUG functions */
00164     void dbg_mesh_of_subsamp(const TVector &at, const Scalar radius, std::vector<TVector> &pts, std::vector<int> &inds) const;
00165     void dbg_keypoints(std::vector<TVector> &keypoints) const {generateKeypoints(keypoints);}
00166         pcl::PolygonMesh::Ptr dbg_Mesh_of_Map() const {return dbg_triangles2mesh(triangulated_input_);}
00167         
00168   protected:
00169     struct Triangle : public cob_3d_features::invariant_surface_feature::SingleTriangle<Scalar, typename S::Samples, typename S::Values> {
00170                 /*struct Tri2D {
00171                         const Eigen::Matrix<Scalar, 2, 1> *p_[3];
00172                 };*/
00173 
00174                 Eigen::Matrix<Scalar, 2, 1> p_[3];
00175                 typename TSurface::Model *model_;
00176                 
00177                 inline static void set(Eigen::Matrix<Scalar, 2, 1> &p, const TPPLPoint &tp) {
00178                         p(0) = tp.x;
00179                         p(1) = tp.y;
00180                 }
00181                 
00182                 void compute(const typename S::Samples &samples);
00183                 void subsample(const typename S::Samples &samples, const TVector &at, const Scalar r2, std::vector<Triangle> &res) const;
00184                 std::complex<Scalar> kernel(const Scalar m, const Scalar n, const Scalar p) const;
00185 
00186                 void print() const;
00187     private:
00188                 Eigen::Matrix<Scalar, 2, 1> intersection_on_line(const TVector &at, const Scalar r2, const Eigen::Matrix<Scalar, 2, 1> &a, const Eigen::Matrix<Scalar, 2, 1> &b)  const;
00189                 //std::complex<Scalar> sub_kernel(const Scalar m, const Scalar n, const Scalar p, const Tri2D &tri, const int depth=0) const;
00190                 std::complex<Scalar> kernel_lin(const Scalar m, const Scalar n, const Scalar p, const Scalar x0, const Scalar y0, const Scalar y1, const Scalar d1, const Scalar d2) const;
00191                 //std::complex<Scalar> kernel_lin_tri(const Scalar m, const Scalar n, const Scalar p, const Tri2D &tri) const;
00192 
00193                 inline Eigen::Matrix<Scalar, 3, 1> at(const Scalar x, const Scalar y) const {
00194                         Eigen::Matrix<Scalar, 2, 1> p;
00195                         p(0)=x;p(1)=y;
00196                         return at(p);
00197                 }
00198 
00199                 inline Eigen::Matrix<Scalar, 3, 1> at(const Eigen::Matrix<Scalar, 2, 1> &p) const {
00200                         Eigen::Matrix<Scalar, 3, 1> v;
00201                         v(0) = p(0); v(1) = p(1);
00202                         v(2) = model_->model(p(0),p(1));
00203                         return v;
00204                 }
00205 
00206                 template<const int Degree>
00207                 Scalar area() const {
00208                         //TODO: at the moment kind of "approximation"
00209                         const Eigen::Matrix<Scalar, 2, 1> mid2 = ((p_[0])+(p_[1])+(p_[2]))/3;
00210                         Eigen::Matrix<Scalar, 3, 1> mid23, mid3, v1,v2,v3;
00211 
00212                         v1.template head<2>() = p_[0];
00213                         v2.template head<2>() = p_[1];
00214                         v3.template head<2>() = p_[2];
00215                         v1(2) = model_->model((p_[0])(0), (p_[0])(1));
00216                         v2(2) = model_->model((p_[1])(0), (p_[1])(1));
00217                         v3(2) = model_->model((p_[2])(0), (p_[2])(1));
00218 
00219                         mid23(0) = mid2(0);
00220                         mid23(1) = mid2(1);
00221                         mid3 = mid23;
00222                         mid3(2) =  (v1(2)+v2(2)+v3(2))/3;
00223                         mid23(2) = model_->model(mid2(0),mid2(1));
00224 
00225 //std::cout<<"area "<<(mid3 - mid23).squaredNorm()<<" "<<mid23(2)<<" "<<mid3(2)<<std::endl;
00226                         return (mid3 - mid23).norm()*(v1-v2).cross(v3-v2).norm();
00227                 }
00228         };
00229         
00230     struct VectorWithParams {
00231       TVector v_;
00232       typename TSurface::TParam::VectorU *param_;
00233     };
00234     
00235     PTSurfaceList input_;
00236     std::vector<Triangle> triangulated_input_;
00237     
00238     TAffine transform_;
00239     EINVARAINCE invariance_;
00240     std::vector<float> radii_;  //descending sorted radius list (reuse of subsampled map)
00241     PResult result_;
00242 
00243     void generateKeypoints(std::vector<TVector> &keypoints) const;
00244     void subsample(const TVector &at, const Scalar r2, std::vector<Triangle> &res) const;
00245         
00246         /* DEBUG functions */
00247         pcl::PolygonMesh::Ptr dbg_triangles2mesh(const std::vector<Triangle> &res) const;
00248   };
00249 }