diff --git a/.gitignore b/.gitignore
index 6b43f90c185cd290052c10a6a508d8f1998a895c..1a57960199ca6913030d90819f96848498a900e9 100644
--- a/.gitignore
+++ b/.gitignore
@@ -35,7 +35,6 @@ src/examples/map_apriltag_save.yaml
\.vscode/
build_release/
-laser.found
.clangd/
Testing/
@@ -43,3 +42,4 @@ Testing/
.ccls-root
.ccls-cache
compile_commands.json
+.clang-format
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 0aabb6a729351c494488494dbfb8d40687eddabf..89daf4975b7398128574c20dfad4125391bee17e 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -151,7 +151,7 @@ stages:
- cd build
- cmake -DCMAKE_BUILD_TYPE=release -DBUILD_TESTS=ON ..
- make -j$(nproc)
- - ctest -j$(nproc)
+ - ctest -j$(nproc) --output-on-failure
- make install
############ LICENSE HEADERS ############
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8687647283fcf148c50d38ba5de14afa9e3efb8f..19ff2105a0d8891aa3dec67ba614fbc42b739232 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -13,7 +13,7 @@ MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...")
# Paths
SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
-set(INCLUDE_INSTALL_DIR include/iri-algorithms/wolf)
+set(INCLUDE_INSTALL_DIR include/wolf)
set(LIB_INSTALL_DIR lib/)
IF (NOT CMAKE_BUILD_TYPE)
@@ -30,7 +30,7 @@ include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
if(COMPILER_SUPPORTS_CXX14)
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++14 support.")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
+ set(CMAKE_CXX_STANDARD 14)
else()
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
endif()
@@ -101,43 +101,41 @@ IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
message(FATAL_ERROR "Bug: Specified CONFIG_DIR: "
"${WOLF_CONFIG_DIR} exists, but is not a directory.")
ENDIF()
+
# Configure config.h
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
-message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
-include_directories("${PROJECT_BINARY_DIR}/conf")
-# ============ INCLUDES ============
-INCLUDE_DIRECTORIES(BEFORE "include")
# ============ HEADERS ============
SET(HDRS_CAPTURE
- include/laser/capture/capture_laser_2d.h
+ include/${PROJECT_NAME}/capture/capture_laser_2d.h
)
SET(HDRS_FACTOR
- include/laser/factor/factor_point_2d.h
- include/laser/factor/factor_point_to_line_2d.h
+ include/${PROJECT_NAME}/factor/factor_point_2d.h
+ include/${PROJECT_NAME}/factor/factor_point_to_line_2d.h
)
SET(HDRS_FEATURE
- include/laser/feature/feature_polyline_2d.h
- include/laser/feature/feature_match_polyline_2d.h
+ include/${PROJECT_NAME}/feature/feature_polyline_2d.h
+ include/${PROJECT_NAME}/feature/feature_match_polyline_2d.h
)
SET(HDRS_LANDMARK
- include/laser/landmark/landmark_polyline_2d.h
- include/laser/landmark/landmark_match_polyline_2d.h
+ include/${PROJECT_NAME}/landmark/landmark_polyline_2d.h
+ include/${PROJECT_NAME}/landmark/landmark_match_polyline_2d.h
)
SET(HDRS_MATH
- include/laser/math/laser_tools.h
+ include/${PROJECT_NAME}/math/laser_tools.h
)
SET(HDRS_PROCESSOR
- include/laser/processor/polyline_2d_utils.h
- include/laser/processor/processor_tracker_feature_polyline_2d.h
+ include/${PROJECT_NAME}/processor/polyline_2d_utils.h
+ include/${PROJECT_NAME}/processor/processor_tracker_feature_polyline_2d.h
)
SET(HDRS_SENSOR
- include/laser/sensor/sensor_laser_2d.h
+ include/${PROJECT_NAME}/sensor/sensor_laser_2d.h
)
SET(HDRS_STATE_BLOCK
- include/laser/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
@@ -170,13 +168,13 @@ SET(SRCS_YAML
if (falkolib_FOUND)
message("Found Falkolib. Compiling some extra classes.")
SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
- include/laser/processor/processor_loop_closure_falko.h
+ include/${PROJECT_NAME}/processor/processor_loop_closure_falko.h
)
SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
src/processor/processor_loop_closure_falko.cpp
)
SET(HDRS_FEATURE ${HDRS_FEATURE}
- include/laser/feature/feature_scene_falko.h
+ include/${PROJECT_NAME}/feature/feature_scene_falko.h
)
SET(SRCS_FEATURE ${SRCS_FEATURE}
src/feature/feature_scene_falko.cpp
@@ -184,7 +182,7 @@ if (falkolib_FOUND)
# falko & CSM
if (csm_FOUND)
SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
- include/laser/processor/processor_loop_closure_falko_icp.h
+ include/${PROJECT_NAME}/processor/processor_loop_closure_falko_icp.h
)
SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
src/processor/processor_loop_closure_falko_icp.cpp
@@ -196,12 +194,12 @@ endif()
if(csm_FOUND)
message("Found CSM. Compiling some extra classes.")
SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
- include/laser/processor/params_icp.h
- include/laser/processor/processor_loop_closure_icp.h
- include/laser/processor/processor_odom_icp.h
+ include/${PROJECT_NAME}/processor/params_icp.h
+ include/${PROJECT_NAME}/processor/processor_loop_closure_icp.h
+ include/${PROJECT_NAME}/processor/processor_odom_icp.h
)
SET(HDRS_FEATURE ${HDRS_FEATURE}
- include/laser/feature/feature_icp_align.h
+ include/${PROJECT_NAME}/feature/feature_icp_align.h
)
SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
src/processor/processor_loop_closure_icp.cpp
@@ -280,9 +278,9 @@ install(
${LIB_INSTALL_DIR}/${PLUGIN_NAME}/cmake
)
-# Specifies include directories to use when compiling the plugin target
-# This way, include_directories does not need to be called in plugins depending on this one
-target_include_directories(${PLUGIN_NAME} INTERFACE
+target_include_directories(${PLUGIN_NAME} PUBLIC
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+ $<BUILD_INTERFACE:${PROJECT_BINARY_DIR}/conf>
$<INSTALL_INTERFACE:${INCLUDE_INSTALL_DIR}>
)
@@ -304,14 +302,9 @@ INSTALL(FILES ${HDRS_SENSOR}
INSTALL(FILES ${HDRS_STATE_BLOCK}
DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/state_block)
-FILE(WRITE laser.found "")
-INSTALL(FILES laser.found
- DESTINATION ${INCLUDE_INSTALL_DIR})
INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/internal)
-INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
-
export(PACKAGE ${PLUGIN_NAME})
FIND_PACKAGE(Doxygen MODULE)
diff --git a/demos/demo_2_lasers_offline.cpp b/demos/demo_2_lasers_offline.cpp
index 8ec341798b6dc098dee0b298983d36f1e8bbb87e..dcaf0da1d28e1a681128e939fcc5423cdc4e89e9 100644
--- a/demos/demo_2_lasers_offline.cpp
+++ b/demos/demo_2_lasers_offline.cpp
@@ -31,8 +31,8 @@
#include <queue>
// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
//Ceres includes
#include "glog/logging.h"
diff --git a/demos/demo_ceres_2_lasers.cpp b/demos/demo_ceres_2_lasers.cpp
index 05ec4631f9c20f4b0a3a0bc93400272d781c00db..9f2cced24f1d952b41343c2358947fe2fe6fba48 100644
--- a/demos/demo_ceres_2_lasers.cpp
+++ b/demos/demo_ceres_2_lasers.cpp
@@ -31,8 +31,8 @@
#include <queue>
// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
//Ceres includes
#include "glog/logging.h"
diff --git a/demos/demo_ceres_2_lasers_polylines.cpp b/demos/demo_ceres_2_lasers_polylines.cpp
index 8dad5d18bace977ea250ec0664a8693b93620ffc..0632ad90a0562b258c4e20a93a17c7e019a8464c 100644
--- a/demos/demo_ceres_2_lasers_polylines.cpp
+++ b/demos/demo_ceres_2_lasers_polylines.cpp
@@ -31,8 +31,8 @@
#include <queue>
// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
//Ceres includes
#include "glog/logging.h"
diff --git a/include/laser/processor/params_icp.h b/include/laser/processor/params_icp.h
index 9c57708c337730b005b57cf4bcd3c94df75edc19..fe3e1ed7c92efdcf5a4f66ce8418a8c7afa34e1d 100644
--- a/include/laser/processor/params_icp.h
+++ b/include/laser/processor/params_icp.h
@@ -22,9 +22,11 @@
#ifndef INCLUDE_LASER_PROCESSOR_PARAMS_ICP_H_
#define INCLUDE_LASER_PROCESSOR_PARAMS_ICP_H_
-#include "core/common/wolf.h"
#include "laser_scan_utils/icp.h"
+#include <core/common/wolf.h>
+#include <core/utils/params_server.h>
+
namespace wolf {
WOLF_PTR_TYPEDEFS(ProcessorOdomIcp);
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/processor/processor_odom_icp.h b/include/laser/processor/processor_odom_icp.h
index 9ef7904912e713aebad83e338fcca6bd9cce2e7f..516f8c2efda39777a672a5eb8ac4560bcb620500 100644
--- a/include/laser/processor/processor_odom_icp.h
+++ b/include/laser/processor/processor_odom_icp.h
@@ -25,11 +25,12 @@
/**************************
* WOLF includes *
**************************/
+#include "laser/processor/params_icp.h"
+#include "laser/sensor/sensor_laser_2d.h"
+
#include "core/common/wolf.h"
#include "core/processor/processor_tracker.h"
#include "core/processor/motion_provider.h"
-#include "laser/processor/params_icp.h"
-#include "laser/sensor/sensor_laser_2d.h"
/**************************
* ICP includes *
@@ -122,12 +123,44 @@ class ProcessorOdomIcp : public ProcessorTracker, public MotionProvider
protected:
void preProcess() override;
+ /** \brief Compute transform from origin to incoming.
+ *
+ * The transform is the result of aligning the scans in origin and incoming.
+ * This alignement takes as prior the current transform from origin to last.
+ *
+ * \return the number of features tracked. This is always zero since this processor is not tracking features.
+ */
unsigned int processKnown() override;
+ /** \brief Compute transform from last to incoming.
+ *
+ * The transform is the result of aligning the scans in last and incoming.
+ * This alignement takes as prior the identity transform.
+ *
+ * \param _max_features number of features to extract. This is ignored since this processor is not extracting features.
+ *
+ * \return the number of features extracted. This is always zero since this processor is not extracting features.
+ */
unsigned int processNew(const int& _max_features) override;
bool voteForKeyFrame() const override;
+ /** \brief advance pointers and update internal values
+ *
+ * We do several things:
+ * - Advance derived pointers
+ * - Advance isometries
+ * - Update extrinsic isometries in case they have changed
+ * - Compute odometry
+ */
void advanceDerived() override;
+ /** \brief advance pointers and update internal values
+ *
+ * We do several things:
+ * - Reset derived pointers
+ * - Reset isometries
+ * - Update extrinsic isometries in case they have changed
+ * - Compute odometry
+ */
void resetDerived() override;
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_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index aa4bbd55eb9e30eee6606fa94ace0d0c8ba68e30..723a198faa4156e39758ec20b080dc944f2b88b7 100644
--- a/src/processor/processor_odom_icp.cpp
+++ b/src/processor/processor_odom_icp.cpp
@@ -21,14 +21,15 @@
//--------LICENSE_END--------
#include "laser/processor/processor_odom_icp.h"
#include "laser/math/laser_tools.h"
+#include "laser/capture/capture_laser_2d.h"
+#include "laser/feature/feature_icp_align.h"
+
#include "core/math/covariance.h"
#include "core/math/rotations.h"
#include "core/math/SE3.h"
#include "core/math/SE2.h"
-#include "laser/capture/capture_laser_2d.h"
#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
#include "core/factor/factor_relative_pose_3d.h"
-#include "laser/feature/feature_icp_align.h"
using namespace laserscanutils;
namespace wolf
@@ -257,9 +258,9 @@ void ProcessorOdomIcp::advanceDerived()
odom_last_ = odom_incoming_;
- // init odometry_
- if (odometry_.empty())
- odometry_ = getProblem()->stateZero("PO");
+ // init odometry
+ if (getOdometry().empty())
+ setOdometry(getProblem()->stateZero("PO"));
// update extrinsics (if necessary)
updateExtrinsicsIsometries();
@@ -270,12 +271,13 @@ void ProcessorOdomIcp::advanceDerived()
Isometry2d rl_T_ri = rl_T_sl_ * so_T_sl.inverse() * so_T_si * rl_T_sl_.inverse();
// 2D
+ auto odometry = getOdometry();
if (getProblem()->getDim() == 2)
{
- auto m_T_rl = laser::trf2isometry(odometry_['P'], odometry_['O']);
+ auto m_T_rl = laser::trf2isometry(odometry['P'], odometry['O']);
auto m_T_ri = m_T_rl * rl_T_ri;
- odometry_['P'] = m_T_ri.translation();
- odometry_['O'] = Vector1d(Rotation2Dd(m_T_ri.rotation()).angle());
+ odometry['P'] = m_T_ri.translation();
+ odometry['O'] = Vector1d(Rotation2Dd(m_T_ri.rotation()).angle());
}
// 3D
else
@@ -284,10 +286,11 @@ void ProcessorOdomIcp::advanceDerived()
convert2dTo3d(rl_T_ri, rl_T3d_ri);
- SE3::compose(odometry_['P'], odometry_['O'],
+ SE3::compose(odometry['P'], odometry['O'],
rl_T3d_ri.head<3>(), rl_T3d_ri.tail<4>(),
- odometry_['P'], odometry_['O']);
+ odometry['P'], odometry['O']);
}
+ setOdometry(odometry);
// advance transforms
trf_origin_last_ = trf_origin_incoming_;
@@ -298,24 +301,25 @@ void ProcessorOdomIcp::resetDerived()
// Using processing_step_ to ensure that origin, last and incoming are different
if (processing_step_ != FIRST_TIME_WITH_KEYFRAME && processing_step_ != FIRST_TIME_WITHOUT_KEYFRAME)
{
- // init odometry_
- if (odometry_.empty())
- odometry_ = getProblem()->stateZero("PO");
+ // init odometry
+ if (getOdometry().empty())
+ setOdometry(getProblem()->stateZero("PO"));
// update extrinsics (if necessary)
updateExtrinsicsIsometries();
// computing odometry
+ auto odometry = getOdometry();
auto so_T_si = laser::trf2isometry(trf_origin_incoming_.res_transf);
auto so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
Isometry2d rl_T_ri = rl_T_sl_ * so_T_sl.inverse() * so_T_si * rl_T_sl_.inverse();
// 2D
if (getProblem()->getDim() == 2)
{
- auto m_T_rl = laser::trf2isometry(odometry_['P'], odometry_['O']);
+ auto m_T_rl = laser::trf2isometry(odometry['P'], odometry['O']);
auto m_T_ri = m_T_rl * rl_T_ri;
- odometry_['P'] = m_T_ri.translation();
- odometry_['O'] = Vector1d(Rotation2Dd(m_T_ri.rotation()).angle());
+ odometry['P'] = m_T_ri.translation();
+ odometry['O'] = Vector1d(Rotation2Dd(m_T_ri.rotation()).angle());
}
// 3D
else
@@ -324,10 +328,11 @@ void ProcessorOdomIcp::resetDerived()
convert2dTo3d(rl_T_ri, rl_T3d_ri);
- SE3::compose(odometry_['P'], odometry_['O'],
+ SE3::compose(odometry['P'], odometry['O'],
rl_T3d_ri.head<3>(), rl_T3d_ri.tail<4>(),
- odometry_['P'], odometry_['O']);
+ odometry['P'], odometry['O']);
}
+ setOdometry(odometry);
// advance transforms
trf_origin_last_ = trf_last_incoming_;
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);