Matrix6d adjoint_se3(Matrix4d T){
Matrix6d AdjT = Matrix6d::Zero();
Matrix3d R = T.block(0,0,3,3);
AdjT.block(0,0,3,3) = R;
AdjT.block(0,3,3,3) = skew( T.block(0,3,3,1) ) * R ;
AdjT.block(3,3,3,3) = R;
return AdjT;
}
Matrix4d inverse_se3(Matrix4d T){
Matrix4d Tinv = Matrix4d::Identity();
Matrix3d R;
Vector3d t;
t = T.block(0,3,3,1);
R = T.block(0,0,3,3);
Tinv.block(0,0,3,3) = R.transpose();
Tinv.block(0,3,3,1) = -R.transpose() * t;
return Tinv;
}
int main(void)
{
const int n =6; // 4I , 2 lambda
int info;
// system params
Coil coil;
Magnet magnet;
coil.d = .0575;
coil.R = 0.075;
magnet.x = 0.01;
magnet.y = 0.0;
magnet.Fx = 1 * pow(10,-8);
magnet.Fy = 1.* pow(10,-8);
magnet.gamma = 35.8 * pow(10,-6);
double th = -19 * PI/180; // convert angle in degrees to radians
double Bmag = 10. * pow(10,-6);
double Bx = Bmag * cos(th);
double By = Bmag * sin(th);
Vector2d mvec(Bx,By);
mvec = magnet.gamma/sqrt(Bx * Bx + By * By)*mvec;
cout << "mvec" << endl << mvec << endl;
MatrixXd Bmat = computeBmat(magnet.x,magnet.y,coil.R,coil.d);
cout << "Bmat" << endl << Bmat << endl;
//MatrixXd
Matrix4d A;
cout << "A" << endl << A << endl;
A.block(0,0,2,4) = Bmat;
cout << "A row 1-2" << endl << A << endl;
A.block(2,0,1,4) = mvec.transpose() * Dx(magnet.x,magnet.y,coil.R,coil.d);
A.block(3,0,1,4) = mvec.transpose() * Dy(magnet.x,magnet.y,coil.R,coil.d);
cout << "A " << endl << A << endl;
cout << "A inverse is:" << endl << A.inverse() << endl;
Vector4d BF;
BF << Bx,By,magnet.Fx,magnet.Fy;
cout << "BF: " << endl << BF << endl;
Vector4d Isolve = A.inverse() * BF;
cout << "Isolve: " << endl << Isolve << endl;
return 0;
}
Vector6d logmap_se3(Matrix4d T){
Matrix3d R, Id3 = Matrix3d::Identity();
Vector3d Vt, t, w;
Matrix3d V = Matrix3d::Identity(), w_hat = Matrix3d::Zero();
Vector6d x;
Vt << T(0,3), T(1,3), T(2,3);
w << 0.f, 0.f, 0.f;
R = T.block(0,0,3,3);
double cosine = (R.trace() - 1.f)/2.f;
if(cosine > 1.f)
cosine = 1.f;
else if (cosine < -1.f)
cosine = -1.f;
double sine = sqrt(1.0-cosine*cosine);
if(sine > 1.f)
sine = 1.f;
else if (sine < -1.f)
sine = -1.f;
double theta = acos(cosine);
if( theta > 0.000001 ){
w_hat = theta*(R-R.transpose())/(2.f*sine);
w = skewcoords(w_hat);
Matrix3d s;
s = skew(w) / theta;
V = Id3 + s * (1.f-cosine) / theta + s * s * (theta - sine) / theta;
}
t = V.inverse() * Vt;
x.head(3) = t;
x.tail(3) = w;
return x;
}
Matrix4d expmap_se3(Vector6d x){
Matrix3d R, V, s, I = Matrix3d::Identity();
Vector3d t, w;
Matrix4d T = Matrix4d::Identity();
w = x.tail(3);
t = x.head(3);
double theta = w.norm();
if( theta < 0.000001 )
R = I;
else{
s = skew(w)/theta;
R = I + s * sin(theta) + s * s * (1.0f-cos(theta));
V = I + s * (1.0f - cos(theta)) / theta + s * s * (theta - sin(theta)) / theta;
t = V * t;
}
T.block(0,0,3,4) << R, t;
return T;
}
MatrixXd der_logarithm_map(Matrix4d T)
{
MatrixXd dlogT_dT = MatrixXd::Zero(6,12);
// Approximate derivative of the logarithm_map wrt the transformation matrix
Matrix3d L1 = Matrix3d::Zero();
Matrix3d L2 = Matrix3d::Zero();
Matrix3d L3 = Matrix3d::Zero();
Matrix3d Vinv = Matrix3d::Identity();
Vector6d x = logmap_se3(T);
// estimates the cosine, sine, and theta
double b;
double cos_ = 0.5 * (T.block(0,0,3,3).trace() - 1.0 );
if(cos_ > 1.f)
cos_ = 1.f;
else if (cos_ < -1.f)
cos_ = -1.f;
double theta = acos(cos_);
double theta2 = theta*theta;
double sin_ = sin(theta);
double cot_ = 1.0 / tan( 0.5*theta );
double csc2_ = pow( 1.0/sin(0.5*theta) ,2);
// if the angle is small...
if( cos_ > 0.9999 )
{
b = 0.5;
L1(1,2) = -b;
L1(2,1) = b;
L2(0,2) = b;
L2(2,0) = -b;
L3(0,1) = -b;
L3(1,0) = b;
// form the full derivative
dlogT_dT.block(3,0,3,3) = L1;
dlogT_dT.block(3,3,3,3) = L2;
dlogT_dT.block(3,6,3,3) = L3;
dlogT_dT.block(0,9,3,3) = Vinv;
}
// if not...
else
{
// rotation part
double k;
Vector3d a;
a(0) = T(2,1) - T(1,2);
a(1) = T(0,2) - T(2,0);
a(2) = T(1,0) - T(0,1);
k = ( theta * cos_ - sin_ ) / ( 4 * pow(sin_,3) );
a = k * a;
L1.block(0,0,3,1) = a;
L2.block(0,1,3,1) = a;
L3.block(0,2,3,1) = a;
// translation part
Matrix3d w_skew = skew( x.tail(3) );
Vinv += w_skew * (1.f-cos_) / theta2 + w_skew * w_skew * (theta - sin_) / pow(theta,3);
Vinv = Vinv.inverse().eval();
// dVinv_dR
Vector3d t;
Matrix3d B, skew_t;
MatrixXd dVinv_dR(3,9);
t = T.block(0,3,3,1);
skew_t = skew( t );
// - form a
a = (theta*cos_-sin_)/(8.0*pow(sin_,3)) * w_skew * t
+ ( (theta*sin_-theta2*cos_)*(0.5*theta*cot_-1.0) - theta*sin_*(0.25*theta*cot_+0.125*theta2*csc2_-1.0))/(4.0*theta2*pow(sin_,4)) * w_skew * w_skew * t;
// - form B
Vector3d w;
Matrix3d dw_dR;
w = x.tail(3);
dw_dR.row(0) << -w(1)*t(1)-w(2)*t(2), 2.0*w(1)*t(0)-w(0)*t(1), 2.0*w(2)*t(0)-w(0)*t(2);
dw_dR.row(1) << -w(1)*t(0)+2.0*w(0)*t(1), -w(0)*t(0)-w(2)*t(2), 2.0*w(2)*t(1)-w(1)*t(2);
dw_dR.row(2) << -w(2)*t(0)+2.0*w(0)*t(2), -w(2)*t(1)+2.0*w(1)*t(2), -w(0)*t(0)-w(1)*t(1);
B = -0.5*theta*skew_t/sin_ - (theta*cot_-2.0)*dw_dR/(8.0*pow(sin_,2));
// - form dVinv_dR
dVinv_dR.col(0) = a;
dVinv_dR.col(1) = -B.col(2);
dVinv_dR.col(2) = B.col(1);
dVinv_dR.col(3) = B.col(2);
dVinv_dR.col(4) = a;
dVinv_dR.col(5) = -B.col(0);
dVinv_dR.col(6) = -B.col(1);
dVinv_dR.col(7) = B.col(0);
dVinv_dR.col(8) = a;
// form the full derivative
dlogT_dT.block(3,0,3,3) = L1;
dlogT_dT.block(3,3,3,3) = L2;
dlogT_dT.block(3,6,3,3) = L3;
dlogT_dT.block(0,9,3,3) = Vinv;
dlogT_dT.block(0,0,3,9) = dVinv_dR;
}
return dlogT_dT;
}
int main()
{
//特徴点の座標
double fpos[8][3]={ -1000, 0, 5000,
1000, 0, 5000,
0, 0, 7000,
0, 2000, 10000,
-1000, 2000, 10000,
0, 3000, 10000,
1000, 2000, 10000,
0, 1000, 10000};
//特徴点オブジェクト生成
feature feat[8];
//特徴点座標設定
for(int i=0;i<8;i++){
feat[i].setPos(fpos[i][0],fpos[i][1],fpos[i][2]);
}
//回転座標変換行列計算
Vector3d axisx;
axisx << 1,0,0;
Vector3d axisy;
axisy << 0,1,0;
Vector3d axisz;
axisz << 0,0,1;
Matrix3d rotx;
rotx = AngleAxisd(0.1,axisx);
Matrix3d roty;
roty = AngleAxisd(0.0,axisy);
Matrix3d rotz;
rotz = AngleAxisd(0.0,axisz);
Matrix3d rot = rotz*roty*rotx;
PRINT_MAT(rot);
double tx=0.0,ty=2000.0,tz=0.0;
Matrix4d T;
T<< 0,0,0,-tx,
0,0,0,-ty,
0,0,0,-tz,
0,0,0,1;
PRINT_MAT(T);
T.block(0,0,3,3)=rot;
PRINT_MAT(T);
double f=0.5,ox=320.0,oy=240.0,kx=10.0,ky=10.0;
Matrix<double,3,4> K;
K<< f*kx, 0, ox, 0,
0, f*ky, oy, 0,
0, 0, 1.0, 0;
PRINT_MAT(K);
Matrix<double,3,4> P;
P=K*T;
PRINT_MAT(P);
Vector4d fp;
Vector3d img;
fp<<0.0,2000.0,10000.0,1.0;
img=P*fp;
PRINT_MAT(img);
//画面上の座標表示
printf("%f,%f\n",img(0)/img(2),img(1)/img(2));
}
