項目實戰——基於計算機視覺的物體位姿定位及機械臂矯正（四）
程序整合
接着（三）裏面的任務一，我把程序整合了一下，湊活着拿着我的這兩個垃圾相機還有我拿書上的圖當標定板。先看程序再看效果吧。好了，閒話少敘，書歸正傳，附上代碼：
/*******************************
* @Function get_depth_information
* @Works 對兩臺相機進行單目標定，隨後進行立體標定、校正、對應，生成具有深度信息的圖像
* @Author Hunt Tiger Tonight
* @Platform VS2015 C++
* @Connect phone：18398621916/QQ:136768916
* @Date 2018-11-06
********************************/
#include <opencv2/opencv.hpp>
#include <iostream>
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <vector>
#include <string>
#include <windows.h>
#include <cstring>
#include <shellapi.h>
#include <tchar.h>
#include <fstream>
#pragma comment(lib, "shell32.lib")
using namespace std;
//保存點雲數據
static void saveXYZ(const char* filename, const cv::Mat& mat)
{
const double max_z = 1.0e4;
FILE* fp = fopen(filename, "wt");
for (int y = 0; y < mat.rows; y++)
{
for (int x = 0; x < mat.cols; x++)
{
cv::Vec3f point = mat.at<cv::Vec3f>(y, x);
if (fabs(point[2] - max_z) < FLT_EPSILON || fabs(point[2]) > max_z) continue;
fprintf(fp, "%f %f %f\n", point[0], point[1], point[2]);
}
}
fclose(fp);
}
vector<cv::Mat> Camera_calibration(
int board_w, //棋盤的寬度
int board_h, //棋盤的高度
int n_boards, //監測標定圖像的數目，後面在輸入參數裏面獲取，爲了保證參數的求解精度，我們至少需要10張以上的圖像
int delay, //相機的拍攝延時爲1s
double image_sf, //縮放比例爲0.5
int cap //選擇調用相機
)
{
int n = 0;
int i = 0;
char filename[1024];
cv::Mat img_grayl;
cv::Mat img_grayr;
vector<cv::Mat> parameter;
int board_n = board_w * board_h;
cv::Size board_sz = cv::Size(board_w, board_h); //board_sz定義爲size類型的數據
//打開攝像頭
cv::VideoCapture capture(cap);
cv::VideoCapture capture1(1);
if (!capture.isOpened())
{
cout << "\n無法打開攝像頭。";
return parameter;
}
//分配儲存面
vector<vector<cv::Point2f>> image_points; //定義棋盤中圖像角點的輸出矩陣（向量中的向量）
vector<vector<cv::Point3f>> object_points; //定義物理座標系中角點的輸出矩陣（向量中的向量）
//相機不斷拍攝圖像，直到找到棋盤，並找齊所有的圖像。
double last_captured_timestamp = 0; //初始化最後一次捕獲圖像時間爲0
cv::Size image_size; //構造size型函數
//開始搜索，直到找到全部圖像
while (image_points.size() < (size_t)n_boards)
{
cv::Mat image0, image,image1; //構造原始圖像矩陣以及輸出圖像矩陣
capture >> image0; //將原始圖像存到capture中
image_size = image0.size(); //獲取image0的大小
cv::resize(image0, image, cv::Size(), image_sf, image_sf, cv::INTER_LINEAR); //縮放圖像，函數解析詳見P268
//尋找棋盤
vector<cv::Point2f> corners; //定義角點輸出矩陣
bool found = cv::findChessboardCorners(image, board_sz, corners); //尋找角點函數，詳見p568
//繪製棋盤
drawChessboardCorners(image, board_sz, corners, found); //繪製角點，詳見p569
//如果找到棋盤了，就把他存入數據中
double timestamp = (double)clock() / CLOCKS_PER_SEC; //獲取時間戳
if (found && timestamp - last_captured_timestamp > 1) //如果尋找到了棋盤
{
if (cap == 0)
{
n++;
sprintf_s(filename, "left%.2d.jpg", n);
cv::cvtColor(image0, img_grayl, cv::COLOR_BGR2GRAY);
cv::imwrite(filename, img_grayl);
cout << "\n保存了 " << filename << "文件到根目錄下" << endl;
capture1 >> image1;
sprintf_s(filename, "right%.2d.jpg", n);
cv::cvtColor(image1, img_grayr, cv::COLOR_BGR2GRAY);
cv::imwrite(filename, img_grayr);
cout << "保存了 " << filename << "文件到根目錄下" << endl;
}
last_captured_timestamp = timestamp; //將當前時間更新爲最後一次時間
image ^= cv::Scalar::all(255); //將圖像進行一次異或運算，255爲白色，即：黑變白，白變黑
cv::Mat mcorners(corners); //複製矩陣（避免破壞原有矩陣）
mcorners *= (1.0 / image_sf); //縮放角座標
image_points.push_back(corners); //在image_points後插入corners，這裏注意一下，此舉相當於，在image圖像上疊加了一個棋盤圖像
object_points.push_back(vector<cv::Point3f>()); //在object_points後插入Point3f類型函數，同理，先加上一個還沒有求解到的圖像，用一個空矩陣表示
//下面這段其實我覺得我的理解有點問題，我的理解是：將輸出圖像所佔內存大小調整到最優，獲取圖像直到數目達到預設值
vector<cv::Point3f> & opts = object_points.back(); //opts即：Options,簡單來說就是將輸出圖像的最後一位大小最優
opts.resize(board_n); //調整容器大小
for (int j = 0; j < board_n; j++)
{
opts[j] = cv::Point3f(static_cast<float>(j / board_w), static_cast<float>(j % board_w), 0.0f); //將三維數據存入opts中,注意，這個地方必須加強制轉換，不然會出錯！！！！！！
}
cout << "\n已收集到" << static_cast<uint>(image_points.size()) << "張棋盤圖像，總共需要" << n_boards << "張棋盤圖像。\n" << endl;
}
cv::imshow("Calibration", image); //顯示圖像
//等待時間爲30ms，如果在這個時間段內, 用戶按下ESC(ASCII碼爲27),則跳出循環,否則,則跳出循環
if (((cv::waitKey(30)) & 255) == 27)
return parameter;
}
//結束循環
cv::destroyWindow("Calibration"); //銷燬窗口
cout << "\n\n正在矯正相機...\n" << endl;
//校準相機
cv::Mat intrinsic_matrix, distortion_coeffs; //instrinsic_matrix:線性內在參數，3*3矩陣， distortion_coeffs：畸變係數：k1、k2、p1、p2
double err = cv::calibrateCamera(
object_points,
image_points,
image_size,
intrinsic_matrix,
distortion_coeffs,
cv::noArray(),
cv::noArray(),
cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_PRINCIPAL_POINT
); //校準相機函數，詳見P582
cout << "***Done!\n\nReprojection error is " << err ;
//計算無畸變和修正轉換映射
cv::Mat map1, map2;
cv::initUndistortRectifyMap(
intrinsic_matrix,
distortion_coeffs,
cv::Mat(),
intrinsic_matrix,
image_size,
CV_16SC2,
map1,
map2
); //計算無畸變和修正轉換映射，詳見P590
//顯示矯正的後的圖像
int secs=5;
clock_t delay1 = secs * CLOCKS_PER_SEC;
clock_t start = clock();
while (clock() - start<delay1)
{
cv::Mat image, image0;
capture >> image0;
if (image0.empty())
break;
cv::remap(
image0,
image,
map1,
map2,
cv::INTER_LINEAR,
cv::BORDER_CONSTANT,
cv::Scalar()
); //利用remap重新傳入圖像
cv::imshow("Undistored", image);
if (((cv::waitKey(30)) & 255) == 27)
break;
}
//cv::waitKey(3000);
cv::destroyWindow("Undistored");
parameter.push_back(intrinsic_matrix);
parameter.push_back(distortion_coeffs);
return parameter;
}
//定義函數，輸入變量包括：含有圖像序列號的文件、棋盤的橫向格數、棋盤的縱向格數、
//單目是否已校準（用於判斷採用Hartlely法還是Bouguet法，這裏選擇Bouguet法
static void StereoCalib(
const char *imageList,
int nx,
int ny,
bool useUncalibrated,
cv::Mat M1,
cv::Mat M2,
cv::Mat D1,
cv::Mat D2)
{
//定義一些量
bool displayCorners = true;
bool showUndistores = true;
bool isVerticalStereo = false;
const char* point_cloud_filename = 0;
const int maxScale = 1;
const float squareSize = 1.f;
FILE* f = fopen(imageList, "rt"); //定義f爲打開圖像列表，模式爲只讀
int i, j, lr;
int N = nx * ny; //定義N爲棋盤角點個數
cv::Size board_sz = cv::Size(nx, ny); //定義board_sz爲size類向量
vector<string> imageNames[2];
vector<cv::Point3f> boardModel;
vector<vector<cv::Point3f>> objectPoints;
vector<vector<cv::Point2f>> points[2];
vector<cv::Point2f> corners[2];
bool found[2] = { false,false };
cv::Size imageSize;
int ddeph = -1;
//讀取含有棋盤的圖片序列
if (!f)
{
cout << "打不開文件" << imageList << endl; //要是打不開或者找不到文件，那gg
return;
}
//將棋盤角點座標存入boardmodel中，其中距離（深度）信息爲0
for (i = 0; i < ny; i++)
for (j = 0; j < nx; j++)
boardModel.push_back(
cv::Point3f((float)(i*squareSize), (float)(j*squareSize), 0.f));
i = 0;
for (;;)
{
char buf[1024]; //申請一個長度爲1024個字節的空間，作爲字符數組使用
lr = i % 2; //lr用以判定讀取圖像爲左邊相機還是右邊相機
cout << "\nlr=" << lr << endl;
if (lr == 0)
found[0] = found[1] = false; //如果lr爲則判斷爲左棋盤
cout << "\nfound=" << found[0]<<found[1] << endl;
if (!fgets(buf, sizeof(buf) - 3, f))
break; //如果沒有讀到則終止循環
size_t len = strlen(buf); //獲取buf的字符串長度，存入len中
while (len > 0 && isspace(buf[len - 1])) //isspace：判斷是否爲空格，製表符，是則返回非零值。（其實這塊我不大理解）
buf[--len] = '\0';
if (buf[0] == '#') //遇到#則繼續
continue;
cv::Mat img = cv::imread(buf, 0); //讀取buf儲存地址所指向的照片，存入img中
if (img.empty()) //要是讀完了，就終止循環
break;
imageSize = img.size(); //將img的大小存入imagesize中
imageNames[lr].push_back(buf); //將buf存入imagename裏面
i++;
//如果在左棋盤沒有找到棋盤，那也沒必要在右棋盤去找了
if (lr == 1 && !found[0])
continue;
//找棋盤
int s;
for (s = 1; s <= maxScale; s++)
{
cv::Mat timg = img;
if (s > 1)
resize(img, timg, cv::Size(), s, s, cv::INTER_CUBIC); //縮放圖像
found[lr] = cv::findChessboardCorners(timg, board_sz, corners[lr]); //本身find函數返回的就是布爾值，直接返回到found，判斷找沒找到
if (found[lr] || s == maxScale)
{
cv::Mat mcorners(corners[lr]); //將角點的位置輸出矩陣存到mcorner中，不破壞
mcorners *= (1. / s); //將角點位置還原爲未縮放的位置
}
if (found[lr])
break;
}
if (displayCorners)
{
cout << buf << endl;
cv::Mat cimg;
cv::cvtColor(img, cimg, cv::COLOR_GRAY2BGR); //將img中的圖像轉換爲RGB圖像
cv::drawChessboardCorners(cimg, cv::Size(nx, ny), corners[lr], found[lr]); //繪製棋盤角點
cv::imshow("Corners", cimg); //顯示圖像cimg並將其命名爲Corners
if ((cv::waitKey(0) & 255) == 27) //如果按下esc退出
exit(-1);
}
else
cout << '.';
if (lr == 1 && found[0] && found[1])
{
objectPoints.push_back(boardModel); //將棋盤的座標位置信息放入objectpoint中
points[0].push_back(corners[0]); //將左側角點位置矩陣放入points[0]中
points[1].push_back(corners[1]); //將右側角點位置矩陣放入points[1]中
}
}
fclose(f); //關閉文件
//立體校正
cv::Mat R, T, E, F;
cout << "\n正在進行相機立體校正";
cv::stereoCalibrate(
objectPoints, //objectPoints，存儲標定角點在世界座標系中的位置
points[0], //imagePoints1，存儲標定角點在第一個攝像機下的投影后的亞像素座標
points[1], //imagePoints2，存儲標定角點在第二個攝像機下的投影后的亞像素座標
M1, //cameraMatrix1，輸入/輸出型的第一個攝像機的相機矩陣
//注意：如果CV_CALIB_USE_INTRINSIC_GUESS , CV_CALIB_FIX_ASPECT_RATIO ,CV_CALIB_FIX_INTRINSIC , or CV_CALIB_FIX_FOCAL_LENGTH其中的一個或多個標誌被設置，該攝像機矩陣的一些或全部參數需要被初始化
D1, //distCoeffs1，第一個攝像機的輸入/輸出型畸變向量
M2, //cameraMatrix2，輸入/輸出型的第一個攝像機的相機矩陣
D2, //distCoeffs2，第一個攝像機的輸入/輸出型畸變向量
imageSize, //imageSize，圖像的大小
R, //R，輸出型，第一和第二個攝像機之間的旋轉矩陣
T, //T，輸出型，第一和第二個攝像機之間的平移矩陣
E, //E，輸出型，本徵矩陣
F, //F，輸出型，基本矩陣
cv::CALIB_FIX_ASPECT_RATIO | cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_SAME_FOCAL_LENGTH,
/*
CV_CALIB_FIX_INTRINSIC 如果該標誌被設置，那麼就會固定輸入的cameraMatrix和distCoeffs不變，只求解R, T, E, F
CV_CALIB_USE_INTRINSIC_GUESS 根據用戶提供的cameraMatrix和distCoeffs爲初始值開始迭代
CV_CALIB_FIX_PRINCIPAL_POINT 迭代過程中不會改變主點的位置
CV_CALIB_FIX_FOCAL_LENGTH 迭代過程中不會改變焦距
CV_CALIB_FIX_ASPECT_RATIO 固定fx/fy的比值，只將fy作爲可變量，進行優化計算。
（當CV_CALIB_USE_INTRINSIC_GUESS沒有被設置，fx和fy將會被忽略。只有fx/fy的比值在計算中會被用到。)
CV_CALIB_SAME_FOCAL_LENGTH 強制保持兩個攝像機的焦距相同
CV_CALIB_ZERO_TANGENT_DIST 切向畸變保持爲零
CV_CALIB_FIX_K1, ..., CV_CALIB_FIX_K6 迭代過程中不改變相應的值。如果設置了 CV_CALIB_USE_INTRINSIC_GUESS 將會使用用戶提供的初始值，否則設置爲零
CV_CALIB_RATIONAL_MODEL 畸變模型的選擇，如果設置了該參數，將會使用更精確的畸變模型，distCoeffs的長度就會變成8
*/
cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 100, 1e-5)
/*TermCriteria模板類，作爲迭代算法的終止條件。
該類變量需要3個參數，一個是類型，第二個參數爲迭代的最大次數，最後一個是特定的閾值。
類型有TermCriteria::COUNT、TermCriteria::EPS、cv::TermCriteria::COUNT|cv::TermCriteria::EPS，
分別代表着迭代終止條件爲達到最大迭代次數終止，迭代到閾值終止，或者兩者都作爲迭代終止條件
這裏採用第三種，最大迭代次數爲100，閾值爲10^-5
*/
);
cout << "\n搞定！按任意鍵可瀏覽圖像，按ESC退出\n\n";
//校正檢查
vector<cv::Point3f> lines[2];
double avgErr = 0;
int nframes = (int)objectPoints.size();
for (i = 0; i < nframes; i++)
{
vector<cv::Point2f>&pt0 = points[0][i];
vector<cv::Point2f>&pt1 = points[1][i];
cv::undistortPoints(pt0, pt0, M1, D1, cv::Mat(), M1); //根據觀察到的點座標計算理想點座標
/*
src， 觀察到的點座標
dst，在非失真和反向透視變換後輸出理想點座標。
cameraMatrix
distCoeffs
R - 對象空間中的整流變換（3x3矩陣），如果矩陣爲空，則使用身份轉換。
P - 新的相機矩陣（3x3）或新的投影矩陣（3x4）。
*/
cv::undistortPoints(pt1, pt1, M2, D2, cv::Mat(), M2);
cv::computeCorrespondEpilines(pt0, 1, F, lines[0]);
cv::computeCorrespondEpilines(pt1, 2, F, lines[1]); //極線計算
for (j = 0; j < N; j++)
{
double err = fabs(pt0[j].x*lines[1][j].x + pt0[j].y*lines[1][j].y + lines[1][j].z) +
fabs(pt1[j].x*lines[0][j].x + pt1[j].y*lines[0][j].y + lines[0][j].z);
avgErr += err;
}
}
cout << "平均誤差爲：" << avgErr / (nframes*N) << endl;
//計算、顯示校準之後的圖像
if (showUndistores)
{
cv::Mat R1, R2, P1, P2, map11, map12, map21, map22, Q;
//如果單目已校準，則採用bouguet法
if (!useUncalibrated)
{
stereoRectify(M1, D1, M2, D2, imageSize, R, T, R1, R2, P1, P2, Q, 0); //bouguet算法
isVerticalStereo = fabs(P2.at<double>(1, 3)) > fabs(P2.at<double>(0, 3)); //判斷圖像是垂直還是水平
//矯正映射
initUndistortRectifyMap(M1, D1, R1, P1, imageSize, CV_16SC2, map11, map12);
initUndistortRectifyMap(M2, D2, R2, P2, imageSize, CV_16SC2, map21, map22);
}
else
{
vector<cv::Point2f> allpoints[2];
for (i = 0; i < nframes; i++)
{
copy(points[0][i].begin(), points[0][i].end(),
back_inserter(allpoints[0]));
copy(points[1][i].begin(), points[1][i].end(),
back_inserter(allpoints[1]));
}
cv::Mat F = findFundamentalMat(allpoints[0], allpoints[1], cv::FM_8POINT);
cv::Mat H1, H2;
cv::stereoRectifyUncalibrated(allpoints[0], allpoints[1], F, imageSize,
H1, H2, 3);
R1 = M1.inv() * H1 * M1;
R2 = M2.inv() * H2 * M2;
cv::initUndistortRectifyMap(M1, D1, R1, P1, imageSize, CV_16SC2, map11, map12);
cv::initUndistortRectifyMap(M2, D2, R2, P2, imageSize, CV_16SC2, map21, map22);
}
//校正並顯示圖像
cv::Mat pair;
if (!isVerticalStereo)
pair.create(imageSize.height, imageSize.width * 2, CV_8UC3);
else
pair.create(imageSize.height * 2, imageSize.width, CV_8UC3);
//進行對應
cv::Ptr<cv::StereoSGBM>stereo = cv::StereoSGBM::create
(-64, 128, 11, 100, 1000, 32, 0, 15, 1000, 16, cv::StereoSGBM::MODE_HH);
for (i = 0; i < nframes; i++)
{
cv::Mat img1 = cv::imread(imageNames[0][i].c_str(), 0);
cv::Mat img2 = cv::imread(imageNames[1][i].c_str(), 0);
cv::Mat img1r, img2r, disp, vdisp;
if (img1.empty() || img2.empty())
continue;
cv::remap(img1, img1r, map11, map12, cv::INTER_LINEAR);
cv::remap(img2, img2r, map21, map22, cv::INTER_LINEAR);
if (!isVerticalStereo || !useUncalibrated)
{
stereo->compute(img1r, img2r, disp);
cv::normalize(disp, vdisp, 0, 256, cv::NORM_MINMAX, CV_8U);
cv::imshow("disparity", vdisp);
}
char* rute = "dispdata.txt";
ofstream o_file(rute); //輸出文件流，將數據輸出到文件
for (int i = 0; i<vdisp.rows; i++)
{
for (int j = 0; j<vdisp.cols; j++)
{
o_file << int(vdisp.at<uchar>(cv::Point(j, i))) << " ";
}
o_file << "\n";
}
point_cloud_filename = "point_cloud.txt";//保存雲點
if (point_cloud_filename)
{
printf("storing the point cloud...");
fflush(stdout);
cv::Mat xyz;
cv::reprojectImageTo3D(vdisp, xyz, Q, true);
saveXYZ(point_cloud_filename, xyz);
printf("\n");
}
if (!isVerticalStereo) //水平對應或垂直對應
{
cv::Mat part = pair.colRange(0, imageSize.width); //提取pair中的一列放入part中
cvtColor(img1r, part, cv::COLOR_GRAY2BGR);
part = pair.colRange(imageSize.width, imageSize.width * 2);
cvtColor(img2r, part, cv::COLOR_GRAY2BGR);
for (j = 0; j < imageSize.height; j += 16)
cv::line(pair, cv::Point(0, j), cv::Point(imageSize.width * 2, j), cv::Scalar(0, 255, 0));
}
else
{
cv::Mat part = pair.rowRange(0, imageSize.height);
cv::cvtColor(img1r, part, cv::COLOR_GRAY2BGR);
part = pair.rowRange(imageSize.height, imageSize.height * 2);
cv::cvtColor(img2r, part, cv::COLOR_GRAY2BGR);
for (j = 0; j < imageSize.width; j += 16)
line(pair, cv::Point(j, 0), cv::Point(j, imageSize.height * 2),
cv::Scalar(0, 255, 0));
}
cv::imshow("對應圖像", pair);
if ((cv::waitKey() & 255) == 27)
break;
}
}
}
int main()
{
int board_w = 9, board_h = 6;
vector<cv::Mat> parameter1, parameter2;
cv::Mat intrinsic_matrix1, distortion_coeffs1, intrinsic_matrix2, distortion_coeffs2;
parameter1= Camera_calibration(9, 6, 14, 500, 0.5, 0);
intrinsic_matrix1 = parameter1[0];
distortion_coeffs1 = parameter1[1];
cout << "\n攝像頭1的內在參數爲：" << intrinsic_matrix1 <<"\n攝像頭1的畸變參數爲："<< distortion_coeffs1 <<endl;
parameter2 = Camera_calibration(9, 6, 14, 500, 0.5, 1);
intrinsic_matrix2 = parameter2[0];
distortion_coeffs2 = parameter2[1];
cout << "\n攝像頭2的內在參數爲：" << intrinsic_matrix2 << "\n攝像頭2的畸變參數爲：" << distortion_coeffs2 << endl;
const char *board_list = "../get_depth_information/list.txt";
StereoCalib(board_list, board_w, board_h, false, intrinsic_matrix1, intrinsic_matrix2, distortion_coeffs1, distortion_coeffs2);
return 0;
}
效果展示
效果不是很理想，主要原因是兩個相機還有標定板的問題。標定板就是歪的。23333.什麼時候等我更新了裝備再看吧，hhhhh。好了，下一篇開始解決第二個任務。
以上就是全部具體內容，可能會有問題，歡迎各路大佬指教。
Hunt Tiger Tonight
2018-11-06
PS:原創內容，轉載請註明出處。