相机模型Omnidirectional Camera(全方位摄像机)

1. 背景

大多数商用相机都可以描述为针孔相机，通过透视投影进行建模。然而，有些投影系统的几何结构无法使用传统针孔模型来描述，因为成像设备引入了非常高的失真。其中一些系统就是全方位摄像机。
有几种方法可以制作全向相机。屈光照相机(Dioptric cameras)使用成形透镜的组合（例如鱼眼透镜；见图1a），并且可以达到甚至大于180度的视野（即略大于半球）。折反射相机(Catadioptric cameras)将标准相机与成型镜（如抛物面镜、双曲面镜或椭圆镜）相结合，在水平面上提供360度的视野，在仰角(elevation)上提供100度以上的视野。在图1b中，您可以看到使用双曲镜的折反射相机示例。最后，多折射相机使用多个具有重叠视场的相机，迄今为止是唯一提供真正全向（球形）视场（即4π立体视场）的相机。
折反射相机最早于1990年由Yagi和Kawato引入机器人学，他们使用折反射相机定位机器人。直到2000年，由于新的制造技术和精密工具，鱼眼相机才开始普及，使其视野增加到180度甚至更多。然而，直到2005年，这些相机才被小型化到1-2厘米的大小，并且它们的视野增加到190度甚至更大）。

2. 模型原理

Ocam模型成像参考D Scaramuzza，可分为两个过程，先将相机坐标系下的坐标 $P=(x, y, z)^T$ 转换成ocam坐标系下的角度 $\Theta = (\theta, \alpha)^T$ ，再根据θ计算畸变系数，进而计算像素坐标 $q= (u, v)^T$ ，需要注意Ocam相机是左手系，存在一个相机坐标系到Ocam坐标系的坐标变换，其θ角度与Fisheye不同。

2.1. Central omnidirectional cameras

当被观察物体的光线在3D中的一个点（称为投影中心或单有效视点）相交时，视觉系统被称为中心（图2）。此属性称为单一有效视点属性。透视相机(perspective camera)是中央投影系统的一个示例，因为所有光线(optical rays)在一个点（即相机光学中心）相交。
所有现代鱼眼相机都是中心的，因此，它们满足单一有效视点特性。相反，中央折反射相机只能通过适当选择镜子形状和相机与镜子之间的距离来构建。正如Baker和Nayar[6]所证明的，满足单视点特性的镜族是一类旋转（扫描）圆锥截面，即双曲、抛物线和椭圆镜。对于双曲线镜和椭圆镜，通过确保相机中心（即针孔或透镜中心）与双曲线（椭圆）的一个焦点重合来实现单视点特性（图3）。对于抛物面反射镜，必须在相机和反射镜之间插入正交透镜，这使得抛物面反射镜反射的平行光线可能会聚到相机中心（图3）。
单一有效视点之所以如此理想，是因为它允许用户从全向相机拍摄的图片中生成几何正确的透视图像（图4）。这是可能的，因为在单视点约束下，感知图像中的每一个像素测量是通过视点的光在特定方向上的辐照度。当全向相机的几何形状已知时，即相机校准时，可以为每个像素预先计算出该方向。因此，可以将每个像素测量的辐照度值映射到视点任意距离的平面上，形成平面透视图像。此外，可以将图像映射到以单个视点为中心的球体上，即球形投影(图4，底部)。单视点属性如此重要的另一个原因是它允许用户应用著名的视极几何理论，这对从运动到结构非常重要。极向几何适用于任何中央相机，无论是透视还是全向。

2.2. Omnidirectional camera model and calibration

直观地说，全向相机的模型比标准透视相机稍微复杂一点。该模型确实应该考虑折反射相机的镜面反射，或者鱼眼相机的镜头折射。由于这一领域的文献相当多，本章回顾了两种不同的预测
这些模型已经成为全方位视觉和机器人技术的标准。此外,
已经为这两个模型开发了Matlab工具箱，全世界的专家和非专家都在使用它们。
第一种模型称为中央折反射相机的统一投影模型。它是由Geyer和Daniilidis(后来由Barreto和Araujo[8]改进)在2000年开发的，他们的优点是提出了一个包括所有三种类型的中央折反射相机的模型，即使用双曲镜、抛物面镜或椭圆镜的相机。这个模型是专门为中央折反射相机开发的，对鱼眼相机无效。用折反射透镜近似鱼眼透镜模型通常是可能的-然而，只有有限的精度-在[9]中进行了研究。
相反，第二种模型将中心折反射相机和鱼眼相机统一在一个通用模型下，也称为泰勒模型。它由Scaramuzza等人在2006年开发[10,11]，其优点是折反射相机和屈光相机都可以用同一个模型来描述，即一个泰勒多项式。

2.3. Unified model for central catadioptric cameras(中央折反射相机的统一模型)

Geyer和Daniilidis在2000年发表的具有里程碑意义的论文中指出，每一种折反射(抛物线、双曲线、椭圆线)和标准透视投影都等价于从一个以单一视点为中心的球体到一个以垂直于平面且距离较远的平面为投影中心的平面的投影映射。从球体的中心。图5总结了这一点。
本节的目标是找到场景点的观看方向和其对应图像点的像素坐标之间的关系。Geyer和Daniilidis的投影模型遵循四个步骤。设P = (x, y, z)为以C为中心的镜像参照系中的场景点(图5)。为了方便起见，我们假设镜子的对称轴与相机的光轴完全对齐。我们还假设相机和镜子的x轴和y轴是对齐的。因此，相机和镜像参考系的不同只是沿z轴的平移。

第一步是将场景点投射到单位球体上;因此:

然后将点坐标转换为以c为中心的新参照系= (0,0，…);因此:

范围在0(平面镜)和1(抛物面镜)之间。的正确值。可以通过知道圆锥曲线的焦点和直侧肌之间的距离d得到，如表1所示。圆锥剖面的直腹侧是通过平行于圆锥剖面准线的焦点的弦。

然后将p投影到距离c1的归一化图像平面上;因此,

最后，通过本征参数矩阵K将该点m映射到摄像机图像点p= (u, v, 1);因此,

很容易证明函数g.1是双射的，它的逆g由:

∝表示g与右边的量成正比。为了得到归一化因子，只要将g(m)归一化到单位球上就足够了。
式(6)可通过对式(3)求逆，加上p_s必须在单位球上的约束而得，因此 $x_s^2 + y_s^2 + z_s^2= 1$ 。从这个约束条件，我们得到了zs作为xm和ym的函数的表达式。更多细节可以在[12]中找到。

可以看出式(6)是中心折反相机投影模型的核心。它表示了归一化图像平面上的点m与镜像参考系中单位向量Ps之间的关系。注意，对于平面镜，我们 $\epsilon= 0$ ，成为透视相机的投影方程 $P_s \propto (x_m, y_m, 1)$ 。
该模型已被证明能够准确地描述所有中心折反相机(抛物面镜、双曲镜和椭圆镜)和标准透视相机。Ying和Hu[9]在2004年提出了对鱼眼镜头模型的扩展。然而，通过折反射光学相机来接近鱼眼相机只能在有限的精度下工作。这主要是因为，虽然可以用精确的参数函数(抛物线、双曲线、椭圆)表示三种中心折射率相机，但鱼眼镜头的投影模型因相机而异，并取决于镜头的视场。为了克服这个问题，提出了一个新的统一模型，将在下一节中进行描述。

2.4. Unified model for catadioptric and fisheye cameras（折射率相机和鱼眼相机的统一模型）

该统一模型由Scaramuzza等人在2006年提出[10,11]。与前一个模型的主要区别在于函数g的选择。针对鱼眼相机参数化模型知识不足的问题，提出了利用泰勒多项式的方法，通过标定过程求出泰勒多项式的系数和度。因此,规范化的关系像点m= (xm, ym, 1)和鱼眼的单位向量Ps(镜子)参考系可以写成:

