Head

GCC Code Coverage Report

Directory:	src/		Exec	Total	Coverage
File:	src/odometry.cpp	Lines:	0	135	0.0 %
Date:	2022-09-12 09:50:59	Branches:	0	254	0.0 %


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2013, PAL Robotics, S.L.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the PAL Robotics nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/*
 * Author: Luca Marchionni
 * Author: Bence Magyar
 * Author: Enrique Fernández
 * Author: Paul Mathieu
 */

#include "odometry.h"

#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/command-direct-getter.h>
#include <dynamic-graph/command-direct-setter.h>
#include <dynamic-graph/factory.h>

#include <boost/bind.hpp>
#include <cmath>

namespace dynamicgraph {
namespace details {
namespace bacc = boost::accumulators;

Odometry::Odometry(size_t velocity_rolling_window_size)
    : x_(0.0),
      y_(0.0),
      heading_(0.0),
      linear_(0.0),
      angular_(0.0),
      wheelSeparation_(0.0),
      left_wheelRadius_(0.0),
      right_wheelRadius_(0.0),
      left_wheel_old_pos_(0.0),
      right_wheel_old_pos_(0.0),
      velocity_rolling_window_size_(velocity_rolling_window_size),
      linear_acc_(RollingWindow::window_size = velocity_rolling_window_size),
      angular_acc_(RollingWindow::window_size = velocity_rolling_window_size),
      integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2)) {}

void Odometry::init() {
  // Reset accumulators:
  resetAccumulators();
}

bool Odometry::update(double left_pos, double right_pos, double dt) {
  /// Get current wheel joint positions:
  const double left_wheel_cur_pos = left_pos * left_wheelRadius_;
  const double right_wheel_cur_pos = right_pos * right_wheelRadius_;

  /// Estimate velocity of wheels using old and current position:
  const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_;
  const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;

  /// Update old position with current:
  left_wheel_old_pos_ = left_wheel_cur_pos;
  right_wheel_old_pos_ = right_wheel_cur_pos;

  /// Compute linear and angular diff:
  const double linear = (right_wheel_est_vel + left_wheel_est_vel) * 0.5;
  const double angular =
      (right_wheel_est_vel - left_wheel_est_vel) / wheelSeparation_;

  /// Integrate odometry:
  integrate_fun_(linear, angular);

  /// We cannot estimate the speed with very small time intervals:
  if (dt < 0.0001) return false;  // Interval too small to integrate with

  /// Estimate speeds using a rolling mean to filter them out:
  linear_acc_(linear / dt);
  angular_acc_(angular / dt);

  linear_ = bacc::rolling_mean(linear_acc_);
  angular_ = bacc::rolling_mean(angular_acc_);

  return true;
}

void Odometry::updateOpenLoop(double linear, double angular, double dt) {
  /// Save last linear and angular velocity:
  linear_ = linear;
  angular_ = angular;

  /// Integrate odometry:
  integrate_fun_(linear * dt, angular * dt);
}

void Odometry::setWheelParams(double wheelSeparation, double left_wheelRadius,
                              double right_wheelRadius) {
  wheelSeparation_ = wheelSeparation;
  left_wheelRadius_ = left_wheelRadius;
  right_wheelRadius_ = right_wheelRadius;
}

void Odometry::setVelocityRollingWindowSize(
    size_t velocity_rolling_window_size) {
  velocity_rolling_window_size_ = velocity_rolling_window_size;

  resetAccumulators();
}

void Odometry::integrateRungeKutta2(double linear, double angular) {
  const double direction = heading_ + angular * 0.5;

  /// Runge-Kutta 2nd order integration:
  x_ += linear * cos(direction);
  y_ += linear * sin(direction);
  heading_ += angular;
}

/**
 * \brief Other possible integration method provided by the class
 * \param linear
 * \param angular
 */
void Odometry::integrateExact(double linear, double angular) {
  if (fabs(angular) < 1e-6)
    integrateRungeKutta2(linear, angular);
  else {
    /// Exact integration (should solve problems when angular is zero):
    const double heading_old = heading_;
    const double r = linear / angular;
    heading_ += angular;
    x_ += r * (sin(heading_) - sin(heading_old));
    y_ += -r * (cos(heading_) - cos(heading_old));
  }
}

