diff --git a/include/publisher_map_grid_2d_gravity.h b/include/publisher_map_grid_2d_gravity.h
new file mode 100644
index 0000000000000000000000000000000000000000..52547e35c90f88b45d7a2d2ba36a0b83d667c7a4
--- /dev/null
+++ b/include/publisher_map_grid_2d_gravity.h
@@ -0,0 +1,40 @@
+#ifndef PUBLISHER_MAP_GRID_2D_GRAVITY_H
+#define PUBLISHER_MAP_GRID_2D_GRAVITY_H
+
+/**************************
+ * WOLF includes *
+ **************************/
+#include "core/problem/problem.h"
+#include "imu/sensor/sensor_imu.h"
+
+#include "publisher.h"
+
+
+namespace wolf
+{
+
+class PublisherMapGrid2dGravity: public Publisher
+{
+ std::string map_frame_id_;
+ ros::Publisher pub_accel_, pub_gyro_;
+ SensorImuPtr sensor_imu_;
+
+
+ public:
+ PublisherMapGrid2dGravity(const std::string& _unique_name,
+ const ParamsServer& _server,
+ const ProblemPtr _problem);
+ WOLF_PUBLISHER_CREATE(PublisherImuBias);
+
+ virtual ~PublisherMapGrid2dGravity(){};
+
+ void initialize(ros::NodeHandle &nh, const std::string& topic) override;
+
+ void publishDerived() override;
+
+};
+
+WOLF_REGISTER_PUBLISHER(PublisherImuBias)
+}
+
+#endif
diff --git a/src/publisher_map_grid_2d_gravity.cpp b/src/publisher_map_grid_2d_gravity.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..12f969b1383148089dfe993c226e3b2ecca796fc
--- /dev/null
+++ b/src/publisher_map_grid_2d_gravity.cpp
@@ -0,0 +1,44 @@
+#include "publisher_map_grid_2d_gravity.h"
+#include <ros/ros.h>
+#include "tf/transform_datatypes.h"
+#include <geometry_msgs/Vector3.h>
+#include <visualization_msgs/MarkerArray.h>
+
+namespace wolf
+{
+
+PublisherMapGrid2dGravity::PublisherMapGrid2dGravity(const std::string& _unique_name,
+ const ParamsServer& _server,
+ const ProblemPtr _problem) :
+ Publisher(_unique_name, _server, _problem)
+{
+ sensor_imu_ = std::static_pointer_cast<SensorImu>(_problem->getSensor(_server.getParam<std::string>(prefix_ + "/sensor_name")));
+}
+
+void PublisherMapGrid2dGravity::initialize(ros::NodeHandle& nh, const std::string& topic)
+{
+ pub_accel_ = nh.advertise<geometry_msgs::Vector3>(topic + "_bias_accel", 1);
+ pub_gyro_ = nh.advertise<geometry_msgs::Vector3>(topic + "_bias_gyro", 1);
+}
+
+void PublisherMapGrid2dGravity::publishDerived()
+{
+ // Fill PoseStamped msg
+ geometry_msgs::Vector3 accel_bias_msg, gyro_bias_msg;
+
+ Eigen::Vector3d accel_bias = sensor_imu_->getIntrinsic()->getState().head(3);
+ Eigen::Vector3d gyro_bias = sensor_imu_->getIntrinsic()->getState().tail(3);
+
+ accel_bias_msg.x = accel_bias(0);
+ accel_bias_msg.y = accel_bias(1);
+ accel_bias_msg.z = accel_bias(2);
+
+ gyro_bias_msg.x = gyro_bias(0);
+ gyro_bias_msg.y = gyro_bias(1);
+ gyro_bias_msg.z = gyro_bias(2);
+
+ pub_accel_.publish(accel_bias_msg);
+ pub_gyro_.publish(gyro_bias_msg);
+}
+
+}
diff --git a/src/subscriber_imu_enableable.cpp b/src/subscriber_imu_enableable.cpp
index 3b4e118facd9aace8e5eec3a04bf08ca91a41e45..2cfb082c511216d05ac210b3403a95050bb348fb 100644
--- a/src/subscriber_imu_enableable.cpp
+++ b/src/subscriber_imu_enableable.cpp
@@ -1,6 +1,7 @@
#include "../include/subscriber_imu_enableable.h"
#include <core/factor/factor_relative_pose_3d.h>
+#include <core/factor/factor_relative_pose_2d.h>
#include "core/capture/capture_void.h"
#include "core/processor/processor_motion.h"
@@ -104,6 +105,9 @@ void SubscriberImuEnableable::callback(const sensor_msgs::Imu::ConstPtr& msg)
// impose zero velocity
last_frame_->getV()->setZero();
last_frame_->getV()->fix();
+
+ // impose zero odometry
+ processor_motion_->setOdometry(sensor_ptr_->getProblem()->stateZero(processor_motion_->getStateStructure()));
}
else
{
@@ -121,11 +125,16 @@ void SubscriberImuEnableable::callback(const sensor_msgs::Imu::ConstPtr& msg)
// impose zero velocity
last_frame_->getV()->setZero();
last_frame_->getV()->fix();
+
+ // impose zero odometry
+ processor_motion_->setOdometry(sensor_ptr_->getProblem()->stateZero(processor_motion_->getStateStructure()));
// add zero displacement and rotation capture & feature & factor with all previous frames
- Eigen::Vector7d zero_motion = Eigen::Vector7d::Zero();
- zero_motion.tail<4>() = Eigen::Quaterniond::Identity().coeffs();
assert(sensor_ptr_->getProblem());
+ auto size = sensor_ptr_->getProblem()->getDim() == 3 ? 7:3;
+ Eigen::VectorXd zero_motion = Eigen::VectorXd::Zero(size);
+ if(sensor_ptr_->getProblem()->getDim() == 3)
+ zero_motion.tail<4>() = Eigen::Quaterniond::Identity().coeffs();
for (auto frm_pair = sensor_ptr_->getProblem()->getTrajectory()->begin();
frm_pair != sensor_ptr_->getProblem()->getTrajectory()->end();
frm_pair++)
@@ -134,15 +143,24 @@ void SubscriberImuEnableable::callback(const sensor_msgs::Imu::ConstPtr& msg)
break;
auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr);
auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
- "FeatureOdom3d",
+ "FeatureZeroOdom",
zero_motion,
- Eigen::Matrix6d::Identity() * 0.01);
- FactorBase::emplace<FactorRelativePose3d>(feature_zero,
- feature_zero,
- frm_pair->second,
- nullptr,
- false,
- TOP_MOTION);
+ Eigen::MatrixXd::Identity(size,size) * 0.01);
+ if(sensor_ptr_->getProblem()->getDim() == 3)
+ FactorBase::emplace<FactorRelativePose3d>(feature_zero,
+ feature_zero,
+ frm_pair->second,
+ nullptr,
+ false,
+ TOP_MOTION);
+ else
+ FactorBase::emplace<FactorRelativePose2d>(feature_zero,
+ feature_zero,
+ frm_pair->second,
+ nullptr,
+ false,
+ TOP_MOTION);
+
}
}
// static initialization ended