From 8b22178a3e86b8ce7551a95dedf4766bc8e1c344 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Mon, 6 Sep 2021 22:30:54 +0200
Subject: [PATCH] implementing processors and factor
---
CMakeLists.txt | 5 ++
.../factor/factor_velocity_direction_3d.h | 75 ++++++++++++++++
.../processor/processor_constant_velocity.h | 81 +++++++++++++++++
...me_factor.h => processor_fix_wing_model.h} | 28 +++---
src/processor/processor_constant_velocity.cpp | 88 +++++++++++++++++++
src/processor/processor_fix_wing_model.cpp | 48 ++++++++++
src/processor/processor_frame_factor.cpp | 36 --------
7 files changed, 311 insertions(+), 50 deletions(-)
create mode 100644 include/core/factor/factor_velocity_direction_3d.h
create mode 100644 include/core/processor/processor_constant_velocity.h
rename include/core/processor/{processor_frame_factor.h => processor_fix_wing_model.h} (65%)
create mode 100644 src/processor/processor_constant_velocity.cpp
create mode 100644 src/processor/processor_fix_wing_model.cpp
delete mode 100644 src/processor/processor_frame_factor.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c6c2db62a..2440c56ee 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -229,6 +229,7 @@ SET(HDRS_FACTOR
include/core/factor/factor_relative_pose_2d_with_extrinsics.h
include/core/factor/factor_relative_pose_3d.h
include/core/factor/factor_pose_3d_with_extrinsics.h
+ include/core/factor/factor_velocity_direction.h
)
SET(HDRS_FEATURE
include/core/feature/feature_base.h
@@ -245,7 +246,9 @@ SET(HDRS_PROCESSOR
include/core/processor/is_motion.h
include/core/processor/motion_buffer.h
include/core/processor/processor_base.h
+ include/core/processor/processor_constant_velocity.h
include/core/processor/processor_diff_drive.h
+ include/core/processor/processor_fix_wing_model.h
include/core/processor/factory_processor.h
include/core/processor/processor_logging.h
include/core/processor/processor_loop_closure.h
@@ -344,7 +347,9 @@ SET(SRCS_PROCESSOR
src/processor/is_motion.cpp
src/processor/motion_buffer.cpp
src/processor/processor_base.cpp
+ src/processor/processor_constant_velocity.cpp
src/processor/processor_diff_drive.cpp
+ src/processor/processor_fix_wing_model.cpp
src/processor/processor_loop_closure.cpp
src/processor/processor_motion.cpp
src/processor/processor_odom_2d.cpp
diff --git a/include/core/factor/factor_velocity_direction_3d.h b/include/core/factor/factor_velocity_direction_3d.h
new file mode 100644
index 000000000..457163bca
--- /dev/null
+++ b/include/core/factor/factor_velocity_direction_3d.h
@@ -0,0 +1,75 @@
+
+#ifndef FACTOR_VELOCITY_DIRECTION_3D_H_
+#define FACTOR_VELOCITY_DIRECTION_3D_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorVelocityDirection3d);
+
+//class
+class FactorVelocityDirection3d: public FactorAutodiff<FactorVelocityDirection3d,1,3,4>
+{
+ public:
+ FactorVelocityDirection3d(FeatureBasePtr _ftr_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorVelocityDirection3d,1,3,4>("FactorVelocityDirection3d",
+ TOP_ABS,
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getV(),
+ _ftr_ptr->getFrame()->getO())
+ {
+// std::cout << "created FactorVelocityDirection3d " << std::endl;
+ }
+
+ ~FactorVelocityDirection3d() override = default;
+
+ template<typename T>
+ bool operator ()(const T* const _v, const T* const _o, T* _residuals) const;
+};
+
+template<typename T>
+inline bool FactorVelocityDirection3d::operator ()(const T* const _v, const T* const _q, T* _residuals) const
+{
+ Eigen::Map<const Eigen::Matrix<T,3,1> > v(_v);
+ Eigen::Map<const Eigen::Quaternion<T> > q(_q);
+
+ // std::cout << "v: " << v(0) << " "
+ // << v(1) << " "
+ // << v(2) << "\n";
+ // std::cout << "q: " << q.coeffs()(0) << " "
+ // << q.coeffs()(1) << " "
+ // << q.coeffs()(2) << " "
+ // << q.coeffs()(3) << "\n";
+
+ // velocity in local coordinates
+ Eigen::Matrix<T,3,1> v_local = q.conjugate() * v;
+
+ // std::cout << "v_local: " << v_local(0) << " "
+ // << v_local(1) << " "
+ // << v_local(2) << "\n";
+
+ // error (angle between measurement and velocity in local coordinates)
+ double error = acos( v_local.dot(getMeasurement().cast<T>()) / (v_local.norm() * getMeasurement().cast<T>().norm()));
+
+ // residual
+ _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * error;
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/core/processor/processor_constant_velocity.h b/include/core/processor/processor_constant_velocity.h
new file mode 100644
index 000000000..0014e1fba
--- /dev/null
+++ b/include/core/processor/processor_constant_velocity.h
@@ -0,0 +1,81 @@
+/*
+ * processor_constant_velocity.h
+ *
+ * Created on: Sep 6, 2021
+ * Author: joanvallve
+ */
+
+#ifndef INCLUDE_CORE_PROCESSOR_PROCESSOR_CONSTANT_VELOCITY_H_
+#define INCLUDE_CORE_PROCESSOR_PROCESSOR_CONSTANT_VELOCITY_H_
+
+#include "core/processor/processor_base.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorConstantVelocity);
+
+struct ParamsProcessorConstantVelocity : public ParamsProcessorBase
+{
+ ParamsProcessorConstantVelocity() = default;
+ ParamsProcessorConstantVelocity(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ParamsProcessorBase(_unique_name, _server)
+ {
+ }
+ std::string print() const override
+ {
+ return ParamsProcessorBase::print();
+ }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorConstantVelocity);
+
+class ProcessorConstantVelocity : public ProcessorBase
+{
+ protected:
+ ParamsProcessorConstantVelocityPtr params_processor_;
+ FrameBasePtr last_keyframe_;
+
+ public:
+ ProcessorConstantVelocity(ParamsProcessorConstantVelocityPtr _params);
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorConstantVelocity, ParamsProcessorConstantVelocity);
+
+ virtual ~ProcessorConstantVelocity() override;
+ void configure(SensorBasePtr _sensor) override;
+
+ protected:
+
+ /** \brief process an incoming capture NEVER CALLED
+ */
+ virtual void processCapture(CaptureBasePtr) override {};
+
+ /** \brief process an incoming key-frame: applies the motion model between consecutive keyframes
+ */
+ virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override;
+
+ /** \brief trigger in capture
+ */
+ virtual bool triggerInCapture(CaptureBasePtr) const override {return false;};
+
+ /** \brief trigger in key-frame
+ */
+ virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override {return true;};
+
+ /** \brief store key frame
+ */
+ virtual bool storeKeyFrame(FrameBasePtr) override {return false;};
+
+ /** \brief store capture
+ */
+ virtual bool storeCapture(CaptureBasePtr) override {return false;};
+
+ /** \brief Vote for KeyFrame generation
+ */
+ virtual bool voteForKeyFrame() const override {return false;};
+};
+
+} /* namespace wolf */
+
+#endif /* INCLUDE_CORE_PROCESSOR_PROCESSOR_CONSTANT_VELOCITY_H_ */
diff --git a/include/core/processor/processor_frame_factor.h b/include/core/processor/processor_fix_wing_model.h
similarity index 65%
rename from include/core/processor/processor_frame_factor.h
rename to include/core/processor/processor_fix_wing_model.h
index c938bd238..97e589b7f 100644
--- a/include/core/processor/processor_frame_factor.h
+++ b/include/core/processor/processor_fix_wing_model.h
@@ -1,24 +1,24 @@
/*
- * processor_frame_factor.h
+ * processor_fix_wing_model.h
*
* Created on: Sep 6, 2021
* Author: joanvallve
*/
-#ifndef INCLUDE_CORE_PROCESSOR_PROCESSOR_FRAME_FACTOR_H_
-#define INCLUDE_CORE_PROCESSOR_PROCESSOR_FRAME_FACTOR_H_
+#ifndef INCLUDE_CORE_PROCESSOR_PROCESSOR_FIX_WING_MODEL_H_
+#define INCLUDE_CORE_PROCESSOR_PROCESSOR_FIX_WING_MODEL_H_
#include "core/processor/processor_base.h"
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorFrameFactor);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorFixWingModel);
-struct ParamsProcessorFrameFactor : public ParamsProcessorBase
+struct ParamsProcessorFixWingModel : public ParamsProcessorBase
{
- ParamsProcessorFrameFactor() = default;
- ParamsProcessorFrameFactor(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ParamsProcessorFixWingModel() = default;
+ ParamsProcessorFixWingModel(std::string _unique_name, const wolf::ParamsServer & _server) :
ParamsProcessorBase(_unique_name, _server)
{
}
@@ -28,17 +28,17 @@ struct ParamsProcessorFrameFactor : public ParamsProcessorBase
}
};
-WOLF_PTR_TYPEDEFS(ProcessorFrameFactor);
+WOLF_PTR_TYPEDEFS(ProcessorFixWingModel);
-class ProcessorFrameFactor : public ProcessorBase
+class ProcessorFixWingModel : public ProcessorBase
{
public:
- ProcessorFrameFactor(ParamsProcessorMotionModelPtr);
+ ProcessorFixWingModel(ParamsProcessorMotionModelPtr);
// Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorFrameFactor, ParamsProcessorFrameFactor);
+ WOLF_PROCESSOR_CREATE(ProcessorFixWingModel, ParamsProcessorFixWingModel);
- virtual ~ProcessorFrameFactor() override;
+ virtual ~ProcessorFixWingModel() override;
void configure(SensorBasePtr _sensor) override {};
protected:
@@ -72,9 +72,9 @@ class ProcessorFrameFactor : public ProcessorBase
virtual bool voteForKeyFrame() const override {return false;};
// ATTRIBUTES
- ParamsProcessorFrameFactorPtr params_processor_;
+ ParamsProcessorFixWingModelPtr params_processor_;
};
} /* namespace wolf */
-#endif /* INCLUDE_CORE_PROCESSOR_PROCESSOR_FRAME_FACTOR_H_ */
+#endif /* INCLUDE_CORE_PROCESSOR_PROCESSOR_FIX_WING_MODEL_H_ */
diff --git a/src/processor/processor_constant_velocity.cpp b/src/processor/processor_constant_velocity.cpp
new file mode 100644
index 000000000..23a7267ee
--- /dev/null
+++ b/src/processor/processor_constant_velocity.cpp
@@ -0,0 +1,88 @@
+/*
+ * processor_constant_velocity.cpp
+ *
+ * Created on: Sep 6, 2021
+ * Author: joanvallve
+ */
+
+#include "core/processor/processor_constant_velocity.h"
+
+#include "core/capture/capture_void.h"
+#include "core/feature/feature_base.h"
+#include "core/factor/factor_block_difference.h"
+
+namespace wolf
+{
+
+ProcessorConstantVelocity::ProcessorConstantVelocity(ParamsProcessorConstantVelocityPtr _params) :
+ ProcessorBase("ProcessorConstantVelocity", 0, _params),
+ params_processor_(_params),
+ last_keyframe_(nullptr)
+{
+ assert(params_processor_->cov_rate.rows() == getProblem()->getDim() && "covariance size is wrong");
+}
+
+ProcessorConstantVelocity::~ProcessorConstantVelocity()
+{
+}
+
+void ProcessorConstantVelocity::configure(SensorBasePtr _sensor)
+{
+ assert(getProblem()->getFrameStructure().find('V') != std::string::npos) && "Processor only works with problems with V");
+}
+
+void ProcessorConstantVelocity::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
+{
+ // TODO: handle if _keyframe_ptr is not the newest one
+
+ // only if not null previous keyframe
+ if (last_keyframe_ and
+ _keyframe_ptr->getTimeStamp() > last_keyframe_->getTimeStamp())
+ {
+ // emplace capture
+ auto cap = CaptureBase::emplace<CapureVoid>(_keyframe_ptr,
+ _keyframe_ptr->getTimeStamp(),
+ getSensor());
+
+ double dt = _keyframe_ptr->getTimeStamp() - last_keyframe_->getTimeStamp();
+
+ // STATE BLOCK V
+ // emplace feature
+ auto fea_v = FeatureBase::emplace<FeatureBase>(cap,
+ "FeatureBase",
+ Eigen::VectorXd::Zero(getProblem()->getDim()),
+ params_processor_->cov_v_rate * dt);
+
+ // emplace factor
+ auto fac_v = FactorBase::emplace<FactorBlockDifference>(fea_v,
+ fea_v,
+ last_keyframe_->getV(),
+ _keyframe_ptr->getV(),
+ last_keyframe_, nullptr, nullptr, nullptr,
+ 0, -1, 0, -1,
+ shared_from_this());
+
+ /*// STATE BLOCK P
+ // emplace feature
+ auto fea_p = FeatureBase::emplace<FeatureBase>(cap,
+ "FeatureBase",
+ Eigen::VectorXd::Zero(getProblem()->getDim()),
+ params_processor_->cov_p_rate * dt);
+
+ // emplace factor ct vel TODO*/
+ }
+
+ // store keyframes
+ last_keyframe_ = _keyframe_ptr;
+}
+
+} /* namespace wolf */
+
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorConstantVelocity);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorConstantVelocity);
+} // namespace wolf
+
diff --git a/src/processor/processor_fix_wing_model.cpp b/src/processor/processor_fix_wing_model.cpp
new file mode 100644
index 000000000..ed3a849ea
--- /dev/null
+++ b/src/processor/processor_fix_wing_model.cpp
@@ -0,0 +1,48 @@
+/*
+ * processor_fix_wing_model.cpp
+ *
+ * Created on: Sep 6, 2021
+ * Author: joanvallve
+ */
+
+#include "core/processor/processor_fix_wing_model.h"
+
+#include "core/capture/capture_void.h"
+#include "core/feature/feature_base.h"
+
+namespace wolf
+{
+
+ProcessorFixWingModel::ProcessorFixWingModel(ParamsProcessorMotionModelPtr _params) :
+ params_motion_model_(_params)
+{
+}
+
+ProcessorFixWingModel::~ProcessorFixWingModel()
+{
+}
+
+void ProcessorFixWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
+{
+ // emplace capture
+ auto cap = CaptureBase::emplace<CapureVoid>(_keyframe_ptr, _keyframe_ptr->getTimeStamp(), getSensor());
+
+ // emplace feature
+ auto fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureBase",
+ measurement,
+ cov);
+
+ // emplace factor
+ auto fac =
+}
+
+} /* namespace wolf */
+
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorFixWingModel);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorFixWingModel);
+} // namespace wolf
+
diff --git a/src/processor/processor_frame_factor.cpp b/src/processor/processor_frame_factor.cpp
deleted file mode 100644
index cfcb44c6e..000000000
--- a/src/processor/processor_frame_factor.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * processor_frame_factor.cpp
- *
- * Created on: Sep 6, 2021
- * Author: joanvallve
- */
-
-#include "../../include/core/processor/processor_frame_factor.h"
-
-namespace wolf
-{
-
-ProcessorFrameFactor::ProcessorFrameFactor(ParamsProcessorMotionModelPtr _params) :
- params_motion_model_(_params)
-{
-}
-
-ProcessorFrameFactor::~ProcessorFrameFactor()
-{
-}
-
-void ProcessorFrameFactor::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
-{
- // TODO
-}
-
-} /* namespace wolf */
-
-
-// Register in the FactoryProcessor
-#include "core/processor/factory_processor.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR(ProcessorFrameFactor);
-WOLF_REGISTER_PROCESSOR_AUTO(ProcessorFrameFactor);
-} // namespace wolf
-
--
GitLab