intro to ceres

Problem ceres_problem;
CostFunction* cost_function =
      new AutoDiffCostFunction<CostFunctor, 1, PARAM_NUM>(
          new CostFunctor(problem.s0, problem.s1));
ceres_problem.AddResidualBlock(cost_function, NULL, opt_params.data());
Solver::Options options;
Solver::Summary summary;
Solve(options, &ceres_problem, &summary);

// 常量设置
problem->SetParameterBlockConstant(&pose_start_iter->second.x);
void Problem::SetParameterLowerBound(double *values, int index, double lower_bound);                                              
void Problem::SetParameterUpperBound(double *values, int index, double upper_bound);

// define cost function
struct CostFunctor {
  template <typename T>
  bool operator()(const T* const x, T* residual) const {
    residual[0] = 10.0 - x[0];
    return true;
  }
};

CostFunction* cost_function
    = new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>(
        new MyScalarCostFunctor(1.0));              ^  ^  ^
                                                    |  |  |
                        Dimension of residual ------+  |  |
                        Dimension of x ----------------+  |
                        Dimension of y -------------------+

/ derivative: http://ceres-solver.org/gradient_tutorial.html
// 1. autodiff
//      - define template operator()
//      - add 
return new ceres::AutoDiffFirstOrderFunction<Rosenbrock, kNumParameters>(
        new Rosenbrock);
// 2. numeric differentiation
//      - define simple double operator()
//      - add 
return new ceres::NumericDiffFirstOrderFunction<Rosenbrock,
                                                    ceres::CENTRAL,
                                                    kNumParameters>(
        new Rosenbrock);

// 3. compute derivative by hand
// f(x,y) = (1-x)^2 + 100(y - x^2)^2;
class Rosenbrock final  : public ceres::FirstOrderFunction {
  public:
    ~Rosenbrock() override {}

    bool Evaluate(const double* parameters,
                           double* cost,
                           double* gradient) const override {
       const double x = parameters[0];
       const double y = parameters[1];

       cost[0] = (1.0 - x) * (1.0 - x) + 100.0 * (y - x * x) * (y - x * x);
       if (gradient) {
         gradient[0] = -2.0 * (1.0 - x) - 200.0 * (y - x * x) * 2.0 * x;
         gradient[1] = 200.0 * (y - x * x);
       }
      return true;
   }

   int NumParameters() const override { return 2; }
};

// mass add
ceres::Problem problem;
for (int i = 0; i < bal_problem.num_observations(); ++i) {
  ceres::CostFunction* cost_function =
      SnavelyReprojectionError::Create(
           bal_problem.observations()[2 * i + 0],
           bal_problem.observations()[2 * i + 1]);
  problem.AddResidualBlock(cost_function,
                           nullptr /* squared loss */,
                           bal_problem.mutable_camera_for_observation(i),
                           bal_problem.mutable_point_for_observation(i));
}

//  change
problem.AddResidualBlock(cost_function, nullptr , &m, &c);
//  to
problem.AddResidualBlock(cost_function, new CauchyLoss(0.5) , &m, &c);
//  to add kernel loss function

class Manifold {
 public:
  virtual ~Manifold();
  virtual int AmbientSize() const = 0;
  virtual int TangentSize() const = 0;
  virtual bool Plus(const double* x,
                    const double* delta,
                    double* x_plus_delta) const = 0;
  virtual bool PlusJacobian(const double* x, double* jacobian) const = 0;
  virtual bool RightMultiplyByPlusJacobian(const double* x,
                                           const int num_rows,
                                           const double* ambient_matrix,
                                           double* tangent_matrix) const;
  virtual bool Minus(const double* y,
                     const double* x,
                     double* y_minus_x) const = 0;
  virtual bool MinusJacobian(const double* x, double* jacobian) const = 0;
};

ceres::Manifold* angle_manifold = AngleManifold::Create();
problem->SetManifold(&yaw_radians, angle_manifold);


// Defines a manifold for updating the angle to be constrained in [-pi to pi).
class AngleManifold {
 public:
  template <typename T>
  bool Plus(const T* x_radians,
            const T* delta_radians,
            T* x_plus_delta_radians) const {
    *x_plus_delta_radians = NormalizeAngle(*x_radians + *delta_radians);
    return true;
  }

  template <typename T>
  bool Minus(const T* y_radians,
             const T* x_radians,
             T* y_minus_x_radians) const {
    *y_minus_x_radians =
        NormalizeAngle(*y_radians) - NormalizeAngle(*x_radians);

    return true;
  }

  static ceres::Manifold* Create() {
    return new ceres::AutoDiffManifold<AngleManifold, 1, 1>;
  }
};