一维标定杆标定双目相机算法实现

目前复现了三种一维标定杆标定算法，其中两种失败（实际做实验出不了效果），目前不清楚问题在哪儿，如果想要代码可以评论该文章，我会发你。
两种失败的算法对应的论文分别是：
A new robust algorithmic for multi-camera calibration with a 1D object under general motions without prior knowledge of any camera intrinsic parameter
论文连接：https://www.sciencedirect.com/science/article/pii/S003132031200163X
Multi-Camera Calibration with One-Dimensional Object under General Motions
论文连接：https://ieeexplore.ieee.org/document/4408994

下面是成功的算法：
红外摄像机组的一维标定物设计和标定
论文连接：https://kns.cnki.net/kcms2/article/abstract?v=fCqJ37DMrBkAtHDzkEpTWzJ3SStzT7roZFRdjaHRRDoty5ycKMs5RsZHjmjl5A-63bJNrqsnA7LiTgVzXa8oK7clRZzs8O4gbrz9AdSGRFVW_IrHzPJ3O1I9YZ3S7bQCQn-wPvMiLGg=&uniplatform=NZKPT&language=CHS

具体实现：
这里只介绍标定算法
实验使用的标定杆是三个球的，如下图所示（论文中的图片）

第一步：计算基础矩阵
求解基础矩阵的博客很多，调用的是opencv里面的方法，这里直接贴出代码

//input1、input2：为1号与2号两台相机拍摄的标定杆上标定球的中心的像素点坐标。数据的排列方式：
//假如1号相机拍摄了80张标定杆图片，那么input1里面的数据为A1B1C1A2B2C2A3B3C3...A80B80C80。
//其中A1B1C1表示为这是第一张图片的A小球的中心坐标像素点、B小球的中心坐标像素点、C小球的中心坐标像素点
//其中A2B2C2表示为这是第二张图片的A小球的中心坐标像素点、B小球的中心坐标像素点、C小球的中心坐标像素点，后面的同理
//2号相机的数据排列与1号相机一样，两台相机的数据要保证对应上。
//F：返回的基础矩阵
//我不知道是否说明清楚，如果有疑问可以评论区交流
void getFundamentalMatrix(std::vector<Eigen::Vector2d> input1,
	std::vector<Eigen::Vector2d> input2,
	Eigen::MatrixXd &F
) {
	std::vector<cv::Point2d> cvPoints1;
	std::vector<cv::Point2d> cvPoints2;
	for (const auto& eigenPoint : input1)
	{
		cvPoints1.emplace_back(eigenPoint.x(), eigenPoint.y());
	}

	for (const auto& eigenPoint : input2)
	{
		cvPoints2.emplace_back(eigenPoint.x(), eigenPoint.y());
	}

	cv::Mat cvF = cv::findFundamentalMat(cvPoints1, cvPoints2, cv::FM_8POINT);

	Eigen::MatrixXd CVF2(3, 3);
	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			CVF2(i, j) = cvF.at<double>(i, j);
		}
	}

	F = CVF2;
}

第二步：计算两台相机的内参
相机的内参为相机的固有参数，不受标定影响，如果你已经有了相机的内参则可以跳过此步骤直接用来估计相机的外参
如果没有相机的内参，则用下面的算法计算估计内参：
相机内参估计的算法论文中有详细介绍，这里贴出论文中该部分算法

