视觉SLAM之鱼眼相机模型

最近研究了视觉SLAM中不同的鱼眼相机模型，其中包括：

Scaramuzza的鱼眼相机模型

代表性的SLAM工作为MultiCo-SLAM,是一个以ORB-SLAM为基础的扩展的多鱼眼相机视觉SLAM系统（Video，Paper）
相机模型论文：A Flexible Technique for Accurate Omnidirectional Camera Calibration and
Structure from Motion
2017年Urban等人在此基础上，投影函数，使结果更加精确，论文地址

Scaramuzza相机模型标定工具：Tools:，其操作流程均可在相应网页中找到。

如上图所示，为Scaramuzza模型投影过程。
投影公式：

上述公式（1）中，${\left[u^{\prime },v^{\prime }\right]}^T$为image plane，$[u,v{]}^T$为sensor plane，对应于针孔相机模型来说，其实就是像素平面和图像平面。
上述公式（2）中，ρ为到图像中心的径向欧氏距离。
上述公式（3）中，为image point到空间三维点的映射方程。经过标定你会发现a1=0，这是因为，通过观察上图投影模型你会发现，当$f(\rho )$取极大值时，图像坐标x=y=0,也就是入射角为0度，$f(\rho )$取导数，$f^{\prime }(\rho )$=0, $\rho =\sqrt{u^2+v^2}$=0，那么a1=0。
上述公式（4）为投影函数，其实在标定的过程中，主要标定的就是f函数的系数。
用一个示例程序说明Scaramuzza相机模型的投影过程.首先进行标定，下面代码中已经给出标定结果，直接拿来取用，3D点采用随机数产生。部分注释已经写在代码中，欢迎共同交流学习。

// cam_model_general.h
#pragma once

#ifndef CAM_MODEL_GENERAL_H
#define CAM_MODEL_GENERAL_H

#include <opencv2/opencv.hpp>

// horner scheme for evaluating polynomials at a value x
template<typename T>
T horner(T* coeffs, int s, T x)
{
   
   
	T res = 0.0;
	for (int i = s - 1; i >= 0; i--)
		res = res * x + coeffs[i];

	return res;
}

// template class implementation of the general atan model
template <typename T>
class cCamModelGeneral
{
   
   
public:
	// construtors
	cCamModelGeneral() :
		c(T(1)),
		d(T(0)),
		e(T(0)),
		u0(T(0)),
		v0(T(0)),
		p((cv::Mat_<T>(1, 1) << T(1))),
		invP((cv::Mat_<T>(1, 1) << T(1))),
		p_deg(1),
		invP_deg(1),
		Iwidth(T(0)),