Commit e79ddd96 authored by asantamaria's avatar asantamaria

added option of setting IMU orientation. commented.

parent 5e1adb03
......@@ -187,8 +187,10 @@ class EskfOdomAlgorithm
* t: Time stamp.
* a: Acc. readings (m/s^2). a = [ax,ay,az].
* w: Gyro. readings (rad/s). w = [wx,wy,wz].
* q: Orientation [OPTIONAL]. q = [qw,qx,qy,qz].
*/
void set_imu_reading(const float& t, const Eigen::Vector3f& a, const Eigen::Vector3f& w);
void set_imu_reading(const float& t, const Eigen::Vector3f& a, const Eigen::Vector3f& w, const Eigen::Quaternionf& q);
/**
* \brief Get pose parameters
......
#include "eskf_odom_alg.h"
#include <rot_fc.h>
EskfOdomAlgorithm::EskfOdomAlgorithm(void)
{
pthread_mutex_init(&this->access_,NULL);
......@@ -45,6 +47,16 @@ void EskfOdomAlgorithm::set_imu_params(Sensor::imu_params& imu)
this->filter_.set_imu_params(imu);
}
void EskfOdomAlgorithm::set_imu_reading(const float& t, const Eigen::Vector3f& a, const Eigen::Vector3f& w, const Eigen::Quaternionf& q)
{
Eigen::Vector3f eul;
atools::q2e(q,eul);
// NOTE: If IMU is facing down, we add the offset
eul(0) = eul(0) + 3.14159;
this->filter_.set_orientation_reading(t,eul);
set_imu_reading(t,a,w);
}
void EskfOdomAlgorithm::set_imu_reading(const float& t, const Eigen::Vector3f& a, const Eigen::Vector3f& w)
{
this->filter_.set_imu_reading(t,a,w);
......
......@@ -445,10 +445,11 @@ void EskfOdomAlgNode::set_imu_reading(const sensor_msgs::Imu::ConstPtr& msg,cons
a << ax, ay, az;
Eigen::Vector3f w;
w << wx, wy, wz;
Eigen::Quaternionf q(msg->orientation.w,msg->orientation.x,msg->orientation.y,msg->orientation.z);
this->alg_.lock();
// OPTIONAL: USE Actual IMU covariance. If first element is zero, then no covariance set.
// // OPTIONAL: USE Actual IMU covariance. If first element is zero, then no covariance set.
// if (msg->angular_velocity_covariance[0]>0.0 && msg->linear_acceleration_covariance[0]>0.0)
// {
// Sensor::imu_params imu = this->alg_.get_imu_params();
......@@ -458,6 +459,8 @@ void EskfOdomAlgNode::set_imu_reading(const sensor_msgs::Imu::ConstPtr& msg,cons
// }
this->alg_.set_imu_reading(msg_time, a, w); // Set values into filter object
// // OPTIONAL: Add orientation.
// this->alg_.set_imu_reading(msg_time, a, w, q); // Set values into filter object
this->alg_.unlock();
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment