//full BA
// Ceres is CPU based
ceres::Problem problem;
ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
//cout << " begin full BA " << endl;
for (int i = 0; i < frame_num; i++)
{
  //double array for ceres
  c_translation[i][0] = c_Translation[i].x();
  c_translation[i][1] = c_Translation[i].y();
  c_translation[i][2] = c_Translation[i].z();
  c_rotation[i][0] = c_Quat[i].w();
  c_rotation[i][1] = c_Quat[i].x();
  c_rotation[i][2] = c_Quat[i].y();
  c_rotation[i][3] = c_Quat[i].z();
  problem.AddParameterBlock(c_rotation[i], 4, local_parameterization);
  problem.AddParameterBlock(c_translation[i], 3);
  // if met corresponding frame
  if (i == l)
  {
    problem.SetParameterBlockConstant(c_rotation[i]); //Hold the.   i. ndicated parameter block constant during optimization
  }
  if (i == l || i == frame_num - 1)
  {
    problem.SetParameterBlockConstant(c_translation[i]);
  }
}

// https://blog.csdn.net/JH_233/article/details/80181499
for (int i = 0; i < feature_num; i++)
{
  if (sfm_f[i].state != true)
    continue;
  for(int j = 0; j < int(sfm_f[i].observation.size()); j++)
  {
  // camera id obtaining
    int l = sfm_f[i].observation[j].first;
  //. register features corresponding camera id
    ceres::CostFunction* cost_function = ReprojectionError3D::Create(
    sfm_f[i].observation[j].second.x(),
    sfm_f[i].observation[j].second.y());
  // and resgistor camera id's releative pose and rotation
     problem.AddResidualBlock(cost_function, NULL, c_rotation[l],   c_translation[l], sfm_f[i].position);
     // 关于observation point 提出 feature ID的 姿态 in SFM
    // which is making cost function and it will be after work hessian matrix(only coupled pose and rotation and points corresponding caemra id)
  }
}

ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
//options.minimizer_progress_to_stdout = true;
options.max_solver_time_in_seconds = 0.2;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
//std::cout << summary.BriefReport() << "\n";
if (summary.termination_type == ceres::CONVERGENCE || summary.final_cost < 5e-03)
{
//cout << "vision only BA converge" << endl;
}
else
{
//cout << "vision only BA not converge " << endl;
return false;
}

// update all the q and T of Frame
for (int i = 0; i < frame_num; i++)
{
  q[i].w() = c_rotation[i][0];
  q[i].x() = c_rotation[i][1];
  q[i].y() = c_rotation[i][2];
  q[i].z() = c_rotation[i][3];
  q[i] = q[i].inverse();
//cout << "final q" << " i " << i <<" " <<q[i].w() << " " << q[i].vec().transpose() << endl;
}
for (int i = 0; i < frame_num; i++)
{
  T[i] = -1 * (q[i] * Vector3d(c_translation[i][0], c_translation[i][1], c_translation[i][2]));
//cout << "final t" << " i " << i <<" " << T[i](0) <<" "<< T[i](1) <<" "<< T[i](2) << endl;
}
// local mapping
for (int i = 0; i < (int)sfm_f.size(); i++)
{
  if(sfm_f[i].state)
    sfm_tracked_points[sfm_f[i].id] = Vector3d(sfm_f[i].position[0], sfm_f[i].position[1], sfm_f[i].position[2]);
}