diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index 754964104fe824e7a684e732d152ec05192485ee..aa4bbd55eb9e30eee6606fa94ace0d0c8ba68e30 100644
--- a/src/processor/processor_odom_icp.cpp
+++ b/src/processor/processor_odom_icp.cpp
@@ -37,6 +37,9 @@ namespace wolf
 ProcessorOdomIcp::ProcessorOdomIcp(ParamsProcessorOdomIcpPtr _params):
                     ProcessorTracker("ProcessorOdomIcp", "PO", 0, _params),
                     MotionProvider("PO", _params),
+                    odom_origin_(Eigen::VectorXd::Zero(3)),
+                    odom_last_(Eigen::VectorXd::Zero(3)),
+                    odom_incoming_(Eigen::VectorXd::Zero(3)),
                     rl_T_sl_(Eigen::Isometry2d::Identity()),
                     ro_T_so_(Eigen::Isometry2d::Identity()),
                     params_odom_icp_(_params)
@@ -107,7 +110,9 @@ void ProcessorOdomIcp::updateExtrinsicsIsometries()
 
 Eigen::Isometry2d ProcessorOdomIcp::computeIsometry2d(const Eigen::VectorXd& x1, const Eigen::VectorXd& x2 ) const
 {
-    assert((x1.size() == 3 and x2.size() == 3) or (x1.size() == 7 and  x2.size() == 7));
+    // any vector not properly filled
+    if (not (x1.size() == 3 and x2.size() == 3) and not (x1.size() == 7 and  x2.size() == 7))
+        return Eigen::Isometry2d::Identity();
 
     // 2D
     if (x1.size() == 3)