iSAM update stuck

[Göktuğ Yıldırım]

My aim is to incrementally update my variables in the Visual Odometry task. When I try to solve my graph using the LevenbergMarguard solver, it solves, but when I try to update iSAM with the same graph, iSAM is stuck. Here is my code snippet stuff:

###################################################

iSAM::iSAM(const Config &config, MapSharedPtr& map_ptr)
: m_config(config), m_K(gtsam::Cal3_S2::shared_ptr(new gtsam::Cal3_S2(config.camera.fx,
  config.camera.fy, 0,config.camera.cx, config.camera.cy))),
  m_isam(3)
{
  m_map_ptr = map_ptr;
  m_measurement_noise = gtsam::noiseModel::Isotropic::Sigma(2, 1.0);
  // Initialize iSAM:
  std::vector<EdgeXYSharedPtr> edges = m_map_ptr->get_edges_of_cam(0);
  if (!edges.empty())
  {
    std::vector<size_t> idx_p3d;
    std::vector<gtsam::Point3> points;

    for (size_t i = 0; i < edges.size(); i++)
    {
      EdgeXYSharedPtr edge = edges[i];
      int id_point3D = edge->get_id_point3D();
      Eigen::MatrixXd mat_point = m_map_ptr->get_map_point(id_point3D);
      gtsam::Point3 point3D(mat_point(0,0), mat_point(1,0), mat_point(2,0));
      points.emplace_back(point3D);
      idx_p3d.push_back(id_point3D);
      m_idx_p3d_store.insert(id_point3D);
      gtsam::Point2 measurement(edge->get_point().x, edge->get_point().y);
      m_graph.emplace_shared<gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> >(measurement,
         m_measurement_noise,gtsam::Symbol('x', 0), gtsam::Symbol('l', id_point3D), m_K);
    }

    // Add prior pose:
    gtsam::Pose3 T0 = gtsam::Pose3::identity();
    // Insert Pose Variable:
    m_initialEstimate.insert(gtsam::Symbol('x', 0), T0);

    auto poseNoise = gtsam::noiseModel::Diagonal::Sigmas(
  (gtsam::Vector(6) << gtsam::Vector3::Constant(0), gtsam::Vector3::Constant(0)).finished());
    m_graph.addPrior(gtsam::Symbol('x', 0), T0, poseNoise);

    // Add a prior on landmark l0
    auto pointNoise = gtsam::noiseModel::Isotropic::Sigma(3, 0.15);
    m_graph.addPrior(gtsam::Symbol('l', 0), points[0], pointNoise);

    // Add initial guesses to all observed landmarks:
    for (size_t i = 0; i < idx_p3d.size(); i++)
      m_initialEstimate.insert(gtsam::Symbol('l', idx_p3d[i]), points[i]);


  } else
  {
    LOG(ERROR) << "No edges in map!";
  }
}



void
iSAM::estimate(const std::vector<int>& tracked_p3d_idx, FrameSharedPtr& frame)
{
  int id_frame = frame->get_frame_id();
  std::vector<EdgeXYSharedPtr> edges = m_map_ptr->get_edges_of_cam(id_frame);
  for (size_t i = 0; i < edges.size(); i++)
  {
    EdgeXYSharedPtr edge = edges[i];
    int id_point3D = edge->get_id_point3D();
    gtsam::Point2 measurement(edge->get_point().x, edge->get_point().y);
    m_graph.emplace_shared<gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> >(measurement,
       m_measurement_noise,gtsam::Symbol('x', id_frame), gtsam::Symbol('l', id_point3D), m_K);
  }

  m_initialEstimate.insert(gtsam::Symbol('x', id_frame), m_map_ptr->get_last_pose_gtsam());

  gtsam::LevenbergMarquardtOptimizer optimizer(m_graph, m_initialEstimate);
  gtsam::Values result = optimizer.optimize();
  auto pose_last = result.at<gtsam::Pose3>(gtsam::Symbol('x', id_frame));
  LOG(INFO) << "pose_last: " << pose_last.translation().transpose();
  LOG(INFO) << "LevenbergMarquardtOptimizer is done. ";

  m_isam.update(m_graph, m_initialEstimate);
  LOG(INFO) << "iSAM is done. ";

  exit(-1);
}

###################################################

The output of this program is:

pose_last: 000.030949 -0.0165851 000.730956

 LevenbergMarquardtOptimizer is done.

How can I solve this problem?