diff --git a/include/core/capture/capture_diff_drive.h b/include/core/capture/capture_diff_drive.h
index 2300a65b2a203ccc6bb9ca155fab6dbb6b2bdd96..f35be53d1fc9c9d60911a10b87421569b4012e5e 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 d18217279601157c9a1b2eb3c4e7f4ba4dae6cbb..9b4358317bea0af940013336f42865f54cedcb45 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 7e4ee02c8b694cc6ae2b8e8b6d9e6803d7587c65..bef7929365af9cc54eda8a91b1d48cbe2594818b 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 ff5ebb2a66837675e2acc290a90cbdbe8435df10..be2545de4105f155c814be1259a63f2046928aac 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 5c947f386efcd4270ed14e02cf1886d8e6acd3ee..1b506aeaeb3498eeaf1b46293589f7364dc5aea9 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 05f8a5c49fb73e228b135fffc4de398e9e4ba038..ec0b14e6a9cc14fe7b4f4cbe30b44ded70aefc04 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 13b11dda7909e80d1c81687eb4e7313e79994a50..5a12a68828d0d6e5a0472a4e70c9e557000fb106 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 447e1d500f072e2754de4ed107aeb61e6e6299c0..dd1e79f31f6ea493173b673e8eee2ab64dbba333 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 80ed5d181c609f0e9eee82ff98e26c1d147a8b3f..e8e0ec769f1810bb4870fe508a708f314adad2ba 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