# 雙目視覺辨識
https://blog.csdn.net/piaoxuezhong/article/details/79016615
https://zhuanlan.zhihu.com/p/81016834

相機校正
https://zhuanlan.zhihu.com/p/55648494
https://blog.csdn.net/wangxiaokun671903/article/details/37966891
https://blog.csdn.net/wangxiaokun671903/article/details/37935113
http://www.vision.caltech.edu/bouguetj/calib_doc/
算法時間與誤差
https://www.ixueshu.com/document/4e0dce557d53fd6c0bb3e8cf84599dc1318947a18e7f9386.html
https://blog.csdn.net/KYJL888/article/details/79240525
https://blog.csdn.net/wintergeng/article/details/51049596
一些經驗
https://www.cnblogs.com/daihengchen/p/5492729.html
https://blog.csdn.net/sunanger_wang/article/details/7744015
https://blog.csdn.net/scyscyao/article/details/5443341
https://blog.csdn.net/rocky69/article/details/7726569
https://www.itread01.com/content/1542248533.html
https://blog.csdn.net/KYJL888/article/details/79240525

## 相機模型與標定(參考Learning OpenCV 3)
### 目的：計算相機的內參以及畸變矩陣

#### 原因：
物體透過鏡頭投影的影像，會根據鏡頭的特性而有不同投影
而我們標定的工作就是要把這些求出來，後續做矯正的時候會使用到

1. Camera Matrix(內參矩陣) & Distortion Coefficients Matrix(畸變矩陣)
[fx .0 cx]
[.0 fy cy] = Camera Matrix M
[.0 .0 1.]

畸變參數(4, 5, 8個係數不等，這裡討論4個，除非變形太多，否則會盡量減少運算量)
k1 k2
p1 p2

2. 旋轉矩陣&平移矩陣

3. 使用函數(OpenCV) Note that. 我們使用一般棋盤格(6x9)作為標定的目標

   //找到每個黑白方格間的頂點(角點)
   cv::findChessboardCorners(           
       cv::InputArray  image,           //讀入的影像
       cv::Size        patternSize,     //棋盤格的角點數量 (row, column)
       cv::OutputArray corners,         //輸出角點位置
       int             flags =          //幫助找到角點的程序
                               cv::CALIB_CB_APATIVE_THRESH |
                               CV::CALIB_CB_NORMALIZE_IMAGE
   );
   
   //輔助用來辨識是否成功抓到所有的角點，會將所有角點用顏色標註好
   cv::drawChessboardCorners(
       cv::InputOutputArray image,
       cv::Size             patternSize,
       cv::InputArray       corners,         //是findChessboardCorners後的輸出矩陣
       bool                 patternWasFound
   );

   //標定函數(Camera Matrix & distortion Coefficients Matrix)
   double cv::calibrateCamera(
       cv::InputArrayOfArrays  objectPoints, // 物體3D
       cv::InputArrayOfArrays  imagePoints,  // 影像2D
       cv::Size                imageSize,
       cv::InputOutputArray    cameraMatrix,
       cv::InputOutputArray    distCoeffs,
       cv::OutputArrayOfArrays rvecs,
       cv::OutputArrayOfArrays tvecs,
       int                     flags,
       cv::TermCriteria        criteria = cv::TermCriteria(
           cv::TermCriteria::COUNT | cv::TermCriteria::EPS,
           30,
           DBL_EPSILON
       )
   );

### 目前進度

#include <opencv2/opencv.hpp>
#include <iostream>

using namespace std;

enum {
	STEREO_BM   = 0, 
	STEREO_SGBM = 1,
	STEREO_HH   = 2,
	STEREO_VAR  = 3,
	STEREO_3WAY = 4
};

void cameraCalibrate(cv::Mat& matrix, cv::Mat& coeffs, int number, cv::Size& image_size)
{
	int n_boards = 25;
	int board_w  = 6;
	int board_h  = 9;
	
	int board_n = board_w * board_h;
	cv::Size board_sz = cv::Size(board_w, board_h);

	cv::VideoCapture camera = cv::VideoCapture(number);
	if (!camera.isOpened())	return;

	vector<vector<cv::Point2f>> image_points;
	vector<vector<cv::Point3f>> object_points;

	double last_captured_timestamp = 0;

	while (image_points.size() < (size_t)n_boards)
	{
		cv::waitKey(10);

		cv::Mat image;
		camera >> image;
		image_size = image.size();

		vector<cv::Point2f> corners;
		bool found = cv::findChessboardCorners(image, board_sz, corners);
		drawChessboardCorners(image, board_sz, corners, found);

		double timestamp = (double)clock() / CLOCKS_PER_SEC;

		if (found && timestamp - last_captured_timestamp > 1)
		{
			last_captured_timestamp = timestamp;
			
			image_points.push_back(corners);
			object_points.push_back(vector<cv::Point3f>());

			vector<cv::Point3f>& opts = object_points.back();
			opts.resize(board_n);

			for (int j = 0; j < board_n; j++)
			{
				opts[j] = cv::Point3f((float)(j / board_w), (float)(j % board_h), 0.f);
			}

		}
		if (number == 1)
			cv::imshow( "Left Calibration", image);
		else
			cv::imshow("Right Calibration", image);
	}
	if (number == 1)
		cv::destroyWindow("Left Calibration");
	else 
		cv::destroyWindow("Right Calibration");

	cout << "Done!" << endl;

	cv::Mat rvecs, tvecs;
	double err = cv::calibrateCamera(
		object_points, 
		image_points, 
		image_size, 
		matrix,
		coeffs,
		rvecs,
		tvecs,
		cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_PRINCIPAL_POINT 
	);
}

