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
|
/*
* =====================================================================================
*
* Filename: camera.cpp
*
* Description: Class for camera
*
* Version: 1.0
* Created: 03/19/2017 09:39:58 PM
* Revision: none
* Compiler: gcc
*
* Author: Martin Miller (MHM), miller7@illinois.edu
* Organization: Aerospace Robotics and Controls Lab (ARC)
*
* =====================================================================================
*/
#include "camera.h"
#include <iostream>
/*
*--------------------------------------------------------------------------------------
* Class: Camera
* Method: Camera
* Description: constructor
*--------------------------------------------------------------------------------------
*/
Camera::Camera (const char *fin)
{
YAML::Node config = YAML::LoadFile(fin);
_K(0,0) = BINNING*config["camera_matrix"]["data"][0].as<double>();
_K(0,1) = BINNING*config["camera_matrix"]["data"][1].as<double>();
_K(0,2) = BINNING*config["camera_matrix"]["data"][2].as<double>();
_K(1,0) = BINNING*config["camera_matrix"]["data"][3].as<double>();
_K(1,1) = BINNING*config["camera_matrix"]["data"][4].as<double>();
_K(1,2) = BINNING*config["camera_matrix"]["data"][5].as<double>();
_K(2,0) = BINNING*config["camera_matrix"]["data"][6].as<double>();
_K(2,1) = BINNING*config["camera_matrix"]["data"][7].as<double>();
_K(2,2) = config["camera_matrix"]["data"][8].as<double>();
_d[0] = config["distortion_coefficients"]["data"][0].as<double>();
_d[1] = config["distortion_coefficients"]["data"][1].as<double>();
_d[2] = config["distortion_coefficients"]["data"][2].as<double>();
_d[3] = config["distortion_coefficients"]["data"][3].as<double>();
_T(0,0) = config["T_cam_imu"][0][0].as<double>();
_T(0,1) = config["T_cam_imu"][0][1].as<double>();
_T(0,2) = config["T_cam_imu"][0][2].as<double>();
_T(0,3) = config["T_cam_imu"][0][3].as<double>();
_T(1,0) = config["T_cam_imu"][1][0].as<double>();
_T(1,1) = config["T_cam_imu"][1][1].as<double>();
_T(1,2) = config["T_cam_imu"][1][2].as<double>();
_T(1,3) = config["T_cam_imu"][1][3].as<double>();
_T(2,0) = config["T_cam_imu"][2][0].as<double>();
_T(2,1) = config["T_cam_imu"][2][1].as<double>();
_T(2,2) = config["T_cam_imu"][2][2].as<double>();
_T(2,3) = config["T_cam_imu"][2][3].as<double>();
_T(3,0) = config["T_cam_imu"][3][0].as<double>();
_T(3,1) = config["T_cam_imu"][3][1].as<double>();
_T(3,2) = config["T_cam_imu"][3][2].as<double>();
_T(3,3) = config["T_cam_imu"][3][3].as<double>();
} /* ----- end of method Camera::Camera (constructor) ----- */
/*
*--------------------------------------------------------------------------------------
* Class: Camera
* Method: Camera :: Rc2b
* Description: Returns the rotation matrix that transforms a point from the
* camera frame to the body frame.
*--------------------------------------------------------------------------------------
*/
Matrix<double,3,3>
Camera::Rc2b ( )
{
return _T.block<3,3>(0,0).transpose();
} /* ----- end of method Camera::Rc2b ----- */
Matrix<double,3,3>
Camera::K ( )
{
return _K ;
} /* ----- end of method Camera::K ----- */
Vector4d
Camera::d ( )
{
return _d;
} /* ----- end of method Camera::d ----- */
Vector3d
Camera::img2body ( Vector3d &xi )
{
Vector3d xc, xb;
xc = K().inverse()*xi;
// Normalize
xc /= xc.norm();
xb = Rc2b()*xc;
return xb;
} /* ----- end of method Camera::img2body ----- */
|