//Img1_Width:相机一拍摄图片的宽
//Img1_Height：相机一拍摄图片的高
//Img2_Width:相机二拍摄图片的宽
//Img2_Height：相机二拍摄图片的高
//FundamentalMatrix:基础矩阵
//K1：返回的相机一的内参
//K2：返回的相机二的内参
void getIntrinsics(double Img1_Width,
	double Img1_Height,
	double Img2_Width,
	double Img2_Height,
	Eigen::MatrixXd FundamentalMatrix,
	Eigen::MatrixXd &K1,
	Eigen::MatrixXd &K2
) {
	//相机主点初始值
	double u1 = Img1_Width / 2;
	double v1 = Img1_Height / 2;
	Eigen::VectorXd P1(3, 1);
	P1 << u1, v1, 1;

	double u2 = Img2_Width / 2;
	double v2 = Img2_Height / 2;
	Eigen::VectorXd P2(3, 1);
	P2 << u2, v2, 1;


	//计算极点e1  利用F * e1 = 0
	// 进行奇异值分解
	Eigen::JacobiSVD<Eigen::MatrixXd> svd1(FundamentalMatrix, Eigen::ComputeFullV);
	// 提取零空间（特征值为0对应的特征向量）
	Eigen::VectorXd solution1 = svd1.matrixV().rightCols(1);
	Eigen::Vector3d e1;
	e1 << solution1(0, 0) / solution1(2, 0), solution1(1, 0) / solution1(2, 0), solution1(2, 0) / solution1(2, 0);
	Eigen::MatrixXd E1(3, 3);
	E1 << 0, -e1.z(), e1.y(),
		e1.z(), 0, -e1.x(),
		-e1.y(), e1.x(), 0;


	//计算极点e2  利用F.t * e2 = 0
	// 进行奇异值分解
	Eigen::MatrixXd Ft = FundamentalMatrix.transpose();
	Eigen::JacobiSVD<Eigen::MatrixXd> svd(Ft, Eigen::ComputeFullV);
	// 提取零空间（特征值为0对应的特征向量）
	Eigen::VectorXd solution = svd.matrixV().rightCols(1);
	Eigen::Vector3d e2;
	e2 << solution(0, 0) / solution(2, 0), solution(1, 0) / solution(2, 0), solution(2, 0) / solution(2, 0);
	Eigen::MatrixXd E2(3, 3);
	E2 << 0, -e2.z(), e2.y(),
		e2.z(), 0, -e2.x(),
		-e2.y(), e2.x(), 0;


	Eigen::MatrixXd I(3, 3);
	I << 1, 0, 0,
		0, 1, 0,
		0, 0, 0;


	//第一台相机的焦距
	double f1 = -(
		(P2.transpose() * E2 * I * FundamentalMatrix * P1 * P1.transpose() * FundamentalMatrix.transpose() * P2)(0, 0) /
		(P2.transpose() * E2 * I * FundamentalMatrix * I * FundamentalMatrix.transpose() * P2)(0, 0)
		);

	double f2 = -(
		(P1.transpose() * E1 * I * FundamentalMatrix.transpose() * P2 * P2.transpose() * FundamentalMatrix * P1)(0, 0) /
		(P1.transpose() * E1 * I * FundamentalMatrix.transpose() * I * FundamentalMatrix * P1)(0, 0)
		);

	f1 = sqrt(f1);
	f2 = sqrt(f2);

	Eigen::MatrixXd K1_temp(3, 3);
	K1_temp << f1, 0, u1,
		0, f1, v1,
		0, 0, 1;

	Eigen::MatrixXd K2_temp(3, 3);
	K2_temp << f2, 0, u2,
		0, f2, v2,
		0, 0, 1;

	K1 = K1_temp;
	K2 = K2_temp;
}

第三步：计算两台相机的外参
这里直接贴出论文中该部分算法流程

