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 | #include "Rigid.h"
#define _USE_MATH_DEFINES
#include <math.h>
#include <iostream>
#define EIGEN_DONT_ALIGN_STATICALLY
#define EIGEN_NO_STATIC_ASSERT
using namespace Eigen;
Matrix4d Rigid::inverse(const Matrix4d &E)
{
Matrix4d Einv = Matrix4d::Identity();
Matrix3d R = E.block<3,3>(0,0);
Vector3d p = E.block<3,1>(0,3);
Matrix3d Rt = R.transpose();
Einv.block<3,3>(0,0) = Rt;
Einv.block<3,1>(0,3) = -Rt * p;
return Einv;
}
Matrix3x6d Rigid::gamma(const Eigen::Vector3d &r)
{
Matrix3x6d G = Matrix3x6d::Zero();
G.block<3,3>(0,0) = bracket3(r).transpose();
G.block<3,3>(0,3) = Matrix3d::Identity();
return G;
}
Matrix6d Rigid::adjoint(const Matrix4d &E)
{
Matrix6d Ad = Matrix6d::Zero();
Matrix3d R = E.block<3,3>(0,0);
Vector3d p = E.block<3,1>(0,3);
Ad.block(0,0,3,3) = R;
Ad.block(3,0,3,3) = bracket3(p) * R;
Ad.block(3,3,3,3) = R;
return Ad;
}
Matrix3d Rigid::bracket3(const Vector3d &a)
{
Matrix3d A = Matrix3d::Zero();
A(0,1) = -a(2);
A(0,2) = a(1);
A(1,0) = a(2);
A(1,2) = -a(0);
A(2,0) = -a(1);
A(2,1) = a(0);
return A;
}
Matrix4d Rigid::bracket6(const Vector6d &a)
{
Matrix4d A = Matrix4d::Zero();
A.block<3,3>(0,0) = bracket3(a.segment<3>(0));
A.block<3,1>(0,3) = a.segment<3>(3);
return A;
}
Vector3d Rigid::unbracket3(const Matrix3d &A)
{
Vector3d a;
a(0) = A(2,1);
a(1) = A(0,2);
a(2) = A(1,0);
return a;
}
Vector6d Rigid::unbracket6(const Matrix4d &A)
{
Vector6d a;
a.segment<3>(0) = unbracket3(A.block<3,3>(0,0));
a(3) = A(0,3);
a(4) = A(1,3);
a(5) = A(2,3);
return a;
}
Matrix4d Rigid::integrate(const Matrix4d &E0, const VectorXd &phi, double h)
{
Matrix3d I = Matrix3d::Identity();
Vector3d w = phi.segment<3>(0);
Vector3d v = phi.segment<3>(3);
Matrix4d phib = Matrix4d::Identity();
phib.block<3,1>(0,3) = h*v;
double wlen = w.norm();
if(wlen > 1e-10) {
w /= wlen;
v /= wlen;
// Rodrigues formula %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double wX = w(0);
double wY = w(1);
double wZ = w(2);
double c = cos(wlen * h);
double s = sin(wlen * h);
double c1 = 1.0 - c;
Matrix3d R;
R << c + wX * wX * c1, -wZ * s + wX * wY * c1, wY * s + wX * wZ * c1,
wZ * s + wX * wY * c1, c + wY * wY * c1, -wX * s + wY * wZ * c1,
-wY * s + wX * wZ * c1, wX * s + wY * wZ * c1, c + wZ * wZ * c1;
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Matrix3d A = I - R;
Vector3d cc = w.cross(v);
Vector3d d = A * cc;
double wv = w.dot(v);
Vector3d p = (wv * wlen * h) * w + d;
phib.block<3,3>(0,0) = R;
phib.block<3,1>(0,3) = p;
//cout << phib << endl;
}
return E0 * phib;
}
Vector6d Rigid::log(const Eigen::Matrix4d & A)
{
Matrix3d R = A.block<3,3>(0,0);
Vector3d p = A.block<3,1>(0,3);
Vector6d phi;
double cosTheta = 0.5*(R.trace() - 1);
double theta = acos(cosTheta);
if (cosTheta > 0.999999999999)
theta = 0;
else if (cosTheta < -0.999999999999)
theta = M_PI;
if (abs(theta) < 1e-8)
{
phi.segment<3>(0) << 0.0, 0.0, 0.0;
phi.segment<3>(3) = p;
}
else
{
double sinTheta = sin(theta);
Matrix3d wBracket = theta / (2 * sinTheta)*(R - R.transpose());
phi.segment<3>(0) = unbracket3(wBracket);
Matrix3d V = Matrix3d::Identity() + ((1 - cosTheta) / (theta * theta) * wBracket) + ((theta - sinTheta) / (theta * theta * theta) * wBracket*wBracket);
phi.segment<3>(3) = V.colPivHouseholderQr().solve(p);
}
return phi;
}
// #include <iostream>
// using namespace std;
// int main(int argc, char **argv)
// {
// Matrix4d E = Matrix4d::Identity();
// E << -0.1817, 0.7236, 0.6658, 2.7694,
// -0.6198, -0.6100, 0.4938, -1.3499,
// 0.7634, -0.3230, 0.5594, 3.0349,
// 0, 0, 0, 1.0000;
// cout << E << endl << endl;
// cout << Rigid::inverse(E) << endl << endl;
// Vector3d r;
// r << 0.7254, -0.0631, 0.7147;
// cout << Rigid::gamma(r) << endl << endl;
// cout << Rigid::adjoint(E) << endl << endl;
// Vector6d p;
// p << -0.2050, -0.1241, 1.4897, 1.4090, 1.4172, 0.6715;
// cout << Rigid::bracket6(p) << endl << endl;
// Vector3d ax;
// ax << -1.2075, 0.7172, 1.6302;
// double a = 0.4889;
// cout << Rigid::aaToMat(ax, a) << endl << endl;
// cout << Rigid::aaToMat(Vector3d(1, 0, 0), a) << endl << endl;
// cout << Rigid::aaToMat(Vector3d(0, 1, 0), a) << endl << endl;
// cout << Rigid::aaToMat(Vector3d(0, 0, 1), a) << endl << endl;
// }
|