diff --git a/CMakeLists.txt b/CMakeLists.txt
index fef41cbba74fb13c7d02841c2ddbb876a0c28959..d920cc8a3840fc704857f591bda8d4b0eed1c420 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -100,7 +100,7 @@ option(_WOLF_TRACE "Enable wolf tracing macro" ON)
# option(BUILD_EXAMPLES "Build examples" OFF)
set(BUILD_TESTS true)
-set(BUILD_EXAMPLES true)
+set(BUILD_EXAMPLES false)
# Does this has any other interest
# but for the examples ?
@@ -254,41 +254,54 @@ ENDIF(GLOG_FOUND)
#HEADERS
-SET(HDRS_BASE
- include/base/diff_drive_tools.h
- include/base/diff_drive_tools.hpp
- include/base/eigen_assert.h
- include/base/eigen_predicates.h
- include/base/factory.h
- include/base/frame_base.h
- include/base/hardware_base.h
- include/base/IMU_tools.h
- include/base/local_parametrization_angle.h
- include/base/local_parametrization_base.h
- include/base/local_parametrization_homogeneous.h
- include/base/local_parametrization_polyline_extreme.h
- include/base/local_parametrization_quaternion.h
- include/base/logging.h
- include/base/make_unique.h
- include/base/map_base.h
- include/base/motion_buffer.h
- include/base/node_base.h
- include/base/pinhole_tools.h
- include/base/problem.h
- include/base/rotations.h
- include/base/singleton.h
- include/base/state_angle.h
- include/base/state_block.h
- include/base/state_homogeneous_3D.h
- include/base/state_quaternion.h
- include/base/SE3.h
- include/base/time_stamp.h
- include/base/track_matrix.h
- include/base/trajectory_base.h
- include/base/wolf.h
- include/base/IMU_tools.h
- include/base/local_parametrization_polyline_extreme.h
+
+SET(HDRS_COMMON
+ include/base/common/factory.h
+ include/base/common/node_base.h
+ include/base/common/time_stamp.h
+ include/base/common/wolf.h
+ )
+SET(HDRS_MATH
+ include/base/math/pinhole_tools.h
+ include/base/math/SE3.h
+ include/base/math/IMU_tools.h
+ include/base/math/rotations.h
+ )
+SET(HDRS_UTILS
+ include/base/utils/eigen_assert.h
+ include/base/utils/eigen_predicates.h
+ include/base/utils/logging.h
+ include/base/utils/singleton.h
+ include/base/utils/make_unique.h
+ )
+SET(HDRS_PROBLEM
+ include/base/problem/problem.h
+ )
+SET(HDRS_HARDWARE
+ include/base/hardware/hardware_base.h
+ )
+SET(HDRS_TRAJECTORY
+ include/base/trajectory/trajectory_base.h
+ )
+SET(HDRS_MAP
+ include/base/map/map_base.h
)
+SET(HDRS_FRAME
+ include/base/frame/frame_base.h
+ )
+SET(HDRS_STATE_BLOCK
+ include/base/state_block/local_parametrization_angle.h
+ include/base/state_block/local_parametrization_base.h
+ include/base/state_block/local_parametrization_homogeneous.h
+ include/base/state_block/local_parametrization_polyline_extreme.h
+ include/base/state_block/local_parametrization_quaternion.h
+ include/base/state_block/state_angle.h
+ include/base/state_block/state_block.h
+ include/base/state_block/state_homogeneous_3D.h
+ include/base/state_block/state_quaternion.h
+ include/base/state_block/local_parametrization_polyline_extreme.h
+ )
+
SET(HDRS_CAPTURE
include/base/capture/capture_base.h
include/base/capture/capture_buffer.h
@@ -383,29 +396,33 @@ SET(HDRS_LANDMARK
include/base/landmark/landmark_polyline_2D.h
)
SET(HDRS_PROCESSOR
+ include/base/processor/diff_drive_tools.h
+ include/base/processor/diff_drive_tools.hpp
+ include/base/processor/motion_buffer.h
+ include/base/processor/processor_IMU.h
+ include/base/processor/processor_IMU.h
+ include/base/processor/processor_base.h
include/base/processor/processor_capture_holder.h
include/base/processor/processor_diff_drive.h
+ include/base/processor/processor_factory.h
+ include/base/processor/processor_factory.h
include/base/processor/processor_frame_nearest_neighbor_filter.h
- include/base/processor/processor_IMU.h
+ include/base/processor/processor_frame_nearest_neighbor_filter.h
+ include/base/processor/processor_logging.h
+ include/base/processor/processor_loopclosure_base.h
+ include/base/processor/processor_motion.h
+ include/base/processor/processor_odom_2D.h
include/base/processor/processor_odom_2D.h
include/base/processor/processor_odom_3D.h
+ include/base/processor/processor_odom_3D.h
+ include/base/processor/processor_tracker.h
+ include/base/processor/processor_tracker_feature.h
+ include/base/processor/processor_tracker_feature_dummy.h
include/base/processor/processor_tracker_feature_dummy.h
include/base/processor/processor_tracker_landmark.h
include/base/processor/processor_tracker_landmark_dummy.h
- include/base/processor/processor_frame_nearest_neighbor_filter.h
- include/base/processor/processor_IMU.h
- include/base/processor/processor_odom_2D.h
- include/base/processor/processor_odom_3D.h
- include/base/processor/processor_tracker_feature_dummy.h
include/base/processor/processor_tracker_landmark_dummy.h
- include/base/processor/processor_factory.h
- include/base/processor/processor_logging.h
- include/base/processor/processor_base.h
- include/base/processor/processor_factory.h
- include/base/processor/processor_loopclosure_base.h
- include/base/processor/processor_motion.h
- include/base/processor/processor_tracker_feature.h
- include/base/processor/processor_tracker.h
+ include/base/processor/track_matrix.h
)
SET(HDRS_SENSOR
include/base/sensor/sensor_base.h
@@ -427,101 +444,55 @@ SET(HDRS_SENSOR
SET(HDRS_SOLVER
include/base/solver/solver_manager.h
)
-# [Add generic derived header before this line]
SET(HDRS_DTASSC
- include/base/track_matrix.h
include/base/association/association_solver.h
include/base/association/association_node.h
include/base/association/association_tree.h
include/base/association/association_nnls.h
)
-SET(HDRS_CORE
- include/base/capture/capture_base.h
- include/base/capture/capture_buffer.h
- include/base/capture/capture_pose.h
- include/base/capture/capture_void.h
- include/base/factor/factor_analytic.h
- include/base/factor/factor_autodiff.h
- include/base/factor/factor_base.h
- include/base/processor/processor_factory.h
- include/base/feature/feature_base.h
- include/base/feature/feature_match.h
- include/base/feature/feature_pose.h
- include/base/frame_base.h
- include/base/hardware_base.h
- include/base/landmark/landmark_base.h
- include/base/local_parametrization_angle.h
- include/base/local_parametrization_base.h
- include/base/local_parametrization_homogeneous.h
- include/base/local_parametrization_quaternion.h
- include/base/processor/processor_logging.h
- include/base/map_base.h
- include/base/motion_buffer.h
- include/base/node_base.h
- include/base/problem.h
- include/base/processor/processor_base.h
- include/base/processor/processor_loopclosure_base.h
- include/base/processor/processor_motion.h
- include/base/processor/processor_tracker_feature.h
- include/base/processor/processor_tracker.h
- include/base/rotations.h
- include/base/sensor/sensor_base.h
- include/base/processor/processor_factory.h
- include/base/singleton.h
- include/base/state_angle.h
- include/base/state_block.h
- include/base/state_homogeneous_3D.h
- include/base/state_quaternion.h
- include/base/SE3.h
- include/base/time_stamp.h
- include/base/track_matrix.h
- include/base/trajectory_base.h
- include/base/wolf.h
- )
#SOURCES
-SET(SRCS_CORE
- src/capture/capture_base.cpp
- src/capture/capture_pose.cpp
- src/capture/capture_void.cpp
- src/factor/factor_analytic.cpp
- src/factor/factor_base.cpp
- src/feature/feature_base.cpp
- src/feature/feature_pose.cpp
- src/frame_base.cpp
- src/hardware_base.cpp
- src/landmark/landmark_base.cpp
- src/local_parametrization_base.cpp
- src/local_parametrization_homogeneous.cpp
- src/local_parametrization_quaternion.cpp
- src/map_base.cpp
- src/motion_buffer.cpp
- src/node_base.cpp
- src/problem.cpp
- src/processor/processor_base.cpp
- src/processor/processor_loopclosure_base.cpp
- src/processor/processor_motion.cpp
- src/processor/processor_tracker.cpp
- src/sensor/sensor_base.cpp
- src/state_block.cpp
- src/time_stamp.cpp
- src/track_matrix.cpp
- src/trajectory_base.cpp
+SET(SRCS_PROBLEM
+ src/problem/problem.cpp
)
-
-SET(SRCS_BASE
- src/capture/capture_motion.cpp
- src/processor/processor_capture_holder.cpp
- # examples/test_processor_tracker_landmark.cpp
+SET(SRCS_HARDWARE
+ src/hardware/hardware_base.cpp
+ )
+SET(SRCS_TRAJECTORY
+ src/trajectory/trajectory_base.cpp
+ )
+SET(SRCS_MAP
+ src/map/map_base.cpp
+ )
+SET(SRCS_FRAME
+ src/frame/frame_base.cpp
+ )
+SET(SRCS_STATE_BLOCK
+ src/state_block/state_block.cpp
+ src/state_block/local_parametrization_base.cpp
+ src/state_block/local_parametrization_homogeneous.cpp
+ src/state_block/local_parametrization_quaternion.cpp
+ src/state_block/local_parametrization_polyline_extreme.cpp
+ )
+SET(SRCS_COMMON
+ src/common/node_base.cpp
+ src/common/time_stamp.cpp
+ )
+SET(SRCS_MATH
+ )
+SET(SRCS_UTILS
)
SET(SRCS
- src/local_parametrization_polyline_extreme.cpp
test/processor_IMU_UnitTester.cpp
)
SET(SRCS_CAPTURE
+ src/capture/capture_motion.cpp
+ src/capture/capture_base.cpp
+ src/capture/capture_pose.cpp
+ src/capture/capture_void.cpp
src/capture/capture_GPS_fix.cpp
src/capture/capture_IMU.cpp
src/capture/capture_odom_2D.cpp
@@ -529,33 +500,48 @@ SET(SRCS_CAPTURE
src/capture/capture_velocity.cpp
src/capture/capture_wheel_joint_position.cpp
)
+SET(SRCS_FACTOR
+ src/factor/factor_analytic.cpp
+ src/factor/factor_base.cpp
+ )
SET(SRCS_FEATURE
- src/feature/feature_corner_2D.cpp
- src/feature/feature_diff_drive.cpp
src/feature/feature_GPS_fix.cpp
src/feature/feature_GPS_pseudorange.cpp
src/feature/feature_IMU.cpp
+ src/feature/feature_base.cpp
+ src/feature/feature_corner_2D.cpp
+ src/feature/feature_diff_drive.cpp
src/feature/feature_odom_2D.cpp
src/feature/feature_polyline_2D.cpp
+ src/feature/feature_pose.cpp
)
SET(SRCS_LANDMARK
src/landmark/landmark_corner_2D.cpp
src/landmark/landmark_container.cpp
src/landmark/landmark_line_2D.cpp
src/landmark/landmark_polyline_2D.cpp
+ src/landmark/landmark_base.cpp
)
SET(SRCS_PROCESSOR
- src/processor/processor_frame_nearest_neighbor_filter.cpp
- src/processor/processor_diff_drive.cpp
+ src/processor/motion_buffer.cpp
src/processor/processor_IMU.cpp
+ src/processor/processor_base.cpp
+ src/processor/processor_capture_holder.cpp
+ src/processor/processor_diff_drive.cpp
+ src/processor/processor_frame_nearest_neighbor_filter.cpp
+ src/processor/processor_loopclosure_base.cpp
+ src/processor/processor_motion.cpp
src/processor/processor_odom_2D.cpp
src/processor/processor_odom_3D.cpp
+ src/processor/processor_tracker.cpp
src/processor/processor_tracker_feature.cpp
src/processor/processor_tracker_feature_dummy.cpp
- src/processor/processor_tracker_landmark_dummy.cpp
src/processor/processor_tracker_landmark.cpp
+ src/processor/processor_tracker_landmark_dummy.cpp
+ src/processor/track_matrix.cpp
)
SET(SRCS_SENSOR
+ src/sensor/sensor_base.cpp
src/sensor/sensor_camera.cpp
src/sensor/sensor_diff_drive.cpp
src/sensor/sensor_GPS.cpp
@@ -603,7 +589,6 @@ IF (laser_scan_utils_FOUND)
SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
include/base/processor/processor_tracker_feature_corner.h
include/base/processor/processor_tracker_landmark_corner.h
- include/base/processor/processor_tracker_landmark_polyline.h
)
SET(HDRS_SENSOR ${HDRS_SENSOR}
include/base/sensor/sensor_laser_2D.h
@@ -612,7 +597,6 @@ IF (laser_scan_utils_FOUND)
src/sensor/sensor_laser_2D.cpp
src/processor/processor_tracker_feature_corner.cpp
src/processor/processor_tracker_landmark_corner.cpp
- src/processor/processor_tracker_landmark_polyline.cpp
)
ENDIF(laser_scan_utils_FOUND)
@@ -706,17 +690,25 @@ ENDIF(YAMLCPP_FOUND)
ADD_LIBRARY(${PROJECT_NAME}
SHARED
${SRCS_BASE}
- ${SRCS_CORE}
- ${SRCS}
${SRCS_CAPTURE}
+ ${SRCS_COMMON}
+ ${SRCS_DTASSC}
${SRCS_FACTOR}
${SRCS_FEATURE}
+ ${SRCS_FRAME}
+ ${SRCS_HARDWARE}
${SRCS_LANDMARK}
+ ${SRCS_MAP}
+ ${SRCS_MATH}
+ ${SRCS_PROBLEM}
${SRCS_PROCESSOR}
${SRCS_SENSOR}
- #${SRCS_DTASSC}
${SRCS_SOLVER}
+ ${SRCS_STATE_BLOCK}
+ ${SRCS_TRAJECTORY}
+ ${SRCS_UTILS}
${SRCS_WRAPPER}
+ ${SRCS}
)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
@@ -766,40 +758,56 @@ ENDIF(BUILD_EXAMPLES)
#=============================================================
INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Targets
- RUNTIME DESTINATION bin
- LIBRARY DESTINATION lib/iri-algorithms
- ARCHIVE DESTINATION lib/iri-algorithms)
+ RUNTIME DESTINATION bin
+ LIBRARY DESTINATION lib/iri-algorithms
+ ARCHIVE DESTINATION lib/iri-algorithms)
install(EXPORT ${PROJECT_NAME}Targets DESTINATION lib/cmake/${PROJECT_NAME})
#install headers
-INSTALL(FILES ${HDRS_BASE}
- DESTINATION include/iri-algorithms/wolf/base)
+INSTALL(FILES ${HDRS_MATH}
+ DESTINATION include/iri-algorithms/wolf/base/math)
+INSTALL(FILES ${HDRS_UTILS}
+ DESTINATION include/iri-algorithms/wolf/base/utils)
+INSTALL(FILES ${HDRS_PROBLEM}
+ DESTINATION include/iri-algorithms/wolf/base/problem)
+INSTALL(FILES ${HDRS_HARDWARE}
+ DESTINATION include/iri-algorithms/wolf/base/hardware)
+INSTALL(FILES ${HDRS_TRAJECTORY}
+ DESTINATION include/iri-algorithms/wolf/base/trajectory)
+INSTALL(FILES ${HDRS_MAP}
+ DESTINATION include/iri-algorithms/wolf/base/map)
+INSTALL(FILES ${HDRS_FRAME}
+ DESTINATION include/iri-algorithms/wolf/base/frame)
+INSTALL(FILES ${HDRS_STATE_BLOCK}
+ DESTINATION include/iri-algorithms/wolf/base/state_block)
+INSTALL(FILES ${HDRS_COMMON}
+ DESTINATION include/iri-algorithms/wolf/base/common)
INSTALL(FILES ${HDRS_DTASSC}
- DESTINATION include/iri-algorithms/wolf/base/association)
+ DESTINATION include/iri-algorithms/wolf/base/association)
INSTALL(FILES ${HDRS_CAPTURE}
- DESTINATION include/iri-algorithms/wolf/base/capture)
+ DESTINATION include/iri-algorithms/wolf/base/capture)
INSTALL(FILES ${HDRS_FACTOR}
- DESTINATION include/iri-algorithms/wolf/base/factor)
+ DESTINATION include/iri-algorithms/wolf/base/factor)
INSTALL(FILES ${HDRS_FEATURE}
- DESTINATION include/iri-algorithms/wolf/base/feature)
+ DESTINATION include/iri-algorithms/wolf/base/feature)
INSTALL(FILES ${HDRS_SENSOR}
- DESTINATION include/iri-algorithms/wolf/base/sensor)
+ DESTINATION include/iri-algorithms/wolf/base/sensor)
INSTALL(FILES ${HDRS_PROCESSOR}
- DESTINATION include/iri-algorithms/wolf/base/processor)
+ DESTINATION include/iri-algorithms/wolf/base/processor)
INSTALL(FILES ${HDRS_LANDMARK}
- DESTINATION include/iri-algorithms/wolf/base/landmark)
+ DESTINATION include/iri-algorithms/wolf/base/landmark)
INSTALL(FILES ${HDRS_WRAPPER}
- DESTINATION include/iri-algorithms/wolf/base/ceres_wrapper)
+ DESTINATION include/iri-algorithms/wolf/base/ceres_wrapper)
INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
- DESTINATION include/iri-algorithms/wolf/base/solver_suitesparse)
+ DESTINATION include/iri-algorithms/wolf/base/solver_suitesparse)
INSTALL(FILES ${HDRS_SOLVER}
- DESTINATION include/iri-algorithms/wolf/base/solver)
+ DESTINATION include/iri-algorithms/wolf/base/solver)
INSTALL(FILES ${HDRS_SERIALIZATION}
- DESTINATION include/iri-algorithms/wolf/base/serialization)
+ DESTINATION include/iri-algorithms/wolf/base/serialization)
INSTALL(FILES ${HDRS_YAML}
- DESTINATION include/iri-algorithms/wolf/base/yaml)
+ DESTINATION include/iri-algorithms/wolf/base/yaml)
INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/Findwolf.cmake"
- DESTINATION "lib/cmake/${PROJECT_NAME}")
+ DESTINATION "lib/cmake/${PROJECT_NAME}")
#install Find*.cmake
configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/wolfConfig.cmake"
diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index f7c8d1c0d158f13cc8065dd8a077b8bc7a0c49bf..3708f51d3aa1141756dc54c27f76039671bfa42e 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/hello_wolf/hello_wolf.cpp
@@ -12,7 +12,7 @@
* \author: jsola
*/
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include "base/sensor/sensor_odom_2D.h"
#include "base/processor/processor_odom_2D.h"
@@ -246,9 +246,17 @@ int main()
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
for (auto kf : problem->getTrajectory()->getFrameList())
if (kf->isKey())
- WOLF_TRACE("KF", kf->id(), "_cov = \n", kf->getCovariance());
+ {
+ Eigen::MatrixXs cov;
+ kf->getCovariance(cov);
+ WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
+ }
for (auto lmk : problem->getMap()->getLandmarkList())
- WOLF_TRACE("L", lmk->id(), "_cov = \n", lmk->getCovariance());
+ {
+ Eigen::MatrixXs cov;
+ lmk->getCovariance(cov);
+ WOLF_TRACE("L", lmk->id(), "_cov = \n", cov);
+ }
std::cout << std::endl;
WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
diff --git a/hello_wolf/sensor_range_bearing.cpp b/hello_wolf/sensor_range_bearing.cpp
index 04d704664e8023826fb0b3faccde05a585a7b47a..591800ae63b6a2a19ed05e6d35358747ba6fc8b1 100644
--- a/hello_wolf/sensor_range_bearing.cpp
+++ b/hello_wolf/sensor_range_bearing.cpp
@@ -6,7 +6,7 @@
*/
#include "sensor_range_bearing.h"
-#include "base/state_angle.h"
+#include "base/state_block/state_angle.h"
namespace wolf
{
diff --git a/include/base/capture/capture_IMU.h b/include/base/capture/capture_IMU.h
index 6e2de9d9d91eee27afb6b46940f2c5558ee0fddb..9308c1bc0aa44a56b472fc8c67c74da449776318 100644
--- a/include/base/capture/capture_IMU.h
+++ b/include/base/capture/capture_IMU.h
@@ -2,7 +2,7 @@
#define CAPTURE_IMU_H
//Wolf includes
-#include "base/IMU_tools.h"
+#include "base/math/IMU_tools.h"
#include "base/capture/capture_motion.h"
namespace wolf {
diff --git a/include/base/capture/capture_base.h b/include/base/capture/capture_base.h
index bcc3c84f326ee575ba9ea1fdc027effcb98f0f4a..9c2f1037497533ddb24ec3ef6c45024d6cbd81d7 100644
--- a/include/base/capture/capture_base.h
+++ b/include/base/capture/capture_base.h
@@ -8,9 +8,9 @@ class FeatureBase;
}
//Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
-#include "base/time_stamp.h"
+#include "base/common/wolf.h"
+#include "base/common/node_base.h"
+#include "base/common/time_stamp.h"
//std includes
@@ -57,7 +57,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
void setTimeStampToNow();
FrameBasePtr getFrame() const;
- void setFramePtr(const FrameBasePtr _frm_ptr);
+ void setFrame(const FrameBasePtr _frm_ptr);
void unlinkFromFrame(){frame_ptr_.reset();}
virtual void setProblem(ProblemPtr _problem) final;
@@ -69,7 +69,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
void getFactorList(FactorBasePtrList& _fac_list);
SensorBasePtr getSensor() const;
- virtual void setSensorPtr(const SensorBasePtr sensor_ptr);
+ virtual void setSensor(const SensorBasePtr sensor_ptr);
// constrained by
virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
@@ -79,8 +79,8 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
// State blocks
const std::vector<StateBlockPtr>& getStateBlockVec() const;
std::vector<StateBlockPtr>& getStateBlockVec();
- StateBlockPtr getStateBlockPtr(unsigned int _i) const;
- void setStateBlockPtr(unsigned int _i, const StateBlockPtr _sb_ptr);
+ StateBlockPtr getStateBlock(unsigned int _i) const;
+ void setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr);
StateBlockPtr getSensorP() const;
StateBlockPtr getSensorO() const;
@@ -113,9 +113,9 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
}
#include "base/sensor/sensor_base.h"
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
#include "base/feature/feature_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
namespace wolf{
@@ -154,24 +154,24 @@ inline std::vector<StateBlockPtr>& CaptureBase::getStateBlockVec()
return state_block_vec_;
}
-inline void CaptureBase::setStateBlockPtr(unsigned int _i, const StateBlockPtr _sb_ptr)
+inline void CaptureBase::setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr)
{
state_block_vec_[_i] = _sb_ptr;
}
inline StateBlockPtr CaptureBase::getSensorP() const
{
- return getStateBlockPtr(0);
+ return getStateBlock(0);
}
inline StateBlockPtr CaptureBase::getSensorO() const
{
- return getStateBlockPtr(1);
+ return getStateBlock(1);
}
inline StateBlockPtr CaptureBase::getSensorIntrinsic() const
{
- return getStateBlockPtr(2);
+ return getStateBlock(2);
}
inline unsigned int CaptureBase::id()
@@ -184,7 +184,7 @@ inline FrameBasePtr CaptureBase::getFrame() const
return frame_ptr_.lock();
}
-inline void CaptureBase::setFramePtr(const FrameBasePtr _frm_ptr)
+inline void CaptureBase::setFrame(const FrameBasePtr _frm_ptr)
{
frame_ptr_ = _frm_ptr;
}
@@ -214,7 +214,7 @@ inline SensorBasePtr CaptureBase::getSensor() const
return sensor_ptr_.lock();
}
-inline void CaptureBase::setSensorPtr(const SensorBasePtr sensor_ptr)
+inline void CaptureBase::setSensor(const SensorBasePtr sensor_ptr)
{
sensor_ptr_ = sensor_ptr;
}
diff --git a/include/base/capture/capture_buffer.h b/include/base/capture/capture_buffer.h
index 434716dcfdf596b09d037a62f136428bc9d3e893..393d7fc2489a5035b168b2684788b835a772ab6e 100644
--- a/include/base/capture/capture_buffer.h
+++ b/include/base/capture/capture_buffer.h
@@ -8,8 +8,8 @@
#ifndef _WOLF_CAPTURE_BUFFER_H_
#define _WOLF_CAPTURE_BUFFER_H_
-#include "base/wolf.h"
-#include "base/time_stamp.h"
+#include "base/common/wolf.h"
+#include "base/common/time_stamp.h"
#include <list>
#include <algorithm>
diff --git a/include/base/capture/capture_laser_2D.h b/include/base/capture/capture_laser_2D.h
index 9abd6bf4b874a8970d71929355aed095e52c3838..96a434547b5e802376472f472be0aeb0e27af940 100644
--- a/include/base/capture/capture_laser_2D.h
+++ b/include/base/capture/capture_laser_2D.h
@@ -28,7 +28,7 @@ class CaptureLaser2D : public CaptureBase
laserscanutils::LaserScan& getScan();
- void setSensorPtr(const SensorBasePtr sensor_ptr);
+ void setSensor(const SensorBasePtr sensor_ptr);
private:
SensorLaser2DPtr laser_ptr_; //specific pointer to sensor laser 2D object
@@ -41,9 +41,9 @@ inline laserscanutils::LaserScan& CaptureLaser2D::getScan()
return scan_;
}
-inline void CaptureLaser2D::setSensorPtr(const SensorBasePtr sensor_ptr)
+inline void CaptureLaser2D::setSensor(const SensorBasePtr sensor_ptr)
{
- CaptureBase::setSensorPtr(sensor_ptr);
+ CaptureBase::setSensor(sensor_ptr);
laser_ptr_ = std::static_pointer_cast<SensorLaser2D>(sensor_ptr);
}
diff --git a/include/base/capture/capture_motion.h b/include/base/capture/capture_motion.h
index ab932da224bd610c130f6bc9f108e8d746c75f7e..0f84eb72d1737c8e208d25ceb9cacd224b3627b3 100644
--- a/include/base/capture/capture_motion.h
+++ b/include/base/capture/capture_motion.h
@@ -10,7 +10,7 @@
// Wolf includes
#include "base/capture/capture_base.h"
-#include "base/motion_buffer.h"
+#include "base/processor/motion_buffer.h"
// STL includes
#include <list>
@@ -97,7 +97,7 @@ class CaptureMotion : public CaptureBase
// Origin frame
FrameBasePtr getOriginFrame();
- void setOriginFramePtr(FrameBasePtr _frame_ptr);
+ void setOriginFrame(FrameBasePtr _frame_ptr);
// member data:
private:
@@ -161,7 +161,7 @@ inline FrameBasePtr CaptureMotion::getOriginFrame()
return origin_frame_ptr_.lock();
}
-inline void CaptureMotion::setOriginFramePtr(FrameBasePtr _frame_ptr)
+inline void CaptureMotion::setOriginFrame(FrameBasePtr _frame_ptr)
{
origin_frame_ptr_ = _frame_ptr;
}
diff --git a/include/base/capture/capture_odom_2D.h b/include/base/capture/capture_odom_2D.h
index eafb6ead7bbe4b12f07234dc896f8fe7729b07bf..ff532803c44cbe2857eae956152742560a34e270 100644
--- a/include/base/capture/capture_odom_2D.h
+++ b/include/base/capture/capture_odom_2D.h
@@ -10,7 +10,7 @@
#include "base/capture/capture_motion.h"
-#include "base/rotations.h"
+#include "base/math/rotations.h"
namespace wolf
{
diff --git a/include/base/capture/capture_odom_3D.h b/include/base/capture/capture_odom_3D.h
index d1f29508bb60fa7bae5c620b7b010f368e00f100..863add2684a022fc1235b6b532be5ed01655a428 100644
--- a/include/base/capture/capture_odom_3D.h
+++ b/include/base/capture/capture_odom_3D.h
@@ -10,7 +10,7 @@
#include "base/capture/capture_motion.h"
-#include "base/rotations.h"
+#include "base/math/rotations.h"
namespace wolf
{
diff --git a/include/base/capture/capture_wheel_joint_position.h b/include/base/capture/capture_wheel_joint_position.h
index 0bade4029f499ba78b6cc71a7f260f05df44a79e..a90cdb205178dd4c36cd302a7793e7e74c668df1 100644
--- a/include/base/capture/capture_wheel_joint_position.h
+++ b/include/base/capture/capture_wheel_joint_position.h
@@ -155,7 +155,7 @@ protected:
// ~CaptureWheelJointPosition() = default;
-//// void setSensorPtr(const SensorBasePtr sensor_ptr) override;
+//// void setSensor(const SensorBasePtr sensor_ptr) override;
// std::size_t getNumWheels() const noexcept
// {
diff --git a/include/base/ceres_wrapper/ceres_manager.h b/include/base/ceres_wrapper/ceres_manager.h
index f937332f15aec0235ff3b9ada7254cffba47461d..d156fd80521b31da81975ea11f55d9949fbb3754 100644
--- a/include/base/ceres_wrapper/ceres_manager.h
+++ b/include/base/ceres_wrapper/ceres_manager.h
@@ -9,7 +9,7 @@
//wolf includes
#include "base/solver/solver_manager.h"
#include "base/ceres_wrapper/cost_function_wrapper.h"
-#include "local_parametrization_wrapper.h"
+#include "base/ceres_wrapper/local_parametrization_wrapper.h"
#include "base/ceres_wrapper/create_numeric_diff_cost_function.h"
namespace ceres {
@@ -48,6 +48,11 @@ class CeresManager : public SolverManager
ceres::Solver::Summary getSummary();
+ std::unique_ptr<ceres::Problem>& getCeresProblem()
+ {
+ return ceres_problem_;
+ }
+
virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks
= CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
diff --git a/include/base/ceres_wrapper/cost_function_wrapper.h b/include/base/ceres_wrapper/cost_function_wrapper.h
index 4a3f42dcbbc1aeea006e7843941707d4a2b9e5c5..2847d9b9a675962fee76e95e3673537568ef4a3f 100644
--- a/include/base/ceres_wrapper/cost_function_wrapper.h
+++ b/include/base/ceres_wrapper/cost_function_wrapper.h
@@ -2,7 +2,7 @@
#define TRUNK_SRC_COST_FUNCTION_WRAPPER_H_
// WOLF
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include "base/factor/factor_analytic.h"
// CERES
diff --git a/include/base/ceres_wrapper/local_parametrization_wrapper.h b/include/base/ceres_wrapper/local_parametrization_wrapper.h
index fc046a7ec002c29b702133e89fb950b456a764d1..31a2913ec22bc0e5dc5ba6b2757e49508d195578 100644
--- a/include/base/ceres_wrapper/local_parametrization_wrapper.h
+++ b/include/base/ceres_wrapper/local_parametrization_wrapper.h
@@ -7,7 +7,7 @@ class LocalParametrizationBase;
}
//Ceres includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include "ceres/ceres.h"
namespace wolf {
@@ -38,7 +38,7 @@ using LocalParametrizationWrapperPtr = std::shared_ptr<LocalParametrizationWrapp
} // namespace wolf
-#include "base/local_parametrization_base.h"
+#include "base/state_block/local_parametrization_base.h"
namespace wolf {
diff --git a/include/base/ceres_wrapper/solver_manager.h b/include/base/ceres_wrapper/solver_manager.h
deleted file mode 100644
index 7252c409d2bf0c503e06b6fe74dec895e3754150..0000000000000000000000000000000000000000
--- a/include/base/ceres_wrapper/solver_manager.h
+++ /dev/null
@@ -1,64 +0,0 @@
-#ifndef SOLVER_MANAGER_H_
-#define SOLVER_MANAGER_H_
-
-//wolf includes
-#include "base/wolf.h"
-#include "base/state_block.h"
-#include "base/factor/factor_base.h"
-
-namespace wolf {
-
-/** \brief Enumeration of covariance blocks to be computed
- *
- * Enumeration of covariance blocks to be computed
- *
- */
-typedef enum
-{
- ALL, ///< All blocks and all cross-covariances
- ALL_MARGINALS, ///< All marginals
- ROBOT_LANDMARKS ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
-} CovarianceBlocksToBeComputed;
-
-WOLF_PTR_TYPEDEFS(SolverManager);
-
-/** \brief Solver manager for WOLF
- *
- */
-
-class SolverManager
-{
- protected:
- ProblemPtr wolf_problem_;
-
- public:
- SolverManager(ProblemPtr _wolf_problem);
-
- virtual ~SolverManager();
-
- virtual std::string solve(const unsigned int& _report_level) = 0;
-
- virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS) = 0;
-
- virtual void computeCovariances(const StateBlockPtrList& st_list) = 0;
-
- virtual void update();
-
- virtual ProblemPtr getProblem();
-
- private:
-
- virtual void addFactor(FactorBasePtr _fac_ptr) = 0;
-
- virtual void removeFactor(FactorBasePtr _fac_ptr) = 0;
-
- virtual void addStateBlock(StateBlockPtr _st_ptr) = 0;
-
- virtual void removeStateBlock(StateBlockPtr _st_ptr) = 0;
-
- virtual void updateStateBlockStatus(StateBlockPtr _st_ptr) = 0;
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/factory.h b/include/base/common/factory.h
similarity index 99%
rename from include/base/factory.h
rename to include/base/common/factory.h
index b161126e45c18e4cfd8a953a32b5247d58fa1d27..03b636e8104ae9e7156a9c9c3649f96f3e1a1aa4 100644
--- a/include/base/factory.h
+++ b/include/base/common/factory.h
@@ -9,7 +9,7 @@
#define FACTORY_H_
// wolf
-#include "base/wolf.h"
+#include "base/common/wolf.h"
// std
#include <string>
@@ -347,7 +347,7 @@ inline std::string LandmarkFactory::getClass()
// Frames
class TimeStamp;
} // namespace wolf
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
namespace wolf{
typedef Factory<FrameBase, const FrameType&, const TimeStamp&, const Eigen::VectorXs&> FrameFactory;
template<>
diff --git a/include/base/node_base.h b/include/base/common/node_base.h
similarity index 99%
rename from include/base/node_base.h
rename to include/base/common/node_base.h
index 5782d9fc7092593ac3f6b13821adcb89f119aed6..65827cbf80c2cf86d9817ea63ef747ca1f74f3b7 100644
--- a/include/base/node_base.h
+++ b/include/base/common/node_base.h
@@ -2,7 +2,7 @@
#define NODE_BASE_H_
// Wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
namespace wolf {
diff --git a/include/base/time_stamp.h b/include/base/common/time_stamp.h
similarity index 99%
rename from include/base/time_stamp.h
rename to include/base/common/time_stamp.h
index 02a5599af607af39d4b16bb3c05a35779cc7bb04..14ed594f6bac7d5aa47b80346691d8dd3abd828b 100644
--- a/include/base/time_stamp.h
+++ b/include/base/common/time_stamp.h
@@ -3,7 +3,7 @@
#define TIME_STAMP_H_
//wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
//C, std
#include <sys/time.h>
diff --git a/include/base/wolf.h b/include/base/common/wolf.h
similarity index 95%
rename from include/base/wolf.h
rename to include/base/common/wolf.h
index 74b7a0bbb3f0f161c7279f1f873a55b1af478935..6b73e59a24673b4267dccd85f7ce81ec8ce68b9e 100644
--- a/include/base/wolf.h
+++ b/include/base/common/wolf.h
@@ -10,7 +10,7 @@
// Enable project-specific definitions and macros
#include "internal/config.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
//includes from Eigen lib
#include <Eigen/Dense>
@@ -31,19 +31,11 @@ namespace wolf {
/**
* \brief scalar type for the Wolf project
*
- * This typedef makes it possible to recompile the whole Wolf project with double, float, or other precisions.
- *
- * To change the project precision, just edit wolf.h and change the precision of this typedef.
- *
* Do NEVER forget to use Wolf scalar definitions when you code!!!
*
* Do NEVER use plain Eigen scalar types!!! (This is redundant with the above. But just to make sure you behave!)
- *
- * The ONLY exception to this rule is when you need special precision. The ONLY example by now is the time stamp which uses double.
*/
-//typedef float Scalar; // Use this for float, 32 bit precision
-typedef double Scalar; // Use this for double, 64 bit precision
-//typedef long double Scalar; // Use this for long double, 128 bit precision
+typedef double Scalar;
/**
* \brief Vector and Matrices size type for the Wolf project
diff --git a/include/base/factor/factor_AHP.h b/include/base/factor/factor_AHP.h
index a6448b06458658949bff014aa93da139f74f0e83..efb0837c8ad23711a2dbb72b3f3479e0db4d566a 100644
--- a/include/base/factor/factor_AHP.h
+++ b/include/base/factor/factor_AHP.h
@@ -6,7 +6,7 @@
#include "base/landmark/landmark_AHP.h"
#include "base/sensor/sensor_camera.h"
//#include "base/feature/feature_point_image.h"
-#include "base/pinhole_tools.h"
+#include "base/math/pinhole_tools.h"
#include <iomanip> //setprecision
diff --git a/include/base/factor/factor_GPS_2D.h b/include/base/factor/factor_GPS_2D.h
index a3ce0e60979b6241fbf2d4f871543764430c8015..bac2e381c4426cfeb8ae1d301d1b84e14d42418a 100644
--- a/include/base/factor/factor_GPS_2D.h
+++ b/include/base/factor/factor_GPS_2D.h
@@ -3,9 +3,9 @@
#define FACTOR_GPS_2D_H_
//Wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
namespace wolf {
diff --git a/include/base/factor/factor_IMU.h b/include/base/factor/factor_IMU.h
index fbb29d1642e91d9fe90f55da673f3b8a87b6c335..942cbf51c2505e05bb5daa0bf0447ddae4c7fdf1 100644
--- a/include/base/factor/factor_IMU.h
+++ b/include/base/factor/factor_IMU.h
@@ -5,7 +5,7 @@
#include "base/feature/feature_IMU.h"
#include "base/sensor/sensor_IMU.h"
#include "base/factor/factor_autodiff.h"
-#include "base/rotations.h"
+#include "base/math/rotations.h"
//Eigen include
diff --git a/include/base/factor/factor_autodiff.h b/include/base/factor/factor_autodiff.h
index be35c772aacb9c7fce4e780d413fa0df675c1655..372891cdd331da1e4837e647cebb3eda215b45b0 100644
--- a/include/base/factor/factor_autodiff.h
+++ b/include/base/factor/factor_autodiff.h
@@ -4,7 +4,7 @@
//Wolf includes
#include "base/factor/factor_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
// CERES
#include "ceres/jet.h"
diff --git a/include/base/factor/factor_autodiff_distance_3D.h b/include/base/factor/factor_autodiff_distance_3D.h
index 0eb1fc7570494b4731656034c9fc56025516c9af..77eb08e2ae049978c7f2978eee3106fcec66ba9c 100644
--- a/include/base/factor/factor_autodiff_distance_3D.h
+++ b/include/base/factor/factor_autodiff_distance_3D.h
@@ -34,7 +34,7 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D,
_feat->getFrame()->getP(),
_frm_other->getP())
{
- setFeaturePtr(_feat);
+ setFeature(_feat);
}
virtual ~FactorAutodiffDistance3D() { /* nothing */ }
diff --git a/include/base/factor/factor_autodiff_trifocal.h b/include/base/factor/factor_autodiff_trifocal.h
index b7e56e0ac17b15d857f05522f42f911de3668291..5b88708c4c222e79aec9497fee0d3932a8a7a01d 100644
--- a/include/base/factor/factor_autodiff_trifocal.h
+++ b/include/base/factor/factor_autodiff_trifocal.h
@@ -2,7 +2,7 @@
#define _FACTOR_AUTODIFF_TRIFOCAL_H_
//Wolf includes
-//#include "base/wolf.h"
+//#include "base/common/wolf.h"
#include "base/factor/factor_autodiff.h"
#include "base/sensor/sensor_camera.h"
@@ -118,7 +118,7 @@ class FactorAutodiffTrifocal : public FactorAutodiff<FactorAutodiffTrifocal, 3,
} // namespace wolf
// Includes for implentation
-#include "base/rotations.h"
+#include "base/math/rotations.h"
namespace wolf
{
@@ -152,7 +152,7 @@ FactorAutodiffTrifocal::FactorAutodiffTrifocal(const FeatureBasePtr& _feature_pr
camera_ptr_(std::static_pointer_cast<SensorCamera>(_processor_ptr->getSensor())),
sqrt_information_upper(Matrix3s::Zero())
{
- setFeaturePtr(_feature_last_ptr);
+ setFeature(_feature_last_ptr);
Matrix3s K_inv = camera_ptr_->getIntrinsicMatrix().inverse();
pixel_canonical_prev_ = K_inv * Vector3s(_feature_prev_ptr ->getMeasurement(0), _feature_prev_ptr ->getMeasurement(1), 1.0);
pixel_canonical_origin_ = K_inv * Vector3s(_feature_origin_ptr->getMeasurement(0), _feature_origin_ptr->getMeasurement(1), 1.0);
diff --git a/include/base/factor/factor_base.h b/include/base/factor/factor_base.h
index f0659caf07e330189967258bdf5c01e4c2c2fb59..0018c5486bb206ea13d18f5e643319f0f80a89c1 100644
--- a/include/base/factor/factor_base.h
+++ b/include/base/factor/factor_base.h
@@ -7,8 +7,8 @@ class FeatureBase;
}
//Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
+#include "base/common/wolf.h"
+#include "base/common/node_base.h"
//std includes
@@ -113,7 +113,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
/** \brief Returns a pointer to the feature constrained from
**/
FeatureBasePtr getFeature() const;
- void setFeaturePtr(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
+ void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
/** \brief Returns a pointer to its capture
**/
@@ -142,22 +142,22 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
/** \brief Returns a pointer to the frame constrained to
**/
FrameBasePtr getFrameOther() const { return frame_other_ptr_.lock(); }
- void setFrameOtherPtr(FrameBasePtr _frm_o) { frame_other_ptr_ = _frm_o; }
+ void setFrameOther(FrameBasePtr _frm_o) { frame_other_ptr_ = _frm_o; }
/** \brief Returns a pointer to the frame constrained to
**/
CaptureBasePtr getCaptureOther() const { return capture_other_ptr_.lock(); }
- void setCaptureOtherPtr(CaptureBasePtr _cap_o) { capture_other_ptr_ = _cap_o; }
+ void setCaptureOther(CaptureBasePtr _cap_o) { capture_other_ptr_ = _cap_o; }
/** \brief Returns a pointer to the feature constrained to
**/
FeatureBasePtr getFeatureOther() const { return feature_other_ptr_.lock(); }
- void setFeatureOtherPtr(FeatureBasePtr _ftr_o) { feature_other_ptr_ = _ftr_o; }
+ void setFeatureOther(FeatureBasePtr _ftr_o) { feature_other_ptr_ = _ftr_o; }
/** \brief Returns a pointer to the landmark constrained to
**/
LandmarkBasePtr getLandmarkOther() const { return landmark_other_ptr_.lock(); }
- void setLandmarkOtherPtr(LandmarkBasePtr _lmk_o){ landmark_other_ptr_ = _lmk_o; }
+ void setLandmarkOther(LandmarkBasePtr _lmk_o){ landmark_other_ptr_ = _lmk_o; }
/**
* @brief getProcessor
@@ -186,8 +186,8 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
// IMPLEMENTATION //
-#include "base/problem.h"
-#include "base/frame_base.h"
+#include "base/problem/problem.h"
+#include "base/frame/frame_base.h"
#include "base/feature/feature_base.h"
#include "base/sensor/sensor_base.h"
#include "base/landmark/landmark_base.h"
diff --git a/include/base/factor/factor_block_absolute.h b/include/base/factor/factor_block_absolute.h
index 4e0da9d764f50d5f55c14c596696b138c9b76035..3990a44123b177461519b97f8ecff84adfdd0fa9 100644
--- a/include/base/factor/factor_block_absolute.h
+++ b/include/base/factor/factor_block_absolute.h
@@ -11,7 +11,7 @@
//Wolf includes
#include "base/factor/factor_analytic.h"
#include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
namespace wolf {
diff --git a/include/base/factor/factor_container.h b/include/base/factor/factor_container.h
index 93d455a8e7e36ce85e1688c5c70a91b020d963c9..7f34ad69f30b4fc8e95216e32e53c762c8880b93 100644
--- a/include/base/factor/factor_container.h
+++ b/include/base/factor/factor_container.h
@@ -2,7 +2,7 @@
#define FACTOR_CONTAINER_H_
//Wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include "base/factor/factor_autodiff.h"
#include "base/landmark/landmark_container.h"
diff --git a/include/base/factor/factor_diff_drive.h b/include/base/factor/factor_diff_drive.h
index b495859c7409031153025fe507c0f10113c92a54..84f535ee73854fa9e207a41b138a98397b170ea9 100644
--- a/include/base/factor/factor_diff_drive.h
+++ b/include/base/factor/factor_diff_drive.h
@@ -10,7 +10,7 @@
//Wolf includes
#include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
#include "base/feature/feature_diff_drive.h"
#include "base/capture/capture_wheel_joint_position.h"
diff --git a/include/base/factor/factor_fix_bias.h b/include/base/factor/factor_fix_bias.h
index 6c1f321dc0f7fcaf965476dc802080e3d3bb23b5..82a13796970e40091e3f99f425a97ebbd0793e4e 100644
--- a/include/base/factor/factor_fix_bias.h
+++ b/include/base/factor/factor_fix_bias.h
@@ -6,8 +6,8 @@
#include "base/capture/capture_IMU.h"
#include "base/feature/feature_IMU.h"
#include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
-#include "base/rotations.h"
+#include "base/frame/frame_base.h"
+#include "base/math/rotations.h"
//#include "ceres/jet.h"
diff --git a/include/base/factor/factor_odom_2D.h b/include/base/factor/factor_odom_2D.h
index e45570d79a8debe284310998cc87f48032521a10..8a9d06747b1171f89b7311f99f0a53f72ab5ca2a 100644
--- a/include/base/factor/factor_odom_2D.h
+++ b/include/base/factor/factor_odom_2D.h
@@ -3,7 +3,7 @@
//Wolf includes
#include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
//#include "ceres/jet.h"
diff --git a/include/base/factor/factor_odom_3D.h b/include/base/factor/factor_odom_3D.h
index ce3456ef0e8c286c7b082a79a41fa268e4608a8e..c3a3cfb834ea5c74e1cdd69d804b72a9ffe8fe48 100644
--- a/include/base/factor/factor_odom_3D.h
+++ b/include/base/factor/factor_odom_3D.h
@@ -9,7 +9,7 @@
#define FACTOR_ODOM_3D_H_
#include "base/factor/factor_autodiff.h"
-#include "base/rotations.h"
+#include "base/math/rotations.h"
namespace wolf
{
diff --git a/include/base/factor/factor_point_2D.h b/include/base/factor/factor_point_2D.h
index 2034260de9483f592e54de2a1e3f125a12c19879..4d9686e3805c9fb5d337ed033e90a904cbb4ae53 100644
--- a/include/base/factor/factor_point_2D.h
+++ b/include/base/factor/factor_point_2D.h
@@ -30,11 +30,11 @@ class FactorPoint2D: public FactorAutodiff<FactorPoint2D, 2,2,1,2,1,2>
const ProcessorBasePtr& _processor_ptr,
unsigned int _ftr_point_id, int _lmk_point_id, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPoint2D,2,2,1,2,1,2>("POINT 2D",
- nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id)),
- feature_point_id_(_ftr_point_id), landmark_point_id_(_lmk_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
+ nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlock(_lmk_point_id)),
+ feature_point_id_(_ftr_point_id), landmark_point_id_(_lmk_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
{
//std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl;
- //std::cout << "landmark state block " << _lmk_ptr->getPointStateBlockPtr(_lmk_point_id)->getVector().transpose() << std::endl;
+ //std::cout << "landmark state block " << _lmk_ptr->getPointStateBlock(_lmk_point_id)->getVector().transpose() << std::endl;
Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
Eigen::MatrixXs measurement_sqrt_covariance = lltOfA.matrixU();
measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition
diff --git a/include/base/factor/factor_point_to_line_2D.h b/include/base/factor/factor_point_to_line_2D.h
index e60c8d3c7ef2677637cb06d86a832064fdaa44ce..d4b891f3ee5d055d3e7500464e9ce59b1780f9f9 100644
--- a/include/base/factor/factor_point_to_line_2D.h
+++ b/include/base/factor/factor_point_to_line_2D.h
@@ -31,8 +31,8 @@ class FactorPointToLine2D: public FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,
unsigned int _ftr_point_id, int _lmk_point_id, int _lmk_point_aux_id,
bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,2,2>("POINT TO LINE 2D",
- nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id), _lmk_ptr->getPointStateBlockPtr(_lmk_point_aux_id)),
- landmark_point_id_(_lmk_point_id), landmark_point_aux_id_(_lmk_point_aux_id), feature_point_id_(_ftr_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_id)), point_aux_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_aux_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
+ nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlock(_lmk_point_id), _lmk_ptr->getPointStateBlock(_lmk_point_aux_id)),
+ landmark_point_id_(_lmk_point_id), landmark_point_aux_id_(_lmk_point_aux_id), feature_point_id_(_ftr_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)), point_aux_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_aux_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
{
//std::cout << "FactorPointToLine2D" << std::endl;
Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
diff --git a/include/base/factor/factor_pose_2D.h b/include/base/factor/factor_pose_2D.h
index 306b5e15ebf9e2e67eb41c8e5072aa050f7d6b32..f1cc286ead2ec56a2d45857add134b1982bbb166 100644
--- a/include/base/factor/factor_pose_2D.h
+++ b/include/base/factor/factor_pose_2D.h
@@ -4,8 +4,8 @@
//Wolf includes
#include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
-#include "base/rotations.h"
+#include "base/frame/frame_base.h"
+#include "base/math/rotations.h"
//#include "ceres/jet.h"
diff --git a/include/base/factor/factor_pose_3D.h b/include/base/factor/factor_pose_3D.h
index b9c4da39516235081a4ec91334fa06958478264c..25273bad2c7c575b8a95d07790a4d7cd0b1fe8e7 100644
--- a/include/base/factor/factor_pose_3D.h
+++ b/include/base/factor/factor_pose_3D.h
@@ -4,8 +4,8 @@
//Wolf includes
#include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
-#include "base/rotations.h"
+#include "base/frame/frame_base.h"
+#include "base/math/rotations.h"
namespace wolf {
diff --git a/include/base/factor/factor_quaternion_absolute.h b/include/base/factor/factor_quaternion_absolute.h
index 1864a6c8eede4bd07190c4da737a7e1f25da117f..abb419afa0ae605265e06dedfb9ce35b9ccb75ff 100644
--- a/include/base/factor/factor_quaternion_absolute.h
+++ b/include/base/factor/factor_quaternion_absolute.h
@@ -10,9 +10,9 @@
//Wolf includes
#include "base/factor/factor_autodiff.h"
-#include "base/local_parametrization_quaternion.h"
-#include "base/frame_base.h"
-#include "base/rotations.h"
+#include "base/state_block/local_parametrization_quaternion.h"
+#include "base/frame/frame_base.h"
+#include "base/math/rotations.h"
namespace wolf {
diff --git a/include/base/feature/feature_IMU.h b/include/base/feature/feature_IMU.h
index dfce11ff78a8e974731efef729c17b0fa72a9fe8..c82083332f925670c61f837fe1605a6a40d6f0ad 100644
--- a/include/base/feature/feature_IMU.h
+++ b/include/base/feature/feature_IMU.h
@@ -4,7 +4,7 @@
//Wolf includes
#include "base/capture/capture_IMU.h"
#include "base/feature/feature_base.h"
-#include "base/wolf.h"
+#include "base/common/wolf.h"
//std includes
diff --git a/include/base/feature/feature_base.h b/include/base/feature/feature_base.h
index cccad80be4c5daec693fc69a858b3bda381b5fd2..7ce447c696201222c2f485f8ace728f140f35b81 100644
--- a/include/base/feature/feature_base.h
+++ b/include/base/feature/feature_base.h
@@ -8,8 +8,8 @@ class FactorBase;
}
//Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
+#include "base/common/wolf.h"
+#include "base/common/node_base.h"
//std includes
@@ -76,7 +76,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
FrameBasePtr getFrame() const;
CaptureBasePtr getCapture() const;
- void setCapturePtr(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
+ void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
FactorBasePtr addFactor(FactorBasePtr _co_ptr);
FactorBasePtrList& getFactorList();
diff --git a/include/base/feature/feature_match.h b/include/base/feature/feature_match.h
index b96ce00239f1fb6293e3dd10b24e6acd345ae8cf..92e4560196e200083294fcad895e25fbb4c711f0 100644
--- a/include/base/feature/feature_match.h
+++ b/include/base/feature/feature_match.h
@@ -2,7 +2,7 @@
#define FEATURE_MATCH_H_
// Wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
//wolf nampseace
namespace wolf {
diff --git a/include/base/frame_base.h b/include/base/frame/frame_base.h
similarity index 89%
rename from include/base/frame_base.h
rename to include/base/frame/frame_base.h
index 2acef22864a24fa8431f62fa6ac1dec4703b64b5..fa6f1c29ecb8c19ffa3d27af8681215b7db57f59 100644
--- a/include/base/frame_base.h
+++ b/include/base/frame/frame_base.h
@@ -10,9 +10,9 @@ class StateBlock;
}
//Wolf includes
-#include "base/wolf.h"
-#include "base/time_stamp.h"
-#include "base/node_base.h"
+#include "base/common/wolf.h"
+#include "base/common/time_stamp.h"
+#include "base/common/node_base.h"
//std includes
@@ -86,17 +86,17 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
const std::vector<StateBlockPtr>& getStateBlockVec() const;
std::vector<StateBlockPtr>& getStateBlockVec();
protected:
- StateBlockPtr getStateBlockPtr(unsigned int _i) const;
- void setStateBlockPtr(unsigned int _i, const StateBlockPtr _sb_ptr);
+ StateBlockPtr getStateBlock(unsigned int _i) const;
+ void setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr);
void resizeStateBlockVec(unsigned int _size);
public:
StateBlockPtr getP() const;
StateBlockPtr getO() const;
StateBlockPtr getV() const;
- void setPPtr(const StateBlockPtr _p_ptr);
- void setOPtr(const StateBlockPtr _o_ptr);
- void setVPtr(const StateBlockPtr _v_ptr);
+ void setP(const StateBlockPtr _p_ptr);
+ void setO(const StateBlockPtr _o_ptr);
+ void setV(const StateBlockPtr _v_ptr);
void registerNewStateBlocks();
void removeStateBlocks();
@@ -113,14 +113,13 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
void getState(Eigen::VectorXs& _state) const;
unsigned int getSize() const;
bool getCovariance(Eigen::MatrixXs& _cov) const;
- Eigen::MatrixXs getCovariance() const;
// Wolf tree access ---------------------------------------------------
public:
virtual void setProblem(ProblemPtr _problem) final;
TrajectoryBasePtr getTrajectory() const;
- void setTrajectoryPtr(TrajectoryBasePtr _trj_ptr);
+ void setTrajectory(TrajectoryBasePtr _trj_ptr);
FrameBasePtr getPreviousFrame() const;
FrameBasePtr getNextFrame() const;
@@ -159,10 +158,10 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
// IMPLEMENTATION //
-#include "base/trajectory_base.h"
+#include "base/trajectory/trajectory_base.h"
#include "base/capture/capture_base.h"
#include "base/factor/factor_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
namespace wolf {
@@ -208,7 +207,7 @@ inline StateBlockPtr FrameBase::getP() const
{
return state_block_vec_[0];
}
-inline void FrameBase::setPPtr(const StateBlockPtr _p_ptr)
+inline void FrameBase::setP(const StateBlockPtr _p_ptr)
{
state_block_vec_[0] = _p_ptr;
}
@@ -217,7 +216,7 @@ inline StateBlockPtr FrameBase::getO() const
{
return state_block_vec_[1];
}
-inline void FrameBase::setOPtr(const StateBlockPtr _o_ptr)
+inline void FrameBase::setO(const StateBlockPtr _o_ptr)
{
state_block_vec_[1] = _o_ptr;
}
@@ -227,18 +226,18 @@ inline StateBlockPtr FrameBase::getV() const
return state_block_vec_[2];
}
-inline void FrameBase::setVPtr(const StateBlockPtr _v_ptr)
+inline void FrameBase::setV(const StateBlockPtr _v_ptr)
{
state_block_vec_[2] = _v_ptr;
}
-inline StateBlockPtr FrameBase::getStateBlockPtr(unsigned int _i) const
+inline StateBlockPtr FrameBase::getStateBlock(unsigned int _i) const
{
assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
return state_block_vec_[_i];
}
-inline void FrameBase::setStateBlockPtr(unsigned int _i, const StateBlockPtr _sb_ptr)
+inline void FrameBase::setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr)
{
assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
state_block_vec_[_i] = _sb_ptr;
@@ -277,7 +276,7 @@ inline FactorBasePtrList& FrameBase::getConstrainedByList()
return constrained_by_list_;
}
-inline void FrameBase::setTrajectoryPtr(TrajectoryBasePtr _trj_ptr)
+inline void FrameBase::setTrajectory(TrajectoryBasePtr _trj_ptr)
{
trajectory_ptr_ = _trj_ptr;
}
diff --git a/include/base/hardware_base.h b/include/base/hardware/hardware_base.h
similarity index 91%
rename from include/base/hardware_base.h
rename to include/base/hardware/hardware_base.h
index 6dd60648b3961dd8f88244b240eebf7c4d88e37d..fb688410b73187cb3d775c00911322afdc602a86 100644
--- a/include/base/hardware_base.h
+++ b/include/base/hardware/hardware_base.h
@@ -8,8 +8,8 @@ class SensorBase;
}
//Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
+#include "base/common/wolf.h"
+#include "base/common/node_base.h"
namespace wolf {
diff --git a/include/base/landmark/landmark_base.h b/include/base/landmark/landmark_base.h
index 0b25830c4d0b0ebe7c4c5ff51ed58638d546c7e3..b150281fcecf0e806c946a78d2e8debd520d2ef2 100644
--- a/include/base/landmark/landmark_base.h
+++ b/include/base/landmark/landmark_base.h
@@ -8,9 +8,9 @@ class StateBlock;
}
//Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
-#include "base/time_stamp.h"
+#include "base/common/wolf.h"
+#include "base/common/node_base.h"
+#include "base/common/time_stamp.h"
//std includes
@@ -63,17 +63,16 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
const std::vector<StateBlockPtr>& getStateBlockVec() const;
std::vector<StateBlockPtr>& getStateBlockVec();
std::vector<StateBlockPtr> getUsedStateBlockVec() const;
- StateBlockPtr getStateBlockPtr(unsigned int _i) const;
- void setStateBlockPtr(unsigned int _i, StateBlockPtr _sb_ptr);
+ StateBlockPtr getStateBlock(unsigned int _i) const;
+ void setStateBlock(unsigned int _i, StateBlockPtr _sb_ptr);
StateBlockPtr getP() const;
StateBlockPtr getO() const;
- void setPPtr(const StateBlockPtr _p_ptr);
- void setOPtr(const StateBlockPtr _o_ptr);
+ void setP(const StateBlockPtr _p_ptr);
+ void setO(const StateBlockPtr _o_ptr);
virtual void registerNewStateBlocks();
Eigen::VectorXs getState() const;
void getState(Eigen::VectorXs& _state) const;
bool getCovariance(Eigen::MatrixXs& _cov) const;
- Eigen::MatrixXs getCovariance() const;
protected:
virtual void removeStateBlocks();
@@ -91,7 +90,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
unsigned int getHits() const;
FactorBasePtrList& getConstrainedByList();
- void setMapPtr(const MapBasePtr _map_ptr);
+ void setMap(const MapBasePtr _map_ptr);
MapBasePtr getMap();
void link(MapBasePtr);
template<typename classType, typename... T>
@@ -101,9 +100,9 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
}
-#include "base/map_base.h"
+#include "base/map/map_base.h"
#include "base/factor/factor_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
namespace wolf{
@@ -124,7 +123,7 @@ inline MapBasePtr LandmarkBase::getMap()
return map_ptr_.lock();
}
-inline void LandmarkBase::setMapPtr(const MapBasePtr _map_ptr)
+inline void LandmarkBase::setMap(const MapBasePtr _map_ptr)
{
map_ptr_ = _map_ptr;
}
@@ -161,35 +160,35 @@ inline std::vector<StateBlockPtr>& LandmarkBase::getStateBlockVec()
return state_block_vec_;
}
-inline StateBlockPtr LandmarkBase::getStateBlockPtr(unsigned int _i) const
+inline StateBlockPtr LandmarkBase::getStateBlock(unsigned int _i) const
{
assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
return state_block_vec_[_i];
}
-inline void LandmarkBase::setStateBlockPtr(unsigned int _i, StateBlockPtr _sb_ptr)
+inline void LandmarkBase::setStateBlock(unsigned int _i, StateBlockPtr _sb_ptr)
{
state_block_vec_[_i] = _sb_ptr;
}
inline StateBlockPtr LandmarkBase::getP() const
{
- return getStateBlockPtr(0);
+ return getStateBlock(0);
}
inline StateBlockPtr LandmarkBase::getO() const
{
- return getStateBlockPtr(1);
+ return getStateBlock(1);
}
-inline void LandmarkBase::setPPtr(const StateBlockPtr _st_ptr)
+inline void LandmarkBase::setP(const StateBlockPtr _st_ptr)
{
- setStateBlockPtr(0, _st_ptr);
+ setStateBlock(0, _st_ptr);
}
-inline void LandmarkBase::setOPtr(const StateBlockPtr _st_ptr)
+inline void LandmarkBase::setO(const StateBlockPtr _st_ptr)
{
- setStateBlockPtr(1, _st_ptr);
+ setStateBlock(1, _st_ptr);
}
inline void LandmarkBase::setDescriptor(const Eigen::VectorXs& _descriptor)
diff --git a/include/base/landmark/landmark_container.h b/include/base/landmark/landmark_container.h
index 65a2c5b710ceb0f42bb9984fee0e8d0a967348a4..122b77071f29457d933e423fd4f7a782246fc040 100644
--- a/include/base/landmark/landmark_container.h
+++ b/include/base/landmark/landmark_container.h
@@ -4,7 +4,7 @@
//Wolf includes
#include "base/landmark/landmark_base.h"
-#include "base/wolf.h"
+#include "base/common/wolf.h"
// Std includes
diff --git a/include/base/landmark/landmark_line_2D.h b/include/base/landmark/landmark_line_2D.h
index 3a0f84c340d12027a74bb5be7ff11f31288c06e2..7caf3b2901990d3d9bccfd0d02a51fa6f5315ced 100644
--- a/include/base/landmark/landmark_line_2D.h
+++ b/include/base/landmark/landmark_line_2D.h
@@ -4,7 +4,7 @@
//Wolf includes
#include "base/landmark/landmark_base.h"
-#include "base/wolf.h"
+#include "base/common/wolf.h"
//std includes
diff --git a/include/base/landmark/landmark_match.h b/include/base/landmark/landmark_match.h
index 0c5d3a4915892a3680d634f24a6a40a73df6e26c..48de1e5c99012d24ea8a6f1906284ff0135c77df 100644
--- a/include/base/landmark/landmark_match.h
+++ b/include/base/landmark/landmark_match.h
@@ -2,7 +2,7 @@
#define LANDMARK_MATCH_H_
// Wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
//wolf nampseace
namespace wolf {
diff --git a/include/base/landmark/landmark_polyline_2D.h b/include/base/landmark/landmark_polyline_2D.h
index 4414a69f6175f36280656c2df33e4e94697d0d7d..d6b5d51f9edee7b70a7376a2e0771b34c1de612e 100644
--- a/include/base/landmark/landmark_polyline_2D.h
+++ b/include/base/landmark/landmark_polyline_2D.h
@@ -74,7 +74,7 @@ class LandmarkPolyline2D : public LandmarkBase
const Eigen::VectorXs getPointVector(int _i) const;
- StateBlockPtr getPointStateBlockPtr(int _i);
+ StateBlockPtr getPointStateBlock(int _i);
/** \brief Gets a vector of all state blocks pointers
**/
diff --git a/include/base/map_base.h b/include/base/map/map_base.h
similarity index 93%
rename from include/base/map_base.h
rename to include/base/map/map_base.h
index a8b447905d59c8ffb30be66cd1b91a6b36dc9ae6..c3d6eb8c92c48eb742bd1b63843c472d4f3f590c 100644
--- a/include/base/map_base.h
+++ b/include/base/map/map_base.h
@@ -9,8 +9,8 @@ class LandmarkBase;
}
//Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
+#include "base/common/wolf.h"
+#include "base/common/node_base.h"
//std includes
diff --git a/include/base/IMU_tools.h b/include/base/math/IMU_tools.h
similarity index 99%
rename from include/base/IMU_tools.h
rename to include/base/math/IMU_tools.h
index eecad244c7a57ee2868bcf464e951a0a5108160d..815ed2aa23098b6c0c9d372666f2c2dedae3850e 100644
--- a/include/base/IMU_tools.h
+++ b/include/base/math/IMU_tools.h
@@ -8,8 +8,8 @@
#ifndef IMU_TOOLS_H_
#define IMU_TOOLS_H_
-#include "base/wolf.h"
-#include "base/rotations.h"
+#include "base/common/wolf.h"
+#include "base/math/rotations.h"
/*
* Most functions in this file are explained in the document:
diff --git a/include/base/SE3.h b/include/base/math/SE3.h
similarity index 99%
rename from include/base/SE3.h
rename to include/base/math/SE3.h
index cea35460771f0bf91d4f56520dd823ad3a1d505d..5a84f0c7e50d55d9d56ea06cc444c8f8d33698a4 100644
--- a/include/base/SE3.h
+++ b/include/base/math/SE3.h
@@ -8,8 +8,8 @@
#ifndef SE3_H_
#define SE3_H_
-#include "base/wolf.h"
-#include "base/rotations.h"
+#include "base/common/wolf.h"
+#include "base/math/rotations.h"
/*
* The functions in this file are related to manipulations of Delta motion magnitudes used in 3D motion.
diff --git a/include/base/pinhole_tools.h b/include/base/math/pinhole_tools.h
similarity index 99%
rename from include/base/pinhole_tools.h
rename to include/base/math/pinhole_tools.h
index 46cb1fb9dd1f12dd18d3a520cc574488e7c9f170..9fed8ba7c0775ac46d93872e98e1b2bee85fe64b 100644
--- a/include/base/pinhole_tools.h
+++ b/include/base/math/pinhole_tools.h
@@ -11,7 +11,7 @@
*
*/
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include <iostream>
diff --git a/include/base/rotations.h b/include/base/math/rotations.h
similarity index 99%
rename from include/base/rotations.h
rename to include/base/math/rotations.h
index d4d3e711f119139e4ee2385897a12b07c60354b2..fc0e8b2bbd925c01009ec8577226cc25cfb6f0f1 100644
--- a/include/base/rotations.h
+++ b/include/base/math/rotations.h
@@ -8,7 +8,7 @@
#ifndef ROTATIONS_H_
#define ROTATIONS_H_
-#include "base/wolf.h"
+#include "base/common/wolf.h"
namespace wolf
{
diff --git a/include/base/problem.h b/include/base/problem/problem.h
similarity index 92%
rename from include/base/problem.h
rename to include/base/problem/problem.h
index d96aae5e29d91ba85d9251d728d89652da32a1c7..938074f78020ebff96905714d70d7f1259a168e1 100644
--- a/include/base/problem.h
+++ b/include/base/problem/problem.h
@@ -14,11 +14,12 @@ struct ProcessorParamsBase;
}
//wolf includes
-#include "base/wolf.h"
-#include "base/frame_base.h"
-#include "base/state_block.h"
+#include "base/common/wolf.h"
+#include "base/frame/frame_base.h"
+#include "base/state_block/state_block.h"
// std includes
+#include <mutex>
namespace wolf {
@@ -38,11 +39,13 @@ class Problem : public std::enable_shared_from_this<Problem>
TrajectoryBasePtr trajectory_ptr_;
MapBasePtr map_ptr_;
ProcessorMotionPtr processor_motion_ptr_;
- StateBlockPtrList state_block_list_;
std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXs> covariances_;
SizeEigen state_size_, state_cov_size_, dim_;
std::map<FactorBasePtr, Notification> factor_notification_map_;
std::map<StateBlockPtr, Notification> state_block_notification_map_;
+ mutable std::mutex mut_factor_notifications_;
+ mutable std::mutex mut_state_block_notifications_;
+ mutable std::mutex mut_covariances_;
bool prior_is_set_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
@@ -87,7 +90,7 @@ class Problem : public std::enable_shared_from_this<Problem>
/** \brief get a sensor pointer by its name
* \param _sensor_name The sensor name, as it was installed with installSensor()
*/
- SensorBasePtr getSensorPtr(const std::string& _sensor_name);
+ SensorBasePtr getSensor(const std::string& _sensor_name);
/** \brief Factory method to install (create, and add to sensor) processors only from its properties
*
@@ -236,17 +239,11 @@ class Problem : public std::enable_shared_from_this<Problem>
bool getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov);
bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col = 0);
bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance);
- Eigen::MatrixXs getFrameCovariance(FrameBaseConstPtr _frame_ptr);
- Eigen::MatrixXs getLastKeyFrameCovariance();
+ bool getLastKeyFrameCovariance(Eigen::MatrixXs& _covariance);
bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance);
- Eigen::MatrixXs getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr);
// Solver management ----------------------------------------
- /** \brief Gets a reference to the state blocks list
- */
- StateBlockPtrList& getStateBlockPtrList();
-
/** \brief Notifies a new state block to be added to the solver manager
*/
StateBlockPtr addStateBlock(StateBlockPtr _state_ptr);
@@ -255,9 +252,9 @@ class Problem : public std::enable_shared_from_this<Problem>
*/
void removeStateBlock(StateBlockPtr _state_ptr);
- /** \brief Gets a map of factor notification to be handled by the solver
+ /** \brief Returns the map of factor notification to be handled by the solver (the map stored in this is emptied)
*/
- std::map<StateBlockPtr,Notification>& getStateBlockNotificationMap();
+ std::map<StateBlockPtr,Notification> consumeStateBlockNotificationMap();
/** \brief Notifies a new factor to be added to the solver manager
*/
@@ -267,9 +264,9 @@ class Problem : public std::enable_shared_from_this<Problem>
*/
void removeFactor(FactorBasePtr _factor_ptr);
- /** \brief Gets a map of factor notification to be handled by the solver
+ /** \brief Returns the map of factor notification to be handled by the solver (the map stored in this is emptied)
*/
- std::map<FactorBasePtr, Notification>& getFactorNotificationMap();
+ std::map<FactorBasePtr, Notification> consumeFactorNotificationMap();
// Print and check ---------------------------------------
/**
@@ -308,14 +305,16 @@ inline ProcessorMotionPtr& Problem::getProcessorMotion()
return processor_motion_ptr_;
}
-inline std::map<StateBlockPtr,Notification>& Problem::getStateBlockNotificationMap()
+inline std::map<StateBlockPtr,Notification> Problem::consumeStateBlockNotificationMap()
{
- return state_block_notification_map_;
+ std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
+ return std::move(state_block_notification_map_);
}
-inline std::map<FactorBasePtr,Notification>& Problem::getFactorNotificationMap()
+inline std::map<FactorBasePtr,Notification> Problem::consumeFactorNotificationMap()
{
- return factor_notification_map_;
+ std::lock_guard<std::mutex> lock(mut_factor_notifications_);
+ return std::move(factor_notification_map_);
}
} // namespace wolf
diff --git a/include/base/diff_drive_tools.h b/include/base/processor/diff_drive_tools.h
similarity index 99%
rename from include/base/diff_drive_tools.h
rename to include/base/processor/diff_drive_tools.h
index 142b8c5022ccca18eaf1edc1214984b93bd816c4..92883eafa55af400e948f9b637389255b035eef0 100644
--- a/include/base/diff_drive_tools.h
+++ b/include/base/processor/diff_drive_tools.h
@@ -8,7 +8,7 @@
#ifndef _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_
#define _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_
-#include "base/eigen_assert.h"
+#include "base/utils/eigen_assert.h"
namespace wolf {
@@ -419,6 +419,6 @@ Eigen::Matrix<typename T::Scalar, 2, 2> computeWheelJointPositionCov(
} // namespace wolf
-#include "base/diff_drive_tools.hpp"
+#include "base/processor/diff_drive_tools.hpp"
#endif /* _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_ */
diff --git a/include/base/diff_drive_tools.hpp b/include/base/processor/diff_drive_tools.hpp
similarity index 100%
rename from include/base/diff_drive_tools.hpp
rename to include/base/processor/diff_drive_tools.hpp
diff --git a/include/base/motion_buffer.h b/include/base/processor/motion_buffer.h
similarity index 98%
rename from include/base/motion_buffer.h
rename to include/base/processor/motion_buffer.h
index f8d0ad5be8e854eaf8badca494218b637795d16e..40fbd29251a47338412a3e2a307917dc1d0b5f78 100644
--- a/include/base/motion_buffer.h
+++ b/include/base/processor/motion_buffer.h
@@ -8,8 +8,8 @@
#ifndef SRC_MOTIONBUFFER_H_
#define SRC_MOTIONBUFFER_H_
-#include "base/wolf.h"
-#include "base/time_stamp.h"
+#include "base/common/wolf.h"
+#include "base/common/time_stamp.h"
#include <list>
#include <algorithm>
diff --git a/include/base/processor/processor_base.h b/include/base/processor/processor_base.h
index eca79571f554d22e9fb795261bc711599ca9fbde..418dcfd31e55d91fb87b2a0d11eb43431801fdbc 100644
--- a/include/base/processor/processor_base.h
+++ b/include/base/processor/processor_base.h
@@ -7,10 +7,10 @@ class SensorBase;
}
// Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
-#include "base/time_stamp.h"
-#include "base/frame_base.h"
+#include "base/common/wolf.h"
+#include "base/common/node_base.h"
+#include "base/common/time_stamp.h"
+#include "base/frame/frame_base.h"
// std
#include <memory>
@@ -184,7 +184,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
SensorBasePtr getSensor();
const SensorBasePtr getSensor() const;
- void setSensorPtr(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
+ void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
virtual bool isMotion();
diff --git a/include/base/processor/processor_diff_drive.h b/include/base/processor/processor_diff_drive.h
index d0c8411f285b11b1f727eccd813f80ae092b9fbb..9d05bab54b8e0ac41bf3567eb62f59c125610d25 100644
--- a/include/base/processor/processor_diff_drive.h
+++ b/include/base/processor/processor_diff_drive.h
@@ -9,7 +9,7 @@
#define _WOLF_PROCESSOR_DIFF_DRIVE_H_
#include "base/processor/processor_motion.h"
-#include "base/diff_drive_tools.h"
+#include "base/processor/diff_drive_tools.h"
namespace wolf {
@@ -29,7 +29,6 @@ struct ProcessorParamsDiffDrive : public ProcessorParamsMotion
// {
// time_tolerance = _time_tolerance;
// }
- Scalar unmeasured_perturbation_std = 0.0001;
};
/**
@@ -62,7 +61,6 @@ protected:
/// @brief Intrinsic params
ProcessorParamsDiffDrivePtr params_motion_diff_drive_;
- MatrixXs unmeasured_perturbation_cov_;
virtual void computeCurrentDelta(const Eigen::VectorXs& _data,
const Eigen::MatrixXs& _data_cov,
diff --git a/include/base/processor/processor_factory.h b/include/base/processor/processor_factory.h
index 8077aae495c053e229fc8fef08790f1ca6e3b099..390a66bec44b331094245686763cec31416fdb00 100644
--- a/include/base/processor/processor_factory.h
+++ b/include/base/processor/processor_factory.h
@@ -15,7 +15,7 @@ struct ProcessorParamsBase;
}
// wolf
-#include "base/factory.h"
+#include "base/common/factory.h"
// std
@@ -155,7 +155,7 @@ namespace wolf
* \code
* #include "base/sensor/sensor_odom_2D.h"
* #include "base/processor/processor_odom_2D.h"
- * #include "base/problem.h"
+ * #include "base/problem/problem.h"
*
* Problem problem(FRM_PO_2D);
* problem.installSensor ( "ODOM 2D" , "Main odometer" , extrinsics , &intrinsics );
diff --git a/include/base/processor/processor_frame_nearest_neighbor_filter.h b/include/base/processor/processor_frame_nearest_neighbor_filter.h
index be1205e27988461473786627ca5e0d3acc62748f..ceb49425395712ff35d2c0e1d61c0882eecfc84c 100644
--- a/include/base/processor/processor_frame_nearest_neighbor_filter.h
+++ b/include/base/processor/processor_frame_nearest_neighbor_filter.h
@@ -3,7 +3,7 @@
// Wolf related headers
#include "base/processor/processor_loopclosure_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
namespace wolf{
diff --git a/include/base/processor/processor_logging.h b/include/base/processor/processor_logging.h
index 4add7a8e9d7945f0aaab54c8e7e7f1acdc4bfd8e..d41d7ce30af37063f429a979aeb41a780b2ab489 100644
--- a/include/base/processor/processor_logging.h
+++ b/include/base/processor/processor_logging.h
@@ -9,7 +9,7 @@
#define _WOLF_PROCESSOR_LOGGING_H_
/// @brief un-comment for IDE highlights.
-//#include "base/logging.h"
+//#include "base/utils/logging.h"
#define __INTERNAL_WOLF_ASSERT_PROCESSOR \
static_assert(std::is_base_of<ProcessorBase, \
diff --git a/include/base/processor/processor_motion.h b/include/base/processor/processor_motion.h
index b66feb38face40a05ce5c96696920ece1ca86c1b..38a4faa0d8dc1b971c789617af036c69ec177b01 100644
--- a/include/base/processor/processor_motion.h
+++ b/include/base/processor/processor_motion.h
@@ -11,7 +11,7 @@
// Wolf
#include "base/capture/capture_motion.h"
#include "base/processor/processor_base.h"
-#include "base/time_stamp.h"
+#include "base/common/time_stamp.h"
// std
#include <iomanip>
@@ -23,10 +23,11 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsMotion);
struct ProcessorParamsMotion : public ProcessorParamsBase
{
- Scalar max_time_span = 0.5;
- unsigned int max_buff_length = 10;
- Scalar dist_traveled = 5;
- Scalar angle_turned = 0.5;
+ Scalar unmeasured_perturbation_std = 1e-4;
+ Scalar max_time_span = 0.5;
+ unsigned int max_buff_length = 10;
+ Scalar dist_traveled = 5;
+ Scalar angle_turned = 0.5;
};
/** \brief class for Motion processors
@@ -472,11 +473,12 @@ class ProcessorMotion : public ProcessorBase
Eigen::MatrixXs jacobian_delta_; ///< jacobian of delta composition w.r.t current delta
Eigen::MatrixXs jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params
Eigen::MatrixXs jacobian_delta_calib_; ///< jacobian of delta wrt calib params
+ Eigen::MatrixXs unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity
};
}
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
namespace wolf{
diff --git a/include/base/processor/processor_odom_2D.h b/include/base/processor/processor_odom_2D.h
index d18c9591e5abd9932a7781144f172b9a72826487..5def5e3ad512036fc227e773c48211284892ec73 100644
--- a/include/base/processor/processor_odom_2D.h
+++ b/include/base/processor/processor_odom_2D.h
@@ -11,7 +11,7 @@
#include "base/processor/processor_motion.h"
#include "base/capture/capture_odom_2D.h"
#include "base/factor/factor_odom_2D.h"
-#include "base/rotations.h"
+#include "base/math/rotations.h"
namespace wolf {
@@ -21,7 +21,6 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom2D);
struct ProcessorParamsOdom2D : public ProcessorParamsMotion
{
Scalar cov_det = 1.0; // 1 rad^2
- Scalar unmeasured_perturbation_std = 0.001; // no particular dimension: the same for displacement and angle
};
class ProcessorOdom2D : public ProcessorMotion
@@ -75,7 +74,6 @@ class ProcessorOdom2D : public ProcessorMotion
protected:
ProcessorParamsOdom2DPtr params_odom_2D_;
- MatrixXs unmeasured_perturbation_cov_;
// Factory method
public:
diff --git a/include/base/processor/processor_odom_3D.h b/include/base/processor/processor_odom_3D.h
index 2cbc26a068f13c8e316bf876dd8d483b7eb0a125..5602eb3839cabffcb83ebde39f0ef7d8a74c8c49 100644
--- a/include/base/processor/processor_odom_3D.h
+++ b/include/base/processor/processor_odom_3D.h
@@ -12,7 +12,7 @@
#include "base/sensor/sensor_odom_3D.h"
#include "base/capture/capture_odom_3D.h"
#include "base/factor/factor_odom_3D.h"
-#include "base/rotations.h"
+#include "base/math/rotations.h"
#include <cmath>
namespace wolf {
diff --git a/include/base/processor/processor_tracker.h b/include/base/processor/processor_tracker.h
index 2461a69fe70597ff5a4b97f9b331a8254f66c6cf..38dc26b30d80c419c69f3640c71645352afa6f66 100644
--- a/include/base/processor/processor_tracker.h
+++ b/include/base/processor/processor_tracker.h
@@ -18,7 +18,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTracker);
struct ProcessorParamsTracker : public ProcessorParamsBase
{
unsigned int min_features_for_keyframe; ///< minimum nbr. of features to vote for keyframe
- unsigned int max_new_features;
+ int max_new_features; ///< maximum nbr. of new features to be processed when adding a keyframe (-1: unlimited. 0: none.)
};
WOLF_PTR_TYPEDEFS(ProcessorTracker);
@@ -181,7 +181,7 @@ class ProcessorTracker : public ProcessorBase
/**\brief Process new Features or Landmarks
*
*/
- virtual unsigned int processNew(const unsigned int& _max_features) = 0;
+ virtual unsigned int processNew(const int& _max_features) = 0;
/**\brief Creates and adds factors from last_ to origin_
*
diff --git a/include/base/processor/processor_tracker_feature.h b/include/base/processor/processor_tracker_feature.h
index d3b0eedd6c9627bb0f56c86689f6f3dada9220f5..17da3af43bd37be3b7f8543aa539554e1d50eb83 100644
--- a/include/base/processor/processor_tracker_feature.h
+++ b/include/base/processor/processor_tracker_feature.h
@@ -12,8 +12,8 @@
#include "base/processor/processor_tracker.h"
#include "base/capture/capture_base.h"
#include "base/feature/feature_match.h"
-#include "base/track_matrix.h"
-#include "base/wolf.h"
+#include "base/processor/track_matrix.h"
+#include "base/common/wolf.h"
namespace wolf
{
@@ -143,17 +143,19 @@ class ProcessorTrackerFeature : public ProcessorTracker
/**\brief Process new Features
*
*/
- virtual unsigned int processNew(const unsigned int& _max_features);
+ virtual unsigned int processNew(const int& _max_features);
/** \brief Detect new Features
- * \param _max_features maximum number of features detected
+ * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+ * \param _features_last_out The list of detected Features.
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
*
- * The function sets the member new_features_last_, the list of newly detected features.
+ * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+ * the list of newly detected features of the capture last_ptr_.
*/
- virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out) = 0;
+ virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out) = 0;
/** \brief Create a new factor and link it to the wolf tree
* \param _feature_ptr pointer to the parent Feature
diff --git a/include/base/processor/processor_tracker_feature_corner.h b/include/base/processor/processor_tracker_feature_corner.h
index e0c2ecbad2f1b89d8e11b15ea174714066ea4f58..566b2aea9ad081f14598c19a41746ce5b2917efc 100644
--- a/include/base/processor/processor_tracker_feature_corner.h
+++ b/include/base/processor/processor_tracker_feature_corner.h
@@ -14,7 +14,7 @@
#include "base/feature/feature_corner_2D.h"
#include "base/landmark/landmark_corner_2D.h"
#include "base/factor/factor_corner_2D.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
#include "base/association/association_tree.h"
#include "base/processor/processor_tracker_feature.h"
@@ -106,16 +106,16 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
virtual bool voteForKeyFrame();
/** \brief Detect new Features
- * \param _capture_ptr Capture for feature detection. Defaults to incoming_ptr_.
- * \param _new_features_list The list of detected Features. Defaults to member new_features_list_.
+ * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+ * \param _features_last_out The list of detected Features.
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
*
- * The function sets the member new_features_list_, the list of newly detected features,
- * to be used for landmark initialization.
+ * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+ * the list of newly detected features of the capture last_ptr_.
*/
- virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
+ virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out);
virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
diff --git a/include/base/processor/processor_tracker_feature_dummy.h b/include/base/processor/processor_tracker_feature_dummy.h
index 586cd57d716519a45f01f66050f39f2bb45f6bee..c0b40edbea015ffb86971363c218659c21d67af7 100644
--- a/include/base/processor/processor_tracker_feature_dummy.h
+++ b/include/base/processor/processor_tracker_feature_dummy.h
@@ -8,7 +8,7 @@
#ifndef PROCESSOR_TRACKER_FEATURE_DUMMY_H_
#define PROCESSOR_TRACKER_FEATURE_DUMMY_H_
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include "base/processor/processor_tracker_feature.h"
#include "base/factor/factor_epipolar.h"
@@ -56,16 +56,16 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
virtual bool voteForKeyFrame();
/** \brief Detect new Features
- * \param _capture_ptr Capture for feature detection. Defaults to incoming_ptr_.
- * \param _new_features_list The list of detected Features. Defaults to member new_features_list_.
+ * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+ * \param _features_last_out The list of detected Features.
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
*
- * The function sets the member new_features_list_, the list of newly detected features,
- * to be used for landmark initialization.
+ * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+ * the list of newly detected features of the capture last_ptr_.
*/
- virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
+ virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out);
virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
diff --git a/include/base/processor/processor_tracker_feature_image.h b/include/base/processor/processor_tracker_feature_image.h
index 800ca1211fbdcdd3d8f99781be90d4ddfdbfad11..7378c8a3d67596e5357a2e40f5f5475f00a21c42 100644
--- a/include/base/processor/processor_tracker_feature_image.h
+++ b/include/base/processor/processor_tracker_feature_image.h
@@ -5,8 +5,8 @@
#include "base/sensor/sensor_camera.h"
#include "base/capture/capture_image.h"
#include "base/feature/feature_point_image.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
#include "base/processor/processor_tracker_feature.h"
#include "base/factor/factor_epipolar.h"
#include "base/processor/processor_params_image.h"
@@ -112,14 +112,16 @@ class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature
virtual bool voteForKeyFrame();
/** \brief Detect new Features
+ * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+ * \param _features_last_out The list of detected Features.
+ * \return The number of detected Features.
*
- * This is intended to create Features that are not among the Features already known in the Map.
- *
- * This function sets new_features_last_, the list of newly detected features.
+ * This function detects Features that do not correspond to known Features/Landmarks in the system.
*
- * \return The number of detected Features.
+ * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+ * the list of newly detected features of the capture last_ptr_.
*/
- virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out);
+ virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out);
virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
diff --git a/include/base/processor/processor_tracker_feature_trifocal.h b/include/base/processor/processor_tracker_feature_trifocal.h
index dba9e91714a2736b7bae8a970dc5f8cde9cc05d6..52b6d902be1fa4b33f4730bcdf90a87e646bd8ea 100644
--- a/include/base/processor/processor_tracker_feature_trifocal.h
+++ b/include/base/processor/processor_tracker_feature_trifocal.h
@@ -85,14 +85,16 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
virtual bool voteForKeyFrame() override;
/** \brief Detect new Features
- * \param _max_features maximum number of features detected
+ * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+ * \param _features_last_out The list of detected Features.
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
*
- * The function sets the member new_features_last_, the list of newly detected features.
+ * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+ * the list of newly detected features of the capture last_ptr_.
*/
- virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out) override;
+ virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out) override;
/** \brief Create a new factor and link it to the wolf tree
* \param _feature_ptr pointer to the parent Feature
diff --git a/include/base/processor/processor_tracker_landmark.h b/include/base/processor/processor_tracker_landmark.h
index 23e8132e308138b41b9c2e79553c82e4cb4b03ba..35a99280c4a2fc9b72bce7dbc00f6f937bcfa993 100644
--- a/include/base/processor/processor_tracker_landmark.h
+++ b/include/base/processor/processor_tracker_landmark.h
@@ -128,18 +128,19 @@ class ProcessorTrackerLandmark : public ProcessorTracker
/** \brief Process new Features
*
*/
- unsigned int processNew(const unsigned int& _max_features);
+ unsigned int processNew(const int& _max_features);
/** \brief Detect new Features
- * \param _max_features The maximum number of features to detect.
+ * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+ * \param _features_last_out The list of detected Features.
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
*
- * The function sets the member new_features_list_, the list of newly detected features,
- * to be used for landmark initialization.
+ * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
+ * the list of newly detected features of the capture last_ptr_.
*/
- virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out) = 0;
+ virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out) = 0;
/** \brief Creates a landmark for each of new_features_last_
**/
diff --git a/include/base/processor/processor_tracker_landmark_corner.h b/include/base/processor/processor_tracker_landmark_corner.h
index a25a2646f21efd437201b8cac0c0899164a07597..2b381719af5c7561f9d721d99d990cdf0ae1bdba 100644
--- a/include/base/processor/processor_tracker_landmark_corner.h
+++ b/include/base/processor/processor_tracker_landmark_corner.h
@@ -14,7 +14,7 @@
#include "base/feature/feature_corner_2D.h"
#include "base/landmark/landmark_corner_2D.h"
#include "base/factor/factor_corner_2D.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
#include "base/association/association_tree.h"
#include "base/processor/processor_tracker_landmark.h"
@@ -135,16 +135,16 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
virtual bool voteForKeyFrame();
/** \brief Detect new Features
- * \param _capture_ptr Capture for feature detection. Defaults to incoming_ptr_.
- * \param _new_features_list The list of detected Features. Defaults to member new_features_list_.
+ * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+ * \param _features_last_out The list of detected Features.
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
*
- * The function sets the member new_features_list_, the list of newly detected features,
- * to be used for landmark initialization.
+ * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
+ * the list of newly detected features of the capture last_ptr_.
*/
- virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
+ virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out);
/** \brief Create one landmark
*
diff --git a/include/base/processor/processor_tracker_landmark_dummy.h b/include/base/processor/processor_tracker_landmark_dummy.h
index 82b446c47d15b282d985a5d6020a9f33c9d79936..c0c3424cb0983f352d6976a7bee0ec07af9bdaa2 100644
--- a/include/base/processor/processor_tracker_landmark_dummy.h
+++ b/include/base/processor/processor_tracker_landmark_dummy.h
@@ -48,15 +48,16 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
virtual bool voteForKeyFrame();
/** \brief Detect new Features
- * \param _max_features maximum number of features to detect.
+ * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+ * \param _features_last_out The list of detected Features.
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
*
- * The function sets the member new_features_list_, the list of newly detected features,
- * to be used for landmark initialization.
+ * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
+ * the list of newly detected features of the capture last_ptr_.
*/
- virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
+ virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out);
/** \brief Create one landmark
*
diff --git a/include/base/processor/processor_tracker_landmark_image.h b/include/base/processor/processor_tracker_landmark_image.h
index d58920cc10b943c74a27dd1c3f61946624747133..f98f87707a9821b3082dabb79039f2f25018966c 100644
--- a/include/base/processor/processor_tracker_landmark_image.h
+++ b/include/base/processor/processor_tracker_landmark_image.h
@@ -7,7 +7,7 @@
#include "base/landmark/landmark_match.h"
#include "base/processor/processor_params_image.h"
#include "base/processor/processor_tracker_landmark.h"
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include <algorithms/activesearch/alg_activesearch.h>
#include <descriptors/descriptor_base.h>
@@ -128,14 +128,16 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
virtual bool voteForKeyFrame();
/** \brief Detect new Features
+ * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+ * \param _features_last_out The list of detected Features.
+ * \return The number of detected Features.
*
- * This is intended to create Features that are not among the Features already known in the Map.
- *
- * This function sets new_features_last_, the list of newly detected features.
+ * This function detects Features that do not correspond to known Features/Landmarks in the system.
*
- * \return The number of detected Features.
+ * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
+ * the list of newly detected features of the capture last_ptr_.
*/
- virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out);
+ virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out);
/** \brief Create one landmark
*
diff --git a/include/base/processor/processor_tracker_landmark_polyline.h b/include/base/processor/processor_tracker_landmark_polyline.h
deleted file mode 100644
index a6f902c930d696dc595ae19f133a0df4fd0e00c2..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_tracker_landmark_polyline.h
+++ /dev/null
@@ -1,248 +0,0 @@
-/*
- * processor_tracker_landmark_polyline.h
- *
- * Created on: May 26, 2016
- * Author: jvallve
- */
-
-#ifndef SRC_PROCESSOR_TRACKER_LANDMARK_POLYLINE_H_
-#define SRC_PROCESSOR_TRACKER_LANDMARK_POLYLINE_H_
-
-// Wolf includes
-#include "base/sensor/sensor_laser_2D.h"
-#include "base/capture/capture_laser_2D.h"
-#include "base/feature/feature_polyline_2D.h"
-#include "base/landmark/landmark_polyline_2D.h"
-#include "base/factor/factor_point_2D.h"
-#include "base/factor/factor_point_to_line_2D.h"
-#include "base/state_block.h"
-#include "base/association/association_tree.h"
-#include "base/processor/processor_tracker_landmark.h"
-
-//laser_scan_utils
-#include "laser_scan_utils/laser_scan.h"
-#include "laser_scan_utils/line_finder_iterative.h"
-#include "laser_scan_utils/polyline.h"
-
-namespace wolf
-{
-
-//some consts.. TODO: this tuning params should be grouped in a struct and passed to the class from ros node, at constructor level
-const Scalar position_error_th_ = 1;
-const Scalar min_features_ratio_th_ = 0.5;
-
-//forward declaration to typedef class pointers
-struct LandmarkPolylineMatch;
-typedef std::shared_ptr<LandmarkPolylineMatch> LandmarkPolylineMatchPtr;
-
-//forward declaration to typedef class pointers
-struct ProcessorParamsPolyline;
-typedef std::shared_ptr<ProcessorParamsPolyline> ProcessorParamsPolylinePtr;
-
-WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkPolyline);
-
-// Match Feature - Landmark
-struct LandmarkPolylineMatch : public LandmarkMatch
-{
- int landmark_match_from_id_;
- int feature_match_from_id_;
- int landmark_match_to_id_;
- int feature_match_to_id_;
-
-// std::vector<unsigned int> landmark_points_match_;
-// std::vector<unsigned int> feature_points_match_;
-// std::vector<unsigned int> feature_points_add_front_;
-// std::vector<unsigned int> feature_points_add_back_;
-
- LandmarkPolylineMatch() :
- landmark_match_from_id_(0),
- feature_match_from_id_(0),
- landmark_match_to_id_(0),
- feature_match_to_id_(0)
- {
- //
- }
- LandmarkPolylineMatch(int _landmark_match_from_id,
- int _feature_match_from_id,
- int _landmark_match_to_id,
- int _feature_match_to_id) :
- landmark_match_from_id_(_landmark_match_from_id),
- feature_match_from_id_(_feature_match_from_id),
- landmark_match_to_id_(_landmark_match_to_id),
- feature_match_to_id_(_landmark_match_to_id)
- {
- //
- }
-};
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsPolyline);
-struct ProcessorParamsPolyline : public ProcessorParamsTrackerLandmark
-{
- laserscanutils::LineFinderIterativeParams line_finder_params;
- Scalar position_error_th;
- unsigned int new_features_th;
- unsigned int loop_frames_th;
-
- // These values below are constant and defined within the class -- provide a setter or accept them at construction time if you need to configure them
- // Scalar aperture_error_th_ = 20.0 * M_PI / 180.; //20 degrees
- // Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees;
- // Scalar position_error_th_ = 1;
- // Scalar min_features_ratio_th_ = 0.5;
-};
-
-class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
-{
- private:
- laserscanutils::LineFinderIterative line_finder_;
- ProcessorParamsPolylinePtr params_;
-
- FeatureBasePtrList polylines_incoming_;
- FeatureBasePtrList polylines_last_;
-
- Eigen::Matrix2s R_sensor_world_, R_world_sensor_;
- Eigen::Matrix2s R_robot_sensor_;
- Eigen::Matrix2s R_current_prev_;
- Eigen::Vector2s t_sensor_world_, t_world_sensor_, t_world_sensor_prev_, t_sensor_world_prev_;
- Eigen::Vector2s t_robot_sensor_;
- Eigen::Vector2s t_current_prev_;
- Eigen::Vector2s t_world_robot_;
- bool extrinsics_transformation_computed_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
- ProcessorTrackerLandmarkPolyline(ProcessorParamsPolylinePtr _params);
-
- virtual ~ProcessorTrackerLandmarkPolyline();
- virtual void configure(SensorBasePtr _sensor) { };
-
- const FeatureBasePtrList& getLastPolylines() const;
-
- protected:
-
- virtual void preProcess();
- void computeTransformations(const TimeStamp& _ts);
- virtual void postProcess();
-
- void advanceDerived();
-
- void resetDerived();
-
- /** \brief Find provided landmarks in the incoming capture
- * \param _landmarks_in input list of landmarks to be found in incoming
- * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
- * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
- */
- virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in, FeatureBasePtrList& _features_incoming_out,
- LandmarkMatchMap& _feature_landmark_correspondences);
-
- /** \brief Vote for KeyFrame generation
- *
- * If a KeyFrame criterion is validated, this function returns true,
- * meaning that it wants to create a KeyFrame at the \b last Capture.
- *
- * WARNING! This function only votes! It does not create KeyFrames!
- */
- virtual bool voteForKeyFrame();
-
- /** \brief Detect new Features
- * \param _capture_ptr Capture for feature detection. Defaults to incoming_ptr_.
- * \param _new_features_list The list of detected Features. Defaults to member new_features_list_.
- * \return The number of detected Features.
- *
- * This function detects Features that do not correspond to known Features/Landmarks in the system.
- *
- * The function sets the member new_features_list_, the list of newly detected features,
- * to be used for landmark initialization.
- */
- virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
-
- /** \brief Creates a landmark for each of new_features_last_
- **/
- virtual void createNewLandmarks();
-
- /** \brief Create one landmark
- *
- * Implement in derived classes to build the type of landmark you need for this tracker.
- */
- virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
-
- /** \brief Establish factors between features in Captures \b last and \b origin
- */
- virtual void establishFactors();
-
- /** \brief look for known objects in the list of unclassified polylines
- */
- void classifyPolilines(LandmarkBasePtrList& _lmk_list);
-
- /** \brief Create a new factor
- * \param _feature_ptr pointer to the Feature to constrain
- * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
- *
- * Implement this method in derived classes.
- */
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
-
- private:
-
- void extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _polyline_list);
-
- void expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_,
- Eigen::MatrixXs& expected_feature_cov_);
-
- Eigen::VectorXs computeSquaredMahalanobisDistances(const Eigen::Vector2s& _feature,
- const Eigen::Matrix2s& _feature_cov,
- const Eigen::Vector2s& _expected_feature,
- const Eigen::Matrix2s& _expected_feature_cov,
- const Eigen::MatrixXs& _mu);
- Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux, const Eigen::Vector3s& _B,
- bool _A_extreme, bool _B_extreme);
- // Factory method
- public:
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
-};
-
-inline ProcessorTrackerLandmarkPolyline::ProcessorTrackerLandmarkPolyline(ProcessorParamsPolylinePtr _params) :
- ProcessorTrackerLandmark("TRACKER LANDMARK POLYLINE", _params),
- line_finder_(_params->line_finder_params),
- params_(_params),
- extrinsics_transformation_computed_(false)
-{
-}
-
-inline unsigned int ProcessorTrackerLandmarkPolyline::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
-{
- // already computed since each scan is computed in preprocess()
- _features_incoming_out = std::move(polylines_last_);
- return _features_incoming_out.size();
-}
-
-inline void ProcessorTrackerLandmarkPolyline::advanceDerived()
-{
- //std::cout << "\tProcessorTrackerLandmarkPolyline::advance:" << std::endl;
- //std::cout << "\t\tcorners_last: " << polylines_last_.size() << std::endl;
- //std::cout << "\t\tcorners_incoming_: " << polylines_incoming_.size() << std::endl;
- ProcessorTrackerLandmark::advanceDerived();
- for (auto polyline : polylines_last_)
- polyline->remove();
- polylines_last_ = std::move(polylines_incoming_);
- //std::cout << "advanced" << std::endl;
-}
-
-inline void ProcessorTrackerLandmarkPolyline::resetDerived()
-{
- //std::cout << "\tProcessorTrackerLandmarkPolyline::reset:" << std::endl;
- //std::cout << "\t\tcorners_last: " << corners_last_.size() << std::endl;
- //std::cout << "\t\tcorners_incoming_: " << polylines_incoming_.size() << std::endl;
- ProcessorTrackerLandmark::resetDerived();
- polylines_last_ = std::move(polylines_incoming_);
-}
-
-inline const FeatureBasePtrList& ProcessorTrackerLandmarkPolyline::getLastPolylines() const
-{
- return polylines_last_;
-}
-
-} // namespace wolf
-
-#endif /* SRC_PROCESSOR_TRACKER_LASER_H_ */
diff --git a/include/base/track_matrix.h b/include/base/processor/track_matrix.h
similarity index 100%
rename from include/base/track_matrix.h
rename to include/base/processor/track_matrix.h
diff --git a/include/base/sensor/sensor_GPS.h b/include/base/sensor/sensor_GPS.h
index a56b8c26776e054dc469d563b113df7c4b68c4d3..6881537b62e614bc11b51592abb9f7ce89b67c6a 100644
--- a/include/base/sensor/sensor_GPS.h
+++ b/include/base/sensor/sensor_GPS.h
@@ -11,7 +11,7 @@
//wolf
#include "base/sensor/sensor_base.h"
//#include "sensor_factory.h"
-//#include "base/factory.h"
+//#include "base/common/factory.h"
// std
diff --git a/include/base/sensor/sensor_base.h b/include/base/sensor/sensor_base.h
index 90ea49a604aa81b7bf765dc0e00df251c8370922..c7e55edc69b50c07be0bd15e1ce617c894243542 100644
--- a/include/base/sensor/sensor_base.h
+++ b/include/base/sensor/sensor_base.h
@@ -9,9 +9,9 @@ class StateBlock;
}
//Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
-#include "base/time_stamp.h"
+#include "base/common/wolf.h"
+#include "base/common/node_base.h"
+#include "base/common/time_stamp.h"
//std includes
@@ -94,7 +94,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
virtual void setProblem(ProblemPtr _problem) final;
HardwareBasePtr getHardware();
- void setHardwarePtr(const HardwareBasePtr _hw_ptr);
+ void setHardware(const HardwareBasePtr _hw_ptr);
ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr);
ProcessorBasePtrList& getProcessorList();
@@ -108,8 +108,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
const std::vector<StateBlockPtr>& getStateBlockVec() const;
std::vector<StateBlockPtr>& getStateBlockVec();
StateBlockPtr getStateBlockPtrStatic(unsigned int _i) const;
- StateBlockPtr getStateBlockPtr(unsigned int _i);
- StateBlockPtr getStateBlockPtr(unsigned int _i, const TimeStamp& _ts);
+ StateBlockPtr getStateBlock(unsigned int _i);
+ StateBlockPtr getStateBlock(unsigned int _i, const TimeStamp& _ts);
void setStateBlockPtrStatic(unsigned int _i, const StateBlockPtr _sb_ptr);
void resizeStateBlockVec(unsigned int _size);
@@ -118,15 +118,15 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
bool isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts);
bool isStateBlockDynamic(unsigned int _i);
- StateBlockPtr getPPtr(const TimeStamp _ts);
- StateBlockPtr getOPtr(const TimeStamp _ts);
- StateBlockPtr getIntrinsicPtr(const TimeStamp _ts);
+ StateBlockPtr getP(const TimeStamp _ts);
+ StateBlockPtr getO(const TimeStamp _ts);
+ StateBlockPtr getIntrinsic(const TimeStamp _ts);
StateBlockPtr getP() ;
StateBlockPtr getO();
StateBlockPtr getIntrinsic();
- void setPPtr(const StateBlockPtr _p_ptr);
- void setOPtr(const StateBlockPtr _o_ptr);
- void setIntrinsicPtr(const StateBlockPtr _intr_ptr);
+ void setP(const StateBlockPtr _p_ptr);
+ void setO(const StateBlockPtr _o_ptr);
+ void setIntrinsic(const StateBlockPtr _intr_ptr);
void removeStateBlocks();
void fix();
@@ -193,8 +193,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
}
-#include "base/problem.h"
-#include "base/hardware_base.h"
+#include "base/problem/problem.h"
+#include "base/hardware/hardware_base.h"
#include "base/capture/capture_base.h"
#include "base/processor/processor_base.h"
@@ -274,22 +274,22 @@ inline HardwareBasePtr SensorBase::getHardware()
return hardware_ptr_.lock();
}
-inline void SensorBase::setPPtr(const StateBlockPtr _p_ptr)
+inline void SensorBase::setP(const StateBlockPtr _p_ptr)
{
setStateBlockPtrStatic(0, _p_ptr);
}
-inline void SensorBase::setOPtr(const StateBlockPtr _o_ptr)
+inline void SensorBase::setO(const StateBlockPtr _o_ptr)
{
setStateBlockPtrStatic(1, _o_ptr);
}
-inline void SensorBase::setIntrinsicPtr(const StateBlockPtr _intr_ptr)
+inline void SensorBase::setIntrinsic(const StateBlockPtr _intr_ptr)
{
setStateBlockPtrStatic(2, _intr_ptr);
}
-inline void SensorBase::setHardwarePtr(const HardwareBasePtr _hw_ptr)
+inline void SensorBase::setHardware(const HardwareBasePtr _hw_ptr)
{
hardware_ptr_ = _hw_ptr;
}
diff --git a/include/base/sensor/sensor_diff_drive.h b/include/base/sensor/sensor_diff_drive.h
index 0f5cf42592ddf2cf1929ff6e31a9aa294a1963e4..4a38829740849938b7ca0388e54d158cf6ee46bb 100644
--- a/include/base/sensor/sensor_diff_drive.h
+++ b/include/base/sensor/sensor_diff_drive.h
@@ -10,7 +10,7 @@
//wolf includes
#include "base/sensor/sensor_base.h"
-#include "base/diff_drive_tools.h"
+#include "base/processor/diff_drive_tools.h"
namespace wolf {
diff --git a/include/base/sensor/sensor_factory.h b/include/base/sensor/sensor_factory.h
index 32884c18f0b7b1e3133648ebb946d216898a8f56..d74c9b1abb4be615f02323efb2bcec33e32c1dcb 100644
--- a/include/base/sensor/sensor_factory.h
+++ b/include/base/sensor/sensor_factory.h
@@ -15,7 +15,7 @@ struct IntrinsicsBase;
}
// wolf
-#include "base/factory.h"
+#include "base/common/factory.h"
namespace wolf
{
diff --git a/include/base/solver/solver_manager.h b/include/base/solver/solver_manager.h
index f64b3571970400704e9ea680e291b705362cdebc..28e43acfa63426072e6883b2a3237535775d1fc8 100644
--- a/include/base/solver/solver_manager.h
+++ b/include/base/solver/solver_manager.h
@@ -2,8 +2,8 @@
#define _WOLF_SOLVER_MANAGER_H_
//wolf includes
-#include "base/wolf.h"
-#include "base/state_block.h"
+#include "base/common/wolf.h"
+#include "base/state_block/state_block.h"
#include "base/factor/factor_base.h"
namespace wolf {
diff --git a/include/base/solver_suitesparse/cost_function_base.h b/include/base/solver_suitesparse/cost_function_base.h
index 8aae707aed16ec7229c3d4a6730a148d6492c785..25d891b953afd62035fe8b8d4d3047f038e34db4 100644
--- a/include/base/solver_suitesparse/cost_function_base.h
+++ b/include/base/solver_suitesparse/cost_function_base.h
@@ -8,7 +8,7 @@
#ifndef TRUNK_SRC_SOLVER_COST_FUNCTION_BASE_H_
#define TRUNK_SRC_SOLVER_COST_FUNCTION_BASE_H_
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include <Eigen/StdVector>
class CostFunctionBase
diff --git a/include/base/solver_suitesparse/cost_function_sparse.h b/include/base/solver_suitesparse/cost_function_sparse.h
index bdcde3834cc43b950fd968a1c559ee778365e0b0..3ed0eb5e352692eb6af4511f0414f6204f914b33 100644
--- a/include/base/solver_suitesparse/cost_function_sparse.h
+++ b/include/base/solver_suitesparse/cost_function_sparse.h
@@ -2,7 +2,7 @@
#define TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_H_
//wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include "cost_function_sparse_base.h"
// CERES JET
diff --git a/include/base/solver_suitesparse/cost_function_sparse_base.h b/include/base/solver_suitesparse/cost_function_sparse_base.h
index 9aeff876c9134363b1c95055f4fbef9b1d87a740..d90a0577ead23d37aaa6f6a37c0b92c5aa06f89f 100644
--- a/include/base/solver_suitesparse/cost_function_sparse_base.h
+++ b/include/base/solver_suitesparse/cost_function_sparse_base.h
@@ -9,7 +9,7 @@
#define TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_BASE_H_
//wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include "cost_function_base.h"
// CERES JET
diff --git a/include/base/solver_suitesparse/qr_solver.h b/include/base/solver_suitesparse/qr_solver.h
index 154b78639d03c71368b65bbaf65ee778421ba313..0d7f49ccd037785f4b409462fc072127dc36d26e 100644
--- a/include/base/solver_suitesparse/qr_solver.h
+++ b/include/base/solver_suitesparse/qr_solver.h
@@ -13,7 +13,7 @@
#include <ctime>
//Wolf includes
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
#include "../factor_sparse.h"
#include "base/factor/factor_odom_2D.h"
#include "base/factor/factor_corner_2D.h"
diff --git a/include/base/solver_suitesparse/solver_manager.h b/include/base/solver_suitesparse/solver_manager.h
index 09c0abe7f8a83cde800e76968da2a5a990e0007c..3d1ef2a276a95526881859da5fcee019308d2e76 100644
--- a/include/base/solver_suitesparse/solver_manager.h
+++ b/include/base/solver_suitesparse/solver_manager.h
@@ -3,8 +3,8 @@
//wolf includes
#include "base/factor/factor_GPS_2D.h"
-#include "base/wolf.h"
-#include "base/state_block.h"
+#include "base/common/wolf.h"
+#include "base/state_block/state_block.h"
#include "../state_point.h"
#include "../state_complex_angle.h"
#include "../state_theta.h"
diff --git a/include/base/local_parametrization_angle.h b/include/base/state_block/local_parametrization_angle.h
similarity index 95%
rename from include/base/local_parametrization_angle.h
rename to include/base/state_block/local_parametrization_angle.h
index 912237c07a8b4a7aa4d711c39c68f3c9cccf8d24..3644c83ec487f6ca4c21816d32f3e0ed87535b94 100644
--- a/include/base/local_parametrization_angle.h
+++ b/include/base/state_block/local_parametrization_angle.h
@@ -8,8 +8,8 @@
#ifndef LOCAL_PARAMETRIZATION_ANGLE_H_
#define LOCAL_PARAMETRIZATION_ANGLE_H_
-#include "base/local_parametrization_base.h"
-#include "base/rotations.h"
+#include "base/state_block/local_parametrization_base.h"
+#include "base/math/rotations.h"
namespace wolf
{
diff --git a/include/base/local_parametrization_base.h b/include/base/state_block/local_parametrization_base.h
similarity index 97%
rename from include/base/local_parametrization_base.h
rename to include/base/state_block/local_parametrization_base.h
index 3b7dcc38dc698936a707101aa40cad076adc910f..cbc0cbb13aff8535d46926530b99f654714f3832 100644
--- a/include/base/local_parametrization_base.h
+++ b/include/base/state_block/local_parametrization_base.h
@@ -8,7 +8,7 @@
#ifndef LOCAL_PARAMETRIZATION_BASE_H_
#define LOCAL_PARAMETRIZATION_BASE_H_
-#include "base/wolf.h"
+#include "base/common/wolf.h"
namespace wolf {
diff --git a/include/base/local_parametrization_homogeneous.h b/include/base/state_block/local_parametrization_homogeneous.h
similarity index 97%
rename from include/base/local_parametrization_homogeneous.h
rename to include/base/state_block/local_parametrization_homogeneous.h
index f06dd2fb150e5f34eede487e3a685fb88f00d130..cd2076d635ec2f674fb42b16bf000a689700b27c 100644
--- a/include/base/local_parametrization_homogeneous.h
+++ b/include/base/state_block/local_parametrization_homogeneous.h
@@ -8,7 +8,7 @@
#ifndef LOCALPARAMETRIZATIONHOMOGENEOUS_H_
#define LOCALPARAMETRIZATIONHOMOGENEOUS_H_
-#include "base/local_parametrization_base.h"
+#include "base/state_block/local_parametrization_base.h"
namespace wolf {
diff --git a/include/base/local_parametrization_polyline_extreme.h b/include/base/state_block/local_parametrization_polyline_extreme.h
similarity index 95%
rename from include/base/local_parametrization_polyline_extreme.h
rename to include/base/state_block/local_parametrization_polyline_extreme.h
index 3a75a8a0ff4eede5a3e865810555852653494713..0e0cc29cd9ab50b54cb09cde36a12d278e1a846a 100644
--- a/include/base/local_parametrization_polyline_extreme.h
+++ b/include/base/state_block/local_parametrization_polyline_extreme.h
@@ -8,7 +8,7 @@
#ifndef LOCAL_PARAMETRIZATION_POLYLINE_EXTREME_H_
#define LOCAL_PARAMETRIZATION_POLYLINE_EXTREME_H_
-#include "base/local_parametrization_base.h"
+#include "base/state_block/local_parametrization_base.h"
namespace wolf {
diff --git a/include/base/local_parametrization_quaternion.h b/include/base/state_block/local_parametrization_quaternion.h
similarity index 97%
rename from include/base/local_parametrization_quaternion.h
rename to include/base/state_block/local_parametrization_quaternion.h
index f92676558d4cbd8da107852779241a20836c2fe0..c28a05d49d6e1cd8548161e5597393817c14c03f 100644
--- a/include/base/local_parametrization_quaternion.h
+++ b/include/base/state_block/local_parametrization_quaternion.h
@@ -8,7 +8,7 @@
#ifndef LOCAL_PARAMETRIZATION_QUATERNION_H_
#define LOCAL_PARAMETRIZATION_QUATERNION_H_
-#include "base/local_parametrization_base.h"
+#include "base/state_block/local_parametrization_base.h"
namespace wolf {
diff --git a/include/base/state_angle.h b/include/base/state_block/state_angle.h
similarity index 84%
rename from include/base/state_angle.h
rename to include/base/state_block/state_angle.h
index c61286e13eb4fcaa82c70c1c8db8784decfb1b86..ae5133b3261212958e3f418757092562ae162457 100644
--- a/include/base/state_angle.h
+++ b/include/base/state_block/state_angle.h
@@ -8,8 +8,8 @@
#ifndef STATE_ANGLE_H_
#define STATE_ANGLE_H_
-#include "base/state_block.h"
-#include "base/local_parametrization_angle.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/local_parametrization_angle.h"
namespace wolf
{
diff --git a/include/base/state_block.h b/include/base/state_block/state_block.h
similarity index 95%
rename from include/base/state_block.h
rename to include/base/state_block/state_block.h
index 09811c41e583e1155c7aa59a5ce9e93098b188ee..86c94d6ce3c36a2ab84fe3101749e76ca47f5e92 100644
--- a/include/base/state_block.h
+++ b/include/base/state_block/state_block.h
@@ -9,7 +9,7 @@ class LocalParametrizationBase;
}
//Wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
//std includes
#include <iostream>
@@ -112,7 +112,7 @@ public:
/** \brief Sets a local parametrization
**/
- void setLocalParametrizationPtr(LocalParametrizationBasePtr _local_param);
+ void setLocalParametrization(LocalParametrizationBasePtr _local_param);
/** \brief Removes the state_block local parametrization
**/
@@ -154,9 +154,9 @@ public:
} // namespace wolf
// IMPLEMENTATION
-#include "base/local_parametrization_base.h"
-#include "base/node_base.h"
-#include "base/problem.h"
+#include "base/state_block/local_parametrization_base.h"
+#include "base/common/node_base.h"
+#include "base/problem/problem.h"
namespace wolf {
@@ -242,7 +242,7 @@ inline void StateBlock::removeLocalParametrization()
local_param_updated_.store(true);
}
-inline void StateBlock::setLocalParametrizationPtr(LocalParametrizationBasePtr _local_param)
+inline void StateBlock::setLocalParametrization(LocalParametrizationBasePtr _local_param)
{
assert(_local_param != nullptr && "setting a null local parametrization");
local_param_ptr_ = _local_param;
diff --git a/include/base/state_homogeneous_3D.h b/include/base/state_block/state_homogeneous_3D.h
similarity index 91%
rename from include/base/state_homogeneous_3D.h
rename to include/base/state_block/state_homogeneous_3D.h
index adfda9018041d3a9d2469dc0aa16ea7e93ccdfe0..369e14f4308817d5db319d8ed86d26fee8fecbec 100644
--- a/include/base/state_homogeneous_3D.h
+++ b/include/base/state_block/state_homogeneous_3D.h
@@ -8,8 +8,8 @@
#ifndef SRC_STATE_HOMOGENEOUS_3D_H_
#define SRC_STATE_HOMOGENEOUS_3D_H_
-#include "base/state_block.h"
-#include "base/local_parametrization_homogeneous.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/local_parametrization_homogeneous.h"
namespace wolf {
diff --git a/include/base/state_quaternion.h b/include/base/state_block/state_quaternion.h
similarity index 92%
rename from include/base/state_quaternion.h
rename to include/base/state_block/state_quaternion.h
index d990ce1f8c6ed89c3150e4958d7ad570b1e2f606..f7ad39f2d05186b4925618d6422f989288e2956e 100644
--- a/include/base/state_quaternion.h
+++ b/include/base/state_block/state_quaternion.h
@@ -8,8 +8,8 @@
#ifndef SRC_STATE_QUATERNION_H_
#define SRC_STATE_QUATERNION_H_
-#include "base/state_block.h"
-#include "base/local_parametrization_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/local_parametrization_quaternion.h"
namespace wolf {
diff --git a/include/base/trajectory_base.h b/include/base/trajectory/trajectory_base.h
similarity index 89%
rename from include/base/trajectory_base.h
rename to include/base/trajectory/trajectory_base.h
index 9150f8fa0daeb9f7999f38f35a5db02712a63c10..37f9762c7c9955381eaba9bc52aef253441d36e1 100644
--- a/include/base/trajectory_base.h
+++ b/include/base/trajectory/trajectory_base.h
@@ -10,8 +10,8 @@ class TimeStamp;
}
//Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
+#include "base/common/wolf.h"
+#include "base/common/node_base.h"
//std includes
@@ -41,7 +41,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
FrameBasePtr getLastKeyFrame();
FrameBasePtr findLastKeyFrame();
FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts);
- void setLastKeyFramePtr(FrameBasePtr _key_frame_ptr);
+ void setLastKeyFrame(FrameBasePtr _key_frame_ptr);
void sortFrame(FrameBasePtr _frm_ptr);
void moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place);
FrameBaseIter computeFrameOrder(FrameBasePtr _frame_ptr);
@@ -66,7 +66,7 @@ inline FrameBasePtr TrajectoryBase::getLastKeyFrame()
return last_key_frame_ptr_;
}
-inline void TrajectoryBase::setLastKeyFramePtr(FrameBasePtr _key_frame_ptr)
+inline void TrajectoryBase::setLastKeyFrame(FrameBasePtr _key_frame_ptr)
{
last_key_frame_ptr_ = _key_frame_ptr;
}
diff --git a/include/base/eigen_assert.h b/include/base/utils/eigen_assert.h
similarity index 100%
rename from include/base/eigen_assert.h
rename to include/base/utils/eigen_assert.h
diff --git a/include/base/eigen_predicates.h b/include/base/utils/eigen_predicates.h
similarity index 99%
rename from include/base/eigen_predicates.h
rename to include/base/utils/eigen_predicates.h
index 55f2eeeb75df3360e504105bd6cb08660b1cc5ea..bc0ab932275487669cd335140c5b22eb0a7611ad 100644
--- a/include/base/eigen_predicates.h
+++ b/include/base/utils/eigen_predicates.h
@@ -8,7 +8,7 @@
#ifndef _WOLF_EIGEN_PREDICATES_H_
#define _WOLF_EIGEN_PREDICATES_H_
-#include "base/rotations.h"
+#include "base/math/rotations.h"
namespace wolf {
diff --git a/include/base/logging.h b/include/base/utils/logging.h
similarity index 99%
rename from include/base/logging.h
rename to include/base/utils/logging.h
index 3593fc718de7b717db2d19f588fe8e97a1e814ce..088a82152f5eeef569d3e71919924d1ef5e4d511 100644
--- a/include/base/logging.h
+++ b/include/base/utils/logging.h
@@ -17,7 +17,7 @@
#include "spdlog/fmt/bundled/ostream.h"
// Wolf includes
-#include "base/singleton.h"
+#include "base/utils/singleton.h"
namespace wolf {
namespace internal {
diff --git a/include/base/make_unique.h b/include/base/utils/make_unique.h
similarity index 100%
rename from include/base/make_unique.h
rename to include/base/utils/make_unique.h
diff --git a/include/base/singleton.h b/include/base/utils/singleton.h
similarity index 100%
rename from include/base/singleton.h
rename to include/base/utils/singleton.h
diff --git a/serialization/cereal/serialization_local_parametrization_base.h b/serialization/cereal/serialization_local_parametrization_base.h
index 0c38a5a6126723e4e61d0bd15a7616a1b24f7c4c..1385e1af088678e853c0ab0ecb4f5d6a34f103a0 100644
--- a/serialization/cereal/serialization_local_parametrization_base.h
+++ b/serialization/cereal/serialization_local_parametrization_base.h
@@ -1,7 +1,7 @@
#ifndef _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_H_
#define _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_H_
-#include "base/local_parametrization_base.h"
+#include "base/state_block/local_parametrization_base.h"
#include <cereal/cereal.hpp>
#include <cereal/types/polymorphic.hpp>
diff --git a/src/capture/capture_IMU.cpp b/src/capture/capture_IMU.cpp
index 6dbc07d59d21a3b33051d270825be9da060366c7..8bfc7af0489e3639c2c9dbb244faaf3804d110dd 100644
--- a/src/capture/capture_IMU.cpp
+++ b/src/capture/capture_IMU.cpp
@@ -1,6 +1,6 @@
#include "base/capture/capture_IMU.h"
#include "base/sensor/sensor_IMU.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_quaternion.h"
namespace wolf {
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 71b536f9c1780ef2a94a3f46898aadb152d79ccb..1c8c9c17b33556e76a7d4f1b917ee486bc3a3b32 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -106,7 +106,7 @@ void CaptureBase::addFeatureList(FeatureBasePtrList& _new_ft_list)
{
for (FeatureBasePtr feature_ptr : _new_ft_list)
{
- feature_ptr->setCapturePtr(shared_from_this());
+ feature_ptr->setCapture(shared_from_this());
if (getProblem() != nullptr)
feature_ptr->setProblem(getProblem());
}
@@ -122,11 +122,11 @@ void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
- _fac_ptr->setCaptureOtherPtr(shared_from_this());
+ _fac_ptr->setCaptureOther(shared_from_this());
return _fac_ptr;
}
-StateBlockPtr CaptureBase::getStateBlockPtr(unsigned int _i) const
+StateBlockPtr CaptureBase::getStateBlock(unsigned int _i) const
{
if (getSensor())
{
@@ -166,7 +166,7 @@ void CaptureBase::removeStateBlocks()
{
getProblem()->removeStateBlock(sbp);
}
- setStateBlockPtr(i, nullptr);
+ setStateBlock(i, nullptr);
}
}
}
@@ -175,7 +175,7 @@ void CaptureBase::fix()
{
for (unsigned int i = 0; i<getStateBlockVec().size(); i++)
{
- auto sbp = getStateBlockPtr(i);
+ auto sbp = getStateBlock(i);
if (sbp != nullptr)
sbp->fix();
}
@@ -186,7 +186,7 @@ void CaptureBase::unfix()
{
for (unsigned int i = 0; i<getStateBlockVec().size(); i++)
{
- auto sbp = getStateBlockPtr(i);
+ auto sbp = getStateBlock(i);
if (sbp != nullptr)
sbp->unfix();
}
@@ -197,7 +197,7 @@ void CaptureBase::fixExtrinsics()
{
for (unsigned int i = 0; i < 2; i++)
{
- auto sbp = getStateBlockPtr(i);
+ auto sbp = getStateBlock(i);
if (sbp != nullptr)
sbp->fix();
}
@@ -208,7 +208,7 @@ void CaptureBase::unfixExtrinsics()
{
for (unsigned int i = 0; i < 2; i++)
{
- auto sbp = getStateBlockPtr(i);
+ auto sbp = getStateBlock(i);
if (sbp != nullptr)
sbp->unfix();
}
@@ -219,7 +219,7 @@ void CaptureBase::fixIntrinsics()
{
for (unsigned int i = 2; i < getStateBlockVec().size(); i++)
{
- auto sbp = getStateBlockPtr(i);
+ auto sbp = getStateBlock(i);
if (sbp != nullptr)
sbp->fix();
}
@@ -230,7 +230,7 @@ void CaptureBase::unfixIntrinsics()
{
for (unsigned int i = 2; i < getStateBlockVec().size(); i++)
{
- auto sbp = getStateBlockPtr(i);
+ auto sbp = getStateBlock(i);
if (sbp != nullptr)
sbp->unfix();
}
@@ -252,7 +252,7 @@ SizeEigen CaptureBase::computeCalibSize() const
SizeEigen sz = 0;
for (unsigned int i = 0; i < state_block_vec_.size(); i++)
{
- auto sb = getStateBlockPtr(i);
+ auto sb = getStateBlock(i);
if (sb && !sb->isFixed())
sz += sb->getSize();
}
@@ -265,7 +265,7 @@ Eigen::VectorXs CaptureBase::getCalibration() const
SizeEigen index = 0;
for (unsigned int i = 0; i < getStateBlockVec().size(); i++)
{
- auto sb = getStateBlockPtr(i);
+ auto sb = getStateBlock(i);
if (sb && !sb->isFixed())
{
calib.segment(index, sb->getSize()) = sb->getState();
@@ -282,7 +282,7 @@ void CaptureBase::setCalibration(const VectorXs& _calib)
SizeEigen index = 0;
for (unsigned int i = 0; i < getStateBlockVec().size(); i++)
{
- auto sb = getStateBlockPtr(i);
+ auto sb = getStateBlock(i);
if (sb && !sb->isFixed())
{
sb->setState(_calib.segment(index, sb->getSize()));
@@ -295,7 +295,7 @@ void CaptureBase::link(FrameBasePtr _frm_ptr)
{
std::cout << "Linking CaptureBase" << std::endl;
_frm_ptr->addCapture(shared_from_this());
- this->setFramePtr(_frm_ptr);
+ this->setFrame(_frm_ptr);
this->setProblem(_frm_ptr->getProblem());
this->registerNewStateBlocks();
}
diff --git a/src/capture/capture_wheel_joint_position.cpp b/src/capture/capture_wheel_joint_position.cpp
index 7b11d9c8de9e6189be3c9b6a4bef5864b392e9f6..1531820a06d63e478ff533cc0c47e42b582bf486 100644
--- a/src/capture/capture_wheel_joint_position.cpp
+++ b/src/capture/capture_wheel_joint_position.cpp
@@ -1,6 +1,6 @@
#include "base/capture/capture_wheel_joint_position.h"
#include "base/sensor/sensor_diff_drive.h"
-#include "base/rotations.h"
+#include "base/math/rotations.h"
namespace wolf {
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 829393980aa4093dfd86438ece963a58eba8a85c..5871b8fa12ee6f9ce5c3aff8741e80444ab9e166 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -1,9 +1,9 @@
#include "base/ceres_wrapper/ceres_manager.h"
#include "base/ceres_wrapper/create_numeric_diff_cost_function.h"
-#include "base/trajectory_base.h"
-#include "base/map_base.h"
+#include "base/trajectory/trajectory_base.h"
+#include "base/map/map_base.h"
#include "base/landmark/landmark_base.h"
-#include "base/make_unique.h"
+#include "base/utils/make_unique.h"
namespace wolf {
@@ -259,6 +259,9 @@ void CeresManager::addFactor(const FactorBasePtr& fac_ptr)
auto loss_func_ptr = (fac_ptr->getApplyLossFunction()) ? new ceres::CauchyLoss(0.5) : nullptr;
+ //std::cout << "Added residual block corresponding to constraint " << std::static_pointer_cast<CostFunctionWrapper>(cost_func_ptr)->getConstraintPtr()->getType() << std::static_pointer_cast<CostFunctionWrapper>(cost_func_ptr)->getConstraintPtr()->id() <<std::endl;
+
+ assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
assert(fac_2_residual_idx_.find(fac_ptr) == fac_2_residual_idx_.end() && "adding a factor that is already in the fac_2_residual_idx_ map");
fac_2_residual_idx_[fac_ptr] = ceres_problem_->AddResidualBlock(cost_func_ptr.get(),
@@ -296,11 +299,15 @@ void CeresManager::addStateBlock(const StateBlockPtr& state_ptr)
state_ptr->getSize(),
local_parametrization_ptr);
+ if (state_ptr->isFixed())
+ ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr));
+
updateStateBlockStatus(state_ptr);
}
void CeresManager::removeStateBlock(const StateBlockPtr& state_ptr)
{
+ //std::cout << "CeresManager::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl;
assert(state_ptr);
ceres_problem_->RemoveParameterBlock(getAssociatedMemBlockPtr(state_ptr));
state_blocks_local_param_.erase(state_ptr);
diff --git a/src/ceres_wrapper/solver_manager.cpp b/src/ceres_wrapper/solver_manager.cpp
index 06ae2d113c1a4c4a988fa6a684be614c90ed4d9a..72e6e556be97e476e6d57175338790632ef78098 100644
--- a/src/ceres_wrapper/solver_manager.cpp
+++ b/src/ceres_wrapper/solver_manager.cpp
@@ -1,6 +1,6 @@
#include "base/solver/solver_manager.h"
-#include "base/trajectory_base.h"
-#include "base/map_base.h"
+#include "base/trajectory/trajectory_base.h"
+#include "base/map/map_base.h"
#include "base/landmark/landmark_base.h"
namespace wolf {
diff --git a/src/node_base.cpp b/src/common/node_base.cpp
similarity index 75%
rename from src/node_base.cpp
rename to src/common/node_base.cpp
index 24cb56c200843b2c816cb243de2096b9ebcd7f98..882e70a6944ba1b268262004dc46af33707b3221 100644
--- a/src/node_base.cpp
+++ b/src/common/node_base.cpp
@@ -1,4 +1,4 @@
-#include "base/node_base.h"
+#include "base/common/node_base.h"
namespace wolf {
diff --git a/src/time_stamp.cpp b/src/common/time_stamp.cpp
similarity index 97%
rename from src/time_stamp.cpp
rename to src/common/time_stamp.cpp
index 55d25fc6ed7fc8d67d609b2d1f7d16d1455aef1b..3e84bf8460686af0fea308d8897966d8934d008e 100644
--- a/src/time_stamp.cpp
+++ b/src/common/time_stamp.cpp
@@ -1,5 +1,5 @@
-#include "base/time_stamp.h"
+#include "base/common/time_stamp.h"
namespace wolf {
diff --git a/src/examples/processor_imu.yaml b/src/examples/processor_imu.yaml
index 7e684c8833a6e9e3123863c71366a989b30e4004..9d3d4fb807f1cecd9b3fe99ffe733a69622af4be 100644
--- a/src/examples/processor_imu.yaml
+++ b/src/examples/processor_imu.yaml
@@ -1,5 +1,6 @@
processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
+unmeasured perturbation std: 0.00001
keyframe vote:
max time span: 2.0 # seconds
max buffer length: 20000 # motion deltas
diff --git a/src/examples/processor_imu_no_vote.yaml b/src/examples/processor_imu_no_vote.yaml
index 4f6ad39556cd9a09a215f043d4beb0066d4a37bb..4a1dd140e695cefc4590feb9213df76639cc7282 100644
--- a/src/examples/processor_imu_no_vote.yaml
+++ b/src/examples/processor_imu_no_vote.yaml
@@ -1,5 +1,6 @@
processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
+unmeasured perturbation std: 0.00001
keyframe vote:
max time span: 999999.0 # seconds
max buffer length: 999999 # motion deltas
diff --git a/src/examples/processor_imu_t1.yaml b/src/examples/processor_imu_t1.yaml
index e0c21758c11ed2a684b2f3f2bc2aeb4c557c84ef..e290b5ac495cffcd20ae7b8bd2ee4a809fc05c9e 100644
--- a/src/examples/processor_imu_t1.yaml
+++ b/src/examples/processor_imu_t1.yaml
@@ -1,5 +1,6 @@
processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
+unmeasured perturbation std: 0.00001
keyframe vote:
max time span: 0.9999 # seconds
max buffer length: 10000 # motion deltas
diff --git a/src/examples/processor_imu_t6.yaml b/src/examples/processor_imu_t6.yaml
index e3a4b17df72c957fec49d935ddcd3a9a8c824a96..b864e9859a4df06048e61ac83ec48ce18fbcf96c 100644
--- a/src/examples/processor_imu_t6.yaml
+++ b/src/examples/processor_imu_t6.yaml
@@ -1,5 +1,6 @@
processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
+unmeasured perturbation std: 0.00001
keyframe vote:
max time span: 5.9999 # seconds
max buffer length: 10000 # motion deltas
diff --git a/src/examples/solver/test_ccolamd.cpp b/src/examples/solver/test_ccolamd.cpp
index 2fae213bcb4493647a2e35b963af6c89bdb34dfc..d9eec5b00f275b9ed4fc1fd2f309a73daa8a171e 100644
--- a/src/examples/solver/test_ccolamd.cpp
+++ b/src/examples/solver/test_ccolamd.cpp
@@ -6,7 +6,7 @@
*/
// Wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
//std includes
#include <cstdlib>
diff --git a/src/examples/solver/test_iQR_wolf2.cpp b/src/examples/solver/test_iQR_wolf2.cpp
index 93b39fe6572185da38ef7fccb8cb48b6302bab1c..dc4094304626dda2d5f044576da3cbaed648d377 100644
--- a/src/examples/solver/test_iQR_wolf2.cpp
+++ b/src/examples/solver/test_iQR_wolf2.cpp
@@ -17,7 +17,7 @@
#include <queue>
//Wolf includes
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
#include "base/factor/factor_base.h"
#include "base/sensor/sensor_laser_2D.h"
#include "wolf_manager.h"
diff --git a/src/examples/test_2_lasers_offline.cpp b/src/examples/test_2_lasers_offline.cpp
index 10824d51e65440e25dd4a2e5b93b810d5f0a2372..9174dc94f6d30e66ae586d97ecc53896e0f2522a 100644
--- a/src/examples/test_2_lasers_offline.cpp
+++ b/src/examples/test_2_lasers_offline.cpp
@@ -17,7 +17,7 @@
#include "glog/logging.h"
//Wolf includes
-#include "base/problem.h"
+#include "base/problem/problem.h"
#include "base/processor/processor_tracker_landmark_corner.h"
#include "base/processor/processor_odom_2D.h"
#include "base/sensor/sensor_laser_2D.h"
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index d4afdc7d00f1805782648bf826159d5ea955d288..d12f491de7ce0895ca301498454e7f3d451bbd4e 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -17,7 +17,7 @@
#include "glog/logging.h"
//Wolf includes
-#include "base/problem.h"
+#include "base/problem/problem.h"
#include "base/processor/processor_tracker_landmark_corner.h"
#include "base/processor/processor_odom_2D.h"
#include "base/sensor/sensor_laser_2D.h"
diff --git a/src/examples/test_ceres_2_lasers_polylines.cpp b/src/examples/test_ceres_2_lasers_polylines.cpp
index 9e9ca5390a97a42ec61f7ad2a76081fa40e72b5c..136ba285e0d9fb2c62b4801dc75e597dcdd257ff 100644
--- a/src/examples/test_ceres_2_lasers_polylines.cpp
+++ b/src/examples/test_ceres_2_lasers_polylines.cpp
@@ -17,7 +17,7 @@
#include "glog/logging.h"
//Wolf includes
-#include "base/problem.h"
+#include "base/problem/problem.h"
#include "base/processor/processor_tracker_landmark_polyline.h"
#include "base/processor/processor_odom_2D.h"
#include "base/sensor/sensor_laser_2D.h"
diff --git a/src/examples/test_diff_drive.cpp b/src/examples/test_diff_drive.cpp
index 306c7dfdc0fae360e7ec0d68a763cebf278d4182..23980071f58e0e30bc3eeebef8c6d51106e2adf2 100644
--- a/src/examples/test_diff_drive.cpp
+++ b/src/examples/test_diff_drive.cpp
@@ -6,8 +6,8 @@
*/
//Wolf
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
#include "base/sensor/sensor_diff_drive.h"
#include "base/capture/capture_wheel_joint_position.h"
#include "base/processor/processor_diff_drive.h"
diff --git a/src/examples/test_eigen_quaternion.cpp b/src/examples/test_eigen_quaternion.cpp
index 1c01f5e1361a9ee43388a03c39928a405342f244..d24a715cbc8d65f2b86d67e3931b9c1199f22398 100644
--- a/src/examples/test_eigen_quaternion.cpp
+++ b/src/examples/test_eigen_quaternion.cpp
@@ -6,7 +6,7 @@
#include <eigen3/Eigen/Geometry>
//Wolf
-#include "base/wolf.h"
+#include "base/common/wolf.h"
int main()
{
diff --git a/src/examples/test_factor_AHP.cpp b/src/examples/test_factor_AHP.cpp
index b3938285e114b46926da4d5b18255e10e4130be6..75031dff808230fb6ff00b8633699960a6e361e6 100644
--- a/src/examples/test_factor_AHP.cpp
+++ b/src/examples/test_factor_AHP.cpp
@@ -1,8 +1,8 @@
-#include "base/pinhole_tools.h"
+#include "base/math/pinhole_tools.h"
#include "base/landmark/landmark_AHP.h"
#include "base/factor/factor_AHP.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
#include "base/sensor/sensor_camera.h"
#include "base/capture/capture_image.h"
diff --git a/src/examples/test_factor_imu.cpp b/src/examples/test_factor_imu.cpp
index 59d98814f8d1995ad6abf6055a3158caa009a33f..3d0483e9647f9e3203ab3bb89036de55e0f9adc9 100644
--- a/src/examples/test_factor_imu.cpp
+++ b/src/examples/test_factor_imu.cpp
@@ -3,11 +3,11 @@
#include "base/processor/processor_IMU.h"
#include "base/sensor/sensor_IMU.h"
#include "base/capture/capture_pose.h"
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
#include "base/factor/factor_odom_3D.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
#include "base/ceres_wrapper/ceres_manager.h"
//#define DEBUG_RESULTS
@@ -54,7 +54,7 @@ int main(int argc, char** argv)
// Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
CaptureIMUPtr imu_ptr( std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6s::Identity()) );
- imu_ptr->setFramePtr(wolf_problem_ptr_->getTrajectory()->getFrameList().back());
+ imu_ptr->setFrame(wolf_problem_ptr_->getTrajectory()->getFrameList().back());
// set variables
using namespace std;
@@ -87,7 +87,7 @@ int main(int argc, char** argv)
delta_preint_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
- feat_imu->setCapturePtr(imu_ptr);
+ feat_imu->setCapture(imu_ptr);
//create a factorIMU
FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, last_frame);
@@ -115,7 +115,7 @@ int main(int argc, char** argv)
//reset origin of motion to new frame
wolf_problem_ptr_->getProcessorMotion()->setOrigin(last_frame);
- imu_ptr->setFramePtr(last_frame);
+ imu_ptr->setFrame(last_frame);
}
/// ******************************************************************************************** ///
@@ -148,7 +148,7 @@ int main(int argc, char** argv)
delta_preint_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
- feat_imu->setCapturePtr(imu_ptr);
+ feat_imu->setCapture(imu_ptr);
//create a factorIMU
FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, last_frame);
@@ -176,7 +176,7 @@ int main(int argc, char** argv)
//reset origin of motion to new frame
wolf_problem_ptr_->getProcessorMotion()->setOrigin(last_frame);
- imu_ptr->setFramePtr(last_frame);
+ imu_ptr->setFrame(last_frame);
}
mpu_clock = 0.004046;
@@ -206,7 +206,7 @@ int main(int argc, char** argv)
delta_preint_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
- feat_imu->setCapturePtr(imu_ptr);
+ feat_imu->setCapture(imu_ptr);
//create a factorIMU
FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, last_frame);
diff --git a/src/examples/test_faramotics_simulation.cpp b/src/examples/test_faramotics_simulation.cpp
index 621f651866200d68a0b3061cb699c6f56d896954..e095e561d3348cb3ff4445c5f874e8f34ca637e9 100644
--- a/src/examples/test_faramotics_simulation.cpp
+++ b/src/examples/test_faramotics_simulation.cpp
@@ -16,10 +16,10 @@
#include "unistd.h"
// wolf
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include "base/feature/feature_base.h"
#include "base/landmark/landmark_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
//faramotics includes
#include "faramotics/dynamicSceneRender.h"
diff --git a/src/examples/test_imuDock.cpp b/src/examples/test_imuDock.cpp
index dff0efd51fe4a71d8d9dc22b6630f4b50fa62946..dab6894d7a558cf53502ffc58c6a407a3fa4bbe5 100644
--- a/src/examples/test_imuDock.cpp
+++ b/src/examples/test_imuDock.cpp
@@ -7,8 +7,8 @@
#include "base/processor/processor_IMU.h"
#include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
#include "base/ceres_wrapper/ceres_manager.h"
#include "base/sensor/sensor_odom_3D.h"
#include "base/processor/processor_odom_3D.h"
diff --git a/src/examples/test_imuDock_autoKFs.cpp b/src/examples/test_imuDock_autoKFs.cpp
index 4415b685cecace3b3aa348d6ca4adfd748219acd..43a54d654c1611e7d9c5a42ef326c382f2dd9566 100644
--- a/src/examples/test_imuDock_autoKFs.cpp
+++ b/src/examples/test_imuDock_autoKFs.cpp
@@ -7,8 +7,8 @@
#include "base/processor/processor_IMU.h"
#include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
#include "base/ceres_wrapper/ceres_manager.h"
#include "base/sensor/sensor_odom_3D.h"
#include "base/processor/processor_odom_3D.h"
diff --git a/src/examples/test_imuPlateform_Offline.cpp b/src/examples/test_imuPlateform_Offline.cpp
index e7ed6a9157f104629a2d6f41d4277e4450763672..4b923de89990eac72ffc20df61b13e28dbb76b66 100644
--- a/src/examples/test_imuPlateform_Offline.cpp
+++ b/src/examples/test_imuPlateform_Offline.cpp
@@ -2,12 +2,12 @@
#include "base/capture/capture_IMU.h"
#include "base/processor/processor_IMU.h"
#include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
#include "base/sensor/sensor_odom_3D.h"
#include "base/factor/factor_odom_3D.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
#include "base/processor/processor_odom_3D.h"
#include "base/ceres_wrapper/ceres_manager.h"
diff --git a/src/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp
index f134ccc124603660a1d687fe5fa3357427677415..e542e18e0ffac35b9c398ba01ab23bd7fc2e13a1 100644
--- a/src/examples/test_imu_constrained0.cpp
+++ b/src/examples/test_imu_constrained0.cpp
@@ -2,12 +2,12 @@
#include "base/capture/capture_IMU.h"
#include "base/processor/processor_IMU.h"
#include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
#include "base/sensor/sensor_odom_3D.h"
#include "base/factor/factor_odom_3D.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
#include "base/processor/processor_odom_3D.h"
#include "base/ceres_wrapper/ceres_manager.h"
diff --git a/src/examples/test_map_yaml.cpp b/src/examples/test_map_yaml.cpp
index e9a2af536ec532ed94aa8340d0d4d11810e4089a..2dd5ab64d964e3621eb671d03ef5e8cf179256cc 100644
--- a/src/examples/test_map_yaml.cpp
+++ b/src/examples/test_map_yaml.cpp
@@ -5,12 +5,12 @@
* \author: jsola
*/
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/map_base.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
+#include "base/map/map_base.h"
#include "base/landmark/landmark_polyline_2D.h"
#include "base/landmark/landmark_AHP.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
#include "base/yaml/yaml_conversion.h"
#include <iostream>
@@ -32,7 +32,7 @@ void print(MapBase& _map)
std::cout << "\nFirst def: " << poly_ptr->isFirstDefined();
std::cout << "\nLast def: " << poly_ptr->isLastDefined();
for (int idx = poly_ptr->getFirstId(); idx <= poly_ptr->getLastId(); idx++)
- std::cout << "\n point: " << idx << ": " << poly_ptr->getPointStateBlockPtr(idx)->getState().transpose();
+ std::cout << "\n point: " << idx << ": " << poly_ptr->getPointStateBlock(idx)->getState().transpose();
break;
}
else if (lmk_ptr->getType() == "AHP")
diff --git a/src/examples/test_mpu.cpp b/src/examples/test_mpu.cpp
index 0e397cac34729accf6ecd24da3829f011044800a..1fbed214fb99a53c9c4d509ccc3e47d27c744cb1 100644
--- a/src/examples/test_mpu.cpp
+++ b/src/examples/test_mpu.cpp
@@ -7,10 +7,10 @@
//Wolf
#include "base/capture/capture_IMU.h"
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
#include <iostream>
#include <fstream>
#include <iomanip>
diff --git a/src/examples/test_processor_imu.cpp b/src/examples/test_processor_imu.cpp
index 7dc68ebd7d36b75ee7dc15299495ede4bc6f8fe2..33ec4cabb2580353719cf4d50ed14976e626cfaa 100644
--- a/src/examples/test_processor_imu.cpp
+++ b/src/examples/test_processor_imu.cpp
@@ -9,10 +9,10 @@
#include "base/capture/capture_IMU.h"
#include "base/processor/processor_IMU.h"
#include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
#include <iostream>
#include <fstream>
#include <iomanip>
diff --git a/src/examples/test_processor_imu_jacobians.cpp b/src/examples/test_processor_imu_jacobians.cpp
index 6cf92cef243dde6230b39220224c1a2a87b18724..22c797d6d0726e600427680bd475dede67ebc5b8 100644
--- a/src/examples/test_processor_imu_jacobians.cpp
+++ b/src/examples/test_processor_imu_jacobians.cpp
@@ -9,10 +9,10 @@
#include "base/capture/capture_IMU.h"
#include "base/sensor/sensor_IMU.h"
#include <test/processor_IMU_UnitTester.h>
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
#include <iostream>
#include <fstream>
#include <iomanip>
diff --git a/src/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp
index ffeb5ce92272894a19ec5c7161ee354412a7072a..79798150de9d17ece60fbcec7020e5693080d6b6 100644
--- a/src/examples/test_processor_odom_3D.cpp
+++ b/src/examples/test_processor_odom_3D.cpp
@@ -6,10 +6,10 @@
*/
#include "base/capture/capture_IMU.h"
-#include "base/problem.h"
+#include "base/problem/problem.h"
#include "base/sensor/sensor_odom_2D.h"
#include "base/processor/processor_odom_3D.h"
-#include "base/map_base.h"
+#include "base/map/map_base.h"
#include "base/landmark/landmark_base.h"
#include "base/ceres_wrapper/ceres_manager.h"
diff --git a/src/examples/test_processor_tracker_feature.cpp b/src/examples/test_processor_tracker_feature.cpp
index ab22be41878fa14bdd84425ce150b30008834696..77e2b35e9956b01622d81f8ca1e7cdae0b60c25e 100644
--- a/src/examples/test_processor_tracker_feature.cpp
+++ b/src/examples/test_processor_tracker_feature.cpp
@@ -9,10 +9,10 @@
#include <iostream>
//Wolf
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
#include "base/sensor/sensor_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
#include "base/processor/processor_tracker_feature_dummy.h"
#include "base/capture/capture_void.h"
diff --git a/src/examples/test_processor_tracker_landmark.cpp b/src/examples/test_processor_tracker_landmark.cpp
index be1a73a7de7e5efe3be0c88677bae1f79455dcbd..433bd153be7061e2cbfe55f2e10dba658d8d191d 100644
--- a/src/examples/test_processor_tracker_landmark.cpp
+++ b/src/examples/test_processor_tracker_landmark.cpp
@@ -9,10 +9,10 @@
#include <iostream>
//Wolf
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
#include "base/sensor/sensor_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
#include "base/processor/processor_tracker_landmark_dummy.h"
#include "base/capture/capture_void.h"
diff --git a/src/examples/test_processor_tracker_landmark_image.cpp b/src/examples/test_processor_tracker_landmark_image.cpp
index d8b22c18d234ff658b22179060a6008ef663e117..be7df536e971273559252aeff8dc51bd808bb14b 100644
--- a/src/examples/test_processor_tracker_landmark_image.cpp
+++ b/src/examples/test_processor_tracker_landmark_image.cpp
@@ -4,9 +4,9 @@
#include "base/processor/processor_tracker_landmark_image.h"
//Wolf
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/state_block.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
+#include "base/state_block/state_block.h"
#include "base/processor/processor_odom_3D.h"
#include "base/sensor/sensor_odom_3D.h"
#include "base/sensor/sensor_camera.h"
diff --git a/src/examples/test_projection_points.cpp b/src/examples/test_projection_points.cpp
index c24e8904eeaf06490b0fa3283e52cd074f1bcf70..b9f01912d4341df33d3fd270ef8d66c9e15f4477 100644
--- a/src/examples/test_projection_points.cpp
+++ b/src/examples/test_projection_points.cpp
@@ -6,7 +6,7 @@
#include <iostream>
//wolf includes
-#include "base/pinhole_tools.h"
+#include "base/math/pinhole_tools.h"
int main(int argc, char** argv)
{
diff --git a/src/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp
index 142b9980846a38c123e01a61c4df2513f724729e..6bd1805dc78a0c65bd27ca022fbb46bd272cf6d4 100644
--- a/src/examples/test_simple_AHP.cpp
+++ b/src/examples/test_simple_AHP.cpp
@@ -5,11 +5,11 @@
* \author: jtarraso
*/
-#include "base/wolf.h"
-#include "base/frame_base.h"
-#include "base/pinhole_tools.h"
+#include "base/common/wolf.h"
+#include "base/frame/frame_base.h"
+#include "base/math/pinhole_tools.h"
#include "base/sensor/sensor_camera.h"
-#include "base/rotations.h"
+#include "base/math/rotations.h"
#include "base/capture/capture_image.h"
#include "base/landmark/landmark_AHP.h"
#include "base/factor/factor_AHP.h"
diff --git a/src/examples/test_sort_keyframes.cpp b/src/examples/test_sort_keyframes.cpp
index e046765f0bd18cb289a4137f452c581440ab08c8..56982c9dc7c6fda44b594a04f92a33f911b8ca49 100644
--- a/src/examples/test_sort_keyframes.cpp
+++ b/src/examples/test_sort_keyframes.cpp
@@ -6,10 +6,10 @@
*/
// Wolf includes
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/frame_base.h"
-#include "base/trajectory_base.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
+#include "base/frame/frame_base.h"
+#include "base/trajectory/trajectory_base.h"
// STL includes
#include <list>
diff --git a/src/examples/test_state_quaternion.cpp b/src/examples/test_state_quaternion.cpp
index 5885553ed2fbbed5b6769d1f361c84cb9195509f..0fabac44e3252fafad9e940e9bcfd1b62b0e8ec0 100644
--- a/src/examples/test_state_quaternion.cpp
+++ b/src/examples/test_state_quaternion.cpp
@@ -5,9 +5,9 @@
* \author: jsola
*/
-#include "base/frame_base.h"
-#include "base/state_quaternion.h"
-#include "base/time_stamp.h"
+#include "base/frame/frame_base.h"
+#include "base/state_block/state_quaternion.h"
+#include "base/common/time_stamp.h"
#include <iostream>
diff --git a/src/examples/test_wolf_factories.cpp b/src/examples/test_wolf_factories.cpp
index b35d0dc1b335d88759ecdcd3b4c277a309b58636..b608bb9583320b361d88ea2a8c48ff973e85c08d 100644
--- a/src/examples/test_wolf_factories.cpp
+++ b/src/examples/test_wolf_factories.cpp
@@ -7,7 +7,7 @@
#include "base/processor/processor_IMU.h"
#include "base/sensor/sensor_GPS_fix.h"
-#include "base/hardware_base.h"
+#include "base/hardware/hardware_base.h"
#include "base/sensor/sensor_camera.h"
#include "base/sensor/sensor_odom_2D.h"
#include "../sensor_imu.h"
@@ -17,9 +17,9 @@
#include "base/processor/processor_odom_3D.h"
#include "../processor_image_feature.h"
-#include "base/problem.h"
+#include "base/problem/problem.h"
-#include "base/factory.h"
+#include "base/common/factory.h"
#include <iostream>
#include <iomanip>
diff --git a/src/examples/test_wolf_logging.cpp b/src/examples/test_wolf_logging.cpp
index b516acd098a93ce8c060e0c380555ec29041be3a..3b7bdfab70281f10e27b17891564a9b633ac92e1 100644
--- a/src/examples/test_wolf_logging.cpp
+++ b/src/examples/test_wolf_logging.cpp
@@ -5,8 +5,8 @@
* \author: Jeremie Deray
*/
-#include "base/wolf.h"
-#include "base/logging.h"
+#include "base/common/wolf.h"
+#include "base/utils/logging.h"
int main(int, char*[])
{
diff --git a/src/examples/test_wolf_root.cpp b/src/examples/test_wolf_root.cpp
index a2e3ccef5d5534daf5be8b1a360e2c30de6b3e6d..ff78c97b0866ab66440ba91d20d8d0db503aed26 100644
--- a/src/examples/test_wolf_root.cpp
+++ b/src/examples/test_wolf_root.cpp
@@ -6,7 +6,7 @@
*/
//Wolf
-#include "base/wolf.h"
+#include "base/common/wolf.h"
//std
#include <iostream>
diff --git a/src/examples/test_yaml.cpp b/src/examples/test_yaml.cpp
index cfd03d18d073cb9bb85ccc93f342e3ef9c86f52a..5181d73acd049da2469ae0cf6a61296317b865df 100644
--- a/src/examples/test_yaml.cpp
+++ b/src/examples/test_yaml.cpp
@@ -5,10 +5,10 @@
* \author: jsola
*/
-#include "base/pinhole_tools.h"
+#include "base/math/pinhole_tools.h"
#include "yaml/yaml_conversion.h"
#include "processor_image_feature.h"
-#include "base/factory.h"
+#include "base/common/factory.h"
#include <yaml-cpp/yaml.h>
diff --git a/src/factor/factor_analytic.cpp b/src/factor/factor_analytic.cpp
index c86bec4aa9b35d7e7879f5a6c7f53c9181e78570..7ad85937a809b9d524d2f5c30493f1c17d3a5102 100644
--- a/src/factor/factor_analytic.cpp
+++ b/src/factor/factor_analytic.cpp
@@ -1,5 +1,5 @@
#include "base/factor/factor_analytic.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
namespace wolf {
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 00db5144913a87af42b40396a239c03849dfcdaa..2404aa0ecd26a346b4a4803281ce3c50c6d4da53 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -1,5 +1,5 @@
#include "base/factor/factor_base.h"
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
#include "base/landmark/landmark_base.h"
namespace wolf {
@@ -150,7 +150,7 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
std::cout << "Linking FactorBase" << std::endl;
// _ftr_ptr->addConstrainedBy(shared_from_this());
_ftr_ptr->addFactor(shared_from_this());
- this->setFeaturePtr(_ftr_ptr);
+ this->setFeature(_ftr_ptr);
this->setProblem(_ftr_ptr->getProblem());
// add factor to be added in solver
if (this->getProblem() != nullptr)
diff --git a/src/feature/feature_IMU.cpp b/src/feature/feature_IMU.cpp
index 0f811cff599f6bbc62e28dd16b0cc6a82648b02b..b35baf2d0a7dc9191e0a17886aea0a6134b060df 100644
--- a/src/feature/feature_IMU.cpp
+++ b/src/feature/feature_IMU.cpp
@@ -13,7 +13,7 @@ FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated,
jacobian_bias_(_dD_db_jacobians)
{
if (_cap_imu_ptr)
- this->setCapturePtr(_cap_imu_ptr);
+ this->setCapture(_cap_imu_ptr);
}
FeatureIMU::FeatureIMU(CaptureMotionPtr _cap_imu_ptr):
@@ -22,7 +22,7 @@ FeatureIMU::FeatureIMU(CaptureMotionPtr _cap_imu_ptr):
gyro_bias_preint_(_cap_imu_ptr->getCalibrationPreint().tail<3>()),
jacobian_bias_(_cap_imu_ptr->getJacobianCalib())
{
- this->setCapturePtr(_cap_imu_ptr);
+ this->setCapture(_cap_imu_ptr);
}
FeatureIMU::~FeatureIMU()
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index a6de1caf9f99b83539b91ea5f4ed74916d354cec..909d6aaa6efeb014cfa3ac2020b10480553266c4 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -65,7 +65,7 @@ FrameBasePtr FeatureBase::getFrame() const
FactorBasePtr FeatureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
- _fac_ptr->setFeatureOtherPtr(shared_from_this());
+ _fac_ptr->setFeatureOther(shared_from_this());
return _fac_ptr;
}
@@ -137,7 +137,7 @@ Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _info) con
{
std::cout << "Linking FeatureBase" << std::endl;
_cpt_ptr->addFeature(shared_from_this());
- this->setCapturePtr(_cpt_ptr);
+ this->setCapture(_cpt_ptr);
this->setProblem(_cpt_ptr->getProblem());
}
diff --git a/src/frame_base.cpp b/src/frame/frame_base.cpp
similarity index 93%
rename from src/frame_base.cpp
rename to src/frame/frame_base.cpp
index cab2798a0106188e55e243eb5babb530fb3828ea..c01462c1ce98f9a72c5247faec316ecbb5cce39a 100644
--- a/src/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -1,11 +1,11 @@
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
#include "base/factor/factor_base.h"
-#include "base/trajectory_base.h"
+#include "base/trajectory/trajectory_base.h"
#include "base/capture/capture_base.h"
-#include "base/state_block.h"
-#include "base/state_angle.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_angle.h"
+#include "base/state_block/state_quaternion.h"
namespace wolf {
@@ -72,7 +72,7 @@ void FrameBase::remove()
removeStateBlocks();
if (getTrajectory()->getLastKeyFrame()->id() == this_F->id())
- getTrajectory()->setLastKeyFramePtr(getTrajectory()->findLastKeyFrame());
+ getTrajectory()->setLastKeyFrame(getTrajectory()->findLastKeyFrame());
// std::cout << "Removed F" << id() << std::endl;
}
@@ -99,14 +99,14 @@ void FrameBase::removeStateBlocks()
{
for (unsigned int i = 0; i < state_block_vec_.size(); i++)
{
- StateBlockPtr sbp = getStateBlockPtr(i);
+ StateBlockPtr sbp = getStateBlock(i);
if (sbp != nullptr)
{
if (getProblem() != nullptr)
{
getProblem()->removeStateBlock(sbp);
}
- setStateBlockPtr(i, nullptr);
+ setStateBlock(i, nullptr);
}
}
}
@@ -119,7 +119,7 @@ void FrameBase::setKey()
registerNewStateBlocks();
if (getTrajectory()->getLastKeyFrame() == nullptr || getTrajectory()->getLastKeyFrame()->getTimeStamp() < time_stamp_)
- getTrajectory()->setLastKeyFramePtr(shared_from_this());
+ getTrajectory()->setLastKeyFrame(shared_from_this());
getTrajectory()->sortFrame(shared_from_this());
@@ -177,11 +177,7 @@ void FrameBase::setState(const Eigen::VectorXs& _state)
Eigen::VectorXs FrameBase::getState() const
{
- SizeEigen size = 0;
- for (StateBlockPtr sb : state_block_vec_)
- if (sb)
- size += sb->getSize();
- Eigen::VectorXs state(size);
+ Eigen::VectorXs state;
getState(state);
@@ -195,10 +191,9 @@ void FrameBase::getState(Eigen::VectorXs& _state) const
if (sb)
size += sb->getSize();
- assert(_state.size() == size && "Wrong state vector size");
+ _state = Eigen::VectorXs(size);
SizeEigen index = 0;
-
for (StateBlockPtr sb : state_block_vec_)
if (sb)
{
@@ -221,11 +216,6 @@ bool FrameBase::getCovariance(Eigen::MatrixXs& _cov) const
return getProblem()->getFrameCovariance(shared_from_this(), _cov);
}
-Eigen::MatrixXs FrameBase::getCovariance() const
-{
- return getProblem()->getFrameCovariance(shared_from_this());
-}
-
FrameBasePtr FrameBase::getPreviousFrame() const
{
//std::cout << "finding previous frame of " << this->frame_id_ << std::endl;
@@ -338,7 +328,7 @@ void FrameBase::getFactorList(FactorBasePtrList& _fac_list)
FactorBasePtr FrameBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
- _fac_ptr->setFrameOtherPtr(shared_from_this());
+ _fac_ptr->setFrameOther(shared_from_this());
return _fac_ptr;
}
@@ -382,13 +372,13 @@ void FrameBase::link(TrajectoryBasePtr _ptr)
{
std::cout << "Linking FrameBase" << std::endl;
_ptr->addFrame(shared_from_this());
- this->setTrajectoryPtr(_ptr);
+ this->setTrajectory(_ptr);
this->setProblem(_ptr->getProblem());
if (this->isKey()) this->registerNewStateBlocks();
}
} // namespace wolf
-#include "base/factory.h"
+#include "base/common/factory.h"
namespace wolf
{
namespace{ const bool WOLF_UNUSED Frame_PO_2D_Registered = FrameFactory::get().registerCreator("PO 2D", FrameBase::create_PO_2D ); }
diff --git a/src/hardware_base.cpp b/src/hardware/hardware_base.cpp
similarity index 90%
rename from src/hardware_base.cpp
rename to src/hardware/hardware_base.cpp
index a9ac36bf4d2c0b1a213348d7f9f58d2db2b2b1f8..fc324977ccf31ea1a7918a5b18a3461e9598fd20 100644
--- a/src/hardware_base.cpp
+++ b/src/hardware/hardware_base.cpp
@@ -1,4 +1,4 @@
-#include "base/hardware_base.h"
+#include "base/hardware/hardware_base.h"
#include "base/sensor/sensor_base.h"
namespace wolf {
@@ -17,7 +17,6 @@ HardwareBase::~HardwareBase()
SensorBasePtr HardwareBase::addSensor(SensorBasePtr _sensor_ptr)
{
sensor_list_.push_back(_sensor_ptr);
-
return _sensor_ptr;
}
diff --git a/src/landmark/landmark_AHP.cpp b/src/landmark/landmark_AHP.cpp
index 1daa37470b0c1ce87d47e4d174344fd09e471d9d..4fde7a42c99df3a63bcb9ceda382ae970d3a03b3 100644
--- a/src/landmark/landmark_AHP.cpp
+++ b/src/landmark/landmark_AHP.cpp
@@ -1,7 +1,7 @@
#include "base/landmark/landmark_AHP.h"
-#include "base/state_homogeneous_3D.h"
-#include "base/factory.h"
+#include "base/state_block/state_homogeneous_3D.h"
+#include "base/common/factory.h"
#include "base/yaml/yaml_conversion.h"
namespace wolf {
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index e1388b7264c255b4111cbf1fa014a7b5de14f394..ccd3ff57699dd999ba1d3b34f84ee03d26519dea 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -1,8 +1,8 @@
#include "base/landmark/landmark_base.h"
#include "base/factor/factor_base.h"
-#include "base/map_base.h"
-#include "base/state_block.h"
+#include "base/map/map_base.h"
+#include "base/state_block/state_block.h"
#include "base/yaml/yaml_conversion.h"
namespace wolf {
@@ -105,11 +105,6 @@ void LandmarkBase::registerNewStateBlocks()
}
}
-Eigen::MatrixXs LandmarkBase::getCovariance() const
-{
- return getProblem()->getLandmarkCovariance(shared_from_this());
-}
-
bool LandmarkBase::getCovariance(Eigen::MatrixXs& _cov) const
{
return getProblem()->getLandmarkCovariance(shared_from_this(), _cov);
@@ -119,25 +114,21 @@ void LandmarkBase::removeStateBlocks()
{
for (unsigned int i = 0; i < state_block_vec_.size(); i++)
{
- auto sbp = getStateBlockPtr(i);
+ auto sbp = getStateBlock(i);
if (sbp != nullptr)
{
if (getProblem() != nullptr)
{
getProblem()->removeStateBlock(sbp);
}
- setStateBlockPtr(i, nullptr);
+ setStateBlock(i, nullptr);
}
}
}
Eigen::VectorXs LandmarkBase::getState() const
{
- SizeEigen size = 0;
- for (StateBlockPtr sb : state_block_vec_)
- if (sb)
- size += sb->getSize();
- Eigen::VectorXs state(size);
+ Eigen::VectorXs state;
getState(state);
@@ -151,10 +142,9 @@ void LandmarkBase::getState(Eigen::VectorXs& _state) const
if (sb)
size += sb->getSize();
- assert(_state.size() == size && "Wrong state vector size");
+ _state = Eigen::VectorXs(size);
SizeEigen index = 0;
-
for (StateBlockPtr sb : state_block_vec_)
if (sb)
{
@@ -184,7 +174,7 @@ YAML::Node LandmarkBase::saveToYaml() const
{
std::cout << "Linking LandmarkBase" << std::endl;
// this->map_ptr_.lock()->addLandmark(shared_from_this());
- this->setMapPtr(_map_ptr);
+ this->setMap(_map_ptr);
_map_ptr->addLandmark(shared_from_this());
this->setProblem(_map_ptr->getProblem());
this->registerNewStateBlocks();
@@ -193,7 +183,7 @@ YAML::Node LandmarkBase::saveToYaml() const
FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
- _fac_ptr->setLandmarkOtherPtr(shared_from_this());
+ _fac_ptr->setLandmarkOther(shared_from_this());
return _fac_ptr;
}
diff --git a/src/landmark/landmark_container.cpp b/src/landmark/landmark_container.cpp
index 3a424869ba5a8775bda6025b7a83aeadda89009a..3a043fd7d61efda2b2927a9335c6ac92d20dfb32 100644
--- a/src/landmark/landmark_container.cpp
+++ b/src/landmark/landmark_container.cpp
@@ -1,6 +1,6 @@
#include "base/landmark/landmark_container.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
namespace wolf {
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp
index 31051f68e2ab9b0869a06a4fedc267e62711e9ae..9a73f495dc369b451521ecc78755f32262d466c5 100644
--- a/src/landmark/landmark_polyline_2D.cpp
+++ b/src/landmark/landmark_polyline_2D.cpp
@@ -7,11 +7,11 @@
#include "base/feature/feature_polyline_2D.h"
#include "base/landmark/landmark_polyline_2D.h"
-#include "base/local_parametrization_polyline_extreme.h"
+#include "base/state_block/local_parametrization_polyline_extreme.h"
#include "base/factor/factor_point_2D.h"
#include "base/factor/factor_point_to_line_2D.h"
-#include "base/state_block.h"
-#include "base/factory.h"
+#include "base/state_block/state_block.h"
+#include "base/common/factory.h"
#include "base/yaml/yaml_conversion.h"
namespace wolf
@@ -26,9 +26,9 @@ LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_pt
point_state_ptr_vector_.push_back(std::make_shared<StateBlock>(_points.col(i).head<2>()));
if (!first_defined_)
- point_state_ptr_vector_.front()->setLocalParametrizationPtr(std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_[1]));
+ point_state_ptr_vector_.front()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_[1]));
if (!last_defined_)
- point_state_ptr_vector_.back()->setLocalParametrizationPtr(std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_[point_state_ptr_vector_.size() - 2]));
+ point_state_ptr_vector_.back()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_[point_state_ptr_vector_.size() - 2]));
assert(point_state_ptr_vector_.front()->hasLocalParametrization() ? !first_defined_ : first_defined_);
assert(point_state_ptr_vector_.back()->hasLocalParametrization() ? !last_defined_ : last_defined_);
@@ -67,7 +67,7 @@ const Eigen::VectorXs LandmarkPolyline2D::getPointVector(int _i) const
return point_state_ptr_vector_[_i-first_id_]->getState();
}
-StateBlockPtr LandmarkPolyline2D::getPointStateBlockPtr(int _i)
+StateBlockPtr LandmarkPolyline2D::getPointStateBlock(int _i)
{
assert(_i-first_id_ >= 0 && _i-first_id_ <= (int)(point_state_ptr_vector_.size()) && "out of range!");
return point_state_ptr_vector_[_i-first_id_];
@@ -230,7 +230,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
assert(_remain_id < getLastId() || last_defined_);
// take a defined extreme as remaining
- StateBlockPtr remove_state = getPointStateBlockPtr(_remove_id);
+ StateBlockPtr remove_state = getPointStateBlock(_remove_id);
std::cout << "state block to remove " << remove_state->getState().transpose() << std::endl;
// Change factors from remove_state to remain_state
diff --git a/src/map_base.cpp b/src/map/map_base.cpp
similarity index 95%
rename from src/map_base.cpp
rename to src/map/map_base.cpp
index dc9f6e2fd3eaee2c0e5c474409bb88185b5c696d..076e8344c86db7ea257f467dbbe9166addabdc86 100644
--- a/src/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -1,8 +1,8 @@
// wolf
-#include "base/map_base.h"
+#include "base/map/map_base.h"
#include "base/landmark/landmark_base.h"
-#include "base/factory.h"
+#include "base/common/factory.h"
// YAML
#include <yaml-cpp/yaml.h>
@@ -38,7 +38,7 @@ void MapBase::addLandmarkList(LandmarkBasePtrList& _landmark_list)
LandmarkBasePtrList lmk_list_copy = _landmark_list; //since _landmark_list will be empty after addDownNodeList()
for (LandmarkBasePtr landmark_ptr : lmk_list_copy)
{
- landmark_ptr->setMapPtr(shared_from_this());
+ landmark_ptr->setMap(shared_from_this());
landmark_ptr->setProblem(getProblem());
landmark_ptr->registerNewStateBlocks();
}
diff --git a/src/problem.cpp b/src/problem/problem.cpp
similarity index 96%
rename from src/problem.cpp
rename to src/problem/problem.cpp
index 5752f79e3cd991de967f8e78ffc71ef27f93a8af..04da40061aac22df86701fcebf6a84f5b46b38a8 100644
--- a/src/problem.cpp
+++ b/src/problem/problem.cpp
@@ -1,8 +1,8 @@
// wolf includes
-#include "base/problem.h"
-#include "base/hardware_base.h"
-#include "base/trajectory_base.h"
-#include "base/map_base.h"
+#include "base/problem/problem.h"
+#include "base/hardware/hardware_base.h"
+#include "base/trajectory/trajectory_base.h"
+#include "base/map/map_base.h"
#include "base/sensor/sensor_base.h"
#include "base/processor/processor_motion.h"
#include "base/processor/processor_tracker.h"
@@ -10,7 +10,7 @@
#include "base/factor/factor_base.h"
#include "base/sensor/sensor_factory.h"
#include "base/processor/processor_factory.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
// IRI libs includes
@@ -143,7 +143,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
const std::string& _corresponding_sensor_name, //
const std::string& _params_filename)
{
- SensorBasePtr sen_ptr = getSensorPtr(_corresponding_sensor_name);
+ SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name);
if (sen_ptr == nullptr)
throw std::runtime_error("Sensor not found. Cannot bind Processor.");
if (_params_filename == "")
@@ -156,7 +156,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
}
}
-SensorBasePtr Problem::getSensorPtr(const std::string& _sensor_name)
+SensorBasePtr Problem::getSensor(const std::string& _sensor_name)
{
auto sen_it = std::find_if(getHardware()->getSensorList().begin(),
getHardware()->getSensorList().end(),
@@ -252,8 +252,6 @@ Eigen::VectorXs Problem::getCurrentState()
void Problem::getCurrentState(Eigen::VectorXs& state)
{
- assert(state.size() == getFrameStructureSize() && "Problem::getCurrentState: bad state size");
-
if (processor_motion_ptr_ != nullptr)
processor_motion_ptr_->getCurrentState(state);
else if (trajectory_ptr_->getLastKeyFrame() != nullptr)
@@ -264,8 +262,6 @@ void Problem::getCurrentState(Eigen::VectorXs& state)
void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
{
- assert(state.size() == getFrameStructureSize() && "Problem::getCurrentState: bad state size");
-
if (processor_motion_ptr_ != nullptr)
{
processor_motion_ptr_->getCurrentState(state);
@@ -282,8 +278,6 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
void Problem::getState(const TimeStamp& _ts, Eigen::VectorXs& state)
{
- assert(state.size() == getFrameStructureSize() && "Problem::getStateAtTimeStamp: bad state size");
-
// try to get the state from processor_motion if any, otherwise...
if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, state))
{
@@ -370,15 +364,8 @@ void Problem::addLandmarkList(LandmarkBasePtrList& _lmk_list)
StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
{
+ std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
//std::cout << "Problem::addStateBlockPtr " << _state_ptr.get() << std::endl;
- if(std::find(state_block_list_.begin(),state_block_list_.end(),_state_ptr) != state_block_list_.end())
- {
- WOLF_WARN("Adding a state block that has already been added");
- return _state_ptr;
- }
-
- // add the state unit to the list
- state_block_list_.push_back(_state_ptr);
// Add add notification
// Check if there is already a notification for this state block
@@ -395,16 +382,8 @@ StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
void Problem::removeStateBlock(StateBlockPtr _state_ptr)
{
+ std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
//std::cout << "Problem::removeStateBlockPtr " << _state_ptr.get() << std::endl;
- //assert(std::find(state_block_list_.begin(),state_block_list_.end(),_state_ptr) != state_block_list_.end() && "Removing a state_block that hasn't been added or already removed");
- if(std::find(state_block_list_.begin(),state_block_list_.end(),_state_ptr) == state_block_list_.end())
- {
- WOLF_WARN("Removing a state_block that hasn't been added or already removed");
- return;
- }
-
- // add the state unit to the list
- state_block_list_.remove(_state_ptr);
// Check if there is already a notification for this state block
auto notification_it = state_block_notification_map_.find(_state_ptr);
@@ -427,6 +406,7 @@ void Problem::removeStateBlock(StateBlockPtr _state_ptr)
FactorBasePtr Problem::addFactor(FactorBasePtr _factor_ptr)
{
+ std::lock_guard<std::mutex> lock(mut_factor_notifications_);
//std::cout << "Problem::addFactorPtr " << _factor_ptr->id() << std::endl;
// Add ADD notification
@@ -445,6 +425,7 @@ FactorBasePtr Problem::addFactor(FactorBasePtr _factor_ptr)
void Problem::removeFactor(FactorBasePtr _factor_ptr)
{
+ std::lock_guard<std::mutex> lock(mut_factor_notifications_);
//std::cout << "Problem::removeFactorPtr " << _factor_ptr->id() << std::endl;
// Check if there is already a notification for this state block
@@ -466,6 +447,7 @@ void Problem::removeFactor(FactorBasePtr _factor_ptr)
void Problem::clearCovariance()
{
+ std::lock_guard<std::mutex> lock(mut_covariances_);
covariances_.clear();
}
@@ -474,6 +456,7 @@ void Problem::addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, c
assert(_state1->getSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
assert(_state2->getSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
+ std::lock_guard<std::mutex> lock(mut_covariances_);
covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] = _cov;
}
@@ -482,6 +465,7 @@ void Problem::addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXs& _
assert(_state1->getSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
assert(_state1->getSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
+ std::lock_guard<std::mutex> lock(mut_covariances_);
covariances_[std::make_pair(_state1, _state1)] = _cov;
}
@@ -501,6 +485,8 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, E
assert(_row + _state1->getSize() <= _cov.rows() && _col + _state2->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
+ std::lock_guard<std::mutex> lock(mut_covariances_);
+
if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end())
_cov.block(_row, _col, _state1->getSize(), _state2->getSize()) =
covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)];
@@ -518,6 +504,8 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, E
bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov)
{
+ std::lock_guard<std::mutex> lock(mut_covariances_);
+
// fill covariance
for (auto it1 = _sb_2_idx.begin(); it1 != _sb_2_idx.end(); it1++)
for (auto it2 = it1; it2 != _sb_2_idx.end(); it2++)
@@ -567,6 +555,16 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs&
const auto& state_bloc_vec = _frame_ptr->getStateBlockVec();
+ // computing size
+ SizeEigen sz = 0;
+ for (const auto& sb : state_bloc_vec)
+ if (sb)
+ sz += sb->getSize();
+
+ // resizing
+ _covariance = Eigen::MatrixXs(sz, sz);
+
+ // filling covariance
for (const auto& sb_i : state_bloc_vec)
{
if (sb_i)
@@ -586,23 +584,10 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs&
return success;
}
-Eigen::MatrixXs Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr)
-{
- SizeEigen sz = 0;
- for (const auto& sb : _frame_ptr->getStateBlockVec())
- if (sb)
- sz += sb->getSize();
-
- Eigen::MatrixXs covariance(sz, sz);
-
- getFrameCovariance(_frame_ptr, covariance);
- return covariance;
-}
-
-Eigen::MatrixXs Problem::getLastKeyFrameCovariance()
+bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXs& cov)
{
FrameBasePtr frm = getLastKeyFrame();
- return getFrameCovariance(frm);
+ return getFrameCovariance(frm, cov);
}
bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance)
@@ -612,6 +597,17 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M
const auto& state_bloc_vec = _landmark_ptr->getStateBlockVec();
+ // computing size
+ SizeEigen sz = 0;
+ for (const auto& sb : state_bloc_vec)
+ if (sb)
+ sz += sb->getSize();
+
+ // resizing
+ _covariance = Eigen::MatrixXs(sz, sz);
+
+ // filling covariance
+
for (const auto& sb_i : state_bloc_vec)
{
if (sb_i)
@@ -631,19 +627,6 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M
return success;
}
-Eigen::MatrixXs Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr)
-{
- SizeEigen sz = 0;
- for (const auto& sb : _landmark_ptr->getStateBlockVec())
- if (sb)
- sz += sb->getSize();
-
- Eigen::MatrixXs covariance(sz, sz);
-
- getLandmarkCovariance(_landmark_ptr, covariance);
- return covariance;
-}
-
MapBasePtr Problem::getMap()
{
return map_ptr_;
@@ -669,11 +652,6 @@ FrameBasePtr Problem::getLastKeyFrame()
return trajectory_ptr_->getLastKeyFrame();
}
-StateBlockPtrList& Problem::getStateBlockPtrList()
-{
- return state_block_list_;
-}
-
FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen::MatrixXs& _prior_cov, const TimeStamp& _ts, const Scalar _time_tolerance)
{
if ( ! prior_is_set_ )
@@ -753,7 +731,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
{
if (i==0) cout << " Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
if (i==2) cout << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
- auto sb = S->getStateBlockPtr(i);
+ auto sb = S->getStateBlock(i);
if (sb)
{
cout << (sb->isFixed() ? " Fix( " : " Est( ") << sb->getState().transpose() << " )";
diff --git a/src/motion_buffer.cpp b/src/processor/motion_buffer.cpp
similarity index 99%
rename from src/motion_buffer.cpp
rename to src/processor/motion_buffer.cpp
index 20d4fc26060fbd12087ae51b256ca85fe528de2d..2828ad4d0262f77af8fe09a45bc28f93a83ff7b7 100644
--- a/src/motion_buffer.cpp
+++ b/src/processor/motion_buffer.cpp
@@ -1,4 +1,4 @@
-#include "base/motion_buffer.h"
+#include "base/processor/motion_buffer.h"
namespace wolf
{
diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp
index dde8bc48263a8f492fdfb34535e17af68469c347..74ab12004ea8c23715474c4805f5e9935c3d4ea6 100644
--- a/src/processor/processor_IMU.cpp
+++ b/src/processor/processor_IMU.cpp
@@ -1,7 +1,7 @@
// wolf
#include "base/processor/processor_IMU.h"
#include "base/factor/factor_IMU.h"
-#include "base/IMU_tools.h"
+#include "base/math/IMU_tools.h"
namespace wolf {
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 115c4fe9e73c755020bf6aa35102020ef734bbff..03db0e2c88bb2c515c21a91bb9b04247097f583a 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -1,7 +1,7 @@
#include "base/processor/processor_base.h"
#include "base/processor/processor_motion.h"
#include "base/capture/capture_base.h"
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
namespace wolf {
@@ -77,7 +77,7 @@ void ProcessorBase::remove()
{
std::cout << "Linking ProcessorBase" << std::endl;
_sen_ptr->addProcessor(shared_from_this());
- this->setSensorPtr(_sen_ptr);
+ this->setSensor(_sen_ptr);
this->setProblem(_sen_ptr->getProblem());
}
/////////////////////////////////////////////////////////////////////////////////////////
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index f6fc883f8f5a96ac2c213ab88fc0fdb5c76be8c8..66bf37a0a773336af43197da68963d041218f574 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -5,7 +5,7 @@
#include "base/capture/capture_wheel_joint_position.h"
#include "base/capture/capture_velocity.h"
-#include "base/rotations.h"
+#include "base/math/rotations.h"
#include "base/factor/factor_odom_2D.h"
#include "base/feature/feature_diff_drive.h"
diff --git a/src/processor/processor_frame_nearest_neighbor_filter.cpp b/src/processor/processor_frame_nearest_neighbor_filter.cpp
index 769cf871be36a1969a0e2c4883b044f9cabdbda3..7c4e63a75031de399473c9fa1c6e764fa28331a9 100644
--- a/src/processor/processor_frame_nearest_neighbor_filter.cpp
+++ b/src/processor/processor_frame_nearest_neighbor_filter.cpp
@@ -200,8 +200,12 @@ bool ProcessorFrameNearestNeighborFilter::computeEllipse2D(const FrameBasePtr& f
Eigen::Vector5s& ellipse)
{
// get 3x3 covariance matrix AKA variance
- const Eigen::MatrixXs covariance =
- getProblem()->getFrameCovariance(frame_ptr);
+ Eigen::MatrixXs covariance;
+ if (!getProblem()->getFrameCovariance(frame_ptr, covariance))
+ {
+ WOLF_WARN("Frame covariance not found!");
+ return false;
+ }
if (!isCovariance(covariance))
{
@@ -245,9 +249,18 @@ bool ProcessorFrameNearestNeighborFilter::computeEllipsoid3D(const FrameBasePtr&
ellipsoid(2) = frame_position(2);
// get 9x9 covariance matrix AKA variance
- const Eigen::MatrixXs covariance = getProblem()->getFrameCovariance(frame_pointer);
+ Eigen::MatrixXs covariance;
+ if (!getProblem()->getFrameCovariance(frame_pointer, covariance))
+ {
+ WOLF_WARN("Frame covariance not found!");
+ return false;
+ }
- if (!isCovariance(covariance)) return false;
+ if (!isCovariance(covariance))
+ {
+ WOLF_WARN("Covariance is not proper !");
+ return false;
+ }
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3s> solver(covariance.block(0,0,3,3));
const Scalar eigenvalue1 = solver.eigenvalues()[0]; // smaller value
@@ -464,10 +477,11 @@ Scalar ProcessorFrameNearestNeighborFilter::MahalanobisDistance(const FrameBaseP
query_pose << query->getP()->getState(),
query->getO()->getState();
- const Eigen::MatrixXs traj_covariance = getProblem()->getFrameCovariance(trajectory);
- const Eigen::MatrixXs query_covariance = getProblem()->getFrameCovariance(query);
-
- if ( !isCovariance(traj_covariance) || !isCovariance(query_covariance))
+ Eigen::MatrixXs traj_covariance, query_covariance;
+ if ( !getProblem()->getFrameCovariance(trajectory, traj_covariance) ||
+ !getProblem()->getFrameCovariance(query, query_covariance) ||
+ !isCovariance(traj_covariance) ||
+ !isCovariance(query_covariance))
return distance;
const Eigen::MatrixXs covariance = traj_covariance * query_covariance.transpose();
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 0004da85f26d9e0b74fae44e19645a8ecf8702ea..ed4ff945caf77d7870f6ff9a8c7674ce011b497d 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -63,7 +63,7 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
}
// Update the existing capture
- _capture_source->setOriginFramePtr(_keyframe_target);
+ _capture_source->setOriginFrame(_keyframe_target);
// re-integrate existing buffer -- note: the result of re-integration is stored in the same buffer!
reintegrateBuffer(_capture_source);
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 9693861d3630bbda063a2283bd899277344ea556..61d843be1db548843212de3fcd9b52516aa3d703 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -53,7 +53,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
case FIRST_TIME_WITH_PACK :
{
PackKeyFramePtr pack = kf_pack_buffer_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
- kf_pack_buffer_.removeUpTo( incoming_ptr_->getTimeStamp() );
+ kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() );
WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
@@ -127,7 +127,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
case RUNNING_WITH_PACK :
{
PackKeyFramePtr pack = kf_pack_buffer_.selectPack( last_ptr_ , params_tracker_->time_tolerance);
- kf_pack_buffer_.removeUpTo( last_ptr_->getTimeStamp() );
+ kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() );
WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
@@ -176,6 +176,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
// We create a KF
// set KF on last
+ last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
last_ptr_->getFrame()->setKey();
// make F; append incoming to new F
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index 4026f4324d08231190e098ea2d94d6a5803df360..5c2fb4c06a9c9a7a3dadf36f9df1db2239300724 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -21,7 +21,7 @@ ProcessorTrackerFeature::~ProcessorTrackerFeature()
{
}
-unsigned int ProcessorTrackerFeature::processNew(const unsigned int& _max_new_features)
+unsigned int ProcessorTrackerFeature::processNew(const int& _max_new_features)
{
/* Rationale: A keyFrame will be created using the last Capture.
* First, we work on the last Capture to detect new Features,
@@ -150,20 +150,7 @@ void ProcessorTrackerFeature::establishFactors()
auto fac_ptr = createFactor(feature_in_last, feature_in_origin);
- // feature_in_last ->addFactor(fac_ptr);
fac_ptr->link(feature_in_last);
- // feature_in_origin->addConstrainedBy(fac_ptr);
-
- // if (fac_ptr != nullptr) // factor links
- // {
- // FrameBasePtr frm = fac_ptr->getFrameOther();
- // if (frm)
- // frm->addConstrainedBy(fac_ptr);
- // CaptureBasePtr cap = fac_ptr->getCaptureOther();
- // if (cap)
- // cap->addConstrainedBy(fac_ptr);
- // }
-
WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
" origin: " , feature_in_origin->id() ,
diff --git a/src/processor/processor_tracker_feature_corner.cpp b/src/processor/processor_tracker_feature_corner.cpp
index c68ea29713f8977c86f0621852d767c846e1a500..a15e68842b09125190893a920ea452a73b1c7532 100644
--- a/src/processor/processor_tracker_feature_corner.cpp
+++ b/src/processor/processor_tracker_feature_corner.cpp
@@ -107,10 +107,13 @@ bool ProcessorTrackerFeatureCorner::voteForKeyFrame()
return incoming_ptr_->getFeatureList().size() < params_tracker_feature_corner_->n_corners_th;
}
-unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
{
// in corners_last_ remain all not tracked corners
_features_incoming_out = std::move(corners_last_);
+ if (_max_features != -1 && _features_incoming_out.size() > _max_features)
+ _features_incoming_out.resize(_max_features);
+
return _features_incoming_out.size();
}
diff --git a/src/processor/processor_tracker_feature_dummy.cpp b/src/processor/processor_tracker_feature_dummy.cpp
index 060b54e83a3aba8e7b483c2e2678b88b7e468678..b5b225f8490b8cf4b4f6823be870fc3f22ab8df7 100644
--- a/src/processor/processor_tracker_feature_dummy.cpp
+++ b/src/processor/processor_tracker_feature_dummy.cpp
@@ -49,12 +49,19 @@ bool ProcessorTrackerFeatureDummy::voteForKeyFrame()
return incoming_ptr_->getFeatureList().size() < params_tracker_feature_->min_features_for_keyframe;
}
-unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
{
+ unsigned int max_features = _max_features;
+
+ if (max_features == -1)
+ {
+ max_features = 10;
+ WOLF_INFO("max_features unlimited, setting it to " , max_features);
+ }
WOLF_INFO("Detecting " , _max_features , " new features..." );
// detecting new features
- for (unsigned int i = 1; i <= _max_features; i++)
+ for (unsigned int i = 0; i < max_features; i++)
{
n_feature_++;
FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE",
diff --git a/src/processor/processor_tracker_feature_image.cpp b/src/processor/processor_tracker_feature_image.cpp
index 3f6bec8fe828580d8b04e5b0c390a8441cc24a9e..c2f68524b6fcae33ff95a838601618a613784ff9 100644
--- a/src/processor/processor_tracker_feature_image.cpp
+++ b/src/processor/processor_tracker_feature_image.cpp
@@ -244,7 +244,7 @@ bool ProcessorTrackerFeatureImage::correctFeatureDrift(const FeatureBasePtr _ori
}
}
-unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out)
{
cv::Rect roi;
KeyPointVector new_keypoints;
@@ -252,7 +252,7 @@ unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int&
cv::KeyPointsFilter keypoint_filter;
unsigned int n_new_features = 0;
- for (unsigned int n_iterations = 0; _max_new_features == 0 || n_iterations < _max_new_features; n_iterations++)
+ for (unsigned int n_iterations = 0; _max_new_features == -1 || n_iterations < _max_new_features; n_iterations++)
{
if (active_search_ptr_->pickEmptyRoi(roi))
diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp
index be79d8c9a60ca4b8670e1309498fed670b9b5b88..4cfe33ee28bbb0e2bfab33f1156c14e9da4bce54 100644
--- a/src/processor/processor_tracker_feature_trifocal.cpp
+++ b/src/processor/processor_tracker_feature_trifocal.cpp
@@ -134,7 +134,7 @@ bool ProcessorTrackerFeatureTrifocal::isInlier(const cv::KeyPoint& _kp_last, con
}
-unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out)
{
// // DEBUG =====================================
// clock_t debug_tStart;
@@ -143,7 +143,7 @@ unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned i
// WOLF_TRACE("======== DetectNewFeatures =========");
// // ===========================================
- for (unsigned int n_iterations = 0; n_iterations < _max_new_features; ++n_iterations)
+ for (unsigned int n_iterations = 0; _max_new_features == -1 || n_iterations < _max_new_features; ++n_iterations)
{
Eigen::Vector2i cell_last;
if (capture_last_->grid_features_->pickEmptyTrackingCell(cell_last))
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index ba2bf986ef9065e0568ca630299c10728d7ffc07..8992bded79c0426fb6e8f62cab4928bdf75b3d6e 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -6,7 +6,7 @@
*/
#include "base/processor/processor_tracker_landmark.h"
-#include "base/map_base.h"
+#include "base/map/map_base.h"
#include <utility>
@@ -60,7 +60,7 @@ void ProcessorTrackerLandmark::resetDerived()
// std::cout << "\t" << match.first->id() << " to " << match.second.landmark_ptr_->id() << std::endl;
}
-unsigned int ProcessorTrackerLandmark::processNew(const unsigned int& _max_features)
+unsigned int ProcessorTrackerLandmark::processNew(const int& _max_features)
{
/* Rationale: A keyFrame will be created using the last Capture.
* First, we work on this Capture to detect new Features,
@@ -135,15 +135,7 @@ void ProcessorTrackerLandmark::establishFactors()
lmk);
if (fac_ptr != nullptr) // factor links
{
- // last_feature->addFactor(fac_ptr);
fac_ptr->link(last_feature);
- // lmk->addConstrainedBy(fac_ptr);
- // FrameBasePtr frm = fac_ptr->getFrameOther();
- // if (frm)
- // frm->addConstrainedBy(fac_ptr);
- // CaptureBasePtr cap = fac_ptr->getCaptureOther();
- // if (cap)
- // cap->addConstrainedBy(fac_ptr);
}
}
}
diff --git a/src/processor/processor_tracker_landmark_corner.cpp b/src/processor/processor_tracker_landmark_corner.cpp
index aeba30f8e908a72a57aad5b5f617edfa249b8982..884f867c7fe46c0bac1d7539c4dde58ddbf02a72 100644
--- a/src/processor/processor_tracker_landmark_corner.cpp
+++ b/src/processor/processor_tracker_landmark_corner.cpp
@@ -1,5 +1,5 @@
#include "base/processor/processor_tracker_landmark_corner.h"
-#include "base/rotations.h"
+#include "base/math/rotations.h"
namespace wolf
{
@@ -375,10 +375,13 @@ LandmarkBasePtr ProcessorTrackerLandmarkCorner::createLandmark(FeatureBasePtr _f
_feature_ptr->getMeasurement()(3));
}
-unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
{
- // already computed since each scan is computed in preprocess()
+ // in corners_last_ remain all not tracked corners, already computed in preprocess()
_features_incoming_out = std::move(corners_last_);
+ if (_max_features != -1 && _features_incoming_out.size() > _max_features)
+ _features_incoming_out.resize(_max_features);
+
return _features_incoming_out.size();
}
diff --git a/src/processor/processor_tracker_landmark_dummy.cpp b/src/processor/processor_tracker_landmark_dummy.cpp
index b2f92d72dd8bdc8d377905977185da6ae776688f..f514cb7efd76a571c7cd6f74c1353b21ca745050 100644
--- a/src/processor/processor_tracker_landmark_dummy.cpp
+++ b/src/processor/processor_tracker_landmark_dummy.cpp
@@ -63,16 +63,25 @@ bool ProcessorTrackerLandmarkDummy::voteForKeyFrame()
return incoming_ptr_->getFeatureList().size() < 4;
}
-unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
{
std::cout << "\tProcessorTrackerLandmarkDummy::detectNewFeatures" << std::endl;
- // detecting 5 new features
- for (unsigned int i = 1; i <= _max_features; i++)
+ unsigned int max_features = _max_features;
+
+ if (max_features == -1)
+ {
+ max_features = 10;
+ WOLF_INFO("max_features unlimited, setting it to " , max_features);
+ }
+
+ // detecting new features
+ for (unsigned int i = 1; i <= max_features; i++)
{
n_feature_++;
- _features_incoming_out.push_back(
- std::make_shared<FeatureBase>("POINT IMAGE", n_feature_ * Eigen::Vector1s::Ones(), Eigen::MatrixXs::Ones(1, 1)));
+ _features_incoming_out.push_back(std::make_shared<FeatureBase>("POINT IMAGE",
+ n_feature_ * Eigen::Vector1s::Ones(),
+ Eigen::MatrixXs::Ones(1, 1)));
std::cout << "\t\tfeature " << _features_incoming_out.back()->getMeasurement() << " detected!" << std::endl;
}
return _features_incoming_out.size();
diff --git a/src/processor/processor_tracker_landmark_image.cpp b/src/processor/processor_tracker_landmark_image.cpp
index a88ec47c932f3c1c7f9098a532285c20940dbeda..515daff2acbd60b429b5ae35af570171861db9ac 100644
--- a/src/processor/processor_tracker_landmark_image.cpp
+++ b/src/processor/processor_tracker_landmark_image.cpp
@@ -4,14 +4,14 @@
#include "base/factor/factor_AHP.h"
#include "base/feature/feature_base.h"
#include "base/feature/feature_point_image.h"
-#include "base/frame_base.h"
-#include "base/logging.h"
-#include "base/map_base.h"
-#include "base/pinhole_tools.h"
-#include "base/problem.h"
+#include "base/frame/frame_base.h"
+#include "base/utils/logging.h"
+#include "base/map/map_base.h"
+#include "base/math/pinhole_tools.h"
+#include "base/problem/problem.h"
#include "base/sensor/sensor_camera.h"
-#include "base/state_block.h"
-#include "base/time_stamp.h"
+#include "base/state_block/state_block.h"
+#include "base/common/time_stamp.h"
// vision_utils
#include <detectors.h>
@@ -225,7 +225,7 @@ bool ProcessorTrackerLandmarkImage::voteForKeyFrame()
// return landmarks_tracked_ < params_tracker_landmark_image_->min_features_for_keyframe;
}
-unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
{
cv::Rect roi;
KeyPointVector new_keypoints;
@@ -233,7 +233,7 @@ unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int
cv::KeyPointsFilter keypoint_filter;
unsigned int n_new_features = 0;
- for (unsigned int n_iterations = 0; n_iterations < _max_features; n_iterations++)
+ for (unsigned int n_iterations = 0; _max_features == -1 || n_iterations < _max_features; n_iterations++)
{
if (active_search_ptr_->pickEmptyRoi(roi))
{
diff --git a/src/processor/processor_tracker_landmark_polyline.cpp b/src/processor/processor_tracker_landmark_polyline.cpp
deleted file mode 100644
index e39e4633e731b06dd7d003e27c92cd1e54f9e4fd..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker_landmark_polyline.cpp
+++ /dev/null
@@ -1,1026 +0,0 @@
-#include "base/processor/processor_tracker_landmark_polyline.h"
-
-namespace wolf
-{
-
-void ProcessorTrackerLandmarkPolyline::preProcess()
-{
- //std::cout << "PreProcess: " << std::endl;
-
- // extract corners of incoming
- extractPolylines(std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_), polylines_incoming_);
-
- // compute transformations
- computeTransformations(incoming_ptr_->getTimeStamp());
-
- //std::cout << "PreProcess: incoming new features: " << polylines_incoming_.size() << std::endl;
-}
-
-void ProcessorTrackerLandmarkPolyline::computeTransformations(const TimeStamp& _ts)
-{
- Eigen::Vector3s vehicle_pose = getProblem()->getState(_ts);
- t_world_robot_ = vehicle_pose.head<2>();
-
- // world_robot
- Eigen::Matrix2s R_world_robot = Eigen::Rotation2Ds(vehicle_pose(2)).matrix();
-
- // robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
- if (getSensor()->extrinsicsInCaptures() || !getSensor()->getP()->isFixed()
- || !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_)
- {
- t_robot_sensor_ = getSensor()->getP()->getState();
- R_robot_sensor_ = Eigen::Rotation2Ds(getSensor()->getO()->getState()(0)).matrix();
- extrinsics_transformation_computed_ = true;
- }
-
- // global_sensor
- R_world_sensor_ = R_world_robot * R_robot_sensor_;
- t_world_sensor_ = t_world_robot_ + R_world_robot * t_robot_sensor_;
-
- // sensor_global
- R_sensor_world_ = R_robot_sensor_.transpose() * R_world_robot.transpose();
- t_sensor_world_ = -R_robot_sensor_.transpose() * t_robot_sensor_ -R_sensor_world_ * t_world_robot_ ;
-
- //std::cout << "t_world_robot_ " << t_world_robot_.transpose() << std::endl;
- //std::cout << "t_robot_sensor_ " << t_robot_sensor_.transpose() << std::endl;
- //std::cout << "R_robot_sensor_ " << std::endl << R_robot_sensor_ << std::endl;
- //std::cout << "t_world_sensor_ " << t_world_sensor_.transpose() << std::endl;
- //std::cout << "R_world_sensor_ " << std::endl << R_world_sensor_ << std::endl;
- //std::cout << "t_sensor_world_ " << t_sensor_world_.transpose() << std::endl;
- //std::cout << "R_sensor_world_ " << std::endl << R_sensor_world_ << std::endl;
-
-}
-
-unsigned int ProcessorTrackerLandmarkPolyline::findLandmarks(const LandmarkBasePtrList& _landmarks_searched,
- FeatureBasePtrList& _features_found,
- LandmarkMatchMap& _feature_landmark_correspondences)
-{
- //std::cout << "ProcessorTrackerLandmarkPolyline::findLandmarks: " << _landmarks_searched.size() << " features: " << polylines_incoming_.size() << std::endl;
-
- // COMPUTING ALL EXPECTED FEATURES
- std::map<LandmarkBasePtr, Eigen::MatrixXs> expected_features;
- std::map<LandmarkBasePtr, Eigen::MatrixXs> expected_features_covs;
- for (auto landmark : _landmarks_searched)
- if (landmark->getType() == "POLYLINE 2D")
- {
- expected_features[landmark] = Eigen::MatrixXs(3, (std::static_pointer_cast<LandmarkPolyline2D>(landmark))->getNPoints());
- expected_features_covs[landmark] = Eigen::MatrixXs(2, 2*(std::static_pointer_cast<LandmarkPolyline2D>(landmark))->getNPoints());
- expectedFeature(landmark, expected_features[landmark], expected_features_covs[landmark]);
- }
-
- // NAIVE NEAREST NEIGHBOR MATCHING
- LandmarkPolylineMatchPtr best_match = nullptr;
- FeaturePolyline2DPtr polyline_feature;
- LandmarkPolyline2DPtr polyline_landmark;
-
- auto next_feature_it = polylines_incoming_.begin();
- auto feature_it = next_feature_it++;
- int max_ftr, max_lmk, max_offset, min_offset, offset, from_ftr, from_lmk, to_ftr, to_lmk, N_overlapped;
-
- // iterate over all polylines features
- while (feature_it != polylines_incoming_.end())
- {
- polyline_feature = std::static_pointer_cast<FeaturePolyline2D>(*feature_it);
- max_ftr = polyline_feature->getNPoints() - 1;
-
- // Check with all landmarks
- for (auto landmark_it = _landmarks_searched.begin(); landmark_it != _landmarks_searched.end(); landmark_it++)
- {
- polyline_landmark = std::static_pointer_cast<LandmarkPolyline2D>(*landmark_it);
-
- // Open landmark polyline
- if (!polyline_landmark->isClosed())
- {
- //std::cout << "MATCHING WITH OPEN LANDMARK" << std::endl;
- //std::cout << "\tfeature " << polyline_feature->id() << ": 0-" << max_ftr << std::endl;
- //std::cout << "\tlandmark " << polyline_landmark->id() << ": 0-" << polyline_landmark->getNPoints() - 1 << std::endl;
- max_lmk = polyline_landmark->getNPoints() - 1;
- max_offset = max_ftr;
- min_offset = -max_lmk;
-
- // Check all overlapping positions between each feature-landmark pair
- for (offset = min_offset; offset <= max_offset; offset++)
- {
- if (offset == min_offset && !polyline_landmark->isLastDefined() && !polyline_feature->isFirstDefined())
- continue;
-
- if (offset == max_offset && !polyline_landmark->isFirstDefined() && !polyline_feature->isLastDefined())
- continue;
-
- from_lmk = std::max(0, -offset);
- from_ftr = std::max(0, offset);
- N_overlapped = std::min(max_ftr - from_ftr, max_lmk - from_lmk)+1;
- to_lmk = from_lmk+N_overlapped-1;
- to_ftr = from_ftr+N_overlapped-1;
-
- //std::cout << "\t\toffset " << offset << std::endl;
- //std::cout << "\t\t\tfrom_lmk " << from_lmk << std::endl;
- //std::cout << "\t\t\tfrom_ftr " << from_ftr << std::endl;
- //std::cout << "\t\t\tN_overlapped " << N_overlapped << std::endl;
-
- // Compute the squared distance for all overlapped points
- Eigen::ArrayXXd d = (polyline_feature->getPoints().block(0,from_ftr, 2,N_overlapped) -
- expected_features[*landmark_it].block(0,from_lmk, 2, N_overlapped)).array();
-
- Eigen::ArrayXd dist2 = d.row(0).pow(2) + d.row(1).pow(2);
- //std::cout << "\t\t\tsquared distances = " << dist2.transpose() << std::endl;
-
- if (offset != min_offset && offset != max_offset)
- {
- // Point-to-line first distance
- bool from_ftr_not_defined = (from_ftr == 0 && !polyline_feature->isFirstDefined());
- bool from_lmk_not_defined = (from_lmk == 0 && !polyline_landmark->isFirstDefined());
- //std::cout << "\t\tfrom_ftr_not_defined " << from_ftr_not_defined << (from_ftr == 0) << !polyline_feature->isFirstDefined() << std::endl;
- //std::cout << "\t\tfrom_lmk_not_defined " << from_lmk_not_defined << (from_lmk == 0) << !polyline_landmark->isFirstDefined() << std::endl;
- if (from_ftr_not_defined || from_lmk_not_defined)
- {
- //std::cout << "\t\t\tFirst feature not defined distance to line" << std::endl;
- //std::cout << "\t\t\tA" << expected_features[*landmark_it].col(from_lmk).transpose() << std::endl;
- //std::cout << "\t\t\tAaux" << expected_features[*landmark_it].col(from_lmk+1).transpose() << std::endl;
- //std::cout << "\t\t\tB" << polyline_feature->getPoints().col(from_ftr).transpose() << std::endl;
- dist2(0) = sqDistPointToLine(expected_features[*landmark_it].col(from_lmk),
- expected_features[*landmark_it].col(from_lmk+1),
- polyline_feature->getPoints().col(from_ftr),
- !from_lmk_not_defined,
- !from_ftr_not_defined);
- }
-
- // Point-to-line last distance
- bool last_ftr_not_defined = !polyline_feature->isLastDefined() && to_ftr == max_ftr;
- bool last_lmk_not_defined = !polyline_landmark->isLastDefined() && to_lmk == max_lmk;
- //std::cout << "\t\tlast_ftr_not_defined " << last_ftr_not_defined << (to_ftr == max_ftr) << !polyline_feature->isLastDefined() << std::endl;
- //std::cout << "\t\tlast_lmk_not_defined " << last_lmk_not_defined << (to_lmk == max_lmk) << !polyline_landmark->isLastDefined() << std::endl;
- if (last_ftr_not_defined || last_lmk_not_defined)
- {
- //std::cout << "\t\t\tLast feature not defined distance to line" << std::endl;
- //std::cout << "\t\t\tA" << expected_features[*landmark_it].col(to_lmk).transpose() << std::endl;
- //std::cout << "\t\t\tAaux" << expected_features[*landmark_it].col(to_lmk-1).transpose() << std::endl;
- //std::cout << "\t\t\tB" << polyline_feature->getPoints().col(to_ftr).transpose() << std::endl;
- dist2(N_overlapped-1) = sqDistPointToLine(expected_features[*landmark_it].col(to_lmk),
- expected_features[*landmark_it].col(to_lmk-1),
- polyline_feature->getPoints().col(to_ftr),
- !last_lmk_not_defined,
- !last_ftr_not_defined);
- }
- }
- //std::cout << "\t\t\tsquared distances = " << dist2.transpose() << std::endl;
-
- // All squared distances should be witin a threshold
- // Choose the most overlapped one
- if ((dist2 < params_->position_error_th*params_->position_error_th).all() && (best_match == nullptr ||
- (N_overlapped >= best_match->feature_match_to_id_-best_match->feature_match_from_id_+1 &&
- dist2.mean() < best_match->normalized_score_ )))
- {
- //std::cout << "BEST MATCH" << std::endl;
- best_match = std::make_shared<LandmarkPolylineMatch>();
- best_match->feature_match_from_id_= from_ftr;
- best_match->landmark_match_from_id_= from_lmk+polyline_landmark->getFirstId();
- best_match->feature_match_to_id_= from_ftr+N_overlapped-1;
- best_match->landmark_match_to_id_= from_lmk+N_overlapped-1+polyline_landmark->getFirstId();
- best_match->landmark_ptr_=polyline_landmark;
- best_match->normalized_score_ = dist2.mean();
- }
- }
- }
- // Closed landmark polyline
- else
- {
- if (polyline_feature->getNPoints() > polyline_landmark->getNPoints())
- continue;
-
- //std::cout << "MATCHING WITH CLOSED LANDMARK" << std::endl;
- //std::cout << "\tfeature " << polyline_feature->id() << ": 0-" << max_ftr << std::endl;
- //std::cout << "\tlandmark " << polyline_landmark->id() << ": 0-" << polyline_landmark->getNPoints() - 1 << std::endl;
-
- max_offset = 0;
- min_offset = -polyline_landmark->getNPoints() + 1;
-
- // Check all overlapping positions between each feature-landmark pair
- for (offset = min_offset; offset <= max_offset; offset++)
- {
- from_lmk = -offset;
- to_lmk = from_lmk+polyline_feature->getNPoints()-1;
- if (to_lmk >= polyline_landmark->getNPoints())
- to_lmk -= polyline_landmark->getNPoints();
-
- //std::cout << "\t\toffset " << offset << std::endl;
- //std::cout << "\t\t\tfrom_lmk " << from_lmk << std::endl;
- //std::cout << "\t\t\tto_lmk " << to_lmk << std::endl;
-
- // Compute the squared distance for all overlapped points
- Eigen::ArrayXXd d = polyline_feature->getPoints().topRows(2).array();
- if (to_lmk > from_lmk)
- d -= expected_features[*landmark_it].block(0,from_lmk, 2, polyline_feature->getNPoints()).array();
- else
- {
- d.leftCols(polyline_landmark->getNPoints()-from_lmk) -= expected_features[*landmark_it].block(0,from_lmk, 2, polyline_landmark->getNPoints()-from_lmk).array();
- d.rightCols(to_lmk+1) -= expected_features[*landmark_it].block(0, 0, 2, to_lmk+1).array();
- }
- Eigen::ArrayXd dist2 = d.row(0).pow(2) + d.row(1).pow(2);
- //std::cout << "\t\t\tsquared distances = " << dist2.transpose() << std::endl;
-
- // Point-to-line first distance
- if (!polyline_feature->isFirstDefined())
- {
- int next_from_lmk = (from_lmk+1 == polyline_landmark->getNPoints() ? 0 : from_lmk+1);
- dist2(0) = sqDistPointToLine(expected_features[*landmark_it].col(from_lmk),
- expected_features[*landmark_it].col(next_from_lmk),
- polyline_feature->getPoints().col(0),
- true,
- false);
- }
-
- // Point-to-line last distance
- if (!polyline_feature->isLastDefined())
- {
- int prev_to_lmk = (to_lmk == 0 ? polyline_landmark->getNPoints()-1 : to_lmk-1);
- dist2(polyline_feature->getNPoints()-1) = sqDistPointToLine(expected_features[*landmark_it].col(to_lmk),
- expected_features[*landmark_it].col(prev_to_lmk),
- polyline_feature->getPoints().col(polyline_feature->getNPoints()-1),
- true,
- false);
- }
- //std::cout << "\t\t\tsquared distances = " << dist2.transpose() << std::endl;
-
- // All squared distances should be witin a threshold
- // Choose the most overlapped one
- if ((dist2 < params_->position_error_th*params_->position_error_th).all() && (best_match == nullptr || dist2.mean() < best_match->normalized_score_ ))
- {
- //std::cout << "BEST MATCH" << std::endl;
- best_match = std::make_shared<LandmarkPolylineMatch>();
- best_match->feature_match_from_id_= 0;
- best_match->landmark_match_from_id_= from_lmk+polyline_landmark->getFirstId();
- best_match->feature_match_to_id_= polyline_feature->getNPoints()-1;
- best_match->landmark_match_to_id_= to_lmk+polyline_landmark->getFirstId();
- best_match->landmark_ptr_=polyline_landmark;
- best_match->normalized_score_ = dist2.mean();
- }
- }
- }
-
- //std::cout << "landmark " << (*landmark_it)->id() << ": 0-" << max_lmk << " - defined " << polyline_landmark->isFirstDefined() << polyline_landmark->isLastDefined() << std::endl;
- //std::cout << "feature " << (*feature_it)->id() << ": 0-" << max_ftr << " - defined " << polyline_feature->isFirstDefined() << polyline_feature->isLastDefined() << std::endl;
- //std::cout << expected_features[*landmark_it] << std::endl;
- //std::cout << "\tmax_offset " << max_offset << std::endl;
- //std::cout << "\tmin_offset " << min_offset << std::endl;
- //if (!polyline_landmark->isFirstDefined() && !polyline_feature->isLastDefined())
- // std::cout << "\tLIMITED max offset " << max_offset << std::endl;
- //if (!polyline_feature->isFirstDefined() && !polyline_landmark->isLastDefined())
- // std::cout << "\tLIMITED min offset " << min_offset << std::endl;
-
- }
- // Match found for this feature
- if (best_match != nullptr)
- {
- //std::cout << "\tclosest landmark: " << best_match->landmark_ptr_->id() << std::endl;
- // match
- matches_landmark_from_incoming_[*feature_it] = best_match;
- // move corner feature to output list
- _features_found.splice(_features_found.end(), polylines_incoming_, feature_it);
- // reset match
- best_match = nullptr;
- }
- //else
- //{
- // std::cout << "\t-------------------------->NO LANDMARK CLOSE ENOUGH!!!!" << std::endl;
- // std::getchar();
- //}
- feature_it = next_feature_it++;
- }
- return matches_landmark_from_incoming_.size();
-}
-
-bool ProcessorTrackerLandmarkPolyline::voteForKeyFrame()
-{
- //std::cout << "------------- ProcessorTrackerLandmarkPolyline::voteForKeyFrame:" << std::endl;
- //std::cout << "polylines_last_.size():" << polylines_last_.size()<< std::endl;
- // option 1: more than TH new features in last
- if (polylines_last_.size() >= params_->new_features_th)
- {
- std::cout << "------------- NEW KEY FRAME: Option 1 - Enough new features" << std::endl;
- //std::cout << "\tnew features in last = " << corners_last_.size() << std::endl;
- return true;
- }
- // option 2: loop closure (if the newest frame from which a matched landmark was observed is old enough)
- for (auto new_ftr : new_features_last_)
- {
- if (last_ptr_->getFrame()->id() - matches_landmark_from_last_[new_ftr]->landmark_ptr_->getConstrainedByList().back()->getCapture()->getFrame()->id() > params_->loop_frames_th)
- {
- std::cout << "------------- NEW KEY FRAME: Option 2 - Loop closure" << std::endl;
- return true;
- }
- }
- return false;
-}
-
-void ProcessorTrackerLandmarkPolyline::extractPolylines(CaptureLaser2DPtr _capture_laser_ptr,
- FeatureBasePtrList& _polyline_list)
-{
- //std::cout << "ProcessorTrackerLandmarkPolyline::extractPolylines: " << std::endl;
- // TODO: sort corners by bearing
- std::list<laserscanutils::Polyline> polylines;
-
- line_finder_.findPolylines(_capture_laser_ptr->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(), polylines);
-
- for (auto&& pl : polylines)
- {
- //std::cout << "new polyline detected: Defined" << pl.first_defined_ << pl.last_defined_ << std::endl;
- //std::cout << "covs: " << std::endl << pl.covs_ << std::endl;
- _polyline_list.push_back(std::make_shared<FeaturePolyline2D>(pl.points_, pl.covs_, pl.first_defined_, pl.last_defined_));
- //std::cout << "new polyline detected: " << std::endl;
- }
- //std::cout << _polyline_list.size() << " polylines extracted" << std::endl;
-}
-
-void ProcessorTrackerLandmarkPolyline::expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_,
- Eigen::MatrixXs& expected_feature_cov_)
-{
- assert(_landmark_ptr->getType() == "POLYLINE 2D" && "ProcessorTrackerLandmarkPolyline::expectedFeature: Bad landmark type");
- LandmarkPolyline2DPtr polyline_landmark = std::static_pointer_cast<LandmarkPolyline2D>(_landmark_ptr);
- assert(expected_feature_.cols() == polyline_landmark->getNPoints() && expected_feature_.rows() == 3 && "ProcessorTrackerLandmarkPolyline::expectedFeature: bad expected_feature_ sizes");
-
- //std::cout << "ProcessorTrackerLandmarkPolyline::expectedFeature" << std::endl;
- //std::cout << "t_world_sensor_: " << t_world_sensor_.transpose() << std::endl;
- //std::cout << "R_sensor_world_: " << std::endl << R_sensor_world_ << std::endl;
-
- expected_feature_ = Eigen::MatrixXs::Zero(3,polyline_landmark->getNPoints());
- expected_feature_cov_ = Eigen::MatrixXs::Zero(2,2*polyline_landmark->getNPoints());
- Eigen::Vector3s col = Eigen::Vector3s::Ones();
-
- ////////// global coordinates points
- if (polyline_landmark->getClassification() == UNCLASSIFIED)
- for (auto i = 0; i < polyline_landmark->getNPoints(); i++)
- {
- //std::cout << "Point " << i+polyline_landmark->getFirstId() << std::endl;
- //std::cout << "First Point " << polyline_landmark->getFirstId() << std::endl;
- //std::cout << "Landmark global position: " << polyline_landmark->getPointVector(i+polyline_landmark->getFirstId()).transpose() << std::endl;
-
- // ------------ expected feature point
- col.head<2>() = R_sensor_world_ * (polyline_landmark->getPointVector(i+polyline_landmark->getFirstId()) - t_world_sensor_);
- expected_feature_.col(i) = col;
-
- //std::cout << "Expected point " << i << ": " << expected_feature_.col(i).transpose() << std::endl;
- // ------------ expected feature point covariance
- // TODO
- expected_feature_cov_.middleCols(i*2, 2) = Eigen::MatrixXs::Identity(2,2);
- }
-
- ////////// landmark with origin
- else
- {
- Eigen::Matrix2s R_world_points = Eigen::Rotation2Ds(polyline_landmark->getO()->getState()(0)).matrix();
- const Eigen::VectorXs& t_world_points = polyline_landmark->getP()->getState();
-
- for (auto i = 0; i < polyline_landmark->getNPoints(); i++)
- {
- //std::cout << "Point " << i+polyline_landmark->getFirstId() << std::endl;
- //std::cout << "First Point " << polyline_landmark->getFirstId() << std::endl;
- //std::cout << "Landmark global position: " << polyline_landmark->getPointVector(i+polyline_landmark->getFirstId()).transpose() << std::endl;
-
- // ------------ expected feature point
- col.head<2>() = R_sensor_world_ * (R_world_points * polyline_landmark->getPointVector(i+polyline_landmark->getFirstId()) + t_world_points - t_world_sensor_);
- expected_feature_.col(i) = col;
-
- //std::cout << "Expected point " << i << ": " << expected_feature_.col(i).transpose() << std::endl;
- // ------------ expected feature point covariance
- // TODO
- expected_feature_cov_.middleCols(i*2, 2) = Eigen::MatrixXs::Identity(2,2);
- }
- }
-}
-
-Eigen::VectorXs ProcessorTrackerLandmarkPolyline::computeSquaredMahalanobisDistances(const Eigen::Vector2s& _feature,
- const Eigen::Matrix2s& _feature_cov,
- const Eigen::Vector2s& _expected_feature,
- const Eigen::Matrix2s& _expected_feature_cov,
- const Eigen::MatrixXs& _mu)
-{
- // ------------------------ d
- Eigen::Vector2s d = _feature - _expected_feature;
-
- // ------------------------ Sigma_d
- Eigen::Matrix2s iSigma_d = (_feature_cov + _expected_feature_cov).inverse();
- Eigen::VectorXs squared_mahalanobis_distances(_mu.cols());
- for (unsigned int i = 0; i < _mu.cols(); i++)
- {
- squared_mahalanobis_distances(i) = (d - _mu.col(i)).transpose() * iSigma_d * (d - _mu.col(i));
- //if ((d - _mu.col(i)).norm() < 1)
- //{
- // std::cout << "distance: " << (d - _mu.col(i)).norm() << std::endl;
- // std::cout << "iSigma_d: " << std::endl << iSigma_d << std::endl;
- // std::cout << "mahalanobis: " << squared_mahalanobis_distances(i) << std::endl;
- //}
- }
-
- return squared_mahalanobis_distances;
-}
-
-Scalar ProcessorTrackerLandmarkPolyline::sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux,
- const Eigen::Vector3s& _B, bool _A_defined, bool _B_defined)
-{
- /* Squared distance from B to the line A_aux-A (match evaluated is A-B)
- *
- * No matter if A_aux is defined or not.
- *
- * Case 1: B not defined
- *
- * The projection of B over the line AAaux must be in [A_aux, inf(in A direction)). Otherwise, return squared distance Aaux-B.
- * Check: the angle BAauxA is <= 90º:
- * (BA)² <= (BAaux)² + (AAaux)²
- *
- * Case 1.1: A not defined
- *
- * No more restrictions: return distance line to point.
- *
- * Case 1.2: A defined
- *
- * The projection of B over the line AAaux must be in (inf(in Aaux direction), A]. Otherwise, return squared distance A-B.
- * Additional check: the angle BAAaux is <= 90º:
- * (BAaux)² <= (BA)² + (AAaux)²
- *
- * Case 2: B is defined and A is not
- *
- * Returns the distance B-Line if the projection of B to the line is in [A, inf). Otherwise, return squared distance A-B.
- * Checks if the angle BAAaux is >= 90º: (BAaux)² >= (BA)² + (AAaux)²
- *
- * ( Case B and A are defined is not point-to-line, is point to point -> assertion )
- *
- */
-
- assert((!_A_defined || !_B_defined) && "ProcessorTrackerLandmarkPolyline::sqDistPointToLine: at least one point must not be defined.");
-
- Scalar AB_sq = (_B-_A).head<2>().squaredNorm();
- Scalar AauxB_sq = (_B-_A_aux).head<2>().squaredNorm();
- Scalar AAaux_sq = (_A_aux-_A).head<2>().squaredNorm();
-
- // Case 1
- if (!_B_defined)
- {
- if (AB_sq <= AauxB_sq + AAaux_sq)
- {
- // Case 1.1
- if (!_A_defined)
- return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq; //squared distance to line
- // Case 1.2
- else if (AauxB_sq <= AB_sq + AAaux_sq)
- return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq; //squared distance to line
- }
- }
- // Case 2
- else if (!_A_defined && _B_defined)
- if (AauxB_sq >= AB_sq + AAaux_sq)
- return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq; //squared distance to line
-
- // Default return A-B squared distance
- return (_A.head<2>() - _B.head<2>()).squaredNorm();
-}
-
-void ProcessorTrackerLandmarkPolyline::createNewLandmarks()
-{
- //std::cout << "ProcessorTrackerLandmarkPolyline::createNewLandmarks" << std::endl;
- FeaturePolyline2DPtr polyline_ft_ptr;
- LandmarkBasePtr new_lmk_ptr;
- for (auto new_feature_ptr : new_features_last_)
- {
- // create new landmark
- new_lmk_ptr = createLandmark(new_feature_ptr);
- //std::cout << "\tfeature: " << new_feature_ptr->id() << std::endl;
- //std::cout << "\tnew_landmark: " << new_lmk_ptr->id() << ": "<< ((LandmarkPolyline2D*)new_lmk_ptr)->getNPoints() << " points" << std::endl;
- new_landmarks_.push_back(new_lmk_ptr);
- // cast
- polyline_ft_ptr = std::static_pointer_cast<FeaturePolyline2D>(new_feature_ptr);
- // create new correspondence
- LandmarkPolylineMatchPtr match = std::make_shared<LandmarkPolylineMatch>();
- match->feature_match_from_id_= 0; // all points match
- match->landmark_match_from_id_ = 0;
- match->feature_match_to_id_= polyline_ft_ptr->getNPoints()-1; // all points match
- match->landmark_match_to_id_ = polyline_ft_ptr->getNPoints()-1;
- match->normalized_score_ = 1.0; // max score
- match->landmark_ptr_ = new_lmk_ptr;
- matches_landmark_from_last_[new_feature_ptr] = match;
- }
-}
-
-LandmarkBasePtr ProcessorTrackerLandmarkPolyline::createLandmark(FeatureBasePtr _feature_ptr)
-{
- assert(_feature_ptr->getType() == "POLYLINE 2D");
- //std::cout << "ProcessorTrackerLandmarkPolyline::createLandmark" << std::endl;
- //std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl;
- //std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl;
-
- FeaturePolyline2DPtr polyline_ptr = std::static_pointer_cast<FeaturePolyline2D>(_feature_ptr);
- // compute feature global pose
- Eigen::MatrixXs points_global = R_world_sensor_ * polyline_ptr->getPoints().topRows<2>() +
- t_world_sensor_ * Eigen::VectorXs::Ones(polyline_ptr->getNPoints()).transpose();
-
- //std::cout << "Feature local points: " << std::endl << polyline_ptr->getPoints().topRows<2>() << std::endl;
- //std::cout << "Landmark global points: " << std::endl << points_global << std::endl;
- //std::cout << "New landmark: extremes defined " << polyline_ptr->isFirstDefined() << polyline_ptr->isLastDefined() << std::endl;
-
- // Create new landmark
- return std::make_shared<LandmarkPolyline2D>(std::make_shared<StateBlock>(Eigen::Vector2s::Zero(), true), std::make_shared<StateBlock>(Eigen::Vector1s::Zero(), true), points_global, polyline_ptr->isFirstDefined(), polyline_ptr->isLastDefined());
-}
-
-ProcessorTrackerLandmarkPolyline::~ProcessorTrackerLandmarkPolyline()
-{
- while (!polylines_last_.empty())
- {
- polylines_last_.front()->remove();
- polylines_last_.pop_front(); // TODO see if this is needed
- }
- while (!polylines_incoming_.empty())
- {
- polylines_incoming_.front()->remove();
- polylines_incoming_.pop_front(); // TODO see if this is needed
- }
-}
-
-void ProcessorTrackerLandmarkPolyline::establishFactors()
-{
- //TODO: update with new index in landmarks
-
- //std::cout << "ProcessorTrackerLandmarkPolyline::establishFactors" << std::endl;
- LandmarkPolylineMatchPtr polyline_match;
- FeaturePolyline2DPtr polyline_feature;
- LandmarkPolyline2DPtr polyline_landmark;
-
- for (auto last_feature : last_ptr_->getFeatureList())
- {
- polyline_feature = std::static_pointer_cast<FeaturePolyline2D>(last_feature);
- polyline_match = std::static_pointer_cast<LandmarkPolylineMatch>(matches_landmark_from_last_[last_feature]);
- polyline_landmark = std::static_pointer_cast<LandmarkPolyline2D>(polyline_match->landmark_ptr_);
-
- assert(polyline_landmark != nullptr && polyline_match != nullptr);
-
- // Modify landmark (only for not closed)
- if (!polyline_landmark->isClosed())
- {
- //std::cout << std::endl << "MODIFY LANDMARK" << std::endl;
- //std::cout << "feature " << polyline_feature->id() << ": " << std::endl;
- //std::cout << "\tpoints " << polyline_feature->getNPoints() << std::endl;
- //std::cout << "\tfirst defined " << polyline_feature->isFirstDefined() << std::endl;
- //std::cout << "\tlast defined " << polyline_feature->isLastDefined() << std::endl;
- //std::cout << "landmark " << polyline_landmark->id() << ": " << std::endl;
- //std::cout << "\tpoints " << polyline_landmark->getNPoints() << std::endl;
- //std::cout << "\tfirst defined " << polyline_landmark->isFirstDefined() << std::endl;
- //std::cout << "\tlast defined " << polyline_landmark->isLastDefined() << std::endl << std::endl;
- //std::cout << "\tmatch from feature point " << polyline_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tmatch to feature point " << polyline_match->feature_match_to_id_ << std::endl;
- //std::cout << "\tmatch from landmark point " << polyline_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tmatch to landmark point " << polyline_match->landmark_match_to_id_ << std::endl;
-
- Eigen::MatrixXs points_global = R_world_sensor_ * polyline_feature->getPoints().topRows<2>() +
- t_world_sensor_ * Eigen::VectorXs::Ones(polyline_feature->getNPoints()).transpose();
- // GROW/CLOSE LANDMARK
- // -----------------Front-----------------
- bool check_front_closing = // Sufficient conditions
- // condition 1: feature first defined point not matched
- (polyline_feature->isFirstDefined() && polyline_match->feature_match_from_id_ > 0) ||
- // condition 2: feature second point not matched
- (polyline_match->feature_match_from_id_ > 1) ||
- // condition 3: matched front points but feature front point defined and landmark front point not defined
- (polyline_match->landmark_match_from_id_ == polyline_landmark->getFirstId() && polyline_feature->isFirstDefined() && !polyline_landmark->isFirstDefined());
-
- // Check closing with landmark's last points
- if (check_front_closing)
- {
- //std::cout << "---------------- Trying to close polyline..." << std::endl;
- //std::cout << "feature " << polyline_feature->id() << ": " << std::endl;
- //std::cout << "\tpoints " << polyline_feature->getNPoints() << std::endl;
- //std::cout << "\tfirst defined " << polyline_feature->isFirstDefined() << std::endl;
- //std::cout << "\tlast defined " << polyline_feature->isLastDefined() << std::endl;
- //std::cout << "\tmatch from feature point " << polyline_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tmatch to feature point " << polyline_match->feature_match_to_id_ << std::endl;
- //std::cout << "landmark " << polyline_landmark->id() << ": " << std::endl;
- //std::cout << "\tpoints " << polyline_landmark->getNPoints() << std::endl;
- //std::cout << "\tfirst defined " << polyline_landmark->isFirstDefined() << std::endl;
- //std::cout << "\tlast defined " << polyline_landmark->isLastDefined() << std::endl << std::endl;
- //std::cout << "\tmatch from landmark point " << polyline_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tmatch to landmark point " << polyline_match->landmark_match_to_id_ << std::endl;
-
- int feat_point_id_matching = polyline_feature->isFirstDefined() ? 0 : 1;
- int lmk_last_defined_point = polyline_landmark->getLastId() - (polyline_landmark->isLastDefined() ? 0 : 1);
- //std::cout << std::endl << "\tfeat point matching " << feat_point_id_matching << std::endl;
- //std::cout << std::endl << "\tlmk last defined point " << lmk_last_defined_point << std::endl;
-
- for (int id_lmk = lmk_last_defined_point; id_lmk > polyline_match->landmark_match_to_id_; id_lmk--)
- {
- //std::cout << "\t\tid_lmk " << id_lmk << std::endl;
- //std::cout << "\t\td2 = " << (points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() << " (th = " << params_->position_error_th*params_->position_error_th << std::endl;
-
- if ((points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() < params_->position_error_th*params_->position_error_th)
- {
- std::cout << "CLOSING POLYLINE" << std::endl;
-
- unsigned int N_back_overlapped = polyline_landmark->getLastId() - id_lmk + 1;
- int N_feature_new_points = polyline_match->feature_match_from_id_ - feat_point_id_matching - N_back_overlapped;
-
- // define first point (if not defined and no points have to be merged)
- if (!polyline_landmark->isFirstDefined() && N_feature_new_points >= 0)
- polyline_landmark->setFirst(points_global.col(polyline_match->feature_match_from_id_), true);
-
- // add other points (if there are)
- if (N_feature_new_points > 0)
- polyline_landmark->addPoints(points_global.middleCols(feat_point_id_matching + N_back_overlapped, N_feature_new_points), // points matrix in global coordinates
- N_feature_new_points-1, // last index to be added
- true, // defined
- false); // front (!back)
-
- // define last point (if not defined and no points have to be merged)
- if (!polyline_landmark->isLastDefined() && N_feature_new_points >= 0)
- polyline_landmark->setLast(points_global.col(feat_point_id_matching + N_back_overlapped - 1), true);
-
- // close landmark
- polyline_landmark->setClosed();
-
- polyline_match->landmark_match_from_id_ = id_lmk - (polyline_feature->isFirstDefined() ? 0 : 1);
- polyline_match->feature_match_from_id_ = 0;
- break;
- }
- }
- }
- // Add new front points (if not matched feature points)
- if (polyline_match->feature_match_from_id_ > 0) // && !polyline_landmark->isClosed()
- {
- assert(!polyline_landmark->isClosed() && "feature not matched points in a closed landmark");
- //std::cout << "Add new front points. Defined: " << polyline_feature->isFirstDefined() << std::endl;
- //std::cout << "\tfeat from " << polyline_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tfeat to " << polyline_match->feature_match_to_id_ << std::endl;
- //std::cout << "\tland from " << polyline_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tland to " << polyline_match->landmark_match_to_id_ << std::endl;
-
- polyline_landmark->addPoints(points_global, // points matrix in global coordinates
- polyline_match->feature_match_from_id_-1, // last feature point index to be added
- polyline_feature->isFirstDefined(), // is defined
- false); // front
-
- polyline_match->landmark_match_from_id_ = polyline_landmark->getFirstId();
- polyline_match->feature_match_from_id_ = 0;
- //std::cout << "\tfeat from " << polyline_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tfeat to " << polyline_match->feature_match_to_id_ << std::endl;
- //std::cout << "\tland from " << polyline_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tland to " << polyline_match->landmark_match_to_id_ << std::endl;
- }
- // Change first point
- else if (polyline_match->landmark_match_from_id_ == polyline_landmark->getFirstId() && !polyline_landmark->isFirstDefined())
- {
- //std::cout << "Change first point. Defined: " << polyline_feature->isFirstDefined() << std::endl;
- //std::cout << "\tpoint " << polyline_landmark->getFirstId() << ": " << polyline_landmark->getPointVector(polyline_landmark->getFirstId()).transpose() << std::endl;
- //std::cout << "\tpoint " << polyline_landmark->getFirstId()+1 << ": " << polyline_landmark->getPointVector(polyline_landmark->getFirstId()+1).transpose() << std::endl;
- //std::cout << "\tfeature point: " << points_global.topLeftCorner(2,1).transpose() << std::endl;
-
- if (// new defined first
- polyline_feature->isFirstDefined() ||
- // new not defined first
- (points_global.topLeftCorner(2,1)-polyline_landmark->getPointVector(polyline_landmark->getFirstId()+1)).squaredNorm() >
- (points_global.topLeftCorner(2,1)-polyline_landmark->getPointVector(polyline_landmark->getFirstId())).squaredNorm() +
- (polyline_landmark->getPointVector(polyline_landmark->getFirstId()+1)-polyline_landmark->getPointVector(polyline_landmark->getFirstId())).squaredNorm())
- polyline_landmark->setFirst(points_global.leftCols(1), polyline_feature->isFirstDefined());
-
- }
- // -----------------Back-----------------
- bool check_back_closing = // Sufficient conditions
- // condition 1: feature last defined point not matched
- (polyline_feature->isLastDefined() && polyline_match->feature_match_to_id_ < polyline_feature->getNPoints()-1) ||
- // condition 2: feature second last point not matched
- (polyline_match->feature_match_to_id_ < polyline_feature->getNPoints() - 2) ||
- // condition 3: matched back points but feature last point defined and landmark last point not defined
- (polyline_match->landmark_match_to_id_ == polyline_landmark->getLastId() && polyline_feature->isLastDefined() && !polyline_landmark->isLastDefined());
-
- // Necessary condition: still open landmark
- check_back_closing = check_back_closing && !polyline_landmark->isClosed();
-
- // Check closing with landmark's first points
- if (check_back_closing)
- {
- int feat_point_id_matching = polyline_feature->getNPoints() - (polyline_feature->isLastDefined() ? 1 : 2);
- int lmk_first_defined_point = polyline_landmark->getFirstId() + (polyline_landmark->isFirstDefined() ? 0 : 1);
- //std::cout << std::endl << "\tfeat point matching " << feat_point_id_matching << std::endl;
- //std::cout << std::endl << "\tlmk first defined point " << lmk_first_defined_point << std::endl;
-
- for (int id_lmk = lmk_first_defined_point; id_lmk < polyline_match->landmark_match_from_id_; id_lmk++)
- {
- //std::cout << "\t\tid_lmk " << id_lmk << std::endl;
- //std::cout << "\t\td2 = " << (points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() << " (th = " << params_->position_error_th*params_->position_error_th << std::endl;
-
- if ((points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() < params_->position_error_th*params_->position_error_th)
- {
- std::cout << "CLOSING POLYLINE" << std::endl;
-
- unsigned int N_front_overlapped = id_lmk - polyline_landmark->getFirstId() + 1;
- int N_feature_new_points = feat_point_id_matching - polyline_match->feature_match_to_id_ - N_front_overlapped;
-
- // define first point (if not defined and no points have to be merged)
- if (!polyline_landmark->isFirstDefined() && N_feature_new_points >= 0)
- polyline_landmark->setFirst(points_global.col(feat_point_id_matching - N_front_overlapped + 1), true);
-
- // add other points (if there are)
- if (N_feature_new_points > 0)
- polyline_landmark->addPoints(points_global.middleCols(polyline_match->feature_match_to_id_ + 1, N_feature_new_points), // points matrix in global coordinates
- N_feature_new_points-1, // last index to be added
- true, // defined
- false); // front (!back)
-
- // define last point (if not defined and no points have to be merged)
- if (!polyline_landmark->isLastDefined() && N_feature_new_points >= 0)
- polyline_landmark->setLast(points_global.col(polyline_match->feature_match_to_id_), true);
-
- // close landmark
- polyline_landmark->setClosed();
-
- polyline_match->landmark_match_to_id_ = id_lmk + (polyline_feature->isLastDefined() ? 0 : 1);
- polyline_match->feature_match_to_id_ = polyline_feature->getNPoints() - 1;
- break;
- }
- }
- }
- // Add new back points (if it wasn't closed)
- if (polyline_match->feature_match_to_id_ < polyline_feature->getNPoints()-1)
- {
- assert(!polyline_landmark->isClosed() && "feature not matched points in a closed landmark");
- //std::cout << "Add back points. Defined: " << polyline_feature->isLastDefined() << std::endl;
- //std::cout << "\tfeat from " << polyline_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tfeat to " << polyline_match->feature_match_to_id_ << std::endl;
- //std::cout << "\tland from " << polyline_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tland to " << polyline_match->landmark_match_to_id_ << std::endl;
-
- polyline_landmark->addPoints(points_global, // points matrix in global coordinates
- polyline_match->feature_match_to_id_+1, // last feature point index to be added
- polyline_feature->isLastDefined(), // is defined
- true); // back
-
- polyline_match->landmark_match_to_id_ = polyline_landmark->getLastId();
- polyline_match->feature_match_to_id_ = polyline_feature->getNPoints()-1;
- //std::cout << "\tfeat from " << polyline_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tfeat to " << polyline_match->feature_match_to_id_ << std::endl;
- //std::cout << "\tland from " << polyline_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tland to " << polyline_match->landmark_match_to_id_ << std::endl;
- }
- // Change last point
- else if (polyline_match->landmark_match_to_id_ == polyline_landmark->getLastId() && !polyline_landmark->isLastDefined()) //&& polyline_match->feature_match_to_id_ == polyline_feature->getNPoints()-1
- {
- //std::cout << "Change last point. Defined: " << (polyline_feature->isLastDefined() ? 1 : 0) << std::endl;
- //std::cout << "\tpoint " << polyline_landmark->getLastId() << ": " << polyline_landmark->getPointVector(polyline_landmark->getLastId()).transpose() << std::endl;
- //std::cout << "\tpoint " << polyline_landmark->getLastId()-1 << ": " << polyline_landmark->getPointVector(polyline_landmark->getLastId()-1).transpose() << std::endl;
- //std::cout << "\tfeature point: " << points_global.topRightCorner(2,1).transpose() << std::endl;
- if (// new defined last
- polyline_feature->isLastDefined() ||
- // new not defined last
- (points_global.topRightCorner(2,1)-polyline_landmark->getPointVector(polyline_landmark->getLastId()-1)).squaredNorm() >
- (points_global.topRightCorner(2,1)-polyline_landmark->getPointVector(polyline_landmark->getLastId())).squaredNorm() +
- (polyline_landmark->getPointVector(polyline_landmark->getLastId()-1)-polyline_landmark->getPointVector(polyline_landmark->getLastId())).squaredNorm())
- polyline_landmark->setLast(points_global.rightCols(1), polyline_feature->isLastDefined());
- }
-
- //std::cout << "MODIFIED LANDMARK" << std::endl;
- //std::cout << "feature " << polyline_feature->id() << ": " << std::endl;
- //std::cout << "\tpoints " << polyline_feature->getNPoints() << std::endl;
- //std::cout << "\tfirst defined " << polyline_feature->isFirstDefined() << std::endl;
- //std::cout << "\tlast defined " << polyline_feature->isLastDefined() << std::endl;
- //std::cout << "landmark " << polyline_landmark->id() << ": " << std::endl;
- //std::cout << "\tpoints " << polyline_landmark->getNPoints() << std::endl;
- //std::cout << "\tfirst defined " << polyline_landmark->isFirstDefined() << std::endl;
- //std::cout << "\tlast defined " << polyline_landmark->isLastDefined() << std::endl << std::endl;
- }
-
- // ESTABLISH CONSTRAINTS
- //std::cout << "ESTABLISH CONSTRAINTS" << std::endl;
- //std::cout << "\tfeature " << polyline_feature->id() << std::endl;
- //std::cout << "\tlandmark " << polyline_landmark->id() << std::endl;
- //std::cout << "\tmatch from feature point " << polyline_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tmatch to feature point " << polyline_match->feature_match_to_id_ << std::endl;
- //std::cout << "\tmatch from landmark point " << polyline_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tmatch to landmark point " << polyline_match->landmark_match_to_id_ << std::endl;
-
- // Factors for all inner and defined feature points
- int lmk_point_id = polyline_match->landmark_match_from_id_;
-
- for (int ftr_point_id = 0; ftr_point_id < polyline_feature->getNPoints(); ftr_point_id++, lmk_point_id++)
- {
- if (lmk_point_id > polyline_landmark->getLastId())
- lmk_point_id -= polyline_landmark->getNPoints();
- if (lmk_point_id < polyline_landmark->getFirstId())
- lmk_point_id += polyline_landmark->getNPoints();
-
- //std::cout << "feature point " << ftr_point_id << std::endl;
- //std::cout << "landmark point " << lmk_point_id << std::endl;
-
- // First not defined point
- if (ftr_point_id == 0 && !polyline_feature->isFirstDefined())
- // first point to line factor
- {
- int lmk_next_point_id = lmk_point_id+1;
- if (lmk_next_point_id > polyline_landmark->getLastId())
- lmk_next_point_id -= polyline_landmark->getNPoints();
- //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_next_point_id << std::endl;
- // last_feature->addFactor(std::make_shared<FactorPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(),
- // ftr_point_id, lmk_point_id, lmk_next_point_id));
- FactorBase::emplace<FactorPointToLine2D>(last_feature, polyline_feature, polyline_landmark, shared_from_this(),
- ftr_point_id, lmk_point_id, lmk_next_point_id);
- //std::cout << "factor added" << std::endl;
- }
-
- // Last not defined point
- else if (ftr_point_id == polyline_feature->getNPoints()-1 && !polyline_feature->isLastDefined())
- // last point to line factor
- {
- int lmk_prev_point_id = lmk_point_id-1;
- if (lmk_prev_point_id < polyline_landmark->getFirstId())
- lmk_prev_point_id += polyline_landmark->getNPoints();
- //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_prev_point_id << std::endl;
- // last_feature->addFactor(std::make_shared<FactorPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(),
- // ftr_point_id, lmk_point_id, lmk_prev_point_id));
- FactorBase::emplace<FactorPointToLine2D>(last_feature, polyline_feature, polyline_landmark, shared_from_this(),
- ftr_point_id, lmk_point_id, lmk_prev_point_id);
- //std::cout << "factor added" << std::endl;
- }
-
- // Defined point
- else
- // point to point factor
- {
- //std::cout << "point-point: landmark point " << lmk_point_id << std::endl;
- //std::cout << "landmark first id:" << polyline_landmark->getFirstId() << std::endl;
- //std::cout << "landmark last id:" << polyline_landmark->getLastId() << std::endl;
- //std::cout << "landmark n points:" << polyline_landmark->getNPoints() << std::endl;
- // last_feature->addFactor(std::make_shared<FactorPoint2D>(polyline_feature, polyline_landmark, shared_from_this(),
- // ftr_point_id, lmk_point_id));
- FactorBase::emplace<FactorPoint2D>(last_feature, polyline_feature, polyline_landmark, shared_from_this(),
- ftr_point_id, lmk_point_id);
- //std::cout << "factor added" << std::endl;
- }
- }
- //std::cout << "Factors established" << std::endl;
- }
-}
-
-void ProcessorTrackerLandmarkPolyline::classifyPolilines(LandmarkBasePtrList& _lmk_list)
-{
- //std::cout << "ProcessorTrackerLandmarkPolyline::classifyPolilines: " << _lmk_list->size() << std::endl;
- std::vector<Scalar> object_L({12, 5.9, 1.2});
- std::vector<Scalar> object_W({2.345, 2.345, 0.9});
- std::vector<Scalar> object_D({sqrt(12*12+2.345*2.345), sqrt(5.9*5.9+2.345*2.345), sqrt(0.9*0.9+1.2*1.2)});
- std::vector<LandmarkClassification> object_class({CONTAINER, SMALL_CONTAINER, PALLET});
-
- for (auto lmk_ptr : _lmk_list)
- if (lmk_ptr->getType() == "POLYLINE 2D")
- {
- LandmarkPolyline2DPtr polyline_ptr = std::static_pointer_cast<LandmarkPolyline2D>(lmk_ptr);
- auto n_defined_points = polyline_ptr->getNPoints() - (polyline_ptr->isFirstDefined() ? 0 : 1) - (polyline_ptr->isLastDefined() ? 0 : 1);
- auto A_id = polyline_ptr->getFirstId() + (polyline_ptr->isFirstDefined() ? 0 : 1);
- auto B_id = A_id + 1;
- auto C_id = B_id + 1;
- auto D_id = C_id + 1;
-
- // necessary conditions
- if (polyline_ptr->getClassification() != UNCLASSIFIED ||
- n_defined_points < 3 ||
- n_defined_points > 4 )
- continue;
-
- //std::cout << "landmark " << lmk_ptr->id() << std::endl;
-
- // consider 3 first defined points
- Scalar dAB = (polyline_ptr->getPointVector(A_id) - polyline_ptr->getPointVector(B_id)).norm();
- Scalar dBC = (polyline_ptr->getPointVector(B_id) - polyline_ptr->getPointVector(C_id)).norm();
- Scalar dAC = (polyline_ptr->getPointVector(A_id) - polyline_ptr->getPointVector(C_id)).norm();
-
- //std::cout << "dAB = " << dAB << " error 1: " << fabs(dAB-CONT_L) << " error 2: " << fabs(dAB-CONT_W) << std::endl;
- //std::cout << "dBC = " << dBC << " error 1: " << fabs(dBC-CONT_W) << " error 2: " << fabs(dBC-CONT_L) << std::endl;
- //std::cout << "dAC = " << dAC << " error 1&2: " << fabs(dAC-CONT_D) << std::endl;
-
- auto classification = -1;
- bool configuration;
-
- for (unsigned int i = 0; i < object_L.size(); i++)
- {
- // check configuration 1
- if(fabs(dAB-object_L[i]) < params_->position_error_th &&
- fabs(dBC-object_W[i]) < params_->position_error_th &&
- fabs(dAC-object_D[i]) < params_->position_error_th)
- {
- configuration = true;
- classification = i;
- break;
- }
-
- // check configuration 2
- if(fabs(dAB-object_W[i]) < params_->position_error_th &&
- fabs(dBC-object_L[i]) < params_->position_error_th &&
- fabs(dAC-object_D[i]) < params_->position_error_th)
- {
- configuration = false;
- classification = i;
- break;
- }
- }
-
- // any container position fitted
- if (classification != -1)
- {
- // if 4 defined checking
- if (n_defined_points == 4)
- {
- //std::cout << "checking with 4th point... " << std::endl;
-
- Scalar dAD = (polyline_ptr->getPointVector(A_id) - polyline_ptr->getPointVector(D_id)).norm();
- Scalar dBD = (polyline_ptr->getPointVector(B_id) - polyline_ptr->getPointVector(D_id)).norm();
- Scalar dCD = (polyline_ptr->getPointVector(C_id) - polyline_ptr->getPointVector(D_id)).norm();
-
- // necessary conditions
- if (fabs(dAD-dBC) > params_->position_error_th ||
- fabs(dBD-dAC) > params_->position_error_th ||
- fabs(dCD-dAB) > params_->position_error_th)
- continue;
- }
-
- // if not 4 defined add/define points
- else
- {
- //std::cout << "adding/defining points... " << std::endl;
- if (!polyline_ptr->isFirstDefined())
- {
- polyline_ptr->defineExtreme(false);
- D_id = polyline_ptr->getFirstId();
- }
- else if (!polyline_ptr->isLastDefined())
- polyline_ptr->defineExtreme(true);
- else
- polyline_ptr->addPoint(Eigen::Vector2s::Zero(), true, true);
- }
- //std::cout << "landmark " << lmk_ptr->id() << " classified as " << object_class[classification] << " in configuration " << configuration << std::endl;
-
- // Close
- polyline_ptr->setClosed();
-
- // Classify
- polyline_ptr->classify(object_class[classification]);
-
- // Unfix origin
- polyline_ptr->getP()->unfix();
- polyline_ptr->getO()->unfix();
-
- // Move origin to B
- polyline_ptr->getP()->setState(polyline_ptr->getPointVector((configuration ? B_id : A_id)));
- Eigen::Vector2s frame_x = (configuration ? polyline_ptr->getPointVector(A_id)-polyline_ptr->getPointVector(B_id) : polyline_ptr->getPointVector(C_id)-polyline_ptr->getPointVector(B_id));
- polyline_ptr->getO()->setState(Eigen::Vector1s::Constant(atan2(frame_x(1),frame_x(0))));
-
- //std::cout << "A: " << polyline_ptr->getPointVector(A_id).transpose() << std::endl;
- //std::cout << "B: " << polyline_ptr->getPointVector(B_id).transpose() << std::endl;
- //std::cout << "C: " << polyline_ptr->getPointVector(C_id).transpose() << std::endl;
- //std::cout << "frame_x: " << frame_x.transpose() << std::endl;
- //std::cout << "frame position: " << polyline_ptr->getP()->getVector().transpose() << std::endl;
- //std::cout << "frame orientation: " << polyline_ptr->getO()->getVector() << std::endl;
-
- // Fix polyline points to its respective relative positions
- if (configuration)
- {
- polyline_ptr->getPointStateBlockPtr(A_id)->setState(Eigen::Vector2s(object_L[classification], 0));
- polyline_ptr->getPointStateBlockPtr(B_id)->setState(Eigen::Vector2s(0, 0));
- polyline_ptr->getPointStateBlockPtr(C_id)->setState(Eigen::Vector2s(0, object_W[classification]));
- polyline_ptr->getPointStateBlockPtr(D_id)->setState(Eigen::Vector2s(object_L[classification], object_W[classification]));
- }
- else
- {
- polyline_ptr->getPointStateBlockPtr(A_id)->setState(Eigen::Vector2s(0, 0));
- polyline_ptr->getPointStateBlockPtr(B_id)->setState(Eigen::Vector2s(0, object_W[classification]));
- polyline_ptr->getPointStateBlockPtr(C_id)->setState(Eigen::Vector2s(object_L[classification], object_W[classification]));
- polyline_ptr->getPointStateBlockPtr(D_id)->setState(Eigen::Vector2s(object_L[classification], 0));
- }
- for (auto id = polyline_ptr->getFirstId(); id <= polyline_ptr->getLastId(); id++)
- {
- polyline_ptr->getPointStateBlockPtr(id)->fix();
- }
- }
- }
-}
-
-void ProcessorTrackerLandmarkPolyline::postProcess()
-{
- //std::cout << "postProcess: " << std::endl;
- //std::cout << "New Last features: " << getNewFeaturesListLast().size() << std::endl;
- //std::cout << "Last features: " << last_ptr_->getFeatureList().size() << std::endl;
- classifyPolilines(getProblem()->getMap()->getLandmarkList());
-}
-
-FactorBasePtr ProcessorTrackerLandmarkPolyline::createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
-{
- // not used
- return nullptr;
-}
-
-ProcessorBasePtr ProcessorTrackerLandmarkPolyline::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr)
-{
- ProcessorParamsPolylinePtr params = std::static_pointer_cast<ProcessorParamsPolyline>(_params);
- ProcessorTrackerLandmarkPolylinePtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkPolyline>(params);
- prc_ptr->setName(_unique_name);
- return prc_ptr;
-}
-
-} //namespace wolf
-
-// Register in the SensorFactory
-#include "base/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("POLYLINE", ProcessorTrackerLandmarkPolyline)
-} // namespace wolf
diff --git a/src/track_matrix.cpp b/src/processor/track_matrix.cpp
similarity index 98%
rename from src/track_matrix.cpp
rename to src/processor/track_matrix.cpp
index 54c2bd50b3525d9a68ed9db58bcf18a1b7310d02..8db44ce4abb4e0d3dc19e5ec3f53403ee642caf3 100644
--- a/src/track_matrix.cpp
+++ b/src/processor/track_matrix.cpp
@@ -5,7 +5,7 @@
* \author: jsola
*/
-#include "base/track_matrix.h"
+#include "base/processor/track_matrix.h"
namespace wolf
{
@@ -50,7 +50,7 @@ void TrackMatrix::add(size_t _track_id, CaptureBasePtr _cap, FeatureBasePtr _ftr
_ftr->setTrackId(_track_id);
if (_cap != _ftr->getCapture())
- _ftr->setCapturePtr(_cap);
+ _ftr->setCapture(_cap);
tracks_[_track_id].emplace(_cap->getTimeStamp(), _ftr);
snapshots_[_cap->id()].emplace(_track_id, _ftr); // will create new snapshot if _cap_id is not present
}
diff --git a/src/sensor/sensor_GPS.cpp b/src/sensor/sensor_GPS.cpp
index 323616be956041905389ad5b083c85c11af3707e..f6c1eace8819cd0cbf16b3fa568eddfec1ddde4b 100644
--- a/src/sensor/sensor_GPS.cpp
+++ b/src/sensor/sensor_GPS.cpp
@@ -1,7 +1,7 @@
#include "base/sensor/sensor_GPS.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
namespace wolf {
diff --git a/src/sensor/sensor_GPS_fix.cpp b/src/sensor/sensor_GPS_fix.cpp
index c69f99b6a2254b277dc250fed85b3f2105159db4..3c781095f36faffe087c4f98426497bbbe37bb69 100644
--- a/src/sensor/sensor_GPS_fix.cpp
+++ b/src/sensor/sensor_GPS_fix.cpp
@@ -1,6 +1,6 @@
#include "base/sensor/sensor_GPS_fix.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
namespace wolf {
diff --git a/src/sensor/sensor_IMU.cpp b/src/sensor/sensor_IMU.cpp
index f97edf645e607b08534ca421db1bd09070bf8d70..d5b6841636a9d9a06d1cebb5a81aecd8cb455647 100644
--- a/src/sensor/sensor_IMU.cpp
+++ b/src/sensor/sensor_IMU.cpp
@@ -1,6 +1,6 @@
#include "base/sensor/sensor_IMU.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
namespace wolf {
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 55a5c8446248e61d2059d4a23600534a3d59fa98..c8cf309c21dc2f78c7498669835c5edff00113c8 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -1,6 +1,6 @@
#include "base/sensor/sensor_base.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
#include "base/factor/factor_block_absolute.h"
#include "base/factor/factor_quaternion_absolute.h"
@@ -262,34 +262,34 @@ CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts)
return capture;
}
-StateBlockPtr SensorBase::getPPtr(const TimeStamp _ts)
+StateBlockPtr SensorBase::getP(const TimeStamp _ts)
{
- return getStateBlockPtr(0, _ts);
+ return getStateBlock(0, _ts);
}
-StateBlockPtr SensorBase::getOPtr(const TimeStamp _ts)
+StateBlockPtr SensorBase::getO(const TimeStamp _ts)
{
- return getStateBlockPtr(1, _ts);
+ return getStateBlock(1, _ts);
}
-StateBlockPtr SensorBase::getIntrinsicPtr(const TimeStamp _ts)
+StateBlockPtr SensorBase::getIntrinsic(const TimeStamp _ts)
{
- return getStateBlockPtr(2, _ts);
+ return getStateBlock(2, _ts);
}
StateBlockPtr SensorBase::getP()
{
- return getStateBlockPtr(0);
+ return getStateBlock(0);
}
StateBlockPtr SensorBase::getO()
{
- return getStateBlockPtr(1);
+ return getStateBlock(1);
}
StateBlockPtr SensorBase::getIntrinsic()
{
- return getStateBlockPtr(2);
+ return getStateBlock(2);
}
SizeEigen SensorBase::computeCalibSize() const
@@ -323,7 +323,7 @@ Eigen::VectorXs SensorBase::getCalibration() const
bool SensorBase::process(const CaptureBasePtr capture_ptr)
{
- capture_ptr->setSensorPtr(shared_from_this());
+ capture_ptr->setSensor(shared_from_this());
for (const auto processor : processor_list_)
{
@@ -339,22 +339,22 @@ ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr)
return _proc_ptr;
}
-StateBlockPtr SensorBase::getStateBlockPtr(unsigned int _i)
+StateBlockPtr SensorBase::getStateBlock(unsigned int _i)
{
CaptureBasePtr cap;
if (isStateBlockDynamic(_i, cap))
- return cap->getStateBlockPtr(_i);
+ return cap->getStateBlock(_i);
return getStateBlockPtrStatic(_i);
}
-StateBlockPtr SensorBase::getStateBlockPtr(unsigned int _i, const TimeStamp& _ts)
+StateBlockPtr SensorBase::getStateBlock(unsigned int _i, const TimeStamp& _ts)
{
CaptureBasePtr cap;
if (isStateBlockDynamic(_i, _ts, cap))
- return cap->getStateBlockPtr(_i);
+ return cap->getStateBlock(_i);
return getStateBlockPtrStatic(_i);
}
@@ -407,7 +407,7 @@ void SensorBase::setProblem(ProblemPtr _problem)
void SensorBase::link(HardwareBasePtr _hw_ptr)
{
std::cout << "Linking SensorBase" << std::endl;
- this->setHardwarePtr(_hw_ptr);
+ this->setHardware(_hw_ptr);
this->getHardware()->addSensor(shared_from_this());
this->setProblem(_hw_ptr->getProblem());
this->registerNewStateBlocks();
diff --git a/src/sensor/sensor_camera.cpp b/src/sensor/sensor_camera.cpp
index 54c08c69f3a263b6d71a1b461baa393e62db0d10..cc9615cf61ebdae48f4dc04015a29f20de7cd2ce 100644
--- a/src/sensor/sensor_camera.cpp
+++ b/src/sensor/sensor_camera.cpp
@@ -1,8 +1,8 @@
#include "base/sensor/sensor_camera.h"
-#include "base/pinhole_tools.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/math/pinhole_tools.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
namespace wolf
{
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index d5fdb9e5ee702a8789307eb875e8d026a9d608b3..43894b7d6aded5d57016666b949bd1538e296355 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -1,7 +1,7 @@
#include "base/sensor/sensor_diff_drive.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
#include "base/capture/capture_motion.h"
-#include "base/eigen_assert.h"
+#include "base/utils/eigen_assert.h"
namespace wolf {
diff --git a/src/sensor/sensor_laser_2D.cpp b/src/sensor/sensor_laser_2D.cpp
index a7a5677af497ccb2f925dce9ae993e160b860fd5..5c8b72ae331b714bbe33ebf64b1de14cb2cf8c0e 100644
--- a/src/sensor/sensor_laser_2D.cpp
+++ b/src/sensor/sensor_laser_2D.cpp
@@ -1,5 +1,5 @@
#include "base/sensor/sensor_laser_2D.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
namespace wolf {
diff --git a/src/sensor/sensor_odom_2D.cpp b/src/sensor/sensor_odom_2D.cpp
index c698db52f69d668ca7bab3c522087cb168047d10..dc3772d7b250183d99a153acf6f22e04a089e1f4 100644
--- a/src/sensor/sensor_odom_2D.cpp
+++ b/src/sensor/sensor_odom_2D.cpp
@@ -1,6 +1,6 @@
#include "base/sensor/sensor_odom_2D.h"
-#include "base/state_block.h"
-#include "base/state_angle.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_angle.h"
namespace wolf {
diff --git a/src/sensor/sensor_odom_3D.cpp b/src/sensor/sensor_odom_3D.cpp
index 0a5c2d7961b5d36e171e6b9d29fafdc30580a74e..5a3736d5e7e111ffbe20359b29c0d562f71a35e4 100644
--- a/src/sensor/sensor_odom_3D.cpp
+++ b/src/sensor/sensor_odom_3D.cpp
@@ -7,8 +7,8 @@
#include "base/sensor/sensor_odom_3D.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
namespace wolf {
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 17f6e562bb242ba315369effd722391f52f7cf4c..2a914ef672f7b31463d8f5309e147c52016101b6 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -1,6 +1,6 @@
#include "base/solver/solver_manager.h"
-#include "base/trajectory_base.h"
-#include "base/map_base.h"
+#include "base/trajectory/trajectory_base.h"
+#include "base/map/map_base.h"
#include "base/landmark/landmark_base.h"
namespace wolf {
@@ -13,26 +13,30 @@ SolverManager::SolverManager(const ProblemPtr& _wolf_problem) :
void SolverManager::update()
{
+ // Consume notification maps
+ auto fac_notification_map = wolf_problem_->consumeFactorNotificationMap();
+ auto sb_notification_map = wolf_problem_->consumeStateBlockNotificationMap();
+
// REMOVE CONSTRAINTS
- auto fac_notification_it = wolf_problem_->getFactorNotificationMap().begin();
- while ( fac_notification_it != wolf_problem_->getFactorNotificationMap().end() )
+ for (auto fac_notification_it = fac_notification_map.begin();
+ fac_notification_it != fac_notification_map.end();
+ /* nothing, next is handled within the for */)
{
if (fac_notification_it->second == REMOVE)
{
removeFactor(fac_notification_it->first);
- fac_notification_it = wolf_problem_->getFactorNotificationMap().erase(fac_notification_it);
+ fac_notification_it = fac_notification_map.erase(fac_notification_it);
}
else
fac_notification_it++;
}
// ADD/REMOVE STATE BLOCS
- auto sb_notification_it = wolf_problem_->getStateBlockNotificationMap().begin();
- while ( sb_notification_it != wolf_problem_->getStateBlockNotificationMap().end() )
+ while ( !sb_notification_map.empty() )
{
- StateBlockPtr state_ptr = sb_notification_it->first;
+ StateBlockPtr state_ptr = sb_notification_map.begin()->first;
- if (sb_notification_it->second == ADD)
+ if (sb_notification_map.begin()->second == ADD)
{
// only add if not added
if (state_blocks_.find(state_ptr) == state_blocks_.end())
@@ -46,7 +50,7 @@ void SolverManager::update()
}
else
{
- WOLF_DEBUG("Tried adding an already registered StateBlock.");
+ WOLF_DEBUG("Tried to add an already added !");
}
}
else
@@ -63,23 +67,24 @@ void SolverManager::update()
}
}
// next notification
- sb_notification_it = wolf_problem_->getStateBlockNotificationMap().erase(sb_notification_it);
+ sb_notification_map.erase(sb_notification_map.begin());
}
// ADD CONSTRAINTS
- fac_notification_it = wolf_problem_->getFactorNotificationMap().begin();
- while (fac_notification_it != wolf_problem_->getFactorNotificationMap().end())
+ while (!fac_notification_map.empty())
{
- assert(wolf_problem_->getFactorNotificationMap().begin()->second == ADD && "unexpected factor notification value after all REMOVE have been processed, this should be ADD");
+ assert(fac_notification_map.begin()->second == ADD && "unexpected factor notification value after all REMOVE have been processed, this should be ADD");
- addFactor(wolf_problem_->getFactorNotificationMap().begin()->first);
- fac_notification_it = wolf_problem_->getFactorNotificationMap().erase(fac_notification_it);
+ // add factor
+ addFactor(fac_notification_map.begin()->first);
+ // remove notification
+ fac_notification_map.erase(fac_notification_map.begin());
}
// UPDATE STATE BLOCKS (state, fix or local parameterization)
- for (auto state_ptr : wolf_problem_->getStateBlockPtrList())
+ for (auto state_pair : state_blocks_)
{
- assert(state_blocks_.find(state_ptr)!=state_blocks_.end() && "Updating the state of an unregistered StateBlock !");
+ auto state_ptr = state_pair.first;
// state update
if (state_ptr->stateUpdated())
@@ -105,9 +110,6 @@ void SolverManager::update()
state_ptr->resetLocalParamUpdated();
}
}
-
- assert(wolf_problem_->getFactorNotificationMap().empty() && "wolf problem's factors notification map not empty after update");
- assert(wolf_problem_->getStateBlockNotificationMap().empty() && "wolf problem's state_blocks notification map not empty after update");
}
wolf::ProblemPtr SolverManager::getProblem()
diff --git a/src/local_parametrization_base.cpp b/src/state_block/local_parametrization_base.cpp
similarity index 88%
rename from src/local_parametrization_base.cpp
rename to src/state_block/local_parametrization_base.cpp
index e2bc1cf4e36aafd25c8209826d3c8afe43828d62..485b3171b0a1d94115a428aa219effe9460f1b10 100644
--- a/src/local_parametrization_base.cpp
+++ b/src/state_block/local_parametrization_base.cpp
@@ -1,4 +1,4 @@
-#include "base/local_parametrization_base.h"
+#include "base/state_block/local_parametrization_base.h"
namespace wolf {
diff --git a/src/local_parametrization_homogeneous.cpp b/src/state_block/local_parametrization_homogeneous.cpp
similarity index 93%
rename from src/local_parametrization_homogeneous.cpp
rename to src/state_block/local_parametrization_homogeneous.cpp
index 14abaecd1a9fd93cfd1da4b9a6e1e1d0f9982dbd..b8982f89083c23207b0ae3032d1fd103c6c7b6ac 100644
--- a/src/local_parametrization_homogeneous.cpp
+++ b/src/state_block/local_parametrization_homogeneous.cpp
@@ -5,9 +5,9 @@
* Author: jsola
*/
-#include "base/local_parametrization_homogeneous.h"
+#include "base/state_block/local_parametrization_homogeneous.h"
#include "iostream"
-#include "base/rotations.h" // we use quaternion algebra here
+#include "base/math/rotations.h" // we use quaternion algebra here
namespace wolf {
diff --git a/src/local_parametrization_polyline_extreme.cpp b/src/state_block/local_parametrization_polyline_extreme.cpp
similarity index 93%
rename from src/local_parametrization_polyline_extreme.cpp
rename to src/state_block/local_parametrization_polyline_extreme.cpp
index da318cafb9e723aa1fecfe9f19d6a3195df7fde8..803c85b4f612b4dcfe9306fe54b2105e1be05506 100644
--- a/src/local_parametrization_polyline_extreme.cpp
+++ b/src/state_block/local_parametrization_polyline_extreme.cpp
@@ -1,6 +1,6 @@
-#include "base/local_parametrization_polyline_extreme.h"
-#include "base/state_block.h"
-#include "base/rotations.h"
+#include "base/state_block/local_parametrization_polyline_extreme.h"
+#include "base/state_block/state_block.h"
+#include "base/math/rotations.h"
namespace wolf {
diff --git a/src/local_parametrization_quaternion.cpp b/src/state_block/local_parametrization_quaternion.cpp
similarity index 97%
rename from src/local_parametrization_quaternion.cpp
rename to src/state_block/local_parametrization_quaternion.cpp
index d2af8f544f21f8f83528c7db6371f0a8407f12d0..1c2655f883ed00474d84feda1c2bbe47bc42e1aa 100644
--- a/src/local_parametrization_quaternion.cpp
+++ b/src/state_block/local_parametrization_quaternion.cpp
@@ -1,6 +1,6 @@
-#include "base/local_parametrization_quaternion.h"
-#include "base/rotations.h"
+#include "base/state_block/local_parametrization_quaternion.h"
+#include "base/math/rotations.h"
#include <iostream>
namespace wolf {
diff --git a/src/state_block.cpp b/src/state_block/state_block.cpp
similarity index 87%
rename from src/state_block.cpp
rename to src/state_block/state_block.cpp
index 46411e7c30aefb15f921bb9f9fa9c7a114a6154b..96df91772539470b66ed81887d68dc72b19c1def 100644
--- a/src/state_block.cpp
+++ b/src/state_block/state_block.cpp
@@ -1,4 +1,4 @@
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
namespace wolf
{
@@ -33,7 +33,7 @@ void StateBlock::setFixed(bool _fixed)
//
//void StateBlock::removeFromProblem(ProblemPtr _problem_ptr)
//{
-// _problem_ptr->removeStateBlockPtr(shared_from_this());
+// _problem_ptr->removeStateBlock(shared_from_this());
//}
}
diff --git a/src/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
similarity index 96%
rename from src/trajectory_base.cpp
rename to src/trajectory/trajectory_base.cpp
index 04bbce792fce08bb662a998c3e2d89a1b0122c3e..80c6e9e83c09fe5e9420ed9a4979873d254d354d 100644
--- a/src/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -1,5 +1,5 @@
-#include "base/trajectory_base.h"
-#include "base/frame_base.h"
+#include "base/trajectory/trajectory_base.h"
+#include "base/frame/frame_base.h"
namespace wolf {
diff --git a/src/yaml/processor_IMU_yaml.cpp b/src/yaml/processor_IMU_yaml.cpp
index 2cfc70f46321ea3af69d657b47540258b4b17ebd..4146484942a63db640b6c4405444195fe27859a5 100644
--- a/src/yaml/processor_IMU_yaml.cpp
+++ b/src/yaml/processor_IMU_yaml.cpp
@@ -10,7 +10,7 @@
#include "base/yaml/yaml_conversion.h"
// wolf
-#include "base/factory.h"
+#include "base/common/factory.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
@@ -30,6 +30,7 @@ static ProcessorParamsBasePtr createProcessorIMUParams(const std::string & _file
ProcessorParamsIMUPtr params = std::make_shared<ProcessorParamsIMU>();
+ params->unmeasured_perturbation_std = config["unmeasured perturbation std"].as<Scalar>();
params->max_time_span = kf_vote["max time span"] .as<Scalar>();
params->max_buff_length = kf_vote["max buffer length"] .as<SizeEigen >();
params->dist_traveled = kf_vote["dist traveled"] .as<Scalar>();
diff --git a/src/yaml/processor_image_yaml.cpp b/src/yaml/processor_image_yaml.cpp
index a02d44208d414294d203661a27da019b94d04afa..47e0a32e2aacad260db22b621341352a825800df 100644
--- a/src/yaml/processor_image_yaml.cpp
+++ b/src/yaml/processor_image_yaml.cpp
@@ -9,7 +9,7 @@
#include "base/yaml/yaml_conversion.h"
// wolf
-#include "base/factory.h"
+#include "base/common/factory.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
diff --git a/src/yaml/processor_odom_3D_yaml.cpp b/src/yaml/processor_odom_3D_yaml.cpp
index 5c2172ea9f26c3561f7fc48c5c56d4be7695ed1c..c63e3d9b5c50e3add064bc334109be163ac578d5 100644
--- a/src/yaml/processor_odom_3D_yaml.cpp
+++ b/src/yaml/processor_odom_3D_yaml.cpp
@@ -10,7 +10,7 @@
// wolf
#include "base/processor/processor_odom_3D.h"
-#include "base/factory.h"
+#include "base/common/factory.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
diff --git a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
index a3f9362b5a91d32d793a6459e409c2849f151bc4..0af63b58212e96fe16fb0e52100696e0c1a2fe15 100644
--- a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
+++ b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
@@ -10,7 +10,7 @@
#include "base/yaml/yaml_conversion.h"
// wolf
-#include "base/factory.h"
+#include "base/common/factory.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
diff --git a/src/yaml/sensor_IMU_yaml.cpp b/src/yaml/sensor_IMU_yaml.cpp
index cc3dbb649b7d2c14c92ec4e78f2d43c0c343b254..79cf183cdad989a3a123488b10f878c86fb142d3 100644
--- a/src/yaml/sensor_IMU_yaml.cpp
+++ b/src/yaml/sensor_IMU_yaml.cpp
@@ -10,7 +10,7 @@
#include "base/yaml/yaml_conversion.h"
// wolf
-#include "base/factory.h"
+#include "base/common/factory.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
diff --git a/src/yaml/sensor_camera_yaml.cpp b/src/yaml/sensor_camera_yaml.cpp
index 5e5e3df19cd91a8735f6625ecd2d551deb7e7b77..2b4bfc4d865d01ede338ae2aaf45ef0ffe21838a 100644
--- a/src/yaml/sensor_camera_yaml.cpp
+++ b/src/yaml/sensor_camera_yaml.cpp
@@ -10,7 +10,7 @@
// wolf
#include "base/sensor/sensor_camera.h"
-#include "base/factory.h"
+#include "base/common/factory.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
diff --git a/src/yaml/sensor_laser_2D_yaml.cpp b/src/yaml/sensor_laser_2D_yaml.cpp
index e687c14463b746d147d791338afc5e34120fe8f3..bd553d20dd3145f8d899710094d0a4d7e98310cd 100644
--- a/src/yaml/sensor_laser_2D_yaml.cpp
+++ b/src/yaml/sensor_laser_2D_yaml.cpp
@@ -10,7 +10,7 @@
// wolf
//#include "base/intrinsics_factory.h"
-#include "base/factory.h"
+#include "base/common/factory.h"
#include "base/sensor/sensor_laser_2D.h"
// yaml library
diff --git a/src/yaml/sensor_odom_3D_yaml.cpp b/src/yaml/sensor_odom_3D_yaml.cpp
index 63a7baa2c4cad96e3ca297950df76611fb4ed7ba..d4e90dd831e5bac30eeb680f8e114b27e412d12d 100644
--- a/src/yaml/sensor_odom_3D_yaml.cpp
+++ b/src/yaml/sensor_odom_3D_yaml.cpp
@@ -10,7 +10,7 @@
// wolf
#include "base/sensor/sensor_odom_3D.h"
-#include "base/factory.h"
+#include "base/common/factory.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
diff --git a/test/gtest_IMU.cpp b/test/gtest_IMU.cpp
index 7487eb2fe49ada12c03120ce83810abf3d0d8943..d2657e4156cf6991d3342505cb558712837cb344 100644
--- a/test/gtest_IMU.cpp
+++ b/test/gtest_IMU.cpp
@@ -8,13 +8,13 @@
//Wolf
#include "base/processor/processor_IMU.h"
#include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include "base/sensor/sensor_odom_3D.h"
#include "base/processor/processor_odom_3D.h"
#include "base/ceres_wrapper/ceres_manager.h"
#include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
// make my life easier
using namespace Eigen;
diff --git a/test/gtest_IMU_tools.cpp b/test/gtest_IMU_tools.cpp
index e41007154f27c8c6de6845db934215df22d7c382..3cc7d33e28f8149d33ee01d109357569e3dc44c5 100644
--- a/test/gtest_IMU_tools.cpp
+++ b/test/gtest_IMU_tools.cpp
@@ -5,7 +5,7 @@
* Author: jsola
*/
-#include "base/IMU_tools.h"
+#include "base/math/IMU_tools.h"
#include "utils_gtest.h"
using namespace Eigen;
diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp
index d6e3b9b1c2a2154677f4a2a77094b4bac93146f0..002aa206ba306ad9cd84e978bb8f0a6700653eb9 100644
--- a/test/gtest_SE3.cpp
+++ b/test/gtest_SE3.cpp
@@ -6,7 +6,7 @@
*/
-#include "base/SE3.h"
+#include "base/math/SE3.h"
#include "utils_gtest.h"
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index 1d4d6b866eab77300671b0b3fa827372e613b35d..c3515fa07e94c83f2b53a2bcb2dab29505446220 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -8,7 +8,7 @@
#include "utils_gtest.h"
#include "base/capture/capture_base.h"
-#include "base/state_angle.h"
+#include "base/state_block/state_angle.h"
using namespace wolf;
using namespace Eigen;
@@ -109,7 +109,7 @@ TEST(CaptureBase, process)
SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2));
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, nullptr));
ASSERT_DEATH({C->process();},""); // No sensor in the capture should fail
- C->setSensorPtr(S);
+ C->setSensor(S);
ASSERT_TRUE(C->process()); // This should not fail (although it does nothing)
}
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 773e5ce58adc2de0df639569d3c6bc9c847df182..a4bfd4edb5f0a4090fb9262d343931dfec9b6c08 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -6,18 +6,18 @@
*/
#include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
-#include "base/problem.h"
+#include "base/problem/problem.h"
#include "base/sensor/sensor_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
#include "base/capture/capture_void.h"
#include "base/factor/factor_pose_2D.h"
#include "base/factor/factor_quaternion_absolute.h"
#include "base/solver/solver_manager.h"
#include "base/ceres_wrapper/ceres_manager.h"
-#include "base/local_parametrization_angle.h"
-#include "base/local_parametrization_quaternion.h"
+#include "base/state_block/local_parametrization_angle.h"
+#include "base/state_block/local_parametrization_quaternion.h"
#include "ceres/ceres.h"
@@ -410,12 +410,10 @@ TEST(CeresManager, AddRemoveFactor)
// FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f));
- ASSERT_TRUE(P->getFactorNotificationMap().begin()->first == c);
-
// remove factor
P->removeFactor(c);
- ASSERT_TRUE(P->getFactorNotificationMap().empty());
+ ASSERT_TRUE(P->consumeFactorNotificationMap().empty()); // add+remove = empty
// update solver
ceres_manager_ptr->update();
@@ -477,7 +475,7 @@ TEST(CeresManager, AddStateBlockLocalParam)
// Local param
LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrizationPtr(local_param_ptr);
+ sb_ptr->setLocalParametrization(local_param_ptr);
// add stateblock
P->addStateBlock(sb_ptr);
@@ -504,7 +502,7 @@ TEST(CeresManager, RemoveLocalParam)
// Local param
LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrizationPtr(local_param_ptr);
+ sb_ptr->setLocalParametrization(local_param_ptr);
// add stateblock
P->addStateBlock(sb_ptr);
@@ -545,7 +543,7 @@ TEST(CeresManager, AddLocalParam)
// Local param
LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrizationPtr(local_param_ptr);
+ sb_ptr->setLocalParametrization(local_param_ptr);
// update solver
ceres_manager_ptr->update();
@@ -634,7 +632,7 @@ TEST(CeresManager, FactorsUpdateLocalParam)
// remove local param
LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationQuaternionGlobal>();
- F->getO()->setLocalParametrizationPtr(local_param_ptr);
+ F->getO()->setLocalParametrization(local_param_ptr);
// update solver
ceres_manager_ptr->update();
diff --git a/test/gtest_eigen_predicates.cpp b/test/gtest_eigen_predicates.cpp
index 649ba85e9b5477d38498b3f80678eaf25e87697b..b3c3010491be08806da65cb6a5e6d953f23943cf 100644
--- a/test/gtest_eigen_predicates.cpp
+++ b/test/gtest_eigen_predicates.cpp
@@ -1,6 +1,6 @@
#include "utils_gtest.h"
-#include "base/eigen_predicates.h"
+#include "base/utils/eigen_predicates.h"
TEST(TestEigenPredicates, TestEigenDynPredZero)
{
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index ef16e5ae648e20251da2438bf28feb327d00b481..68d4ad11574d3ffab8bca44b0cb092c673dd0e92 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -6,9 +6,9 @@
*/
#include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
-#include "base/problem.h"
+#include "base/problem/problem.h"
#include "base/capture/capture_IMU.h"
#include "base/sensor/sensor_base.h"
#include "base/sensor/sensor_odom_3D.h"
diff --git a/test/gtest_factor_IMU.cpp b/test/gtest_factor_IMU.cpp
index 1dd8cbb7d98f579d30e71aeb4306d45a0b38b052..18e3d8b966f57c5c06f3e1066b1d735c9d3e3ce9 100644
--- a/test/gtest_factor_IMU.cpp
+++ b/test/gtest_factor_IMU.cpp
@@ -14,7 +14,7 @@
#include "ceres_wrapper/ceres_manager.h"
#include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
#include <iostream>
#include <fstream>
diff --git a/test/gtest_factor_autodiff_distance_3D.cpp b/test/gtest_factor_autodiff_distance_3D.cpp
index ac4ec5a9cc27bcecc67a368efedfcff1d2245a94..7b3c813201ab9196fdf321e6c76c5948b9900ee4 100644
--- a/test/gtest_factor_autodiff_distance_3D.cpp
+++ b/test/gtest_factor_autodiff_distance_3D.cpp
@@ -6,10 +6,10 @@
*/
#include "base/factor/factor_autodiff_distance_3D.h"
-#include "base/problem.h"
-#include "base/logging.h"
+#include "base/problem/problem.h"
+#include "base/utils/logging.h"
#include "base/ceres_wrapper/ceres_manager.h"
-#include "base/rotations.h"
+#include "base/math/rotations.h"
#include "utils_gtest.h"
diff --git a/test/gtest_factor_autodiff_trifocal.cpp b/test/gtest_factor_autodiff_trifocal.cpp
index 9cfc55e642217fde93aa249562280d8cc666e777..50b1988d84ee617bac7de900f1ab393b3f60b5a7 100644
--- a/test/gtest_factor_autodiff_trifocal.cpp
+++ b/test/gtest_factor_autodiff_trifocal.cpp
@@ -1,6 +1,6 @@
#include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
#include "base/ceres_wrapper/ceres_manager.h"
#include "base/processor/processor_tracker_feature_trifocal.h"
diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp
index 6ba19ec9cec9d468625f3178d6877bfd9e160f5c..715be59f53a14c3e814168286762b58f642cb6f2 100644
--- a/test/gtest_feature_IMU.cpp
+++ b/test/gtest_feature_IMU.cpp
@@ -2,12 +2,12 @@
#include "base/capture/capture_IMU.h"
#include "base/processor/processor_IMU.h"
#include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
#include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
class FeatureIMU_test : public testing::Test
{
@@ -61,7 +61,7 @@ class FeatureIMU_test : public testing::Test
// Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
// give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
imu_ptr = std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6s::Identity(), Eigen::Vector6s::Zero());
- imu_ptr->setFramePtr(origin_frame); //to get ptr to Frm in processorIMU and then get biases
+ imu_ptr->setFrame(origin_frame); //to get ptr to Frm in processorIMU and then get biases
//process data
data_ << 2, 0, 9.8, 0, 0, 0;
@@ -91,7 +91,7 @@ class FeatureIMU_test : public testing::Test
calib_preint,
dD_db_jacobians,
imu_ptr);
- feat_imu->setCapturePtr(imu_ptr); //associate the feature to a capture
+ feat_imu->setCapture(imu_ptr); //associate the feature to a capture
}
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 41334dfe8ae0f1e4f97648cff841e3076308fff5..85f3d873d1dccba7c87ba5409a2f29ddf583d25c 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -6,9 +6,9 @@
*/
#include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
#include "base/sensor/sensor_odom_2D.h"
#include "base/processor/processor_odom_2D.h"
#include "base/factor/factor_odom_2D.h"
@@ -137,7 +137,7 @@ TEST(FrameBase, LinksToTree)
ASSERT_TRUE(F1->getCaptureList().empty());
}
-#include "base/state_quaternion.h"
+#include "base/state_block/state_quaternion.h"
TEST(FrameBase, GetSetState)
{
// Create PQV_3D state blocks
diff --git a/test/gtest_local_param.cpp b/test/gtest_local_param.cpp
index 42e01489ed722497bdaad1d818ae572e1d2c0100..45d570671bbe43e6c51d8a186ed1a5cc081550ba 100644
--- a/test/gtest_local_param.cpp
+++ b/test/gtest_local_param.cpp
@@ -6,13 +6,13 @@
*/
#include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
-#include "base/local_parametrization_quaternion.h"
-#include "base/local_parametrization_homogeneous.h"
-#include "base/rotations.h"
+#include "base/state_block/local_parametrization_quaternion.h"
+#include "base/state_block/local_parametrization_homogeneous.h"
+#include "base/math/rotations.h"
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include <iostream>
diff --git a/test/gtest_make_posdef.cpp b/test/gtest_make_posdef.cpp
index 1dfcce9353f5726f5e69593f99e8a7dc3782cffd..06f9fd20e97dc4bb64033da673e9e04c29bc277c 100644
--- a/test/gtest_make_posdef.cpp
+++ b/test/gtest_make_posdef.cpp
@@ -1,5 +1,5 @@
#include "utils_gtest.h"
-#include "base/wolf.h"
+#include "base/common/wolf.h"
using namespace Eigen;
using namespace wolf;
diff --git a/test/gtest_motion_buffer.cpp b/test/gtest_motion_buffer.cpp
index 6d0301692fc5bea5b444a6aa582e8170d489294c..6aee437dc13357de1618f036d27a90dfc094acd5 100644
--- a/test/gtest_motion_buffer.cpp
+++ b/test/gtest_motion_buffer.cpp
@@ -6,11 +6,11 @@
*/
#include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
-#include "base/motion_buffer.h"
+#include "base/processor/motion_buffer.h"
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include <iostream>
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index 78f5ee81cb8b882640b78753c9ebf82c8015b3e5..0259ebeb2ef16978934fa2b5e469405351bd9362 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -13,8 +13,8 @@
// Wolf includes
#include "base/sensor/sensor_odom_2D.h"
-#include "base/state_block.h"
-#include "base/wolf.h"
+#include "base/state_block/state_block.h"
+#include "base/common/wolf.h"
#include "base/ceres_wrapper/ceres_manager.h"
// STL includes
@@ -92,7 +92,9 @@ void show(const ProblemPtr& problem)
}
}
cout << " state: \n" << F->getState().transpose() << endl;
- cout << " covariance: \n" << problem->getFrameCovariance(F) << endl;
+ Eigen::MatrixXs cov;
+ problem->getFrameCovariance(F,cov);
+ cout << " covariance: \n" << cov << endl;
}
}
}
@@ -169,12 +171,18 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D)
0, 1.91, 0.48,
0, 0.48, 0.14;
+ // get covariances
+ MatrixXs P0_solver, P1_solver, P2_solver;
+ ASSERT_TRUE(Pr->getFrameCovariance(F0, P0_solver));
+ ASSERT_TRUE(Pr->getFrameCovariance(F1, P1_solver));
+ ASSERT_TRUE(Pr->getFrameCovariance(F2, P2_solver));
+
ASSERT_POSE2D_APPROX(F0->getState(), Vector3s(0,0,0), 1e-6);
- ASSERT_MATRIX_APPROX(Pr->getFrameCovariance(F0), P0, 1e-6);
+ ASSERT_MATRIX_APPROX(P0_solver, P0, 1e-6);
ASSERT_POSE2D_APPROX(F1->getState(), Vector3s(2,0,0), 1e-6);
- ASSERT_MATRIX_APPROX(Pr->getFrameCovariance(F1), P1, 1e-6);
+ ASSERT_MATRIX_APPROX(P1_solver, P1, 1e-6);
ASSERT_POSE2D_APPROX(F2->getState(), Vector3s(4,0,0), 1e-6);
- ASSERT_MATRIX_APPROX(Pr->getFrameCovariance(F2), P2, 1e-6);
+ ASSERT_MATRIX_APPROX(P2_solver, P2, 1e-6);
}
TEST(Odom2D, VoteForKfAndSolve)
@@ -291,7 +299,9 @@ TEST(Odom2D, VoteForKfAndSolve)
// std::cout << report << std::endl;
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- ASSERT_MATRIX_APPROX(problem->getLastKeyFrameCovariance() , integrated_cov_vector[5], 1e-6);
+ MatrixXs computed_cov;
+ ASSERT_TRUE(problem->getLastKeyFrameCovariance(computed_cov));
+ ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[5], 1e-6);
}
TEST(Odom2D, KF_callback)
@@ -325,7 +335,7 @@ TEST(Odom2D, KF_callback)
params->angle_turned = 6.28;
params->max_time_span = 100;
params->cov_det = 100;
- params->unmeasured_perturbation_std = 0.001;
+ params->unmeasured_perturbation_std = 0.000001;
Matrix3s unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3s::Identity();
ProcessorBasePtr prc_base = problem->installProcessor("ODOM 2D", "odom", sensor_odom2d, params);
ProcessorOdom2DPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2D>(prc_base);
@@ -419,7 +429,9 @@ TEST(Odom2D, KF_callback)
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
ASSERT_POSE2D_APPROX(problem->getLastKeyFrame()->getState() , integrated_pose_vector[n_split], 1e-6);
- ASSERT_MATRIX_APPROX(problem->getLastKeyFrameCovariance() , integrated_cov_vector [n_split], 1e-6);
+ MatrixXs computed_cov;
+ ASSERT_TRUE(problem->getLastKeyFrameCovariance(computed_cov));
+ ASSERT_MATRIX_APPROX(computed_cov, integrated_cov_vector [n_split], 1e-6);
////////////////////////////////////////////////////////////////
// Split between keyframes, exact timestamp
@@ -455,12 +467,16 @@ TEST(Odom2D, KF_callback)
problem->print(4,1,1,1);
// check the split KF
- ASSERT_POSE2D_APPROX(keyframe_1->getState() , integrated_pose_vector[m_split], 1e-6);
- ASSERT_MATRIX_APPROX(problem->getFrameCovariance(keyframe_1) , integrated_cov_vector [m_split], 1e-6);
+ MatrixXs KF1_cov;
+ ASSERT_TRUE(problem->getFrameCovariance(keyframe_1, KF1_cov));
+ ASSERT_POSE2D_APPROX(keyframe_1->getState(), integrated_pose_vector[m_split], 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_cov, integrated_cov_vector [m_split], 1e-6);
// check other KF in the future of the split KF
- ASSERT_POSE2D_APPROX(problem->getLastKeyFrame()->getState() , integrated_pose_vector[n_split], 1e-6);
- ASSERT_MATRIX_APPROX(problem->getFrameCovariance(keyframe_2) , integrated_cov_vector [n_split], 1e-6);
+ MatrixXs KF2_cov;
+ ASSERT_TRUE(problem->getFrameCovariance(keyframe_2, KF2_cov));
+ ASSERT_POSE2D_APPROX(problem->getLastKeyFrame()->getState(), integrated_pose_vector[n_split], 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_cov, integrated_cov_vector [n_split], 1e-6);
// Check full trajectory
t = t0;
diff --git a/test/gtest_odom_3D.cpp b/test/gtest_odom_3D.cpp
index ab4f82bb186b949e5dd0d71b5e9e812972a6708e..34cb26dc38598ad2860c765b984fa891ff8e95c4 100644
--- a/test/gtest_odom_3D.cpp
+++ b/test/gtest_odom_3D.cpp
@@ -7,8 +7,8 @@
#include "utils_gtest.h"
-#include "base/wolf.h"
-#include "base/logging.h"
+#include "base/common/wolf.h"
+#include "base/utils/logging.h"
#include "base/processor/processor_odom_3D.h"
diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp
index 8f68a5bd532c5a1408d0ba9654ec9f27283c7ba3..d4ed20575f2ae0920b992be6f7116abc1e2fc439 100644
--- a/test/gtest_pack_KF_buffer.cpp
+++ b/test/gtest_pack_KF_buffer.cpp
@@ -13,7 +13,7 @@
#include "base/processor/processor_tracker_feature_dummy.h"
#include "base/capture/capture_void.h"
-#include "base/problem.h"
+#include "base/problem/problem.h"
// STL
#include <iterator>
diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
index e39c870881bf6e2abb57e771a2b79a3ed3706ce2..79f7aff496e524368e17ee087fed609efb2dd721 100644
--- a/test/gtest_param_prior.cpp
+++ b/test/gtest_param_prior.cpp
@@ -6,9 +6,9 @@
*/
#include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
-#include "base/problem.h"
+#include "base/problem/problem.h"
#include "base/ceres_wrapper/ceres_manager.h"
#include "base/sensor/sensor_odom_3D.h"
diff --git a/test/gtest_pinhole.cpp b/test/gtest_pinhole.cpp
index fb36c6d127b11d14b86c8887fa32aa1b23d07391..378757f09922e7ae48cf16b96a41b497cb7e3717 100644
--- a/test/gtest_pinhole.cpp
+++ b/test/gtest_pinhole.cpp
@@ -5,7 +5,7 @@
* Author: jsola
*/
-#include "base/pinhole_tools.h"
+#include "base/math/pinhole_tools.h"
#include "utils_gtest.h"
using namespace Eigen;
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 09d758e651873862fbfd746dbf3cb351dceb8e5f..b4ccf0efb1fc87e43594f332a6bd0f9fd1a1baf3 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -6,9 +6,9 @@
*/
#include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
-#include "base/problem.h"
+#include "base/problem/problem.h"
#include "base/sensor/sensor_base.h"
#include "base/sensor/sensor_odom_3D.h"
#include "base/processor/processor_odom_3D.h"
@@ -211,13 +211,11 @@ TEST(Problem, StateBlocks)
// 2 state blocks, fixed
SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs, wolf_root + "/src/examples/sensor_odom_3D.yaml");
- ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int) 2);
- ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int) 2);
+ ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int) 2);
// 3 state blocks, fixed
SensorBasePtr St = P->installSensor ("CAMERA", "camera", xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
- ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int) (2 + 3));
- ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int) (2 + 3));
+ ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int) (/*2 + */3)); // consume empties the notification map, so only should contain notification since last call
ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
params->time_tolerance = 0.1;
@@ -230,15 +228,13 @@ TEST(Problem, StateBlocks)
// 2 state blocks, estimated
P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0);
- ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 3 + 2));
- ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int)(2 + 3 + 2));
+ ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int)(/*2 + 3*/ + 2)); // consume empties the notification map, so only should contain notification since last call
// P->print(4,1,1,1);
// change some SB properties
St->unfixExtrinsics();
- ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 3 + 2));
- ASSERT_EQ(P->getStateBlockNotificationMap().size(),(unsigned int)(2 + 3 + 2 /*+ 2*/)); // XXX: 2 more notifications on the same SB!
+ ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // changes in state_blocks status (fix/state/localparam) does not raise a notification in problem, only ADD/REMOVE
// P->print(4,1,1,1);
}
@@ -267,11 +263,20 @@ TEST(Problem, Covariances)
St->unfixExtrinsics();
FrameBasePtr F = P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0);
- Eigen::MatrixXs Cov = P->getFrameCovariance(F);
+ // set covariance (they are not computed without a solver)
+ P->addCovarianceBlock(F->getP(), Eigen::Matrix3s::Identity());
+ P->addCovarianceBlock(F->getO(), Eigen::Matrix4s::Identity());
+ P->addCovarianceBlock(F->getP(), F->getO(), Eigen::Matrix<Scalar,3,4>::Zero());
+
+ // get covariance
+ Eigen::MatrixXs Cov;
+ ASSERT_TRUE(P->getFrameCovariance(F, Cov));
// FIXME Frame covariance should be 6x6, but it is actually 7x7 (the state of the state blocks, not of the local parametrizations)
+ // JV: The local parameterization projects the covariance to the state size.
ASSERT_EQ(Cov.cols() , 7);
ASSERT_EQ(Cov.rows() , 7);
+ ASSERT_MATRIX_APPROX(Cov, Eigen::Matrix7s::Identity(), 1e-12);
}
diff --git a/test/gtest_processor_IMU.cpp b/test/gtest_processor_IMU.cpp
index 8a966f77b39390e80ad2ad5e68655bbb764ebf85..822fc5b816eda32d729067f0476f8dd4b98b6ac5 100644
--- a/test/gtest_processor_IMU.cpp
+++ b/test/gtest_processor_IMU.cpp
@@ -8,12 +8,12 @@
#include "base/capture/capture_IMU.h"
#include "base/processor/processor_IMU.h"
#include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
-#include "base/rotations.h"
+#include "base/math/rotations.h"
#include "base/ceres_wrapper/ceres_manager.h"
#include <cmath>
diff --git a/test/gtest_processor_IMU_jacobians.cpp b/test/gtest_processor_IMU_jacobians.cpp
index 1e3b0940cdf2434a059b071b95ca47847ebadd48..627549043e314e3861763eb4ba1253a6631e38ba 100644
--- a/test/gtest_processor_IMU_jacobians.cpp
+++ b/test/gtest_processor_IMU_jacobians.cpp
@@ -9,10 +9,10 @@
#include "base/capture/capture_IMU.h"
#include "base/sensor/sensor_IMU.h"
#include "test/processor_IMU_UnitTester.h"
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
#include <iostream>
#include <fstream>
#include <iomanip>
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 754229c69d1dc25cede623abcc5f66528ceb75ea..096824ee97d758c29ec83e5ef25c91a398bb125e 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -14,7 +14,7 @@
#include "base/processor/processor_tracker_feature_dummy.h"
#include "base/capture/capture_void.h"
-#include "base/problem.h"
+#include "base/problem/problem.h"
// STL
#include <iterator>
diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
index b37be3fa117325cf8f9b5c227ca44a5e5a62b01b..4c40933db6faa786a7c8dcf203c094d65e99ec10 100644
--- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
+++ b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
@@ -7,7 +7,7 @@
*/
#include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
#include "base/sensor/sensor_odom_2D.h"
#include "base/processor/processor_frame_nearest_neighbor_filter.h"
@@ -164,7 +164,7 @@ TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated)
1.2,
sensor_ptr);
// Make 3rd frame last Keyframe
- problem->getTrajectory()->setLastKeyFramePtr(F3);
+ problem->getTrajectory()->setLastKeyFrame(F3);
// trigger search for loopclosure
processor_ptr_point2d->process(incomming_dummy);
@@ -197,7 +197,7 @@ TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance)
position_covariance_matrix);
// Make 3rd frame last Keyframe
- problem->getTrajectory()->setLastKeyFramePtr(F3);
+ problem->getTrajectory()->setLastKeyFrame(F3);
// trigger search for loopclosure
processor_ptr_ellipse2d->process(incomming_dummy);
@@ -209,7 +209,7 @@ TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance)
ASSERT_EQ(testloops[1]->id(), (unsigned int) 2);
// Make 4th frame last Keyframe
- problem->getTrajectory()->setLastKeyFramePtr(F4);
+ problem->getTrajectory()->setLastKeyFrame(F4);
// trigger search for loopclosure again
processor_ptr_ellipse2d->process(incomming_dummy);
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 205c79f9d60a78a4b006e204b6537adea720274c..0a02d13a13dbd9d25871cbc718aaaf3bb3902568 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -7,8 +7,8 @@
#include "utils_gtest.h"
-#include "base/wolf.h"
-#include "base/logging.h"
+#include "base/common/wolf.h"
+#include "base/utils/logging.h"
#include "base/sensor/sensor_odom_2D.h"
#include "base/processor/processor_odom_2D.h"
diff --git a/test/gtest_processor_tracker_feature_trifocal.cpp b/test/gtest_processor_tracker_feature_trifocal.cpp
index 71154fd5fc333a498c9e4c2e4ea9ffe7a30e0918..b94ef87e7b80fe18827dee79d814a7550ee9f32f 100644
--- a/test/gtest_processor_tracker_feature_trifocal.cpp
+++ b/test/gtest_processor_tracker_feature_trifocal.cpp
@@ -1,7 +1,7 @@
#include "utils_gtest.h"
-#include "base/wolf.h"
-#include "base/logging.h"
+#include "base/common/wolf.h"
+#include "base/utils/logging.h"
#include "vision_utils.h"
diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp
index 5d656eda2fe23cde71e16b1ca15c389e11d65940..fb4583063808187c7e8ed1feb8fcaf9ad7f43eb2 100644
--- a/test/gtest_rotation.cpp
+++ b/test/gtest_rotation.cpp
@@ -9,8 +9,8 @@
#include <Eigen/Geometry>
//Wolf
-#include "base/wolf.h"
-#include "base/rotations.h"
+#include "base/common/wolf.h"
+#include "base/math/rotations.h"
//std
#include <iostream>
diff --git a/test/gtest_shared_from_this.cpp b/test/gtest_shared_from_this.cpp
index cebdce1f8da148e95c39422c422d9a44422c3dbe..9c9055ff5d3f7911d4d87b83e44f77e4bcc5b163 100644
--- a/test/gtest_shared_from_this.cpp
+++ b/test/gtest_shared_from_this.cpp
@@ -1,5 +1,5 @@
#include "utils_gtest.h"
-#include "base/node_base.h"
+#include "base/common/node_base.h"
class CChildBase;
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index be165e8ca382eb6cd5a9e19294f2b6b15efff184..56d3aa2ec37ec58753c922966c818412145ce635 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -6,16 +6,16 @@
*/
#include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
-#include "base/problem.h"
+#include "base/problem/problem.h"
#include "base/sensor/sensor_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
#include "base/capture/capture_void.h"
#include "base/factor/factor_pose_2D.h"
#include "base/solver/solver_manager.h"
-#include "base/local_parametrization_base.h"
-#include "base/local_parametrization_angle.h"
+#include "base/state_block/local_parametrization_base.h"
+#include "base/state_block/local_parametrization_angle.h"
#include <iostream>
@@ -263,7 +263,7 @@ TEST(SolverManager, AddUpdateLocalParamStateBlock)
// Local param
LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrizationPtr(local_ptr);
+ sb_ptr->setLocalParametrization(local_ptr);
// Fix state block
sb_ptr->fix();
@@ -298,7 +298,7 @@ TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock)
// Local param
LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrizationPtr(local_ptr);
+ sb_ptr->setLocalParametrization(local_ptr);
// check flags
ASSERT_FALSE(sb_ptr->stateUpdated());
@@ -445,13 +445,14 @@ TEST(SolverManager, AddUpdatedStateBlock)
ASSERT_TRUE(sb_ptr->fixUpdated());
ASSERT_TRUE(sb_ptr->stateUpdated());
- // == When an ADD is notified, all previous notifications should be cleared (if not consumption of notifs) ==
+ // == When an ADD is notified: a ADD notification should be stored ==
// add state_block
P->addStateBlock(sb_ptr);
- ASSERT_EQ(P->getStateBlockNotificationMap().size(),1);
- ASSERT_EQ(P->getStateBlockNotificationMap().begin()->second,ADD);
+ auto state_block_notification_map = P->consumeStateBlockNotificationMap();
+ ASSERT_EQ(state_block_notification_map.size(),1);
+ ASSERT_EQ(state_block_notification_map.begin()->second,ADD);
// == Insert OTHER notifications ==
@@ -462,37 +463,30 @@ TEST(SolverManager, AddUpdatedStateBlock)
// Fix --> FLAG
sb_ptr->unfix();
- ASSERT_EQ(P->getStateBlockNotificationMap().size(),1); // only ADD notification (fix and state are flags in sb)
+ ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // No new notifications (fix and set state are flags in sb)
- // == REMOVE should clear the previous notification (ADD) in the stack ==
+ // == When an REMOVE is notified: a REMOVE notification should be stored ==
// remove state_block
P->removeStateBlock(sb_ptr);
- ASSERT_EQ(P->getStateBlockNotificationMap().size(),0);// ADD + REMOVE = EMPTY
+ state_block_notification_map = P->consumeStateBlockNotificationMap();
+ ASSERT_EQ(state_block_notification_map.size(),1);
+ ASSERT_EQ(state_block_notification_map.begin()->second,REMOVE);
- // == UPDATES + REMOVE should clear the list of notifications ==
+ // == ADD + REMOVE: notification map should be empty ==
+ P->addStateBlock(sb_ptr);
+ P->removeStateBlock(sb_ptr);
+ ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty());
+ // == UPDATES should clear the list of notifications ==
// add state_block
P->addStateBlock(sb_ptr);
// update solver
solver_manager_ptr->update();
- ASSERT_EQ(P->getStateBlockNotificationMap().size(),0); // After solver_manager->update, notifications should be empty
-
- // Fix
- sb_ptr->fix();
-
- // Set State
- state_2 = 2*state;
- sb_ptr->setState(state_2);
-
- // remove state_block
- P->removeStateBlock(sb_ptr);
-
- ASSERT_EQ(P->getStateBlockNotificationMap().size(),1);
- ASSERT_EQ(P->getStateBlockNotificationMap().begin()->second, REMOVE);
+ ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // After solver_manager->update, notifications should be empty
}
TEST(SolverManager, AddFactor)
@@ -570,12 +564,10 @@ TEST(SolverManager, AddRemoveFactor)
auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f, f));
- ASSERT_TRUE(P->getFactorNotificationMap().begin()->first == c);
-
// add factor
P->removeFactor(c);
- ASSERT_TRUE(P->getFactorNotificationMap().empty());
+ ASSERT_TRUE(P->consumeFactorNotificationMap().empty()); // ADD+REMOVE = empty
// update solver
solver_manager_ptr->update();
diff --git a/test/gtest_time_stamp.cpp b/test/gtest_time_stamp.cpp
index aad321781d7682c8c60dd83c9e0970e17118b164..a8389ce8b36f453a759ba16000ec106fe68160a3 100644
--- a/test/gtest_time_stamp.cpp
+++ b/test/gtest_time_stamp.cpp
@@ -1,5 +1,5 @@
#include "utils_gtest.h"
-#include "base/time_stamp.h"
+#include "base/common/time_stamp.h"
#include <thread>
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index e3d533a5fb31f4f524cde8630446ba0e5dda259b..99d47c9c3ffd95da4c6f98f6cefd0d4da07c4ed0 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -7,7 +7,7 @@
#include "utils_gtest.h"
-#include "base/track_matrix.h"
+#include "base/processor/track_matrix.h"
using namespace wolf;
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index e70f26c025b28587e99a1b393189f75c9577d7c4..3366ae781f4784b483984a6f951705ba17240326 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -6,11 +6,11 @@
*/
#include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
-#include "base/problem.h"
-#include "base/trajectory_base.h"
-#include "base/frame_base.h"
+#include "base/problem/problem.h"
+#include "base/trajectory/trajectory_base.h"
+#include "base/frame/frame_base.h"
#include <iostream>
@@ -31,10 +31,12 @@ struct DummyNotificationProcessor
FAIL() << "problem_ is nullptr !";
}
- auto sb_noti_pair = problem_->getStateBlockNotificationMap().begin();
- while (sb_noti_pair != problem_->getStateBlockNotificationMap().end())
+ auto sb_noti_map = problem_->consumeStateBlockNotificationMap();
+ ASSERT_TRUE(problem_->consumeStateBlockNotificationMap().empty()); // consume should empty the notification map
+
+ while (!sb_noti_map.empty())
{
- switch (sb_noti_pair->second)
+ switch (sb_noti_map.begin()->second)
{
case ADD:
{
@@ -47,9 +49,9 @@ struct DummyNotificationProcessor
default:
throw std::runtime_error("SolverManager::update: State Block notification must be ADD or REMOVE.");
}
- sb_noti_pair = problem_->getStateBlockNotificationMap().erase(sb_noti_pair);
+ sb_noti_map.erase(sb_noti_map.begin());
}
- ASSERT_TRUE(problem_->getStateBlockNotificationMap().empty());
+ ASSERT_TRUE(sb_noti_map.empty());
}
ProblemPtr problem_;
@@ -126,24 +128,20 @@ TEST(TrajectoryBase, Add_Remove_Frame)
f1->link(T);
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 1);
- ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
+ ASSERT_EQ(P->consumeStateBlockNotificationMap(). size(), (unsigned int) 2);
std::cout << __LINE__ << std::endl;
// T->addFrame(f2); // KF
f2->link(T);
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 4);
- ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
std::cout << __LINE__ << std::endl;
// T->addFrame(f3); // F
f3->link(T);
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 3);
- ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 4);
- ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
+ ASSERT_EQ(P->consumeStateBlockNotificationMap(). size(), (unsigned int) 2); // consumeStateBlockNotificationMap empties the notification map, 2 state blocks were notified since last call to consumeStateBlockNotificationMap
std::cout << __LINE__ << std::endl;
ASSERT_EQ(T->getLastFrame()->id(), f3->id());
@@ -151,14 +149,14 @@ TEST(TrajectoryBase, Add_Remove_Frame)
std::cout << __LINE__ << std::endl;
N.update();
+ ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // consumeStateBlockNotificationMap was called in update() so it should be empty
std::cout << __LINE__ << std::endl;
// remove frames and keyframes
f2->remove(); // KF
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
+ ASSERT_EQ(P->consumeStateBlockNotificationMap(). size(), (unsigned int) 2); // consumeStateBlockNotificationMap empties the notification map, 2 state blocks were notified since last call to consumeStateBlockNotificationMap
std::cout << __LINE__ << std::endl;
ASSERT_EQ(T->getLastFrame()->id(), f3->id());
@@ -168,8 +166,6 @@ TEST(TrajectoryBase, Add_Remove_Frame)
f3->remove(); // F
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 1);
- ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
std::cout << __LINE__ << std::endl;
ASSERT_EQ(T->getLastKeyFrame()->id(), f1->id());
@@ -177,13 +173,11 @@ TEST(TrajectoryBase, Add_Remove_Frame)
f1->remove(); // KF
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 0);
- ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 0);
- ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
std::cout << __LINE__ << std::endl;
N.update();
- ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 0);
+ ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // consumeStateBlockNotificationMap was called in update() so it should be empty
std::cout << __LINE__ << std::endl;
}
diff --git a/test/processor_IMU_UnitTester.h b/test/processor_IMU_UnitTester.h
index c114086a16cae9cb28859270b3784461d7bb7773..2538981e0fcba72cd2c0b81901445363fd207a37 100644
--- a/test/processor_IMU_UnitTester.h
+++ b/test/processor_IMU_UnitTester.h
@@ -196,8 +196,8 @@ namespace wolf {
/////////////////////////////////////////////////////////
// Wolf
-#include "base/state_block.h"
-#include "base/rotations.h"
+#include "base/state_block/state_block.h"
+#include "base/math/rotations.h"
namespace wolf{
diff --git a/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp b/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp
index 8532cc9f2d32246ed536a340574251a6b34ce379..ae4a43a3bdd7733931da9b42fb87e279f529a7a2 100644
--- a/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp
+++ b/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp
@@ -5,7 +5,7 @@
* Author: Jeremie Deray
*/
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include "../../utils_gtest.h"
#include "../../../serialization/cereal/serialization_eigen_geometry.h"
diff --git a/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp b/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp
index 5ca60e7f111f5b9e56392bcf59af3123c29637b6..c7f7249a5747d2e848bf07ea892b8486ee1f2eaa 100644
--- a/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp
+++ b/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp
@@ -5,7 +5,7 @@
* Author: Jeremie Deray
*/
-#include "base/wolf.h"
+#include "base/common/wolf.h"
#include "../../utils_gtest.h"
#include "../../../serialization/cereal/serialization_eigen_sparse.h"
diff --git a/wolf_scripts/include_refactor.sh b/wolf_scripts/include_refactor.sh
index 6d2c2fd8d6fae80bb3c327ead22dfa4c097de20b..2801608bc3747dc71915205f13833c4a6ab36d23 100755
--- a/wolf_scripts/include_refactor.sh
+++ b/wolf_scripts/include_refactor.sh
@@ -1,17 +1,36 @@
#!/bin/bash
-for ff in $(find ~/workspace/wip/wolf/templinks/ -follow | cut -d '/' -f 8- | grep ".h$\|.cpp$"); do
- for f in $(cat ~/workspace/wip/wolf/files.txt); do
- path=$(ag -g /$f$ -l ~/workspace/wip/wolf/ | cut -d '/' -f 8-)
- matches=$(echo $path | wc -w)
- if [ $matches -gt 1 ]; then
- # echo $f " -> " $path
- path=$(echo $path | cut -d ' ' -f 1)
- fi
- # echo $f " now in -> " $path " modifying file "$ff
- # sed -i -E "s:(#include[[:space:]]+)."$f".:\1\""$path"\":gp" ~/workspace/wip/wolf/$ff
- sed -i -E "s:(#include[[:space:]]+).(\.\.\/)+(.+\/)+"$f".:\1\""$path"\":g" ~/workspace/wip/wolf/$ff
+for folder in problem hardware trajectory map frame state_block common math utils; do
+ for ff in $(find include/base/$folder src/$folder -type f); do
+ name=$(echo $ff | rev | cut -d '/' -f 1 | rev)
+ old="base/$name"
+ new="base/$folder/$name"
+ # echo "%%%%%%%%% "$ff " ¬¬ $name"
+ # echo "$old ºº $new"
+ # for target in $(find include/base src test -type f); do
+ for target in $(find hello_wolf -type f); do
+ # out=$(sed -E -n "s:$old:$new:gp" $target)
+ out=$(sed -i -E "s:$old:$new:g" $target)
+ if [[ $out ]]; then
+ echo ">>> changing : $old -> $new @ $target"
+ echo $out
+ fi
+ done
done
done
+
+# for ff in $(find ~/workspace/wip/wolf/templinks/ -follow | cut -d '/' -f 8- | grep ".h$\|.cpp$"); do
+# for f in $(cat ~/workspace/wip/wolf/files.txt); do
+# path=$(ag -g /$f$ -l ~/workspace/wip/wolf/ | cut -d '/' -f 8-)
+# matches=$(echo $path | wc -w)
+# if [ $matches -gt 1 ]; then
+# # echo $f " -> " $path
+# path=$(echo $path | cut -d ' ' -f 1)
+# fi
+# # echo $f " now in -> " $path " modifying file "$ff
+# # sed -i -E "s:(#include[[:space:]]+)."$f".:\1\""$path"\":gp" ~/workspace/wip/wolf/$ff
+# sed -i -E "s:(#include[[:space:]]+).(\.\.\/)+(.+\/)+"$f".:\1\""$path"\":g" ~/workspace/wip/wolf/$ff
+# done
+# done
# for f in $(cat ~/workspace/wip/wolf/files.txt); do
# path=$(ag -g /$f$ -l ~/workspace/wip/wolf/ | cut -d '/' -f 7-)
# matches=$(echo $path | wc -w)
diff --git a/wolf_scripts/substitution.sh b/wolf_scripts/substitution.sh
index 3085c1f82dbeb0a8e69be38df3e3115938fa7bd2..04ebdd6be9a348b77d7d94fc75af1725eb336cc1 100755
--- a/wolf_scripts/substitution.sh
+++ b/wolf_scripts/substitution.sh
@@ -10,7 +10,8 @@ for file in $(ag 'Ptr\(' . -o); do
# subs_line=${line}s/${subs}/${subs%List}PtrList/gp
# echo $subs_line
# sed -n -e $line's/Ptr\( \)*(\( \)*)/\1(\2)/gp' $target
- sed -i -e $line's/Ptr\( \)*(\( \)*)/\1(\2)/g' $target
+ # sed -n -e $line's/Ptr\( \)*(\( \)*)/\1(\2)/gp' $target
+ sed -i -e $line's/(/(/g' $target
done
# for file in $(ag -l -g constraint $folder); do