kinrccarpath.h

Go to the documentation of this file.

#ifndef GCOP_KINRCCARPATH_H
#define GCOP_KINRCCARPATH_H

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

namespace gcop {
  
  using namespace std;
  using namespace Eigen;

  
  class KinRccarPath {
  public:
    KinRccarPath(KinRccar &sys, bool wheel_odometry = false, bool yaw_odometry = false, bool forces = false, bool estimate_length = false);

    virtual void AddControl(const Vector2d &u, const Vector2d& v, double h);
    virtual void AddObservation(const vector< pair<int, Vector3d> > &z, Matrix4d cam_transform = MatrixXd::Identity(4,4));

    KinRccar &sys;     

    bool wheel_odometry;
    bool yaw_odometry;
    bool extforce;         
    bool forces;           
    bool estimate_length;  
    
    vector<double> ts;     

    vector<Matrix4d> xs;      
    vector<Vector2d> us;   
    vector<Vector2d> vs;   
    
    vector<Matrix4d> xos;     
    vector<Vector2d> uos;     

    VectorXd p;            

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

    double cp;             
    Vector2d cv;           
    Vector2d cw;           

  };

KinRccarPath::KinRccarPath(KinRccar &sys, bool wheel_odometry, bool yaw_odometry, bool forces, bool estimate_length) : 
  sys(sys), wheel_odometry(wheel_odometry), yaw_odometry(yaw_odometry), forces(forces), extforce(false), estimate_length(estimate_length),  p(estimate_length + extforce*3)
{
  Matrix4d x = MatrixXd::Identity(4,4);
  xs.push_back(x);
  xos.push_back(x);
  ts.push_back(0);

  cw = Vector2d(.01, pow(1.5*M_PI/180.,2));
  cv = Vector2d(.01, pow(2.5*M_PI/180.,2));
  cp = .01;

  if(estimate_length)
  {
    p(0) = .25;    
  }
}

void KinRccarPath::AddControl(const Vector2d &u, const Vector2d& v, double h)
{
  Eigen::Matrix4d xn;
  double t = ts.back();

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

  sys.Step(xn, t, xos.back(), u, h);
  xos.push_back(xn);
  uos.push_back(u);

  vs.push_back(v);
  ts.push_back(t + h);
  zs.push_back(vector<pair<int, Vector3d> >());
}

void KinRccarPath::AddObservation(const vector< pair<int, Vector3d> > &z, Matrix4d cam_transform)
{
  for(int i = 0; i < z.size(); i++)
  {
    Vector4d z_h(z[i].second(0), z[i].second(1), z[i].second(2), 1);
    z_h = cam_transform*z_h;
    pair<int, Vector3d> zi(z[i].first, Vector3d(z_h(0), z_h(1), z_h(2)));
    if(3*z[i].first >= p.size()-estimate_length-extforce*3)
    {
        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*zi.second + pxn;
    } 
    zs.back().push_back(zi);
  }
}

} // namespace gcop
#endif