本文整理汇总了C++中Vector6d::head方法的典型用法代码示例。如果您正苦于以下问题:C++ Vector6d::head方法的具体用法?C++ Vector6d::head怎么用?C++ Vector6d::head使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Vector6d
的用法示例。
在下文中一共展示了Vector6d::head方法的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: if
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;
}
示例2: gc_asd_to_av
Vector6d gc_asd_to_av(Vector4d asd) {
Vector6d av;
Vector3d aa = asd.head(3);
// double d_inv = asd(3);
// double sig_d_inv = (1.0 - exp(-asd(3))) / (2.0 * (1.0 + exp(-asd(3))));
// double sig_d_inv = -log(1.0/asd(3) - 1.0);
// double sig_d_inv = log( (2.0 * asd(3) + 1.0) / (1.0 - 2.0*asd(3)) );
// double sig_d_inv = atan(asd(3)) / 2.0;
// double sig_d_inv = atan2(asd(3), 1.0) / 2.0;
// double sig_d_inv = atan2(asd(3), 1.0);
// double sig_d_inv = atan2(asd(3), 1.0) * 1.0;
// double sig_d_inv = tan(4.0 * asd(3));
double sig_d_inv = log(asd(3));
// cout << "sig_d_inv = " << sig_d_inv << endl;
// double sig_d_inv = cos(asd(3)) / sin(asd(3));
// double sig_d_inv = sin(asd(3)) / cos(asd(3));
// double sig_d_inv = sin(asd(3)) / cos(asd(3));
Matrix3d R = gc_Rodriguez(aa);
// av.head(3) = R.col(2) / sig_d_inv;
av.head(3) = R.col(2) * sig_d_inv;
av.tail(3) = R.col(0);
return av;
}
示例3: gc_aid_to_av
Vector6d gc_aid_to_av(Vector4d aid) {
Vector6d av;
Vector3d aa = aid.head(3);
double d = 1.0 / aid(3);
Matrix3d R = gc_Rodriguez(aa);
av.head(3) = R.col(2) * d;
av.tail(3) = R.col(0);
// Vector6d av;
// double a = aid[0];
// double b = aid[1];
// double g = aid[2];
// double t = aid[3];
//
// double s1 = sin(a);
// double c1 = cos(a);
// double s2 = sin(b);
// double c2 = cos(b);
// double s3 = sin(g);
// double c3 = cos(g);
//
// Matrix3d R;
// R <<
// c2 * c3, s1 * s2 * c3 - c1 * s3, c1 * s2 * c3 + s1 * s3,
// c2 * s3, s1 * s2 * s3 + c1 * c3, c1 * s2 * s3 - s1 * c3,
// -s2, s1 * c2, c1 * c2;
//
// double d = 1.0 / t;
// av.head(3) = -R.col(2) * d;
// av.tail(3) = R.col(1);
return av;
}
示例4: gc_Rt_to_wt
Vector6d gc_Rt_to_wt( pose_t Rt ) {
Vector6d wt;
wt.head(3) = gc_Rodriguez( Rt.R );
wt.tail(3) = Rt.t;
return wt;
}
示例5: apply
static Vector6d apply(const Matrix4d &T, const Vector6d &q, double w) {
Vector6d q_t;
q_t << q.head(3), w, 0, 0;
q_t.head(4) = T * q_t.head(4);
q_t.tail(3) = q.tail(3);
return q_t;
}
示例6: gc_av_to_orth
Vector4d gc_av_to_orth(Vector6d av) {
Vector4d orth;
Vector3d a = av.head(3);
Vector3d v = av.tail(3); // v
Vector3d n = a.cross(v); // n
Vector3d x = n / n.norm();
Vector3d y = v / v.norm();
Vector3d z = x.cross(y);
orth[0] = atan2( y(2), z(2) );
orth[1] = asin( - x(2) );
orth[2] = atan2( x(1), x(0) );
Vector2d w( n.norm(), v.norm() );
w = w / w.norm();
orth[3] = asin( w(1) );
// MatrixXd A(3,2), Q, R;
//
// A.col(0) = n;
// A.col(1) = v;
//
// Eigen::FullPivHouseholderQR<MatrixXd> qr(A);
// // Q = qr.householderQ();
// Q = qr.matrixQ();
// R = qr.matrixQR().triangularView<Upper>();
// // std::cout << Q << "\n\n" << R << "\n\n" << Q * R - A << "\n";
//
// // double sigma1 = R(0,0);
// // double sigma2 = R(1,1);
//
// // cout << "\ntheta from sigma1 = " << acos(sigma1) << endl;
// // cout << "theta from sigma2 = " << asin(sigma2) << endl;
//
// // cout << "\nsigma1 = " << sigma1<< endl;
// // cout << "sigma2 = " << sigma2<< endl;
//
// // sigma2 /= sqrt(sigma1*sigma1 + sigma2*sigma2);
//
// Vector3d x = Q.col(0);
// Vector3d y = Q.col(1);
// Vector3d z = Q.col(2);
//
// orth[0] = atan2( y(2), z(2) );
// orth[1] = asin( - x(2) );
// orth[2] = atan2( x(1), x(0) );
// // orth[3] = asin(sigma2);
//
// Vector2d w( n.norm(), v.norm() );
// w = w / w.norm();
// orth[3] = asin( w(1) );
return orth;
}
示例7: gc_av_to_asd
Vector4d gc_av_to_asd(Vector6d av) {
Vector4d asd;
Vector3d a = av.head(3);
Vector3d x = av.tail(3); // v
Vector3d y = a.cross(x); // n
// Vector2d w(y.norm(), x.norm());
// w /= w.norm();
// asd(3) = asin(w(1));
double depth = x.norm() / y.norm();
// double sig_d = log( (2.0*depth + 1.0) / (1.0 - 2.0*depth));
// double quotient = depth / 0.5;
// int integer_quotient = (int)quotient;
// double floating_quotient = quotient - (double)integer_quotient;
// depth = depth * floating_quotient;
// double sig_d = log(2.0*depth + 1.0) - log(1.0 - 2.0*depth);
// double sig_d = atan(1.0 / (1.0*depth) );
// double sig_d = atan(depth);
// double sig_d = atan2(1.0, depth);
// double sig_d = atan(depth);
// double sig_d = atan2(1.0, depth) / 4.0;
double sig_d = 1.0 / exp(-depth);
asd(3) = sig_d;
// cout << "sig_d = " << sig_d << endl;
// asd(3) = depth;
// double sig_d = tan(1.0/depth);
// double sig_d = tan(2.0/depth);
// double sig_d = tan(2.0*depth);
// double sig_d = (1.0 - exp(-1.0/depth)) / (2.0*(1.0 + exp(-1.0/depth)));
// double sig_d = (1.0 - exp(-depth)) / (2.0*(1.0 + exp(-depth)));
// double sig_d = 1.0 / (1.0 + exp(-1.0/depth));
// double sig_d = 1.0 / (1.0 + exp(-depth));
x /= x.norm();
y /= y.norm();
Vector3d z = x.cross(y);
Matrix3d R;
R.col(0) = x;
R.col(1) = y;
R.col(2) = z;
Vector3d aa = gc_Rodriguez(R);
asd(0) = aa(0);
asd(1) = aa(1);
asd(2) = aa(2);
return asd;
}
示例8:
pair<Matrix4d, Matrix4d> C3Trajectory::transformation_pair(const Vector6d &q) {
Matrix4d R;
R.block<3,3>(0, 0) = AttitudeHelpers::EulerToRotation(q.tail(3));
R.block<1,3>(3, 0).fill(0);
R.block<3,1>(0, 3).fill(0);
R(3, 3) = 1;
Matrix4d T = Matrix4d::Identity();
T.block<3,1>(0, 3) = -q.head(3);
pair<Matrix4d, Matrix4d> result;
result.first = R.transpose()*T; // NED -> BODY
T.block<3,1>(0, 3) = q.head(3);
result.second = T*R; // BODY -> NED
return result;
}
示例9: gc_plk_to_pose
Vector6d gc_plk_to_pose( Vector6d plk_w, pose_t T ) {
Vector3d nw = plk_w.head(3);
Vector3d vw = plk_w.tail(3);
Vector3d nc = T.R * nw + gc_skew_symmetric(T.t) * T.R * vw;
Vector3d vc = T.R * vw;
Vector6d plk_c;
plk_c.head(3) = nc;
plk_c.tail(3) = vc;
return plk_c;
}
示例10: gc_line_to_pose
Vector6d gc_line_to_pose(Vector6d line_w, pose_t T) {
Vector6d line_c;
Vector3d cp0, dv0;
cp0 = line_w.head(3);
dv0 = line_w.tail(3);
Vector3d cp1 = gc_point_to_pose( T, cp0 );
Vector3d dv1 = T.R * dv0;
line_c.head(3) = cp1;
line_c.tail(3) = dv1;
return line_c;
}
示例11: skew
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;
}
示例12: gc_orth_to_av
Vector6d gc_orth_to_av(Vector4d auth) {
Vector6d av;
double a = auth[0];
double b = auth[1];
double g = auth[2];
double t = auth[3];
double s1 = sin(a);
double c1 = cos(a);
double s2 = sin(b);
double c2 = cos(b);
double s3 = sin(g);
double c3 = cos(g);
Matrix3d R;
R <<
c2 * c3, s1 * s2 * c3 - c1 * s3, c1 * s2 * c3 + s1 * s3,
c2 * s3, s1 * s2 * s3 + c1 * c3, c1 * s2 * s3 - s1 * c3,
-s2, s1 * c2, c1 * c2;
double d = cos(t) / sin(t);
av.head(3) = -R.col(2) * d;
av.tail(3) = R.col(1);
// Vector3d u1 = R.col(0);
// Vector3d u2 = R.col(1);
//
// double sigma1 = cos(t);
// double sigma2 = sin(t);
//
// double d = cos(t) / sin(t);
//
// // Vector3d n = sigma1 * u1;
// // Vector3d v = sigma2 * u2;
// Vector3d n = u1;
// Vector3d v = u2;
// av.head(3) = -v.cross(n) * d;
// av.tail(3) = v;
return av;
}
示例13: gc_av_to_aid
Vector4d gc_av_to_aid(Vector6d av) {
Vector4d aid;
Vector3d a = av.head(3);
Vector3d x = av.tail(3); // v
Vector3d y = a.cross(x); // n
aid(3) = x.norm() / y.norm();
x /= x.norm();
y /= y.norm();
Vector3d z = x.cross(y);
Matrix3d R;
R.col(0) = x;
R.col(1) = y;
R.col(2) = z;
Vector3d aa = gc_Rodriguez(R);
aid(0) = aa(0);
aid(1) = aa(1);
aid(2) = aa(2);
// Vector4d aid;
// Vector3d a = av.head(3);
// Vector3d v = av.tail(3); // v
// Vector3d n = a.cross(v); // n
//
// aid(3) = v.norm() / n.norm();
//
// Vector3d x = n / n.norm();
// Vector3d y = v / v.norm();
// Vector3d z = x.cross(y);
//
// aid[0] = atan2( y(2), z(2) );
// aid[1] = asin( - x(2) );
// aid[2] = atan2( x(1), x(0) );
return aid;
}
示例14: gc_wt_to_Rt
pose_t gc_wt_to_Rt( Vector6d wt ) {
Vector3d w = wt.head(3);
return pose_t( gc_Rodriguez( w ), wt.tail(3) );
}
示例15:
Vector6d reverse_se3(Vector6d x){
Vector6d x_out;
x_out.head(3) = x.tail(3);
x_out.tail(3) = x.head(3);
return x_out;
}