diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2fb76c6fa1af45a45ca824267f55b7898f57c449..b904e07222b0f64629797db2541adda052a0c9cd 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -197,6 +197,8 @@ IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
ENDIF()
# Configure config.h
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
+message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
+include_directories("${PROJECT_BINARY_DIR}/conf")
# include spdlog (logging library)
FIND_PATH(SPDLOG_INCLUDE_DIR spdlog.h /usr/local/include/spdlog /usr/include/spdlog)
IF (SPDLOG_INCLUDE_DIR)
@@ -207,7 +209,8 @@ ELSE (SPDLOG_INCLUDE_DIR)
ENDIF (SPDLOG_INCLUDE_DIR)
INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
-
+include_directories("include")
+include_directories("templinks")
IF(Ceres_FOUND)
INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
ENDIF(Ceres_FOUND)
@@ -283,8 +286,6 @@ include/base/constraint/constraint_pose_2D.h
include/base/constraint/constraint_pose_3D.h
include/base/constraint/constraint_quaternion_absolute.h
include/base/constraint/constraint_relative_2D_analytic.h
-src/sensor/diff_drive_tools.hpp
- temp/diff_drive_tools.hpp
include/base/feature/feature_corner_2D.h
include/base/feature/feature_GPS_fix.h
include/base/feature/feature_GPS_pseudorange.h
@@ -460,7 +461,6 @@ src/processor/processor_base.cpp
src/processor/processor_loopclosure_base.cpp
src/processor/processor_motion.cpp
src/processor/processor_tracker.cpp
-examples/test_processor_tracker_feature.cpp
src/sensor/sensor_base.cpp
src/state_block.cpp
src/time_stamp.cpp
@@ -471,7 +471,7 @@ src/trajectory_base.cpp
SET(SRCS_BASE
src/capture/capture_motion.cpp
src/processor/processor_capture_holder.cpp
-examples/test_processor_tracker_landmark.cpp
+# examples/test_processor_tracker_landmark.cpp
)
SET(SRCS
@@ -506,7 +506,7 @@ src/processor/processor_frame_nearest_neighbor_filter.cpp
src/processor/processor_diff_drive.cpp
src/processor/processor_IMU.cpp
src/processor/processor_odom_2D.cpp
-examples/test_processor_odom_3D.cpp
+src/processor/processor_odom_3D.cpp
src/processor/processor_tracker_feature_dummy.cpp
src/processor/processor_tracker_landmark_dummy.cpp
)
@@ -560,11 +560,14 @@ include/base/processor/processor_tracker_landmark_corner.h
include/base/processor/processor_tracker_landmark_polyline.h
)
SET(SRCS ${SRCS}
-examples/test_capture_laser_2D.cpp
+# examples/test_capture_laser_2D.cpp
src/sensor/sensor_laser_2D.cpp
+src/processor/processor_tracker_feature.cpp
src/processor/processor_tracker_feature_corner.cpp
src/processor/processor_tracker_landmark_corner.cpp
src/processor/processor_tracker_landmark_polyline.cpp
+src/processor/processor_tracker_landmark.cpp
+src/processor/processor_tracker_landmark_dummy.cpp
)
ENDIF(laser_scan_utils_FOUND)
@@ -600,7 +603,7 @@ src/capture/capture_image.cpp
src/feature/feature_point_image.cpp
src/landmark/landmark_AHP.cpp
src/processor/processor_tracker_feature_image.cpp
-examples/test_processor_tracker_landmark_image.cpp
+# examples/test_processor_tracker_landmark_image.cpp
)
SET(SRCS_LANDMARK ${SRCS_LANDMARK}
src/landmark/landmark_point_3D.cpp
@@ -611,9 +614,7 @@ src/processor/processor_tracker_feature_trifocal.cpp
ENDIF(vision_utils_FOUND)
#SUBDIRECTORIES
-
-ADD_SUBDIRECTORY(hello_wolf)
-
+add_subdirectory(hello_wolf)
IF (cereal_FOUND)
ADD_SUBDIRECTORY(serialization/cereal)
ENDIF(cereal_FOUND)
@@ -669,9 +670,8 @@ ADD_LIBRARY(${PROJECT_NAME}
${SRCS_SOLVER}
${SRCS_WRAPPER}
)
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${PROJECT_NAME}_core ${CMAKE_THREAD_LIBS_INIT})
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${PROJECT_NAME}_core)
#Link the created libraries
#=============================================================
IF (Ceres_FOUND)
@@ -700,6 +700,13 @@ ENDIF (YAMLCPP_FOUND)
IF (GLOG_FOUND)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY})
ENDIF (GLOG_FOUND)
+IF (GLOG_FOUND)
+ IF(BUILD_TESTS)
+ MESSAGE("Building tests.")
+ add_subdirectory(test)
+ ENDIF(BUILD_TESTS)
+ENDIF (GLOG_FOUND)
+
#-END_SRC --------------------------------------------------------------------------------------------------------------------------------
FIND_PACKAGE(Doxygen)
diff --git a/hello_wolf/capture_range_bearing.h b/hello_wolf/capture_range_bearing.h
index 6e18876cb415a91c1c0dcddf1e9047e563ed5f80..5a7e5a77100b8d98eba73a6f4d371340faf5c2a8 100644
--- a/hello_wolf/capture_range_bearing.h
+++ b/hello_wolf/capture_range_bearing.h
@@ -8,7 +8,7 @@
#ifndef HELLO_WOLF_CAPTURE_RANGE_BEARING_H_
#define HELLO_WOLF_CAPTURE_RANGE_BEARING_H_
-#include "capture_base.h"
+#include "base/capture/capture_base.h"
namespace wolf
{
diff --git a/hello_wolf/constraint_bearing.h b/hello_wolf/constraint_bearing.h
index 1ce1c7d783d84333fa8ae68894a86ab1aa565d42..7849f17d4a2dc3847e49f019cc6f538f859cf6db 100644
--- a/hello_wolf/constraint_bearing.h
+++ b/hello_wolf/constraint_bearing.h
@@ -8,7 +8,7 @@
#ifndef HELLO_WOLF_CONSTRAINT_BEARING_H_
#define HELLO_WOLF_CONSTRAINT_BEARING_H_
-#include "constraint_autodiff.h"
+#include "base/constraint/constraint_autodiff.h"
namespace wolf
{
@@ -46,7 +46,6 @@ class ConstraintBearing : public ConstraintAutodiff<ConstraintBearing, 1, 2, 1,
} /* namespace wolf */
-
//////////////// IMPLEMENTATION //////////////////////////////////////////
namespace wolf
diff --git a/hello_wolf/constraint_range_bearing.h b/hello_wolf/constraint_range_bearing.h
index 89b5137862da7ffe4e12fac7b7aa5942bda2968c..cf7af72a4574f0006c68c582e85bb151b81f9bf0 100644
--- a/hello_wolf/constraint_range_bearing.h
+++ b/hello_wolf/constraint_range_bearing.h
@@ -8,7 +8,7 @@
#ifndef HELLO_WOLF_CONSTRAINT_RANGE_BEARING_H_
#define HELLO_WOLF_CONSTRAINT_RANGE_BEARING_H_
-#include "constraint_autodiff.h"
+#include "base/constraint/constraint_autodiff.h"
namespace wolf
{
@@ -60,7 +60,6 @@ class ConstraintRangeBearing : public ConstraintAutodiff<ConstraintRangeBearing,
} /* namespace wolf */
-
////////////// IMPLEMENTATION //////////////////////////////////
namespace wolf
diff --git a/hello_wolf/feature_range_bearing.cpp b/hello_wolf/feature_range_bearing.cpp
index ce35c61f318dfa5e78d48d28e135f16486e9b559..3be9365676824e14689e63a6f969f33123736fb6 100644
--- a/hello_wolf/feature_range_bearing.cpp
+++ b/hello_wolf/feature_range_bearing.cpp
@@ -16,7 +16,6 @@ FeatureRangeBearing::FeatureRangeBearing(const Eigen::VectorXs& _measurement, co
//
}
-
FeatureRangeBearing::~FeatureRangeBearing()
{
//
diff --git a/hello_wolf/feature_range_bearing.h b/hello_wolf/feature_range_bearing.h
index e3d30bba2b08c6bba72a8a83808dcdba2b1a4631..9da7215a59095a1d96a4f9e4392e391d4b048513 100644
--- a/hello_wolf/feature_range_bearing.h
+++ b/hello_wolf/feature_range_bearing.h
@@ -8,7 +8,7 @@
#ifndef HELLO_WOLF_FEATURE_RANGE_BEARING_H_
#define HELLO_WOLF_FEATURE_RANGE_BEARING_H_
-#include "feature_base.h"
+#include "base/feature/feature_base.h"
namespace wolf
{
diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index 51ef873b3f277175296a249558b336e64fa4f44c..646f1c2d6a6254b28748adbd5fe1933701857842 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/hello_wolf/hello_wolf.cpp
@@ -12,11 +12,10 @@
* \author: jsola
*/
+#include "base/wolf.h"
-#include "wolf.h"
-
-#include "sensor_odom_2D.h"
-#include "processor_odom_2D.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/processor/processor_odom_2D.h"
#include "sensor_range_bearing.h"
#include "processor_range_bearing.h"
#include "capture_range_bearing.h"
@@ -24,24 +23,21 @@
#include "constraint_range_bearing.h"
#include "landmark_point_2D.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/ceres_wrapper/ceres_manager.h"
int main()
{
/*
* ============= PROBLEM DEFINITION ==================
*
- *
* We have a planar robot with a range-and-bearing sensor 'S' mounted at its front-left corner, looking forward:
*
* ^ Y
* |
* ------------------S-> sensor at location (1,1) and orientation 0 degrees, that is, at pose (1,1,0).
* | | |
- * | | |
* | +--------|--> X robot axes X, Y
* | |
- * | |
* -------------------
*
* The robot performs a straight trajectory with 3 keyframes 'KF', and observes 3 landmarks 'L'.
@@ -62,7 +58,6 @@ int main()
* KF1->---KF2->---KF3-> KEYFRAMES -- robot poses
* (0,0,0) (1,0,0) (2,0,0) keyframe poses in world frame
* |
- * |
* * prior Initial robot pose in world frame
* (0,0,0)
*
@@ -82,7 +77,6 @@ int main()
* - Observations have ranges 1 or sqrt(2)
* - Observations have bearings pi/2 or 3pi/4
*
- *
* The robot starts at (0,0,0) with a map with no previously known landmarks.
* At each keyframe, it does:
* - Create a motion factor to the previous keyframe
@@ -143,7 +137,6 @@ int main()
params_rb->time_tolerance = 0.01;
ProcessorBasePtr processor_rb = problem->installProcessor("RANGE BEARING", "processor RB", sensor_rb, params_rb);
-
// SELF CALIBRATION ===================================================
// NOTE: SELF-CALIBRATION OF SENSOR ORIENTATION
@@ -154,7 +147,6 @@ int main()
// The position is however not observable, and thus self-calibration would not work. You can try uncommenting it too.
// sensor_rb->getPPtr()->unfix();
-
// CONFIGURE ==========================================================
// Motion data
@@ -165,7 +157,6 @@ int main()
VectorXi ids;
VectorXs ranges, bearings;
-
// SET OF EVENTS =======================================================
std::cout << std::endl;
WOLF_TRACE("======== BUILD PROBLEM =======")
@@ -219,7 +210,6 @@ int main()
sensor_rb ->process(cap_rb); // L1 : (1,2), L2 : (2,2), L3 : (3,2)
problem->print(1,0,1,0);
-
// SOLVE ================================================================
// SOLVE with exact initial guess
@@ -283,7 +273,6 @@ int main()
*
*/
-
/*
* ============= DETAILED DESCRIPTION OF THE PRINTED RESULT ==================
*
diff --git a/hello_wolf/landmark_point_2D.h b/hello_wolf/landmark_point_2D.h
index cb588f8eb7eeb223aa31506379098a6a8d9df715..1bd845763a3506b4428d13b7da97de5a70ffca08 100644
--- a/hello_wolf/landmark_point_2D.h
+++ b/hello_wolf/landmark_point_2D.h
@@ -8,7 +8,7 @@
#ifndef HELLO_WOLF_LANDMARK_POINT_2D_H_
#define HELLO_WOLF_LANDMARK_POINT_2D_H_
-#include "landmark_base.h"
+#include "base/landmark/landmark_base.h"
namespace wolf
{
diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp
index 4f8e0dabf7c410408bfb12e7167c40aab4973402..670a61ea61917ef371a6b5a7f31687dcc7a18a0e 100644
--- a/hello_wolf/processor_range_bearing.cpp
+++ b/hello_wolf/processor_range_bearing.cpp
@@ -161,9 +161,8 @@ Eigen::Vector2s ProcessorRangeBearing::rect(Scalar range, Scalar bearing) const
} /* namespace wolf */
-
// Register in the SensorFactory
-#include "processor_factory.h"
+#include "base/processor/processor_factory.h"
namespace wolf
{
WOLF_REGISTER_PROCESSOR("RANGE BEARING", ProcessorRangeBearing)
diff --git a/hello_wolf/processor_range_bearing.h b/hello_wolf/processor_range_bearing.h
index 5d3d2a8dd83b2390c204d2a0d58e9239ca233548..6e8c77cd694ac807960c16b8f2fe4e3a7005e4a8 100644
--- a/hello_wolf/processor_range_bearing.h
+++ b/hello_wolf/processor_range_bearing.h
@@ -8,7 +8,7 @@
#ifndef HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_
#define HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_
-#include "processor_base.h"
+#include "base/processor/processor_base.h"
#include "sensor_range_bearing.h"
#include "Eigen/Geometry"
@@ -24,7 +24,6 @@ struct ProcessorParamsRangeBearing : public ProcessorParamsBase
// We do not need special parameters, but in case you need they should be defined here.
};
-
using namespace Eigen;
WOLF_PTR_TYPEDEFS(ProcessorRangeBearing);
diff --git a/hello_wolf/sensor_range_bearing.cpp b/hello_wolf/sensor_range_bearing.cpp
index d7de5aa0b79c17fec3dc86a6a0d293e14bf35a96..04d704664e8023826fb0b3faccde05a585a7b47a 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 "state_angle.h"
+#include "base/state_angle.h"
namespace wolf
{
@@ -28,7 +28,6 @@ SensorRangeBearing::~SensorRangeBearing()
//
}
-
SensorBasePtr SensorRangeBearing::create(const std::string& _unique_name, //
const Eigen::VectorXs& _extrinsics, //
const IntrinsicsBasePtr _intrinsics)
@@ -45,12 +44,10 @@ SensorBasePtr SensorRangeBearing::create(const std::string& _unique_name, //
} /* namespace wolf */
-
// Register in the SensorFactory
-#include "sensor_factory.h"
+#include "base/sensor/sensor_factory.h"
namespace wolf
{
WOLF_REGISTER_SENSOR("RANGE BEARING", SensorRangeBearing)
} // namespace wolf
-
diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h
index 3e4e7c7441c56906506ab63a896f3ce68913a74a..01ac8d93e50c2795a5a0a702e917238f4ddb27e8 100644
--- a/hello_wolf/sensor_range_bearing.h
+++ b/hello_wolf/sensor_range_bearing.h
@@ -8,7 +8,7 @@
#ifndef HELLO_WOLF_SENSOR_RANGE_BEARING_H_
#define HELLO_WOLF_SENSOR_RANGE_BEARING_H_
-#include "sensor_base.h"
+#include "base/sensor/sensor_base.h"
namespace wolf
{
@@ -20,8 +20,6 @@ struct IntrinsicsRangeBearing : public IntrinsicsBase
Scalar noise_bearing_degrees_std = 0.5;
};
-
-
WOLF_PTR_TYPEDEFS(SensorRangeBearing)
class SensorRangeBearing : public SensorBase
diff --git a/include/base/IMU_tools.h b/include/base/IMU_tools.h
index 6d231223554ec4ea5e196cb7220a40e0eb42c001..eecad244c7a57ee2868bcf464e951a0a5108160d 100644
--- a/include/base/IMU_tools.h
+++ b/include/base/IMU_tools.h
@@ -8,9 +8,8 @@
#ifndef IMU_TOOLS_H_
#define IMU_TOOLS_H_
-
-#include "wolf.h"
-#include "rotations.h"
+#include "base/wolf.h"
+#include "base/rotations.h"
/*
* Most functions in this file are explained in the document:
@@ -41,8 +40,6 @@
* - body2delta: construct a delta from body magnitudes of linAcc and angVel
*/
-
-
namespace wolf
{
namespace imu {
@@ -110,7 +107,6 @@ inline void inverse(const MatrixBase<D1>& d,
inverse(dp, dq, dv, dt, idp, idq, idv);
}
-
template<typename D, class T>
inline Matrix<typename D::Scalar, 10, 1> inverse(const MatrixBase<D>& d,
T dt)
@@ -247,7 +243,6 @@ inline void between(const MatrixBase<D1>& d1,
between(dp1, dq1, dv1, dp2, dq2, dv2, dt, diff_p, diff_q, diff_v);
}
-
template<typename D1, typename D2, class T>
inline Matrix<typename D1::Scalar, 10, 1> between(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
@@ -445,7 +440,6 @@ inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const
J_do_dq2 = jac_SO3_right_inv(diff_o);
}
-
template<typename D1, typename D2, typename D3>
inline void diff(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
@@ -511,7 +505,6 @@ inline Matrix<typename D1::Scalar, 9, 1> diff(const MatrixBase<D1>& d1,
return ret;
}
-
template<typename D1, typename D2, typename D3, typename D4, typename D5>
inline void body2delta(const MatrixBase<D1>& a,
const MatrixBase<D2>& w,
@@ -611,8 +604,6 @@ Matrix<typename D1::Scalar, 6, 1> motion2data(const MatrixBase<D1>& motion, cons
return data;
}
-
-
} // namespace imu
} // namespace wolf
diff --git a/include/base/association/association_nnls.h b/include/base/association/association_nnls.h
index 0883a729254a1b42836262c0d561c8f4ea352767..6563a1f66a513f6359200813e095e446e91327fc 100644
--- a/include/base/association/association_nnls.h
+++ b/include/base/association/association_nnls.h
@@ -7,8 +7,7 @@
#include <vector>
//pipol tracker
-#include "association_solver.h"
-
+#include "base/association/association_solver.h"
namespace wolf
{
diff --git a/include/base/association/association_node.h b/include/base/association/association_node.h
index cc4422e81ef922057e1eb6200229a8266a22d78c..21a10d4801bc3aea1487c39e5357e08f86552700 100644
--- a/include/base/association/association_node.h
+++ b/include/base/association/association_node.h
@@ -1,5 +1,4 @@
-
-
+
#ifndef association_node_H
#define association_node_H
@@ -10,7 +9,7 @@
#include <algorithm> //find()
//pipol tracker
-#include "matrix.h"
+#include "base/association/matrix.h"
//constants
const double PROB_ZERO_ = 1e-3;
diff --git a/include/base/association/association_solver.h b/include/base/association/association_solver.h
index 24efc159b01a944f59dd37964165846f323e0123..6ac986e073d7419514e174301426ad83b9e7b4d5 100644
--- a/include/base/association/association_solver.h
+++ b/include/base/association/association_solver.h
@@ -7,7 +7,7 @@
#include <vector>
//matrix class
-#include "matrix.h"
+#include "base/association/matrix.h"
namespace wolf
{
diff --git a/include/base/association/association_tree.h b/include/base/association/association_tree.h
index 29ae34a2c0537130fccd7500d19f00a39432fd44..b6566a60459986d19434492b1f5603b78fd40fe9 100644
--- a/include/base/association/association_tree.h
+++ b/include/base/association/association_tree.h
@@ -9,12 +9,11 @@
//#include <memory>
//pipol tracker
-#include "matrix.h"
-#include "association_solver.h"
-#include "association_node.h"
+#include "base/association/matrix.h"
+#include "base/association/association_solver.h"
+#include "base/association/association_node.h"
#include <map>
-
namespace wolf
{
diff --git a/src/association/matrix.h b/include/base/association/matrix.h
similarity index 99%
rename from src/association/matrix.h
rename to include/base/association/matrix.h
index 10031b131908c199e4ebc71d62fea8e6b6b6ac87..c76bba51e4de2db56a380e48e7002a16bba78bcc 100644
--- a/src/association/matrix.h
+++ b/include/base/association/matrix.h
@@ -7,7 +7,6 @@
#include <vector>
#include <assert.h> //assert
-
template <typename T>
class Matrixx
{
diff --git a/include/base/capture/capture_GPS.h b/include/base/capture/capture_GPS.h
index 9466195277363af43a46b4654e25fd9ab15f1a61..5f5088bd6dbf5e9674bcd12c3c13caa6a213c8e5 100644
--- a/include/base/capture/capture_GPS.h
+++ b/include/base/capture/capture_GPS.h
@@ -1,10 +1,9 @@
#ifndef CAPTURE_GPS_H_
#define CAPTURE_GPS_H_
-
// Wolf includes
#include "raw_gps_utils/satellites_obs.h"
-#include "capture_base.h"
+#include "base/capture/capture_base.h"
namespace wolf {
diff --git a/include/base/capture/capture_GPS_fix.h b/include/base/capture/capture_GPS_fix.h
index 6f65b733b3bd1b6beb396900f269f4023d5a68ae..8cb4afbf4cc340a09693ecf2e426cf747df5b058 100644
--- a/include/base/capture/capture_GPS_fix.h
+++ b/include/base/capture/capture_GPS_fix.h
@@ -2,8 +2,8 @@
#define CAPTURE_GPS_FIX_H_
//Wolf includes
-#include "feature_GPS_fix.h"
-#include "capture_base.h"
+#include "base/feature/feature_GPS_fix.h"
+#include "base/capture/capture_base.h"
//std includes
//
diff --git a/include/base/capture/capture_IMU.h b/include/base/capture/capture_IMU.h
index 01c1f5aabaac75985e561c0f1a817924e8d428ce..6e2de9d9d91eee27afb6b46940f2c5558ee0fddb 100644
--- a/include/base/capture/capture_IMU.h
+++ b/include/base/capture/capture_IMU.h
@@ -2,8 +2,8 @@
#define CAPTURE_IMU_H
//Wolf includes
-#include "IMU_tools.h"
-#include "capture_motion.h"
+#include "base/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 9da734e64f3b60ed580c69b4f970cf650b9cf616..293ab9d0303aa493930ae11336b7abfe58d7d0f9 100644
--- a/include/base/capture/capture_base.h
+++ b/include/base/capture/capture_base.h
@@ -8,9 +8,9 @@ class FeatureBase;
}
//Wolf includes
-#include "wolf.h"
-#include "node_base.h"
-#include "time_stamp.h"
+#include "base/wolf.h"
+#include "base/node_base.h"
+#include "base/time_stamp.h"
//std includes
@@ -109,10 +109,10 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
}
-#include "sensor_base.h"
-#include "frame_base.h"
-#include "feature_base.h"
-#include "state_block.h"
+#include "base/sensor/sensor_base.h"
+#include "base/frame_base.h"
+#include "base/feature/feature_base.h"
+#include "base/state_block.h"
namespace wolf{
@@ -148,7 +148,6 @@ inline void CaptureBase::setStateBlockPtr(unsigned int _i, const StateBlockPtr _
state_block_vec_[_i] = _sb_ptr;
}
-
inline StateBlockPtr CaptureBase::getSensorPPtr() const
{
return getStateBlockPtr(0);
@@ -164,7 +163,6 @@ inline StateBlockPtr CaptureBase::getSensorIntrinsicPtr() const
return getStateBlockPtr(2);
}
-
inline unsigned int CaptureBase::id()
{
return capture_id_;
@@ -195,7 +193,6 @@ inline ConstraintBaseList& CaptureBase::getConstrainedByList()
return constrained_by_list_;
}
-
inline TimeStamp CaptureBase::getTimeStamp() const
{
return time_stamp_;
diff --git a/include/base/capture/capture_buffer.h b/include/base/capture/capture_buffer.h
index 13cb1b9d0e507f497274407bd5867db2ff7378fc..434716dcfdf596b09d037a62f136428bc9d3e893 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 "wolf.h"
-#include "time_stamp.h"
+#include "base/wolf.h"
+#include "base/time_stamp.h"
#include <list>
#include <algorithm>
@@ -79,7 +79,6 @@ public:
std::list<CaptureBasePtr> container_;
};
-
CaptureBuffer::CaptureBuffer(const Scalar _buffer_dt, const int _max_capture) :
max_capture_(_max_capture), buffer_dt_(_buffer_dt)
{
diff --git a/include/base/capture/capture_image.h b/include/base/capture/capture_image.h
index ca10a873ffe10e49e767a4265cfea15ddf229e7d..4e9771cf08808342e9339dfe9c3f108e49aae542 100644
--- a/include/base/capture/capture_image.h
+++ b/include/base/capture/capture_image.h
@@ -2,9 +2,9 @@
#define CAPTURE_IMAGE_H
//Wolf includes
-#include "capture_base.h"
-#include "feature_point_image.h"
-#include "sensor_camera.h"
+#include "base/capture/capture_base.h"
+#include "base/feature/feature_point_image.h"
+#include "base/sensor/sensor_camera.h"
// Vision Utils includes
#include "vision_utils/vision_utils.h"
diff --git a/include/base/capture/capture_laser_2D.h b/include/base/capture/capture_laser_2D.h
index f1cbf9eae7810b1e70341de0bec2709d524271fc..9abd6bf4b874a8970d71929355aed095e52c3838 100644
--- a/include/base/capture/capture_laser_2D.h
+++ b/include/base/capture/capture_laser_2D.h
@@ -8,8 +8,8 @@ class SensorLaser2D;
}
//wolf includes
-#include "capture_base.h"
-#include "sensor_laser_2D.h"
+#include "base/capture/capture_base.h"
+#include "base/sensor/sensor_laser_2D.h"
//laserscanutils includes
#include "laser_scan_utils/laser_scan.h"
@@ -18,7 +18,6 @@ namespace wolf {
WOLF_PTR_TYPEDEFS(CaptureLaser2D);
-
class CaptureLaser2D : public CaptureBase
{
public:
diff --git a/include/base/capture/capture_motion.h b/include/base/capture/capture_motion.h
index a07ddc8754d21e9e43e2f67a29fd354d79fd4ca9..b867d2ec16332223ee87fbdfb2468150333abc43 100644
--- a/include/base/capture/capture_motion.h
+++ b/include/base/capture/capture_motion.h
@@ -9,8 +9,8 @@
#define SRC_CAPTURE_MOTION_H_
// Wolf includes
-#include "capture_base.h"
-#include "motion_buffer.h"
+#include "base/capture/capture_base.h"
+#include "base/motion_buffer.h"
// STL includes
#include <list>
diff --git a/include/base/capture/capture_odom_2D.h b/include/base/capture/capture_odom_2D.h
index 208f99fa4148e273833945d27b590a27cdc8a9b5..eafb6ead7bbe4b12f07234dc896f8fe7729b07bf 100644
--- a/include/base/capture/capture_odom_2D.h
+++ b/include/base/capture/capture_odom_2D.h
@@ -8,9 +8,9 @@
#ifndef CAPTURE_ODOM_2D_H_
#define CAPTURE_ODOM_2D_H_
-#include "capture_motion.h"
+#include "base/capture/capture_motion.h"
-#include "rotations.h"
+#include "base/rotations.h"
namespace wolf
{
@@ -44,8 +44,6 @@ inline Eigen::VectorXs CaptureOdom2D::correctDelta(const VectorXs& _delta, const
return delta;
}
-
-
} /* namespace wolf */
#endif /* CAPTURE_ODOM_2D_H_ */
diff --git a/include/base/capture/capture_odom_3D.h b/include/base/capture/capture_odom_3D.h
index 21fee194376afe8205984ae07ad4a9c35a2a4df4..d1f29508bb60fa7bae5c620b7b010f368e00f100 100644
--- a/include/base/capture/capture_odom_3D.h
+++ b/include/base/capture/capture_odom_3D.h
@@ -8,9 +8,9 @@
#ifndef CAPTURE_ODOM_3D_H_
#define CAPTURE_ODOM_3D_H_
-#include "capture_motion.h"
+#include "base/capture/capture_motion.h"
-#include "rotations.h"
+#include "base/rotations.h"
namespace wolf
{
@@ -39,5 +39,4 @@ class CaptureOdom3D : public CaptureMotion
} /* namespace wolf */
-
#endif /* CAPTURE_ODOM_3D_H_ */
diff --git a/include/base/capture/capture_pose.h b/include/base/capture/capture_pose.h
index 9d47d39c98976458bff882e1770b7f03ee1357a5..51fb833e43a8f525fefa559be72e99fa7906549c 100644
--- a/include/base/capture/capture_pose.h
+++ b/include/base/capture/capture_pose.h
@@ -2,10 +2,10 @@
#define CAPTURE_POSE_H_
//Wolf includes
-#include "capture_base.h"
-#include "constraint_pose_2D.h"
-#include "constraint_pose_3D.h"
-#include "feature_pose.h"
+#include "base/capture/capture_base.h"
+#include "base/constraint/constraint_pose_2D.h"
+#include "base/constraint/constraint_pose_3D.h"
+#include "base/feature/feature_pose.h"
//std includes
//
diff --git a/include/base/capture/capture_velocity.h b/include/base/capture/capture_velocity.h
index 55b1bcf1aa596c83efc797b552fa03861a84f6c9..4853fce2ac370b86e88c2d4776c514a3f4315455 100644
--- a/include/base/capture/capture_velocity.h
+++ b/include/base/capture/capture_velocity.h
@@ -9,7 +9,7 @@
#define _WOLF_CAPTURE_VELOCITY_H_
//wolf includes
-#include "capture_motion.h"
+#include "base/capture/capture_motion.h"
namespace wolf {
diff --git a/include/base/capture/capture_void.h b/include/base/capture/capture_void.h
index 50b3b3c7d55d87ae0d4c17d350e12ab6c90ceb54..b6b7a755ddd0a6044c28143988bb820b3337d9c7 100644
--- a/include/base/capture/capture_void.h
+++ b/include/base/capture/capture_void.h
@@ -2,8 +2,7 @@
#define CAPTURE_VOID_H_
//Wolf includes
-#include "capture_base.h"
-
+#include "base/capture/capture_base.h"
namespace wolf {
diff --git a/include/base/capture/capture_wheel_joint_position.h b/include/base/capture/capture_wheel_joint_position.h
index 02829cd1a89414652cd7f1cff05b768d51ee252f..0bade4029f499ba78b6cc71a7f260f05df44a79e 100644
--- a/include/base/capture/capture_wheel_joint_position.h
+++ b/include/base/capture/capture_wheel_joint_position.h
@@ -9,7 +9,7 @@
#define CAPTURE_WHEEL_JOINT_POSITION_H_
//wolf includes
-#include "../capture_motion.h"
+#include "base/capture/capture_motion.h"
namespace wolf {
@@ -60,7 +60,6 @@ protected:
Eigen::MatrixXs positions_cov_;
};
-
/// @todo Enforce some logic on the wheel joint pos data
//template <typename E>
@@ -178,7 +177,6 @@ protected:
//using CaptureDiffDriveWheelJointPosition = CaptureWheelJointPosition<DiffDriveController>;
-
} // namespace wolf
#endif /* CAPTURE_WHEEL_JOINT_POSITION_H_ */
diff --git a/include/base/ceres_wrapper/ceres_manager.h b/include/base/ceres_wrapper/ceres_manager.h
index 8852bc06d3a0fe24fa2b760aa3766cb2e24cbf66..5eb3436f55bf2ce49f814cd6e26302b0f90a9796 100644
--- a/include/base/ceres_wrapper/ceres_manager.h
+++ b/include/base/ceres_wrapper/ceres_manager.h
@@ -7,10 +7,10 @@
#include "glog/logging.h"
//wolf includes
-#include "../solver/solver_manager.h"
-#include "cost_function_wrapper.h"
+#include "base/solver/solver_manager.h"
+#include "base/ceres_wrapper/cost_function_wrapper.h"
#include "local_parametrization_wrapper.h"
-#include "create_numeric_diff_cost_function.h"
+#include "base/ceres_wrapper/create_numeric_diff_cost_function.h"
namespace ceres {
typedef std::shared_ptr<CostFunction> CostFunctionPtr;
diff --git a/include/base/ceres_wrapper/cost_function_wrapper.h b/include/base/ceres_wrapper/cost_function_wrapper.h
index ffd88191475c23ae409651baedb0099f3aaa4064..119839dee31898d2efcf06782e2c2c4b7df6d4f5 100644
--- a/include/base/ceres_wrapper/cost_function_wrapper.h
+++ b/include/base/ceres_wrapper/cost_function_wrapper.h
@@ -2,8 +2,8 @@
#define TRUNK_SRC_COST_FUNCTION_WRAPPER_H_
// WOLF
-#include "../wolf.h"
-#include "../constraint_analytic.h"
+#include "base/wolf.h"
+#include "base/constraint/constraint_analytic.h"
// CERES
#include "ceres/cost_function.h"
@@ -56,6 +56,4 @@ inline ConstraintBasePtr CostFunctionWrapper::getConstraintPtr() const
} // namespace wolf
-
-
#endif /* TRUNK_SRC_COST_FUNCTION_WRAPPER_H_ */
diff --git a/include/base/ceres_wrapper/create_numeric_diff_cost_function.h b/include/base/ceres_wrapper/create_numeric_diff_cost_function.h
index d6650b20397a11bec5b1e57f707e95b51c2dc21d..8f8e9686b753c8912842b311cb9186010fad36f1 100644
--- a/include/base/ceres_wrapper/create_numeric_diff_cost_function.h
+++ b/include/base/ceres_wrapper/create_numeric_diff_cost_function.h
@@ -12,9 +12,8 @@
#include "ceres/numeric_diff_cost_function.h"
// Constraints
-#include "../constraint_odom_2D.h"
-#include "../constraint_base.h"
-
+#include "base/constraint/constraint_odom_2D.h"
+#include "base/constraint/constraint_base.h"
namespace wolf {
@@ -29,7 +28,6 @@ std::shared_ptr<ceres::NumericDiffCostFunction<T, ceres::CENTRAL, T::residualSiz
T::block5Size,T::block6Size,T::block7Size,T::block8Size,T::block9Size> >(std::static_pointer_cast<T>(_constraint_ptr).get());
};
-
inline std::shared_ptr<ceres::CostFunction> createNumericDiffCostFunction(ConstraintBasePtr _ctr_ptr)
{
// switch (_ctr_ptr->getTypeId())
diff --git a/include/base/ceres_wrapper/local_parametrization_wrapper.h b/include/base/ceres_wrapper/local_parametrization_wrapper.h
index d9e0d9e82671ceb232e47760e2f74a119304ff3e..4ae4182cc525ee5224d74c8287a9b5b6a9189b6f 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 "../wolf.h"
+#include "base/wolf.h"
#include "ceres/ceres.h"
namespace wolf {
@@ -38,7 +38,7 @@ using LocalParametrizationWrapperPtr = std::shared_ptr<LocalParametrizationWrapp
} // namespace wolf
-#include "../local_parametrization_base.h"
+#include "base/local_parametrization_base.h"
namespace wolf {
diff --git a/include/base/ceres_wrapper/qr_manager.h b/include/base/ceres_wrapper/qr_manager.h
index ae7fae7a1fa475e7521bd94438e7e351be24d42b..d09a5147b96a63bab51c72cbf69316e4326d0723 100644
--- a/include/base/ceres_wrapper/qr_manager.h
+++ b/include/base/ceres_wrapper/qr_manager.h
@@ -8,8 +8,8 @@
#ifndef SRC_CERES_WRAPPER_QR_MANAGER_H_
#define SRC_CERES_WRAPPER_QR_MANAGER_H_
-#include "solver_manager.h"
-#include "sparse_utils.h"
+#include "base/solver/solver_manager.h"
+#include "base/solver_suitesparse/sparse_utils.h"
namespace wolf
{
diff --git a/include/base/ceres_wrapper/solver_manager.h b/include/base/ceres_wrapper/solver_manager.h
index f7725df4ab188d0ef9f15245ff7a5dbd48a9ec91..6ffda3f7c32763c6896a7e5cb2e4c90f8bffce92 100644
--- a/include/base/ceres_wrapper/solver_manager.h
+++ b/include/base/ceres_wrapper/solver_manager.h
@@ -2,9 +2,9 @@
#define SOLVER_MANAGER_H_
//wolf includes
-#include "../wolf.h"
-#include "../state_block.h"
-#include "../constraint_base.h"
+#include "base/wolf.h"
+#include "base/state_block.h"
+#include "base/constraint/constraint_base.h"
namespace wolf {
diff --git a/include/base/constraint/constraint_AHP.h b/include/base/constraint/constraint_AHP.h
index a97ac9840b29f40c3638289c90b39614669c8856..340acc9c3d30c90ba1a74c1e06be6b880bcab08a 100644
--- a/include/base/constraint/constraint_AHP.h
+++ b/include/base/constraint/constraint_AHP.h
@@ -2,11 +2,11 @@
#define CONSTRAINT_AHP_H
//Wolf includes
-#include "constraint_autodiff.h"
-#include "landmark_AHP.h"
-#include "sensor_camera.h"
-//#include "feature_point_image.h"
-#include "pinhole_tools.h"
+#include "base/constraint/constraint_autodiff.h"
+#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 <iomanip> //setprecision
@@ -68,7 +68,6 @@ inline ConstraintAHP::ConstraintAHP(const FeatureBasePtr& _ftr_ptr,
ConstraintAutodiff<ConstraintAHP, 2, 3, 4, 3, 4, 4>("AHP",
_landmark_ptr->getAnchorFrame(),
nullptr,
- nullptr,
_landmark_ptr,
_processor_ptr,
_apply_loss_function,
@@ -115,7 +114,6 @@ inline void ConstraintAHP::expectation(const T* const _current_frame_p,
{
using namespace Eigen;
-
// All involved transforms typedef
typedef Eigen::Transform<T, 3, Eigen::Affine> TransformType;
@@ -198,5 +196,4 @@ inline ConstraintAHPPtr ConstraintAHP::create(const FeatureBasePtr& _ftr_ptr,
} // namespace wolf
-
#endif // CONSTRAINT_AHP_H
diff --git a/include/base/constraint/constraint_GPS_2D.h b/include/base/constraint/constraint_GPS_2D.h
index 5feb77fb97c19d71d48ad740bf387712ab8e169d..2f85d06631f3d09b5ba587c442238ba70e545242 100644
--- a/include/base/constraint/constraint_GPS_2D.h
+++ b/include/base/constraint/constraint_GPS_2D.h
@@ -3,9 +3,9 @@
#define CONSTRAINT_GPS_2D_H_
//Wolf includes
-#include "wolf.h"
-#include "constraint_autodiff.h"
-#include "frame_base.h"
+#include "base/wolf.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/frame_base.h"
namespace wolf {
diff --git a/include/base/constraint/constraint_GPS_pseudorange_2D.h b/include/base/constraint/constraint_GPS_pseudorange_2D.h
index 7a04463b9c277bb3fb265fad1d0bd37cccfa06fc..3c15a0246738935058805680111b7f5b1f9dbc3c 100644
--- a/include/base/constraint/constraint_GPS_pseudorange_2D.h
+++ b/include/base/constraint/constraint_GPS_pseudorange_2D.h
@@ -4,9 +4,9 @@
#define LIGHT_SPEED_ 299792458
//Wolf includes
-#include "sensor_GPS.h"
-#include "feature_GPS_pseudorange.h"
-#include "constraint_autodiff.h"
+#include "base/sensor/sensor_GPS.h"
+#include "base/feature/feature_GPS_pseudorange.h"
+#include "base/constraint/constraint_autodiff.h"
//std
#include <string>
@@ -31,10 +31,6 @@ class ConstraintGPSPseudorange2D : public ConstraintAutodiff<ConstraintGPSPseudo
bool _apply_loss_function = false,
ConstraintStatus _status = CTR_ACTIVE) :
ConstraintAutodiff<ConstraintGPSPseudorange2D, 1, 2, 1, 3, 1, 3, 1>("GPS PR 2D",
- nullptr,
- nullptr,
- nullptr,
- nullptr,
nullptr,
_apply_loss_function,
_status,
@@ -98,7 +94,6 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c
//Filling Eigen vectors
Eigen::Matrix<T, 4, 1> sensor_p_base(_sensor_p[0], _sensor_p[1], _sensor_p[2], T(1)); //sensor position with respect base frame
-
/*
* Base-to-map transform matrix
*/
@@ -175,7 +170,6 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c
T_lon_lat(2, 0) = T(sin(lat));
T_lon_lat(2, 2) = T(cos(lat));
-
Eigen::Matrix<T, 4, 4> T_lat_enu = Eigen::Matrix<T, 4, 4>::Zero();
T_lat_enu(0, 2) = T_lat_enu(1, 0) = T_lat_enu(2, 1) = T_lat_enu(3, 3) = T(1);
@@ -190,7 +184,6 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c
//sensor position with respect to ecef coordinate system
Eigen::Matrix<T, 4, 1> sensor_p_ecef = T_ecef_map * sensor_p_map;
-
/*
* calculate the residual
*/
@@ -209,7 +202,6 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c
// normalizing by the covariance
_residual[0] = _residual[0] / T(getMeasurementCovariance()(0, 0));//T(sqrt(getMeasurementCovariance()(0, 0)));
-
if (verbose_level_ >= 1)
{
aux.str(std::string());
diff --git a/include/base/constraint/constraint_GPS_pseudorange_3D.h b/include/base/constraint/constraint_GPS_pseudorange_3D.h
index 4c44d262e7e2ac9cb0246420882e46c43be843b4..02323d2c2ff76d4fe45065ebff59317d8d5b51ad 100644
--- a/include/base/constraint/constraint_GPS_pseudorange_3D.h
+++ b/include/base/constraint/constraint_GPS_pseudorange_3D.h
@@ -4,9 +4,9 @@
#define LIGHT_SPEED 299792458
//Wolf includes
-#include "sensor_GPS.h"
-#include "feature_GPS_pseudorange.h"
-#include "constraint_autodiff.h"
+#include "base/sensor/sensor_GPS.h"
+#include "base/feature/feature_GPS_pseudorange.h"
+#include "base/constraint/constraint_autodiff.h"
namespace wolf {
@@ -45,7 +45,6 @@ class ConstraintGPSPseudorange3D: public ConstraintAutodiff<ConstraintGPSPseudor
//std::cout << "ConstraintGPSPseudorange3D() pr=" << pseudorange_ << "\tsat_pos=(" << sat_position_[0] << ", " << sat_position_[1] << ", " << sat_position_[2] << ")" << std::endl;
}
-
virtual ~ConstraintGPSPseudorange3D() = default;
template<typename T>
@@ -66,7 +65,6 @@ class ConstraintGPSPseudorange3D: public ConstraintAutodiff<ConstraintGPSPseudor
return std::make_shared<ConstraintGPSPseudorange3D>(_feature_ptr);
}
-
};
/*
@@ -115,7 +113,6 @@ inline bool ConstraintGPSPseudorange3D::operator ()(const T* const _vehicle_p, c
for (int i = 0; i < 3; ++i)
T_map_base(i, 3) = _vehicle_p[i];
-
/*
* Compute sensor_p wrt ECEF
*/
diff --git a/include/base/constraint/constraint_IMU.h b/include/base/constraint/constraint_IMU.h
index 9ccf54eebc59b1ef1f33ab1514fed028cafce056..87dd49f0f6e7f5e35fe036e91704936e0f836daf 100644
--- a/include/base/constraint/constraint_IMU.h
+++ b/include/base/constraint/constraint_IMU.h
@@ -2,14 +2,13 @@
#define CONSTRAINT_IMU_THETA_H_
//Wolf includes
-#include "feature_IMU.h"
-#include "sensor_IMU.h"
-#include "constraint_autodiff.h"
-#include "rotations.h"
+#include "base/feature/feature_IMU.h"
+#include "base/sensor/sensor_IMU.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/rotations.h"
//Eigen include
-
namespace wolf {
WOLF_PTR_TYPEDEFS(ConstraintIMU);
@@ -101,7 +100,6 @@ class ConstraintIMU : public ConstraintAutodiff<ConstraintIMU, 15, 3, 4, 3, 6, 3
*/
Eigen::VectorXs expectation() const;
-
private:
/// Preintegrated delta
Eigen::Vector3s dp_preint_;
@@ -183,8 +181,6 @@ inline ConstraintIMU::ConstraintIMU(const FeatureIMUPtr& _ftr_ptr,
//
}
-
-
template<typename T>
inline bool ConstraintIMU::operator ()(const T* const _p1,
const T* const _q1,
@@ -267,12 +263,10 @@ inline bool ConstraintIMU::residual(const Eigen::MatrixBase<D1> & _p1,
* results in :
* res = W.sqrt * ( ( diff ( D_preint , D_exp ) ) - J_preint * (b - b_preint) )
*
- *
* NOTE: See optimization report at the end of this file for comparisons of both methods.
*/
#define METHOD_1 // if commented, then METHOD_2 will be applied
-
//needed typedefs
typedef typename D1::Scalar T;
@@ -285,7 +279,6 @@ inline bool ConstraintIMU::residual(const Eigen::MatrixBase<D1> & _p1,
imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, (T)dt_, dp_exp, dq_exp, dv_exp);
-
// 2. Corrected integrated delta: delta_corr = delta_preint (+) J_bias * (bias_current - bias_preint)
// 2.a. Compute the delta step in tangent space: step = J_bias * (bias - bias_preint)
@@ -309,7 +302,6 @@ inline bool ConstraintIMU::residual(const Eigen::MatrixBase<D1> & _p1,
dp_step, do_step, dv_step,
dp_correct, dq_correct, dv_correct);
-
// 3. Delta error in minimal form: D_err = diff(D_exp , D_corr)
// Note the Dt here is zero because it's the delta-time between the same time stamps!
Eigen::Matrix<T, 9, 1> d_error;
@@ -361,7 +353,6 @@ inline bool ConstraintIMU::residual(const Eigen::MatrixBase<D1> & _p1,
_res.segment(9,3) = sqrt_A_r_dt_inv.cast<T>() * ab_error;
_res.tail(3) = sqrt_W_r_dt_inv.cast<T>() * wb_error;
-
//////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////// PRINT VALUES ///////////////////////////////////
#if 0
@@ -396,14 +387,10 @@ inline bool ConstraintIMU::residual(const Eigen::MatrixBase<D1> & _p1,
WOLF_TRACE("-----------------------------------------")
#endif
/////////////////////////////////////////////////////////////////////////////////////////////
- /////////////////////////////////////////////////////////////////////////////////////////////
-
-
return true;
}
-
inline Eigen::VectorXs ConstraintIMU::expectation() const
{
FrameBasePtr frm_current = getFeaturePtr()->getFramePtr();
@@ -448,25 +435,20 @@ inline void ConstraintIMU::expectation(const Eigen::MatrixBase<D1> & _p1,
imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, _dt, _pe, _qe, _ve);
}
-
} // namespace wolf
#endif
-
/*
* Optimization results
* ================================================
*
- *
* Using gtest_IMU.cpp
*
* Conclusion: Residuals with method 1 and 2 are essentially identical, after exactly the same number of iterations.
*
* You can verify this by looking at the 'Iterations' and 'Final costs' in the Ceres reports below.
*
- *
- *
* With Method 1:
*
[ RUN ] Process_Constraint_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
@@ -490,8 +472,6 @@ inline void ConstraintIMU::expectation(const Eigen::MatrixBase<D1> & _p1,
[ OK ] Process_Constraint_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (52 ms)
[----------] 2 tests from Process_Constraint_IMU_ODO (120 ms total)
*
-*
-*
* With Method 2:
*
[ RUN ] Process_Constraint_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
@@ -515,5 +495,4 @@ inline void ConstraintIMU::expectation(const Eigen::MatrixBase<D1> & _p1,
[ OK ] Process_Constraint_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (59 ms)
[----------] 2 tests from Process_Constraint_IMU_ODO (127 ms total)
*
-*
*/
diff --git a/include/base/constraint/constraint_analytic.h b/include/base/constraint/constraint_analytic.h
index 17ff4a079a55efc4908611801f11d95b9aa86305..3b5015be0841fab26b3e540dcda757987bdea753 100644
--- a/include/base/constraint/constraint_analytic.h
+++ b/include/base/constraint/constraint_analytic.h
@@ -3,7 +3,7 @@
#define CONSTRAINT_ANALYTIC_H_
//Wolf includes
-#include "constraint_base.h"
+#include "base/constraint/constraint_base.h"
#include <Eigen/StdVector>
namespace wolf {
@@ -37,7 +37,6 @@ class ConstraintAnalytic: public ConstraintBase
StateBlockPtr _state8Ptr = nullptr,
StateBlockPtr _state9Ptr = nullptr ) ;
-
ConstraintAnalytic(const std::string& _tp,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
@@ -57,7 +56,6 @@ class ConstraintAnalytic: public ConstraintBase
StateBlockPtr _state8Ptr = nullptr,
StateBlockPtr _state9Ptr = nullptr );
-
virtual ~ConstraintAnalytic() = default;
/** \brief Returns a vector of pointers to the states
diff --git a/include/base/constraint/constraint_autodiff.h b/include/base/constraint/constraint_autodiff.h
index ed61b0756065e19a8d989a2c7c83593deb0eb27a..e6a68703678e5dc9c3a3fcf8519d1867cbfea014 100644
--- a/include/base/constraint/constraint_autodiff.h
+++ b/include/base/constraint/constraint_autodiff.h
@@ -3,8 +3,8 @@
#define CONSTRAINT_AUTODIFF_H_
//Wolf includes
-#include "constraint_base.h"
-#include "state_block.h"
+#include "base/constraint/constraint_base.h"
+#include "base/state_block.h"
// CERES
#include "ceres/jet.h"
@@ -299,7 +299,6 @@ class ConstraintAutodiff : public ConstraintBase
}
};
-
////////////////// SPECIALIZATION 9 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
@@ -2159,17 +2158,9 @@ class ConstraintAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0> : public ConstraintBase
}
};
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// STATIC CONST VECTORS INITIALIZATION
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// state_block_sizes_
// 10 BLOCKS
template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
diff --git a/include/base/constraint/constraint_autodiff_distance_3D.h b/include/base/constraint/constraint_autodiff_distance_3D.h
index a77e3880fa5d7a85a4a18e74b1b0a2e05aaed2dd..6e9452ff6686a1eb44d5f6d7ebddd3ad79e784d5 100644
--- a/include/base/constraint/constraint_autodiff_distance_3D.h
+++ b/include/base/constraint/constraint_autodiff_distance_3D.h
@@ -8,7 +8,7 @@
#ifndef CONSTRAINT_AUTODIFF_DISTANCE_3D_H_
#define CONSTRAINT_AUTODIFF_DISTANCE_3D_H_
-#include "constraint_autodiff.h"
+#include "base/constraint/constraint_autodiff.h"
namespace wolf
{
diff --git a/include/base/constraint/constraint_autodiff_trifocal.h b/include/base/constraint/constraint_autodiff_trifocal.h
index ac54629a62fcee98f469cce5b65f3864c3147de1..d3844c350c17753d3baa0a32a627555466fefe23 100644
--- a/include/base/constraint/constraint_autodiff_trifocal.h
+++ b/include/base/constraint/constraint_autodiff_trifocal.h
@@ -2,9 +2,9 @@
#define _CONSTRAINT_AUTODIFF_TRIFOCAL_H_
//Wolf includes
-//#include "wolf.h"
-#include "constraint_autodiff.h"
-#include "sensor_camera.h"
+//#include "base/wolf.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/sensor/sensor_camera.h"
#include <common_class/trifocaltensor.h>
#include <vision_utils.h>
@@ -108,7 +108,6 @@ class ConstraintAutodiffTrifocal : public ConstraintAutodiff<ConstraintAutodiffT
MatrixBase<D3>& _J_e_m2,
MatrixBase<D4>& _J_e_m3);
-
private:
FeatureBaseWPtr feature_prev_ptr_; // To look for measurements
SensorCameraPtr camera_ptr_; // To look for intrinsics
@@ -118,10 +117,8 @@ class ConstraintAutodiffTrifocal : public ConstraintAutodiff<ConstraintAutodiffT
} // namespace wolf
-
-
// Includes for implentation
-#include "rotations.h"
+#include "base/rotations.h"
namespace wolf
{
@@ -136,7 +133,6 @@ ConstraintAutodiffTrifocal::ConstraintAutodiffTrifocal(
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
ConstraintStatus _status) : ConstraintAutodiff( "TRIFOCAL PLP",
- nullptr,
nullptr,
_feature_origin_ptr,
nullptr,
@@ -217,7 +213,6 @@ inline FeatureBasePtr ConstraintAutodiffTrifocal::getFeaturePrevPtr()
return feature_prev_ptr_.lock();
}
-
template<typename T>
bool ConstraintAutodiffTrifocal::operator ()( const T* const _prev_pos,
const T* const _prev_quat,
@@ -379,7 +374,6 @@ inline Matrix<T, 3, 1> ConstraintAutodiffTrifocal::error_jacobians(const vision_
J_e2_m3.setZero(); // Not involved in epipolar c1->c2
-
// Compact Jacobians
_J_e_m1.topRows(2) = J_e1_m1;
_J_e_m1.row(2) = J_e2_m1;
@@ -397,8 +391,6 @@ inline Matrix<T, 3, 1> ConstraintAutodiffTrifocal::error_jacobians(const vision_
}
-
} // namespace wolf
-
#endif /* _CONSTRAINT_AUTODIFF_TRIFOCAL_H_ */
diff --git a/include/base/constraint/constraint_base.h b/include/base/constraint/constraint_base.h
index 3341a55c882d64262fb2bc6f6a7251a0f3acf6c9..1a0b4f14f77f559f983b7dbce0b4028bea45baa9 100644
--- a/include/base/constraint/constraint_base.h
+++ b/include/base/constraint/constraint_base.h
@@ -7,8 +7,8 @@ class FeatureBase;
}
//Wolf includes
-#include "wolf.h"
-#include "node_base.h"
+#include "base/wolf.h"
+#include "base/node_base.h"
//std includes
@@ -178,16 +178,15 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
// void print(const std::string& name, const Eigen::Matrix<Scalar, R, C>& mat) const; // Normal print if Scalar type is wolf::Scalar
};
-
}
// IMPLEMENTATION //
-#include "problem.h"
-#include "frame_base.h"
-#include "feature_base.h"
-#include "sensor_base.h"
-#include "landmark_base.h"
+#include "base/problem.h"
+#include "base/frame_base.h"
+#include "base/feature/feature_base.h"
+#include "base/sensor/sensor_base.h"
+#include "base/landmark/landmark_base.h"
namespace wolf{
diff --git a/include/base/constraint/constraint_block_absolute.h b/include/base/constraint/constraint_block_absolute.h
index fe8eec7a9f01095838c0572d4517e4493d85fa3a..22b4ee1b1ebb371e6cc76ad5e8bd5392df6e64ee 100644
--- a/include/base/constraint/constraint_block_absolute.h
+++ b/include/base/constraint/constraint_block_absolute.h
@@ -9,9 +9,8 @@
#define CONSTRAINT_BLOCK_ABSOLUTE_H_
//Wolf includes
-#include "constraint_autodiff.h"
-#include "frame_base.h"
-
+#include "base/constraint/constraint_autodiff.h"
+#include "base/frame_base.h"
namespace wolf {
diff --git a/include/base/constraint/constraint_container.h b/include/base/constraint/constraint_container.h
index c796d1e326f11b1fcb78d17341a1bb6f782211dd..1e578f10dc4628031c94b3654264c2a90a435190 100644
--- a/include/base/constraint/constraint_container.h
+++ b/include/base/constraint/constraint_container.h
@@ -2,9 +2,9 @@
#define CONSTRAINT_CONTAINER_H_
//Wolf includes
-#include "wolf.h"
-#include "constraint_autodiff.h"
-#include "landmark_container.h"
+#include "base/wolf.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/landmark/landmark_container.h"
namespace wolf {
@@ -116,7 +116,6 @@ class ConstraintContainer: public ConstraintAutodiff<ConstraintContainer,3,2,1,2
return true;
}
-
public:
static ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr,
const NodeBasePtr& _correspondant_ptr,
diff --git a/include/base/constraint/constraint_corner_2D.h b/include/base/constraint/constraint_corner_2D.h
index 96afcfd509c35f24fe27d01f149532019cea427f..4692ad29a31a437492805b479a510a5e0e7383e9 100644
--- a/include/base/constraint/constraint_corner_2D.h
+++ b/include/base/constraint/constraint_corner_2D.h
@@ -2,8 +2,8 @@
#define CONSTRAINT_CORNER_2D_THETA_H_
//Wolf includes
-#include "constraint_autodiff.h"
-#include "landmark_corner_2D.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/landmark/landmark_corner_2D.h"
namespace wolf {
@@ -111,6 +111,4 @@ inline bool ConstraintCorner2D::operator ()(const T* const _robotP, const T* con
} // namespace wolf
-
-
#endif
diff --git a/include/base/constraint/constraint_diff_drive.h b/include/base/constraint/constraint_diff_drive.h
index 8d6a898a04faf8e8c2fd22481d4e7308a1513255..01722378488cc6a02c2fae0fff0f78635516731a 100644
--- a/include/base/constraint/constraint_diff_drive.h
+++ b/include/base/constraint/constraint_diff_drive.h
@@ -9,10 +9,10 @@
#define WOLF_CONSTRAINT_DIFF_DRIVE_H_
//Wolf includes
-#include "../constraint_autodiff.h"
-#include "../frame_base.h"
-#include "../features/feature_diff_drive.h"
-#include "../captures/capture_wheel_joint_position.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/frame_base.h"
+#include "base/feature/feature_diff_drive.h"
+#include "base/capture/capture_wheel_joint_position.h"
namespace
{
diff --git a/include/base/constraint/constraint_epipolar.h b/include/base/constraint/constraint_epipolar.h
index 3172173cf53c8ac2fc11136e9b5ba1399371cf1a..2887af376692cce2ca2420074657a9ab4af92fee 100644
--- a/include/base/constraint/constraint_epipolar.h
+++ b/include/base/constraint/constraint_epipolar.h
@@ -1,7 +1,7 @@
#ifndef CONSTRAINT_EPIPOLAR_H
#define CONSTRAINT_EPIPOLAR_H
-#include "constraint_base.h"
+#include "base/constraint/constraint_base.h"
namespace wolf {
@@ -19,7 +19,6 @@ class ConstraintEpipolar : public ConstraintBase
virtual ~ConstraintEpipolar() = default;
-
/** \brief Evaluate the constraint given the input parameters and returning the residuals and jacobians
**/
virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const override {return true;};
diff --git a/include/base/constraint/constraint_fix_bias.h b/include/base/constraint/constraint_fix_bias.h
index ed69c90491762e41f7876fd4a3f6f871b38275f7..55dbbf8db732dbc9f18bade35b4b2dce790b3c5d 100644
--- a/include/base/constraint/constraint_fix_bias.h
+++ b/include/base/constraint/constraint_fix_bias.h
@@ -3,15 +3,14 @@
#define CONSTRAINT_FIX_BIAS_H_
//Wolf includes
-#include "capture_IMU.h"
-#include "feature_IMU.h"
-#include "constraint_autodiff.h"
-#include "frame_base.h"
-#include "rotations.h"
+#include "base/capture/capture_IMU.h"
+#include "base/feature/feature_IMU.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/frame_base.h"
+#include "base/rotations.h"
//#include "ceres/jet.h"
-
namespace wolf {
WOLF_PTR_TYPEDEFS(ConstraintFixBias);
diff --git a/include/base/constraint/constraint_odom_2D.h b/include/base/constraint/constraint_odom_2D.h
index 11333302fc5f3fa12a01354e5f6b08cc60fc9065..5df37fbb98b330d561a3658bf07ed6c83e1ae74a 100644
--- a/include/base/constraint/constraint_odom_2D.h
+++ b/include/base/constraint/constraint_odom_2D.h
@@ -2,8 +2,8 @@
#define CONSTRAINT_ODOM_2D_THETA_H_
//Wolf includes
-#include "constraint_autodiff.h"
-#include "frame_base.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/frame_base.h"
//#include "ceres/jet.h"
@@ -73,7 +73,6 @@ inline bool ConstraintOdom2D::operator ()(const T* const _p1, const T* const _o1
// Residuals
res = getMeasurementSquareRootInformationUpper().cast<T>() * er;
-
////////////////////////////////////////////////////////
// print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
// using ceres::Jet;
@@ -92,7 +91,6 @@ inline bool ConstraintOdom2D::operator ()(const T* const _p1, const T* const _o1
// }
////////////////////////////////////////////////////////
-
return true;
}
diff --git a/include/base/constraint/constraint_odom_2D_analytic.h b/include/base/constraint/constraint_odom_2D_analytic.h
index d69655e2714928729b610ba8162075db642c68b3..43048d3ffa702042cc022ddebd499d7251b21d10 100644
--- a/include/base/constraint/constraint_odom_2D_analytic.h
+++ b/include/base/constraint/constraint_odom_2D_analytic.h
@@ -2,7 +2,7 @@
#define CONSTRAINT_ODOM_2D_ANALYTIC_H_
//Wolf includes
-#include "constraint_relative_2D_analytic.h"
+#include "base/constraint/constraint_relative_2D_analytic.h"
#include <Eigen/StdVector>
namespace wolf {
@@ -93,7 +93,6 @@ class ConstraintOdom2DAnalytic : public ConstraintRelative2DAnalytic
// jacobians[3] = getMeasurementSquareRootInformationUpper() * jacobians[0];
// }
-
public:
static ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr,
const NodeBasePtr& _correspondant_ptr,
diff --git a/include/base/constraint/constraint_odom_3D.h b/include/base/constraint/constraint_odom_3D.h
index 26811e4707fcc90f85a230a98903d3f77c6150cf..9047b7891cec383e1575c4fbab3b772b7efea610 100644
--- a/include/base/constraint/constraint_odom_3D.h
+++ b/include/base/constraint/constraint_odom_3D.h
@@ -8,8 +8,8 @@
#ifndef CONSTRAINT_ODOM_3D_H_
#define CONSTRAINT_ODOM_3D_H_
-#include "constraint_autodiff.h"
-#include "rotations.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/rotations.h"
namespace wolf
{
@@ -49,7 +49,6 @@ class ConstraintOdom3D : public ConstraintAutodiff<ConstraintOdom3D,6,3,4,3,4>
template<typename T>
void printRes(const Eigen::Matrix<T, 6, 1>& r) const;
-
};
template<typename T>
@@ -71,7 +70,6 @@ inline void ConstraintOdom3D::printRes (const Eigen::Matrix<Scalar,6,1> & r) co
std::cout << r.transpose() << std::endl;
}
-
inline ConstraintOdom3D::ConstraintOdom3D(const FeatureBasePtr& _ftr_current_ptr,
const FrameBasePtr& _frame_past_ptr,
const ProcessorBasePtr& _processor_ptr,
diff --git a/include/base/constraint/constraint_point_2D.h b/include/base/constraint/constraint_point_2D.h
index 50d190409986f90bb0828a24c973e8c9f2508e19..1900991f7d47b01d0486a7ffca5465056033fbf1 100644
--- a/include/base/constraint/constraint_point_2D.h
+++ b/include/base/constraint/constraint_point_2D.h
@@ -2,9 +2,9 @@
#define CONSTRAINT_POINT_2D_THETA_H_
//Wolf includes
-#include "constraint_autodiff.h"
-#include "feature_polyline_2D.h"
-#include "landmark_polyline_2D.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/feature/feature_polyline_2D.h"
+#include "base/landmark/landmark_polyline_2D.h"
namespace wolf {
diff --git a/include/base/constraint/constraint_point_to_line_2D.h b/include/base/constraint/constraint_point_to_line_2D.h
index 324ae24d600cc646472340e60fd92732f833d003..842b84a800996c8bdb1ca6cf2adf3607dc53d358 100644
--- a/include/base/constraint/constraint_point_to_line_2D.h
+++ b/include/base/constraint/constraint_point_to_line_2D.h
@@ -2,9 +2,9 @@
#define CONSTRAINT_POINT_TO_LINE_2D_H_
//Wolf includes
-#include "constraint_autodiff.h"
-#include "feature_polyline_2D.h"
-#include "landmark_polyline_2D.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/feature/feature_polyline_2D.h"
+#include "base/landmark/landmark_polyline_2D.h"
namespace wolf {
diff --git a/include/base/constraint/constraint_pose_2D.h b/include/base/constraint/constraint_pose_2D.h
index 652c1c3777887c8fe37b1c9d057a52b3b3d09471..b6fe79ef378c0b1bfb6e722b786678ae195f274c 100644
--- a/include/base/constraint/constraint_pose_2D.h
+++ b/include/base/constraint/constraint_pose_2D.h
@@ -3,13 +3,12 @@
#define CONSTRAINT_POSE_2D_H_
//Wolf includes
-#include "constraint_autodiff.h"
-#include "frame_base.h"
-#include "rotations.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/frame_base.h"
+#include "base/rotations.h"
//#include "ceres/jet.h"
-
namespace wolf {
WOLF_PTR_TYPEDEFS(ConstraintPose2D);
diff --git a/include/base/constraint/constraint_pose_3D.h b/include/base/constraint/constraint_pose_3D.h
index 8823eefb76f4ed40277b652632ed086b262d3074..5fa1ec6c25e606cc4cb451e41c18b421fe38b98d 100644
--- a/include/base/constraint/constraint_pose_3D.h
+++ b/include/base/constraint/constraint_pose_3D.h
@@ -3,10 +3,9 @@
#define CONSTRAINT_POSE_3D_H_
//Wolf includes
-#include "constraint_autodiff.h"
-#include "frame_base.h"
-#include "rotations.h"
-
+#include "base/constraint/constraint_autodiff.h"
+#include "base/frame_base.h"
+#include "base/rotations.h"
namespace wolf {
diff --git a/include/base/constraint/constraint_quaternion_absolute.h b/include/base/constraint/constraint_quaternion_absolute.h
index 1b147a03df6a951507a71c8405f1e495d57db76c..eb2e2431a983f14c6300db4b621c85348cf1a334 100644
--- a/include/base/constraint/constraint_quaternion_absolute.h
+++ b/include/base/constraint/constraint_quaternion_absolute.h
@@ -9,11 +9,10 @@
#define CONSTRAINT_QUATERNION_ABSOLUTE_H_
//Wolf includes
-#include "constraint_autodiff.h"
-#include "local_parametrization_quaternion.h"
-#include "frame_base.h"
-#include "rotations.h"
-
+#include "base/constraint/constraint_autodiff.h"
+#include "base/local_parametrization_quaternion.h"
+#include "base/frame_base.h"
+#include "base/rotations.h"
namespace wolf {
diff --git a/include/base/constraint/constraint_relative_2D_analytic.h b/include/base/constraint/constraint_relative_2D_analytic.h
index 32b8b30593a73717be3fccc4459652da17dce616..a9655a9146e8f7dce7efc6378effd3c56a59cb66 100644
--- a/include/base/constraint/constraint_relative_2D_analytic.h
+++ b/include/base/constraint/constraint_relative_2D_analytic.h
@@ -2,8 +2,8 @@
#define CONSTRAINT_RELATIVE_2D_ANALYTIC_H_
//Wolf includes
-#include "constraint_analytic.h"
-#include "landmark_base.h"
+#include "base/constraint/constraint_analytic.h"
+#include "base/landmark/landmark_base.h"
#include <Eigen/StdVector>
namespace wolf {
@@ -93,7 +93,6 @@ class ConstraintRelative2DAnalytic : public ConstraintAnalytic
};
-
/// IMPLEMENTATION ///
inline Eigen::VectorXs ConstraintRelative2DAnalytic::evaluateResiduals(
diff --git a/src/sensor/diff_drive_tools.h b/include/base/diff_drive_tools.h
similarity index 99%
rename from src/sensor/diff_drive_tools.h
rename to include/base/diff_drive_tools.h
index ccf243cc1b66c74285e9cf366f7aa71759739b20..142b8c5022ccca18eaf1edc1214984b93bd816c4 100644
--- a/src/sensor/diff_drive_tools.h
+++ b/include/base/diff_drive_tools.h
@@ -8,7 +8,7 @@
#ifndef _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_
#define _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_
-#include "../eigen_assert.h"
+#include "base/eigen_assert.h"
namespace wolf {
@@ -359,7 +359,6 @@ void integrateExact(const Eigen::MatrixBase<T0> &data,
delta_cov = jacobian * data_cov * jacobian.transpose();
}
-
/**
* @brief integrate. Helper function to call either
* `integrateRung` or `integrateExact` depending on the
@@ -420,6 +419,6 @@ Eigen::Matrix<typename T::Scalar, 2, 2> computeWheelJointPositionCov(
} // namespace wolf
-#include "diff_drive_tools.hpp"
+#include "base/diff_drive_tools.hpp"
#endif /* _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_ */
diff --git a/src/sensor/diff_drive_tools.hpp b/include/base/diff_drive_tools.hpp
similarity index 100%
rename from src/sensor/diff_drive_tools.hpp
rename to include/base/diff_drive_tools.hpp
diff --git a/include/base/eigen_predicates.h b/include/base/eigen_predicates.h
index 65e737b1b328d2dc546ab4a1bddc1783dcef1db5..55f2eeeb75df3360e504105bd6cb08660b1cc5ea 100644
--- a/include/base/eigen_predicates.h
+++ b/include/base/eigen_predicates.h
@@ -8,7 +8,7 @@
#ifndef _WOLF_EIGEN_PREDICATES_H_
#define _WOLF_EIGEN_PREDICATES_H_
-#include "rotations.h"
+#include "base/rotations.h"
namespace wolf {
@@ -97,5 +97,4 @@ auto pred_angle_zero = [](const Scalar lhs, const Scalar precision)
} // namespace wolf
-
#endif /* _WOLF_EIGEN_PREDICATES_H_ */
diff --git a/include/base/factory.h b/include/base/factory.h
index c63d6df8d04f62f167610eace6ec8c5d61ca500e..b161126e45c18e4cfd8a953a32b5247d58fa1d27 100644
--- a/include/base/factory.h
+++ b/include/base/factory.h
@@ -9,7 +9,7 @@
#define FACTORY_H_
// wolf
-#include "wolf.h"
+#include "base/wolf.h"
// std
#include <string>
@@ -308,8 +308,6 @@ inline std::string Factory<TypeBase, TypeInput...>::getClass()
} // namespace wolf
-
-
namespace wolf
{
@@ -349,7 +347,7 @@ inline std::string LandmarkFactory::getClass()
// Frames
class TimeStamp;
} // namespace wolf
-#include "frame_base.h"
+#include "base/frame_base.h"
namespace wolf{
typedef Factory<FrameBase, const FrameType&, const TimeStamp&, const Eigen::VectorXs&> FrameFactory;
template<>
@@ -378,7 +376,6 @@ inline std::string FrameFactory::getClass()
namespace{ const bool WOLF_UNUSED FrameName##Registered = \
wolf::FrameFactory::get().registerCreator(FrameType, FrameName::create); }\
-
} /* namespace wolf */
#endif /* FACTORY_H_ */
diff --git a/include/base/feature/feature_GPS_fix.h b/include/base/feature/feature_GPS_fix.h
index 436d7f59306b3767f3eab644d6d0e23b583f1e1a..688176fdfac976bd483e3fb4694155d7843af754 100644
--- a/include/base/feature/feature_GPS_fix.h
+++ b/include/base/feature/feature_GPS_fix.h
@@ -2,8 +2,8 @@
#define FEATURE_GPS_FIX_H_
//Wolf includes
-#include "constraint_GPS_2D.h"
-#include "feature_base.h"
+#include "base/constraint/constraint_GPS_2D.h"
+#include "base/feature/feature_base.h"
//std includes
diff --git a/include/base/feature/feature_GPS_pseudorange.h b/include/base/feature/feature_GPS_pseudorange.h
index 5adaa5b6cca19ef3e15e2cae673629d2dca7a2b1..57f327e2520b94827a222e0b80703a24c3049c01 100644
--- a/include/base/feature/feature_GPS_pseudorange.h
+++ b/include/base/feature/feature_GPS_pseudorange.h
@@ -3,12 +3,11 @@
#define FEATURE_GPS_PSEUDORANGE_H_
//Wolf includes
-#include "feature_base.h"
+#include "base/feature/feature_base.h"
//std includes
#include <iomanip>
-
namespace wolf {
WOLF_PTR_TYPEDEFS(FeatureGPSPseudorange);
diff --git a/include/base/feature/feature_IMU.h b/include/base/feature/feature_IMU.h
index 37e6884db6a5415647ad3e32e305483202fe973d..dfce11ff78a8e974731efef729c17b0fa72a9fe8 100644
--- a/include/base/feature/feature_IMU.h
+++ b/include/base/feature/feature_IMU.h
@@ -2,13 +2,12 @@
#define FEATURE_IMU_H_
//Wolf includes
-#include "capture_IMU.h"
-#include "feature_base.h"
-#include "wolf.h"
+#include "base/capture/capture_IMU.h"
+#include "base/feature/feature_base.h"
+#include "base/wolf.h"
//std includes
-
namespace wolf {
//WOLF_PTR_TYPEDEFS(CaptureIMU);
@@ -18,7 +17,6 @@ class FeatureIMU : public FeatureBase
{
public:
-
/** \brief Constructor from and measures
*
* \param _measurement the measurement
@@ -58,7 +56,6 @@ class FeatureIMU : public FeatureBase
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};
-
inline const Eigen::Vector3s& FeatureIMU::getAccBiasPreint() const
{
return acc_bias_preint_;
diff --git a/include/base/feature/feature_base.h b/include/base/feature/feature_base.h
index 1eb4fbf851feb0257b11b210dd597c1ab3710286..1ab46cccceebfae070368341aa2edde0000e694a 100644
--- a/include/base/feature/feature_base.h
+++ b/include/base/feature/feature_base.h
@@ -8,14 +8,13 @@ class ConstraintBase;
}
//Wolf includes
-#include "wolf.h"
-#include "node_base.h"
+#include "base/wolf.h"
+#include "base/node_base.h"
//std includes
namespace wolf {
-
//class FeatureBase
class FeatureBase : public NodeBase, public std::enable_shared_from_this<FeatureBase>
{
@@ -97,7 +96,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
// IMPLEMENTATION
-#include "constraint_base.h"
+#include "base/constraint/constraint_base.h"
namespace wolf{
diff --git a/include/base/feature/feature_corner_2D.h b/include/base/feature/feature_corner_2D.h
index d43b820259cbc546e3fe426497c1bdc7bac99fe6..8e206749db1261edc254e9fd09eeb9cde60337cc 100644
--- a/include/base/feature/feature_corner_2D.h
+++ b/include/base/feature/feature_corner_2D.h
@@ -2,11 +2,10 @@
#define FEATURE_CORNER_2D_H_
//Wolf includes
-#include "feature_base.h"
+#include "base/feature/feature_base.h"
//std includes
-
namespace wolf {
WOLF_PTR_TYPEDEFS(FeatureCorner2D);
diff --git a/src/feature/feature_diff_drive.h b/include/base/feature/feature_diff_drive.h
similarity index 95%
rename from src/feature/feature_diff_drive.h
rename to include/base/feature/feature_diff_drive.h
index 5c837469b577cf411eb8c830e2941823d1a0d91f..52877f4f5e134d444951c7b1667f67b6d02711fe 100644
--- a/src/feature/feature_diff_drive.h
+++ b/include/base/feature/feature_diff_drive.h
@@ -9,7 +9,7 @@
#define _WOLF_FEATURE_DIFF_DRIVE_H_
//Wolf includes
-#include "../feature_base.h"
+#include "base/feature/feature_base.h"
namespace wolf {
diff --git a/include/base/feature/feature_line_2D.h b/include/base/feature/feature_line_2D.h
index cd99621a080c3767f8cba5e9dafde6c73d769a58..5d5e6162dc84217058f520676b768c24dddb671d 100644
--- a/include/base/feature/feature_line_2D.h
+++ b/include/base/feature/feature_line_2D.h
@@ -2,13 +2,12 @@
#define FEATURE_LINE_2D_H_
//Wolf includes
-#include "feature_base.h"
+#include "base/feature/feature_base.h"
namespace wolf {
WOLF_PTR_TYPEDEFS(FeatureLine2D);
-
/** \brief class FeatureLine2D
*
* Line segment feature.
diff --git a/include/base/feature/feature_match.h b/include/base/feature/feature_match.h
index e776b1fcd4c309f1e88fdf185250fca9042b9f54..b96ce00239f1fb6293e3dd10b24e6acd345ae8cf 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 "wolf.h"
+#include "base/wolf.h"
//wolf nampseace
namespace wolf {
@@ -34,4 +34,3 @@ struct FeatureMatch
#endif
-
diff --git a/include/base/feature/feature_motion.h b/include/base/feature/feature_motion.h
index 61b9bb59a227d5a0668588676f71be177e5a8571..92331b6b1d7f41bbb342c4ef528b64f488fc1c48 100644
--- a/include/base/feature/feature_motion.h
+++ b/include/base/feature/feature_motion.h
@@ -8,8 +8,8 @@
#ifndef FEATURE_MOTION_H_
#define FEATURE_MOTION_H_
-#include "feature_base.h"
-#include "capture_motion.h"
+#include "base/feature/feature_base.h"
+#include "base/capture/capture_motion.h"
namespace wolf
{
diff --git a/include/base/feature/feature_odom_2D.h b/include/base/feature/feature_odom_2D.h
index 3a43ee64ff9b0c23fefbcc286a95b2c27c01c170..92fd84fa7482903beb435672d4c480f6708d1d09 100644
--- a/include/base/feature/feature_odom_2D.h
+++ b/include/base/feature/feature_odom_2D.h
@@ -2,13 +2,12 @@
#define FEATURE_ODOM_2D_H_
//Wolf includes
-#include "feature_base.h"
-#include "constraint_odom_2D.h"
-#include "constraint_odom_2D_analytic.h"
+#include "base/feature/feature_base.h"
+#include "base/constraint/constraint_odom_2D.h"
+#include "base/constraint/constraint_odom_2D_analytic.h"
//std includes
-
namespace wolf {
WOLF_PTR_TYPEDEFS(FeatureOdom2D);
diff --git a/include/base/feature/feature_point_image.h b/include/base/feature/feature_point_image.h
index 6e82b4305acdd3ce8e5393b1c145b9fcc04f3929..aa07d80dfd836d51440383bea1acf7ab5cea0451 100644
--- a/include/base/feature/feature_point_image.h
+++ b/include/base/feature/feature_point_image.h
@@ -1,9 +1,8 @@
#ifndef FEATURE_IMAGE_H
#define FEATURE_IMAGE_H
-
//Wolf includes
-#include "feature_base.h"
+#include "base/feature/feature_base.h"
// Vision Utils includes
#include <vision_utils.h>
diff --git a/include/base/feature/feature_polyline_2D.h b/include/base/feature/feature_polyline_2D.h
index 5c621a0bf62db0727a248edce98e178093869394..d289a58f35c353db68df8521f5f6badc5853e390 100644
--- a/include/base/feature/feature_polyline_2D.h
+++ b/include/base/feature/feature_polyline_2D.h
@@ -8,7 +8,7 @@
#ifndef FEATURE_POLYLINE_2D_H_
#define FEATURE_POLYLINE_2D_H_
-#include "feature_base.h"
+#include "base/feature/feature_base.h"
namespace wolf
{
diff --git a/include/base/feature/feature_pose.h b/include/base/feature/feature_pose.h
index f4d7ba0612f8e313d1be4e54618ab540d6a78007..1431f920765c2450a2d71d02b3d6adf4e95700e7 100644
--- a/include/base/feature/feature_pose.h
+++ b/include/base/feature/feature_pose.h
@@ -1,9 +1,8 @@
#ifndef FEATURE_POSE_H_
#define FEATURE_POSE_H_
-
//Wolf includes
-#include "feature_base.h"
+#include "base/feature/feature_base.h"
//std includes
//
diff --git a/include/base/frame_base.h b/include/base/frame_base.h
index f7e5f8379772502c5169d03aa11a7cbdae39f2d3..16d13dbee1f0484dee1f911df14b07fd476a007d 100644
--- a/include/base/frame_base.h
+++ b/include/base/frame_base.h
@@ -10,9 +10,9 @@ class StateBlock;
}
//Wolf includes
-#include "wolf.h"
-#include "time_stamp.h"
-#include "node_base.h"
+#include "base/wolf.h"
+#include "base/time_stamp.h"
+#include "base/node_base.h"
//std includes
@@ -26,7 +26,6 @@ typedef enum
KEY_FRAME = 1 ///< key frame. It plays at optimizations.
} FrameType;
-
//class FrameBase
class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase>
{
@@ -68,9 +67,6 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
virtual ~FrameBase();
virtual void remove();
-
-
-
// Frame properties -----------------------------------------------
public:
unsigned int id();
@@ -160,14 +156,13 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
// IMPLEMENTATION //
-#include "trajectory_base.h"
-#include "capture_base.h"
-#include "constraint_base.h"
-#include "state_block.h"
+#include "base/trajectory_base.h"
+#include "base/capture/capture_base.h"
+#include "base/constraint/constraint_base.h"
+#include "base/state_block.h"
namespace wolf {
-
inline unsigned int FrameBase::id()
{
return frame_id_;
diff --git a/include/base/hardware_base.h b/include/base/hardware_base.h
index 9d60cdd1086cb16b7e57d9df135ab1536c941e09..f3679dc4ab7d7694bd3058eaf86a6158e9218283 100644
--- a/include/base/hardware_base.h
+++ b/include/base/hardware_base.h
@@ -8,8 +8,8 @@ class SensorBase;
}
//Wolf includes
-#include "wolf.h"
-#include "node_base.h"
+#include "base/wolf.h"
+#include "base/node_base.h"
namespace wolf {
diff --git a/include/base/landmark/landmark_AHP.h b/include/base/landmark/landmark_AHP.h
index e8d695b19505f0d0335671e6c9a8f0f40d31901f..a706f87dadcfa938d4ec0666a2d1a241ff40c253 100644
--- a/include/base/landmark/landmark_AHP.h
+++ b/include/base/landmark/landmark_AHP.h
@@ -2,7 +2,7 @@
#define LANDMARK_AHP_H
//Wolf includes
-#include "landmark_base.h"
+#include "base/landmark/landmark_base.h"
// yaml
#include <yaml-cpp/yaml.h>
@@ -10,7 +10,6 @@
// Vision utils
#include <vision_utils/vision_utils.h>
-
namespace wolf {
WOLF_PTR_TYPEDEFS(LandmarkAHP);
diff --git a/include/base/landmark/landmark_base.h b/include/base/landmark/landmark_base.h
index 66485387abcbdfdaec03f0f6fa93cc92d7998f1b..8b365e9be506f9271571542970683032326819f9 100644
--- a/include/base/landmark/landmark_base.h
+++ b/include/base/landmark/landmark_base.h
@@ -8,9 +8,9 @@ class StateBlock;
}
//Wolf includes
-#include "wolf.h"
-#include "node_base.h"
-#include "time_stamp.h"
+#include "base/wolf.h"
+#include "base/node_base.h"
+#include "base/time_stamp.h"
//std includes
@@ -19,7 +19,6 @@ class StateBlock;
namespace wolf {
-
//class LandmarkBase
class LandmarkBase : public NodeBase, public std::enable_shared_from_this<LandmarkBase>
{
@@ -75,7 +74,6 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
bool getCovariance(Eigen::MatrixXs& _cov) const;
Eigen::MatrixXs getCovariance() const;
-
protected:
virtual void removeStateBlocks();
@@ -85,7 +83,6 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
Scalar getDescriptor(unsigned int _ii) const;
void setDescriptor(const Eigen::VectorXs& _descriptor);
-
// Navigate wolf tree
virtual void setProblem(ProblemPtr _problem) final;
@@ -100,9 +97,9 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
}
-#include "map_base.h"
-#include "constraint_base.h"
-#include "state_block.h"
+#include "base/map_base.h"
+#include "base/constraint/constraint_base.h"
+#include "base/state_block.h"
namespace wolf{
diff --git a/include/base/landmark/landmark_container.h b/include/base/landmark/landmark_container.h
index 0f51f3c5a91d18335f9933246f2b1588c02162be..65a2c5b710ceb0f42bb9984fee0e8d0a967348a4 100644
--- a/include/base/landmark/landmark_container.h
+++ b/include/base/landmark/landmark_container.h
@@ -3,8 +3,8 @@
#define LANDMARK_CONTAINER_H_
//Wolf includes
-#include "landmark_base.h"
-#include "wolf.h"
+#include "base/landmark/landmark_base.h"
+#include "base/wolf.h"
// Std includes
@@ -36,7 +36,6 @@ class LandmarkContainer : public LandmarkBase
*
* B ---------------------------- A
* | |
- * | |
* | <---+ |
* | | |
* | v |
@@ -103,7 +102,6 @@ class LandmarkContainer : public LandmarkBase
};
-
} // namespace wolf
#endif
diff --git a/include/base/landmark/landmark_corner_2D.h b/include/base/landmark/landmark_corner_2D.h
index 1eb99a975666814dececc52874b1c5d4905386fe..c1ce86fbdd1979a4e4e04150964207ab9e0ee994 100644
--- a/include/base/landmark/landmark_corner_2D.h
+++ b/include/base/landmark/landmark_corner_2D.h
@@ -3,11 +3,10 @@
#define LANDMARK_CORNER_H_
//Wolf includes
-#include "landmark_base.h"
+#include "base/landmark/landmark_base.h"
// Std includes
-
namespace wolf {
WOLF_PTR_TYPEDEFS(LandmarkCorner2D);
diff --git a/include/base/landmark/landmark_line_2D.h b/include/base/landmark/landmark_line_2D.h
index 514cd550b294a3e65255df942f450bade9dd4902..3a0f84c340d12027a74bb5be7ff11f31288c06e2 100644
--- a/include/base/landmark/landmark_line_2D.h
+++ b/include/base/landmark/landmark_line_2D.h
@@ -3,17 +3,15 @@
#define LANDMARK_LINE_2D_H_
//Wolf includes
-#include "landmark_base.h"
-#include "wolf.h"
+#include "base/landmark/landmark_base.h"
+#include "base/wolf.h"
//std includes
-
namespace wolf {
WOLF_PTR_TYPEDEFS(LandmarkLine2D);
-
//class LandmarkLine2D
class LandmarkLine2D : public LandmarkBase
{
diff --git a/include/base/landmark/landmark_match.h b/include/base/landmark/landmark_match.h
index ae47aa619ea9069eb9ae983f66ead491ac82ed39..0c5d3a4915892a3680d634f24a6a40a73df6e26c 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 "wolf.h"
+#include "base/wolf.h"
//wolf nampseace
namespace wolf {
@@ -33,7 +33,6 @@ struct LandmarkMatch
}
};
-
}//end namespace
#endif
diff --git a/include/base/landmark/landmark_polyline_2D.h b/include/base/landmark/landmark_polyline_2D.h
index a44a00b4cf5cf3aef2c560693e3232560e01a63a..4414a69f6175f36280656c2df33e4e94697d0d7d 100644
--- a/include/base/landmark/landmark_polyline_2D.h
+++ b/include/base/landmark/landmark_polyline_2D.h
@@ -9,7 +9,7 @@
#define LANDMARK_POLYLINE_2D_H_
// Wolf
-#include "landmark_base.h"
+#include "base/landmark/landmark_base.h"
// STL
#include <deque>
diff --git a/include/base/local_parametrization_angle.h b/include/base/local_parametrization_angle.h
index 2445de112ac121ac774b22488cc41588f9e74702..912237c07a8b4a7aa4d711c39c68f3c9cccf8d24 100644
--- a/include/base/local_parametrization_angle.h
+++ b/include/base/local_parametrization_angle.h
@@ -8,8 +8,8 @@
#ifndef LOCAL_PARAMETRIZATION_ANGLE_H_
#define LOCAL_PARAMETRIZATION_ANGLE_H_
-#include "local_parametrization_base.h"
-#include "rotations.h"
+#include "base/local_parametrization_base.h"
+#include "base/rotations.h"
namespace wolf
{
diff --git a/include/base/local_parametrization_base.h b/include/base/local_parametrization_base.h
index acb9ec683416fb46da4ba9dee60e48f3a2752d0a..3b7dcc38dc698936a707101aa40cad076adc910f 100644
--- a/include/base/local_parametrization_base.h
+++ b/include/base/local_parametrization_base.h
@@ -8,8 +8,7 @@
#ifndef LOCAL_PARAMETRIZATION_BASE_H_
#define LOCAL_PARAMETRIZATION_BASE_H_
-#include "wolf.h"
-
+#include "base/wolf.h"
namespace wolf {
diff --git a/include/base/local_parametrization_homogeneous.h b/include/base/local_parametrization_homogeneous.h
index 852c9aba7c26e6494d570874a5402f2e62f773b3..f06dd2fb150e5f34eede487e3a685fb88f00d130 100644
--- a/include/base/local_parametrization_homogeneous.h
+++ b/include/base/local_parametrization_homogeneous.h
@@ -8,8 +8,7 @@
#ifndef LOCALPARAMETRIZATIONHOMOGENEOUS_H_
#define LOCALPARAMETRIZATIONHOMOGENEOUS_H_
-#include "local_parametrization_base.h"
-
+#include "base/local_parametrization_base.h"
namespace wolf {
diff --git a/include/base/local_parametrization_polyline_extreme.h b/include/base/local_parametrization_polyline_extreme.h
index 62f7760d7ed6cd0724b498cf82f589826c5e577b..3a75a8a0ff4eede5a3e865810555852653494713 100644
--- a/include/base/local_parametrization_polyline_extreme.h
+++ b/include/base/local_parametrization_polyline_extreme.h
@@ -8,8 +8,7 @@
#ifndef LOCAL_PARAMETRIZATION_POLYLINE_EXTREME_H_
#define LOCAL_PARAMETRIZATION_POLYLINE_EXTREME_H_
-#include "local_parametrization_base.h"
-
+#include "base/local_parametrization_base.h"
namespace wolf {
diff --git a/include/base/local_parametrization_quaternion.h b/include/base/local_parametrization_quaternion.h
index 6cbcae7bbd6b84cb8f56f363872dea707e1aa3f1..f92676558d4cbd8da107852779241a20836c2fe0 100644
--- a/include/base/local_parametrization_quaternion.h
+++ b/include/base/local_parametrization_quaternion.h
@@ -8,8 +8,7 @@
#ifndef LOCAL_PARAMETRIZATION_QUATERNION_H_
#define LOCAL_PARAMETRIZATION_QUATERNION_H_
-#include "local_parametrization_base.h"
-
+#include "base/local_parametrization_base.h"
namespace wolf {
diff --git a/include/base/logging.h b/include/base/logging.h
index 03d21c002bb74fadb1b684c63f5ca45372f4589f..3593fc718de7b717db2d19f588fe8e97a1e814ce 100644
--- a/include/base/logging.h
+++ b/include/base/logging.h
@@ -17,7 +17,7 @@
#include "spdlog/fmt/bundled/ostream.h"
// Wolf includes
-#include "singleton.h"
+#include "base/singleton.h"
namespace wolf {
namespace internal {
diff --git a/include/base/map_base.h b/include/base/map_base.h
index ef08f9469c9efcbf7b8232619e494574d830193b..f9e5d32b6d344fa25077b30dd035835bb5378299 100644
--- a/include/base/map_base.h
+++ b/include/base/map_base.h
@@ -9,8 +9,8 @@ class LandmarkBase;
}
//Wolf includes
-#include "wolf.h"
-#include "node_base.h"
+#include "base/wolf.h"
+#include "base/node_base.h"
//std includes
diff --git a/include/base/motion_buffer.h b/include/base/motion_buffer.h
index 33dfa226e5972d3bd47f6794d0bb87cee38134f6..f8d0ad5be8e854eaf8badca494218b637795d16e 100644
--- a/include/base/motion_buffer.h
+++ b/include/base/motion_buffer.h
@@ -8,8 +8,8 @@
#ifndef SRC_MOTIONBUFFER_H_
#define SRC_MOTIONBUFFER_H_
-#include "wolf.h"
-#include "time_stamp.h"
+#include "base/wolf.h"
+#include "base/time_stamp.h"
#include <list>
#include <algorithm>
@@ -51,7 +51,6 @@ struct Motion
}; ///< One instance of the buffered data, corresponding to a particular time stamp.
-
/** \brief class for motion buffers.
*
* It consists of a buffer of pre-integrated motions (aka. delta-integrals) that is being filled
@@ -117,7 +116,6 @@ inline void MotionBuffer::setCalibrationPreint(const VectorXs& _calib_preint)
calib_preint_ = _calib_preint;
}
-
} // namespace wolf
#endif /* SRC_MOTIONBUFFER_H_ */
diff --git a/include/base/node_base.h b/include/base/node_base.h
index bedc866122037c8be8a1d8cee8db2dd009d28f88..2426f53a53357005f9e0fcab1ed0af72b7494b7a 100644
--- a/include/base/node_base.h
+++ b/include/base/node_base.h
@@ -2,8 +2,7 @@
#define NODE_BASE_H_
// Wolf includes
-#include "wolf.h"
-
+#include "base/wolf.h"
namespace wolf {
diff --git a/include/base/pinhole_tools.h b/include/base/pinhole_tools.h
index ac721bd97cf9bda8c4d0e7e3b9888586a977daf2..46cb1fb9dd1f12dd18d3a520cc574488e7c9f170 100644
--- a/include/base/pinhole_tools.h
+++ b/include/base/pinhole_tools.h
@@ -7,12 +7,11 @@
* \date 06/04/2010
* \author jsola
*
- *
* ## Add a description here ##
*
*/
-#include "wolf.h"
+#include "base/wolf.h"
#include <iostream>
@@ -131,7 +130,6 @@ projectPointToNormalizedPlane(const MatrixBase<Derived1>& _v,
_dist = _v.norm();
}
-
/**
* Canonical back-projection.
* \param _u the 2D point in the image plane
@@ -262,7 +260,6 @@ Matrix<typename Derived2::Scalar, 2, 1> distortPoint(const MatrixBase<Derived1>
}
}
-
/**
* Radial distortion: ud = (1 + d_0 * r^2 + d_1 * r^4 + d_2 * r^6 + etc) * u, with jacobians
* \param d the radial distortion parameters vector
@@ -398,7 +395,6 @@ void undistortPoint(const MatrixBase<Derived1>& c,
}
}
-
/**
* Pixellization from k = [u_0, v_0, a_u, a_v]
* \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
@@ -426,8 +422,6 @@ Matrix<typename Derived2::Scalar, 2, 1> pixellizePoint(const MatrixBase<Derived1
return u;
}
-
-
/**
* Pixellization from k = [u_0, v_0, a_u, a_v] with jacobians
* \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
@@ -459,7 +453,6 @@ void pixellizePoint(const MatrixBase<Derived1>& k,
U_ud(1, 1) = a_v;
}
-
/**
* Depixellization from k = [u_0, v_0, a_u, a_v]
* \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
@@ -487,7 +480,6 @@ Matrix<typename Derived2::Scalar, 2, 1> depixellizePoint(const MatrixBase<Derive
return ud;
}
-
/**
* Depixellization from k = [u_0, v_0, a_u, a_v], with Jacobians
* \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
@@ -518,7 +510,6 @@ void depixellizePoint(const MatrixBase<Derived1>& k,
UD_u(1, 1) = 1.0 / a_v;
}
-
/**
* Project a point into a pin-hole camera with radial distortion
* \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
@@ -594,8 +585,6 @@ void projectPoint(const MatrixBase<Derived1>& k,
u = pixellizePoint( k, distortPoint( d, up ));
}
-
-
/**
* Project a point into a pin-hole camera with radial distortion
* \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
@@ -630,7 +619,6 @@ void projectPoint(const MatrixBase<Derived1>& k,
U_v.noalias() = U_ud * UD_up * UP_v;
}
-
/**
* Back-Project a point from a pin-hole camera with radial distortion
* \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
@@ -688,7 +676,6 @@ void backprojectPoint(const MatrixBase<Derived1>& k,
P_u.noalias() = P_up * UP_ud * UD_u;
}
-
/**
* Determine if a pixel is inside the region of interest
* \param pix the pixel to test
@@ -713,7 +700,6 @@ bool isInImage(const MatrixBase<VPix> & pix, const int & width, const int & heig
return isInRoi(pix, 0, 0, width, height);
}
-
/**
* Compute distortion correction parameters.
*
@@ -764,7 +750,6 @@ void computeCorrectionModel(const Vk & k, const Vd & d, Vc & c)
// this does not work:
// jmath::LinearSolvers::solve_Cholesky(Rd, (rc - rd), c);
-
// therefore we solve manually the pseudo-inverse:
Eigen::MatrixXs RdtRd(size, size);
RdtRd = Rd.transpose() * Rd;
@@ -780,9 +765,6 @@ void computeCorrectionModel(const Vk & k, const Vd & d, Vc & c)
} // namespace pinhole
-
-
} // namespace wolf
-
#endif // PINHOLETOOLS_H
diff --git a/include/base/problem.h b/include/base/problem.h
index 70cb8b7c4e58e31ad0a329056cffd411e537ead7..f0ade3c321d356a81367fbc1a9dc22d32a4d8da1 100644
--- a/include/base/problem.h
+++ b/include/base/problem.h
@@ -14,16 +14,14 @@ struct ProcessorParamsBase;
}
//wolf includes
-#include "wolf.h"
-#include "frame_base.h"
-#include "state_block.h"
+#include "base/wolf.h"
+#include "base/frame_base.h"
+#include "base/state_block.h"
// std includes
-
namespace wolf {
-
enum Notification
{
ADD,
@@ -91,7 +89,6 @@ class Problem : public std::enable_shared_from_this<Problem>
*/
SensorBasePtr getSensorPtr(const std::string& _sensor_name);
-
/** \brief Factory method to install (create, and add to sensor) processors only from its properties
*
* This method creates a Processor, and adds it to the specified sensor's list of processors
@@ -129,7 +126,6 @@ class Problem : public std::enable_shared_from_this<Problem>
void clearProcessorMotion();
ProcessorMotionPtr& getProcessorMotionPtr();
-
// Trajectory branch ----------------------------------
TrajectoryBasePtr getTrajectoryPtr();
virtual FrameBasePtr setPrior(const Eigen::VectorXs& _prior_state, //
@@ -198,7 +194,6 @@ class Problem : public std::enable_shared_from_this<Problem>
FrameBasePtr getLastKeyFramePtr ( );
FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts);
-
/** \brief Give the permission to a processor to create a new keyFrame
*
* This should implement a black list of processors that have forbidden keyframe creation.
@@ -225,8 +220,6 @@ class Problem : public std::enable_shared_from_this<Problem>
Eigen::VectorXs zeroState ( );
bool priorIsSet() const;
-
-
// Map branch -----------------------------------------
MapBasePtr getMapPtr();
LandmarkBasePtr addLandmark(LandmarkBasePtr _lmk_ptr);
@@ -235,8 +228,6 @@ class Problem : public std::enable_shared_from_this<Problem>
void saveMap(const std::string& _filename_dot_yaml, //
const std::string& _map_name = "Map automatically saved by Wolf");
-
-
// Covariances -------------------------------------------
void clearCovariance();
void addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXs& _cov);
@@ -329,5 +320,4 @@ inline std::map<ConstraintBasePtr,Notification>& Problem::getConstraintNotificat
} // namespace wolf
-
#endif // PROBLEM_H
diff --git a/include/base/processor/processor_GPS.h b/include/base/processor/processor_GPS.h
index a490bd2c1df80b40f6719a1266b705f154c716c3..35658410c4a0565c7da75976d72766b377e1712e 100644
--- a/include/base/processor/processor_GPS.h
+++ b/include/base/processor/processor_GPS.h
@@ -6,12 +6,11 @@
#define WOLF_PROCESSOR_GPS_H
// Wolf includes
-#include "processor_base.h"
-#include "capture_GPS.h"
+#include "base/processor/processor_base.h"
+#include "base/capture/capture_GPS.h"
// Std includes
-
namespace wolf {
WOLF_PTR_TYPEDEFS(ProcessorGPS);
diff --git a/include/base/processor/processor_IMU.h b/include/base/processor/processor_IMU.h
index 92eb67efbdae685dbca5df0e86e0205b21a1c800..98c7cffeef534a179e2127eb4b775e437a92c8d6 100644
--- a/include/base/processor/processor_IMU.h
+++ b/include/base/processor/processor_IMU.h
@@ -2,10 +2,9 @@
#define PROCESSOR_IMU_H
// Wolf
-#include "capture_IMU.h"
-#include "feature_IMU.h"
-#include "processor_motion.h"
-
+#include "base/capture/capture_IMU.h"
+#include "base/feature/feature_IMU.h"
+#include "base/processor/processor_motion.h"
namespace wolf {
WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsIMU);
diff --git a/include/base/processor/processor_base.h b/include/base/processor/processor_base.h
index bcf5c46b62730d593165f99fdb8d021296964812..6a1d23cfb3fd19a625407c4de20eb6d17784e371 100644
--- a/include/base/processor/processor_base.h
+++ b/include/base/processor/processor_base.h
@@ -7,10 +7,10 @@ class SensorBase;
}
// Wolf includes
-#include "wolf.h"
-#include "node_base.h"
-#include "time_stamp.h"
-#include "frame_base.h"
+#include "base/wolf.h"
+#include "base/node_base.h"
+#include "base/time_stamp.h"
+#include "base/frame_base.h"
// std
#include <memory>
@@ -35,8 +35,6 @@ class PackKeyFrame
WOLF_PTR_TYPEDEFS(PackKeyFrame);
-
-
/** \brief Buffer of Key frame class objects
*
* Object and functions to manage a buffer of KFPack objects.
@@ -216,8 +214,8 @@ inline void ProcessorBase::setVotingActive(bool _voting_active)
}
-#include "sensor_base.h"
-#include "constraint_base.h"
+#include "base/sensor/sensor_base.h"
+#include "base/constraint/constraint_base.h"
namespace wolf {
diff --git a/include/base/processor/processor_capture_holder.h b/include/base/processor/processor_capture_holder.h
index b79743f45e763ca630fa4fa7de31fb106cb16fd6..9067b4cae4870f3c53cc7400e7d943ae20822a42 100644
--- a/include/base/processor/processor_capture_holder.h
+++ b/include/base/processor/processor_capture_holder.h
@@ -9,9 +9,9 @@
#define _WOLF_PROCESSOR_CAPTURE_HOLDER_H_
//Wolf includes
-#include "processor_base.h"
-#include "capture_base.h"
-#include "capture_buffer.h"
+#include "base/processor/processor_base.h"
+#include "base/capture/capture_base.h"
+#include "base/capture/capture_buffer.h"
namespace wolf {
diff --git a/src/processor/processor_diff_drive.h b/include/base/processor/processor_diff_drive.h
similarity index 98%
rename from src/processor/processor_diff_drive.h
rename to include/base/processor/processor_diff_drive.h
index f06b5940fe4dd3b09fcf91863347a727541d61e7..e219b552f566153659b3f3643ce9a3210b89efe4 100644
--- a/src/processor/processor_diff_drive.h
+++ b/include/base/processor/processor_diff_drive.h
@@ -8,8 +8,8 @@
#ifndef _WOLF_PROCESSOR_DIFF_DRIVE_H_
#define _WOLF_PROCESSOR_DIFF_DRIVE_H_
-#include "../processor_motion.h"
-#include "../sensors/diff_drive_tools.h"
+#include "base/processor/processor_motion.h"
+#include "base/diff_drive_tools.h"
namespace wolf {
diff --git a/include/base/processor/processor_factory.h b/include/base/processor/processor_factory.h
index 00d2d50f6bfdc5605b3890ed6f49a2ca38023ba7..8077aae495c053e229fc8fef08790f1ca6e3b099 100644
--- a/include/base/processor/processor_factory.h
+++ b/include/base/processor/processor_factory.h
@@ -15,7 +15,7 @@ struct ProcessorParamsBase;
}
// wolf
-#include "factory.h"
+#include "base/factory.h"
// std
@@ -125,8 +125,8 @@ namespace wolf
* and bind it to a SensorOdom2D:
*
* \code
- * #include "sensor_odom_2D.h" // provides SensorOdom2D and SensorFactory
- * #include "processor_odom_2D.h" // provides ProcessorOdom2D and ProcessorFactory
+ * #include "base/sensor/sensor_odom_2D.h" // provides SensorOdom2D and SensorFactory
+ * #include "base/processor/processor_odom_2D.h" // provides ProcessorOdom2D and ProcessorFactory
*
* // Note: SensorOdom2D::create() is already registered, automatically.
* // Note: ProcessorOdom2D::create() is already registered, automatically.
@@ -153,9 +153,9 @@ namespace wolf
* The example 1 above can be accomplished as follows (we obviated for simplicity all the parameter creation),
*
* \code
- * #include "sensor_odom_2D.h"
- * #include "processor_odom_2D.h"
- * #include "problem.h"
+ * #include "base/sensor/sensor_odom_2D.h"
+ * #include "base/processor/processor_odom_2D.h"
+ * #include "base/problem.h"
*
* Problem problem(FRM_PO_2D);
* problem.installSensor ( "ODOM 2D" , "Main odometer" , extrinsics , &intrinsics );
@@ -175,7 +175,6 @@ inline std::string ProcessorFactory::getClass()
return "ProcessorFactory";
}
-
#define WOLF_REGISTER_PROCESSOR(ProcessorType, ProcessorName) \
namespace{ const bool WOLF_UNUSED ProcessorName##Registered = \
wolf::ProcessorFactory::get().registerCreator(ProcessorType, ProcessorName::create); }\
diff --git a/include/base/processor/processor_frame_nearest_neighbor_filter.h b/include/base/processor/processor_frame_nearest_neighbor_filter.h
index 1d9fb56b6db4ac05462699b35a4ff80eeeef3f52..be1205e27988461473786627ca5e0d3acc62748f 100644
--- a/include/base/processor/processor_frame_nearest_neighbor_filter.h
+++ b/include/base/processor/processor_frame_nearest_neighbor_filter.h
@@ -2,8 +2,8 @@
#define _WOLF_SRC_PROCESSOR_FRAME_NEAREST_NEIGHBOR_FILTER_H
// Wolf related headers
-#include "processor_loopclosure_base.h"
-#include "state_block.h"
+#include "base/processor/processor_loopclosure_base.h"
+#include "base/state_block.h"
namespace wolf{
diff --git a/include/base/processor/processor_logging.h b/include/base/processor/processor_logging.h
index 167beb892bb2dfe777426d14d535daf7e28cd1ac..4add7a8e9d7945f0aaab54c8e7e7f1acdc4bfd8e 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 "logging.h"
+//#include "base/logging.h"
#define __INTERNAL_WOLF_ASSERT_PROCESSOR \
static_assert(std::is_base_of<ProcessorBase, \
diff --git a/include/base/processor/processor_loopclosure_base.h b/include/base/processor/processor_loopclosure_base.h
index 78dd9afce1c87054283410e4392c78ef6602e108..1ded95bf036f2569e95f1ec9f41b5141789f70d5 100644
--- a/include/base/processor/processor_loopclosure_base.h
+++ b/include/base/processor/processor_loopclosure_base.h
@@ -2,7 +2,7 @@
#define _WOLF_PROCESSOR_LOOPCLOSURE_BASE_H
// Wolf related headers
-#include "processor_base.h"
+#include "base/processor/processor_base.h"
namespace wolf{
@@ -35,7 +35,6 @@ class ProcessorLoopClosureBase : public ProcessorBase
{
protected:
-
ProcessorParamsLoopClosurePtr params_loop_closure_;
// Frames that are possible loop closure candidates according to
@@ -96,7 +95,6 @@ protected:
*/
virtual void postProcess() { }
-
/** Find a loop closure between incoming data and all keyframe data
*
* This is called by process() .
diff --git a/include/base/processor/processor_motion.h b/include/base/processor/processor_motion.h
index e399bddcce6562e265d540c9e5699cd8e7f64193..0ce05f202e9eb68290871cbcd2319b8c12528a99 100644
--- a/include/base/processor/processor_motion.h
+++ b/include/base/processor/processor_motion.h
@@ -9,9 +9,9 @@
#define PROCESSOR_MOTION_H_
// Wolf
-#include "capture_motion.h"
-#include "processor_base.h"
-#include "time_stamp.h"
+#include "base/capture/capture_motion.h"
+#include "base/processor/processor_base.h"
+#include "base/time_stamp.h"
// std
#include <iomanip>
@@ -91,7 +91,6 @@ struct ProcessorParamsMotion : public ProcessorParamsBase
*
* \code statePlusDelta(R_old, delta_R, R_new) \endcode
*
- *
* ### Defining (or not) the fromSensorFrame():
*
* In most cases, one will be interested in avoiding the \b fromSensorFrame() issue.
@@ -201,7 +200,6 @@ class ProcessorMotion : public ProcessorBase
MotionBuffer& getBuffer();
const MotionBuffer& getBuffer() const;
-
// Helper functions:
protected:
@@ -237,11 +235,9 @@ class ProcessorMotion : public ProcessorBase
PackKeyFramePtr computeProcessingStep();
-
// These are the pure virtual functions doing the mathematics
protected:
-
/** \brief convert raw CaptureMotion data to the delta-state format
*
* This function accepts raw data and time step dt,
@@ -355,7 +351,6 @@ class ProcessorMotion : public ProcessorBase
*/
virtual Eigen::VectorXs deltaZero() const = 0;
-
/** \brief Interpolate motion to an intermediate time-stamp
*
* @param _ref The motion reference
@@ -449,8 +444,6 @@ class ProcessorMotion : public ProcessorBase
void setDistTraveled(const Scalar& _dist_traveled);
void setAngleTurned(const Scalar& _angle_turned);
-
-
protected:
// Attributes
SizeEigen x_size_; ///< The size of the state vector
@@ -481,7 +474,7 @@ class ProcessorMotion : public ProcessorBase
}
-#include "frame_base.h"
+#include "base/frame_base.h"
namespace wolf{
@@ -558,7 +551,6 @@ inline const MotionBuffer& ProcessorMotion::getBuffer() const
return last_ptr_->getBuffer();
}
-
inline MotionBuffer& ProcessorMotion::getBuffer()
{
return last_ptr_->getBuffer();
@@ -631,8 +623,6 @@ inline void ProcessorMotion::setAngleTurned(const Scalar& _angle_turned)
params_motion_->angle_turned = _angle_turned;
}
-
-
} // namespace wolf
#endif /* PROCESSOR_MOTION2_H_ */
diff --git a/include/base/processor/processor_odom_2D.h b/include/base/processor/processor_odom_2D.h
index bf797ca8b24f488234c1f7eb5a3c7cf3f317e46c..b7ea895dff86ca53d06ec5eb38e9e0473de7711c 100644
--- a/include/base/processor/processor_odom_2D.h
+++ b/include/base/processor/processor_odom_2D.h
@@ -8,11 +8,10 @@
#ifndef SRC_PROCESSOR_ODOM_2D_H_
#define SRC_PROCESSOR_ODOM_2D_H_
-#include "processor_motion.h"
-#include "capture_odom_2D.h"
-#include "constraint_odom_2D.h"
-#include "rotations.h"
-
+#include "base/processor/processor_motion.h"
+#include "base/capture/capture_odom_2D.h"
+#include "base/constraint/constraint_odom_2D.h"
+#include "base/rotations.h"
namespace wolf {
diff --git a/include/base/processor/processor_odom_3D.h b/include/base/processor/processor_odom_3D.h
index 9a9828d4b2dcc8574de9b22ba8d55503e7a3875a..77766efc3cef5cb5db3451c85e8f536e5d27c5ac 100644
--- a/include/base/processor/processor_odom_3D.h
+++ b/include/base/processor/processor_odom_3D.h
@@ -8,14 +8,13 @@
#ifndef SRC_PROCESSOR_ODOM_3D_H_
#define SRC_PROCESSOR_ODOM_3D_H_
-#include "processor_motion.h"
-#include "sensor_odom_3D.h"
-#include "capture_odom_3D.h"
-#include "constraint_odom_3D.h"
-#include "rotations.h"
+#include "base/processor/processor_motion.h"
+#include "base/sensor/sensor_odom_3D.h"
+#include "base/capture/capture_odom_3D.h"
+#include "base/constraint/constraint_odom_3D.h"
+#include "base/rotations.h"
#include <cmath>
-
namespace wolf {
WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom3D);
@@ -25,7 +24,6 @@ struct ProcessorParamsOdom3D : public ProcessorParamsMotion
//
};
-
WOLF_PTR_TYPEDEFS(ProcessorOdom3D);
/** \brief Processor for 3d odometry integration.
diff --git a/include/base/processor/processor_params_image.h b/include/base/processor/processor_params_image.h
index fde468176228453f2bd25551c6beb87617b29b67..6ce3eaace3119ed1e68725bb33d2ca0df4684bbb 100644
--- a/include/base/processor/processor_params_image.h
+++ b/include/base/processor/processor_params_image.h
@@ -2,8 +2,8 @@
#define PROCESSOR_IMAGE_PARAMS_H
// wolf
-#include "processor_tracker_feature.h"
-#include "processor_tracker_landmark.h"
+#include "base/processor/processor_tracker_feature.h"
+#include "base/processor/processor_tracker_landmark.h"
namespace wolf
{
diff --git a/include/base/processor/processor_tracker.h b/include/base/processor/processor_tracker.h
index 5d18402b68ba98ca8ce687e79a9222376ac7b365..f132626d7a028855fbaae0856e09c0995665f9f9 100644
--- a/include/base/processor/processor_tracker.h
+++ b/include/base/processor/processor_tracker.h
@@ -8,8 +8,8 @@
#ifndef SRC_PROCESSOR_TRACKER_H_
#define SRC_PROCESSOR_TRACKER_H_
-#include "processor_base.h"
-#include "capture_motion.h"
+#include "base/processor/processor_base.h"
+#include "base/capture/capture_motion.h"
namespace wolf {
diff --git a/include/base/processor/processor_tracker_feature.h b/include/base/processor/processor_tracker_feature.h
index fc35c5465326d31eeba705523430ace7f3fda131..52bd493f6870a1424621f5085ebed10b9558df60 100644
--- a/include/base/processor/processor_tracker_feature.h
+++ b/include/base/processor/processor_tracker_feature.h
@@ -9,11 +9,11 @@
#define PROCESSOR_TRACKER_FEATURE_H_
//wolf includes
-#include "processor_tracker.h"
-#include "capture_base.h"
-#include "feature_match.h"
-#include "track_matrix.h"
-#include "wolf.h"
+#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"
namespace wolf
{
diff --git a/include/base/processor/processor_tracker_feature_corner.h b/include/base/processor/processor_tracker_feature_corner.h
index efc40a1451b6015d86ce06e2538474bd0ae425a3..5bc33bf47a51b4b267c64ecf916ccc271047c280 100644
--- a/include/base/processor/processor_tracker_feature_corner.h
+++ b/include/base/processor/processor_tracker_feature_corner.h
@@ -9,14 +9,14 @@
#define PROCESSOR_TRACKER_FEATURE_CORNER_H_
// Wolf includes
-#include "sensor_laser_2D.h"
-#include "capture_laser_2D.h"
-#include "feature_corner_2D.h"
-#include "landmark_corner_2D.h"
-#include "constraint_corner_2D.h"
-#include "state_block.h"
-#include "data_association/association_tree.h"
-#include "processor_tracker_feature.h"
+#include "base/sensor/sensor_laser_2D.h"
+#include "base/capture/capture_laser_2D.h"
+#include "base/feature/feature_corner_2D.h"
+#include "base/landmark/landmark_corner_2D.h"
+#include "base/constraint/constraint_corner_2D.h"
+#include "base/state_block.h"
+#include "base/association/association_tree.h"
+#include "base/processor/processor_tracker_feature.h"
//laser_scan_utils
//#include "laser_scan_utils/scan_basics.h"
@@ -25,7 +25,6 @@
#include "laser_scan_utils/line_finder_iterative.h"
#include "laser_scan_utils/corner_finder.h"
-
namespace wolf
{
WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeatureCorner);
diff --git a/include/base/processor/processor_tracker_feature_dummy.h b/include/base/processor/processor_tracker_feature_dummy.h
index 3c163d276c282c38b69691b2fa8e0924b38fb3e8..dbc024b7c1187c5a9cf4b0753aaa8d0fc56fe050 100644
--- a/include/base/processor/processor_tracker_feature_dummy.h
+++ b/include/base/processor/processor_tracker_feature_dummy.h
@@ -8,9 +8,9 @@
#ifndef PROCESSOR_TRACKER_FEATURE_DUMMY_H_
#define PROCESSOR_TRACKER_FEATURE_DUMMY_H_
-#include "wolf.h"
-#include "processor_tracker_feature.h"
-#include "constraint_epipolar.h"
+#include "base/wolf.h"
+#include "base/processor/processor_tracker_feature.h"
+#include "base/constraint/constraint_epipolar.h"
namespace wolf
{
diff --git a/include/base/processor/processor_tracker_feature_image.h b/include/base/processor/processor_tracker_feature_image.h
index 792c5d64527e7c6308fd071383be2ea22b82785b..48f7e7a705bd0b2006f5ec009653b895e211b11d 100644
--- a/include/base/processor/processor_tracker_feature_image.h
+++ b/include/base/processor/processor_tracker_feature_image.h
@@ -2,14 +2,14 @@
#define PROCESSOR_TRACKER_FEATURE_IMAGE_H
// Wolf includes
-#include "sensor_camera.h"
-#include "capture_image.h"
-#include "feature_point_image.h"
-#include "state_block.h"
-#include "state_quaternion.h"
-#include "processor_tracker_feature.h"
-#include "constraint_epipolar.h"
-#include "processor_params_image.h"
+#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/processor/processor_tracker_feature.h"
+#include "base/constraint/constraint_epipolar.h"
+#include "base/processor/processor_params_image.h"
// vision_utils
#include <detectors/detector_base.h>
diff --git a/include/base/processor/processor_tracker_landmark.h b/include/base/processor/processor_tracker_landmark.h
index bae94066b540cc1a566e9e9fe5c146262797c8bf..293570afa29406dfc8515ef9c7d684af2da35cf9 100644
--- a/include/base/processor/processor_tracker_landmark.h
+++ b/include/base/processor/processor_tracker_landmark.h
@@ -9,9 +9,9 @@
#define PROCESSOR_TRACKER_LANDMARK_H_
//wolf includes
-#include "processor_tracker.h"
-#include "capture_base.h"
-#include "landmark_match.h"
+#include "base/processor/processor_tracker.h"
+#include "base/capture/capture_base.h"
+#include "base/landmark/landmark_match.h"
namespace wolf
{
@@ -23,7 +23,6 @@ struct ProcessorParamsTrackerLandmark : public ProcessorParamsTracker
//
};
-
WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
/** \brief Landmark tracker processor
@@ -168,6 +167,6 @@ class ProcessorTrackerLandmark : public ProcessorTracker
}// namespace wolf
// IMPLEMENTATION
-#include "landmark_base.h"
+#include "base/landmark/landmark_base.h"
#endif /* PROCESSOR_TRACKER_LANDMARK_H_ */
diff --git a/include/base/processor/processor_tracker_landmark_corner.h b/include/base/processor/processor_tracker_landmark_corner.h
index bb9eff8db34f6f46dcdd4c34d15ea7487ff1e74c..bd81b1643980f41468180735b85d9dcabd550346 100644
--- a/include/base/processor/processor_tracker_landmark_corner.h
+++ b/include/base/processor/processor_tracker_landmark_corner.h
@@ -9,14 +9,14 @@
#define SRC_PROCESSOR_TRACKER_LANDMARK_CORNER_H_
// Wolf includes
-#include "sensor_laser_2D.h"
-#include "capture_laser_2D.h"
-#include "feature_corner_2D.h"
-#include "landmark_corner_2D.h"
-#include "constraint_corner_2D.h"
-#include "state_block.h"
-#include "data_association/association_tree.h"
-#include "processor_tracker_landmark.h"
+#include "base/sensor/sensor_laser_2D.h"
+#include "base/capture/capture_laser_2D.h"
+#include "base/feature/feature_corner_2D.h"
+#include "base/landmark/landmark_corner_2D.h"
+#include "base/constraint/constraint_corner_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/scan_basics.h"
@@ -34,7 +34,6 @@ const Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees;
const Scalar position_error_th_ = 1;
const Scalar min_features_ratio_th_ = 0.5;
-
//forward declaration to typedef class pointers
struct ProcessorParamsLaser;
typedef std::shared_ptr<ProcessorParamsLaser> ProcessorParamsLaserPtr;
@@ -62,7 +61,6 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
laserscanutils::CornerFinder corner_finder_;
//TODO: add corner_finder_params
-
FeatureBaseList corners_incoming_;
FeatureBaseList corners_last_;
unsigned int new_corners_th_;
diff --git a/include/base/processor/processor_tracker_landmark_dummy.h b/include/base/processor/processor_tracker_landmark_dummy.h
index 410b9c72c129ee2403b318bd6701f1d85b1f7cdc..1d9d66bbaeef70cdf9366180ff55b796d78b73ac 100644
--- a/include/base/processor/processor_tracker_landmark_dummy.h
+++ b/include/base/processor/processor_tracker_landmark_dummy.h
@@ -8,7 +8,7 @@
#ifndef PROCESSOR_TRACKER_LANDMARK_DUMMY_H_
#define PROCESSOR_TRACKER_LANDMARK_DUMMY_H_
-#include "processor_tracker_landmark.h"
+#include "base/processor/processor_tracker_landmark.h"
namespace wolf
{
diff --git a/include/base/processor/processor_tracker_landmark_image.h b/include/base/processor/processor_tracker_landmark_image.h
index 6d86249e7bd10f151c074c96166ff15f2a730441..1b7a0dbd6d5d33d3adb40742768b5470bb2a3c11 100644
--- a/include/base/processor/processor_tracker_landmark_image.h
+++ b/include/base/processor/processor_tracker_landmark_image.h
@@ -3,11 +3,11 @@
// Wolf includes
-#include "landmark_AHP.h"
-#include "landmark_match.h"
-#include "processor_params_image.h"
-#include "processor_tracker_landmark.h"
-#include "wolf.h"
+#include "base/landmark/landmark_AHP.h"
+#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 <algorithms/activesearch/alg_activesearch.h>
#include <descriptors/descriptor_base.h>
@@ -102,7 +102,6 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
image_last_ = image_incoming_;
}
-
/**
* \brief Does the drawing of the features.
*
@@ -147,8 +146,6 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
public:
static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
-
-
/** \brief Create a new constraint
* \param _feature_ptr pointer to the Feature to constrain
* \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
@@ -190,8 +187,6 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
};
-
} // namespace wolf
-
#endif // PROCESSOR_TRACKER_LANDMARK_IMAGE_H
diff --git a/include/base/processor/processor_tracker_landmark_polyline.h b/include/base/processor/processor_tracker_landmark_polyline.h
index 3c4c6903171739283dff0cc47fe69fc1a824e304..e8d70824a898a85130775aed2bb712945ef4e6fc 100644
--- a/include/base/processor/processor_tracker_landmark_polyline.h
+++ b/include/base/processor/processor_tracker_landmark_polyline.h
@@ -9,15 +9,15 @@
#define SRC_PROCESSOR_TRACKER_LANDMARK_POLYLINE_H_
// Wolf includes
-#include "sensor_laser_2D.h"
-#include "capture_laser_2D.h"
-#include "feature_polyline_2D.h"
-#include "landmark_polyline_2D.h"
-#include "constraint_point_2D.h"
-#include "constraint_point_to_line_2D.h"
-#include "state_block.h"
-#include "data_association/association_tree.h"
-#include "processor_tracker_landmark.h"
+#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/constraint/constraint_point_2D.h"
+#include "base/constraint/constraint_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"
@@ -31,7 +31,6 @@ namespace wolf
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;
@@ -40,10 +39,8 @@ typedef std::shared_ptr<LandmarkPolylineMatch> LandmarkPolylineMatchPtr;
struct ProcessorParamsPolyline;
typedef std::shared_ptr<ProcessorParamsPolyline> ProcessorParamsPolylinePtr;
-
WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkPolyline);
-
// Match Feature - Landmark
struct LandmarkPolylineMatch : public LandmarkMatch
{
diff --git a/include/base/rotations.h b/include/base/rotations.h
index 2ce22553796f83d6f3cdfbe497f57f5dab3ae326..d4d3e711f119139e4ee2385897a12b07c60354b2 100644
--- a/include/base/rotations.h
+++ b/include/base/rotations.h
@@ -8,7 +8,7 @@
#ifndef ROTATIONS_H_
#define ROTATIONS_H_
-#include "wolf.h"
+#include "base/wolf.h"
namespace wolf
{
@@ -96,7 +96,6 @@ vee(const Eigen::MatrixBase<Derived>& _m)
return (Eigen::Matrix<T, 3, 1>() << _m(2, 1), _m(0, 2), _m(1, 0)).finished();
}
-
////////////////////////////////////////////////////
// Rotation conversions q, R, Euler, back and forth
//
@@ -161,7 +160,6 @@ e2q(const Eigen::MatrixBase<D>& _euler)
return Eigen::Quaternion<T>(az * ay * ax);
}
-
/** \brief Euler angles to rotation matrix
* \param _e = (roll pitch yaw) in ZYX convention
* \return equivalent rotation matrix
@@ -175,7 +173,6 @@ e2R(const Eigen::MatrixBase<Derived>& _e)
return e2q(_e).matrix();
}
-
template <typename Derived>
inline typename Eigen::MatrixBase<Derived>::Scalar
getYaw(const Eigen::MatrixBase<Derived>& R)
@@ -199,7 +196,6 @@ R2e(const Eigen::MatrixBase<Derived>& _R)
return e;
}
-
template<typename Derived>
inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1>
q2e(const Eigen::MatrixBase<Derived>& _q)
@@ -235,7 +231,6 @@ q2e(const Eigen::MatrixBase<Derived>& _q)
return e;
}
-
template<typename Derived>
inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1>
q2e(const Eigen::QuaternionBase<Derived>& _q)
@@ -243,9 +238,6 @@ q2e(const Eigen::QuaternionBase<Derived>& _q)
return q2e(_q.coeffs());
}
-
-
-
///////////////////////////////////////////////////////////////
// Rotation conversions - exp and log maps
@@ -295,7 +287,6 @@ log_q(const Eigen::QuaternionBase<Derived>& _q)
// Eigen::AngleAxis<T> aa(_q);
// return aa.angle() * aa.axis();
-
// In the meanwhile, we have a custom implementation as follows
typedef typename Derived::Scalar T;
@@ -411,8 +402,6 @@ R2v(const Eigen::MatrixBase<Derived>& _R)
return log_R(_R);
}
-
-
/////////////////////////////////////////////////////////////////
// Jacobians of SO(3)
@@ -651,7 +640,6 @@ diff(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
return minus(q1, q2);
}
-
} // namespace wolf
#endif /* ROTATIONS_H_ */
diff --git a/include/base/sensor/sensor_GPS.h b/include/base/sensor/sensor_GPS.h
index c43cd13cb8a766fd6cfea67000d16d4262ee5f2a..17b6f20331e6e990b876cb52b1b83277d404f404 100644
--- a/include/base/sensor/sensor_GPS.h
+++ b/include/base/sensor/sensor_GPS.h
@@ -9,9 +9,9 @@
*/
//wolf
-#include "sensor_base.h"
+#include "base/sensor/sensor_base.h"
//#include "sensor_factory.h"
-//#include "factory.h"
+//#include "base/factory.h"
// std
@@ -40,7 +40,6 @@ public:
public:
static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_p, const IntrinsicsBasePtr _intrinsics);
-
};
} // namespace wolf
diff --git a/include/base/sensor/sensor_GPS_fix.h b/include/base/sensor/sensor_GPS_fix.h
index c319d159f0adce4734f832a24c40ba361c8ba64b..e74ff5e89f31b3fc0759f8d62bd76448c956c593 100644
--- a/include/base/sensor/sensor_GPS_fix.h
+++ b/include/base/sensor/sensor_GPS_fix.h
@@ -3,13 +3,10 @@
#define SENSOR_GPS_FIX_H_
//wolf includes
-#include "sensor_base.h"
+#include "base/sensor/sensor_base.h"
// std includes
-
-
-
namespace wolf {
WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsGPSFix);
diff --git a/include/base/sensor/sensor_IMU.h b/include/base/sensor/sensor_IMU.h
index 2f5d9baa576f697772800359a46a239170f39144..17c1c873d921032c05700a558abf6151e3f3255b 100644
--- a/include/base/sensor/sensor_IMU.h
+++ b/include/base/sensor/sensor_IMU.h
@@ -2,7 +2,7 @@
#define SENSOR_IMU_H
//wolf includes
-#include "sensor_base.h"
+#include "base/sensor/sensor_base.h"
#include <iostream>
namespace wolf {
diff --git a/include/base/sensor/sensor_base.h b/include/base/sensor/sensor_base.h
index b52e8816e8d91455cda89c1ee2687c6754569f92..8707d13ac8bcac4b288f139dec53045625f02ac9 100644
--- a/include/base/sensor/sensor_base.h
+++ b/include/base/sensor/sensor_base.h
@@ -9,15 +9,14 @@ class StateBlock;
}
//Wolf includes
-#include "wolf.h"
-#include "node_base.h"
-#include "time_stamp.h"
+#include "base/wolf.h"
+#include "base/node_base.h"
+#include "base/time_stamp.h"
//std includes
namespace wolf {
-
/** \brief base struct for intrinsic sensor parameters
*
* Derive from this struct to create structs of sensor intrinsic parameters.
@@ -162,10 +161,10 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
}
-#include "problem.h"
-#include "hardware_base.h"
-#include "capture_base.h"
-#include "processor_base.h"
+#include "base/problem.h"
+#include "base/hardware_base.h"
+#include "base/capture/capture_base.h"
+#include "base/processor/processor_base.h"
namespace wolf{
@@ -273,8 +272,6 @@ inline void SensorBase::setIntrinsicDynamic(bool _intrinsic_dynamic)
intrinsic_dynamic_ = _intrinsic_dynamic;
}
-
-
} // namespace wolf
#endif
diff --git a/include/base/sensor/sensor_camera.h b/include/base/sensor/sensor_camera.h
index d8641510db8225e874e06526665199e8bc316d2f..2214fc14a317f07c072f64462feac9870388dee6 100644
--- a/include/base/sensor/sensor_camera.h
+++ b/include/base/sensor/sensor_camera.h
@@ -2,7 +2,7 @@
#define SENSOR_CAMERA_H
//wolf includes
-#include "sensor_base.h"
+#include "base/sensor/sensor_base.h"
namespace wolf
{
@@ -20,7 +20,6 @@ struct IntrinsicsCamera : public IntrinsicsBase
virtual ~IntrinsicsCamera() = default;
};
-
WOLF_PTR_TYPEDEFS(SensorCamera);
/**Pin-hole camera sensor
*/
diff --git a/src/sensor/sensor_diff_drive.h b/include/base/sensor/sensor_diff_drive.h
similarity index 97%
rename from src/sensor/sensor_diff_drive.h
rename to include/base/sensor/sensor_diff_drive.h
index 925c840e51f93ade1e6517cfd8409befaf851d8f..0f5cf42592ddf2cf1929ff6e31a9aa294a1963e4 100644
--- a/src/sensor/sensor_diff_drive.h
+++ b/include/base/sensor/sensor_diff_drive.h
@@ -9,8 +9,8 @@
#define WOLF_SENSOR_DIFF_DRIVE_H_
//wolf includes
-#include "../sensor_base.h"
-#include "diff_drive_tools.h"
+#include "base/sensor/sensor_base.h"
+#include "base/diff_drive_tools.h"
namespace wolf {
diff --git a/include/base/sensor/sensor_factory.h b/include/base/sensor/sensor_factory.h
index 3e7e1aabc300d286fbf1c37373de0ba7614f75f1..32884c18f0b7b1e3133648ebb946d216898a8f56 100644
--- a/include/base/sensor/sensor_factory.h
+++ b/include/base/sensor/sensor_factory.h
@@ -15,7 +15,7 @@ struct IntrinsicsBase;
}
// wolf
-#include "factory.h"
+#include "base/factory.h"
namespace wolf
{
@@ -174,7 +174,7 @@ namespace wolf
*
* \code
* #include "sensor_factory.h"
- * #include "sensor_camera.h" // provides SensorCamera
+ * #include "base/sensor/sensor_camera.h" // provides SensorCamera
*
* // Note: SensorCamera::create() is already registered, automatically.
*
diff --git a/include/base/sensor/sensor_laser_2D.h b/include/base/sensor/sensor_laser_2D.h
index 9cdcf5567499c2993d67f53535f0e1d34ed2682d..6b927c5b4bbbdc23a05cad52cc9f1be4a8590733 100644
--- a/include/base/sensor/sensor_laser_2D.h
+++ b/include/base/sensor/sensor_laser_2D.h
@@ -4,7 +4,7 @@
#define SENSOR_LASER_2D_H_
//wolf
-#include "sensor_base.h"
+#include "base/sensor/sensor_base.h"
//laser_scan_utils
#include "laser_scan_utils/laser_scan.h"
diff --git a/include/base/sensor/sensor_odom_2D.h b/include/base/sensor/sensor_odom_2D.h
index 4ba0fc37dee5a0d4e487548c894f32fff4fc2a86..75110b2cd69595555c9a6969f3e3cbbefbf13065 100644
--- a/include/base/sensor/sensor_odom_2D.h
+++ b/include/base/sensor/sensor_odom_2D.h
@@ -3,7 +3,7 @@
#define SENSOR_ODOM_2D_H_
//wolf includes
-#include "sensor_base.h"
+#include "base/sensor/sensor_base.h"
namespace wolf {
@@ -59,7 +59,6 @@ class SensorOdom2D : public SensorBase
public:
static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics);
-
};
} // namespace wolf
diff --git a/include/base/sensor/sensor_odom_3D.h b/include/base/sensor/sensor_odom_3D.h
index 19f037162677691f08b2deff390c42b49ab706fd..76c9a3d5b9e9314e8e45f50f3520175ba03e893b 100644
--- a/include/base/sensor/sensor_odom_3D.h
+++ b/include/base/sensor/sensor_odom_3D.h
@@ -9,7 +9,7 @@
#define SRC_SENSOR_ODOM_3D_H_
//wolf includes
-#include "sensor_base.h"
+#include "base/sensor/sensor_base.h"
namespace wolf {
@@ -60,7 +60,6 @@ class SensorOdom3D : public SensorBase
public:
static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics);
-
};
inline Scalar SensorOdom3D::getDispVarToDispNoiseFactor() const
diff --git a/include/base/solver/solver_manager.h b/include/base/solver/solver_manager.h
index 9f1189495669a0b02648c2935f09494e40438c0d..4801fd57f08f9a546b4255f51f9e11e4ca1bd354 100644
--- a/include/base/solver/solver_manager.h
+++ b/include/base/solver/solver_manager.h
@@ -2,9 +2,9 @@
#define _WOLF_SOLVER_MANAGER_H_
//wolf includes
-#include "../wolf.h"
-#include "../state_block.h"
-#include "../constraint_base.h"
+#include "base/wolf.h"
+#include "base/state_block.h"
+#include "base/constraint/constraint_base.h"
namespace wolf {
diff --git a/include/base/solver_suitesparse/ccolamd_ordering.h b/include/base/solver_suitesparse/ccolamd_ordering.h
index d88c0d41bb95e7472303a5b6d3562097dbe71a60..bae2eff4d4e15611126c3b9806aeff2664740a14 100644
--- a/include/base/solver_suitesparse/ccolamd_ordering.h
+++ b/include/base/solver_suitesparse/ccolamd_ordering.h
@@ -8,8 +8,6 @@
#ifndef TRUNK_SRC_WOLF_SOLVER_CCOLAMD_ORDERING_H_
#define TRUNK_SRC_WOLF_SOLVER_CCOLAMD_ORDERING_H_
-
-
//std includes
#include <iostream>
@@ -79,5 +77,4 @@ class CCOLAMDOrdering
};
}
-
#endif /* TRUNK_SRC_WOLF_SOLVER_CCOLAMD_ORDERING_H_ */
diff --git a/include/base/solver_suitesparse/cost_function_base.h b/include/base/solver_suitesparse/cost_function_base.h
index 76ba74da37e4fda29c140ecd033ede9597834dd8..8aae707aed16ec7229c3d4a6730a148d6492c785 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 "wolf.h"
+#include "base/wolf.h"
#include <Eigen/StdVector>
class CostFunctionBase
@@ -75,7 +75,6 @@ class CostFunctionBase
virtual void evaluateResidualJacobians() = 0;
-
void getResidual(Eigen::VectorXs &residual)
{
residual.resize(residual_.size());
diff --git a/include/base/solver_suitesparse/cost_function_sparse.h b/include/base/solver_suitesparse/cost_function_sparse.h
index 37033bcb95a802de0657b6d11b662d8c49e5416a..713e29bd3308ac58d4d9c48c267fef2e18817614 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 "wolf.h"
+#include "base/wolf.h"
#include "cost_function_sparse_base.h"
// CERES JET
@@ -229,7 +229,6 @@ class CostFunctionSparse<ConstraintT,
BLOCK_5_SIZE,
BLOCK_6_SIZE,
0,
- 0,
0> : CostFunctionSparseBase<ConstraintT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
@@ -240,7 +239,6 @@ class CostFunctionSparse<ConstraintT,
BLOCK_5_SIZE,
BLOCK_6_SIZE,
0,
- 0,
0>
{
public:
@@ -255,7 +253,6 @@ class CostFunctionSparse<ConstraintT,
BLOCK_5_SIZE,
BLOCK_6_SIZE,
0,
- 0,
0>(_constraint_ptr)
{
@@ -284,8 +281,6 @@ class CostFunctionSparse<ConstraintT,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
0,
- 0,
- 0,
0> : CostFunctionSparseBase<ConstraintT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
@@ -295,8 +290,6 @@ class CostFunctionSparse<ConstraintT,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
0,
- 0,
- 0,
0>
{
public:
@@ -310,8 +303,6 @@ class CostFunctionSparse<ConstraintT,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
0,
- 0,
- 0,
0>(_constraint_ptr)
{
@@ -338,9 +329,6 @@ class CostFunctionSparse<ConstraintT,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
0,
- 0,
- 0,
- 0,
0> : CostFunctionSparseBase<ConstraintT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
@@ -349,9 +337,6 @@ class CostFunctionSparse<ConstraintT,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
0,
- 0,
- 0,
- 0,
0>
{
public:
@@ -364,9 +349,6 @@ class CostFunctionSparse<ConstraintT,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
0,
- 0,
- 0,
- 0,
0>(_constraint_ptr)
{
@@ -391,10 +373,6 @@ class CostFunctionSparse<ConstraintT,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
0,
- 0,
- 0,
- 0,
- 0,
0> : CostFunctionSparseBase<ConstraintT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
@@ -402,10 +380,6 @@ class CostFunctionSparse<ConstraintT,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
0,
- 0,
- 0,
- 0,
- 0,
0>
{
public:
@@ -417,10 +391,6 @@ class CostFunctionSparse<ConstraintT,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
0,
- 0,
- 0,
- 0,
- 0,
0>(_constraint_ptr)
{
@@ -443,22 +413,12 @@ class CostFunctionSparse<ConstraintT,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
0,
- 0,
- 0,
- 0,
- 0,
- 0,
0> : CostFunctionSparseBase<ConstraintT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
0,
- 0,
- 0,
- 0,
- 0,
- 0,
0>
{
public:
@@ -469,11 +429,6 @@ class CostFunctionSparse<ConstraintT,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
0,
- 0,
- 0,
- 0,
- 0,
- 0,
0>(_constraint_ptr)
{
@@ -494,23 +449,11 @@ class CostFunctionSparse<ConstraintT,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
0> : CostFunctionSparseBase<ConstraintT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
0>
{
public:
@@ -520,12 +463,6 @@ class CostFunctionSparse<ConstraintT,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
0>(_constraint_ptr)
{
@@ -544,24 +481,10 @@ class CostFunctionSparse<ConstraintT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
0> : CostFunctionSparseBase<ConstraintT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
0>
{
public:
@@ -570,13 +493,6 @@ class CostFunctionSparse<ConstraintT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
0>(_constraint_ptr)
{
diff --git a/include/base/solver_suitesparse/cost_function_sparse_base.h b/include/base/solver_suitesparse/cost_function_sparse_base.h
index 4b54001f6fa2247f6d6250f28c2e050720f1d74a..d10fbab5886f344a0eb9c73262daba7c533b6393 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 "wolf.h"
+#include "base/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 493ee2dc8980b7236a762b47d0353b9b54eae0f6..6d032e8b2aadfbeac183e0e2db23a00d934db711 100644
--- a/include/base/solver_suitesparse/qr_solver.h
+++ b/include/base/solver_suitesparse/qr_solver.h
@@ -13,12 +13,12 @@
#include <ctime>
//Wolf includes
-#include "state_block.h"
+#include "base/state_block.h"
#include "../constraint_sparse.h"
-#include "../constraint_odom_2D.h"
-#include "../constraint_corner_2D.h"
-#include "../constraint_container.h"
-#include "sparse_utils.h"
+#include "base/constraint/constraint_odom_2D.h"
+#include "base/constraint/constraint_corner_2D.h"
+#include "base/constraint/constraint_container.h"
+#include "base/solver_suitesparse/sparse_utils.h"
// wolf solver
#include "solver/ccolamd_ordering.h"
@@ -28,7 +28,7 @@
#include <eigen3/Eigen/OrderingMethods>
#include <eigen3/Eigen/SparseQR>
#include <Eigen/StdVector>
-#include "../constraint_pose_2D.h"
+#include "base/constraint/constraint_pose_2D.h"
namespace wolf
{
diff --git a/include/base/solver_suitesparse/solver_QR.h b/include/base/solver_suitesparse/solver_QR.h
index 2fa0d11d18d90b10375884b7d14305604f0ea158..9451ceec95d774f6abd4e2686f9670a3f6bfce81 100644
--- a/include/base/solver_suitesparse/solver_QR.h
+++ b/include/base/solver_suitesparse/solver_QR.h
@@ -29,5 +29,4 @@ class SolverQR
private:
};
-
#endif /* TRUNK_SRC_SOLVER_SOLVER_QR_H_ */
diff --git a/include/base/solver_suitesparse/solver_manager.h b/include/base/solver_suitesparse/solver_manager.h
index b9084d4fb9136e69fbf5fb9172f35efab00c6fdf..479cc4509156514312f434cda2d0dd0b3a4d3758 100644
--- a/include/base/solver_suitesparse/solver_manager.h
+++ b/include/base/solver_suitesparse/solver_manager.h
@@ -2,9 +2,9 @@
#define CERES_MANAGER_H_
//wolf includes
-#include "../constraint_GPS_2D.h"
-#include "../wolf.h"
-#include "../state_block.h"
+#include "base/constraint/constraint_GPS_2D.h"
+#include "base/wolf.h"
+#include "base/state_block.h"
#include "../state_point.h"
#include "../state_complex_angle.h"
#include "../state_theta.h"
@@ -21,7 +21,6 @@ class SolverManager
{
protected:
-
public:
SolverManager(ceres::Problem::Options _options);
diff --git a/include/base/state_angle.h b/include/base/state_angle.h
index 81da7a8822d1f720a2927eb414909307c9d4229e..c61286e13eb4fcaa82c70c1c8db8784decfb1b86 100644
--- a/include/base/state_angle.h
+++ b/include/base/state_angle.h
@@ -8,8 +8,8 @@
#ifndef STATE_ANGLE_H_
#define STATE_ANGLE_H_
-#include "state_block.h"
-#include "local_parametrization_angle.h"
+#include "base/state_block.h"
+#include "base/local_parametrization_angle.h"
namespace wolf
{
diff --git a/include/base/state_block.h b/include/base/state_block.h
index cdfc94c24dbc488aa8a299c5f058aa263f049924..06962ced0ebb901d5c00649275007faaf8e3b844 100644
--- a/include/base/state_block.h
+++ b/include/base/state_block.h
@@ -9,7 +9,7 @@ class LocalParametrizationBase;
}
//Wolf includes
-#include "wolf.h"
+#include "base/wolf.h"
//std includes
#include <iostream>
@@ -154,9 +154,9 @@ public:
} // namespace wolf
// IMPLEMENTATION
-#include "local_parametrization_base.h"
-#include "node_base.h"
-#include "problem.h"
+#include "base/local_parametrization_base.h"
+#include "base/node_base.h"
+#include "base/problem.h"
namespace wolf {
diff --git a/include/base/state_homogeneous_3D.h b/include/base/state_homogeneous_3D.h
index f8d4951a89bbbe011c491578f200ed42874376f2..adfda9018041d3a9d2469dc0aa16ea7e93ccdfe0 100644
--- a/include/base/state_homogeneous_3D.h
+++ b/include/base/state_homogeneous_3D.h
@@ -8,8 +8,8 @@
#ifndef SRC_STATE_HOMOGENEOUS_3D_H_
#define SRC_STATE_HOMOGENEOUS_3D_H_
-#include "state_block.h"
-#include "local_parametrization_homogeneous.h"
+#include "base/state_block.h"
+#include "base/local_parametrization_homogeneous.h"
namespace wolf {
diff --git a/include/base/state_quaternion.h b/include/base/state_quaternion.h
index b68fc574ebbe91d21e5a7080646a2ad1e30bd84b..d990ce1f8c6ed89c3150e4958d7ad570b1e2f606 100644
--- a/include/base/state_quaternion.h
+++ b/include/base/state_quaternion.h
@@ -8,8 +8,8 @@
#ifndef SRC_STATE_QUATERNION_H_
#define SRC_STATE_QUATERNION_H_
-#include "state_block.h"
-#include "local_parametrization_quaternion.h"
+#include "base/state_block.h"
+#include "base/local_parametrization_quaternion.h"
namespace wolf {
diff --git a/include/base/time_stamp.h b/include/base/time_stamp.h
index d38d25b5fa10b40d0374a5c518681802fac1ffea..02a5599af607af39d4b16bb3c05a35779cc7bb04 100644
--- a/include/base/time_stamp.h
+++ b/include/base/time_stamp.h
@@ -3,7 +3,7 @@
#define TIME_STAMP_H_
//wolf includes
-#include "wolf.h"
+#include "base/wolf.h"
//C, std
#include <sys/time.h>
diff --git a/include/base/track_matrix.h b/include/base/track_matrix.h
index 6f32d4cdfe78f1040263fabf7442eb9a2a98a8dd..5aa6159c4118a03f822317b915af42e52929c354 100644
--- a/include/base/track_matrix.h
+++ b/include/base/track_matrix.h
@@ -8,8 +8,8 @@
#ifndef TRACK_MATRIX_H_
#define TRACK_MATRIX_H_
-#include "feature_base.h"
-#include "capture_base.h"
+#include "base/feature/feature_base.h"
+#include "base/capture/capture_base.h"
#include <map>
#include <vector>
diff --git a/include/base/trajectory_base.h b/include/base/trajectory_base.h
index cb394c5bb8cc4f815c4cbf87c81980ef2eb6fc3a..4e8d261d337af082e399f2a20188a122ef44bf6c 100644
--- a/include/base/trajectory_base.h
+++ b/include/base/trajectory_base.h
@@ -10,8 +10,8 @@ class TimeStamp;
}
//Wolf includes
-#include "wolf.h"
-#include "node_base.h"
+#include "base/wolf.h"
+#include "base/node_base.h"
//std includes
@@ -71,13 +71,11 @@ inline void TrajectoryBase::setLastKeyFramePtr(FrameBasePtr _key_frame_ptr)
last_key_frame_ptr_ = _key_frame_ptr;
}
-
inline std::string TrajectoryBase::getFrameStructure() const
{
return frame_structure_;
}
-
} // namespace wolf
#endif
diff --git a/include/base/wolf.h b/include/base/wolf.h
index bb54390300816aa1259742c1bc2bf76d4bc003ae..d642f730a22538e9bfa33c6d6d12544a5c82e381 100644
--- a/include/base/wolf.h
+++ b/include/base/wolf.h
@@ -10,7 +10,7 @@
// Enable project-specific definitions and macros
#include "internal/config.h"
-#include "logging.h"
+#include "base/logging.h"
//includes from Eigen lib
#include <Eigen/Dense>
@@ -45,7 +45,6 @@ namespace wolf {
typedef double Scalar; // Use this for double, 64 bit precision
//typedef long double Scalar; // Use this for long double, 128 bit precision
-
/**
* \brief Vector and Matrices size type for the Wolf project
*
@@ -205,7 +204,6 @@ struct MatrixSizeCheck
//
// End of check matrix sizes /////////////////////////////////////////////////
-
/////////////////////////////////////////////////////////////////////////
// TYPEDEFS FOR POINTERS, LISTS AND ITERATORS IN THE WOLF TREE
/////////////////////////////////////////////////////////////////////////
@@ -227,7 +225,6 @@ struct MatrixSizeCheck
typedef std::shared_ptr<StructName> StructName##Ptr; \
typedef std::shared_ptr<const StructName> StructName##ConstPtr; \
-
// NodeBase
WOLF_PTR_TYPEDEFS(NodeBase);
@@ -289,7 +286,6 @@ WOLF_PTR_TYPEDEFS(StateQuaternion);
// - - Local Parametrization
WOLF_PTR_TYPEDEFS(LocalParametrizationBase);
-
// ==================================================
// Some dangling functions
@@ -373,6 +369,4 @@ bool makePosDef(Eigen::Matrix<T,N,N,RC>& M, const T& eps = Constants::EPS)
} // namespace wolf
-
-
#endif /* WOLF_H_ */
diff --git a/serialization/cereal/io.h b/serialization/cereal/io.h
index 524af2e41cfecc21a15dfd10b2ed6f99c1a959bd..8a8e58c2d8a873b4d327e1c8639fc007cb8604aa 100644
--- a/serialization/cereal/io.h
+++ b/serialization/cereal/io.h
@@ -167,7 +167,6 @@ using SerializerJSON = Serializer<Extensions::JSON,
cereal::JSONInputArchive,
cereal::JSONOutputArchive>;
-
} /* namespace serialization */
template <typename... T>
diff --git a/serialization/cereal/serialization_local_parametrization_base.h b/serialization/cereal/serialization_local_parametrization_base.h
index b57b9f5649556cf89684810aa162d1495e1258d0..0c38a5a6126723e4e61d0bd15a7616a1b24f7c4c 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 "../../local_parametrization_base.h"
+#include "base/local_parametrization_base.h"
#include <cereal/cereal.hpp>
#include <cereal/types/polymorphic.hpp>
@@ -21,7 +21,6 @@ inline void serialize(
//
}
-
} //namespace cereal
#endif /* _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_H_ */
diff --git a/serialization/cereal/serialization_local_parametrization_homogeneous.h b/serialization/cereal/serialization_local_parametrization_homogeneous.h
index aa04a9904eafc13e70d5a68515cf1ae356a18d58..2b36232d53cceff76ab2ad5e6c0732f8b449cc0b 100644
--- a/serialization/cereal/serialization_local_parametrization_homogeneous.h
+++ b/serialization/cereal/serialization_local_parametrization_homogeneous.h
@@ -1,7 +1,7 @@
#ifndef _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_HOMOGENEOUS_H_
#define _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_HOMOGENEOUS_H_
-#include "../../local_parametrization_homogeneous.h"
+#include "base/local_parametrization_homogeneous.h"
#include "serialization_local_parametrization_base.h"
diff --git a/serialization/cereal/serialization_local_parametrization_quaternion.h b/serialization/cereal/serialization_local_parametrization_quaternion.h
index 10b04f67cd9de48610f5fc40c0711413d37410a6..95c71f4c288a6010aa50b65a38bea9eecc9b9c4b 100644
--- a/serialization/cereal/serialization_local_parametrization_quaternion.h
+++ b/serialization/cereal/serialization_local_parametrization_quaternion.h
@@ -1,7 +1,7 @@
#ifndef WOLF_IO_SERIALIZATION_LOCAL_PARAMETRIZATION_QUATERNION_H_
#define WOLF_IO_SERIALIZATION_LOCAL_PARAMETRIZATION_QUATERNION_H_
-#include "../../local_parametrization_quaternion.h"
+#include "base/local_parametrization_quaternion.h"
#include "serialization_local_parametrization_base.h"
diff --git a/serialization/cereal/serialization_node_base.h b/serialization/cereal/serialization_node_base.h
index dbf3292d5df155dd34c24e5ce2816137e2b7fd95..5a0a039a3ca43444f1866563bc3c135eeedaa21e 100644
--- a/serialization/cereal/serialization_node_base.h
+++ b/serialization/cereal/serialization_node_base.h
@@ -2,7 +2,7 @@
#define _WOLF_IO_CEREAL_NODE_BASE_H_
// Wolf includes
-#include "../../node_base.h"
+#include "base/node_base.h"
#include <cereal/cereal.hpp>
#include <cereal/types/polymorphic.hpp>
diff --git a/serialization/cereal/serialization_processor_odom2d_params.h b/serialization/cereal/serialization_processor_odom2d_params.h
index 4e34becad92d9918c9d8cfb7b8d571a343ef4b82..2056f6de9eecf0fc3e312b8553fa5cfd030ea942 100644
--- a/serialization/cereal/serialization_processor_odom2d_params.h
+++ b/serialization/cereal/serialization_processor_odom2d_params.h
@@ -2,7 +2,7 @@
#define _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM2D_PARAMS_H_
// Wolf includes
-#include "../../processor_odom_2D.h"
+#include "base/processor/processor_odom_2D.h"
#include "serialization_processor_params_base.h"
namespace cereal {
diff --git a/serialization/cereal/serialization_processor_odom3d_params.h b/serialization/cereal/serialization_processor_odom3d_params.h
index 9b658a233d1832f914a2986d133ab61207e3ff52..1556a39e438b0a2b273edded902070cccbd847b5 100644
--- a/serialization/cereal/serialization_processor_odom3d_params.h
+++ b/serialization/cereal/serialization_processor_odom3d_params.h
@@ -2,7 +2,7 @@
#define _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM3D_PARAMS_H_
// Wolf includes
-#include "../../processor_odom_3D.h"
+#include "base/processor/processor_odom_3D.h"
#include "serialization_processor_params_base.h"
namespace cereal {
diff --git a/serialization/cereal/serialization_processor_params_base.h b/serialization/cereal/serialization_processor_params_base.h
index 03cbce7825047da46621a4f3bdf9bb5f8bcb2148..2c75b85c9a374f6af4bf87d09993cf49da1d50a3 100644
--- a/serialization/cereal/serialization_processor_params_base.h
+++ b/serialization/cereal/serialization_processor_params_base.h
@@ -2,7 +2,7 @@
#define _WOLF_SERIALIZATION_CEREAL_PROCESSOR_PARAM_BASE_H_
// Wolf includes
-#include "../../processor_base.h"
+#include "base/processor/processor_base.h"
#include <cereal/cereal.hpp>
#include <cereal/types/polymorphic.hpp>
diff --git a/serialization/cereal/serialization_sensor_intrinsic_base.h b/serialization/cereal/serialization_sensor_intrinsic_base.h
index 2398fbad04a33b4934638c0e37d9a9c72588f905..3e3b1cdb46e85572cc185232b41bf64a78da10b6 100644
--- a/serialization/cereal/serialization_sensor_intrinsic_base.h
+++ b/serialization/cereal/serialization_sensor_intrinsic_base.h
@@ -2,7 +2,7 @@
#define _WOLF_SERIALIZATION_CEREAL_SENSOR_INTRINSIC_BASE_H_
// Wolf includes
-#include "../../sensor_base.h"
+#include "base/sensor/sensor_base.h"
#include <cereal/cereal.hpp>
#include <cereal/types/polymorphic.hpp>
diff --git a/serialization/cereal/serialization_sensor_odom2d_intrinsic.h b/serialization/cereal/serialization_sensor_odom2d_intrinsic.h
index d4bf370448af5d4b72ef5d7132b51d5e891647f3..ccfe569a72d0b69faf5af9ac01425c7edfabad69 100644
--- a/serialization/cereal/serialization_sensor_odom2d_intrinsic.h
+++ b/serialization/cereal/serialization_sensor_odom2d_intrinsic.h
@@ -2,7 +2,7 @@
#define _WOLF_SERIALIZATION_CEREAL_SENSOR_ODOM2D_INTRINSIC_H_
// Wolf includes
-#include "../../sensor_odom_2D.h"
+#include "base/sensor/sensor_odom_2D.h"
#include "serialization_sensor_intrinsic_base.h"
diff --git a/serialization/cereal/serialization_time_stamp.h b/serialization/cereal/serialization_time_stamp.h
index 770e622f01d7778d6d9aab4b114bb4256cab0996..6cc9e29c7f95d4a4ef2254a890737ab2a3f81d54 100644
--- a/serialization/cereal/serialization_time_stamp.h
+++ b/serialization/cereal/serialization_time_stamp.h
@@ -2,7 +2,7 @@
#define _WOLF_IO_CEREAL_TIME_STAMP_H_
// Wolf includes
-#include "../../time_stamp.h"
+#include "base/time_stamp.h"
#include <cereal/cereal.hpp>
diff --git a/src/SE3.h b/src/SE3.h
index cecd71757360a0bee2d36d50bbf225eda914e606..cea35460771f0bf91d4f56520dd823ad3a1d505d 100644
--- a/src/SE3.h
+++ b/src/SE3.h
@@ -8,9 +8,8 @@
#ifndef SE3_H_
#define SE3_H_
-
-#include "wolf.h"
-#include "rotations.h"
+#include "base/wolf.h"
+#include "base/rotations.h"
/*
* The functions in this file are related to manipulations of Delta motion magnitudes used in 3D motion.
@@ -34,8 +33,6 @@
* - interpolate: dd = D1 * exp ( log( D1.inv() * D2 ) * t ) = D1 (+) ( (D2 (-) D1) * t)
*/
-
-
namespace wolf
{
namespace three_D {
@@ -95,7 +92,6 @@ inline void inverse(const MatrixBase<D1>& d,
inverse(dp, dq, idp, idq);
}
-
template<typename D>
inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d)
{
@@ -208,7 +204,6 @@ inline void between(const MatrixBase<D1>& d1,
between(dp1, dq1, dp2, dq2, dp12, dq12);
}
-
template<typename D1, typename D2>
inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2 )
@@ -320,7 +315,6 @@ inline void minus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
J_do_dq2 = jac_SO3_right_inv(diff_o);
}
-
template<typename D1, typename D2, typename D3>
inline void minus(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
diff --git a/src/association/association_nnls.cpp b/src/association/association_nnls.cpp
index 0c42108eaaf308dc83fea0184febb8990334e57f..5d8286a782576b248f367b7770df440e21dfe3f4 100644
--- a/src/association/association_nnls.cpp
+++ b/src/association/association_nnls.cpp
@@ -1,4 +1,4 @@
-#include "association_nnls.h"
+#include "base/association/association_nnls.h"
namespace wolf
{
diff --git a/src/association/association_node.cpp b/src/association/association_node.cpp
index 360c9c5e818edbb2ef777b17b440d48a3dce97fd..f1705db6e1ff175890c3642d3c0bd1e7cd175b77 100644
--- a/src/association/association_node.cpp
+++ b/src/association/association_node.cpp
@@ -1,5 +1,5 @@
-#include "association_node.h"
+#include "base/association/association_node.h"
AssociationNode::AssociationNode(const unsigned int _det_idx, const unsigned int _tar_idx, const double _prob, AssociationNode * _un_ptr, bool _is_root) :
is_root_(_is_root),
diff --git a/src/association/association_solver.cpp b/src/association/association_solver.cpp
index 0e31e4cfcd2d18e477e6e17f461ad9cdc81d8241..f640efa3e3c9b93b4144ab31d068ccdc23a71842 100644
--- a/src/association/association_solver.cpp
+++ b/src/association/association_solver.cpp
@@ -1,5 +1,5 @@
-#include "association_solver.h"
+#include "base/association/association_solver.h"
namespace wolf
{
diff --git a/src/association/association_tree.cpp b/src/association/association_tree.cpp
index e42f51b5d7dc92015b4287db617e3255abd34a8f..1337e4babb19b03ce0965a0658de3c719871e4a6 100644
--- a/src/association/association_tree.cpp
+++ b/src/association/association_tree.cpp
@@ -1,5 +1,5 @@
-#include "association_tree.h"
+#include "base/association/association_tree.h"
namespace wolf
{
diff --git a/src/capture/capture_GPS.cpp b/src/capture/capture_GPS.cpp
index 8c27eebd193b883d43bfffc0f4cc0db2d0499726..5c6520810890c564502bbb297705e96413289fb4 100644
--- a/src/capture/capture_GPS.cpp
+++ b/src/capture/capture_GPS.cpp
@@ -1,4 +1,4 @@
-#include "capture_GPS.h"
+#include "base/capture/capture_GPS.h"
namespace wolf {
@@ -11,7 +11,6 @@ CaptureGPS::CaptureGPS(const TimeStamp &_ts, SensorBasePtr _sensor_ptr, rawgpsut
// std::cout << obs_.toString() << std::endl;
}
-
CaptureGPS::~CaptureGPS()
{
//std::cout << "deleting CaptureGPS " << id() << std::endl;
@@ -22,5 +21,4 @@ rawgpsutils::SatellitesObs &CaptureGPS::getData()
return obs_;
}
-
} //namespace wolf
diff --git a/src/capture/capture_GPS_fix.cpp b/src/capture/capture_GPS_fix.cpp
index f312736875819d3907d6a146d8ce54347aaa9852..052bf8f63487d7691f4fc38db3a234785df3fcac 100644
--- a/src/capture/capture_GPS_fix.cpp
+++ b/src/capture/capture_GPS_fix.cpp
@@ -1,5 +1,4 @@
-#include "capture_GPS_fix.h"
-
+#include "base/capture/capture_GPS_fix.h"
namespace wolf {
@@ -32,5 +31,4 @@ void CaptureGPSFix::process()
getFeatureList().front()->addConstraint(std::make_shared <ConstraintGPS2D>(getFeatureList().front()));
}
-
} //namespace wolf
diff --git a/src/capture/capture_IMU.cpp b/src/capture/capture_IMU.cpp
index 3f5195dad224971f5c8fde352f09b543b61e6c4a..6dbc07d59d21a3b33051d270825be9da060366c7 100644
--- a/src/capture/capture_IMU.cpp
+++ b/src/capture/capture_IMU.cpp
@@ -1,11 +1,9 @@
-#include "capture_IMU.h"
-#include "sensor_IMU.h"
-#include "state_quaternion.h"
+#include "base/capture/capture_IMU.h"
+#include "base/sensor/sensor_IMU.h"
+#include "base/state_quaternion.h"
namespace wolf {
-
-
CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::Vector6s& _acc_gyro_data,
@@ -27,7 +25,6 @@ CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
//
}
-
CaptureIMU::~CaptureIMU()
{
//
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 880303f494e5e7fc2e00d3471a04c2c8c6a09dde..051b93c9ff33929900026a534e2948120480d30d 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -1,5 +1,5 @@
-#include "capture_base.h"
-#include "sensor_base.h"
+#include "base/capture/capture_base.h"
+#include "base/sensor/sensor_base.h"
namespace wolf{
@@ -59,7 +59,6 @@ CaptureBase::CaptureBase(const std::string& _type,
// WOLF_TRACE("New Capture ", id(), " -- type ", getType(), " -- t = ", getTimeStamp(), " s");
}
-
CaptureBase::~CaptureBase()
{
removeStateBlocks();
@@ -294,8 +293,5 @@ void CaptureBase::setCalibration(const VectorXs& _calib)
}
}
-
-
-
} // namespace wolf
diff --git a/src/capture/capture_image.cpp b/src/capture/capture_image.cpp
index 6a7950d6253696a2e3aa1def2ea67139e99c157e..1d8ac8b8d7b5959173948948e89d403470d34d45 100644
--- a/src/capture/capture_image.cpp
+++ b/src/capture/capture_image.cpp
@@ -1,4 +1,4 @@
-#include "capture_image.h"
+#include "base/capture/capture_image.h"
namespace wolf {
@@ -45,5 +45,4 @@ std::vector<cv::KeyPoint>& CaptureImage::getKeypoints()
return frame_.getKeyPoints();
}
-
} // namespace wolf
diff --git a/src/capture/capture_laser_2D.cpp b/src/capture/capture_laser_2D.cpp
index 8d1a4ffeaccf3858608644ad16a39d68c5571dfb..0c3c6fe5834ced132b19037e2d8e9dff9a9e04fd 100644
--- a/src/capture/capture_laser_2D.cpp
+++ b/src/capture/capture_laser_2D.cpp
@@ -1,4 +1,4 @@
-#include "capture_laser_2D.h"
+#include "base/capture/capture_laser_2D.h"
namespace wolf {
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index 07fcd6c6810d50b02572983bce7f175fe391ed01..439228aa9b96d647f664241cbbf264cc1390e8eb 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -5,7 +5,7 @@
* \author: jsola
*/
-#include "capture_motion.h"
+#include "base/capture/capture_motion.h"
namespace wolf
{
@@ -46,15 +46,11 @@ CaptureMotion::CaptureMotion(const std::string & _type,
//
}
-
-
-
CaptureMotion::~CaptureMotion()
{
//
}
-
Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current)
{
VectorXs calib_preint = getCalibrationPreint();
diff --git a/src/capture/capture_odom_2D.cpp b/src/capture/capture_odom_2D.cpp
index 5d8eabccb3ab7e01ef77e14709778da904e43f97..99b6e42812740e045613186ea1079a13d5b2f416 100644
--- a/src/capture/capture_odom_2D.cpp
+++ b/src/capture/capture_odom_2D.cpp
@@ -5,7 +5,7 @@
* Author: jsola
*/
-#include "capture_odom_2D.h"
+#include "base/capture/capture_odom_2D.h"
namespace wolf
{
diff --git a/src/capture/capture_odom_3D.cpp b/src/capture/capture_odom_3D.cpp
index f6ba1dea9e315c88cb36676c89ede40675028169..26f4e6d6fd3697b4099084e327699bda8bb23150 100644
--- a/src/capture/capture_odom_3D.cpp
+++ b/src/capture/capture_odom_3D.cpp
@@ -5,7 +5,7 @@
* Author: jsola
*/
-#include "capture_odom_3D.h"
+#include "base/capture/capture_odom_3D.h"
namespace wolf
{
@@ -44,5 +44,3 @@ Eigen::VectorXs CaptureOdom3D::correctDelta(const VectorXs& _delta, const Vector
} /* namespace wolf */
-
-
diff --git a/src/capture/capture_pose.cpp b/src/capture/capture_pose.cpp
index c8422aa32a8ec4183c5c48cdcb74002437202709..96aab020b3ed0c240fb7e05fe81dd40fba12f76d 100644
--- a/src/capture/capture_pose.cpp
+++ b/src/capture/capture_pose.cpp
@@ -1,4 +1,4 @@
-#include "capture_pose.h"
+#include "base/capture/capture_pose.h"
namespace wolf{
@@ -30,5 +30,4 @@ void CapturePose::emplaceFeatureAndConstraint()
throw std::runtime_error("Wrong data size in CapturePose. Use 3 for 2D. Use 7 for 3D.");
}
-
} // namespace wolf
diff --git a/src/capture/capture_velocity.cpp b/src/capture/capture_velocity.cpp
index 07062242ad612868f343315af659b98d7ab75161..9c4e7675f67d8bcbc11c8b5a2d796a281bf20505 100644
--- a/src/capture/capture_velocity.cpp
+++ b/src/capture/capture_velocity.cpp
@@ -1,4 +1,4 @@
-#include "capture_velocity.h"
+#include "base/capture/capture_velocity.h"
namespace wolf {
diff --git a/src/capture/capture_void.cpp b/src/capture/capture_void.cpp
index 721353d2fbd356abe5d17685df72b7597f00df65..44bf8778833b24e0df94bd11ae3985df2d3bd46a 100644
--- a/src/capture/capture_void.cpp
+++ b/src/capture/capture_void.cpp
@@ -1,4 +1,4 @@
-#include "capture_void.h"
+#include "base/capture/capture_void.h"
namespace wolf {
@@ -13,5 +13,4 @@ CaptureVoid::~CaptureVoid()
//std::cout << "deleting CaptureVoid " << id() << std::endl;
}
-
} // namespace wolf
diff --git a/src/capture/capture_wheel_joint_position.cpp b/src/capture/capture_wheel_joint_position.cpp
index a43786df6fd0866088e0e42909acc29a9ad7017c..9290da89ae42095e4a2578d80bfedfdf13592c73 100644
--- a/src/capture/capture_wheel_joint_position.cpp
+++ b/src/capture/capture_wheel_joint_position.cpp
@@ -1,6 +1,6 @@
-#include "capture_wheel_joint_position.h"
-#include "../sensors/sensor_diff_drive.h"
-#include "../rotations.h"
+#include "base/capture/capture_wheel_joint_position.h"
+#include "base/sensor/sensor_diff_drive.h"
+#include "base/rotations.h"
namespace wolf {
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 26babf9af4af7e2b68efe1486b7f1d82a965b0df..e7e1455675969eb3203f5573bd468b6018ea6fa4 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -1,9 +1,9 @@
-#include "ceres_manager.h"
-#include "create_numeric_diff_cost_function.h"
-#include "../trajectory_base.h"
-#include "../map_base.h"
-#include "../landmark_base.h"
-#include "../make_unique.h"
+#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/landmark/landmark_base.h"
+#include "base/make_unique.h"
namespace wolf {
diff --git a/src/ceres_wrapper/local_parametrization_wrapper.cpp b/src/ceres_wrapper/local_parametrization_wrapper.cpp
index 7b7ae9414c8bf32121136a03f10d8ff4cafdf943..5cade15532096a249e66df8da32287fa1ecbf797 100644
--- a/src/ceres_wrapper/local_parametrization_wrapper.cpp
+++ b/src/ceres_wrapper/local_parametrization_wrapper.cpp
@@ -1,4 +1,4 @@
-#include "local_parametrization_wrapper.h"
+#include "base/ceres_wrapper/local_parametrization_wrapper.h"
namespace wolf {
diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp
index 36caa290b20d8be1bbe741dab144590c61791031..9c63afd0315f90410aa60a073120efa36bc84d25 100644
--- a/src/ceres_wrapper/qr_manager.cpp
+++ b/src/ceres_wrapper/qr_manager.cpp
@@ -154,7 +154,6 @@ void QRManager::addConstraint(ConstraintBasePtr _ctr_ptr)
pending_changes_ = true;
}
-
void QRManager::removeConstraint(ConstraintBasePtr _ctr_ptr)
{
//std::cout << "remove constraint " << _ctr_ptr->id() << std::endl;
diff --git a/src/ceres_wrapper/solver_manager.cpp b/src/ceres_wrapper/solver_manager.cpp
index 2a5d8efec9aacf1fd3276a0ea749b079577e2d17..cdaae51c3c9a23dd5f227c5e938a9910036af873 100644
--- a/src/ceres_wrapper/solver_manager.cpp
+++ b/src/ceres_wrapper/solver_manager.cpp
@@ -1,7 +1,7 @@
-#include "solver_manager.h"
-#include "../trajectory_base.h"
-#include "../map_base.h"
-#include "../landmark_base.h"
+#include "base/solver/solver_manager.h"
+#include "base/trajectory_base.h"
+#include "base/map_base.h"
+#include "base/landmark/landmark_base.h"
namespace wolf {
diff --git a/src/constraint/constraint_analytic.cpp b/src/constraint/constraint_analytic.cpp
index 8b0422380bc4ad0d39f2e7f38955ea3b0bd948a8..23ab82362dc7391747099d82b705c6d13b5896c8 100644
--- a/src/constraint/constraint_analytic.cpp
+++ b/src/constraint/constraint_analytic.cpp
@@ -1,5 +1,5 @@
-#include "constraint_analytic.h"
-#include "state_block.h"
+#include "base/constraint/constraint_analytic.h"
+#include "base/state_block.h"
namespace wolf {
diff --git a/src/constraint/constraint_base.cpp b/src/constraint/constraint_base.cpp
index 3e33cc6cc6d84dc1bded350956e29a548dacef22..f521cf214ecd8de80205a7dfbe8572433eca353c 100644
--- a/src/constraint/constraint_base.cpp
+++ b/src/constraint/constraint_base.cpp
@@ -1,6 +1,6 @@
-#include "constraint_base.h"
-#include "frame_base.h"
-#include "landmark_base.h"
+#include "base/constraint/constraint_base.h"
+#include "base/frame_base.h"
+#include "base/landmark/landmark_base.h"
namespace wolf {
diff --git a/examples/.gitignore b/src/examples/.gitignore
similarity index 100%
rename from examples/.gitignore
rename to src/examples/.gitignore
diff --git a/examples/ACTIVESEARCH.yaml b/src/examples/ACTIVESEARCH.yaml
similarity index 100%
rename from examples/ACTIVESEARCH.yaml
rename to src/examples/ACTIVESEARCH.yaml
diff --git a/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
similarity index 100%
rename from examples/CMakeLists.txt
rename to src/examples/CMakeLists.txt
diff --git a/examples/Test_ORB.png b/src/examples/Test_ORB.png
similarity index 100%
rename from examples/Test_ORB.png
rename to src/examples/Test_ORB.png
diff --git a/examples/camera_params.yaml b/src/examples/camera_params.yaml
similarity index 100%
rename from examples/camera_params.yaml
rename to src/examples/camera_params.yaml
diff --git a/examples/camera_params_canonical.yaml b/src/examples/camera_params_canonical.yaml
similarity index 100%
rename from examples/camera_params_canonical.yaml
rename to src/examples/camera_params_canonical.yaml
diff --git a/examples/camera_params_ueye.yaml b/src/examples/camera_params_ueye.yaml
similarity index 100%
rename from examples/camera_params_ueye.yaml
rename to src/examples/camera_params_ueye.yaml
diff --git a/examples/camera_params_ueye_radial_dist.yaml b/src/examples/camera_params_ueye_radial_dist.yaml
similarity index 100%
rename from examples/camera_params_ueye_radial_dist.yaml
rename to src/examples/camera_params_ueye_radial_dist.yaml
diff --git a/examples/camera_params_ueye_sim.yaml b/src/examples/camera_params_ueye_sim.yaml
similarity index 100%
rename from examples/camera_params_ueye_sim.yaml
rename to src/examples/camera_params_ueye_sim.yaml
diff --git a/examples/input_M3500b_toro.graph b/src/examples/input_M3500b_toro.graph
similarity index 100%
rename from examples/input_M3500b_toro.graph
rename to src/examples/input_M3500b_toro.graph
diff --git a/examples/map_polyline_example.yaml b/src/examples/map_polyline_example.yaml
similarity index 100%
rename from examples/map_polyline_example.yaml
rename to src/examples/map_polyline_example.yaml
diff --git a/examples/processor_image_feature.yaml b/src/examples/processor_image_feature.yaml
similarity index 100%
rename from examples/processor_image_feature.yaml
rename to src/examples/processor_image_feature.yaml
diff --git a/examples/processor_image_vision_utils.yaml b/src/examples/processor_image_vision_utils.yaml
similarity index 100%
rename from examples/processor_image_vision_utils.yaml
rename to src/examples/processor_image_vision_utils.yaml
diff --git a/examples/processor_imu.yaml b/src/examples/processor_imu.yaml
similarity index 100%
rename from examples/processor_imu.yaml
rename to src/examples/processor_imu.yaml
diff --git a/examples/processor_imu_no_vote.yaml b/src/examples/processor_imu_no_vote.yaml
similarity index 100%
rename from examples/processor_imu_no_vote.yaml
rename to src/examples/processor_imu_no_vote.yaml
diff --git a/examples/processor_imu_t1.yaml b/src/examples/processor_imu_t1.yaml
similarity index 100%
rename from examples/processor_imu_t1.yaml
rename to src/examples/processor_imu_t1.yaml
diff --git a/examples/processor_imu_t6.yaml b/src/examples/processor_imu_t6.yaml
similarity index 100%
rename from examples/processor_imu_t6.yaml
rename to src/examples/processor_imu_t6.yaml
diff --git a/examples/processor_odom_3D.yaml b/src/examples/processor_odom_3D.yaml
similarity index 100%
rename from examples/processor_odom_3D.yaml
rename to src/examples/processor_odom_3D.yaml
diff --git a/examples/processor_tracker_feature_trifocal.yaml b/src/examples/processor_tracker_feature_trifocal.yaml
similarity index 100%
rename from examples/processor_tracker_feature_trifocal.yaml
rename to src/examples/processor_tracker_feature_trifocal.yaml
diff --git a/examples/sensor_imu.yaml b/src/examples/sensor_imu.yaml
similarity index 100%
rename from examples/sensor_imu.yaml
rename to src/examples/sensor_imu.yaml
diff --git a/examples/sensor_odom_3D.yaml b/src/examples/sensor_odom_3D.yaml
similarity index 100%
rename from examples/sensor_odom_3D.yaml
rename to src/examples/sensor_odom_3D.yaml
diff --git a/examples/sensor_odom_3D_HQ.yaml b/src/examples/sensor_odom_3D_HQ.yaml
similarity index 100%
rename from examples/sensor_odom_3D_HQ.yaml
rename to src/examples/sensor_odom_3D_HQ.yaml
diff --git a/examples/solver/test_SPQR.cpp b/src/examples/solver/test_SPQR.cpp
similarity index 99%
rename from examples/solver/test_SPQR.cpp
rename to src/examples/solver/test_SPQR.cpp
index bbd535de6a3b5106fb36ba9ff6def36190807d40..04592dbd047fed1d4b1d577c2df53cd5bbe57766 100644
--- a/examples/solver/test_SPQR.cpp
+++ b/src/examples/solver/test_SPQR.cpp
@@ -45,7 +45,6 @@ int main (int argc, char **argv)
std::cout << "solved x_" << std::endl << x_ << std::endl;
std::cout << "ordering: " << solver.colsPermutation().indices().transpose() << std::endl;
-
///////////////////////////////////////////////////////////////////////
// Directly in suitesparse
cholmod_common Common, *cc ;
diff --git a/examples/solver/test_ccolamd.cpp b/src/examples/solver/test_ccolamd.cpp
similarity index 99%
rename from examples/solver/test_ccolamd.cpp
rename to src/examples/solver/test_ccolamd.cpp
index 41c742ad6e24579cc5f3aeaebc1c702ff5e77a51..ebba25db70ed2f76db699d8aeb4e361e26aa50a2 100644
--- a/examples/solver/test_ccolamd.cpp
+++ b/src/examples/solver/test_ccolamd.cpp
@@ -6,7 +6,7 @@
*/
// Wolf includes
-#include "wolf.h"
+#include "base/wolf.h"
//std includes
#include <cstdlib>
diff --git a/examples/solver/test_ccolamd_blocks.cpp b/src/examples/solver/test_ccolamd_blocks.cpp
similarity index 99%
rename from examples/solver/test_ccolamd_blocks.cpp
rename to src/examples/solver/test_ccolamd_blocks.cpp
index 8c5c91219a1c11f3e35472a4c46b335db689921f..b3d4d646baf22fa72babfe2cd09cae7bbcbf4623 100644
--- a/examples/solver/test_ccolamd_blocks.cpp
+++ b/src/examples/solver/test_ccolamd_blocks.cpp
@@ -5,7 +5,6 @@
* Author: jvallve
*/
-
//std includes
#include <cstdlib>
#include <iostream>
@@ -52,7 +51,6 @@ void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, i
perm_blocks.indices() = idx_blocks;
}
-
//main
int main(int argc, char *argv[])
{
@@ -180,6 +178,3 @@ int main(int argc, char *argv[])
//std::cout << "x = " << x_block_ordered.transpose() << std::endl;
}
-
-
-
diff --git a/examples/solver/test_iQR.cpp b/src/examples/solver/test_iQR.cpp
similarity index 99%
rename from examples/solver/test_iQR.cpp
rename to src/examples/solver/test_iQR.cpp
index 79bfb03b31d1de81602113003d1e5028f15a328a..868e84440bc1dce0931527e780f9e2a4d8234dfe 100644
--- a/examples/solver/test_iQR.cpp
+++ b/src/examples/solver/test_iQR.cpp
@@ -12,7 +12,6 @@
* Author: jvallve
*/
-
//std includes
#include <cstdlib>
#include <iostream>
@@ -97,7 +96,6 @@ void augment_permutation(PermutationMatrix<Dynamic, Dynamic, int> &perm, const i
perm.indices() = new_indices;
}
-
//main
int main(int argc, char *argv[])
{
@@ -350,8 +348,3 @@ int main(int argc, char *argv[])
}
}
-
-
-
-
-
diff --git a/examples/solver/test_iQR_wolf.cpp b/src/examples/solver/test_iQR_wolf.cpp
similarity index 99%
rename from examples/solver/test_iQR_wolf.cpp
rename to src/examples/solver/test_iQR_wolf.cpp
index dd8707b8b5a8980258ec705bf8bfc6df175864b4..70624a0d65a7d074f2f1f8355a0a8cc5348ebf3e 100644
--- a/examples/solver/test_iQR_wolf.cpp
+++ b/src/examples/solver/test_iQR_wolf.cpp
@@ -5,7 +5,6 @@
* Author: jvallve
*/
-
//std includes
#include <cstdlib>
#include <string>
@@ -222,7 +221,6 @@ class SolverQR
A_nodes_.coeffRef(A_nodes_.rows()-1, ordered_node) = 1;
-
assert(_meas.jacobians.at(j).cols() == nodes_.at(_meas.nodes_idx.at(j)).dim);
assert(_meas.jacobians.at(j).rows() == _meas.dim);
@@ -396,7 +394,6 @@ class SolverQR
return 1;
}
-
void nodePermutation2VariablesPermutation(const PermutationMatrix<Dynamic, Dynamic, int> &_perm_nodes, PermutationMatrix<Dynamic, Dynamic, int> &perm_variables)
{
ArrayXi locations = perm_nodes_2_locations(_perm_nodes);
@@ -561,8 +558,3 @@ int main(int argc, char *argv[])
}
}
-
-
-
-
-
diff --git a/examples/solver/test_iQR_wolf2.cpp b/src/examples/solver/test_iQR_wolf2.cpp
similarity index 99%
rename from examples/solver/test_iQR_wolf2.cpp
rename to src/examples/solver/test_iQR_wolf2.cpp
index 07aba80d45c4160f2272d8ae7b13a49027fc5d70..d3fb71cadd8f50bc0eda0ea36decf7af2872d0be 100644
--- a/examples/solver/test_iQR_wolf2.cpp
+++ b/src/examples/solver/test_iQR_wolf2.cpp
@@ -17,9 +17,9 @@
#include <queue>
//Wolf includes
-#include "state_block.h"
-#include "constraint_base.h"
-#include "sensor_laser_2D.h"
+#include "base/state_block.h"
+#include "base/constraint/constraint_base.h"
+#include "base/sensor/sensor_laser_2D.h"
#include "wolf_manager.h"
// wolf solver
@@ -35,14 +35,12 @@
//Ceres includes
#include "glog/logging.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/ceres_wrapper/ceres_manager.h"
//laser_scan_utils
#include "iri-algorithms/laser_scan_utils/corner_detector.h"
#include "iri-algorithms/laser_scan_utils/entities.h"
-
-
//function travel around
void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double& rotation_)
{
@@ -96,7 +94,6 @@ void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double
pose.rt.setEuler(pose.rt.head() + rotation_, pose.rt.pitch(), pose.rt.roll());
}
-
//main
int main(int argc, char *argv[])
{
@@ -118,7 +115,6 @@ int main(int argc, char *argv[])
// INITIALIZATION ============================================================================================
-
//init random generators
Scalar odom_std_factor = 0.1;
Scalar gps_std = 10;
@@ -432,8 +428,3 @@ int main(int argc, char *argv[])
return 0;
}
-
-
-
-
-
diff --git a/examples/solver/test_incremental_ccolamd_blocks.cpp b/src/examples/solver/test_incremental_ccolamd_blocks.cpp
similarity index 99%
rename from examples/solver/test_incremental_ccolamd_blocks.cpp
rename to src/examples/solver/test_incremental_ccolamd_blocks.cpp
index de4f82f00b083cb6edf88180ece6fb238cec09b6..792250a81b7e23e3465d865ec6ed69c041b3d35f 100644
--- a/examples/solver/test_incremental_ccolamd_blocks.cpp
+++ b/src/examples/solver/test_incremental_ccolamd_blocks.cpp
@@ -5,7 +5,6 @@
* Author: jvallve
*/
-
//std includes
#include <cstdlib>
#include <iostream>
@@ -54,7 +53,6 @@ void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, i
perm_blocks.indices() = idx_blocks;
}
-
//main
int main(int argc, char *argv[])
{
@@ -143,7 +141,6 @@ int main(int argc, char *argv[])
// r.h.v
b.segment(i*dim, dim) = VectorXd::LinSpaced(Sequential, dim, dim*i, dim *(i+1)-1);
-
std::cout << "Solving factor graph:" << std::endl;
std::cout << "Factors: " << std::endl << factors * MatrixXi::Identity((i+1), (i+1)) << std::endl << std::endl;
// std::cout << "H: " << std::endl << H * MatrixXd::Identity(dim*(i+1), dim*(i+1)) << std::endl << std::endl;
@@ -160,7 +157,6 @@ int main(int argc, char *argv[])
x = solver.solve(b);
time1 += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
// SOLVING WITH REORDERING ------------------------------------
// Order with previous orderings
acc_permutation.conservativeResize(dim*(i+1));
@@ -198,7 +194,6 @@ int main(int argc, char *argv[])
x_ordered = acc_permutation_matrix.inverse() * x_ordered;
time2 += ((double) clock() - t2) / CLOCKS_PER_SEC;
-
// SOLVING WITH BLOCK REORDERING ------------------------------------
// Order with previous orderings
acc_permutation_b.conservativeResize(dim*(i+1));
@@ -245,7 +240,6 @@ int main(int argc, char *argv[])
x_b_ordered = acc_permutation_b_matrix.inverse() * x_b_ordered;
time3 += ((double) clock() - t3) / CLOCKS_PER_SEC;
-
// RESULTS ------------------------------------
std::cout << "========================= RESULTS " << i << ":" << std::endl;
std::cout << "NO REORDERING: solved in " << time1*1e3 << " ms" << std::endl;
@@ -266,6 +260,3 @@ int main(int argc, char *argv[])
//std::cout << "x = " << x_b_ordered.transpose() << std::endl;
}
-
-
-
diff --git a/examples/solver/test_permutations.cpp b/src/examples/solver/test_permutations.cpp
similarity index 99%
rename from examples/solver/test_permutations.cpp
rename to src/examples/solver/test_permutations.cpp
index 5d28ec06c19c21df2ba3c5455d13195f0136eb81..c33c744c673024b5a66aece490d98b2de5e5543b 100644
--- a/examples/solver/test_permutations.cpp
+++ b/src/examples/solver/test_permutations.cpp
@@ -5,7 +5,6 @@
* Author: jvallve
*/
-
//std includes
#include <cstdlib>
#include <iostream>
@@ -19,7 +18,6 @@
// eigen includes
#include <eigen3/Eigen/OrderingMethods>
-
using namespace Eigen;
//main
@@ -111,6 +109,5 @@ int main(int argc, char *argv[])
std::cout << "mapped_a = " << mapped_a.transpose() << std::endl << std::endl;
std::cout << "maps are affected of the reorderings in mapped vectors" << std::endl;
-
// Map<>
}
diff --git a/examples/test_2_lasers_offline.cpp b/src/examples/test_2_lasers_offline.cpp
similarity index 97%
rename from examples/test_2_lasers_offline.cpp
rename to src/examples/test_2_lasers_offline.cpp
index 5c5672c79a21e9e6b12e082ff5873565b9c3cb52..d8d91b6381b718f79591bf21f96a1df08ab0f66f 100644
--- a/examples/test_2_lasers_offline.cpp
+++ b/src/examples/test_2_lasers_offline.cpp
@@ -1,5 +1,5 @@
//std includes
-#include <sensor_GPS_fix.h>
+#include "base/sensor/sensor_GPS_fix.h"
#include <cstdlib>
#include <iostream>
#include <fstream>
@@ -17,12 +17,12 @@
#include "glog/logging.h"
//Wolf includes
-#include "../problem.h"
-#include "../processor_tracker_landmark_corner.h"
-#include "../processor_odom_2D.h"
-#include "../sensor_laser_2D.h"
-#include "../sensor_odom_2D.h"
-#include "../ceres_wrapper/ceres_manager.h"
+#include "base/problem.h"
+#include "base/processor/processor_tracker_landmark_corner.h"
+#include "base/processor/processor_odom_2D.h"
+#include "base/sensor/sensor_laser_2D.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/ceres_wrapper/ceres_manager.h"
// laserscanutils
#include "laser_scan_utils/line_finder_iterative.h"
@@ -31,7 +31,7 @@
//C includes for sleep, time and main args
#include "unistd.h"
-#include "../capture_pose.h"
+#include "base/capture/capture_pose.h"
//faramotics includes
#include "faramotics/dynamicSceneRender.h"
#include "faramotics/rangeScan2D.h"
@@ -88,7 +88,6 @@ int main(int argc, char** argv)
else
std::cout << "Simulated data files opened correctly..." << std::endl;
-
unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
// INITIALIZATION ============================================================================================
@@ -226,7 +225,6 @@ int main(int argc, char** argv)
}
mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
// SOLVE OPTIMIZATION ---------------------------
std::cout << "SOLVING..." << std::endl;
t1 = clock();
diff --git a/examples/test_analytic_odom_constraint.cpp b/src/examples/test_analytic_odom_constraint.cpp
similarity index 99%
rename from examples/test_analytic_odom_constraint.cpp
rename to src/examples/test_analytic_odom_constraint.cpp
index 029b0280bc33d469361960af15a18432a8f4fa55..854d4543f742cdfef89f7b7e0fe963f5af76844b 100644
--- a/examples/test_analytic_odom_constraint.cpp
+++ b/src/examples/test_analytic_odom_constraint.cpp
@@ -12,8 +12,8 @@
//Wolf includes
#include "wolf_manager.h"
-#include "capture_void.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/capture/capture_void.h"
+#include "base/ceres_wrapper/ceres_manager.h"
// EIGEN
//#include <Eigen/CholmodSupport>
@@ -28,8 +28,6 @@ void insertSparseBlock(const Eigen::SparseMatrix<Scalar>& ins, Eigen::SparseMatr
original.makeCompressed();
}
-}
-
int main(int argc, char** argv)
{
@@ -71,8 +69,6 @@ int main(int argc, char** argv)
CeresManager* ceres_manager_autodiff = new CeresManager(wolf_problem_autodiff, ceres_options);
CeresManager* ceres_manager_analytic = new CeresManager(wolf_problem_analytic, ceres_options);
-
-
// load graph from .txt
offLineFile_.open(file_path_.c_str(), std::ifstream::in);
if (offLineFile_.is_open())
diff --git a/examples/test_autodiff.cpp b/src/examples/test_autodiff.cpp
similarity index 99%
rename from examples/test_autodiff.cpp
rename to src/examples/test_autodiff.cpp
index c027f9fcc6bf273a469cf1007cbbf643b44c0ae6..a605635467398570350ae7424c5cab116ebcbf40 100644
--- a/examples/test_autodiff.cpp
+++ b/src/examples/test_autodiff.cpp
@@ -17,8 +17,8 @@
//Wolf includes
#include "wolf_manager.h"
-#include "sensor_laser_2D.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/sensor/sensor_laser_2D.h"
+#include "base/ceres_wrapper/ceres_manager.h"
//C includes for sleep, time and main args
#include "unistd.h"
@@ -49,7 +49,6 @@ void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double
pose.moveForward(displacement_);
pose.rt.setEuler(pose.rt.head() + rotation_, pose.rt.pitch(), pose.rt.roll());
}
-}
int main(int argc, char** argv)
{
diff --git a/examples/test_capture_laser_2D.cpp b/src/examples/test_capture_laser_2D.cpp
similarity index 99%
rename from examples/test_capture_laser_2D.cpp
rename to src/examples/test_capture_laser_2D.cpp
index f5c43ac41a2732c51104b5b08fec1f08b2980392..8ca0e376b7448bbee9763a48bc76f42fad5b52d1 100644
--- a/examples/test_capture_laser_2D.cpp
+++ b/src/examples/test_capture_laser_2D.cpp
@@ -3,7 +3,7 @@
#include <random>
//wolf
-#include "capture_laser_2D.h"
+#include "base/capture/capture_laser_2D.h"
// Eigen in std vector
#include <Eigen/StdVector>
diff --git a/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
similarity index 98%
rename from examples/test_ceres_2_lasers.cpp
rename to src/examples/test_ceres_2_lasers.cpp
index 679777dd47ed49f034ffce24844e998ba3bd2a1a..8978383534cc5c7cf9b3fea30c13eaf9a3ad381c 100644
--- a/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -1,5 +1,5 @@
//std includes
-#include <sensor_GPS_fix.h>
+#include "base/sensor/sensor_GPS_fix.h"
#include <cstdlib>
#include <iostream>
#include <fstream>
@@ -17,12 +17,12 @@
#include "glog/logging.h"
//Wolf includes
-#include "problem.h"
-#include "processor_tracker_landmark_corner.h"
-#include "processor_odom_2D.h"
-#include "sensor_laser_2D.h"
-#include "sensor_odom_2D.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/problem.h"
+#include "base/processor/processor_tracker_landmark_corner.h"
+#include "base/processor/processor_odom_2D.h"
+#include "base/sensor/sensor_laser_2D.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/ceres_wrapper/ceres_manager.h"
// laserscanutils
#include "laser_scan_utils/line_finder_iterative.h"
@@ -31,7 +31,7 @@
//C includes for sleep, time and main args
#include "unistd.h"
-#include "../capture_pose.h"
+#include "base/capture/capture_pose.h"
//faramotics includes
#include "faramotics/dynamicSceneRender.h"
#include "faramotics/rangeScan2D.h"
@@ -71,7 +71,6 @@ class FaramoticsRobot
myScanner->loadAssimpModel(modelFileName);
}
-
//function travel around
Eigen::Vector3s motionCampus(unsigned int ii, double& displacement_, double& rotation_)
{
diff --git a/examples/test_ceres_2_lasers_polylines.cpp b/src/examples/test_ceres_2_lasers_polylines.cpp
similarity index 98%
rename from examples/test_ceres_2_lasers_polylines.cpp
rename to src/examples/test_ceres_2_lasers_polylines.cpp
index 9668e1c4a2690d18d8c995e3963ebffbaf1478f3..70928589ae5cdd3ab79ee9c520266d01ee6f853d 100644
--- a/examples/test_ceres_2_lasers_polylines.cpp
+++ b/src/examples/test_ceres_2_lasers_polylines.cpp
@@ -1,5 +1,5 @@
//std includes
-#include <sensor_GPS_fix.h>
+#include "base/sensor/sensor_GPS_fix.h"
#include <cstdlib>
#include <iostream>
#include <fstream>
@@ -17,12 +17,12 @@
#include "glog/logging.h"
//Wolf includes
-#include "problem.h"
-#include "processor_tracker_landmark_polyline.h"
-#include "processor_odom_2D.h"
-#include "sensor_laser_2D.h"
-#include "sensor_odom_2D.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/problem.h"
+#include "base/processor/processor_tracker_landmark_polyline.h"
+#include "base/processor/processor_odom_2D.h"
+#include "base/sensor/sensor_laser_2D.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/ceres_wrapper/ceres_manager.h"
// laserscanutils
#include "laser_scan_utils/line_finder_iterative.h"
@@ -31,7 +31,7 @@
//C includes for sleep, time and main args
#include "unistd.h"
-#include "../capture_pose.h"
+#include "base/capture/capture_pose.h"
//faramotics includes
#include "faramotics/dynamicSceneRender.h"
#include "faramotics/rangeScan2D.h"
@@ -71,7 +71,6 @@ class FaramoticsRobot
myScanner->loadAssimpModel(modelFileName);
}
-
//function travel around
Eigen::Vector3s motionCampus(unsigned int ii, double& displacement_, double& rotation_)
{
diff --git a/examples/test_constraint_AHP.cpp b/src/examples/test_constraint_AHP.cpp
similarity index 96%
rename from examples/test_constraint_AHP.cpp
rename to src/examples/test_constraint_AHP.cpp
index 8e555cac0ba244dc67bde5d8ce86990672a0a244..7dc1eabe72c1085e4aa1192058837c861a753419 100644
--- a/examples/test_constraint_AHP.cpp
+++ b/src/examples/test_constraint_AHP.cpp
@@ -1,10 +1,10 @@
-#include "pinhole_tools.h"
-#include "landmark_AHP.h"
-#include "constraint_AHP.h"
-#include "state_block.h"
-#include "state_quaternion.h"
-#include "sensor_camera.h"
-#include "capture_image.h"
+#include "base/pinhole_tools.h"
+#include "base/landmark/landmark_AHP.h"
+#include "base/constraint/constraint_AHP.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
+#include "base/sensor/sensor_camera.h"
+#include "base/capture/capture_image.h"
int main()
{
@@ -45,7 +45,6 @@ int main()
// one-liner API
ProcessorBasePtr processor_ptr = wolf_problem_ptr_->installProcessor("IMAGE LANDMARK", "ORB", "PinHole", wolf_root + "/src/examples/processor_image_feature.yaml");
-
// create the current frame
Eigen::Vector7s frame_pos_ori;
frame_pos_ori.setRandom();
@@ -71,7 +70,6 @@ int main()
image_ptr = std::make_shared< CaptureImage>(t, camera_ptr_, frame);
last_frame->addCapture(image_ptr);
-
// create the feature
cv::KeyPoint kp; kp.pt = {10,20};
cv::Mat desc;
@@ -82,14 +80,12 @@ int main()
FrameBasePtr anchor_frame = std::make_shared< FrameBase>(t,std::make_shared<StateBlock>(frame_val.head(3)), std::make_shared<StateQuaternion>(frame_val.tail(4)));
//FrameBasePtr anchor_frame = wolf_problem_ptr_->getTrajectoryPtr()->getLastFramePtr();
-
// create the landmark
Eigen::Vector2s point2D;
point2D[0] = feat_point_image_ptr->getKeypoint().pt.x;
point2D[1] = feat_point_image_ptr->getKeypoint().pt.y;
std::cout << "point2D: " << point2D.transpose() << std::endl;
-
Scalar distance = 2; // arbitrary value
Eigen::Vector4s vec_homogeneous;
@@ -115,8 +111,6 @@ int main()
std::cout << "Landmark AHP created" << std::endl;
-
-
// Create the constraint
ConstraintAHPPtr constraint_ptr = std::make_shared<ConstraintAHP>(feat_point_image_ptr, std::static_pointer_cast<LandmarkAHP>(landmark), processor_ptr);
@@ -146,7 +140,6 @@ int main()
landmark_.data(), expectation.data());
// current_frame p; current_frame o; anchor_frame p; anchor_frame o; homogeneous_vector landmark, residual
-
std::cout << "expectation computed" << std::endl;
std::cout << "expectation = " << expectation[0] << " " << expectation[1] << std::endl;
diff --git a/examples/test_constraint_imu.cpp b/src/examples/test_constraint_imu.cpp
similarity index 97%
rename from examples/test_constraint_imu.cpp
rename to src/examples/test_constraint_imu.cpp
index e4a3b8f2b37a5669bf10b32c16a2e8246bc9e52e..77464a9e4365484f836f5a5ae8a7f3d161ceef91 100644
--- a/examples/test_constraint_imu.cpp
+++ b/src/examples/test_constraint_imu.cpp
@@ -1,14 +1,14 @@
//Wolf
-#include <capture_IMU.h>
-#include <processor_IMU.h>
-#include <sensor_IMU.h>
-#include "../capture_pose.h"
-#include "wolf.h"
-#include "problem.h"
-#include "constraint_odom_3D.h"
-#include "state_block.h"
-#include "state_quaternion.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/capture/capture_IMU.h"
+#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/constraint/constraint_odom_3D.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
+#include "base/ceres_wrapper/ceres_manager.h"
//#define DEBUG_RESULTS
@@ -56,7 +56,6 @@ int main(int argc, char** argv)
CaptureIMUPtr imu_ptr( std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6s::Identity()) );
imu_ptr->setFramePtr(wolf_problem_ptr_->getTrajectoryPtr()->getFrameList().back());
-
// set variables
using namespace std;
Eigen::VectorXs state_vec;
@@ -237,7 +236,6 @@ int main(int argc, char** argv)
wolf_problem_ptr_->print(4,1,1,1);
}
-
///having a look at covariances
Eigen::MatrixXs predelta_cov;
predelta_cov.resize(9,9);
diff --git a/examples/test_constraint_odom_3D.cpp b/src/examples/test_constraint_odom_3D.cpp
similarity index 73%
rename from examples/test_constraint_odom_3D.cpp
rename to src/examples/test_constraint_odom_3D.cpp
index 41cf14f2dba85cc5fd82db7fbb56fd22376d28b1..5cdbfccf519adadafd23f9f7210b1143290b04ed 100644
--- a/examples/test_constraint_odom_3D.cpp
+++ b/src/examples/test_constraint_odom_3D.cpp
@@ -5,7 +5,7 @@
* Author: jsola
*/
-#include "../src/constraint_odom_3D.h"
+#include "base/constraint/constraint_odom_3D.h"
namespace wolf
{
diff --git a/examples/test_diff_drive.cpp b/src/examples/test_diff_drive.cpp
similarity index 97%
rename from examples/test_diff_drive.cpp
rename to src/examples/test_diff_drive.cpp
index 821f5bdd8038417b50fa0714e69b68ae58f33ede..a36a926924b214a6a8c5f2cc0559d553b6bba653 100644
--- a/examples/test_diff_drive.cpp
+++ b/src/examples/test_diff_drive.cpp
@@ -6,11 +6,11 @@
*/
//Wolf
-#include "wolf.h"
-#include "problem.h"
-#include "../sensors/sensor_diff_drive.h"
-#include "../captures/capture_wheel_joint_position.h"
-#include "../processors/processor_diff_drive.h"
+#include "base/wolf.h"
+#include "base/problem.h"
+#include "base/sensor/sensor_diff_drive.h"
+#include "base/capture/capture_wheel_joint_position.h"
+#include "processor/processor_diff_drive.h"
//std
#include <iostream>
diff --git a/examples/test_eigen_quaternion.cpp b/src/examples/test_eigen_quaternion.cpp
similarity index 97%
rename from examples/test_eigen_quaternion.cpp
rename to src/examples/test_eigen_quaternion.cpp
index cea2324871950a1cf754430a2435449a8e061bb9..1c01f5e1361a9ee43388a03c39928a405342f244 100644
--- a/examples/test_eigen_quaternion.cpp
+++ b/src/examples/test_eigen_quaternion.cpp
@@ -6,7 +6,7 @@
#include <eigen3/Eigen/Geometry>
//Wolf
-#include "wolf.h"
+#include "base/wolf.h"
int main()
{
diff --git a/examples/test_eigen_template.cpp b/src/examples/test_eigen_template.cpp
similarity index 99%
rename from examples/test_eigen_template.cpp
rename to src/examples/test_eigen_template.cpp
index 3ea96954d23c4d6d73eefeb1876c0e9e3a480916..47e5aa4191e1d680b4cbc5a8702d7c55f74ed52e 100644
--- a/examples/test_eigen_template.cpp
+++ b/src/examples/test_eigen_template.cpp
@@ -5,7 +5,6 @@
* \author: jsola
*/
-
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <iostream>
@@ -57,12 +56,10 @@ inline Eigen::Quaternion<typename Derived::Scalar> v2q(const Eigen::MatrixBase<D
}
}
-
int main(void)
{
using namespace Eigen;
-
VectorXd x(10);
x << 1,2,3,4,5,6,7,8,9,10;
diff --git a/examples/test_faramotics_simulation.cpp b/src/examples/test_faramotics_simulation.cpp
similarity index 98%
rename from examples/test_faramotics_simulation.cpp
rename to src/examples/test_faramotics_simulation.cpp
index 63f0cd8e7648a923e63ba1d7490e144d7f8a5677..1a3feedafe7777a48a60923e62e71a8a33b3939f 100644
--- a/examples/test_faramotics_simulation.cpp
+++ b/src/examples/test_faramotics_simulation.cpp
@@ -16,10 +16,10 @@
#include "unistd.h"
// wolf
-#include "wolf.h"
-#include "feature_base.h"
-#include "landmark_base.h"
-#include "state_block.h"
+#include "base/wolf.h"
+#include "base/feature/feature_base.h"
+#include "base/landmark/landmark_base.h"
+#include "base/state_block.h"
//faramotics includes
#include "faramotics/dynamicSceneRender.h"
@@ -60,7 +60,6 @@ class FaramoticsRobot
myScanner->loadAssimpModel(modelFileName);
}
-
//function travel around
Eigen::Vector3s motionCampus(unsigned int ii, double& displacement_, double& rotation_)
{
@@ -216,7 +215,6 @@ int main(int argc, char** argv)
// ROBOT MOVEMENT ---------------------------
ground_truth = robot.motionCampus(step, odom_data(0), odom_data(1));
-
// LIDAR DATA ---------------------------
scan1 = robot.computeScan(1);
scan2 = robot.computeScan(2);
diff --git a/examples/test_fcn_ptr.cpp b/src/examples/test_fcn_ptr.cpp
similarity index 99%
rename from examples/test_fcn_ptr.cpp
rename to src/examples/test_fcn_ptr.cpp
index 70b38393f03e1212907686c0a099d780d6d1a701..8ca82790e934cb0c9236ad83cfe7a92d1cc6d284 100644
--- a/examples/test_fcn_ptr.cpp
+++ b/src/examples/test_fcn_ptr.cpp
@@ -15,7 +15,6 @@ double divide (double _n, double _d) { return _n/_d; }
double mult (double _x, double _y) { return _x*_y; }
double mult_div (double _x, double _y, double _z) { return _x*_y/_z; }
-
//======== test_simple usage of function pointers ============
typedef double (*FcnType)(double);
@@ -119,7 +118,6 @@ void test_var()
std::cout << "4*3/6 = " << run_v((FcnTypeV)mult_div, 3, 4.0, 3.0, 6.0) << std::endl;
}
-
//####################################################################################
int main()
diff --git a/examples/test_image.cpp b/src/examples/test_image.cpp
similarity index 97%
rename from examples/test_image.cpp
rename to src/examples/test_image.cpp
index c5afcc61b462553ad01de0e4f27de31f6854d8ea..feb794e1f9263bb3732bcfadb41d61fae0b5be8c 100644
--- a/examples/test_image.cpp
+++ b/src/examples/test_image.cpp
@@ -1,10 +1,10 @@
// Testing things for the 3D image odometry
//Wolf includes
-#include "sensor_camera.h"
-#include "capture_image.h"
-#include "processor_tracker_feature_image.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/sensor/sensor_camera.h"
+#include "base/capture/capture_image.h"
+#include "base/processor/processor_tracker_feature_image.h"
+#include "base/ceres_wrapper/ceres_manager.h"
// Vision utils includes
#include <vision_utils.h>
diff --git a/examples/test_imuDock.cpp b/src/examples/test_imuDock.cpp
similarity index 97%
rename from examples/test_imuDock.cpp
rename to src/examples/test_imuDock.cpp
index d68e8dd4391d9733c81c55c8f78d821f140fe700..bcaa85d9c56b2586473609e1ec388fe6f6169f9f 100644
--- a/examples/test_imuDock.cpp
+++ b/src/examples/test_imuDock.cpp
@@ -5,21 +5,21 @@
* \author: Dinesh Atchuthan
*/
-#include <processor_IMU.h>
-#include <sensor_IMU.h>
-#include "wolf.h"
-#include "problem.h"
-#include "ceres_wrapper/ceres_manager.h"
-#include "sensor_odom_3D.h"
-#include "processor_odom_3D.h"
+#include "base/processor/processor_IMU.h"
+#include "base/sensor/sensor_IMU.h"
+#include "base/wolf.h"
+#include "base/problem.h"
+#include "base/ceres_wrapper/ceres_manager.h"
+#include "base/sensor/sensor_odom_3D.h"
+#include "base/processor/processor_odom_3D.h"
//Constraints headers
-#include "constraint_fix_bias.h"
+#include "base/constraint/constraint_fix_bias.h"
//std
#include <iostream>
#include <fstream>
-#include "../constraint_pose_3D.h"
+#include "base/constraint/constraint_pose_3D.h"
#define OUTPUT_RESULTS
//#define ADD_KF3
diff --git a/examples/test_imuDock_autoKFs.cpp b/src/examples/test_imuDock_autoKFs.cpp
similarity index 97%
rename from examples/test_imuDock_autoKFs.cpp
rename to src/examples/test_imuDock_autoKFs.cpp
index 03677027f7dc687d4e55a303ccbbefe2030a4c3a..d0665c7463d3bb087c967641f0e326fe54eaa2e6 100644
--- a/examples/test_imuDock_autoKFs.cpp
+++ b/src/examples/test_imuDock_autoKFs.cpp
@@ -5,21 +5,21 @@
* \author: Dinesh Atchuthan
*/
-#include <processor_IMU.h>
-#include <sensor_IMU.h>
-#include "wolf.h"
-#include "problem.h"
-#include "ceres_wrapper/ceres_manager.h"
-#include "sensor_odom_3D.h"
-#include "processor_odom_3D.h"
+#include "base/processor/processor_IMU.h"
+#include "base/sensor/sensor_IMU.h"
+#include "base/wolf.h"
+#include "base/problem.h"
+#include "base/ceres_wrapper/ceres_manager.h"
+#include "base/sensor/sensor_odom_3D.h"
+#include "base/processor/processor_odom_3D.h"
//Constraints headers
-#include "constraint_fix_bias.h"
+#include "base/constraint/constraint_fix_bias.h"
//std
#include <iostream>
#include <fstream>
-#include "../constraint_pose_3D.h"
+#include "base/constraint/constraint_pose_3D.h"
#define OUTPUT_RESULTS
//#define AUTO_KFS
diff --git a/examples/test_imuPlateform_Offline.cpp b/src/examples/test_imuPlateform_Offline.cpp
similarity index 96%
rename from examples/test_imuPlateform_Offline.cpp
rename to src/examples/test_imuPlateform_Offline.cpp
index 9d834d73e8408b1207a8f48d9225b4ec2f08d989..e1f9d23fb11994758d55f4cd145fb92c1e2d103d 100644
--- a/examples/test_imuPlateform_Offline.cpp
+++ b/src/examples/test_imuPlateform_Offline.cpp
@@ -1,15 +1,15 @@
//Wolf
-#include <capture_IMU.h>
-#include <processor_IMU.h>
-#include <sensor_IMU.h>
-#include "wolf.h"
-#include "problem.h"
-#include "sensor_odom_3D.h"
-#include "constraint_odom_3D.h"
-#include "state_block.h"
-#include "state_quaternion.h"
-#include "processor_odom_3D.h"
-#include "ceres_wrapper/ceres_manager.h"
+#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/sensor/sensor_odom_3D.h"
+#include "base/constraint/constraint_odom_3D.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
+#include "base/processor/processor_odom_3D.h"
+#include "base/ceres_wrapper/ceres_manager.h"
//std
#include <iostream>
@@ -93,7 +93,6 @@ int main(int argc, char** argv)
SensorIMUPtr sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
ProcessorIMUPtr processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
// SENSOR + PROCESSOR ODOM 3D
SensorBasePtr sen1_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D.yaml");
ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
@@ -177,7 +176,6 @@ int main(int argc, char** argv)
std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
<< (wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
-
// Print statistics
std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
@@ -235,7 +233,6 @@ int main(int argc, char** argv)
for(int i = 0; i<16; i++)
cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
debug_results << std::setprecision(16) << ts.get() << "\t" << frm_state(0) << "\t" << frm_state(1) << "\t" << frm_state(2)
<< "\t" << frm_state(3) << "\t" << frm_state(4) << "\t" << frm_state(5) << "\t" << frm_state(6)
<< "\t" << frm_state(7) << "\t" << frm_state(8) << "\t" << frm_state(9)
diff --git a/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp
similarity index 97%
rename from examples/test_imu_constrained0.cpp
rename to src/examples/test_imu_constrained0.cpp
index 87330497c75beb1e0464382fd4ec1437bc848272..d20f108d8288c6d2381d7cc1619281171104c3d7 100644
--- a/examples/test_imu_constrained0.cpp
+++ b/src/examples/test_imu_constrained0.cpp
@@ -1,15 +1,15 @@
//Wolf
-#include <capture_IMU.h>
-#include <processor_IMU.h>
-#include <sensor_IMU.h>
-#include "wolf.h"
-#include "problem.h"
-#include "sensor_odom_3D.h"
-#include "constraint_odom_3D.h"
-#include "state_block.h"
-#include "state_quaternion.h"
-#include "processor_odom_3D.h"
-#include "ceres_wrapper/ceres_manager.h"
+#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/sensor/sensor_odom_3D.h"
+#include "base/constraint/constraint_odom_3D.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
+#include "base/processor/processor_odom_3D.h"
+#include "base/ceres_wrapper/ceres_manager.h"
//std
#include <iostream>
@@ -34,7 +34,6 @@ int main(int argc, char** argv)
// FOR IMU, file content is : Timestampt\t Ax\t Ay\t Az\t Wx\t Wy\t Wz
// FOR ODOM, file content is : Timestampt\t Δpx\t Δpy\t Δpz\t Δox\t Δoy\t Δoz
-
std::ifstream imu_data_input;
std::ifstream odom_data_input;
const char * filename_imu;
@@ -115,7 +114,6 @@ int main(int argc, char** argv)
SensorIMUPtr sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
ProcessorIMUPtr processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
// SENSOR + PROCESSOR ODOM 3D
SensorBasePtr sen1_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml");
ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
@@ -195,7 +193,6 @@ int main(int argc, char** argv)
t = ts;
//std::string report = ceres_manager_wolf_diff->solve(1); //0: nothing, 1: BriefReport, 2: FullReport
-
Eigen::VectorXs frm_state(16);
frm_state = origin_KF->getState();
@@ -234,7 +231,6 @@ int main(int argc, char** argv)
std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
<< (wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal()).array().sqrt() << std::endl;
-
// Print statistics
std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
@@ -300,7 +296,6 @@ int main(int argc, char** argv)
for(int i = 0; i<16; i++)
cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
debug_results << std::setprecision(16) << ts.get() << "\t" << frm_state(0) << "\t" << frm_state(1) << "\t" << frm_state(2)
<< "\t" << frm_state(3) << "\t" << frm_state(4) << "\t" << frm_state(5) << "\t" << frm_state(6)
<< "\t" << frm_state(7) << "\t" << frm_state(8) << "\t" << frm_state(9)
diff --git a/examples/test_kf_callback.cpp b/src/examples/test_kf_callback.cpp
similarity index 93%
rename from examples/test_kf_callback.cpp
rename to src/examples/test_kf_callback.cpp
index 2822294e75b479df9f493810c73e28bd558aa4f6..8f049c8d62e8dc807dbf81c3aad3326dbc6a1090 100644
--- a/examples/test_kf_callback.cpp
+++ b/src/examples/test_kf_callback.cpp
@@ -5,12 +5,10 @@
* Author: jsola
*/
-
-
-#include "../sensor_odom_2D.h"
-#include "../processor_odom_2D.h"
-#include "../processor_tracker_feature_dummy.h"
-#include "../capture_void.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/processor/processor_odom_2D.h"
+#include "base/processor/processor_tracker_feature_dummy.h"
+#include "base/capture/capture_void.h"
int main()
{
diff --git a/examples/test_list_remove.cpp b/src/examples/test_list_remove.cpp
similarity index 99%
rename from examples/test_list_remove.cpp
rename to src/examples/test_list_remove.cpp
index 458ae19db089b34524c7058a34a3330961256c58..b0795d0a687e92b774c50de49c2645a488676ced 100644
--- a/examples/test_list_remove.cpp
+++ b/src/examples/test_list_remove.cpp
@@ -5,8 +5,6 @@
* Author: jsola
*/
-
-
#include <memory>
#include <list>
#include <algorithm>
diff --git a/examples/test_map_yaml.cpp b/src/examples/test_map_yaml.cpp
similarity index 94%
rename from examples/test_map_yaml.cpp
rename to src/examples/test_map_yaml.cpp
index 80273a3845d5a6368ba4b570b44e45f3fa1e1188..7990919ca49bfee5be8950726b008490d523ed29 100644
--- a/examples/test_map_yaml.cpp
+++ b/src/examples/test_map_yaml.cpp
@@ -5,13 +5,13 @@
* \author: jsola
*/
-#include "wolf.h"
-#include "problem.h"
-#include "map_base.h"
-#include "landmark_polyline_2D.h"
-#include "landmark_AHP.h"
-#include "state_block.h"
-#include "../yaml/yaml_conversion.h"
+#include "base/wolf.h"
+#include "base/problem.h"
+#include "base/map_base.h"
+#include "base/landmark/landmark_polyline_2D.h"
+#include "base/landmark/landmark_AHP.h"
+#include "base/state_block.h"
+#include "base/yaml/yaml_conversion.h"
#include <iostream>
using namespace wolf;
@@ -88,7 +88,6 @@ int main()
std::string thisfile = __FILE__;
problem->getMapPtr()->save(filename, "Example generated by test file " + thisfile);
-
std::cout << "Clearing map... " << std::endl;
problem->getMapPtr()->getLandmarkList().clear();
diff --git a/examples/test_matrix_prod.cpp b/src/examples/test_matrix_prod.cpp
similarity index 99%
rename from examples/test_matrix_prod.cpp
rename to src/examples/test_matrix_prod.cpp
index a99ff1424ced7fee782f6a1d1f0268fa3dc49e0c..b03068283ac33b65a430534a3deb8cf0856195ed 100644
--- a/examples/test_matrix_prod.cpp
+++ b/src/examples/test_matrix_prod.cpp
@@ -88,8 +88,6 @@ EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,9,1>)
std::cout << "Time Co = C * C: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N \
<< "ns <-- this is the Eigen default!" << std::endl;
-
-
/**
* We multiply matrices and see how long it takes.
* We compare different combinations of row-major and column-major to see which one is the fastest.
diff --git a/examples/test_mpu.cpp b/src/examples/test_mpu.cpp
similarity index 97%
rename from examples/test_mpu.cpp
rename to src/examples/test_mpu.cpp
index 4acbf55b7ad78818c5b16688471eee6126bd6828..08db29a812d7f6b2d1ce89605e059ff56b0abd03 100644
--- a/examples/test_mpu.cpp
+++ b/src/examples/test_mpu.cpp
@@ -6,11 +6,11 @@
*/
//Wolf
-#include <capture_IMU.h>
-#include "wolf.h"
-#include "problem.h"
-#include "state_block.h"
-#include "state_quaternion.h"
+#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 <iostream>
#include <fstream>
#include <iomanip>
@@ -18,8 +18,8 @@
#include <cmath>
#include <termios.h>
#include <fcntl.h>
-#include <processor_IMU.h>
-#include <sensor_IMU.h>
+#include "base/processor/processor_IMU.h"
+#include "base/sensor/sensor_IMU.h"
#define DEBUG_RESULTS
#define FROM_FILE
@@ -194,7 +194,6 @@ int main(int argc, char** argv)
std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
<< (wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
-
// Print statistics
std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
diff --git a/examples/test_processor_imu.cpp b/src/examples/test_processor_imu.cpp
similarity index 97%
rename from examples/test_processor_imu.cpp
rename to src/examples/test_processor_imu.cpp
index 119e2fe629e74a2e94815ffdb21234b5f9aa997f..f9b57a323450fb99fb84f192f4d20c4a1bbe0085 100644
--- a/examples/test_processor_imu.cpp
+++ b/src/examples/test_processor_imu.cpp
@@ -6,13 +6,13 @@
*/
//Wolf
-#include <capture_IMU.h>
-#include <processor_IMU.h>
-#include <sensor_IMU.h>
-#include "wolf.h"
-#include "problem.h"
-#include "state_block.h"
-#include "state_quaternion.h"
+#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 <iostream>
#include <fstream>
#include <iomanip>
@@ -21,7 +21,6 @@
//#define DEBUG_RESULTS
-
int main(int argc, char** argv)
{
using namespace wolf;
@@ -185,7 +184,6 @@ int main(int argc, char** argv)
// std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
// << (problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
-
// Print statistics
std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
diff --git a/examples/test_processor_imu_jacobians.cpp b/src/examples/test_processor_imu_jacobians.cpp
similarity index 99%
rename from examples/test_processor_imu_jacobians.cpp
rename to src/examples/test_processor_imu_jacobians.cpp
index 6cdb01c976419b4ef10785c6e94bb4be7504bf90..e876a21d97080d8c9b20bf0ef3e1691124f3018a 100644
--- a/examples/test_processor_imu_jacobians.cpp
+++ b/src/examples/test_processor_imu_jacobians.cpp
@@ -6,13 +6,13 @@
*/
//Wolf
-#include <capture_IMU.h>
-#include <sensor_IMU.h>
+#include "base/capture/capture_IMU.h"
+#include "base/sensor/sensor_IMU.h"
#include <test/processor_IMU_UnitTester.h>
-#include "wolf.h"
-#include "problem.h"
-#include "state_block.h"
-#include "state_quaternion.h"
+#include "base/wolf.h"
+#include "base/problem.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
#include <iostream>
#include <fstream>
#include <iomanip>
@@ -192,7 +192,6 @@ int main(int argc, char** argv)
else
std::cout<< "\t\tdDq_dab_ jacobian is not Zero :" << dDq_dab << std::endl;
-
/* Numerical method to check jacobians wrt noise
dDp_dP = [dDp_dPx, dDp_dPy, dDp_dPz]
diff --git a/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp
similarity index 91%
rename from examples/test_processor_odom_3D.cpp
rename to src/examples/test_processor_odom_3D.cpp
index b7c2c9a9d7313387990b6a9b1b02de18707e5b37..c9321b51e9da6693783ea1a220d4a158623d37d3 100644
--- a/examples/test_processor_odom_3D.cpp
+++ b/src/examples/test_processor_odom_3D.cpp
@@ -5,14 +5,13 @@
* \author: jsola
*/
-
-#include <capture_IMU.h>
-#include "problem.h"
-#include "sensor_odom_2D.h"
-#include "processor_odom_3D.h"
-#include "map_base.h"
-#include "landmark_base.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/capture/capture_IMU.h"
+#include "base/problem.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/processor/processor_odom_3D.h"
+#include "base/map_base.h"
+#include "base/landmark/landmark_base.h"
+#include "base/ceres_wrapper/ceres_manager.h"
#include <cstdlib>
@@ -25,8 +24,6 @@ using Eigen::Vector7s;
using Eigen::Quaternions;
using Eigen::VectorXs;
-
-
int main (int argc, char** argv)
{
cout << "\n========= Test ProcessorOdom3D ===========" << endl;
diff --git a/examples/test_processor_tracker_feature.cpp b/src/examples/test_processor_tracker_feature.cpp
similarity index 87%
rename from examples/test_processor_tracker_feature.cpp
rename to src/examples/test_processor_tracker_feature.cpp
index 943e1801078adc795409deebe50cb9b49f16dabb..ab22be41878fa14bdd84425ce150b30008834696 100644
--- a/examples/test_processor_tracker_feature.cpp
+++ b/src/examples/test_processor_tracker_feature.cpp
@@ -9,12 +9,12 @@
#include <iostream>
//Wolf
-#include "wolf.h"
-#include "problem.h"
-#include "sensor_base.h"
-#include "state_block.h"
-#include "processor_tracker_feature_dummy.h"
-#include "capture_void.h"
+#include "base/wolf.h"
+#include "base/problem.h"
+#include "base/sensor/sensor_base.h"
+#include "base/state_block.h"
+#include "base/processor/processor_tracker_feature_dummy.h"
+#include "base/capture/capture_void.h"
int main()
{
diff --git a/examples/test_processor_tracker_landmark.cpp b/src/examples/test_processor_tracker_landmark.cpp
similarity index 93%
rename from examples/test_processor_tracker_landmark.cpp
rename to src/examples/test_processor_tracker_landmark.cpp
index bbf3e4132ed3dc6cae6342908a95d1c6382fe559..15ed24d723b3c0ceee70cc4bd40ef388c910ac5b 100644
--- a/examples/test_processor_tracker_landmark.cpp
+++ b/src/examples/test_processor_tracker_landmark.cpp
@@ -9,12 +9,12 @@
#include <iostream>
//Wolf
-#include "wolf.h"
-#include "problem.h"
-#include "sensor_base.h"
-#include "state_block.h"
-#include "processor_tracker_landmark_dummy.h"
-#include "capture_void.h"
+#include "base/wolf.h"
+#include "base/problem.h"
+#include "base/sensor/sensor_base.h"
+#include "base/state_block.h"
+#include "base/processor/processor_tracker_landmark_dummy.h"
+#include "base/capture/capture_void.h"
void print_problem_pointers(wolf::ProblemPtr wolf_problem_ptr_)
{
@@ -87,9 +87,6 @@ int main()
wolf_problem_ptr_->print(2);
-
return 0;
}
-
-
diff --git a/examples/test_processor_tracker_landmark_image.cpp b/src/examples/test_processor_tracker_landmark_image.cpp
similarity index 96%
rename from examples/test_processor_tracker_landmark_image.cpp
rename to src/examples/test_processor_tracker_landmark_image.cpp
index 5a19159012454151da1f0c52aa06a830163addba..f5b084e13fb1e336cbc3d80fc9a532d22e5ba043 100644
--- a/examples/test_processor_tracker_landmark_image.cpp
+++ b/src/examples/test_processor_tracker_landmark_image.cpp
@@ -1,18 +1,18 @@
//std
#include <iostream>
-#include "processor_tracker_landmark_image.h"
+#include "base/processor/processor_tracker_landmark_image.h"
//Wolf
-#include "wolf.h"
-#include "problem.h"
-#include "state_block.h"
-#include "processor_odom_3D.h"
-#include "sensor_odom_3D.h"
-#include "sensor_camera.h"
-#include "capture_image.h"
-#include "capture_pose.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/wolf.h"
+#include "base/problem.h"
+#include "base/state_block.h"
+#include "base/processor/processor_odom_3D.h"
+#include "base/sensor/sensor_odom_3D.h"
+#include "base/sensor/sensor_camera.h"
+#include "base/capture/capture_image.h"
+#include "base/capture/capture_pose.h"
+#include "base/ceres_wrapper/ceres_manager.h"
// Vision utils includes
#include <vision_utils.h>
@@ -221,12 +221,10 @@ int main(int argc, char** argv)
// std::cout << "current ts: " << t.get() << std::endl;
// std::cout << " dt: " << t_prev - t_prev_prev << "; dx: " << dx.transpose() << std::endl;
-
// Cleanup map ---------------------------------------
cleanupMap(problem, t, 2, 5); // dt, min_ctr
-
// Solve -----------------------------------------------
// solve only when new KFs are added
if (problem->getTrajectoryPtr()->getFrameList().size() > number_of_KFs)
@@ -236,7 +234,6 @@ int main(int argc, char** argv)
std::cout << summary << std::endl;
}
-
// Finish loop -----------------------------------------
cv::Mat image_graphics = frame_buff.back()->getImage().clone();
prc_img_ptr->drawTrackerRoi(image_graphics, cv::Scalar(255.0, 0.0, 255.0)); //tracker roi
diff --git a/examples/test_projection_points.cpp b/src/examples/test_projection_points.cpp
similarity index 99%
rename from examples/test_projection_points.cpp
rename to src/examples/test_projection_points.cpp
index 7945c939f82355435c422c691433d9b48937ccda..c24e8904eeaf06490b0fa3283e52cd074f1bcf70 100644
--- a/examples/test_projection_points.cpp
+++ b/src/examples/test_projection_points.cpp
@@ -6,7 +6,7 @@
#include <iostream>
//wolf includes
-#include "pinhole_tools.h"
+#include "base/pinhole_tools.h"
int main(int argc, char** argv)
{
@@ -41,7 +41,6 @@ int main(int argc, char** argv)
for (int ii = 0; ii < points3D.rows(); ++ii)
std::cout << "points2D- X: " << points2D(ii,0) << "; Y: " << points2D(ii,1) << std::endl;
-
std::cout << std::endl << " ========= PinholeTools DUALITY TEST ===========" << std::endl << std::endl;
//================================= projectPoint and backprojectPoint to/from NormalizedPlane
@@ -67,14 +66,12 @@ int main(int argc, char** argv)
pinhole::backprojectPointFromNormalizedPlane(project_point_normalized_output,
backproject_point_normalized_depth);
-
std::cout << "TEST project and backproject PointToNormalizedPlane" << std::endl;
std::cout << std:: endl << "Original " << project_point_normalized_test.transpose() << std::endl;
std::cout << std:: endl << "Project " << project_point_normalized_output.transpose() << std::endl;
std::cout << std:: endl << "Alternate project" << project_point_normalized_output.transpose() << std::endl;
std::cout << std:: endl << "Backproject " << backproject_point_normalized_output.transpose() << std::endl;
-
//================================= projectPoint and backprojectPoint to/from NormalizedPlane WITH JACOBIANS
Eigen::Vector3s pp_normalized_test;
@@ -92,7 +89,6 @@ int main(int argc, char** argv)
Eigen::Matrix<Scalar, 3, 2> bpp_normalized_jacobian;
Eigen::Vector3s bpp_normalized_jacobian_depth;
-
pinhole::projectPointToNormalizedPlane(pp_normalized_test,
pp_normalized_output,
pp_normalized_jacobian);
@@ -123,13 +119,11 @@ int main(int argc, char** argv)
std::cout << "\n--> Jacobian" << std::endl << bpp_normalized_jacobian << std::endl;
std::cout << "\n--> Jacobian - depth" << bpp_normalized_jacobian_depth.transpose() << std::endl;
-
Eigen::Matrix2s test_jacobian; // (2x3) * (3x2) = (2x2)
test_jacobian = pp_normalized_jacobian * bpp_normalized_jacobian;
std::cout << "\n\n\n==> Jacobian Testing" << std::endl << test_jacobian << std::endl;
-
//================================= IsInRoi / IsInImage
Eigen::Vector2s pix;
@@ -166,8 +160,6 @@ int main(int argc, char** argv)
std::cout << "width: " << image_width << "; height: " << image_height << std::endl;
std::cout << "is_in_image: " << is_in_image << std::endl;
-
-
//================================= computeCorrectionModel
Eigen::Vector2s distortion2;
@@ -194,11 +186,8 @@ int main(int argc, char** argv)
std::cout << std::endl << "correction" << std::endl;
std::cout << "c1: " << correction_test2[0] << "; c2: " << correction_test2[1] << std::endl;
-
-
//================================= distortPoint
-
Eigen::Vector2s distorting_point;
distorting_point[0] = 0.35355;
distorting_point[1] = 0.35355;
@@ -237,8 +226,6 @@ int main(int argc, char** argv)
std::cout << "\n--> Jacobian" << std::endl <<
distortion_jacobian2 << std::endl;
-
-
Eigen::Vector2s corrected_point5;
Eigen::Matrix2s corrected_jacobian2;
pinhole::undistortPoint(correction_test2,
@@ -280,7 +267,6 @@ int main(int argc, char** argv)
std::cout << std::endl << "Depixelized" << std::endl;
std::cout << "x: " << depixelize_output3[0] << "; y: " << depixelize_output3[1] << std::endl;
-
////
Eigen::Vector2s pixelize_output4;
@@ -298,7 +284,6 @@ int main(int argc, char** argv)
std::cout << "\n--> Jacobian" << std::endl <<
pixelize_jacobian2 << std::endl;
-
Eigen::Vector2s depixelize_output4;
Eigen::Matrix2s depixelize_jacobian2;
pinhole::depixellizePoint(k_test2,
@@ -317,12 +302,8 @@ int main(int argc, char** argv)
std::cout << "\n\n\n==> Jacobian Testing" << std::endl <<
test_jacobian_pix << std::endl;
-
-
-
//================================= projectPoint Complete
-
// //distortion
// distortion2;
@@ -332,7 +313,6 @@ int main(int argc, char** argv)
// //3Dpoint
// project_point_normalized_test;
-
Eigen::Vector2s point2D_test5;
point2D_test5 = pinhole::projectPoint(k_test2,
distortion2,
@@ -393,7 +373,6 @@ int main(int argc, char** argv)
std::cout << "\n-->Jacobian" << std::endl <<
jacobian_test4 << std::endl;
-
/////////////////////////////
// //correction
@@ -414,8 +393,6 @@ int main(int argc, char** argv)
std::cout << std::endl << "First function output" << std::endl;
std::cout << "x: " << point3D_backproj5[0] << "; y: " << point3D_backproj5[1] << "; z: " << point3D_backproj5[2] << std::endl;
-
-
//jacobian
Eigen::Vector3s point3D_backproj4;
Eigen::MatrixXs jacobian_backproj2(3,2);
@@ -435,20 +412,12 @@ int main(int argc, char** argv)
std::cout << "\n--> Jacobian - depth" << std::endl <<
depth_jacobian2[0] << " " << depth_jacobian2[1] << " " << depth_jacobian2[2] << " " << std::endl;
-
-
-
Eigen::Matrix2s test_jacobian_complete;
test_jacobian_complete = jacobian_test4 * jacobian_backproj2;
std::cout << "\n\n\n==> Jacobian Testing" << std::endl <<
test_jacobian_complete << std::endl;
-
-
-
-
-
/* Test */
std::cout << "\n\n\n\nTEST\n" << std::endl;
@@ -476,8 +445,6 @@ int main(int argc, char** argv)
Eigen::Vector2s test_point2D = {123,321};
std::cout << "\ntest_point2D ORIGINAL:\n" << test_point2D << std::endl;
-
-
test_point2D = pinhole::depixellizePoint(K_params,
test_point2D);
std::cout << "\ntest_point2D DEPIXELIZED:\n" << test_point2D << std::endl;
@@ -488,8 +455,6 @@ int main(int argc, char** argv)
2);
std::cout << "\ntest_point3D BACKPROJECTED:\n" << test_point3D << std::endl;
-
-
test_point2D = pinhole::projectPointToNormalizedPlane(test_point3D);
std::cout << "\ntest_point2D NORMALIZED:\n" << test_point2D << std::endl;
test_point2D = pinhole::distortPoint(distortion_vector,
@@ -501,9 +466,3 @@ int main(int argc, char** argv)
}
-
-
-
-
-
-
diff --git a/examples/test_sh_ptr.cpp b/src/examples/test_sh_ptr.cpp
similarity index 99%
rename from examples/test_sh_ptr.cpp
rename to src/examples/test_sh_ptr.cpp
index 74bdd8bd7158c0ba5501c6b844e7fc68d9f9fea5..294555ed1d5e7a06119463e933bf116f5b65ae14 100644
--- a/examples/test_sh_ptr.cpp
+++ b/src/examples/test_sh_ptr.cpp
@@ -39,7 +39,6 @@ class c; // constraint
class M; // map
class L; // landmark
-
//////////////////////////////////////////////////////////////////////////////////
// DECLARE WOLF TREE
@@ -318,8 +317,6 @@ class M : public enable_shared_from_this<M>
shared_ptr<L> add_L(shared_ptr<L> _L);
};
-
-
class L : public enable_shared_from_this<L>
{
private:
@@ -522,7 +519,6 @@ void c::remove()
}
}
-
shared_ptr<L> M::add_L(shared_ptr<L> _L)
{
L_list_.push_back(_L);
@@ -544,8 +540,6 @@ void L::remove()
}
}
-
-
}
//////////////////////////////////////////////////////////////////////////////////
@@ -770,7 +764,6 @@ int main()
cout<<endl;
print_c(Pp);
-
//------------------------------------------------------------------
// Several tests. Uncomment the desired test.
// Run only one test at a time, otherwise you'll get segfaults!
@@ -800,4 +793,3 @@ int main()
return 1;
}
-
diff --git a/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp
similarity index 96%
rename from examples/test_simple_AHP.cpp
rename to src/examples/test_simple_AHP.cpp
index 6a3279326aafb627e7c12a57b39aef26fb93ad4d..818250e17f27518cd834bd154eb5ec01d7ebef1e 100644
--- a/examples/test_simple_AHP.cpp
+++ b/src/examples/test_simple_AHP.cpp
@@ -5,15 +5,15 @@
* \author: jtarraso
*/
-#include "wolf.h"
-#include "frame_base.h"
-#include "pinhole_tools.h"
-#include "sensor_camera.h"
-#include "rotations.h"
-#include "capture_image.h"
-#include "landmark_AHP.h"
-#include "constraint_AHP.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/wolf.h"
+#include "base/frame_base.h"
+#include "base/pinhole_tools.h"
+#include "base/sensor/sensor_camera.h"
+#include "base/rotations.h"
+#include "base/capture/capture_image.h"
+#include "base/landmark/landmark_AHP.h"
+#include "base/constraint/constraint_AHP.h"
+#include "base/ceres_wrapper/ceres_manager.h"
// Vision utils
#include <vision_utils/vision_utils.h>
@@ -61,12 +61,10 @@
* 0 160 320 480 640
* +----+----+----+----+
* |
- * |
* | 320
* |
* *
*
- *
* projected pixels:
* p0 = (320,240) // at optical axis or relation 1:0
* p1 = ( 0 ,240) // at 45 deg or relation 1:1
@@ -129,7 +127,6 @@ int main(int argc, char** argv)
// std::cout << "lmk hmg in C frame: " << lmk_hmg_c.transpose() << std::endl;
// ============================================================================================================
-
// Captures------------------
cv::Mat cv_image;
cv_image.zeros(2,2,0);
@@ -199,7 +196,6 @@ int main(int argc, char** argv)
FeaturePointImagePtr feat_4 = std::make_shared<FeaturePointImage>(kp_2, 0, desc, Eigen::Matrix2s::Identity());
image_2->addFeature(feat_4);
-
// New landmark with measured pixels from kf2 (anchor) kf3 and kf4 (measurements)
Scalar unknown_distance = 2; // the real distance is 1
Matrix3s K = camera->getIntrinsicMatrix();
@@ -241,7 +237,6 @@ int main(int argc, char** argv)
CeresManager ceres_manager(problem, ceres_options);
-
std::string summary = ceres_manager.solve(SolverManager::ReportVerbosity::FULL);// 0: nothing, 1: BriefReport, 2: FullReport
std::cout << summary << std::endl;
@@ -260,18 +255,3 @@ int main(int argc, char** argv)
}
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/examples/test_sort_keyframes.cpp b/src/examples/test_sort_keyframes.cpp
similarity index 95%
rename from examples/test_sort_keyframes.cpp
rename to src/examples/test_sort_keyframes.cpp
index a1e908a616b018e86dcb8d42d3516285fbe7183b..967d37006e16bacb3af02c088c6397f2efabea57 100644
--- a/examples/test_sort_keyframes.cpp
+++ b/src/examples/test_sort_keyframes.cpp
@@ -6,10 +6,10 @@
*/
// Wolf includes
-#include "../wolf.h"
-#include "../problem.h"
-#include "../frame_base.h"
-#include "../trajectory_base.h"
+#include "base/wolf.h"
+#include "base/problem.h"
+#include "base/frame_base.h"
+#include "base/trajectory_base.h"
// STL includes
#include <list>
@@ -75,7 +75,5 @@ int main()
printFrames(problem_ptr);
-
-
return 0;
}
diff --git a/examples/test_sparsification.cpp b/src/examples/test_sparsification.cpp
similarity index 98%
rename from examples/test_sparsification.cpp
rename to src/examples/test_sparsification.cpp
index ea8113d1bccfc185a9ea69fedbd90f0967969922..41b3b8eae524684c795439a5d7021cf8fdf589f8 100644
--- a/examples/test_sparsification.cpp
+++ b/src/examples/test_sparsification.cpp
@@ -14,10 +14,10 @@
#include <queue>
//Wolf includes
-#include "capture_void.h"
-#include "feature_odom_2D.h"
-#include "constraint_base.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/capture/capture_void.h"
+#include "base/feature/feature_odom_2D.h"
+#include "base/constraint/constraint_base.h"
+#include "base/ceres_wrapper/ceres_manager.h"
// EIGEN
//#include <Eigen/CholmodSupport>
@@ -297,7 +297,6 @@ int main(int argc, char** argv)
// covariance
bl_ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);//ALL_MARGINALS
-
// t1 = clock();
// double t_sigma_manual = 0;
// t_sigma_manual += ((double) clock() - t1) / CLOCKS_PER_SEC;
diff --git a/examples/test_state_quaternion.cpp b/src/examples/test_state_quaternion.cpp
similarity index 90%
rename from examples/test_state_quaternion.cpp
rename to src/examples/test_state_quaternion.cpp
index ec23b9e0f4d2ccc05b02b9e151783c6a73b8ae30..4d2aa60cc3f0f677c831f469195432e061f6fe30 100644
--- a/examples/test_state_quaternion.cpp
+++ b/src/examples/test_state_quaternion.cpp
@@ -5,10 +5,9 @@
* \author: jsola
*/
-
-#include "frame_base.h"
-#include "state_quaternion.h"
-#include "time_stamp.h"
+#include "base/frame_base.h"
+#include "base/state_quaternion.h"
+#include "base/time_stamp.h"
#include <iostream>
@@ -36,6 +35,5 @@ int main (void)
std::cout << "Done" << std::endl;
-
return 1;
}
diff --git a/examples/test_tracker_ORB.cpp b/src/examples/test_tracker_ORB.cpp
similarity index 99%
rename from examples/test_tracker_ORB.cpp
rename to src/examples/test_tracker_ORB.cpp
index f695a3cb4d7bd0c2e43974c1aef81328846306fe..536e06d62b129b0cf7a62a8de84bafd5b7c106f2 100644
--- a/examples/test_tracker_ORB.cpp
+++ b/src/examples/test_tracker_ORB.cpp
@@ -11,7 +11,7 @@
#include <matchers/bruteforce_hamming_2/matcher_bruteforce_hamming_2.h>
//Wolf
-#include "../processor_tracker_landmark_image.h"
+#include "base/processor/processor_tracker_landmark_image.h"
int main(int argc, char** argv)
{
@@ -244,7 +244,6 @@ int main(int argc, char** argv)
current_keypoints.clear();
current_descriptors.release();
-
std::cout << "\nAFTER THE ADVANCE" << std::endl;
// for(unsigned int i = 0; i < target_keypoints.size(); i++)
// {
diff --git a/examples/test_virtual_hierarchy.cpp b/src/examples/test_virtual_hierarchy.cpp
similarity index 100%
rename from examples/test_virtual_hierarchy.cpp
rename to src/examples/test_virtual_hierarchy.cpp
diff --git a/examples/test_wolf_autodiffwrapper.cpp b/src/examples/test_wolf_autodiffwrapper.cpp
similarity index 99%
rename from examples/test_wolf_autodiffwrapper.cpp
rename to src/examples/test_wolf_autodiffwrapper.cpp
index 5728739a81573f2e99853077d5452ed8b85dd755..a8de6a82b12647cb6e2165bbc9400d2da91b56cd 100644
--- a/examples/test_wolf_autodiffwrapper.cpp
+++ b/src/examples/test_wolf_autodiffwrapper.cpp
@@ -12,8 +12,8 @@
//Wolf includes
#include "wolf_manager.h"
-#include "capture_void.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/capture/capture_void.h"
+#include "base/ceres_wrapper/ceres_manager.h"
int main(int argc, char** argv)
{
@@ -57,8 +57,6 @@ int main(int argc, char** argv)
CeresManager* ceres_manager_ceres_diff = new CeresManager(wolf_problem_ceres_diff, ceres_options, false);
CeresManager* ceres_manager_wolf_diff = new CeresManager(wolf_problem_wolf_diff, ceres_options, true);
-
-
// load graph from .txt
offLineFile_.open(file_path_.c_str(), std::ifstream::in);
if (offLineFile_.is_open())
diff --git a/examples/test_wolf_factories.cpp b/src/examples/test_wolf_factories.cpp
similarity index 92%
rename from examples/test_wolf_factories.cpp
rename to src/examples/test_wolf_factories.cpp
index 3b2b68c18cf8b8c347ce78037e38d00d4f990406..4108864a7319361776556d02a9efe43277643623 100644
--- a/examples/test_wolf_factories.cpp
+++ b/src/examples/test_wolf_factories.cpp
@@ -5,27 +5,26 @@
* \author: jsola
*/
-#include <processor_IMU.h>
-#include <sensor_GPS_fix.h>
-#include "../hardware_base.h"
-#include "../sensor_camera.h"
-#include "../sensor_odom_2D.h"
+#include "base/processor/processor_IMU.h"
+#include "base/sensor/sensor_GPS_fix.h"
+#include "base/hardware_base.h"
+#include "base/sensor/sensor_camera.h"
+#include "base/sensor/sensor_odom_2D.h"
#include "../sensor_imu.h"
//#include "../sensor_gps.h"
-#include "../processor_odom_2D.h"
-#include "../processor_odom_3D.h"
+#include "base/processor/processor_odom_2D.h"
+#include "base/processor/processor_odom_3D.h"
#include "../processor_image_feature.h"
-#include "../problem.h"
+#include "base/problem.h"
-#include "../factory.h"
+#include "base/factory.h"
#include <iostream>
#include <iomanip>
#include <cstdlib>
-
int main(void)
{
using namespace wolf;
@@ -34,8 +33,6 @@ int main(void)
using std::make_shared;
using std::static_pointer_cast;
-
-
//==============================================================================================
std::string wolf_root = _WOLF_ROOT_DIR;
std::string wolf_config = wolf_root + "/src/examples";
@@ -105,7 +102,6 @@ int main(void)
<< " | name: " << setw(17) << prc->getName()
<< " | bound to sensor " << setw(2) << prc->getSensorPtr()->id() << ": " << prc->getSensorPtr()->getName() << endl;
-
return 0;
}
diff --git a/examples/test_wolf_imported_graph.cpp b/src/examples/test_wolf_imported_graph.cpp
similarity index 99%
rename from examples/test_wolf_imported_graph.cpp
rename to src/examples/test_wolf_imported_graph.cpp
index fc8129dc55d1e0152cb621bdde48be0df6368cd3..cbfe23b787c559b5896c1cd178fdd5b260aaa79b 100644
--- a/examples/test_wolf_imported_graph.cpp
+++ b/src/examples/test_wolf_imported_graph.cpp
@@ -12,8 +12,8 @@
//Wolf includes
#include "wolf_manager.h"
-#include "capture_void.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/capture/capture_void.h"
+#include "base/ceres_wrapper/ceres_manager.h"
// EIGEN
//#include <Eigen/CholmodSupport>
@@ -28,7 +28,6 @@ void insertSparseBlock(const Eigen::SparseMatrix<Scalar>& ins, Eigen::SparseMatr
original.makeCompressed();
}
-}
int main(int argc, char** argv)
{
@@ -76,8 +75,6 @@ int main(int argc, char** argv)
CeresManager* ceres_manager_full = new CeresManager(wolf_problem_full, ceres_options);
CeresManager* ceres_manager_prun = new CeresManager(wolf_problem_prun, ceres_options);
-
-
// load graph from .txt
offLineFile_.open(file_path_.c_str(), std::ifstream::in);
if (offLineFile_.is_open())
@@ -362,7 +359,6 @@ int main(int argc, char** argv)
// std::cout << "Sigma_ij" << std::endl << Sigma_ij << std::endl;
// std::cout << "Sigma(i,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameToPtr()]*3, frame_ptr_2_index_prun[(*c_it)->getCapturePtr()->getFramePtr()]*3) << std::endl;
-
//jacobian
xi = *(*c_it)->getFrameOtherPtr()->getPPtr()->getPtr();
yi = *((*c_it)->getFrameOtherPtr()->getPPtr()->getPtr()+1);
diff --git a/examples/test_wolf_logging.cpp b/src/examples/test_wolf_logging.cpp
similarity index 87%
rename from examples/test_wolf_logging.cpp
rename to src/examples/test_wolf_logging.cpp
index 0f92079cd11d4c3a2f48bd369af749907f50a9a8..b516acd098a93ce8c060e0c380555ec29041be3a 100644
--- a/examples/test_wolf_logging.cpp
+++ b/src/examples/test_wolf_logging.cpp
@@ -5,8 +5,8 @@
* \author: Jeremie Deray
*/
-#include "wolf.h"
-#include "logging.h"
+#include "base/wolf.h"
+#include "base/logging.h"
int main(int, char*[])
{
diff --git a/examples/test_wolf_prunning.cpp b/src/examples/test_wolf_prunning.cpp
similarity index 99%
rename from examples/test_wolf_prunning.cpp
rename to src/examples/test_wolf_prunning.cpp
index 3191326c0d50a5c26b3d9ead26d4a86beca9ebb6..13b3dd86d1602f3c3b9fdf9764bcde1f001cdd12 100644
--- a/examples/test_wolf_prunning.cpp
+++ b/src/examples/test_wolf_prunning.cpp
@@ -12,9 +12,9 @@
//Wolf includes
#include "wolf_manager.h"
-#include "capture_void.h"
-#include "constraint_base.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/capture/capture_void.h"
+#include "base/constraint/constraint_base.h"
+#include "base/ceres_wrapper/ceres_manager.h"
// EIGEN
//#include <Eigen/CholmodSupport>
@@ -31,7 +31,6 @@ void insertSparseBlock(const Eigen::SparseMatrix<Scalar>& ins, Eigen::SparseMatr
original.makeCompressed();
}
-}
int main(int argc, char** argv)
{
@@ -403,7 +402,6 @@ int main(int argc, char** argv)
// std::cout << "Sigma_34" << std::endl << Sigma_34 << std::endl;
// std::cout << "Sigma_44" << std::endl << Sigma_44 << std::endl;
-
// jacobians
((ConstraintAnalytic*)(*c_it))->evaluatePureJacobians(jacobians);
Eigen::MatrixXs& J1 = jacobians[0];
@@ -434,7 +432,6 @@ int main(int argc, char** argv)
J4 * Sigma_34.transpose() * J3.transpose() +
J4 * Sigma_44 * J4.transpose()) ).determinant() );
-
// std::cout << "part = " << std::endl << (J1 * Sigma_11 * J1.transpose() +
// J1 * Sigma_12 * J2.transpose() +
// J1 * Sigma_13 * J3.transpose() +
diff --git a/examples/test_wolf_root.cpp b/src/examples/test_wolf_root.cpp
similarity index 92%
rename from examples/test_wolf_root.cpp
rename to src/examples/test_wolf_root.cpp
index f6dc51ce92008b48d015ef496076af14caff5afe..a2e3ccef5d5534daf5be8b1a360e2c30de6b3e6d 100644
--- a/examples/test_wolf_root.cpp
+++ b/src/examples/test_wolf_root.cpp
@@ -6,7 +6,7 @@
*/
//Wolf
-#include "wolf.h"
+#include "base/wolf.h"
//std
#include <iostream>
diff --git a/examples/test_wolf_tree.cpp b/src/examples/test_wolf_tree.cpp
similarity index 100%
rename from examples/test_wolf_tree.cpp
rename to src/examples/test_wolf_tree.cpp
diff --git a/examples/test_yaml.cpp b/src/examples/test_yaml.cpp
similarity index 98%
rename from examples/test_yaml.cpp
rename to src/examples/test_yaml.cpp
index 56a33fe1d29230364e8be5305612ca043eca770d..cfd03d18d073cb9bb85ccc93f342e3ef9c86f52a 100644
--- a/examples/test_yaml.cpp
+++ b/src/examples/test_yaml.cpp
@@ -5,10 +5,10 @@
* \author: jsola
*/
-#include "pinhole_tools.h"
+#include "base/pinhole_tools.h"
#include "yaml/yaml_conversion.h"
#include "processor_image_feature.h"
-#include "factory.h"
+#include "base/factory.h"
#include <yaml-cpp/yaml.h>
@@ -17,7 +17,6 @@
#include <iostream>
#include <fstream>
-
int main()
{
@@ -69,8 +68,6 @@ int main()
else
std::cout << "Bad configuration file. No sensor type found." << std::endl;
-
-
// // Processor Image parameters
//
// ProcessorParamsImage p;
@@ -93,6 +90,5 @@ int main()
// p.min_features_for_keyframe = alg["minimum features for new keyframe"].as<unsigned int>();
// }
-
return 0;
}
diff --git a/examples/test_yaml_conversions.cpp b/src/examples/test_yaml_conversions.cpp
similarity index 97%
rename from examples/test_yaml_conversions.cpp
rename to src/examples/test_yaml_conversions.cpp
index d6393d8b84265a18888c82dc5f26feb992faf3b7..78cf30da7db266a24f578dba6c1df27de6df9921 100644
--- a/examples/test_yaml_conversions.cpp
+++ b/src/examples/test_yaml_conversions.cpp
@@ -5,7 +5,7 @@
* \author: jsola
*/
-#include "../yaml/yaml_conversion.h"
+#include "base/yaml/yaml_conversion.h"
#include <yaml-cpp/yaml.h>
@@ -14,13 +14,11 @@
#include <iostream>
//#include <fstream>
-
int main()
{
using namespace Eigen;
-
std::cout << "\nTrying different yaml specs for matrix..." << std::endl;
YAML::Node mat_sized_23, mat_sized_33, mat_sized_bad, mat_23, mat_33, mat_bad;
diff --git a/examples/vision_utils_active_search.yaml b/src/examples/vision_utils_active_search.yaml
similarity index 100%
rename from examples/vision_utils_active_search.yaml
rename to src/examples/vision_utils_active_search.yaml
diff --git a/src/feature/feature_GPS_fix.cpp b/src/feature/feature_GPS_fix.cpp
index ca370b59b4b50ec482a0887458ea55b7824a96ac..d4b5a12f241b5055e037f4cbd893051687dfb25e 100644
--- a/src/feature/feature_GPS_fix.cpp
+++ b/src/feature/feature_GPS_fix.cpp
@@ -1,4 +1,4 @@
-#include "feature_GPS_fix.h"
+#include "base/feature/feature_GPS_fix.h"
namespace wolf {
diff --git a/src/feature/feature_GPS_pseudorange.cpp b/src/feature/feature_GPS_pseudorange.cpp
index 37bbfd8fa0dec3abbb966441e6fa4a37adb0b2ab..7979f30e45768cc0ebb9a4b1f8b2c376a4dde601 100644
--- a/src/feature/feature_GPS_pseudorange.cpp
+++ b/src/feature/feature_GPS_pseudorange.cpp
@@ -1,4 +1,4 @@
-#include "feature_GPS_pseudorange.h"
+#include "base/feature/feature_GPS_pseudorange.h"
namespace wolf {
diff --git a/src/feature/feature_IMU.cpp b/src/feature/feature_IMU.cpp
index 796f4834a7b056f7636f138b8b2f2d71d3915789..0f811cff599f6bbc62e28dd16b0cc6a82648b02b 100644
--- a/src/feature/feature_IMU.cpp
+++ b/src/feature/feature_IMU.cpp
@@ -1,4 +1,4 @@
-#include "feature_IMU.h"
+#include "base/feature/feature_IMU.h"
namespace wolf {
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 6459ffc244964171a58b839590fc1a5b392fa21b..314457be284f5fc16bfba370c41c47f8ff963ffa 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -1,6 +1,6 @@
-#include "feature_base.h"
-#include "constraint_base.h"
-#include "capture_base.h"
+#include "base/feature/feature_base.h"
+#include "base/constraint/constraint_base.h"
+#include "base/capture/capture_base.h"
namespace wolf {
diff --git a/src/feature/feature_corner_2D.cpp b/src/feature/feature_corner_2D.cpp
index 59db17e3b425f0243f2048b5d9a65f4f30155bcf..27803ef4a9902ec53b6689a6f7fb4ea1c5fddb57 100644
--- a/src/feature/feature_corner_2D.cpp
+++ b/src/feature/feature_corner_2D.cpp
@@ -1,5 +1,5 @@
-#include "feature_corner_2D.h"
+#include "base/feature/feature_corner_2D.h"
namespace wolf {
diff --git a/src/feature/feature_diff_drive.cpp b/src/feature/feature_diff_drive.cpp
index c33d5fdec4e487e4e1b8893625ba140b60b4f54e..c2b8d9ed1172314d3a761c1c06d9c50fb2f3e758 100644
--- a/src/feature/feature_diff_drive.cpp
+++ b/src/feature/feature_diff_drive.cpp
@@ -1,4 +1,4 @@
-#include "feature_diff_drive.h"
+#include "base/feature/feature_diff_drive.h"
namespace wolf {
diff --git a/src/feature/feature_line_2D.cpp b/src/feature/feature_line_2D.cpp
index d6c54f4e9bca64727157953b87831d69377b0045..c69ba2c1ddf6922773f473c520ed07c4f6d2df57 100644
--- a/src/feature/feature_line_2D.cpp
+++ b/src/feature/feature_line_2D.cpp
@@ -1,5 +1,5 @@
-#include "feature_corner_2D.h"
+#include "base/feature/feature_corner_2D.h"
namespace wolf {
diff --git a/src/feature/feature_odom_2D.cpp b/src/feature/feature_odom_2D.cpp
index 23c28cc7bac2b39a9bb798c7826cfc7295a7e8ae..4f37086f9f5b8a79178a24f0253ac512f5afd53b 100644
--- a/src/feature/feature_odom_2D.cpp
+++ b/src/feature/feature_odom_2D.cpp
@@ -1,4 +1,4 @@
-#include "feature_odom_2D.h"
+#include "base/feature/feature_odom_2D.h"
namespace wolf {
diff --git a/src/feature/feature_point_image.cpp b/src/feature/feature_point_image.cpp
index 5e240fcbe60e52fb5e25d64235acc229fd7e3f0a..e7d3781b6935b78cff79a0b25ee8998e3a7c6d44 100644
--- a/src/feature/feature_point_image.cpp
+++ b/src/feature/feature_point_image.cpp
@@ -1,5 +1,5 @@
-#include "feature_point_image.h"
+#include "base/feature/feature_point_image.h"
namespace wolf {
diff --git a/src/feature/feature_polyline_2D.cpp b/src/feature/feature_polyline_2D.cpp
index c2a19ff5a86c3fad960186f125571429c8044b05..0f14a701c40b55aacdc042cfd2634dc9392ea180 100644
--- a/src/feature/feature_polyline_2D.cpp
+++ b/src/feature/feature_polyline_2D.cpp
@@ -5,7 +5,7 @@
* \author: jvallve
*/
-#include "feature_polyline_2D.h"
+#include "base/feature/feature_polyline_2D.h"
namespace wolf
{
diff --git a/src/feature/feature_pose.cpp b/src/feature/feature_pose.cpp
index 35193d2b07b2207721c23da3aa776171e42a0d2b..15d40e60a99284e14275a37e191b10c12c99ab04 100644
--- a/src/feature/feature_pose.cpp
+++ b/src/feature/feature_pose.cpp
@@ -1,5 +1,4 @@
-#include "feature_pose.h"
-
+#include "base/feature/feature_pose.h"
namespace wolf {
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 4f0f619fe36767466f7db210e36ac4da03452892..5c1b7fee23ab834725340ac91f5df77c9db4b051 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -1,11 +1,11 @@
-#include "frame_base.h"
-#include "constraint_base.h"
-#include "trajectory_base.h"
-#include "capture_base.h"
-#include "state_block.h"
-#include "state_angle.h"
-#include "state_quaternion.h"
+#include "base/frame_base.h"
+#include "base/constraint/constraint_base.h"
+#include "base/trajectory_base.h"
+#include "base/capture/capture_base.h"
+#include "base/state_block.h"
+#include "base/state_angle.h"
+#include "base/state_quaternion.h"
namespace wolf {
@@ -226,8 +226,6 @@ 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;
@@ -386,7 +384,7 @@ FrameBasePtr FrameBase::create_POV_3D(const FrameType & _tp,
} // namespace wolf
-#include "factory.h"
+#include "base/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_base.cpp
index b2171c3cd5c5da6c5bbd1cbb660512f8d364d8bd..3349005129e275a5dd930cd0927d408149994680 100644
--- a/src/hardware_base.cpp
+++ b/src/hardware_base.cpp
@@ -1,6 +1,5 @@
-#include "hardware_base.h"
-#include "sensor_base.h"
-
+#include "base/hardware_base.h"
+#include "base/sensor/sensor_base.h"
namespace wolf {
diff --git a/src/landmark/landmark_AHP.cpp b/src/landmark/landmark_AHP.cpp
index 78b2bdb32264fd5e371bb9be25bd3342c1150fe4..f1a68397809ec79b0418798cc6a2fef626a843fe 100644
--- a/src/landmark/landmark_AHP.cpp
+++ b/src/landmark/landmark_AHP.cpp
@@ -1,7 +1,7 @@
-#include "landmark_AHP.h"
+#include "base/landmark/landmark_AHP.h"
-#include "state_homogeneous_3D.h"
-#include "factory.h"
+#include "base/state_homogeneous_3D.h"
+#include "base/factory.h"
#include "yaml/yaml_conversion.h"
namespace wolf {
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 830ae2940f201c291d9d12f6be529a591320f8ca..1ae594cb348c66819b3efffdbcf1acf0878e8fa2 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -1,9 +1,9 @@
-#include "landmark_base.h"
-#include "constraint_base.h"
-#include "map_base.h"
-#include "state_block.h"
-#include "yaml/yaml_conversion.h"
+#include "base/landmark/landmark_base.h"
+#include "base/constraint/constraint_base.h"
+#include "base/map_base.h"
+#include "base/state_block.h"
+#include "base/yaml/yaml_conversion.h"
namespace wolf {
@@ -120,7 +120,6 @@ void LandmarkBase::removeStateBlocks()
}
}
-
Eigen::VectorXs LandmarkBase::getState() const
{
SizeEigen size = 0;
@@ -153,7 +152,6 @@ void LandmarkBase::getState(Eigen::VectorXs& _state) const
}
}
-
YAML::Node LandmarkBase::saveToYaml() const
{
YAML::Node node;
diff --git a/src/landmark/landmark_container.cpp b/src/landmark/landmark_container.cpp
index a1ef7ed100e31b72619507bcb4d0663e1d50fe0c..6cef90a382ded760d9ad637803f7bff8205f58b2 100644
--- a/src/landmark/landmark_container.cpp
+++ b/src/landmark/landmark_container.cpp
@@ -1,6 +1,6 @@
-#include "landmark_container.h"
-#include "state_block.h"
+#include "base/landmark/landmark_container.h"
+#include "base/state_block.h"
namespace wolf {
diff --git a/src/landmark/landmark_corner_2D.cpp b/src/landmark/landmark_corner_2D.cpp
index 200b5a7ccbd2fd8db8870bf2a85c164f1207fa93..558292f90e54923dcacabe157d5818e40a1199e2 100644
--- a/src/landmark/landmark_corner_2D.cpp
+++ b/src/landmark/landmark_corner_2D.cpp
@@ -1,5 +1,5 @@
-#include "landmark_corner_2D.h"
+#include "base/landmark/landmark_corner_2D.h"
namespace wolf {
@@ -14,7 +14,6 @@ LandmarkCorner2D::~LandmarkCorner2D()
//
}
-
Scalar LandmarkCorner2D::getAperture() const
{
return descriptor_(0);
diff --git a/src/landmark/landmark_line_2D.cpp b/src/landmark/landmark_line_2D.cpp
index 80c916932c4a88a4781f764d6ae8bb9d0abf8ec7..aa2a14d9f8a59f6d45b4b58f37d1a6307ee5a7a7 100644
--- a/src/landmark/landmark_line_2D.cpp
+++ b/src/landmark/landmark_line_2D.cpp
@@ -1,5 +1,5 @@
-#include "landmark_line_2D.h"
+#include "base/landmark/landmark_line_2D.h"
namespace wolf {
@@ -43,8 +43,6 @@ void LandmarkLine2D::updateExtremePoints(Eigen::Vector3s & _q1, Eigen::Vector3s
//compute all necessary scalar products.
//s_p1q1 = p1p2.dot()
-
-
}
const Eigen::Vector3s & LandmarkLine2D::point1() const
diff --git a/src/landmark/landmark_point_3D.h b/src/landmark/landmark_point_3D.h
index cb29852b8b05a04220cbe4703bffa12bf8cdd5c9..7ee1c0507b3f3f63d0015bf509fb7eb629db309c 100644
--- a/src/landmark/landmark_point_3D.h
+++ b/src/landmark/landmark_point_3D.h
@@ -1,9 +1,8 @@
#ifndef LANDMARK_POINT_3D_H
#define LANDMARK_POINT_3D_H
-
//Wolf includes
-#include "landmark_base.h"
+#include "base/landmark/landmark_base.h"
// Vision Utils includes
#include <vision_utils/vision_utils.h>
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp
index f386803187e25ac323fe85ef6e352b0f0e7de956..3e1fcf45c828bd7295882983f5ead485f79047f4 100644
--- a/src/landmark/landmark_polyline_2D.cpp
+++ b/src/landmark/landmark_polyline_2D.cpp
@@ -5,19 +5,18 @@
* \author: jvallve
*/
-#include "feature_polyline_2D.h"
-#include "landmark_polyline_2D.h"
-#include "local_parametrization_polyline_extreme.h"
-#include "constraint_point_2D.h"
-#include "constraint_point_to_line_2D.h"
-#include "state_block.h"
-#include "factory.h"
-#include "yaml/yaml_conversion.h"
+#include "base/feature/feature_polyline_2D.h"
+#include "base/landmark/landmark_polyline_2D.h"
+#include "base/local_parametrization_polyline_extreme.h"
+#include "base/constraint/constraint_point_2D.h"
+#include "base/constraint/constraint_point_to_line_2D.h"
+#include "base/state_block.h"
+#include "base/factory.h"
+#include "base/yaml/yaml_conversion.h"
namespace wolf
{
-
LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXs& _points, const bool _first_extreme, const bool _last_extreme, unsigned int _first_id, LandmarkClassification _class) :
LandmarkBase("POLYLINE 2D", _p_ptr, _o_ptr), first_id_(_first_id), first_defined_(_first_extreme), last_defined_(_last_extreme), closed_(false), classification_(_class)
{
@@ -337,7 +336,6 @@ void LandmarkPolyline2D::removeStateBlocks()
LandmarkBase::removeStateBlocks();
}
-
// static
LandmarkBasePtr LandmarkPolyline2D::create(const YAML::Node& _lmk_node)
{
diff --git a/src/local_parametrization_base.cpp b/src/local_parametrization_base.cpp
index 7b8552f39ab23a7e8494dca6da69899ce1b8f2ae..e2bc1cf4e36aafd25c8209826d3c8afe43828d62 100644
--- a/src/local_parametrization_base.cpp
+++ b/src/local_parametrization_base.cpp
@@ -1,5 +1,4 @@
-#include "local_parametrization_base.h"
-
+#include "base/local_parametrization_base.h"
namespace wolf {
diff --git a/src/local_parametrization_homogeneous.cpp b/src/local_parametrization_homogeneous.cpp
index 482178155cd2226486311185b3ad1340dd59ae30..14abaecd1a9fd93cfd1da4b9a6e1e1d0f9982dbd 100644
--- a/src/local_parametrization_homogeneous.cpp
+++ b/src/local_parametrization_homogeneous.cpp
@@ -5,9 +5,9 @@
* Author: jsola
*/
-#include "local_parametrization_homogeneous.h"
+#include "base/local_parametrization_homogeneous.h"
#include "iostream"
-#include "rotations.h" // we use quaternion algebra here
+#include "base/rotations.h" // we use quaternion algebra here
namespace wolf {
diff --git a/src/local_parametrization_polyline_extreme.cpp b/src/local_parametrization_polyline_extreme.cpp
index 43f64d0690dda9ae1203dd9fa27dbfe18c7375bb..da318cafb9e723aa1fecfe9f19d6a3195df7fde8 100644
--- a/src/local_parametrization_polyline_extreme.cpp
+++ b/src/local_parametrization_polyline_extreme.cpp
@@ -1,6 +1,6 @@
-#include "local_parametrization_polyline_extreme.h"
-#include "state_block.h"
-#include "rotations.h"
+#include "base/local_parametrization_polyline_extreme.h"
+#include "base/state_block.h"
+#include "base/rotations.h"
namespace wolf {
diff --git a/src/local_parametrization_quaternion.cpp b/src/local_parametrization_quaternion.cpp
index 46e9ca9f8ba3b49155a7a913d1df4f0f515ce5a3..d2af8f544f21f8f83528c7db6371f0a8407f12d0 100644
--- a/src/local_parametrization_quaternion.cpp
+++ b/src/local_parametrization_quaternion.cpp
@@ -1,6 +1,6 @@
-#include "local_parametrization_quaternion.h"
-#include "rotations.h"
+#include "base/local_parametrization_quaternion.h"
+#include "base/rotations.h"
#include <iostream>
namespace wolf {
@@ -57,7 +57,6 @@ bool LocalParametrizationQuaternion<DQ_LOCAL>::minus(Eigen::Map<const Eigen::Vec
return true;
}
-
////////// GLOBAL PERTURBATION //////////////
template <>
@@ -110,6 +109,4 @@ bool LocalParametrizationQuaternion<DQ_GLOBAL>::minus(Eigen::Map<const Eigen::Ve
return true;
}
-
-
} // namespace wolf
diff --git a/src/map_base.cpp b/src/map_base.cpp
index ebcf2d0d8aa3b27c2d52d990c2de838a5efc11dd..5e8008a6a36783a463de9ecf5ac1497d5fa749c0 100644
--- a/src/map_base.cpp
+++ b/src/map_base.cpp
@@ -1,8 +1,8 @@
// wolf
-#include "map_base.h"
-#include "landmark_base.h"
-#include "factory.h"
+#include "base/map_base.h"
+#include "base/landmark/landmark_base.h"
+#include "base/factory.h"
// YAML
#include <yaml-cpp/yaml.h>
@@ -14,8 +14,6 @@
#include <iostream>
#include <sstream>
-
-
namespace wolf {
MapBase::MapBase() :
@@ -85,7 +83,6 @@ void MapBase::save(const std::string& _map_file_yaml, const std::string& _map_na
}
emitter << YAML::EndSeq << YAML::EndMap;
-
std::ofstream fout(_map_file_yaml);
fout << emitter.c_str();
fout.close();
diff --git a/src/motion_buffer.cpp b/src/motion_buffer.cpp
index dbdb539f678daba38cdf0739d9fb7a007ba8dae4..20d4fc26060fbd12087ae51b256ca85fe528de2d 100644
--- a/src/motion_buffer.cpp
+++ b/src/motion_buffer.cpp
@@ -1,4 +1,4 @@
-#include "motion_buffer.h"
+#include "base/motion_buffer.h"
namespace wolf
{
@@ -60,7 +60,6 @@ void Motion::resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_
jacobian_calib_.resize(_delta_cov_s, _calib_s);
}
-
////////////////////////////////////////////////////////////////////////////////////////
MotionBuffer::MotionBuffer(SizeEigen _data_size,
@@ -221,6 +220,5 @@ void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, b
}
}
-
}
diff --git a/src/node_base.cpp b/src/node_base.cpp
index f6f4ceb5322b10c6432c348927e911c598ff73fd..24cb56c200843b2c816cb243de2096b9ebcd7f98 100644
--- a/src/node_base.cpp
+++ b/src/node_base.cpp
@@ -1,4 +1,4 @@
-#include "node_base.h"
+#include "base/node_base.h"
namespace wolf {
diff --git a/src/problem.cpp b/src/problem.cpp
index fec23b17497b920b575f3f80c05a9db5882e9bc8..40ef35f2f1b2c7a044736b85cce3c5f98a4d492c 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -1,20 +1,20 @@
// wolf includes
-#include "problem.h"
-#include "hardware_base.h"
-#include "trajectory_base.h"
-#include "map_base.h"
-#include "sensor_base.h"
-#include "factory.h"
-#include "sensor_factory.h"
-#include "processor_factory.h"
-#include "constraint_base.h"
-#include "state_block.h"
-#include "processor_motion.h"
-#include "sensor_GPS.h"
-
-#include "processor_tracker.h"
+#include "base/problem.h"
+#include "base/hardware_base.h"
+#include "base/trajectory_base.h"
+#include "base/map_base.h"
+#include "base/sensor/sensor_base.h"
+#include "base/factory.h"
+#include "base/sensor/sensor_factory.h"
+#include "base/processor/processor_factory.h"
+#include "base/constraint/constraint_base.h"
+#include "base/state_block.h"
+#include "base/processor/processor_motion.h"
+#include "base/sensor/sensor_GPS.h"
+
+#include "base/processor/processor_tracker.h"
//#include "processors/processor_tracker_feature_trifocal.h"
-#include "capture_pose.h"
+#include "base/capture/capture_pose.h"
// IRI libs includes
@@ -30,7 +30,6 @@ namespace
std::string uppercase(std::string s) {for (auto & c: s) c = std::toupper(c); return s;}
}
-
Problem::Problem(const std::string& _frame_structure) :
hardware_ptr_(std::make_shared<HardwareBase>()),
trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
@@ -214,7 +213,6 @@ void Problem::clearProcessorMotion()
processor_motion_ptr_.reset();
}
-
FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
FrameType _frame_key_type, //
const Eigen::VectorXs& _frame_state, //
@@ -264,7 +262,6 @@ void Problem::getCurrentState(Eigen::VectorXs& state)
state = zeroState();
}
-
void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
{
assert(state.size() == getFrameStructureSize() && "Problem::getCurrentState: bad state size");
@@ -677,8 +674,6 @@ StateBlockList& Problem::getStateBlockList()
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_ )
@@ -1401,6 +1396,4 @@ void Problem::print(const std::string& depth, bool constr_by, bool metric, bool
print(0, constr_by, metric, state_blocks);
}
-
-
} // namespace wolf
diff --git a/src/processor/processor_GPS.cpp b/src/processor/processor_GPS.cpp
index ad30cff7ebd53124e8dedbf24428971639126c8f..ffb3f7d0eec5c25a58b303b8dc0a7b97f2c0d754 100644
--- a/src/processor/processor_GPS.cpp
+++ b/src/processor/processor_GPS.cpp
@@ -2,11 +2,11 @@
// Created by ptirindelli on 16/12/15.
//
-#include "constraint_GPS_pseudorange_2D.h"
-#include "feature_GPS_pseudorange.h"
-#include "processor_GPS.h"
+#include "base/constraint/constraint_GPS_pseudorange_2D.h"
+#include "base/feature/feature_GPS_pseudorange.h"
+#include "base/processor/processor_GPS.h"
-#include "capture_GPS.h"
+#include "base/capture/capture_GPS.h"
namespace wolf
{
@@ -68,7 +68,6 @@ ProcessorBasePtr ProcessorGPS::create(const std::string& _unique_name, const Pro
} // namespace wolf
-
// Register in the SensorFactory
#include "processor_factory.h"
namespace wolf {
diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp
index e69126258778b9c7584d2c551e280ac77464ad83..568b61a784bde288cd046d11c81b644c7efb52bd 100644
--- a/src/processor/processor_IMU.cpp
+++ b/src/processor/processor_IMU.cpp
@@ -1,7 +1,7 @@
// wolf
-#include "processor_IMU.h"
-#include "constraint_IMU.h"
-#include "IMU_tools.h"
+#include "base/processor/processor_IMU.h"
+#include "base/constraint/constraint_IMU.h"
+#include "base/IMU_tools.h"
namespace wolf {
@@ -329,9 +329,8 @@ void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
} // namespace wolf
-
// Register in the SensorFactory
-#include "processor_factory.h"
+#include "base/processor/processor_factory.h"
namespace wolf
{
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index ea4bdc4e82e1edbcb83c9500656f0b0d05de6ab8..aa259d706262904969e9e27245a50e06456584a0 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -1,7 +1,7 @@
-#include "processor_base.h"
-#include "processor_motion.h"
-#include "capture_base.h"
-#include "frame_base.h"
+#include "base/processor/processor_base.h"
+#include "base/processor/processor_motion.h"
+#include "base/capture/capture_base.h"
+#include "base/frame_base.h"
namespace wolf {
@@ -73,8 +73,6 @@ void ProcessorBase::remove()
}
}
-
-
/////////////////////////////////////////////////////////////////////////////////////////
void PackKeyFrameBuffer::removeUpTo(const TimeStamp& _time_stamp)
@@ -186,6 +184,4 @@ bool PackKeyFrameBuffer::checkTimeTolerance(const TimeStamp& _time_stamp1, const
return pass;
}
-
-
} // namespace wolf
diff --git a/src/processor/processor_capture_holder.cpp b/src/processor/processor_capture_holder.cpp
index 10ded0c0b2855abb81fc50b560c161cc18772fd5..58e3550ab2421abc6c35c07e4d5059fb94df2011 100644
--- a/src/processor/processor_capture_holder.cpp
+++ b/src/processor/processor_capture_holder.cpp
@@ -6,7 +6,7 @@
*/
//Wolf includes
-#include "processor_capture_holder.h"
+#include "base/processor/processor_capture_holder.h"
namespace wolf {
@@ -121,7 +121,6 @@ CaptureBasePtr ProcessorCaptureHolder::findCaptureContainingTimeStamp(const Time
// return capt;
-
return buffer_.getCapture(_ts);
}
@@ -146,7 +145,7 @@ ProcessorBasePtr ProcessorCaptureHolder::create(const std::string& _unique_name,
} // namespace wolf
// Register in the ProcessorFactory
-#include "processor_factory.h"
+#include "base/processor/processor_factory.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR("CAPTURE HOLDER", ProcessorCaptureHolder)
} // namespace wolf
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index dba867cc3fd603cf913de574035d2e42a46a545d..423e429dbc8190597db82847098558a4de68be58 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -1,13 +1,13 @@
-#include "processor_diff_drive.h"
+#include "base/processor/processor_diff_drive.h"
-#include "../sensors/sensor_diff_drive.h"
+#include "base/sensor/sensor_diff_drive.h"
-#include "../captures/capture_wheel_joint_position.h"
-#include "../captures/capture_velocity.h"
+#include "base/capture/capture_wheel_joint_position.h"
+#include "base/capture/capture_velocity.h"
-#include "../rotations.h"
-#include "../constraint_odom_2D.h"
-#include "../features/feature_diff_drive.h"
+#include "base/rotations.h"
+#include "base/constraint/constraint_odom_2D.h"
+#include "base/feature/feature_diff_drive.h"
namespace wolf
{
@@ -259,7 +259,7 @@ ProcessorBasePtr ProcessorDiffDrive::create(const std::string& _unique_name,
} // namespace wolf
// Register in the ProcessorFactory
-#include "processor_factory.h"
+#include "base/processor/processor_factory.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR("DIFF DRIVE", ProcessorDiffDrive)
} // namespace wolf
diff --git a/src/processor/processor_frame_nearest_neighbor_filter.cpp b/src/processor/processor_frame_nearest_neighbor_filter.cpp
index f705e368feebebd6991219a2472619f6cbb23b2a..4901170c2e99579a6fbe3618a261f4917bcd472a 100644
--- a/src/processor/processor_frame_nearest_neighbor_filter.cpp
+++ b/src/processor/processor_frame_nearest_neighbor_filter.cpp
@@ -1,4 +1,4 @@
-#include "processor_frame_nearest_neighbor_filter.h"
+#include "base/processor/processor_frame_nearest_neighbor_filter.h"
namespace wolf
{
diff --git a/src/processor/processor_loopclosure_base.cpp b/src/processor/processor_loopclosure_base.cpp
index 160185238b25ecff2d63f8103a968a0d1eeefad4..6953cd103d87b2fdad85df70114bdf1875ef847b 100644
--- a/src/processor/processor_loopclosure_base.cpp
+++ b/src/processor/processor_loopclosure_base.cpp
@@ -5,7 +5,7 @@
* \author: Tessa Johanna
*/
-#include "processor_loopclosure_base.h"
+#include "base/processor/processor_loopclosure_base.h"
namespace wolf
{
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 0abab214309a12779c0d916ac559ca4afbbd1339..0fe59c59aa026654b2ed5a3d8b924b3d5cfdee48 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -1,4 +1,4 @@
-#include "processor_motion.h"
+#include "base/processor/processor_motion.h"
namespace wolf
{
@@ -88,7 +88,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
// and give the part of the buffer before the new keyframe to the capture for the KF callback
existing_capture->getBuffer().split(ts_from_callback, capture_for_keyframe_callback->getBuffer());
-
// interpolate individual delta
if (!existing_capture->getBuffer().get().empty() && capture_for_keyframe_callback->getBuffer().get().back().ts_ != ts_from_callback)
{
@@ -100,7 +99,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
capture_for_keyframe_callback->getBuffer().get().push_back(motion_interpolated);
}
-
// create motion feature and add it to the capture
auto new_feature = emplaceFeature(capture_for_keyframe_callback);
@@ -131,7 +129,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
break;
}
-
case RUNNING_WITH_PACK_AFTER_ORIGIN :
{
// extract pack elements
@@ -185,8 +182,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
break;
}
-
-
default :
break;
}
@@ -256,7 +251,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
postProcess();
}
-
bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
{
CaptureMotionPtr capture_motion;
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index 19818dffcc1aee098c316f1b13bf603703333c39..c375d52faca184c57b3dad8976aac24c9decaca5 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -1,4 +1,4 @@
-#include "processor_odom_2D.h"
+#include "base/processor/processor_odom_2D.h"
namespace wolf
{
@@ -184,13 +184,10 @@ ProcessorBasePtr ProcessorOdom2D::create(const std::string& _unique_name, const
return prc_ptr;
}
-
-
}
-
// Register in the ProcessorFactory
-#include "processor_factory.h"
+#include "base/processor/processor_factory.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR("ODOM 2D", ProcessorOdom2D)
} // namespace wolf
diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp
index 49da6be8103ab17a3a50ed75dd87492db3dedb40..4f6d75911f97be52b232e06104a7302eb6c5b246 100644
--- a/src/processor/processor_odom_3D.cpp
+++ b/src/processor/processor_odom_3D.cpp
@@ -1,4 +1,4 @@
-#include "processor_odom_3D.h"
+#include "base/processor/processor_odom_3D.h"
namespace wolf
{
@@ -15,7 +15,6 @@ ProcessorOdom3D::ProcessorOdom3D(ProcessorParamsOdom3DPtr _params, SensorOdom3DP
jacobian_delta_.setZero(delta_cov_size_, delta_cov_size_);
}
-
ProcessorOdom3D::~ProcessorOdom3D()
{
}
@@ -200,7 +199,6 @@ Motion ProcessorOdom3D::interpolate(const Motion& _motion_ref, Motion& _motion_s
// assert(_motion_second.delta_integr_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
// assert(_motion_second.delta_integr_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
-
using namespace Eigen;
// Interpolate between motion_ref and motion, as in:
//
@@ -347,9 +345,8 @@ void ProcessorOdom3D::remap(const Eigen::VectorXs& _x1, const Eigen::VectorXs& _
}
-
// Register in the SensorFactory
-#include "processor_factory.h"
+#include "base/processor/processor_factory.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR("ODOM 3D", ProcessorOdom3D)
} // namespace wolf
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 630a856e7f9b1786b13b0d02fa5f3570d989e4b5..f8138e1fc7e1b0d152bf0576ee6fcb454e75ca0b 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -6,7 +6,7 @@
*/
// wolf
-#include "processor_tracker.h"
+#include "base/processor/processor_tracker.h"
// std
#include <cmath>
@@ -233,7 +233,6 @@ void ProcessorTracker::computeProcessingStep()
else
step = RUNNING;
-
// Then combine with the existence (or not) of a keyframe callback pack
switch (step)
{
@@ -277,8 +276,5 @@ void ProcessorTracker::computeProcessingStep()
}
}
-
-
-
} // namespace wolf
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index 0ce945f30c65152485f07a74a2fe7549937b0286..cfb6aa984f179b891cfc43f32524625e8d070a16 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -5,7 +5,7 @@
* \author: jsola
*/
-#include "processor_tracker_feature.h"
+#include "base/processor/processor_tracker_feature.h"
namespace wolf
{
diff --git a/src/processor/processor_tracker_feature_corner.cpp b/src/processor/processor_tracker_feature_corner.cpp
index 387333fd7151c0d59fc89cf3124ead333f098a52..0a8e7c7c232fd465f07c3704bbf63f7d615a20cc 100644
--- a/src/processor/processor_tracker_feature_corner.cpp
+++ b/src/processor/processor_tracker_feature_corner.cpp
@@ -5,8 +5,8 @@
* \author: jvallve
*/
-#include "processor_tracker_feature_corner.h"
-#include "feature_corner_2D.h"
+#include "base/processor/processor_tracker_feature_corner.h"
+#include "base/feature/feature_corner_2D.h"
namespace wolf
{
diff --git a/src/processor/processor_tracker_feature_dummy.cpp b/src/processor/processor_tracker_feature_dummy.cpp
index dc81126f93f78ef01930150ae69fadf802c173ab..6ce3d4fe18fbabc83d63582a9ac2090c63ce53be 100644
--- a/src/processor/processor_tracker_feature_dummy.cpp
+++ b/src/processor/processor_tracker_feature_dummy.cpp
@@ -5,8 +5,8 @@
* \author: jvallve
*/
-#include "processor_tracker_feature_dummy.h"
-#include "feature_base.h"
+#include "base/processor/processor_tracker_feature_dummy.h"
+#include "base/feature/feature_base.h"
namespace wolf
{
diff --git a/src/processor/processor_tracker_feature_image.cpp b/src/processor/processor_tracker_feature_image.cpp
index 1a860c2170a547a03b8851b38ad9514698e77080..6ec2b8aab6306ebc0e596fa04e61cc9d7143b8ca 100644
--- a/src/processor/processor_tracker_feature_image.cpp
+++ b/src/processor/processor_tracker_feature_image.cpp
@@ -1,5 +1,5 @@
// Wolf includes
-#include "processor_tracker_feature_image.h"
+#include "base/processor/processor_tracker_feature_image.h"
// Vision utils
#include <detectors.h>
@@ -382,7 +382,6 @@ void ProcessorTrackerFeatureImage::drawFeatures(cv::Mat _image)
}
}
-
ProcessorBasePtr ProcessorTrackerFeatureImage::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sensor_ptr)
{
ProcessorTrackerFeatureImagePtr prc_ptr = std::make_shared<ProcessorTrackerFeatureImage>(std::static_pointer_cast<ProcessorParamsTrackerFeatureImage>(_params));
@@ -390,10 +389,8 @@ ProcessorBasePtr ProcessorTrackerFeatureImage::create(const std::string& _unique
return prc_ptr;
}
-
} // namespace wolf
-
// Register in the SensorFactory
#include "processor_factory.h"
namespace wolf {
diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp
index 7ffb993755e37f242c9ab8df551fdb29f417d2d2..5557e10d6a3b8010b7b53434dba1c8a766ad8b9f 100644
--- a/src/processor/processor_tracker_feature_trifocal.cpp
+++ b/src/processor/processor_tracker_feature_trifocal.cpp
@@ -1,11 +1,11 @@
// wolf
-#include "processor_tracker_feature_trifocal.h"
+#include "processor/processor_tracker_feature_trifocal.h"
-#include "sensor_camera.h"
-#include "feature_point_image.h"
+#include "base/sensor/sensor_camera.h"
+#include "base/feature/feature_point_image.h"
#include "constraints/constraint_autodiff_trifocal.h"
-#include "capture_image.h"
+#include "base/capture/capture_image.h"
// vision_utils
#include <vision_utils.h>
@@ -18,7 +18,6 @@
namespace wolf {
-
//// DEBUG =====================================
//debug_tTmp = clock();
//WOLF_TRACE("======== DetectNewFeatures =========");
@@ -29,7 +28,6 @@ namespace wolf {
//WOLF_TRACE("--> TIME: Detect New Features: detect ",debug_comp_time_);
//// ===========================================
-
// Constructor
ProcessorTrackerFeatureTrifocal::ProcessorTrackerFeatureTrifocal(ProcessorParamsTrackerFeatureTrifocalPtr _params_tracker_feature_trifocal) :
ProcessorTrackerFeature("TRACKER FEATURE TRIFOCAL", _params_tracker_feature_trifocal ),
diff --git a/src/processor/processor_tracker_feature_trifocal.h b/src/processor/processor_tracker_feature_trifocal.h
index 9065c718cffa4d0ebca5166391ca30730442e560..dca3b791a6885b5e89296b6f988838a511c212fa 100644
--- a/src/processor/processor_tracker_feature_trifocal.h
+++ b/src/processor/processor_tracker_feature_trifocal.h
@@ -2,8 +2,8 @@
#define _PROCESSOR_TRACKER_FEATURE_TRIFOCAL_H_
//Wolf includes
-#include "../processor_tracker_feature.h"
-#include "../capture_image.h"
+#include "base/processor/processor_tracker_feature.h"
+#include "base/capture/capture_image.h"
// Vision utils
#include <vision_utils.h>
@@ -104,7 +104,6 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
*/
virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) override;
-
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief advance pointers
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index 8e3f5d0d91f55d82a04e8fe8e93965906185daff..08bbf8c52b66b01a49fa49389fce11da4affe0b1 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -5,12 +5,11 @@
* \author: jvallve
*/
-#include "processor_tracker_landmark.h"
-#include "map_base.h"
+#include "base/processor/processor_tracker_landmark.h"
+#include "base/map_base.h"
#include <utility>
-
namespace wolf
{
diff --git a/src/processor/processor_tracker_landmark_corner.cpp b/src/processor/processor_tracker_landmark_corner.cpp
index e687e5fcdb63d9e865b2b816a2cc69f05e33c48d..522034204bb5dd4f4b3140d3f55db773faa070af 100644
--- a/src/processor/processor_tracker_landmark_corner.cpp
+++ b/src/processor/processor_tracker_landmark_corner.cpp
@@ -1,5 +1,5 @@
-#include "processor_tracker_landmark_corner.h"
-#include "rotations.h"
+#include "base/processor/processor_tracker_landmark_corner.h"
+#include "base/rotations.h"
namespace wolf
{
@@ -422,7 +422,7 @@ ProcessorTrackerLandmarkCorner::ProcessorTrackerLandmarkCorner(ProcessorParamsLa
} //namespace wolf
// Register in the SensorFactory
-#include "processor_factory.h"
+#include "base/processor/processor_factory.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR("TRACKER LANDMARK CORNER", ProcessorTrackerLandmarkCorner)
} // namespace wolf
diff --git a/src/processor/processor_tracker_landmark_dummy.cpp b/src/processor/processor_tracker_landmark_dummy.cpp
index e806c23e5948de12922d86d6db58ff2b1be61b02..06c10b37f1f8b9607b773c2c808e101c39dfb412 100644
--- a/src/processor/processor_tracker_landmark_dummy.cpp
+++ b/src/processor/processor_tracker_landmark_dummy.cpp
@@ -5,9 +5,9 @@
* \author: jvallve
*/
-#include "processor_tracker_landmark_dummy.h"
-#include "landmark_corner_2D.h"
-#include "constraint_corner_2D.h"
+#include "base/processor/processor_tracker_landmark_dummy.h"
+#include "base/landmark/landmark_corner_2D.h"
+#include "base/constraint/constraint_corner_2D.h"
namespace wolf
{
diff --git a/src/processor/processor_tracker_landmark_image.cpp b/src/processor/processor_tracker_landmark_image.cpp
index da3f1c7b499deb2e1ecfd73a03aafa1a83586bb1..04bba415fcb6331473d8c33211c58f9e7cff4429 100644
--- a/src/processor/processor_tracker_landmark_image.cpp
+++ b/src/processor/processor_tracker_landmark_image.cpp
@@ -1,17 +1,17 @@
-#include "processor_tracker_landmark_image.h"
-
-#include "capture_image.h"
-#include "constraint_AHP.h"
-#include "feature_base.h"
-#include "feature_point_image.h"
-#include "frame_base.h"
-#include "logging.h"
-#include "map_base.h"
-#include "pinhole_tools.h"
-#include "problem.h"
-#include "sensor_camera.h"
-#include "state_block.h"
-#include "time_stamp.h"
+#include "base/processor/processor_tracker_landmark_image.h"
+
+#include "base/capture/capture_image.h"
+#include "base/constraint/constraint_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/sensor/sensor_camera.h"
+#include "base/state_block.h"
+#include "base/time_stamp.h"
// vision_utils
#include <detectors.h>
@@ -196,7 +196,6 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList
_features_incoming_out.push_back(incoming_point_ptr);
-
_feature_landmark_correspondences[incoming_point_ptr] = std::make_shared<LandmarkMatch>(landmark_in_ptr, normalized_score);
feat_lmk_found_.push_back(incoming_point_ptr);
@@ -302,7 +301,6 @@ LandmarkBasePtr ProcessorTrackerLandmarkImage::createLandmark(FeatureBasePtr _fe
point3D.normalize();
-
vec_homogeneous = {point3D(0),point3D(1),point3D(2),1/distance};
LandmarkAHPPtr lmk_ahp_ptr = std::make_shared<LandmarkAHP>(vec_homogeneous, anchor_frame, getSensorPtr(), feat_point_image_ptr->getDescriptor());
@@ -330,7 +328,6 @@ ConstraintBasePtr ProcessorTrackerLandmarkImage::createConstraint(FeatureBasePtr
}
}
-
// ==================================================================== My own functions
void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorXs& _current_state,
@@ -361,7 +358,6 @@ void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorX
* We use Eigen::Transform which is like using homogeneous transform matrices with a simpler API
*/
-
// Assert frame is 3D with at least PQ
assert((_current_state.size() == 7 || _current_state.size() == 16) && "Wrong state size! Should be 7 for 3D pose or 16 for IMU.");
@@ -493,7 +489,6 @@ void ProcessorTrackerLandmarkImage::drawLandmarks(cv::Mat _image)
}
}
-
//namespace wolf{
ProcessorBasePtr ProcessorTrackerLandmarkImage::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr)
@@ -505,7 +500,6 @@ ProcessorBasePtr ProcessorTrackerLandmarkImage::create(const std::string& _uniqu
} // namespace wolf
-
// Register in the SensorFactory
#include "processor_factory.h"
namespace wolf {
diff --git a/src/processor/processor_tracker_landmark_polyline.cpp b/src/processor/processor_tracker_landmark_polyline.cpp
index 0475ab31adbb4cb0d5600e52f327b68c926f4c61..70efbcde6d4ecee513151c81ebadb3b7b144dc03 100644
--- a/src/processor/processor_tracker_landmark_polyline.cpp
+++ b/src/processor/processor_tracker_landmark_polyline.cpp
@@ -1,4 +1,4 @@
-#include "processor_tracker_landmark_polyline.h"
+#include "base/processor/processor_tracker_landmark_polyline.h"
namespace wolf
{
@@ -268,7 +268,6 @@ unsigned int ProcessorTrackerLandmarkPolyline::findLandmarks(const LandmarkBaseL
//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)
@@ -444,7 +443,6 @@ Scalar ProcessorTrackerLandmarkPolyline::sqDistPointToLine(const Eigen::Vector3s
* 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 )
*
*/
@@ -510,7 +508,6 @@ LandmarkBasePtr ProcessorTrackerLandmarkPolyline::createLandmark(FeatureBasePtr
//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>() +
@@ -1017,7 +1014,7 @@ ProcessorBasePtr ProcessorTrackerLandmarkPolyline::create(const std::string& _un
} //namespace wolf
// Register in the SensorFactory
-#include "processor_factory.h"
+#include "base/processor/processor_factory.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR("POLYLINE", ProcessorTrackerLandmarkPolyline)
} // namespace wolf
diff --git a/src/sensor/sensor_GPS.cpp b/src/sensor/sensor_GPS.cpp
index faeb7d112aef66fac024d23a7659c6a07c36c6a0..7745e65b1124834e266916846109e3cf96ad621a 100644
--- a/src/sensor/sensor_GPS.cpp
+++ b/src/sensor/sensor_GPS.cpp
@@ -1,7 +1,7 @@
-#include "sensor_GPS.h"
-#include "state_block.h"
-#include "state_quaternion.h"
+#include "base/sensor/sensor_GPS.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
namespace wolf {
@@ -34,7 +34,6 @@ StateBlockPtr SensorGPS::getMapOPtr() const
return getStateBlockPtrStatic(4);
}
-
// Define the factory method
SensorBasePtr SensorGPS::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_p,
const IntrinsicsBasePtr _intrinsics)
@@ -50,9 +49,8 @@ SensorBasePtr SensorGPS::create(const std::string& _unique_name, const Eigen::Ve
} // namespace wolf
-
// Register in the SensorFactory
-#include "sensor_factory.h"
+#include "base/sensor/sensor_factory.h"
namespace wolf {
WOLF_REGISTER_SENSOR("GPS",SensorGPS)
} // namespace wolf
diff --git a/src/sensor/sensor_GPS_fix.cpp b/src/sensor/sensor_GPS_fix.cpp
index fa1f46eb16bbeef20789cfdaf7165433910e21d9..788dc06dc58023d6502d01a2d01029b0bf96af5f 100644
--- a/src/sensor/sensor_GPS_fix.cpp
+++ b/src/sensor/sensor_GPS_fix.cpp
@@ -1,6 +1,6 @@
-#include "sensor_GPS_fix.h"
-#include "state_block.h"
-#include "state_quaternion.h"
+#include "base/sensor/sensor_GPS_fix.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
namespace wolf {
@@ -22,7 +22,6 @@ SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFix
//
}
-
SensorGPSFix::~SensorGPSFix()
{
//
@@ -42,9 +41,8 @@ SensorBasePtr SensorGPSFix::create(const std::string& _unique_name, const Eigen:
} // namespace wolf
-
// Register in the SensorFactory
-#include "sensor_factory.h"
+#include "base/sensor/sensor_factory.h"
namespace wolf {
WOLF_REGISTER_SENSOR("GPS FIX", SensorGPSFix)
} // namespace wolf
diff --git a/src/sensor/sensor_IMU.cpp b/src/sensor/sensor_IMU.cpp
index 9e5da5f0ddba954357e7c58492a82947423ff5af..94a30bb4347f0f42c137393b7e9df4b08cda1ae0 100644
--- a/src/sensor/sensor_IMU.cpp
+++ b/src/sensor/sensor_IMU.cpp
@@ -1,6 +1,6 @@
-#include "sensor_IMU.h"
-#include "state_block.h"
-#include "state_quaternion.h"
+#include "base/sensor/sensor_IMU.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
namespace wolf {
@@ -40,7 +40,6 @@ SensorIMU::SensorIMU(const Eigen::VectorXs& _extrinsics, const IntrinsicsIMU& _p
assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 2D.");
}
-
SensorIMU::~SensorIMU()
{
//
@@ -61,9 +60,8 @@ SensorBasePtr SensorIMU::create(const std::string& _unique_name, const Eigen::Ve
} // namespace wolf
-
// Register in the SensorFactory
-#include "sensor_factory.h"
+#include "base/sensor/sensor_factory.h"
namespace wolf {
WOLF_REGISTER_SENSOR("IMU", SensorIMU)
} // namespace wolf
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 429aaf9f142d08a3374d8242df8e79379e5c6d90..a05fec1ceb4b1b90912dd4ec380212848c68f98b 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -1,6 +1,5 @@
-#include "sensor_base.h"
-#include "state_block.h"
-
+#include "base/sensor/sensor_base.h"
+#include "base/state_block.h"
namespace wolf {
@@ -103,8 +102,6 @@ void SensorBase::removeStateBlocks()
}
}
-
-
void SensorBase::fix()
{
for( auto sbp : state_block_vec_)
@@ -165,8 +162,6 @@ void SensorBase::unfixIntrinsics()
updateCalibSize();
}
-
-
void SensorBase::registerNewStateBlocks()
{
if (getProblem() != nullptr)
@@ -283,7 +278,6 @@ SizeEigen SensorBase::computeCalibSize() const
return sz;
}
-
Eigen::VectorXs SensorBase::getCalibration() const
{
SizeEigen index = 0;
@@ -301,7 +295,6 @@ Eigen::VectorXs SensorBase::getCalibration() const
return calib;
}
-
bool SensorBase::process(const CaptureBasePtr capture_ptr)
{
capture_ptr->setSensorPtr(shared_from_this());
diff --git a/src/sensor/sensor_camera.cpp b/src/sensor/sensor_camera.cpp
index 7a9a9a2c7904b0ea47143c96a4d0bdd1b36c8c50..3a3dae9bd4a3064517c150cbe808a4a24b43061f 100644
--- a/src/sensor/sensor_camera.cpp
+++ b/src/sensor/sensor_camera.cpp
@@ -1,8 +1,8 @@
-#include "sensor_camera.h"
+#include "base/sensor/sensor_camera.h"
-#include "pinhole_tools.h"
-#include "state_block.h"
-#include "state_quaternion.h"
+#include "base/pinhole_tools.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
namespace wolf
{
@@ -51,7 +51,6 @@ Eigen::Matrix3s SensorCamera::setIntrinsicMatrix(Eigen::Vector4s _pinhole_model)
return K;
}
-
// Define the factory method
SensorBasePtr SensorCamera::create(const std::string& _unique_name, //
const Eigen::VectorXs& _extrinsics_pq, //
@@ -66,12 +65,11 @@ SensorBasePtr SensorCamera::create(const std::string& _unique_name, //
return sen_ptr;
}
-
} // namespace wolf
// Register in the SensorFactory
-#include "sensor_factory.h"
-//#include "factory.h"
+#include "base/sensor/sensor_factory.h"
+//#include "base/factory.h"
namespace wolf
{
WOLF_REGISTER_SENSOR("CAMERA", SensorCamera)
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index 4c57144c96375f6c077d90ea20fde5ad027e488d..d710d9e77a0794674b873366233e8841a68f7b76 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -1,7 +1,7 @@
-#include "sensor_diff_drive.h"
-#include "../state_block.h"
-#include "../capture_motion.h"
-#include "../eigen_assert.h"
+#include "base/sensor/sensor_diff_drive.h"
+#include "base/state_block.h"
+#include "base/capture/capture_motion.h"
+#include "base/eigen_assert.h"
namespace wolf {
@@ -46,7 +46,6 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name,
return odo;
}
-
/// @todo Further work to enforce wheel model
//const IntrinsicsDiffDrive& SensorDiffDrive::getIntrinsics()
@@ -127,9 +126,8 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name,
} // namespace wolf
-
// Register in the SensorFactory
-#include "sensor_factory.h"
+#include "base/sensor/sensor_factory.h"
namespace wolf {
WOLF_REGISTER_SENSOR("DIFF DRIVE", SensorDiffDrive)
} // namespace wolf
diff --git a/src/sensor/sensor_laser_2D.cpp b/src/sensor/sensor_laser_2D.cpp
index 227734568759f63d635c978e30db9cbd2068ff32..a7a5677af497ccb2f925dce9ae993e160b860fd5 100644
--- a/src/sensor/sensor_laser_2D.cpp
+++ b/src/sensor/sensor_laser_2D.cpp
@@ -1,5 +1,5 @@
-#include "sensor_laser_2D.h"
-#include "state_block.h"
+#include "base/sensor/sensor_laser_2D.h"
+#include "base/state_block.h"
namespace wolf {
@@ -79,14 +79,10 @@ SensorBasePtr SensorLaser2D::create(const std::string& _unique_name, const Eigen
return sen;
}
-
} // namespace wolf
-
-
-
// Register in the SensorFactory and the ParameterFactory
-#include "sensor_factory.h"
+#include "base/sensor/sensor_factory.h"
//#include "intrinsics_factory.h"
namespace wolf {
WOLF_REGISTER_SENSOR("LASER 2D", SensorLaser2D)
diff --git a/src/sensor/sensor_odom_2D.cpp b/src/sensor/sensor_odom_2D.cpp
index fdb7ab4804809072cada12774e8af9a08ed9c3f4..7f0abaed4648650da90136d189c34b9d5a7c66f4 100644
--- a/src/sensor/sensor_odom_2D.cpp
+++ b/src/sensor/sensor_odom_2D.cpp
@@ -1,6 +1,6 @@
-#include "sensor_odom_2D.h"
-#include "state_block.h"
-#include "state_angle.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/state_block.h"
+#include "base/state_angle.h"
namespace wolf {
@@ -27,7 +27,6 @@ SensorOdom2D::SensorOdom2D(Eigen::VectorXs _extrinsics, IntrinsicsOdom2DPtr _int
//
}
-
SensorOdom2D::~SensorOdom2D()
{
//
@@ -69,9 +68,8 @@ SensorBasePtr SensorOdom2D::create(const std::string& _unique_name, const Eigen:
} // namespace wolf
-
// Register in the SensorFactory
-#include "sensor_factory.h"
+#include "base/sensor/sensor_factory.h"
namespace wolf {
WOLF_REGISTER_SENSOR("ODOM 2D", SensorOdom2D)
} // namespace wolf
diff --git a/src/sensor/sensor_odom_3D.cpp b/src/sensor/sensor_odom_3D.cpp
index c5f3fb388c1ab12635dd8bd0d64bbc506fab3207..aa6efc42b82aac106dbb39c509bcb382d77cd2b8 100644
--- a/src/sensor/sensor_odom_3D.cpp
+++ b/src/sensor/sensor_odom_3D.cpp
@@ -5,10 +5,10 @@
* \author: jsola
*/
-#include "sensor_odom_3D.h"
+#include "base/sensor/sensor_odom_3D.h"
-#include "state_block.h"
-#include "state_quaternion.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
namespace wolf {
@@ -44,7 +44,6 @@ SensorOdom3D::SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, IntrinsicsOdom
//
}
-
SensorOdom3D::~SensorOdom3D()
{
//
@@ -69,9 +68,8 @@ SensorBasePtr SensorOdom3D::create(const std::string& _unique_name, const Eigen:
} // namespace wolf
-
// Register in the SensorFactory
-#include "sensor_factory.h"
+#include "base/sensor/sensor_factory.h"
namespace wolf {
WOLF_REGISTER_SENSOR("ODOM 3D", SensorOdom3D)
}
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index b6a2e6a40e160a7604063de97ed811933d39e2dd..f8541646827009f9209e2e4dc2ef4feadbccdead 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -1,7 +1,7 @@
-#include "solver_manager.h"
-#include "../trajectory_base.h"
-#include "../map_base.h"
-#include "../landmark_base.h"
+#include "base/solver/solver_manager.h"
+#include "base/trajectory_base.h"
+#include "base/map_base.h"
+#include "base/landmark/landmark_base.h"
namespace wolf {
diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp
index cb39335b0705f8edf6222d78db36a0e18c21d504..a25c72beffa514c822c5275ff61ed8bf294ec4dc 100644
--- a/src/solver_suitesparse/solver_manager.cpp
+++ b/src/solver_suitesparse/solver_manager.cpp
@@ -1,4 +1,4 @@
-#include "ceres_manager.h"
+#include "base/ceres_wrapper/ceres_manager.h"
SolverManager::SolverManager()
{
diff --git a/src/state_block.cpp b/src/state_block.cpp
index 2599ea21c66351c6a74d40cf4b05bd9f2161d3c4..46411e7c30aefb15f921bb9f9fa9c7a114a6154b 100644
--- a/src/state_block.cpp
+++ b/src/state_block.cpp
@@ -1,4 +1,4 @@
-#include "state_block.h"
+#include "base/state_block.h"
namespace wolf
{
diff --git a/src/time_stamp.cpp b/src/time_stamp.cpp
index 543ceb2cad3007420c84d62f8e4eb3df9d1497cd..55d25fc6ed7fc8d67d609b2d1f7d16d1455aef1b 100644
--- a/src/time_stamp.cpp
+++ b/src/time_stamp.cpp
@@ -1,5 +1,5 @@
-#include "time_stamp.h"
+#include "base/time_stamp.h"
namespace wolf {
@@ -10,8 +10,6 @@ std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts)
return os;
}
-
-
TimeStamp::TimeStamp() :
//time_stamp_(0)
time_stamp_nano_(0)
diff --git a/src/track_matrix.cpp b/src/track_matrix.cpp
index e51b38614f5e2e50f6256d90642468a0faecf34b..8bb0e0abe4213f51f3a6ff4de7a5da0a4cd7602f 100644
--- a/src/track_matrix.cpp
+++ b/src/track_matrix.cpp
@@ -5,7 +5,7 @@
* \author: jsola
*/
-#include "track_matrix.h"
+#include "base/track_matrix.h"
namespace wolf
{
diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp
index 3dcb41267e8dea55255dd6f2a4d1ca523997dfc1..557d072fd1fe1cbba36f69ed58c87c67b9185da5 100644
--- a/src/trajectory_base.cpp
+++ b/src/trajectory_base.cpp
@@ -1,6 +1,5 @@
-#include "trajectory_base.h"
-#include "frame_base.h"
-
+#include "base/trajectory_base.h"
+#include "base/frame_base.h"
namespace wolf {
@@ -39,7 +38,6 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
return _frame_ptr;
}
-
void TrajectoryBase::getConstraintList(ConstraintBaseList & _ctr_list)
{
for(auto fr_ptr : getFrameList())
diff --git a/src/yaml/processor_IMU_yaml.cpp b/src/yaml/processor_IMU_yaml.cpp
index 0e6587e29472cf8854d6fa3df030edc3b817c19e..38b83b82f3908983df2f0ad27867bbfe818064a2 100644
--- a/src/yaml/processor_IMU_yaml.cpp
+++ b/src/yaml/processor_IMU_yaml.cpp
@@ -6,11 +6,11 @@
*/
// wolf yaml
-#include "processor_IMU.h"
-#include "yaml_conversion.h"
+#include "base/processor/processor_IMU.h"
+#include "base/yaml/yaml_conversion.h"
// wolf
-#include "../factory.h"
+#include "base/factory.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
diff --git a/src/yaml/processor_image_yaml.cpp b/src/yaml/processor_image_yaml.cpp
index 7de4b1587de6fd5dbe5aa4b86e00ac46d68dbbb2..051174ef5b23b76fa950279e8f1e6f74e879991d 100644
--- a/src/yaml/processor_image_yaml.cpp
+++ b/src/yaml/processor_image_yaml.cpp
@@ -6,15 +6,15 @@
*/
// wolf yaml
-#include "yaml_conversion.h"
+#include "base/yaml/yaml_conversion.h"
// wolf
-#include "../factory.h"
+#include "base/factory.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
-#include "../processor_params_image.h"
+#include "base/processor/processor_params_image.h"
namespace wolf
{
@@ -64,7 +64,5 @@ static ProcessorParamsBasePtr createProcessorParamsImage(const std::string & _fi
const bool WOLF_UNUSED registered_prc_image_feature_par = ProcessorParamsFactory::get().registerCreator("IMAGE FEATURE", createProcessorParamsImage);
const bool WOLF_UNUSED registered_prc_image_landmark_par = ProcessorParamsFactory::get().registerCreator("IMAGE LANDMARK", createProcessorParamsImage);
-
-}
}
diff --git a/src/yaml/processor_odom_3D_yaml.cpp b/src/yaml/processor_odom_3D_yaml.cpp
index 1b5a3f40db3589a8e51201273a93872cc827d8ce..af8934b07d182dab640a70b7f53511ce75abb447 100644
--- a/src/yaml/processor_odom_3D_yaml.cpp
+++ b/src/yaml/processor_odom_3D_yaml.cpp
@@ -6,11 +6,11 @@
*/
// wolf yaml
-#include "yaml_conversion.h"
+#include "base/yaml/yaml_conversion.h"
// wolf
-#include "../processor_odom_3D.h"
-#include "../factory.h"
+#include "base/processor/processor_odom_3D.h"
+#include "base/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 9e339d41aee83258fc9ecc2aaa0e9c76efdb8311..2a6ae97f7ecf2b81207d278d366a42a6124fa494 100644
--- a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
+++ b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
@@ -5,13 +5,12 @@
* Author: asantamaria
*/
-
// wolf yaml
-#include "../processors/processor_tracker_feature_trifocal.h"
-#include "yaml_conversion.h"
+#include "processor/processor_tracker_feature_trifocal.h"
+#include "base/yaml/yaml_conversion.h"
// wolf
-#include "../factory.h"
+#include "base/factory.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
@@ -58,7 +57,6 @@ static ProcessorParamsBasePtr createProcessorParamsTrackerFeatureTrifocal(const
params->max_euclidean_distance = algorithm["max euclidean distance"] .as<Scalar>();
params->min_track_length_for_constraint = algorithm["min track length for constraint"].as<int>();
-
YAML::Node noise = config["noise"];
params->pixel_noise_std = noise ["pixel noise std"].as<Scalar>();
diff --git a/src/yaml/sensor_IMU_yaml.cpp b/src/yaml/sensor_IMU_yaml.cpp
index 658becb6bf8dd50722454d2c83947357a3e57a2b..c64914c6054aca6cd27513085289903dabdf051c 100644
--- a/src/yaml/sensor_IMU_yaml.cpp
+++ b/src/yaml/sensor_IMU_yaml.cpp
@@ -6,11 +6,11 @@
*/
// wolf yaml
-#include "sensor_IMU.h"
-#include "yaml_conversion.h"
+#include "base/sensor/sensor_IMU.h"
+#include "base/yaml/yaml_conversion.h"
// wolf
-#include "../factory.h"
+#include "base/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 852adc977ac61c465613ed08a6dfd267f2cceedd..d1a76b78b6a58c46f68480d8efd1a5b0fe4ce663 100644
--- a/src/yaml/sensor_camera_yaml.cpp
+++ b/src/yaml/sensor_camera_yaml.cpp
@@ -6,11 +6,11 @@
*/
// wolf yaml
-#include "yaml_conversion.h"
+#include "base/yaml/yaml_conversion.h"
// wolf
-#include "../sensor_camera.h"
-#include "../factory.h"
+#include "base/sensor/sensor_camera.h"
+#include "base/factory.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
@@ -59,7 +59,6 @@ static IntrinsicsBasePtr createIntrinsicsCamera(const std::string & _filename_do
intrinsics_cam->width = width;
intrinsics_cam->height = height;
-
//=========================================
// ===== this part for debugging only =====
//=========================================
@@ -80,9 +79,6 @@ static IntrinsicsBasePtr createIntrinsicsCamera(const std::string & _filename_do
// std::cout << "\tintrinsic : " << intrinsic.transpose() << std::endl;
// std::cout << "\tdistoriton : " << distortion.transpose() << std::endl;
//=========================================
- //=========================================
-
-
return intrinsics_cam;
}
diff --git a/src/yaml/sensor_laser_2D_yaml.cpp b/src/yaml/sensor_laser_2D_yaml.cpp
index 160386e6ea9f3b005ab1ea073ffe6bcf269c3d4c..6a6b3f4ae06efdec4b8b48750a4e8f3bde22328c 100644
--- a/src/yaml/sensor_laser_2D_yaml.cpp
+++ b/src/yaml/sensor_laser_2D_yaml.cpp
@@ -5,14 +5,13 @@
* \author: jsola
*/
-
// wolf yaml
-#include "yaml_conversion.h"
+#include "base/yaml/yaml_conversion.h"
// wolf
-//#include "../intrinsics_factory.h"
-#include "../factory.h"
-#include "../sensor_laser_2D.h"
+//#include "base/intrinsics_factory.h"
+#include "base/factory.h"
+#include "base/sensor/sensor_laser_2D.h"
// yaml library
#include <yaml-cpp/yaml.h>
@@ -29,7 +28,6 @@ IntrinsicsBasePtr createIntrinsicsLaser2D(const std::string& _filename_dot_yaml)
return params;
}
-
// register into factory
const bool WOLF_UNUSED registered_laser_params = IntrinsicsFactory::get().registerCreator("LASER 2D", createIntrinsicsLaser2D);
diff --git a/src/yaml/sensor_odom_3D_yaml.cpp b/src/yaml/sensor_odom_3D_yaml.cpp
index a6012aeef037ba22493f562969614ff885341aa4..f423169a9a013c2c2251f8bc10c7597abe13a079 100644
--- a/src/yaml/sensor_odom_3D_yaml.cpp
+++ b/src/yaml/sensor_odom_3D_yaml.cpp
@@ -6,11 +6,11 @@
*/
// wolf yaml
-#include "yaml_conversion.h"
+#include "base/yaml/yaml_conversion.h"
// wolf
-#include "../sensor_odom_3D.h"
-#include "../factory.h"
+#include "base/sensor/sensor_odom_3D.h"
+#include "base/factory.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
diff --git a/test/gtest_IMU.cpp b/test/gtest_IMU.cpp
index c423f80a77d1f6c445097a0e02f658e997bb7f63..e9261f75b76930b75cdac6b0f02eb0d11a052413 100644
--- a/test/gtest_IMU.cpp
+++ b/test/gtest_IMU.cpp
@@ -6,15 +6,15 @@
*/
//Wolf
-#include "processor_IMU.h"
-#include "sensor_IMU.h"
-#include "wolf.h"
-#include "sensor_odom_3D.h"
-#include "processor_odom_3D.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/processor/processor_IMU.h"
+#include "base/sensor/sensor_IMU.h"
+#include "base/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 "logging.h"
+#include "base/logging.h"
// make my life easier
using namespace Eigen;
@@ -24,7 +24,6 @@ using std::make_shared;
using std::static_pointer_cast;
using std::string;
-
class Process_Constraint_IMU : public testing::Test
{
public:
@@ -83,7 +82,6 @@ class Process_Constraint_IMU : public testing::Test
bool p0_fixed, q0_fixed, v0_fixed;
bool p1_fixed, q1_fixed, v1_fixed;
-
virtual void SetUp( )
{
string wolf_root = _WOLF_ROOT_DIR;
@@ -114,7 +112,6 @@ class Process_Constraint_IMU : public testing::Test
}
-
/* Integrate one step of acc and angVel motion, obtain Delta_preintegrated
* Input:
@@ -153,7 +150,6 @@ class Process_Constraint_IMU : public testing::Test
Delta = Delta_plus;
}
-
/* Integrate acc and angVel motion, obtain Delta_preintegrated
* Input:
* N: number of steps
@@ -258,8 +254,6 @@ class Process_Constraint_IMU : public testing::Test
return trajectory;
}
-
-
MotionBuffer integrateWithProcessor(int N, const TimeStamp& t0, const Quaternions q0, const MatrixXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, VectorXs& D_preint, VectorXs& D_corrected)
{
Vector6s motion_now;
@@ -288,8 +282,6 @@ class Process_Constraint_IMU : public testing::Test
return processor_imu->getBuffer();
}
-
-
// Initial configuration of variables
bool configureAll()
{
@@ -326,8 +318,6 @@ class Process_Constraint_IMU : public testing::Test
return true;
}
-
-
// Integrate using all methods
virtual void integrateAll()
{
@@ -338,7 +328,6 @@ class Process_Constraint_IMU : public testing::Test
D_exact = integrateDelta(q0, motion, bias_null, bias_null, dt, J_D_bias);
x1_exact = imu::composeOverState(x0, D_exact, DT );
-
// ===================================== INTEGRATE USING IMU_TOOLS
// pre-integrate
if (motion.cols() == 1)
@@ -363,7 +352,6 @@ class Process_Constraint_IMU : public testing::Test
x1_corrected = imu::composeOverState(x0, D_corrected , DT );
}
-
// Integrate Trajectories all methods
virtual void integrateAllTrajectories()
{
@@ -371,7 +359,6 @@ class Process_Constraint_IMU : public testing::Test
Trj_D_exact.resize(10,cols); Trj_D_preint_imu.resize(10,cols); Trj_D_preint_prc.resize(10,cols); Trj_D_corrected_imu.resize(10,cols); Trj_D_corrected_prc.resize(10,cols);
Trj_x_exact.resize(10,cols); Trj_x_preint_imu.resize(10,cols); Trj_x_preint_prc.resize(10,cols); Trj_x_corrected_imu.resize(10,cols); Trj_x_corrected_prc.resize(10,cols);
-
// ===================================== INTEGRATE EXACTLY WITH IMU_TOOLS with no bias at all
MotionBuffer Buf_exact = integrateDeltaTrajectory(q0, motion, bias_null, bias_null, dt, J_D_bias);
@@ -425,7 +412,6 @@ class Process_Constraint_IMU : public testing::Test
MotionBuffer Buf_D_preint_prc = integrateWithProcessor(num_integrations, t0, q0, motion, bias_real, bias_preint, dt, D_preint, D_corrected);
-
// Build trajectories preintegrated and corrected with processorIMU
Dt = 0;
col = 0;
@@ -451,8 +437,6 @@ class Process_Constraint_IMU : public testing::Test
x1_corrected = imu::composeOverState(x0, D_corrected , DT );
}
-
-
// Set state_blocks status
void setFixedBlocks()
{
@@ -465,8 +449,6 @@ class Process_Constraint_IMU : public testing::Test
KF_1->getVPtr()->setFixed(v1_fixed);
}
-
-
void setKfStates()
{
// This perturbs states to estimate around the exact value, then assigns to the keyframe
@@ -495,8 +477,6 @@ class Process_Constraint_IMU : public testing::Test
KF_1->setState(x_pert);
}
-
-
virtual void buildProblem()
{
// ===================================== SET KF in Wolf tree
@@ -519,8 +499,6 @@ class Process_Constraint_IMU : public testing::Test
setKfStates();
}
-
-
string solveProblem(SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::BRIEF)
{
string report = ceres_manager->solve(verbose);
@@ -542,8 +520,6 @@ class Process_Constraint_IMU : public testing::Test
return report;
}
-
-
string runAll(SolverManager::ReportVerbosity verbose)
{
configureAll();
@@ -553,8 +529,6 @@ class Process_Constraint_IMU : public testing::Test
return report;
}
-
-
void printAll(std::string report = "")
{
WOLF_TRACE(report);
@@ -585,8 +559,6 @@ class Process_Constraint_IMU : public testing::Test
WOLF_TRACE("X0_optim : ", x0_optim .transpose() ); // optimal state after solving using processor
}
-
-
virtual void assertAll()
{
// check delta and state integrals
@@ -729,7 +701,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimat
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
// ================================================================================================================ //
-
// ===================================== RUN ALL
string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -774,7 +745,6 @@ TEST_F(Process_Constraint_IMU, test_capture) // F_ixed___e_stimated
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
// ================================================================================================================ //
-
// ===================================== RUN ALL
Eigen::Vector6s initial_bias((Eigen::Vector6s()<< .002, .0007, -.001, .003, -.002, .001).finished());
sensor_imu->getIntrinsicPtr()->setState(initial_bias);
@@ -786,7 +756,6 @@ TEST_F(Process_Constraint_IMU, test_capture) // F_ixed___e_stimated
ASSERT_MATRIX_APPROX(KF_0->getCaptureOf(sensor_imu)->getCalibration(), KF_1->getCaptureOf(sensor_imu)->getCalibration(), 1e-8 );
}
-
TEST_F(Process_Constraint_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimated
{
@@ -822,7 +791,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimat
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
// ================================================================================================================ //
-
// ===================================== RUN ALL
string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -832,7 +800,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimat
}
-
TEST_F(Process_Constraint_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimated
{
@@ -868,7 +835,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimat
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
// ================================================================================================================ //
-
// ===================================== RUN ALL
string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -878,7 +844,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimat
}
-
TEST_F(Process_Constraint_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
{
@@ -914,7 +879,6 @@ TEST_F(Process_Constraint_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
// ================================================================================================================ //
-
// ===================================== RUN ALL
string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -924,7 +888,6 @@ TEST_F(Process_Constraint_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
}
-
TEST_F(Process_Constraint_IMU, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
{
@@ -960,7 +923,6 @@ TEST_F(Process_Constraint_IMU, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
// ================================================================================================================ //
-
// ===================================== RUN ALL
string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1005,7 +967,6 @@ TEST_F(Process_Constraint_IMU, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
// ================================================================================================================ //
-
// ===================================== RUN ALL
string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1050,7 +1011,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ix
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
// ================================================================================================================ //
-
// ===================================== RUN ALL
string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1095,7 +1055,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixe
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
// ================================================================================================================ //
-
// ===================================== RUN ALL
string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1140,7 +1099,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixe
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
// ================================================================================================================ //
-
// ===================================== RUN ALL
string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1185,7 +1143,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixe
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
// ================================================================================================================ //
-
// ===================================== RUN ALL
string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1230,7 +1187,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixe
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
// ================================================================================================================ //
-
// ===================================== RUN ALL
string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1275,7 +1231,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixe
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
// ================================================================================================================ //
-
// ===================================== RUN ALL
string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1320,7 +1275,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_st
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
// ================================================================================================================ //
-
// ===================================== RUN ALL
string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1365,7 +1319,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_st
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
// ================================================================================================================ //
-
// ===================================== RUN ALL
string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1410,7 +1363,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stim
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
// ================================================================================================================ //
-
// ===================================== RUN ALL
string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1455,7 +1407,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stim
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
// ================================================================================================================ //
-
// ===================================== RUN ALL
string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1500,7 +1451,6 @@ TEST_F(Process_Constraint_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b)
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
// ================================================================================================================ //
-
// ===================================== RUN ALL
configureAll();
integrateAllTrajectories();
@@ -1561,8 +1511,6 @@ int main(int argc, char **argv)
return RUN_ALL_TESTS();
}
-
-
/* Some notes :
*
* - Process_Constraint_IMU_ODO.MotionConstant_PQv_b__PQv_b :
diff --git a/test/gtest_IMU_tools.cpp b/test/gtest_IMU_tools.cpp
index 9f0f8c398e1f7b51690228f5ff3f75be4e66b01d..e41007154f27c8c6de6845db934215df22d7c382 100644
--- a/test/gtest_IMU_tools.cpp
+++ b/test/gtest_IMU_tools.cpp
@@ -5,10 +5,9 @@
* Author: jsola
*/
-#include "IMU_tools.h"
+#include "base/IMU_tools.h"
#include "utils_gtest.h"
-
using namespace Eigen;
using namespace wolf;
using namespace imu;
@@ -376,7 +375,6 @@ TEST(motion2data, AllRandom)
Vector6s bias;
Quaternions q;
-
motion .setRandom();
bias .setRandom();
q.coeffs() .setRandom().normalize();
@@ -390,7 +388,6 @@ TEST(motion2data, AllRandom)
ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
}
-
/* Integrate acc and angVel motion, obtain Delta_preintegrated
* Input:
* N: number of steps
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index 00fdf888326cb1da7be91afca719d2ff3fd765d1..d1ec1884cc79fb858f4b97f8078ba8281c508753 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -5,11 +5,10 @@
* Author: jsola
*/
-
#include "utils_gtest.h"
-#include "capture_base.h"
-#include "state_angle.h"
+#include "base/capture/capture_base.h"
+#include "base/state_angle.h"
using namespace wolf;
using namespace Eigen;
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 0df86c75e583f51f49a4dd918c79337be92a5e64..097969799576f40f960c04e8a736532eb9989ec1 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -6,18 +6,18 @@
*/
#include "utils_gtest.h"
-#include "../src/logging.h"
-
-#include "../problem.h"
-#include "../sensor_base.h"
-#include "../state_block.h"
-#include "../capture_void.h"
-#include "../constraint_pose_2D.h"
-#include "../constraint_quaternion_absolute.h"
-#include "../solver/solver_manager.h"
-#include "../ceres_wrapper/ceres_manager.h"
-#include "../local_parametrization_angle.h"
-#include "../local_parametrization_quaternion.h"
+#include "base/logging.h"
+
+#include "base/problem.h"
+#include "base/sensor/sensor_base.h"
+#include "base/state_block.h"
+#include "base/capture/capture_void.h"
+#include "base/constraint/constraint_pose_2D.h"
+#include "base/constraint/constraint_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 "ceres/ceres.h"
@@ -629,7 +629,6 @@ TEST(CeresManager, ConstraintsUpdateLocalParam)
ceres_manager_ptr->check();
}
-
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_constraint_IMU.cpp b/test/gtest_constraint_IMU.cpp
index bc212aaaa24eb34bb28df4fba4c724f404c3ef62..7ef93e9a37102913f4d7543ce7deb48a6b086669 100644
--- a/test/gtest_constraint_IMU.cpp
+++ b/test/gtest_constraint_IMU.cpp
@@ -6,15 +6,15 @@
*/
//Wolf
-#include "capture_IMU.h"
-#include "constraint_pose_3D.h"
-#include "processor_IMU.h"
-#include "sensor_IMU.h"
-#include "processor_odom_3D.h"
+#include "base/capture/capture_IMU.h"
+#include "base/constraint/constraint_pose_3D.h"
+#include "base/processor/processor_IMU.h"
+#include "base/sensor/sensor_IMU.h"
+#include "base/processor/processor_odom_3D.h"
#include "ceres_wrapper/ceres_manager.h"
#include "utils_gtest.h"
-#include "logging.h"
+#include "base/logging.h"
#include <iostream>
#include <fstream>
@@ -90,7 +90,6 @@ class ConstraintIMU_biasTest_Static_NullBias : public testing::Test
//===================================================== END{INITIALIZATION}
-
//===================================================== PROCESS DATA
// PROCESS DATA
@@ -436,7 +435,6 @@ class ConstraintIMU_biasTest_Move_NullBias : public testing::Test
//===================================================== END{INITIALIZATION}
-
//===================================================== PROCESS DATA
// PROCESS DATA
@@ -1011,7 +1009,6 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
// WOLF_TRACE("KF2 delta cor: ", last_CM ->getDeltaCorrected(origin_CB->getCalibration()).transpose());
// WOLF_TRACE("KF2 jacob : ", last_CM ->getJacobianCalib().row(0));
-
// ==================================================== show problem status
problem->print(4,1,1,1);
@@ -1335,8 +1332,6 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
virtual void TearDown(){}
};
-
-
// tests with following conditions :
// var(b1,b2), invar(p1,q1,v1,p2,q2,v2), factor : imu(p,q,v)
@@ -1642,7 +1637,6 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_E
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
//==============================================================
//WOLF_INFO("Starting error bias 1e-2")
epsilon_bias = 0.01;
@@ -1723,7 +1717,6 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
//==============================================================
//WOLF_INFO("Starting error bias 1e-2")
epsilon_bias = 0.01;
@@ -2843,6 +2836,5 @@ int main(int argc, char **argv)
// ::testing::GTEST_FLAG(filter) = "ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot.VarB1B2_InvarP1Q1V1P2Q2V2_initOK";
// ::testing::GTEST_FLAG(filter) = "ConstraintIMU_biasTest_Move_NonNullBiasRot.VarB1B2V1P2V2_InvarP1Q1Q2_initOK";
-
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_constraint_absolute.cpp b/test/gtest_constraint_absolute.cpp
index a0587e29ce96207c1e7bdaa05fb4f7e4a9b5cbc4..53252fc5fdf43c5c58b29105245038d38be0d748 100644
--- a/test/gtest_constraint_absolute.cpp
+++ b/test/gtest_constraint_absolute.cpp
@@ -5,14 +5,12 @@
* \author: datchuth
*/
-
#include "utils_gtest.h"
-#include "constraint_block_absolute.h"
-#include "constraint_quaternion_absolute.h"
-#include "capture_motion.h"
-
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/constraint/constraint_block_absolute.h"
+#include "base/constraint/constraint_quaternion_absolute.h"
+#include "base/capture/capture_motion.h"
+#include "base/ceres_wrapper/ceres_manager.h"
using namespace Eigen;
using namespace wolf;
diff --git a/test/gtest_constraint_autodiff.cpp b/test/gtest_constraint_autodiff.cpp
index c5820537f54d0aaf8e251cc4867a88b82f1d0e33..5138bb89960eba132e32d9153a53c02bc603e700 100644
--- a/test/gtest_constraint_autodiff.cpp
+++ b/test/gtest_constraint_autodiff.cpp
@@ -5,15 +5,14 @@
* Author: jvallve
*/
-
#include "utils_gtest.h"
-#include "sensor_odom_2D.h"
-#include "capture_void.h"
-#include "feature_odom_2D.h"
-#include "constraint_odom_2D.h"
-#include "constraint_odom_2D_analytic.h"
-#include "constraint_autodiff.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/capture/capture_void.h"
+#include "base/feature/feature_odom_2D.h"
+#include "base/constraint/constraint_odom_2D.h"
+#include "base/constraint/constraint_odom_2D_analytic.h"
+#include "base/constraint/constraint_autodiff.h"
using namespace wolf;
using namespace Eigen;
diff --git a/test/gtest_constraint_autodiff_distance_3D.cpp b/test/gtest_constraint_autodiff_distance_3D.cpp
index bc6e402e423c2f6531a94591a1add61733872607..ae3313afe1f6d926221330e428282a6a06fa902e 100644
--- a/test/gtest_constraint_autodiff_distance_3D.cpp
+++ b/test/gtest_constraint_autodiff_distance_3D.cpp
@@ -5,11 +5,11 @@
* \author: jsola
*/
-#include "constraints/constraint_autodiff_distance_3D.h"
-#include "problem.h"
-#include "logging.h"
-#include "ceres_wrapper/ceres_manager.h"
-#include "rotations.h"
+#include "base/constraint/constraint_autodiff_distance_3D.h"
+#include "base/problem.h"
+#include "base/logging.h"
+#include "base/ceres_wrapper/ceres_manager.h"
+#include "base/rotations.h"
#include "utils_gtest.h"
diff --git a/test/gtest_constraint_autodiff_trifocal.cpp b/test/gtest_constraint_autodiff_trifocal.cpp
index aa34b7ea76b6efb83d3cbf69b75057af5fc66e38..1615cc4abfb93f9fedc22885279a03abe636dff9 100644
--- a/test/gtest_constraint_autodiff_trifocal.cpp
+++ b/test/gtest_constraint_autodiff_trifocal.cpp
@@ -1,10 +1,10 @@
#include "utils_gtest.h"
-#include "logging.h"
+#include "base/logging.h"
#include "ceres_wrapper/ceres_manager.h"
#include "processors/processor_tracker_feature_trifocal.h"
-#include "capture_image.h"
+#include "base/capture/capture_image.h"
#include "constraints/constraint_autodiff_trifocal.h"
using namespace Eigen;
@@ -278,7 +278,6 @@ TEST_F(ConstraintAutodiffTrifocalTest, error_jacobians)
ASSERT_MATRIX_APPROX(J_r_m1, Jn_r_m1, 1e-6);
-
// jacs wrt m2
pix0 = c123->getPixelCanonicalOrigin();
for (int i=0; i<3; i++)
@@ -295,7 +294,6 @@ TEST_F(ConstraintAutodiffTrifocalTest, error_jacobians)
ASSERT_MATRIX_APPROX(J_r_m2, Jn_r_m2, 1e-6);
-
// jacs wrt m3
pix0 = c123->getPixelCanonicalLast();
for (int i=0; i<3; i++)
@@ -671,7 +669,6 @@ class ConstraintAutodiffTrifocalMultiPointTest : public ConstraintAutodiffTrifoc
*
*/
-
public:
std::vector<FeatureBasePtr> fv1, fv2, fv3;
std::vector<ConstraintAutodiffTrifocalPtr> cv123;
@@ -735,7 +732,6 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point)
*
*/
-
S ->getPPtr()->fix(); // do not calibrate sensor pos
S ->getOPtr()->fix(); // do not calibrate sensor ori
F1->getPPtr()->fix(); // Cam 1 acts as reference
@@ -801,7 +797,6 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point_scale)
*
*/
-
S ->getPPtr()->fix(); // do not calibrate sensor pos
S ->getOPtr()->fix(); // do not calibrate sensor ori
F1->getPPtr()->fix(); // Cam 1 acts as reference
@@ -868,7 +863,6 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point_distance)
*
*/
-
S ->getPPtr()->fix(); // do not calibrate sensor pos
S ->getOPtr()->fix(); // do not calibrate sensor ori
F1->getPPtr()->fix(); // Cam 1 acts as reference
diff --git a/test/gtest_constraint_odom_3D.cpp b/test/gtest_constraint_odom_3D.cpp
index 68408277551644f6e04abd3a1bb3894ef0bb57af..d71050ec6534cd1e0ed1c01a94f35fd927ba6e93 100644
--- a/test/gtest_constraint_odom_3D.cpp
+++ b/test/gtest_constraint_odom_3D.cpp
@@ -5,14 +5,12 @@
* \author: jsola
*/
-
#include "utils_gtest.h"
-#include "constraint_odom_3D.h"
-#include "capture_motion.h"
-
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/constraint/constraint_odom_3D.h"
+#include "base/capture/capture_motion.h"
+#include "base/ceres_wrapper/ceres_manager.h"
using namespace Eigen;
using namespace wolf;
diff --git a/test/gtest_constraint_pose_2D.cpp b/test/gtest_constraint_pose_2D.cpp
index dd95c82d83a69a9e979e1dc3f5f5f26f9d6ea0cc..125893e67c05a659327f7ac7d81391219845d50b 100644
--- a/test/gtest_constraint_pose_2D.cpp
+++ b/test/gtest_constraint_pose_2D.cpp
@@ -5,14 +5,12 @@
* \author: jsola
*/
-
-#include "../constraint_pose_2D.h"
+#include "base/constraint/constraint_pose_2D.h"
#include "utils_gtest.h"
-#include "capture_motion.h"
-
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/capture/capture_motion.h"
+#include "base/ceres_wrapper/ceres_manager.h"
using namespace Eigen;
using namespace wolf;
@@ -65,7 +63,6 @@ TEST(ConstraintPose2D, solve)
}
-
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_constraint_pose_3D.cpp b/test/gtest_constraint_pose_3D.cpp
index 48fbedebd3b2a9a1f2932d8b18ef0979552d3ce3..05de4bb61427285fa846fd25a7b7e4d379bca30e 100644
--- a/test/gtest_constraint_pose_3D.cpp
+++ b/test/gtest_constraint_pose_3D.cpp
@@ -5,14 +5,12 @@
* \author: jsola
*/
-
-#include "../constraint_pose_3D.h"
+#include "base/constraint/constraint_pose_3D.h"
#include "utils_gtest.h"
-#include "capture_motion.h"
-
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/capture/capture_motion.h"
+#include "base/ceres_wrapper/ceres_manager.h"
using namespace Eigen;
using namespace wolf;
@@ -24,7 +22,6 @@ Vector7s pose6toPose7(Vector6s _pose6)
return (Vector7s() << _pose6.head<3>() , v2q(_pose6.tail<3>()).coeffs()).finished();
}
-
// Input pose6 and covariance
Vector6s pose(Vector6s::Random());
Vector7s pose7 = pose6toPose7(pose);
@@ -46,7 +43,6 @@ CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 3D"
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 3D", pose7, data_cov));
ConstraintPose3DPtr ctr0 = std::static_pointer_cast<ConstraintPose3D>(fea0->addConstraint(std::make_shared<ConstraintPose3D>(fea0)));
-
////////////////////////////////////////////////////////
TEST(ConstraintPose3D, check_tree)
@@ -73,7 +69,6 @@ TEST(ConstraintPose3D, solve)
}
-
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_constraint_sparse.cpp b/test/gtest_constraint_sparse.cpp
index 875c68c8efab907902abeb247ced9db189f725f7..900f39d5b57c773a83acfadb6285a3cf88fecb05 100644
--- a/test/gtest_constraint_sparse.cpp
+++ b/test/gtest_constraint_sparse.cpp
@@ -5,7 +5,6 @@
* \author: jsola
*/
-
#include "utils_gtest.h"
#include "constraint_sparse.h"
diff --git a/test/gtest_eigen_predicates.cpp b/test/gtest_eigen_predicates.cpp
index 23fa8e234ed83dd0c32b9cb2d902b2587c08723d..649ba85e9b5477d38498b3f80678eaf25e87697b 100644
--- a/test/gtest_eigen_predicates.cpp
+++ b/test/gtest_eigen_predicates.cpp
@@ -1,6 +1,6 @@
#include "utils_gtest.h"
-#include "../eigen_predicates.h"
+#include "base/eigen_predicates.h"
TEST(TestEigenPredicates, TestEigenDynPredZero)
{
diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp
index 970556a3efee1b5166fc593f397caba84d039d68..5ce136d6625931f585621b1e664430defc70969c 100644
--- a/test/gtest_feature_IMU.cpp
+++ b/test/gtest_feature_IMU.cpp
@@ -1,13 +1,13 @@
//Wolf
-#include "capture_IMU.h"
-#include "processor_IMU.h"
-#include "sensor_IMU.h"
-#include "wolf.h"
-#include "problem.h"
-#include "state_block.h"
-#include "state_quaternion.h"
+#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 "utils_gtest.h"
-#include "../src/logging.h"
+#include "base/logging.h"
class FeatureIMU_test : public testing::Test
{
diff --git a/test/gtest_feature_base.cpp b/test/gtest_feature_base.cpp
index bc867f48f9eb80de6e09eed68ba77b13834833e2..f9da19cff79a01ca755472f3e816fc4edeac5312 100644
--- a/test/gtest_feature_base.cpp
+++ b/test/gtest_feature_base.cpp
@@ -5,11 +5,10 @@
* Author: jsola
*/
-#include "feature_base.h"
+#include "base/feature/feature_base.h"
#include "utils_gtest.h"
-
using namespace wolf;
using namespace Eigen;
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index ae6b78068133529dcf9045572bba5f9715b9a62c..e5a4d4f3c837e855929bd3f24fff98692227194e 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -5,20 +5,17 @@
* Author: jsola
*/
-
-
#include "utils_gtest.h"
-#include "../logging.h"
+#include "base/logging.h"
-#include "../frame_base.h"
-#include "../sensor_odom_2D.h"
-#include "../processor_odom_2D.h"
-#include "../constraint_odom_2D.h"
-#include "../capture_motion.h"
+#include "base/frame_base.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/processor/processor_odom_2D.h"
+#include "base/constraint/constraint_odom_2D.h"
+#include "base/capture/capture_motion.h"
#include <iostream>
-
using namespace Eigen;
using namespace std;
using namespace wolf;
@@ -61,7 +58,6 @@ TEST(FrameBase, LinksBasic)
ASSERT_EQ(F->getHits() , (unsigned int) 0);
}
-
TEST(FrameBase, LinksToTree)
{
// Problem with 2 frames and one motion constraint between them
@@ -120,8 +116,6 @@ TEST(FrameBase, LinksToTree)
F1->getOPtr()->fix();
ASSERT_TRUE(F1->isFixed());
-
-
// set key
F1->setKey();
ASSERT_TRUE(F1->isKey());
@@ -131,7 +125,7 @@ TEST(FrameBase, LinksToTree)
ASSERT_TRUE(F1->getCaptureList().empty());
}
-#include "state_quaternion.h"
+#include "base/state_quaternion.h"
TEST(FrameBase, GetSetState)
{
// Create PQV_3D state blocks
@@ -166,13 +160,9 @@ TEST(FrameBase, GetSetState)
ASSERT_TRUE((x2 - x).isMuchSmallerThan(1, Constants::EPS_SMALL));
}
-
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
-
-
-
diff --git a/test/gtest_local_param.cpp b/test/gtest_local_param.cpp
index bdd413a8223216c5c394683b2d9a82844900ccee..42e01489ed722497bdaad1d818ae572e1d2c0100 100644
--- a/test/gtest_local_param.cpp
+++ b/test/gtest_local_param.cpp
@@ -6,14 +6,13 @@
*/
#include "utils_gtest.h"
-#include "../src/logging.h"
+#include "base/logging.h"
+#include "base/local_parametrization_quaternion.h"
+#include "base/local_parametrization_homogeneous.h"
+#include "base/rotations.h"
-#include "../src/local_parametrization_quaternion.h"
-#include "../src/local_parametrization_homogeneous.h"
-#include "../src/rotations.h"
-
-#include "../src/wolf.h"
+#include "base/wolf.h"
#include <iostream>
@@ -32,7 +31,6 @@
} \
}
-
using namespace Eigen;
using namespace std;
using namespace wolf;
@@ -126,7 +124,6 @@ TEST(TestLocalParametrization, QuaternionGlobal)
ASSERT_MATRIX_APPROX(da_m, da2_m, 1e-10);
-
}
TEST(TestLocalParametrization, Homogeneous)
diff --git a/test/gtest_make_posdef.cpp b/test/gtest_make_posdef.cpp
index d2b82d1ea5f3d817106968ced142cf821efb05c7..1dfcce9353f5726f5e69593f99e8a7dc3782cffd 100644
--- a/test/gtest_make_posdef.cpp
+++ b/test/gtest_make_posdef.cpp
@@ -1,5 +1,5 @@
#include "utils_gtest.h"
-#include "wolf.h"
+#include "base/wolf.h"
using namespace Eigen;
using namespace wolf;
diff --git a/test/gtest_motion_buffer.cpp b/test/gtest_motion_buffer.cpp
index 3bed9443308c578e0975041339be1eed43e93091..6d0301692fc5bea5b444a6aa582e8170d489294c 100644
--- a/test/gtest_motion_buffer.cpp
+++ b/test/gtest_motion_buffer.cpp
@@ -5,13 +5,12 @@
* Author: jsola
*/
-
#include "utils_gtest.h"
-#include "../logging.h"
+#include "base/logging.h"
-#include "../motion_buffer.h"
+#include "base/motion_buffer.h"
-#include "wolf.h"
+#include "base/wolf.h"
#include <iostream>
@@ -184,4 +183,3 @@ int main(int argc, char **argv)
return RUN_ALL_TESTS();
}
-
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index d8e10b949a137e718d1ca51502e94671d8379e09..fcf60798e88d353bf18371d2a5c3f455b98f597e 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -8,15 +8,14 @@
#include "utils_gtest.h"
// Classes under test
-#include "../processor_odom_2D.h"
-#include "../constraint_odom_2D.h"
+#include "base/processor/processor_odom_2D.h"
+#include "base/constraint/constraint_odom_2D.h"
// Wolf includes
-#include "../sensor_odom_2D.h"
-#include "../state_block.h"
-#include "../wolf.h"
-#include "../ceres_wrapper/ceres_manager.h"
-#include "../capture_pose.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/state_block.h"
+#include "base/wolf.h"
+#include "base/ceres_wrapper/ceres_manager.h"
// STL includes
#include <map>
@@ -27,11 +26,11 @@
// General includes
#include <iostream>
#include <iomanip> // std::setprecision
+#include "base/capture/capture_pose.h"
using namespace wolf;
using namespace Eigen;
-
VectorXs plus(const VectorXs& _pose, const Vector2s& _data)
{
VectorXs _pose_plus_data(3);
@@ -98,7 +97,6 @@ void show(const ProblemPtr& problem)
}
}
-
TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D)
{
using std::cout;
@@ -131,7 +129,6 @@ TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D)
// KF1 and motion from KF0
t += dt;
- t += dt;
FrameBasePtr F1 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t);
CaptureBasePtr C1 = F1->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t));
FeatureBasePtr f1 = C1->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov));
@@ -140,7 +137,6 @@ TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D)
// KF2 and motion from KF1
t += dt;
- t += dt;
FrameBasePtr F2 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t);
CaptureBasePtr C2 = F2->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t));
FeatureBasePtr f2 = C2->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov));
@@ -471,8 +467,6 @@ TEST(Odom2D, KF_callback)
}
}
-
-
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_odom_3D.cpp b/test/gtest_odom_3D.cpp
index bc36ac8457f36b9747ff16e46e89bdc5e8e6f8cc..864f19c1443cd64064c9fa474961c454ea617e9c 100644
--- a/test/gtest_odom_3D.cpp
+++ b/test/gtest_odom_3D.cpp
@@ -5,19 +5,15 @@
* \author: jsola
*/
-
-
-
#include "utils_gtest.h"
-#include "wolf.h"
-#include "logging.h"
+#include "base/wolf.h"
+#include "base/logging.h"
-#include "processor_odom_3D.h"
+#include "base/processor/processor_odom_3D.h"
#include <iostream>
-
#define JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, dx) \
{ VectorXs Do(7); \
prc_ptr->deltaPlusDelta(D, d, dt, Do); \
@@ -155,7 +151,6 @@ TEST(ProcessorOdom3D, deltaPlusDelta_Jac)
}
-
TEST(ProcessorOdom3D, Interpolate0) // basic test
{
/* Conditions:
@@ -177,7 +172,6 @@ TEST(ProcessorOdom3D, Interpolate0) // basic test
WOLF_DEBUG("ref delta= ", ref.delta_.transpose());
WOLF_DEBUG("ref Delta= ", ref.delta_integr_.transpose());
-
// set final
final.ts_ = 5;
final.delta_ << 1,0,0, 0,0,0,1;
@@ -271,7 +265,6 @@ TEST(ProcessorOdom3D, Interpolate1) // delta algebra test
// Motion structures
Motion R(0.0,6,7,6,0), I(0.0,6,7,6,0), S(0.0,6,7,6,0), F(0.0,6,7,6,0);
-
/////////// start experiment ///////////////
// set origin and ref states
@@ -315,7 +308,6 @@ TEST(ProcessorOdom3D, Interpolate1) // delta algebra test
Dp_of = q_o.conjugate() * (p_f - p_o);
Dq_of = q_o.conjugate() * q_f;
-
// set ref
R.ts_ = t_r;
R.delta_ = dx_or; // origin to ref
@@ -405,7 +397,6 @@ TEST(ProcessorOdom3D, Interpolate2) // timestamp out of bounds test
// Motion structures
Motion R(0.0,6,7,6,0), I(0.0,6,7,6,0), S(0.0,6,7,6,0), F(0.0,6,7,6,0);
-
/////////// start experiment ///////////////
// set final and ref deltas
@@ -495,13 +486,9 @@ TEST(ProcessorOdom3D, Interpolate2) // timestamp out of bounds test
}
-
-
-
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
-
diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp
index 1204254c1024266c98cfe2c8e3764f76453fe2ee..8f68a5bd532c5a1408d0ba9654ec9f27283c7ba3 100644
--- a/test/gtest_pack_KF_buffer.cpp
+++ b/test/gtest_pack_KF_buffer.cpp
@@ -7,13 +7,13 @@
//Wolf
#include "utils_gtest.h"
-#include "processor_odom_2D.h"
-#include "sensor_odom_2D.h"
+#include "base/processor/processor_odom_2D.h"
+#include "base/sensor/sensor_odom_2D.h"
-#include "processor_tracker_feature_dummy.h"
-#include "capture_void.h"
+#include "base/processor/processor_tracker_feature_dummy.h"
+#include "base/capture/capture_void.h"
-#include "problem.h"
+#include "base/problem.h"
// STL
#include <iterator>
diff --git a/test/gtest_pinhole.cpp b/test/gtest_pinhole.cpp
index b46e682ab406df2db2f84a23c72f9669e777df8c..fb36c6d127b11d14b86c8887fa32aa1b23d07391 100644
--- a/test/gtest_pinhole.cpp
+++ b/test/gtest_pinhole.cpp
@@ -5,10 +5,9 @@
* Author: jsola
*/
-#include "pinhole_tools.h"
+#include "base/pinhole_tools.h"
#include "utils_gtest.h"
-
using namespace Eigen;
using namespace wolf;
using namespace pinhole;
@@ -200,4 +199,3 @@ int main(int argc, char **argv)
return RUN_ALL_TESTS();
}
-
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 7ae62578b7d99441e56539a94830696e6489253c..1cf49acaa2bf803f3aa7108ae243bfe7cf4495c8 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -6,13 +6,13 @@
*/
#include "utils_gtest.h"
-#include "../src/logging.h"
+#include "base/logging.h"
-#include "../problem.h"
-#include "../sensor_base.h"
-#include "../sensor_odom_3D.h"
-#include "../processor_odom_3D.h"
-#include "../processor_tracker_feature_dummy.h"
+#include "base/problem.h"
+#include "base/sensor/sensor_base.h"
+#include "base/sensor/sensor_odom_3D.h"
+#include "base/processor/processor_odom_3D.h"
+#include "base/processor/processor_tracker_feature_dummy.h"
#include <iostream>
@@ -127,7 +127,6 @@ TEST(Problem, SetOrigin_PO_2D)
// check that the constraint is absolute (no pointers to other F, f, or L)
ConstraintBasePtr c = f->getConstraintList().front();
ASSERT_FALSE(c->getFrameOtherPtr());
- ASSERT_FALSE(c->getFrameOtherPtr());
ASSERT_FALSE(c->getLandmarkOtherPtr());
// check that the Feature measurement and covariance are the ones provided
@@ -137,7 +136,6 @@ TEST(Problem, SetOrigin_PO_2D)
// P->print(4,1,1,1);
}
-
TEST(Problem, SetOrigin_PO_3D)
{
ProblemPtr P = Problem::create("PO 3D");
@@ -166,7 +164,6 @@ TEST(Problem, SetOrigin_PO_3D)
// check that the constraint is absolute (no pointers to other F, f, or L)
ConstraintBasePtr c = f->getConstraintList().front();
ASSERT_FALSE(c->getFrameOtherPtr());
- ASSERT_FALSE(c->getFrameOtherPtr());
ASSERT_FALSE(c->getLandmarkOtherPtr());
// check that the Feature measurement and covariance are the ones provided
@@ -176,8 +173,6 @@ TEST(Problem, SetOrigin_PO_3D)
// P->print(4,1,1,1);
}
-
-
TEST(Problem, emplaceFrame_factory)
{
ProblemPtr P = Problem::create("PO 2D");
@@ -203,7 +198,6 @@ TEST(Problem, emplaceFrame_factory)
ASSERT_EQ(f2->getProblem(), P);
}
-
TEST(Problem, StateBlocks)
{
std::string wolf_root = _WOLF_ROOT_DIR;
@@ -235,7 +229,6 @@ TEST(Problem, StateBlocks)
ASSERT_EQ(P->getStateBlockList().size(), (unsigned int)(2 + 3 + 2));
ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int)(2 + 3 + 2));
-
// P->print(4,1,1,1);
// change some SB properties
@@ -277,7 +270,6 @@ TEST(Problem, Covariances)
}
-
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_processor_IMU.cpp b/test/gtest_processor_IMU.cpp
index 8dfb9bd57848a69fa88337308a5637f3f3a7d2af..a6ee8c8ac4df0bc05094d2fdc0b72a653cb2bab7 100644
--- a/test/gtest_processor_IMU.cpp
+++ b/test/gtest_processor_IMU.cpp
@@ -5,16 +5,16 @@
* \author: jsola
*/
-#include "capture_IMU.h"
-#include "processor_IMU.h"
-#include "sensor_IMU.h"
-#include "wolf.h"
+#include "base/capture/capture_IMU.h"
+#include "base/processor/processor_IMU.h"
+#include "base/sensor/sensor_IMU.h"
+#include "base/wolf.h"
#include "utils_gtest.h"
-#include "logging.h"
+#include "base/logging.h"
-#include "rotations.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/rotations.h"
+#include "base/ceres_wrapper/ceres_manager.h"
#include <cmath>
#include <iostream>
@@ -78,12 +78,10 @@ class ProcessorIMUt : public testing::Test
}
};
-
TEST(ProcessorIMU_constructors, ALL)
{
using namespace wolf;
-
//constructor with ProcessorIMUParamsPtr argument only
ProcessorParamsIMUPtr param_ptr = std::make_shared<ProcessorParamsIMU>();
param_ptr->max_time_span = 2.0;
@@ -248,7 +246,6 @@ ASSERT_MATRIX_APPROX(mot_int.data_, mot_int_gt.data_, 1e-6);
//ASSERT_MATRIX_APPROX(mot_int.delta_, mot_int_gt.delta_, 1e-6); // FIXME: delta_p not correctly interpolated
ASSERT_MATRIX_APPROX(mot_final.delta_integr_, mot_sec.delta_integr_, 1e-6);
-
}
TEST_F(ProcessorIMUt, acc_x)
@@ -351,7 +348,6 @@ TEST_F(ProcessorIMUt, acc_z)
ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS);
}
-
TEST_F(ProcessorIMUt, check_Covariance)
{
t.set(0); // clock in 0,1 ms ticks
@@ -394,7 +390,6 @@ TEST_F(ProcessorIMUt, gyro_x)
VectorXs x(10);
x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
-
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
//do so for 5s
diff --git a/test/gtest_processor_IMU_jacobians.cpp b/test/gtest_processor_IMU_jacobians.cpp
index dc9ab0397b04c55875630bfc573b69b5067c314c..1e3b0940cdf2434a059b071b95ca47847ebadd48 100644
--- a/test/gtest_processor_IMU_jacobians.cpp
+++ b/test/gtest_processor_IMU_jacobians.cpp
@@ -6,13 +6,13 @@
*/
//Wolf
-#include "capture_IMU.h"
-#include "sensor_IMU.h"
+#include "base/capture/capture_IMU.h"
+#include "base/sensor/sensor_IMU.h"
#include "test/processor_IMU_UnitTester.h"
-#include "wolf.h"
-#include "problem.h"
-#include "state_block.h"
-#include "state_quaternion.h"
+#include "base/wolf.h"
+#include "base/problem.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
#include <iostream>
#include <fstream>
#include <iomanip>
@@ -110,7 +110,6 @@ class ProcessorIMU_jacobians : public testing::Test
}
};
-
class ProcessorIMU_jacobians_Dq : public testing::Test
{
public:
@@ -470,7 +469,6 @@ TEST_F(ProcessorIMU_jacobians, dDv_dO)
//dDo_dP = dDo_dV = [0, 0, 0]
-
TEST_F(ProcessorIMU_jacobians, dDo_dO)
{
using namespace wolf;
@@ -506,7 +504,6 @@ TEST_F(ProcessorIMU_jacobians, dDp_dp)
//dDv_dp = [0, 0, 0]
-
TEST_F(ProcessorIMU_jacobians, dDv_dv)
{
using namespace wolf;
@@ -544,7 +541,6 @@ TEST_F(ProcessorIMU_jacobians, dDo_do)
"\ndDo_do_a - dDo_do_ : \n" << dDo_do_a - dDo_do << std::endl;
}
-
int main(int argc, char **argv)
{
using namespace wolf;
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 23baef7b289909e86555b96e160f8823910523f9..aba23263605f27b70cd04886374bb82013abf6e5 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -8,13 +8,13 @@
//Wolf
#include "utils_gtest.h"
-#include "processor_odom_2D.h"
-#include "sensor_odom_2D.h"
+#include "base/processor/processor_odom_2D.h"
+#include "base/sensor/sensor_odom_2D.h"
-#include "processor_tracker_feature_dummy.h"
-#include "capture_void.h"
+#include "base/processor/processor_tracker_feature_dummy.h"
+#include "base/capture/capture_void.h"
-#include "problem.h"
+#include "base/problem.h"
// STL
#include <iterator>
@@ -23,7 +23,6 @@
using namespace wolf;
using namespace Eigen;
-
TEST(ProcessorBase, KeyFrameCallback)
{
diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
index dcb32770e5b93e28da0b13309b5e4d09da16b79a..4e2b427fb500e01122994abd8ded959b8c460bff 100644
--- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
+++ b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
@@ -6,12 +6,11 @@
* \author: tessajohanna
*/
-
#include "utils_gtest.h"
-#include "../logging.h"
+#include "base/logging.h"
-#include "../sensor_odom_2D.h"
-#include "../processor_frame_nearest_neighbor_filter.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/processor/processor_frame_nearest_neighbor_filter.h"
#include <iostream>
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 94a2f0c15f61439d8f5d800d893d6ae95f45016a..3e379c9a5dd72300a5c2cfa614d2d5b5933f9542 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -5,15 +5,14 @@
* Author: jsola
*/
-
#include "utils_gtest.h"
-#include "wolf.h"
-#include "logging.h"
+#include "base/wolf.h"
+#include "base/logging.h"
-#include "sensor_odom_2D.h"
-#include "processor_odom_2D.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/processor/processor_odom_2D.h"
+#include "base/ceres_wrapper/ceres_manager.h"
using namespace Eigen;
using namespace wolf;
@@ -58,7 +57,6 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{
};
-
TEST_F(ProcessorMotion_test, IntegrateStraight)
{
data << 1, 0; // advance straight
diff --git a/test/gtest_processor_tracker_feature_trifocal.cpp b/test/gtest_processor_tracker_feature_trifocal.cpp
index 71a8e7f469ceee695ae6da01fd6743949895ff2e..47e42c577f69b7b1443b6752ed15c248721f00d6 100644
--- a/test/gtest_processor_tracker_feature_trifocal.cpp
+++ b/test/gtest_processor_tracker_feature_trifocal.cpp
@@ -1,14 +1,14 @@
#include "utils_gtest.h"
-#include "wolf.h"
-#include "logging.h"
+#include "base/wolf.h"
+#include "base/logging.h"
#include "vision_utils.h"
#include "processors/processor_tracker_feature_trifocal.h"
-#include "processor_odom_3D.h"
-#include "capture_image.h"
-#include "sensor_camera.h"
+#include "base/processor/processor_odom_3D.h"
+#include "base/capture/capture_image.h"
+#include "base/sensor/sensor_camera.h"
using namespace Eigen;
using namespace wolf;
@@ -21,7 +21,6 @@ using namespace wolf;
// }
//};
-
//TEST(ProcessorTrackerFeatureTrifocal, Constructor)
//{
// std::cout << "\033[1;33m [WARN]:\033[0m gtest for ProcessorTrackerFeatureTrifocal Constructor is empty." << std::endl;
@@ -146,7 +145,6 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
}
}
-
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp
index 125d8a3a31f9126b7bd9069309c0e56e32351011..5d656eda2fe23cde71e16b1ca15c389e11d65940 100644
--- a/test/gtest_rotation.cpp
+++ b/test/gtest_rotation.cpp
@@ -9,8 +9,8 @@
#include <Eigen/Geometry>
//Wolf
-#include "wolf.h"
-#include "../rotations.h"
+#include "base/wolf.h"
+#include "base/rotations.h"
//std
#include <iostream>
@@ -27,7 +27,6 @@
using namespace wolf;
using namespace Eigen;
-
TEST(exp_q, unit_norm)
{
Vector3s v0 = Vector3s::Random();
@@ -41,7 +40,6 @@ TEST(exp_q, unit_norm)
}
}
-
TEST(rotations, pi2pi)
{
ASSERT_NEAR(M_PI_2, pi2pi((Scalar)M_PI_2), 1e-10);
@@ -219,7 +217,6 @@ TEST(rotations, AngleAxis_limits)
}
}
-
TEST(compose, Quat_compos_const_rateOfTurn)
{
using namespace wolf;
@@ -480,7 +477,6 @@ TEST(Minus_and_diff, Random)
q1 .coeffs().setRandom().normalize();
q2 .coeffs().setRandom().normalize();
-
Vector3s vr = log_q(q1.conjugate() * q2);
Vector3s vl = log_q(q2 * q1.conjugate());
@@ -751,12 +747,10 @@ TEST(Conversions, e2R_R2q_q2e)
ASSERT_MATRIX_APPROX(eo, e, 1e-10);
}
-
int main(int argc, char **argv)
{
using namespace wolf;
-
/*
LIST OF FUNCTIONS :
- pi2pi
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index aacedf97642e245a84430069e125e7485277201a..cd809e75cd0a414fdac833de89bcb1d768baf84f 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -5,11 +5,10 @@
* \author: jsola
*/
-#include "sensor_base.h"
+#include "base/sensor/sensor_base.h"
#include "utils_gtest.h"
-
using namespace wolf;
TEST(SensorBase, setNoiseStd)
diff --git a/test/gtest_shared_from_this.cpp b/test/gtest_shared_from_this.cpp
index 99a9983dbbc9aa6a55815a3310853d82dd1e30b1..cebdce1f8da148e95c39422c422d9a44422c3dbe 100644
--- a/test/gtest_shared_from_this.cpp
+++ b/test/gtest_shared_from_this.cpp
@@ -1,5 +1,5 @@
#include "utils_gtest.h"
-#include "node_base.h"
+#include "base/node_base.h"
class CChildBase;
@@ -21,7 +21,6 @@ class CParentBase : public wolf::NodeBase
}
};
-
class CParentDerived : public CParentBase
{
public:
@@ -51,8 +50,6 @@ class CChildDerived : public CChildBase
CChildDerived(std::shared_ptr<CParentBase> _parent_ptr) : CChildBase(_parent_ptr){};
};
-
-
TEST(TestTest, SingleTest)
{
std::shared_ptr<CParentDerived> parent_derived_ptr = std::make_shared<CParentDerived>();
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 62bc03454f7bfe68dfb2fecf7709b3ed8736170d..9d5fe610b2ef612d2f21d8be00bfafd55e2fbca9 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -6,16 +6,16 @@
*/
#include "utils_gtest.h"
-#include "../src/logging.h"
-
-#include "../problem.h"
-#include "../sensor_base.h"
-#include "../state_block.h"
-#include "../capture_void.h"
-#include "../constraint_pose_2D.h"
-#include "../solver/solver_manager.h"
-#include "../local_parametrization_base.h"
-#include "../local_parametrization_angle.h"
+#include "base/logging.h"
+
+#include "base/problem.h"
+#include "base/sensor/sensor_base.h"
+#include "base/state_block.h"
+#include "base/capture/capture_void.h"
+#include "base/constraint/constraint_pose_2D.h"
+#include "base/solver/solver_manager.h"
+#include "base/local_parametrization_base.h"
+#include "base/local_parametrization_angle.h"
#include <iostream>
diff --git a/test/gtest_time_stamp.cpp b/test/gtest_time_stamp.cpp
index a5feaf39b5e6467866ee89235ccd9579ffa6f568..aad321781d7682c8c60dd83c9e0970e17118b164 100644
--- a/test/gtest_time_stamp.cpp
+++ b/test/gtest_time_stamp.cpp
@@ -1,5 +1,5 @@
#include "utils_gtest.h"
-#include "../time_stamp.h"
+#include "base/time_stamp.h"
#include <thread>
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index 98eedac4d13fc841159738b57d99d133b2defdfd..9d65d8c0fef5d11aca4cd94792f031feecbe122f 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -7,7 +7,7 @@
#include "utils_gtest.h"
-#include "track_matrix.h"
+#include "base/track_matrix.h"
using namespace wolf;
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index 92c8480399d9bdd77038672f48465e2d93294f5c..acbd4f242566d8fe97e71043b62ce96cc5d1c41b 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -5,13 +5,12 @@
* Author: jsola
*/
-
#include "utils_gtest.h"
-#include "../src/logging.h"
+#include "base/logging.h"
-#include "../problem.h"
-#include "../trajectory_base.h"
-#include "../frame_base.h"
+#include "base/problem.h"
+#include "base/trajectory_base.h"
+#include "base/frame_base.h"
#include <iostream>
@@ -56,7 +55,6 @@ struct DummyNotificationProcessor
ProblemPtr problem_;
};
-
/// Set to true if you want debug info
bool debug = false;
diff --git a/test/processor_IMU_UnitTester.h b/test/processor_IMU_UnitTester.h
index 4c2bece6cbbd6b46a6cf15906e7c515b5c6ee50f..c114086a16cae9cb28859270b3784461d7bb7773 100644
--- a/test/processor_IMU_UnitTester.h
+++ b/test/processor_IMU_UnitTester.h
@@ -3,8 +3,8 @@
#define PROCESSOR_IMU_UNITTESTER_H
// Wolf
-#include "processor_IMU.h"
-#include "processor_motion.h"
+#include "base/processor/processor_IMU.h"
+#include "base/processor/processor_motion.h"
namespace wolf {
struct IMU_jac_bias{ //struct used for checking jacobians by finite difference
@@ -65,7 +65,6 @@ namespace wolf {
Eigen::Matrix3s dDv_dwb_;
Eigen::Matrix3s dDq_dwb_;
-
public:
void copyfrom(IMU_jac_bias const& right){
@@ -197,9 +196,8 @@ namespace wolf {
/////////////////////////////////////////////////////////
// Wolf
-#include "state_block.h"
-#include "rotations.h"
-
+#include "base/state_block.h"
+#include "base/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 b47964a5d11f57a5a78b2e882c085fd33a571f9c..8532cc9f2d32246ed536a340574251a6b34ce379 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 "../../../wolf.h"
+#include "base/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 a124da37a9a8c5183c894bf074553a0504b417cd..5ca60e7f111f5b9e56392bcf59af3123c29637b6 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 "../../../wolf.h"
+#include "base/wolf.h"
#include "../../utils_gtest.h"
#include "../../../serialization/cereal/serialization_eigen_sparse.h"
diff --git a/test/serialization/cereal/gtest_serialization_local_parametrization.cpp b/test/serialization/cereal/gtest_serialization_local_parametrization.cpp
index 667c5d8443ec7ff0e7ecdf9d0ada0f04200db6b8..3df34a27aa683fb5099cb8c108a25fe0f473f31b 100644
--- a/test/serialization/cereal/gtest_serialization_local_parametrization.cpp
+++ b/test/serialization/cereal/gtest_serialization_local_parametrization.cpp
@@ -198,7 +198,6 @@ TEST(TestSerialization, SerializationLocalParametrizationHomogeneousPtrBIN)
PRINTF("All good at TestSerialization::SerializationLocalParametrizationHomogeneousPtrBIN !\n");
}
-
//////////////////////////////////////
/// LocalParametrizationQuaternion ///
//////////////////////////////////////
diff --git a/test/utils_gtest.h b/test/utils_gtest.h
index 0883d86ff494abe6f89e7bc53a35c1b0ab556c9a..9cb2166e8429cd236f77c502891c14880e57ec30 100644
--- a/test/utils_gtest.h
+++ b/test/utils_gtest.h
@@ -11,7 +11,6 @@
#include <gtest/gtest.h>
-
// Macros for testing equalities and inequalities.
//
// * {ASSERT|EXPECT}_EQ(expected, actual): Tests that expected == actual
@@ -48,7 +47,6 @@
//
// * {ASSERT|EXPECT}_NO_FATAL_FAILURE(statement);
-
// http://stackoverflow.com/a/29155677
namespace testing
@@ -145,5 +143,4 @@ TEST(Test, Foo)
} // namespace testing
-
#endif /* WOLF_UTILS_GTEST_H */
diff --git a/wolf_scripts/include_refactor.sh b/wolf_scripts/include_refactor.sh
index bae2b8ef1b6afd1d784e9c5003199a7b431b82bb..6d2c2fd8d6fae80bb3c327ead22dfa4c097de20b 100755
--- a/wolf_scripts/include_refactor.sh
+++ b/wolf_scripts/include_refactor.sh
@@ -1,22 +1,21 @@
#!/bin/bash
-# First parameter $1 is the path to the wolf root
-# Second parameter $2 is the name of the new folder
-
-hdrs_folder=$1/include/base/$2
-srcs_folder=$1/src/$2
-#Fix the includes in general
-core_headers=$(ag -g .*\\.h -l $1/include/base/$2/ | rev | cut -d '/' -f 1 | rev)
-for f in $core_headers; do
- ch_files=$(ag \#include[[:space:]]+\"\(.*\/\)*"$2/$f"\" $1/include/base/ -l)
- for fp in $ch_files; do
- # echo ".h -> "$fp
- sed -i -E "s/(#include[[:space:]]+\")("$2"\/)?"$f"\"/\1base\/"$2"\/"$f"\"/gp" $fp
- sed -i -E "s/(#include[[:space:]]+<)("$2"\/)?"$f">/\1base\/"$2"\/"$f">/gp" $fp
+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
- cs_files=$(ag \#include[[:space:]]+\"\(.*\/\)*"$2/$f"\" $1/src/ -l)
- for fp in $cs_files; do
- # echo ".cpp -> "$fp
- sed -i -E "s/(#include[[:space:]]+\")("$2"\/)?"$f"\"/\1base\/"$2"\/"$f"\"/gp" $fp
- sed -i -E "s/(#include[[:space:]]+<)("$2"\/)?"$f">/\1base\/"$2"\/"$f">/gp" $fp
- done
-done
\ No newline at end of file
+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)
+# if [ $matches -gt 1 ]; then
+# echo $f " -> " $(echo $path | cut -d ' ' -f 1)
+# fi
+# done
\ No newline at end of file