Method OptimizeXYYaw

sloam/src/core/sloam.cpp:55–114

  }

  void sloam::OptimizeXYYaw(const SE3 &poseEstimate, const bool optimize, const std::vector<ObjectMatch<Cylinder>> &allTMatch, double* out)
  {

    auto t = poseEstimate.translation();
    auto q = poseEstimate.unit_quaternion();
    double quat[4] = {q.w(), q.x(), q.y(), q.z()};
    double rpy[3];
    ceres::QuaternionToAngleAxis(quat, rpy);
    double params[6] = {t[0], t[1], t[2], rpy[0], rpy[1], rpy[2]};

    bool success = true;
    if(optimize)
    {
      ceres::LossFunction *loss = NULL;
      loss = new ceres::HuberLoss(0.1);
      ceres::Problem::Options problem_options;
      ceres::Problem problem(problem_options);

      problem.AddParameterBlock(params, 6);
      // setting z, roll and pitch as constant
      ceres::SubsetParameterization *subset_parameterization =
        new ceres::SubsetParameterization(6, {2, 3, 4});
      problem.SetParameterization(params, subset_parameterization);

      for (auto tMatch : allTMatch)
      {
        Vector3 root = tMatch.object.model.root;
        Vector3 ray = tMatch.object.model.ray;
        double radius = (double)tMatch.object.model.radius;
        double weight = 1.0;
        ceres::CostFunction *cost =
            new ceres::AutoDiffCostFunction<XYYawCylinderCost, 1, 6>(
                new XYYawCylinderCost(tMatch.feature, root, ray, radius, weight));
        problem.AddResidualBlock(cost, loss, params);
      }

      ceres::Solver::Options options;
      options.max_num_iterations = 50;

      options.linear_solver_type = ceres::DENSE_QR;
      options.logging_type = ceres::SILENT;
      ceres::Solver::Summary summary;
      ceres::Solve(options, &problem, &summary);
      success = summary.termination_type == 0;
    }
    
    // bool x_diff = abs(abs(params[0]) - abs(t[0])) < 0.5;
    // bool y_diff = abs(abs(params[1]) - abs(t[1])) < 0.5;
    if (optimize && success)
    {
      out[0] = params[0];
      out[1] = params[1];
      out[2] = params[5];
    } else
    {
      out[0] = t[0];
      out[1] = t[1];
      out[2] = rpy[2];