kinbody3dtrack.h

Go to the documentation of this file.

#ifndef GCOP_KINBODY3DTRACK_H
#define GCOP_KINBODY3DTRACK_H

#include <Eigen/Dense>
#include <vector>
#include <type_traits>
#include "se3.h"
#include <iostream>
#include "utils.h"
#include "kinbody3d.h"

namespace gcop {
  
  using namespace std;
  using namespace Eigen;

  
  template <int _nu = 6>
  class Kinbody3dTrack {
  public:
    typedef Matrix<double, _nu, 1> Vectorud;
    typedef Matrix<double, 6, 1> Vector6d;
    Kinbody3dTrack(Kinbody3d<_nu> &sys, int nf, double vd0, double t0, double tf,
                double r = 25,
                bool extforce = false,
                bool forces = false);

    virtual void Get(Matrix4d &x, double vd, double t) const;

    virtual void MakeTrue();

    virtual void Add(const Vectorud &u, const Matrix4d &x, double h);

    virtual void Add2(const Vectorud &u, const Matrix4d &x, double h);

    Kinbody3d<_nu> &sys;     

    double t0;       
    double tf;       
    double r;       
    double w;      
    double h;      
    double dmax;    

    bool extforce;         

    bool forces;           
    
    vector<double> ts;     

    vector<Matrix4d> xs;      

    vector<Vectorud> us;   
    
    vector<Vector3d> ls;   
    vector<bool> observed; 

    vector<Matrix4d> xos;     
    vector<Vectorud> uos;     

    bool init; 
    VectorXd p;            
    vector<int> pis;       

    double pr;     

    vector< vector<pair<int, Vector3d> > > Is;  

    vector< vector<pair<int, Vector3d> > > Js;  
    vector<int> cis;       

    double cp;             

    Vector6d cv;           

