From da42fd62b262c1e2486019451b63d8935b3ca3d0 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Mon, 6 Sep 2021 23:06:09 +0200
Subject: [PATCH] compiling
---
.../factor/factor_velocity_direction_3d.h | 2 +-
.../processor/processor_constant_velocity.h | 6 +++++-
.../core/processor/processor_fix_wing_model.h | 11 ++++++++--
src/processor/processor_constant_velocity.cpp | 11 +++++-----
src/processor/processor_fix_wing_model.cpp | 20 ++++++++++++-------
5 files changed, 33 insertions(+), 17 deletions(-)
diff --git a/include/core/factor/factor_velocity_direction_3d.h b/include/core/factor/factor_velocity_direction_3d.h
index 457163bca..05cc4e8d0 100644
--- a/include/core/factor/factor_velocity_direction_3d.h
+++ b/include/core/factor/factor_velocity_direction_3d.h
@@ -62,7 +62,7 @@ inline bool FactorVelocityDirection3d::operator ()(const T* const _v, const T* c
// << 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()));
+ T error = acos( v_local.dot(getMeasurement().cast<T>()) / (v_local.norm() * getMeasurement().cast<T>().norm()));
// residual
_residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * error;
diff --git a/include/core/processor/processor_constant_velocity.h b/include/core/processor/processor_constant_velocity.h
index 0014e1fba..156e9be87 100644
--- a/include/core/processor/processor_constant_velocity.h
+++ b/include/core/processor/processor_constant_velocity.h
@@ -17,14 +17,18 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorConstantVelocity);
struct ParamsProcessorConstantVelocity : public ParamsProcessorBase
{
+ Eigen::MatrixXd cov_v_rate;
+
ParamsProcessorConstantVelocity() = default;
ParamsProcessorConstantVelocity(std::string _unique_name, const wolf::ParamsServer & _server) :
ParamsProcessorBase(_unique_name, _server)
{
+ cov_v_rate = _server.getParam<Eigen::VectorXd> (prefix + _unique_name + "/cov_rate_diagonal").asDiagonal();
}
std::string print() const override
{
- return ParamsProcessorBase::print();
+ return ParamsProcessorBase::print() + "\n"
+ + "cov_v_rate: print not implemented\n";
}
};
diff --git a/include/core/processor/processor_fix_wing_model.h b/include/core/processor/processor_fix_wing_model.h
index 97e589b7f..040cdc39b 100644
--- a/include/core/processor/processor_fix_wing_model.h
+++ b/include/core/processor/processor_fix_wing_model.h
@@ -17,14 +17,21 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorFixWingModel);
struct ParamsProcessorFixWingModel : public ParamsProcessorBase
{
+ Eigen::Vector3d velocity_local;
+ double angle_stdev;
+
ParamsProcessorFixWingModel() = default;
ParamsProcessorFixWingModel(std::string _unique_name, const wolf::ParamsServer & _server) :
ParamsProcessorBase(_unique_name, _server)
{
+ velocity_local = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/velocity_local");
+ angle_stdev = _server.getParam<double> (prefix + _unique_name + "/angle_stdev");
}
std::string print() const override
{
- return ParamsProcessorBase::print();
+ return ParamsProcessorBase::print() + "\n"
+ + "velocity_local: print not implemented\n"
+ + "angle_stdev: " + std::to_string(angle_stdev) + "\n";
}
};
@@ -33,7 +40,7 @@ WOLF_PTR_TYPEDEFS(ProcessorFixWingModel);
class ProcessorFixWingModel : public ProcessorBase
{
public:
- ProcessorFixWingModel(ParamsProcessorMotionModelPtr);
+ ProcessorFixWingModel(ParamsProcessorFixWingModelPtr _params);
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorFixWingModel, ParamsProcessorFixWingModel);
diff --git a/src/processor/processor_constant_velocity.cpp b/src/processor/processor_constant_velocity.cpp
index 23a7267ee..496cbe3cc 100644
--- a/src/processor/processor_constant_velocity.cpp
+++ b/src/processor/processor_constant_velocity.cpp
@@ -7,7 +7,7 @@
#include "core/processor/processor_constant_velocity.h"
-#include "core/capture/capture_void.h"
+#include "core/capture/capture_base.h"
#include "core/feature/feature_base.h"
#include "core/factor/factor_block_difference.h"
@@ -19,7 +19,6 @@ ProcessorConstantVelocity::ProcessorConstantVelocity(ParamsProcessorConstantVelo
params_processor_(_params),
last_keyframe_(nullptr)
{
- assert(params_processor_->cov_rate.rows() == getProblem()->getDim() && "covariance size is wrong");
}
ProcessorConstantVelocity::~ProcessorConstantVelocity()
@@ -28,7 +27,8 @@ ProcessorConstantVelocity::~ProcessorConstantVelocity()
void ProcessorConstantVelocity::configure(SensorBasePtr _sensor)
{
- assert(getProblem()->getFrameStructure().find('V') != std::string::npos) && "Processor only works with problems with V");
+ 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");
}
void ProcessorConstantVelocity::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
@@ -40,9 +40,8 @@ void ProcessorConstantVelocity::processKeyFrame(FrameBasePtr _keyframe_ptr, cons
_keyframe_ptr->getTimeStamp() > last_keyframe_->getTimeStamp())
{
// emplace capture
- auto cap = CaptureBase::emplace<CapureVoid>(_keyframe_ptr,
- _keyframe_ptr->getTimeStamp(),
- getSensor());
+ auto cap = CaptureBase::emplace<CaptureBase>(_keyframe_ptr, "CaptureBase",
+ _keyframe_ptr->getTimeStamp(), getSensor());
double dt = _keyframe_ptr->getTimeStamp() - last_keyframe_->getTimeStamp();
diff --git a/src/processor/processor_fix_wing_model.cpp b/src/processor/processor_fix_wing_model.cpp
index ed3a849ea..02ee2d6b9 100644
--- a/src/processor/processor_fix_wing_model.cpp
+++ b/src/processor/processor_fix_wing_model.cpp
@@ -7,14 +7,16 @@
#include "core/processor/processor_fix_wing_model.h"
-#include "core/capture/capture_void.h"
+#include "core/capture/capture_base.h"
#include "core/feature/feature_base.h"
+#include "core/factor/factor_velocity_direction_3d.h"
namespace wolf
{
-ProcessorFixWingModel::ProcessorFixWingModel(ParamsProcessorMotionModelPtr _params) :
- params_motion_model_(_params)
+ProcessorFixWingModel::ProcessorFixWingModel(ParamsProcessorFixWingModelPtr _params) :
+ ProcessorBase("ProcessorFixWingModel", 3, _params),
+ params_processor_(_params)
{
}
@@ -25,15 +27,19 @@ 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());
+ auto cap = CaptureBase::emplace<CaptureBase>(_keyframe_ptr, "CaptureBase",
+ _keyframe_ptr->getTimeStamp(), getSensor());
// emplace feature
auto fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureBase",
- measurement,
- cov);
+ params_processor_->velocity_local,
+ Eigen::Matrix1d(params_processor_->angle_stdev * params_processor_->angle_stdev));
// emplace factor
- auto fac =
+ auto fac = FactorBase::emplace<FactorVelocityDirection3d>(fea,
+ fea,
+ shared_from_this(),
+ false);
}
} /* namespace wolf */
--
GitLab