diff --git a/src/processor_odom_2D.cpp b/src/processor_odom_2D.cpp index fb1f567761e1a8d35b15aa019a78524fd47bfd16..19818dffcc1aee098c316f1b13bf603703333c39 100644 --- a/src/processor_odom_2D.cpp +++ b/src/processor_odom_2D.cpp @@ -5,10 +5,6 @@ namespace wolf ProcessorOdom2D::ProcessorOdom2D(ProcessorParamsOdom2DPtr _params) : ProcessorMotion("ODOM 2D", 3, 3, 3, 2, 0, _params), params_odom_2D_(_params) -// dist_traveled_th_(_params.dist_traveled_th_), -// theta_traveled_th_(_params.theta_traveled_th_), -// cov_det_th_(_params->cov_det_th)//, -// elapsed_time_th_(_params.elapsed_time_th_) { unmeasured_perturbation_cov_ = _params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std * Matrix3s::Identity(); } @@ -21,16 +17,17 @@ void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXs& _data, const Ei const Eigen::VectorXs& _calib, const Scalar _dt, Eigen::VectorXs& _delta, Eigen::MatrixXs& _delta_cov, Eigen::MatrixXs& _jacobian_calib) { - //std::cout << "ProcessorOdom2d::data2delta" << std::endl; 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"); + // data is [dtheta, dr] // delta is [dx, dy, dtheta] // motion model is 1/2 turn + straight + 1/2 turn _delta(0) = cos(_data(1) / 2) * _data(0); _delta(1) = sin(_data(1) / 2) * _data(0); _delta(2) = _data(1); + // Fill delta covariance Eigen::MatrixXs J(delta_cov_size_, data_size_); J(0, 0) = cos(_data(1) / 2); @@ -39,35 +36,29 @@ void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXs& _data, const Ei J(0, 1) = -_data(0) * sin(_data(1) / 2) / 2; J(1, 1) = _data(0) * cos(_data(1) / 2) / 2; J(2, 1) = 1; + // Since input data is size 2, and delta is size 3, the noise model must be given by: // 1. Covariance of the input data: J*Q*J.tr - // 2. Fix variance term to be added: var*Id + // 2. Fixed variance term to be added: var*Id _delta_cov = J * _data_cov * J.transpose() + unmeasured_perturbation_cov_; - //std::cout << "data :" << _data.transpose() << std::endl; - //std::cout << "data cov :" << std::endl << _data_cov << std::endl; - //std::cout << "delta :" << delta_.transpose() << std::endl; - //std::cout << "delta cov :" << std::endl << delta_cov_ << std::endl; - // jacobian_delta_calib_ not used in this class yet. In any case, set to zero with: - // jacobian_delta_calib_.setZero(); } void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, const Scalar _Dt2, Eigen::VectorXs& _delta1_plus_delta2) { - // This is just a frame composition in 2D - //std::cout << "ProcessorOdom2d::deltaPlusDelta" << std::endl; 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"); - _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)); + + // This is just a frame composition in 2D + _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 ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, const Scalar _Dt2, Eigen::VectorXs& _delta1_plus_delta2, Eigen::MatrixXs& _jacobian1, Eigen::MatrixXs& _jacobian2) { - //std::cout << "ProcessorOdom2d::deltaPlusDelta jacobians" << std::endl; 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"); @@ -75,12 +66,16 @@ void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen 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"); + + // This is just a frame composition in 2D _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(); @@ -89,10 +84,10 @@ void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen void ProcessorOdom2D::statePlusDelta(const Eigen::VectorXs& _x, const Eigen::VectorXs& _delta, const Scalar _Dt, Eigen::VectorXs& _x_plus_delta) { - // This is just a frame composition in 2D - //std::cout << "ProcessorOdom2d::statePlusDelta" << std::endl; 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)); }