void calDispWithSGBM(cv::Mat imgL, cv::Mat imgR, cv::Mat& imgDisparity8U)
{
	cv::Size imgSize = imgL.size();
	int numberOfDisparities = ((imgSize.width / 8) + 15) & -16;
	cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(0, 16, 3);

	sgbm->setPreFilterCap(63);

	int SADWindowSize = 9;
	int sgbmWinSize = SADWindowSize > 0 ? SADWindowSize : 3;
	sgbm->setBlockSize(sgbmWinSize);

	int cn = imgL.channels();
	sgbm->setP1(8 * cn * sgbmWinSize * sgbmWinSize);
	sgbm->setP2(32 * cn * sgbmWinSize * sgbmWinSize);

	sgbm->setMinDisparity(0);
	sgbm->setNumDisparities(numberOfDisparities);
	sgbm->setUniquenessRatio(10);
	sgbm->setSpeckleWindowSize(100);
	sgbm->setSpeckleRange(32);
	sgbm->setDisp12MaxDiff(1);

	int alg = STEREO_SGBM;
	if (alg == STEREO_HH)
		sgbm->setMode(cv::StereoSGBM::MODE_HH);
	else if (alg == STEREO_SGBM)
		sgbm->setMode(cv::StereoSGBM::MODE_SGBM);
	else if (alg == STEREO_3WAY)
		sgbm->setMode(cv::StereoSGBM::MODE_SGBM_3WAY);

	cv::Mat imgDisparity16S = cv::Mat(imgL.rows, imgL.cols, CV_16S);
	sgbm->compute(imgL, imgR, imgDisparity16S);

	//--Display it as a CV_8UC1 image：16位有符号转为8位无符号
	imgDisparity16S.convertTo(imgDisparity8U, CV_8U, 255 / (numberOfDisparities * 16.));
}

int main() 
{
	cv::Mat imgL, imgR, R, T, E, F, R1, R2, P1, P2, Q, map11, map12, map21, map22;
	cv::Size image_size;
	vector<vector<cv::Point3f>> object_points;

	cv::Mat cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2;
	cameraCalibrate(cameraMatrix1, distCoeffs1, 1, image_size);
	cameraCalibrate(cameraMatrix2, distCoeffs2, 0, image_size);

	for (int i = 0; i < 25; i++)
	{
		object_points.push_back(vector<cv::Point3f>());
		vector<cv::Point3f>& opts = object_points.back();
		opts.resize(54);
		for (int j = 0; j < 54; j++)
		{
			opts[j] = cv::Point3f((float)(j / 6), (float)(j % 9), 0.f);
		}
	}

	cv::VideoCapture capture1 = cv::VideoCapture(1);
	cv::VideoCapture capture2 = cv::VideoCapture(0);

	if (!capture1.isOpened() || !capture2.isOpened())
	{
		return -1;
	}

	while (true) 
	{
		capture1 >> imgL;
		capture2 >> imgR;

		cv::stereoCalibrate(
			object_points,
			imgL,	imgR,
			cameraMatrix1,	distCoeffs1,
			cameraMatrix2,	distCoeffs2,
			image_size,
			R,	T,	E,	F,
			cv::CALIB_FIX_INTRINSIC,
			cv::TermCriteria(
				cv::TermCriteria::COUNT
				| cv::TermCriteria::EPS,
				30,
				1e-6
			)
		);

		cv::stereoRectify(
			cameraMatrix1,	distCoeffs1,
			cameraMatrix2,	distCoeffs2,
			image_size,
			R,	T,	R1,	R2,	P1,	P2,
			cv::noArray(),
			0
		);

		cv::initUndistortRectifyMap(
			cameraMatrix1,
			distCoeffs1,
			R1,
			cameraMatrix1,
			image_size,
			CV_8UC3,
			map11,
			map12
		);

		cv::initUndistortRectifyMap(
			cameraMatrix2,
			distCoeffs2,
			R2,
			cameraMatrix2,
			image_size,
			CV_8UC3,
			map21,
			map22
		);

		//--And create the image in which we will save our disparities
		cv::Mat imgDisparity8U = cv::Mat(map22.rows, map12.cols, CV_8UC1);

		calDispWithSGBM(map12, map22, imgDisparity8U);

		cv::imshow("Disparity", imgDisparity8U);

		//cv::imshow("Left ", imgL);
		//cv::imshow("Right", imgR);

		if ((cv::waitKey(30) & 255) == 27)	break;
	}

	return 0;
	
}

問題：校正第二隻眼睛的時候，會強制關閉系統，跳出error

http://wiki.ros.org/stereo_image_proc
http://wiki.ros.org/image_proc
http://wiki.ros.org/camera_calibration/Tutorials/StereoCalibration
http://wiki.ros.org/usb_cam

https://ros-planning.github.io/moveit_tutorials/
http://fugjo16.blogspot.com/2017/05/ros-moveit.html