Method OptimizePose

sloam/src/core/sloam.cpp:174–255

  }

  bool sloam::OptimizePose(const SE3 &poseEstimate,
                           const std::vector<ObjectMatch<Cylinder>> &allTMatch,
                           const std::vector<ObjectMatch<Plane>> &allGMatch,
                           SE3 &tf)
  {

    ceres::LossFunction *loss = NULL;
    loss = new ceres::HuberLoss(0.1);
    ceres::Problem::Options problem_options;
    ceres::Problem problem(problem_options);

    auto t = poseEstimate.translation();
    auto q = poseEstimate.unit_quaternion();
    double para_t[3] = {t[0], t[1], t[2]};
    double para_q[4] = {q.x(), q.y(), q.z(), q.w()};
    ceres::LocalParameterization *q_parameterization =
        new ceres::EigenQuaternionParameterization();

    problem.AddParameterBlock(para_q, 4, q_parameterization);
    problem.AddParameterBlock(para_t, 3);

    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<CylinderCost, 1, 3, 4>(
              new CylinderCost(tMatch.feature, root, ray, radius, weight));
      problem.AddResidualBlock(cost, loss, para_t, para_q);
    }

    std::vector<ceres::ResidualBlockId> residual_block_ids;
    for (auto gMatch : allGMatch)
    {
      double weight = 1.0;
      // auto norm_feature = -(gMatch.feature - gMatch.object.model.centroid);
      auto norm_feature = gMatch.feature;
      ceres::CostFunction *cost =
          new ceres::AutoDiffCostFunction<PlaneCost, 1, 3, 4>(
              new PlaneCost(norm_feature, gMatch.object.model.plane, weight));
      auto rbid = problem.AddResidualBlock(cost, loss, para_t, para_q);
      residual_block_ids.push_back(rbid);
    }

    // SOLVE
    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);

    // ceres::Problem::EvaluateOptions evoptions;
    // evoptions.residual_blocks = residual_block_ids;
    // double total_cost = 0.0;