Skip to content
Snippets Groups Projects
Commit 0d0b3ca8 authored by Jeremie Deray's avatar Jeremie Deray
Browse files

add ProcessorDiffDrive

parent ee2e361f
No related branches found
No related tags found
1 merge request!142Diff drive 2
......@@ -436,6 +436,9 @@ ENDIF(OpenCV_FOUND)
# Add the capture sub-directory
ADD_SUBDIRECTORY(captures)
# Add the processor sub-directory
ADD_SUBDIRECTORY(processors)
# Add the sensor sub-directory
ADD_SUBDIRECTORY(sensors)
......@@ -481,6 +484,7 @@ ADD_LIBRARY(${PROJECT_NAME}
${SRCS}
${SRCS_CAPTURE}
${SRCS_SENSOR}
${SRCS_PROCESSOR}
#${SRCS_DTASSC}
${SRCS_WRAPPER}
)
......@@ -534,6 +538,8 @@ INSTALL(FILES ${HDRS_CAPTURE}
DESTINATION include/iri-algorithms/wolf/captures)
INSTALL(FILES ${HDRS_SENSOR}
DESTINATION include/iri-algorithms/wolf/sensors)
INSTALL(FILES ${HDRS_PROCESSOR}
DESTINATION include/iri-algorithms/wolf/processors)
INSTALL(FILES ${HDRS_WRAPPER}
DESTINATION include/iri-algorithms/wolf/ceres_wrapper)
INSTALL(FILES ${HDRS_SOLVER}
......
INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
# Forward var to parent scope
SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
${CMAKE_CURRENT_SOURCE_DIR}/processor_diff_drive.h PARENT_SCOPE)
SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
${CMAKE_CURRENT_SOURCE_DIR}/processor_diff_drive.cpp PARENT_SCOPE)
#include "processor_diff_drive.h"
#include "../sensors/sensor_diff_drive.h"
#include "../captures/capture_wheel_joint_position.h"
#include "../captures/capture_velocity.h"
#include "../rotations.h"
#include "../constraint_odom_2D.h"
#include "../features/feature_diff_drive.h"
namespace wolf
{
ProcessorDiffDrive::ProcessorDiffDrive(const ProcessorParamsDiffDrive &params) :
ProcessorMotion("DIFF DRIVE", 3, 3, 3, 2, 0.15, 3),
unmeasured_perturbation_cov_(Matrix3s::Identity()*
params.unmeasured_perturbation_std_*
params.unmeasured_perturbation_std_),
params_(params)
{
//
}
void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXs& _data,
const Eigen::MatrixXs& _data_cov,
const Eigen::VectorXs& _calib,
const Scalar _dt,
Eigen::VectorXs& _delta,
Eigen::MatrixXs& _delta_cov,
Eigen::MatrixXs& _jacobian_delta_calib)
{
assert(_data.size() == data_size_ && "Wrong _data vector size");
assert(_data_cov.rows() == data_size_ && "Wrong _data_cov size");
assert(_data_cov.cols() == data_size_ && "Wrong _data_cov size");
assert(_calib.size() == calib_size_ && "Wrong _calib vector size");
/// Retrieve sensor intrinsics
const IntrinsicsDiffDrive& intrinsics =
*(std::static_pointer_cast<SensorDiffDrive>(getSensorPtr())->getIntrinsics());
VelocityComand<Scalar> vel;
Eigen::MatrixXs J_vel_data;
Eigen::MatrixXs J_vel_calib;
switch (intrinsics.model_) {
case DiffDriveModel::Two_Factor_Model:
std::tie(vel, J_vel_data, J_vel_calib) =
wheelPositionIncrementToVelocity<DiffDriveModel::Two_Factor_Model>(
_data, _data_cov,
intrinsics.left_radius_, intrinsics.right_radius_, intrinsics.separation_,
_calib, _dt);
break;
case DiffDriveModel::Three_Factor_Model:
std::tie(vel, J_vel_data, J_vel_calib) =
wheelPositionIncrementToVelocity<DiffDriveModel::Three_Factor_Model>(
_data, _data_cov,
intrinsics.left_radius_, intrinsics.right_radius_, intrinsics.separation_,
_calib, _dt);
break;
case DiffDriveModel::Five_Factor_Model:
// std::tie(vel, J_vel_data, J_vel_calib) =
// wheelPositionIncrementToVelocity<DiffDriveModel::Two_Factor_Model>(
// data, data_cov,
// intrinsics.left_radius_, intrinsics.right_radius_, intrinsics.separation_,
// _calib, _dt);
throw std::runtime_error("DiffDriveModel::Five_Factor_Model not implemented !");
break;
default:
throw std::runtime_error("Unknown DiffDrive model.");
break;
}
/// Integrate delta vel to zero vel thus
/// Convert delta vel to delta 2d pose
Eigen::MatrixXs J_delta_vel;
integrate(vel.comand, vel.comand_cov, _delta, _delta_cov, J_delta_vel);
_delta_cov += unmeasured_perturbation_cov_;
_jacobian_delta_calib = J_delta_vel * J_vel_calib;
}
bool ProcessorDiffDrive::voteForKeyFrame()
{
// Distance criterion
if (getBuffer().get().back().delta_integr_.head<2>().norm() > params_.dist_traveled_th_)
{
//WOLF_PROCESSOR_DEBUG("vote for key-frame on distance criterion.");
return true;
}
// else
// {
// WOLF_PROCESSOR_DEBUG(getBuffer().get().back().delta_integr_.head<2>().norm(),
// " < ", params_.dist_traveled_th_);
// }
if (std::abs(getBuffer().get().back().delta_integr_.tail<1>()(0)) > params_.theta_traveled_th_)
{
//WOLF_PROCESSOR_DEBUG("vote for key-frame on rotation criterion.");
return true;
}
// else
// {
// WOLF_PROCESSOR_DEBUG(getBuffer().get().back().delta_integr_.tail<1>()(0),
// " < ", params_.theta_traveled_th_);
// }
return false;
}
void ProcessorDiffDrive::deltaPlusDelta(const Eigen::VectorXs& _delta1,
const Eigen::VectorXs& _delta2,
const Scalar /*_Dt2*/,
Eigen::VectorXs& _delta1_plus_delta2)
{
assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
assert(_delta1_plus_delta2.size() == delta_size_ && "Wrong _delta1_plus_delta2 vector size");
/// Simple 2d frame composition
_delta1_plus_delta2.head<2>() = _delta1.head<2>() + Eigen::Rotation2Ds(_delta1(2)).matrix() * _delta2.head<2>();
_delta1_plus_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
}
void ProcessorDiffDrive::deltaPlusDelta(const Eigen::VectorXs& _delta1,
const Eigen::VectorXs& _delta2,
const Scalar /*_Dt2*/,
Eigen::VectorXs& _delta1_plus_delta2,
MatrixXs& _jacobian1, MatrixXs& _jacobian2)
{
using std::sin;
using std::cos;
assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
assert(_delta1_plus_delta2.size() == delta_size_ && "Wrong _delta1_plus_delta2 vector size");
assert(_jacobian1.rows() == delta_cov_size_ && "Wrong _jacobian1 size");
assert(_jacobian1.cols() == delta_cov_size_ && "Wrong _jacobian1 size");
assert(_jacobian2.rows() == delta_cov_size_ && "Wrong _jacobian2 size");
assert(_jacobian2.cols() == delta_cov_size_ && "Wrong _jacobian2 size");
/// Simple 2d frame composition
_delta1_plus_delta2.head<2>() = _delta1.head<2>() +
Eigen::Rotation2Ds(_delta1(2)).matrix() * _delta2.head<2>();
_delta1_plus_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
// Jac wrt delta_integrated
_jacobian1 = Eigen::MatrixXs::Identity(delta_cov_size_,delta_cov_size_);
_jacobian1(0,2) = -sin(_delta1(2))*_delta2(0) - cos(_delta1(2))*_delta2(1);
_jacobian1(1,2) = cos(_delta1(2))*_delta2(0) - sin(_delta1(2))*_delta2(1);
// jac wrt delta
_jacobian2 = Eigen::MatrixXs::Identity(delta_cov_size_, delta_cov_size_);
_jacobian2.topLeftCorner<2,2>() = Eigen::Rotation2Ds(_delta1(2)).matrix();
}
void ProcessorDiffDrive::statePlusDelta(const Eigen::VectorXs& _x,
const Eigen::VectorXs& _delta,
const Scalar /*_Dt*/,
Eigen::VectorXs& _x_plus_delta)
{
assert(_x.size() == x_size_ && "Wrong _x vector size");
assert(_x_plus_delta.size() == x_size_ && "Wrong _x_plus_delta vector size");
// This is just a frame composition in 2D
_x_plus_delta.head<2>() = _x.head<2>() + Eigen::Rotation2Ds(_x(2)).matrix() * _delta.head<2>();
_x_plus_delta(2) = pi2pi(_x(2) + _delta(2));
}
Eigen::VectorXs ProcessorDiffDrive::deltaZero() const
{
return Eigen::VectorXs::Zero(delta_size_);
}
Motion ProcessorDiffDrive::interpolate(const Motion& _ref,
Motion& /*_second*/,
TimeStamp& _ts)
{
// TODO: Implement actual interpolation
// Implementation: motion ref keeps the same
//
Motion _interpolated(_ref);
_interpolated.ts_ = _ts;
_interpolated.data_ = Vector3s::Zero();
_interpolated.data_cov_ = Matrix3s::Zero();
_interpolated.delta_ = deltaZero();
_interpolated.delta_cov_ = Eigen::MatrixXs::Zero(delta_size_, delta_size_);
_interpolated.delta_integr_ = _ref.delta_integr_;
_interpolated.delta_integr_cov_ = _ref.delta_integr_cov_;
_interpolated.jacobian_delta_integr_. setIdentity();
_interpolated.jacobian_delta_ . setZero();
_interpolated.jacobian_calib_ . setZero();
return _interpolated;
}
CaptureMotionPtr ProcessorDiffDrive::createCapture(const TimeStamp& _ts,
const SensorBasePtr& _sensor,
const VectorXs& _data,
const MatrixXs& _data_cov,
const FrameBasePtr& _frame_origin)
{
StateBlockPtr i_ptr = _sensor->isIntrinsicDynamic()?
std::make_shared<StateBlock>(3, false) : nullptr;
return std::make_shared<CaptureWheelJointPosition>(_ts, _sensor, _data, _data_cov,
_frame_origin, nullptr, nullptr, i_ptr);
}
ConstraintBasePtr ProcessorDiffDrive::emplaceConstraint(FeatureBasePtr _feature,
CaptureBasePtr _capture_origin)
{
ConstraintOdom2DPtr ctr_odom =
std::make_shared<ConstraintOdom2D>(_feature, _capture_origin->getFramePtr(),
shared_from_this());
_feature->addConstraint(ctr_odom);
_capture_origin->getFramePtr()->addConstrainedBy(ctr_odom);
return ctr_odom;
}
FeatureBasePtr ProcessorDiffDrive::createFeature(CaptureMotionPtr _capture_motion)
{
FeatureBasePtr key_feature_ptr = std::make_shared<FeatureDiffDrive>(
_capture_motion->getBuffer().get().back().delta_integr_,
_capture_motion->getBuffer().get().back().delta_integr_cov_,
_capture_motion->getBuffer().getCalibrationPreint(),
_capture_motion->getBuffer().get().back().jacobian_calib_);
WOLF_INFO(_capture_motion->getBuffer().getCalibrationPreint());
WOLF_INFO(_capture_motion->getBuffer().get().back().jacobian_calib_);
return key_feature_ptr;
}
bool ProcessorDiffDrive::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol)
{
return ProcessorMotion::keyFrameCallback(_keyframe_ptr, _time_tol);
}
ProcessorBasePtr ProcessorDiffDrive::create(const std::string& _unique_name,
const ProcessorParamsBasePtr _params,
const SensorBasePtr _sensor_ptr)
{
const auto params = std::dynamic_pointer_cast<ProcessorParamsDiffDrive>(_params);
if (params == nullptr)
{
WOLF_ERROR("ProcessorDiffDrive::create : input arg"
" _params is NOT of type 'ProcessorParamsDiffDrive' !");
return nullptr;
}
const auto sensor_ptr = std::dynamic_pointer_cast<SensorDiffDrive>(_sensor_ptr);
if (sensor_ptr == nullptr)
{
WOLF_ERROR("ProcessorDiffDrive::create : input arg"
" '_sensor_ptr' is NOT of type 'SensorDiffDrive' !");
return nullptr;
}
ProcessorBasePtr prc_ptr = std::make_shared<ProcessorDiffDrive>(*params);
prc_ptr->setName(_unique_name);
return prc_ptr;
}
} // namespace wolf
// Register in the ProcessorFactory
#include "processor_factory.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR("DIFF DRIVE", ProcessorDiffDrive)
} // namespace wolf
/**
* \file processor_diff_drive.h
*
* Created on: Oct 15, 2016
* \author: Jeremie Deray
*/
#ifndef _WOLF_PROCESSOR_DIFF_DRIVE_H_
#define _WOLF_PROCESSOR_DIFF_DRIVE_H_
#include "../processor_motion.h"
#include "../sensors/diff_drive_tools.h"
namespace wolf {
WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsDiffDrive);
struct ProcessorParamsDiffDrive : public ProcessorParamsBase
{
ProcessorParamsDiffDrive(const Scalar _dist_travel_th,
const Scalar _theta_traveled_th,
const Scalar _cov_det_th,
const Scalar _unmeasured_perturbation_std = 0.0001) :
dist_traveled_th_(_dist_travel_th),
theta_traveled_th_(_theta_traveled_th),
cov_det_th_(_cov_det_th),
unmeasured_perturbation_std_(_unmeasured_perturbation_std)
{
//
}
Scalar dist_traveled_th_;
Scalar theta_traveled_th_;
Scalar cov_det_th_;
Scalar unmeasured_perturbation_std_ = 0.0001;
};
/**
* @brief The ProcessorDiffDrive class.
*
* Velocity motion model.
*
* Integrate odometry from joint position.
*
* velocity : data is [d_vx, d_w]
* position : data is [left_position_increment, right_position_increment]
*
* delta is [dx, dy, dtheta]
*/
WOLF_PTR_TYPEDEFS(ProcessorDiffDrive);
class ProcessorDiffDrive : public ProcessorMotion
{
public:
ProcessorDiffDrive(const ProcessorParamsDiffDrive& params);
virtual ~ProcessorDiffDrive() = default;
virtual bool voteForKeyFrame() override;
protected:
/// @brief Covariance to be added to the unmeasured perturbation.
Matrix3s unmeasured_perturbation_cov_;
/// @brief Intrinsic params
ProcessorParamsDiffDrive params_;
virtual void computeCurrentDelta(const Eigen::VectorXs& _data,
const Eigen::MatrixXs& _data_cov,
const Eigen::VectorXs& _calib,
const Scalar _dt,
Eigen::VectorXs& _delta,
Eigen::MatrixXs& _delta_cov,
Eigen::MatrixXs& _jacobian_delta_calib) override;
virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
const Eigen::VectorXs& _delta2,
const Scalar _Dt2,
Eigen::VectorXs& _delta1_plus_delta2) override;
virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
const Eigen::VectorXs& _delta2,
const Scalar _Dt2,
Eigen::VectorXs& _delta1_plus_delta2,
Eigen::MatrixXs& _jacobian1,
Eigen::MatrixXs& _jacobian2) override;
virtual void statePlusDelta(const Eigen::VectorXs& _x,
const Eigen::VectorXs& _delta,
const Scalar _Dt,
Eigen::VectorXs& _x_plus_delta) override;
virtual Eigen::VectorXs deltaZero() const override;
virtual Motion interpolate(const Motion& _ref,
Motion& _second,
TimeStamp& _ts) override;
virtual CaptureMotionPtr createCapture(const TimeStamp& _ts,
const SensorBasePtr& _sensor,
const VectorXs& _data,
const MatrixXs& _data_cov,
const FrameBasePtr& _frame_origin) override;
virtual ConstraintBasePtr emplaceConstraint(FeatureBasePtr _feature,
CaptureBasePtr _capture_origin) override;
virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_motion) override;
bool keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol) override;
public:
/// @brief Factory method
static ProcessorBasePtr create(const std::string& _unique_name,
const ProcessorParamsBasePtr _params,
const SensorBasePtr _sensor_ptr);
};
} // namespace wolf
#endif /* _WOLF_PROCESSOR_DIFF_DRIVE_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