fixed dimension problem

include/publisher_map_grid_2d_gravity.h

+#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

src/publisher_map_grid_2d_gravity.cpp

+#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);
+}
+
+}

src/subscriber_imu_enableable.cpp

 #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