From 5cfc52c410ef53ef42609cfc3282705fc3642b78 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Mon, 3 Sep 2018 16:47:06 +0200 Subject: [PATCH] some little bugs fixed --- src/features/feature_diff_drive.cpp | 2 +- src/features/feature_diff_drive.h | 4 ++-- src/sensors/sensor_diff_drive.cpp | 4 ++-- src/sensors/sensor_diff_drive.h | 3 +++ 4 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/features/feature_diff_drive.cpp b/src/features/feature_diff_drive.cpp index ae1f8a2b3..c33d5fdec 100644 --- a/src/features/feature_diff_drive.cpp +++ b/src/features/feature_diff_drive.cpp @@ -13,7 +13,7 @@ FeatureDiffDrive::FeatureDiffDrive(const Eigen::VectorXs& _delta_preintegrated, // } -const Eigen::VectorXs& FeatureDiffDrive::getJacobianFactor() const +const Eigen::MatrixXs& FeatureDiffDrive::getJacobianFactor() const { return jacobian_diff_drive_factors_; } diff --git a/src/features/feature_diff_drive.h b/src/features/feature_diff_drive.h index ac13c4907..5c837469b 100644 --- a/src/features/feature_diff_drive.h +++ b/src/features/feature_diff_drive.h @@ -26,12 +26,12 @@ public: virtual ~FeatureDiffDrive() = default; - const Eigen::VectorXs& getJacobianFactor() const; + const Eigen::MatrixXs& getJacobianFactor() const; protected: Eigen::VectorXs diff_drive_factors_; - Eigen::VectorXs jacobian_diff_drive_factors_; + Eigen::MatrixXs jacobian_diff_drive_factors_; }; } /* namespace wolf */ diff --git a/src/sensors/sensor_diff_drive.cpp b/src/sensors/sensor_diff_drive.cpp index 0931b700c..4c57144c9 100644 --- a/src/sensors/sensor_diff_drive.cpp +++ b/src/sensors/sensor_diff_drive.cpp @@ -33,7 +33,7 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name, StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(2), true); StateBlockPtr ori_ptr = std::make_shared<StateBlock>(_extrinsics_po.tail(1), true); - StateBlockPtr int_ptr = std::make_shared<StateBlock>(params->factors_, true); + StateBlockPtr int_ptr = std::make_shared<StateBlock>(params->factors_, false); SensorBasePtr odo = std::make_shared<SensorDiffDrive>(pos_ptr, ori_ptr, int_ptr, params); @@ -41,7 +41,7 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name, /// @todo make calibration optional at creation //if (calibrate) - odo->unfixIntrinsics(); + // odo->unfixIntrinsics(); return odo; } diff --git a/src/sensors/sensor_diff_drive.h b/src/sensors/sensor_diff_drive.h index f4556a84a..749772902 100644 --- a/src/sensors/sensor_diff_drive.h +++ b/src/sensors/sensor_diff_drive.h @@ -14,6 +14,9 @@ namespace wolf { +WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsDiffDrive); +WOLF_STRUCT_PTR_TYPEDEFS(SensorDiffDrive); + struct IntrinsicsDiffDrive : public IntrinsicsBase { Scalar left_radius_; -- GitLab