g2o test

  g2o::SparseOptimizer optimizer;
  auto linearSolver = g2o::make_unique<SlamLinearSolver>();
  linearSolver->setBlockOrdering(false);
  // g2o::OptimizationAlgorithmGaussNewton *solver = new g2o::OptimizationAlgorithmGaussNewton(
  // g2o::make_unique<SlamBlockSolver>(std::move(linearSolver)));
  g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<SlamBlockSolver>(std::move(linearSolver)));

  optimizer.setAlgorithm(solver); 
 
  // Adding poses
  for (auto it = frames.begin(); it != frames.end(); it++)
  {
      int idx = it - frames.begin();
      std::map<int,int>::iterator loopClosuresIt = loops.find(idx);
      if(loopClosuresIt != loops.end())
          continue;

      Pose pose(keyFrame.poseWorld.toMatrix() * it->poseFromKeyFrame.toMatrix());
      SE2 p = SE2(pose.x, pose.y, pose.t);
      VertexSE2 *drone = new VertexSE2;
      drone->setId(it->id);
      drone->setEstimate(p);
      optimizer.addVertex(drone);
  }

  // Adding the odometry constraints
  for (auto it = frames.begin() + 1; it != frames.end(); it++)
  {
      Frame &prevFrame = *(it - 1);
      Frame &currFrame = *it;

      EdgeSE2 *odometry = new EdgeSE2;
      odometry->vertices()[0] = optimizer.vertex(prevFrame.id);
      odometry->vertices()[1] = optimizer.vertex(currFrame.id);
      std::cout << "Before optimizing Frame : " << currFrame.id << " Odometry " << currFrame.odometry << " Pose " << currFrame.poseFromPrevFrame << std::endl;
      odometry->setMeasurement(SE2(currFrame.odometry.dx, currFrame.odometry.dy, currFrame.odometry.dt));
      odometry->setInformation(Eigen::DiagonalMatrix<double, 3>(currFrame.odometry.varX,currFrame.odometry.varY,currFrame.odometry.varT).inverse());
      odometry->setRobustKernel(new g2o::RobustKernelHuber);
      optimizer.addEdge(odometry);
  }