bugfix: covariance size for 3d case

src/subscriber_imu_enableable.cpp

@@ -132,6 +132,7 @@ void SubscriberImuEnableable::callback(const sensor_msgs::Imu::ConstPtr& msg)
                         // add zero displacement and rotation capture & feature & factor with all previous frames
                         assert(sensor_ptr_->getProblem());
                         auto size = sensor_ptr_->getProblem()->getDim() == 3 ? 7:3;
+                        auto cov_size = sensor_ptr_->getProblem()->getDim() == 3 ? 6:3;
                         Eigen::VectorXd zero_motion = Eigen::VectorXd::Zero(size);
                         if(sensor_ptr_->getProblem()->getDim() == 3) 
                           zero_motion.tail<4>() = Eigen::Quaterniond::Identity().coeffs();
@@ -145,7 +146,7 @@ void SubscriberImuEnableable::callback(const sensor_msgs::Imu::ConstPtr& msg)
                             auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
                                                                                   "FeatureZeroOdom",
                                                                                   zero_motion,
-                                                                                  Eigen::MatrixXd::Identity(size,size) * 0.01);
+                                                                                  Eigen::MatrixXd::Identity(cov_size,cov_size) * 0.01);
                             if(sensor_ptr_->getProblem()->getDim() == 3) 
                               FactorBase::emplace<FactorRelativePose3d>(feature_zero,
                                                                         feature_zero,