    Vectorud cw;           

  };

template <int _nu>
Kinbody3dTrack<_nu>::Kinbody3dTrack(Kinbody3d<_nu> &sys, int nf, double vd0, double t0, double tf,
                         double r,
                         bool extforce,
                         bool forces) : 
  sys(sys), t0(t0), tf(tf), r(r), w(4), h(4), dmax(20),
  extforce(extforce), forces(forces),
  ts(1,t0), ls(nf), observed(nf, false), p(extforce*3), pr(.75),
  Is(1), Js(nf), cis(nf), cp(.01)
{
  Matrix4d x;
  Get(x, vd0, t0);
  xs.push_back(x);
  xos.push_back(x);
  cw = MatrixXd::Constant(_nu, 1, 0.1);
}


template <int _nu>
void Kinbody3dTrack<_nu>::Add(const Vectorud &u, const Matrix4d &x, double h)
{
  // add noisy controls/state to list, but use true controls/state for generating measurements
  Eigen::Matrix4d xn;
  double t = ts.back();
  
  // noisy controls
  Vectorud un;
  for(int i = 0; i < un.size(); i++)
  {
    un[i] = u[i] + sqrt(cw[i])*random_normal();
  }

  sys.Step(xn, t, xs.back(), un, h);
  us.push_back(un);
  xs.push_back(xn);
  ts.push_back(t + h);

  sys.Step(xn, t, xos.back(), un, h);
  xos.push_back(xn);
  uos.push_back(u);
  
  // time-stage 
  int k = xs.size()-1;

  /*
  sys.force->fext << 0, .5, 0;
  if (extforce) {
    p.head<2>() << .5, 0;
  }
  */
  
  /* // add noise
  for (int k = 0; k < N; ++k) {
    double t = k*h;
    Get(xs[k+1], r, vd, t+h);
    Vector3d f;
    us[k] = sys.I.cwiseProduct(xs[k+1].second - xs[k].second)/h + sys.force->D.cwiseProduct(xs[k].second);
    // add the constant external force
    us[k].tail<2>() -= xs[k].first.topLeftCorner<2,2>().transpose()*sys.force->fext.tail<2>();
  }
  */

  //  srand (time(NULL));
  
  const Matrix3d &R = x.block<3,3>(0,0);
  const Vector3d &px = x.block<3,1>(0,3);
  
  Is.resize(Is.size()+1);
  assert(k == Is.size()-1);

  for (int l = 0; l < ls.size(); ++l) {
    const Vector3d &pf = ls[l];
    //p.segment<2>(2*extforce + 2*l);
    
    double d = (px - pf).norm();
    if (d < dmax) {
      // add to feature vector if not observed
      Vector3d z = R.transpose()*(pf - px);
      z(0) += sqrt(cp)*random_normal();
      z(1) += sqrt(cp)*random_normal();
      z(2) += sqrt(cp)*random_normal();

      if (!observed[l]) {
        if (!init) {
          p.resize(3);
          init= true;
        } else {
          p.conservativeResize(p.size() + 3);
        }
        const Matrix3d &Rn = xs.back().block<3,3>(0,0);
        const Vector3d &pxn = xs.back().block<3,1>(0,3);
         
        // Initialize landmark position from estimated state
        p.tail<3>() = Rn*z + pxn;
        pis.push_back(l);
        observed[l] = true;
        cis[l] = p.size()/3-1;
      }

      //        zs[k].push_back();      // add feature l to pose k
      
      Is[k].push_back(make_pair(l, z)); // add feature l to pose k      
      Js[l].push_back(make_pair(k, z)); // add pose k to feature l
      
      //        cout << "k=" << k << " l=" << l << " d=" << d << endl;
    }
  }
}


// add commanded control u, after which true noisy state x occured
template <int _nu>
void Kinbody3dTrack<_nu>::Add2(const Vectorud &u, const Matrix4d &x, double h)
{
  // add noisy controls/state to list, but use true controls/state for generating measurements
  Eigen::Matrix4d xn;
  double t = ts.back();
  

  sys.Step(xn, t, xs.back(), u, h);
  us.push_back(u);
  xs.push_back(xn);
  ts.push_back(t + h);

  sys.Step(xn, t, xos.back(), u, h);
  xos.push_back(xn);
  uos.push_back(u);
  
  // time-stage 
  int k = xs.size()-1;

  /*
  sys.force->fext << 0, .5, 0;
  if (extforce) {
    p.head<2>() << .5, 0;
  }
  */
  
  /* // add noise
  for (int k = 0; k < N; ++k) {
    double t = k*h;
    Get(xs[k+1], r, vd, t+h);
    Vector3d f;
    us[k] = sys.I.cwiseProduct(xs[k+1].second - xs[k].second)/h + sys.force->D.cwiseProduct(xs[k].second);
    // add the constant external force
    us[k].tail<2>() -= xs[k].first.topLeftCorner<2,2>().transpose()*sys.force->fext.tail<2>();
  }
  */

  //  srand (time(NULL));
  
  const Matrix3d &R = x.block<3,3>(0,0);
  const Vector3d &px = x.block<3,1>(0,3);
  
  Is.resize(Is.size()+1);
  assert(k == Is.size()-1);

  for (int l = 0; l < ls.size(); ++l) {
    const Vector3d &pf = ls[l];
    //p.segment<2>(2*extforce + 2*l);
    
    double d = (px - pf).norm();
    if (d < dmax) {

      Vector3d z = R.transpose()*(pf - px);
      z(0) += sqrt(cp)*random_normal();
      z(1) += sqrt(cp)*random_normal();
      z(2) += sqrt(cp)*random_normal();
      
      // add to feature vector if not observed
      if (!observed[l]) {
        if (!init) {
          p.resize(3);
          init= true;
        } else {
          p.conservativeResize(p.size() + 3);
        }
        const Matrix3d &Rn = xs.back().block<3,3>(0,0);
        const Vector3d &pxn = xs.back().block<3,1>(0,3);
         
        // Initialize landmark position from estimated state
        p.tail<3>() = Rn*z + pxn;
        // Initialize landmark position with its true position
        //p.tail<3>() = pf;
        pis.push_back(l);
        observed[l] = true;
        cis[l] = p.size()/3-1;
      }
      
      Is[k].push_back(make_pair(l, z)); // add feature l to pose k      
      Js[l].push_back(make_pair(k, z)); // add pose k to feature l
      
      //        cout << "k=" << k << " l=" << l << " d=" << d << endl;
    }
  }
}


// Makes the ground truth features to be observed
template <int _nu>
void Kinbody3dTrack<_nu>::MakeTrue()
{
  //  srand (time(NULL));
  //  int nf = (p.size() - extforce*2)/2;
  int i = 0;  
  for (int l =0; l < ls.size();) {
    double a = RND*2*M_PI;
    if (a>M_PI && a<1.5*M_PI || a>0 && a<M_PI/2)
      continue;
    double z = (RND-0.5)*1.5*this->h;
    double r_rand = 3*w*(RND-0.5) + r; 
    ls[l] = Vector3d(r_rand*cos(a), r_rand*sin(a), z);
    ++l;
  }
}


// Get the desired state on the track for some time t
template <int _nu>
void Kinbody3dTrack<_nu>::Get(Matrix4d &x, double vd, double t) const
{
  double a = 1.3*t/tf*2*M_PI;
  x.setIdentity();
  x(0,0) = cos(a+M_PI/2.); x(0,1) = -sin(a+M_PI/2.);
  x(1,0) = sin(a+M_PI/2.); x(1,1) = cos(a+M_PI/2.);
 
  x(0,3) = r*cos(a); x(1,3) = r*sin(a); x(2,3) = 0;
}
}

#endif