diff --git a/src/node.cpp b/src/node.cpp
index 2be5fe4597c4b29b8b8e0216a6b8ea109498fe43..ecec4392f624eee7fd989638d3ca80a4e21c2389 100644
--- a/src/node.cpp
+++ b/src/node.cpp
@@ -127,7 +127,9 @@ bool WolfRosNode::updateTf()
 
     //Get map2base from Wolf result, and builds base2map pose
     tf::Transform T_map2base;
-    if (current_state.size() != 2 or loc_ts == TimeStamp(0))
+    if (current_state.count("P") == 0 or
+        current_state.count("O") == 0 or
+        loc_ts == TimeStamp(0))
     {
         if (state_available_)
         {
diff --git a/src/visualizer.cpp b/src/visualizer.cpp
index c9b24dfb66be8a05aef4620ea64e3943b973f1d3..b8f81a61a391d4a388252ec4525c4b72e50aeb63 100644
--- a/src/visualizer.cpp
+++ b/src/visualizer.cpp
@@ -178,59 +178,37 @@ void Visualizer::publishLandmarks(const ProblemPtr problem)
 
 void Visualizer::publishFactors(const ProblemPtr problem)
 {
-    // delete all
+    // first marker: DELETEALL
     factors_marker_array_.markers.clear();
     factors_marker_array_.markers.push_back(factor_marker_);
     factors_marker_array_.markers.front().action = visualization_msgs::Marker::DELETEALL;
-    factors_publisher_.publish(factors_marker_array_);
 
     // Get a list of factors of the trajectory (discarded all prior factors for extrinsics/intrinsics..)
     FactorBasePtrList fac_list;
     problem->getTrajectory()->getFactorList(fac_list);
 
     // Iterate over the list of factors
-    int marker_i = 0;
     for (auto fac : fac_list)
     {
         auto factor_marker = factor_marker_;
         auto factor_text_marker = factor_text_marker_;
+
         // fill marker
         fillFactorMarker(fac, factor_marker, factor_text_marker);
 
         // Store marker in marker array
-        factor_marker.id = marker_i;
+        factor_marker.id = factors_marker_array_.markers.size();
         factor_marker.header.stamp = ros::Time::now();
-
-        if (factors_marker_array_.markers.size() < marker_i+1)
-        {
-            factor_marker.action = visualization_msgs::Marker::ADD;
-            factors_marker_array_.markers.push_back(factor_marker);
-        }
-        else
-        {
-            factor_marker.action = visualization_msgs::Marker::MODIFY;
-            factors_marker_array_.markers[marker_i] = factor_marker;
-        }
-        marker_i++;
+        factor_marker.action = visualization_msgs::Marker::ADD;
+        factors_marker_array_.markers.push_back(factor_marker);
 
         // Store text marker in marker array
-        factor_text_marker.id = marker_i;
+        factor_text_marker.id = factor_marker.id;
         factor_text_marker.header.stamp = ros::Time::now();
-
-        if (factors_marker_array_.markers.size() < marker_i + 1) {
-          factor_text_marker.action = visualization_msgs::Marker::ADD;
-          factors_marker_array_.markers.push_back(factor_text_marker);
-        } else {
-          factor_text_marker.action = visualization_msgs::Marker::MODIFY;
-          factors_marker_array_.markers[marker_i] = factor_text_marker;
-        }
-        marker_i++;
+        factor_text_marker.action = visualization_msgs::Marker::ADD;
+        factors_marker_array_.markers.push_back(factor_text_marker);
     }
 
-    // rest of markers (if any) action: DELETE
-    for (auto i = marker_i; i < factors_marker_array_.markers.size(); i++)
-        factors_marker_array_.markers[i].action = visualization_msgs::Marker::DELETE;
-
     // publish marker array
     factors_publisher_.publish(factors_marker_array_);
 }