其中， $\rho = \sqrt{x_m^2 + y_m^2}$ 。读者可能已经注意到，多项式的一阶项(即 $\alpha_1 \rho$ )不见了。这是根据观察得出的，在ρ = 0处计算的多项式的一阶导数对于折反射相机和鱼眼相机都必须是零(这是通过微分(6)对折反射相机进行验证的直接方法)。还要注意，由于它的多项式性质，这个表达式可以包含折反射相机、鱼眼相机和透视相机。这可以通过适当地选择多项式的次数来实现。正如作者所强调的，阶数为3或4的多项式能够非常准确地建模所有折反射相机和市场上可用的多种鱼眼相机。这一模型适用于广泛的商业相机是其成功的根源。

2.5. Omnidirectional camera calibration

全向摄像机的标定与标准视角摄像机的标定类似。同样，最流行的方法利用了用户在不同位置和方向上显示的平面网格。对于全向相机来说，标定图像是在相机周围拍摄的，而不是在单面拍摄的，这一点非常重要。这是为了补偿相机和后视镜之间可能出现的偏差。
值得一提的是，目前有三种开源的Matlab校准工具箱，它们的区别主要在于所采用的投影模型和校准模式的类型。

3. ocam图转多张针孔图

3.1. Python实现

import numpy as np

""" Reads file containing the ocamcalib parameters exported from the Matlab toolbox """
def get_ocam_model(filename):
    o = {}
    with open(filename) as f:
        lines = [l for l in f]
        
        l = lines[2]
        data = l.split()
        o['length_pol'] = int(data[0])
        o['pol'] = [float(d) for d in data[1:]]
        
        l = lines[6]
        data = l.split()
        o['length_invpol'] = int(data[0])
        o['invpol'] = [float(d) for d in data[1:]]
        
        l = lines[10]
        data = l.split()
        o['xc'] = float(data[0])
        o['yc'] = float(data[1])
        
        l = lines[14]
        data = l.split()
        o['c'] = float(data[0])
        o['d'] = float(data[1])
        o['e'] = float(data[2])
                
        l = lines[18]
        data = l.split()
        o['height'] = int(data[0])
        o['width'] = int(data[1])

    return o


def get_ocam_model_from_json(camera_name, camera_calib_dict):
    o = {}
    o['length_pol'] = camera_calib_dict[camera_name]['intrinsic_param']['cam2world_len']
    o['pol'] = [float(d) for d in camera_calib_dict[camera_name]['intrinsic_param']['cam2world']]
        
    o['length_invpol'] = camera_calib_dict[camera_name]['intrinsic_param']['world2cam_len']
    o['invpol'] = [float(d) for d in camera_calib_dict[camera_name]['intrinsic_param']['world2cam']]
        
    o['xc'] = camera_calib_dict[camera_name]['intrinsic_param']['center'][0]
    o['yc'] = camera_calib_dict[camera_name]['intrinsic_param']['center'][1]
        
    o['c'] = camera_calib_dict[camera_name]['intrinsic_param']['affine_c']
    o['d'] = camera_calib_dict[camera_name]['intrinsic_param']['affine_d']
    o['e'] = camera_calib_dict[camera_name]['intrinsic_param']['affine_e']
                
    o['width']  = camera_calib_dict[camera_name]['ImgSize'][0]
    o['height'] = camera_calib_dict[camera_name]['ImgSize'][1]
    print ('o = ', o)
    return o


def cam2world(point2D, o):
    point3D = []
    
    invdet = 1.0/(o['c']-o['d']*o['e'])

    xp = invdet*((point2D[0]-o['xc']) - o['d']*(point2D[1]-o['yc']))
    yp = invdet*(-o['e']*(point2D[0]-o['xc']) + o['c']*(point2D[1]-o['yc']))
    
    r = np.linalg.norm([xp,yp])
    
    zp = o['pol'][0]
    r_i = 1.0
    
    for i in range(1,o['length_pol']):
        r_i *= r
        zp += r_i*o['pol'][i]
        
    invnorm = 1.0/np.linalg.norm([xp,yp,zp])
    
    point3D.append(invnorm*xp)
    point3D.append(invnorm*yp)
    point3D.append(invnorm*zp)
    
    return point3D


