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