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/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));
}
}