def world2cam_bak(point3D, o):
    point2D = []    
    
    norm = np.linalg.norm(point3D[:2])

    if norm != 0:
        theta = np.arctan(point3D[2]/norm)
        invnorm = 1.0/norm
        t = theta
        rho = o['invpol'][0]
        t_i = 1.0
        
        for i in range(1,o['length_invpol']):
            t_i *= t
            rho += t_i*o['invpol'][i]
            
        x = point3D[0]*invnorm*rho
        y = point3D[1]*invnorm*rho
         
        point2D.append(x*o['c']+y*o['d']+o['xc'])
        point2D.append(x*o['e']+y+o['yc'])
    else:
        point2D.append(o['xc'])
        point2D.append(o['yc'])
        
    return point2D


def world2cam(point3D, o):
    point2D = []    
    
    norm = np.linalg.norm(point3D[:2])

    if norm != 0:
        theta = np.arctan(point3D[2]/norm)
        invnorm = 1.0/norm
        t = theta
        rho = o['invpol'][0]
        t_i = 1.0
        
        for i in range(1,o['length_invpol']):
            t_i *= t
            rho += t_i*o['invpol'][i]
            
        x = point3D[0]*invnorm*rho
        y = point3D[1]*invnorm*rho
         
        point2D.append(x        + y*o['e'] + o['xc'])
        point2D.append(x*o['d'] + y*o['c'] + o['yc'])
        
    else:
        point2D.append(o['xc'])
        point2D.append(o['yc'])
        
    return point2D


def create_perspective_undistortion_LUT(o, sf):

    mapx = np.zeros((o['height'],o['width']))    
    mapy = np.zeros((o['height'],o['width']))    
    
    Nxc = o['height']/2.0
    Nyc = o['width']/2.0   
    Nz = -o['width']/sf       

    for i in range(o['height']):
        for j in range(o['width']):
            M = []
            M.append(i - Nxc)
            M.append(j - Nyc)
            M.append(Nz)
            m = world2cam(M, o)     
            mapx[i,j] = m[1]
            mapy[i,j] = m[0]
            
    return mapx, mapy


# horizontal_angle_offset, in degree, [-90, 90], default = 0   
def create_perspective_undistortion_LUT_Focal(o, persp_height, persp_width, focal, horizontal_angle_offset):
    
    mapx = np.zeros((persp_height,persp_width))    
    mapy = np.zeros((persp_height,persp_width))    
    
    Nxc = persp_width/2.0   
    Nyc = persp_height/2.0
    Nz = -focal       

    theat = horizontal_angle_offset*np.pi/180.0
    rotation = np.array([ np.cos(theat), 0, np.sin(theat),
                                      0, 1,             0,
                         -np.sin(theat), 0, np.cos(theat)]).reshape(3,3)
    #print(rotation)
    for i in range(persp_height):
        for j in range(persp_width):
            M = []
            M.append(j - Nxc)
            M.append(i - Nyc)
            M.append(Nz)
            M_ary = np.array(M).reshape(3,-1)
            #print(M_ary)
            M_ary_rot = np.dot(rotation, M_ary).astype(np.float64)
            #print(M_ary_rot)
            
            m = world2cam(M_ary_rot.reshape(3).tolist(), o)     
            mapx[i,j] = m[0]
            mapy[i,j] = m[1]
            
    return mapx, mapy
  
          
def create_panoramic_undistortion_LUT(Rmin, Rmax, o):

    mapx = np.zeros((o['height'],o['width']))    
    mapy = np.zeros((o['height'],o['width']))    
    
    for i in range(o['height']):
        for j in range(o['width']):
            theta = -(float(j))/o['width']*2*np.pi
            rho = Rmax - float(Rmax-Rmin)/o['height']*i
            mapx[i,j] = o['yc'] + rho*np.sin(theta)
            mapy[i,j] = o['xc'] + rho*np.cos(theta)
            
    return mapx, mapy

3.2. C++实现

#include <map>
#include <string>
#include <math.h>
#include <Eigen/Dense>
#include <opencv2/core/core.hpp>
#include <jsoncpp/json/json.h>

typedef std::pair<cv::Mat, cv::Mat> LUT;

struct OcamModel{
    int length_pol, length_invpol, height, width;
    float xc, yc, c, d, e;
    std::vector<float> pol, invpol;
};

// used
OcamModel get_ocam_model(std::string filename){
    OcamModel o;
    return o;
}

