From e756fe407442874cb6e3fa6aa8c50108743017c9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu> Date: Thu, 23 Jun 2022 10:05:42 +0200 Subject: [PATCH] Replace StateBlock by some derived types --- demos/demo_analytic_autodiff_factor.cpp | 2 +- demos/hello_wolf/landmark_point_2d.cpp | 3 ++- src/sensor/sensor_odom_2d.cpp | 6 ++++-- src/sensor/sensor_odom_3d.cpp | 6 ++++-- 4 files changed, 11 insertions(+), 6 deletions(-) diff --git a/demos/demo_analytic_autodiff_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp index 29a27f5d9..92950ca3f 100644 --- a/demos/demo_analytic_autodiff_factor.cpp +++ b/demos/demo_analytic_autodiff_factor.cpp @@ -80,7 +80,7 @@ int main(int argc, char** argv) // Wolf problem ProblemPtr wolf_problem_autodiff = new Problem(FRM_PO_2d); ProblemPtr wolf_problem_analytic = new Problem(FRM_PO_2d); - SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2); + SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StatePoint2d>(Eigen::VectorXd::Zero(2)), std::make_shared<StateAngle>(0), std::make_shared<StateParams2>(Eigen::VectorXd::Zero(2)), 2); // Ceres wrapper SolverCeres* ceres_manager_autodiff = new SolverCeres(wolf_problem_autodiff, ceres_options); diff --git a/demos/hello_wolf/landmark_point_2d.cpp b/demos/hello_wolf/landmark_point_2d.cpp index c2c11187e..2eee6e3b8 100644 --- a/demos/hello_wolf/landmark_point_2d.cpp +++ b/demos/hello_wolf/landmark_point_2d.cpp @@ -27,12 +27,13 @@ */ #include "landmark_point_2d.h" +#include "core/state_block/state_block_derived.h" namespace wolf { LandmarkPoint2d::LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy) : - LandmarkBase("LandmarkPoint2d", std::make_shared<StateBlock>(_xy)) + LandmarkBase("LandmarkPoint2d", std::make_shared<StatePoint2d>(_xy)) { setId(_id); } diff --git a/src/sensor/sensor_odom_2d.cpp b/src/sensor/sensor_odom_2d.cpp index 711545f66..9a4a9d1f1 100644 --- a/src/sensor/sensor_odom_2d.cpp +++ b/src/sensor/sensor_odom_2d.cpp @@ -20,17 +20,19 @@ // //--------LICENSE_END-------- #include "core/sensor/sensor_odom_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 { SensorOdom2d::SensorOdom2d(Eigen::VectorXd _extrinsics, const ParamsSensorOdom2d& _intrinsics) : - SensorBase("SensorOdom2d", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), nullptr, 2), + SensorBase("SensorOdom2d", std::make_shared<StatePoint2d>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), nullptr, 2), k_disp_to_disp_(_intrinsics.k_disp_to_disp), k_rot_to_rot_(_intrinsics.k_rot_to_rot) { assert(_extrinsics.size() == 3 && "Wrong extrinsics vector size! Should be 3 for 2d."); + getStateBlock('P')->setNonTransformable(); + getStateBlock('O')->setNonTransformable(); // } diff --git a/src/sensor/sensor_odom_3d.cpp b/src/sensor/sensor_odom_3d.cpp index b44561ff4..112dc72eb 100644 --- a/src/sensor/sensor_odom_3d.cpp +++ b/src/sensor/sensor_odom_3d.cpp @@ -28,14 +28,14 @@ #include "core/sensor/sensor_odom_3d.h" -#include "core/state_block/state_block.h" +#include "core/state_block/state_block_derived.h" #include "core/state_block/state_quaternion.h" #include "core/math/rotations.h" namespace wolf { SensorOdom3d::SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorOdom3d& _intrinsics) : - SensorBase("SensorOdom3d", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6), + SensorBase("SensorOdom3d", std::make_shared<StatePoint3d>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6), k_disp_to_disp_(_intrinsics.k_disp_to_disp), k_disp_to_rot_(_intrinsics.k_disp_to_rot), k_rot_to_rot_(_intrinsics.k_rot_to_rot), @@ -43,6 +43,8 @@ SensorOdom3d::SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, const ParamsSe min_rot_var_(_intrinsics.min_rot_var) { assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3d."); + getStateBlock('P')->setNonTransformable(); + getStateBlock('O')->setNonTransformable(); noise_cov_ = (Eigen::Vector6d() << min_disp_var_, min_disp_var_, min_disp_var_, min_rot_var_, min_rot_var_, min_rot_var_).finished().asDiagonal(); setNoiseCov(noise_cov_); // sets also noise_std_ -- GitLab