From a5ba14dd964ec3ee384cffa1fb42c67425a4b297 Mon Sep 17 00:00:00 2001 From: otim Date: Mon, 8 Jun 2015 10:36:01 +0200 Subject: [PATCH 1/3] correctly initialize orientation --- .../position_pose_msf/position_pose_sensormanager.h | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/msf_updates/src/position_pose_msf/position_pose_sensormanager.h b/msf_updates/src/position_pose_msf/position_pose_sensormanager.h index 92d3e317..e5710913 100644 --- a/msf_updates/src/position_pose_msf/position_pose_sensormanager.h +++ b/msf_updates/src/position_pose_msf/position_pose_sensormanager.h @@ -186,8 +186,15 @@ class PositionPoseSensorManager : public msf_core::MSF_SensorManagerROS< Eigen::Quaterniond yawq(cos(yawinit / 2), 0, 0, sin(yawinit / 2)); yawq.normalize(); - q = yawq; - q_wv = (q * q_ic * q_vc.conjugate()).conjugate(); + q_wv = yawq.conjugate(); + + if (q_vc.w() == 1) { // If there is no pose measurement, only apply q_wv. + q = q_wv; + } else { // If there is a pose measurement, apply q_ic and q_wv to get initial attitud + q = (q_ic * q_vc.conjugate() * q_wv).conjugate(); + } + + q.normalize(); MSF_WARN_STREAM("q " << STREAMQUAT(q)); MSF_WARN_STREAM("q_wv " << STREAMQUAT(q_wv)); From 2ac6c0b9fb87890c978e0a6aefea5325625e1dc5 Mon Sep 17 00:00:00 2001 From: otim Date: Mon, 8 Jun 2015 11:16:15 +0200 Subject: [PATCH 2/3] fixed typo in comment --- msf_updates/src/position_pose_msf/position_pose_sensormanager.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/msf_updates/src/position_pose_msf/position_pose_sensormanager.h b/msf_updates/src/position_pose_msf/position_pose_sensormanager.h index e5710913..ff8f6732 100644 --- a/msf_updates/src/position_pose_msf/position_pose_sensormanager.h +++ b/msf_updates/src/position_pose_msf/position_pose_sensormanager.h @@ -190,7 +190,7 @@ class PositionPoseSensorManager : public msf_core::MSF_SensorManagerROS< if (q_vc.w() == 1) { // If there is no pose measurement, only apply q_wv. q = q_wv; - } else { // If there is a pose measurement, apply q_ic and q_wv to get initial attitud + } else { // If there is a pose measurement, apply q_ic and q_wv to get initial attitude. q = (q_ic * q_vc.conjugate() * q_wv).conjugate(); } From 4bf4dad6ca67d77cabdf00400a55280a12a300ac Mon Sep 17 00:00:00 2001 From: otim Date: Mon, 8 Jun 2015 12:25:08 +0200 Subject: [PATCH 3/3] renamed yawq to q_wv_yaw --- .../src/position_pose_msf/position_pose_sensormanager.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/msf_updates/src/position_pose_msf/position_pose_sensormanager.h b/msf_updates/src/position_pose_msf/position_pose_sensormanager.h index ff8f6732..ad9e937e 100644 --- a/msf_updates/src/position_pose_msf/position_pose_sensormanager.h +++ b/msf_updates/src/position_pose_msf/position_pose_sensormanager.h @@ -183,10 +183,10 @@ class PositionPoseSensorManager : public msf_core::MSF_SensorManagerROS< // here we take the attitude from the pose sensor and augment it with // global yaw init. double yawinit = config_.position_yaw_init / 180 * M_PI; - Eigen::Quaterniond yawq(cos(yawinit / 2), 0, 0, sin(yawinit / 2)); - yawq.normalize(); + Eigen::Quaterniond q_wv_yaw(cos(yawinit / 2), 0, 0, sin(yawinit / 2)); + q_wv_yaw.normalize(); - q_wv = yawq.conjugate(); + q_wv = q_wv_yaw.conjugate(); if (q_vc.w() == 1) { // If there is no pose measurement, only apply q_wv. q = q_wv;