OcamModel get_ocam_model_from_json(std::string camera_name, std::map<std::string, Json::Value> camera_calib_dict){
    OcamModel o;
    o.length_pol = camera_calib_dict[camera_name]["intrinsic_param"]["cam2world_len"].asInt();
    Json::Value vec = camera_calib_dict[camera_name]["intrinsic_param"]["cam2world"];
    for(auto x : vec)
        o.pol.push_back(x.asFloat());

    o.length_invpol = camera_calib_dict[camera_name]["intrinsic_param"]["world2cam_len"].asInt();
    vec = camera_calib_dict[camera_name]["intrinsic_param"]["world2cam"];
    for(auto x : vec)
        o.invpol.push_back(x.asFloat());

    o.xc = camera_calib_dict[camera_name]["intrinsic_param"]["center"][0].asFloat();
    o.yc = camera_calib_dict[camera_name]["intrinsic_param"]["center"][1].asFloat();

    o.c = camera_calib_dict[camera_name]["intrinsic_param"]["affine_c"].asFloat();
    o.d = camera_calib_dict[camera_name]["intrinsic_param"]["affine_d"].asFloat();
    o.e = camera_calib_dict[camera_name]["intrinsic_param"]["affine_e"].asFloat();

    o.width = camera_calib_dict[camera_name]["ImgSize"][0].asInt();
    o.height = camera_calib_dict[camera_name]["ImgSize"][1].asInt();

    // std::cout << "o.length_pol = " << o.length_pol << std::endl;
    // std::cout << "o.length_invpol = " << o.length_invpol << std::endl;
    // std::cout << "o.height = " << o.height << std::endl;
    // std::cout << "o.width = " << o.width << std::endl;
    // std::cout << "o.xc = " << o.xc << std::endl;
    // std::cout << "o.yc = " << o.yc << std::endl;
    // std::cout << "o.c = " << o.c << std::endl;
    // std::cout << "o.d = " << o.d << std::endl;
    // std::cout << "o.e = " << o.e << std::endl;

    return o;
}

cv::Vec3f cam2world(cv::Vec2f p2, OcamModel o){
    float invdet = 1.0 / (o.c-o.d*o.e);
    float xp = invdet * ((p2(0)-o.xc) - o.d*(p2(1) - o.yc));
    float yp = invdet * (-o.e*(p2(0) - o.xc) + o.c * (p2(1) - o.yc));
    float r = sqrt(xp * xp + yp * yp);
    float zp = o.pol[0];
    float r_i = 1.0;

    for(int i = 1; i<o.length_pol; i++){
        r_i *= r;
        zp += r_i * o.pol[i];
    }

    float invnorm = 1.0 / sqrt(xp * xp + yp * yp + zp * zp);
    return cv::Vec3f(invnorm * xp, invnorm * yp, invnorm * zp);
}

cv::Vec2f world2cam(cv::Vec3f p3, OcamModel o){
    float norm = sqrt(p3(0) * p3(0) + p3(1) * p3(1));
    if(norm == 0)
        return cv::Vec2f(o.xc, o.yc);

    float theta = atan(p3(2) / norm);
    float invnorm = 1.0 / norm;
    float t = theta;
    float rho = o.invpol[0];
    float t_i = 1.0;

    for(int i = 1; i<o.length_invpol; i++){
        t_i *= t;
        rho += t_i * o.invpol[i];
    }

    float x = p3[0] * invnorm * rho;
    float y = p3[1] * invnorm * rho;
    return cv::Vec2f(x + y*o.e + o.xc, x*o.d + y*o.c + o.yc);
}

