diff --git a/CMakeLists.txt b/CMakeLists.txt
index cd81990329ddee7e5b82b04ce5cab1a28b7a3b29..b4a40c33cf8d2573b7999f6411b064558b96d0db 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -135,7 +135,7 @@ ENDIF(cereal_FOUND)
# YAML with yaml-cpp
-INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake)
+FIND_PACKAGE(YamlCpp REQUIRED)
IF(YAMLCPP_FOUND)
MESSAGE("yaml-cpp Library FOUND: yaml-cpp related sources will be built.")
ELSEIF(YAMLCPP_FOUND)
@@ -194,7 +194,9 @@ ELSE (SPDLOG_INCLUDE_DIR)
ENDIF (SPDLOG_INCLUDE_DIR)
INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
-include_directories("include")
+INCLUDE_DIRECTORIES("include")
+INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
+
IF(Ceres_FOUND)
INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
ENDIF(Ceres_FOUND)
@@ -212,10 +214,6 @@ IF(Suitesparse_FOUND)
INCLUDE_DIRECTORIES(${Suitesparse_INCLUDE_DIRS})
ENDIF(Suitesparse_FOUND)
-IF(YAMLCPP_FOUND)
- INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
-ENDIF(YAMLCPP_FOUND)
-
IF(GLOG_FOUND)
INCLUDE_DIRECTORIES(${GLOG_INCLUDE_DIR})
ENDIF(GLOG_FOUND)
@@ -235,14 +233,15 @@ SET(HDRS_MATH
include/core/math/rotations.h
)
SET(HDRS_UTILS
+ include/core/utils/converter.h
include/core/utils/eigen_assert.h
include/core/utils/eigen_predicates.h
+ include/core/utils/loader.hpp
include/core/utils/logging.h
include/core/utils/make_unique.h
- include/core/utils/singleton.h
include/core/utils/params_server.hpp
- include/core/utils/converter.h
- include/core/utils/loader.hpp
+ include/core/utils/singleton.h
+ include/core/utils/utils_gtest.h
)
SET(HDRS_PROBLEM
include/core/problem/problem.h
@@ -288,6 +287,7 @@ SET(HDRS_FACTOR
include/core/factor/factor_base.h
include/core/factor/factor_block_absolute.h
include/core/factor/factor_diff_drive.h
+ include/core/factor/factor_feature_dummy.h
include/core/factor/factor_odom_2D.h
include/core/factor/factor_odom_2D_analytic.h
include/core/factor/factor_odom_3D.h
@@ -351,6 +351,7 @@ SET(HDRS_DTASSC
SET(HDRS_YAML
include/core/yaml/parser_yaml.hpp
+ include/core/yaml/yaml_conversion.h
)
#SOURCES
SET(SRCS_PROBLEM
@@ -439,6 +440,8 @@ SET(SRCS_SOLVER
src/solver/solver_manager.cpp
)
SET(SRCS_YAML
+ src/yaml/processor_odom_3D_yaml.cpp
+ src/yaml/sensor_odom_3D_yaml.cpp
)
#OPTIONALS
#optional HDRS and SRCS
@@ -474,18 +477,6 @@ IF (Suitesparse_FOUND)
#DOES NOTHING?!
#ADD_SUBDIRECTORY(solver_suitesparse)
ENDIF(Suitesparse_FOUND)
-# LEAVE YAML FILES ALWAYS IN THE LAST POSITION !!
-IF(YAMLCPP_FOUND)
- # headers
- SET(HDRS_YAML ${HDRS_YAML}
- include/core/yaml/yaml_conversion.h
- )
- # sources
- SET(SRCS_YAML ${SRCS_YAML}
- src/yaml/processor_odom_3D_yaml.cpp
- src/yaml/sensor_odom_3D_yaml.cpp
- )
-ENDIF(YAMLCPP_FOUND)
# create the shared library
ADD_LIBRARY(${PROJECT_NAME}
@@ -512,20 +503,15 @@ ADD_LIBRARY(${PROJECT_NAME}
${SRCS_WRAPPER}
${SRCS_YAML}
)
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT} dl)
#Link the created libraries
#=============================================================
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT} dl)
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${YAMLCPP_LIBRARY})
IF (Ceres_FOUND)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES})
ENDIF(Ceres_FOUND)
-
-
-IF (YAMLCPP_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${YAMLCPP_LIBRARY})
-ENDIF (YAMLCPP_FOUND)
-
IF (GLOG_FOUND)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY})
ENDIF (GLOG_FOUND)
diff --git a/cmake_modules/wolfConfig.cmake b/cmake_modules/wolfConfig.cmake
index 54a4b4edf5f01dd03e237eeb97d30178ccb14645..a72b121bde57c392cf9f13fe4a5ed73e309ddf98 100644
--- a/cmake_modules/wolfConfig.cmake
+++ b/cmake_modules/wolfConfig.cmake
@@ -11,7 +11,7 @@ ENDIF(wolf_INCLUDE_DIR)
FIND_LIBRARY(
wolf_LIBRARY
- NAMES libwolf.so
+ NAMES libwolf.so libwolf.dylib
PATHS /usr/local/lib/iri-algorithms)
IF(wolf_LIBRARY)
MESSAGE("Found wolf lib: ${wolf_LIBRARY}")
diff --git a/codetemplates eclipse.xml b/codetemplates eclipse.xml
index a83cc56bd43bb272eff012dd89a401c33283610f..8fb5a247ead13c2795a1a7d70da79187d010ad7b 100644
--- a/codetemplates eclipse.xml
+++ b/codetemplates eclipse.xml
@@ -23,7 +23,7 @@ ${declarations}
${namespace_end}</template><template autoinsert="false" context="org.eclipse.cdt.core.cxxSource.contenttype_context" deleted="false" description="Default template for newly created C++ test files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.cpptestfile" name="Default C++ test template">${filecomment}
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
${includes}
diff --git a/hello_wolf/CMakeLists.txt b/hello_wolf/CMakeLists.txt
index f8f41bd407e9e4d9fc9eaf71f92b7111b0772feb..b998b6f1c15ccb59d84e0a70ebbb6051a7301220 100644
--- a/hello_wolf/CMakeLists.txt
+++ b/hello_wolf/CMakeLists.txt
@@ -20,7 +20,10 @@ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
${CMAKE_CURRENT_SOURCE_DIR}/processor_range_bearing.cpp
${CMAKE_CURRENT_SOURCE_DIR}/sensor_range_bearing.cpp
)
+
+
add_library(hellowolf SHARED ${SRCS_HELLOWOLF})
+TARGET_LINK_LIBRARIES(hellowolf ${PROJECT_NAME})
ADD_EXECUTABLE(hello_wolf hello_wolf.cpp)
TARGET_LINK_LIBRARIES(hello_wolf ${PROJECT_NAME} hellowolf)
add_library(sensor_odom SHARED ../src/sensor/sensor_odom_2D.cpp ../src/processor/processor_odom_2D.cpp)
diff --git a/hello_wolf/factor_bearing.h b/hello_wolf/factor_bearing.h
index 56857fe0a188a5eb3ab30d8f4f6a967102f8b7d0..1b5ff40497096ac74f35024ce0b37b1c88076266 100644
--- a/hello_wolf/factor_bearing.h
+++ b/hello_wolf/factor_bearing.h
@@ -57,7 +57,7 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
{
// 1. produce a transformation matrix to transform from robot frame to world frame
- Transform<T, 2, Affine> H_w_r = Translation<T,2>(_p1[0], _p1[1]) * Rotation2D<T>(*_o1) ; // Robot frame = robot-to-world transform
+ Transform<T, 2, Isometry> H_w_r = Translation<T,2>(_p1[0], _p1[1]) * Rotation2D<T>(*_o1) ; // Robot frame = robot-to-world transform
// Map input pointers into meaningful Eigen elements
Map<const Matrix<T, 2, 1>> point_w(_p2);
Map<const Matrix<T, 1, 1>> res(_res);
diff --git a/hello_wolf/factor_range_bearing.h b/hello_wolf/factor_range_bearing.h
index c4a321badc8673aaa8c7ff57c61a90a987d5d831..ffe896f75932fb269068ad1210ad37350e62ce30 100644
--- a/hello_wolf/factor_range_bearing.h
+++ b/hello_wolf/factor_range_bearing.h
@@ -97,11 +97,11 @@ inline bool FactorRangeBearing::operator ()(const T* const _p_w_r, // robot posi
Map<Matrix<T, 2, 1>> res(_res); // residual
// 1. produce transformation matrices to transform from sensor frame --> to robot frame --> to world frame
- Transform<T, 2, Affine> H_w_r = Translation<T,2>(_p_w_r[0], _p_w_r[1]) * Rotation2D<T>(*_o_w_r) ; // Robot frame = robot-to-world transform
- Transform<T, 2, Affine> H_r_s = Translation<T,2>(_p_r_s[0], _p_r_s[1]) * Rotation2D<T>(*_o_r_s) ; // Sensor frame = sensor-to-robot transform
+ Transform<T, 2, Isometry> H_w_r = Translation<T,2>(_p_w_r[0], _p_w_r[1]) * Rotation2D<T>(*_o_w_r) ; // Robot frame = robot-to-world transform
+ Transform<T, 2, Isometry> H_r_s = Translation<T,2>(_p_r_s[0], _p_r_s[1]) * Rotation2D<T>(*_o_r_s) ; // Sensor frame = sensor-to-robot transform
// 2. Transform world-referenced landmark point to sensor-referenced point
- Transform<T, 2, Affine> H_w_s = H_w_r * H_r_s; // world-to-sensor transform
+ Transform<T, 2, Isometry> H_w_s = H_w_r * H_r_s; // world-to-sensor transform
Matrix<T, 2, 1> lmk_s = H_w_s.inverse() * lmk; // point in sensor frame
// 3. Get the expected range-and-bearing of the point
diff --git a/hello_wolf/processor_range_bearing.h b/hello_wolf/processor_range_bearing.h
index 30b2bd8654a8bc8117f761e492f889fb44918a4a..00592e5d6053dc0e75292b077529c19cbdc1a00d 100644
--- a/hello_wolf/processor_range_bearing.h
+++ b/hello_wolf/processor_range_bearing.h
@@ -30,7 +30,7 @@ WOLF_PTR_TYPEDEFS(ProcessorRangeBearing);
class ProcessorRangeBearing : public ProcessorBase
{
public:
- typedef Eigen::Transform<Scalar, 2, Eigen::Affine> Trf;
+ typedef Eigen::Transform<Scalar, 2, Eigen::Isometry> Trf;
ProcessorRangeBearing(const SensorRangeBearingPtr _sensor_ptr, ProcessorParamsBasePtr _params);
virtual ~ProcessorRangeBearing() {/* empty */}
diff --git a/include/core/ceres_wrapper/ceres_manager.h b/include/core/ceres_wrapper/ceres_manager.h
index c65b27415643c2c54b4c5a5144465b4a0767884b..413ecce13ebd2bb179134e255fe1d1af228efe13 100644
--- a/include/core/ceres_wrapper/ceres_manager.h
+++ b/include/core/ceres_wrapper/ceres_manager.h
@@ -90,9 +90,9 @@ class CeresManager : public SolverManager
ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _fac_ptr);
- virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr);
+ virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override;
- virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr);
+ virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) override;
};
inline ceres::Solver::Summary CeresManager::getSummary()
@@ -105,7 +105,7 @@ inline ceres::Solver::Options& CeresManager::getSolverOptions()
return ceres_options_;
}
-inline bool CeresManager::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr)
+inline bool CeresManager::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const
{
return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end()
&& fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end();
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 872364392a3b86749d2a6cc1d6f984fb40770b7b..d3e9e1ef41788c1c94dd3759fe06524ca4e1930a 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -17,6 +17,7 @@
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Sparse>
+
#include <libgen.h>
//includes from std lib
diff --git a/include/core/factor/factor_feature_dummy.h b/include/core/factor/factor_feature_dummy.h
index 52831f246cf132bd1c536004e4757baec35b48a8..4731ecaac631c57db2be44315b1d32084e03fd13 100644
--- a/include/core/factor/factor_feature_dummy.h
+++ b/include/core/factor/factor_feature_dummy.h
@@ -6,7 +6,6 @@
namespace wolf {
WOLF_PTR_TYPEDEFS(FactorFeatureDummy);
-
class FactorFeatureDummy : public FactorBase
{
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index 049cb951d07c5e1c0b70cdf9be9a1d9dea992261..f5ed62680e815ec182a9a0db28efe1c3894c6090 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -69,7 +69,7 @@ public:
virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr);
- virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr);
+ virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const;
protected:
@@ -94,7 +94,7 @@ protected:
virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) = 0;
- virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) = 0;
+ virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0;
};
} // namespace wolf
diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h
index 1824bbf870095023d17789374d4e244bf9e2434a..96e64575a653cef6f9f01d0c0d9b372e0540a170 100644
--- a/include/core/utils/converter.h
+++ b/include/core/utils/converter.h
@@ -6,6 +6,7 @@
#include <eigen3/Eigen/Geometry>
#include <regex>
#include <iostream>
+#include <array>
namespace utils{
template <typename A>
@@ -249,4 +250,4 @@ struct converter<std::map<std::string,A>>{
}
};
}
-#endif
\ No newline at end of file
+#endif
diff --git a/test/utils_gtest.h b/include/core/utils/utils_gtest.h
similarity index 100%
rename from test/utils_gtest.h
rename to include/core/utils/utils_gtest.h
diff --git a/include/core/yaml/parser_yaml.hpp b/include/core/yaml/parser_yaml.hpp
index 100208bfc7ca2e672afe5fd17f6fdf7c2df6d495..78ff43caa595ff4f8db37b0b860b3aef4ebf66fd 100644
--- a/include/core/yaml/parser_yaml.hpp
+++ b/include/core/yaml/parser_yaml.hpp
@@ -6,6 +6,7 @@
#include <map>
#include <iostream>
#include <algorithm>
+#include <numeric>
using namespace std;
namespace {
@@ -180,7 +181,7 @@ void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){
The following example shows how the header remains unafected:
@my_main_config | @some_path
- cov_det: 1 | - my_value : 23
- - follow: "some_path" |
+ - follow: "@some_path" |
- var: 1.2 |
Resulting map:
cov_det -> 1
@@ -193,7 +194,7 @@ void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){
Which would result from the following yaml files
@my_main_config | @some_path
- cov_det: 1 | - my_value : 23
- - $follow: "some_path" |
+ - $follow: "@some_path" |
- var: 1.2 |
*/
regex rr("follow");
@@ -321,4 +322,4 @@ map<string, string> parserYAML::fetchAsMap(YAML::Node n){
}
return m;
}
-#endif
\ No newline at end of file
+#endif
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index f6d2914da4ff77e594ee0b2722627897f6fb23e5..e03be2d6ce6c5a7dac729efeef040a4d312ae11e 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -165,7 +165,7 @@ bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr)
return state_blocks_.find(state_ptr) != state_blocks_.end() && isStateBlockRegisteredDerived(state_ptr);
}
-bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr)
+bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const
{
return isFactorRegisteredDerived(fac_ptr);
}
diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp
index a6146b6615adcd9bea24d3c12fab7d70ee013a12..c922ae4b0b6e80ddcf75090d09bcf04d58406e1e 100644
--- a/test/gtest_SE3.cpp
+++ b/test/gtest_SE3.cpp
@@ -7,7 +7,7 @@
#include "core/math/SE3.h"
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index 8bb84231d13a800845d80703025e2c382d187f8b..633669c7b4a5fea47832bf22ab009cb3671cad7a 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -5,7 +5,7 @@
* Author: jsola
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/capture/capture_base.h"
#include "core/state_block/state_angle.h"
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 4ab28374d25f09b63cceb7fa709ddfa9367ec347..28cd36ced23e26ab9921905c7227640e9bbf1134 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -5,7 +5,7 @@
* Author: jvallve
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/utils/logging.h"
#include "core/problem/problem.h"
diff --git a/test/gtest_converter.cpp b/test/gtest_converter.cpp
index 69bb7ce355646daadaf3b91a1e816e0f0c163587..b49f30a2d3e3f519dcc9e525aa8652830c809b51 100644
--- a/test/gtest_converter.cpp
+++ b/test/gtest_converter.cpp
@@ -1,4 +1,4 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/utils/converter.h"
using namespace std;
diff --git a/test/gtest_eigen_predicates.cpp b/test/gtest_eigen_predicates.cpp
index c7c68b4f3a2df00fab257fb4f47646d429f361be..0207f811e5e3797571905a4330e2d3f5a1a63371 100644
--- a/test/gtest_eigen_predicates.cpp
+++ b/test/gtest_eigen_predicates.cpp
@@ -1,4 +1,4 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/utils/eigen_predicates.h"
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index eb4b405f46a0cf0b429171f1c8c51b050321fe4f..cf6ae9004d91a009c87768373f5974c27c879086 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -5,7 +5,7 @@
* Author: jcasals
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/utils/logging.h"
#include "core/problem/problem.h"
@@ -83,10 +83,8 @@ TEST(Emplace, Feature)
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem());
ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame());
- auto cov = Eigen::MatrixXs(2,2);
- cov(0,0) = 1;
- cov(1,1) = 1;
- FeatureBase::emplace<FeatureBase>(cpt, "Dummy", Eigen::VectorXs(5), cov);
+ auto cov = Eigen::MatrixXs::Identity(2,2);
+ FeatureBase::emplace<FeatureBase>(cpt, "Dummy", Eigen::VectorXs(2), cov);
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture());
@@ -103,10 +101,8 @@ TEST(Emplace, Factor)
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem());
ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame());
- auto cov = Eigen::MatrixXs(2,2);
- cov(0,0) = 1;
- cov(1,1) = 1;
- auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXs(5), cov);
+ auto cov = Eigen::MatrixXs::Identity(2,2);
+ auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXs(2), cov);
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture());
@@ -121,17 +117,15 @@ TEST(Emplace, EmplaceDerived)
auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
// LandmarkBase::emplace<LandmarkBase>(MapBaseWPtr(P->getMap()),"Dummy", nullptr, nullptr);
auto sen = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Eigen::VectorXs(3), IntrinsicsOdom2D());
- auto cov = Eigen::MatrixXs(2,2);
- cov(0,0) = 1;
- cov(1,1) = 1;
- auto cpt = CaptureBase::emplace<CaptureOdom2D>(frm, TimeStamp(0), sen, Eigen::Vector6s(), cov, frm);
+ auto cov = Eigen::MatrixXs::Identity(2,2);
+ auto cpt = CaptureBase::emplace<CaptureOdom2D>(frm, TimeStamp(0), sen, Eigen::VectorXs(2), cov, frm);
auto cpt2 = std::static_pointer_cast<CaptureOdom2D>(cpt);
auto m = Eigen::Matrix<Scalar,9,6>();
for(int i = 0; i < 9; i++)
for(int j = 0; j < 6; j++)
m(i,j) = 1;
- auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXs(5), cov);
+ auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXs(2), cov);
ASSERT_EQ(sen, P->getHardware()->getSensorList().front());
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
}
@@ -143,4 +137,4 @@ int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
-}
\ No newline at end of file
+}
diff --git a/test/gtest_example.cpp b/test/gtest_example.cpp
index 0cadea4ffe640770290f82ea4b5e17542468d404..acfe5d6b72fa8bedf739497883b047d44efdd854 100644
--- a/test/gtest_example.cpp
+++ b/test/gtest_example.cpp
@@ -1,4 +1,4 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
TEST(TestTest, DummyTestExample)
{
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index da1cc8c050276e2e5f349773493027d0a96b0111..af452608f8a602ad2128ea5c44b81cf832b6004b 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -5,7 +5,7 @@
* \author: datchuth
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/factor/factor_block_absolute.h"
#include "core/factor/factor_quaternion_absolute.h"
#include "core/capture/capture_motion.h"
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index efa61acc9cc027c965aca8f8f27ac7f28e2fde58..108ae75cdb54fa50a3caf5eaa9733a096b893578 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -5,7 +5,7 @@
* Author: jvallve
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/sensor/sensor_odom_2D.h"
#include "core/capture/capture_void.h"
diff --git a/test/gtest_factor_autodiff_distance_3D.cpp b/test/gtest_factor_autodiff_distance_3D.cpp
index c605a558d48b12a0f4dad46ad718f929f1bbc62c..6fb4c644e4abc8045d1c842d533e5a73286cb431 100644
--- a/test/gtest_factor_autodiff_distance_3D.cpp
+++ b/test/gtest_factor_autodiff_distance_3D.cpp
@@ -11,7 +11,7 @@
#include "core/ceres_wrapper/ceres_manager.h"
#include "core/math/rotations.h"
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
using namespace wolf;
using namespace Eigen;
diff --git a/test/gtest_factor_odom_3D.cpp b/test/gtest_factor_odom_3D.cpp
index 09ae42b0c6f523f8a0ed98f49c570c82cbec9d7e..7c239323a93e009a7e8f4c71eca650022ee2cd94 100644
--- a/test/gtest_factor_odom_3D.cpp
+++ b/test/gtest_factor_odom_3D.cpp
@@ -5,7 +5,7 @@
* \author: jsola
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/factor/factor_odom_3D.h"
#include "core/capture/capture_motion.h"
diff --git a/test/gtest_factor_pose_2D.cpp b/test/gtest_factor_pose_2D.cpp
index bc50e62780e9812cbcdab2ffbe03a927ad3a650f..c28a8946ef393b2e41de96d424108dd44691d84a 100644
--- a/test/gtest_factor_pose_2D.cpp
+++ b/test/gtest_factor_pose_2D.cpp
@@ -6,7 +6,7 @@
*/
#include "core/factor/factor_pose_2D.h"
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/capture/capture_motion.h"
diff --git a/test/gtest_factor_pose_3D.cpp b/test/gtest_factor_pose_3D.cpp
index 45ee64674e3988bfb6a2c0a0c2c48c83fc0b438e..eda34b179b9d579cfc535dc3d488c5fef74cbc7b 100644
--- a/test/gtest_factor_pose_3D.cpp
+++ b/test/gtest_factor_pose_3D.cpp
@@ -6,7 +6,7 @@
*/
#include "core/factor/factor_pose_3D.h"
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/capture/capture_motion.h"
diff --git a/test/gtest_factor_sparse.cpp b/test/gtest_factor_sparse.cpp
index 6c8f212be2030c5d04707eb5b5798848f36d8710..53d8b91a196989936c9d908955ed2b1bc7c73eda 100644
--- a/test/gtest_factor_sparse.cpp
+++ b/test/gtest_factor_sparse.cpp
@@ -5,7 +5,7 @@
* \author: jsola
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "factor_sparse.h"
diff --git a/test/gtest_feature_base.cpp b/test/gtest_feature_base.cpp
index 3a93ef2cb46396defc8f3b3f541042ab85e4a1a8..713c044e2684e088606a99b02d3009ce35239f9e 100644
--- a/test/gtest_feature_base.cpp
+++ b/test/gtest_feature_base.cpp
@@ -7,7 +7,7 @@
#include "core/feature/feature_base.h"
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
using namespace wolf;
using namespace Eigen;
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 4cbc927bfe4e67fb4464c10aad4b1596c45e0987..4f189f559920003b3a0b8a033a56cde340d997ef 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -5,7 +5,7 @@
* Author: jsola
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/utils/logging.h"
#include "core/frame/frame_base.h"
diff --git a/test/gtest_local_param.cpp b/test/gtest_local_param.cpp
index 520a694f6622d4601bfa2689422dbed35a2ef413..267d912b6f8d8499b2e956b3bf90f2a5a7b95be5 100644
--- a/test/gtest_local_param.cpp
+++ b/test/gtest_local_param.cpp
@@ -5,7 +5,7 @@
* author: jsola
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/utils/logging.h"
#include "core/state_block/local_parametrization_quaternion.h"
diff --git a/test/gtest_make_posdef.cpp b/test/gtest_make_posdef.cpp
index 5a0b707a6487eda22e37f2601408cda64929015d..18222fd3b3ec94e009ec649a11730db1a95d4422 100644
--- a/test/gtest_make_posdef.cpp
+++ b/test/gtest_make_posdef.cpp
@@ -1,4 +1,4 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/common/wolf.h"
using namespace Eigen;
diff --git a/test/gtest_motion_buffer.cpp b/test/gtest_motion_buffer.cpp
index 3ef0bfaa050b6125427b2b16256c56719f54fbaf..8bd5f665921b35b0087d4636e86cd74691869cda 100644
--- a/test/gtest_motion_buffer.cpp
+++ b/test/gtest_motion_buffer.cpp
@@ -5,7 +5,7 @@
* Author: jsola
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/utils/logging.h"
#include "core/processor/motion_buffer.h"
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index faa22aff39d4af1878f051e6687a7a327e789f67..a25cf1733f6f78f43bb717613956f38306bbfd1b 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -5,7 +5,7 @@
* \author: jsola
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
// Classes under test
#include "core/processor/processor_odom_2D.h"
diff --git a/test/gtest_odom_3D.cpp b/test/gtest_odom_3D.cpp
index ce52ec3932b0162ca400cff88680ea27b7b4b196..3c13449cede9e839e14c50e47d7db78e2134dc1d 100644
--- a/test/gtest_odom_3D.cpp
+++ b/test/gtest_odom_3D.cpp
@@ -5,7 +5,7 @@
* \author: jsola
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/common/wolf.h"
#include "core/utils/logging.h"
diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp
index c2b8958d0ca9170f4ea317404b0c0cf2ccb9457f..6597f0f53207c98759e882a96221a2ac0fcb00d0 100644
--- a/test/gtest_pack_KF_buffer.cpp
+++ b/test/gtest_pack_KF_buffer.cpp
@@ -5,7 +5,7 @@
* Author: jsola
*/
//Wolf
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/processor/processor_odom_2D.h"
#include "core/sensor/sensor_odom_2D.h"
diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
index e5ddb8653a7b8e8246c1a2e8707c6eb186e66ae5..177961d922ebf117b0a696fd870cfe76f48b86ad 100644
--- a/test/gtest_param_prior.cpp
+++ b/test/gtest_param_prior.cpp
@@ -5,7 +5,7 @@
* Author: jvallve
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/utils/logging.h"
#include "core/problem/problem.h"
diff --git a/test/gtest_param_server.cpp b/test/gtest_param_server.cpp
index ab349ceb38fc304d6ad581a157d98fe4de42d390..3e83f8f89955a57342a0ec5247afb63303de69ce 100644
--- a/test/gtest_param_server.cpp
+++ b/test/gtest_param_server.cpp
@@ -1,4 +1,4 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/utils/converter.h"
#include "core/common/wolf.h"
#include "core/yaml/parser_yaml.hpp"
@@ -18,7 +18,7 @@ parserYAML parse(string _file, string _path_root)
TEST(ParamsServer, Default)
{
- auto parser = parse("/test/params1.yaml", wolf_root);
+ auto parser = parse("/test/yaml/params1.yaml", wolf_root);
auto params = parser.getParams();
paramsServer server = paramsServer(params, parser.sensorsSerialization(), parser.processorsSerialization());
EXPECT_EQ(server.getParam<double>("should_not_exist", "2.6"), 2.6);
diff --git a/test/gtest_parser_yaml.cpp b/test/gtest_parser_yaml.cpp
index 4af9e7760ef8d8d9b14fb6da4240852ee7919885..97451a6060ccd3adb43fd990c929c9cfee7b8dbc 100644
--- a/test/gtest_parser_yaml.cpp
+++ b/test/gtest_parser_yaml.cpp
@@ -1,4 +1,4 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/utils/converter.h"
#include "core/common/wolf.h"
#include "core/yaml/parser_yaml.hpp"
@@ -17,7 +17,7 @@ parserYAML parse(string _file, string _path_root)
TEST(ParserYAML, RegularParse)
{
- auto parser = parse("/test/params1.yaml", wolf_root);
+ auto parser = parse("/test/yaml/params1.yaml", wolf_root);
auto params = parser.getParams();
// for(auto it : params)
// cout << it.first << " %% " << it.second << endl;
@@ -26,13 +26,13 @@ TEST(ParserYAML, RegularParse)
}
TEST(ParserYAML, ParseMap)
{
- auto parser = parse("/test/params2.yaml", wolf_root);
+ auto parser = parse("/test/yaml/params2.yaml", wolf_root);
auto params = parser.getParams();
EXPECT_EQ(params["processor1/mymap"], "[{k1:v1},{k2:v2},{k3:[v3,v4,v5]}]");
}
TEST(ParserYAML, JumpFile)
{
- auto parser = parse("/test/params3.yaml", wolf_root);
+ auto parser = parse("/test/yaml/params3.yaml", wolf_root);
auto params = parser.getParams();
EXPECT_EQ(params["my_proc_test/max_buff_length"], "100");
EXPECT_EQ(params["my_proc_test/jump/voting_active"], "false");
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 29f4f132dc646705cca3c633798a8785c2494fea..44fac86d12c9134180c83896d6906ee954a65abf 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -5,7 +5,7 @@
* Author: jsola
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/utils/logging.h"
#include "core/problem/problem.h"
@@ -46,9 +46,9 @@ public:
virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr){};
virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr){};
virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr){return true;};
- virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr){return true;};
+ virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const {return true;};
virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
- virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr){return true;};
+ virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;};
};
TEST(Problem, create)
@@ -111,7 +111,7 @@ TEST(Problem, Installers)
ProblemPtr P = Problem::create("PO", 3);
Eigen::Vector7s xs;
- SensorBasePtr S = P->installSensor ("ODOM 3D", "odometer", xs, wolf_root + "/demos/sensor_odom_3D.yaml");
+ SensorBasePtr S = P->installSensor ("ODOM 3D", "odometer", xs, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
// install processor tracker (dummy installation under an Odometry sensor -- it's OK for this test)
ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
@@ -126,7 +126,7 @@ TEST(Problem, Installers)
ASSERT_FALSE(P->getProcessorMotion());
// install processor motion
- ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/demos/processor_odom_3D.yaml");
+ ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
// check motion processor is set
ASSERT_TRUE(P->getProcessorMotion());
@@ -243,7 +243,7 @@ TEST(Problem, StateBlocks)
Eigen::Vector3s xs2d;
// 2 state blocks, fixed
- SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs3d, wolf_root + "/demos/sensor_odom_3D.yaml");
+ SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs3d, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
// 2 state blocks, fixed
@@ -256,7 +256,7 @@ TEST(Problem, StateBlocks)
params->min_features_for_keyframe = 10;
auto pt = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(St, ProcessorTrackerFeatureDummy(params));
- ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/demos/processor_odom_3D.yaml");
+ ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
// 2 state blocks, estimated
auto KF = P->emplaceFrame("PO", 3, KEY, xs3d, 0);
@@ -302,7 +302,7 @@ TEST(Problem, Covariances)
Eigen::Vector7s xs3d;
Eigen::Vector3s xs2d;
- SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs3d, wolf_root + "/demos/sensor_odom_3D.yaml");
+ SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs3d, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
SensorBasePtr St = P->installSensor ("ODOM 2D", "other odometer", xs2d, "");
ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
@@ -313,7 +313,7 @@ TEST(Problem, Covariances)
auto pt = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(St, ProcessorTrackerFeatureDummy(params));
// St->addProcessor(pt);
- ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/demos/processor_odom_3D.yaml");
+ ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
// 4 state blocks, estimated
St->unfixExtrinsics();
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 9128e1ee66c9a1442a87a2998ec43ba29878f2bf..cff4bc083e1fa349e7986df8d6e4af1bb56b15f6 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -6,7 +6,7 @@
*/
//Wolf
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/processor/processor_odom_2D.h"
#include "core/sensor/sensor_odom_2D.h"
diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
index 95b16425310926f60e6fbe29d4ce16629d90f55e..82b3d2ae02e1e1821b9dca28b35792f6855a8f5e 100644
--- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
+++ b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
@@ -6,7 +6,7 @@
* \author: tessajohanna
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/utils/logging.h"
#include "core/sensor/sensor_odom_2D.h"
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 60d175a6d3d6ee66f58b4549f65f73f068b83703..57d7a5c685f6e60aa13d57829e8f83b9afcb3233 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -5,7 +5,7 @@
* Author: jsola
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/common/wolf.h"
#include "core/utils/logging.h"
diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp
index 513cddd55abc972f4f06df308a2eb35becffd366..35335523c9d3fbe02e8801f4cc88a4cbe8f747b4 100644
--- a/test/gtest_rotation.cpp
+++ b/test/gtest_rotation.cpp
@@ -18,7 +18,7 @@
#include <iomanip>
#include <ctime>
#include <cmath>
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
//#define write_results
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index d8420b2b3fa2d9635149c28b34ce3853bd63ca1a..499629f8fc934eaa5853670dd7d24a69fdc8885c 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -7,7 +7,7 @@
#include "core/sensor/sensor_base.h"
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
using namespace wolf;
diff --git a/test/gtest_shared_from_this.cpp b/test/gtest_shared_from_this.cpp
index 6b14fff0c5b9db4d16f8c0e9b22393157387fd9d..b2d5217f4be5f08525da8042478b32c871a7afa8 100644
--- a/test/gtest_shared_from_this.cpp
+++ b/test/gtest_shared_from_this.cpp
@@ -1,4 +1,4 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/common/node_base.h"
class CChildBase;
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 496dab16c21dc0de603c0b2169d47a89a85514ed..815817a6eac6faabf045a438819d09ae2d2529be 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -5,7 +5,7 @@
* Author: jvallve
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/utils/logging.h"
#include "core/problem/problem.h"
@@ -45,7 +45,7 @@ class SolverManagerWrapper : public SolverManager
return state_block_fixed_.at(st);
};
- bool isFactorRegistered(const FactorBasePtr& fac_ptr)
+ bool isFactorRegistered(const FactorBasePtr& fac_ptr) const
{
return std::find(factors_.begin(), factors_.end(), fac_ptr) != factors_.end();
};
@@ -63,7 +63,7 @@ class SolverManagerWrapper : public SolverManager
virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
- virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr){return true;};
+ virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;};
// The following are dummy implementations
bool hasConverged() { return true; }
diff --git a/test/gtest_time_stamp.cpp b/test/gtest_time_stamp.cpp
index 03b1d0582280f294d1fbe5bd217b8294f861ca73..7a39a5a6d0e794b8745339442b4959a6be76d8f6 100644
--- a/test/gtest_time_stamp.cpp
+++ b/test/gtest_time_stamp.cpp
@@ -1,4 +1,4 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/common/time_stamp.h"
#include <thread>
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index 2f956b0846a505568c084147a1121904cc2a80da..ff930b512b8f8da960b3921f81bc2bd73eac03b5 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -5,7 +5,7 @@
* \author: jsola
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/processor/track_matrix.h"
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index 50a78bc0f7b4705a9e17ddf1429ad6ee81aafe48..2edffe94d62ca65ce10bce645eeb5d08d685676f 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -5,7 +5,7 @@
* Author: jsola
*/
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "core/utils/logging.h"
#include "core/problem/problem.h"
@@ -38,9 +38,9 @@ struct DummySolverManager : public SolverManager
virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr){};
virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr){};
virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr){return true;};
- virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr){return true;};
+ virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const {return true;};
virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
- virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr){return true;};
+ virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;};
};
/// Set to true if you want debug info
diff --git a/test/params1.yaml b/test/params1.yaml
index 834984f6cc03f111dd446cca4e802603b81ae961..4ad74198329b61d86b023f17f9a7016224d5489a 100644
--- a/test/params1.yaml
+++ b/test/params1.yaml
@@ -1,30 +1 @@
-config:
- sensors:
- -
- type: "ODOM 2D"
- name: "odom"
- intrinsic:
- k_disp_to_disp: 0.1
- k_rot_to_rot: 0.1
- extrinsic:
- pos: [1,2,3]
- -
- type: "RANGE BEARING"
- name: "rb"
- processors:
- -
- type: "ODOM 2D"
- name: "processor1"
- sensor name: "odom"
- -
- type: "RANGE BEARING"
- name: "rb_processor"
- sensor name: "rb"
- -
- type: "ODOM 2D"
- name: "my_proc_test"
- sensor name: "odom"
- follow: "/test/params3.1.yaml"
-files:
- - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so"
- - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so"
\ No newline at end of file
+ follow: "/test/yaml/params3.1.yaml"
\ No newline at end of file
diff --git a/test/params3.yaml b/test/params3.yaml
index 3ee99dcc40a597e0b83a346aedd1e6c63fa52754..78489d218fad394f90364a1e1d758daa0270e7cd 100644
--- a/test/params3.yaml
+++ b/test/params3.yaml
@@ -1,31 +1 @@
-config:
- sensors:
- -
- type: "ODOM 2D"
- name: "odom"
- intrinsic:
- k_disp_to_disp: 0.1
- k_rot_to_rot: 0.1
- extrinsic:
- pos: [1,2,3]
- -
- type: "RANGE BEARING"
- name: "rb"
- processors:
- -
- type: "ODOM 2D"
- name: "processor1"
- sensor name: "odom"
- -
- type: "RANGE BEARING"
- name: "rb_processor"
- sensor name: "rb"
- -
- type: "ODOM 2D"
- name: "my_proc_test"
- sensor name: "odom"
- follow: "/test/params3.1.yaml"
- jump: "@/test/params3.1.yaml"
-files:
- - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so"
- - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so"
\ No newline at end of file
+ jump: "@/test/yaml/params3.1.yaml"
\ No newline at end of file
diff --git a/test/yaml/params1.yaml b/test/yaml/params1.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..940d3e5854a5ec612f5223251243e643c7bef79b
--- /dev/null
+++ b/test/yaml/params1.yaml
@@ -0,0 +1,30 @@
+config:
+ sensors:
+ -
+ type: "ODOM 2D"
+ name: "odom"
+ intrinsic:
+ k_disp_to_disp: 0.1
+ k_rot_to_rot: 0.1
+ extrinsic:
+ pos: [1,2,3]
+ -
+ type: "RANGE BEARING"
+ name: "rb"
+ processors:
+ -
+ type: "ODOM 2D"
+ name: "processor1"
+ sensor name: "odom"
+ -
+ type: "RANGE BEARING"
+ name: "rb_processor"
+ sensor name: "rb"
+ -
+ type: "ODOM 2D"
+ name: "my_proc_test"
+ sensor name: "odom"
+ follow: "/test/yaml/params3.1.yaml"
+files:
+ - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so"
+ - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so"
\ No newline at end of file
diff --git a/test/params2.yaml b/test/yaml/params2.yaml
similarity index 100%
rename from test/params2.yaml
rename to test/yaml/params2.yaml
diff --git a/test/params3.1.yaml b/test/yaml/params3.1.yaml
similarity index 100%
rename from test/params3.1.yaml
rename to test/yaml/params3.1.yaml
diff --git a/test/yaml/params3.yaml b/test/yaml/params3.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..ac82cf0dd818e6c6dec00c61ff86b75c38a3fc11
--- /dev/null
+++ b/test/yaml/params3.yaml
@@ -0,0 +1,31 @@
+config:
+ sensors:
+ -
+ type: "ODOM 2D"
+ name: "odom"
+ intrinsic:
+ k_disp_to_disp: 0.1
+ k_rot_to_rot: 0.1
+ extrinsic:
+ pos: [1,2,3]
+ -
+ type: "RANGE BEARING"
+ name: "rb"
+ processors:
+ -
+ type: "ODOM 2D"
+ name: "processor1"
+ sensor name: "odom"
+ -
+ type: "RANGE BEARING"
+ name: "rb_processor"
+ sensor name: "rb"
+ -
+ type: "ODOM 2D"
+ name: "my_proc_test"
+ sensor name: "odom"
+ follow: "/test/yaml/params3.1.yaml"
+ jump: "@/test/yaml/params3.1.yaml"
+files:
+ - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so"
+ - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so"
\ No newline at end of file
diff --git a/test/yaml/processor_odom_3D.yaml b/test/yaml/processor_odom_3D.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f501e333800ec1bbb4b7c751a32aa67a99bec74c
--- /dev/null
+++ b/test/yaml/processor_odom_3D.yaml
@@ -0,0 +1,8 @@
+processor type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+processor name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
+time tolerance: 0.01 # seconds
+keyframe vote:
+ max time span: 0.2 # seconds
+ max buffer length: 10 # motion deltas
+ dist traveled: 0.5 # meters
+ angle turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_3D.yaml b/test/yaml/sensor_odom_3D.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..9ea77803548cfd9d033f54e40b6d92a72710afb8
--- /dev/null
+++ b/test/yaml/sensor_odom_3D.yaml
@@ -0,0 +1,8 @@
+sensor type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+sensor name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
+motion variances:
+ disp_to_disp: 0.02 # m^2 / m
+ disp_to_rot: 0.02 # rad^2 / m
+ rot_to_rot: 0.01 # rad^2 / rad
+ min_disp_var: 0.01 # m^2
+ min_rot_var: 0.01 # rad^2
diff --git a/wolf_scripts/templates/gtest_template.cpp b/wolf_scripts/templates/gtest_template.cpp
index c797c1b5ad21d8c09ce89d3cb116cb6da299bacb..ccfec27db00cba6d92ac7f5d6b91718db1896be2 100644
--- a/wolf_scripts/templates/gtest_template.cpp
+++ b/wolf_scripts/templates/gtest_template.cpp
@@ -1,4 +1,4 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
#include "wolf.h"
#include "logging.h"
diff --git a/wolf_scripts/wolf_uninstall.sh b/wolf_scripts/wolf_uninstall.sh
new file mode 100755
index 0000000000000000000000000000000000000000..b381616ecf9c37423d5cd11e18bb3f5502cec641
--- /dev/null
+++ b/wolf_scripts/wolf_uninstall.sh
@@ -0,0 +1,20 @@
+#!/bin/bash
+
+echo “=â€
+echo “====================== UNINSTALLING WOLF ======================â€
+echo “=â€
+
+echo "cd /usr/local/include/iri-algorithms/"
+cd /usr/local/include/iri-algorithms/
+echo "sudo rm -rf wolf"
+sudo rm -rf wolf
+
+echo "cd /usr/local/lib/iri-algorithms/"
+echo "sudo rm libwolf*.*"
+cd /usr/local/lib/iri-algorithms/
+sudo rm libwolf*.*
+
+echo "cd /usr/local/lib/cmake/"
+echo "sudo rm -rf wolf*"
+cd /usr/local/lib/cmake/
+sudo rm -rf wolf*
\ No newline at end of file
diff --git a/wolf_scripts/wolf_update.sh b/wolf_scripts/wolf_update.sh
new file mode 100755
index 0000000000000000000000000000000000000000..7a0ceac5be717ff17fbe5acab186fda636de7ec4
--- /dev/null
+++ b/wolf_scripts/wolf_update.sh
@@ -0,0 +1,111 @@
+#!/bin/bash
+. ./generic_func/functions.sh
+# First parameter $1 is the path to the folder with the plugins
+declare -A success
+cd $1
+abs_path=$(pwd)
+# for folder in $(cd $abs_path && ls -d *); do
+for folder in wolf IMU laser gnss vision apriltag; do
+ echo ""
+ echo "==========================================================================================================================="
+ echo "${CYAN} Updating & installing $folder ${NC}"
+ echo "==========================================================================================================================="
+ echo ""
+ sleep 1
+ cd $abs_path
+ echo $(pwd)
+ cd $folder
+ is_repo=$(git rev-parse --is-inside-work-tree 2>/dev/null)
+ success[$folder]=false
+ if [ $is_repo ]; then
+ current_branch=$(git status | head -n 1 | cut -d' ' -f 3)
+ # if git checkout devel; then
+ if git pull; then
+ if [ ! -d build ];
+ then
+ mkdir build
+ fi
+ cd build
+ if ! cmake ..; then
+ echo ""
+ echo "==========================================================================================================================="
+ echo "${YELLOW} CMake has failed, avorting process for $folder ${NC}"
+ echo "==========================================================================================================================="
+ echo ""
+ sleep 1
+ # git checkout $current_branch
+ continue
+ fi
+ if ! make -j"$(nproc)"; then
+ echo ""
+ echo "==========================================================================================================================="
+ echo "${RED} Make has failed, avorting process for $folder ${NC}"
+ echo "==========================================================================================================================="
+ echo ""
+ sleep 1
+ # git checkout $current_branch
+ continue
+ fi
+ if ! ctest -j"$(nproc)"; then
+ echo ""
+ echo "==========================================================================================================================="
+ echo "${RED} Tests have not succeeded, avorting installation ${NC}"
+ echo "==========================================================================================================================="
+ echo ""
+ sleep 1
+ # git checkout $current_branch
+ continue
+ fi
+ if ! sudo make install; then
+ echo ""
+ echo "==========================================================================================================================="
+ echo "${RED} Installation for $folder failed ${NC}"
+ echo "==========================================================================================================================="
+ echo ""
+ sleep 1
+ continue
+ fi
+ echo ""
+ echo "==========================================================================================================================="
+ echo "${GREEN} Everything went smoothly on $folder ${NC}"
+ echo "==========================================================================================================================="
+ echo ""
+ sleep 1
+ # git checkout $current_branch
+ success[$folder]=true
+ else
+ # echo "No devel branch to checkout, skipping $folder"
+ echo ""
+ echo "==========================================================================================================================="
+ echo "${ORANGE} Something went wrong when pulling... skipping $folder ${NC}"
+ echo "==========================================================================================================================="
+ echo ""
+ sleep 1
+ fi
+ else
+ echo ""
+ echo "==========================================================================================================================="
+ echo "${CYAN} Skipping $folder... Not a git repo ${NC}"
+ echo "==========================================================================================================================="
+ echo ""
+ sleep 1
+ fi
+ # echo ""
+ # echo "==========================================================================================================================="
+ # echo "${YELLOW} Finished with $folder ${NC}"
+ # echo "==========================================================================================================================="
+ # echo ""
+done
+echo ""
+echo "==========================================================================================================================="
+echo "${CYAN} SUMMARY ${NC}"
+echo "==========================================================================================================================="
+echo ""
+for fl in "${!success[@]}";
+do
+ if "${success[$fl]}"; then
+ echo "${GREEN} Plugin ${YELLOW}$fl${GREEN} compiles, passess test and installs correctly ${NC}"
+ else
+ echo "${RED} Something went wrong with plugin ${YELLOW}$fl${RED}. Either it is not a plugin, it doesn't compile, it fails some test or there has been some error installing. ${NC}"
+ fi
+done
\ No newline at end of file