void Odometry::resetAccumulators() {
  linear_acc_ = RollingMeanAcc(RollingWindow::window_size =
                                   velocity_rolling_window_size_);
  angular_acc_ = RollingMeanAcc(RollingWindow::window_size =
                                    velocity_rolling_window_size_);
}

}  // namespace details

namespace bacc = boost::accumulators;

Odometry::Odometry(const std::string& name)
    : Entity(name),
      wheelsPositionSIN(
          NULL, "Odometry(" + name + ")::input(vector)::wheelsPosition"),
      baseVelocityEstimationSOUT(
          boost::bind(&Odometry::computeVelocityEstimation, this, _1, _2),
          wheelsPositionSIN,
          "Odometry(" + name + ")::output(vector)::baseVelocityEstimation"),
      baseVelocityFilteredEstimationSOUT(
          boost::bind(&Odometry::computeVelocityFilteredEstimation, this, _1,
                      _2),
          baseVelocityEstimationSOUT,
          "Odometry(" + name +
              ")::output(vector)::baseVelocityFilteredEstimation"),
      baseVelocitySIN(NULL,
                      "Odometry(" + name + ")::input(vector)::baseVelocity"),
      baseConfigSOUT(boost::bind(&Odometry::computeBaseConfig, this, _1, _2),
                     baseVelocitySIN,
                     "Odometry(" + name + ")::output(vector)::baseConfig"),
      basePoseSOUT(
          boost::bind(&Odometry::computeBasePose, this, _1, _2), baseConfigSOUT,
          "Odometry(" + name + ")::output(matrixHomogeneous)::basePose")

      ,
      dt_(1.),
      x_(0.0),
      y_(0.0),
      heading_(0.0),
      wheelSeparationInv_(0.0),
      wheelRadii_(0.0, 0.0),
      wheelOldPos_(0.0, 0.0),
      rollingWindowSize_(10),
      linear_acc_(RollingWindow::window_size = rollingWindowSize_),
      angular_acc_(RollingWindow::window_size = rollingWindowSize_) {
  signalRegistration(wheelsPositionSIN << baseVelocityEstimationSOUT
                                       << baseVelocityFilteredEstimationSOUT
                                       << baseVelocitySIN << baseConfigSOUT
                                       << basePoseSOUT);

  // By default, use the velocity estimation
  // (i.e. loop closed with wheel position sensors).
  baseVelocitySIN.plug(&baseVelocityEstimationSOUT);

  addCommand("setWheelParams",
             command::makeCommandVoid3(
                 *this, &Odometry::setWheelParams,
                 command::docCommandVoid3(
                     "Sets the wheel parameters: radius and separation",
                     "double: Separation between left and right wheels [m]",
                     "double: Left wheel radius [m]",
                     "double: Right wheel radius [m]")));
  addCommand("setBasePose",
             command::makeCommandVoid3(
                 *this, &Odometry::setBasePose,
                 command::docCommandVoid3(
                     "Set the base pose to integrate from.", "double: x [m]",
                     "double: y [m]", "double: heading [rad]")));
  addCommand("setPeriod",
             command::makeDirectSetter(*this, &dt_,
                                       "Set period (only affects the velocity "
                                       "estimation and nothing else)."));
  addCommand("getPeriod", command::makeDirectGetter(*this, &dt_, "the period"));
}

Vector& Odometry::computeVelocityEstimation(Vector& vel, int time) {
  vel.resize(2);
  const Vector& wheelCurAng = wheelsPositionSIN(time);

  /// Get current wheel joint positions:
  Eigen::Array2d wheelCurPos = wheelCurAng.array() * wheelRadii_;

  /// Estimate velocity of wheels using old and current position:
  Eigen::Array2d wheelEstVel = (wheelCurPos - wheelOldPos_) / dt_;

  /// Update old position with current:
  wheelOldPos_ = wheelCurPos;

  /// Compute linear and angular diff:
  vel[0] = (wheelEstVel[1] + wheelEstVel[0]) * 0.5;
  vel[1] = (wheelEstVel[1] - wheelEstVel[0]) * wheelSeparationInv_;

  /// Estimate speeds using a rolling mean to filter them out:
  linear_acc_(vel[0]);
  angular_acc_(vel[1]);

  return vel;
}

