1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
// 定义一个 cost function 结构体
struct CostFunctor {
  // 重载() 传入待优化参数和残差
  template <typename T>
  bool operator()(const T* const x, T* residual) const {
    residual[0] = T(10.0) - x[0];
    return true;
  }
};

int main(int argc, char** argv) {
  google::InitGoogleLogging(argv[0]);

  // The variable to solve for with its initial value.
  const double initial_x = 5.0;
  double x = initial_x;

  // Build the problem.
  Problem problem;

  // Set up the only cost function (also known as residual). This uses
  // auto-differentiation to obtain the derivative (jacobian).
  // 参数：CostFunctor结构体，残差residual维度, 第一类待优化参数维度, 第二类待优化参数维度, ...
  CostFunction* cost_function =
      new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
    
  // 参数依次是 cost_function loss_function, 第一类待优化参数, 第二类待优化参数, ...
  problem.AddResidualBlock(cost_function, NULL, &x);

  // Run the solver!
  Solver::Options options;
  options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = true;

  // 开始求解
  Solver::Summary summary;
  Solve(options, &problem, &summary);
		
  // 输出log  
  std::cout << summary.BriefReport() << "\n";
  std::cout << "x : " << initial_x << " -> " << x << "\n";
  return 0;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
struct CurveFittingCost {
  CurveFittingCost(double x, double y) : x_(x), y_(y) {}

  /**
   * @param[in] abc: 模型参数，是3维的
   * @param[out] residual: 残差，1维
   */
  template <typename T>
  bool operator()(const T* const abc, T* residual) const {
    /* error = y - exp(a*x^2 + b*x + c) */
    residual[0] =
        T(y_) - ceres::exp(abc[0] * T(x_) * T(x_) + abc[1] * T(x_) + abc[2]);
    return true;
  }

 private:
  const double x_, y_;
};

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
  /// abc参数的初始估计值
  double abc_hat[3] = {0, 0, 0};  
  /// 构建最小二乘问题
  ceres::Problem problem;
  for (int i = 0; i < samples_num; i++) {
    // 添加误差项，参数：使用自动求导，模板参数：误差函数，残差维度，待估计参数维度
    auto cost_func = new ceres::AutoDiffCostFunction<CurveFittingCost, 1, 3>(
        new CurveFittingCost(x_sample[i], y_sample[i]));
    // 向问题中添加误差项，参数：误差函数，核函数(nullptr 表示为空)，待估计参数
    problem.AddResidualBlock(cost_func, nullptr, abc_hat);
  }

  /// 配置求解器
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = true;

1
2
3
4
5
6
7
8
9
10
11
12
13
/*
* 数据集格式：  
* 第一行 相机数 点数 观测结果数  
* 其余行 相机角标 点角标 观测结果  
* 最后是 参数，即待优化项的初始值  
*     参数每行一个  
*        前面是相机参数，每9个一组，为一个相机的参数  
*            相机的姿态  
*            相机的位置  
*            相机的内参  
*        后面是点的参数，每3个一组，为一个点的参数  
*            点的xyz坐标
*/

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
// Templated pinhole camera model for used with Ceres.  The camera is
// parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for
// focal length and 2 for radial distortion. The principal point is not modeled
// (i.e. it is assumed be located at the image center).
// 重投影误差
struct SnavelyReprojectionError {
  // 传入约束数值，常量，不会被优化
  SnavelyReprojectionError(double observed_x, double observed_y)
      : observed_x(observed_x), observed_y(observed_y) {}
    
  // 传入被优化的参数和残差
  template <typename T>
  bool operator()(const T* const camera, const T* const point,
                  T* residuals) const {
    // camera[0,1,2] are the angle-axis rotation.
    T p[3];
    ceres::AngleAxisRotatePoint(camera, point, p);
    // camera[3,4,5] are the translation.
    p[0] += camera[3];
    p[1] += camera[4];
    p[2] += camera[5];
    // Compute the center of distortion. The sign change comes from
    // the camera model that Noah Snavely's Bundler assumes, whereby
    // the camera coordinate system has a negative z axis.
    T xp = -p[0] / p[2];
    T yp = -p[1] / p[2];
    // Apply second and fourth order radial distortion.
    const T& l1 = camera[7];
    const T& l2 = camera[8];
    T r2 = xp * xp + yp * yp;
    T distortion = 1.0 + r2 * (l1 + l2 * r2);
    // Compute final projected point position.
    const T& focal = camera[6];
    T predicted_x = focal * distortion * xp;
    T predicted_y = focal * distortion * yp;
    // The error is the difference between the predicted and observed position.
    residuals[0] = predicted_x - observed_x;
    residuals[1] = predicted_y - observed_y;
    return true;
  }
  // Factory to hide the construction of the CostFunction object from
  // the client code.
  static ceres::CostFunction* Create(const double observed_x,
                                     const double observed_y) {
    // 残差的维度2，相机参数维度9，点参数维度3
    return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
        new SnavelyReprojectionError(observed_x, observed_y)));
  }
  double observed_x;
  double observed_y;
};

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
  const double* observations = bal_problem.observations();
  // Create residuals for each observation in the bundle adjustment problem. The
  // parameters for cameras and points are added automatically.
  ceres::Problem problem;
  for (int i = 0; i < bal_problem.num_observations(); ++i) {
    // Each Residual block takes a point and a camera as input and outputs a 2
    // dimensional residual. Internally, the cost function stores the observed
    // image location and compares the reprojection against the observation.
    ceres::CostFunction* cost_function = SnavelyReprojectionError::Create(
        observations[2 * i + 0], 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));
  }
  // Make Ceres automatically detect the bundle structure. Note that the
  // standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower
  // for standard bundle adjustment problems.
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  options.minimizer_progress_to_stdout = true;

