diff --git a/src/constraint_block_absolute.h b/src/constraint_block_absolute.h index 1e901f10451a858d219a7fc39a7e3eb64c73f06a..e3def4493f0444ada07ed097e737610e5459671b 100644 --- a/src/constraint_block_absolute.h +++ b/src/constraint_block_absolute.h @@ -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 * */ - ConstraintBlockAbsolute(StateBlockPtr _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), + ConstraintBlockAbsolute(StateBlockPtr _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_constrained_start_(_start_idx), sb_constrained_size_(_size == -1 ? sb_size_ : _size) diff --git a/src/processor_odom_2D.cpp b/src/processor_odom_2D.cpp index 19818dffcc1aee098c316f1b13bf603703333c39..5041ae985c60770b0c8124f753bcc802574f88ee 100644 --- a/src/processor_odom_2D.cpp +++ b/src/processor_odom_2D.cpp @@ -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.cols() == data_size_ && "Wrong _data_cov size"); - // data is [dtheta, dr] + // data is [dr, dtheta] // delta is [dx, dy, dtheta] // motion model is 1/2 turn + straight + 1/2 turn _delta(0) = cos(_data(1) / 2) * _data(0);