diff --git a/CMakeLists.txt b/CMakeLists.txt
index 176bdf1a2db64c9aac6da0dfb421d10c65fb8ba3..5c994422a00d3320b60ca2ad19fe9dc9f9f1b99b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -197,9 +197,8 @@ SET(HDRS_PROCESSOR
include/core/processor/motion_provider.h
include/core/processor/processor_base.h
include/core/processor/processor_diff_drive.h
- include/core/processor/processor_fix_wing_model.h
+ include/core/processor/processor_fixed_wing_model.h
include/core/processor/factory_processor.h
- include/core/processor/processor_logging.h
include/core/processor/processor_loop_closure.h
include/core/processor/processor_motion.h
include/core/processor/processor_odom_2d.h
@@ -214,7 +213,7 @@ SET(HDRS_SENSOR
include/core/sensor/sensor_base.h
include/core/sensor/sensor_diff_drive.h
include/core/sensor/factory_sensor.h
- include/core/sensor/sensor_model.h
+ include/core/sensor/sensor_motion_model.h
include/core/sensor/sensor_odom_2d.h
include/core/sensor/sensor_odom_3d.h
include/core/sensor/sensor_pose.h
@@ -249,11 +248,9 @@ SET(HDRS_UTILS
include/core/utils/check_log.h
include/core/utils/converter.h
include/core/utils/eigen_assert.h
- include/core/utils/eigen_predicates.h
include/core/utils/graph_search.h
include/core/utils/loader.h
include/core/utils/logging.h
- include/core/utils/make_unique.h
include/core/utils/params_server.h
include/core/utils/singleton.h
include/core/utils/utils_gtest.h
@@ -309,7 +306,7 @@ SET(SRCS_PROCESSOR
src/processor/motion_provider.cpp
src/processor/processor_base.cpp
src/processor/processor_diff_drive.cpp
- src/processor/processor_fix_wing_model.cpp
+ src/processor/processor_fixed_wing_model.cpp
src/processor/processor_loop_closure.cpp
src/processor/processor_motion.cpp
src/processor/processor_odom_2d.cpp
@@ -323,7 +320,7 @@ SET(SRCS_PROCESSOR
SET(SRCS_SENSOR
src/sensor/sensor_base.cpp
src/sensor/sensor_diff_drive.cpp
- src/sensor/sensor_model.cpp
+ src/sensor/sensor_motion_model.cpp
src/sensor/sensor_odom_2d.cpp
src/sensor/sensor_odom_3d.cpp
src/sensor/sensor_pose.cpp
diff --git a/include/core/processor/processor_fix_wing_model.h b/include/core/processor/processor_fixed_wing_model.h
similarity index 77%
rename from include/core/processor/processor_fix_wing_model.h
rename to include/core/processor/processor_fixed_wing_model.h
index 97c8ad6f3919c35744e824cde99d56c4d2b7e1b8..bccbc2317135376f7cf7dd1f70113fb933fc72b0 100644
--- a/include/core/processor/processor_fix_wing_model.h
+++ b/include/core/processor/processor_fixed_wing_model.h
@@ -26,31 +26,31 @@
* Author: joanvallve
*/
-#ifndef INCLUDE_CORE_PROCESSOR_PROCESSOR_FIX_WING_MODEL_H_
-#define INCLUDE_CORE_PROCESSOR_PROCESSOR_FIX_WING_MODEL_H_
+#ifndef INCLUDE_CORE_PROCESSOR_PROCESSOR_FIXED_WING_MODEL_H_
+#define INCLUDE_CORE_PROCESSOR_PROCESSOR_FIXED_WING_MODEL_H_
#include "core/processor/processor_base.h"
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorFixWingModel);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorFixedWingModel);
-struct ParamsProcessorFixWingModel : public ParamsProcessorBase
+struct ParamsProcessorFixedWingModel : public ParamsProcessorBase
{
Eigen::Vector3d velocity_local;
double angle_stdev;
double min_vel_norm;
- ParamsProcessorFixWingModel() = default;
- ParamsProcessorFixWingModel(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ParamsProcessorFixedWingModel() = default;
+ ParamsProcessorFixedWingModel(std::string _unique_name, const wolf::ParamsServer & _server) :
ParamsProcessorBase(_unique_name, _server)
{
velocity_local = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/velocity_local");
angle_stdev = _server.getParam<double> (prefix + _unique_name + "/angle_stdev");
min_vel_norm = _server.getParam<double> (prefix + _unique_name + "/min_vel_norm");
- assert(std::abs(velocity_local.norm() - 1.0) < wolf::Constants::EPS && "ParamsProcessorFixWingModel: 'velocity_local' must be normalized");
+ assert(std::abs(velocity_local.norm() - 1.0) < wolf::Constants::EPS && "ParamsProcessorFixedWingModel: 'velocity_local' must be normalized");
}
std::string print() const override
{
@@ -61,17 +61,17 @@ struct ParamsProcessorFixWingModel : public ParamsProcessorBase
}
};
-WOLF_PTR_TYPEDEFS(ProcessorFixWingModel);
+WOLF_PTR_TYPEDEFS(ProcessorFixedWingModel);
-class ProcessorFixWingModel : public ProcessorBase
+class ProcessorFixedWingModel : public ProcessorBase
{
public:
- ProcessorFixWingModel(ParamsProcessorFixWingModelPtr _params);
+ ProcessorFixedWingModel(ParamsProcessorFixedWingModelPtr _params);
// Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorFixWingModel, ParamsProcessorFixWingModel);
+ WOLF_PROCESSOR_CREATE(ProcessorFixedWingModel, ParamsProcessorFixedWingModel);
- virtual ~ProcessorFixWingModel() override;
+ virtual ~ProcessorFixedWingModel() override;
void configure(SensorBasePtr _sensor) override;
protected:
@@ -105,9 +105,9 @@ class ProcessorFixWingModel : public ProcessorBase
virtual bool voteForKeyFrame() const override {return false;};
// ATTRIBUTES
- ParamsProcessorFixWingModelPtr params_processor_;
+ ParamsProcessorFixedWingModelPtr params_processor_;
};
} /* namespace wolf */
-#endif /* INCLUDE_CORE_PROCESSOR_PROCESSOR_FIX_WING_MODEL_H_ */
+#endif /* INCLUDE_CORE_PROCESSOR_PROCESSOR_FIXED_WING_MODEL_H_ */
diff --git a/include/core/processor/processor_logging.h b/include/core/processor/processor_logging.h
deleted file mode 100644
index 52eab893a2647dc4ef0af352b4ea7bdcd7215e33..0000000000000000000000000000000000000000
--- a/include/core/processor/processor_logging.h
+++ /dev/null
@@ -1,52 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/**
- * \file processor_logging.h
- *
- * Created on: Oct 5, 2017
- * \author: Jeremie Deray
- */
-
-#ifndef _WOLF_PROCESSOR_LOGGING_H_
-#define _WOLF_PROCESSOR_LOGGING_H_
-
-/// @brief un-comment for IDE highlights.
-
-
-#define __INTERNAL_WOLF_ASSERT_PROCESSOR \
- static_assert(std::is_base_of<ProcessorBase, \
- typename std::remove_pointer<decltype(this)>::type>::value, \
- "This macro can be used only within the body of a " \
- "non-static " \
- "ProcessorBase (and derived) function !");
-
-#define WOLF_PROCESSOR_INFO(...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_INFO_NAMED(getType(), __VA_ARGS__);
-#define WOLF_PROCESSOR_WARN(...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_WARN_NAMED(getType(), __VA_ARGS__);
-#define WOLF_PROCESSOR_ERROR(...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_ERROR_NAMED(getType(), __VA_ARGS__);
-#define WOLF_PROCESSOR_DEBUG(...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_DEBUG_NAMED(getType(), __VA_ARGS__);
-
-#define WOLF_PROCESSOR_INFO_COND(cond, ...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_INFO_NAMED_COND(getType(), cond, __VA_ARGS__);
-#define WOLF_PROCESSOR_WARN_COND(cond, ...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_WARN_NAMED_COND(getType(), cond, __VA_ARGS__);
-#define WOLF_PROCESSOR_ERROR_COND(cond, ...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_ERROR_NAMED_COND(getType(), cond, __VA_ARGS__);
-#define WOLF_PROCESSOR_DEBUG_COND(cond, ...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_DEBUG_NAMED_COND(getType(), cond, __VA_ARGS__);
-
-#endif /* _WOLF_PROCESSOR_LOGGING_H_ */
diff --git a/include/core/sensor/sensor_model.h b/include/core/sensor/sensor_motion_model.h
similarity index 80%
rename from include/core/sensor/sensor_model.h
rename to include/core/sensor/sensor_motion_model.h
index 7070ff1c72da933fa44a0ca180a596224ab66ee5..68da8c7d481aa502c8352a350e03498de7f758e0 100644
--- a/include/core/sensor/sensor_model.h
+++ b/include/core/sensor/sensor_motion_model.h
@@ -19,26 +19,27 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#ifndef SRC_SENSOR_MODEL_H_
-#define SRC_SENSOR_MODEL_H_
+#ifndef SRC_SENSOR_MOTION_MODEL_H_
+#define SRC_SENSOR_MOTION_MODEL_H_
//wolf includes
#include "core/sensor/sensor_base.h"
namespace wolf {
-WOLF_PTR_TYPEDEFS(SensorModel);
-class SensorModel : public SensorBase
+WOLF_PTR_TYPEDEFS(SensorMotionModel);
+
+class SensorMotionModel : public SensorBase
{
public:
- SensorModel();
- ~SensorModel() override;
+ SensorMotionModel();
+ ~SensorMotionModel() override;
static SensorBasePtr create(const std::string& _unique_name,
const ParamsServer& _server)
{
- auto sensor = std::make_shared<SensorModel>();
+ auto sensor = std::make_shared<SensorMotionModel>();
sensor ->setName(_unique_name);
return sensor;
}
@@ -47,7 +48,7 @@ class SensorModel : public SensorBase
const Eigen::VectorXd& _extrinsics,
const ParamsSensorBasePtr _intrinsics)
{
- auto sensor = std::make_shared<SensorModel>();
+ auto sensor = std::make_shared<SensorMotionModel>();
sensor ->setName(_unique_name);
return sensor;
}
@@ -56,4 +57,4 @@ class SensorModel : public SensorBase
} /* namespace wolf */
-#endif /* SRC_SENSOR_POSE_H_ */
+#endif /* SRC_SENSOR_MOTION_MODEL_H_ */
diff --git a/include/core/utils/eigen_predicates.h b/include/core/utils/eigen_predicates.h
deleted file mode 100644
index 3a83601e84d95eba96d1193c651c220eca0ebf64..0000000000000000000000000000000000000000
--- a/include/core/utils/eigen_predicates.h
+++ /dev/null
@@ -1,121 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/**
- * \file eigen_predicates.h
- * \brief Some utils for comparing Eigen types
- * \author Jeremie Deray
- * Created on: 24/10/2017
- */
-
-#ifndef _WOLF_EIGEN_PREDICATES_H_
-#define _WOLF_EIGEN_PREDICATES_H_
-
-#include "core/math/rotations.h"
-
-namespace wolf {
-
-/// @brief check that each Matrix/Vector elem is approx equal to value +- precision
-///
-/// @note we overload this rather than using Eigen::Matrix::isConstant() since it is
-/// defined as :
-/// abs(x - y) <= (min)(abs(x), abs(y)) * prec
-/// which does not play nice with y = 0
-auto is_constant = [](const Eigen::MatrixXd& lhs, const double val, const double precision)
- {
- for (Eigen::MatrixXd::Index j = 0; j < lhs.cols(); ++j)
- for (Eigen::MatrixXd::Index i = 0; i < lhs.rows(); ++i)
- if (std::abs(lhs.coeff(i, j) - val) > precision)
- return false;
- return true;
- };
-
-/// @brief check that each element of the Matrix/Vector difference
-/// is approx 0 +- precision
-auto pred_zero = [](const Eigen::MatrixXd& lhs, const double precision)
- { return is_constant(lhs, 0, precision); };
-
-/// @brief check that each element of the Matrix/Vector difference
-/// is approx 0 +- precision
-auto pred_diff_zero = [](const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, const double precision)
- { return pred_zero(lhs - rhs, precision); };
-
-/// @brief check that the Matrix/Vector difference norm is approx 0 +- precision
-auto pred_diff_norm_zero = [](const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, const double precision)
- { return std::abs((lhs - rhs).norm()) <= std::abs(precision); };
-
-/// @brief check that the lhs Matrix/Vector is elem-wise approx the rhs +-precision
-auto pred_is_approx = [](const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, const double precision)
- { return lhs.isApprox(rhs, precision); };
-
-/// @brief check that angle θ of rotation required to get from lhs quaternion to rhs
-/// is <= precision
-auto pred_quat_is_approx = [](const Eigen::Quaterniond& lhs, const Eigen::Quaterniond& rhs, const double precision)
- { return pred_zero( log_q(rhs * lhs.inverse()), precision ); };
-
-/// @brief check that angle θ of rotation required to get from lhs quaternion to identity
-/// is <= precision
-auto pred_quat_identity = [](const Eigen::Quaterniond& lhs, const double precision)
- { return pred_quat_is_approx(lhs, Eigen::Quaterniond::Identity(), precision); };
-
-/// @brief check that rotation angle to get from lhs angle to rhs is <= precision
-auto pred_angle_is_approx = [](const double lhs, const double rhs, const double precision)
- { return std::abs(pi2pi(lhs - rhs)) <= std::abs(precision); };
-
-/// @brief check that rotation angle to get from lhs angle to 0 is <= precision
-auto pred_angle_zero = [](const double lhs, const double precision)
- { return pred_angle_is_approx(lhs, 0, precision); };
-
-/// @brief check that the lhs pose is approx rhs +- precision
-///
-/// @note
-/// Comparison is :
-/// d = inv(lhs) * rhs
-/// d.translation().elem_wise() ~ 0 (+- precision)
-///
-/// if pose 3d :
-/// d.rotation_as_quaternion() ~ quaternion.getIdentity (+- precision)
-///
-/// else if pose 2d:
-/// d.rotation_angle() ~ 0 (+- precision)
-///
-/// else throw std::runtime
-///
-/// @see pred_zero for translation comparison
-/// @see pred_quat_identity for 3d rotation comparison
-/// @see pred_angle_zero for 2d rotation comparison
-//auto pred_pose_is_approx = [](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs, const double precision)
-// {
-// const Eigen::MatrixXd d = lhs.inverse() * rhs;
-// const bool tok = pred_zero(d.rightCols(1), precision);
-
-// const bool qok = (lhs.rows() == 3)?
-// pred_quat_identity(Eigen::Quaterniond(d.block(0,0,3,1)),
-// precision)
-// : (lhs.rows() == 2)? pred_angle_zero(getYaw(d), precision)
-// : throw std::runtime_error("Canno't compare pose in Dim > 3 !");
-
-// return tok && qok;
-// };
-
-} // namespace wolf
-
-#endif /* _WOLF_EIGEN_PREDICATES_H_ */
diff --git a/include/core/utils/logging.h b/include/core/utils/logging.h
index b34ceccb16c5e12567256f2749896d5369d017a8..a0e1556898fd453f5543ba90ec3645ccf282525c 100644
--- a/include/core/utils/logging.h
+++ b/include/core/utils/logging.h
@@ -222,16 +222,6 @@ inline void Logger::set_pattern(const std::string& p)
using LoggerPtr = std::unique_ptr<Logger>;
-/// dummy namespace to avoid colision with c++14
-/// @todo use std version once we move to cxx14
-namespace not_std {
-template <typename T, typename... Args>
-std::unique_ptr<T> make_unique(Args&&... args)
-{
- return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
-}
-} /* namespace not_std */
-
class LoggerManager
{
public:
@@ -265,7 +255,7 @@ protected:
/// @note would be easier with cpp17 map.try_emplace...
const bool created = existsImpl(name) ?
false :
- logger_map_.emplace(name, not_std::make_unique<Logger>(name)).second;
+ logger_map_.emplace(name, std::make_unique<Logger>(name)).second;
return created;
}
diff --git a/include/core/utils/make_unique.h b/include/core/utils/make_unique.h
deleted file mode 100644
index 0bfdbb8372a54fb59d2ab59d8ca1b78432b38dec..0000000000000000000000000000000000000000
--- a/include/core/utils/make_unique.h
+++ /dev/null
@@ -1,45 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/**
- * \file make_unique.h
- *
- * Created on: Oct 11, 2017
- * \author: Jeremie Deray
- */
-
-#ifndef _WOLF_MAKE_UNIQUE_H_
-#define _WOLF_MAKE_UNIQUE_H_
-
-#include <memory>
-
-namespace wolf {
-
-/// @note this appears only in cpp14
-template <typename T, typename... Args>
-std::unique_ptr<T> make_unique(Args&&... args)
-{
- return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
-}
-
-}
-
-#endif /* _WOLF_MAKE_UNIQUE_H_ */
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index aed7fef36c99f71de9b664fa456620c7ef6523cd..ab8753351aee84c5b4f290af171b370663060499 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -27,7 +27,6 @@
#include "core/trajectory/trajectory_base.h"
#include "core/map/map_base.h"
#include "core/landmark/landmark_base.h"
-#include "core/utils/make_unique.h"
namespace wolf
{
@@ -47,8 +46,8 @@ SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem,
, n_interrupted_(0)
, n_no_convergence_(0)
{
- covariance_ = wolf::make_unique<ceres::Covariance>(params_ceres_->covariance_options);
- ceres_problem_ = wolf::make_unique<ceres::Problem>(params_ceres_->problem_options);
+ covariance_ = std::make_unique<ceres::Covariance>(params_ceres_->covariance_options);
+ ceres_problem_ = std::make_unique<ceres::Problem>(params_ceres_->problem_options);
if (params_ceres_->interrupt_on_problem_change)
getSolverOptions().callbacks.push_back(new IterationUpdateCallback(wolf_problem_,
diff --git a/src/processor/processor_fix_wing_model.cpp b/src/processor/processor_fixed_wing_model.cpp
similarity index 83%
rename from src/processor/processor_fix_wing_model.cpp
rename to src/processor/processor_fixed_wing_model.cpp
index 4c75926cc84925fa827f580a2d126ac294651d4b..a994d8a3cef2b93dcf0e5897392a1e9cf8d1533d 100644
--- a/src/processor/processor_fix_wing_model.cpp
+++ b/src/processor/processor_fixed_wing_model.cpp
@@ -26,7 +26,7 @@
* Author: joanvallve
*/
-#include "core/processor/processor_fix_wing_model.h"
+#include "../../include/core/processor/processor_fixed_wing_model.h"
#include "core/capture/capture_base.h"
#include "core/feature/feature_base.h"
@@ -35,22 +35,22 @@
namespace wolf
{
-ProcessorFixWingModel::ProcessorFixWingModel(ParamsProcessorFixWingModelPtr _params) :
- ProcessorBase("ProcessorFixWingModel", 3, _params),
+ProcessorFixedWingModel::ProcessorFixedWingModel(ParamsProcessorFixedWingModelPtr _params) :
+ ProcessorBase("ProcessorFixedWingModel", 3, _params),
params_processor_(_params)
{
}
-ProcessorFixWingModel::~ProcessorFixWingModel()
+ProcessorFixedWingModel::~ProcessorFixedWingModel()
{
}
-void ProcessorFixWingModel::configure(SensorBasePtr _sensor)
+void ProcessorFixedWingModel::configure(SensorBasePtr _sensor)
{
assert(_sensor->getProblem()->getFrameStructure().find('V') != std::string::npos && "Processor only works with problems with V");
}
-void ProcessorFixWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr)
+void ProcessorFixedWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr)
{
if (_keyframe_ptr->getV()->isFixed())
return;
@@ -81,7 +81,7 @@ void ProcessorFixWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr)
// Register in the FactoryProcessor
#include "core/processor/factory_processor.h"
namespace wolf {
-WOLF_REGISTER_PROCESSOR(ProcessorFixWingModel);
-WOLF_REGISTER_PROCESSOR_AUTO(ProcessorFixWingModel);
+WOLF_REGISTER_PROCESSOR(ProcessorFixedWingModel);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorFixedWingModel);
} // namespace wolf
diff --git a/src/sensor/sensor_model.cpp b/src/sensor/sensor_motion_model.cpp
similarity index 78%
rename from src/sensor/sensor_model.cpp
rename to src/sensor/sensor_motion_model.cpp
index 292f1a05e1bfd5aaf1931998106a505afaf10df8..9ef4a8098144ab88374b54b16e61526a93db6c68 100644
--- a/src/sensor/sensor_model.cpp
+++ b/src/sensor/sensor_motion_model.cpp
@@ -19,18 +19,18 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "core/sensor/sensor_model.h"
+#include "../../include/core/sensor/sensor_motion_model.h"
namespace wolf {
-SensorModel::SensorModel() :
- SensorBase("SensorModel", nullptr, nullptr, nullptr, 6)
+SensorMotionModel::SensorMotionModel() :
+ SensorBase("SensorMotionModel", nullptr, nullptr, nullptr, 6)
{
//
}
-SensorModel::~SensorModel()
+SensorMotionModel::~SensorMotionModel()
{
//
}
@@ -40,6 +40,6 @@ SensorModel::~SensorModel()
// Register in the FactorySensor
#include "core/sensor/factory_sensor.h"
namespace wolf {
-WOLF_REGISTER_SENSOR(SensorModel);
-WOLF_REGISTER_SENSOR_AUTO(SensorModel);
+WOLF_REGISTER_SENSOR(SensorMotionModel);
+WOLF_REGISTER_SENSOR_AUTO(SensorMotionModel);
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index a3da6a740948958190273e4526f997a6872a0cb1..743787f40026b6449169470e1d3eb66c14bddc38 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -68,10 +68,6 @@ target_link_libraries(gtest_factor_autodiff ${PLUGIN_NAME})
wolf_add_gtest(gtest_factory_state_block gtest_factory_state_block.cpp)
target_link_libraries(gtest_factory_state_block ${PLUGIN_NAME})
-# FeatureBase classes test
-wolf_add_gtest(gtest_eigen_predicates gtest_eigen_predicates.cpp)
-target_link_libraries(gtest_eigen_predicates ${PLUGIN_NAME})
-
# Node Emplace test
wolf_add_gtest(gtest_emplace gtest_emplace.cpp)
target_link_libraries(gtest_emplace ${PLUGIN_NAME} dummy)
@@ -233,11 +229,11 @@ target_link_libraries(gtest_map_yaml ${PLUGIN_NAME})
wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
target_link_libraries(gtest_param_prior ${PLUGIN_NAME})
-# ProcessorDiffDriveSelfcalib class test
-wolf_add_gtest(gtest_processor_fix_wing_model gtest_processor_fix_wing_model.cpp)
-target_link_libraries(gtest_processor_fix_wing_model ${PLUGIN_NAME})
+# ProcessorFixedWingModel class test
+wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp)
+target_link_libraries(gtest_processor_fixed_wing_model ${PLUGIN_NAME})
-# ProcessorFixWingModel class test
+# ProcessorDiffDrive class test
wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
target_link_libraries(gtest_processor_diff_drive ${PLUGIN_NAME})
diff --git a/test/gtest_eigen_predicates.cpp b/test/gtest_eigen_predicates.cpp
deleted file mode 100644
index 6dc7184ec5d1d39db31a2e26d06964023c26e65f..0000000000000000000000000000000000000000
--- a/test/gtest_eigen_predicates.cpp
+++ /dev/null
@@ -1,199 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#include "core/utils/utils_gtest.h"
-
-#include "core/utils/eigen_predicates.h"
-
-TEST(TestEigenPredicates, TestEigenDynPredZero)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::MatrixXd::Zero(4,4);
- B = Eigen::MatrixXd::Random(4,4);
- C = Eigen::MatrixXd::Ones(4,4) * (wolf::Constants::EPS/2.);
-
- EXPECT_TRUE(wolf::pred_zero(A, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_zero(B, wolf::Constants::EPS));
- EXPECT_TRUE(wolf::pred_zero(C, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenFixPredZero)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::Matrix4d::Zero();
- B = Eigen::Matrix4d::Random();
- C = Eigen::Matrix4d::Ones() * (wolf::Constants::EPS/2.);
-
- EXPECT_TRUE(wolf::pred_zero(A, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_zero(B, wolf::Constants::EPS));
- EXPECT_TRUE(wolf::pred_zero(C, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenDynPredDiffZero)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::MatrixXd::Random(4,4);
- B = Eigen::MatrixXd::Random(4,4);
- C = A;
-
- EXPECT_TRUE(wolf::pred_diff_zero(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_diff_zero(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenFixPredDiffZero)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::Matrix4d::Random();
- B = Eigen::Matrix4d::Random();
- C = A;
-
- EXPECT_TRUE(wolf::pred_diff_zero(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_diff_zero(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenDynPredDiffNorm)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::MatrixXd::Random(4,4);
- B = Eigen::MatrixXd::Random(4,4);
- C = A;
-
- EXPECT_TRUE(wolf::pred_diff_norm_zero(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_diff_norm_zero(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenFixPredDiffNorm)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::Matrix4d::Random();
- B = Eigen::Matrix4d::Random();
- C = A;
-
- EXPECT_TRUE(wolf::pred_diff_norm_zero(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_diff_norm_zero(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenDynPredIsApprox)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::MatrixXd::Random(4,4);
- B = Eigen::MatrixXd::Random(4,4);
- C = A;
-
- EXPECT_TRUE(wolf::pred_is_approx(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_is_approx(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenFixPredIsApprox)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::Matrix4d::Random();
- B = Eigen::Matrix4d::Random();
- C = A;
-
- EXPECT_TRUE(wolf::pred_is_approx(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_is_approx(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenPredQuatIsApprox)
-{
- Eigen::Quaterniond A, B, C;
-
- /// @todo which version of Eigen provides this ?
-// A = Eigen::Quaterniond::UnitRandom();
-
- A.coeffs() = Eigen::Vector4d::Random().normalized();
- B.coeffs() = Eigen::Vector4d::Random().normalized();
- C = A;
-
- EXPECT_TRUE(wolf::pred_quat_is_approx(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_quat_is_approx(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenPredQuatIsIdentity)
-{
- Eigen::Quaterniond A, B;
-
- A = Eigen::Quaterniond::Identity();
- B.coeffs() = Eigen::Vector4d::Random().normalized();
-
- EXPECT_TRUE(wolf::pred_quat_identity(A, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_quat_identity(B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenPredAngleIsApprox)
-{
- double a = M_PI;
- double b = -M_PI;
- double c = 0;
-
- EXPECT_TRUE(wolf::pred_angle_is_approx(a, b, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_angle_is_approx(a, c, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenPredAngleIsZero)
-{
- double a = 0;
- double b = M_PI;
- double c = 2.*M_PI;
-
- EXPECT_TRUE(wolf::pred_angle_zero(a, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_angle_zero(b, wolf::Constants::EPS));
- EXPECT_TRUE(wolf::pred_angle_zero(c, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_processor_fix_wing_model.cpp b/test/gtest_processor_fixed_wing_model.cpp
similarity index 94%
rename from test/gtest_processor_fix_wing_model.cpp
rename to test/gtest_processor_fixed_wing_model.cpp
index 41eaed71fd6bb47d2353bb6818349daad0aae326..b2bcb1990ccd266756c7f106478b79aadff3cc88 100644
--- a/test/gtest_processor_fix_wing_model.cpp
+++ b/test/gtest_processor_fixed_wing_model.cpp
@@ -23,12 +23,12 @@
#include "core/utils/utils_gtest.h"
#include "core/problem/problem.h"
#include "core/ceres_wrapper/solver_ceres.h"
-#include "core/processor/processor_fix_wing_model.h"
#include "core/state_block/state_quaternion.h"
// STL
#include <iterator>
#include <iostream>
+#include "../include/core/processor/processor_fixed_wing_model.h"
using namespace wolf;
using namespace Eigen;
@@ -58,11 +58,11 @@ class ProcessorFixWingModelTest : public testing::Test
2);
// Emplace processor
- ParamsProcessorFixWingModelPtr params = std::make_shared<ParamsProcessorFixWingModel>();
+ ParamsProcessorFixedWingModelPtr params = std::make_shared<ParamsProcessorFixedWingModel>();
params->velocity_local = (Vector3d() << 1, 0, 0).finished();
params->angle_stdev = 0.1;
params->min_vel_norm = 0;
- processor = ProcessorBase::emplace<ProcessorFixWingModel>(sensor, params);
+ processor = ProcessorBase::emplace<ProcessorFixedWingModel>(sensor, params);
}
FrameBasePtr emplaceFrame(TimeStamp ts, const Vector10d& x)