//K1：相机一内参
//K2：相机二内参
//F：基础矩阵
//points1、points2：为1号与2号两台相机拍摄的标定杆上标定球的中心的像素点坐标。数据排列方式与求解基础矩阵的数据排列一样（就是一个东西）
//AC：标定杆A点与C点的真实距离，单位为mm
//R_out：返回的旋转矩阵
//t_out：返回的平移矩阵
void getRt(Eigen::MatrixXd K1,
	Eigen::MatrixXd K2,
	Eigen::MatrixXd F,
	std::vector<Eigen::Vector2d> points1,
	std::vector<Eigen::Vector2d> points2,
	double AC,
	Eigen::MatrixXd &R_out,
	Eigen::MatrixXd &t_out
) {
	//本征矩阵
	Eigen::MatrixXd E;
	E = K2.transpose() * F * K1;

	Eigen::JacobiSVD<Eigen::MatrixXd> svd(E, Eigen::ComputeThinU | Eigen::ComputeThinV);
	Eigen::MatrixXd C = svd.singularValues();
	Eigen::MatrixXd V = svd.matrixV();
	Eigen::MatrixXd U = svd.matrixU();
	Eigen::MatrixXd S = U.transpose() * E * V.transpose().inverse();

	/*cout << "U " << endl << U << endl << endl;
	cout << "S " << endl << S << endl << endl;
	cout << "V " << endl << V << endl << endl;

	cout << "U * S * V.t: " << endl << U*S*V.transpose() << endl << endl;*/


	Eigen::MatrixXd W(3, 3);
	W << 0, 1, 0,
		-1, 0, 0,
		0, 0, 1;

	Eigen::MatrixXd zze(1, 3);
	zze << 0, 0, 1;

	//四种可能Rt
	Eigen::MatrixXd R1 = U * W * V.transpose();
	Eigen::MatrixXd t1 = U * zze.transpose();

	Eigen::MatrixXd R2 = U * W * V.transpose();
	Eigen::MatrixXd t2 = -U * zze.transpose();

	Eigen::MatrixXd R3 = U * W.transpose() * V.transpose();
	Eigen::MatrixXd t3 = U * zze.transpose();

	Eigen::MatrixXd R4 = U * W.transpose() * V.transpose();
	Eigen::MatrixXd t4 = -U * zze.transpose();

	Eigen::MatrixXd P1(3, 4);
	Eigen::MatrixXd P2(3, 4);

	P1 << 1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, 0;

	P1 = K1 * P1;

	//存储正确的Rt
	Eigen::MatrixXd R;
	Eigen::MatrixXd t;

	for (int i = 0; i < 4; i++) {
		Eigen::MatrixXd R_temp;
		Eigen::MatrixXd t_temp;
		if (i == 0) {
			R_temp = R1;
			t_temp = t1;
		}
		if (i == 1) {
			R_temp = R2;
			t_temp = t2;
		}
		if (i == 2) {
			R_temp = R3;
			t_temp = t3;
		}
		if (i == 3) {
			R_temp = R4;
			t_temp = t4;
		}

		Eigen::MatrixXd P(3, 4);
		P << R_temp(0, 0), R_temp(0, 1), R_temp(0, 2), t_temp(0, 0),
			R_temp(1, 0), R_temp(1, 1), R_temp(1, 2), t_temp(1, 0),
			R_temp(2, 0), R_temp(2, 1), R_temp(2, 2), t_temp(2, 0);
		P = K2 * P;
		std::vector<Eigen::Vector3d> point3D;
		get3Dpoint(points1, points2, P1, P, point3D);
		bool flag = false;
		for (int j = 0; j < point3D.size(); j++) {
			if (point3D[j].z() < 0) {
				flag = true;
				break;
			}
		}
		if (flag == false) {
			R = R_temp;
			t = t_temp;
			break;
		}
	}

	/*cout << "R: " << endl << R << endl << endl;
	cout << "t: " << endl << t << endl << endl;*/

	P2 << R(0, 0), R(0, 1), R(0, 2), t(0, 0),
		R(1, 0), R(1, 1), R(1, 2), t(1, 0),
		R(2, 0), R(2, 1), R(2, 2), t(2, 0);
	P2 = K2 * P2;

	std::vector<Eigen::Vector3d> point3D;

	double lambda = 0;

	double L_Li = 0;

	get3Dpoint(points1, points2, P1, P2, point3D);
	for (int i = 0; i < point3D.size();) {
		Eigen::Vector3d A = point3D[i];
		Eigen::Vector3d C = point3D[i + 2];

		double AC_ = (C - A).norm();

		L_Li += (AC / AC_);

		i += 3;
	}

	lambda = L_Li / (points1.size() / 3);

	t = lambda * t;

	R_out = R;
	t_out = t;
}


