/// \brief Log: SO3 -> so3.
///
/// Pseudo-inverse of log from SO3 -> { v \in so3, ||v|| < 2pi }.
template <typename D> Eigen::Matrix<typename D::Scalar,3,1,D::Options>
log3(const Eigen::MatrixBase<D> & R)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D, 3, 3);
Eigen::AngleAxis<typename D::Scalar> angleAxis(R);
return angleAxis.axis() * angleAxis.angle();
}
void setRotFromQuaternion(const Eigen::MatrixBase<Derived>& quat, Eigen::MatrixBase<DerivedOther>& A)
{
typedef typename Derived::Scalar PREC;
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 4);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(DerivedOther, 3, 3);
// ASSERTMSG(quat.rows() == 4 && quat.cols()==1 && A.rows() == 3 && A.cols() == 3, "IN: "<<
// quat.rows()<<","<<quat.cols()<<"; OUT: "<<A.rows()<<","<<A.cols() );
// No check if quaternion is unit...(performance)
PREC fTx = 2.0 * quat(1);
PREC fTy = 2.0 * quat(2);
PREC fTz = 2.0 * quat(3);
PREC fTwx = fTx * quat(0);
PREC fTwy = fTy * quat(0);
PREC fTwz = fTz * quat(0);
PREC fTxx = fTx * quat(1);
PREC fTxy = fTy * quat(1);
PREC fTxz = fTz * quat(1);
PREC fTyy = fTy * quat(2);
PREC fTyz = fTz * quat(2);
PREC fTzz = fTz * quat(3);
A(0, 0) = 1.0f - (fTyy + fTzz);
A(0, 1) = fTxy - fTwz;
A(0, 2) = fTxz + fTwy;
A(1, 0) = fTxy + fTwz;
A(1, 1) = 1.0f - (fTxx + fTzz);
A(1, 2) = fTyz - fTwx;
A(2, 0) = fTxz - fTwy;
A(2, 1) = fTyz + fTwx;
A(2, 2) = 1.0f - (fTxx + fTyy);
}
inline bool Transformation::liftJacobian(const Eigen::MatrixBase<Derived_jacobian> & jacobian) const
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived_jacobian, 6, 7);
const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian).setZero();
const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian).template topLeftCorner<3,3>()
= Eigen::Matrix3d::Identity();
const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian).template bottomRightCorner<3,4>()
= 2*okvis::kinematics::oplus(q_.inverse()).template topLeftCorner<3,4>();
return true;
}
inline bool Transformation::oplus(
const Eigen::MatrixBase<Derived_delta> & delta,
const Eigen::MatrixBase<Derived_jacobian> & jacobian) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived_delta, 6);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived_jacobian, 7, 6);
if (!oplus(delta)) {
return false;
}
return oplusJacobian(jacobian);
}
/// \brief Log: SE3 -> se3.
///
/// Pseudo-inverse of exp from SE3 -> { v,w \in se3, ||w|| < 2pi }.
template <typename D> MotionTpl<typename D::Scalar,D::Options>
log6(const Eigen::MatrixBase<D> & M)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D, 4, 4);
typedef typename SE3Tpl<typename D::Scalar,D::Options>::Vector3 Vector3;
typedef typename SE3Tpl<typename D::Scalar,D::Options>::Matrix3 Matrix3;
Matrix3 rot(M.template block<3,3>(0,0));
Vector3 trans(M.template block<3,1>(0,3));
SE3Tpl<typename D::Scalar,D::Options> m(rot, trans);
return log6(m);
}
inline void addSkew(const Eigen::MatrixBase<Vector3Like> & v,
const Eigen::MatrixBase<Matrix3Like> & M)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like,3,3);
Matrix3Like & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,M);
M_(0,1) -= v[2]; M_(0,2) += v[1];
M_(1,0) += v[2]; M_(1,2) -= v[0];
M_(2,0) -= v[1]; M_(2,1) += v[0]; ;
}
void applyRandomRotTrans(MatrixBase<Derived> & points) {
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3, Eigen::Dynamic)
Quaternion q;
q.coeffs().setRandom();
q.normalize();
Matrix33 R = q.matrix();
Vector3 trans;
trans.setRandom();
points = R*points;
points.colwise() += trans;
}
void applyRandomRotTrans(MatrixBase<Derived> & points, Gen & g) {
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3, Eigen::Dynamic)
Quaternion q;
q.coeffs() = q.coeffs().unaryExpr(g); // TODO: Check if q=[0,0,0,0] is correctly normalized !! otherwise crash! NaN
q.normalize();
Matrix33 R = q.matrix();
Vector3 trans;
trans = trans.unaryExpr(g);
trans.unaryExpr(g);
points = R*points;
points.colwise() += trans;
}
inline void skew(const Eigen::MatrixBase<Vector3> & v,
const Eigen::MatrixBase<Matrix3> & M)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
Matrix3 & M_ = const_cast<Eigen::MatrixBase<Matrix3> &>(M).derived();
typedef typename Matrix3::RealScalar Scalar;
M_(0,0) = Scalar(0); M_(0,1) = -v[2]; M_(0,2) = v[1];
M_(1,0) = v[2]; M_(1,1) = Scalar(0); M_(1,2) = -v[0];
M_(2,0) = -v[1]; M_(2,1) = v[0]; M_(2,2) = Scalar(0);
}
inline bool Transformation::oplusJacobian(
const Eigen::MatrixBase<Derived_jacobian> & jacobian) const {
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived_jacobian, 7, 6);
Eigen::Matrix<double, 4, 3> S = Eigen::Matrix<double, 4, 3>::Zero();
const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian).setZero();
const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian)
.template topLeftCorner<3, 3>().setIdentity();
S(0, 0) = 0.5;
S(1, 1) = 0.5;
S(2, 2) = 0.5;
const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian)
.template bottomRightCorner<4, 3>() = okvis::kinematics::oplus(q_) * S;
return true;
}
inline void unSkew(const Eigen::MatrixBase<Matrix3> & M,
const Eigen::MatrixBase<Vector3> & v)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
assert((M + M.transpose()).isMuchSmallerThan(M));
Vector3 & v_ = const_cast<Eigen::MatrixBase<Vector3> &>(v).derived();
typedef typename Vector3::RealScalar Scalar;
v_[0] = Scalar(0.5) * (M(2,1) - M(1,2));
v_[1] = Scalar(0.5) * (M(0,2) - M(2,0));
v_[2] = Scalar(0.5) * (M(1,0) - M(0,1));
}