1
2
VERTEX_SE2 ID x_meters y_meters yaw_radians
EDGE_SE2 ID_A ID_B A_x_B A_y_B A_yaw_B I_11 I_12 I_13 I_22 I_23 I_33

1
2
VERTEX_SE3:QUAT ID x y z q_x q_y q_z q_w
EDGE_SE3:QUAT ID_a ID_b x_ab y_ab z_ab q_x_ab q_y_ab q_z_ab q_w_ab I_11 I_12 I_13 ... I_16 I_22 I_23 ... I_26 ... I_66 // NOLINT

1
2
3
4
         Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();  
         information(0,0) = information(1,1) = information(2,2) = 100;  
         information(3,3) = information(4,4) = information(5,5) = 100;  
         edge->setInformation( information );  

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
class PoseGraph2dErrorTerm {  
 public:  
  PoseGraph2dErrorTerm(double x_ab,  
                       double y_ab,  
                       double yaw_ab_radians,  
                       const Eigen::Matrix3d& sqrt_information)  
      : p_ab_(x_ab, y_ab),  
        yaw_ab_radians_(yaw_ab_radians),  
        sqrt_information_(sqrt_information) {}  
  
  template <typename T>  
  bool operator()(const T* const x_a,  
                  const T* const y_a,  
                  const T* const yaw_a,  
                  const T* const x_b,  
                  const T* const y_b,  
                  const T* const yaw_b,  
                  T* residuals_ptr) const {  
    const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a);  
    const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b);  
  
    Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals_map(residuals_ptr);  
  
    residuals_map.template head<2>() =  
        RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) - p_ab_.cast<T>();  
    residuals_map(2) = ceres::examples::NormalizeAngle(  
        (*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));  
  
    // Scale the residuals by the square root information matrix to account for  
    // the measurement uncertainty.    residuals_map = sqrt_information_.template cast<T>() * residuals_map;  
  
    return true;  }  
  
  static ceres::CostFunction* Create(double x_ab,  
                                     double y_ab,  
                                     double yaw_ab_radians,  
                                     const Eigen::Matrix3d& sqrt_information) {  
    return (new ceres::  
                AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1, 1, 1>(  
                    new PoseGraph2dErrorTerm(  
                        x_ab, y_ab, yaw_ab_radians, sqrt_information)));  
  }  
  
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW  
  
 private:  
  // The position of B relative to A in the A frame.  
  const Eigen::Vector2d p_ab_;  
  // The orientation of frame B relative to frame A.  
  const double yaw_ab_radians_;  
  // The inverse square root of the measurement covariance matrix.  
  const Eigen::Matrix3d sqrt_information_;  
};

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
class PoseGraph3dErrorTerm {  
 public:  
  PoseGraph3dErrorTerm(const Pose3d& t_ab_measured,  
                       const Eigen::Matrix<double, 6, 6>& sqrt_information)  
      : t_ab_measured_(t_ab_measured), sqrt_information_(sqrt_information) {}  
  
  template <typename T>  
  bool operator()(const T* const p_a_ptr,  
                  const T* const q_a_ptr,  
                  const T* const p_b_ptr,  
                  const T* const q_b_ptr,  
                  T* residuals_ptr) const {  
    Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_a(p_a_ptr);  
    Eigen::Map<const Eigen::Quaternion<T>> q_a(q_a_ptr);  
  
    Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_b(p_b_ptr);  
    Eigen::Map<const Eigen::Quaternion<T>> q_b(q_b_ptr);  
  
    // Compute the relative transformation between the two frames.  
    Eigen::Quaternion<T> q_a_inverse = q_a.conjugate();  
    Eigen::Quaternion<T> q_ab_estimated = q_a_inverse * q_b;  
  
    // Represent the displacement between the two frames in the A frame.  
    Eigen::Matrix<T, 3, 1> p_ab_estimated = q_a_inverse * (p_b - p_a);  
  
    // Compute the error between the two orientation estimates.  
    Eigen::Quaternion<T> delta_q =  
        t_ab_measured_.q.template cast<T>() * q_ab_estimated.conjugate();  
  
    // Compute the residuals.  
    // [ position         ]   [ delta_p          ]    // [ orientation (3x1)] = [ 2 * delta_q(0:2) ]    Eigen::Map<Eigen::Matrix<T, 6, 1>> residuals(residuals_ptr);  
    residuals.template block<3, 1>(0, 0) =  
        p_ab_estimated - t_ab_measured_.p.template cast<T>();  
    residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec();  
  
    // Scale the residuals by the measurement uncertainty.  
    residuals.applyOnTheLeft(sqrt_information_.template cast<T>());  
  
    return true;  }  
  
