assertion on sensor

src/wolf_subscriber_imu.cpp

@@ -6,6 +6,7 @@ using namespace wolf;
 WolfSubscriberImu::WolfSubscriberImu(const SensorBasePtr& sensor_ptr) :
         WolfSubscriber(sensor_ptr)
 {
+    assert(std::dynamic_pointer_cast<SensorIMU>(sensor_ptr) != nullptr);
 }
 
 void WolfSubscriberImu::initSubscriber(ros::NodeHandle& nh, const std::string& topic)
@@ -27,6 +28,13 @@ void WolfSubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
     cov.topLeftCorner<3,3>()    = Eigen::Map<const Eigen::Matrix3d>(msg->linear_acceleration_covariance.data());
     cov.bottomRightCorner<3,3>()= Eigen::Map<const Eigen::Matrix3d>(msg->angular_velocity_covariance.data());
 
+    // if covariance not filled
+    if (cov.topLeftCorner<3,3>().isApprox(Eigen::Matrix3d::Zero(),1e-9))
+        cov.topLeftCorner<3,3>() = std::pow(std::static_pointer_cast<SensorIMU>(sensor_ptr_)->getAccelNoise(),2.0)*Eigen::Matrix3d::Identity();
+
+    if (cov.bottomRightCorner<3,3>().isApprox(Eigen::Matrix3d::Zero(),1e-9))
+        cov.bottomRightCorner<3,3>() = std::pow(std::static_pointer_cast<SensorIMU>(sensor_ptr_)->getGyroNoise(),2.0)*Eigen::Matrix3d::Identity();
+
     CaptureIMUPtr new_capture = std::make_shared<CaptureIMU>(TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec),
                                                              sensor_ptr_,
                                                              data,