From cd451b396f90d35aa9811d6a1503096f64af244c Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@student.kit.edu>
Date: Fri, 1 Mar 2019 09:50:25 +0100
Subject: [PATCH] Ensure Pose() constructs an Identity pose
---
source/RobotAPI/libraries/core/Pose.cpp | 25 ++++++++++---------------
source/RobotAPI/libraries/core/Pose.h | 2 ++
2 files changed, 12 insertions(+), 15 deletions(-)
diff --git a/source/RobotAPI/libraries/core/Pose.cpp b/source/RobotAPI/libraries/core/Pose.cpp
index f0ef6e15b..f1370001d 100644
--- a/source/RobotAPI/libraries/core/Pose.cpp
+++ b/source/RobotAPI/libraries/core/Pose.cpp
@@ -173,7 +173,7 @@ namespace armarx
}
- Quaternion::Quaternion()
+ Quaternion::Quaternion() : Quaternion(Eigen::Quaternionf::Identity())
{
}
@@ -203,13 +203,7 @@ namespace armarx
Matrix3f Quaternion::toEigen() const
{
- Matrix3f rot;
- rot = Quaternionf(
- this->qw,
- this->qx,
- this->qy,
- this->qz);
- return rot;
+ return toEigenQuaternion().toRotationMatrix();
}
Eigen::Quaternionf Quaternion::toEigenQuaternion() const
@@ -274,6 +268,14 @@ namespace armarx
qw = obj->getFloat("qw");
}
+
+ Pose::Pose()
+ {
+ this->orientation = new Quaternion();
+ this->position = new Vector3();
+ init();
+ }
+
Pose::Pose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v)
{
this->orientation = new Quaternion(m);
@@ -288,13 +290,6 @@ namespace armarx
init();
}
- Pose::Pose()
- {
- this->orientation = new Quaternion();
- this->position = new Vector3();
- init();
- }
-
Pose::Pose(const Pose& source) :
IceUtil::Shared(source),
PoseBase(source)
diff --git a/source/RobotAPI/libraries/core/Pose.h b/source/RobotAPI/libraries/core/Pose.h
index 1573d2da6..74be8b0fa 100644
--- a/source/RobotAPI/libraries/core/Pose.h
+++ b/source/RobotAPI/libraries/core/Pose.h
@@ -160,6 +160,8 @@ namespace armarx
virtual public QuaternionBase
{
public:
+
+ /// Construct an identity quaternion.
Quaternion();
Quaternion(const Eigen::Matrix4f&);
Quaternion(const Eigen::Matrix3f&);
--
GitLab