//该函数的作用为三维重建，该函数在getRt函数中调用
//input1、input2：为1号与2号两台相机拍摄的标定杆上标定球的中心的像素点坐标。数据排列方式与求解基础矩阵的数据排列一样（就是一个东西）
//P1：相机一的投影矩阵
//P2：相机二的投影矩阵
//projection3Dpoint：返回的三维点
void get3Dpoint(std::vector<Eigen::Vector2d> input1,
	std::vector<Eigen::Vector2d> input2,
	Eigen::MatrixXd P1,
	Eigen::MatrixXd P2,
	std::vector<Eigen::Vector3d> &projection3Dpoint
) {
	if (input1.size() != input2.size()) {
		std::cout << "getFundamentalMatrix函数输入两组点的个数不相同" << std::endl;
		exit(0);
	}
	//求解方程,求解Q
	//q1 = P1Q
	//q2 = P2Q
	for (int i = 0; i < input1.size(); i++) {
		Eigen::MatrixXd A(4, 4);
		for (int j = 0; j < 4;) {
			Eigen::Vector2d point;
			Eigen::MatrixXd P;
			if (j == 0) {
				point = input1[i];
				P = P1;
			}
			else {
				point = input2[i];
				P = P2;
			}

			A(j, 0) = point.x() * P(2, 0) - P(0, 0);
			A(j, 1) = point.x() * P(2, 1) - P(0, 1);
			A(j, 2) = point.x() * P(2, 2) - P(0, 2);
			A(j, 3) = point.x() * P(2, 3) - P(0, 3);

			A(j + 1, 0) = point.y() * P(2, 0) - P(1, 0);
			A(j + 1, 1) = point.y() * P(2, 1) - P(1, 1);
			A(j + 1, 2) = point.y() * P(2, 2) - P(1, 2);
			A(j + 1, 3) = point.y() * P(2, 3) - P(1, 3);

			j += 2;
		}



		// 进行奇异值分解
		Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullV);

		// 提取零空间（特征值为0对应的特征向量）
		Eigen::VectorXd solution = svd.matrixV().rightCols(1);



		Eigen::Vector3d projectionP;
		projectionP << solution(0, 0) / solution(3, 0), solution(1, 0) / solution(3, 0), solution(2, 0) / solution(3, 0);


		projection3Dpoint.push_back(projectionP);

	}
}

第四步：使用BA算法优化内外参数
优化主要就是下面两个公式

调用BA算法前需要算出角度来用A表示B和C，此时先要使用前面求出来的内参以及外参重建三维点

	//初始化相机一、二投影矩阵
	Eigen::MatrixXd P1_temp(3, 4);
	Eigen::MatrixXd P2_temp(3, 4);
	Eigen::MatrixXd P1;  //相机一投影矩阵
	Eigen::MatrixXd P2;  //相机二投影矩阵
	P1_temp << 1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, 0;

	P1_temp = K1 * P1_temp;
	P1 = P1_temp;
	
	//这里的R和t和为上诉调用getRt函数得到的外参
	P2_temp << R(0, 0), R(0, 1), R(0, 2), t(0, 0),
		R(1, 0), R(1, 1), R(1, 2), t(1, 0),
		R(2, 0), R(2, 1), R(2, 2), t(2, 0);
	
	P2_temp = K2 * P2_temp;
	P2 = P2_temp;
	//存储计算得到的三维点
	std::vector<Eigen::Vector3d> true_point3D
	//三维重建初始化各个点三维坐标
	//points1, points2：为1号与2号两台相机拍摄的标定杆上标定球的中心的像素点坐标。数据排列方式与求解基础矩阵的数据排列一样（就是一个东西）
	get3Dpoint(points1, points2, P1, P2, true_point3D);

算出来三维点后，可以调用下面的函数查看误差

