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)); }