LUT create_perspective_undistortion_LUT_Focal(OcamModel o, int persp_height, int persp_width, int focal, int horizontal_angle_offset){
    cv::Mat mapx(persp_height, persp_width, CV_32FC1), mapy(persp_height, persp_width, CV_32FC1);
    // std::vector<std::vector<float>> mapy(persp_height, std::vector<float>(persp_width));

    float Nxc = (float)persp_width / 2.0;
    float Nyc = (float)persp_height / 2.0;
    float Nz = -(float)focal;

    float theta = (float)horizontal_angle_offset * M_PI / 180.0;
    Eigen::Matrix3f rotation;
    rotation << cos(theta), 0, sin(theta), 0, 1, 0, -sin(theta), 0, cos(theta);

    for (int i = 0; i<persp_height; i++) {
        // uchar *ptrx = mapx.ptr<uchar>(i);
        // uchar *ptry = mapy.ptr<uchar>(i);
        for(int j = 0; j<persp_width; j++){
            Eigen::Vector3f p3(j-Nxc, i-Nyc, Nz);
            Eigen::Vector3f v3 = rotation * p3;
            cv::Vec2f m = world2cam(cv::Vec3f(v3[0], v3[1], v3[2]), o);
            // std::cout << m << std::endl;
            // *ptrx = m[0]; ptrx++;
            // *ptry = m[1]; ptry++;
            mapx.at<float>(i, j) = m[0];
            mapy.at<float>(i, j) = m[1];
        }
    }
    return {mapx, mapy};
}

LUT create_panoramic_undistortion_LUT(float Rmin, float Rmax, OcamModel o){
    cv::Mat mapx(o.height, o.width, CV_32FC1), mapy(o.height, o.width, CV_32FC1);

    for(int i = 0; i<o.height; i++){
        for(int j = 0; j<o.width; i++){
            float theta = -2 * M_PI * (float)j / o.width;
            float rho = Rmax - (float)(Rmax - Rmin) * (float)i / (float)o.height;
            mapx.at<float>(i, j) = o.yc + rho * sin(theta);
            mapy.at<float>(i, j) = o.xc + rho * cos(theta);
        }
    }
    return {mapx, mapy};
}

4. 坐标投影到像素

4.1. 3D到2D投影

$u=A_{11} \cdot \frac{X}{\sqrt{X^2+Y^2}} f(\theta)+A_{12} \cdot \frac{Y}{\sqrt{X^2+Y^2}} f(\theta)+B_1\\ v=A_{21} \cdot \frac{X}{\sqrt{X^2+Y^2}} f(\theta)+A_{22} \cdot \frac{Y}{\sqrt{X^2+Y^2}} f(\theta)+B_2\\ \theta=arctan2(\frac{-Z}{\sqrt{X^2+Y^2}}) \\ f(\theta)=w_{n-1}\theta^{n-1}+w_{n-2}\theta^{n-2}+...+w_{1}\theta^{1}+w_{0}$

class SVCCalibrationSDK:
    def __init__(self, intrinsic_param, width, height):
        self.image_size = np.array([height, width])  # rows, cols
        self.world2cam = np.array(intrinsic_param["world2cam"]).reshape(-1, 1)
        self.world2cam_len = np.array(intrinsic_param["world2cam_len"])
        self.svc_rotation = np.array([[1., intrinsic_param["affine_e"]],
                                     [intrinsic_param["affine_d"], intrinsic_param["affine_c"]]])
        self.svc_translation = np.array(intrinsic_param["center"])

    def cam_to_pixel(self, points):
        num_points = len(points)

        if num_points == 0:
            return np.empty((0, 3))
        else:
            norm = np.sqrt(np.sum(points[:, 0:2] * points[:, 0:2], axis=1, keepdims=True))
            theta = np.arctan(points[:, 2:3] * (-1 / norm))

            poly_theta = np.power(theta, np.arange(self.world2cam_len))
            rho = (poly_theta @ self.world2cam)

            pixels = (points[:, 0:2] * (rho / norm)) @ self.svc_rotation + self.svc_translation

        return pixels

4.2. 2D到3D投影

$[u', v']=A^{inv}*([u,v]-B)\\ |f(\theta)|=\sqrt{u'^2+v'^2}\\ X=\frac{u' \cdot |Z|}{|f^{inv}(|f(\theta)|)|} \quad Y=\frac{v' \cdot |Z|}{|f^{inv}(|f(\theta)|)|} \quad \Longrightarrow \quad u'=\frac{X \cdot |f^{inv}(|f(\theta)|)|}{|Z|} \quad v'=\frac{Y \cdot |f^{inv}(|f(\theta)|)|}{|Z|}\\ Z=(-1 \quad if \quad f^{inv}(|f(\theta)|)\ >\ 0\quad else\quad 1)*|Z|$