//points1, points2：为1号与2号两台相机拍摄的标定杆上标定球的中心的像素点坐标。数据排列方式与求解基础矩阵的数据排列一样（就是一个东西）
//P1：相机一的投影矩阵
//P2：相机二的投影矩阵
//true_point3D：重建出来的三维点
//AverageReProjectError1：返回的相机一的平均重投影误差
//AverageReProjectError2：返回的相机二的平均重投影误差
//AverageDistanceAB：返回的AB的平均距离
//AverageDistanceBC：返回的BC的平均距离
//flag：是否打印每个点的数据
void getAverageError(std::vector<Eigen::Vector2d> points1,
	std::vector<Eigen::Vector2d> points2,
	Eigen::MatrixXd P1,
	Eigen::MatrixXd P2,
	std::vector<Eigen::Vector3d> true_point3D,
	double &AverageReProjectError1,
	double &AverageReProjectError2,
	double &AverageDistanceAB,
	double &AverageDistanceBC,
	bool flag
) {
	double E_erro1 = 0;
	double E_erro2 = 0;
	for (int i = 0; i < true_point3D.size(); i++) {
		Eigen::MatrixXd tdpoint(4, 1);
		tdpoint << true_point3D[i].x(), true_point3D[i].y(), true_point3D[i].z(), 1;

		Eigen::MatrixXd PPP1 = P1 * tdpoint;
		Eigen::MatrixXd PPP2 = P2 * tdpoint;
		PPP1(0, 0) = PPP1(0, 0) / PPP1(2, 0);
		PPP1(1, 0) = PPP1(1, 0) / PPP1(2, 0);
		PPP1(2, 0) = PPP1(2, 0) / PPP1(2, 0);

		PPP2(0, 0) = PPP2(0, 0) / PPP2(2, 0);
		PPP2(1, 0) = PPP2(1, 0) / PPP2(2, 0);
		PPP2(2, 0) = PPP2(2, 0) / PPP2(2, 0);
		camera1_everyReproErro.push_back((Eigen::Vector2d(PPP1(0, 0), PPP1(1, 0)) - points1[i]).norm());
		camera2_everyReproErro.push_back((Eigen::Vector2d(PPP2(0, 0), PPP2(1, 0)) - points2[i]).norm());
		if (flag == true) {
			std::cout << "相机1重投影 ： " << std::endl << P1 * tdpoint << std::endl << ",,,," << std::endl << PPP1 << " ,原始像素点" << points1[i] << std::endl << std::endl;
			std::cout << "相机2重投影 ： " << std::endl << P2 * tdpoint << std::endl << ",,,," << std::endl << PPP2 << " ,原始像素点" << points2[i] << std::endl << std::endl;
			std::cout << "相机1差值： " << (Eigen::Vector2d(PPP1(0, 0), PPP1(1, 0)) - points1[i]).norm() << std::endl;
			std::cout << "相机2差值： " << (Eigen::Vector2d(PPP2(0, 0), PPP2(1, 0)) - points2[i]).norm() << std::endl;
		}

		E_erro1 += (Eigen::Vector2d(PPP1(0, 0), PPP1(1, 0)) - points1[i]).norm();
		E_erro2 += (Eigen::Vector2d(PPP2(0, 0), PPP2(1, 0)) - points2[i]).norm();
	}
	std::cout << "相机1平均欧式重投影误差： " << E_erro1 / points1.size() << std::endl;
	std::cout << "相机2平均欧式重投影误差： " << E_erro2 / points2.size() << std::endl;
	std::cout << std::endl;
	AverageReProjectError1 = E_erro1 / points1.size();
	AverageReProjectError2 = E_erro2 / points2.size();


	double sum_AB = 0;
	double sum_BC = 0;
	for (int i = 0; i < true_point3D.size(); ) {
		Eigen::Vector3d A = true_point3D[i];
		Eigen::Vector3d B = true_point3D[i + 1];
		Eigen::Vector3d C = true_point3D[i + 2];

		if (flag == true) {
			std::cout << "A到B的距离" << (B - A).norm() << std::endl;
			std::cout << "B到C的距离" << (C - B).norm() << std::endl;
		}

		sum_AB += (B - A).norm();
		sum_BC += (C - B).norm();

		i += 3;
	}
	std::cout << "A到B平均距离： " << sum_AB / (true_point3D.size() / 3) << std::endl;
	std::cout << "B到C平均距离： " << sum_BC / (true_point3D.size() / 3) << std::endl << std::endl;

	AverageDistanceAB = sum_AB / (true_point3D.size() / 3);
	AverageDistanceBC = sum_BC / (true_point3D.size() / 3);
}

接下来就是算出角度，使用角度和A来表示B和C

	std::vector<Eigen::Vector2d> angle;  //使用A点和两个角度来表示BC两个点，存储的角度
	std::vector<Eigen::Vector3d> true_point3D_A;  //只包含A点的坐标
	//使用角度来表示B，C,计算角度
	//AC、BC，为实际标定杆种AC与BC的距离，单位为mm
	getAngle(AC, BC, true_point3D, angle, true_point3D_A);

