Ceresカメラ標定最適化テストdemo

#include 
#include 
#include 
#include 
#include 
#include 

struct cost_function_define
{
	cost_function_define(Point3d p1, Point2d p2) :_p1(p1), _p2(p2) {}
	template
	bool operator()(const T* const cere_r, const T* const cere_t, T* residual ) const
	{
		T p_1[3];
		T p_2[3];
		p_1[0] = T(_p1.x);
		p_1[1] = T(_p1.y);
		p_1[2] = T(_p1.z);
		ceres::AngleAxisRotatePoint(cere_r, p_1, p_2);

		p_2[0] = p_2[0] + cere_t[0];
		p_2[1] = p_2[1] + cere_t[1];
		p_2[2] = p_2[2] + cere_t[2];

		const T x = p_2[0] / p_2[2];
		const T y = p_2[1] / p_2[2];

		// k
		const T u = x * 2.5369888929649419e+03 + 1.0405942910231893e+03; // 1/dx * x + u0 
		const T v = y * 2.5381142635722595e+03 + 8.0521116119573003e+02; // 1/dy * y + v0

		
		residual[0] = u - T(_p2.x);
		residual[1] = v - T(_p2.y);
		return true;
	}
	Point3d _p1;
	Point2d _p2;
};

int main()
{
	//get p1  ， 10 
	int m_cout = 0;
	std::vector<:point3d> xyz;
	for (int i = 0; i <8; i++) {
		for (int j = 0; j < 11; j++) {
			while (m_cout < 10)
			{
				xyz.push_back(cv::Point3d(float(j * 30), float(i * 30), 0));
				m_cout++;
			}	
		}
	}
	//get p2   
	std::vector<:point2d> uv;
	uv.push_back(Point2d(1909.77, 876.49));
	uv.push_back(Point2d(1821.21, 874.34));
	uv.push_back(Point2d(1733.04, 872.244));
	uv.push_back(Point2d(1645.1, 870.194));
	uv.push_back(Point2d(1557.51, 868.058));

	uv.push_back(Point2d(1470.14, 865.96));
	uv.push_back(Point2d(1383.11, 864.019));
	uv.push_back(Point2d(1296.37, 861.854));
	uv.push_back(Point2d(1209.98, 859.821));
	uv.push_back(Point2d(1124.06, 857.769));
	// r，t 
	double r[3] = { -1.3650572646848666e-01, 2.1781412792101808e-02,-3.1170656409694666e+00 };
	double t[3] = { 2.9807767560664848e+02, 2.3489402214480698e+01,8.3744422992667978e+02 };

	// 
	ceres::Problem problem;
	for (int i = 0; i < 10; i++)
	{
		problem.AddResidualBlock(
			new ceres::AutoDiffCostFunction(
				new cost_function_define(xyz[i], uv[i])
				),
			nullptr,
			r,
			t

		);
	}

	// 
	ceres::Solver::Options options;
	options.linear_solver_type = ceres::DENSE_QR;
	options.minimizer_progress_to_stdout = true;

	ceres::Solver::Summary summary;
	chrono::steady_clock::time_point  t1 = chrono::steady_clock::now();
	ceres::Solve(options, &problem, &summary);

	chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
	chrono::duration time_used = chrono::duration_cast<:duration>>(t2 - t1);
	cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
	cout << r[0]<