devbox
AllinToyou
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

视觉SLAM之鱼眼相机模型_scaramuzza模型

作者：AllinToyou | 2024-03-23 03:07:16

赞

踩

scaramuzza模型

最近研究了视觉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投影模型
如上图所示，为Scaramuzza模型投影过程。
投影公式：
在这里插入图片描述
上述公式（1）中， [ u ′ , v ′ ] T \left[u^{\prime}, v^{\prime}\right]^{T} [u′,v′]T为image plane， [ u , v ] T [u, v]^{T} [u,v]T为sensor plane，对应于针孔相机模型来说，其实就是像素平面和图像平面。
上述公式（2）中，ρ为到图像中心的径向欧氏距离。
上述公式（3）中，为image point到空间三维点的映射方程。经过标定你会发现a1=0，这是因为，通过观察上图投影模型你会发现，当 f ( ρ ) f(\rho) f(ρ)取极大值时，图像坐标x=y=0,也就是入射角为0度， f ( ρ ) f(\rho) f(ρ)取导数， f ′ ( ρ ) f^{\prime}(\rho) f′(ρ)=0, ρ = u 2 + v 2 \rho=\sqrt{u^{2}+v^{2}} ρ=u2+v2 ​=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)), Iheight(T(0))
	{}

	cCamModelGeneral(cv::Vec<T, 5> cdeu0v0,
		cv::Mat_<T> p_,
		cv::Mat_<T> invP_) :
		c(cdeu0v0[0]),
		d(cdeu0v0[1]),
		e(cdeu0v0[2]),
		u0(cdeu0v0[3]),
		v0(cdeu0v0[4]),
		p(p_),
		invP(invP_)
	{
		// initialize degree of polynomials
		p_deg = (p_.rows > 1) ? p_.rows : p_deg = p_.cols;
		invP_deg = (p_.rows > 1) ? invP_deg = invP_.rows : invP_deg = invP_.cols;

		cde1 = (cv::Mat_<T>(2, 2) << c, d, e, T(1));
	}

	cCamModelGeneral(cv::Vec<T, 5> cdeu0v0,
		cv::Mat_<T> p_,
		cv::Mat_<T> invP_,
		int Iw_, int Ih_) :
		c(cdeu0v0[0]),
		d(cdeu0v0[1]),
		e(cdeu0v0[2]),
		u0(cdeu0v0[3]),
		v0(cdeu0v0[4]),
		p(p_),
		invP(invP_),
		Iwidth(Iw_),
		Iheight(Ih_)
	{
		// initialize degree of polynomials
		p_deg = (p_.rows > 1) ? p_.rows : p_deg = p_.cols;
		invP_deg = (p_.rows > 1) ? invP_deg = invP_.rows : invP_deg = invP_.cols;

		cde1 = (cv::Mat_<T>(2, 2) << c, d, e, T(1));
	}

	~cCamModelGeneral() {}


	template <typename T> inline void
		WorldToImg(const T& x, const T& y, const T& z,    // 3D scene point
			T& u, T& v)					  // 2D image point
	{
		T norm = sqrt(x*x + y*y);
		if (norm == T(0))
			norm = 1e-14;

		T theta = atan(-z / norm);
		T rho = horner<T>((T*)invP.data, invP_deg, theta);

		T uu = x / norm * rho;
		T vv = y / norm * rho;

		u = uu*c + vv*d + u0;
		v = uu*e + vv + v0;

	}

	template <typename T> inline void
		WorldToImg(const cv::Point3_<T>& X,			// 3D scene point
			cv::Point_<T>& m)			// 2D image point
	{
		T norm = sqrt(X.x*X.x + X.y*X.y);

		if (norm == T(0))
			norm = 1e-14;

		T theta = atan(-X.z / norm);

		T rho = horner<T>((T*)invP.data, invP_deg, theta);

		T uu = X.x / norm * rho;
		T vv = X.y / norm * rho;

		m.x = uu*c + vv*d + u0;
		m.y = uu*e + vv + v0;
	}

	// fastest by about factor 2
	template <typename T> inline void
		WorldToImg(const cv::Vec<T, 3>& X,			// 3D scene point
			cv::Vec<T, 2>& m)			// 2D image point
	{

		double norm = cv::sqrt(X(0)*X(0) + X(1)*X(1));

		if (norm == 0.0)
			norm = 1e-14;

		double theta = atan(-X(2) / norm);

		double rho = horner<T>((T*)invP.data, invP_deg, theta);

		double uu = X(0) / norm * rho;
		double vv = X(1) / norm * rho;

		m(0) = uu*c + vv*d + u0;
		m(1) = uu*e + vv + v0;
	}

	template <typename T> inline void
		ImgToWorld(T& x, T& y, T& z,				// 3D scene point
			const T& u, const T& v) 			    // 2D image point
	{
		T invAff = c - d*e;
		T u_t = u - u0;
		T v_t = v - v0;
		// inverse affine matrix image to sensor plane conversion
		x = (1 * u_t - d * v_t) / invAff;
		y = (-e * u_t + c * v_t) / invAff;
		T X2 = x*x;
		T Y2 = y*y;
		z = -horner<T>((T*)p.data, p_deg, sqrt(X2 + Y2));

		// normalize vectors spherically
		T norm = sqrt(X2 + Y2 + z*z);
		x /= norm;
		y /= norm;
		z /= norm;
	}

	template <typename T> inline void
		ImgToWorld(cv::Point3_<T>& X,						// 3D scene point
			const cv::Point_<T>& m) 			            // 2D image point
	{
		T invAff = c - d*e;
		T u_t = m.x - u0;
		T v_t = m.y - v0;
		// inverse affine matrix image to sensor plane conversion
		X.x = (1 * u_t - d * v_t) / invAff;
		X.y = (-e * u_t + c * v_t) / invAff;
		T X2 = X.x*X.x;
		T Y2 = X.y*X.y;
		X.z = -horner<T>((T*)p.data, p_deg, sqrt(X2 + Y2));

		// normalize vectors spherically
		T norm = sqrt(X2 + Y2 + X.z*X.z);
		X.x /= norm;
		X.y /= norm;
		X.z /= norm;
	}

	template <typename T> inline void
		ImgToWorld(cv::Vec<T, 3>& X,						// 3D scene point
			const cv::Vec<T, 2>& m) 			            // 2D image point
	{
		T invAff = c - d*e;
		T u_t = m(0) - u0;
		T v_t = m(1) - v0;
		// inverse affine matrix image to sensor plane conversion
		X(0) = (1 * u_t - d * v_t) / invAff;
		X(1) = (-e * u_t + c * v_t) / invAff;
		T X2 = X(0)*X(0);
		T Y2 = X(1)*X(1);
		X(2) = -horner<T>((T*)p.data, p_deg, sqrt(X2 + Y2));

		// normalize vectors spherically
		T norm = sqrt(X2 + Y2 + X(2)*X(2));
		X(0) /= norm;
		X(1) /= norm;
		X(2) /= norm;
	}

	// get functions
	T Get_c() { return c; }
	T Get_d() { return d; }
	T Get_e() { return e; }

	T Get_u0() { return u0; }
	T Get_v0() { return v0; }

	int GetInvDeg() { return invP_deg; }
	int GetPolDeg() { return p_deg; }

	cv::Mat_<T> Get_invP() { return invP; }
	cv::Mat_<T> Get_P() { return p; }

	T GetWidth() { return Iwidth; }
	T GetHeight() { return Iheight; }

	cv::Mat GetMirrorMask(int pyrL) { return mirrorMasks[pyrL]; }
	void SetMirrorMasks(std::vector<cv::Mat> mirrorMasks_) { mirrorMasks = mirrorMasks_; }

	bool isPointInMirrorMask(const T& u, const T& v, int pyr)
	{
		int ur = cvRound(u);
		int vr = cvRound(v);
		// check image bounds
		if (ur >= mirrorMasks[pyr].cols || ur <= 0 ||
			vr >= mirrorMasks[pyr].rows || vr <= 0)
			return false;
		// check mirror
		if (mirrorMasks[pyr].at<uchar>(vr, ur) > 0)
			return true;
		else return false;
	}

