双目测距 #双目测距 #测距 #opencv #计算机视觉

双目测距具有一定的准确度，但是其测距方法又分为好几种。下面的代码是基于在两个相机中寻找特征点然后进行匹配的算法。x,y方向误差较小，可忽略不计，深度方向误差比其他两个方向的误差大，但是做一般的项目应该够用了。下文代码是对棋盘格进行的测距，使用者可以把代码中的角点检测部分换成自己的特征点检测就可以了。相机的内参在代码前面已经标注的很清楚了，如果自己标定的相机旋转矩阵是3*3的用第二段代码Rodrigues公式进行转换就好了。奥利给bro！

#include<iostream>
#include <vector>
#include<fstream>
#include <opencv2/opencv.hpp>
using namespace cv;
using namespace std;


cv::Mat intrinsic_left = (cv::Mat_<double>(3, 3) << 1575.26208905704, -0.378715827631599, 621.557253467198, 0, 1575.23412522919, 524.666684143856,0,0,1);
cv::Mat distortion_left = (cv::Mat_<double>(1, 5) << -0.00679014370494508, -1.79019351240471, 0.000799734826740669, -0.000493053991172994, 15.4279469216416);
cv::Mat intrinsic_right = (cv::Mat_<double>(3, 3) << 1579.77019730382, 1.22654331986371, 600.994061493216, 0, 1578.71050664298, 494.651363640351,0,0,1);
cv::Mat distortion_right = (cv::Mat_<double>(1, 5) << -0.0432178824606311, -0.105130182050673, 0.00144889097656888, 0.00310629154859543, 0.893761490858914);
cv::Mat R = (cv::Mat_<double>(3, 1) << 0.01345270813866129, 0.009600147184628165, -0.003183689182796215);
cv::Mat T = (cv::Mat_<double>(3, 1) << -129.495013753560,-1.06061832082729, 2.46355590019097);

cv::Mat undistort_image(const cv::Mat& img, const cv::Mat& intrinsic, const cv::Mat& distortion);
bool triangulate_points(const std::vector<cv::Point2f>& corners_left, const std::vector<cv::Point2f>& corners_right,
	const cv::Mat& m_left, const cv::Mat& kc_left, const cv::Mat& m_right, const cv::Mat& kc_right,
	const cv::Mat& rot, const cv::Mat& trans,std::vector<cv::Point3f>& points);

int main()
{
	cv::Mat img_left = cv::imread("D:/摄像机照片/工业相机测试照片/-45/left44.jpg", 0);
	cv::Mat img_right = cv::imread("D:/摄像机照片/工业相机测试照片/-45/right44.jpg", 0);
	cv::Mat undistort_img_left = undistort_image(img_left, intrinsic_left, distortion_left);
	cv::Mat undistort_img_right = undistort_image(img_right, intrinsic_right,distortion_right);
	cv::Size board_size(8, 11);     //标定板的尺寸
	std::vector<cv::Point2f> corners_left, corners_right;
	bool find_board = cv::findChessboardCorners(undistort_img_left, board_size, corners_left);
	if (find_board)
	{
		cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS,40,0.01);
		//亚像素检测
		cv::cornerSubPix(undistort_img_left, corners_left, cv::Size(9,9), cv::Size(-1, -1), criteria);

		//******cv::cornerSubPix(undistort_img_left, corners_left, cv::Size(9, 9), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.001));
		//cv::Mat img_rgb;
		//cv::cvtColor(img_left, img_rgb, cv::COLOR_GRAY2RGB);
		//cv::drawChessboardCorners(img_rgb, board_size, corners_left, true);
		//cv::namedWindow("left", 0);
		//cv::imshow("left", img_rgb);
	}
	else
	{
		std::cout << "[ERROR] 左图片角点提取失败." << std::endl;
		return -1;
	}
	std::cout << "[ERROR] 左图像角点提取成功." << std::endl;

	find_board = cv::findChessboardCorners(undistort_img_right, board_size, corners_right);
	if (find_board)
	{
		cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 40, 0.01);
		//亚像素检测
		cv::cornerSubPix(undistort_img_right, corners_right, cv::Size(9, 9), cv::Size(-1, -1), criteria);
		//********cv::cornerSubPix(undistort_img_right, corners_right, cv::Size(9, 9), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.001));
		//cv::Mat img_rgb;
		//cv::cvtColor(img_right, img_rgb, cv::COLOR_GRAY2RGB);
		//cv::drawChessboardCorners(img_rgb, board_size, corners_right, true);
		//cv::namedWindow("right", 0);
		//cv::imshow("right", img_rgb);
		//cv::waitKey(0);
	}
	else
	{
		std::cout << "[ERROR] 右图片角点提取失败." << std::endl;
		return -1;
	}
	std::cout << "[ERROR] 右图像角点提取成功." << std::endl;

	std::vector<cv::Point3f> points;
	if (!triangulate_points(corners_left,corners_right,intrinsic_left,distortion_left,intrinsic_right,distortion_right,R,T,points))
	{
		std::cout << "[ERROR] 重建三维点失败." << std::endl;
		return false;
	}
	std::fstream _w;
	_w.open("points.txt", std::ios::out);
	for (int i = 0; i < points.size();++i)
	{
		_w << points[i].x << " " << points[i].y << " " << points[i].z << std::endl;
	}
	_w.close();
	_w.clear();
	std::cout << "[INFO] 计算成功." << std::endl;
	return 0;
}

cv::Mat undistort_image(const cv::Mat& img, const cv::Mat& intrinsic, const cv::Mat& distortion)
{
	cv::Mat img_undistort;
	cv::Mat eye_rot = cv::Mat::eye(3, 3, CV_32FC1);
	cv::Mat mapx, mapy;
	cv::initUndistortRectifyMap(intrinsic, distortion, eye_rot, intrinsic, img.size(),CV_32FC1, mapx, mapy);
	cv::remap(img, img_undistort, mapx, mapy, cv::INTER_LINEAR);
	return img_undistort;
}

