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
// Copyright (c) 2012-2017 VideoStitch SAS
// Copyright (c) 2018 stitchEm
#ifndef __SO3_PARAMETERZATION_HPP__
#define __SO3_PARAMETERZATION_HPP__
#include <ceres/ceres.h>
#include <Eigen/Dense>
namespace VideoStitch {
namespace Calibration {
class SO3Parameterization : public ceres::LocalParameterization {
public:
virtual ~SO3Parameterization() {}
virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const {
double* ptrBase = (double*)x;
double* ptrResult = (double*)x_plus_delta;
Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rotation(ptrBase);
Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rotationResult(ptrResult);
Eigen::Vector3d axis;
axis(0) = delta[0];
axis(1) = delta[1];
axis(2) = delta[2];
double angle = axis.norm();
axis.normalize();
Eigen::AngleAxisd aa(angle, axis);
Eigen::Matrix3d Rupdate;
Rupdate = aa.toRotationMatrix();
rotationResult = Rupdate * rotation;
return true;
}
virtual bool ComputeJacobian(const double* /*x*/, double* jacobian) const {
double* row[9];
for (int i = 0; i < 9; i++) {
row[i] = &jacobian[i * 3];
for (int j = 0; j < 3; j++) {
row[i][j] = 0;
}
}
row[1][2] = 1;
row[2][1] = -1;
row[3][2] = -1;
row[5][0] = 1;
row[6][1] = 1;
row[7][0] = -1;
return true;
}
virtual int GlobalSize() const { return 9; }
virtual int LocalSize() const { return 3; }
};
} // namespace Calibration
} // namespace VideoStitch
#endif