diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2f15abfc15d6b56197f9cf0b8cd0e05f13d263fc..cc9d51716804ca2b1e8728856e64a5aa2e6692a6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -15,11 +15,25 @@ find_package(catkin REQUIRED COMPONENTS
   tf
   tf_conversions
   tf2_ros
+  message_generation
 )
 
 ## System dependencies are found with CMake's conventions
 find_package(wolfcore REQUIRED)
 
+## Generate messages in the 'msg' folder
+add_message_files(
+  FILES
+  LandmarkDetection.msg
+  LandmarkDetectionArray.msg
+)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  geometry_msgs
+  std_msgs
+)
 
 ###################################
 ## catkin specific configuration ##
diff --git a/msg/LandmarkDetection.msg b/msg/LandmarkDetection.msg
new file mode 100644
index 0000000000000000000000000000000000000000..00a6c9508ee809b836f42107da0e4905932da649
--- /dev/null
+++ b/msg/LandmarkDetection.msg
@@ -0,0 +1,2 @@
+geometry_msgs/PoseWithCovariance pose
+int32 id
\ No newline at end of file
diff --git a/msg/LandmarkDetectionArray.msg b/msg/LandmarkDetectionArray.msg
new file mode 100644
index 0000000000000000000000000000000000000000..d59391da0cd9b0d219bb20cfd688a4d1faa9dd95
--- /dev/null
+++ b/msg/LandmarkDetectionArray.msg
@@ -0,0 +1,2 @@
+Header header
+LandmarkDetection[] detections
\ No newline at end of file
diff --git a/src/node.cpp b/src/node.cpp
index 42edbae98b0162a42bb12f1d7d17a416c032f63a..4df79958f0cebce854c69224d677276459bcadd0 100644
--- a/src/node.cpp
+++ b/src/node.cpp
@@ -67,6 +67,16 @@ WolfRosNode::WolfRosNode()
     ROS_INFO("Creating problem...");
     problem_ptr_ = Problem::autoSetup(server);
 
+    // // HACK -> SENSOR PRIORS
+    // auto imu = problem_ptr_->findSensor("IMU");
+    // double std_acc = server.getParam<double>("sensor/IMU/ab_initial_stdev");
+    // double std_gyro = server.getParam<double>("sensor/IMU/wb_initial_stdev");
+    // Array<double,6,1> std_bias;
+    // std_bias << std_acc, std_acc, std_acc, std_gyro, std_gyro, std_gyro;
+    // imu->addPriorParameter('I',                   // bias
+    //                      Vector6d::Zero(),            // mean
+    //                      std_bias.square().matrix().asDiagonal());  // cov
+
     // SOLVER
     ROS_INFO("Creating solver...");
     solver_ = FactorySolver::create("SolverCeres", problem_ptr_, server);
diff --git a/src/publisher_graph.cpp b/src/publisher_graph.cpp
index 2d573aec38f55639538495b97d3feadf9e621770..fae0e1f7974a69950eee22f957eab586fbebee09 100644
--- a/src/publisher_graph.cpp
+++ b/src/publisher_graph.cpp
@@ -321,6 +321,11 @@ void PublisherGraph::publishFactors()
     // Iterate over the list of factors
     for (auto fac : fac_list)
     {
+        // check factor is valid (it is not being removed) 
+        // (this is redundant with getFactorList() checks but just in case)
+        if (fac->isRemoving())
+            continue;
+
         // Try to fill marker
         if (not fillFactorMarker(fac, factor_marker, factor_text_marker))
             continue;
@@ -537,8 +542,10 @@ bool PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac,
     geometry_msgs::Point point1, point2;
 
     // point1 -> frame ------------------------------------------------------
-    if (not fac->getCapture() or
-        not fac->getCapture()->getFrame() or
+    if (not fac->getCapture() or 
+        fac->getCapture()->isRemoving() or
+        not fac->getCapture()->getFrame() or 
+        fac->getCapture()->getFrame()->isRemoving() or
         not fac->getCapture()->getFrame()->getP())
         return false;
 
@@ -551,11 +558,11 @@ bool PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac,
 
     // point2 -> other ------------------------------------------------------
     // FRAME
-    if (fac->getFrameOther() != nullptr)
+    if (fac->getFrameOther() != nullptr and not fac->getFrameOther()->isRemoving())
     {
         // special case: Motion from ProcessorMotion
         auto proc_motion = std::dynamic_pointer_cast<const ProcessorMotion>(fac->getProcessor());
-        if (proc_motion and fac->getCaptureOther())
+        if (proc_motion and fac->getCaptureOther() and not fac->getCaptureOther()->isRemoving())
         {
             // Get state of other
             auto x_other = fac->getFrameOther()->getState(proc_motion->getStateStructure());
@@ -627,7 +634,7 @@ bool PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac,
         }
     }
     // CAPTURE
-    else if (fac->getCaptureOther() != nullptr)
+    else if (fac->getCaptureOther() != nullptr and not fac->getCaptureOther()->isRemoving())
     {
         if (fac->getCaptureOther()->isRemoving() or
             not fac->getCaptureOther()->getFrame() or
@@ -642,7 +649,7 @@ bool PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac,
             point2.z = 0;
     }
     // FEATURE
-    else if (fac->getFeatureOther() != nullptr)
+    else if (fac->getFeatureOther() != nullptr and not fac->getFeatureOther()->isRemoving())
     {
         if (fac->getFeatureOther()->isRemoving() or
             not fac->getFeatureOther()->getCapture() or
@@ -658,7 +665,7 @@ bool PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac,
             point2.z = 0;
     }
     // LANDMARK
-    else if (fac->getLandmarkOther() != nullptr)
+    else if (fac->getLandmarkOther() != nullptr and not fac->getLandmarkOther()->isRemoving())
     {
         if (fac->getLandmarkOther()->isRemoving() or
             not fac->getLandmarkOther()->getP())