bool triangulate_points(const std::vector<cv::Point2f>& corners_left, const std::vector<cv::Point2f>& corners_right,
	const cv::Mat& m_left, const cv::Mat& kc_left, const cv::Mat& m_right, const cv::Mat& kc_right,
	const cv::Mat& rot, const cv::Mat& trans, std::vector<cv::Point3f>& points)
{
	if (corners_left.size()!=corners_right.size())
	{
		std::cout << "[ERROR] 输入角点数不相等." << std::endl;
		return false;
	}
	cv::Mat zero31 = cv::Mat::zeros(3, 1, CV_64FC1);
	cv::Mat pro_left;
	cv::hconcat(m_left, zero31, pro_left);

	cv::Mat R_rod, homo_transform;
	cv::Rodrigues(R, R_rod);
	cv::hconcat(R_rod, T, homo_transform);
	cv::Mat pro_right = m_right*homo_transform;
//距离计算
	for (int i = 0; i < corners_left.size();++i)
	{
		cv::Mat A(4, 3, CV_32FC1);
		A.at<float>(0, 0) = corners_left[i].x*pro_left.at<double>(2, 0) - pro_left.at<double>(0, 0);
		A.at<float>(0, 1) = corners_left[i].x*pro_left.at<double>(2, 1) - pro_left.at<double>(0, 1);
		A.at<float>(0, 2) = corners_left[i].x*pro_left.at<double>(2, 2) - pro_left.at<double>(0, 2);
		
		A.at<float>(1, 0) = corners_left[i].y*pro_left.at<double>(2, 0) - pro_left.at<double>(1, 0);
		A.at<float>(1, 1) = corners_left[i].y*pro_left.at<double>(2, 1) - pro_left.at<double>(1, 1);
		A.at<float>(1, 2) = corners_left[i].y*pro_left.at<double>(2, 2) - pro_left.at<double>(1, 2);

		A.at<float>(2, 0) = corners_right[i].x*pro_right.at<double>(2, 0) - pro_right.at<double>(0, 0);
		A.at<float>(2, 1) = corners_right[i].x*pro_right.at<double>(2, 1) - pro_right.at<double>(0, 1);
		A.at<float>(2, 2) = corners_right[i].x*pro_right.at<double>(2, 2) - pro_right.at<double>(0, 2);

		A.at<float>(3, 0) = corners_right[i].y*pro_right.at<double>(2, 0) - pro_right.at<double>(1, 0);
		A.at<float>(3, 1) = corners_right[i].y*pro_right.at<double>(2, 1) - pro_right.at<double>(1, 1);
		A.at<float>(3, 2) = corners_right[i].y*pro_right.at<double>(2, 2) - pro_right.at<double>(1, 2);
		
		cv::Mat B(4, 1, CV_32FC1);
		B.at<float>(0, 0) = pro_left.at<double>(0, 3) - corners_left[i].x*pro_left.at<double>(2, 3);
		B.at<float>(1, 0) = pro_left.at<double>(1, 3) - corners_left[i].y*pro_left.at<double>(2, 3);
		B.at<float>(2, 0) = pro_right.at<double>(0, 3) - corners_right[i].x*pro_right.at<double>(2, 3);
		B.at<float>(3, 0) = pro_right.at<double>(1, 3) - corners_right[i].y*pro_right.at<double>(2, 3);
		cv::Mat p = A.inv(cv::DECOMP_SVD)*B;
		cv::Point3f pnt;
		pnt.x = p.at<float>(0, 0);
		pnt.y = p.at<float>(1, 0);
		pnt.z = p.at<float>(2, 0);
		points.push_back(pnt);
	}
	return true;
}

Rodrigues转换

#include<iostream>
#include <vector>
#include<fstream>
#include<stdlib.h>
#include <opencv2/opencv.hpp>
using namespace cv;
using namespace std;

int main()
{
	Mat rotate = (Mat_<double>(3, 3) << 0.999948851856039, 0.00324811134025067, 0.00957827983042831,
									    -0.00311896641074822, 0.999904446639143, -0.0134673545954139,
										-0.00962110806078380, 0.0134367914321585, 0.999863434132728);
	Mat R;
	Rodrigues(rotate,R);
	cout << R << endl;
	system("pause");
	return 0;
}

双目测距

继续阅读

自监督｜「CoCLR」视频自监督对比学习笔记

ubuntu14.04 编译安装OpenCV 3.2

opencv学习笔记（十四）——图像像素的访问

opencv028-轮廓发现

qt-opencv图像增强之边缘检测（多线程）1 .pro项目文件配置2 ui设计3 多线程逻辑4 Canny边缘检测和Laplace算子5 主要代码6 运行结果

DLP面曝光3D打印机模型数据矫正

视频对象分割（Video Object Segmentation）研究小记任务定义与数据集技术路线分类基于神经网络的模型总结

opencv视觉跟踪——消除背景建模

图形处理单元(GPU)的演进

VS2010 / MFC + OpenCV 2.4.9打开图片

2021-09-30三维点云测量正方形包裹体积

DOG算子

Gsl在VS2008下的配置和使用

如何用Opencv求图像的灰度投影曲线

opencv——图像的灰度处理（线性变换/拉伸/直方图/均衡化）

cs231n斯坦福基于卷积神经网络的CV学习笔记（一）KNN和线性分类器/分类器损失/反向传播一，KNN图像分类算法二，线性分类器三，线性分类器损失四，反向传播五，神经网络