程式碼是最為耐心、最能忍耐和最令人愉快的夥伴，在任何艱難困苦的時刻，它都不會拋棄你（開場白）
長時間不寫部落格，我總感覺自己沒有做事情，最近一直在做目標檢測，想結合一下雙目視覺，做立體檢測，於是就研究了一下雙目視覺，參考了很多人的相關部落格，在這裡，特意表示感謝一下。
使用Opencv實現張正友法相機標定之前，有幾個問題事先要確認一下，那就是相機為什麼需要標定，標定需要的輸入和輸出分別是哪些？

相機標定的目的：獲取攝像機的內參和外參矩陣（同時也會得到每一幅標定影象的選擇和平移矩陣），內參和外參係數可以對之後相機拍攝的影象就進行矯正，得到畸變相對很小的影象。

相機標定的輸入：標定影象上所有內角點的影象座標，標定板影象上所有內角點的空間三維座標（一般情況下假定影象位於Z=0平面上）。

相機標定的輸出：攝像機的內參、外參係數。

這三個基礎的問題就決定了使用Opencv實現張正友法標定相機的標定流程、標定結果評價以及使用標定結果矯正原始影象的完整流程：

1. 準備標定圖片

2. 對每一張標定圖片，提取角點資訊
   3.對每一張標定圖片，進一步提取亞畫素角點資訊

3. 在棋盤標定圖上繪製找到的內角點（非必須，僅為了顯示）

4. 相機標定

5. 對標定結果進行評價

6. 檢視標定效果——利用標定結果對棋盤圖進行矯正

7. 準備標定圖片

標定圖片需要使用標定板在不同位置、不同角度、不同姿態下拍攝，最少需要3張，以10~20張為宜。標定板需要是黑白相間的矩形構成的棋盤圖，製作精度要求較高，如下圖所示：

//! finds checkerboard pattern of the specified size in the image
CV_EXPORTS_W bool findChessboardCorners( InputArray image, Size patternSize, OutputArray corners, int flags=CALIB_CB_ADAPTIVE_THRESH+CALIB_CB_NORMALIZE_IMAGE );

第一個引數Image，傳入拍攝的棋盤圖Mat影象，必須是8位的灰度或者彩色影象；

第二個引數patternSize，每個棋盤圖上內角點的行列數，一般情況下，行列數不要相同，便於後續標定程式識別標定板的方向；

第三個引數corners，用於儲存檢測到的內角點影象座標位置，一般用元素是Point2f的向量來表示：vector image_points_buf;

第四個引數flage：用於定義棋盤圖上內角點查詢的不同處理方式，有預設值。

3. 對每一張標定圖片，進一步提取亞畫素角點資訊

為了提高標定精度，需要在初步提取的角點資訊上進一步提取亞畫素資訊，降低相機標定偏差，常用的方法是cornerSubPix，另一個方法是使用find4QuadCornerSubpix函式，這個方法是專門用來獲取棋盤圖上內角點的精確位置的，或許在相機標定的這個特殊場合下它的檢測精度會比cornerSubPix更高？

cornerSubPix函式原型：

//! adjusts the corner locations with sub-pixel accuracy to maximize the certain cornerness criteria CV_EXPORTS_W void cornerSubPix( InputArray image, InputOutputArray corners, Size winSize, Size zeroZone,TermCriteria criteria );

第一個引數image，輸入的Mat矩陣，最好是8位灰度影象，檢測效率更高；

第二個引數corners，初始的角點座標向量，同時作為亞畫素座標位置的輸出，所以需要是浮點型資料，一般用元素是Pointf2f/Point2d的向量來表示：vector<Point2f/Point2d> iamgePointsBuf；

第三個引數winSize，大小為搜尋視窗的一半；

第四個引數zeroZone，死區的一半尺寸，死區為不對搜尋區的中央位置做求和運算的區域。它是用來避免自相關矩陣出現某些可能的奇異性。當值為（-1，-1）時表示沒有死區；

