Method OptimizeZRollPitch

sloam/src/core/sloam.cpp:116–172

  }

  void sloam::OptimizeZRollPitch(const SE3 &poseEstimate, const bool optimize, const std::vector<ObjectMatch<Plane>> &allGMatch, 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 x, y and yaw as constant
      ceres::SubsetParameterization *subset_parameterization =
        new ceres::SubsetParameterization(6, {0, 1, 5});
      problem.SetParameterization(params, subset_parameterization);

      for (auto gMatch : allGMatch)
      {
        double weight = 1.0;
        ceres::CostFunction *cost =
            new ceres::AutoDiffCostFunction<ZRollPitchPlaneCost, 1, 6>(
                new ZRollPitchPlaneCost(gMatch.feature, gMatch.object.model.plane, weight));
        auto rbid = 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);
      // if converged
      success = summary.termination_type == 0;
    }

    double output[3];
    if (optimize && success)
    {
      out[0] = params[2];
      out[1] = params[3];
      out[2] = params[4];
    } else
    {
      out[0] = t[2];
      out[1] = rpy[0];
      out[2] = rpy[1];
    }
  }