  static ceres::CostFunction* Create(  
      const Pose3d& t_ab_measured,  
      const Eigen::Matrix<double, 6, 6>& sqrt_information) {  
    return new ceres::AutoDiffCostFunction<PoseGraph3dErrorTerm, 6, 3, 4, 3, 4>(  
        new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information));  
  }  
  
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW  
  
 private:  
  // The measurement for the position of B relative to A in the A frame.  
  const Pose3d t_ab_measured_;  
  // The square root of the measurement information matrix.  
  const Eigen::Matrix<double, 6, 6> sqrt_information_;  
};

1
AutoDiffCostFunction

1
2
3
4
5
6
7
8
struct CostFunctor {
  template <typename T>
  bool operator()(const T* const x, T* residual) const {
    residual[0] = T(10.0) - x[0];
    return true;
  }
};

1
2
3
  CostFunction* cost_function =
      new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
  problem.AddResidualBlock(cost_function, NULL, &x);

1
AutoDiffCostFunction

1
2
3
4
5
6
struct NumericDiffCostFunctor {
  bool operator()(const double* const x, double* residual) const {
    residual[0] = 10.0 - x[0];
    return true;
  }
};

1
2
3
4
CostFunction* cost_function =
  new NumericDiffCostFunction<NumericDiffCostFunctor, ceres::CENTRAL, 1, 1>(
      new NumericDiffCostFunctor);
problem.AddResidualBlock(cost_function, nullptr, &x);

1
2
3
CostFunction* cost_function =
    new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
problem.AddResidualBlock(cost_function, nullptr, &x);

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
class QuadraticCostFunction : public ceres::SizedCostFunction<1, 1> {
 public:
  virtual ~QuadraticCostFunction() {}
  virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) const {
    const double x = parameters[0][0];
    residuals[0] = 10 - x;

    // Compute the Jacobian if asked for.
    if (jacobians != nullptr && jacobians[0] != nullptr) {
      jacobians[0][0] = -1;
    }
    return true;
  }
};

1
2
3
4
5
6
7
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);

// brief report
std::cout << summary.BriefReport() << std::endl;
// full log
std::cout << summary.FullReport() << std::endl;

1
2
3
4
5
6
ceres::Solver::Options options;
options.max_num_iterations = 100; // 最大迭代次数
options.gradient_tolerance = 1e-16; // 迭代收敛条件
options.function_tolerance = 1e-16; // 迭代收敛条件
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; // 求解器的种类
options.num_linear_solver_threads = 4; // 求解线程

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
enum LinearSolverType {
  // These solvers are for general rectangular systems formed from the
  // normal equations A'A x = A'b. They are direct solvers and do not
  // assume any special problem structure.

  // Solve the normal equations using a dense Cholesky solver; based
  // on Eigen.
  DENSE_NORMAL_CHOLESKY,

  // Solve the normal equations using a dense QR solver; based on
  // Eigen.
  DENSE_QR,

  // Solve the normal equations using a sparse cholesky solver; requires
  // SuiteSparse or CXSparse.
  SPARSE_NORMAL_CHOLESKY,

  // Specialized solvers, specific to problems with a generalized
  // bi-partitite structure.

  // Solves the reduced linear system using a dense Cholesky solver;
  // based on Eigen.
  DENSE_SCHUR,

  // Solves the reduced linear system using a sparse Cholesky solver;
  // based on CHOLMOD.
  SPARSE_SCHUR,

  // Solves the reduced linear system using Conjugate Gradients, based
  // on a new Ceres implementation.  Suitable for large scale
  // problems.
  ITERATIVE_SCHUR,

  // Conjugate gradients on the normal equations.
  CGNR
};

1
2
3
4
5
6
7
8
9
10
TrivialLoss
HuberLoss
SoftLOneLoss
CauchyLoss
ArctanLoss
TolerantLoss
TukeyLoss
ComposedLoss
ScaledLoss
LossFunctionWrapper

1
2
3
4
    problem->AddResidualBlock(
        cost_function, loss_function, &pose_begin_iter->second.x,
        &pose_begin_iter->second.y, &pose_end_iter->second.x,
        &pose_end_iter->second.y);

1
2
3
4
  std::map<int, Point2d>::iterator pose_start_iter = poses->begin();
  CHECK(pose_start_iter != poses->end()) << "There are no poses.";
  problem->SetParameterBlockConstant(&pose_start_iter->second.x);
  problem->SetParameterBlockConstant(&pose_start_iter->second.y);