下面为getAngle函数的实现

//AC：标定杆种AC的距离，单位为mm
//BC：标定杆种BC的距离，单位为mm
//true_point3D：三维重建出来的点
//angle：返回的角度
//point3DA：返回的只包含A点的坐标
void getAngle(double AC,
	double BC,
	std::vector<Eigen::Vector3d> true_point3D,
	std::vector<Eigen::Vector2d> &angle,
	std::vector<Eigen::Vector3d> &point3DA
) {
	double d1 = AC;
	double d2 = BC;
	for (int i = 0; i < true_point3D.size();) {
		Eigen::Vector3d A = true_point3D[i];
		Eigen::Vector3d B = true_point3D[i + 1];
		Eigen::Vector3d C = true_point3D[i + 2];

		Eigen::Vector3d nj = (C - A) / (C - A).norm();

		double cosphi = nj.z();
		double phi = acos(cosphi);

		double costheta = nj.x() / sin(phi);
		double theta = acos(costheta);

		Eigen::Vector2d angle_temp;
		angle_temp << phi, theta;

		angle.push_back(angle_temp);

		i += 3;
	}

	double erroB = 0;
	double erroC = 0;
	std::vector<Eigen::Vector3d> true_point3D_A;
	for (int i = 0, j = 0; i < true_point3D.size(); ) {
		Eigen::Vector3d A = true_point3D[i];
		Eigen::Vector3d B = true_point3D[i + 1];
		Eigen::Vector3d C = true_point3D[i + 2];

		true_point3D_A.push_back(A);

		Eigen::Vector3d nj;
		double phi = angle[j].x();
		double theta = angle[j].y();

		nj << sin(phi) * cos(theta), sin(phi) * sin(theta), cos(phi);

		Eigen::Vector3d b = A + (d1 - d2) * nj;
		Eigen::Vector3d c = A + d1 * nj;

		erroB += (B - b).norm();
		erroC += (C - c).norm();

		i += 3;
		j += 1;
	}
	point3DA = true_point3D_A;
	std::cout << "用角度表示B误差：  " << erroB / (true_point3D.size() / 3) << std::endl;
	std::cout << "用角度表示C误差：  " << erroC / (true_point3D.size() / 3) << std::endl << std::endl;
}

准备工作做完了，接下来就可以调用BA算法优化

	std::vector<Eigen::MatrixXd> K_all;//将前面算出来的相机内参存入
	K_all.push_back(K1);
	K_all.push_back(K2);


	Eigen::MatrixXd R0(3, 3);//相机一旋转矩阵
	R0 << 1, 0, 0,
		0, 1, 0,
		0, 0, 1;


	Eigen::MatrixXd t0(3, 1);//相机一平移矩阵
	t0 << 0, 0, 0;

	std::vector<Eigen::MatrixXd> R_all;//将旋转矩阵存入，其中R为上面调用getRt算出来的R
	R_all.push_back(R0);
	R_all.push_back(R);

	std::vector<Eigen::MatrixXd> t_all;//将平移矩阵存入，其中t为上面调用getRt算出来的t
	t_all.push_back(t0);
	t_all.push_back(t);
	
	//points1, points2：为1号与2号两台相机拍摄的标定杆上标定球的中心的像素点坐标。数据排列方式与求解基础矩阵的数据排列一样（就是一个东西）
	std::vector<std::vector<Eigen::Vector2d>> points;
	points.push_back(points1);
	points.push_back(points2);

	//BA优化
	bundle_adjustment(K_all, R_all, t_all, true_point3D_A, angle, points, AC, BC);

BA算法实现

//优化相机内参，外参，A点的三维坐标，以及欧拉角angle
//K 相机内参
//R 旋转矩阵（3*3）
//t 平移矩阵（3*1）
//true_point3D_A 只包含A点的三维坐标
//angle 欧拉角
//points points[0],表示第一台相机检测到的所有图片的一维标定杆的三个点的像素坐标
//AC 一维标定杆AC的长度，一维标定杆三个点按照ABC排列
//BC 一维标定杆BC的长度
void bundle_adjustment(
	std::vector<Eigen::MatrixXd> &K,
	std::vector<Eigen::MatrixXd> &R,
	std::vector<Eigen::MatrixXd> &t,
	std::vector<Eigen::Vector3d> &true_point3D_A,
	std::vector<Eigen::Vector2d> &angle,
	std::vector<std::vector<Eigen::Vector2d>> points,
	double AC,
	double BC,
	double max_num_iterations = 200
) {
	double d1 = AC;
	double d2 = BC;

	//相机数量
	int camera_nums = K.size();


	std::vector<Eigen::MatrixXd> intrinsic;
	std::vector<Eigen::MatrixXd> extrinsics;

	//初始化内参
	for (int i = 0; i < K.size(); i++) {
		Eigen::MatrixXd intrinsic_temp(4, 1);
		intrinsic_temp << K[i](0, 0), K[i](1, 1), K[i](0, 2), K[i](1, 2);
		intrinsic.push_back(intrinsic_temp);
	}

	//初始化外参
	for (int i = 0; i < R.size(); i++) {
		Eigen::MatrixXd extrinsics_temp(6, 1);

		cv::Mat rotation_matrix_temp = (cv::Mat_<double>(3, 3) <<
			R[i](0, 0), R[i](0, 1), R[i](0, 2),
			R[i](1, 0), R[i](1, 1), R[i](1, 2),
			R[i](2, 0), R[i](2, 1), R[i](2, 2));

		cv::Mat rotation_vector_temp;
		cv::Rodrigues(rotation_matrix_temp, rotation_vector_temp);

		extrinsics_temp << rotation_vector_temp.at<double>(0, 0),
			rotation_vector_temp.at<double>(1, 0),
			rotation_vector_temp.at<double>(2, 0),
			t[i](0, 0),
			t[i](1, 0),
			t[i](2, 0);

		extrinsics.push_back(extrinsics_temp);
	}

	// 创建Ceres Problem
	ceres::Problem ceres_problem;

	for (int i = 0; i < intrinsic.size(); i++) {
		ceres_problem.AddParameterBlock(intrinsic[i].data(), 4);
	}

	for (int i = 0; i < extrinsics.size(); i++) {
		ceres_problem.AddParameterBlock(extrinsics[i].data(), 6);
	}

	for (int i = 0; i < angle.size(); i++) {
		ceres_problem.AddParameterBlock(angle[i].data(), 2);
	}

	//将第一台相机的外参设定为恒定不变，不参与优化
	ceres_problem.SetParameterBlockConstant(extrinsics[0].data());


	固定相机1和2内参
	//ceres_problem.SetParameterBlockConstant(intrinsic[0].data());
	//ceres_problem.SetParameterBlockConstant(intrinsic[1].data());


	//损失函数
	ceres::LossFunction* loss_function = new ceres::HuberLoss(4);

	// 添加重投影误差代价函数
	for (int j = 0; j < camera_nums; j++) {
		for (size_t i = 0, k = 0; i < points[j].size();) {
			ceres::CostFunction* cost_function =
				new ceres::AutoDiffCostFunction<ReprojectionError, 6, 4, 6, 3, 2>(
					new ReprojectionError(points[j][i], points[j][i + 1], points[j][i + 2], d1, d2));
			ceres_problem.AddResidualBlock(cost_function, loss_function, intrinsic[j].data(), extrinsics[j].data(), true_point3D_A[k].data(), angle[k].data());

			i += 3;
			k += 1;
		}
	}

	// Solve BA
	ceres::Solver::Options ceres_config_options;

	//是否将优化过程的进展信息输出到标准输出。
	ceres_config_options.minimizer_progress_to_stdout = false;

	//设置日志记录的类型。这个字段控制了在优化过程中生成的日志消息的详细程度。
	//SILENT 表示没有信息输出
	//PER_MINIMIZER_ITERATION 表示输出每步详细信息
	ceres_config_options.logging_type = ceres::PER_MINIMIZER_ITERATION;

	//线程数，控制求解过程中使用的线程数量。
	ceres_config_options.num_threads = 1;

	//预条件器类型，用于加速迭代过程。
	ceres_config_options.preconditioner_type = ceres::JACOBI;

	//线性求解器类型，可以选择使用稠密矩阵、稀疏矩阵或其他线性求解器。
	//使用稀疏 Schur 分解线性求解器来处理线性方程组。
	ceres_config_options.linear_solver_type = ceres::SPARSE_SCHUR;

	//用于设置稀疏线性代数库的类型。
	ceres_config_options.sparse_linear_algebra_library_type = ceres::EIGEN_SPARSE;

	//最大迭代次数，限制优化过程的最大迭代次数。
	ceres_config_options.max_num_iterations = max_num_iterations;

	ceres::Solver::Summary summary;
	ceres::Solve(ceres_config_options, &ceres_problem, &summary);
	std::cout << summary.FullReport() << "
";

	if (!summary.IsSolutionUsable())
	{
		std::cout << "Bundle Adjustment failed." << std::endl;
	}
	else
	{
		// Display statistics about the minimization
		std::cout << std::endl
			<< "Bundle Adjustment statistics (approximated RMSE):
"
			<< " #views: " << camera_nums << "
"
			<< " #residuals: " << summary.num_residuals << "
"
			<< " Initial RMSE: " << std::sqrt(summary.initial_cost / summary.num_residuals) << "
"
			<< " Final RMSE: " << std::sqrt(summary.final_cost / summary.num_residuals) << "
"
			<< " Time (s): " << summary.total_time_in_seconds << "
"
			<< std::endl;
	}

	K.clear();
	for (int i = 0; i < intrinsic.size(); i++) {
		Eigen::MatrixXd K_temp(3, 3);
		K_temp << intrinsic[i](0, 0), 0, intrinsic[i](2, 0),
			0, intrinsic[i](1, 0), intrinsic[i](3, 0),
			0, 0, 1;
		K.push_back(K_temp);
	}

	R.clear();
	t.clear();
	for (int i = 0; i < extrinsics.size(); i++) {
		cv::Mat cvMatR;
		cv::Mat cv_vector = (cv::Mat_<double>(3, 1) <<
			extrinsics[i](0, 0), extrinsics[i](1, 0), extrinsics[i](2, 0));

		cv::Rodrigues(cv_vector, cvMatR);
		Eigen::MatrixXd R_temp(3, 3);
		for (int k = 0; k < 3; k++) {
			for (int j = 0; j < 3; j++) {
				R_temp(k, j) = cvMatR.at<double>(k, j);
			}
		}
		R.push_back(R_temp);

		Eigen::MatrixXd t_temp(3, 1);
		t_temp << extrinsics[i](3, 0), extrinsics[i](4, 0), extrinsics[i](5, 0);
		t.push_back(t_temp);
	}
}

