Skip to content
Snippets Groups Projects
Commit ff9c9aca authored by Mederic Fourmy's avatar Mederic Fourmy
Browse files

[skip-ci] Beginning to code a zero vel kin factor

parent c202064e
No related branches found
No related tags found
2 merge requests!23After cmake and const refactor,!11Resolve "Turn ProcessorPointFeetNomove into a processor tracker"
/**
* \file factor_point_feet_nomove.h
*
* Created on: Feb 22, 2020
* \author: mfourmy
*/
#ifndef FACTOR_POINT_FEET_ZERO_VELOCITY_H_
#define FACTOR_POINT_FEET_ZERO_VELOCITY_H_
#include "core/factor/factor_autodiff.h"
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorPointFeetNomove);
class FactorPointFeetNomove : public FactorAutodiff<FactorPointFeetNomove,
3,
3,
4,
3
>
{
public:
FactorPointFeetNomove(const FeatureBasePtr& _ftr_current_ptr,
const StateBlockPtr& _bias_imu,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
~FactorPointFeetNomove() override { /* nothing */ }
/*
Factor state blocks:
_pb: W_p_WB -> base position in world frame
_qb: W_q_B -> base orientation in world frame
_pbm: W_p_WB -> base position in world frame, previous KF
_qbm: W_q_B -> base orientation in world frame, previous KF
*/
template<typename T>
bool operator () (
const T* const _vb,
const T* const _qb,
const T* const _bi,
T* _res) const;
// Vector9d residual() const;
// double cost() const;
};
} /* namespace wolf */
namespace wolf {
FactorPointFeetNomove::FactorPointFeetNomove(
const FeatureBasePtr& _ftr_current_ptr,
const StateBlockPtr& _bias_imu,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
FactorAutodiff("FactorPointFeetNomove",
TOP_GEOM,
_ftr_current_ptr,
nullptr, // _frame_other_ptr
nullptr, // _capture_other_ptr
nullptr, // _feature_other_ptr
nullptr, // _landmark_other_ptr
_processor_ptr,
_apply_loss_function,
_status,
_ftr_current_ptr->getFrame()->getV(),
_ftr_current_ptr->getFrame()->getO(),
_bias_imu
)
{
}
template<typename T>
bool FactorPointFeetNomove::operator () (
const T* const _vb,
const T* const _qb,
const T* const _bi,
T* _res) const
{
Map<Matrix<T,3,1> > res(_res);
// State variables instanciation
Map<const Matrix<T,3,1> > vb(_vb);
Map<const Quaternion<T> > qb(_qb);
Map<const Matrix<T,3,1> > bw(_pbm+3); // gyro bias
Map<const Quaternion<T> > qbm(_qbm);
// Measurements retrieval
Eigen::Matrix<T,6,1> meas = getMeasurement().cast<T>();
Eigen::Matrix<T,3,1> b_v_bl = meas.head<3>();
Eigen::Matrix<T,3,1> i_omg_i = meas.segment<3>(3);
Eigen::Matrix<T,3,1> b_p_bl = meas.tail<3>();
Matrix<T,3,1> err = qb.conjugate()*vb + (i_omg_i - bw).cross(b_p_bl) + b_v_bl;
res = getMeasurementSquareRootInformationUpper() * err;
return true;
}
} // namespace wolf
#endif /* FACTOR__POINT_FEET_ZERO_VELOCITY_H_ */
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment