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
|
/************************************************************************
*
* Copyright (C) 2017-2023 IRCAD France
* Copyright (C) 2017-2018 IHU Strasbourg
*
* This file is part of Sight.
*
* Sight is free software: you can redistribute it and/or modify it under
* the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Sight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with Sight. If not, see <https://www.gnu.org/licenses/>.
*
***********************************************************************/
#include "geometry/eigen/helper.hpp"
#include <Eigen/Geometry>
namespace sight::geometry::eigen::helper
{
//-------------------------------------------------------------------------------------------------
data::matrix4::sptr from_eigen(const Eigen::Matrix4f& _mat)
{
data::matrix4::sptr trf = std::make_shared<data::matrix4>();
for(unsigned int r = 0 ; r < 4 ; ++r)
{
for(unsigned int c = 0 ; c < 4 ; ++c)
{
(*trf)(r, c) = static_cast<double>(_mat(r, c));
}
}
return trf;
}
//-------------------------------------------------------------------------------------------------
data::matrix4::sptr from_eigen(const Eigen::Matrix4d& _mat)
{
data::matrix4::sptr trf = std::make_shared<data::matrix4>();
for(unsigned int r = 0 ; r < 4 ; ++r)
{
for(unsigned int c = 0 ; c < 4 ; ++c)
{
(*trf)(r, c) = _mat(r, c);
}
}
return trf;
}
//-------------------------------------------------------------------------------------------------
rvec_tvec_t eigen_mat_to_rvec_tvec(const Eigen::Matrix4d& _mat)
{
Eigen::AngleAxisd angle_axis;
Eigen::Matrix3d rotation;
for(unsigned int r = 0 ; r < 3 ; ++r)
{
for(unsigned int c = 0 ; c < 3 ; ++c)
{
rotation(r, c) = _mat(r, c);
}
}
angle_axis.fromRotationMatrix(rotation);
Eigen::Vector3d rvec = angle_axis.angle() * angle_axis.axis();
Eigen::Vector3d tvec = _mat.block<3, 1>(0, 3);
return std::make_pair(rvec, tvec);
}
//-------------------------------------------------------------------------------------------------
rvec_tvec_t f4s_mat_to_rvec_tvec(const data::matrix4::csptr _mat)
{
Eigen::Matrix4d eigen_mat = to_eigen<double>(_mat);
return eigen_mat_to_rvec_tvec(eigen_mat);
}
//-------------------------------------------------------------------------------------------------
EigenMatrix to_eigen(const std::array<float, 16>& _farray)
{
Eigen::Matrix<double, 4, 4, Eigen::RowMajor> mat;
for(unsigned int r = 0 ; r < 4 ; ++r)
{
for(unsigned int c = 0 ; c < 4 ; ++c)
{
mat(r, c) = static_cast<double>(_farray[std::size_t(4) * r + c]);
}
}
return mat;
}
//-------------------------------------------------------------------------------------------------
EigenMatrix to_eigen(const std::array<double, 16>& _farray)
{
Eigen::Matrix<double, 4, 4, Eigen::RowMajor> mat;
for(unsigned int r = 0 ; r < 4 ; ++r)
{
for(unsigned int c = 0 ; c < 4 ; ++c)
{
mat(r, c) = _farray[std::size_t(4) * r + c];
}
}
return mat;
}
//-------------------------------------------------------------------------------------------------
rvec_tvec_t float16_to_rvec_tvec(const std::array<float, 16>& _farray)
{
Eigen::Matrix4d eigen_mat = to_eigen(_farray);
return eigen_mat_to_rvec_tvec(eigen_mat);
}
//-------------------------------------------------------------------------------------------------
} // namespace sight::geometry::eigen::helper
|
