fixed publising of bias

src/subscriber_imu.cpp

@@ -38,6 +38,9 @@ SubscriberImu::SubscriberImu(const std::string& _unique_name,
         imu_x_neg_ = imu_x_axis < 0;
         imu_y_neg_ = imu_y_axis < 0;
         imu_z_neg_ = imu_z_axis < 0;
+        //WOLF_INFO("SubscriberImu: parameters: imu_x_axis = ", imu_x_axis, " imu_y_axis = ", imu_y_axis, " imu_z_axis = ", imu_z_axis);
+        //WOLF_INFO("SubscriberImu: parameters: imu_x_axis_ = ", imu_x_axis_, " imu_y_axis_ = ", imu_y_axis_, " imu_z_axis_ = ", imu_z_axis_);
+        //WOLF_INFO("SubscriberImu: parameters: imu_x_neg_ = ", imu_x_neg_, " imu_y_neg_ = ", imu_y_neg_, " imu_z_neg_ = ", imu_z_neg_);
     }
     catch(...)
     {
@@ -88,10 +91,10 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
     data(imu_y_axis_+3) = (imu_y_neg_ ? -msg->angular_velocity.y : msg->angular_velocity.y) * (in_degrees_ ?  M_PI/180.0 : 1.0);
     data(imu_z_axis_+3) = (imu_z_neg_ ? -msg->angular_velocity.z : msg->angular_velocity.z) * (in_degrees_ ?  M_PI/180.0 : 1.0);
 
-    WOLF_DEBUG("imu_x_axis_ = ", imu_x_neg_ ? -(imu_x_axis_+1) : (imu_x_axis_+1));
-    WOLF_DEBUG("imu_y_axis_ = ", imu_y_neg_ ? -(imu_y_axis_+1) : (imu_y_axis_+1));
-    WOLF_DEBUG("imu_z_axis_ = ", imu_z_neg_ ? -(imu_z_axis_+1) : (imu_z_axis_+1));
-    WOLF_DEBUG("data      = ", data.transpose());
+    //WOLF_INFO("imu_x_axis_ = ", imu_x_neg_ ? -(imu_x_axis_+1) : (imu_x_axis_+1));
+    //WOLF_INFO("imu_y_axis_ = ", imu_y_neg_ ? -(imu_y_axis_+1) : (imu_y_axis_+1));
+    //WOLF_INFO("imu_z_axis_ = ", imu_z_neg_ ? -(imu_z_axis_+1) : (imu_z_axis_+1));
+    //WOLF_INFO("data      = ", data.transpose());
 
     Eigen::Matrix6d cov(Eigen::Matrix6d::Zero());
     if (cov_source_=="msg")
@@ -108,9 +111,18 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
         cov.topLeftCorner<3,3>().isApprox(Eigen::Matrix3d::Zero(),1e-9) or
         cov.bottomRightCorner<3,3>().isApprox(Eigen::Matrix3d::Zero(),1e-9))
     {
-        cov = sensor_ptr_->getNoiseCov();
+        if(sensor_ptr_->getProblem()->getDim() == 3)
+        {
+            cov = sensor_ptr_->getNoiseCov();
+        }
+        else
+        {
+            cov.topLeftCorner<2,2>() = sensor_ptr_->getNoiseCov().topLeftCorner<2,2>();    
+            cov.topRightCorner<2,1>() = sensor_ptr_->getNoiseCov().topRightCorner<2,1>();
+            cov.bottomLeftCorner<1,2>() = sensor_ptr_->getNoiseCov().bottomLeftCorner<1,2>();    
+            cov(5,5) = sensor_ptr_->getNoiseCov()(2,2);
+        }
     }
-
     // Create capture and process
     auto capture = std::make_shared<CaptureImu>(TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec),
                                                 sensor_ptr_,
@@ -189,7 +201,22 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
 
             pub_imu_corrected_.publish(imu_corrected);
         }
-        WOLF_WARN_COND(sensor_ptr_->getProblem()->getDim() == 2, "SubscriberImu: Corrected IMU not implemented in 2D");
+        else
+        {
+            // fill and publish
+            sensor_msgs::Imu imu_corrected = *msg;
+
+            imu_corrected.linear_acceleration.x = data(0) - bias(0);
+            imu_corrected.linear_acceleration.y = data(1) - bias(1);
+            imu_corrected.linear_acceleration.z = data(2);
+            imu_corrected.angular_velocity.x    = data(3);
+            imu_corrected.angular_velocity.y    = data(4);
+            imu_corrected.angular_velocity.z    = data(5) - bias(5);
+
+            pub_imu_corrected_.publish(imu_corrected);
+            
+
+        }
     }
     // Publish IMU message with axis remapping
     if (pub_imu_remapped_.getNumSubscribers() != 0)