Skip to content
Snippets Groups Projects

Resolve "Processor motion model"

Merged Joan Vallvé Navarro requested to merge 407-processor-motion-model into devel
6 files
+ 459
186
Compare changes
  • Side-by-side
  • Inline
Files
6
#ifndef FACTOR_VELOCITY_DIRECTION_3D_H_
#define FACTOR_VELOCITY_DIRECTION_3D_H_
#ifndef FACTOR_VELOCITY_LOCAL_DIRECTION_3D_H_
#define FACTOR_VELOCITY_LOCAL_DIRECTION_3D_H_
//Wolf includes
#include "core/factor/factor_autodiff.h"
@@ -11,21 +11,21 @@
namespace wolf {
WOLF_PTR_TYPEDEFS(FactorVelocityDirection3d);
WOLF_PTR_TYPEDEFS(FactorVelocityLocalDirection3d);
//class
class FactorVelocityDirection3d: public FactorAutodiff<FactorVelocityDirection3d,1,3,4>
class FactorVelocityLocalDirection3d: public FactorAutodiff<FactorVelocityLocalDirection3d,1,3,4>
{
protected:
double min_vel_sq_norm_; // stored in squared norm for efficiency
public:
FactorVelocityDirection3d(FeatureBasePtr _ftr_ptr,
FactorVelocityLocalDirection3d(FeatureBasePtr _ftr_ptr,
const double& _min_vel_norm,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorVelocityDirection3d,1,3,4>("FactorVelocityDirection3d",
FactorAutodiff<FactorVelocityLocalDirection3d,1,3,4>("FactorVelocityDirection3d",
TOP_ABS,
_ftr_ptr,
nullptr, nullptr, nullptr, nullptr,
@@ -41,14 +41,14 @@ class FactorVelocityDirection3d: public FactorAutodiff<FactorVelocityDirection3d
assert(abs(_ftr_ptr->getMeasurement().norm() - 1.0) < wolf::Constants::EPS && "velocity direction measurement must be normalized");
}
~FactorVelocityDirection3d() override = default;
~FactorVelocityLocalDirection3d() 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
inline bool FactorVelocityLocalDirection3d::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);
Loading