private:
	// affin parameters
	T c;
	T d;
	T e;
	cv::Mat_<T> cde1;
	// principal point
	T u0;
	T v0;
	// polynomial
	cv::Mat_<T> p;
	int p_deg;
	// inverse polynomial
	cv::Mat_<T> invP;
	int invP_deg;
	// image width and height
	int Iwidth;
	int Iheight;
	// mirror mask on pyramid levels
	std::vector<cv::Mat> mirrorMasks;
};


#endif

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129
  • 130
  • 131
  • 132
  • 133
  • 134
  • 135
  • 136
  • 137
  • 138
  • 139
  • 140
  • 141
  • 142
  • 143
  • 144
  • 145
  • 146
  • 147
  • 148
  • 149
  • 150
  • 151
  • 152
  • 153
  • 154
  • 155
  • 156
  • 157
  • 158
  • 159
  • 160
  • 161
  • 162
  • 163
  • 164
  • 165
  • 166
  • 167
  • 168
  • 169
  • 170
  • 171
  • 172
  • 173
  • 174
  • 175
  • 176
  • 177
  • 178
  • 179
  • 180
  • 181
  • 182
  • 183
  • 184
  • 185
  • 186
  • 187
  • 188
  • 189
  • 190
  • 191
  • 192
  • 193
  • 194
  • 195
  • 196
  • 197
  • 198
  • 199
  • 200
  • 201
  • 202
  • 203
  • 204
  • 205
  • 206
  • 207
  • 208
  • 209
  • 210
  • 211
  • 212
  • 213
  • 214
  • 215
  • 216
  • 217
  • 218
  • 219
  • 220
  • 221
  • 222
  • 223
  • 224
  • 225
  • 226
  • 227
  • 228
  • 229
  • 230
  • 231
  • 232
  • 233
  • 234
  • 235
  • 236
  • 237
  • 238
  • 239
  • 240
  • 241
  • 242
  • 243
  • 244
  • 245
  • 246
  • 247
  • 248
  • 249
  • 250
  • 251
  • 252
  • 253
  • 254
  • 255
  • 256
  • 257
  • 258
  • 259
  • 260
  • 261
  • 262
