Rationalize includes

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>

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>

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
 {

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
 

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>

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>

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>

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>

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);