inline Pose::Pose(const Quaterniond& orientation,
const Vector3d& position)
: m_orientation(orientation)
, m_position(position)
{
CHECK(std::fabs(orientation.norm() - 1) < 1e-15)
<< "Your quaternion is not normalized:"
<< orientation.norm();
// Or, one may prefer
m_orientation.normalize();
}
void SO3::
setQuaternion(const Quaterniond& quaternion)
{
assert(quaternion.norm()!=0);
unit_quaternion_ = quaternion;
unit_quaternion_.normalize();
}