项目实战——基于计算机视觉的物体位姿定位及机械臂矫正(三)
拖了很久,一直没写,今天终于抽时间写了。orz…
立体校正、标定、对应
原理就不多说了,网上有很多资料,可以参考《learning opencv3》的第19章:投影与三维视觉。或者参考下面的链接,这个讲的也不错:
程序+最详细注解
事先声明,该程序所用到的图片是参考的《Learning Opencv3:Computer Vision in C++ with the Opencv Library》一书中给出的图片。(我自己的标定板还没到,就先用这个了)好了,闲话少叙,书归正传,附上代码:
/*******************************
* @Function Stereo calibration and rectification
* @Works 立体校正、标定、对应
* @Author Hunt Tiger Tonight
* @Platform VS2015 C++
* @Connect phone:18398621916/QQ:136768916
* @Date 2018-11-03
********************************/
#pragma warning(disable : 4996)
#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>
using namespace std;
//保存点云数据(x,y为坐标,z为深度)
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);
}
//定义函数,输入变量包括:含有图像序列号的文件、棋盘的横向格数、棋盘的纵向格数、
//单目是否已校准(用于判断采用Hartlely法还是Bouguet法,这里选择Bouguet法
static void StereoCalib(
const char *imageList,
int nx,
int ny,
bool useUncalibrated)
{
//定义一些量
cv::Mat xyz;
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 }; //定义左右相机,00为左,10为右
cv::Size imageSize;
//读取含有棋盘的图片序列
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 M1 = cv::Mat::eye(3, 3, CV_64F);
cv::Mat M2 = cv::Mat::eye(3, 3, CV_64F); //创建两个3*3的单位矩阵
cv::Mat D1, D2, 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;
const char *board_list = "../stereoData/example_19-03_list.txt";
StereoCalib(board_list, board_w, board_h, false);
return 0;
}
效果展示
这样看不太方便,我把视差图数据和点云数据专门存到了txt文件中:dispdata.txt point_cloud.txt这两个文件就是了。下面我把这两个数据导入到matlab里面,并且polt出来:
这样看着没什么感觉对吧,感觉什么都不是啊,来让我们旋转一下:
是不是瞬间感觉很相近了。其中点云图的距离信息就是我们最终需要的东西。
接下来我有两个任务,其一:将单目标定和立体校正这些写到一起,这样立体校正的时候就可以直接调用单目标定的矩阵信息了。其二:进行图像分割,准确的识别出我们之前设计出的标志所在,这样根据标志的距离信息,就可以准确还原牛奶盒的位姿信息了。当然这里有一个小隐患,上面的视差图还有点云图并不完整,就是说有一些点没有计算出来距离信息,如果后面识别到了标志,但是没有就算出来位置就有麻烦了。见招拆招吧,真要是出了这个问题再想办法解决吧。接下来我我先完成第一个任务。
以上就是相机立体标定、校正、对应的具体内容,可能会有问题,欢迎各路大佬指教。
Hunt Tiger Tonight
2018-11-06
PS:原创内容,转载请注明出处。