From b7fc31db8f25f4fb040d9009913458bab8481264 Mon Sep 17 00:00:00 2001
From: acoromin <acoromin@224674b8-e365-4e73-a4a8-558dbbfec58c>
Date: Wed, 27 May 2015 16:02:33 +0000
Subject: [PATCH] Adding quaternion class, adapting sensor_* to allow 3D
rotations and translations in the mounting point, bringing up some common
orientation functions to StateOrientation, adding constants, ...
---
src/CMakeLists.txt | 2 +
src/capture_laser_2D.cpp | 16 ++---
src/capture_odom_2D.cpp | 2 -
src/examples/CMakeLists.txt | 5 ++
src/examples/test_ceres_2_lasers.cpp | 14 ++--
src/examples/test_eigen_quaternion.cpp | 32 +++++++++
src/examples/test_state_quaternion.cpp | 47 +++++++++++++
src/sensor_base.cpp | 6 +-
src/sensor_base.h | 8 +--
src/sensor_gps_fix.cpp | 2 +-
src/sensor_gps_fix.h | 2 +-
src/sensor_laser_2D.cpp | 2 +-
src/sensor_laser_2D.h | 2 +-
src/sensor_odom_2D.cpp | 2 +-
src/sensor_odom_2D.h | 2 +-
src/state_complex_angle.cpp | 14 ++--
src/state_complex_angle.h | 3 +-
src/state_orientation.cpp | 7 ++
src/state_orientation.h | 3 +-
src/state_quaternion.cpp | 84 ++++++++++++++++++++++
src/state_quaternion.h | 96 ++++++++++++++++++++++++++
src/state_theta.cpp | 20 ++----
src/state_theta.h | 4 +-
src/wolf.h | 1 +
src/wolf_manager.h | 3 +-
25 files changed, 324 insertions(+), 55 deletions(-)
create mode 100644 src/examples/test_eigen_quaternion.cpp
create mode 100644 src/examples/test_state_quaternion.cpp
create mode 100644 src/state_quaternion.cpp
create mode 100644 src/state_quaternion.h
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index f1f4bed05..75e70ac07 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -66,6 +66,7 @@ SET(HDRS
state_base.h
state_point.h
state_orientation.h
+ state_quaternion.h
state_theta.h
state_complex_angle.h
time_stamp.h
@@ -103,6 +104,7 @@ SET(SRCS
sensor_gps_fix.cpp
state_base.cpp
state_orientation.cpp
+ state_quaternion.cpp
state_theta.cpp
state_complex_angle.cpp
time_stamp.cpp
diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp
index 8c5749ae4..c07a5179a 100644
--- a/src/capture_laser_2D.cpp
+++ b/src/capture_laser_2D.cpp
@@ -111,7 +111,7 @@ void CaptureLaser2D::establishConstraints()
WolfScalar& robot_orientation = *(getFramePtr()->getOPtr()->getPtr());
// Sensor transformation
- Eigen::Vector2s t_sensor = getSensorPtr()->getPPtr()->getVector();
+ Eigen::Vector2s t_sensor = getSensorPtr()->getPPtr()->getVector().head(2);
Eigen::Matrix2s R_sensor = getSensorPtr()->getOPtr()->getRotationMatrix().topLeftCorner<2, 2>();
//Brute force closest (xy and theta) landmark search //TODO: B&B
@@ -223,7 +223,7 @@ void CaptureLaser2D::establishConstraints()
void CaptureLaser2D::establishConstraintsMHTree()
{
//local declarations
- WolfScalar prob, dm, apert_diff;
+ WolfScalar prob, dm2, apert_diff;
unsigned int ii, jj, kk;
std::vector<std::pair<unsigned int, unsigned int> > ft_lk_pairs;
std::vector<bool> associated_mask;
@@ -237,9 +237,9 @@ void CaptureLaser2D::establishConstraintsMHTree()
WolfScalar& robot_orientation = *(getFramePtr()->getOPtr()->getPtr());
// Sensor transformation
- Eigen::Vector2s t_sensor = getSensorPtr()->getPPtr()->getVector();
+ Eigen::Vector2s t_sensor = getSensorPtr()->getPPtr()->getVector().head(2);
Eigen::Matrix2s R_sensor = getSensorPtr()->getOPtr()->getRotationMatrix().topLeftCorner<2,2>();
-
+
//tree object allocation and sizing
AssociationTree tree;
tree.resize( getFeatureListPtr()->size() , getTop()->getMapPtr()->getLandmarkListPtr()->size() );
@@ -255,8 +255,8 @@ void CaptureLaser2D::establishConstraintsMHTree()
apert_diff = fabs( (*i_it)->getMeasurement(3) - (*j_it)->getDescriptor(0) );
if (apert_diff < MAX_ACCEPTED_APERTURE_DIFF)
{
- dm = sqrt(computeMahalanobisDistance(*i_it, *j_it));
- if (dm<5) prob = erfc(dm/1.4142136);// sqrt(2) = 1.4142136
+ dm2 = computeMahalanobisDistance(*i_it, *j_it);//Mahalanobis squared
+ if (dm2 < 5*5) prob = erfc( sqrt(dm2)/1.4142136 );// sqrt(2) = 1.4142136
else prob = 0;
tree.setScore(ii,jj,prob);
}
@@ -267,7 +267,7 @@ void CaptureLaser2D::establishConstraintsMHTree()
// Grows tree and make association pairs
tree.solve(ft_lk_pairs,associated_mask);
-
+
//print tree & score table
// std::cout << "-------------" << std::endl;
// tree.printTree();
@@ -347,7 +347,7 @@ WolfScalar CaptureLaser2D::computeMahalanobisDistance(const FeatureBase* _featur
Eigen::Vector2s p_robot = getFramePtr()->getPPtr()->getVector();
Eigen::Matrix2s R_robot = ((StateOrientation*) (getFramePtr()->getOPtr()))->getRotationMatrix().topLeftCorner<2,2>();
- Eigen::Vector2s p_sensor = getSensorPtr()->getPPtr()->getVector();
+ Eigen::Vector2s p_sensor = getSensorPtr()->getPPtr()->getVector().head(2);
Eigen::Matrix2s R_sensor = getSensorPtr()->getOPtr()->getRotationMatrix().topLeftCorner<2, 2>();
Eigen::Vector2s p_landmark = _landmark_ptr->getPPtr()->getVector();
diff --git a/src/capture_odom_2D.cpp b/src/capture_odom_2D.cpp
index d4d914a26..8da893452 100644
--- a/src/capture_odom_2D.cpp
+++ b/src/capture_odom_2D.cpp
@@ -29,11 +29,9 @@ CaptureOdom2D::~CaptureOdom2D()
inline void CaptureOdom2D::processCapture()
{
- //std::cout << "CaptureOdom2D::addFeature..." << std::endl;
// ADD FEATURE
addFeature(new FeatureOdom2D(data_, data_covariance_));
- //std::cout << "CaptureOdom2D::addConstraints..." << std::endl;
// ADD CONSTRAINT
addConstraints();
}
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index d9a608578..bb321e759 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -62,3 +62,8 @@ IF(faramotics_FOUND)
${PROJECT_NAME})
ENDIF (laser_scan_utils_FOUND)
ENDIF(faramotics_FOUND)
+
+# Testing a eigen quaternion
+ADD_EXECUTABLE(test_state_quaternion test_state_quaternion.cpp)
+TARGET_LINK_LIBRARIES(test_state_quaternion ${PROJECT_NAME})
+
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index 0d4c80f34..4acd945f3 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -165,13 +165,13 @@ int main(int argc, char** argv)
// Wolf manager initialization
Eigen::Vector3s odom_pose = Eigen::Vector3s::Zero();
Eigen::Vector3s gps_pose = Eigen::Vector3s::Zero();
- Eigen::Vector3s laser_1_pose, laser_2_pose;
- laser_1_pose << 1.2, 0, 0; //laser 1
- laser_2_pose << -1.2, 0, M_PI; //laser 2
- SensorOdom2D odom_sensor(new StatePoint2D(odom_pose.data()), new StateTheta(&odom_pose(2)), odom_std_factor, odom_std_factor);
- SensorGPSFix gps_sensor(new StatePoint2D(gps_pose.data()), new StateTheta(&gps_pose(2)), gps_std);
- SensorLaser2D laser_1_sensor(new StatePoint2D(laser_1_pose.data()), new StateTheta(&laser_1_pose(2)));
- SensorLaser2D laser_2_sensor(new StatePoint2D(laser_2_pose.data()), new StateTheta(&laser_2_pose(2)));
+ Eigen::Vector4s laser_1_pose, laser_2_pose; //xyz + theta
+ laser_1_pose << 1.2, 0, 0, 0; //laser 1
+ laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
+ SensorOdom2D odom_sensor(new StatePoint3D(odom_pose.data()), new StateTheta(&odom_pose(3)), odom_std_factor, odom_std_factor);
+ SensorGPSFix gps_sensor(new StatePoint3D(gps_pose.data()), new StateTheta(&gps_pose(3)), gps_std);
+ SensorLaser2D laser_1_sensor(new StatePoint3D(laser_1_pose.data()), new StateTheta(&laser_1_pose(3)));
+ SensorLaser2D laser_2_sensor(new StatePoint3D(laser_2_pose.data()), new StateTheta(&laser_2_pose(3)));
// Initial pose
pose_odom << 2, 8, 0;
diff --git a/src/examples/test_eigen_quaternion.cpp b/src/examples/test_eigen_quaternion.cpp
new file mode 100644
index 000000000..0a696a82c
--- /dev/null
+++ b/src/examples/test_eigen_quaternion.cpp
@@ -0,0 +1,32 @@
+
+//std
+#include <iostream>
+
+//Eigen
+#include <eigen3/Eigen/Geometry>
+
+//Wolf
+#include "wolf.h"
+
+int main()
+{
+ std::cout << std::endl << "Eigen Quatenrnion test" << std::endl;
+
+ WolfScalar q1[4];
+ Eigen::Map<Eigen::Quaternions> q1_map(q1);
+
+ //try to find out how eigen sorts storage (real part tail or head ? )
+ q1_map.w() = 1;
+ q1_map.x() = 2;
+ q1_map.y() = 3;
+ q1_map.z() = 4;
+ std::cout << "q1[0]=" << q1[0] << "; q1_map.x()=" << q1_map.x() << std::endl;
+ std::cout << "q1[1]=" << q1[1] << "; q1_map.y()=" << q1_map.y() << std::endl;
+ std::cout << "q1[2]=" << q1[2] << "; q1_map.z()=" << q1_map.z() << std::endl;
+ std::cout << "q1[3]=" << q1[3] << "; q1_map.w()=" << q1_map.w() << std::endl;
+ std::cout << std::endl << "RESULT: Eigen stores REAL part in the LAST memory position of the quaternion." << std::endl;
+
+ std::cout << std::endl << "End of Eigen Quatenrnion test" << std::endl;
+ return 0;
+}
+
diff --git a/src/examples/test_state_quaternion.cpp b/src/examples/test_state_quaternion.cpp
new file mode 100644
index 000000000..be5ac860a
--- /dev/null
+++ b/src/examples/test_state_quaternion.cpp
@@ -0,0 +1,47 @@
+
+//std
+#include <iostream>
+
+//Eigen
+#include <eigen3/Eigen/Geometry>
+
+//Wolf
+#include "wolf.h"
+#include "state_quaternion.h"
+
+int main()
+{
+ std::cout << std::endl << "Eigen Quatenrnion test" << std::endl;
+
+ WolfScalar q1[4];
+ Eigen::Map<Eigen::Quaternions> q1_map(q1);
+
+ //try to find out how eigen sorts storage (real part tail or head ? )
+ std::cout << std::endl << "************************** TEST #1 ***************************" << std::endl;
+ q1_map.w() = -1;
+ q1_map.x() = 2;
+ q1_map.y() = 5;
+ q1_map.z() = 9;
+ std::cout << "q1[0]=" << q1[0] << "; q1_map.x()=" << q1_map.x() << std::endl;
+ std::cout << "q1[1]=" << q1[1] << "; q1_map.y()=" << q1_map.y() << std::endl;
+ std::cout << "q1[2]=" << q1[2] << "; q1_map.z()=" << q1_map.z() << std::endl;
+ std::cout << "q1[3]=" << q1[3] << "; q1_map.w()=" << q1_map.w() << std::endl;
+ std::cout << std::endl << "RESULT: Eigen stores REAL part in the LAST memory position of the quaternion." << std::endl;
+
+ //rot matrix
+ std::cout << std::endl << "************************** TEST #2 ***************************" << std::endl;
+ StateQuaternion sq(q1);
+ Eigen::Matrix3s RM;
+ sq.normalize();
+ std::cout << "Rot matrix through StateQuaternion class" << std::endl;
+ RM = sq.getRotationMatrix();
+ std::cout << RM << std::endl;
+
+ std::cout << "Rot matrix through Eigen::Map class" << std::endl;
+ RM = q1_map.toRotationMatrix();
+ std::cout << RM << std::endl;
+
+ std::cout << std::endl << "End of Eigen Quatenrnion tests" << std::endl;
+ return 0;
+}
+
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index 221db8e06..63beea954 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -1,6 +1,6 @@
#include "sensor_base.h"
-SensorBase::SensorBase(const SensorType & _tp, StateBase* _p_ptr, StateOrientation* _o_ptr, const Eigen::VectorXs & _params) :
+SensorBase::SensorBase(const SensorType & _tp, StatePoint3D* _p_ptr, StateOrientation* _o_ptr, const Eigen::VectorXs & _params) :
NodeBase("SENSOR"),
type_(_tp),
p_ptr_(_p_ptr),
@@ -10,7 +10,7 @@ SensorBase::SensorBase(const SensorType & _tp, StateBase* _p_ptr, StateOrientati
params_ = _params;
}
-SensorBase::SensorBase(const SensorType & _tp, StateBase* _p_ptr, StateOrientation* _o_ptr, unsigned int _params_size) :
+SensorBase::SensorBase(const SensorType & _tp, StatePoint3D* _p_ptr, StateOrientation* _o_ptr, unsigned int _params_size) :
NodeBase("SENSOR"),
type_(_tp),
p_ptr_(_p_ptr),
@@ -30,7 +30,7 @@ const SensorType SensorBase::getSensorType() const
return type_;
}
-StateBase* SensorBase::getPPtr() const
+StatePoint3D* SensorBase::getPPtr() const
{
return p_ptr_;
}
diff --git a/src/sensor_base.h b/src/sensor_base.h
index 9ab7161b3..58512587c 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -14,7 +14,7 @@ class SensorBase : public NodeBase
{
protected:
SensorType type_; //indicates sensor type. Enum defined at wolf.h
- StateBase* p_ptr_; // sensor position state unit pointer
+ StatePoint3D* p_ptr_; // sensor position state unit pointer
StateOrientation* o_ptr_; // sensor orientation state unit pointer
Eigen::VectorXs params_;//sensor intrinsic params: offsets, scale factors, sizes, ...
@@ -36,7 +36,7 @@ class SensorBase : public NodeBase
* \param _params Vector containing the sensor parameters
*
**/
- SensorBase(const SensorType & _tp, StateBase* _p_ptr, StateOrientation* _o_ptr, const Eigen::VectorXs & _params);
+ SensorBase(const SensorType & _tp, StatePoint3D* _p_ptr, StateOrientation* _o_ptr, const Eigen::VectorXs & _params);
/** \brief Constructor with parameter size
*
@@ -47,13 +47,13 @@ class SensorBase : public NodeBase
* \param _params_size size of the vector containing the sensor parameters
*
**/
- SensorBase(const SensorType & _tp, StateBase* _p_ptr, StateOrientation* _o_ptr, unsigned int _params_size);
+ SensorBase(const SensorType & _tp, StatePoint3D* _p_ptr, StateOrientation* _o_ptr, unsigned int _params_size);
~SensorBase();
const SensorType getSensorType() const;
- StateBase* getPPtr() const;
+ StatePoint3D* getPPtr() const;
StateOrientation* getOPtr() const;
};
diff --git a/src/sensor_gps_fix.cpp b/src/sensor_gps_fix.cpp
index b798dbb92..9af26aa72 100644
--- a/src/sensor_gps_fix.cpp
+++ b/src/sensor_gps_fix.cpp
@@ -1,6 +1,6 @@
#include "sensor_gps_fix.h"
-SensorGPSFix::SensorGPSFix(StateBase* _p_ptr, StateOrientation* _o_ptr, const double& _noise) :
+SensorGPSFix::SensorGPSFix(StatePoint3D* _p_ptr, StateOrientation* _o_ptr, const double& _noise) :
SensorBase(GPS_FIX, _p_ptr, _o_ptr, Eigen::VectorXs::Constant(1,_noise))
{
//
diff --git a/src/sensor_gps_fix.h b/src/sensor_gps_fix.h
index 3b918a0e6..ec85cac55 100644
--- a/src/sensor_gps_fix.h
+++ b/src/sensor_gps_fix.h
@@ -16,7 +16,7 @@ class SensorGPSFix : public SensorBase
* \param _noise noise standard deviation
*
**/
- SensorGPSFix(StateBase* _p_ptr, StateOrientation* _o_ptr, const double& _noise);
+ SensorGPSFix(StatePoint3D* _p_ptr, StateOrientation* _o_ptr, const double& _noise);
/** \brief Destructor
*
diff --git a/src/sensor_laser_2D.cpp b/src/sensor_laser_2D.cpp
index 434a43ab0..456ec6418 100644
--- a/src/sensor_laser_2D.cpp
+++ b/src/sensor_laser_2D.cpp
@@ -7,7 +7,7 @@
// params_ << _angle_min, _angle_max, _angle_increment, _range_min, _range_max, _range_stdev, _time_increment, _scan_time;
// }
-SensorLaser2D::SensorLaser2D(StateBase* _p_ptr, StateOrientation* _o_ptr) :
+SensorLaser2D::SensorLaser2D(StatePoint3D* _p_ptr, StateOrientation* _o_ptr) :
SensorBase(LIDAR, _p_ptr, _o_ptr, 8)
{
setDefaultScanParams();
diff --git a/src/sensor_laser_2D.h b/src/sensor_laser_2D.h
index cae219d61..e500434a6 100644
--- a/src/sensor_laser_2D.h
+++ b/src/sensor_laser_2D.h
@@ -29,7 +29,7 @@ class SensorLaser2D : public SensorBase
* \param _params struct with scan parameters. See laser_scan_utils library API for reference
*
**/
- SensorLaser2D(StateBase* _p_ptr, StateOrientation* _o_ptr);
+ SensorLaser2D(StatePoint3D* _p_ptr, StateOrientation* _o_ptr);
//SensorLaser2D(const Eigen::VectorXs & _sp, const laserscanutils::ScanParams & _params);
/** \brief Destructor
diff --git a/src/sensor_odom_2D.cpp b/src/sensor_odom_2D.cpp
index 309ac30a7..19ac3eae2 100644
--- a/src/sensor_odom_2D.cpp
+++ b/src/sensor_odom_2D.cpp
@@ -1,6 +1,6 @@
#include "sensor_odom_2D.h"
-SensorOdom2D::SensorOdom2D(StateBase* _p_ptr, StateOrientation* _o_ptr, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor) :
+SensorOdom2D::SensorOdom2D(StatePoint3D* _p_ptr, StateOrientation* _o_ptr, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor) :
SensorBase(ODOM_2D, _p_ptr, _o_ptr, 2)
{
params_ << _disp_noise_factor, _rot_noise_factor;
diff --git a/src/sensor_odom_2D.h b/src/sensor_odom_2D.h
index aca82aeff..675edd79d 100644
--- a/src/sensor_odom_2D.h
+++ b/src/sensor_odom_2D.h
@@ -17,7 +17,7 @@ class SensorOdom2D : public SensorBase
* \param _rot_noise_factor rotation noise factor
*
**/
- SensorOdom2D(StateBase* _p_ptr, StateOrientation* _o_ptr, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor);
+ SensorOdom2D(StatePoint3D* _p_ptr, StateOrientation* _o_ptr, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor);
/** \brief Destructor
*
diff --git a/src/state_complex_angle.cpp b/src/state_complex_angle.cpp
index b2211a82f..2772c6bf5 100644
--- a/src/state_complex_angle.cpp
+++ b/src/state_complex_angle.cpp
@@ -28,15 +28,15 @@ unsigned int StateComplexAngle::getStateSize() const
return BLOCK_SIZE;
}
-Eigen::Matrix3s StateComplexAngle::getRotationMatrix() const
+void StateComplexAngle::rotationMatrix(Eigen::Matrix3s& R) const
{
- Eigen::Matrix3s R(Eigen::Matrix3s::Identity());
- R(0,0) = *state_ptr_;
- R(1,1) = *state_ptr_;
- R(0,1) = -*(state_ptr_+1);
- R(1,0) = *(state_ptr_+1);
+ R = Eigen::Matrix3s::Identity();
- return R;
+ R(0,0) = *state_ptr_;
+ R(1,0) = *(state_ptr_+1);
+
+ R(0,1) = -*(state_ptr_+1);
+ R(1,1) = *state_ptr_;
}
Eigen::Map<const Eigen::VectorXs> StateComplexAngle::getVector() const
diff --git a/src/state_complex_angle.h b/src/state_complex_angle.h
index ee7fca41c..c58c16133 100644
--- a/src/state_complex_angle.h
+++ b/src/state_complex_angle.h
@@ -60,7 +60,8 @@ class StateComplexAngle : public StateOrientation
* Returns the 3x3 rotation matrix of the orientation
*
**/
- virtual Eigen::Matrix3s getRotationMatrix() const;
+ //virtual Eigen::Matrix3s getRotationMatrix() const;
+ void rotationMatrix(Eigen::Matrix3s& R) const;
/** \brief Returns a (mapped) vector of the state unit
*
diff --git a/src/state_orientation.cpp b/src/state_orientation.cpp
index e48be883c..58ba497e5 100644
--- a/src/state_orientation.cpp
+++ b/src/state_orientation.cpp
@@ -18,3 +18,10 @@ StateOrientation::~StateOrientation()
{
//
}
+
+Eigen::Matrix3s StateOrientation::getRotationMatrix() const
+{
+ Eigen::Matrix3s R(Eigen::Matrix3s::Identity());
+ rotationMatrix(R);
+ return R;
+}
diff --git a/src/state_orientation.h b/src/state_orientation.h
index 7a854b86d..8c75a1edb 100644
--- a/src/state_orientation.h
+++ b/src/state_orientation.h
@@ -44,7 +44,8 @@ class StateOrientation : public StateBase
* Returns the 3x3 rotation matrix of the orientation
*
**/
- virtual Eigen::Matrix3s getRotationMatrix() const = 0;
+ Eigen::Matrix3s getRotationMatrix() const;
+ virtual void rotationMatrix(Eigen::Matrix3s& R) const = 0;
/** \brief Returns a (mapped) vector of the state unit
*
diff --git a/src/state_quaternion.cpp b/src/state_quaternion.cpp
new file mode 100644
index 000000000..58796d454
--- /dev/null
+++ b/src/state_quaternion.cpp
@@ -0,0 +1,84 @@
+#include "state_quaternion.h"
+
+StateQuaternion::StateQuaternion(Eigen::VectorXs& _st_remote, const unsigned int _idx) :
+ StateOrientation(_st_remote, _idx)
+{
+ //
+}
+
+StateQuaternion::StateQuaternion(WolfScalar* _st_ptr) :
+ StateOrientation(_st_ptr)
+{
+ //
+}
+
+StateQuaternion::~StateQuaternion()
+{
+ //
+}
+
+StateType StateQuaternion::getStateType() const
+{
+ return ST_QUATERNION;
+}
+
+unsigned int StateQuaternion::getStateSize() const
+{
+ return BLOCK_SIZE;
+}
+
+// Eigen::Matrix3s StateQuaternion::getRotationMatrix() const
+// {
+// Eigen::Matrix3s R(Eigen::Matrix3s::Identity());
+// this->getRotationMatrix(R);
+// return R;
+// }
+
+void StateQuaternion::rotationMatrix(Eigen::Matrix3s& R) const
+{
+ WolfScalar qi,qj,qk,qr;
+ qi = *state_ptr_;
+ qj = *(state_ptr_+1);
+ qk = *(state_ptr_+2);
+ qr = *(state_ptr_+3);
+
+ R(0,0) = 1 - 2*qj*qj - 2*qk*qk;
+ R(1,0) = 2*(qi*qj + qk*qr);
+ R(2,0) = 2*(qi*qk - qj*qr);
+
+ R(0,1) = 2*(qi*qj - qk*qr);
+ R(1,1) = 1 - 2*qi*qi - 2*qk*qk;
+ R(2,1) = 2*(qi*qr + qj*qk);
+
+ R(0,2) = 2*(qi*qk + qj*qr);
+ R(1,2) = 2*(qj*qk - qi*qr);
+ R(2,2) = 1 - 2*qi*qi - 2*qj*qj;
+
+ //std::cout << "StateQuaternion::getRotationMatrix()" << R << std::endl;
+}
+
+Eigen::Map<const Eigen::VectorXs> StateQuaternion::getVector() const
+{
+ return Eigen::Map<const Eigen::VectorXs>(state_ptr_, 4);
+}
+
+Eigen::Map<const Eigen::Quaternions> StateQuaternion::getQuaternion() const
+{
+ return Eigen::Map<const Eigen::Quaternion<WolfScalar> >(state_ptr_);
+}
+
+void StateQuaternion::normalize()
+{
+ Eigen::Map<Eigen::Quaternions> q_map(state_ptr_);
+ q_map.normalize();
+}
+
+void StateQuaternion::print(unsigned int _ntabs, std::ostream& _ost) const
+{
+ printTabs(_ntabs);
+ _ost << nodeLabel() << " " << nodeId() << std::endl;
+ printTabs(_ntabs);
+ for (unsigned int i = 0; i < BLOCK_SIZE; i++)
+ _ost << *(this->state_ptr_ + i) << " ";
+ _ost << std::endl;
+}
diff --git a/src/state_quaternion.h b/src/state_quaternion.h
new file mode 100644
index 000000000..67622c22d
--- /dev/null
+++ b/src/state_quaternion.h
@@ -0,0 +1,96 @@
+#ifndef STATE_QUATERNION_H_
+#define STATE_QUATERNION_H_
+
+//std includes
+#include <iostream>
+#include <vector>
+#include <cmath>
+
+//Wolf includes
+#include "wolf.h"
+#include "state_orientation.h"
+
+//class StateQuaternion
+//Last component is the real part.
+class StateQuaternion : public StateOrientation
+{
+ public:
+ static const unsigned int BLOCK_SIZE = 4;
+
+ public:
+
+ /** \brief Constructor with whole state storage and index where starts the state unit
+ *
+ * Constructor with whole state storage and index where starts the state unit
+ * \param _st_remote is the whole state vector
+ * \param _idx is the index of the first element of the state in the whole state vector
+ *
+ **/
+ StateQuaternion(Eigen::VectorXs& _st_remote, const unsigned int _idx);
+
+ /** \brief Constructor with scalar pointer
+ *
+ * Constructor with scalar pointer
+ * \param _st_ptr is the pointer of the first element of the state unit
+ *
+ **/
+ StateQuaternion(WolfScalar* _st_ptr);
+
+ /** \brief Destructor
+ *
+ * Destructor
+ *
+ **/
+ virtual ~StateQuaternion();
+
+ /** \brief Returns the parametrization of the state unit
+ *
+ * Returns the parametrizationType (see wolf.h) of the state unit
+ *
+ **/
+ virtual StateType getStateType() const;
+
+ /** \brief Returns the state unit size
+ *
+ * Returns the parametrizationType (see wolf.h) of the state unit
+ *
+ **/
+ virtual unsigned int getStateSize() const;
+
+ /** \brief Returns the 3x3 rotation matrix of the orientation
+ *
+ * Returns the 3x3 rotation matrix of the orientation
+ *
+ **/
+ //virtual Eigen::Matrix3s getRotationMatrix() const;
+ virtual void rotationMatrix(Eigen::Matrix3s& R) const;
+
+ /** \brief Returns a (mapped) vector of the state unit
+ *
+ * Returns a (mapped) vector of the state unit
+ *
+ **/
+ virtual Eigen::Map<const Eigen::VectorXs> getVector() const;
+
+ /** \brief Returns a (mapped) quaternion of the state unit
+ *
+ * Returns a (mapped) quaternion of the state unit
+ *
+ **/
+ virtual Eigen::Map<const Eigen::Quaternions> getQuaternion() const;
+
+ /** \brief Normalizes the quaternion
+ *
+ * Normalizes the quaternion
+ *
+ **/
+ void normalize();
+
+ /** \brief Prints all the elements of the state unit
+ *
+ * Prints all the elements of the state unit
+ *
+ **/
+ virtual void print(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const;
+};
+#endif
diff --git a/src/state_theta.cpp b/src/state_theta.cpp
index 39e9fbb26..f11b78f18 100644
--- a/src/state_theta.cpp
+++ b/src/state_theta.cpp
@@ -27,26 +27,20 @@ unsigned int StateTheta::getStateSize() const
return BLOCK_SIZE;
}
-Eigen::Matrix3s StateTheta::getRotationMatrix() const
-{
- Eigen::Matrix3s R(Eigen::Matrix3s::Identity());
- R(0, 0) = cos(*state_ptr_);
- R(1, 1) = cos(*state_ptr_);
- R(0, 1) = -sin(*state_ptr_);
- R(1, 0) = sin(*state_ptr_);
+// Eigen::Matrix3s StateTheta::getRotationMatrix() const
+// {
+// Eigen::Matrix3s R(Eigen::Matrix3s::Identity());
+// this->getRotationMatrix(R);
+// return R;
+// }
- //std::cout << "StateTheta::getRotationMatrix()" << R << std::endl;
- return R;
-}
-
-void StateTheta::getRotationMatrix(Eigen::Matrix3s& R) const
+void StateTheta::rotationMatrix(Eigen::Matrix3s& R) const
{
R = Eigen::Matrix3s::Identity();
R(0, 0) = cos(*state_ptr_);
R(1, 1) = cos(*state_ptr_);
R(0, 1) = -sin(*state_ptr_);
R(1, 0) = sin(*state_ptr_);
-
//std::cout << "StateTheta::getRotationMatrix()" << R << std::endl;
}
diff --git a/src/state_theta.h b/src/state_theta.h
index 5de9d1176..c9d2294fc 100644
--- a/src/state_theta.h
+++ b/src/state_theta.h
@@ -61,8 +61,8 @@ class StateTheta : public StateOrientation
* Returns the 3x3 rotation matrix of the orientation
*
**/
- virtual Eigen::Matrix3s getRotationMatrix() const;
- void getRotationMatrix(Eigen::Matrix3s& R) const;
+ //virtual Eigen::Matrix3s getRotationMatrix() const;
+ void rotationMatrix(Eigen::Matrix3s& R) const;
/** \brief Returns a (mapped) vector of the state unit
*
diff --git a/src/wolf.h b/src/wolf.h
index 7f207ddd2..ee11044dd 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -64,6 +64,7 @@ typedef Matrix<WolfScalar, 2, 1> Vector2s; ///< 2-vector of real
typedef Matrix<WolfScalar, 3, 1> Vector3s; ///< 3-vector of real scalar_t type
typedef Matrix<WolfScalar, 4, 1> Vector4s; ///< 4-vector of real scalar_t type
typedef Matrix<WolfScalar, 6, 1> Vector6s; ///< 6-vector of real scalar_t type
+typedef Matrix<WolfScalar, 7, 1> Vector7s; ///< 7-vector of real scalar_t type
typedef Matrix<WolfScalar, Dynamic, 1> VectorXs; ///< variable size vector of real scalar_t type
typedef Matrix<WolfScalar, 1, 2> RowVector2s; ///< 2-row-vector of real scalar_t type
typedef Matrix<WolfScalar, 1, 3> RowVector3s; ///< 3-row-vector of real scalar_t type
diff --git a/src/wolf_manager.h b/src/wolf_manager.h
index 4a853600c..dda55e349 100644
--- a/src/wolf_manager.h
+++ b/src/wolf_manager.h
@@ -38,6 +38,7 @@
#include "trajectory_base.h"
#include "map_base.h"
#include "wolf_problem.h"
+#include "state_quaternion.h"
class WolfManager
{
@@ -81,7 +82,7 @@ class WolfManager
&& "Wrong init_frame state vector or covariance matrix size");
- // Set initial covariance with a fake ODOM 2D capture to a fix frame
+ // Set initial covariance with a fake ODOM 2D capture to a fixed frame
createFrame(_init_frame, TimeStamp(0));
problem_->getTrajectoryPtr()->getFrameListPtr()->back()->fix();
last_capture_relative_->integrateCapture((CaptureRelative*)(new CaptureOdom2D(TimeStamp(0), nullptr, Eigen::Vector3s::Zero(), _init_frame_cov)));
--
GitLab