Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • mobile_robotics/wolf_projects/wolf_lib/wolf
1 result
Show changes
Showing
with 305 additions and 419 deletions
......@@ -31,7 +31,7 @@ struct ProcessorParamsRangeBearing : public ProcessorParamsBase
{
//
}
std::string print()
std::string print() const
{
return "\n" + ProcessorParamsBase::print();
}
......@@ -45,17 +45,12 @@ class ProcessorRangeBearing : public ProcessorBase
public:
typedef Eigen::Transform<Scalar, 2, Eigen::Isometry> Trf;
ProcessorRangeBearing(const SensorRangeBearingPtr _sensor_ptr, ProcessorParamsBasePtr _params);
ProcessorRangeBearing(ProcessorParamsBasePtr _params);
virtual ~ProcessorRangeBearing() {/* empty */}
virtual void configure(SensorBasePtr _sensor) override { }
virtual void configure(SensorBasePtr _sensor) override;
// Factory method for high level API
static ProcessorBasePtr create(const std::string& _unique_name,
const ProcessorParamsBasePtr _params,
const SensorBasePtr sensor_ptr = nullptr);
static ProcessorBasePtr createAutoConf(const std::string& _unique_name,
const ParamsServer& _server,
const SensorBasePtr _sensor_ptr = nullptr);
WOLF_PROCESSOR_CREATE(ProcessorRangeBearing, ProcessorParamsRangeBearing);
protected:
// Implementation of pure virtuals from ProcessorBase
......@@ -84,6 +79,11 @@ class ProcessorRangeBearing : public ProcessorBase
Eigen::Vector2s rect (Scalar range, Scalar bearing) const;
};
inline void ProcessorRangeBearing::configure(SensorBasePtr _sensor)
{
H_r_s = transform(_sensor->getP()->getState(), _sensor->getO()->getState());
}
} /* namespace wolf */
#endif /* HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_ */
......@@ -23,34 +23,17 @@ SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const
assert(_extrinsics.size() == 3 && "Bad extrinsics vector size. Must be 3 for 2D");
}
SensorRangeBearing::~SensorRangeBearing()
SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const IntrinsicsRangeBearingPtr _intr) :
SensorRangeBearing(_extrinsics, Eigen::Vector2s(_intr->noise_range_metres_std, toRad(_intr->noise_bearing_degrees_std)))
{
//
}
SensorBasePtr SensorRangeBearing::create(const std::string& _unique_name, //
const Eigen::VectorXs& _extrinsics, //
const IntrinsicsBasePtr _intrinsics)
SensorRangeBearing::~SensorRangeBearing()
{
IntrinsicsRangeBearingPtr _intrinsics_rb = std::static_pointer_cast<IntrinsicsRangeBearing>(_intrinsics);
Eigen::Vector2s noise_std(_intrinsics_rb->noise_range_metres_std, toRad(_intrinsics_rb->noise_bearing_degrees_std));
SensorRangeBearingPtr sensor = std::make_shared<SensorRangeBearing>(_extrinsics, noise_std);
return sensor;
//
}
SensorBasePtr SensorRangeBearing::createAutoConf(const std::string& _unique_name, //
const ParamsServer& _server)
{
IntrinsicsRangeBearing intr(_unique_name, _server);
Eigen::Vector2s noise_std(intr.noise_range_metres_std, intr.noise_bearing_degrees_std);
SensorRangeBearingPtr sensor = std::make_shared<SensorRangeBearing>(Eigen::Vector3s(1,1,0), noise_std);
sensor->setName(_unique_name);
return sensor;
}
} /* namespace wolf */
......@@ -59,9 +42,5 @@ SensorBasePtr SensorRangeBearing::createAutoConf(const std::string& _unique_name
namespace wolf
{
WOLF_REGISTER_SENSOR("RANGE BEARING", SensorRangeBearing)
} // namespace wolf
#include "core/sensor/autoconf_sensor_factory.h"
namespace wolf
{
WOLF_REGISTER_SENSOR_AUTO("RANGE BEARING", SensorRangeBearing)
} // namespace wolf
......@@ -26,15 +26,15 @@ struct IntrinsicsRangeBearing : public IntrinsicsBase
IntrinsicsRangeBearing(std::string _unique_name, const ParamsServer& _server):
IntrinsicsBase(_unique_name, _server)
{
noise_range_metres_std = _server.getParam<Scalar>(_unique_name + "/noise range metres std", "0.05");
noise_bearing_degrees_std = _server.getParam<Scalar>(_unique_name + "/noise bearing degrees std", "0.5");
noise_range_metres_std = _server.getParam<Scalar>(_unique_name + "/noise_range_metres_std");
noise_bearing_degrees_std = _server.getParam<Scalar>(_unique_name + "/noise_bearing_degrees_std");
}
std::string print() const
{
return "" + IntrinsicsBase::print() + "\n"
+ "noise_range_metres_std: " + std::to_string(noise_range_metres_std) + "\n"
+ "noise_bearing_degrees_std: " + std::to_string(noise_bearing_degrees_std) + "\n";
}
std::string print()
{
return "" + IntrinsicsBase::print()
+ "noise_range_metres_std: " + std::to_string(noise_range_metres_std) + "\n"
+ "noise_bearing_degrees_std: " + std::to_string(noise_bearing_degrees_std) + "\n";
}
};
WOLF_PTR_TYPEDEFS(SensorRangeBearing)
......@@ -43,14 +43,10 @@ class SensorRangeBearing : public SensorBase
{
public:
SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const Eigen::Vector2s& _noise_std);
SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const IntrinsicsRangeBearingPtr _intr);
WOLF_SENSOR_CREATE(SensorRangeBearing, IntrinsicsRangeBearing, 3);
virtual ~SensorRangeBearing();
// Factory method for high level API
static SensorBasePtr create(const std::string& _unique_name, //
const Eigen::VectorXs& _extrinsics, //
const IntrinsicsBasePtr _intrinsics);
static SensorBasePtr createAutoConf(const std::string& _unique_name, //
const ParamsServer& _server);
};
} /* namespace wolf */
......
config:
problem:
frame_structure: "PO" # keyframes have position and orientation
dimension: 2 # space is 2D
prior:
timestamp: 0.0
state: [0,0,0]
cov: [[3,3],.01,0,0,0,.01,0,0,0,.01]
time_tolerance: 0.1
sensors:
- type: "ODOM 2D"
name: "sen odom"
extrinsic:
pose: [0,0, 0]
follow: "hello_wolf/yaml/sensor_odom_2D.yaml" # config parameters in this file
- type: "RANGE BEARING"
name: "sen rb"
extrinsic:
pose: [1,1, 0]
noise_range_metres_std: 0.2 # parser only considers first appearence so the following file parsing will not overwrite this param
follow: hello_wolf/yaml/sensor_range_bearing.yaml # config parameters in this file
processors:
- type: "ODOM 2D"
name: "prc odom"
sensor_name: "sen odom" # attach processor to this sensor
follow: hello_wolf/yaml/processor_odom_2D.yaml # config parameters in this file
- type: "RANGE BEARING"
name: "prc rb"
sensor_name: "sen rb" # attach processor to this sensor
follow: hello_wolf/yaml/processor_range_bearing.yaml # config parameters in this file
files:
# - "/Users/jsola/dev/wolf lib/core/lib/libhellowolf.dylib"
\ No newline at end of file
type: "ODOM 2D"
time_tolerance: 0.1
unmeasured_perturbation_std: 0.001
keyframe_vote:
voting_active: true
voting_aux_active: false
max_time_span: 999
dist_traveled: 0.95
angle_turned: 999
max_buff_length: 999
cov_det: 999
type: "RANGE BEARING"
time_tolerance: 0.1
keyframe_vote:
voting_active: true
voting_aux_active: false
\ No newline at end of file
type: "ODOM 2D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
k_disp_to_disp: 0.1 # m^2 / m
k_rot_to_rot: 0.1 # rad^2 / rad
type: "RANGE BEARING"
noise_range_metres_std: 0.1
noise_bearing_degrees_std: 0.5
......@@ -56,7 +56,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
bool process();
unsigned int id();
unsigned int id() const;
TimeStamp getTimeStamp() const;
void setTimeStamp(const TimeStamp& _ts);
void setTimeStampToNow();
......@@ -68,7 +68,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
public:
const FeatureBasePtrList& getFeatureList() const;
void getFactorList(FactorBasePtrList& _fac_list);
void getFactorList(FactorBasePtrList& _fac_list) const;
SensorBasePtr getSensor() const;
virtual void setSensor(const SensorBasePtr sensor_ptr);
......@@ -107,7 +107,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
void move(FrameBasePtr);
void link(FrameBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<CaptureBase> emplace(FrameBasePtr _frm_ptr, T&&... all);
static std::shared_ptr<classType> emplace(FrameBasePtr _frm_ptr, T&&... all);
protected:
SizeEigen computeCalibSize() const;
......@@ -130,9 +130,9 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
namespace wolf{
template<typename classType, typename... T>
std::shared_ptr<CaptureBase> CaptureBase::emplace(FrameBasePtr _frm_ptr, T&&... all)
std::shared_ptr<classType> CaptureBase::emplace(FrameBasePtr _frm_ptr, T&&... all)
{
CaptureBasePtr cpt = std::make_shared<classType>(std::forward<T>(all)...);
std::shared_ptr<classType> cpt = std::make_shared<classType>(std::forward<T>(all)...);
cpt->link(_frm_ptr);
return cpt;
}
......@@ -177,7 +177,7 @@ inline StateBlockPtr CaptureBase::getSensorIntrinsic() const
return getStateBlock(2);
}
inline unsigned int CaptureBase::id()
inline unsigned int CaptureBase::id() const
{
return capture_id_;
}
......
/**
* \file diff_drive_tools.h
*
* Created on: Oct 20, 2016
* \author: Jeremie Deray
*/
#ifndef CAPTURE_DIFF_DRIVE_H_
#define CAPTURE_DIFF_DRIVE_H_
//wolf includes
#include "core/capture/capture_motion.h"
namespace wolf {
WOLF_PTR_TYPEDEFS(CaptureDiffDrive)
/**
* @brief The CaptureWheelJointPosition class
*
* Represents a list of wheel positions in radian.
*/
class CaptureDiffDrive : public CaptureMotion
{
public:
/**
* \brief Constructor
**/
CaptureDiffDrive(const TimeStamp& _ts,
const SensorBasePtr& _sensor_ptr,
const Eigen::VectorXs& _data,
const Eigen::MatrixXs& _data_cov,
FrameBasePtr _origin_frame_ptr,
StateBlockPtr _p_ptr = nullptr,
StateBlockPtr _o_ptr = nullptr,
StateBlockPtr _intr_ptr = nullptr);
virtual ~CaptureDiffDrive() = default;
virtual Eigen::VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const override;
};
} // namespace wolf
#endif /* CAPTURE_DIFF_DRIVE_H_ */
/**
* \file capture_motion2.h
* \file capture_motion.h
*
* Created on: Mar 16, 2016
* \author: jsola
......@@ -57,7 +57,8 @@ class CaptureMotion : public CaptureBase
CaptureMotion(const std::string & _type,
const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_cov,
const Eigen::VectorXs& _data,
const Eigen::MatrixXs& _data_cov,
SizeEigen _delta_size,
SizeEigen _delta_cov_size,
FrameBasePtr _origin_frame_ptr,
......@@ -83,17 +84,17 @@ class CaptureMotion : public CaptureBase
// Buffer's initial conditions for pre-integration
VectorXs getCalibrationPreint() const;
void setCalibrationPreint(const VectorXs& _calib_preint);
MatrixXs getJacobianCalib();
MatrixXs getJacobianCalib(const TimeStamp& _ts);
MatrixXs getJacobianCalib() const;
MatrixXs getJacobianCalib(const TimeStamp& _ts) const;
// Get delta preintegrated, and corrected for changes on calibration params
VectorXs getDeltaCorrected(const VectorXs& _calib_current);
VectorXs getDeltaCorrected(const VectorXs& _calib_current, const TimeStamp& _ts);
VectorXs getDeltaPreint();
VectorXs getDeltaPreint(const TimeStamp& _ts);
MatrixXs getDeltaPreintCov();
MatrixXs getDeltaPreintCov(const TimeStamp& _ts);
virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error);
VectorXs getDeltaCorrected(const VectorXs& _calib_current) const;
VectorXs getDeltaCorrected(const VectorXs& _calib_current, const TimeStamp& _ts) const;
VectorXs getDeltaPreint() const;
VectorXs getDeltaPreint(const TimeStamp& _ts) const;
MatrixXs getDeltaPreintCov() const;
MatrixXs getDeltaPreintCov(const TimeStamp& _ts) const;
virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const;
// Origin frame
FrameBasePtr getOriginFrame();
......@@ -101,10 +102,11 @@ class CaptureMotion : public CaptureBase
// member data:
private:
Eigen::VectorXs data_; ///< Motion data in form of vector mandatory
Eigen::MatrixXs data_cov_; ///< Motion data covariance
MotionBuffer buffer_; ///< Buffer of motions between this Capture and the next one.
FrameBaseWPtr origin_frame_ptr_; ///< Pointer to the origin frame of the motion
Eigen::VectorXs data_; ///< Motion data in form of vector mandatory
Eigen::MatrixXs data_cov_; ///< Motion data covariance
Eigen::VectorXs calib_preint_; ///< Calibration parameters used during pre-integration
MotionBuffer buffer_; ///< Buffer of motions between this Capture and the next one.
FrameBaseWPtr origin_frame_ptr_; ///< Pointer to the origin frame of the motion
};
inline const Eigen::VectorXs& CaptureMotion::getData() const
......@@ -140,17 +142,17 @@ inline MotionBuffer& CaptureMotion::getBuffer()
return buffer_;
}
inline Eigen::MatrixXs CaptureMotion::getJacobianCalib()
inline Eigen::MatrixXs CaptureMotion::getJacobianCalib() const
{
return getBuffer().get().back().jacobian_calib_;
}
inline Eigen::MatrixXs CaptureMotion::getJacobianCalib(const TimeStamp& _ts)
inline Eigen::MatrixXs CaptureMotion::getJacobianCalib(const TimeStamp& _ts) const
{
return getBuffer().getMotion(_ts).jacobian_calib_;
}
inline Eigen::VectorXs CaptureMotion::correctDelta(const VectorXs& _delta, const VectorXs& _delta_error)
inline Eigen::VectorXs CaptureMotion::correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const
{
WOLF_DEBUG("WARNING: using Cartesian sum for delta correction. \nIf your deltas lie on a manifold, derive this function and implement the proper delta correction!")
return _delta + _delta_error;
......@@ -168,30 +170,30 @@ inline void CaptureMotion::setOriginFrame(FrameBasePtr _frame_ptr)
inline VectorXs CaptureMotion::getCalibrationPreint() const
{
return getBuffer().getCalibrationPreint();
return calib_preint_;
}
inline void CaptureMotion::setCalibrationPreint(const VectorXs& _calib_preint)
{
getBuffer().setCalibrationPreint(_calib_preint);
calib_preint_ = _calib_preint;
}
inline VectorXs CaptureMotion::getDeltaPreint()
inline VectorXs CaptureMotion::getDeltaPreint() const
{
return getBuffer().get().back().delta_integr_;
}
inline VectorXs CaptureMotion::getDeltaPreint(const TimeStamp& _ts)
inline VectorXs CaptureMotion::getDeltaPreint(const TimeStamp& _ts) const
{
return getBuffer().getMotion(_ts).delta_integr_;
}
inline MatrixXs CaptureMotion::getDeltaPreintCov()
inline MatrixXs CaptureMotion::getDeltaPreintCov() const
{
return getBuffer().get().back().delta_integr_cov_;
}
inline MatrixXs CaptureMotion::getDeltaPreintCov(const TimeStamp& _ts)
inline MatrixXs CaptureMotion::getDeltaPreintCov(const TimeStamp& _ts) const
{
return getBuffer().getMotion(_ts).delta_integr_cov_;
}
......
......@@ -33,11 +33,11 @@ class CaptureOdom2D : public CaptureMotion
virtual ~CaptureOdom2D();
virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) override;
virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const override;
};
inline Eigen::VectorXs CaptureOdom2D::correctDelta(const VectorXs& _delta, const VectorXs& _delta_error)
inline Eigen::VectorXs CaptureOdom2D::correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const
{
Vector3s delta = _delta + _delta_error;
delta(2) = pi2pi(delta(2));
......
......@@ -33,7 +33,7 @@ class CaptureOdom3D : public CaptureMotion
virtual ~CaptureOdom3D();
virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) override;
virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const override;
};
......
/**
* \file diff_drive_tools.h
*
* Created on: Oct 20, 2016
* \author: Jeremie Deray
*/
#ifndef CAPTURE_WHEEL_JOINT_POSITION_H_
#define CAPTURE_WHEEL_JOINT_POSITION_H_
//wolf includes
#include "core/capture/capture_motion.h"
namespace wolf {
WOLF_PTR_TYPEDEFS(CaptureWheelJointPosition)
/**
* @brief The CaptureWheelJointPosition class
*
* Represents a list of wheel positions in radian.
*/
class CaptureWheelJointPosition : public CaptureMotion
{
protected:
using NodeBase::node_type_;
public:
/**
* \brief Constructor
**/
CaptureWheelJointPosition(const TimeStamp& _ts,
const SensorBasePtr& _sensor_ptr,
const Eigen::VectorXs& _positions,
FrameBasePtr _origin_frame_ptr);
CaptureWheelJointPosition(const TimeStamp& _ts,
const SensorBasePtr& _sensor_ptr,
const Eigen::VectorXs& _positions,
const Eigen::MatrixXs& _positions_cov,
FrameBasePtr _origin_frame_ptr,
StateBlockPtr _p_ptr = nullptr,
StateBlockPtr _o_ptr = nullptr,
StateBlockPtr _intr_ptr = nullptr);
virtual ~CaptureWheelJointPosition() = default;
virtual VectorXs correctDelta(const VectorXs& _delta,
const VectorXs& _delta_error) override;
const Eigen::VectorXs& getPositions() const;
const Eigen::MatrixXs& getPositionsCov() const;
protected:
Eigen::VectorXs positions_;
Eigen::MatrixXs positions_cov_;
};
/// @todo Enforce some logic on the wheel joint pos data
//template <typename E>
//constexpr typename std::underlying_type<E>::type val(E&& e) noexcept
//{
// return static_cast<typename std::underlying_type<E>::type>(std::forward<E>(e));
//}
//template <std::size_t N>
//struct NumWheelTraits
//{
// static constexpr std::size_t num_wheel = N;
// struct Positions
// {
// using data_t = Eigen::Matrix<Scalar, num_wheel, 1>;
// };
//};
//template <typename Derived>
//struct MobileBaseControllerTraits
//{
// using controller_t = Derived;
// static constexpr decltype(Derived::num_wheel) num_wheel = Derived::num_wheel;
// using wheel_index_t = typename Derived::WheelsIndexes;
// MobileBaseControllerTraits()
// {
// static_assert(true, "");
// }
//};
//struct DiffDriveController : NumWheelTraits<2>
//{
// enum class WheelsIndexes : std::size_t
// {
// Left = 0,
// Right = 1
// };
// class Positions
// {
// Eigen::Matrix<Scalar, num_wheel, 1> values_;
// public:
// Positions(const Scalar left_wheel_value,
// const Scalar rigth_wheel_value) :
// values_(Eigen::Matrix<Scalar, num_wheel, 1>(left_wheel_value, rigth_wheel_value))
// { }
// };
//};
//struct DiffDriveFourWheelsController : NumWheelTraits<4>
//{
// enum class WheelsIndexes : std::size_t
// {
// Front_Left = 0,
// Front_Right = 1,
// Rear_Left = 2,
// Rear_Right = 3
// };
//};
///**
// * @brief The CaptureWheelJointPosition class
// *
// * Represents a list of wheel positions.
// */
//template <typename ControllerType>
//class CaptureWheelJointPosition final :
// public CaptureBase, MobileBaseControllerTraits<ControllerType>
//{
//public:
// using MobileBaseControllerTraits<ControllerType>::controller_t;
// using typename MobileBaseControllerTraits<ControllerType>::wheel_index_t;
// using MobileBaseControllerTraits<ControllerType>::num_wheel;
// /**
// * \brief Constructor with ranges
// **/
// CaptureWheelJointPosition(const TimeStamp& _ts,
// const SensorBasePtr& _sensor_ptr,
// const Eigen::Matrix<Scalar, num_wheel, 1>& _positions) :
// CaptureBase("WHEELS POSITION", _ts, _sensor_ptr),
// positions_(_positions)
// {
// //
// }
// ~CaptureWheelJointPosition() = default;
//// void setSensor(const SensorBasePtr sensor_ptr) override;
// std::size_t getNumWheels() const noexcept
// {
// return num_wheel;
// }
// template <wheel_index_t wheel_index>
// const Scalar& getPosition() const
// {
// return positions_(val(wheel_index));
// }
//protected:
// Eigen::Matrix<Scalar, num_wheel, 1> positions_;
// //SensorLaser2DPtr laser_ptr_; //specific pointer to sensor laser 2D object
//};
//using CaptureDiffDriveWheelJointPosition = CaptureWheelJointPosition<DiffDriveController>;
} // namespace wolf
#endif /* CAPTURE_WHEEL_JOINT_POSITION_H_ */
......@@ -267,9 +267,9 @@ inline bool Factory<TypeBase, TypeInput...>::registerCreator(const std::string&
{
bool reg = callbacks_.insert(typename CallbackMap::value_type(_type, createFn)).second;
if (reg)
std::cout << std::setw(22) << std::left << getClass() << " <-- registered " << _type << std::endl;
std::cout << std::setw(26) << std::left << getClass() << " <-- registered " << _type << std::endl;
else
std::cout << std::setw(22) << std::left << getClass() << " X-- skipping " << _type << ": already registered." << std::endl;
std::cout << std::setw(26) << std::left << getClass() << " X-- skipping " << _type << ": already registered." << std::endl;
return reg;
}
......@@ -314,26 +314,6 @@ namespace wolf
// Some specializations
//======================
// Intrinsics
struct IntrinsicsBase;
typedef Factory<IntrinsicsBase,
const std::string&> IntrinsicsFactory;
template<>
inline std::string IntrinsicsFactory::getClass()
{
return "IntrinsicsFactory";
}
// ProcessorParams
struct ProcessorParamsBase;
typedef Factory<ProcessorParamsBase,
const std::string&> ProcessorParamsFactory;
template<>
inline std::string ProcessorParamsFactory::getClass()
{
return "ProcessorParamsFactory";
}
// Landmarks from YAML
class LandmarkBase;
typedef Factory<LandmarkBase,
......@@ -344,20 +324,6 @@ inline std::string LandmarkFactory::getClass()
return "LandmarkFactory";
}
// Frames
class TimeStamp;
} // namespace wolf
#include "core/frame/frame_base.h"
namespace wolf{
typedef Factory<FrameBase, const FrameType&, const TimeStamp&, const Eigen::VectorXs&> FrameFactory;
template<>
inline std::string FrameFactory::getClass()
{
return "FrameFactory";
}
//#define UNUSED(x) (void)x;
//#define UNUSED(x) (void)(sizeof((x), 0));
#ifdef __GNUC__
#define WOLF_UNUSED __attribute__((used))
......@@ -372,10 +338,6 @@ inline std::string FrameFactory::getClass()
# define UNUSED(x) x
#endif
#define WOLF_REGISTER_FRAME(FrameType, FrameName) \
namespace{ const bool WOLF_UNUSED FrameName##Registered = \
wolf::FrameFactory::get().registerCreator(FrameType, FrameName::create); }\
} /* namespace wolf */
#endif /* FACTORY_H_ */
......@@ -13,10 +13,10 @@ namespace wolf {
}
virtual ~ParamsBase() = default;
std::string print()
std::string print() const
{
return "";
return "";
}
};
}
#endif
\ No newline at end of file
#endif
......@@ -138,7 +138,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
/** \brief Gets the apply loss function flag
*/
bool getApplyLossFunction();
bool getApplyLossFunction() const;
/** \brief Returns a pointer to the frame constrained to
**/
......@@ -165,7 +165,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
void link(FeatureBasePtr ftr);
template<typename classType, typename... T>
static std::shared_ptr<FactorBase> emplace(FeatureBasePtr _oth_ptr, T&&... all);
static std::shared_ptr<classType> emplace(FeatureBasePtr _oth_ptr, T&&... all);
private:
......@@ -191,9 +191,9 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
namespace wolf{
template<typename classType, typename... T>
std::shared_ptr<FactorBase> FactorBase::emplace(FeatureBasePtr _ftr_ptr, T&&... all)
std::shared_ptr<classType> FactorBase::emplace(FeatureBasePtr _ftr_ptr, T&&... all)
{
FactorBasePtr ctr = std::make_shared<classType>(std::forward<T>(all)...);
std::shared_ptr<classType> ctr = std::make_shared<classType>(std::forward<T>(all)...);
ctr->link(_ftr_ptr);
return ctr;
}
......@@ -214,7 +214,7 @@ inline FactorStatus FactorBase::getStatus() const
return status_;
}
inline bool FactorBase::getApplyLossFunction()
inline bool FactorBase::getApplyLossFunction() const
{
return apply_loss_function_;
}
......
......@@ -3,135 +3,149 @@
*
* Created on: Oct 27, 2017
* \author: Jeremie Deray
* \adapted: Joan Sola - July 2019
*/
#ifndef WOLF_FACTOR_DIFF_DRIVE_H_
#define WOLF_FACTOR_DIFF_DRIVE_H_
//Wolf includes
#include "core/capture/capture_diff_drive.h"
#include "core/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
#include "core/feature/feature_diff_drive.h"
#include "core/capture/capture_wheel_joint_position.h"
#include "core/feature/feature_motion.h"
namespace
{
constexpr std::size_t RESIDUAL_SIZE = 3;
constexpr std::size_t POSITION_STATE_BLOCK_SIZE = 2;
constexpr std::size_t ORIENTATION_STATE_BLOCK_SIZE = 1;
/// @todo This should be dynamic (2/3/5)
constexpr std::size_t INTRINSICS_STATE_BLOCK_SIZE = 3;
constexpr std::size_t POSITION_SIZE = 2;
constexpr std::size_t ORIENTATION_SIZE = 1;
constexpr std::size_t INTRINSICS_SIZE = 3;
constexpr std::size_t RESIDUAL_SIZE = 3;
}
namespace wolf {
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorDiffDrive);
class FactorDiffDrive :
public FactorAutodiff< FactorDiffDrive,
RESIDUAL_SIZE, POSITION_STATE_BLOCK_SIZE, ORIENTATION_STATE_BLOCK_SIZE,
INTRINSICS_STATE_BLOCK_SIZE >
class FactorDiffDrive : public FactorAutodiff<FactorDiffDrive,
RESIDUAL_SIZE,
POSITION_SIZE,
ORIENTATION_SIZE,
POSITION_SIZE,
ORIENTATION_SIZE,
INTRINSICS_SIZE>
{
using Base = FactorAutodiff<FactorDiffDrive,
RESIDUAL_SIZE, POSITION_STATE_BLOCK_SIZE, ORIENTATION_STATE_BLOCK_SIZE,
INTRINSICS_STATE_BLOCK_SIZE>;
public:
FactorDiffDrive(const FeatureDiffDrivePtr& _ftr_ptr,
const CaptureWheelJointPositionPtr& _capture_origin_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
const bool _apply_loss_function = false,
const FactorStatus _status = FAC_ACTIVE) :
Base("DIFF DRIVE", _capture_origin_ptr->getFrame(), _capture_origin_ptr,
nullptr, nullptr, _processor_ptr,
_apply_loss_function, _status,
_frame_ptr->getP(), _frame_ptr->getO(),
_capture_origin_ptr->getFrame()->getP(),
_capture_origin_ptr->getFrame()->getO(),
_capture_origin_ptr->getFrame()->getV(),
_capture_origin_ptr->getSensorIntrinsic(),
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_ftr_ptr->getFrame()->getV(),
_ftr_ptr->getCapture()->getSensorIntrinsic()),
J_delta_calib_(_ftr_ptr->getJacobianFactor())
{
//
}
/**
* \brief Default destructor (not recommended)
*
* Default destructor.
*
**/
virtual ~FactorDiffDrive() = default;
template<typename T>
bool operator ()(const T* const _p1, const T* const _o1, const T* const _c1,
const T* const _p2, const T* const _o2, const T* const _c2,
T* _residuals) const;
/**
* \brief Returns the jacobians computation method
**/
virtual JacobianMethod getJacobianMethod() const
{
return JAC_AUTO;
}
protected:
Eigen::MatrixXs J_delta_calib_;
using Base = FactorAutodiff<FactorDiffDrive, // @suppress("Type cannot be resolved")
RESIDUAL_SIZE,
POSITION_SIZE,
ORIENTATION_SIZE,
POSITION_SIZE,
ORIENTATION_SIZE,
INTRINSICS_SIZE>;
public:
FactorDiffDrive(const FeatureMotionPtr& _ftr_ptr,
const CaptureBasePtr& _capture_origin_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
const bool _apply_loss_function = false,
const FactorStatus _status = FAC_ACTIVE) :
Base("DIFF DRIVE",
_capture_origin_ptr->getFrame(),
_capture_origin_ptr,
nullptr,
nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_capture_origin_ptr->getFrame()->getP(),
_capture_origin_ptr->getFrame()->getO(),
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_capture_origin_ptr->getSensorIntrinsic()),
calib_preint_(_ftr_ptr->getCalibrationPreint()),
J_delta_calib_(_ftr_ptr->getJacobianCalibration())
{
//
}
/**
* \brief Default destructor (not recommended)
*
* Default destructor.
*
**/
virtual ~FactorDiffDrive() = default;
template<typename T>
bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
const T* const _o2, const T* const _c, T* _residuals) const;
VectorXs residual();
protected:
Eigen::Vector3s calib_preint_;
Eigen::MatrixXs J_delta_calib_;
};
template<typename T>
inline bool
FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1, const T* const _c1,
const T* const _p2, const T* const _o2, const T* const _c2,
T* _residuals) const
inline bool FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
const T* const _o2, const T* const _c, T* _residuals) const
{
// MAPS
Eigen::Map<Eigen::Matrix<T, RESIDUAL_SIZE, 1> > residuals_map(_residuals);
// MAPS
Eigen::Map<Eigen::Matrix<T, RESIDUAL_SIZE, 1> > residuals(_residuals);
Eigen::Map<const Eigen::Matrix<T, POSITION_SIZE, 1> > p1(_p1);
Eigen::Map<const Eigen::Matrix<T, POSITION_SIZE, 1> > p2(_p2);
Eigen::Map<const Eigen::Matrix<T, POSITION_STATE_BLOCK_SIZE, 1> > p1_map(_p1);
Eigen::Map<const Eigen::Matrix<T, POSITION_STATE_BLOCK_SIZE, 1> > p2_map(_p2);
Eigen::Map<const Eigen::Matrix<T, INTRINSICS_SIZE, 1> > c(_c);
Eigen::Map<const Eigen::Matrix<T, INTRINSICS_STATE_BLOCK_SIZE, 1> > c1_map(_c1);
Eigen::Map<const Eigen::Matrix<T, INTRINSICS_STATE_BLOCK_SIZE, 1> > c2_map(_c2);
// Compute corrected delta
// Compute corrected delta
/// Is this my delta_preint ? Yes!
const Eigen::Matrix<T, 3, 1> delta_preint = getMeasurement().cast<T>();
/// Is this my delta_preint ?
const Eigen::Matrix<T, 3, 1> measurement = getMeasurement().cast<T>();
Matrix<T, 3, 1> c_preint = calib_preint_.cast<T>();
Eigen::Matrix<T, 3, 1> delta_corrected = measurement + J_delta_calib_.cast<T>() * (c2_map - c1_map);
Eigen::Matrix<T, 3, 1> delta_corrected = delta_preint + J_delta_calib_.cast<T>() * (c - c_preint);
// First 2d pose residual
// 2d pose residual
Eigen::Matrix<T, 3, 1> delta_predicted;
Eigen::Matrix<T, 3, 1> delta_predicted;
// position
delta_predicted.head(2) = Eigen::Rotation2D<T>(-_o1[0]) * (p2_map - p1_map);
// position
delta_predicted.head(2) = Eigen::Rotation2D<T>(-_o1[0]) * (p2 - p1);
// orientation
delta_predicted(2) = _o2[0] - _o1[0];
// orientation
delta_predicted(2) = pi2pi(_o2[0] - _o1[0]);
// residual
residuals_map = delta_corrected - delta_predicted;
// residual
residuals = delta_corrected - delta_predicted;
// angle remapping
while (residuals_map(2) > T(M_PI))
residuals_map(2) = residuals_map(2) - T(2. * M_PI);
while (residuals_map(2) <= T(-M_PI))
residuals_map(2) = residuals_map(2) + T(2. * M_PI);
// angle remapping
while (residuals(2) > T(M_PI))
residuals(2) = residuals(2) - T(2. * M_PI);
while (residuals(2) <= T(-M_PI))
residuals(2) = residuals(2) + T(2. * M_PI);
// weighted residual
residuals_map = getMeasurementSquareRootInformationUpper().cast<T>() * residuals_map;
// weighted residual
residuals = getMeasurementSquareRootInformationUpper().cast<T>() * residuals;
return true;
return true;
}
inline Eigen::VectorXs FactorDiffDrive::residual()
{
VectorXs residual(3);
operator ()(getFrameOther()->getP()->getState().data(), getFrameOther()->getO()->getState().data(),
getCapture()->getFrame()->getP()->getState().data(),
getCapture()->getFrame()->getO()->getState().data(),
getCaptureOther()->getSensorIntrinsic()->getState().data(), residual.data());
return residual;
}
} // namespace wolf
......
......@@ -60,7 +60,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
// properties
unsigned int id();
unsigned int id() const;
unsigned int trackId(){return track_id_;}
void setTrackId(unsigned int _tr_id){track_id_ = _tr_id;}
unsigned int landmarkId(){return landmark_id_;}
......@@ -98,7 +98,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
void link(CaptureBasePtr cap_ptr);
template<typename classType, typename... T>
static std::shared_ptr<FeatureBase> emplace(CaptureBasePtr _cpt_ptr, T&&... all);
static std::shared_ptr<classType> emplace(CaptureBasePtr _cpt_ptr, T&&... all);
protected:
......@@ -122,9 +122,9 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
namespace wolf{
template<typename classType, typename... T>
std::shared_ptr<FeatureBase> FeatureBase::emplace(CaptureBasePtr _cpt_ptr, T&&... all)
std::shared_ptr<classType> FeatureBase::emplace(CaptureBasePtr _cpt_ptr, T&&... all)
{
FeatureBasePtr ftr = std::make_shared<classType>(std::forward<T>(all)...);
std::shared_ptr<classType> ftr = std::make_shared<classType>(std::forward<T>(all)...);
ftr->link(_cpt_ptr);
return ftr;
}
......@@ -139,7 +139,7 @@ inline const FactorBasePtrList& FeatureBase::getConstrainedByList() const
return constrained_by_list_;
}
inline unsigned int FeatureBase::id()
inline unsigned int FeatureBase::id() const
{
return feature_id_;
}
......
......@@ -9,29 +9,25 @@
#define _WOLF_FEATURE_DIFF_DRIVE_H_
//Wolf includes
#include "core/feature/feature_base.h"
#include "core/feature/feature_motion.h"
namespace wolf {
WOLF_PTR_TYPEDEFS(FeatureDiffDrive)
class FeatureDiffDrive : public FeatureBase
class FeatureDiffDrive : public FeatureMotion
{
public:
FeatureDiffDrive(const Eigen::VectorXs& _delta_preintegrated,
const Eigen::MatrixXs& _delta_preintegrated_covariance,
const Eigen::VectorXs& _diff_drive_factors,
const Eigen::MatrixXs& _jacobian_diff_drive_factors);
const Eigen::VectorXs& _diff_drive_params,
const Eigen::MatrixXs& _jacobian_diff_drive_params);
virtual ~FeatureDiffDrive() = default;
const Eigen::MatrixXs& getJacobianFactor() const;
protected:
Eigen::VectorXs diff_drive_factors_;
Eigen::MatrixXs jacobian_diff_drive_factors_;
};
} /* namespace wolf */
......