diff --git a/include/node.h b/include/node.h
index 4e599aa5612080129cb5ae1afaeaf511a5e65dd0..cf719c85d4050abf9301c242e5c514ad16333b37 100644
--- a/include/node.h
+++ b/include/node.h
@@ -64,6 +64,10 @@ class WolfRosNode
     protected:
         // solver
         SolverManagerPtr solver_;
+        bool compute_cov_;
+        SolverManager::CovarianceBlocksToBeComputed cov_enum_;
+        ros::Time last_cov_stamp_;
+        double cov_period_;
 
         // transforms
         tf::TransformBroadcaster tfb_;
diff --git a/include/publisher_pose.h b/include/publisher_pose.h
index dff4bac7531ea8fccf2c580fae43532f03aab046..890b6dd86a9f9153eee8730af897468660e57bc2 100644
--- a/include/publisher_pose.h
+++ b/include/publisher_pose.h
@@ -13,6 +13,7 @@
  **************************/
 #include <ros/ros.h>
 #include <geometry_msgs/PoseArray.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
 #include <visualization_msgs/Marker.h>
 #include <tf/transform_listener.h>
 
@@ -21,16 +22,17 @@ namespace wolf
 
 class PublisherPose: public Publisher
 {
-        bool pose_array_, marker_;
+        bool pose_array_, marker_, pose_with_cov_;
 
         geometry_msgs::PoseArray pose_array_msg_;
         visualization_msgs::Marker marker_msg_;
+        geometry_msgs::PoseWithCovarianceStamped pose_with_cov_msg_;
         std_msgs::ColorRGBA marker_color_;
         bool extrinsics_;
         SensorBasePtr sensor_;
         std::string frame_id_, map_frame_id_;
 
-        ros::Publisher pub_pose_array_, pub_marker_;
+        ros::Publisher pub_pose_array_, pub_marker_, pub_pose_with_cov_;
 
     public:
         PublisherPose(const std::string& _unique_name,
@@ -44,7 +46,7 @@ class PublisherPose: public Publisher
 
         void publishDerived() override;
 
-        void publishPose(const geometry_msgs::Pose pose, const ros::Time& stamp);
+        void publishPose();
 
     protected:
 
diff --git a/src/node.cpp b/src/node.cpp
index 686314acbc3e1fa5312fef71a9ac6f832f1a95d1..e7762e89935aee2ac0a6eceeb71b3f9aa68b8e03 100644
--- a/src/node.cpp
+++ b/src/node.cpp
@@ -39,6 +39,13 @@ WolfRosNode::WolfRosNode()
     // SOLVER
     ROS_INFO("Creating solver...");
     solver_ = FactorySolver::create("SolverCeres", problem_ptr_, server);
+    // covariance
+    compute_cov_ = server.getParam<bool>("solver/compute_cov");
+    if (compute_cov_)
+    {
+        cov_enum_   = (SolverManager::CovarianceBlocksToBeComputed)server.getParam<int>("solver/cov_enum");
+        cov_period_ = server.getParam<double>("solver/cov_period");
+    }
 
     // ROS SUBSCRIBERS
     ROS_INFO("Creating subscribers...");
@@ -91,7 +98,22 @@ void WolfRosNode::solve()
 
     std::string report = solver_->solve();
     if (!report.empty())
+    {
         std::cout << report << std::endl;
+        problem_ptr_->print(4,1,1,1);
+    }
+
+    if (compute_cov_ and (ros::Time::now() - last_cov_stamp_).toSec() > cov_period_)
+    {
+        if (solver_->computeCovariances(cov_enum_))
+        {
+            last_cov_stamp_ = ros::Time::now();
+            if (solver_->getVerbosity() != SolverManager::ReportVerbosity::QUIET)
+                ROS_INFO("Covariances computed successfully!");
+        }
+        else if (solver_->getVerbosity() != SolverManager::ReportVerbosity::QUIET)
+            ROS_WARN("Failed to compute covariances");
+    }
 }
 
 void WolfRosNode::visualize()
@@ -184,7 +206,7 @@ void WolfRosNode::broadcastTf()
 
 void WolfRosNode::solveLoop()
 {
-    ros::Rate solverRate(1/solver_->getPeriod());
+    ros::Rate solverRate(1/(solver_->getPeriod()+1e-9)); // 1ns added to allow pausing if rosbag paused
     WOLF_DEBUG("Started solver loop");
 
     while (ros::ok())
diff --git a/src/publisher_pose.cpp b/src/publisher_pose.cpp
index 11fc3937928ac16df25970ceb80e7f187a4aac4b..328dd6bc3ce597c10b1d4b0d7b7a42e50d62d922 100644
--- a/src/publisher_pose.cpp
+++ b/src/publisher_pose.cpp
@@ -15,8 +15,9 @@ PublisherPose::PublisherPose(const std::string& _unique_name,
                              const ProblemPtr _problem) :
         Publisher(_unique_name, _server, _problem)
 {
-    pose_array_ = _server.getParam<bool>(prefix_ + "/pose_array_msg");
-    marker_     = _server.getParam<bool>(prefix_ + "/marker_msg");
+    pose_array_     = _server.getParam<bool>(prefix_ + "/pose_array_msg");
+    marker_         = _server.getParam<bool>(prefix_ + "/marker_msg");
+    pose_with_cov_  = _server.getParam<bool>(prefix_ + "/pose_with_cov_msg");
     if (marker_)
     {
         Eigen::Vector4d col = _server.getParam<Eigen::Vector4d>(prefix_ + "/marker_color");
@@ -55,22 +56,30 @@ void PublisherPose::initialize(ros::NodeHandle& nh, const std::string& topic)
 
         pub_marker_ = nh.advertise<visualization_msgs::Marker>(topic + "_marker", 1);
     }
+    if (pose_with_cov_)
+    {
+        pose_with_cov_msg_.header.frame_id = frame_id_;
+
+        pub_pose_with_cov_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>(topic + "_pose_with_cov", 1);
+    }
 }
 
 void PublisherPose::publishDerived()
 {
+    if (not pose_array_ and not marker_ and not pose_with_cov_)
+        return;
+
     VectorComposite current_state = problem_->getState("PO");
     TimeStamp loc_ts = problem_->getTimeStamp();
 
     // state not ready
     if (current_state.count('P') == 0 or
         current_state.count('O') == 0 or
-        loc_ts == TimeStamp(0))
+        not loc_ts.ok())
     {
         return;
     }
 
-
     // fill vector and quaternion
     Eigen::Vector3d p = Eigen::Vector3d::Zero();
     Eigen::Quaterniond q;
@@ -119,36 +128,63 @@ void PublisherPose::publishDerived()
         q = q_frame_ * q;
     }
 
+    // Covariance
+    Eigen::MatrixXd cov(6,6);
+    auto KF = problem_->getLastKeyFrame();
+    WOLF_INFO("Getting cov of KF ", KF->id());
+    bool success(true);
+    success = success && problem_->getCovarianceBlock(KF->getP(), KF->getP(), cov, 0, 0);
+    success = success && problem_->getCovarianceBlock(KF->getP(), KF->getO(), cov, 0, 3);
+    success = success && problem_->getCovarianceBlock(KF->getO(), KF->getP(), cov, 3, 0);
+    success = success && problem_->getCovarianceBlock(KF->getO(), KF->getO(), cov, 3, 3);
+
+    if (success)
+    {
+        if (problem_->getDim() == 2)
+            throw std::runtime_error("not implemented");
+        else
+            std::copy(cov.data(), cov.data() + cov.size(), pose_with_cov_msg_.pose.covariance.data());
+    }
+    else
+    {
+        WOLF_WARN("Last KF covariance could not be recovered, using the previous one");
+        //pose_with_cov_msg_.pose.covariance[0] = -1; // not valid
+    }
+
     // Fill Pose msg
-    geometry_msgs::Pose pose_msg;
-    pose_msg.position.x = p(0);
-    pose_msg.position.y = p(1);
-    pose_msg.position.z = p(2);
-
-    pose_msg.orientation.x = q.x();
-    pose_msg.orientation.y = q.y();
-    pose_msg.orientation.z = q.z();
-    pose_msg.orientation.w = q.w();
-    publishPose(pose_msg, ros::Time(loc_ts.getSeconds(), loc_ts.getNanoSeconds()));
+    pose_with_cov_msg_.header.stamp = ros::Time(loc_ts.getSeconds(), loc_ts.getNanoSeconds());
+    pose_with_cov_msg_.pose.pose.position.x = p(0);
+    pose_with_cov_msg_.pose.pose.position.y = p(1);
+    pose_with_cov_msg_.pose.pose.position.z = p(2);
+
+    pose_with_cov_msg_.pose.pose.orientation.x = q.x();
+    pose_with_cov_msg_.pose.pose.orientation.y = q.y();
+    pose_with_cov_msg_.pose.pose.orientation.z = q.z();
+    pose_with_cov_msg_.pose.pose.orientation.w = q.w();
+    publishPose();
 }
 
-void PublisherPose::publishPose(const geometry_msgs::Pose pose, const ros::Time& stamp)
+void PublisherPose::publishPose()
 {
     // fill msgs and publish
     if (pose_array_)
     {
-        pose_array_msg_.header.stamp = stamp;
-        pose_array_msg_.poses.push_back(pose);
+        pose_array_msg_.header.stamp = pose_with_cov_msg_.header.stamp;
+        pose_array_msg_.poses.push_back(pose_with_cov_msg_.pose.pose);
 
         pub_pose_array_.publish(pose_array_msg_);
     }
     if (marker_)
     {
-        marker_msg_.header.stamp = stamp;
-        marker_msg_.points.push_back(pose.position);
+        marker_msg_.header.stamp = pose_with_cov_msg_.header.stamp;
+        marker_msg_.points.push_back(pose_with_cov_msg_.pose.pose.position);
 
         pub_marker_.publish(marker_msg_);
     }
+    if (pose_with_cov_ and pose_with_cov_msg_.pose.covariance[0] > 0)
+    {
+        pub_pose_with_cov_.publish(pose_with_cov_msg_);
+    }
 }
 
 bool PublisherPose::listenTf()