Vector6d gc_pipi_plk( Vector4d pi1, Vector4d pi2){
Vector6d plk;
Matrix4d dp = pi1 * pi2.transpose() - pi2 * pi1.transpose();
plk << dp(0,3), dp(1,3), dp(2,3), - dp(1,2), dp(0,2), - dp(0,1);
return plk;
}
void testRotationConversionFunctions()
{
int ntests = 100;
default_random_engine generator;
// quat2axis, axis2quat
for (int i = 0; i < ntests; i++) {
Vector4d q = uniformlyRandomQuat(generator);
auto a = quat2axis(q);
auto q_back = axis2quat(a);
valuecheck(acos(abs(q.transpose() * q_back)), 0.0, 1e-6);
}
// quat2rotmat, rotmat2quat
for (int i = 0; i < ntests; i++) {
Vector4d q = uniformlyRandomQuat(generator);
Matrix3d R = quat2rotmat(q);
Vector4d q_back = rotmat2quat(R);
valuecheck(acos(abs(q.transpose() * q_back)), 0.0, 1e-6);
}
// quat2rpy, rpy2quat
for (int i = 0; i < ntests; i++) {
Vector4d q = uniformlyRandomQuat(generator);
Vector3d rpy = quat2rpy(q);
Vector4d q_back = rpy2quat(rpy);
valuecheck(acos(abs(q.transpose() * q_back)), 0.0, 1e-6);
}
// rotmat2axis, axis2rotmat
for (int i = 0; i < ntests; i++) {
Matrix3d R = uniformlyRandomRotmat(generator);
Vector4d a = rotmat2axis(R);
Matrix3d R_back = axis2rotmat(a);
valuecheck(R, R_back, 1e-6);
}
// rotmat2rpy, rpy2rotmat
for (int i = 0; i < ntests; i++) {
Matrix3d R = uniformlyRandomRotmat(generator);
Vector3d rpy = rotmat2rpy(R);
Matrix3d R_back = rpy2rotmat(rpy);
valuecheck(R, R_back, 1e-6);
}
// rpy2axis, axis2rpy
for (int i = 0; i < ntests; i++) {
Vector3d rpy = uniformlyRandomRPY(generator);
Vector4d axis = rpy2axis(rpy);
Vector3d rpy_back = axis2rpy(axis);
valuecheck(rpy, rpy_back, 1e-6);
}
}
ClothoidPtr ClothoidFitter::getCurveWithZeroCurvature(double param) const
{
Matrix<double, 5, 5> lhs;
Matrix<double, 5, 1> rhs;
Vector4d constraint;
constraint << 6 * param, 2, 0, 0; // second derivative of ax^3+bx^2+cx+d is 6ax+2b
//For constrained least squares,
//lhs is now [A^T A C]
// [ C^T 0]
lhs << _getLhs(_totalLength), constraint,
constraint.transpose(), 0;
rhs << _rhs, 0;
Vector4d abcd = (lhs.inverse() * rhs).head<4>();
return getClothoidWithParams(abcd);
}
bool scso3bracket_tests()
{
bool failed = false;
vector<Vector4d> vecs;
Vector4d tmp;
tmp << 0,0,0,0;
vecs.push_back(tmp);
tmp << 1,0,0,0;
vecs.push_back(tmp);
tmp << 1,0,0,0.1;
vecs.push_back(tmp);
tmp << 0,1,0,0.1;
vecs.push_back(tmp);
tmp << 0,0,1,-0.1;
vecs.push_back(tmp);
tmp << -1,1,0,-0.1;
vecs.push_back(tmp);
tmp << 20,-1,0,2;
vecs.push_back(tmp);
for (unsigned int i=0; i<vecs.size(); ++i)
{
Vector4d resDiff = vecs[i] - ScSO3::vee(ScSO3::hat(vecs[i]));
if (resDiff.norm()>SMALL_EPS)
{
cerr << "Hat-vee Test" << endl;
cerr << "Test case: " << i << endl;
cerr << resDiff.transpose() << endl;
cerr << endl;
}
for (unsigned int j=0; j<vecs.size(); ++j)
{
Vector4d res1 = ScSO3::lieBracket(vecs[i],vecs[j]);
Matrix3d hati = ScSO3::hat(vecs[i]);
Matrix3d hatj = ScSO3::hat(vecs[j]);
Vector4d res2 = ScSO3::vee(hati*hatj-hatj*hati);
Vector4d resDiff = res1-res2;
if (resDiff.norm()>SMALL_EPS)
{
cerr << "ScSO3 Lie Bracket Test" << endl;
cerr << "Test case: " << i << ", " <<j<< endl;
cerr << vecs[i].transpose() << endl;
cerr << vecs[j].transpose() << endl;
cerr << resDiff.transpose() << endl;
cerr << endl;
failed = true;
}
}
Vector4d omega = vecs[i];
Matrix3d exp_x = ScSO3::exp(omega).matrix();
Matrix3d expmap_hat_x = (ScSO3::hat(omega)).exp();
Matrix3d DiffR = exp_x-expmap_hat_x;
double nrm = DiffR.norm();
if (isnan(nrm) || nrm>SMALL_EPS)
{
cerr << "expmap(hat(x)) - exp(x)" << endl;
cerr << "Test case: " << i << endl;
cerr << exp_x <<endl;
cerr << expmap_hat_x <<endl;
cerr << DiffR <<endl;
cerr << endl;
failed = true;
}
}
return failed;
}
