Skip to content

Instantly share code, notes, and snippets.

@DmitriyKorchemkin

DmitriyKorchemkin/mix_local_parameterizations_bounds.cpp

Last active October 21, 2021 10:14
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save DmitriyKorchemkin/c506c8f5814f06d16fca49f689f285f3 to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save DmitriyKorchemkin/c506c8f5814f06d16fca49f689f285f3 to your computer and use it in GitHub Desktop.
Download ZIP

Revisions

  1. DmitriyKorchemkin revised this gist Oct 21, 2021. No changes.
  2. DmitriyKorchemkin created this gist Oct 21, 2021.
    100 changes: 100 additions & 0 deletions mix_local_parameterizations_bounds.cpp
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,100 @@
    #include <Eigen/Dense>
    #include <ceres/ceres.h>
    #include <iostream>

    using Vector3d = Eigen::Vector3d;

    struct VectorDiff {
    template <typename T> bool operator()(const T *point, T *residual) const {
    Eigen::Map<const Eigen::Matrix<T, 3, 1>> map(point);
    Eigen::Map<Eigen::Matrix<T, 3, 1>> res(residual);

    res = ref_point - map;
    return true;
    }

    VectorDiff(const Vector3d &ref_point) : ref_point(ref_point) {}
    static ceres::CostFunction *Cost(const Vector3d &ref_point) {
    return new ceres::AutoDiffCostFunction<VectorDiff, 3, 3>(
    new VectorDiff(ref_point));
    }
    const Vector3d ref_point;
    };

    struct NormCheckCallback : ceres::EvaluationCallback {
    void PrepareForEvaluation(bool evaluate_jacobians,
    bool new_evaluation_point) {
    if (!new_evaluation_point)
    return;
    std::cout << "\t\t\tEvaluating at " << ref.transpose()
    << " (norm^2: " << ref.squaredNorm() << ')' << std::endl;
    }

    NormCheckCallback(Vector3d &ref) : ref(ref) {}
    Vector3d &ref;
    };

    int main() {
    const Vector3d ref_point = Vector3d::UnitX();
    const Vector3d init = Vector3d(-1., 0.01, 0.).normalized();
    Vector3d estimate;

    NormCheckCallback norm_check(estimate);
    ceres::Problem::Options problem_options;
    problem_options.evaluation_callback = &norm_check;
    ceres::Problem problem(problem_options);
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;
    options.max_num_iterations = 10;
    ceres::Solver::Summary summary;

    problem.AddParameterBlock(estimate.data(), 3,
    new ceres::HomogeneousVectorParameterization(3));
    problem.AddResidualBlock(VectorDiff::Cost(ref_point), nullptr,
    estimate.data());

    // This should converge (and it will)
    estimate = init;
    std::cout
    << "\n\n\n\n\n"
    << "############################################################\n"
    << "# Test #1: without boundaries #\n"
    << "############################################################\n\n\n";
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << '\n' << summary.FullReport() << '\n';
    std::cout << "Error: " << (ref_point - estimate).transpose() << std::endl;
    std::cout << "Squared norm: " << estimate.squaredNorm() << std::endl;

    // This should stop at one of 4 points (-sqrt(3)/2, +-0.5, 0) (-sqrt(3)/2, 0,
    // +-0.5) But instead norm will degrade at some point
    std::cout
    << "\n\n\n\n\n"
    << "############################################################\n"
    << "# Test #2: with boundaries, starting from feasible point #\n"
    << "############################################################\n\n\n";
    estimate = init;
    problem.SetParameterLowerBound(estimate.data(), 1, -0.5);
    problem.SetParameterUpperBound(estimate.data(), 1, 0.5);
    problem.SetParameterLowerBound(estimate.data(), 2, -0.5);
    problem.SetParameterUpperBound(estimate.data(), 2, 0.5);

    ceres::Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << '\n' << summary.FullReport() << '\n';
    std::cout << "Error: " << (ref_point - estimate).transpose() << std::endl;
    std::cout << "Squared norm: " << estimate.squaredNorm() << std::endl;

    // Starting from infeasible point will degrade norm at the iteration #0
    std::cout
    << "\n\n\n\n\n"
    << "############################################################\n"
    << "# Test #3: with boundaries, starting from infeasible point #\n"
    << "############################################################\n\n\n";
    estimate = Vector3d::UnitY();
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << '\n' << summary.FullReport() << '\n';
    std::cout << "Error: " << (ref_point - estimate).transpose() << std::endl;
    std::cout << "Squared norm: " << estimate.squaredNorm() << std::endl;

    return 0;
    }