blob: 9e38b92a94bd501a8ea4a945f103af5b29147d10 (
plain)
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
|
// -*- C++ -*-
/*
Copyright (c) 2008 University of North Carolina at Chapel Hill
This file is part of SSBA (Simple Sparse Bundle Adjustment).
SSBA 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.
SSBA 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 SSBA. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef V3D_MATH_UTILITIES_H
#define V3D_MATH_UTILITIES_H
#include "Math/v3d_linear.h"
#include "Math/v3d_linear_utils.h"
#include <vector>
namespace V3D
{
template <typename Vec, typename Mat>
inline void
createRotationMatrixRodriguez(Vec const& omega, Mat& R)
{
assert(omega.size() == 3);
assert(R.num_rows() == 3);
assert(R.num_cols() == 3);
double const theta = norm_L2(omega);
makeIdentityMatrix(R);
if (fabs(theta) > 1e-6)
{
Matrix3x3d J, J2;
makeCrossProductMatrix(omega, J);
multiply_A_B(J, J, J2);
double const c1 = sin(theta)/theta;
double const c2 = (1-cos(theta))/(theta*theta);
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j)
R[i][j] += c1*J[i][j] + c2*J2[i][j];
}
} // end createRotationMatrixRodriguez()
template <typename T> inline double sqr(T x) { return x*x; }
} // namespace V3D
#endif
|
