diff --git a/.gitignore b/.gitignore
index 8e5e4a16cd41fac9e48dfe5181cd8f0eb0e1234d..1a57960199ca6913030d90819f96848498a900e9 100644
--- a/.gitignore
+++ b/.gitignore
@@ -42,3 +42,4 @@ Testing/
.ccls-root
.ccls-cache
compile_commands.json
+.clang-format
diff --git a/CMakeLists.txt b/CMakeLists.txt
index ea1678434df8cc25bf748e986269fefdf4d247fd..eed24e71a8fb858814517298ece26d30e58e774a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -133,8 +133,9 @@ SET(HDRS_SENSOR
include/${PROJECT_NAME}/sensor/sensor_laser_2d.h
)
SET(HDRS_STATE_BLOCK
- include/${PROJECT_NAME}/state_block/local_parametrization_polyline_extreme.h
- )
+include/${PROJECT_NAME}/state_block/local_parametrization_polyline_extreme.h
+include/${PROJECT_NAME}/state_block/state_polyline_point_2d.h
+)
# ============ SOURCES ============
SET(SRCS_CAPTURE
diff --git a/include/laser/processor/processor_loop_closure_falko.h b/include/laser/processor/processor_loop_closure_falko.h
index cf9bfb58bfa78df8ecb470c0a71972f0170ee2c5..c320e7e9ca1deb58a8b51357f2fd914a5be54594 100644
--- a/include/laser/processor/processor_loop_closure_falko.h
+++ b/include/laser/processor/processor_loop_closure_falko.h
@@ -174,7 +174,7 @@ struct ParamsProcessorLoopClosureFalko : public ParamsProcessorLoopClosure
* \tparam L Loop closure Type <LoopClosureFalkoAht> or <LoopClosureFalkoNn>
* \param _param_falko parameter struct with falko lib parameters
**/
-template <typename D, typename Extr, template <typename, typename> typename L>
+template <class D, class Extr, template <class, class> class L>
class ProcessorLoopClosureFalko : public ProcessorLoopClosure
{
public:
diff --git a/include/laser/processor/processor_loop_closure_falko_icp.h b/include/laser/processor/processor_loop_closure_falko_icp.h
index a3e41335bcebd0d73b655cbbcf7aee0846ab498b..5f195111f88a206b41e973d040a7c3651c00ad01 100644
--- a/include/laser/processor/processor_loop_closure_falko_icp.h
+++ b/include/laser/processor/processor_loop_closure_falko_icp.h
@@ -78,7 +78,7 @@ struct ParamsProcessorLoopClosureFalkoIcp : public ParamsProcessorLoopClosureFal
* \param _param_falko parameter struct with falko lib parameters
**/
-template <typename D, typename Extr, template <typename, typename> typename L>
+template <class D, class Extr, template <class, class> class L>
class ProcessorLoopClosureFalkoIcp : public ProcessorLoopClosureFalko<D, Extr, L>
{
@@ -188,7 +188,7 @@ class ProcessorLoopClosureFalkoIcp : public ProcessorLoopClosureFalko<D, Extr, L
}
};
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpNnBsc);
+WOLF_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpNnBsc);
/** \brief A class that implements the loop closure with BSC descriptors and NN matcher
**/
@@ -204,7 +204,7 @@ class ProcessorLoopClosureFalkoIcpNnBsc
~ProcessorLoopClosureFalkoIcpNnBsc() = default;
};
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpAhtBsc);
+WOLF_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpAhtBsc);
/** \brief A class that implements the loop closure with BSC descriptors and NN matcher
**/
@@ -220,7 +220,7 @@ class ProcessorLoopClosureFalkoIcpAhtBsc
~ProcessorLoopClosureFalkoIcpAhtBsc() = default;
};
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpNnCgh);
+WOLF_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpNnCgh);
/** \brief A class that implements the loop closure with Cgh descriptors and NN matcher
**/
@@ -236,7 +236,7 @@ class ProcessorLoopClosureFalkoIcpNnCgh
~ProcessorLoopClosureFalkoIcpNnCgh() = default;
};
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpAhtCgh);
+WOLF_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpAhtCgh);
/** \brief A class that implements the loop closure with Cgh descriptors and NN matcher
**/
diff --git a/include/laser/state_block/state_polyline_point_2d.h b/include/laser/state_block/state_polyline_point_2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..dd7b9eb516bdadb5536947c850b59ee1b2f5f5bd
--- /dev/null
+++ b/include/laser/state_block/state_polyline_point_2d.h
@@ -0,0 +1,49 @@
+//--------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 "laser/state_block/local_parametrization_polyline_extreme.h"
+
+#include <core/state_block/state_block_derived.h>
+
+namespace wolf
+{
+
+class StatePolylinePoint2d : public StatePoint2d
+{
+ public:
+ StatePolylinePoint2d(const Vector2d &_point,
+ StateBlockPtr _reference_point = nullptr,
+ bool _fixed = false,
+ bool _transformable = true);
+
+ ~StatePolylinePoint2d() {}
+};
+
+inline StatePolylinePoint2d::StatePolylinePoint2d(const Vector2d &_point,
+ StateBlockPtr _reference_point,
+ bool _fixed,
+ bool _transformable)
+ : StatePoint2d(_point, _fixed, _transformable)
+{
+ if (_reference_point) local_param_ptr_ = std::make_shared<LocalParametrizationPolylineExtreme>(_reference_point);
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/src/landmark/landmark_polyline_2d.cpp b/src/landmark/landmark_polyline_2d.cpp
index 7be128506e5bd3451cb5399bf68b3e9d0c22139e..1755556a42f0492a2528f83db190ee323fdaceec 100644
--- a/src/landmark/landmark_polyline_2d.cpp
+++ b/src/landmark/landmark_polyline_2d.cpp
@@ -26,12 +26,14 @@
* \author: jvallve
*/
+#include "laser/state_block/state_polyline_point_2d.h"
#include "laser/feature/feature_polyline_2d.h"
#include "laser/landmark/landmark_polyline_2d.h"
#include "laser/state_block/local_parametrization_polyline_extreme.h"
#include "laser/factor/factor_point_2d.h"
#include "laser/factor/factor_point_to_line_2d.h"
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_angle.h"
+#include "core/state_block/state_block_derived.h"
#include "core/common/factory.h"
#include "core/yaml/yaml_conversion.h"
@@ -51,7 +53,7 @@ LandmarkPolyline2d::LandmarkPolyline2d(StateBlockPtr _p_ptr, StateBlockPtr _o_pt
assert(_points.cols() >= 2 && "2 points at least needed.");
for (auto i = 0; i < _points.cols(); i++)
- point_state_map_[i+_first_id] = std::make_shared<StateBlock>(_points.col(i).head<2>());
+ point_state_map_[i+_first_id] = std::make_shared<StatePolylinePoint2d>(_points.col(i).head<2>());
if (!first_defined_)
firstStateBlock()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(std::next(point_state_map_.begin())->second));
@@ -190,24 +192,23 @@ void LandmarkPolyline2d::addPoint(const Eigen::VectorXd& _point, const bool& _de
// add new extreme
if (_back)
{
- StateBlockPtr new_sb_ptr = std::make_shared<StateBlock>(_point.head<2>(),
- false,
- (!_defined ?
- std::make_shared<LocalParametrizationPolylineExtreme>(lastStateBlock()) :
- nullptr));
- auto point_id = getLastId()+1;
- point_state_map_[point_id]= new_sb_ptr;
- HasStateBlocks::addStateBlock(idToChar(point_id),new_sb_ptr,getProblem());
+ StateBlockPtr new_sb_ptr = std::make_shared<StatePolylinePoint2d>(
+ _point.head<2>(), (_defined ? lastStateBlock() : nullptr), false, true);
+ auto point_id = getLastId() + 1;
+ point_state_map_[point_id] = new_sb_ptr;
+ HasStateBlocks::addStateBlock(idToChar(point_id), new_sb_ptr, getProblem());
last_defined_ = _defined;
}
else
{
- StateBlockPtr new_sb_ptr = std::make_shared<StateBlock>(_point.head<2>(),
- false,
- (!_defined ?
- std::make_shared<LocalParametrizationPolylineExtreme>(firstStateBlock()) :
- nullptr));
+ StateBlockPtr new_sb_ptr = std::make_shared<StatePolylinePoint2d>(
+ _point.head<2>(), (_defined ? firstStateBlock() : nullptr), false, true);
+ // StateBlockPtr new_sb_ptr = std::make_shared<StateBlock>(_point.head<2>(),
+ // false,
+ // (!_defined ?
+ // std::make_shared<LocalParametrizationPolylineExtreme>(firstStateBlock()) :
+ // nullptr));
auto point_id = getLastId()-1;
point_state_map_[point_id] = new_sb_ptr;
HasStateBlocks::addStateBlock(idToChar(point_id),new_sb_ptr,getProblem());
@@ -237,11 +238,13 @@ void LandmarkPolyline2d::addPoints(const Eigen::MatrixXd& _points, const unsigne
{
for (int i = _idx; i < _points.cols(); i++)
{
- StateBlockPtr new_sb_ptr = std::make_shared<StateBlock>(_points.block(0,i,2,1),
- false,
- (i == _points.cols()-1 && !_defined ?
- std::make_shared<LocalParametrizationPolylineExtreme>(lastStateBlock()) :
- nullptr));
+ StateBlockPtr new_sb_ptr = std::make_shared<StatePolylinePoint2d>(
+ _points.block(0,i,2,1), (i == _points.cols() - 1 && !_defined ? lastStateBlock() : nullptr), false, true);
+ // StateBlockPtr new_sb_ptr = std::make_shared<StateBlock>(_points.block(0,i,2,1),
+ // false,
+ // (i == _points.cols()-1 && !_defined ?
+ // std::make_shared<LocalParametrizationPolylineExtreme>(lastStateBlock())
+ // : nullptr));
auto point_id = getLastId()+1;
point_state_map_[point_id]= new_sb_ptr;
HasStateBlocks::addStateBlock(idToChar(point_id),new_sb_ptr,getProblem());
@@ -252,11 +255,13 @@ void LandmarkPolyline2d::addPoints(const Eigen::MatrixXd& _points, const unsigne
{
for (int i = _idx; i >= 0; i--)
{
- StateBlockPtr new_sb_ptr = std::make_shared<StateBlock>(_points.block(0,i,2,1),
- false,
- (i == 0 && !_defined ?
- std::make_shared<LocalParametrizationPolylineExtreme>(firstStateBlock()) :
- nullptr));
+ StateBlockPtr new_sb_ptr = std::make_shared<StatePolylinePoint2d>(
+ _points.block(0,i,2,1), (i == _points.cols() - 1 && !_defined ? firstStateBlock() : nullptr), false, true);
+ // StateBlockPtr new_sb_ptr = std::make_shared<StateBlock>(_points.block(0,i,2,1),
+ // false,
+ // (i == 0 && !_defined ?
+ // std::make_shared<LocalParametrizationPolylineExtreme>(firstStateBlock()) :
+ // nullptr));
auto point_id = getLastId()-1;
point_state_map_[point_id] = new_sb_ptr;
HasStateBlocks::addStateBlock(idToChar(point_id),new_sb_ptr,getProblem());
@@ -951,8 +956,8 @@ LandmarkBasePtr LandmarkPolyline2d::create(const YAML::Node& _lmk_node)
points.col(i) = _lmk_node["points"][i].as<Eigen::Vector2d>();
// Create a new landmark
- LandmarkPolyline2dPtr lmk_ptr = std::make_shared<LandmarkPolyline2d>(std::make_shared<StateBlock>(pos, pos_fixed),
- std::make_shared<StateBlock>(ori, ori_fixed),
+ LandmarkPolyline2dPtr lmk_ptr = std::make_shared<LandmarkPolyline2d>(std::make_shared<StatePoint2d>(pos, pos_fixed),
+ std::make_shared<StateAngle>(ori(0), ori_fixed),
points,
first_defined,
last_defined,
diff --git a/src/processor/processor_tracker_feature_polyline_2d.cpp b/src/processor/processor_tracker_feature_polyline_2d.cpp
index d5a6c8ce3958b1fada4636b17c136818231ffc30..7bceb852bb19fc51932e66a854e240513b22bd5b 100644
--- a/src/processor/processor_tracker_feature_polyline_2d.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2d.cpp
@@ -27,6 +27,8 @@
*/
#include "laser/processor/processor_tracker_feature_polyline_2d.h"
+#include "core/state_block/state_angle.h"
+#include "core/state_block/state_block_derived.h"
namespace wolf
{
@@ -636,8 +638,8 @@ LandmarkBasePtr ProcessorTrackerFeaturePolyline2d::emplaceLandmark(FeatureBasePt
// Create new landmark
return LandmarkBase::emplace<LandmarkPolyline2d>(getProblem()->getMap(),
- std::make_shared<StateBlock>(Eigen::Vector2d::Zero(), true),
- std::make_shared<StateBlock>(Eigen::Vector1d::Zero(), true),
+ std::make_shared<StatePoint2d>(Eigen::Vector2d::Zero(), true),
+ std::make_shared<StateAngle>(Eigen::Vector1d::Zero(), true),
points_global,
polyline_ptr->isFirstDefined(),
polyline_ptr->isLastDefined());
diff --git a/src/sensor/sensor_laser_2d.cpp b/src/sensor/sensor_laser_2d.cpp
index 55740c1a7ee57533e1b55d35e785d77f48ba89f5..9e25cf73563090b6f4ee03ecb7a00a15173fd688 100644
--- a/src/sensor/sensor_laser_2d.cpp
+++ b/src/sensor/sensor_laser_2d.cpp
@@ -21,7 +21,7 @@
//--------LICENSE_END--------
#include "laser/sensor/sensor_laser_2d.h"
-#include <core/state_block/state_block.h>
+#include <core/state_block/state_block_derived.h>
#include <core/state_block/state_angle.h>
namespace wolf {
@@ -62,7 +62,7 @@ SensorLaser2d::SensorLaser2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const P
SensorLaser2d::SensorLaser2d(const Eigen::VectorXd& _extrinsics, const ParamsSensorLaser2dPtr _params) :
SensorBase("SensorLaser2d",
- std::make_shared<StateBlock>(_extrinsics.head(2), true),
+ std::make_shared<StatePoint2d>(_extrinsics.head(2), true),
std::make_shared<StateAngle>(_extrinsics(2), true),
nullptr,
8),
diff --git a/test/gtest_landmark_polyline.cpp b/test/gtest_landmark_polyline.cpp
index 93dd5c8fc27e83ea3685b9ab4bfd2f742c9e234f..091cdd323484ee5c90d17b542eabbfe415ed7524 100644
--- a/test/gtest_landmark_polyline.cpp
+++ b/test/gtest_landmark_polyline.cpp
@@ -19,9 +19,12 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include <core/utils/utils_gtest.h>
#include "laser/landmark/landmark_polyline_2d.h"
+#include <core/utils/utils_gtest.h>
+#include "core/state_block/state_angle.h"
+#include "core/state_block/state_block_derived.h"
+
using namespace wolf;
using namespace Eigen;
@@ -47,8 +50,8 @@ class LandmarkPolylineTest : public testing::Test
//std::vector<double> object_W({2.345, 2.345, 0.9});
//std::vector<PolylineClassType> object_class({CONTAINER, SMALL_CONTAINER, PALLET});
- StateBlockPtr lmk_p_ptr = std::make_shared<StateBlock>(Vector2d::Zero());
- StateBlockPtr lmk_o_ptr = std::make_shared<StateBlock>(Vector1d::Zero());
+ StateBlockPtr lmk_p_ptr = std::make_shared<StatePoint2d>(Vector2d::Zero());
+ StateBlockPtr lmk_o_ptr = std::make_shared<StateAngle>(Vector1d::Zero());
MatrixXd points = MatrixXd::Zero(2,5);
points << 1, 1,-1,-1, 1,
@@ -91,8 +94,8 @@ class LandmarkPolylineTest : public testing::Test
classification_groundtruth.push_back(candidate_class);
// create a landmark polyline using the points
- StateBlockPtr p_ptr = std::make_shared<StateBlock>(center_pose.head<2>());
- StateBlockPtr o_ptr = std::make_shared<StateBlock>(center_pose.tail<1>());
+ StateBlockPtr p_ptr = std::make_shared<StatePoint2d>(center_pose.head<2>());
+ StateBlockPtr o_ptr = std::make_shared<StateAngle>(center_pose.tail<1>());
lmks_class_list.push_back(std::make_shared<LandmarkPolyline2d>(p_ptr, o_ptr, class_points, true, true));
}
}
diff --git a/test/gtest_processor_loop_closure_falko.cpp b/test/gtest_processor_loop_closure_falko.cpp
index 13b2338a8737c6c8ed9296a3d32ab40df531d72e..153af5af437b4a0088582a33491b9b5dce04f71a 100644
--- a/test/gtest_processor_loop_closure_falko.cpp
+++ b/test/gtest_processor_loop_closure_falko.cpp
@@ -20,17 +20,19 @@
//
//--------LICENSE_END--------
#include "laser/internal/config.h"
+#include "laser/processor/processor_loop_closure_falko.h"
+#include "laser/sensor/sensor_laser_2d.h"
+#include "data/scan_data.h"
#include "core/capture/capture_void.h"
#include "core/problem/problem.h"
#include "core/utils/utils_gtest.h"
#include "core/yaml/parser_yaml.h"
#include "core/utils/params_server.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
-#include "laser/processor/processor_loop_closure_falko.h"
-#include "laser/sensor/sensor_laser_2d.h"
-#include "data/scan_data.h"
// STL
#include <iostream>
@@ -53,7 +55,7 @@ class ProcessorLoopClosureFalkoPublic : public ProcessorLoopClosureFalkoAhtBsc
~ProcessorLoopClosureFalkoPublic() = default;
- std::shared_ptr<LoopClosureFalko<bsc, bscExtractor, aht_matcher>> getLoopClosure()
+ std::shared_ptr<LoopClosureFalkoAht<bsc, bscExtractor>> getLoopClosure()
{
return loop_closure_;
}
@@ -87,8 +89,8 @@ class ProcessorLoopClosureFalkoTest : public testing::Test
_params.angle_step_ = 0.00701248;
sensor =
SensorBase::emplace<SensorLaser2d>(problem->getHardware(),
- std::make_shared<StateBlock>(Vector2d::Zero()),
- std::make_shared<StateBlock>(Vector1d::Zero()),
+ std::make_shared<StatePoint2d>(Vector2d::Zero()),
+ std::make_shared<StateAngle>(Vector1d::Zero()),
_params);
// Emplace processor
diff --git a/test/gtest_processor_loop_closure_falko_icp.cpp b/test/gtest_processor_loop_closure_falko_icp.cpp
index a6e4baf9bf24842fe69b34385905677c32a03b61..2755d46ffa948bbd32da6740cb0836407647f4c5 100644
--- a/test/gtest_processor_loop_closure_falko_icp.cpp
+++ b/test/gtest_processor_loop_closure_falko_icp.cpp
@@ -19,18 +19,21 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "laser/internal/config.h"
-#include "core/capture/capture_void.h"
-#include "core/problem/problem.h"
-#include "core/utils/utils_gtest.h"
+#include "laser/internal/config.h"
#include "laser/processor/processor_loop_closure_falko_icp.h"
#include "laser/sensor/sensor_laser_2d.h"
+#include "data/scan_data.h"
+
+#include "core/capture/capture_void.h"
+#include "core/problem/problem.h"
+#include "core/utils/utils_gtest.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
#include "core/yaml/parser_yaml.h"
#include "core/utils/params_server.h"
-#include "data/scan_data.h"
// STL
#include <iostream>
@@ -61,8 +64,8 @@ class ProcessorLoopClosureFalkoIcpTest : public testing::Test
_params.range_min_ = 0;
_params.range_max_ = 200;
sensor =
- SensorBase::emplace<SensorLaser2d>(problem->getHardware(), std::make_shared<StateBlock>(Vector2d::Zero()),
- std::make_shared<StateBlock>(Vector1d::Zero()), _params);
+ SensorBase::emplace<SensorLaser2d>(problem->getHardware(), std::make_shared<StatePoint2d>(Vector2d::Zero()),
+ std::make_shared<StateAngle>(Vector1d::Zero()), _params);
// Emplace processor
ParserYaml parser = ParserYaml("test/yaml/params_processor_loop_closure_falko.yaml", laser_root_dir);