From f868a9c18be6e4b49735ebaa8caf42ce033e2c0d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sat, 11 Apr 2020 23:35:38 +0200
Subject: [PATCH] WIP Fixing odometers
---
include/core/capture/capture_diff_drive.h | 2 +-
include/core/capture/capture_odom_2d.h | 8 ++--
include/core/capture/capture_odom_3d.h | 2 +-
.../processor/processor_tracker_feature.h | 8 ++--
include/core/state_block/state_composite.h | 3 ++
src/capture/capture_diff_drive.cpp | 8 ++--
src/capture/capture_motion.cpp | 24 +++++------
src/capture/capture_odom_3d.cpp | 8 ++--
src/state_block/state_composite.cpp | 41 ++++++++++++++++++-
9 files changed, 73 insertions(+), 31 deletions(-)
diff --git a/include/core/capture/capture_diff_drive.h b/include/core/capture/capture_diff_drive.h
index 2300a65b2..f35be53d1 100644
--- a/include/core/capture/capture_diff_drive.h
+++ b/include/core/capture/capture_diff_drive.h
@@ -39,7 +39,7 @@ public:
virtual ~CaptureDiffDrive() = default;
- virtual Eigen::VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const override;
+ virtual VectorComposite correctDelta(const VectorComposite& _delta, const VectorComposite& _delta_error) const override;
};
diff --git a/include/core/capture/capture_odom_2d.h b/include/core/capture/capture_odom_2d.h
index d18217279..9b4358317 100644
--- a/include/core/capture/capture_odom_2d.h
+++ b/include/core/capture/capture_odom_2d.h
@@ -33,14 +33,14 @@ class CaptureOdom2d : public CaptureMotion
virtual ~CaptureOdom2d();
- virtual VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const override;
+ virtual VectorComposite correctDelta(const VectorComposite& _delta, const VectorComposite& _delta_error) const override;
};
-inline Eigen::VectorXd CaptureOdom2d::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const
+inline VectorComposite CaptureOdom2d::correctDelta(const VectorComposite& _delta, const VectorComposite& _delta_error) const
{
- Vector3d delta = _delta + _delta_error;
- delta(2) = pi2pi(delta(2));
+ VectorComposite delta = _delta + _delta_error;
+ delta.at("O")(0) = pi2pi(delta.at("O")(0));
return delta;
}
diff --git a/include/core/capture/capture_odom_3d.h b/include/core/capture/capture_odom_3d.h
index 7e4ee02c8..bef792936 100644
--- a/include/core/capture/capture_odom_3d.h
+++ b/include/core/capture/capture_odom_3d.h
@@ -33,7 +33,7 @@ class CaptureOdom3d : public CaptureMotion
virtual ~CaptureOdom3d();
- virtual VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const override;
+ virtual VectorComposite correctDelta(const VectorComposite& _delta, const VectorComposite& _delta_error) const override;
};
diff --git a/include/core/processor/processor_tracker_feature.h b/include/core/processor/processor_tracker_feature.h
index ff5ebb2a6..be2545de4 100644
--- a/include/core/processor/processor_tracker_feature.h
+++ b/include/core/processor/processor_tracker_feature.h
@@ -111,7 +111,7 @@ class ProcessorTrackerFeature : public ProcessorTracker
* - Create the factors, of the correct type, derived from FactorBase
* (through FactorAnalytic or FactorSparse).
*/
- virtual unsigned int processKnown();
+ virtual unsigned int processKnown() override;
/** \brief Track provided features in \b _capture
* \param _features_in input list of features in \b last to track
@@ -147,7 +147,7 @@ class ProcessorTrackerFeature : public ProcessorTracker
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
- virtual bool voteForKeyFrame() const = 0;
+ virtual bool voteForKeyFrame() const override = 0;
// We overload the advance and reset functions to update the lists of matches
virtual void advanceDerived() override;
@@ -156,7 +156,7 @@ class ProcessorTrackerFeature : public ProcessorTracker
/**\brief Process new Features
*
*/
- virtual unsigned int processNew(const int& _max_features);
+ virtual unsigned int processNew(const int& _max_features) override;
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
@@ -190,7 +190,7 @@ class ProcessorTrackerFeature : public ProcessorTracker
/** \brief Emplaces a new factor for each correspondence between a feature in Capture \b last and a feature in Capture \b origin
*/
- virtual void establishFactors();
+ virtual void establishFactors() override;
};
} // namespace wolf
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index 5c947f386..1b506aeae 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.h
@@ -34,6 +34,9 @@ namespace Eigen
std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x);
wolf::VectorComposite& operator + (wolf::VectorComposite& _v1, const wolf::VectorComposite& _v2);
+wolf::VectorComposite operator + (const wolf::VectorComposite& _v1, const wolf::VectorComposite& _v2);
+wolf::VectorComposite& operator - (wolf::VectorComposite& _v1, const wolf::VectorComposite& _v2);
+wolf::VectorComposite operator - (const wolf::VectorComposite& _v1, const wolf::VectorComposite& _v2);
}
namespace wolf{
diff --git a/src/capture/capture_diff_drive.cpp b/src/capture/capture_diff_drive.cpp
index 05f8a5c49..ec0b14e6a 100644
--- a/src/capture/capture_diff_drive.cpp
+++ b/src/capture/capture_diff_drive.cpp
@@ -28,11 +28,11 @@ CaptureDiffDrive::CaptureDiffDrive(const TimeStamp& _ts,
//
}
-Eigen::VectorXd CaptureDiffDrive::correctDelta(const VectorXd& _delta,
- const VectorXd& _delta_error) const
+VectorComposite CaptureDiffDrive::correctDelta(const VectorComposite& _delta,
+ const VectorComposite& _delta_error) const
{
- Vector3d delta_corrected = _delta + _delta_error;
- delta_corrected(2) = pi2pi(delta_corrected(2));
+ VectorComposite delta_corrected = _delta + _delta_error;
+ delta_corrected.at("O")(0) = pi2pi(delta_corrected.at("O")(0));
return delta_corrected;
}
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index 13b11dda7..5a12a6882 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -51,23 +51,23 @@ CaptureMotion::~CaptureMotion()
//
}
-VectorComposite CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current) const
+VectorComposite CaptureMotion::getDeltaCorrected(const VectorComposite& _calib_current) const
{
- VectorXd calib_preint = getCalibrationPreint();
- VectorXd delta_preint = getBuffer().get().back().delta_integr_;
- MatrixXd jac_calib = getBuffer().get().back().jacobian_calib_;
- VectorXd delta_error = jac_calib * (_calib_current - calib_preint);
- VectorXd delta_corrected = correctDelta(delta_preint, delta_error);
+ VectorComposite calib_preint = getCalibrationPreint();
+ VectorComposite delta_preint = getBuffer().get().back().delta_integr_;
+ MatrixComposite jac_calib = getBuffer().get().back().jacobian_calib_;
+ VectorComposite delta_error = jac_calib * (_calib_current - calib_preint);
+ VectorComposite delta_corrected = correctDelta(delta_preint, delta_error);
return delta_corrected;
}
-VectorComposite CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current, const TimeStamp& _ts) const
+VectorComposite CaptureMotion::getDeltaCorrected(const VectorComposite& _calib_current, const TimeStamp& _ts) const
{
- Motion motion = getBuffer().getMotion(_ts);
- VectorXd delta_preint = motion.delta_integr_;
- MatrixXd jac_calib = motion.jacobian_calib_;
- VectorXd delta_error = jac_calib * (_calib_current - calib_preint_);
- VectorXd delta_corrected = correctDelta(delta_preint, delta_error);
+ Motion motion = getBuffer().getMotion(_ts);
+ VectorComposite delta_preint = motion.delta_integr_;
+ MatrixComposite jac_calib = motion.jacobian_calib_;
+ VectorComposite delta_error = jac_calib * (_calib_current - calib_preint_);
+ VectorComposite delta_corrected = correctDelta(delta_preint, delta_error);
return delta_corrected;
}
diff --git a/src/capture/capture_odom_3d.cpp b/src/capture/capture_odom_3d.cpp
index 447e1d500..dd1e79f31 100644
--- a/src/capture/capture_odom_3d.cpp
+++ b/src/capture/capture_odom_3d.cpp
@@ -34,11 +34,11 @@ CaptureOdom3d::~CaptureOdom3d()
//
}
-Eigen::VectorXd CaptureOdom3d::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const
+VectorComposite CaptureOdom3d::correctDelta(const VectorComposite& _delta, const VectorComposite& _delta_error) const
{
- VectorXd delta(7);
- delta.head(3) = _delta.head(3) + _delta_error.head(3);
- delta.tail(4) = (Quaterniond(_delta.data()+3) * exp_q(_delta_error.tail(3))).coeffs();
+ VectorComposite delta;
+ delta.at("P") = _delta.at("P") + _delta_error.at("P");
+ delta.at("O") = (Quaterniond(_delta.at("O").data()) * exp_q(_delta_error.at("O"))).coeffs();
return delta;
}
diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp
index 80ed5d181..e8e0ec769 100644
--- a/src/state_block/state_composite.cpp
+++ b/src/state_block/state_composite.cpp
@@ -16,7 +16,7 @@ std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x)
return _os;
}
-wolf::VectorComposite& operator + (const wolf::VectorComposite& _v1, const wolf::VectorComposite& _v2)
+wolf::VectorComposite& operator + (wolf::VectorComposite& _v1, const wolf::VectorComposite& _v2)
{
wolf::VectorComposite v;
for (const auto& pair_key_vec1 : _v1)
@@ -29,6 +29,45 @@ wolf::VectorComposite& operator + (const wolf::VectorComposite& _v1, const wolf:
return v;
}
+wolf::VectorComposite operator + (const wolf::VectorComposite& _v1, const wolf::VectorComposite& _v2)
+{
+ wolf::VectorComposite v;
+ for (const auto& pair_key_vec1 : _v1)
+ {
+ const auto &key = pair_key_vec1.first;
+ const auto &vec1 = pair_key_vec1.second;
+ const auto& vec2 = _v2.at(key);
+ v.at(key) = vec1 + vec2;
+ }
+ return v;
+}
+
+wolf::VectorComposite& operator - (wolf::VectorComposite& _v1, const wolf::VectorComposite& _v2)
+{
+ wolf::VectorComposite v;
+ for (const auto& pair_key_vec1 : _v1)
+ {
+ const auto &key = pair_key_vec1.first;
+ const auto &vec1 = pair_key_vec1.second;
+ const auto& vec2 = _v2.at(key);
+ v.at(key) = vec1 - vec2;
+ }
+ return v;
+}
+
+wolf::VectorComposite operator - (const wolf::VectorComposite& _v1, const wolf::VectorComposite& _v2)
+{
+ wolf::VectorComposite v;
+ for (const auto& pair_key_vec1 : _v1)
+ {
+ const auto &key = pair_key_vec1.first;
+ const auto &vec1 = pair_key_vec1.second;
+ const auto& vec2 = _v2.at(key);
+ v.at(key) = vec1 - vec2;
+ }
+ return v;
+}
+
}
namespace wolf
--
GitLab