Skip to content
Snippets Groups Projects
Commit a4324c7b authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Use class StatePolylinePoint2d

parent 6b0a0fc8
No related branches found
No related tags found
2 merge requests!42devel->main,!40Resolve "follow core 465 Migrate from StateBlock to StateDerived"
Hide whitespace changes
Inline Side-by-side
src/landmark/landmark_polyline_2d.cpp
+ 32
− 27
View file @ a4324c7b
......@@ -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,
......
src/processor/processor_tracker_feature_polyline_2d.cpp
+ 4
− 2
View file @ a4324c7b
......@@ -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());
......
test/gtest_landmark_polyline.cpp
+ 8
− 5
View file @ a4324c7b
......@@ -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));
}
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment