Skip to content
Snippets Groups Projects
Commit 017ab7db authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

Merge branch 'feature/constraint_sensor_params' of...

Merge branch 'feature/constraint_sensor_params' of https://gitlab.iri.upc.edu/mobile_robotics/wolf into feature/constraint_sensor_params
parents dd9ed552 ad705126
No related branches found
No related tags found
1 merge request!243Constraint prior sensor params
Pipeline #2506 passed
...@@ -35,8 +35,15 @@ class ConstraintBlockAbsolute : public ConstraintAnalytic ...@@ -35,8 +35,15 @@ class ConstraintBlockAbsolute : public ConstraintAnalytic
* \param _start_idx (default=-1) the size of the state segment that is constrained, -1 = all the * \param _start_idx (default=-1) the size of the state segment that is constrained, -1 = all the
* *
*/ */
ConstraintBlockAbsolute(StateBlockPtr _sb_ptr, unsigned int _start_idx = 0, int _size = -1, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) : ConstraintBlockAbsolute(StateBlockPtr _sb_ptr,
ConstraintAnalytic("BLOCK ABS", _apply_loss_function, _status, _sb_ptr), unsigned int _start_idx = 0,
int _size = -1,
bool _apply_loss_function = false,
ConstraintStatus _status = CTR_ACTIVE) :
ConstraintAnalytic("BLOCK ABS",
_apply_loss_function,
_status,
_sb_ptr),
sb_size_(_sb_ptr->getSize()), sb_size_(_sb_ptr->getSize()),
sb_constrained_start_(_start_idx), sb_constrained_start_(_start_idx),
sb_constrained_size_(_size == -1 ? sb_size_ : _size) sb_constrained_size_(_size == -1 ? sb_size_ : _size)
......
...@@ -21,7 +21,7 @@ void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXs& _data, const Ei ...@@ -21,7 +21,7 @@ void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXs& _data, const Ei
assert(_data_cov.rows() == data_size_ && "Wrong _data_cov size"); assert(_data_cov.rows() == data_size_ && "Wrong _data_cov size");
assert(_data_cov.cols() == data_size_ && "Wrong _data_cov size"); assert(_data_cov.cols() == data_size_ && "Wrong _data_cov size");
// data is [dtheta, dr] // data is [dr, dtheta]
// delta is [dx, dy, dtheta] // delta is [dx, dy, dtheta]
// motion model is 1/2 turn + straight + 1/2 turn // motion model is 1/2 turn + straight + 1/2 turn
_delta(0) = cos(_data(1) / 2) * _data(0); _delta(0) = cos(_data(1) / 2) * _data(0);
......
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