diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 69e55ea8556dc03a323e0aa62e90d125b93015a0..9dfb7cae3e62adac06662e9414c75764045148e2 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -37,13 +37,11 @@ struct ParamsProcessorBase;
//wolf includes
#include "core/common/wolf.h"
+#include "core/utils/params_server.h"
#include "core/frame/frame_base.h"
#include "core/state_block/state_block.h"
-#include "core/utils/params_server.h"
-#include "core/sensor/factory_sensor.h"
-#include "core/processor/factory_processor.h"
-#include <core/processor/motion_provider.h>
#include "core/state_block/state_composite.h"
+#include "core/processor/motion_provider.h"
// std includes
#include <mutex>
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 88b5f411dc6b718cab9f5e1a2a70071abffa4f8e..66367305a0b9ad82e9ef5987c3bdacb9e9a1eeea 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -23,7 +23,7 @@
// wolf
#include "core/map/map_base.h"
#include "core/landmark/landmark_base.h"
-//#include "core/common/factory.h"
+#include "core/common/factory.h"
// YAML
#include <yaml-cpp/yaml.h>
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index aa07e14a3cf1e3e4c2e55f93d9f71cf88b038807..604770acc4629ba97125e1da097d6e5871083983 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -21,40 +21,18 @@
//--------LICENSE_END--------
// wolf includes
#include "core/problem/problem.h"
-#include "core/hardware/hardware_base.h"
-#include "core/trajectory/trajectory_base.h"
-#include "core/map/map_base.h"
#include "core/map/factory_map.h"
-#include "core/sensor/sensor_base.h"
#include "core/sensor/factory_sensor.h"
#include "core/processor/factory_processor.h"
-#include "core/processor/processor_motion.h"
-#include "core/processor/processor_tracker.h"
-#include "core/capture/capture_pose.h"
#include "core/capture/capture_void.h"
-#include "core/factor/factor_base.h"
#include "core/factor/factor_block_absolute.h"
#include "core/factor/factor_quaternion_absolute.h"
-#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
#include "core/state_block/state_angle.h"
#include "core/tree_manager/factory_tree_manager.h"
#include "core/tree_manager/tree_manager_base.h"
-
-#include "core/utils/params_server.h"
#include "core/utils/loader.h"
-#include "core/utils/check_log.h"
-#include "core/math/covariance.h"
-#include "core/state_block/factory_state_block.h"
-
-// C++ includes
-#include <algorithm>
-#include <map>
-#include <sstream>
-#include <stdexcept>
-#include <string>
-#include <vector>
-#include <unordered_set>
+
namespace wolf
{
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 512d941b7669c13e22a87aee23c5a8f84ed44329..3d3794e75b1a87461832695c26341b8f40fc11f5 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -216,10 +216,6 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
last_ptr_->getFrame()->link(getProblem());
- // // make F; append incoming to new F
- // FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp());
- // incoming_ptr_->link(frm);
-
// Establish factors
establishFactors();
@@ -232,7 +228,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// make F; append incoming to new F
FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
- last_ptr_->getFrame()->getState());
+ getProblem()->getState(incoming_ptr_->getTimeStamp()));
incoming_ptr_ ->link(frame);
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
@@ -250,7 +246,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Replace last frame for a new one in incoming
FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
- last_ptr_->getFrame()->getState());
+ getProblem()->getState(incoming_ptr_->getTimeStamp()));
incoming_ptr_->link(frame);
last_ptr_->unlink(); // unlink last (destroying the frame) instead of frame destruction that would implicitly destroy last
diff --git a/src/yaml/processor_odom_3d_yaml.cpp b/src/yaml/processor_odom_3d_yaml.cpp
index a8dd748ea29aa2a63eb72003b5c37465d62c3d17..771d19b7414b2a4a7342c8afea09d08d19f1b750 100644
--- a/src/yaml/processor_odom_3d_yaml.cpp
+++ b/src/yaml/processor_odom_3d_yaml.cpp
@@ -31,7 +31,7 @@
// wolf
#include "core/processor/processor_odom_3d.h"
-#include "core/common/factory.h"
+#include "core/processor/factory_processor.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
diff --git a/src/yaml/sensor_odom_2d_yaml.cpp b/src/yaml/sensor_odom_2d_yaml.cpp
index 68a44ff61adaabb99cb16cc1bc1c72017de6d813..dcb0cf0c10e428a6dfce876a1765e8c0d245e1a2 100644
--- a/src/yaml/sensor_odom_2d_yaml.cpp
+++ b/src/yaml/sensor_odom_2d_yaml.cpp
@@ -33,6 +33,7 @@
// wolf
#include "core/sensor/sensor_odom_2d.h"
+#include "core/sensor/factory_sensor.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
diff --git a/src/yaml/sensor_odom_3d_yaml.cpp b/src/yaml/sensor_odom_3d_yaml.cpp
index 02e0ff3d8e5296398a5f9cfc52b66fd21cf15d85..0cb47d441d9b8a0786a9fcf8d00952a682072c4a 100644
--- a/src/yaml/sensor_odom_3d_yaml.cpp
+++ b/src/yaml/sensor_odom_3d_yaml.cpp
@@ -31,7 +31,7 @@
// wolf
#include "core/sensor/sensor_odom_3d.h"
-#include "core/common/factory.h"
+#include "core/sensor/factory_sensor.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
diff --git a/src/yaml/sensor_pose_yaml.cpp b/src/yaml/sensor_pose_yaml.cpp
index 102729bd91dcfca4f51a567f47d790ce1de6c6b7..bfbe151c43315c21e5f636e2632fa9ceb614c9d4 100644
--- a/src/yaml/sensor_pose_yaml.cpp
+++ b/src/yaml/sensor_pose_yaml.cpp
@@ -31,7 +31,7 @@
// wolf
#include "core/sensor/sensor_pose.h"
-#include "core/common/factory.h"
+#include "core/sensor/factory_sensor.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
diff --git a/test/gtest_processor_odom_3d.cpp b/test/gtest_processor_odom_3d.cpp
index a5e0beabb5447222a27bd02138144997d3389a00..23c0afdf5a4bcf4ef2586d7e63b692ae4c953906 100644
--- a/test/gtest_processor_odom_3d.cpp
+++ b/test/gtest_processor_odom_3d.cpp
@@ -149,7 +149,7 @@ TEST(ProcessorOdom3d, deltaPlusDelta_Jac)
MatrixXd J_D(7,7), J_d(7,7);
- JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, Constants::EPS);
+ JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, 1000*Constants::EPS);
WOLF_DEBUG("J_D:\n ", J_D);
WOLF_DEBUG("J_d:\n ", J_d);