Vector& Odometry::computeVelocityFilteredEstimation(Vector& vel, int time) {
  baseVelocityEstimationSOUT.recompute(time);

  vel.resize(2);
  vel[0] = bacc::rolling_mean(linear_acc_);
  vel[1] = bacc::rolling_mean(angular_acc_);

  return vel;
}

Vector& Odometry::computeBaseConfig(Vector& base, int time) {
  base.resize(3);
  const Vector& vel = baseVelocitySIN(time);
  integrateExact(vel[0] * dt_, vel[1] * dt_);

  base << x_, y_, heading_;
  return base;
}

sot::MatrixHomogeneous& Odometry::computeBasePose(sot::MatrixHomogeneous& M,
                                                  int time) {
  const Vector& base = baseConfigSOUT(time);
  M.setIdentity();
  M.translation().head<2>() = base.head<2>();
  double c = cos(base(2)), s = sin(base(2));
  M.linear().topLeftCorner<2, 2>() << c, -s, s, c;
  return M;
}

void Odometry::setBasePose(const double& x, const double& y,
                           const double& heading) {
  x_ = x;
  y_ = y;
  heading_ = heading;

  resetAccumulators();
}

void Odometry::setWheelParams(const double& wheelSeparation,
                              const double& leftWheelRadius,
                              const double& rightWheelRadius) {
  wheelSeparationInv_ = 1 / wheelSeparation;
  wheelRadii_ << leftWheelRadius, rightWheelRadius;
}

void Odometry::setVelocityRollingWindowSize(const size_t& rollingWindowSize) {
  rollingWindowSize_ = rollingWindowSize;
  resetAccumulators();
}

void Odometry::integrateRungeKutta2(double linear, double angular) {
  const double direction = heading_ + angular * 0.5;

  /// Runge-Kutta 2nd order integration:
  x_ += linear * cos(direction);
  y_ += linear * sin(direction);
  heading_ += angular;
}

/**
 * \brief Other possible integration method provided by the class
 * \param linear
 * \param angular
 */
void Odometry::integrateExact(double linear, double angular) {
  if (fabs(angular) < 1e-6)
    integrateRungeKutta2(linear, angular);
  else {
    /// Exact integration (should solve problems when angular is zero):
    const double heading_old = heading_;
    const double r = linear / angular;
    heading_ += angular;
    x_ += r * (sin(heading_) - sin(heading_old));
    y_ += -r * (cos(heading_) - cos(heading_old));
  }
}

void Odometry::resetAccumulators() {
  linear_acc_ = RollingMeanAcc(RollingWindow::window_size = rollingWindowSize_);
  angular_acc_ =
      RollingMeanAcc(RollingWindow::window_size = rollingWindowSize_);
}

DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Odometry, "Odometry");

}  // namespace dynamicgraph


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*********************************************************************
2			* Software License Agreement (BSD License)
3			*
4			* Copyright (c) 2013, PAL Robotics, S.L.
5			* All rights reserved.
6			*
7			* Redistribution and use in source and binary forms, with or without
8			* modification, are permitted provided that the following conditions
9			* are met:
10			*
11			* * Redistributions of source code must retain the above copyright
12			* notice, this list of conditions and the following disclaimer.
13			* * Redistributions in binary form must reproduce the above
14			* copyright notice, this list of conditions and the following
15			* disclaimer in the documentation and/or other materials provided
16			* with the distribution.
17			* * Neither the name of the PAL Robotics nor the names of its
18			* contributors may be used to endorse or promote products derived
19			* from this software without specific prior written permission.
20			*
21			* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22			* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23			* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24			* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25			* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26			* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27			* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28			* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29			* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30			* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31			* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32			* POSSIBILITY OF SUCH DAMAGE.
33			*********************************************************************/
34
35			/*
36			* Author: Luca Marchionni
37			* Author: Bence Magyar
38			* Author: Enrique Fernández
39			* Author: Paul Mathieu
40			*/
41
42			#include "odometry.h"
43
44			#include <dynamic-graph/command-bind.h>
45			#include <dynamic-graph/command-direct-getter.h>
46			#include <dynamic-graph/command-direct-setter.h>
47			#include <dynamic-graph/factory.h>
48
49			#include <boost/bind.hpp>
50			#include <cmath>
51
52			namespace dynamicgraph {
53			namespace details {
54			namespace bacc = boost::accumulators;
55
56			Odometry::Odometry(size_t velocity_rolling_window_size)
57			: x_(0.0),
58			y_(0.0),
59			heading_(0.0),
60			linear_(0.0),
61			angular_(0.0),
62			wheelSeparation_(0.0),
63			left_wheelRadius_(0.0),
64			right_wheelRadius_(0.0),
65			left_wheel_old_pos_(0.0),
66			right_wheel_old_pos_(0.0),
67			velocity_rolling_window_size_(velocity_rolling_window_size),
68			linear_acc_(RollingWindow::window_size = velocity_rolling_window_size),
69			angular_acc_(RollingWindow::window_size = velocity_rolling_window_size),
70			integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2)) {}
71
72			void Odometry::init() {
73			// Reset accumulators:
74			resetAccumulators();
75			}
76
77			bool Odometry::update(double left_pos, double right_pos, double dt) {
78			/// Get current wheel joint positions:
79			const double left_wheel_cur_pos = left_pos * left_wheelRadius_;
80			const double right_wheel_cur_pos = right_pos * right_wheelRadius_;
81
82			/// Estimate velocity of wheels using old and current position:
83			const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_;
84			const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;
85
86			/// Update old position with current:
87			left_wheel_old_pos_ = left_wheel_cur_pos;
88			right_wheel_old_pos_ = right_wheel_cur_pos;
89
90			/// Compute linear and angular diff:
91			const double linear = (right_wheel_est_vel + left_wheel_est_vel) * 0.5;
92			const double angular =
93			(right_wheel_est_vel - left_wheel_est_vel) / wheelSeparation_;
94
95			/// Integrate odometry:
96			integrate_fun_(linear, angular);
97
98			/// We cannot estimate the speed with very small time intervals:
99			if (dt < 0.0001) return false; // Interval too small to integrate with
100
101			/// Estimate speeds using a rolling mean to filter them out:
102			linear_acc_(linear / dt);
103			angular_acc_(angular / dt);
104
105			linear_ = bacc::rolling_mean(linear_acc_);
106			angular_ = bacc::rolling_mean(angular_acc_);
107
108			return true;
109			}
110
111			void Odometry::updateOpenLoop(double linear, double angular, double dt) {
112			/// Save last linear and angular velocity:
113			linear_ = linear;
114			angular_ = angular;
115
116			/// Integrate odometry:
117			integrate_fun_(linear * dt, angular * dt);
118			}
119
120			void Odometry::setWheelParams(double wheelSeparation, double left_wheelRadius,
121			double right_wheelRadius) {
122			wheelSeparation_ = wheelSeparation;
123			left_wheelRadius_ = left_wheelRadius;
124			right_wheelRadius_ = right_wheelRadius;
125			}
126
127			void Odometry::setVelocityRollingWindowSize(
128			size_t velocity_rolling_window_size) {
129			velocity_rolling_window_size_ = velocity_rolling_window_size;
130
131			resetAccumulators();
132			}
133
134			void Odometry::integrateRungeKutta2(double linear, double angular) {
135			const double direction = heading_ + angular * 0.5;
136
137			/// Runge-Kutta 2nd order integration:
138			x_ += linear * cos(direction);
139			y_ += linear * sin(direction);
140			heading_ += angular;
141			}
142
143			/**
144			* \brief Other possible integration method provided by the class
145			* \param linear
146			* \param angular
147			*/
148			void Odometry::integrateExact(double linear, double angular) {
149			if (fabs(angular) < 1e-6)
150			integrateRungeKutta2(linear, angular);
151			else {
152			/// Exact integration (should solve problems when angular is zero):
153			const double heading_old = heading_;
154			const double r = linear / angular;
155			heading_ += angular;
156			x_ += r * (sin(heading_) - sin(heading_old));
157			y_ += -r * (cos(heading_) - cos(heading_old));
158			}
159			}
160
161			void Odometry::resetAccumulators() {
162			linear_acc_ = RollingMeanAcc(RollingWindow::window_size =
163			velocity_rolling_window_size_);
164			angular_acc_ = RollingMeanAcc(RollingWindow::window_size =
165			velocity_rolling_window_size_);
166			}
167
168			} // namespace details
169
170			namespace bacc = boost::accumulators;
171
172			Odometry::Odometry(const std::string& name)
173			: Entity(name),
174			wheelsPositionSIN(
175			NULL, "Odometry(" + name + ")::input(vector)::wheelsPosition"),
176			baseVelocityEstimationSOUT(
177			boost::bind(&Odometry::computeVelocityEstimation, this, _1, _2),
178			wheelsPositionSIN,
179			"Odometry(" + name + ")::output(vector)::baseVelocityEstimation"),
180			baseVelocityFilteredEstimationSOUT(
181			boost::bind(&Odometry::computeVelocityFilteredEstimation, this, _1,
182			_2),
183			baseVelocityEstimationSOUT,
184			"Odometry(" + name +
185			")::output(vector)::baseVelocityFilteredEstimation"),
186			baseVelocitySIN(NULL,
187			"Odometry(" + name + ")::input(vector)::baseVelocity"),
188			baseConfigSOUT(boost::bind(&Odometry::computeBaseConfig, this, _1, _2),
189			baseVelocitySIN,
190			"Odometry(" + name + ")::output(vector)::baseConfig"),
191			basePoseSOUT(
192			boost::bind(&Odometry::computeBasePose, this, _1, _2), baseConfigSOUT,
193			"Odometry(" + name + ")::output(matrixHomogeneous)::basePose")
194
195			,
196			dt_(1.),
197			x_(0.0),
198			y_(0.0),
199			heading_(0.0),
200			wheelSeparationInv_(0.0),
201			wheelRadii_(0.0, 0.0),
202			wheelOldPos_(0.0, 0.0),
203			rollingWindowSize_(10),
204			linear_acc_(RollingWindow::window_size = rollingWindowSize_),
205			angular_acc_(RollingWindow::window_size = rollingWindowSize_) {
206			signalRegistration(wheelsPositionSIN << baseVelocityEstimationSOUT
207			<< baseVelocityFilteredEstimationSOUT
208			<< baseVelocitySIN << baseConfigSOUT
209			<< basePoseSOUT);
210
211			// By default, use the velocity estimation
212			// (i.e. loop closed with wheel position sensors).
213			baseVelocitySIN.plug(&baseVelocityEstimationSOUT);
214
215			addCommand("setWheelParams",
216			command::makeCommandVoid3(
217			*this, &Odometry::setWheelParams,
218			command::docCommandVoid3(
219			"Sets the wheel parameters: radius and separation",
220			"double: Separation between left and right wheels [m]",
221			"double: Left wheel radius [m]",
222			"double: Right wheel radius [m]")));
223			addCommand("setBasePose",
224			command::makeCommandVoid3(
225			*this, &Odometry::setBasePose,
226			command::docCommandVoid3(
227			"Set the base pose to integrate from.", "double: x [m]",
228			"double: y [m]", "double: heading [rad]")));
229			addCommand("setPeriod",
230			command::makeDirectSetter(*this, &dt_,
231			"Set period (only affects the velocity "
232			"estimation and nothing else)."));
233			addCommand("getPeriod", command::makeDirectGetter(*this, &dt_, "the period"));
234			}
235
236			Vector& Odometry::computeVelocityEstimation(Vector& vel, int time) {
237			vel.resize(2);
238			const Vector& wheelCurAng = wheelsPositionSIN(time);
239
240			/// Get current wheel joint positions:
241			Eigen::Array2d wheelCurPos = wheelCurAng.array() * wheelRadii_;
242
243			/// Estimate velocity of wheels using old and current position:
244			Eigen::Array2d wheelEstVel = (wheelCurPos - wheelOldPos_) / dt_;
245
246			/// Update old position with current:
247			wheelOldPos_ = wheelCurPos;
248
249			/// Compute linear and angular diff:
250			vel[0] = (wheelEstVel[1] + wheelEstVel[0]) * 0.5;
251			vel[1] = (wheelEstVel[1] - wheelEstVel[0]) * wheelSeparationInv_;
252
253			/// Estimate speeds using a rolling mean to filter them out:
254			linear_acc_(vel[0]);
255			angular_acc_(vel[1]);
256
257			return vel;
258			}
259
260			Vector& Odometry::computeVelocityFilteredEstimation(Vector& vel, int time) {
261			baseVelocityEstimationSOUT.recompute(time);
262
263			vel.resize(2);
264			vel[0] = bacc::rolling_mean(linear_acc_);
265			vel[1] = bacc::rolling_mean(angular_acc_);
266
267			return vel;
268			}
269
270			Vector& Odometry::computeBaseConfig(Vector& base, int time) {
271			base.resize(3);
272			const Vector& vel = baseVelocitySIN(time);
273			integrateExact(vel[0] * dt_, vel[1] * dt_);
274
275			base << x_, y_, heading_;
276			return base;
277			}
278
279			sot::MatrixHomogeneous& Odometry::computeBasePose(sot::MatrixHomogeneous& M,
280			int time) {
281			const Vector& base = baseConfigSOUT(time);
282			M.setIdentity();
283			M.translation().head<2>() = base.head<2>();
284			double c = cos(base(2)), s = sin(base(2));
285			M.linear().topLeftCorner<2, 2>() << c, -s, s, c;
286			return M;
287			}
288
289			void Odometry::setBasePose(const double& x, const double& y,
290			const double& heading) {
291			x_ = x;
292			y_ = y;
293			heading_ = heading;
294
295			resetAccumulators();
296			}
297
298			void Odometry::setWheelParams(const double& wheelSeparation,
299			const double& leftWheelRadius,
300			const double& rightWheelRadius) {
301			wheelSeparationInv_ = 1 / wheelSeparation;
302			wheelRadii_ << leftWheelRadius, rightWheelRadius;
303			}
304
305			void Odometry::setVelocityRollingWindowSize(const size_t& rollingWindowSize) {
306			rollingWindowSize_ = rollingWindowSize;
307			resetAccumulators();
308			}
309
310			void Odometry::integrateRungeKutta2(double linear, double angular) {
311			const double direction = heading_ + angular * 0.5;
312
313			/// Runge-Kutta 2nd order integration:
314			x_ += linear * cos(direction);
315			y_ += linear * sin(direction);
316			heading_ += angular;
317			}
318
319			/**
320			* \brief Other possible integration method provided by the class
321			* \param linear
322			* \param angular
323			*/
324			void Odometry::integrateExact(double linear, double angular) {
325			if (fabs(angular) < 1e-6)
326			integrateRungeKutta2(linear, angular);
327			else {
328			/// Exact integration (should solve problems when angular is zero):
329			const double heading_old = heading_;
330			const double r = linear / angular;
331			heading_ += angular;
332			x_ += r * (sin(heading_) - sin(heading_old));
333			y_ += -r * (cos(heading_) - cos(heading_old));
334			}
335			}
336
337			void Odometry::resetAccumulators() {
338			linear_acc_ = RollingMeanAcc(RollingWindow::window_size = rollingWindowSize_);
339			angular_acc_ =
340			RollingMeanAcc(RollingWindow::window_size = rollingWindowSize_);
341			}
342
343			DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Odometry, "Odometry");
344
345			} // namespace dynamicgraph