第五個引數criteria，定義求角點的迭代過程的終止條件，可以為迭代次數和角點精度兩者的組合；

find4QuadCornerSubpix函式原型：

//! finds subpixel-accurate positions of the chessboard cornersCV_EXPORTS bool find4QuadCornerSubpix(InputArray img, InputOutputArray corners, Size region_size);

第一個引數img，輸入的Mat矩陣，最好是8位灰度影象，檢測效率更高；

第二個引數corners，初始的角點座標向量，同時作為亞畫素座標位置的輸出，所以需要是浮點型資料，一般用元素是Pointf2f/Point2d的向量來表示：vector<Point2f/Point2d> iamgePointsBuf；

第三個引數region_size，角點搜尋視窗的尺寸；

在其中一個標定的棋盤圖上分別執行cornerSubPix和find4QuadCornerSubpix尋找亞畫素角點，兩者定位到的亞畫素角點座標分別為：
4. 在棋盤標定圖上繪製找到的內角點（非必須，僅為了顯示）

//! draws the checkerboard pattern (found or partly found) in the imageCV_EXPORTS_W void drawChessboardCorners( InputOutputArray image, Size patternSize, InputArray corners, bool patternWasFound );

第一個引數image，8位灰度或者彩色影象；

第二個引數patternSize，每張標定棋盤上內角點的行列數；

第三個引數corners，初始的角點座標向量，同時作為亞畫素座標位置的輸出，所以需要是浮點型資料，一般用元素是Pointf2f/Point2d的向量來表示：vector<Point2f/Point2d> iamgePointsBuf；

第四個引數patternWasFound，標誌位，用來指示定義的棋盤內角點是否被完整的探測到，true表示別完整的探測到，函式會用直線依次連線所有的內角點，作為一個整體，false表示有未被探測到的內角點，這時候函式會以（紅色）圓圈標記處檢測到的內角點；

以下是drawChessboardCorners函式中第四個引數patternWasFound設定為true和false時內角點的繪製效果：

patternWasFound=ture時，依次連線各個內角點：
5. 相機標定
獲取到棋盤標定圖的內角點影象座標之後，就可以使用calibrateCamera函式進行標定，計算相機內參和外參係數，

calibrateCamera函式原型：

//! finds intrinsic and extrinsic camera parameters from several fews of a known calibration pattern.
CV_EXPORTS_W double calibrateCamera( InputArrayOfArrays objectPoints,
                                     InputArrayOfArrays imagePoints,
                                     Size imageSize,
                                     CV_OUT InputOutputArray cameraMatrix,
                                     CV_OUT InputOutputArray distCoeffs,
                                     OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
                                     int flags=0, TermCriteria criteria = TermCriteria(
                                         TermCriteria::COUNT+TermCriteria::EPS, 30, DBL_EPSILON) );

第一個引數objectPoints，為世界座標系中的三維點。在使用時，應該輸入一個三維座標點的向量的向量，即vector<vector> object_points。需要依據棋盤上單個黑白矩陣的大小，計算出（初始化）每一個內角點的世界座標。

第二個引數imagePoints，為每一個內角點對應的影象座標點。和objectPoints一樣，應該輸入vector<vector> image_points_seq形式的變數；

第三個引數imageSize，為影象的畫素尺寸大小，在計算相機的內參和畸變矩陣時需要使用到該引數；

第四個引數cameraMatrix為相機的內參矩陣。輸入一個Mat cameraMatrix即可，如Mat cameraMatrix=Mat(3,3,CV_32FC1,Scalar::all(0));

第五個引數distCoeffs為畸變矩陣。輸入一個Mat distCoeffs=Mat(1,5,CV_32FC1,Scalar::all(0))即可；

第六個引數rvecs為旋轉向量；應該輸入一個Mat型別的vector，即vectorrvecs;

第七個引數tvecs為位移向量，和rvecs一樣，應該為vector tvecs；

第八個引數flags為標定時所採用的演算法。有如下幾個引數：

