surface_tri_spline.h

Go to the documentation of this file.

/*
 * surface_tri_spline.h
 *
 *  Created on: 01.10.2012
 *      Author: josh
 */

#ifndef SURFACE_TRI_SPLINE_H_
#define SURFACE_TRI_SPLINE_H_

#include <cob_3d_mapping_slam/trispline/trispline.h>
#include <cob_3d_mapping_slam/trispline/topology.h>


namespace Slam_Surface
{
  class SurfaceTriSpline : public Surface, public cob_3d_marker::MarkerCreator
  {
#if 1
    ParametricSurface::Topology top_;
#else
  public:
    struct TRIANGLE {
      size_t i_[3];

      Eigen::Vector3f add_cross_; //temporary control point, calculated from the normals
      Eigen::Matrix2f _T_; //temporary inverse matrix for barycentric coordinates

      Eigen::Vector3f I_[3]; 
      Eigen::Vector3f nI_[3]; 

      Eigen::Vector3f pS_, nS_; 
      Eigen::Vector3f pb_[3];
      Eigen::Matrix2f _Tb_[3]; //temporary inverse matrix for barycentric coordinates

      TRIANGLE(const size_t i1, const size_t i2, const size_t i3) {
        i_[0] = i1;
        i_[1] = i2;
        i_[2] = i3;
      }

      bool update1(const std::vector<Eigen::Vector3f> &pts, const std::vector<Eigen::Vector3f> &normals, const std::vector<Eigen::Vector2f> &uv_pts,
                   const Surface *surf=NULL);

      bool update2(const std::vector<Eigen::Vector3f> &pts, const std::vector<Eigen::Vector3f> &normals, const std::vector<Eigen::Vector2f> &uv_pts,
                   const Surface *surf=NULL);

      void getWeight(const Eigen::Vector3f &pt, Eigen::Matrix3f &w) const;
      void getWeightD1(const Eigen::Vector3f &pt, Eigen::Matrix3f &w) const;
      Eigen::Vector3f triNurbsBasis(const Eigen::Vector3f &pt, const Eigen::Vector3f &p1, const Eigen::Vector3f &p2, const Eigen::Vector3f &p3) const;
      Eigen::Vector3f project2world(const Eigen::Vector2f &pt, const std::vector<Eigen::Vector3f> &pts, const std::vector<Eigen::Vector2f> &uv_pts) const;
      Eigen::Vector3f normalAt(const Eigen::Vector2f &pt, const std::vector<Eigen::Vector3f> &pts, const std::vector<Eigen::Vector2f> &uv_pts) const;
      void transform(const Eigen::Matrix3f &rot, const Eigen::Vector3f &tr) {add_cross_=rot*add_cross_;}

      bool isIn(const Eigen::Vector2f &pt, const std::vector<Eigen::Vector2f> &uv_pts) const;
    };

    std::vector<Eigen::Vector3f> pts_, normals_;
    std::vector<Eigen::Vector2f> uv_pts_;
    std::vector<TRIANGLE> triangles_;

    static int getLineID(const size_t i1, const size_t i2) {return (std::min(i1,i2)<<16)|(std::max(i1,i2));}

    void addTriangle(const size_t i1, const size_t i2, const size_t i3, const Surface *surf=NULL);
    void addPoint(
        const Eigen::Vector3f &p1, const Eigen::Vector3f &n1, const Eigen::Vector2f &uv1
    );

    std::vector<TRIANGLE> &getTriangles() {return triangles_;}
#endif
  public:

    SurfaceTriSpline(): top_(0.025f) {}

    virtual void init(const boost::array<float, 6> &params, const float min_x, const float max_x, const float min_y, const float max_y, const float weight);
    virtual void init(const PolynomialSurface *params, const float min_x, const float max_x, const float min_y, const float max_y, const float weight);
    virtual void init(const PolynomialSurface *params, const std::vector<Eigen::Vector3f> &pts, const float weight);

    virtual int getSurfaceType() const {return 3;}
    virtual const char *getName() const {return "Triangular B-Spline";}

    virtual Eigen::Vector2f nextPoint(const Eigen::Vector3f &v) const;

    virtual Eigen::Vector3f project2world(const Eigen::Vector2f &pt) const ;

    virtual Eigen::Vector3f normalAt(const Eigen::Vector2f &v) const ;

    virtual float merge(const Surface &o, const float this_w, const float o_w, const SWINDOW &wind_t, const SWINDOW &wind_o);
    virtual float _merge(SurfaceNurbs o, const float this_w, const float o_w, const SWINDOW &wind_t, const SWINDOW &wind_o) {ROS_ASSERT(0);return 0;}

    virtual void transform(const Eigen::Matrix3f &rot, const Eigen::Vector3f &tr);

    virtual bool fitsCurvature(const Surface &o, const float thr) const {ROS_ASSERT(0);return true;}

    virtual float area() const {ROS_ASSERT(0);return 1;}

    void print() const {top_.print();}

    void marker(cob_3d_marker::MarkerContainer &mc) const {
      if(mc.get(0))
        top_.add( *(cob_3d_marker::MarkerList_Triangles*)mc.get(0).get() );

      if(mc.get(2))
        top_.add( *(cob_3d_marker::MarkerList_Line*)mc.get(2).get() );
      if(mc.get(3))
        top_.add( *(cob_3d_marker::MarkerList_Arrow*)mc.get(3).get() );
      if(mc.get(4))
        top_.add( *(cob_3d_marker::MarkerList_Text*)mc.get(4).get() );
    }
  };

#include "impl/surface_tri_spline.hpp"
}



#endif /* SURFACE_TRI_SPLINE_H_ */