此时就已经优化完成了，得到了优化后的内外参数

	K1 = K_all[0];
	K2 = K_all[1];
	R = R_all[1];
	t = t_all[1];

	std::cout << "优化后的K1 " << std::endl << K_all[0] << std::endl << std::endl;
	std::cout << "优化后的K2 " << std::endl << K_all[1] << std::endl << std::endl;

	std::cout << "优化后的R1 " << std::endl << R_all[0] << std::endl << std::endl;
	std::cout << "优化后的t1 " << std::endl << t_all[0] << std::endl << std::endl;

	std::cout << "优化后的R2 " << std::endl << R_all[1] << std::endl << std::endl;
	std::cout << "优化后的t2 " << std::endl << t_all[1] << std::endl << std::endl;

到此整个流程就结束了
值得一提的是：
1.由于我实验用的相机畸变系数很小，所以整个流程并没有考虑相机的畸变。
2.一维标定杆标定双目相机的难点除了标定算法，还在于一维标定杆小球圆心坐标的提取，这篇论文使用的红外相机提取小球中心像素坐标应该不难，但是使用一般的相机提取一维标定杆小球中心圆心坐标的难度随着场景的复杂程度变大而变大，我先是使用一篇论文的圆心提取算法，失败。matlab中的圆心提取算法，失败。最后设计了一个神经网络，通过过拟合的方式来训练网络，最后才成功提取到了一维标定杆小球中心圆心坐标。
据我所知这应该是第一篇带代码的关于一维标定杆标定算法的博客
希望对你有所帮助