Mixing local parameterizations and upper/lower bounds in ceres-solver resulting in drifting away from initial manifold

mix_local_parameterizations_bounds.cpp

#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;
}