// cam_model_general.cpp
#include <iostream>
#include <chrono>

#include "cam_model_general.h"

using namespace std;
using namespace cv;

double time2double(std::chrono::steady_clock::time_point start,
	std::chrono::steady_clock::time_point end)
{
	return static_cast<double>(
		std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count() * (double)1e-9);
}

double dRand(double fMin, double fMax)
{
	return fMin + (double)rand() / RAND_MAX * (fMax - fMin);
}

int main()
{
	// number of iterations for speed test
	long iterations = 1e8;

	// take some real world cam model
	// this is the camera model of data set Fisheye1_
	// ATTENTION!! I also switched the principal point coordinates
	Vec<double, 5> interior_orientation(0.998883018922937, -0.0115128845387445,	//cdeu0v0 均是标定得到
		0.0107836324042904, 544.763473297893, 378.781825009886);
	Mat_<double> p = (Mat_<double>(5, 1) << -338.405137634369,  // poly系数，标定得到
		0.0,
		0.00120189826837736,
		-1.27438189154991e-06,
		2.85466623521256e-09);
	// attention: this is the reverse order of findinvpoly 
	// as matlab evaluates the polynomials differently
	Mat_<double> pInv = (Mat_<double>(11, 1) << 510.979186217526, // invPoly系数，标定得到
		291.393724562448,
		-13.8758863124724,
		42.4238251854176,
		23.054291112414,
		-7.18539785128328,
		14.1452111052043,
		18.5034196957122,
		-2.39675686593404,
		-7.18896323060144,
		-1.85081569557094);

	// here comes the camera model
	cCamModelGeneral<double> camModel(interior_orientation, p, pInv);

	// test the correctness of the implementation, at least internally
	double x0 = dRand(0, 5);//不应该是随机数产生的3D点么？为什么每次运行都一样呢？
	double y0 = dRand(0, 5);
	double z0 = dRand(0, 5);

	Vec3d vec3d(x0, y0, z0);
	Vec3d vec3d_normalized = (1 / norm(vec3d)) * vec3d;
	Vec2d projection;
	Vec3d unprojected;
	camModel.WorldToImg(vec3d, projection);//3D点投影到2D点
	cout << "projected point: " << projection << endl;

	camModel.ImgToWorld(unprojected, projection);//把2D点投影到3D点，但这个3D点是归一化的三维点
	cout << "unprojected:" << unprojected << endl;
	cerr << "difference after unproject: " << norm(vec3d_normalized - unprojected) << endl;

	std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
	// timings
	for (int i = 0; i < iterations; ++i)
	{
		camModel.WorldToImg(vec3d, projection);
		camModel.ImgToWorld(unprojected, projection);
	}
	std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
	cout << "total time for " << iterations << " iterations of world2cam and cam2world: " << time2double(begin, end) << " s" << endl;
	cout << "time for one iteration: " << time2double(begin, end) / iterations * 1e9 << " nano seconds" << endl;
	return 0;
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81

mei相机模型

数学模型如下所示。也是VINS-mono所用的相机模型。具体可参考网页
相机模型的参考论文
在这里插入图片描述

未完待续~~仿佛说的不够细致，仿佛什么都没说。

本文内容由网友自发贡献，转载请注明出处：【wpsshop博客】
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号