1.单目相机模型
齐次坐标与非齐次坐标转换:
void visionagin:: Myhomogeneous()
{
vector<Point3f> point3;
point3.push_back(Point3f(18, 14, 2));
point3.push_back(Point3f(25, 45, 5));
Mat point4;
convertPointsToHomogeneous(point3, point4);
Mat point2;
convertPointsFromHomogeneous(point3, point2);
for (int i = 0; i < point3.size(); i++)
{
cout << "非齐次:" << point3[i] << endl;
cout << "齐次: " << point4.at<Vec4f>(i, 0) << endl;
}
for (int j = 0; j < point3.size(); ++j)
{
cout << "非齐次: " << point2.at<Vec2f>(j, 0) << endl;
cout << "齐次:" << point3[j] << endl;
}
}
2.标定板角点提取
void visionagin::Mychessboard()
{
Mat imgboard = imread("C:\\Users\\86176\\Downloads\\visionimage\\left01.jpg");
if (imgboard.empty())
{
cout << "读取失败!" << endl;
}
Mat gry;
cvtColor(imgboard, gry, COLOR_BGR2GRAY);
vector<Point2f> conners;
Size boardsize(6, 9);
findChessboardCorners(gry, boardsize, conners);
find4QuadCornerSubpix(gry, conners, Size(5, 5));
drawChessboardCorners(imgboard, boardsize, conners, true);
imshow("out", imgboard);
}
3.单目相机标定
void visionagin:: Mycailbratecamera()
{
ifstream ft("C:\\Users\\86176\\Downloads\\visionimage\\totalphotos.txt");
vector<Mat> images;
string imgname;//图像名称
string source = "C:\\Users\\86176\\Downloads\\visionimage\\";
while (getline(ft, imgname))
{
cout << imgname << endl;
Mat img = imread(source+imgname);//注意路径
images.push_back(img);
}
Size bordsize(9, 6);
vector<vector<Point2f>> points2;//多幅图像的角点组的集合
vector<vector<Point3f>> points3;//多幅图像的角点组的三维世界坐标
//计算每副图像角点,
for (int i = 0; i < images.size(); ++i)
{
vector<Point2f> point2;
Mat tempimg = images[i];
Mat gry;
cvtColor(images[i], gry, COLOR_BGR2GRAY);
findChessboardCorners(gry, bordsize,point2);//图像用灰度图
find4QuadCornerSubpix(gry, point2, Size(5, 5));//棋盘角点细化
points2.push_back(point2);
}
Size squaresize(10, 10);//棋盘每个方格尺寸
//计算每副图像的角点组的三维坐标
for (int i = 0; i < images.size(); ++i)
{
vector<Point3f> temppoints3;//单幅图像的角点的三维坐标
for (int j = 0; j < bordsize.height; ++j)
{
for (int k = 0; k < bordsize.width; ++k)
{
Point3f temppoint3;//单个角点
temppoint3.y = j * 10;
temppoint3.x = k * 10;
temppoint3.z = 0;
temppoints3.push_back(temppoint3);
}
}
points3.push_back(temppoints3);
}
Size imgsize;
imgsize.width = images[0].cols;
imgsize.height = images[0].rows;
//相机的内参矩阵 3*3 cv_32fc1
Mat cameraM = Mat(3, 3, CV_32FC1, Scalar::all(0));
//畸变系数 k1 k2 p1 p2 k3
Mat distcoeff = Mat(1,5 , CV_32FC1, Scalar::all(0));
vector<Mat>rvecs;//每幅图像的旋转矩阵
vector<Mat>tvecs;//每幅图像的平移矩阵
calibrateCamera(points3, points2, imgsize, cameraM, distcoeff, rvecs, tvecs);
cout << "内参矩阵:" << cameraM << endl;
cout << "畸变系数" << distcoeff << endl;
}
4.单目相机校正
实现1:
void visionagin:: Myundisort1()
{
ifstream ift("C:\\Users\\86176\\Downloads\\visionimage\\totalphotos.txt");
string imagename;
string temp("C:\\Users\\86176\\Downloads\\visionimage\\");
vector<Mat>imgs;
while (getline(ift, imagename))
{
Mat img = imread(temp + imagename);
imgs.push_back(img);
}
//导入上次标定得到内参和畸变系数
Mat cameraM = (Mat_<float>(3, 3) << 532.0162975827582, 0, 332.1725192389595,
0, 531.5651587897565, 233.3880748229786,
0, 0, 1);
Mat distcoeff=(Mat_<float>(1, 5) << -0.2851884066280067, 0.0800972142572013, 0.001274031468563325, -0.002415106464876228, 0.1065791099058786);
for (int i = 0; i < imgs.size(); ++i)
{
imshow("原图像" + to_string(i), imgs[i]);
Mat temp;
undistort(imgs[i], temp, cameraM, distcoeff);
imshow("矫正后图像" + to_string(i), temp);
}
}
实现2:
void visionagin::Myundisortremap()
{
ifstream ift("C:\\Users\\86176\\Downloads\\visionimage\\totalphotos.txt");
string imagename;
string temp("C:\\Users\\86176\\Downloads\\visionimage\\");
vector<Mat>imgs;
while (getline(ift, imagename))
{
Mat img = imread(temp + imagename);
imgs.push_back(img);
}
//导入上次标定得到内参和畸变系数
Mat cameraM = (Mat_<float>(3, 3) << 532.0162975827582, 0, 332.1725192389595,
0, 531.5651587897565, 233.3880748229786,
0, 0, 1);
Mat distcoeff = (Mat_<float>(1, 5) << -0.2851884066280067, 0.0800972142572013, 0.001274031468563325, -0.002415106464876228, 0.1065791099058786);
Size imgsize;
imgsize.width = imgs[0].cols;
imgsize.height = imgs[0].rows;
Mat r = Mat::eye(3, 3, CV_32FC1);
Mat remapx = Mat(imgsize, CV_32FC1);
Mat remapy = Mat(imgsize, CV_32FC1);
initUndistortRectifyMap(cameraM, distcoeff, r, cameraM, imgsize, CV_32FC1, remapx, remapy);
for (int i = 0; i < imgs.size(); ++i)
{
imshow("原图像" + to_string(i), imgs[i]);
Mat temp;
remap(imgs[i], temp, remapx, remapy, INTER_LINEAR);
imshow("矫正后图像" + to_string(i), temp);
}
}
5.单目投影
void visionagin:: Myprojectpoints()
{
Mat imgboard = imread("C:\\Users\\86176\\Downloads\\visionimage\\left01.jpg");
Mat cameraM = (Mat_<float>(3, 3) << 532.0162975827582, 0, 332.1725192389595,
0, 531.5651587897565, 233.3880748229786,
0, 0, 1);
Mat distcoeff = (Mat_<float>(1, 5) << -0.2851884066280067, 0.0800972142572013, 0.001274031468563325, -0.002415106464876228, 0.1065791099058786);
Mat rvec = (Mat_<float>(1, 3) <<-1.977853, -2.002220, 0.130029);
Mat tvec = (Mat_<float>(1, 3) << -26.88155, -42.79936, 159.19703);
vector<Point3f> points3;
Size bordsize(9, 6);
Size square(10, 10);
for (int i = 0; i < bordsize.height; ++i)
{
for (int j = 0; j < bordsize.width; ++j)
{
Point3f temp;
temp.x = i * 10;
temp.y = 10 * j;
temp.z = 0;
points3.push_back(temp);
}
}
vector<Point2f> propoints2;//投影到二维的结果
projectPoints(points3, rvec, tvec, cameraM, distcoeff, propoints2);
vector<Point2f>points2;
Mat gray;
cvtColor(imgboard, gray, COLOR_BGR2GRAY);
findChessboardCorners(gray, bordsize, points2);
find4QuadCornerSubpix(gray, points2, Size(5, 5));
//计算投影到二维的点与计算出的角点的误差
double e = 0;
for (int k = 0; k < points2.size(); ++k)
{
double ex = pow(points2[k].x-propoints2[k].x, 2);
double ey = pow(points2[k].y- propoints2[k].y, 2);
e += sqrt(ex+ey);
}
e /= points2.size();
cout << "误差:" << e << endl;
}
6.单目位姿估计
void visionagin:: Mysolvepnpandransac()
{
Mat imgboard = imread("C:\\Users\\86176\\Downloads\\visionimage\\left01.jpg");
Mat boardgry;
cvtColor(imgboard, boardgry, COLOR_BGR2GRAY);
Size boardsize(9, 6);
Size squaresize(10, 10);
vector<Point2f> points2;
findChessboardCorners(boardgry, boardsize, points2);
find4QuadCornerSubpix(boardgry, points2, Size(5,5));
vector<Point3f> points3;
for (int i = 0; i < boardsize.height; i++)
{
for (int j = 0; j < boardsize.width; ++j)
{
Point3f temp;
temp.x = squaresize.width * i;
temp.y = squaresize.height * j;
temp.z = 0;
points3.push_back(temp);
}
}
//使用之前标定得到的内参,畸变系数;
Mat cameraM = (Mat_<float>(3, 3) << 532.0162975827582, 0, 332.1725192389595,
0, 531.5651587897565, 233.3880748229786,
0, 0, 1);
Mat distcoeff=(Mat_<float>(1,5)<< -0.2851884066280067, 0.0800972142572013, 0.001274031468563325, -0.002415106464876228, 0.1065791099058786);
Mat rvec, tvec;//旋转,平移向量
solvePnP(points3, points2, cameraM, distcoeff, rvec, tvec);
cout << "世界坐标系转化到相机坐标系的旋转向量:" << rvec << endl;
cout << "世界坐标系转化到相机坐标系的平移向量:" << tvec << endl;
//向量转化成矩阵
Mat R, T;
Rodrigues(rvec, R);
Rodrigues(tvec, T);
cout << "世界坐标系转化到相机坐标系的旋转矩阵:" << R << endl;
cout << "世界坐标系转化到相机坐标系的平移矩阵:" << T << endl;
Mat ransacrvec, ransactvec;
//用solvepnpransac计算 r,t
solvePnPRansac(points3, points2, cameraM, distcoeff, ransacrvec, ransactvec );
Mat ransacR, ransacT;
Rodrigues(ransacrvec, ransacR);
Rodrigues(ransactvec, ransacT);
cout << "ransac方法,世界坐标系转化到相机坐标系的旋转矩阵:" << ransacR << endl;
cout << "ransac方法,世界坐标系转化到相机坐标系的平移矩阵:" << ransacT << endl;
}