ps4/eyes----ROS开发

**

1.相机准备：*

• 市面上的双目摄像头都很贵，所以去限于上淘了一款便宜的摄像头PS4的第一第二代摄像头。
  成本是两百块左右，但是使用效果不错，所以写下博客记录开发过程防止遗忘。国外几个打捞将开发方案开源，我自己也参考的他们的方案，学到一些东西，想着在此基础上从零开始开发双目的视觉slam。
  您必须安装v4l2-utils，

sudo apt-get install libv4l-dev

然后使用：

v4l2-ctl -d /dev/video2 --all`

检查所有可能的控件

v4l2-ctl -d /dev/video2 --set- ctrl auto_exposure = 1

停用自动曝光

v4l2-ctl -d /dev/video2 --set-ctrl gain_automatic = 0

停用自动增益…

v4l2-ctl -d /dev/video2 --set-ctrl曝光= 100
设定固定曝光！

2.相机驱动开发：

$ lsusb	

Bus 006 Device 007: ID 05a9:0580 OmniVision Technologies, Inc. PlayStation(R)Camera adaptor
Bus 006 Device 006: ID 054c:0d0b Sony Corp. PlayStation(R)Camera adaptor
Bus 006 Device 001: ID 1d6b:0003 Linux Foundation 3.0 root hub
Bus 005 Device 005: ID 054c:0d0a **Sony** Corp. PlayStation(R)Camera adaptor
Bus 005 Device 002: ID 8087:0029 Intel Corp. 
Bus 005 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub
Bus 004 Device 001: ID 1d6b:0003 Linux Foundation 3.0 root hub
Bus 003 Device 003: ID 13d3:56a2 IMC Networks USB2.0 HD UVC WebCam
Bus 003 Device 002: ID 1bcf:0005 Sunplus Innovation Technology Inc. Optical Mouse
Bus 003 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub
Bus 002 Device 001: ID 1d6b:0003 Linux Foundation 3.0 root hub
Bus 001 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub

确保有sony的标识设备。

Install Pyusb 1.0

=> $ git clone https://github.com/walac/pyusb.git
=> $ cd pyusb/
=> $ sudo python setup.py install

先贴上网址：
https://haochihros.wordpress.com/category/ps4eye-x-ros/
https://ps4eye.tumblr.com/
https://github.com/longjie/ps4eye
https://blog.csdn.net/w_tom/article/details/76759369

过程中我用的IDE为最新的clion：ubuntu16版本以上直接用命令就可以下载（自行百度安装）：
clion中python的配置如下：
https://blog.csdn.net/arris1992/article/details/103972110
camera_init.py：

#!/usr/bin/env python
import usb.core
import usb.util
import os
import sys

# check if initialized device already exists
dev = usb.core.find(idVendor=0x05a9, idProduct=0x058a)
if dev is not None:
    print('PS4 camera already initialized')
    sys.exit()

# find uninitialized device
dev = usb.core.find(idVendor=0x05a9, idProduct=0x0580)
if dev is None:
    print('PS4 camera not found')
    sys.exit()

# set the active configuration. With no arguments, the first
# configuration will be the active one
dev.set_configuration()

# helper function for chunking a file
def read_chunks(infile, chunk_size):
    while True:
        chunk = infile.read(chunk_size)
        if chunk:
            yield chunk
        else:
            return

chunk_size=512
index=0x14
value=0
script_dir=os.path.abspath(os.path.dirname(__file__))
firmware=open(script_dir + "/firmware.bin","rb")

# transfer 512b chunks of the firmware
for chunk in read_chunks(firmware, chunk_size):
    ret = dev.ctrl_transfer(0x40, 0x0, value, index, chunk)
    value+=chunk_size
    if value>=65536:
        value=0
        index+=1
    if len(chunk)!=ret:
        print("sent %d/%d bytes" % (ret,len(chunk)))

# command reboots device with new firmware and product id
try:
    ret = dev.ctrl_transfer(0x40, 0x0, 0x2200, 0x8018, [0x5b])
except:
    print('PS4 camera firmware uploaded and device reset')

运行：sudo python camera_init.py
获取图像：camera_driver.py

import numpy as np
import cv2

import time

#cap = cv2.VideoCapture('http://root:root@192.168.70.52/mjpg/1/video.mjpg')
cap = cv2.VideoCapture(2)

cap.set(cv2.CAP_PROP_FRAME_WIDTH, 3448)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 808)

def decode(frame):
    left = np.zeros((800,1264,3), np.uint8)
    right = np.zeros((800,1264,3), np.uint8)

    for i in range(800):
        right[i] = frame[i, 64: 1280 + 48]
        left[i] = frame[i, 1280 + 48: 1280 + 48 + 1264]

    return (left, right)

while(True):

    ret, frame = cap.read()
    #cv2.normalize(frame, frame, 0, 255, cv2.NORM_MINMAX)

    left, right = decode(frame)

    cv2.imshow('left',left)
    cv2.imshow('right',right)


    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

cap.release()
cv2.destroyAllWindows()

运行：python camera_driver.py
只要摄像头正常，python有opencv模块，这两步很容易通过获得双目的高清图像。

3.相机参数标定

先贴连接：https://blog.csdn.net/betrapped/article/details/89247561
原理

首先先简单的了解一下相机标定的原理。
摄像机标定(Camera calibration)简单来说是从世界坐标系换到图像坐标系的过程，也就是求最终的投影矩阵 P 的过程。

一般来说，标定的过程分为两个部分：

第一步是从世界坐标系转换为相机坐标系，这一步是三维点到三维点的转换，包括 R，t （相机外参）等参数；
第二部是从相机坐标系转为图像坐标系，这一步是三维点到二维点的转换，包括 K（相机内参）等参数；

世界坐标系转换为相机坐标系

公式表达如下：


相机坐标系转换为图像坐标系
其中：

C 点表示camera centre，即相机的中心点，也是相机坐标系的中心点；

   Z 轴表示principal axis，即相机的主轴；

   p 点所在的平面表示image plane，即相机的像平面，也就是图片坐标系所在的二维平面；

   p 点表示principal point，即主点，主轴与像平面相交的点；

   C 点到 p 点的距离，也就是右边图中的 f表示focal length，即相机的焦距；

   像平面上的 x 和 y 坐标轴是与相机坐标系上的 X 和 Y 坐标轴互相平行的；

   相机坐标系是以 X ， Y， Z三个轴组成的且原点在 C 点，度量值为米（m）；

   像平面坐标系是以 x ， y （小写）两个轴组成的且原点在 p 点，度量值为米（m）；

   图像坐标系一般指图片相对坐标系，在这里可以认为和像平面坐标系在一个平面上，不过原点是在图片的角上，而且度量值为像素的个数（pixel）；

通过对上面坐标系的介绍，可以得到以下的转换公式：
加上偏移量：
如上图所示，其中主点 p p p 是像平面坐标系的原点，但在图像坐标系中的位置为 ( p x , p y ) (p_{x},p_{y}) (px​,py​)，在这里，图形坐标系的原点是图片的左下角，所以可以得到：
把这个换成矩阵计算：
整理一下：最后，可以得到矩阵K，就是相机内参：投影矩阵P（在这里可以认为旋转矩阵 R 为单位矩阵 I，平移矩阵 t 都为0）：最终的转换式：
从上面两个转换的过程，我们可以得到从世界坐标轴转换到图像的过程可以把投影矩阵P表示为：
在这里， K K K 一般称为相机内参（intrinsic parameters），描述了相机的内部参数，包括焦距 f f f、主点 p p p 的位置、以及像素与真实环境的大小比例等，这个是固有属性，是提供好的； R R R 和 t t t 称为相机外参（extrinsic parameters）， R R R 在这里是旋转矩阵，可以转换为三维的旋转向量，分别表示绕 x x x， y y y， z z z 三个轴的旋转角度， t t t 目前就是一个平移向量，分别表示在 x x x， y y y， z z z 三个方向上的平移量。
畸变参数

在几何光学和阴极射线管(CRT)显示中，畸变（distortion） 是对直线投影（rectilinear projection）的一种偏移。简单来说直线投影是场景内的一条直线投影到图片上也保持为一条直线。那畸变简单来说就是一条直线投影到图片上不能保持为一条直线了，这是一种光学畸变（optical aberration）。
畸变一般可以分为两大类，包括径向畸变和切向畸变。主要的一般径向畸变有时也会有轻微的切向畸变。
相机标定的结果

了解完一定原理之后，开始实践一下。
首先，要明确通过相机标记我们需要得到以下的参数：

摄像机的内参和外参矩阵，内参和外参系数可以对之后相机拍摄的图像就进行矫正，得到畸变相对很小的图像。
每一幅标定图像的选择和平移矩阵

流程

相机标定的流程可以简单的表述为：

准备图片。这里要注意，拍摄不同角度的图片时只需要对焦一次就好了，如果反复对焦，得到的每张图片的焦距就会一直变化。
对每张图片提取角点信息
对每张图片，进一步提取亚像素角点信息
相机标定
畸变矫正

实验数据准备

主要准备图片即可，10张左右的标定图片，黑白棋盘格即可。
可以给图片进行一定的缩小，加快运算速度。
拍摄过程中，如果方格纸是横着摆的就一直横着摆着拍，因为横纵角点个数是要提前设置好的。