diff --git a/include/laser/processor/processor_odom_icp.h b/include/laser/processor/processor_odom_icp.h
index 68b2a5f3d191dbffeec539bc4e6fd6ed154cb608..a323fe5429f24c8f567ebb863e3a6d4e8b4ded34 100644
--- a/include/laser/processor/processor_odom_icp.h
+++ b/include/laser/processor/processor_odom_icp.h
@@ -43,7 +43,7 @@ class ProcessorOdomIcp : public ProcessorTracker, public MotionProvider
// Useful sensor stuff
SensorLaser2dPtr sensor_laser_; // casted pointer to parent
laserscanutils::LaserScanParams laser_scan_params_;
- SizeEigen dim_; // dimension of the problem (2 or 3)
+ unsigned int dim_; // dimension of the problem (2 or 3)
// temporary and carry-on transformations
laserscanutils::icpOutput trf_origin_last_;
diff --git a/schema/processor/ProcessorOdomIcp.schema b/schema/processor/ProcessorOdomIcp.schema
index 70ebdb448788f979668dae829ece7331980be573..7981c597013b626f1acd4f5dd0603e58b9633ac9 100644
--- a/schema/processor/ProcessorOdomIcp.schema
+++ b/schema/processor/ProcessorOdomIcp.schema
@@ -2,6 +2,12 @@ follow: ProcessorTracker.schema
follow: MotionProvider.schema
+dimension:
+ _mandatory: true
+ _type: unsigned int
+ _options: [2, 3]
+ _doc: "The dimension of the problem representation: 2D or 3D"
+
keyframe_vote:
dist_traveled:
_mandatory: true
diff --git a/src/processor/processor_loop_closure_falko.cpp b/src/processor/processor_loop_closure_falko.cpp
index 8f1dbfe8ddfef7f0e0c16803077540b5bee3280b..57aa8dcd831edaf0bf45954882ad16a5eae153db 100644
--- a/src/processor/processor_loop_closure_falko.cpp
+++ b/src/processor/processor_loop_closure_falko.cpp
@@ -22,7 +22,7 @@
namespace wolf
{
ProcessorLoopClosureFalko::ProcessorLoopClosureFalko(const YAML::Node& _params)
- : ProcessorLoopClosure("ProcessorLoopClosureFalko", 2, _params)
+ : ProcessorLoopClosure("ProcessorLoopClosureFalko", {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, _params)
{
// PARAMS
min_keypoints_ = _params["min_keypoints"].as<int>();
diff --git a/src/processor/processor_loop_closure_icp.cpp b/src/processor/processor_loop_closure_icp.cpp
index f854b5c5d4e7efb3767de02e4e98c480ea265df0..c2f3da0d2752014fcb860878f5306c535feb4936 100644
--- a/src/processor/processor_loop_closure_icp.cpp
+++ b/src/processor/processor_loop_closure_icp.cpp
@@ -28,7 +28,7 @@ using namespace Eigen;
namespace wolf
{
ProcessorLoopClosureIcp::ProcessorLoopClosureIcp(const YAML::Node& _params)
- : ProcessorLoopClosure("ProcessorLoopClosureIcp", 2, _params), last_loop_closure_(nullptr)
+ : ProcessorLoopClosure("ProcessorLoopClosureIcp", {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, _params), last_loop_closure_(nullptr)
{
icp_params_ = getIcpParams(_params["icp"]);
recent_frames_ignored_ = _params["recent_frames_ignored"].as<int>();
diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index 8593d11c275b568113b18eb7d914cae43525075f..a336bc663f2a24598f088c1a3e1c9ccc973fabcf 100644
--- a/src/processor/processor_odom_icp.cpp
+++ b/src/processor/processor_odom_icp.cpp
@@ -39,9 +39,13 @@ using namespace Eigen;
namespace wolf
{
ProcessorOdomIcp::ProcessorOdomIcp(const YAML::Node& _params)
- : ProcessorTracker("ProcessorOdomIcp", "PO", 0, _params),
- MotionProvider({}, _params), // uninitialized types
- dim_(0), // uninitialized
+ : ProcessorTracker("ProcessorOdomIcp",
+ {{'P', _params["dimension"].as<unsigned int>() == 2 ? "StatePoint2d" : "StatePoint3d"},
+ {'O', _params["dimension"].as<unsigned int>() == 2 ? "StateAngle" : "StateQuaternion"}},
+ _params),
+ MotionProvider({{'P', _params["dimension"].as<unsigned int>() == 2 ? "StatePoint2d" : "StatePoint3d"},
+ {'O', _params["dimension"].as<unsigned int>() == 2 ? "StateAngle" : "StateQuaternion"}},
+ _params), // uninitialized types
odom_origin_(VectorXd::Zero(3)),
odom_last_(VectorXd::Zero(3)),
odom_incoming_(VectorXd::Zero(3)),
@@ -58,6 +62,7 @@ ProcessorOdomIcp::ProcessorOdomIcp(const YAML::Node& _params)
trf_last_incoming_.res_covar = Matrix3d::Identity();
// PARAMS
+ dim_ = _params["dimension"].as<unsigned int>();
icp_params_ = getIcpParams(_params["icp"]);
initial_guess_ = _params["initial_guess"].as<std::string>();
dist_th_ = _params["keyframe_vote"]["dist_traveled"].as<double>();
@@ -117,17 +122,6 @@ void ProcessorOdomIcp::configure(SensorBasePtr _sensor)
ro_T_so_ = laser::trf2isometry(sensor_laser_->getP()->getState(), sensor_laser_->getO()->getState());
rl_T_sl_ = ro_T_so_;
}
-
- // initialize problem dimension
- if (_sensor->getProblem())
- dim_ = _sensor->getProblem()->getDim();
- else
- throw std::runtime_error(
- "ProcessorOdomIcp::configure: SensorLaser2d not linked to a problem. Impossible to find out dimension.");
-
- // Motion provider state_types
- MotionProvider::setStateTypes(dim_ == 2 ? TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}}
- : TypeComposite{{'P', "StatePoint3d"}, {'O', "StateQuaternion"}});
}
void ProcessorOdomIcp::updateExtrinsicsIsometries()
@@ -469,31 +463,31 @@ FactorBasePtr ProcessorOdomIcp::emplaceFactor(FeatureBasePtr _feature)
VectorComposite ProcessorOdomIcp::getState(StateKeys _keys) const
{
+ // We do not have any info of where to find a valid state
+ // Further checking here for origin_ptr is redundant: if last=null, then origin=null too.
+ if (origin_ptr_ == nullptr or origin_ptr_->isRemoving() or last_ptr_ == nullptr)
+ {
+ WOLF_WARN("Processor has no state. Returning an empty VectorComposite");
+ return VectorComposite(); // return empty state
+ }
+
// empty _keys means all keys
if (_keys.empty())
- _keys = state_types_.getKeys();
+ _keys = getStateKeys();
else
{
- if (not state_types_.has(_keys))
+ if (not getStateTypes().has(_keys))
{
StateKeys keys_available;
for (auto key : _keys)
{
- if (state_types_.has(key)) keys_available += key;
+ if (getStateTypes().has(key)) keys_available += key;
}
+ WOLF_DEBUG("Processor has no all keys asked (", _keys, "). Returning the available ones: ", keys_available);
_keys = keys_available;
- WOLF_DEBUG("Processor has no all keys asked (", _keys, "). Returning the available ones: ", _keys);
}
}
- // We do not have any info of where to find a valid state
- // Further checking here for origin_ptr is redundant: if last=null, then origin=null too.
- if (origin_ptr_ == nullptr or origin_ptr_->isRemoving() or last_ptr_ == nullptr)
- {
- WOLF_WARN("Processor has no state. Returning an empty VectorComposite");
- return VectorComposite(); // return empty state
- }
-
// NOTE: we cannot update extrinsics since the function is const
// origin-last transf.
diff --git a/src/processor/processor_tracker_feature_polyline_2d.cpp b/src/processor/processor_tracker_feature_polyline_2d.cpp
index d90c83383412bc374d389738aeac8a27497f63fc..ac0c0be34e86a4a3922f230e9a050e204129e626 100644
--- a/src/processor/processor_tracker_feature_polyline_2d.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2d.cpp
@@ -24,7 +24,7 @@
namespace wolf
{
ProcessorTrackerFeaturePolyline2d::ProcessorTrackerFeaturePolyline2d(const YAML::Node& _params)
- : ProcessorTrackerFeature("ProcessorTrackerFeature", "PO", 3, _params),
+ : ProcessorTrackerFeature("ProcessorTrackerFeature", {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, _params),
line_finder_(getLineFinderIterativeParams(_params["line_finder"])),
extrinsics_transformation_computed_(false)
{
@@ -1394,8 +1394,8 @@ bool ProcessorTrackerFeaturePolyline2d::tryUpdateMatchTransformation(LandmarkMat
void ProcessorTrackerFeaturePolyline2d::computeTransformations()
{
// WOLF_DEBUG("ProcessorTrackerFeaturePolyline::computeTransformations: ");
- const auto& vehicle_pose_incoming = getProblem()->getState(incoming_ptr_->getTimeStamp()).vector("PO");
- const auto& vehicle_pose_last = getProblem()->getState(last_ptr_->getTimeStamp()).vector("PO");
+ const auto& vehicle_pose_incoming = getProblem()->getState(incoming_ptr_->getTimeStamp(), "PO").vector("PO");
+ const auto& vehicle_pose_last = getProblem()->getState(last_ptr_->getTimeStamp(), "PO").vector("PO");
// world_robot
Eigen::Matrix2d R_world_robot_incoming = Eigen::Rotation2Dd(vehicle_pose_incoming(2)).matrix();
diff --git a/test/gtest_processor_odom_icp.cpp b/test/gtest_processor_odom_icp.cpp
index d2218ab1e942ff7b5986da272bb5bdcb2687dc81..5eb6119f931856d58d03bb9059de318ff1d6764f 100644
--- a/test/gtest_processor_odom_icp.cpp
+++ b/test/gtest_processor_odom_icp.cpp
@@ -74,7 +74,7 @@ class ProcessorOdomIcp_Test : public testing::Test
TEST(ProcessorOdomIcp, Constructor_file)
{
auto prc = FactoryProcessorYaml::create(
- "ProcessorOdomIcp", laser_dir + "/test/yaml/processor_odom_icp.yaml", {laser_dir, wolf_schema_dir});
+ "ProcessorOdomIcp", laser_dir + "/test/yaml/processor_odom_icp_2d.yaml", {laser_dir, wolf_schema_dir});
ASSERT_TRUE(prc); // not nullptr
ASSERT_EQ(prc->getType(), "ProcessorOdomIcp");
@@ -82,7 +82,7 @@ TEST(ProcessorOdomIcp, Constructor_file)
TEST(ProcessorOdomIcp, creator_node)
{
- yaml_schema_cpp::YamlServer server({laser_dir, wolf_schema_dir}, laser_dir + "/test/yaml/processor_odom_icp.yaml");
+ yaml_schema_cpp::YamlServer server({laser_dir, wolf_schema_dir}, laser_dir + "/test/yaml/processor_odom_icp_2d.yaml");
auto node = server.getNode();
auto prc = FactoryProcessorNode::create("ProcessorOdomIcp", server.getNode(), {laser_dir, wolf_schema_dir});
@@ -93,7 +93,7 @@ TEST(ProcessorOdomIcp, creator_node)
TEST_F(ProcessorOdomIcp_Test, zero_motion_2d)
{
- init(2, laser_pose_offset_2d, laser_dir + "/test/yaml/processor_odom_icp_vote_all.yaml");
+ init(2, laser_pose_offset_2d, laser_dir + "/test/yaml/processor_odom_icp_vote_all_2d.yaml");
TimeStamp t(t0);
int N = 7;
@@ -146,7 +146,7 @@ TEST_F(ProcessorOdomIcp_Test, zero_motion_2d)
TEST_F(ProcessorOdomIcp_Test, motion_2d)
{
- init(2, laser_pose_offset_2d, laser_dir + "/test/yaml/processor_odom_icp.yaml");
+ init(2, laser_pose_offset_2d, laser_dir + "/test/yaml/processor_odom_icp_2d.yaml");
TimeStamp t(t0);
// squared infinite shaped motion
@@ -254,7 +254,7 @@ TEST_F(ProcessorOdomIcp_Test, motion_2d)
// 3D
TEST_F(ProcessorOdomIcp_Test, setup_sloped)
{
- init(3, (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(), laser_dir + "/test/yaml/processor_odom_icp.yaml");
+ init(3, (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(), laser_dir + "/test/yaml/processor_odom_icp_3d.yaml");
}
TEST_F(ProcessorOdomIcp_Test, motion_3d)
@@ -313,7 +313,7 @@ TEST_F(ProcessorOdomIcp_Test, motion_3d)
traj_3d_file.close();
// init problem
- init(3, gt_poses_3d.row(0), laser_dir + "/test/yaml/processor_odom_icp.yaml");
+ init(3, gt_poses_3d.row(0), laser_dir + "/test/yaml/processor_odom_icp_3d.yaml");
TimeStamp t(t0);
for (int i = 0; i < gt_poses_3d.rows(); i++)
diff --git a/test/yaml/processor_odom_icp.yaml b/test/yaml/processor_odom_icp_2d.yaml
similarity index 95%
rename from test/yaml/processor_odom_icp.yaml
rename to test/yaml/processor_odom_icp_2d.yaml
index f94f821b575ef5cd32abd5d0cded3b333df382e9..ef3db71e3a8e3969b1c762100b5b3840c4acc6e4 100644
--- a/test/yaml/processor_odom_icp.yaml
+++ b/test/yaml/processor_odom_icp_2d.yaml
@@ -14,6 +14,7 @@ state_provider : true
state_provider_order : 1
# from processor odom ICP
+dimension : 2
keyframe_vote:
dist_traveled : 1e9
angle_turned : 1e9
diff --git a/test/yaml/processor_odom_icp_3d.yaml b/test/yaml/processor_odom_icp_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..7915997a54c22d3a3718fe5dca4746112454170e
--- /dev/null
+++ b/test/yaml/processor_odom_icp_3d.yaml
@@ -0,0 +1,29 @@
+type: ProcessorOdomIcp
+plugin: laser
+name: cool_processor_icp
+sensor_name: cool_laser
+
+# from processor base
+time_tolerance : 0.04
+apply_loss_function : false
+keyframe_vote:
+ voting_active : true
+
+# from motion provider
+state_provider : true
+state_provider_order : 1
+
+# from processor odom ICP
+dimension : 3
+keyframe_vote:
+ dist_traveled : 1e9
+ angle_turned : 1e9
+ time_span : 1.05
+ invalid_icp : false
+ consecutive_invalid_icp_patience : 1.0
+ consecutive_invalid_icp_timeout : 5.0
+
+initial_guess : "zero"
+
+icp:
+ follow: ./icp.yaml
\ No newline at end of file
diff --git a/test/yaml/processor_odom_icp_vote_all.yaml b/test/yaml/processor_odom_icp_vote_all_2d.yaml
similarity index 95%
rename from test/yaml/processor_odom_icp_vote_all.yaml
rename to test/yaml/processor_odom_icp_vote_all_2d.yaml
index 35c04cf4af2d4321b61808476a0983242f55448e..23a7b9b503a1b1120b12f05d64d5f135cb4afb7d 100644
--- a/test/yaml/processor_odom_icp_vote_all.yaml
+++ b/test/yaml/processor_odom_icp_vote_all_2d.yaml
@@ -14,6 +14,7 @@ state_provider : true
state_provider_order : 1
# from processor odom ICP
+dimension : 2
keyframe_vote:
dist_traveled : 0
angle_turned : 0