diff --git a/demos/demo_analytic_autodiff_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp index 29a27f5d9c1662a6e6ab7bd4c9d7c02515476c4e..92950ca3f6c16ceedb8a352aa51a3d2f3aaaafd0 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 c2c11187eedd016c49116f84ee14141067d7fa47..2eee6e3b8d43638a010b7959d700b7d1b4fa05a0 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 711545f66b48b99fcfaf891954db3ddba7b56d7a..9a4a9d1f1f41323e3209da09afcf968ef3dc69b4 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 b44561ff402bef4971b211cfa2eae43724457fc3..112dc72eb98a50dfdf9f34bbceba7d3994078758 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_