gtsam::Rot3::quaternion() vs gtsam::Rot3::toQuaternion()

[RyGuy101]

Hello, I encountered a bug here because gtsam::Rot3::quaternion() does not behave as expected (the w, x, y, z are in the wrong order). This is probably due to the specific version of GTSAM I'm using, as the behavior has not always been the same (see
borglab/gtsam#1219). It may be a good idea to switch to toQuaternion() which should always be correct.

Current code:

D-LIOM/src/cartographer/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc

Lines 1107 to 1111 in 82063e0

	Eigen::Quaterniond rot = Eigen::Quaterniond(
	prev_pose_.rotation().quaternion().w(),
	prev_pose_.rotation().quaternion().x(),
	prev_pose_.rotation().quaternion().y(),
	prev_pose_.rotation().quaternion().z());

Proposed change:

  Eigen::Quaterniond rot = Eigen::Quaterniond(
    prev_pose_.rotation().toQuaternion().w(),
    prev_pose_.rotation().toQuaternion().x(),
    prev_pose_.rotation().toQuaternion().y(),
    prev_pose_.rotation().toQuaternion().z());