From d114f33bfbe52bcdbf17a32859c4b84584f63401 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Tue, 7 Sep 2021 23:35:55 +0200
Subject: [PATCH] wip
---
include/core/factor/factor_velocity_direction_3d.h | 13 ++++++++++++-
include/core/processor/processor_fix_wing_model.h | 7 +++++--
src/processor/processor_constant_velocity.cpp | 4 ++--
src/processor/processor_fix_wing_model.cpp | 9 +++++++++
4 files changed, 28 insertions(+), 5 deletions(-)
diff --git a/include/core/factor/factor_velocity_direction_3d.h b/include/core/factor/factor_velocity_direction_3d.h
index 05cc4e8d0..0704416d7 100644
--- a/include/core/factor/factor_velocity_direction_3d.h
+++ b/include/core/factor/factor_velocity_direction_3d.h
@@ -16,8 +16,12 @@ WOLF_PTR_TYPEDEFS(FactorVelocityDirection3d);
//class
class FactorVelocityDirection3d: public FactorAutodiff<FactorVelocityDirection3d,1,3,4>
{
+ protected:
+ double min_vel_norm_;
+
public:
FactorVelocityDirection3d(FeatureBasePtr _ftr_ptr,
+ const double& _min_vel_norm,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
@@ -29,7 +33,8 @@ class FactorVelocityDirection3d: public FactorAutodiff<FactorVelocityDirection3d
_apply_loss_function,
_status,
_ftr_ptr->getFrame()->getV(),
- _ftr_ptr->getFrame()->getO())
+ _ftr_ptr->getFrame()->getO()),
+ min_vel_norm_(_min_vel_norm)
{
// std::cout << "created FactorVelocityDirection3d " << std::endl;
}
@@ -46,6 +51,12 @@ inline bool FactorVelocityDirection3d::operator ()(const T* const _v, const T* c
Eigen::Map<const Eigen::Matrix<T,3,1> > v(_v);
Eigen::Map<const Eigen::Quaternion<T> > q(_q);
+ if (v.norm() < min_vel_norm_)
+ {
+ _residuals[0] = T(0);
+ return true;
+ }
+
// std::cout << "v: " << v(0) << " "
// << v(1) << " "
// << v(2) << "\n";
diff --git a/include/core/processor/processor_fix_wing_model.h b/include/core/processor/processor_fix_wing_model.h
index 040cdc39b..73e7f3cc2 100644
--- a/include/core/processor/processor_fix_wing_model.h
+++ b/include/core/processor/processor_fix_wing_model.h
@@ -19,6 +19,7 @@ struct ParamsProcessorFixWingModel : public ParamsProcessorBase
{
Eigen::Vector3d velocity_local;
double angle_stdev;
+ double min_vel_norm;
ParamsProcessorFixWingModel() = default;
ParamsProcessorFixWingModel(std::string _unique_name, const wolf::ParamsServer & _server) :
@@ -26,12 +27,14 @@ struct ParamsProcessorFixWingModel : public ParamsProcessorBase
{
velocity_local = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/velocity_local");
angle_stdev = _server.getParam<double> (prefix + _unique_name + "/angle_stdev");
+ min_vel_norm = _server.getParam<double> (prefix + _unique_name + "/min_vel_norm");
}
std::string print() const override
{
return ParamsProcessorBase::print() + "\n"
+ "velocity_local: print not implemented\n"
- + "angle_stdev: " + std::to_string(angle_stdev) + "\n";
+ + "angle_stdev: " + std::to_string(angle_stdev) + "\n"
+ + "min_vel_norm: " + std::to_string(min_vel_norm) + "\n";
}
};
@@ -46,7 +49,7 @@ class ProcessorFixWingModel : public ProcessorBase
WOLF_PROCESSOR_CREATE(ProcessorFixWingModel, ParamsProcessorFixWingModel);
virtual ~ProcessorFixWingModel() override;
- void configure(SensorBasePtr _sensor) override {};
+ void configure(SensorBasePtr _sensor) override;
protected:
diff --git a/src/processor/processor_constant_velocity.cpp b/src/processor/processor_constant_velocity.cpp
index 496cbe3cc..dd98e0a93 100644
--- a/src/processor/processor_constant_velocity.cpp
+++ b/src/processor/processor_constant_velocity.cpp
@@ -27,8 +27,8 @@ ProcessorConstantVelocity::~ProcessorConstantVelocity()
void ProcessorConstantVelocity::configure(SensorBasePtr _sensor)
{
- assert(params_processor_->cov_v_rate.rows() == getProblem()->getDim() && "cov_v_rate size is wrong");
- assert(getProblem()->getFrameStructure().find('V') != std::string::npos && "Processor only works with problems with V");
+ assert(params_processor_->cov_v_rate.rows() == _sensor->getProblem()->getDim() && "cov_v_rate size is wrong");
+ assert(_sensor->getProblem()->getFrameStructure().find('V') != std::string::npos && "Processor only works with problems with V");
}
void ProcessorConstantVelocity::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
diff --git a/src/processor/processor_fix_wing_model.cpp b/src/processor/processor_fix_wing_model.cpp
index 02ee2d6b9..2fab50466 100644
--- a/src/processor/processor_fix_wing_model.cpp
+++ b/src/processor/processor_fix_wing_model.cpp
@@ -24,8 +24,16 @@ ProcessorFixWingModel::~ProcessorFixWingModel()
{
}
+void ProcessorFixWingModel::configure(SensorBasePtr _sensor)
+{
+ assert(_sensor->getProblem()->getFrameStructure().find('V') != std::string::npos && "Processor only works with problems with V");
+}
+
void ProcessorFixWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
{
+ if (_keyframe_ptr->getV()->isFixed())
+ return;
+
// emplace capture
auto cap = CaptureBase::emplace<CaptureBase>(_keyframe_ptr, "CaptureBase",
_keyframe_ptr->getTimeStamp(), getSensor());
@@ -38,6 +46,7 @@ void ProcessorFixWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr, const do
// emplace factor
auto fac = FactorBase::emplace<FactorVelocityDirection3d>(fea,
fea,
+ params_processor_->min_vel_norm,
shared_from_this(),
false);
}
--
GitLab