CV_CALIB_USE_INTRINSIC_GUESS：使用該引數時，在cameraMatrix矩陣中應該有fx,fy,u0,v0的估計值。否則的話，將初始化(u0,v0）影象的中心點，使用最小二乘估算出fx，fy。
CV_CALIB_FIX_PRINCIPAL_POINT：在進行優化時會固定光軸點。當CV_CALIB_USE_INTRINSIC_GUESS引數被設定，光軸點將保持在中心或者某個輸入的值。
CV_CALIB_FIX_ASPECT_RATIO：固定fx/fy的比值，只將fy作為可變數，進行優化計算。當CV_CALIB_USE_INTRINSIC_GUESS沒有被設定，fx和fy將會被忽略。只有fx/fy的比值在計算中會被用到。
CV_CALIB_ZERO_TANGENT_DIST：設定切向畸變引數（p1,p2）為零。
CV_CALIB_FIX_K1,…,CV_CALIB_FIX_K6：對應的徑向畸變在優化中保持不變。
CV_CALIB_RATIONAL_MODEL：計算k4，k5，k6三個畸變引數。如果沒有設定，則只計算其它5個畸變引數。

第九個引數criteria是最優迭代終止條件設定。

在使用該函式進行標定運算之前，需要對棋盤上每一個內角點的空間座標系的位置座標進行初始化，標定的結果是生成相機的內參矩陣cameraMatrix、相機的5個畸變係數distCoeffs，另外每張影象都會生成屬於自己的平移向量和旋轉向量。
6. 對標定結果進行評價
對標定結果進行評價的方法是通過得到的攝像機內外引數，對空間的三維點進行重新投影計算，得到空間三維點在影象上新的投影點的座標，計算投影座標和亞畫素角點座標之間的偏差，偏差越小，標定結果越好。

對空間三維座標點進行反向投影的函式是projectPoints，函式原型是：

//! projects points from the model coordinate space to the image coordinates. Also computes derivatives of the image coordinates w.r.t the intrinsic and extrinsic camera parameters
CV_EXPORTS_W void projectPoints( InputArray objectPoints,
                                 InputArray rvec, InputArray tvec,
                                 InputArray cameraMatrix, InputArray distCoeffs,
                                 OutputArray imagePoints,
                                 OutputArray jacobian=noArray(),
                                 double aspectRatio=0 );

第一個引數objectPoints，為相機座標系中的三維點座標；

第二個引數rvec為旋轉向量，每一張影象都有自己的選擇向量；

第三個引數tvec為位移向量，每一張影象都有自己的平移向量；

第四個引數cameraMatrix為求得的相機的內參數矩陣；

第五個引數distCoeffs為相機的畸變矩陣；

第六個引數iamgePoints為每一個內角點對應的影象上的座標點；

第七個引數jacobian是雅可比行列式；

第八個引數aspectRatio是跟相機感測器的感光單元有關的可選引數，如果設定為非0，則函式預設感光單元的dx/dy是固定的，會依此對雅可比矩陣進行調整；

下邊顯示了某一張標定圖片上的亞畫素角點座標和根據標定結果把空間三維座標點映射回影象座標點的對比：
7. 檢視標定效果——利用標定結果對棋盤圖進行矯正

利用求得的相機的內參和外引數據，可以對影象進行畸變的矯正，這裡有兩種方法可以達到矯正的目的，分別說明一下。

方法一：使用initUndistortRectifyMap和remap兩個函式配合實現。

initUndistortRectifyMap用來計算畸變對映，remap把求得的對映應用到影象上。

initUndistortRectifyMap的函式原型：

//! initializes maps for cv::remap() to correct lens distortion and optionally rectify the image
CV_EXPORTS_W void initUndistortRectifyMap( InputArray cameraMatrix, InputArray distCoeffs,
                           InputArray R, InputArray newCameraMatrix,
                           Size size, int m1type, OutputArray map1, OutputArray map2 );

第一個引數cameraMatrix為之前求得的相機的內參矩陣；

第二個引數distCoeffs為之前求得的相機畸變矩陣；

第三個引數R，可選的輸入，是第一和第二相機座標之間的旋轉矩陣；

第四個引數newCameraMatrix，輸入的校正後的3X3攝像機矩陣；

第五個引數size，攝像機採集的無失真的影象尺寸；

第六個引數m1type，定義map1的資料型別，可以是CV_32FC1或者CV_16SC2；

第七個引數map1和第八個引數map2，輸出的X/Y座標重對映引數；

remap函式原型：

//! warps the image using the precomputed maps. The maps are stored in either floating-point or integer fixed-point format
CV_EXPORTS_W void remap( InputArray src, OutputArray dst,
                         InputArray map1, InputArray map2,
                         int interpolation, int borderMode=BORDER_CONSTANT,
                         const Scalar& borderValue=Scalar());

第一個引數src，輸入引數，代表畸變的原始影象；

第二個引數dst，矯正後的輸出影象，跟輸入影象具有相同的型別和大小；

第三個引數map1和第四個引數map2，X座標和Y座標的對映；

第五個引數interpolation，定義影象的插值方式；

第六個引數borderMode，定義邊界填充方式；

方法二：使用undistort函式實現

undistort函式原型：

//! corrects lens distortion for the given camera matrix and distortion coefficients
CV_EXPORTS_W void undistort( InputArray src, OutputArray dst,
                             InputArray cameraMatrix,
                             InputArray distCoeffs,
                             InputArray newCameraMatrix=noArray() );

第一個引數src，輸入引數，代表畸變的原始影象；

第二個引數dst，矯正後的輸出影象，跟輸入影象具有相同的型別和大小；

第三個引數cameraMatrix為之前求得的相機的內參矩陣；

第四個引數distCoeffs為之前求得的相機畸變矩陣；

第五個引數newCameraMatrix，預設跟cameraMatrix保持一致；

方法一相比方法二執行效率更高一些，推薦使用。
標定之前我們先寫個拍照程式,python程式碼

import cv2
import time

AUTO = True  # 自動拍照，或手動按s鍵拍照
INTERVAL = 2 # 自動拍照間隔

cv2.namedWindow("left")
cv2.namedWindow("right")
cv2.moveWindow("left", 0, 0)
cv2.moveWindow("right", 400, 0)
left_camera = cv2.VideoCapture(0)
right_camera = cv2.VideoCapture(1)

counter = 0
utc = time.time()
pattern = (12, 8) # 棋盤格尺寸
folder = "./snapshot/" # 拍照檔案目錄

def shot(pos, frame):
    global counter
    path = folder + pos + "_" + str(counter) + ".jpg"

    cv2.imwrite(path, frame)
    print("snapshot saved into: " + path)

while True:
    ret, left_frame = left_camera.read()
    ret, right_frame = right_camera.read()

    cv2.imshow("left", left_frame)
    cv2.imshow("right", right_frame)

    now = time.time()
    if AUTO and now - utc >= INTERVAL:
        shot("left", left_frame)
        shot("right", right_frame)
        counter += 1
        utc = now

    key = cv2.waitKey(1)
    if key == ord("q"):
        break
    elif key == ord("s"):
        shot("left", left_frame)
        shot("right", right_frame)
        counter += 1

left_camera.release()
right_camera.release()
cv2.destroyWindow("left")
cv2.destroyWindow("right")

弄了半天解釋，下面我們正式開始上完整程式碼 (c++標定）：

#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>
#include <fstream>

using namespace cv;
using namespace std;

void main()
{
	ifstream fin("calibdata.txt"); /* 標定所用影象檔案的路徑 */
	ofstream fout("caliberation_result.txt");  /* 儲存標定結果的檔案 */
											   //讀取每一幅影象，從中提取出角點，然後對角點進行亞畫素精確化	
	Mat img;
	//img = imread("left01.jpg");
	//cout << "hello";
	//imshow("XIAORUN", img);
	cout << "開始提取角點………………";
	int image_count = 0;  /* 影象數量 */
	Size image_size;  /* 影象的尺寸 */
	Size board_size = Size(6, 9);    /* 標定板上每行、列的角點數 */
	vector<Point2f> image_points_buf;  /* 快取每幅影象上檢測到的角點 */
	vector<vector<Point2f>> image_points_seq; /* 儲存檢測到的所有角點 */
	string filename;
	int count = -1;//用於儲存角點個數。
	while (getline(fin, filename))
	{
		image_count++;
		// 用於觀察檢驗輸出
		cout << "image_count = " << image_count << endl;
		/* 輸出檢驗*/
		cout << "-->count = " << count;
		Mat imageInput = imread(filename);
		//imshow("xiaorun", filename);
		if (image_count == 1)  //讀入第一張圖片時獲取影象寬高資訊
		{
			printf("hello");
			image_size.width = imageInput.cols;
			image_size.height = imageInput.rows;
			cout << "image_size.width = " << image_size.width << endl;
			cout << "image_size.height = " << image_size.height << endl;
		}

		/* 提取角點 */
		if (0 == findChessboardCorners(imageInput, board_size, image_points_buf))
		{
			cout << "can not find chessboard corners!\n"; //找不到角點
			printf("hello");
			exit(1);
		}
		else
		{
			Mat view_gray;
			cvtColor(imageInput, view_gray, CV_RGB2GRAY);
			/* 亞畫素精確化 */
			find4QuadCornerSubpix(view_gray, image_points_buf, Size(11, 11)); //對粗提取的角點進行精確化
			image_points_seq.push_back(image_points_buf);  //儲存亞畫素角點
														   /* 在影象上顯示角點位置 */
			drawChessboardCorners(view_gray, board_size, image_points_buf, true); //用於在圖片中標記角點
			imshow("Camera Calibration", view_gray);//顯示圖片
			//printf("world");
			waitKey(500);//暫停0.5S		
		}
	}
	int total = image_points_seq.size();
	cout << "total = " << total << endl;
	int CornerNum = board_size.width*board_size.height;  //每張圖片上總的角點數
	for (int ii = 0; ii<total; ii++)
	{
		if (0 == ii%CornerNum)// 24 是每幅圖片的角點個數。此判斷語句是為了輸出 圖片號，便於控制檯觀看 
		{
			int i = -1;
			i = ii / CornerNum;
			int j = i + 1;
			cout << "--> 第 " << j << "圖片的資料 --> : " << endl;
		}
		if (0 == ii % 3)	// 此判斷語句，格式化輸出，便於控制檯檢視
		{
			cout << endl;
		}
		else
		{
			cout.width(10);
		}
		//輸出所有的角點
		cout << " -->" << image_points_seq[ii][0].x;
		cout << " -->" << image_points_seq[ii][0].y;
	}
	cout << "角點提取完成！\n";

	//以下是攝像機標定
	cout << "開始標定………………";
	/*棋盤三維資訊*/
	Size square_size = Size(10, 10);  /* 實際測量得到的標定板上每個棋盤格的大小 */
	vector<vector<Point3f> > object_points; /* 儲存標定板上角點的三維座標 */
										   /*內外引數*/
	//Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /* 攝像機內參數矩陣 */
	Mat cameraMatrix = Mat(3, 3, CV_64F, Scalar::all(0)); /* 攝像機內參數矩陣 */
	vector<int> point_counts;  // 每幅影象中角點的數量
	Mat distCoeffs = Mat(1, 5, CV_64F, Scalar::all(0)); /* 攝像機的5個畸變係數：k1,k2,p1,p2,k3 */
	vector<Mat> tvecsMat;  /* 每幅影象的旋轉向量 */
	vector<Mat> rvecsMat; /* 每幅影象的平移向量 */
						  /* 初始化標定板上角點的三維座標 */
	int i, j, t;
	
	for (t = 0; t<image_count; t++)
	{
		vector<Point3f> tempPointSet;
		for (i = 0; i<board_size.height; i++)
		{
			for (j = 0; j<board_size.width; j++)
			{
				Point3f realPoint;
				/* 假設標定板放在世界座標系中z=0的平面上 */
				realPoint.x = i*square_size.width;
				realPoint.y = j*square_size.height;
				realPoint.z = 0;
				tempPointSet.push_back(realPoint);
			}
		}
		object_points.push_back(tempPointSet);
	}
	/* 初始化每幅影象中的角點數量，假定每幅影象中都可以看到完整的標定板 */
	for (i = 0; i<image_count; i++)
	{
		point_counts.push_back(board_size.width*board_size.height);
	}
	/* 開始標定 */
	calibrateCamera(object_points, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, 0);
	cout << "標定完成！\n";
	//對標定結果進行評價
	cout << "開始評價標定結果………………\n";
	double total_err = 0.0; /* 所有影象的平均誤差的總和 */
	double err = 0.0; /* 每幅影象的平均誤差 */
	vector<Point2f> image_points2; /* 儲存重新計算得到的投影點 */
	cout << "\t每幅影象的標定誤差：\n";
	fout << "每幅影象的標定誤差：\n";
	for (i = 0; i<image_count; i++)
	{
		vector<Point3f> tempPointSet = object_points[i];
		/* 通過得到的攝像機內外引數，對空間的三維點進行重新投影計算，得到新的投影點 */
		projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points2);
		/* 計算新的投影點和舊的投影點之間的誤差*/
		vector<Point2f> tempImagePoint = image_points_seq[i];
		Mat tempImagePointMat = Mat(1, tempImagePoint.size(), CV_32FC2);
		Mat image_points2Mat = Mat(1, image_points2.size(), CV_32FC2);
		for (int j = 0; j < tempImagePoint.size(); j++)
		{
			image_points2Mat.at<Vec2f>(0, j) = Vec2f(image_points2[j].x, image_points2[j].y);
			tempImagePointMat.at<Vec2f>(0, j) = Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
		}
		err = norm(image_points2Mat, tempImagePointMat, NORM_L2);
		total_err += err /= point_counts[i];
		std::cout << "第" << i + 1 << "幅影象的平均誤差：" << err << "畫素" << endl;
		fout << "第" << i + 1 << "幅影象的平均誤差：" << err << "畫素" << endl;
	}
	std::cout << "總體平均誤差：" << total_err / image_count << "畫素" << endl;
	fout << "總體平均誤差：" << total_err / image_count << "畫素" << endl << endl;
	std::cout << "評價完成！" << endl;
	//儲存定標結果  	
	std::cout << "開始儲存定標結果………………" << endl;
	Mat rotation_matrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /* 儲存每幅影象的旋轉矩陣 */
	fout << "相機內參數矩陣：" << endl;
	fout << cameraMatrix << endl << endl;
	fout << "畸變係數：\n";
	fout << distCoeffs << endl << endl << endl;
	for (int i = 0; i<image_count; i++)
	{
		fout << "第" << i + 1 << "幅影象的旋轉向量：" << endl;
		fout << tvecsMat[i] << endl;
		/* 將旋轉向量轉換為相對應的旋轉矩陣 */
		Rodrigues(tvecsMat[i], rotation_matrix);
		fout << "第" << i + 1 << "幅影象的旋轉矩陣：" << endl;
		fout << rotation_matrix << endl;
		fout << "第" << i + 1 << "幅影象的平移向量：" << endl;
		fout << rvecsMat[i] << endl << endl;
	}
	std::cout << "完成儲存" << endl;
	fout << endl;
	system("pause");
	return;
}

以下是我的工程目錄圖，我用的VS

在進行完立體標定後，我們將得到如下的資料：

每幅影象的標定誤差：
第1幅影象的平均誤差：0.0645257畫素
第2幅影象的平均誤差：0.0556487畫素
第3幅影象的平均誤差：0.0668032畫素
第4幅影象的平均誤差：0.0669439畫素
第5幅影象的平均誤差：0.0699647畫素
第6幅影象的平均誤差：0.0570037畫素
第7幅影象的平均誤差：0.0579455畫素
第8幅影象的平均誤差：0.0736451畫素
第9幅影象的平均誤差：0.0551833畫素
第10幅影象的平均誤差：0.0538387畫素
第11幅影象的平均誤差：0.0715703畫素
第12幅影象的平均誤差：0.0566535畫素
第13幅影象的平均誤差：0.0547182畫素
總體平均誤差：0.0618804畫素

相機內參數矩陣：
[531.4017560022933, 0, 340.5121891820963;
 0, 531.3698083055915, 232.0990886525732;
 0, 0, 1]

畸變係數：
[-0.257272028772218, -0.1302531787363336, 0.0008829663197048428, -0.0006837432466250165, 0.5184868514845824]


第1幅影象的旋轉向量：
[-28.89378134491334;
 6.940661936528073;
 167.1204988426566]
第1幅影象的旋轉矩陣：
[0.9955462542172515, -0.09422195396417896, 0.003143104384498384;
 0.09415810515313683, 0.9954210224680247, 0.01646934311632131;
 -0.00468048586906741, -0.01610004409572948, 0.9998594309362416]
第1幅影象的平移向量：
[-2.948281786953482;
 -0.01894177912098307;
 0.4152828312972277]

第2幅影象的旋轉向量：
[26.03544643046547;
 43.91450787470743;
 136.0862657817351]
第2幅影象的旋轉矩陣：
[0.6825105514846874, -0.6756261709260313, 0.2787626665676329;
 0.7111305927667627, 0.7019288669448416, -0.03986409136959795;
 -0.1687383392808543, 0.2254443233025845, 0.9595322975009123]
第2幅影象的平移向量：
[-2.015742838932253;
 1.703940309126829;
 0.8331268576092087]

第3幅影象的旋轉向量：
[-33.77454120034904;
 5.743476206332025;
 114.6452900299365]
第3幅影象的旋轉矩陣：
[0.965621241947457, -0.2599369928735916, 0.002894276366398319;
 0.2589247530787128, 0.9627310687215274, 0.07814641105919914;
 -0.02309955287398991, -0.07471043470720912, 0.9969376919361017]
第3幅影象的平移向量：
[2.802519696251568;
 0.5076535731226879;
 -0.2745520274610763]

第4幅影象的旋轉向量：
[-39.39511332708067;
 23.70843751957927;
 125.784028616703]
第4幅影象的旋轉矩陣：
[-0.2746493468889726, -0.9353480887148267, -0.2229163277801369;
 0.7900186121940573, -0.3516621879557613, 0.502199460323428;
 -0.5481225489253625, -0.03817929412243534, 0.8355261892124484]
第4幅影象的平移向量：
[3.002234256141708;
 -0.00400244227416397;
 -0.3671838709588458]

第5幅影象的旋轉向量：
[-24.66419344573827;
 -33.35496576817189;
 127.9981045127136]
第5幅影象的旋轉矩陣：
[-0.7973055508582415, -0.4012524790882016, -0.4508883526952193;
 0.5702727751718082, -0.7455077586910236, -0.3449741202312586;
 -0.1977190442736369, -0.5321791331469685, 0.8232208390064367]
第5幅影象的平移向量：
[2.202107819324158;
 1.745118658236513;
 -0.5795807069710756]

第6幅影象的旋轉向量：
[22.65481087785099;
 -30.94432086018879;
 154.5260983737024]
第6幅影象的旋轉矩陣：
[-0.5031681185939065, -0.8628775660656608, 0.04758308955153689;
 0.7780182281326339, -0.47627662896687, -0.4096928232143093;
 0.376177459617167, -0.1691238560362068, 0.9109816903733131]
第6幅影象的平移向量：
[-1.842008361814392;
 -2.052208601005723;
 0.3800526676603065]

第7幅影象的旋轉向量：
[-36.53410155359971;
 -41.84414929889709;
 171.0877268216408]
第7幅影象的旋轉矩陣：
[-0.6205995951224508, 0.7679973189172715, -0.1582285077656138;
 -0.6082734212882752, -0.5988575334947455, -0.52093