#include <Eigen/StdVector>
#include <iostream>
#include <stdint.h>


#include "g2o/types/sba/types_six_dof_expmap.h"
#include <cmath>
#include <g2o/types/slam3d/se3quat.h>
#include <g2o/types/slam3d/vertex_se3.h>

using namespace Eigen;
using namespace std;
#define PI 3.14159265

int main(int argc, const char* argv[]){


  /* Poses:
  The camera moves on circle on the z plane
  Create the Gt poses and add them as vertices VertexSE3Expmap
  */
  vector<g2o::SE3Quat,
      aligned_allocator<g2o::SE3Quat> > true_poses;
  int n=20;
  double angle_step=360.0/(double)n/180.0*PI;
  int overall_vertex_counter=0;
  for (int i=0; i<n; i++)
  {
    Vector3d t(sin(angle_step*i),
      -cos(angle_step*i),
      -1 );

    Eigen::Quaterniond q(1.0, 0, 0, 0);
    g2o::SE3Quat pose(q, t);
    true_poses.push_back(pose);

    //create a vertex representing the pose
    g2o::VertexSE3 * v_se3
        = new g2o::VertexSE3();
    v_se3->setId(overall_vertex_counter);
    v_se3->setEstimate(pose);


    if (i<2){
      v_se3->setFixed(true);
    }
	v_se3->setHessianIndex(overall_vertex_counter);

    //optimizer.addVertex(v_se3);
	for (int di = 0; di < v_se3->dimension(); di++)
	{
		std::cout << "hessian(" << di << "," << di << ")=" << v_se3->hessian(di, di) << "\n";
	}
    overall_vertex_counter++;

  }


}