使用Ceres优化重投影误差进行相机标定（仅个人笔记）

重投影误差的类

class ProjectErrorCostFunctionPinehole
{
public:
	ProjectErrorCostFunctionPinehole(const Eigen::Vector3d obj_pt, const Eigen::Vector2d img_pixel) : m_obj_pt(obj_pt), m_img_pixel(img_pixel){}

	template <typename T>
	bool operator()(const T* const camK, const T* const camD, const T* const camQvec, const T* const camTvec, T* residuals) const 
    {
        // 上面的参数类型为数组

        // 以下步骤为将世界坐标点转换为相机坐标点再投影为像素点
		T fx = camK[0];
		T fy = camK[1];
		T cx = camK[2];
		T cy = camK[3];

		T k1 = camD[0];
		T k2 = camD[1];
		T p1 = camD[2];
		T p2 = camD[3];
		T k3 = camD[4];


		Eigen::Quaternion<T> qvec(camQvec);
		Eigen::Matrix<T, 3, 1> tvec;
		tvec << camTvec[0], camTvec[1], camTvec[2];

		Eigen::Matrix<T, 3, 1> obj = m_obj_pt.cast<T>();
		Eigen::Matrix<T, 2, 1> img = m_img_pixel.cast<T>();

		Eigen::Matrix<T, 3, 1> obj_cam_coor = qvec.toRotationMatrix() * obj + tvec;
		T x = obj_cam_coor(0);
		T y = obj_cam_coor(1);
		T z = obj_cam_coor(2);

		T a = x / z;
		T b = y / z;
		T r2 = (a * a + b * b);
		T r4 = r2 * r2;
		T r6 = r2 * r4;

		T xd = a*(1.0 + k1*r2 + k2*r4 + k3*r6) + 2.0 * p1*a*b + p2*(r2 + 2.0 * a*a);
		T yd = b*(1.0 + k1*r2 + k2*r4 + k3*r6) + 2.0 * p2*a*b + p1*(r2 + 2.0 * b*b);

		T ud = fx * xd + cx;
		T vd = fy * yd + cy;

        // 残差 二维
		residuals[0] = ud - img(0);
		residuals[1] = vd - img(1);

		return true;
	}
    
    // 生成误差函数
	static ceres::CostFunction* Create(Eigen::Vector3d obj_pt, Eigen::Vector2d img_pixel
    {        // 自动求导模板参数：误差类型，输出维度，输入维度
		return (new ceres::AutoDiffCostFunction<ProjectErrorCostFunctionPinehole, 2, 4, 5, 4, 3>(new ProjectErrorCostFunctionPinehole(obj_pt, img_pixel)));
    }

	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
	const Eigen::Vector3d m_obj_pt;
	const Eigen::Vector2d m_img_pixel;

};

 当你使用opencv标定函数求出K，D以及R，t后，再使用Ceres进行优化

	ceres::Problem problem;
	ceres::LocalParameterization* qvec_parameterization = new ceres::EigenQuaternionParameterization;
    // obj_pts为两层嵌套的Vector容器，储存不同角度拍摄的棋盘格角点
	for (int i = 0; i < obj_pts.size(); i++)
	{
		for (int j = 0; j < obj_pts[i].size(); j++)
		{
			Eigen::Vector3d obj(obj_pts[i][j].x, obj_pts[i][j].y, obj_pts[i][j].z);
			Eigen::Vector2d img(img_pts[i][j].x, img_pts[i][j].y);
			ceres::CostFunction* cost_function = ProjectErrorCostFunctionPinehole::Create(obj, img);
			problem.AddResidualBlock(cost_function, new ceres::CauchyLoss(1.0), camK, camD, qvec_list[i].coeffs().data(), tvec_list[i].data());
		}
		problem.SetParameterization(qvec_list[i].coeffs().data(), qvec_parameterization);
	}
	
	ceres::Solver::Options options;
	options.minimizer_progress_to_stdout = true;
	options.max_num_iterations = 200;
	options.function_tolerance = 1e-9;
	options.gradient_tolerance = 1e-9;
	options.parameter_tolerance = 1e-9;

	ceres::Solver::Summary summary;
	ceres::Solve(options, &problem, &summary);
	std::cout << summary.BriefReport() << "\n";