diff --git a/src/subscriber_imu_enableable.cpp b/src/subscriber_imu_enableable.cpp index 2cfb082c511216d05ac210b3403a95050bb348fb..15edd1b832c42ff8e45d7fef43b6221f9f6c694a 100644 --- a/src/subscriber_imu_enableable.cpp +++ b/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,