摘要：双目三维重建系统双目标定立体校正双目测距点云显示目录双目三维重建系统双目标定立体校正双目测距点云显示双目三维重建系统双目标定立体校正双目测距点云显示项目结构项目结构双目相机标定和校准双目相机标定和校准双目摄像头双目摄
目录
双目三维重建系统(双目标定+立体校正+双目测距+点云显示)Python
博客将实现Python版本的双目三维重建系统，项目代码实现包含：`双目标定`，`立体校正（含消除畸变)`，`立体匹配`，`视差计算`和`深度距离计算/3D坐标计算` 的知识点。限于篇幅，本博客不会过多赘述算法原理，而是手把手教你，如果搭建一套属于自己的双目三维重建的系统。项目代码包含：
【尊重原创，转载请注明出处】：https://panjinquan.blog.csdn.net/article/details/121301896
先放一张动图，这是最终重建的效果：
【完整的项目代码】：https://download.csdn.net/download/guyuealian/42517006
三维重建中，除了双目相机，还有TOF和结构光3D 相机
飞行时间(Time of flight,TOF)，代表公司微软Kinect2，PMD，SoftKinect， 联想 Phab，在手机中一般用于3D建模、AR应用，AR测距(华为TOF镜头)
双目视觉(Stereo Camera)，代表公司 Leap Motion， ZED， 大疆;
结构光(Structured-light)，代表公司有奥比中光，苹果iPhone X(Prime Sense)，微软 Kinect1，英特尔RealSense, Mantis Vision 等，在手机（iPhone，华为）中3D结构光主要用于人脸解锁、支付、美颜等场景。
关于3D相机技术(飞行时间+双目+结构光)的区别，可以参考我的一篇博客《3D相机技术调研(飞行时间TOF+双目+结构光)》：
3D相机技术调研(飞行时间TOF+双目+结构光)_pan_jinquan的博客-CSDN博客1.深度估计3D相机方案目前市面上常有的 3D 相机方案就就是这3种：飞行时间(Time of flight,TOF)，代表公司微软Kinect2，PMD，SoftKinect， 联想 Phab，在手机中一般用于3D建模、AR应用，AR测距(华为TOF镜头)双目视觉(Stereo Camera)，代表公司 Leap Motion， ZED， 大疆;结构光(Structured-light)，代表公司有奥比中光，苹果iPhone X(Prime Sense)，微软Kin..https://panjinquan.blog.csdn.net/article/details/119649838果你对结构光三维重建感兴趣，可参考鄙人的博客《结构光三维重建-3D Scanning Software实现三维重建》
.├── config # 相机参数文件├── core # 相机核心算法包├── data # 相机采集的数据├── demo # demo文件├── libs # 第三方依赖包├── scripts # 脚本│ ├── mono_camera_calibration.sh # 单目相机校准脚本│ └── stereo_camera_calibration.sh # 双目相机校准脚本├── get_stereo_images.py # 采集标定文件├── mono_camera_calibration.py # 单目相机标定├── stereo_camera_calibration.py # 双目相机标定└── README.md下面这款双目摄像头（RGB+RGB）是在某宝购买的（几百元，链接就不发了，免得说我打广告），作为本项目的双目相机，其基线是固定的6cm，是单USB连接线的双目摄像头(左右摄像头被拼接在同一个视频中显示)，基本满足我们测试需求。一般基线越长，可测量的距离越远，网友也可以根据自己需要购买。
一点注意事项：
- 双目相机三维重建也可以使用RGB+IR(红外)的摄像头，甚至IR+IR的也是可以，本人亲测，RGB+IR的相机，其效果也是杠杠的。
- 基线不太建议太小，作为测试，一般baseline在3~9cm就可以满足需求，有些无人车的双目基线更是恐怖到1~2米长
- 从双目三维重建原理中可知，左右摄像头的成像平面尽可能在一个平面内，成像平面不在同一个平面的，尽管可以立体矫正，其效果也差很多。
- 一分钱，一分货，相机的质量好坏，直接决定了你的成像效果
width=8 height=11left_video=0right_video=-1save_dir="data/camera"detect=Truepython get_stereo_images.py / --left_video $left_video / --right_video $right_video / --width $width / --height $height / --save_dir $save_dir / --detect $detect /参数说明：
- 参数width指的是棋盘格宽方向黑白格子相交点个数
- 参数height指的是棋盘格长方向黑白格子相交点个数
- 参数left_video是左路相机ID，一般就是相机连接主板的USB接口号
- 参数right_video是右路相机ID，一般就是相机连接主板的USB接口号
- PS：如果你的双目相机是单USB连接线的双目摄像头(左右摄像头被拼接在同一个视频中显示)，则设置left_video=相机ID，而right_video=-1，
- 参数
detect建议设置True，这样可实时检测棋盘格，方面调整角度- 按键盘
s或者c保存左右视图图片
| left_image | right_image |
下面是采集双目摄像头标定板左右视图的Python代码：get_stereo_images.py，除了OpenCV，没啥依赖，直接干就完事。
import osimport argparseimport cv2class StereoCamera(object): """采集双目标定图片，按键盘【c】或【s】保存图片""" def __init__(self, chess_width, chess_height, detect=False): """ :param chess_width: chessboard width size，即棋盘格宽方向黑白格子相交点个数， :param chess_height: chessboard height size，即棋盘格长方向黑白格子相交点个数 :param detect: 是否实时检测棋盘格，方便采集数据 """ self.chess_width = chess_width self.chess_height = chess_height self.detect = detect self.criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) def detect_chessboard(self, image): """检测棋盘格""" gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) ret, corners = cv2.findChessboardCorners(gray, (self.chess_width, self.chess_height), None) if ret: # 角点精检测 corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), self.criteria) # Draw and display the corners image = cv2.drawChessboardCorners(image, (self.chess_width, self.chess_height), corners2, ret) return image def capture2(self, left_video, right_video, save_dir): """ 用于采集双USB连接线的双目摄像头 :param left_video:int or str,左路视频路径或者摄像头ID :param right_video:int or str,右视频路径或者摄像头ID :param save_dir: str,保存左右图片的路径 :return: """ self.create_file(save_dir) capL = cv2.VideoCapture(left_video) capR = cv2.VideoCapture(right_video) widthL, heightL, numFramesL, fpsL = self.get_video_info(capL) widthR, heightR, numFramesR, fpsR = self.get_video_info(capR) print("capL:/n", widthL, heightL, numFramesL, fpsL) print("capR:/n", widthR, heightR, numFramesR, fpsR) save_videoL = self.create_file(save_dir, "video", "left_video.avi") save_videoR = self.create_file(save_dir, "video", "right_video.avi") writerL = self.get_video_writer(save_videoL, widthL, heightL, fpsL) writerR = self.get_video_writer(save_videoR, widthR, heightR, fpsR) i = 0 while True: isuccessL, frameL = capL.read() isuccessR, frameR = capR.read() if not (isuccessL and isuccessR): print("No more frames") break if self.detect: l = self.detect_chessboard(frameL.copy()) r = self.detect_chessboard(frameR.copy()) else: l = frameL.copy() r = frameR.copy() cv2.imshow("left", l) cv2.imshow("right", r) key = cv2.waitKey(10) if key == ord("q"): break elif key == ord("c") or key == ord("s"): print("save image:{:0=3d}".format(i)) cv2.imwrite(os.path.join(save_dir, "left_{:0=3d}.png".format(i)), frameL) cv2.imwrite(os.path.join(save_dir, "right_{:0=3d}.png".format(i)), frameR) i += 1 writerL.write(frameL) writerR.write(frameR) capL.release() capR.release() cv2.destroyAllWindows() def capture1(self, video, save_dir): """ 用于采集单USB连接线的双目摄像头(左右摄像头被拼接在同一个视频中显示) :param video:int or str,视频路径或者摄像头ID :param save_dir: str,保存左右图片的路径 """ self.create_file(save_dir) cap = cv2.VideoCapture(video) width, height, numFrames, fps = self.get_video_info(cap) print("capL:/n", width, height, numFrames, fps) save_videoL = self.create_file(save_dir, "video", "left_video.avi") save_videoR = self.create_file(save_dir, "video", "right_video.avi") writerL = self.get_video_writer(save_videoL, int(width / 2), height, fps) writerR = self.get_video_writer(save_videoR, int(width / 2), height, fps) i = 0 while True: isuccess, frame = cap.read() if not isuccess: print("No more frames") break # 分离左右摄像头 frameL = frame[:, :int(width / 2), :] frameR = frame[:, int(width / 2):, :] if self.detect: l = self.detect_chessboard(frameL.copy()) r = self.detect_chessboard(frameR.copy()) else: l = frameL.copy() r = frameR.copy() cv2.imshow("left", l) cv2.imshow("right", r) key = cv2.waitKey(10) if key == ord("q"): break elif key == ord("c") or key == ord("s"): print("save image:{:0=3d}".format(i)) cv2.imwrite(os.path.join(save_dir, "left_{:0=3d}.png".format(i)), frameL) cv2.imwrite(os.path.join(save_dir, "right_{:0=3d}.png".format(i)), frameR) i += 1 writerL.write(frameL) writerR.write(frameR) cap.release() cv2.destroyAllWindows() @staticmethod def get_video_info(video_cap): width = int(video_cap.get(cv2.CAP_PROP_FRAME_WIDTH)) height = int(video_cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) numFrames = int(video_cap.get(cv2.CAP_PROP_FRAME_COUNT)) fps = int(video_cap.get(cv2.CAP_PROP_FPS)) return width, height, numFrames, fps @staticmethod def get_video_writer(save_path, width, height, fps): if not os.path.exists(os.path.dirname(save_path)): os.makedirs(os.path.dirname(save_path)) fourcc = cv2.VideoWriter_fourcc(*"XVID") frameSize = (int(width), int(height)) video_writer = cv2.VideoWriter(save_path, fourcc, fps, frameSize) print("video:width:{},height:{},fps:{}".format(width, height, fps)) return video_writer @staticmethod def create_file(parent_dir, dir1=None, filename=None): out_path = parent_dir if dir1: out_path = os.path.join(parent_dir, dir1) if not os.path.exists(out_path): os.makedirs(out_path) if filename: out_path = os.path.join(out_path, filename) return out_pathdef str2bool(v): return v.lower() in ("yes", "true", "t", "y", "1")def get_parser(): width = 8 height = 11 left_video = -1 right_video = 0 save_dir = "data/camera" parser = argparse.ArgumentParser(description="Camera calibration") parser.add_argument("--width", type=int, default=width, help="chessboard width size") parser.add_argument("--height", type=int, default=height, help="chessboard height size") parser.add_argument("--left_video", type=int, default=left_video, help="left video file or camera ID") parser.add_argument("--right_video", type=int, default=right_video, help="right video file or camera ID") parser.add_argument("--detect", type=str2bool, nargs="?", const=True, help="detect chessboard ") parser.add_argument("--save_dir", type=str, default=save_dir, help="YML file to save calibrate matrices") return parserif __name__ == "__main__": args = get_parser().parse_args() stereo = StereoCamera(args.width, args.height, detect=args.detect) if args.left_video > -1 and args.right_video > -1: # 双USB连接线的双目摄像头 stereo.capture2(left_video=args.left_video, right_video=args.right_video, save_dir=args.save_dir) elif args.left_video > -1: # 单USB连接线的双目摄像头(左右摄像头被拼接在同一个视频中显示) stereo.capture1(video=args.left_video, save_dir=args.save_dir) elif args.right_video > -1: # 单USB连接线的双目摄像头(左右摄像头被拼接在同一个视频中显示) stereo.capture1(video=args.right_video, save_dir=args.save_dir) else: raise Exception("Error: Check your camera{}".format(args.left_video, args.right_video))双目标定的目标是获得左右两个相机的内参、外参和畸变系数，其中内参包括左右相机的fx，fy，cx，cy，外参包括左相机相对于右相机的旋转矩阵和平移向量，畸变系数包括径向畸变系数（k1， k2，k3）和切向畸变系数（p1，p2）。
双目标定工具最常用的莫过于是MATLAB的工具箱： Stereo Camera Calibrator App，网上有太多教程，我就不赘述了。
我采用的是Deepin系统，懒得去安装Matlab了，所以就参考各路神仙，使用OpenCV实现了单目和双目的标定程序。
bash scripts/mono_camera_calibration.sh#!/usr/bin/env bashimage_dir=data/lenacv-camera # 棋盘格图片save_dir=configs/lenacv-camera # 保存标定结果width=8height=11square_size=20 #mmimage_format=png # 图片格式，如png,jpgshow=True # 是否显示检测结果# left camera calibrationpython mono_camera_calibration.py / --image_dir $image_dir / --image_format $image_format / --square_size $square_size / --width $width / --height $height / --prefix left / --save_dir $save_dir / --show $show# right camera calibrationpython mono_camera_calibration.py / --image_dir $image_dir / --image_format $image_format / --square_size $square_size / --width $width / --height $height / --prefix right / --save_dir $save_dir / --show $show一点注意事项：
- 标定代码会显示每一张图像的棋盘格的角点检测效果，如果发现有检测不到，或者角点检测出错，则需要自己手动删除这些图像，避免引入太大的误差
- 若误差超过0.1，建议重新调整摄像头并标定，不然效果会差很多
执行后，在$save_dir目录下会生成left_cam.yml和right_cam.yml左右相机参数文件
%YAML:1.0---K: !!opencv-matrix rows: 3 cols: 3 dt: d data: [ 7.6327773983725410e+02, 0., 2.8768149780495781e+02, 0., 7.6350419482076416e+02, 2.1897333674659842e+02, 0., 0., 1. ]D: !!opencv-matrix rows: 1 cols: 5 dt: d data: [ 3.5020967324140520e-02, -4.0770563420764315e-02, -4.4231047037511739e-04, -1.0552565305999332e-03, -9.7750323762439514e-02 ]其中K是相机内参矩阵，D是畸变系数矩阵
这是完整的代码
bash scripts/stereo_camera_calibration.shimage_dir=data/lenacv-camera # 棋盘格图片save_dir=configs/lenacv-camera # 保存标定结果width=8height=11square_size=20 #mmimage_format=png # 图片格式，如png,jpg#show=True # 是否显示检测结果show=False # 是否显示检测结果# stereo camera calibrationpython stereo_camera_calibration.py / --left_file $save_dir/left_cam.yml / --right_file $save_dir/right_cam.yml / --left_prefix left / --right_prefix right / --width $width / --height $height / --left_dir $image_dir / --right_dir $image_dir / --image_format $image_format / --square_size $square_size / --save_dir $save_dir /一点注意事项：
- 若误差超过0.1，建议重新调整摄像头并标定
执行后，在$save_dir目录下会生成stereo_cam.yml相机参数文件，这个文件，包含了左右相机的相机内参矩阵(K1,K2)，畸变系数矩阵(D1,D2)，左右摄像机之间的旋转矩阵R和平移矩阵T，以及本质矩阵E和基本矩阵F等.
有了双目相机内外参数信息(stereo_cam.yml)，下面就可以进行立体矫正，计算视差了
这部分基础知识来源于：https://blog.csdn.net/dulingwen/article/details/100115157
立体校正的目的是将拍摄于同一场景的左右两个视图进行数学上的投影变换，使得两个成像平面平行于基线，且同一个点在左右两幅图中位于同一行，简称共面行对准。只有达到共面行对准以后才可以应用三角原理计算距离。
def get_rectify_image(self, imgL, imgR): """ 畸变校正和立体校正 根据更正map对图片进行重构 获取用于畸变校正和立体校正的映射矩阵以及用于计算像素空间坐标的重投影矩阵 :param imgL: :param imgR: :return: """ # camera_params.get_rectify_transform(K1, D1, K2, D2, R, T, image_size) left_map_x, left_map_y = self.camera_config["left_map_x"], self.camera_config["left_map_y"] right_map_x, right_map_y = self.camera_config["right_map_x"], self.camera_config["right_map_y"] rectifiedL = cv2.remap(imgL, left_map_x, left_map_y, cv2.INTER_LINEAR, borderValue=cv2.BORDER_CONSTANT) rectifiedR = cv2.remap(imgR, right_map_x, right_map_y, cv2.INTER_LINEAR, borderValue=cv2.BORDER_CONSTANT) return rectifiedL, rectifiedR立体匹配的目的是为左图中的每一个像素点在右图中找到其对应点（世界中相同的物理点），这样就可以计算出视差：（xi和xj分别表示两个对应点在图像中的列坐标）。
大部分立体匹配算法的计算过程可以分成以下几个阶段：匹配代价计算、代价聚合、视差优化、视差细化。立体匹配是立体视觉中一个很难的部分，主要困难在于：
1.图像中可能存在重复纹理和弱纹理，这些区域很难匹配正确；
2.由于左右相机的拍摄位置不同，图像中几乎必然存在遮挡区域，在遮挡区域，左图中有一些像素点在右图中并没有对应的点，反之亦然；
3.左右相机所接收的光照情况不同；
4.过度曝光区域难以匹配；
5.倾斜表面、弯曲表面、非朗伯体表面；
6.较高的图像噪声等。
常用的立体匹配方法基本上可以分为两类：局部方法，例如BM、SGM、ELAS、Patch Match等，非局部的，即全局方法，例如Dynamic Programming、Graph Cut、Belief Propagation等，局部方法计算量小，匹配质量相对较低，全局方法省略了代价聚合而采用了优化能量函数的方法，匹配质量较高，但是计算量也比较大。
目前OpenCV中已经实现的方法有BM、binaryBM、SGBM、binarySGBM、BM(cuda)、Bellief Propogation(cuda)、Constant Space Bellief Propogation(cuda)这几种方法。比较好用的是SGBM算法，它的核心是基于SGM算法，但和SGM算法又有一些不同，比如匹配代价部分用的是BT代价（原图+梯度图）而不是HMI代价等等。有关SGM算法的原理解释，可以参考另一篇博客 : 《双目立体匹配算法：SGM 》https://blog.csdn.net/dulingwen/article/details/104142149
def get_disparity(self, imgL, imgR, use_wls=True): """ :param imgL: 畸变校正和立体校正后的左视图 :param imgR：畸变校正和立体校正后的右视图 :param use_wls：是否使用WLS滤波器对视差图进行滤波 :return dispL:ndarray(np.float32),返回视差图 """ dispL = disparity.get_disparity_filter(imgL, imgR, use_wls=use_wls) # dispL = disparity.get_disparity_simple(imgL, imgR) return dispL| 参数 | 类型 | 说明 |
|---|---|---|
| calibration_file | str | 双目相机的配置文件，如"configs/lenacv-camera/stereo_cam.yml" |
| left_video | str | 左路相机ID或者视频文件 |
| right_video | str | 右路相机ID或者视频文件 |
| left_file | str | 左路测试图像文件 |
| right_file | str | 右路测试图像文件 |
| filter | bool | 是否对视差图进行滤波 |
python demo.py / --stereo_file "configs/lenacv-camera/stereo_cam.yml" / --left_video "data/lenacv-video/left_video.avi" / --right_video "data/lenacv-video/right_video.avi" / --filter True在立体匹配生成视差图之后，还可以对视差图进行滤波后处理，例如Guided Filter、Fast Global Smooth Filter（一种快速WLS滤波方法）、Bilatera Filter、TDSR、RBS等。 视差图滤波能够将稀疏视差转变为稠密视差，并在一定程度上降低视差图噪声，改善视差图的视觉效果，但是比较依赖初始视差图的质量。
| 左视图 | 右视图 |
|---|---|
| 视差图(未滤波) | 深度图(未滤波) |
| 视差图(滤波后) | 深度图(滤波后) |
可以看到，使用WLS滤波后，视差图的整体效果都有明显改善
得到了视差图之后，就可以计算像素深度了,在opencv中使用StereoRectify()函数可以得到一个重投影矩阵Q，它是一个4*4的视差图到深度图的映射矩阵(disparity-to-depth mapping matrix )，使用Q矩阵和cv2.reprojectImageTo3D即可实现将像素坐标转换为三维坐标，该函数会返回一个3通道的矩阵，分别存储X、Y、Z坐标（左摄像机坐标系下）。
def reprojectImageTo3D(disparity, Q, _3dImage=None, handleMissingValues=None, ddepth=None): """ :param disparity: 输入视差图 :param Q: 输入4*4的视差图到深度图的映射矩阵，即重投影矩阵 通过stereoRectify得到 (disparity-to-depth mapping matrix) :param _3dImage: 映射后存储三维坐标的图像 contains 3D coordinates of the point (x,y) computed from the disparity map :param handleMissingValues: 计算得到的非正常值是否给值，如果为true则给值10000 :param ddepth: 输出类型 -1 即默认为CV_32FC3 还可以是 CV_16S, CV_32S, CV_32F :return: """运算如下：
重投影矩阵Q中和为左相机主点在图像中的坐标，f为焦距，为两台相机投影中心间的平移（负值），即基线baseline,相当于平移向量T[0], 是右相机主点在图像中的坐标。
其中Z即是深度距离depth：
其中 f 为焦距长度（像素焦距），b为基线长度，d为视差，与为两个相机主点的列坐标。
这里有个地方需要注意，如果获得视差图像是CV_16S类型的，这样的视差图的每个像素值由一个16bit表示，其中低位的4位存储的是视差值得小数部分，所以真实视差值应该是该值除以16。在进行映射后应该乘以16，以获得毫米级真实位置。
def get_depth(self, disparity, Q, scale=1.0, method=False): """ reprojectImageTo3D(disparity, Q),输入的Q,单位必须是毫米(mm) :param disparity: 视差图 :param Q: 重投影矩阵Q=[[1, 0, 0, -cx] [0, 1, 0, -cy] [0, 0, 0, f] [1, 0, -1/Tx, (cx-cx`)/Tx]] 其中f为焦距，Tx相当于平移向量T的第一个参数 :param scale: 单位变换尺度,默认scale=1.0,单位为毫米 :return depth:ndarray(np.uint16),depth返回深度图, 即距离 """ # 将图片扩展至3d空间中，其z方向的值则为当前的距离 if method: points_3d = cv2.reprojectImageTo3D(disparity, Q) # 单位是毫米(mm) x, y, depth = cv2.split(points_3d) else: # baseline = abs(camera_config["T"][0]) baseline = 1 / Q[3, 2] # 基线也可以由T[0]计算 fx = abs(Q[2, 3]) depth = (fx * baseline) / disparity depth = depth * scale # depth = np.asarray(depth, dtype=np.uint16) depth = np.asarray(depth, dtype=np.float32) return depthdemo.py后，鼠标点击图像任意区域，终端会打印对应距离(x,y)=(203,273),depth=633.881653mm(x,y)=(197,329),depth=640.386047mm(x,y)=(222,292),depth=631.549072mm(x,y)=(237,270),depth=630.389221mm(x,y)=(208,246),depth=652.560669mm双目测距的精度 说明：
根据上式可以看出，某点像素的深度精度取决于该点处估计的视差d的精度。假设视差d的误差恒定，当测量距离越远，得到的深度精度则越差，因此使用双目相机不适宜测量太远的目标。
如果想要对与较远的目标能够得到较为可靠的深度，一方面需要提高相机的基线距离，但是基线距离越大，左右视图的重叠区域就会变小，内容差异变大，从而提高立体匹配的难度，另一方面可以选择更大焦距的相机，然而焦距越大，相机的视域则越小，导致离相机较近的物体的距离难以估计。
恢复三维坐标后，就可以使用python-pcl和Open3D库显示点云图
PCL Python版比较难安装，如果安装不了，那可以采用Open3D勉强凑合使用吧
如下图所示，你可以用鼠标旋转坐标轴，放大点云
| 2D-RGB | Open3D点云显示 | PCL点云显示 |
def show_3dcloud_for_open3d(self, frameL, frameR, points_3d): """ 使用open3d显示点云 :param frameL: :param frameR: :param points_3d: :return: """ if self.use_open3d: x, y, depth = cv2.split(points_3d) # depth = points_3d[:, :, 2] self.open3d_viewer.show(color_image=frameL, depth_image=depth) def show_3dcloud_for_pcl(self, frameL, frameR, points_3d): """ 使用PCL显示点云 :param frameL: :param frameR: :param points_3d: :return: """ if self.use_pcl: self.pcl_viewer.add_3dpoints(points_3d, frameL) self.pcl_viewer.show()【完整的项目代码】：https://download.csdn.net/download/guyuealian/42517006
# -*-coding: utf-8 -*-""" @Author : panjq @E-mail : pan_jinquan@163.com @Date : 2020-04-10 18:24:06"""import osimport cv2import argparseimport numpy as npfrom core.utils import image_utils, file_utilsfrom core import camera_params, stereo_matcherclass StereoDepth(object): """双目测距""" def __init__(self, stereo_file, width=640, height=480, filter=True, use_open3d=True, use_pcl=True): """ :param stereo_file: 双目相机内外参数配置文件 :param width: 相机分辨率width :param height:相机分辨率height :param filter: 是否使用WLS滤波器对视差图进行滤波 :param use_open3d: 是否使用open3d显示点云 :param use_pcl: 是否使用PCL显示点云 """ self.count = 0 self.filter = filter self.camera_config = camera_params.get_stereo_coefficients(stereo_file) self.use_pcl = use_pcl self.use_open3d = use_open3d # 初始化3D点云 if self.use_pcl: # 使用open3d显示点云 from core.utils_pcl import pcl_tools self.pcl_viewer = pcl_tools.PCLCloudViewer() if self.use_open3d: # 使用PCL显示点云 from core.utils_3d import open3d_visual self.open3d_viewer = open3d_visual.Open3DVisual(camera_intrinsic=self.camera_config["K1"], depth_width=width, depth_height=height) self.open3d_viewer.show_image_pcd(True) self.open3d_viewer.show_origin_pcd(True) self.open3d_viewer.show_image_pcd(True) assert (width, height) == self.camera_config["size"], Exception("Error:{}".format(self.camera_config["size"])) def test_pair_image_file(self, left_file, right_file): """ 测试一对左右图像 :param left_file: 左路图像文件 :param right_file: 右路图像文件 :return: """ frameR = cv2.imread(left_file) frameL = cv2.imread(right_file) self.task(frameR, frameL, waitKey=0) def capture1(self, video): """ 用于采集单USB连接线的双目摄像头(左右摄像头被拼接在同一个视频中显示) :param video:int or str,视频路径或者摄像头ID :param save_dir: str,保存左右图片的路径 """ cap = image_utils.get_video_capture(video) width, height, numFrames, fps = image_utils.get_video_info(cap) self.count = 0 while True: success, frame = cap.read() if not success: print("No more frames") break frameL = frame[:, :int(width / 2), :] frameR = frame[:, int(width / 2):, :] self.count += 1 self.task(frameL, frameR, waitKey=5) if cv2.waitKey(1) & 0xFF == ord("q"): # Get key to stop stream. Press q for exit break cap.release() cv2.destroyAllWindows() def capture2(self, left_video, right_video): """ 用于采集双USB连接线的双目摄像头 :param left_video:int or str,左路视频路径或者摄像头ID :param right_video:int or str,右视频路径或者摄像头ID :return: """ capL = image_utils.get_video_capture(left_video) capR = image_utils.get_video_capture(right_video) width, height, numFrames, fps = image_utils.get_video_info(capL) width, height, numFrames, fps = image_utils.get_video_info(capR) self.count = 0 while True: successL, frameL = capL.read() successR, frameR = capR.read() if not (successL and successR): print("No more frames") break self.count += 1 self.task(frameL, frameR, waitKey=50) if cv2.waitKey(1) & 0xFF == ord("q"): # Get key to stop stream. Press q for exit break capL.release() capR.release() cv2.destroyAllWindows() def get_3dpoints(self, disparity, Q, scale=1.0): """ 计算像素点的3D坐标（左相机坐标系下） reprojectImageTo3D(disparity, Q),输入的Q,单位必须是毫米(mm) :param disparity: 视差图 :param Q: 重投影矩阵Q=[[1, 0, 0, -cx] [0, 1, 0, -cy] [0, 0, 0, f] [1, 0, -1/Tx, (cx-cx`)/Tx]] 其中f为焦距，Tx相当于平移向量T的第一个参数 :param scale: 单位变换尺度,默认scale=1.0,单位为毫米 :return points_3d:ndarray(np.float32),返回三维坐标points_3d，三个通道分布表示(X,Y,Z) 其中Z是深度图depth, 即距离,单位是毫米(mm) """ # 返回三维坐标points_3d，三个通道分布表示(X,Y,Z) # depth = stereo_matcher.get_depth(disparity, Q, scale=1.0) points_3d = cv2.reprojectImageTo3D(disparity, Q) # x, y, depth = cv2.split(points_3d) # baseline = abs(camera_config["T"][0]) # baseline = 1 / Q[3, 2] # 基线也可以由T[0]计算 # fx = abs(Q[2, 3]) # depth = (fx * baseline) / disparity points_3d = points_3d * scale points_3d = np.asarray(points_3d, dtype=np.float32) return points_3d def get_disparity(self, imgL, imgR, use_wls=True): """ :param imgL: 畸变校正和立体校正后的左视图 :param imgR：畸变校正和立体校正后的右视图 :param use_wls：是否使用WLS滤波器对视差图进行滤波 :return dispL:ndarray(np.float32),返回视差图 """ dispL = stereo_matcher.get_filter_disparity(imgL, imgR, use_wls=use_wls) # dispL = disparity.get_simple_disparity(imgL, imgR) return dispL def get_rectify_image(self, imgL, imgR): """ 畸变校正和立体校正 根据更正map对图片进行重构 获取用于畸变校正和立体校正的映射矩阵以及用于计算像素空间坐标的重投影矩阵 :param imgL: :param imgR: :return: """ # camera_params.get_rectify_transform(K1, D1, K2, D2, R, T, image_size) left_map_x, left_map_y = self.camera_config["left_map_x"], self.camera_config["left_map_y"] right_map_x, right_map_y = self.camera_config["right_map_x"], self.camera_config["right_map_y"] rectifiedL = cv2.remap(imgL, left_map_x, left_map_y, cv2.INTER_LINEAR, borderValue=cv2.BORDER_CONSTANT) rectifiedR = cv2.remap(imgR, right_map_x, right_map_y, cv2.INTER_LINEAR, borderValue=cv2.BORDER_CONSTANT) return rectifiedL, rectifiedR def task(self, frameL, frameR, waitKey=5): """ :param frameL: 左路视频帧图像(BGR) :param frameR: 右路视频帧图像(BGR) """ # 畸变校正和立体校正 rectifiedL, rectifiedR = self.get_rectify_image(imgL=frameL, imgR=frameR) # 绘制等间距平行线，检查立体校正的效果 # calibrate_tools.draw_line_rectify_image(rectifiedL, rectifiedR) # We need grayscale for disparity map. grayL = cv2.cvtColor(rectifiedL, cv2.COLOR_BGR2GRAY) grayR = cv2.cvtColor(rectifiedR, cv2.COLOR_BGR2GRAY) # Get the disparity map dispL = self.get_disparity(grayL, grayR, self.filter) points_3d = self.get_3dpoints(disparity=dispL, Q=self.camera_config["Q"]) self.show_3dcloud_for_open3d(frameL, frameR, points_3d) self.show_3dcloud_for_pcl(frameL, frameR, points_3d) self.show_2dimage(frameL, frameR, points_3d, dispL, waitKey=waitKey) def show_3dcloud_for_open3d(self, frameL, frameR, points_3d): """ 使用open3d显示点云 :param frameL: :param frameR: :param points_3d: :return: """ if self.use_open3d: x, y, depth = cv2.split(points_3d) # depth = points_3d[:, :, 2] self.open3d_viewer.show(color_image=frameL, depth_image=depth) def show_3dcloud_for_pcl(self, frameL, frameR, points_3d): """ 使用PCL显示点云 :param frameL: :param frameR: :param points_3d: :return: """ if self.use_pcl: self.pcl_viewer.add_3dpoints(points_3d, frameL) self.pcl_viewer.show() def show_2dimage(self, frameL, frameR, points_3d, dispL, waitKey=0): """ :param frameL: :param frameR: :param dispL: :param points_3d: :return: """ x, y, depth = cv2.split(points_3d) # depth = points_3d[:, :, 2] depth_colormap = stereo_matcher.get_visual_depth(depth) dispL_colormap = stereo_matcher.get_visual_disparity(dispL) image_utils.addMouseCallback("left", depth, info="depth=%fmm") image_utils.addMouseCallback("right", depth, info="depth=%fmm") image_utils.addMouseCallback("disparity-color", depth, info="depth=%fmm") image_utils.addMouseCallback("depth-color", depth, info="depth=%fmm") result = {"frameL": frameL, "frameR": frameR, "disparity": dispL_colormap, "depth": depth_colormap} cv2.imshow("left", frameL) cv2.imshow("right", frameR) cv2.imshow("disparity-color", dispL_colormap) cv2.imshow("depth-color", depth_colormap) key = cv2.waitKey(waitKey) self.save_images(result, self.count, key) if self.count <= 2: cv2.moveWindow("left", 700, 0) cv2.moveWindow("right", 1400, 0) cv2.moveWindow("disparity-color", 700, 700) cv2.moveWindow("depth-color", 1400, 700) def save_images(self, result, count, key, save_dir="./data/temp"): """ :param result: :param count: :param key: :param save_dir: :return: """ if key == ord("q"): exit(0) elif key == ord("c") or key == ord("s"): file_utils.create_dir(save_dir) print("save image:{:0=4d}".format(count)) cv2.imwrite(os.path.join(save_dir, "left_{:0=4d}.png".format(count)), result["frameL"]) cv2.imwrite(os.path.join(save_dir, "right_{:0=4d}.png".format(count)), result["frameR"]) cv2.imwrite(os.path.join(save_dir, "disparity_{:0=4d}.png".format(count)), result["disparity"]) cv2.imwrite(os.path.join(save_dir, "depth_{:0=4d}.png".format(count)), result["depth"])def str2bool(v): return v.lower() in ("yes", "true", "t", "y", "1")def get_parser(): stereo_file = "configs/lenacv-camera/stereo_cam.yml" # left_video = None # right_video = None left_video = "data/lenacv-video/left_video.avi" right_video = "data/lenacv-video/right_video.avi" left_file = "docs/left.png" right_file = "docs/right.png" parser = argparse.ArgumentParser(description="Camera calibration") parser.add_argument("--stereo_file", type=str, default=stereo_file, help="stereo calibration file") parser.add_argument("--left_video", default=left_video, help="left video file or camera ID") parser.add_argument("--right_video", default=right_video, help="right video file or camera ID") parser.add_argument("--left_file", type=str, default=left_file, help="left image file") parser.add_argument("--right_file", type=str, default=right_file, help="right image file") parser.add_argument("--filter", type=str2bool, nargs="?", default=True, help="use disparity filter") return parserif __name__ == "__main__": args = get_parser().parse_args() stereo = StereoDepth(args.stereo_file, filter=args.filter) if args.left_video is not None and args.right_video is not None: # 双USB连接线的双目摄像头 stereo.capture2(left_video=args.left_video, right_video=args.right_video) elif args.left_video is not None: # 单USB连接线的双目摄像头(左右摄像头被拼接在同一个视频中显示) stereo.capture1(video=args.left_video) elif args.right_video is not None: # 单USB连接线的双目摄像头(左右摄像头被拼接在同一个视频中显示) stereo.capture1(video=args.right_video) if args.left_file and args.right_file: # 测试一对左右图像 stereo.test_pair_image_file(args.left_file, args.right_file)文章版权归作者所有，未经允许请勿转载,若此文章存在违规行为，您可以联系管理员删除。
转载请注明本文地址：https://www.ucloud.cn/yun/123597.html
摘要：现在这种天然的特征描述机制，给图像预处理提供了不错的工具，它能将图像处理和视觉预处理合二为一。前面运动估计已经用深度学习求解了，现在这两个问题自然也是。和上个问题类似，采用深度图学习做合成图像，也属于空间。 ...
摘要：一简介最近在研究异构传感器定位方面的东西，读了一些论文。这个方向的一个基本逻辑就是，先用激光雷达等高精度设备建立一个高精度的点云地图，然后再用视觉里程计结合异构传感器定位的一些方法，实现一个累计误差可控的视觉里程计。 ...
摘要：步骤标定得出焦距对图片进行高斯滤波处理平滑操作，过滤操作，去噪操作边缘检测画出检测出的轮廓并返回最小的外接矩形可以设置画出全部轮廓或者是最大的轮廓计算距离检测摄像头只需要把摄像头获取到的画面逐帧分析，就相当于对照片分析，视频也是一个道理。 闲来无事，用C++做了一个简易的单目测距。算法用的c...
摘要：双活体，依然是前最可靠的防攻击段之。详解云识客活体检测技术以下，我们分析一种多重人脸区域共享的深度学习算法。光流法辅助单目活体判断最后，针对单目活体，云识客也采用光流法辅助活体判断的校验机制。 以下这张照⽚，是真⼈实拍还是对着照⽚翻拍的？ showImg(https://segmentfault.com/img/bVbuoHD); 如果告诉你，这张照⽚，是对着照⽚翻拍的照⽚，你会不会惊...
摘要：代表第一章下的代表各章下的小细节内容。描述了李群的局部性质，是单位元附近的正交空间。否则，一个群可能存在多个李代数注意点旋转矩阵的导数，可以由旋转向量指定。误差项是将点的投影位置与观测位置做差，所以称为重投影误差。 1）代表第一章 1）下的1.2代表各章下的小细节内容。层次按1. ——1.1...
阅读 2847·2023-04-25 18:54
阅读 2985·2021-11-17 09:33
阅读 2443·2021-11-02 14:40
阅读 2954·2021-09-23 11:58
阅读 2280·2019-08-30 13:50
阅读 1040·2019-08-29 12:46
阅读 2954·2019-08-28 17:51
阅读 549·2019-08-26 11:47
