Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • mobile_robotics/wolf_projects/wolf_lib/wolf
1 result
Show changes
View open merge request
Hide whitespace changes
Inline Side-by-side
Showing
with 3188 additions and 3341 deletions
include/core/math/rotations.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/**
* \file rotations.h
*
* Created on: Sep 6, 2016
* \author: jsola
*/
#ifndef ROTATIONS_H_
#define ROTATIONS_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/common/wolf.h"
namespace wolf
{
//////////////////////////////////////////////////////////////
/** \brief Return angle between -pi and pi
* This implementation uses std::remainder which makes it incompatible for ceres::jet (this implementation was included in ceres_wrapper/wolf_jet.h)
* This implementation uses std::remainder which makes it incompatible for ceres::jet (this implementation was included
* in ceres_wrapper/wolf_jet.h)
* @param angle
* @return formatted angle
*/
template<typename T>
template <typename T>
inline T pi2pi(const T& _angle)
{
return remainder(_angle,2*M_PI);
}
// inline long double pi2pi(const long double& _angle)
// {
// return std::remainder(_angle,2*M_PI);
// }
// inline double pi2pi(const double& _angle)
// {
// return std::remainder(_angle,2*M_PI);
// }
// inline float pi2pi(const float& _angle)
// {
// return std::remainder(_angle,2*M_PI);
// }
return remainder(_angle, 2 * M_PI);
}
/** \brief Conversion to radians
*
* @param deg angle in degrees
* @return angle in radians
*/
template<typename T>
inline T
toRad(const T deg)
template <typename T>
inline T toRad(const T deg)
{
return (T)M_TORAD * deg;
}
......@@ -76,9 +52,8 @@ toRad(const T deg)
* @param rad angle in radians
* @return angle in degrees
*/
template<typename T>
inline T
toDeg(const T rad)
template <typename T>
inline T toDeg(const T rad)
{
return (T)M_TODEG * rad;
}
......@@ -91,19 +66,16 @@ toDeg(const T rad)
* @param _v a 3d vector
* @return the skew-symmetric matrix V so that V*u = _v.cross(u), for u in R^3
*/
template<typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
skew(const Eigen::MatrixBase<Derived>& _v)
template <typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3> skew(const Eigen::MatrixBase<Derived>& _v)
{
MatrixSizeCheck<3,1>::check(_v);
MatrixSizeCheck<3, 1>::check(_v);
typedef typename Derived::Scalar T;
Eigen::Matrix<T, 3, 3> sk;
sk << (T)0.0 , -_v(2), +_v(1),
+_v(2), (T)0.0, -_v(0),
-_v(1), +_v(0), (T)0.0;
sk << (T)0.0, -_v(2), +_v(1), +_v(2), (T)0.0, -_v(0), -_v(1), +_v(0), (T)0.0;
return sk;
}
......@@ -113,11 +85,10 @@ skew(const Eigen::MatrixBase<Derived>& _v)
* @param _m A skew-symmetric matrix
* @return a 3-vector v such that skew(v) = _m
*/
template<typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
vee(const Eigen::MatrixBase<Derived>& _m)
template <typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 1> vee(const Eigen::MatrixBase<Derived>& _m)
{
MatrixSizeCheck<3,3>::check(_m);
MatrixSizeCheck<3, 3>::check(_m);
typedef typename Derived::Scalar T;
......@@ -134,9 +105,8 @@ vee(const Eigen::MatrixBase<Derived>& _m)
* @param _q a right-handed unit quaternion
* @return the equivalent rotation matrix
*/
template<typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
q2R(const Eigen::QuaternionBase<Derived>& _q)
template <typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3> q2R(const Eigen::QuaternionBase<Derived>& _q)
{
return _q.matrix();
}
......@@ -146,13 +116,12 @@ q2R(const Eigen::QuaternionBase<Derived>& _q)
* @param _q a right-handed unit quaternion
* @return the equivalent rotation matrix
*/
template<typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
q2R(const Eigen::MatrixBase<Derived>& _q)
template <typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3> q2R(const Eigen::MatrixBase<Derived>& _q)
{
MatrixSizeCheck<4,1>::check(_q);
Eigen::Quaternion<typename Derived::Scalar> q(_q(3),_q(0),_q(1),_q(2));
return q2R( q );
MatrixSizeCheck<4, 1>::check(_q);
Eigen::Quaternion<typename Derived::Scalar> q(_q(3), _q(0), _q(1), _q(2));
return q2R(q);
}
/** \brief rotation matrix to quaternion conversion
......@@ -160,11 +129,10 @@ q2R(const Eigen::MatrixBase<Derived>& _q)
* @param _R a rotation matrix
* @return the equivalent right-handed unit quaternion
*/
template<typename Derived>
inline Eigen::Quaternion<typename Derived::Scalar>
R2q(const Eigen::MatrixBase<Derived>& _R)
template <typename Derived>
inline Eigen::Quaternion<typename Derived::Scalar> R2q(const Eigen::MatrixBase<Derived>& _R)
{
MatrixSizeCheck<3,3>::check(_R);
MatrixSizeCheck<3, 3>::check(_R);
return Eigen::Quaternion<typename Derived::Scalar>(_R);
}
......@@ -173,11 +141,10 @@ R2q(const Eigen::MatrixBase<Derived>& _R)
* \param _euler = (roll pitch yaw) in ZYX convention
* \return equivalent quaternion
*/
template<typename D>
inline Eigen::Quaternion<typename D::Scalar>
e2q(const Eigen::MatrixBase<D>& _euler)
template <typename D>
inline Eigen::Quaternion<typename D::Scalar> e2q(const Eigen::MatrixBase<D>& _euler)
{
MatrixSizeCheck<3,1>::check(_euler);
MatrixSizeCheck<3, 1>::check(_euler);
typedef typename D::Scalar T;
......@@ -192,9 +159,8 @@ e2q(const Eigen::MatrixBase<D>& _euler)
* \param _e = (roll pitch yaw) in ZYX convention
* \return equivalent rotation matrix
*/
template<typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
e2R(const Eigen::MatrixBase<Derived>& _e)
template <typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3> e2R(const Eigen::MatrixBase<Derived>& _e)
{
MatrixSizeCheck<3, 1>::check(_e);
......@@ -202,66 +168,62 @@ e2R(const Eigen::MatrixBase<Derived>& _e)
}
template <typename Derived>
inline typename Eigen::MatrixBase<Derived>::Scalar
getYaw(const Eigen::MatrixBase<Derived>& R)
inline typename Eigen::MatrixBase<Derived>::Scalar getYaw(const Eigen::MatrixBase<Derived>& R)
{
MatrixSizeCheck<3, 3>::check(R);
using std::atan2;
return atan2( R(1, 0), R(0, 0) );
return atan2(R(1, 0), R(0, 0));
}
template<typename Derived>
inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1>
R2e(const Eigen::MatrixBase<Derived>& _R)
template <typename Derived>
inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1> R2e(const Eigen::MatrixBase<Derived>& _R)
{
Eigen::Matrix<typename Derived::Scalar, 3, 1> e;
e(0) = atan2( _R(2,1), _R(2,2));
e(1) = atan2(-_R(2,0), sqrt(_R(0,0)*_R(0,0) + _R(1,0)*_R(1,0)));
e(2) = atan2( _R(1,0), _R(0,0));
e(0) = atan2(_R(2, 1), _R(2, 2));
e(1) = atan2(-_R(2, 0), sqrt(_R(0, 0) * _R(0, 0) + _R(1, 0) * _R(1, 0)));
e(2) = atan2(_R(1, 0), _R(0, 0));
return e;
}
template<typename Derived>
inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1>
q2e(const Eigen::MatrixBase<Derived>& _q)
template <typename Derived>
inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1> q2e(const Eigen::MatrixBase<Derived>& _q)
{
typedef typename Derived::Scalar T;
Eigen::Matrix<T, 3, 1> e;
double w = _q(3);
double x = _q(0);
double y = _q(1);
double z = _q(2);
double xx = x*x;
double xy = x*y;
double xz = x*z;
double yy = y*y;
double yz = y*z;
double zz = z*z;
double wx = w*x;
double wy = w*y;
double wz = w*z;
double r00 = T(1) - T(2) * ( yy + zz );
double r10 = T(2) * ( xy + wz );
double r20 = T(2) * ( xz - wy );
double r21 = T(2) * ( yz + wx );
double r22 = T(1) - T(2) * ( xx + yy );
e(0) = atan2( r21, r22);
e(1) = atan2(-r20, sqrt(r00*r00 + r10*r10));
e(2) = atan2( r10, r00);
Eigen::Matrix<T, 3, 1> e;
double w = _q(3);
double x = _q(0);
double y = _q(1);
double z = _q(2);
double xx = x * x;
double xy = x * y;
double xz = x * z;
double yy = y * y;
double yz = y * z;
double zz = z * z;
double wx = w * x;
double wy = w * y;
double wz = w * z;
double r00 = T(1) - T(2) * (yy + zz);
double r10 = T(2) * (xy + wz);
double r20 = T(2) * (xz - wy);
double r21 = T(2) * (yz + wx);
double r22 = T(1) - T(2) * (xx + yy);
e(0) = atan2(r21, r22);
e(1) = atan2(-r20, sqrt(r00 * r00 + r10 * r10));
e(2) = atan2(r10, r00);
return e;
}
template<typename Derived>
inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1>
q2e(const Eigen::QuaternionBase<Derived>& _q)
template <typename Derived>
inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1> q2e(const Eigen::QuaternionBase<Derived>& _q)
{
return q2e(_q.coeffs());
}
......@@ -274,28 +236,27 @@ q2e(const Eigen::QuaternionBase<Derived>& _q)
* @param _v a rotation vector with _v.norm() the rotated angle and _v.normalized() the rotation axis.
* @return the right-handed unit quaternion performing the rotation encoded by _v
*/
template<typename Derived>
inline Eigen::Quaternion<typename Derived::Scalar>
exp_q(const Eigen::MatrixBase<Derived>& _v)
template <typename Derived>
inline Eigen::Quaternion<typename Derived::Scalar> exp_q(const Eigen::MatrixBase<Derived>& _v)
{
using std::sqrt;
using std::cos;
using std::sin;
using std::sqrt;
MatrixSizeCheck<3,1>::check(_v);
MatrixSizeCheck<3, 1>::check(_v);
typedef typename Derived::Scalar T;
T angle_squared = _v.squaredNorm();
T angle = sqrt(angle_squared); // Allow ceres::Jet to use its own sqrt() version.
T angle = sqrt(angle_squared); // Allow ceres::Jet to use its own sqrt() version.
if (angle > (T)(wolf::Constants::EPS_SMALL))
{
return Eigen::Quaternion<T> ( Eigen::AngleAxis<T>(angle, _v.normalized()) );
return Eigen::Quaternion<T>(Eigen::AngleAxis<T>(angle, _v.normalized()));
}
else
{
return Eigen::Quaternion<T> ( (T)1.0 , _v(0,0)/(T)2 , _v(1,0)/(T)2 , _v(2,0)/(T)2 );
return Eigen::Quaternion<T>((T)1.0, _v(0, 0) / (T)2, _v(1, 0) / (T)2, _v(2, 0) / (T)2);
}
}
......@@ -304,11 +265,9 @@ exp_q(const Eigen::MatrixBase<Derived>& _v)
* @param _q a unit right-handed quaternion
* @return a rotation vector v such that _q = exp_q(v)
*/
template<typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
log_q(const Eigen::QuaternionBase<Derived>& _q)
template <typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 1> log_q(const Eigen::QuaternionBase<Derived>& _q)
{
// Will try this implementation once Eigen accepts it!
// see https://forum.kde.org/viewtopic.php?f=74&t=143269&p=385299#p385265
// typedef typename Derived::Scalar T;
......@@ -319,11 +278,11 @@ log_q(const Eigen::QuaternionBase<Derived>& _q)
typedef typename Derived::Scalar T;
Eigen::Matrix<T, 3, 1> vec = _q.vec();
const T sin_angle_squared = vec.squaredNorm();
Eigen::Matrix<T, 3, 1> vec = _q.vec();
const T sin_angle_squared = vec.squaredNorm();
if (sin_angle_squared > (T)wolf::Constants::EPS_SMALL)
{
const T sin_angle = sqrt(sin_angle_squared); // Allow ceres::Jet to use its own sqrt() version.
const T sin_angle = sqrt(sin_angle_squared); // Allow ceres::Jet to use its own sqrt() version.
const T& cos_angle = _q.w();
/* If (cos_angle < 0) then angle >= pi/2 , means : angle for angle_axis vector >= pi (== 2*angle)
......@@ -335,7 +294,8 @@ log_q(const Eigen::QuaternionBase<Derived>& _q)
angle - pi = atan(sin(angle - pi), cos(angle - pi))
= atan(-sin(angle), -cos(angle))
*/
const T two_angle = T(2.0) * ((cos_angle < T(0.0)) ? atan2(-sin_angle, -cos_angle) : atan2(sin_angle, cos_angle));
const T two_angle =
T(2.0) * ((cos_angle < T(0.0)) ? atan2(-sin_angle, -cos_angle) : atan2(sin_angle, cos_angle));
const T k = two_angle / sin_angle;
return vec * k;
}
......@@ -351,9 +311,8 @@ log_q(const Eigen::QuaternionBase<Derived>& _q)
* @param _v a rotation vector
* @return the rotation matrix performing the rotation encoded by _v
*/
template<typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
exp_R(const Eigen::MatrixBase<Derived>& _v)
template <typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3> exp_R(const Eigen::MatrixBase<Derived>& _v)
{
using std::sqrt;
......@@ -362,7 +321,7 @@ exp_R(const Eigen::MatrixBase<Derived>& _v)
typedef typename Derived::Scalar T;
T angle_squared = _v.squaredNorm();
T angle = sqrt(angle_squared); // Allow ceres::Jet to use its own sqrt() version.
T angle = sqrt(angle_squared); // Allow ceres::Jet to use its own sqrt() version.
if (angle > wolf::Constants::EPS_SMALL)
return Eigen::AngleAxis<T>(angle, _v.normalized()).toRotationMatrix();
......@@ -375,9 +334,8 @@ exp_R(const Eigen::MatrixBase<Derived>& _v)
* @param _R a 3d rotation matrix
* @return the rotation vector v such that _R = exp_R(v)
*/
template<typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
log_R(const Eigen::MatrixBase<Derived>& _R)
template <typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 1> log_R(const Eigen::MatrixBase<Derived>& _R)
{
return log_q(R2q(_R));
}
......@@ -387,9 +345,8 @@ log_R(const Eigen::MatrixBase<Derived>& _R)
* @param _v a rotation vector
* @return the equivalent right-handed unit quaternion
*/
template<typename Derived>
inline Eigen::Quaternion<typename Derived::Scalar>
v2q(const Eigen::MatrixBase<Derived>& _v)
template <typename Derived>
inline Eigen::Quaternion<typename Derived::Scalar> v2q(const Eigen::MatrixBase<Derived>& _v)
{
return exp_q(_v);
}
......@@ -399,9 +356,8 @@ v2q(const Eigen::MatrixBase<Derived>& _v)
* @param _q a right-handed unit quaternion
* @return the equivalent rotation vector
*/
template<typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
q2v(const Eigen::QuaternionBase<Derived>& _q)
template <typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 1> q2v(const Eigen::QuaternionBase<Derived>& _q)
{
return log_q(_q);
}
......@@ -411,9 +367,8 @@ q2v(const Eigen::QuaternionBase<Derived>& _q)
* @param _v a rotation vector
* @return the equivalent rotation matrix
*/
template<typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
v2R(const Eigen::MatrixBase<Derived>& _v)
template <typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3> v2R(const Eigen::MatrixBase<Derived>& _v)
{
return exp_R(_v);
}
......@@ -423,9 +378,8 @@ v2R(const Eigen::MatrixBase<Derived>& _v)
* @param _R a rotation matrix
* @return the equivalent rotation vector
*/
template<typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
R2v(const Eigen::MatrixBase<Derived>& _R)
template <typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 1> R2v(const Eigen::MatrixBase<Derived>& _R)
{
return log_R(_R);
}
......@@ -447,23 +401,22 @@ R2v(const Eigen::MatrixBase<Derived>& _R)
* exp(theta+d_theta) = exp(theta)*exp(Jr(theta)*d_theta)
*/
template<typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
jac_SO3_right(const Eigen::MatrixBase<Derived>& _theta)
template <typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_right(const Eigen::MatrixBase<Derived>& _theta)
{
using std::sqrt;
using std::cos;
using std::sin;
using std::sqrt;
MatrixSizeCheck<3, 1>::check(_theta);
typedef typename Derived::Scalar T;
T theta2 = _theta.squaredNorm();
T theta2 = _theta.squaredNorm();
Eigen::Matrix<T, 3, 3> W(skew(_theta));
if (theta2 <= Constants::EPS_SMALL)
return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W; // Small angle approximation
T theta = sqrt(theta2); // rotation angle
return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W; // Small angle approximation
T theta = sqrt(theta2); // rotation angle
Eigen::Matrix<T, 3, 3> M1, M2;
M1.noalias() = ((T)1 - cos(theta)) / theta2 * W;
M2.noalias() = (theta - sin(theta)) / (theta2 * theta) * (W * W);
......@@ -479,7 +432,8 @@ jac_SO3_right(const Eigen::MatrixBase<Derived>& _theta)
*
* where Jrinv = jac_SO3_right_inv(theta);
*
* This maps a perturbation on the manifold (expmap(theta)) to a perturbation in the tangent space (Jrinv * theta) so that
* This maps a perturbation on the manifold (expmap(theta)) to a perturbation in the tangent space (Jrinv * theta) so
* that
*
* log( exp(theta) * exp(d_theta) ) = theta + Jrinv(theta) * d_theta
*
......@@ -487,26 +441,25 @@ jac_SO3_right(const Eigen::MatrixBase<Derived>& _theta)
*
* log( R * exp(d_theta) ) = log(R) + Jrinv(theta) * d_theta
*/
template<typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
jac_SO3_right_inv(const Eigen::MatrixBase<Derived>& _theta)
template <typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_right_inv(const Eigen::MatrixBase<Derived>& _theta)
{
using std::sqrt;
using std::cos;
using std::sin;
using std::sqrt;
MatrixSizeCheck<3, 1>::check(_theta);
typedef typename Derived::Scalar T;
T theta2 = _theta.squaredNorm();
T theta2 = _theta.squaredNorm();
Eigen::Matrix<T, 3, 3> W(skew(_theta));
if (theta2 <= Constants::EPS_SMALL)
return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W; // Small angle approximation
T theta = sqrt(theta2); // rotation angle
return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W; // Small angle approximation
T theta = sqrt(theta2); // rotation angle
Eigen::Matrix<T, 3, 3> M;
M.noalias() = ((T)1.0 / theta2 - ((T)1.0 + cos(theta)) / ((T)2.0 * theta * sin(theta))) * (W * W);
return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W + M; //is this really more optimized?
return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W + M; // is this really more optimized?
}
/** \brief Compute Jl (Left Jacobian)
......@@ -522,23 +475,22 @@ jac_SO3_right_inv(const Eigen::MatrixBase<Derived>& _theta)
*
* exp(theta+d_theta) = exp(Jl(theta)*d_theta)*exp(theta)
*/
template<typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
jac_SO3_left(const Eigen::MatrixBase<Derived>& _theta)
template <typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_left(const Eigen::MatrixBase<Derived>& _theta)
{
using std::sqrt;
using std::cos;
using std::sin;
using std::sqrt;
MatrixSizeCheck<3, 1>::check(_theta);
typedef typename Derived::Scalar T;
T theta2 = _theta.squaredNorm();
T theta2 = _theta.squaredNorm();
Eigen::Matrix<T, 3, 3> W(skew(_theta));
if (theta2 <= Constants::EPS_SMALL)
return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W; // Small angle approximation
T theta = sqrt(theta2); // rotation angle
return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W; // Small angle approximation
T theta = sqrt(theta2); // rotation angle
Eigen::Matrix<T, 3, 3> M1, M2;
M1.noalias() = ((T)1 - cos(theta)) / theta2 * W;
M2.noalias() = (theta - sin(theta)) / (theta2 * theta) * (W * W);
......@@ -553,7 +505,8 @@ jac_SO3_left(const Eigen::MatrixBase<Derived>& _theta)
*
* where Jlinv = jac_SO3_left_inv(theta);
*
* This maps a perturbation on the manifold (expmap(theta)) to a perturbation in the tangent space (Jlinv * theta) so that
* This maps a perturbation on the manifold (expmap(theta)) to a perturbation in the tangent space (Jlinv * theta) so
* that
*
* log( exp(d_theta) * exp(theta) ) = theta + Jlinv(theta) * d_theta
*
......@@ -561,113 +514,107 @@ jac_SO3_left(const Eigen::MatrixBase<Derived>& _theta)
*
* log( exp(d_theta) * R ) = log(R) + Jlinv(theta) * d_theta
*/
template<typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
jac_SO3_left_inv(const Eigen::MatrixBase<Derived>& _theta)
template <typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_left_inv(const Eigen::MatrixBase<Derived>& _theta)
{
using std::sqrt;
using std::cos;
using std::sin;
using std::sqrt;
MatrixSizeCheck<3, 1>::check(_theta);
typedef typename Derived::Scalar T;
T theta2 = _theta.squaredNorm();
T theta2 = _theta.squaredNorm();
Eigen::Matrix<T, 3, 3> W(skew(_theta));
if (theta2 <= Constants::EPS_SMALL)
return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W; // Small angle approximation
T theta = sqrt(theta2); // rotation angle
return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W; // Small angle approximation
T theta = sqrt(theta2); // rotation angle
Eigen::Matrix<T, 3, 3> M;
M.noalias() = ((T)1.0 / theta2 - ((T)1.0 + cos(theta)) / ((T)2.0 * theta * sin(theta))) * (W * W);
return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W + M; //is this really more optimized?
return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W + M; // is this really more optimized?
}
template<typename D1, typename D2, typename D3, typename D4, typename D5>
inline void
compose(const Eigen::QuaternionBase<D1>& _q1,
const Eigen::QuaternionBase<D2>& _q2,
Eigen::QuaternionBase<D3>& _q_comp,
Eigen::MatrixBase<D4>& _J_comp_q1,
Eigen::MatrixBase<D5>& _J_comp_q2)
template <typename D1, typename D2, typename D3, typename D4, typename D5>
inline void compose(const Eigen::QuaternionBase<D1>& _q1,
const Eigen::QuaternionBase<D2>& _q2,
Eigen::QuaternionBase<D3>& _q_comp,
Eigen::MatrixBase<D4>& _J_comp_q1,
Eigen::MatrixBase<D5>& _J_comp_q2)
{
MatrixSizeCheck<3, 3>::check(_J_comp_q1);
MatrixSizeCheck<3, 3>::check(_J_comp_q2);
_q_comp = _q1 * _q2;
_J_comp_q1 = q2R(_q2.conjugate()); // R2.tr
_J_comp_q2 . setIdentity();
_J_comp_q1 = q2R(_q2.conjugate()); // R2.tr
_J_comp_q2.setIdentity();
}
template<typename D1, typename D2, typename D3, typename D4, typename D5>
inline void
between(const Eigen::QuaternionBase<D1>& _q1,
const Eigen::QuaternionBase<D2>& _q2,
Eigen::QuaternionBase<D3>& _q_between,
Eigen::MatrixBase<D4>& _J_between_q1,
Eigen::MatrixBase<D5>& _J_between_q2)
template <typename D1, typename D2, typename D3, typename D4, typename D5>
inline void between(const Eigen::QuaternionBase<D1>& _q1,
const Eigen::QuaternionBase<D2>& _q2,
Eigen::QuaternionBase<D3>& _q_between,
Eigen::MatrixBase<D4>& _J_between_q1,
Eigen::MatrixBase<D5>& _J_between_q2)
{
MatrixSizeCheck<3, 3>::check(_J_between_q1);
MatrixSizeCheck<3, 3>::check(_J_between_q2);
_q_between = _q1.conjugate() * _q2;
_J_between_q1 = -q2R(_q2.conjugate()*_q1); // - R2.tr * R1
_J_between_q2 . setIdentity();
_J_between_q1 = -q2R(_q2.conjugate() * _q1); // - R2.tr * R1
_J_between_q2.setIdentity();
}
template<typename D1, typename D2>
inline Eigen::Quaternion<typename D1::Scalar>
plus_right(const Eigen::QuaternionBase<D1>& q, const Eigen::MatrixBase<D2>& v)
template <typename D1, typename D2>
inline Eigen::Quaternion<typename D1::Scalar> plus_right(const Eigen::QuaternionBase<D1>& q,
const Eigen::MatrixBase<D2>& v)
{
MatrixSizeCheck<3,1>::check(v);
MatrixSizeCheck<3, 1>::check(v);
return q * exp_q(v);
}
template<typename D1, typename D2>
inline Eigen::Matrix<typename D2::Scalar, 3, 1>
minus_right(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
template <typename D1, typename D2>
inline Eigen::Matrix<typename D2::Scalar, 3, 1> minus_right(const Eigen::QuaternionBase<D1>& q1,
const Eigen::QuaternionBase<D2>& q2)
{
return log_q(q1.conjugate() * q2);
}
template<typename D1, typename D2>
inline Eigen::Quaternion<typename D1::Scalar>
plus_left(const Eigen::MatrixBase<D2>& v, const Eigen::QuaternionBase<D1>& q)
template <typename D1, typename D2>
inline Eigen::Quaternion<typename D1::Scalar> plus_left(const Eigen::MatrixBase<D2>& v,
const Eigen::QuaternionBase<D1>& q)
{
MatrixSizeCheck<3,1>::check(v);
MatrixSizeCheck<3, 1>::check(v);
return exp_q(v) * q;
}
template<typename D1, typename D2>
inline Eigen::Matrix<typename D2::Scalar, 3, 1>
minus_left(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
template <typename D1, typename D2>
inline Eigen::Matrix<typename D2::Scalar, 3, 1> minus_left(const Eigen::QuaternionBase<D1>& q1,
const Eigen::QuaternionBase<D2>& q2)
{
return log_q(q2 * q1.conjugate());
}
template<typename D1, typename D2>
inline Eigen::Quaternion<typename D1::Scalar>
plus(const Eigen::QuaternionBase<D1>& q, const Eigen::MatrixBase<D2>& v)
template <typename D1, typename D2>
inline Eigen::Quaternion<typename D1::Scalar> plus(const Eigen::QuaternionBase<D1>& q, const Eigen::MatrixBase<D2>& v)
{
return plus_right(q, v);
}
template<typename D1, typename D2>
inline Eigen::Matrix<typename D2::Scalar, 3, 1>
minus(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
template <typename D1, typename D2>
inline Eigen::Matrix<typename D2::Scalar, 3, 1> minus(const Eigen::QuaternionBase<D1>& q1,
const Eigen::QuaternionBase<D2>& q2)
{
return minus_right(q1, q2);
}
template<typename D1, typename D2>
inline Eigen::Matrix<typename D2::Scalar, 3, 1>
diff(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
template <typename D1, typename D2>
inline Eigen::Matrix<typename D2::Scalar, 3, 1> diff(const Eigen::QuaternionBase<D1>& q1,
const Eigen::QuaternionBase<D2>& q2)
{
return minus(q1, q2);
}
} // namespace wolf
#endif /* ROTATIONS_H_ */
} // namespace wolf
include/core/problem/problem.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef PROBLEM_H_
#define PROBLEM_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// Fwd refs
namespace wolf{
namespace wolf
{
class SolverManager;
class HardwareBase;
class TrajectoryBase;
......@@ -33,428 +30,405 @@ class StateBlock;
class TimeStamp;
struct ParamsSensorBase;
struct ParamsProcessorBase;
}
} // namespace wolf
//wolf includes
// wolf includes
#include "core/common/wolf.h"
#include "core/utils/params_server.h"
#include "core/utils/loader.h"
#include "core/common/node_state_blocks.h"
#include "core/frame/frame_base.h"
#include "core/state_block/state_block.h"
#include "core/state_block/state_composite.h"
#include "core/composite/prior_composite.h"
#include "core/composite/type_composite.h"
#include "core/composite/vector_composite.h"
#include "core/processor/motion_provider.h"
// std includes
#include <mutex>
namespace wolf {
// yaml includes
#include "yaml-cpp/yaml.h"
namespace wolf
{
enum Notification
{
ADD,
REMOVE
};
struct PriorOptions
{
std::string mode = "";
VectorComposite state;
MatrixComposite cov;
};
WOLF_STRUCT_PTR_TYPEDEFS(PriorOptions);
/** \brief Wolf problem node element in the Wolf Tree
*/
// Class Problem
class Problem : public std::enable_shared_from_this<Problem>
{
friend SolverManager; // Enable SolverManager to acces protected functions (consumeXXXNotificationMap())
friend SolverManager; // Enable SolverManager to acces protected functions (consumeXXXNotificationMap())
friend ProcessorBase;
friend ProcessorMotion;
friend MotionProvider;
protected:
TreeManagerBasePtr tree_manager_;
HardwareBasePtr hardware_ptr_;
TrajectoryBasePtr trajectory_ptr_;
MapBasePtr map_ptr_;
std::map<int, MotionProviderPtr> motion_provider_map_;
std::map<std::pair<StateBlockConstPtr, StateBlockConstPtr>, Eigen::MatrixXd> covariances_;
SizeEigen state_size_, state_cov_size_, dim_;
std::map<FactorBasePtr, Notification> factor_notification_map_;
std::map<StateBlockPtr, Notification> state_block_notification_map_;
mutable std::mutex mut_notifications_;
mutable std::mutex mut_covariances_;
StateStructure frame_structure_;
PriorOptionsPtr prior_options_;
std::atomic_bool transformed_;
VectorComposite transformation_;
mutable std::mutex mut_transform_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map); // USE create() below !!
void setup();
public:
static ProblemPtr create(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map = std::make_shared<MapBase>()); // USE THIS AS A CONSTRUCTOR!
static ProblemPtr autoSetup(ParamsServer &_server);
virtual ~Problem();
// Properties -----------------------------------------
public:
SizeEigen getDim() const;
const StateStructure& getFrameStructure() const;
protected:
void appendToStructure(const StateStructure& _structure);
// Tree manager --------------------------------------
public:
void setTreeManager(TreeManagerBasePtr _gm);
TreeManagerBaseConstPtr getTreeManager() const;
TreeManagerBasePtr getTreeManager();
// Hardware branch ------------------------------------
HardwareBaseConstPtr getHardware() const;
HardwareBasePtr getHardware();
/** \brief Factory method to install (create and add) sensors only from its properties
* \param _sen_type type of sensor
* \param _unique_sensor_name unique sensor name, used to identify the particular instance of the sensor
* \param _extrinsics a vector of extrinsic parameters: size 2 for 2d position, 3 for 2d pose, 3 for 3d position, 7 for 3d pose.
* \param _intrinsics a base-pointer to a derived struct defining the intrinsic parameters.
*/
SensorBasePtr installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
const Eigen::VectorXd& _extrinsics, //
ParamsSensorBasePtr _intrinsics = nullptr);
/** \brief Factory method to install (create and add) sensors only from its properties -- Helper method loading parameters from file
* \param _sen_type type of sensor
* \param _unique_sensor_name unique sensor name, used to identify the particular instance of the sensor
* \param _extrinsics a vector of extrinsic parameters: size 2 for 2d position, 3 for 2d pose, 3 for 3d position, 7 for 3d pose.
* \param _intrinsics_filename the name of a file containing the intrinsic parameters in a format compatible with the intrinsics creator registered in FactoryParamsSensor under the key _sen_type.
*/
SensorBasePtr installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
const Eigen::VectorXd& _extrinsics, //
const std::string& _intrinsics_filename);
/**
Custom installSensor using the parameters server
*/
SensorBasePtr installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name,
const ParamsServer& _server);
/** \brief get a sensor pointer by its name
* \param _sensor_name The sensor name, as it was installed with installSensor()
*/
SensorBaseConstPtr findSensor(const std::string& _sensor_name) const;
SensorBasePtr findSensor(const std::string& _sensor_name);
/** \brief get a processor pointer by its name
* \param _processor_name The processor name, as it was installed with installProcessor()
*/
ProcessorBaseConstPtr findProcessor(const std::string& _processor_name) const;
ProcessorBasePtr findProcessor(const std::string& _processor_name);
/** \brief Factory method to install (create, and add to sensor) processors only from its properties
*
* This method creates a Processor, and adds it to the specified sensor's list of processors
* \param _prc_type type of processor
* \param _unique_processor_name unique processor name, used to identify the particular instance of the processor
* \param _corresponding_sensor_ptr pointer to the sensor where the processor will be installed.
* \param _prc_params a base-pointer to a derived struct defining the processor parameters.
*/
ProcessorBasePtr installProcessor(const std::string& _prc_type, //
const std::string& _unique_processor_name, //
SensorBasePtr _corresponding_sensor_ptr, //
ParamsProcessorBasePtr _prc_params = nullptr);
/** \brief Factory method to install (create, and add to sensor) processors only from its properties
*
* This method creates a Processor, and adds it to the specified sensor's list of processors
*
* This method is a helper wrapper around the version accepting a sensor pointer instead of a sensor name.
* \param _prc_type type of processor
* \param _unique_processor_name unique processor name, used to identify the particular instance of the processor
* \param _corresponding_sensor_name corresponding sensor name, used to bind the processor to the particular instance of the sensor
* \param _params_filename name of formatted file (xml, yaml, etc) defining the processor parameters.
*/
ProcessorBasePtr installProcessor(const std::string& _prc_type, //
const std::string& _unique_processor_name, //
const std::string& _corresponding_sensor_name, //
const std::string& _params_filename = "");
/**
Custom installProcessor to be used with parameters server
*/
ProcessorBasePtr installProcessor(const std::string& _prc_type, //
const std::string& _unique_processor_name, //
const std::string& _corresponding_sensor_name, //
const ParamsServer& _server);
protected:
/** \brief Set the processor motion
*
* Add a new processor of type is motion to the processor is motion list.
*/
void addMotionProvider(MotionProviderPtr _processor_motion_ptr);
void removeMotionProvider(MotionProviderPtr proc);
public:
std::map<int,MotionProviderConstPtr> getMotionProviderMap() const;
std::map<int,MotionProviderPtr> getMotionProviderMap();
// Trajectory branch ----------------------------------
TrajectoryBaseConstPtr getTrajectory() const;
TrajectoryBasePtr getTrajectory();
// Prior
bool isPriorSet() const;
void setPriorOptions(const std::string& _mode,
const VectorComposite& _state = VectorComposite(),
const VectorComposite& _cov = VectorComposite());
FrameBasePtr applyPriorOptions(const TimeStamp& _ts);
FrameBasePtr setPriorFactor(const VectorComposite &_state,
const VectorComposite &_sigma,
const TimeStamp &_ts);
FrameBasePtr setPriorFix(const VectorComposite &_state,
const TimeStamp &_ts);
FrameBasePtr setPriorInitialGuess(const VectorComposite &_state,
const TimeStamp &_ts);
/** \brief Emplace frame from string frame_structure, dimension and vector
* \param _time_stamp Time stamp of the frame
* \param _frame_structure String indicating the frame structure.
* \param _dim variable indicating the dimension of the problem
* \param _frame_state State vector; must match the size and format of the chosen frame structure
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
* - Create a Frame
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
const StateStructure& _frame_structure, //
const SizeEigen _dim, //
const Eigen::VectorXd& _frame_state);
/** \brief Emplace frame from string frame_structure and state
* \param _time_stamp Time stamp of the frame
* \param _frame_structure String indicating the frame structure.
* \param _frame_state State vector; must match the size and format of the chosen frame structure
*
* - The dimension is taken from Problem
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
* - Create a Frame
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
const StateStructure& _frame_structure, //
const VectorComposite& _frame_state);
/** \brief Emplace frame from state
* \param _time_stamp Time stamp of the frame
* \param _frame_state State; must be part of the problem's frame structure
*
* - The structure is taken from Problem
* - The dimension is taken from Problem
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
* - Create a Frame
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
const VectorComposite& _frame_state);
/** \brief Emplace frame from string frame_structure and dimension
* \param _time_stamp Time stamp of the frame
* \param _frame_structure String indicating the frame structure.
* \param _dim variable indicating the dimension of the problem
*
* - The dimension is taken from Problem
* - The state is taken from Problem
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
* - Create a Frame
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
const StateStructure& _frame_structure, //
const SizeEigen _dim);
/** \brief Emplace frame from state vector
* \param _time_stamp Time stamp of the frame
* \param _frame_state State vector; must match the size and format of the chosen frame structure
*
* - The structure is taken from Problem
* - The dimension is taken from Problem
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
* - Create a Frame
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
const Eigen::VectorXd& _frame_state);
/** \brief Emplace frame, guess all values
* \param _time_stamp Time stamp of the frame
*
* - The structure is taken from Problem
* - The dimension is taken from Problem
* - The state is taken from Problem
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
* - Create a Frame
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp);
// Frame getters
FrameBaseConstPtr getLastFrame( ) const;
FrameBasePtr getLastFrame( );
FrameBaseConstPtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts);
/** \brief Give the permission to a processor to create a new key Frame
*
* This should implement a black list of processors that have forbidden key frame creation.
* - This decision is made at problem level, not at processor configuration level.
* - Therefore, if you want to permanently configure a processor for not creating key frames, see Processor::voting_active_ and its accessors.
*/
bool permitKeyFrame(ProcessorBasePtr _processor_ptr) const;
/** \brief New key frame callback
*
* New key frame callback: It should be called by any processor that creates a new key frame. It calls the keyFrameCallback of the rest of processors.
*/
void keyFrameCallback(FrameBasePtr _keyframe_ptr, //
ProcessorBasePtr _processor_ptr);
// State getters
TimeStamp getTimeStamp ( ) const;
VectorComposite getState ( const StateStructure& _structure = "" ) const;
VectorComposite getState ( const TimeStamp& _ts, const StateStructure& _structure = "" ) const;
VectorComposite getOdometry ( const StateStructure& _structure = "" ) const;
// Zero state provider
VectorComposite stateZero ( const StateStructure& _structure = "" ) const;
// Map branch -----------------------------------------
MapBaseConstPtr getMap() const;
MapBasePtr getMap();
void setMap(MapBasePtr);
void loadMap(const std::string& _filename_dot_yaml);
void saveMap(const std::string& _filename_dot_yaml, //
const std::string& _map_name = "Map automatically saved by Wolf");
// All branches -------------------------------------------
/**
* \brief perturb all states states with random noise
* following an uniform distribution in [ -amplitude, amplitude ]
*/
void perturb(double amplitude = 0.01);
// transform states
void transform(const VectorComposite& _transformation);
bool isTransformed() const;
void resetTransformed();
VectorComposite getTransformation() const;
// Covariances
void clearCovariance();
void addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXd& _cov);
void addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXd& _cov);
bool getCovarianceBlock(StateBlockConstPtr _state1, StateBlockConstPtr _state2, Eigen::MatrixXd& _cov, const int _row = 0, const int _col=0) const;
bool getCovarianceBlock(std::map<StateBlockConstPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const;
bool getCovarianceBlock(StateBlockConstPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col = 0) const;
bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd& _covariance) const;
bool getLastFrameCovariance(Eigen::MatrixXd& _covariance) const;
bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXd& _covariance) const;
// Solver management ----------------------------------------
/** \brief Notifies a new/removed state block to update the solver manager
*/
StateBlockPtr notifyStateBlock(StateBlockPtr _state_ptr, Notification _notif);
/** \brief Returns the size of the map of state block notification
*/
SizeStd getStateBlockNotificationMapSize() const;
/** \brief Returns if the state block has been notified, and the notification via parameter
*/
bool getStateBlockNotification(const StateBlockPtr& sb_ptr, Notification& notif) const;
/** \brief Notifies a new/removed factor to update the solver manager
*/
FactorBasePtr notifyFactor(FactorBasePtr _factor_ptr, Notification _notif);
/** \brief Returns the size of the map of factor notification
*/
SizeStd getFactorNotificationMapSize() const;
/** \brief Returns if the factor has been notified, and the notification via parameter
*/
bool getFactorNotification(const FactorBasePtr& fac_ptr, Notification& notif) const;
protected:
/** \brief Returns the map of state block notification to be handled by the solver (the map stored in this is emptied)
*/
void consumeNotifications(std::map<StateBlockPtr,Notification>&,
std::map<FactorBasePtr,Notification>&);
/** \brief Returns the map of state block notification to be handled by the solver (the map stored in this is emptied)
*/
std::map<StateBlockPtr,Notification> consumeStateBlockNotificationMap();
/** \brief Returns the map of factor notification to be handled by the solver (the map stored in this is emptied)
*/
std::map<FactorBasePtr, Notification> consumeFactorNotificationMap();
public:
// Print and check ---------------------------------------
/**
* \brief print wolf tree
* \param depth : levels to show ( 0: H, T, M : 1: H:S, T:F, M:L ; 2: H:S:p, T:F:C ; 3: T:F:C:f ; 4: T:F:C:f:c )
* \param constr_by : show factors pointing to F, C, f and L.
* \param metric : show metric info (status, time stamps, state vectors, measurements)
* \param state_blocks : show state blocks
*/
void print(int depth = 4, //
bool constr_by = false, //
bool metric = true, //
bool state_blocks = false,
std::ostream& stream = std::cout) const;
bool check(int verbose_level = 0) const;
bool check(bool verbose, std::ostream& stream) const;
protected:
HardwareBasePtr hardware_ptr_;
TrajectoryBasePtr trajectory_ptr_;
MapBasePtr map_ptr_;
TreeManagerBasePtr tree_manager_;
std::map<unsigned int, MotionProviderPtr> motion_provider_map_;
SizeEigen dim_;
TypeComposite frame_types_;
VectorComposite first_frame_values_;
PriorComposite first_frame_priors_;
unsigned int first_frame_status_; ///< 0: not set, 1: set, 2: applied
std::map<std::pair<StateBlockConstPtr, StateBlockConstPtr>, Eigen::MatrixXd> covariances_;
std::map<FactorBasePtr, Notification> factor_notification_map_;
std::map<StateBlockPtr, Notification> state_block_notification_map_;
mutable std::mutex mut_notifications_;
mutable std::mutex mut_covariances_;
std::atomic_bool transformed_;
VectorComposite transformation_;
mutable std::mutex mut_transform_;
LoaderPtr loader_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
Problem(SizeEigen _dim,
const TypeComposite& _frame_types = {},
LoaderPtr _loader = nullptr); // USE create() below !!
void setup();
public:
static ProblemPtr create(SizeEigen _dim,
const TypeComposite& _frame_types = {},
LoaderPtr _loader = nullptr); // USE THIS AS A CONSTRUCTOR!
static ProblemPtr autoSetup(const std::string& _input_yaml_file,
std::vector<std::string> _primary_schema_folders = {},
LoaderPtr _loader = nullptr,
bool _skip_yaml_validation = false);
static ProblemPtr autoSetup(YAML::Node _param_node,
std::vector<std::string> _primary_schema_folders = {},
LoaderPtr _loader = nullptr,
bool _skip_yaml_validation = false);
virtual ~Problem();
// Properties -----------------------------------------
LoaderPtr getLoader();
SizeEigen getDim() const;
TypeComposite getFrameTypes(StateKeys _keys) const;
protected:
void addFrameTypes(const TypeComposite& _types);
// Tree manager --------------------------------------
public:
void setTreeManager(TreeManagerBasePtr _gm);
TreeManagerBaseConstPtr getTreeManager() const;
TreeManagerBasePtr getTreeManager();
// Hardware branch ------------------------------------
HardwareBaseConstPtr getHardware() const;
HardwareBasePtr getHardware();
/** \brief Factory method to install (create and add) sensors only from a YAML node -- Helper method loading
* parameters from file
* \param _sensor_node YAML node containing all necessary information to call the factory and create the sensor.
* \param _schema_folders a vector of paths where the schema files (to validate the YAML node) are placed, if empty the node is not validated.
*/
SensorBasePtr installSensor(const YAML::Node& _sensor_node, std::vector<std::string> _schema_folders = {});
/** \brief Factory method to install (create and add) sensors only from its properties -- Helper method loading
* parameters from file
* \param _params_yaml_filename the name of a file containing the parameters in a format compatible with the intrinsics creator registered in FactoryParamsSensor under the key _sen_type.
* \param _schema_folders a vector of paths where the schema files (to validate the YAML file) are placed
*/
SensorBasePtr installSensor(const std::string& _params_yaml_filename, std::vector<std::string> _schema_folders);
/** \brief Factory method to install (create, and add to sensor) processors only from a YAML node -- Helper method
* loading parameters from file
* \param _processor_node YAML node containing all necessary information to call the factory and create the processor.
* \param _schema_folders a vector of paths where the schema files (to validate the YAML node) are placed, if empty the node is not validated.
*/
ProcessorBasePtr installProcessor(const YAML::Node& _processor_node,
std::vector<std::string> _schema_folders = {});
/** \brief Factory method to install (create, and add to sensor) processors only from a YAML node -- Helper method
* loading parameters from file
* \param _sensor_corresponding corresponding sensor pointer, used to bind the processor to the particular instance of the sensor
* \param _params_yaml_filename name of formatted yaml file defining the processor parameters.
* \param _schema_folders a vector of paths where the schema files (to validate the YAML node) are placed, if empty the node is not validated.
*/
ProcessorBasePtr installProcessor(SensorBasePtr _sensor_corresponding,
const std::string& _params_yaml_filename,
std::vector<std::string> _schema_folders);
/** \brief Factory method to install (create, and add to sensor) processors only from its properties
*
* This method creates a Processor, and adds it to the specified sensor's list of processors
*
* This method is a helper wrapper around the version accepting a sensor pointer instead of a sensor name.
* \param _params_yaml_filename name of formatted yaml file defining the processor parameters.
* \param _schema_folders a vector of paths where the schema files (to validate the YAML file) are placed
*/
ProcessorBasePtr installProcessor(const std::string& _params_yaml_filename,
std::vector<std::string> _schema_folders);
/** \brief get a sensor pointer by its name
* \param _sensor_name The sensor name, as it was installed with installSensor()
*/
SensorBaseConstPtr findSensor(const std::string& _sensor_name) const;
SensorBasePtr findSensor(const std::string& _sensor_name);
/** \brief get a processor pointer by its name
* \param _processor_name The processor name, as it was installed with installProcessor()
*/
ProcessorBaseConstPtr findProcessor(const std::string& _processor_name) const;
ProcessorBasePtr findProcessor(const std::string& _processor_name);
protected:
/** \brief Set the processor motion
*
* Add a new processor of type is motion to the processor is motion list.
*/
void addMotionProvider(MotionProviderPtr _processor_motion_ptr);
void removeMotionProvider(MotionProviderPtr proc);
public:
std::map<unsigned int, MotionProviderConstPtr> getMotionProviderMap() const;
std::map<unsigned int, MotionProviderPtr> getMotionProviderMap();
// Trajectory branch ----------------------------------
TrajectoryBaseConstPtr getTrajectory() const;
TrajectoryBasePtr getTrajectory();
// First Frame
bool isFirstFrameApplied() const;
void setFirstFrameOptions(const YAML::Node& _first_frame_node);
void setFirstFrameOptions(const TypeComposite& _first_frame_types,
const VectorComposite& _first_frame_values,
const PriorComposite& _first_frame_priors);
FrameBasePtr applyFirstFrameOptions(const TimeStamp& _ts);
FrameBasePtr emplaceFirstFrame(const TimeStamp& _ts, const YAML::Node& _first_frame_node);
FrameBasePtr emplaceFirstFrame(const TimeStamp& _ts,
const TypeComposite& _first_frame_types,
const VectorComposite& _first_frame_values,
const PriorComposite& _first_frame_priors);
/** \brief Emplace frame from timestamp, types and and state
* \param _time_stamp Time stamp of the frame
* \param _frame_types TypeComposite indicating the types of each key.
* \param _frame_state VectorComposite with the states
* \param _frame_priors PriorComposite with the priors on the states (not all keys required)
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
* - Create a Frame
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp,
const TypeComposite& _frame_types,
const VectorComposite& _frame_state,
const PriorComposite& _frame_priors = {});
/** \brief Emplace frame from timestamp and keys
* \param _time_stamp Time stamp of the frame
* \param _frame_keys String containing the keys of the desired frame. Empty means all keys available.
*
* - The frame_types are taken from Problem
* - The states are taken from Problem
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
* - Create a Frame
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, const StateKeys& _frame_keys);
/** \brief Emplace frame from timestamps and state
* \param _time_stamp Time stamp of the frame
* \param _frame_state State; must be part of the problem's frame state_keys
*
* - The frame_types are taken from Problem
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
* - Create a Frame
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, const VectorComposite& _frame_state);
/** \brief Emplace frame from timestamp and state vector
* \param _time_stamp Time stamp of the frame
* \param _frame_keys String containing the keys of the desired frame. Empty means all keys available.
* \param _frame_state State vector; must match the size and format of the chosen frame state_keys
*
* - The frame_types are taken from Problem
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
* - Create a Frame
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp,
const StateKeys& _frame_keys,
const Eigen::VectorXd& _frame_state);
/** \brief Take a frame and emplace to it the missing states with the frame timestamp
* \param _frame_ptr Frame to which states have to be emplaced
* \param _frame_keys the keys that the frame is requested to have.
*
* - The frame_types are taken from Problem.
* - Only the missing keys are emplaced.
* - The states are taken from Problem::getState(_frame_ptr->getTimeStamp(), _frame_keys)
* - The states are emplaced to the frame as unfixed.
*/
void emplaceStatesToFrame(FrameBasePtr _frame_ptr, const StateKeys& _frame_keys);
/** \brief Take a frame and emplace to it the missing states
* \param _frame_ptr Frame to which states have to be emplaced
* \param _frame_states the states vectors to be added to the frame
*
* - The frame_types are taken from Problem.
* - Only the missing keys are emplaced.
* - The states are emplaced to the frame as unfixed.
*/
void emplaceStatesToFrame(FrameBasePtr _frame_ptr, const VectorComposite& _frame_states);
// Frame getters
FrameBaseConstPtr getLastFrame() const;
FrameBasePtr getLastFrame();
FrameBaseConstPtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts);
/** \brief Give the permission to a processor to create a new key Frame
*
* This should implement a black list of processors that have forbidden key frame creation.
* - This decision is made at problem level, not at processor configuration level.
* - Therefore, if you want to permanently configure a processor for not creating key frames, see
* Processor::voting_active_ and its accessors.
*/
bool permitKeyFrame(ProcessorBaseConstPtr _processor_ptr) const;
/** \brief New key frame callback
*
* New key frame callback: It should be called by any processor that creates a new key frame. It calls the
* keyFrameCallback of the rest of processors.
*/
void keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _processor_ptr);
// State getters
TimeStamp getTimeStamp() const;
VectorComposite getState(const StateKeys& _keys) const;
VectorComposite getState(const TimeStamp& _ts, const StateKeys& _keys) const;
VectorComposite getOdometry(const StateKeys& _keys) const;
// Zero state provider
VectorComposite stateZero(TypeComposite _types) const;
VectorComposite stateZero(const StateKeys& _keys) const;
// Map branch -----------------------------------------
MapBaseConstPtr getMap() const;
MapBasePtr getMap();
void setMap(MapBasePtr);
bool saveMap(const std::string& _filename_dot_yaml,
const std::string& _map_name = "Map automatically saved by Wolf");
// All branches -------------------------------------------
/**
* \brief perturb all states states with random noise
* following an uniform distribution in [ -amplitude, amplitude ]
*/
void perturb(double amplitude = 0.01);
// transform states
void transform(const VectorComposite& _transformation);
bool isTransformed() const;
void resetTransformed();
VectorComposite getTransformation() const;
// Covariances
void clearCovariance();
void addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXd& _cov);
void addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXd& _cov);
bool getCovarianceBlock(StateBlockConstPtr _state1,
StateBlockConstPtr _state2,
Eigen::MatrixXd& _cov,
const int _row = 0,
const int _col = 0) const;
bool getCovarianceBlock(std::map<StateBlockConstPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const;
bool getCovarianceBlock(StateBlockConstPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col = 0) const;
bool getCovariance(NodeStateBlocksConstPtr _has_states_ptr,
const StateKeys& _keys,
Eigen::MatrixXd& _covariance) const;
bool getLastFrameCovariance(const StateKeys& _keys, Eigen::MatrixXd& _covariance) const;
// Solver management ----------------------------------------
/** \brief Notifies a new/removed state block to update the solver manager
*/
StateBlockPtr notifyStateBlock(StateBlockPtr _state_ptr, Notification _notif);
/** \brief Returns the size of the map of state block notification
*/
SizeStd getStateBlockNotificationMapSize() const;
/** \brief Returns if the state block has been notified, and the notification via parameter
*/
bool getStateBlockNotification(const StateBlockPtr& sb_ptr, Notification& notif) const;
/** \brief Notifies a new/removed factor to update the solver manager
*/
FactorBasePtr notifyFactor(FactorBasePtr _factor_ptr, Notification _notif);
/** \brief Returns the size of the map of factor notification
*/
SizeStd getFactorNotificationMapSize() const;
/** \brief Returns if the factor has been notified, and the notification via parameter
*/
bool getFactorNotification(const FactorBasePtr& fac_ptr, Notification& notif) const;
protected:
/** \brief Returns the map of state block notification to be handled by the solver (the map stored in this is
* emptied)
*/
void consumeNotifications(std::map<StateBlockPtr, Notification>&, std::map<FactorBasePtr, Notification>&);
/** \brief Returns the map of state block notification to be handled by the solver (the map stored in this is
* emptied)
*/
std::map<StateBlockPtr, Notification> consumeStateBlockNotificationMap();
/** \brief Returns the map of factor notification to be handled by the solver (the map stored in this is emptied)
*/
std::map<FactorBasePtr, Notification> consumeFactorNotificationMap();
public:
// Print and check ---------------------------------------
/**
* \brief print wolf tree
* \param depth : levels to show ( 0: H, T, M : 1: H:S, T:F, M:L ; 2: H:S:p, T:F:C ; 3: T:F:C:f ; 4: T:F:C:f:c )
* \param factored_by : show factors pointing to F, C, f and L.
* \param metric : show metric info (status, time stamps, state vectors, measurements)
* \param state_blocks : show state blocks
*/
void print(int depth = 4, //
bool factored_by = false, //
bool metric = true, //
bool state_blocks = false,
std::ostream& stream = std::cout) const;
bool check(int verbose_level = 0) const;
bool check(bool verbose, std::ostream& stream) const;
};
} // namespace wolf
} // namespace wolf
// IMPLEMENTATION
namespace wolf
{
inline LoaderPtr Problem::getLoader()
{
return loader_;
}
inline TreeManagerBaseConstPtr Problem::getTreeManager() const
{
......@@ -466,20 +440,19 @@ inline TreeManagerBasePtr Problem::getTreeManager()
return tree_manager_;
}
inline bool Problem::isPriorSet() const
inline bool Problem::isFirstFrameApplied() const
{
return prior_options_ == nullptr;
return first_frame_status_ == 2;
}
inline std::map<int,MotionProviderConstPtr> Problem::getMotionProviderMap() const
inline std::map<unsigned int, MotionProviderConstPtr> Problem::getMotionProviderMap() const
{
std::map<int,MotionProviderConstPtr> map_const;
for (auto&& pair : motion_provider_map_)
map_const[pair.first] = pair.second;
std::map<unsigned int, MotionProviderConstPtr> map_const;
for (auto&& pair : motion_provider_map_) map_const[pair.first] = pair.second;
return map_const;
}
inline std::map<int,MotionProviderPtr> Problem::getMotionProviderMap()
inline std::map<unsigned int, MotionProviderPtr> Problem::getMotionProviderMap()
{
return motion_provider_map_;
}
......@@ -496,8 +469,8 @@ inline wolf::SizeStd Problem::getFactorNotificationMapSize() const
return factor_notification_map_.size();
}
inline void Problem::consumeNotifications(std::map<StateBlockPtr,Notification>& _sb_notification_map,
std::map<FactorBasePtr,Notification>& _fac_notification_map)
inline void Problem::consumeNotifications(std::map<StateBlockPtr, Notification>& _sb_notification_map,
std::map<FactorBasePtr, Notification>& _fac_notification_map)
{
std::lock_guard<std::mutex> lock(mut_notifications_);
......@@ -505,8 +478,4 @@ inline void Problem::consumeNotifications(std::map<StateBlockPtr,Notification>&
_fac_notification_map = std::move(factor_notification_map_);
}
} // namespace wolf
#endif // PROBLEM_H
} // namespace wolf
include/core/processor/buffer.h 0 → 100644
Edit View file @ 44e08b4a
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// Wolf includes
#include "core/common/wolf.h"
namespace wolf
{
/** \brief Buffer for arbitrary type objects
*
* Object and functions to manage a buffer of objects.
*/
template <typename T>
class Buffer
{
public:
typedef typename std::map<TimeStamp, T>::iterator Iterator; // buffer iterator
typedef typename std::map<TimeStamp, T>::const_iterator ConstIterator; // buffer iterator
Buffer(){};
~Buffer(void){};
/**\brief Select an element from the buffer
*
* Select from the buffer the closest element (w.r.t. time stamp),
* respecting a defined time tolerances
*/
T select(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
/**\brief Select an element iterator from the buffer
*
* Select from the buffer the iterator pointing to the closest element (w.r.t. time stamp),
* respecting a defined time tolerances
*/
ConstIterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
Iterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance);
T selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
T selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
T selectFirst() const;
T selectLast() const;
/**\brief Buffer size
*
*/
SizeStd size(void) const;
/**\brief Add a element to the buffer
*
*/
void emplace(const TimeStamp& _time_stamp, const T& _element);
/** \brief returns the container with elements of the buffer
*
* elements are ordered from most recent to oldest
*/
const std::map<TimeStamp, T>& getContainer() const;
std::map<TimeStamp, T>& getContainer();
/**\brief Remove all elements in the buffer with a time stamp older than the specified
*
*/
void removeUpTo(const TimeStamp& _time_stamp);
/**\brief Remove all elements in the buffer with a time stamp older than the specified
*
*/
void removeUpToLower(const TimeStamp& _time_stamp);
/**\brief Clear the buffer
*
*/
void clear();
/**\brief is the buffer empty ?
*
*/
bool empty() const;
/**\brief Check time tolerance
*
* Check if the time distance between two time stamps is smaller than
* the time tolerance.
*/
static bool checkTimeTolerance(const TimeStamp& _time_stamp1,
const TimeStamp& _time_stamp2,
const double& _time_tolerance);
/**\brief Check time tolerance
*
* Check if the time distance between two time stamps is smaller than
* the minimum time tolerance of the two frames.
*/
static bool doubleCheckTimeTolerance(const TimeStamp& _time_stamp1,
const double& _time_tolerance1,
const TimeStamp& _time_stamp2,
const double& _time_tolerance2);
protected:
std::map<TimeStamp, T> container_; // Main buffer container
};
} // namespace wolf
#include "core/common/time_stamp.h"
namespace wolf
{
template <typename T>
inline typename Buffer<T>::ConstIterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp,
const double& _time_tolerance) const
{
Buffer<T>::ConstIterator post = container_.upper_bound(_time_stamp);
bool prev_exists = (post != container_.begin());
bool post_exists = (post != container_.end());
bool post_ok = post_exists && checkTimeTolerance(post->first, _time_stamp, _time_tolerance);
if (prev_exists)
{
Buffer<T>::ConstIterator prev = std::prev(post);
bool prev_ok = checkTimeTolerance(prev->first, _time_stamp, _time_tolerance);
if (prev_ok && !post_ok)
return prev;
else if (!prev_ok && post_ok)
return post;
else if (prev_ok && post_ok)
{
if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
return post;
else
return prev;
}
}
else if (post_ok)
return post;
return container_.end();
}
template <typename T>
inline typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp,
const double& _time_tolerance)
{
Buffer<T>::Iterator post = container_.upper_bound(_time_stamp);
bool prev_exists = (post != container_.begin());
bool post_exists = (post != container_.end());
bool post_ok = post_exists && checkTimeTolerance(post->first, _time_stamp, _time_tolerance);
if (prev_exists)
{
Buffer<T>::Iterator prev = std::prev(post);
bool prev_ok = checkTimeTolerance(prev->first, _time_stamp, _time_tolerance);
if (prev_ok && !post_ok)
return prev;
else if (!prev_ok && post_ok)
return post;
else if (prev_ok && post_ok)
{
if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
return post;
else
return prev;
}
}
else if (post_ok)
return post;
return container_.end();
}
template <typename T>
inline T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
if (container_.empty()) return T(); // nullprt in case of T being a pointer
auto it = selectIterator(_time_stamp, _time_tolerance);
// end is returned from selectIterator if an element of the buffer complying with the time stamp
// and time tolerance has not been found
if (it != container_.end())
{
return it->second;
}
return T(); // nullprt in case of T being a pointer
}
template <typename T>
inline T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
// There is no element
if (container_.empty()) return T(); // nullprt in case of T being a pointer
// Checking on begin() since elements are ordered in time
// Return first element if is older than time stamp
if (container_.begin()->first < _time_stamp) return container_.begin()->second;
// Return first element if despite being newer, it is within the time tolerance
if (checkTimeTolerance(container_.begin()->first, _time_stamp, _time_tolerance)) return container_.begin()->second;
// otherwise return nullptr (no element before the provided ts or within the tolerance was found)
return T(); // nullprt in case of T being a pointer
}
template <typename T>
inline T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
// There is no element
if (container_.empty()) return T(); // nullprt in case of T being a pointer
// Checking on rbegin() since elements are ordered in time
// Return last element if is newer than time stamp
if (container_.rbegin()->first > _time_stamp) return container_.rbegin()->second;
// Return last element if despite being older, it is within the time tolerance
if (checkTimeTolerance(container_.rbegin()->first, _time_stamp, _time_tolerance))
return container_.rbegin()->second;
// otherwise return nullptr (no element after the provided ts or within the tolerance was found)
return T(); // nullprt in case of T being a pointer
}
template <typename T>
inline T Buffer<T>::selectFirst() const
{
// There is no element
if (container_.empty()) return T(); // nullprt in case of T being a pointer
// Returning first map element
return container_.begin()->second;
}
template <typename T>
inline T Buffer<T>::selectLast() const
{
// There is no element
if (container_.empty()) return T(); // nullprt in case of T being a pointer
// Returning last map element
return container_.rbegin()->second;
}
template <typename T>
inline void Buffer<T>::emplace(const TimeStamp& _time_stamp, const T& _element)
{
container_.emplace(_time_stamp, _element);
}
template <typename T>
inline const std::map<TimeStamp, T>& Buffer<T>::getContainer() const
{
return container_;
}
template <typename T>
inline std::map<TimeStamp, T>& Buffer<T>::getContainer()
{
return container_;
}
template <typename T>
inline void Buffer<T>::clear()
{
container_.clear();
}
template <typename T>
inline bool Buffer<T>::empty() const
{
return container_.empty();
}
template <typename T>
inline SizeStd Buffer<T>::size(void) const
{
return container_.size();
}
template <typename T>
inline void Buffer<T>::removeUpTo(const TimeStamp& _time_stamp)
{
Buffer::Iterator post = container_.upper_bound(_time_stamp);
container_.erase(container_.begin(), post); // erasing by range
}
template <typename T>
inline void Buffer<T>::removeUpToLower(const TimeStamp& _time_stamp)
{
Buffer::Iterator post = container_.lower_bound(_time_stamp);
container_.erase(container_.begin(), post); // erasing by range
}
template <typename T>
inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1,
const double& _time_tolerance1,
const TimeStamp& _time_stamp2,
const double& _time_tolerance2)
{
double time_diff = std::fabs(_time_stamp1 - _time_stamp2);
double time_tol = std::min(_time_tolerance1, _time_tolerance2);
bool pass = time_diff <= time_tol;
return pass;
}
template <typename T>
inline bool Buffer<T>::checkTimeTolerance(const TimeStamp& _time_stamp1,
const TimeStamp& _time_stamp2,
const double& _time_tolerance)
{
double time_diff = std::fabs(_time_stamp1 - _time_stamp2);
bool pass = time_diff <= _time_tolerance;
return pass;
}
// SPECIFIC BUFFERS
// fwd declarations
class VectorComposite;
/** \brief Buffer of Frames
*/
class BufferFrame : public Buffer<FrameBasePtr>
{
public:
void emplace(FrameBasePtr _frm);
};
/** \brief Buffer of Captures
*/
class BufferCapture : public Buffer<CaptureBasePtr>
{
public:
void emplace(CaptureBasePtr _cap);
};
/** \brief Buffer of VectorComposite
*/
class BufferVectorComposite : public Buffer<VectorComposite>
{
};
} // namespace wolf
include/core/processor/factory_processor.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/**
* \file factory_processor.h
*
* Created on: May 4, 2016
* \author: jsola
*/
#ifndef FACTORY_PROCESSOR_H_
#define FACTORY_PROCESSOR_H_
namespace wolf
{
class ProcessorBase;
struct ParamsProcessorBase;
}
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// wolf
#include "core/common/factory.h"
#include "core/utils/params_server.h"
// std
// yaml
#include "yaml-cpp/yaml.h"
namespace wolf
{
/** \brief Processor factory class
/** \brief Processor factories classes
*
* This factory can create objects of classes deriving from ProcessorBase.
* These factories can create objects of classes deriving from `ProcessorBase`.
*
* Specific object creation is invoked by create(TYPE, params), and the TYPE of processor is identified with a string.
* For example, the following processor types are implemented,
* Specific object creation is invoked by `create(TYPE, params ... )`, and the TYPE of Processor is identified with a
* string. Currently, the following Processor types are implemented,
* - "ProcessorOdom2d" for ProcessorOdom2d
* - "ProcessorOdom3d" for ProcessorOdom3d
* - "ProcessorDiffDrive" for ProcessorDiffDrive
*
* among others.
*
* There are two factories, from a YAML file and from a YAML node with the following objectives:
*
* - The creator from YAML file validates the YAML params before creating the object.
* - The creator from YAML node only validates the YAML params `_schema_folders` vector is not empty.
*
* Validation of YAML params is performed via `yaml_schema_cpp` library, the schema file (with the name of the class)
* will be recursively searched in the folders provided in `_schema_folders` vector.
*
* Find general Factory documentation in class Factory:
* - Access the factory
* - Register/unregister creators
* - Invoke object creation
*
*
* This documentation shows you how to use the FactoryProcessor specifically:
* - Write a processor creator
* This documentation shows you how to use the FactoryProcessorNode specifically:
* - Write processor creators.
* - Create processors
*
* #### Write processor creators
* Processor creators have the following API:
*
* \code
* static ProcessorBasePtr create(const std::string& _name, ParamsProcessorBasePtr _params_processor);
* static ProcessorBasePtr create(const YAML::Node& _yaml_node,
* std::vector<std::string>& _folders_schema);
* static ProcessorBasePtr create(const std::string& _yaml_filepath,
* std::vector<std::string>& _folders_schema);
* \endcode
*
* They follow the general implementation shown below:
* They follow the general implementations shown below (example for class `ProcessorDiffDrive`):
*
* \code
* static ProcessorBasePtr create(const std::string& _unique_name, ParamsProcessorBasePtr _params_processor)
* {
* // cast processor parameters to good type --- example: ParamsProcessorOdom3d
* auto params_processor_ptr = std::static_pointer_cast<ParamsProcessorOdom3d>(_params_processor);
*
* // Do create the Processor object --- example: ProcessorOdom3d
* auto prc_ptr = std::make_shared<ProcessorOdom3d>(params_processor_ptr);
*
* // Complete the processor setup with a unique name identifying the processor
* prc_ptr->setName(_unique_name);
*
* return prc_ptr;
* }
* static ProcessorBasePtr create(const YAML::Node& _input_node,
* std::vector<std::string>& _folders_schema={})
* {
* // validate the _input_node if provided folders where to find the schema file
* if (not _folders_schema.empty())
* {
* auto server = yaml_schema_cpp::YamlServer(_folders_schema);
* server.setYaml(_input_node);
* if (not server.applySchema("ProcessorDiffDrive"))
* {
* WOLF_ERROR(server.getLog());
* return nullptr;
* }
* }
* // Do create the Processor object
* return std::make_shared<ProcessorDiffDrive>(params);
* }
* static ProcessorBasePtr create(const std::string& _yaml_filepath,
* std::vector<std::string>& _folders_schema)
* {
* // parse the yaml file
* auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath);
* // Check that the yaml has all necessary inforamtion
* if (not server.applySchema("ProcessorDiffDrive"))
* {
* WOLF_ERROR(server.getLog());
* return nullptr;
* }
* // Do create the Processor object (calling the previous creator)
* return create(server.getNode(), {});
* }
* \endcode
*
*
* #### Creating processors
* Note: Prior to invoking the creation of a processor of a particular type,
* you must register the creator for this type into the factory.
*
* To create a ProcessorOdom2d, you type:
* To create e.g. a ProcessorDiffDrive in 3D, you type:
*
* \code
* auto prc_odom2d_ptr = FactoryProcessor::create("ProcessorOdom2d", "main odometry", params_ptr);
* auto proc_diff_drive_ptr = FactoryProcessorNode::create("ProcessorDiffDrive", yaml_node);
* \endcode
*
* #### Example 1 : Create a sensor and its processor
* We provide the necessary steps to create a processor of class ProcessorOdom2d in our application,
* and bind it to a SensorOdom2d:
* or:
*
* \code
* #include "core/sensor/sensor_odom_2d.h" // provides SensorOdom2d and FactorySensor
* #include "core/processor/processor_odom_2d.h" // provides ProcessorOdom2d and FactoryProcessor
* auto proc_diff_drive_ptr = FactoryProcessorFile::create("ProcessorDiffDrive",
* "path_of_params_file.yaml",
* schema_folders_vector);
* \endcode
*
* // Note: SensorOdom2d::create() is already registered, automatically.
* // Note: ProcessorOdom2d::create() is already registered, automatically.
* where ABSOLUTELY ALL input parameters are important. DO NOT USE IT WITH DUMMY PARAMETERS!
*
* // First create the sensor (See FactorySensor for details)
* SensorBasePtr sensor_ptr = FactorySensor::create ( "SensorOdom2d" , "Main odometer" , extrinsics , &intrinsics );
* We RECOMMEND using the macro ````WOLF_PROCESSOR_CREATE(ProcessorClass)```` to automatically
* generate the processor creators, provided in 'processor_base.h'.
*
* // To create a odometry integrator, provide a type="ProcessorOdom2d", a name="main odometry", and a pointer to the parameters struct:
* To do so, you should implement a constructor with the API:
*
* auto params = make_shared<ParamsProcessorOdom2d>({...}); // fill in the derived struct (note: ProcessorOdom2d actually has no input params)
* \code
* ProcessorDerived(const YAML::Node& _params)
* \endcode
*
* ProcessorBasePtr processor_ptr =
* FactoryProcessor::create ( "ProcessorOdom2d" , "main odometry" , params );
* In case of processors templated to the dimension of the problem (non-type template), there is
* also the ````WOLF_PROCESSOR_TEMPLATE_DIM_CREATE(ProcessorDerived, DIM)````. See `processor_odom.h` for an example.
*
* // Bind processor to sensor
* sensor_ptr->addProcessor(processor_ptr);
* \endcode
* #### Registering processor creator into the factory
* Registration can be done manually or automatically. It involves the call to static functions.
* It is advisable to put these calls within unnamed namespaces.
*
* - __Manual registration__: you control registration at application level.
* Put the code either at global scope (you must define a dummy variable for this),
* \code
* namespace {
* const bool registered_proc_diff_drive_node = FactoryProcessorNode::registerCreator("ProcessorDiffDrive",
* ProcessorDiffDrive::create);
* }
* namespace {
* const bool registered_proc_diff_drive_file = FactoryProcessorFile::registerCreator("ProcessorDiffDrive",
* ProcessorDiffDrive::create);
* }
* main () { ... }
* \endcode
* or inside your main(), where a direct call is possible:
* \code
* main () {
* FactoryProcessorNode::registerCreator("ProcessorDiffDrive", ProcessorDiffDrive::create);
* FactoryProcessorFile::registerCreator("ProcessorDiffDrive", ProcessorDiffDrive::create);
* ...
* }
* \endcode
*
* - __Automatic registration__: registration is performed at library level.
* Put the code at the last line of the processor_xxx.cpp file,
* \code
* namespace {
* const bool registered_proc_diff_drive_node = FactoryProcessorNode::registerCreator("ProcessorDiffDrive",
* ProcessorDiffDrive::create);
* const bool registered_proc_diff_drive_file = FactoryProcessorFile::registerCreator("ProcessorDiffDrive",
* ProcessorDiffDrive::create);
* }
* \endcode
* Automatic registration is recommended in wolf, and implemented in the classes shipped with it using the macro
* WOLF_REGISTER_PROCESSOR(ProcessorType).
* You are free to comment out these lines and place them wherever you consider it more convenient for your design.
*
* #### Creating processors
* We finally provide the necessary steps to create a processor of class ProcessorDiffDrive in our application:
*
* \code
* #include "vision/processor/processor_diff_drive.h" // provides ProcessorDiffDrive
*
* // Note: ProcessorDiffDrive::create() is already registered, automatically.
*
* using namespace wolf;
* int main() {
*
* // To create a proc_diff_drive, provide:
* // a type = "ProcessorDiffDrive",
* // a name = "Front-left proc_diff_drive",
* // the problem dimension dim = 3, and
* // the yaml node or a yaml file
*
* ProcessorBasePtr proc_diff_drive_1_ptr =
* FactoryProcessorNode::create ( "ProcessorDiffDrive", parameter_node );
*
* // A second proc_diff_drive... with a different name specified in the yaml file or the parameters!
*
* ProcessorBasePtr proc_diff_drive_2_ptr =
* FactoryProcessorNode::create( "ProcessorDiffDrive", yaml_filename );
*
* return 0;
* }
* \endcode
*
* #### See also
* - Problem::installProcessor() : to install processors in WOLF Problem.
*/
typedef Factory<ProcessorBase,
const std::string&,
const ParamsProcessorBasePtr> FactoryProcessor;
template<>
inline std::string FactoryProcessor::getClass() const
typedef Factory<ProcessorBasePtr, const YAML::Node&, const std::vector<std::string>&> FactoryProcessorNode;
template <>
inline std::string FactoryProcessorNode::getClass() const
{
return "FactoryProcessor";
return "FactoryProcessor";
}
// ParamsProcessor factory
struct ParamsProcessorBase;
typedef Factory<ParamsProcessorBase,
const std::string&> FactoryParamsProcessor;
template<>
inline std::string FactoryParamsProcessor::getClass() const
typedef Factory<ProcessorBasePtr, const std::string&, const std::vector<std::string>&> FactoryProcessorYaml;
template <>
inline std::string FactoryProcessorYaml::getClass() const
{
return "FactoryParamsProcessor";
return "FactoryProcessorYaml";
}
#define WOLF_REGISTER_PROCESSOR(ProcessorType) \
namespace{ const bool WOLF_UNUSED ProcessorType##Registered = \
wolf::FactoryProcessor::registerCreator(#ProcessorType, ProcessorType::create); } \
typedef Factory<ProcessorBase,
const std::string&,
const ParamsServer&> AutoConfFactoryProcessor;
template<>
inline std::string AutoConfFactoryProcessor::getClass() const
{
return "AutoConfFactoryProcessor";
}
#define WOLF_REGISTER_PROCESSOR_AUTO(ProcessorType) \
namespace{ const bool WOLF_UNUSED ProcessorType##AutoConfRegistered = \
wolf::AutoConfFactoryProcessor::registerCreator(#ProcessorType, ProcessorType::create); } \
#define WOLF_REGISTER_PROCESSOR(ProcessorType) \
namespace \
{ \
const bool WOLF_UNUSED ProcessorType##NodeRegistered = \
FactoryProcessorNode::registerCreator(#ProcessorType, ProcessorType::create); \
} \
namespace \
{ \
const bool WOLF_UNUSED ProcessorType##YamlRegistered = \
FactoryProcessorYaml::registerCreator(#ProcessorType, ProcessorType::create); \
}
} /* namespace wolf */
#endif /* PROCESSOR_FACTORY_H_ */
include/core/processor/motion_buffer.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/**
* \file motion_buffer.h
*
* Created on: Apr 14, 2016
* \author: jsola
*/
#ifndef SRC_MOTIONBUFFER_H_
#define SRC_MOTIONBUFFER_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/common/wolf.h"
#include "core/common/time_stamp.h"
......@@ -35,42 +24,47 @@
#include <list>
#include <algorithm>
namespace wolf {
namespace wolf
{
using namespace Eigen;
struct Motion
{
public:
SizeEigen data_size_, delta_size_, cov_size_, calib_size_;
TimeStamp ts_; ///< Time stamp
Eigen::VectorXd data_; ///< instantaneous motion data
Eigen::MatrixXd data_cov_; ///< covariance of the instantaneous data
Eigen::VectorXd delta_; ///< instantaneous motion delta
Eigen::MatrixXd delta_cov_; ///< covariance of the instantaneous delta
Eigen::VectorXd delta_integr_; ///< the integrated motion or delta-integral
Eigen::MatrixXd delta_integr_cov_; ///< covariance of the integrated delta
Eigen::MatrixXd jacobian_delta_; ///< Jacobian of the integration wrt delta_
Eigen::MatrixXd jacobian_delta_integr_; ///< Jacobian of the integration wrt delta_integr_
Eigen::MatrixXd jacobian_calib_; ///< Jacobian of delta_integr wrt extra states (TBD by the derived processors)
public:
Motion() = delete; // completely delete unpredictable stuff like this
Motion(const TimeStamp& _ts, SizeEigen _data_size, SizeEigen _delta_size, SizeEigen _cov_size, SizeEigen _calib_size);
Motion(const TimeStamp& _ts,
const VectorXd& _data,
const MatrixXd& _data_cov,
const VectorXd& _delta,
const MatrixXd& _delta_cov,
const VectorXd& _delta_int,
const MatrixXd& _delta_integr_cov,
const MatrixXd& _jac_delta,
const MatrixXd& _jac_delta_int,
const MatrixXd& _jacobian_calib);
~Motion();
private:
void resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_s, SizeEigen _calib_s);
public:
SizeEigen data_size_, delta_size_, cov_size_, calib_size_;
TimeStamp ts_; ///< Time stamp
Eigen::VectorXd data_; ///< instantaneous motion data
Eigen::MatrixXd data_cov_; ///< covariance of the instantaneous data
Eigen::VectorXd delta_; ///< instantaneous motion delta
Eigen::MatrixXd delta_cov_; ///< covariance of the instantaneous delta
Eigen::VectorXd delta_integr_; ///< the integrated motion or delta-integral
Eigen::MatrixXd delta_integr_cov_; ///< covariance of the integrated delta
Eigen::MatrixXd jacobian_delta_; ///< Jacobian of the integration wrt delta_
Eigen::MatrixXd jacobian_delta_integr_; ///< Jacobian of the integration wrt delta_integr_
Eigen::MatrixXd jacobian_calib_; ///< Jacobian of delta_integr wrt extra states (TBD by the derived processors)
public:
Motion() = delete; // completely delete unpredictable stuff like this
Motion(const TimeStamp& _ts,
SizeEigen _data_size,
SizeEigen _delta_size,
SizeEigen _cov_size,
SizeEigen _calib_size);
Motion(const TimeStamp& _ts,
const VectorXd& _data,
const MatrixXd& _data_cov,
const VectorXd& _delta,
const MatrixXd& _delta_cov,
const VectorXd& _delta_int,
const MatrixXd& _delta_integr_cov,
const MatrixXd& _jac_delta,
const MatrixXd& _jac_delta_int,
const MatrixXd& _jacobian_calib);
~Motion();
}; ///< One instance of the buffered data, corresponding to a particular time stamp.
private:
void resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_s, SizeEigen _calib_s);
}; ///< One instance of the buffered data, corresponding to a particular time stamp.
/** \brief class for motion buffers.
*
......@@ -95,14 +89,16 @@ struct Motion
*/
class MotionBuffer : public std::list<Motion>
{
public:
MotionBuffer() ;
const Motion& getMotion(const TimeStamp& _ts) const;
void getMotion(const TimeStamp& _ts, Motion& _motion) const;
void split(const TimeStamp& _ts, MotionBuffer& _oldest_buffer);
void print(bool show_data = 0, bool show_delta = 0, bool show_delta_int = 0, bool show_jacs = 0, bool show_covs = 0);
public:
MotionBuffer();
const Motion& getMotion(const TimeStamp& _ts) const;
void getMotion(const TimeStamp& _ts, Motion& _motion) const;
void split(const TimeStamp& _ts, MotionBuffer& _oldest_buffer);
void print(bool show_data = 0,
bool show_delta = 0,
bool show_delta_int = 0,
bool show_jacs = 0,
bool show_covs = 0);
};
} // namespace wolf
#endif /* SRC_MOTIONBUFFER_H_ */
} // namespace wolf
include/core/processor/motion_provider.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/**
* \file motion_provider.h
*
* Created on: Mar 10, 2020
* \author: jsola
*/
#ifndef PROCESSOR_MOTION_PROVIDER_H_
#define PROCESSOR_MOTION_PROVIDER_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/common/wolf.h"
#include "core/state_block/state_composite.h"
#include "core/utils/params_server.h"
#include "core/composite/vector_composite.h"
#include "core/composite/type_composite.h"
#include "yaml-cpp/yaml.h"
namespace wolf
{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsMotionProvider);
struct ParamsMotionProvider
{
bool state_getter = true;
int state_priority = 1;
ParamsMotionProvider() = default;
ParamsMotionProvider(std::string _unique_name, const ParamsServer& _server)
{
state_getter = _server.getParam<bool>("processor/" + _unique_name + "/state_getter");
state_priority = _server.getParam<double>("processor/" + _unique_name + "/state_priority");
}
std::string print() const
{
return "state_getter: " + std::to_string(state_getter) + "\n"
+ "state_priority: " + std::to_string(state_priority) + "\n";
}
};
class TimeStamp;
WOLF_PTR_TYPEDEFS(MotionProvider);
class MotionProvider
{
public:
MotionProvider(const StateStructure& _structure, ParamsMotionProviderPtr _params);
virtual ~MotionProvider();
// Queries to the processor:
virtual TimeStamp getTimeStamp() const = 0;
virtual VectorComposite getState(const StateStructure& _structure = "") const = 0;
virtual VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const = 0;
VectorComposite getOdometry ( ) const;
void setOdometry(const VectorComposite&);
bool isStateGetter() const;
int getStatePriority() const;
void setStatePriority(int);
public:
const StateStructure& getStateStructure ( ) const { return state_structure_; };
void setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; };
void addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr);
protected:
StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
ParamsMotionProviderPtr params_motion_provider_;
private:
VectorComposite odometry_;
mutable std::mutex mut_odometry_;
public:
MotionProvider(const TypeComposite& _state_types, const YAML::Node& _params);
virtual ~MotionProvider();
// Queries to the processor:
virtual TimeStamp getTimeStamp() const = 0;
virtual VectorComposite getState(StateKeys _keys = "") const = 0;
virtual VectorComposite getState(const TimeStamp& _ts, StateKeys _keys = "") const = 0;
VectorComposite getOdometry() const;
void setOdometry(const VectorComposite&);
bool isStateProvider() const;
unsigned int getOrder() const;
void setOrder(unsigned int);
protected:
bool is_state_provider_;
unsigned int state_provider_order_;
private:
TypeComposite state_types_;
VectorComposite odometry_;
mutable std::mutex mut_odometry_;
};
inline MotionProvider::MotionProvider(const StateStructure& _structure, ParamsMotionProviderPtr _params) :
state_structure_(_structure),
params_motion_provider_(_params)
{
//
}
}
} // namespace wolf
///// IMPLEMENTATION ///////
namespace wolf{
inline MotionProvider::~MotionProvider()
namespace wolf
{
}
inline MotionProvider::~MotionProvider() {}
inline bool MotionProvider::isStateGetter() const
inline bool MotionProvider::isStateProvider() const
{
return params_motion_provider_->state_getter;
return is_state_provider_;
}
inline int MotionProvider::getStatePriority() const
inline unsigned int MotionProvider::getOrder() const
{
return params_motion_provider_->state_priority;
if (not isStateProvider())
throw std::runtime_error("MotionProvider::getOrder: not a motion provider, doesn't have order");
return state_provider_order_;
}
inline void MotionProvider::setStatePriority(int _priority)
inline void MotionProvider::setOrder(unsigned int _order)
{
params_motion_provider_->state_priority = _priority;
if (not isStateProvider())
throw std::runtime_error("MotionProvider::getOrder: not a motion provider, cannot have order");
state_provider_order_ = _order;
}
} /* namespace wolf */
#endif /* PROCESSOR_MOTION_PROVIDER_H_ */
include/core/processor/processor_base.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef PROCESSOR_BASE_H_
#define PROCESSOR_BASE_H_
// Fwd refs
namespace wolf{
class SensorBase;
}
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
#include <core/processor/motion_provider.h>
#include "core/sensor/sensor_base.h"
#include "core/processor/buffer.h"
#include "core/processor/motion_provider.h"
#include "core/processor/factory_processor.h"
#include "core/frame/frame_base.h"
#include "core/common/time_stamp.h"
#include "core/common/params_base.h"
#include "core/processor/buffer.h"
#include "core/common/profiling_unit.h"
// std
#include <memory>
#include <map>
#include <chrono>
namespace wolf {
// yaml
#include "yaml-cpp/yaml.h"
namespace wolf
{
/*
* Macro for defining Autoconf processor creator for WOLF's high level API.
*
......@@ -52,368 +48,267 @@ namespace wolf {
* In order to use this macro, the derived processor class, ProcessorClass,
* must have a constructor available with the API:
*
* ProcessorClass(const ParamsProcessorClassPtr _params);
*/
#define WOLF_PROCESSOR_CREATE(ProcessorClass, ParamsProcessorClass) \
static ProcessorBasePtr create(const std::string& _unique_name, \
const ParamsServer& _server) \
{ \
auto params = std::make_shared<ParamsProcessorClass>(_unique_name, _server); \
\
auto processor = std::make_shared<ProcessorClass>(params); \
\
processor ->setName(_unique_name); \
\
return processor; \
} \
static ProcessorBasePtr create(const std::string& _unique_name, const ParamsProcessorBasePtr _params) \
{ \
auto params = std::static_pointer_cast<ParamsProcessorClass>(_params); \
\
auto processor = std::make_shared<ProcessorClass>(params); \
\
processor ->setName(_unique_name); \
\
return processor; \
} \
/** \brief Buffer for arbitrary type objects
* ProcessorClass(const YAML::Node& _params);
*
* Object and functions to manage a buffer of objects.
* Also, there should be the schema file 'SensorClass.schema' containing the specifications
* of the user input yaml file.
*/
template <typename T>
class Buffer
#define WOLF_PROCESSOR_CREATE(ProcessorClass) \
static ProcessorBasePtr create(const YAML::Node& _input_node, \
const std::vector<std::string>& _folders_schema = {}) \
{ \
if (not _folders_schema.empty()) \
{ \
auto server = yaml_schema_cpp::YamlServer(_folders_schema); \
server.setYaml(_input_node); \
if (not server.applySchema(#ProcessorClass)) \
{ \
WOLF_ERROR(server.getLog()); \
return nullptr; \
} \
} \
return std::make_shared<ProcessorClass>(_input_node); \
} \
static ProcessorBasePtr create(const std::string& _yaml_filepath, \
const std::vector<std::string>& _folders_schema) \
{ \
auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \
if (not server.applySchema(#ProcessorClass)) \
{ \
WOLF_ERROR(server.getLog()); \
return nullptr; \
} \
return create(server.getNode(), {}); \
}
#define WOLF_PROCESSOR_TEMPLATE_DIM_CREATE(ProcessorClass, Dim) \
static ProcessorBasePtr create(const YAML::Node& _input_node, \
const std::vector<std::string>& _folders_schema = {}) \
{ \
if (not _folders_schema.empty()) \
{ \
auto server = yaml_schema_cpp::YamlServer(_folders_schema); \
server.setYaml(_input_node); \
if (not server.applySchema(#ProcessorClass + std::to_string(Dim) + "d")) \
{ \
WOLF_ERROR(server.getLog()); \
return nullptr; \
} \
} \
return std::make_shared<ProcessorClass<Dim>>(_input_node); \
} \
static ProcessorBasePtr create(const std::string& _yaml_filepath, \
const std::vector<std::string>& _folders_schema) \
{ \
auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \
if (not server.applySchema(#ProcessorClass + std::to_string(Dim) + "d")) \
{ \
WOLF_ERROR(server.getLog()); \
return nullptr; \
} \
return create(server.getNode(), {}); \
}
// class ProcessorBase
class ProcessorBase : public NodeBase, public std::enable_shared_from_this<ProcessorBase>
{
public:
typedef typename std::map<TimeStamp,T>::iterator Iterator; // buffer iterator
typedef typename std::map<TimeStamp,T>::const_iterator ConstIterator; // buffer iterator
Buffer(){};
~Buffer(void){};
/**\brief Select an element from the buffer
*
* Select from the buffer the closest element (w.r.t. time stamp),
* respecting a defined time tolerances
*/
T select(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
/**\brief Select an element iterator from the buffer
*
* Select from the buffer the iterator pointing to the closest element (w.r.t. time stamp),
* respecting a defined time tolerances
*/
ConstIterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
Iterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance);
T selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
T selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
T selectFirst() const;
T selectLast() const;
/**\brief Buffer size
*
*/
SizeStd size(void) const;
/**\brief Add a element to the buffer
*
*/
void emplace(const TimeStamp& _time_stamp, const T& _element);
/** \brief returns the container with elements of the buffer
*
* elements are ordered from most recent to oldest
*/
const std::map<TimeStamp,T>& getContainer() const;
std::map<TimeStamp,T>& getContainer();
/**\brief Remove all elements in the buffer with a time stamp older than the specified
*
*/
void removeUpTo(const TimeStamp& _time_stamp);
/**\brief Remove all elements in the buffer with a time stamp older than the specified
*
*/
void removeUpToLower(const TimeStamp& _time_stamp);
/**\brief Clear the buffer
*
*/
void clear();
/**\brief is the buffer empty ?
*
*/
bool empty() const;
protected:
/**\brief Check time tolerance
*
* Check if the time distance between two time stamps is smaller than
* the time tolerance.
*/
static bool checkTimeTolerance(const TimeStamp& _time_stamp1,
const TimeStamp& _time_stamp2,
const double& _time_tolerance);
/**\brief Check time tolerance
*
* Check if the time distance between two time stamps is smaller than
* the minimum time tolerance of the two frames.
*/
static bool doubleCheckTimeTolerance(const TimeStamp& _time_stamp1,
const double& _time_tolerance1,
const TimeStamp& _time_stamp2,
const double& _time_tolerance2);
protected:
std::map<TimeStamp,T> container_; // Main buffer container
};
friend SensorBase;
protected:
unsigned int processor_id_;
YAML::Node params_;
BufferFrame buffer_frame_;
BufferCapture buffer_capture_;
private:
TypeComposite state_types_; ///< State type composite of frames created or used by this processor
SensorBaseWPtr sensor_ptr_;
static unsigned int processor_id_count_;
// CHECKING captures period and time_tolerance
double capture_period_mean_;
TimeStamp prev_capture_stamp_;
// PROFILING
ProfilingUnit profiling_capture_, profiling_kf_;
public:
void startCaptureProfiling();
void stopCaptureProfiling();
void startKFProfiling();
void stopKFProfiling();
virtual void printProfiling(std::ostream& stream = std::cout) const;
/** \brief constructor
*
* \param _type TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: type name
* \param _state_types TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR:
* The composite of state types of the frames created or used by this processor
* \param _params params yaml node
*/
ProcessorBase(const std::string& _type, const TypeComposite& _state_types, const YAML::Node& _params);
~ProcessorBase() override;
virtual void configure(SensorBasePtr _sensor) = 0;
virtual void remove();
bool hasChildren() const override;
const TypeComposite& getStateTypes() const;
StateKeys getStateKeys() const;
unsigned int id() const;
protected:
/** \brief process an incoming capture
*
* Each derived processor should implement this function. It will be called if:
* - A new capture arrived and triggerInCapture() returned true.
*/
virtual void processCapture(CaptureBasePtr) = 0;
/** \brief process an incoming key-frame
*
* Each derived processor should implement this function. It will be called if:
* - A new KF arrived and triggerInKF() returned true.
*/
virtual void processKeyFrame(FrameBasePtr _keyframe_ptr) = 0;
/** \brief Buffer of Frames
*
* Object and functions to manage a buffer of FrameBasePtr objects.
*/
class BufferFrame : public Buffer<FrameBasePtr> { };
/** \brief trigger in capture
*
* Returns true if processCapture() should be called after the provided capture arrived.
*/
virtual bool triggerInCapture(CaptureBasePtr) const = 0;
/** \brief trigger in key-frame
*
* Returns true if processKeyFrame() should be called after the provided KF arrived.
*/
virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const = 0;
/** \brief Buffer of Captures
*
* Object and functions to manage a buffer of CaptureBasePtr objects.
*/
class BufferCapture : public Buffer<CaptureBasePtr> {};
/** \brief store key frame
*
* Returns true if the key frame should be stored
*/
virtual bool storeKeyFrame(FrameBasePtr) = 0;
/** \brief store capture
*
* Returns true if the capture should be stored
*/
virtual bool storeCapture(CaptureBasePtr) = 0;
/** \brief Vote for KeyFrame generation
*
* If a KeyFrame criterion is validated, this function returns true,
* meaning that it wants to create a KeyFrame at the \b last Capture.
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
virtual bool voteForKeyFrame() const = 0;
virtual bool permittedKeyFrame() const final;
/** \brief base struct for processor parameters
*
* Derive from this struct to create structs of processor parameters.
*/
struct ParamsProcessorBase : public ParamsBase
{
std::string prefix = "processor/";
ParamsProcessorBase() = default;
ParamsProcessorBase(std::string _unique_name, const ParamsServer& _server):
ParamsBase(_unique_name, _server)
{
time_tolerance = _server.getParam<double>(prefix + _unique_name + "/time_tolerance");
voting_active = _server.getParam<bool>(prefix + _unique_name + "/keyframe_vote/voting_active");
apply_loss_function = _server.getParam<bool>(prefix + _unique_name + "/apply_loss_function");
}
void setProblem(ProblemPtr) override;
~ParamsProcessorBase() override = default;
public:
/**\brief notify a new keyframe made by another processor
*
* It stores the new KF in buffer_frame_ and calls triggerInKF()
*
*/
void keyFrameCallback(FrameBasePtr _keyframe);
/** maximum time difference between a Keyframe time stamp and
* a particular Capture of this processor to allow assigning
* this Capture to the Keyframe.
/**\brief notify a new capture
*
* It stores the new capture in buffer_capture_ and calls triggerInCapture()
*/
double time_tolerance;
bool voting_active; ///< Whether this processor is allowed to vote for a Key Frame or not
bool apply_loss_function; ///< Whether this processor emplaces factors with loss function or not
void captureCallback(CaptureBasePtr _capture);
SensorBaseConstPtr getSensor() const;
SensorBasePtr getSensor();
std::string print() const override
private:
void setSensor(SensorBasePtr _sen_ptr)
{
return "voting_active: " + std::to_string(voting_active) + "\n"
+ "time_tolerance: " + std::to_string(time_tolerance) + "\n"
+ "apply_loss_function: " + std::to_string(apply_loss_function) + "\n";
sensor_ptr_ = _sen_ptr;
}
public:
YAML::Node getParams() const;
bool isMotionProvider() const;
bool applyLossFunction() const;
bool isVotingActive() const;
double getTimeTolerance() const;
void setApplyLossFunction(bool _apply_loss_function);
void setVotingActive(bool _voting_active);
void setTimeTolerance(double _time_tolerance);
bool checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2) const;
bool checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts) const;
bool checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts) const;
bool checkTimeTolerance(const FrameBasePtr _frm, const CaptureBasePtr _cap) const;
void link(SensorBasePtr);
template <typename classType>
static std::shared_ptr<classType> emplace(SensorBasePtr _sen_ptr,
const YAML::Node& _params,
const std::vector<std::string> _folders_schema = {});
virtual void printHeader(int depth, //
bool factored_by, //
bool metric, //
bool state_blocks,
std::ostream& stream,
std::string _tabs = "") const;
void print(int depth, //
bool factored_by, //
bool metric, //
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
virtual CheckLog localCheck(bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
bool check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
};
//class ProcessorBase
class ProcessorBase : public NodeBase, public std::enable_shared_from_this<ProcessorBase>
inline bool ProcessorBase::hasChildren() const
{
friend SensorBase;
protected:
unsigned int processor_id_;
ParamsProcessorBasePtr params_;
BufferFrame buffer_frame_;
BufferCapture buffer_capture_;
int dim_;
private:
SensorBaseWPtr sensor_ptr_;
static unsigned int processor_id_count_;
public:
ProcessorBase(const std::string& _type, int _dim, ParamsProcessorBasePtr _params);
~ProcessorBase() override;
virtual void configure(SensorBasePtr _sensor) = 0;
virtual void remove();
unsigned int id() const;
// PROFILING
unsigned int n_capture_callback_;
unsigned int n_kf_callback_;
std::chrono::microseconds acc_duration_capture_;
std::chrono::microseconds acc_duration_kf_;
std::chrono::microseconds max_duration_capture_;
std::chrono::microseconds max_duration_kf_;
std::chrono::time_point<std::chrono::high_resolution_clock> start_capture_;
std::chrono::time_point<std::chrono::high_resolution_clock> start_kf_;
void startCaptureProfiling();
void stopCaptureProfiling();
void startKFProfiling();
void stopKFProfiling();
void printProfiling(std::ostream& stream = std::cout) const;
protected:
/** \brief process an incoming capture
*
* Each derived processor should implement this function. It will be called if:
* - A new capture arrived and triggerInCapture() returned true.
*/
virtual void processCapture(CaptureBasePtr) = 0;
/** \brief process an incoming key-frame
*
* Each derived processor should implement this function. It will be called if:
* - A new KF arrived and triggerInKF() returned true.
*/
virtual void processKeyFrame(FrameBasePtr _keyframe_ptr) = 0;
/** \brief trigger in capture
*
* Returns true if processCapture() should be called after the provided capture arrived.
*/
virtual bool triggerInCapture(CaptureBasePtr) const = 0;
/** \brief trigger in key-frame
*
* Returns true if processKeyFrame() should be called after the provided KF arrived.
*/
virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const = 0;
/** \brief store key frame
*
* Returns true if the key frame should be stored
*/
virtual bool storeKeyFrame(FrameBasePtr) = 0;
/** \brief store capture
*
* Returns true if the capture should be stored
*/
virtual bool storeCapture(CaptureBasePtr) = 0;
/** \brief Vote for KeyFrame generation
*
* If a KeyFrame criterion is validated, this function returns true,
* meaning that it wants to create a KeyFrame at the \b last Capture.
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
virtual bool voteForKeyFrame() const = 0;
virtual bool permittedKeyFrame() final;
void setProblem(ProblemPtr) override;
public:
/**\brief notify a new keyframe made by another processor
*
* It stores the new KF in buffer_frame_ and calls triggerInKF()
*
*/
void keyFrameCallback(FrameBasePtr _keyframe);
/**\brief notify a new capture
*
* It stores the new capture in buffer_capture_ and calls triggerInCapture()
*/
void captureCallback(CaptureBasePtr _capture);
SensorBaseConstPtr getSensor() const;
SensorBasePtr getSensor();
private:
void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
public:
bool isMotionProvider() const;
bool isVotingActive() const;
void setVotingActive(bool _voting_active = true);
int getDim() const;
void setTimeTolerance(double _time_tolerance);
double getTimeTolerance() const;
bool checkTimeTolerance (const TimeStamp& _ts1, const TimeStamp& _ts2) const;
bool checkTimeTolerance (const CaptureBasePtr _cap, const TimeStamp& _ts) const;
bool checkTimeTolerance (const FrameBasePtr _frm, const TimeStamp& _ts) const;
bool checkTimeTolerance (const FrameBasePtr _frm, const CaptureBasePtr _cap) const;
void link(SensorBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(SensorBasePtr _sen_ptr, T&&... all);
virtual void printHeader(int depth, //
bool constr_by, //
bool metric, //
bool state_blocks,
std::ostream& stream ,
std::string _tabs = "") const;
void print(int depth, //
bool constr_by, //
bool metric, //
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
virtual CheckLog localCheck(bool _verbose, ProcessorBaseConstPtr _prc_ptr, std::ostream& _stream, std::string _tabs = "") const;
bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
return false;
};
inline void ProcessorBase::startCaptureProfiling()
{
start_capture_ = std::chrono::high_resolution_clock::now();
profiling_capture_.startProfiling();
}
inline void ProcessorBase::stopCaptureProfiling()
inline void ProcessorBase::startKFProfiling()
{
auto duration_capture = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_capture_);
max_duration_capture_ = std::max(max_duration_capture_, duration_capture);
acc_duration_capture_ += duration_capture;
profiling_kf_.startProfiling();
}
inline void ProcessorBase::startKFProfiling()
inline YAML::Node ProcessorBase::getParams() const
{
start_kf_ = std::chrono::high_resolution_clock::now();
return Clone(params_);
}
inline void ProcessorBase::stopKFProfiling()
inline bool ProcessorBase::applyLossFunction() const
{
auto duration_kf = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_kf_);
return params_["apply_loss_function"].as<bool>();
}
max_duration_kf_ = std::max(max_duration_kf_, duration_kf);
acc_duration_kf_ += duration_kf;
inline void ProcessorBase::setApplyLossFunction(bool _apply_loss_function)
{
params_["apply_loss_function"] = _apply_loss_function;
}
inline bool ProcessorBase::isVotingActive() const
{
return params_->voting_active;
return params_["keyframe_vote"]["voting_active"].as<bool>();
}
inline void ProcessorBase::setVotingActive(bool _voting_active)
{
params_->voting_active = _voting_active;
params_["keyframe_vote"]["voting_active"] = _voting_active;
}
inline bool ProcessorBase::isMotionProvider() const
......@@ -437,255 +332,34 @@ inline SensorBasePtr ProcessorBase::getSensor()
return sensor_ptr_.lock();
}
inline int ProcessorBase::getDim() const
inline StateKeys ProcessorBase::getStateKeys() const
{
return state_types_.getKeys();
}
inline const TypeComposite& ProcessorBase::getStateTypes() const
{
return dim_;
return state_types_;
}
inline void ProcessorBase::setTimeTolerance(double _time_tolerance)
{
params_->time_tolerance = _time_tolerance;
params_["time_tolerance"] = _time_tolerance;
}
inline double ProcessorBase::getTimeTolerance() const
{
return params_->time_tolerance;
return params_["time_tolerance"].as<double>();
}
template<typename classType, typename... T>
std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... all)
template <typename classType>
std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr,
const YAML::Node& _params,
const std::vector<std::string> _folders_schema)
{
std::shared_ptr<classType> prc = std::make_shared<classType>(std::forward<T>(all)...);
std::shared_ptr<classType> prc = std::static_pointer_cast<classType>(classType::create(_params, _folders_schema));
prc->configure(_sen_ptr);
prc->link(_sen_ptr);
return prc;
}
/////////////////////////////////////////////////////////////////////////////////////////
template <typename T>
typename Buffer<T>::ConstIterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
Buffer<T>::ConstIterator post = container_.upper_bound(_time_stamp);
bool prev_exists = (post != container_.begin());
bool post_exists = (post != container_.end());
bool post_ok = post_exists && checkTimeTolerance(post->first, _time_stamp, _time_tolerance);
if (prev_exists)
{
Buffer<T>::ConstIterator prev = std::prev(post);
bool prev_ok = checkTimeTolerance(prev->first, _time_stamp, _time_tolerance);
if (prev_ok && !post_ok)
return prev;
else if (!prev_ok && post_ok)
return post;
else if (prev_ok && post_ok)
{
if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
return post;
else
return prev;
}
}
else if (post_ok)
return post;
return container_.end();
}
template <typename T>
typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance)
{
Buffer<T>::Iterator post = container_.upper_bound(_time_stamp);
bool prev_exists = (post != container_.begin());
bool post_exists = (post != container_.end());
bool post_ok = post_exists && checkTimeTolerance(post->first, _time_stamp, _time_tolerance);
if (prev_exists)
{
Buffer<T>::Iterator prev = std::prev(post);
bool prev_ok = checkTimeTolerance(prev->first, _time_stamp, _time_tolerance);
if (prev_ok && !post_ok)
return prev;
else if (!prev_ok && post_ok)
return post;
else if (prev_ok && post_ok)
{
if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
return post;
else
return prev;
}
}
else if (post_ok)
return post;
return container_.end();
}
template <typename T>
T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
if (container_.empty())
return nullptr;
auto it = selectIterator(_time_stamp, _time_tolerance);
// end is returned from selectIterator if an element of the buffer complying with the time stamp
// and time tolerance has not been found
if (it != container_.end()){
return it->second;
}
return nullptr;
}
template <typename T>
T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
// There is no element
if (container_.empty())
return nullptr;
// Checking on begin() since elements are ordered in time
// Return first element if is older than time stamp
if (container_.begin()->first < _time_stamp)
return container_.begin()->second;
// Return first element if despite being newer, it is within the time tolerance
if (checkTimeTolerance(container_.begin()->first, _time_stamp, _time_tolerance))
return container_.begin()->second;
// otherwise return nullptr (no element before the provided ts or within the tolerance was found)
return nullptr;
}
template <typename T>
T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
// There is no element
if (container_.empty())
return nullptr;
// Checking on rbegin() since elements are ordered in time
// Return last element if is newer than time stamp
if (container_.rbegin()->first > _time_stamp)
return container_.rbegin()->second;
// Return last element if despite being older, it is within the time tolerance
if (checkTimeTolerance(container_.rbegin()->first, _time_stamp, _time_tolerance))
return container_.rbegin()->second;
// otherwise return nullptr (no element after the provided ts or within the tolerance was found)
return nullptr;
}
template <typename T>
T Buffer<T>::selectFirst() const
{
// There is no element
if (container_.empty())
return nullptr;
// Returning first map element
return container_.begin()->second;
}
template <typename T>
T Buffer<T>::selectLast() const
{
// There is no element
if (container_.empty())
return nullptr;
// Returning last map element
return container_.rbegin()->second;
}
template <typename T>
void Buffer<T>::emplace(const TimeStamp& _time_stamp, const T& _element)
{
container_.emplace(_time_stamp, _element);
}
template <typename T>
const std::map<TimeStamp,T>& Buffer<T>::getContainer() const
{
return container_;
}
template <typename T>
std::map<TimeStamp,T>& Buffer<T>::getContainer()
{
return container_;
}
template <typename T>
inline void Buffer<T>::clear()
{
container_.clear();
}
template <typename T>
inline bool Buffer<T>::empty() const
{
return container_.empty();
}
template <typename T>
inline SizeStd Buffer<T>::size(void) const
{
return container_.size();
}
template <typename T>
inline void Buffer<T>::removeUpTo(const TimeStamp& _time_stamp)
{
Buffer::Iterator post = container_.upper_bound(_time_stamp);
container_.erase(container_.begin(), post); // erasing by range
}
template <typename T>
inline void Buffer<T>::removeUpToLower(const TimeStamp& _time_stamp)
{
Buffer::Iterator post = container_.lower_bound(_time_stamp);
container_.erase(container_.begin(), post); // erasing by range
}
template <typename T>
inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1,
const double& _time_tolerance1,
const TimeStamp& _time_stamp2,
const double& _time_tolerance2)
{
double time_diff = std::fabs(_time_stamp1 - _time_stamp2);
double time_tol = std::min(_time_tolerance1, _time_tolerance2);
bool pass = time_diff <= time_tol;
return pass;
}
template <typename T>
inline bool Buffer<T>::checkTimeTolerance(const TimeStamp& _time_stamp1,
const TimeStamp& _time_stamp2,
const double& _time_tolerance)
{
double time_diff = std::fabs(_time_stamp1 - _time_stamp2);
bool pass = time_diff <= _time_tolerance;
return pass;
}
} // namespace wolf
#endif
} // namespace wolf
include/core/processor/processor_diff_drive.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/**
* \file processor_diff_drive.h
*
* Created on: Jul 22, 2019
* \author: jsola
*/
#ifndef PROCESSOR_PROCESSOR_DIFF_DRIVE_H_
#define PROCESSOR_PROCESSOR_DIFF_DRIVE_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/processor/processor_odom_2d.h"
namespace wolf
{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorDiffDrive);
struct ParamsProcessorDiffDrive : public ParamsProcessorOdom2d
{
ParamsProcessorDiffDrive() = default;
ParamsProcessorDiffDrive(std::string _unique_name, const wolf::ParamsServer & _server) :
ParamsProcessorOdom2d(_unique_name, _server)
{
}
std::string print() const override
{
return ParamsProcessorOdom2d::print();
}
};
WOLF_PTR_TYPEDEFS(ProcessorDiffDrive);
class ProcessorDiffDrive : public ProcessorOdom2d
{
public:
ProcessorDiffDrive(ParamsProcessorDiffDrivePtr _params_motion);
~ProcessorDiffDrive() override;
void configure(SensorBasePtr _sensor) override;
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorDiffDrive, ParamsProcessorDiffDrive);
protected:
// Motion integration
void computeCurrentDelta(const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _calib,
const double _dt,
Eigen::VectorXd& _delta,
Eigen::MatrixXd& _delta_cov,
Eigen::MatrixXd& _jacobian_calib) const override;
CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
const VectorXd& _data,
const MatrixXd& _data_cov,
const VectorXd& _calib,
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin) override;
void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
ParamsProcessorDiffDrivePtr params_prc_diff_drv_selfcal_;
double radians_per_tick_;
public:
ProcessorDiffDrive(const YAML::Node& _params);
~ProcessorDiffDrive() override;
void configure(SensorBasePtr _sensor) override;
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorDiffDrive);
protected:
// Motion integration
void computeCurrentDelta(const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _calib,
const double _dt,
Eigen::VectorXd& _delta,
Eigen::MatrixXd& _delta_cov,
Eigen::MatrixXd& _jacobian_calib) const override;
CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
const VectorXd& _data,
const MatrixXd& _data_cov,
const VectorXd& _calib,
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin) override;
void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
double radians_per_tick_;
};
inline Eigen::VectorXd ProcessorDiffDrive::getCalibration (const CaptureBaseConstPtr _capture) const
inline Eigen::VectorXd ProcessorDiffDrive::getCalibration(const CaptureBaseConstPtr _capture) const
{
if (_capture)
return _capture->getStateBlock('I')->getState();
else
return getSensor()->getStateBlockDynamic('I')->getState();
return getSensor()->getStateBlock('I')->getState();
}
inline void ProcessorDiffDrive::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
inline void ProcessorDiffDrive::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)
{
_capture->getStateBlock('I')->setState(_calibration);
}
}
#endif /* PROCESSOR_PROCESSOR_DIFF_DRIVE_H_ */
} // namespace wolf
include/core/processor/processor_fixed_wing_model.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/*
* processor_fix_wing_model.h
*
* Created on: Sep 6, 2021
* Author: joanvallve
*/
#ifndef INCLUDE_CORE_PROCESSOR_PROCESSOR_FIXED_WING_MODEL_H_
#define INCLUDE_CORE_PROCESSOR_PROCESSOR_FIXED_WING_MODEL_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/processor/processor_base.h"
namespace wolf
{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorFixedWingModel);
struct ParamsProcessorFixedWingModel : public ParamsProcessorBase
{
Eigen::Vector3d velocity_local;
double angle_stdev;
double min_vel_norm;
ParamsProcessorFixedWingModel() = default;
ParamsProcessorFixedWingModel(std::string _unique_name, const wolf::ParamsServer & _server) :
ParamsProcessorBase(_unique_name, _server)
{
velocity_local = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/velocity_local");
angle_stdev = _server.getParam<double> (prefix + _unique_name + "/angle_stdev");
min_vel_norm = _server.getParam<double> (prefix + _unique_name + "/min_vel_norm");
assert(std::abs(velocity_local.norm() - 1.0) < wolf::Constants::EPS && "ParamsProcessorFixedWingModel: 'velocity_local' must be normalized");
}
std::string print() const override
{
return ParamsProcessorBase::print() + "\n"
+ "velocity_local: print not implemented\n"
+ "angle_stdev: " + std::to_string(angle_stdev) + "\n"
+ "min_vel_norm: " + std::to_string(min_vel_norm) + "\n";
}
};
WOLF_PTR_TYPEDEFS(ProcessorFixedWingModel);
class ProcessorFixedWingModel : public ProcessorBase
{
public:
ProcessorFixedWingModel(ParamsProcessorFixedWingModelPtr _params);
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorFixedWingModel, ParamsProcessorFixedWingModel);
virtual ~ProcessorFixedWingModel() override;
void configure(SensorBasePtr _sensor) override;
protected:
/** \brief process an incoming capture NEVER CALLED
*/
virtual void processCapture(CaptureBasePtr) override {};
/** \brief process an incoming key-frame: applies the motion model between consecutive keyframes
*/
virtual void processKeyFrame(FrameBasePtr _keyframe_ptr) override;
/** \brief trigger in capture
*/
virtual bool triggerInCapture(CaptureBasePtr) const override {return false;};
/** \brief trigger in key-frame
*/
virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override {return true;};
/** \brief store key frame
*/
virtual bool storeKeyFrame(FrameBasePtr) override {return false;};
/** \brief store capture
*/
virtual bool storeCapture(CaptureBasePtr) override {return false;};
/** \brief Vote for KeyFrame generation
*/
virtual bool voteForKeyFrame() const override {return false;};
// ATTRIBUTES
ParamsProcessorFixedWingModelPtr params_processor_;
public:
ProcessorFixedWingModel(const YAML::Node& _params);
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorFixedWingModel);
virtual ~ProcessorFixedWingModel() override;
void configure(SensorBasePtr _sensor) override;
protected:
/** \brief process an incoming capture NEVER CALLED
*/
virtual void processCapture(CaptureBasePtr) override{};
/** \brief process an incoming key-frame: applies the motion model between consecutive keyframes
*/
virtual void processKeyFrame(FrameBasePtr _keyframe_ptr) override;
/** \brief trigger in capture
*/
virtual bool triggerInCapture(CaptureBasePtr) const override
{
return false;
};
/** \brief trigger in key-frame
*/
virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override
{
return true;
};
/** \brief store key frame
*/
virtual bool storeKeyFrame(FrameBasePtr) override
{
return false;
};
/** \brief store capture
*/
virtual bool storeCapture(CaptureBasePtr) override
{
return false;
};
/** \brief Vote for KeyFrame generation
*/
virtual bool voteForKeyFrame() const override
{
return false;
};
// Params
Eigen::Vector3d velocity_local_;
double angle_stdev_;
double min_vel_norm_;
};
} /* namespace wolf */
#endif /* INCLUDE_CORE_PROCESSOR_PROCESSOR_FIXED_WING_MODEL_H_ */
include/core/processor/processor_landmark_external.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef PROCESSOR_LANDMARK_EXTERNAL_H_
#define PROCESSOR_LANDMARK_EXTERNAL_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/common/wolf.h"
#include "core/processor/processor_tracker.h"
#include "core/processor/track_matrix.h"
#include "core/landmark/landmark_external.h"
#include "core/landmark/landmark_base.h"
#include "core/feature/feature_landmark_external.h"
namespace wolf
{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLandmarkExternal);
struct ParamsProcessorLandmarkExternal : public ParamsProcessorTracker
{
bool use_orientation; ///< use orientation measure or not when emplacing factors
unsigned int new_features_for_keyframe; ///< for keyframe voting: amount of new features with respect to origin
///< (sufficient condition if more than min_features_for_keyframe)
double time_span; ///< for keyframe voting: time span since last frame (sufficient condition if more than
///< min_features_for_keyframe)
double quality_th; ///< min quality to consider the detection
// Matching distance threshold to previous detection considering motion (necessary condition)
double match_dist_th_id; ///< Match by ID
double match_dist_th_type; ///< Match by TYPE
double match_dist_th_unknown; ///< No ID/TYPE information
unsigned int track_length_th; ///< Track length threshold to emplace factors (necessary condition)
bool close_loops_by_id; ///< Close loop if ID matches (ID unchanged guaranteed)
bool close_loops_by_type; ///< Close loop if TYPE matches (also distance check)
ParamsProcessorLandmarkExternal() = default;
ParamsProcessorLandmarkExternal(std::string _unique_name, const wolf::ParamsServer& _server)
: ParamsProcessorTracker(_unique_name, _server)
{
use_orientation = _server.getParam<bool>(prefix + _unique_name + "/use_orientation");
new_features_for_keyframe =
_server.getParam<unsigned int>(prefix + _unique_name + "/keyframe_vote/new_features_for_keyframe");
time_span = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/time_span");
quality_th = _server.getParam<double>(prefix + _unique_name + "/quality_th");
match_dist_th_id = _server.getParam<double>(prefix + _unique_name + "/match_dist_th_id");
match_dist_th_type = _server.getParam<double>(prefix + _unique_name + "/match_dist_th_type");
match_dist_th_unknown = _server.getParam<double>(prefix + _unique_name + "/match_dist_th_unknown");
track_length_th = _server.getParam<unsigned int>(prefix + _unique_name + "/track_length_th");
close_loops_by_id = _server.getParam<bool>(prefix + _unique_name + "/close_loops_by_id");
close_loops_by_type = _server.getParam<bool>(prefix + _unique_name + "/close_loops_by_type");
}
};
WOLF_PTR_TYPEDEFS(ProcessorLandmarkExternal);
// Class
class ProcessorLandmarkExternal : public ProcessorTracker
{
public:
ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tracker_feature);
~ProcessorLandmarkExternal() override{};
ProcessorLandmarkExternal(const YAML::Node& _params);
~ProcessorLandmarkExternal() override = default;
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorLandmarkExternal, ParamsProcessorLandmarkExternal);
WOLF_PROCESSOR_CREATE(ProcessorLandmarkExternal);
void configure(SensorBasePtr _sensor) override{};
protected:
ParamsProcessorLandmarkExternalPtr params_tfle_;
TrackMatrix track_matrix_;
// std::set<SizeStd> lmks_ids_origin_;
TrackMatrix track_matrix_;
/** Pre-process incoming Capture
*
......@@ -138,13 +89,13 @@ class ProcessorLandmarkExternal : public ProcessorTracker
/** \brief Emplaces a landmark or modifies (if needed) a landmark
* \param _feature_ptr pointer to the Feature
*/
LandmarkExternalPtr emplaceLandmark(FeatureLandmarkExternalPtr _feature_ptr);
LandmarkBasePtr emplaceLandmark(FeatureLandmarkExternalPtr _feature_ptr);
/** \brief Modifies (if needed) a landmark
* \param _landmark pointer to the landmark
* \param _feature pointer to the Feature.
*/
void modifyLandmark(LandmarkExternalPtr _landmark, FeatureLandmarkExternalPtr _feature);
void modifyLandmark(LandmarkBasePtr _landmark, FeatureLandmarkExternalPtr _feature);
/** Post-process
*
......@@ -167,14 +118,43 @@ class ProcessorLandmarkExternal : public ProcessorTracker
LandmarkBasePtr _lmk,
const VectorComposite& _pose_frm,
const VectorComposite& _pose_sen) const;
// PROCESSOR PARAMETERS
int dim_; ///< dimension of the problem (2D or 3D)
bool use_orientation_; ///< use orientation measure or not when emplacing factors
unsigned int new_features_for_keyframe_; ///< for keyframe voting: amount of new features with respect to origin
///< (sufficient condition if more than min_features_for_keyframe)
double time_span_; ///< for keyframe voting: time span since last frame (sufficient condition if more than
///< min_features_for_keyframe)
double quality_th_; ///< min quality to consider the detection
// Matching distance threshold to previous detection considering motion (necessary condition)
double match_dist_th_id_; ///< Match by ID
double match_dist_th_type_; ///< Match by TYPE
double match_dist_th_unknown_; ///< No ID/TYPE information
unsigned int track_length_th_; ///< Track length threshold to emplace factors (necessary condition)
bool close_loops_by_id_; ///< Close loop if ID matches (ID unchanged guaranteed)
bool close_loops_by_type_; ///< Close loop if TYPE matches (also distance check)
};
inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tfle)
: ProcessorTracker("ProcessorLandmarkExternal", "PO", 0, _params_tfle),
params_tfle_(_params_tfle)//,lmks_ids_origin_()
{
//
}
inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(const YAML::Node& _params)
: ProcessorTracker("ProcessorLandmarkExternal",
{{'P', _params["dimension"].as<int>() == 2 ? "StatePoint2d" : "StatePoint3d"},
{'O', _params["dimension"].as<int>() == 2 ? "StateAngle" : "StateQuaternion"}},
_params),
dim_(_params["dimension"].as<int>()),
use_orientation_(_params["use_orientation"].as<bool>()),
new_features_for_keyframe_(_params["keyframe_vote"]["new_features_for_keyframe"].as<unsigned int>()),
time_span_(_params["keyframe_vote"]["time_span"].as<double>()),
quality_th_(_params["quality_th"].as<double>()),
match_dist_th_id_(_params["match_dist_th_id"].as<double>()),
match_dist_th_type_(_params["match_dist_th_type"].as<double>()),
match_dist_th_unknown_(_params["match_dist_th_unknown"].as<double>()),
track_length_th_(_params["track_length_th"].as<unsigned int>()),
close_loops_by_id_(_params["close_loops_by_id"].as<bool>()),
close_loops_by_type_(_params["close_loops_by_type"].as<bool>()){};
} // namespace wolf
#endif
\ No newline at end of file
include/core/processor/processor_loop_closure.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef _WOLF_PROCESSOR_LOOP_CLOSURE_BASE_H
#define _WOLF_PROCESSOR_LOOP_CLOSURE_BASE_H
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// Wolf related headers
#include "core/processor/processor_base.h"
namespace wolf{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosure);
struct ParamsProcessorLoopClosure : public ParamsProcessorBase
namespace wolf
{
int max_loops=-1;
ParamsProcessorLoopClosure() = default;
ParamsProcessorLoopClosure(std::string _unique_name, const ParamsServer& _server):
ParamsProcessorBase(_unique_name, _server)
{
max_loops = _server.getParam<int>(prefix + _unique_name + "/max_loops");
}
std::string print() const override
{
return "\n" + ParamsProcessorBase::print()
+ "max_loops: " + std::to_string(max_loops) + "\n";
}
};
WOLF_STRUCT_PTR_TYPEDEFS(MatchLoopClosure);
/** \brief Match between a capture and a capture
......@@ -56,9 +32,9 @@ WOLF_STRUCT_PTR_TYPEDEFS(MatchLoopClosure);
*/
struct MatchLoopClosure
{
CaptureBasePtr capture_reference; ///< Capture reference
CaptureBasePtr capture_target; ///< Capture target
double normalized_score; ///< normalized similarity score (0 is bad, 1 is good)
CaptureBasePtr capture_reference; ///< Capture reference
CaptureBasePtr capture_target; ///< Capture target
double normalized_score; ///< normalized similarity score (0 is bad, 1 is good)
};
/** \brief General loop closure processor
......@@ -75,57 +51,77 @@ struct MatchLoopClosure
*/
class ProcessorLoopClosure : public ProcessorBase
{
protected:
ParamsProcessorLoopClosurePtr params_loop_closure_;
public:
ProcessorLoopClosure(const std::string& _type, const TypeComposite& _state_types, const YAML::Node& _params);
public:
~ProcessorLoopClosure() override = default;
void configure(SensorBasePtr _sensor) override{};
ProcessorLoopClosure(const std::string& _type, int _dim, ParamsProcessorLoopClosurePtr _params_loop_closure);
int getMaxLoops() const;
void setMaxLoops(int _max_loops);
~ProcessorLoopClosure() override = default;
void configure(SensorBasePtr _sensor) override { };
protected:
/** \brief Process a capture (linked to a frame)
* If voteFindLoopClosures() returns true, findLoopClosures() is called.
* emplaceFactors() is called for pairs of current capture and each capture returned by findLoopClosures()
*/
virtual void process(CaptureBasePtr);
protected:
/** \brief Returns if findLoopClosures() has to be called for the given capture
*/
virtual bool voteFindLoopClosures(CaptureBasePtr cap) = 0;
/** \brief Process a capture (linked to a frame)
* If voteFindLoopClosures() returns true, findLoopClosures() is called.
* emplaceFactors() is called for pairs of current capture and each capture returned by findLoopClosures()
*/
virtual void process(CaptureBasePtr);
/** \brief detects and emplaces all features of the given capture
*/
virtual void emplaceFeatures(CaptureBasePtr cap) = 0;
/** \brief Returns if findLoopClosures() has to be called for the given capture
*/
virtual bool voteFindLoopClosures(CaptureBasePtr cap) = 0;
/** \brief Find captures that correspond to loop closures with the given capture
*/
virtual std::map<double, MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) = 0;
/** \brief detects and emplaces all features of the given capture
*/
virtual void emplaceFeatures(CaptureBasePtr cap) = 0;
/** \brief validates a loop closure
*/
virtual bool validateLoopClosure(MatchLoopClosurePtr) = 0;
/** \brief Find captures that correspond to loop closures with the given capture
*/
virtual std::map<double,MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) = 0;
/** \brief emplaces the factor(s) corresponding to a Loop Closure between two captures
*/
virtual void emplaceFactors(MatchLoopClosurePtr) = 0;
/** \brief validates a loop closure
*/
virtual bool validateLoopClosure(MatchLoopClosurePtr) = 0;
void processCapture(CaptureBasePtr) override;
void processKeyFrame(FrameBasePtr _frm) override;
/** \brief emplaces the factor(s) corresponding to a Loop Closure between two captures
*/
virtual void emplaceFactors(MatchLoopClosurePtr) = 0;
void processCapture(CaptureBasePtr) override;
void processKeyFrame(FrameBasePtr _frm) override;
bool triggerInCapture(CaptureBasePtr _cap) const override { return true;};
bool triggerInKeyFrame(FrameBasePtr _frm) const override { return true;};
bool triggerInCapture(CaptureBasePtr _cap) const override
{
return true;
};
bool triggerInKeyFrame(FrameBasePtr _frm) const override
{
return true;
};
bool storeKeyFrame(FrameBasePtr _frm) override { return false;};
bool storeCapture(CaptureBasePtr _cap) override { return false;};
bool storeKeyFrame(FrameBasePtr _frm) override
{
return false;
};
bool storeCapture(CaptureBasePtr _cap) override
{
return false;
};
bool voteForKeyFrame() const override { return false;};
bool voteForKeyFrame() const override
{
return false;
};
};
} // namespace wolf
inline int ProcessorLoopClosure::getMaxLoops() const
{
return params_["max_loops"].as<int>();
}
inline void ProcessorLoopClosure::setMaxLoops(int _max_loops)
{
params_["max_loops"] = _max_loops;
}
#endif /* _WOLF_PROCESSOR_LOOP_CLOSURE_BASE_H */
} // namespace wolf
include/core/processor/processor_motion.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/**
* \file processor_motion.h
*
* Created on: 15/03/2016
* \author: jsola
*/
#ifndef PROCESSOR_MOTION_H_
#define PROCESSOR_MOTION_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// Wolf
#include "core/processor/motion_provider.h"
#include "core/capture/capture_motion.h"
#include "core/processor/processor_base.h"
#include "core/common/time_stamp.h"
#include "core/utils/params_server.h"
// std
#include <iomanip>
namespace wolf
{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorMotion);
struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsMotionProvider
{
double max_time_span = 0.5;
unsigned int max_buff_length = 10;
double dist_traveled = 5;
double angle_turned = 0.5;
double unmeasured_perturbation_std = 1e-4;
ParamsProcessorMotion() = default;
ParamsProcessorMotion(std::string _unique_name, const ParamsServer& _server):
ParamsProcessorBase(_unique_name, _server),
ParamsMotionProvider(_unique_name, _server)
{
max_time_span = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/max_time_span");
max_buff_length = _server.getParam<unsigned int>(prefix + _unique_name + "/keyframe_vote/max_buff_length");
dist_traveled = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/dist_traveled");
angle_turned = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/angle_turned");
unmeasured_perturbation_std = _server.getParam<double>(prefix + _unique_name + "/unmeasured_perturbation_std");
}
std::string print() const override
{
return ParamsProcessorBase::print() + "\n" +
ParamsMotionProvider::print() + "\n"
+ "max_time_span: " + std::to_string(max_time_span) + "\n"
+ "max_buff_length: " + std::to_string(max_buff_length) + "\n"
+ "dist_traveled: " + std::to_string(dist_traveled) + "\n"
+ "angle_turned: " + std::to_string(angle_turned) + "\n"
+ "unmeasured_perturbation_std: " + std::to_string(unmeasured_perturbation_std) + "\n";
}
};
/** \brief class for Motion processors
*
* This processor integrates motion data into vehicle states.
......@@ -103,7 +56,7 @@ struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsMotionPr
*
* which are called at the beginning and at the end of process(). See the doc of these functions for more info.
*/
/* // TODO: JS: review these instructions from here onwards:
/* // TODO: JS: review these instructions from here onwards:
*
* while the integrated motion refers to the robot frame.
*
......@@ -155,415 +108,424 @@ struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsMotionPr
*/
class ProcessorMotion : public ProcessorBase, public MotionProvider
{
public:
typedef enum {
FIRST_TIME_WITHOUT_KF,
FIRST_TIME_WITH_KF_BEFORE_INCOMING,
FIRST_TIME_WITH_KF_ON_INCOMING,
FIRST_TIME_WITH_KF_AFTER_INCOMING,
RUNNING_WITHOUT_KF,
RUNNING_WITH_KF_BEFORE_ORIGIN,
RUNNING_WITH_KF_ON_ORIGIN,
RUNNING_WITH_KF_AFTER_ORIGIN
} ProcessingStep ;
protected:
ParamsProcessorMotionPtr params_motion_;
ProcessingStep processing_step_; ///< State machine controlling the processing step
bool bootstrapping_; ///< processor is bootstrapping
// This is the main public interface
public:
ProcessorMotion(const std::string& _type,
std::string _state_structure,
int _dim,
SizeEigen _state_size,
SizeEigen _delta_size,
SizeEigen _delta_cov_size,
SizeEigen _data_size,
SizeEigen _calib_size,
ParamsProcessorMotionPtr _params_motion);
~ProcessorMotion() override;
// Instructions to the processor:
virtual void resetDerived();
// Queries to the processor:
TimeStamp getTimeStamp() const override;
VectorComposite getState(const StateStructure& _structure = "") const override;
VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override;
/** \brief Gets the delta preintegrated covariance from all integrations so far
* \return the delta preintegrated covariance matrix
*/
const Eigen::MatrixXd getCurrentDeltaPreintCov() const;
/** \brief Provide the motion integrated so far
* \return the integrated motion
*/
Motion getMotion() const;
/** \brief Provide the motion integrated until a given timestamp
* \return the integrated motion
*/
Motion getMotion(const TimeStamp& _ts) const;
/** \brief Finds the capture that contains the closest previous motion of _ts
* \return a pointer to the capture (if it exists) or a nullptr (otherwise)
*/
CaptureMotionConstPtr findCaptureContainingTimeStamp(const TimeStamp& _ts) const;
CaptureMotionPtr findCaptureContainingTimeStamp(const TimeStamp& _ts);
/** Set the origin of all motion for this processor
* \param _origin_frame the keyframe to be the origin
*/
void setOrigin(FrameBasePtr _origin_frame);
/** Set the origin of all motion for this processor
* \param _x_origin the state at the origin
* \param _ts_origin origin timestamp.
*/
FrameBasePtr setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin);
// Helper functions:
MotionBuffer& getBuffer();
const MotionBuffer& getBuffer() const;
protected:
/** \brief process an incoming capture
*
* Each derived processor should implement this function. It will be called if:
* - A new capture arrived and triggerInCapture() returned true.
*/
void processCapture(CaptureBasePtr) override;
/** \brief process an incoming key-frame
*
* The ProcessorMotion only processes incoming captures (it is not called).
*/
void processKeyFrame(FrameBasePtr _keyframe_ptr) override {};
/** \brief trigger in capture
*
* Returns true if processCapture() should be called after the provided capture arrived.
*/
bool triggerInCapture(CaptureBasePtr) const override {return true;}
/** \brief trigger in key-frame
*
* The ProcessorMotion only processes incoming captures, then it returns false.
*/
bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override {return false;}
/** \brief store key frame
*
* Returns true if the key frame should be stored
*/
bool storeKeyFrame(FrameBasePtr) override { return true;}
/** \brief store capture
*
* Returns true if the capture should be stored
*/
bool storeCapture(CaptureBasePtr) override { return false;}
bool voteForKeyFrame() const override;
double updateDt();
/** \brief Make one step of motion integration
*
* Integrate motiondata in incoming_ptr_ and store all results in the MotionBuffer in last_ptr_
*/
void integrateOneStep();
void reintegrateBuffer(CaptureMotionPtr _capture_ptr) const;
void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
const TimeStamp ts_split,
const wolf::CaptureMotionPtr& capture_target) const;
/** Pre-process incoming Capture
*
* This is called by process() just after assigning incoming_ptr_ to a valid Capture.
*
* Overload this function to prepare stuff on derived classes.
*
* Typical uses of prePrecess() are:
* - casting base types to derived types
* - initializing counters, flags, or any derived variables
* - initializing algorithms needed for processing the derived data
*/
virtual void preProcess(){ };
/**
* @brief Bootstrap the processor with some initialization steps
*/
virtual void bootstrap(){};
/** Post-process
*
* This is called by process() after finishing the processing algorithm.
*
* Overload this function to post-process stuff on derived classes.
*
* Typical uses of postPrecess() are:
* - resetting and/or clearing variables and/or algorithms at the end of processing
* - drawing / printing / logging the results of the processing
*/
virtual void postProcess(){ };
FrameBasePtr computeProcessingStep();
void assertsCaptureMotion(CaptureMotionPtr _capture, std::string error_prefix) const;
// These are the pure virtual functions doing the mathematics
public:
/** \brief convert raw CaptureMotion data to the delta-state format
*
* This function accepts raw data and time step dt,
* and computes the value of the delta-state and its covariance. Note that these values are
* held by the members delta_ and delta_cov_.
*
* @param _data measured motion data
* @param _data_cov covariance
* @param _dt time step
* @param _delta computed delta
* @param _delta_cov covariance
* @param _calib current state of the calibrated parameters
* @param _jacobian_calib Jacobian of the delta wrt calib
*
* Rationale:
*
* The delta-state format must be compatible for integration using
* the composition functions doing the math in this class: statePlusDelta() and deltaPlusDelta().
* See the class documentation for some Eigen::VectorXd suggestions.
*
* The data format is generally not the same as the delta format,
* because it is the format of the raw data provided by the Capture,
* which is unaware of the needs of this processor.
*
* Additionally, sometimes the data format is in the form of a
* velocity, or a higher derivative, while the delta is in the form of an increment.
* In such cases, converting from data to delta-state needs integrating
* the data over the period dt.
*
* Two trivial implementations would establish:
* - If `_data` is an increment:
*
* delta_ = _data;
* delta_cov_ = _data_cov
* - If `_data` is a velocity:
*
* delta_ = _data * _dt
* delta_cov_ = _data_cov * _dt.
*
* However, other more complicated relations are possible.
* In general, we'll have a nonlinear
* function relating `delta_` to `_data` and `_dt`, as follows:
*
* delta_ = f ( _data, _dt)
*
* The delta covariance is obtained by classical uncertainty propagation of the data covariance,
* that is, through the Jacobians of `f()`,
*
* delta_cov_ = F_data * _data_cov * F_data.transpose
*
* where `F_data = d_f/d_data` is the Jacobian of `f()`.
*/
virtual void computeCurrentDelta(const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _calib,
const double _dt,
Eigen::VectorXd& _delta,
Eigen::MatrixXd& _delta_cov,
Eigen::MatrixXd& _jacobian_calib) const = 0;
/** \brief composes a delta-state on top of another delta-state
* \param _delta1 the first delta-state
* \param _delta2 the second delta-state
* \param _dt2 the second delta-state's time delta
* \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state.
*
* This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2.
*/
virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
const Eigen::VectorXd& _delta2,
const double _dt2,
Eigen::VectorXd& _delta1_plus_delta2) const = 0;
/** \brief composes a delta-state on top of another delta-state, and computes the Jacobians
* \param _delta1 the first delta-state
* \param _delta2 the second delta-state
* \param _dt2 the second delta-state's time delta
* \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state.
* \param _jacobian1 the jacobian of the composition w.r.t. _delta1.
* \param _jacobian2 the jacobian of the composition w.r.t. _delta2.
*
* This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2 and its jacobians.
*/
virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
const Eigen::VectorXd& _delta2,
const double _dt2,
Eigen::VectorXd& _delta1_plus_delta2,
Eigen::MatrixXd& _jacobian1,
Eigen::MatrixXd& _jacobian2) const = 0;
/** \brief composes a delta-state on top of a state
* \param _x the initial state
* \param _delta the delta-state
* \param _x_plus_delta the updated state. It has the same format as the initial state.
* \param _dt the time interval spanned by the Delta
*
* This function implements the composition (+) so that _x2 = _x1 (+) _delta.
*/
virtual void statePlusDelta(const VectorComposite& _x,
const Eigen::VectorXd& _delta,
const double _dt,
VectorComposite& _x_plus_delta) const = 0;
/** \brief Delta zero
* \return a delta state equivalent to the null motion.
*
* Examples (see documentation of the the class for info on Eigen::VectorXd):
* - 2d odometry: a 3-vector with all zeros, e.g. VectorXd::Zero(3)
* - 3d odometry: delta type is a PQ vector: 7-vector with [0,0,0, 0,0,0,1]
* - IMU: PQVBB 10-vector with [0,0,0, 0,0,0,1, 0,0,0] // No biases in the delta !!
*/
virtual Eigen::VectorXd deltaZero() const = 0;
/** \brief correct the preintegrated delta
* This produces a delta correction according to the appropriate manifold.
* @param delta_preint : the preintegrated delta to correct
* @param delta_step : an increment in the tangent space of the manifold
*
* We want to do
*
* delta_corr = delta_preint (+) d_step
*
* where the operator (+) is the right-plus operator on the delta's manifold.
*
* Note: usually in motion pre-integration we have
*
* delta_step = Jac_delta_calib * (calib - calib_pre)
*
*/
virtual Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
const Eigen::VectorXd& delta_step) const = 0;
/** \brief merge two consecutive capture motions into the second one
* Merge two consecutive capture motions into the second one.
* The first capture motion is not removed.
* If the second capture has feature and factor emplaced, they are replaced by a new ones.
* @param cap_prev : the first capture motion to be merged (input)
* @param cap_target : the second capture motion (modified)
*/
void mergeCaptures(CaptureMotionPtr cap_prev,
CaptureMotionPtr cap_target);
protected:
/** \brief emplace a CaptureMotion
* \param _frame_own frame owning the Capture to create
* \param _sensor Sensor that produced the Capture
* \param _ts time stamp
* \param _data a vector of motion data
* \param _data_cov covariances matrix of the motion data data
* \param _calib calibration vector
* \param _calib_preint calibration vector used during pre-integration
* \param _capture_origin_ptr capture acting as the origin of motions for the MorionBuffer of the created MotionCapture
*/
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
const VectorXd& _data,
const MatrixXd& _data_cov,
const VectorXd& _calib,
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin_ptr) = 0;
/** \brief emplace the features and factors corresponding to given capture and link them to the capture
* \param _capture_own: the parent capture
* \param _capture_origin: the capture constrained by this motion factor
*
* Typical factors to add for a ProcessorMotionDerived can be:
* - A preintegrated motion factor -- this is the main factor
* - A calibration drift factor -- only for dynamic sensor calibration parameters
* - An instantaneous factor for observing non-typical states of the Frame -- useful for complex dynamic robot models
*/
virtual void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) = 0;
virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) = 0;
Motion motionZero(const TimeStamp& _ts) const;
public:
virtual VectorXd getCalibration (const CaptureBaseConstPtr _capture = nullptr) const = 0;
bool hasCalibration() const {return calib_size_ > 0;}
//getters
CaptureMotionConstPtr getOrigin() const;
CaptureMotionConstPtr getLast() const;
CaptureMotionConstPtr getIncoming() const;
CaptureMotionPtr getOrigin();
CaptureMotionPtr getLast();
CaptureMotionPtr getIncoming();
double getMaxTimeSpan() const;
double getMaxBuffLength() const;
double getDistTraveled() const;
double getAngleTurned() const;
void setMaxTimeSpan(const double& _max_time_span);
void setMaxBuffLength(const double& _max_buff_length);
void setDistTraveled(const double& _dist_traveled);
void setAngleTurned(const double& _angle_turned);
void printHeader(int depth, //
bool constr_by, //
bool metric, //
bool state_blocks,
std::ostream& stream ,
std::string _tabs = "") const override;
protected:
// Attributes
SizeEigen x_size_; ///< The size of the state vector
SizeEigen data_size_; ///< the size of the incoming data
SizeEigen delta_size_; ///< the size of the deltas
SizeEigen delta_cov_size_; ///< the size of the delta covariances matrix
SizeEigen calib_size_; ///< the size of the calibration parameters (TBD in derived classes)
CaptureMotionPtr origin_ptr_;
CaptureMotionPtr last_ptr_;
CaptureMotionPtr incoming_ptr_;
FrameBasePtr last_frame_ptr_;
protected:
// helpers to avoid allocation
mutable double dt_; ///< Time step
mutable Eigen::VectorXd x_; ///< current state
mutable Eigen::VectorXd delta_; ///< current delta
mutable Eigen::MatrixXd delta_cov_; ///< current delta covariance
mutable Eigen::VectorXd delta_integrated_; ///< integrated delta
mutable Eigen::MatrixXd delta_integrated_cov_; ///< integrated delta covariance
mutable Eigen::VectorXd calib_preint_; ///< calibration vector used during pre-integration
mutable Eigen::MatrixXd jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated
mutable Eigen::MatrixXd jacobian_delta_; ///< jacobian of delta composition w.r.t current delta
mutable Eigen::MatrixXd jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params
mutable Eigen::MatrixXd jacobian_delta_calib_; ///< jacobian of delta wrt calib params
Eigen::MatrixXd unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity
public:
typedef enum
{
FIRST_TIME_WITHOUT_KF,
FIRST_TIME_WITH_KF_BEFORE_INCOMING,
FIRST_TIME_WITH_KF_ON_INCOMING,
FIRST_TIME_WITH_KF_AFTER_INCOMING,
RUNNING_WITHOUT_KF,
RUNNING_WITH_KF_BEFORE_ORIGIN,
RUNNING_WITH_KF_ON_ORIGIN,
RUNNING_WITH_KF_AFTER_ORIGIN
} ProcessingStep;
protected:
ProcessingStep processing_step_; ///< State machine controlling the processing step
bool bootstrapping_; ///< processor is bootstrapping
// This is the main public interface
public:
ProcessorMotion(const std::string& _type,
TypeComposite _state_types,
SizeEigen _state_size,
SizeEigen _delta_size,
SizeEigen _delta_cov_size,
SizeEigen _data_size,
SizeEigen _calib_size,
const YAML::Node& _params);
~ProcessorMotion() override;
// Instructions to the processor:
virtual void resetDerived();
// Queries to the processor:
TimeStamp getTimeStamp() const override;
VectorComposite getState(StateKeys _keys = "") const override;
VectorComposite getState(const TimeStamp& _ts, StateKeys _keys = "") const override;
/** \brief Gets the delta preintegrated covariance from all integrations so far
* \return the delta preintegrated covariance matrix
*/
const Eigen::MatrixXd getCurrentDeltaPreintCov() const;
/** \brief Provide the motion integrated so far
* \return the integrated motion
*/
Motion getMotion() const;
/** \brief Provide the motion integrated until a given timestamp
* \return the integrated motion
*/
Motion getMotion(const TimeStamp& _ts) const;
/** \brief Finds the capture that contains the closest previous motion of _ts
* \return a pointer to the capture (if it exists) or a nullptr (otherwise)
*/
CaptureMotionConstPtr findCaptureContainingTimeStamp(const TimeStamp& _ts) const;
CaptureMotionPtr findCaptureContainingTimeStamp(const TimeStamp& _ts);
/** Set the origin of all motion for this processor
* \param _origin_frame the keyframe to be the origin
*/
void setOrigin(FrameBasePtr _origin_frame);
/** Set the origin of all motion for this processor
* \param _x_origin the state at the origin
* \param _ts_origin origin timestamp.
*/
FrameBasePtr setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin);
// Helper functions:
MotionBuffer& getBuffer();
const MotionBuffer& getBuffer() const;
protected:
/** \brief process an incoming capture
*
* Each derived processor should implement this function. It will be called if:
* - A new capture arrived and triggerInCapture() returned true.
*/
void processCapture(CaptureBasePtr) override;
/** \brief process an incoming key-frame
*
* The ProcessorMotion only processes incoming captures (it is not called).
*/
void processKeyFrame(FrameBasePtr _keyframe_ptr) override{};
/** \brief trigger in capture
*
* Returns true if processCapture() should be called after the provided capture arrived.
*/
bool triggerInCapture(CaptureBasePtr) const override
{
return true;
}
/** \brief trigger in key-frame
*
* The ProcessorMotion only processes incoming captures, then it returns false.
*/
bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override
{
return false;
}
/** \brief store key frame
*
* Returns true if the key frame should be stored
*/
bool storeKeyFrame(FrameBasePtr) override
{
return true;
}
/** \brief store capture
*
* Returns true if the capture should be stored
*/
bool storeCapture(CaptureBasePtr) override
{
return false;
}
bool voteForKeyFrame() const override;
double updateDt();
/** \brief Make one step of motion integration
*
* Integrate motiondata in incoming_ptr_ and store all results in the MotionBuffer in last_ptr_
*/
void integrateOneStep();
void reintegrateBuffer(CaptureMotionPtr _capture_ptr) const;
void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
const TimeStamp ts_split,
const wolf::CaptureMotionPtr& capture_target) const;
/** Pre-process incoming Capture
*
* This is called by process() just after assigning incoming_ptr_ to a valid Capture.
*
* Overload this function to prepare stuff on derived classes.
*
* Typical uses of prePrecess() are:
* - casting base types to derived types
* - initializing counters, flags, or any derived variables
* - initializing algorithms needed for processing the derived data
*/
virtual void preProcess(){};
/**
* @brief Bootstrap the processor with some initialization steps
*/
virtual void bootstrap(){};
/** Post-process
*
* This is called by process() after finishing the processing algorithm.
*
* Overload this function to post-process stuff on derived classes.
*
* Typical uses of postPrecess() are:
* - resetting and/or clearing variables and/or algorithms at the end of processing
* - drawing / printing / logging the results of the processing
*/
virtual void postProcess(){};
FrameBasePtr computeProcessingStep();
void assertsCaptureMotion(CaptureMotionPtr _capture, std::string error_prefix) const;
// These are the pure virtual functions doing the mathematics
public:
/** \brief convert raw CaptureMotion data to the delta-state format
*
* This function accepts raw data and time step dt,
* and computes the value of the delta-state and its covariance. Note that these values are
* held by the members delta_ and delta_cov_.
*
* @param _data measured motion data
* @param _data_cov covariance
* @param _dt time step
* @param _delta computed delta
* @param _delta_cov covariance
* @param _calib current state of the calibrated parameters
* @param _jacobian_calib Jacobian of the delta wrt calib
*
* Rationale:
*
* The delta-state format must be compatible for integration using
* the composition functions doing the math in this class: statePlusDelta() and deltaPlusDelta().
* See the class documentation for some Eigen::VectorXd suggestions.
*
* The data format is generally not the same as the delta format,
* because it is the format of the raw data provided by the Capture,
* which is unaware of the needs of this processor.
*
* Additionally, sometimes the data format is in the form of a
* velocity, or a higher derivative, while the delta is in the form of an increment.
* In such cases, converting from data to delta-state needs integrating
* the data over the period dt.
*
* Two trivial implementations would establish:
* - If `_data` is an increment:
*
* delta_ = _data;
* delta_cov_ = _data_cov
* - If `_data` is a velocity:
*
* delta_ = _data * _dt
* delta_cov_ = _data_cov * _dt.
*
* However, other more complicated relations are possible.
* In general, we'll have a nonlinear
* function relating `delta_` to `_data` and `_dt`, as follows:
*
* delta_ = f ( _data, _dt)
*
* The delta covariance is obtained by classical uncertainty propagation of the data covariance,
* that is, through the Jacobians of `f()`,
*
* delta_cov_ = F_data * _data_cov * F_data.transpose
*
* where `F_data = d_f/d_data` is the Jacobian of `f()`.
*/
virtual void computeCurrentDelta(const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _calib,
const double _dt,
Eigen::VectorXd& _delta,
Eigen::MatrixXd& _delta_cov,
Eigen::MatrixXd& _jacobian_calib) const = 0;
/** \brief composes a delta-state on top of another delta-state
* \param _delta1 the first delta-state
* \param _delta2 the second delta-state
* \param _dt2 the second delta-state's time delta
* \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state.
*
* This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2.
*/
virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
const Eigen::VectorXd& _delta2,
const double _dt2,
Eigen::VectorXd& _delta1_plus_delta2) const = 0;
/** \brief composes a delta-state on top of another delta-state, and computes the Jacobians
* \param _delta1 the first delta-state
* \param _delta2 the second delta-state
* \param _dt2 the second delta-state's time delta
* \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state.
* \param _jacobian1 the jacobian of the composition w.r.t. _delta1.
* \param _jacobian2 the jacobian of the composition w.r.t. _delta2.
*
* This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2 and its
* jacobians.
*/
virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
const Eigen::VectorXd& _delta2,
const double _dt2,
Eigen::VectorXd& _delta1_plus_delta2,
Eigen::MatrixXd& _jacobian1,
Eigen::MatrixXd& _jacobian2) const = 0;
/** \brief composes a delta-state on top of a state
* \param _x the initial state
* \param _delta the delta-state
* \param _x_plus_delta the updated state. It has the same format as the initial state.
* \param _dt the time interval spanned by the Delta
*
* This function implements the composition (+) so that _x2 = _x1 (+) _delta.
*/
virtual void statePlusDelta(const VectorComposite& _x,
const Eigen::VectorXd& _delta,
const double _dt,
VectorComposite& _x_plus_delta) const = 0;
/** \brief Delta zero
* \return a delta state equivalent to the null motion.
*
* Examples (see documentation of the the class for info on Eigen::VectorXd):
* - 2d odometry: a 3-vector with all zeros, e.g. VectorXd::Zero(3)
* - 3d odometry: delta type is a PQ vector: 7-vector with [0,0,0, 0,0,0,1]
* - IMU: PQVBB 10-vector with [0,0,0, 0,0,0,1, 0,0,0] // No biases in the delta !!
*/
virtual Eigen::VectorXd deltaZero() const = 0;
/** \brief correct the preintegrated delta
* This produces a delta correction according to the appropriate manifold.
* @param delta_preint : the preintegrated delta to correct
* @param delta_step : an increment in the tangent space of the manifold
*
* We want to do
*
* delta_corr = delta_preint (+) d_step
*
* where the operator (+) is the right-plus operator on the delta's manifold.
*
* Note: usually in motion pre-integration we have
*
* delta_step = Jac_delta_calib * (calib - calib_pre)
*
*/
virtual Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
const Eigen::VectorXd& delta_step) const = 0;
/** \brief merge two consecutive capture motions into the second one
* Merge two consecutive capture motions into the second one.
* The first capture motion is not removed.
* If the second capture has feature and factor emplaced, they are replaced by a new ones.
* @param cap_prev : the first capture motion to be merged (input)
* @param cap_target : the second capture motion (modified)
*/
void mergeCaptures(CaptureMotionPtr cap_prev, CaptureMotionPtr cap_target);
protected:
/** \brief emplace a CaptureMotion
* \param _frame_own frame owning the Capture to create
* \param _sensor Sensor that produced the Capture
* \param _ts time stamp
* \param _data a vector of motion data
* \param _data_cov covariances matrix of the motion data data
* \param _calib calibration vector
* \param _calib_preint calibration vector used during pre-integration
* \param _capture_origin_ptr capture acting as the origin of motions for the MorionBuffer of the created MotionCapture
*/
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
const VectorXd& _data,
const MatrixXd& _data_cov,
const VectorXd& _calib,
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin_ptr) = 0;
/** \brief emplace the features and factors corresponding to given capture and link them to the capture
* \param _capture_own: the parent capture
* \param _capture_origin: the capture constrained by this motion factor
*
* Typical factors to add for a ProcessorMotionDerived can be:
* - A preintegrated motion factor -- this is the main factor
* - A calibration drift factor -- only for dynamic sensor calibration parameters
* - An instantaneous factor for observing non-typical states of the Frame -- useful for complex dynamic robot
* models
*/
virtual void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) = 0;
virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) = 0;
Motion motionZero(const TimeStamp& _ts) const;
public:
virtual VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const = 0;
bool hasCalibration() const
{
return calib_size_ > 0;
}
// getters
CaptureMotionConstPtr getOrigin() const;
CaptureMotionConstPtr getLast() const;
CaptureMotionConstPtr getIncoming() const;
CaptureMotionPtr getOrigin();
CaptureMotionPtr getLast();
CaptureMotionPtr getIncoming();
Eigen::MatrixXd getUnmeasuredPerturbationCov() const;
double getMaxTimeSpan() const;
double getMaxBuffLength() const;
double getMaxDistTraveled() const;
double getMaxAngleTurned() const;
void setMaxTimeSpan(const double& _max_time_span);
void setMaxBuffLength(const double& _max_buff_length);
void setMaxDistTraveled(const double& _max_dist_traveled);
void setMaxAngleTurned(const double& _max_angle_turned);
void printHeader(int depth, //
bool factored_by, //
bool metric, //
bool state_blocks,
std::ostream& stream,
std::string _tabs = "") const override;
protected:
// Attributes
SizeEigen x_size_; ///< The size of the state vector
SizeEigen data_size_; ///< the size of the incoming data
SizeEigen delta_size_; ///< the size of the deltas
SizeEigen delta_cov_size_; ///< the size of the delta covariances matrix
SizeEigen calib_size_; ///< the size of the calibration parameters (TBD in derived classes)
CaptureMotionPtr origin_ptr_;
CaptureMotionPtr last_ptr_;
CaptureMotionPtr incoming_ptr_;
protected:
// helpers to avoid allocation
mutable double dt_; ///< Time step
mutable Eigen::VectorXd x_; ///< current state
mutable Eigen::VectorXd delta_; ///< current delta
mutable Eigen::MatrixXd delta_cov_; ///< current delta covariance
mutable Eigen::VectorXd delta_integrated_; ///< integrated delta
mutable Eigen::MatrixXd delta_integrated_cov_; ///< integrated delta covariance
mutable Eigen::VectorXd calib_preint_; ///< calibration vector used during pre-integration
mutable Eigen::MatrixXd jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated
mutable Eigen::MatrixXd jacobian_delta_; ///< jacobian of delta composition w.r.t current delta
mutable Eigen::MatrixXd jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params
mutable Eigen::MatrixXd jacobian_delta_calib_; ///< jacobian of delta wrt calib params
Eigen::MatrixXd unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity
};
}
} // namespace wolf
#include "core/frame/frame_base.h"
namespace wolf{
namespace wolf
{
inline void ProcessorMotion::resetDerived()
{
// Blank function, to be implemented in derived classes
......@@ -612,15 +574,15 @@ inline MotionBuffer& ProcessorMotion::getBuffer()
inline Motion ProcessorMotion::motionZero(const TimeStamp& _ts) const
{
return Motion(_ts,
VectorXd::Zero(data_size_), // data
Eigen::MatrixXd::Zero(data_size_, data_size_), // Cov data
VectorXd::Zero(data_size_), // data
Eigen::MatrixXd::Zero(data_size_, data_size_), // Cov data
deltaZero(),
Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Cov delta
Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Cov delta
deltaZero(),
Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Cov delta_integr
Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Jac delta
Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Jac delta_integr
Eigen::MatrixXd::Zero(delta_cov_size_, calib_size_) // Jac calib
Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Cov delta_integr
Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Jac delta
Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Jac delta_integr
Eigen::MatrixXd::Zero(delta_cov_size_, calib_size_) // Jac calib
);
}
......@@ -654,43 +616,49 @@ inline CaptureMotionPtr ProcessorMotion::getIncoming()
return incoming_ptr_;
}
inline Eigen::MatrixXd ProcessorMotion::getUnmeasuredPerturbationCov() const
{
return unmeasured_perturbation_cov_;
}
inline double ProcessorMotion::getMaxTimeSpan() const
{
return params_motion_->max_time_span;
return params_["keyframe_vote"]["max_time_span"].as<double>();
}
inline double ProcessorMotion::getMaxBuffLength() const
{
return params_motion_->max_buff_length;
return params_["keyframe_vote"]["max_buff_length"].as<unsigned int>();
}
inline double ProcessorMotion::getDistTraveled() const
inline double ProcessorMotion::getMaxDistTraveled() const
{
return params_motion_->dist_traveled;
return params_["keyframe_vote"]["max_dist_traveled"].as<double>();
}
inline double ProcessorMotion::getAngleTurned() const
inline double ProcessorMotion::getMaxAngleTurned() const
{
return params_motion_->angle_turned;
return params_["keyframe_vote"]["max_angle_turned"].as<double>();
}
inline void ProcessorMotion::setMaxTimeSpan(const double& _max_time_span)
{
params_motion_->max_time_span = _max_time_span;
params_["keyframe_vote"]["max_time_span"] = _max_time_span;
}
inline void ProcessorMotion::setMaxBuffLength(const double& _max_buff_length)
{
params_motion_->max_buff_length = _max_buff_length;
params_["keyframe_vote"]["max_buff_length"] = _max_buff_length;
}
inline void ProcessorMotion::setDistTraveled(const double& _dist_traveled)
inline void ProcessorMotion::setMaxDistTraveled(const double& _max_dist_traveled)
{
params_motion_->dist_traveled = _dist_traveled;
params_["keyframe_vote"]["max_dist_traveled"] = _max_dist_traveled;
}
inline void ProcessorMotion::setAngleTurned(const double& _angle_turned)
inline void ProcessorMotion::setMaxAngleTurned(const double& _max_angle_turned)
{
params_motion_->angle_turned = _angle_turned;
params_["keyframe_vote"]["max_angle_turned"] = _max_angle_turned;
}
} // namespace wolf
#endif /* PROCESSOR_MOTION2_H_ */
} // namespace wolf
include/core/processor/processor_odom_2d.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/**
* \file processor_odom_2d.h
*
* Created on: Apr 15, 2016
* \author: jvallve
*/
#ifndef SRC_PROCESSOR_ODOM_2d_H_
#define SRC_PROCESSOR_ODOM_2d_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/processor/processor_motion.h"
#include "core/capture/capture_odom_2d.h"
#include "core/math/rotations.h"
#include "core/utils/params_server.h"
#include "core/math/SE2.h"
namespace wolf {
WOLF_PTR_TYPEDEFS(ProcessorOdom2d);
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdom2d);
struct ParamsProcessorOdom2d : public ParamsProcessorMotion
namespace wolf
{
double cov_det = 1.0;
ParamsProcessorOdom2d() = default;
ParamsProcessorOdom2d(std::string _unique_name, const wolf::ParamsServer & _server) :
ParamsProcessorMotion(_unique_name, _server)
{
cov_det = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/cov_det");
}
std::string print() const override
{
return ParamsProcessorMotion::print() + "\n"
+ "cov_det: " + std::to_string(cov_det) + "\n";
}
};
WOLF_PTR_TYPEDEFS(ProcessorOdom2d);
class ProcessorOdom2d : public ProcessorMotion
{
public:
ProcessorOdom2d(ParamsProcessorOdom2dPtr _params);
~ProcessorOdom2d() override;
void configure(SensorBasePtr _sensor) override { };
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorOdom2d, ParamsProcessorOdom2d);
bool voteForKeyFrame() const override;
protected:
void computeCurrentDelta(const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _calib,
const double _dt,
Eigen::VectorXd& _delta,
Eigen::MatrixXd& _delta_cov,
Eigen::MatrixXd& _jacobian_calib) const override;
void deltaPlusDelta(const Eigen::VectorXd& _delta1,
const Eigen::VectorXd& _delta2,
const double _Dt2,
Eigen::VectorXd& _delta1_plus_delta2) const override;
void deltaPlusDelta(const Eigen::VectorXd& _delta1,
const Eigen::VectorXd& _delta2,
const double _Dt2,
Eigen::VectorXd& _delta1_plus_delta2,
Eigen::MatrixXd& _jacobian1,
Eigen::MatrixXd& _jacobian2) const override;
void statePlusDelta(const VectorComposite& _x,
const Eigen::VectorXd& _delta,
const double _Dt,
VectorComposite& _x_plus_delta) const override;
Eigen::VectorXd deltaZero() const override;
Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
const Eigen::VectorXd& delta_step) const override;
CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
const VectorXd& _data,
const MatrixXd& _data_cov,
const VectorXd& _calib,
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin_ptr) override;
void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
ParamsProcessorOdom2dPtr params_odom_2d_;
public:
ProcessorOdom2d(const YAML::Node& _params);
~ProcessorOdom2d() override;
void configure(SensorBasePtr _sensor) override{};
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorOdom2d);
double getMaxCovDet() const;
bool voteForKeyFrame() const override;
protected:
void computeCurrentDelta(const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _calib,
const double _dt,
Eigen::VectorXd& _delta,
Eigen::MatrixXd& _delta_cov,
Eigen::MatrixXd& _jacobian_calib) const override;
void deltaPlusDelta(const Eigen::VectorXd& _delta1,
const Eigen::VectorXd& _delta2,
const double _Dt2,
Eigen::VectorXd& _delta1_plus_delta2) const override;
void deltaPlusDelta(const Eigen::VectorXd& _delta1,
const Eigen::VectorXd& _delta2,
const double _Dt2,
Eigen::VectorXd& _delta1_plus_delta2,
Eigen::MatrixXd& _jacobian1,
Eigen::MatrixXd& _jacobian2) const override;
void statePlusDelta(const VectorComposite& _x,
const Eigen::VectorXd& _delta,
const double _Dt,
VectorComposite& _x_plus_delta) const override;
Eigen::VectorXd deltaZero() const override;
Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
const Eigen::VectorXd& delta_step) const override;
CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
const VectorXd& _data,
const MatrixXd& _data_cov,
const VectorXd& _calib,
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin_ptr) override;
void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
};
inline Eigen::VectorXd ProcessorOdom2d::deltaZero() const
......@@ -118,23 +84,24 @@ inline Eigen::VectorXd ProcessorOdom2d::deltaZero() const
return Eigen::VectorXd::Zero(delta_size_);
}
inline Eigen::VectorXd ProcessorOdom2d::correctDelta (const Eigen::VectorXd& delta_preint,
const Eigen::VectorXd& delta_step) const
inline Eigen::VectorXd ProcessorOdom2d::correctDelta(const Eigen::VectorXd& delta_preint,
const Eigen::VectorXd& delta_step) const
{
VectorXd delta_corrected(3);
SE2::plus(delta_preint, delta_step, delta_corrected);
return delta_corrected;
}
inline VectorXd ProcessorOdom2d::getCalibration (const CaptureBaseConstPtr _capture) const
inline VectorXd ProcessorOdom2d::getCalibration(const CaptureBaseConstPtr _capture) const
{
return VectorXd::Zero(0);
}
inline void ProcessorOdom2d::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
inline void ProcessorOdom2d::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) {}
inline double ProcessorOdom2d::getMaxCovDet() const
{
return params_["keyframe_vote"]["max_cov_det"].as<double>();
}
} // namespace wolf
#endif /* SRC_PROCESSOR_ODOM_2d_H_ */
} // namespace wolf
include/core/processor/processor_odom_3d.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/**
* \file processor_odom_3d.h
*
* Created on: Mar 18, 2016
* \author: jsola
*/
#ifndef SRC_PROCESSOR_ODOM_3d_H_
#define SRC_PROCESSOR_ODOM_3d_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/processor/processor_motion.h"
#include "core/sensor/sensor_odom_3d.h"
#include "core/capture/capture_odom_3d.h"
#include "core/math/rotations.h"
#include "core/factor/factor_relative_pose_3d.h"
#include <cmath>
namespace wolf {
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdom3d);
struct ParamsProcessorOdom3d : public ParamsProcessorMotion
namespace wolf
{
ParamsProcessorOdom3d() = default;
ParamsProcessorOdom3d(std::string _unique_name, const ParamsServer& _server):
ParamsProcessorMotion(_unique_name, _server)
{
//
}
std::string print() const override
{
return ParamsProcessorMotion::print();
}
};
WOLF_PTR_TYPEDEFS(ProcessorOdom3d);
/** \brief Processor for 3d odometry integration.
......@@ -63,7 +35,8 @@ WOLF_PTR_TYPEDEFS(ProcessorOdom3d);
* The odometry data is extracted from Captures of the type CaptureOdometry3d.
* This data comes in the form of a 6-vector, or a 7-vector, containing the following components:
* - a 3d position increment in the local frame of the robot (dx, dy, dz).
* - a 3d orientation increment in the local frame of the robot (droll, dpitch, dyaw), or quaternion (dqx, dqy, dqz, dqw).
* - a 3d orientation increment in the local frame of the robot (droll, dpitch, dyaw), or quaternion (dqx, dqy, dqz,
* dqw).
*
* The produced integrated deltas are in the form of 7-vectors with the following components:
* - a 3d position increment in the local frame of the robot (Dx, Dy, Dz)
......@@ -80,66 +53,60 @@ WOLF_PTR_TYPEDEFS(ProcessorOdom3d);
*/
class ProcessorOdom3d : public ProcessorMotion
{
public:
ProcessorOdom3d(ParamsProcessorOdom3dPtr _params);
~ProcessorOdom3d() override;
void configure(SensorBasePtr _sensor) override;
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorOdom3d, ParamsProcessorOdom3d);
public:
// Motion integration
void computeCurrentDelta(const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _calib,
const double _dt,
Eigen::VectorXd& _delta,
Eigen::MatrixXd& _delta_cov,
Eigen::MatrixXd& _jacobian_calib) const override;
void deltaPlusDelta(const Eigen::VectorXd& _delta1,
const Eigen::VectorXd& _delta2,
const double _Dt2,
Eigen::VectorXd& _delta1_plus_delta2) const override;
void deltaPlusDelta(const Eigen::VectorXd& _delta1,
const Eigen::VectorXd& _delta2,
const double _Dt2,
Eigen::VectorXd& _delta1_plus_delta2,
Eigen::MatrixXd& _jacobian1,
Eigen::MatrixXd& _jacobian2) const override;
void statePlusDelta(const VectorComposite& _x,
const Eigen::VectorXd& _delta,
const double _Dt,
VectorComposite& _x_plus_delta) const override;
Eigen::VectorXd deltaZero() const override;
Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
const Eigen::VectorXd& delta_step) const override;
bool voteForKeyFrame() const override;
CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
const VectorXd& _data,
const MatrixXd& _data_cov,
const VectorXd& _calib,
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin) override;
void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
ParamsProcessorOdom3dPtr params_odom_3d_;
public:
ProcessorOdom3d(const YAML::Node& _params);
~ProcessorOdom3d() override;
void configure(SensorBasePtr _sensor) override;
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorOdom3d);
public:
// Motion integration
void computeCurrentDelta(const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _calib,
const double _dt,
Eigen::VectorXd& _delta,
Eigen::MatrixXd& _delta_cov,
Eigen::MatrixXd& _jacobian_calib) const override;
void deltaPlusDelta(const Eigen::VectorXd& _delta1,
const Eigen::VectorXd& _delta2,
const double _Dt2,
Eigen::VectorXd& _delta1_plus_delta2) const override;
void deltaPlusDelta(const Eigen::VectorXd& _delta1,
const Eigen::VectorXd& _delta2,
const double _Dt2,
Eigen::VectorXd& _delta1_plus_delta2,
Eigen::MatrixXd& _jacobian1,
Eigen::MatrixXd& _jacobian2) const override;
void statePlusDelta(const VectorComposite& _x,
const Eigen::VectorXd& _delta,
const double _Dt,
VectorComposite& _x_plus_delta) const override;
Eigen::VectorXd deltaZero() const override;
Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
const Eigen::VectorXd& delta_step) const override;
bool voteForKeyFrame() const override;
CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
const VectorXd& _data,
const MatrixXd& _data_cov,
const VectorXd& _calib,
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin) override;
void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
};
inline Eigen::VectorXd ProcessorOdom3d::deltaZero() const
{
return (Eigen::VectorXd(7) << 0,0,0, 0,0,0,1).finished(); // p, q
return (Eigen::VectorXd(7) << 0, 0, 0, 0, 0, 0, 1).finished(); // p, q
}
} // namespace wolf
#endif /* SRC_PROCESSOR_ODOM_3d_H_ */
} // namespace wolf
include/core/processor/processor_pose.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef PROCESSOR_POSE_NOMOVE_H
#define PROCESSOR_POSE_NOMOVE_H
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// Wolf
#include "core/sensor/sensor_base.h"
#include "core/processor/processor_base.h"
#include "core/capture/capture_pose.h"
#include "core/sensor/sensor_pose.h"
#include "core/factor/factor_pose_3d_with_extrinsics.h"
namespace wolf
{
// class
template <unsigned int DIM>
class ProcessorPose : public ProcessorBase
{
public:
ProcessorPose(const YAML::Node& _params);
~ProcessorPose() override = default;
WOLF_PROCESSOR_TEMPLATE_DIM_CREATE(ProcessorPose, DIM);
namespace wolf {
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorPose);
void configure(SensorBasePtr _sensor) override;
struct ParamsProcessorPose : public ParamsProcessorBase
{
ParamsProcessorPose() = default;
ParamsProcessorPose(std::string _unique_name, const ParamsServer& _server):
ParamsProcessorBase(_unique_name, _server)
void processCapture(CaptureBasePtr _cap) override;
void processKeyFrame(FrameBasePtr _frm) override;
bool triggerInCapture(CaptureBasePtr _cap) const override
{
}
~ParamsProcessorPose() override = default;
std::string print() const override
return true;
};
bool triggerInKeyFrame(FrameBasePtr _frm) const override
{
return "\n" + ParamsProcessorBase::print() + "\n";
}
return true;
};
bool storeKeyFrame(FrameBasePtr _frm) override
{
return true;
};
bool storeCapture(CaptureBasePtr _cap) override
{
return true;
};
bool voteForKeyFrame() const override
{
return false;
};
void createFactorIfNecessary();
};
WOLF_PTR_TYPEDEFS(ProcessorPose);
//class
class ProcessorPose : public ProcessorBase{
public:
ProcessorPose(ParamsProcessorPosePtr _params_pfnomove);
~ProcessorPose() override = default;
WOLF_PROCESSOR_CREATE(ProcessorPose, ParamsProcessorPose);
void configure(SensorBasePtr _sensor) override;
void processCapture(CaptureBasePtr _cap) override;
void processKeyFrame(FrameBasePtr _frm) override;
bool triggerInCapture(CaptureBasePtr _cap) const override { return true;};
bool triggerInKeyFrame(FrameBasePtr _frm) const override { return true;};
bool storeKeyFrame(FrameBasePtr _frm) override { return true;};
bool storeCapture(CaptureBasePtr _cap) override { return true;};
bool voteForKeyFrame() const override { return false;};
void createFactorIfNecessary();
protected:
ParamsProcessorPosePtr params_pfnomove_;
};
typedef ProcessorPose<2> ProcessorPose2d;
typedef ProcessorPose<3> ProcessorPose3d;
WOLF_DECLARED_PTR_TYPEDEFS(ProcessorPose2d);
WOLF_DECLARED_PTR_TYPEDEFS(ProcessorPose3d);
} // namespace wolf
////////////////////////////////////////////////////////////////////////////////////////////////
// IMPLEMENTATION //
////////////////////////////////////////////////////////////////////////////////////////////////
#include "core/sensor/sensor_pose.h"
#include "core/capture/capture_pose.h"
#include "core/factor/factor_pose_3d_with_extrinsics.h"
#include "core/factor/factor_pose_2d_with_extrinsics.h"
namespace wolf
{
template <unsigned int DIM>
inline ProcessorPose<DIM>::ProcessorPose(const YAML::Node& _params)
: ProcessorBase(
"ProcessorPose",
{{'P', DIM == 2 ? "StatePoint2d" : "StatePoint3d"}, {'O', DIM == 2 ? "StateAngle" : "StateQuaternion"}},
_params)
{
static_assert(DIM == 2 or DIM == 3);
}
template <unsigned int DIM>
void ProcessorPose<DIM>::configure(SensorBasePtr _sensor)
{
if (not std::dynamic_pointer_cast<SensorPose<DIM>>(_sensor))
{
throw std::runtime_error("Configuring a ProcessorPose with a sensor " + _sensor->getType() +
", should be SensorPose" + std::to_string(DIM) + "d");
}
}
template <unsigned int DIM>
void ProcessorPose<DIM>::createFactorIfNecessary()
{
auto kf_it_last = buffer_frame_.getContainer().end();
auto kf_it = buffer_frame_.getContainer().begin();
while (kf_it != buffer_frame_.getContainer().end())
{
TimeStamp t = kf_it->first;
auto cap_it = buffer_capture_.selectIterator(t, getTimeTolerance());
// if capture with corresponding timestamp is not found, assume you will get it later
if (cap_it != buffer_capture_.getContainer().end())
{
// if a corresponding capture exists, link it to the KF and create a factor
auto cap = std::static_pointer_cast<CapturePose>(cap_it->second);
cap->link(kf_it->second);
FeatureBasePtr feat =
FeatureBase::emplace<FeatureBase>(cap, "Pose", cap->getData(), cap->getDataCovariance());
if (DIM == 2)
{
FactorBase::emplace<FactorPose2dWithExtrinsics>(feat,
feat->getMeasurement(),
feat->getMeasurementSquareRootInformationUpper(),
kf_it->second,
cap->getSensor(),
shared_from_this(),
applyLossFunction());
}
else
{
FactorBase::emplace<FactorPose3dWithExtrinsics>(feat,
feat->getMeasurement(),
feat->getMeasurementSquareRootInformationUpper(),
kf_it->second,
cap->getSensor(),
shared_from_this(),
applyLossFunction());
}
// erase removes range [first, last): it does not removes last
// so increment the iterator so that it points to the next element in the container
buffer_capture_.getContainer().erase(buffer_capture_.getContainer().begin(), std::next(cap_it));
// we cannot erase on the kf buffer since we are looping over it so we store the iterator for later
kf_it_last = kf_it;
}
kf_it++;
}
// whatever happened, remove very old captures
buffer_capture_.removeUpTo(buffer_frame_.getContainer().begin()->first - 5);
} /* namespace wolf */
// now we erase the kf buffer if there was a match
if (kf_it_last != buffer_frame_.getContainer().end())
{
buffer_frame_.getContainer().erase(buffer_frame_.getContainer().begin(), std::next(kf_it_last));
}
}
/////////////////////////////////////////////////////////
// IMPLEMENTATION. Put your implementation includes here
/////////////////////////////////////////////////////////
template <unsigned int DIM>
inline void ProcessorPose<DIM>::processCapture(CaptureBasePtr _capture)
{
if (!_capture)
{
WOLF_ERROR("Received capture is nullptr.");
return;
}
// nothing to do if any of the two buffer is empty
if (buffer_frame_.empty())
{
WOLF_DEBUG("Frame buffer empty, time ", _capture->getTimeStamp());
return;
}
if (buffer_frame_.empty())
{
WOLF_DEBUG("Capture buffer empty, time ", _capture->getTimeStamp());
return;
}
createFactorIfNecessary();
}
namespace wolf{
template <unsigned int DIM>
inline void ProcessorPose<DIM>::processKeyFrame(FrameBasePtr _keyframe_ptr)
{
if (!_keyframe_ptr)
{
WOLF_ERROR("Received KF is nullptr.");
return;
}
// nothing to do if any of the two buffer is empty
if (buffer_frame_.empty())
{
WOLF_DEBUG("Frame buffer empty, time ", _keyframe_ptr->getTimeStamp());
return;
}
if (buffer_frame_.empty())
{
WOLF_DEBUG("Capture buffer empty, time ", _keyframe_ptr->getTimeStamp());
return;
}
} // namespace wolf
createFactorIfNecessary();
}
#endif // PROCESSOR_POSE_NOMOVE_H
} // namespace wolf
include/core/processor/processor_tracker.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/*
* processor_tracker.h
*
* Created on: Apr 7, 2016
* Author: jvallve
*/
#ifndef SRC_PROCESSOR_TRACKER_H_
#define SRC_PROCESSOR_TRACKER_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/processor/processor_base.h"
#include "core/capture/capture_motion.h"
namespace wolf {
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTracker);
struct ParamsProcessorTracker : public ParamsProcessorBase
namespace wolf
{
unsigned int min_features_for_keyframe; ///< minimum nbr. of features to vote for keyframe
int max_new_features; ///< maximum nbr. of new features to be processed when adding a keyframe (-1: unlimited. 0: none.)
ParamsProcessorTracker() = default;
ParamsProcessorTracker(std::string _unique_name, const wolf::ParamsServer & _server):
ParamsProcessorBase(_unique_name, _server)
{
min_features_for_keyframe = _server.getParam<unsigned int>(prefix + _unique_name + "/keyframe_vote/min_features_for_keyframe");
max_new_features = _server.getParam<int>(prefix + _unique_name + "/max_new_features");
}
std::string print() const override
{
return ParamsProcessorBase::print() + "\n"
+ "min_features_for_keyframe: " + std::to_string(min_features_for_keyframe) + "\n"
+ "max_new_features: " + std::to_string(max_new_features) + "\n";
}
};
WOLF_PTR_TYPEDEFS(ProcessorTracker);
/** \brief General tracker processor
......@@ -106,197 +72,217 @@ WOLF_PTR_TYPEDEFS(ProcessorTracker);
*/
class ProcessorTracker : public ProcessorBase
{
public:
typedef enum {
FIRST_TIME_WITH_KEYFRAME,
FIRST_TIME_WITHOUT_KEYFRAME,
SECOND_TIME_WITH_KEYFRAME,
SECOND_TIME_WITHOUT_KEYFRAME,
RUNNING_WITH_KEYFRAME,
RUNNING_WITHOUT_KEYFRAME
} ProcessingStep ;
protected:
ParamsProcessorTrackerPtr params_tracker_; ///< parameters for the tracker
ProcessingStep processing_step_; ///< State machine controlling the processing step
CaptureBasePtr origin_ptr_; ///< Pointer to the origin of the tracker.
CaptureBasePtr last_ptr_; ///< Pointer to the last tracked capture.
CaptureBasePtr incoming_ptr_; ///< Pointer to the incoming capture being processed.
FrameBasePtr last_frame_ptr_;
FeatureBasePtrList new_features_last_; ///< List of new features in \b last for landmark initialization and new key-frame creation.
FeatureBasePtrList new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming
FeatureBasePtrList known_features_last_; ///< list of the known features in previous captures successfully tracked in \b last
FeatureBasePtrList known_features_incoming_; ///< list of the known features in \b last successfully tracked in \b incoming
StateStructure state_structure_; ///< Structure of frames created or used by this processor
public:
ProcessorTracker(const std::string& _type,
const StateStructure& _structure,
int _dim,
ParamsProcessorTrackerPtr _params_tracker);
~ProcessorTracker() override;
const StateStructure& getStateStructure() const;
virtual CaptureBaseConstPtr getOrigin() const;
virtual CaptureBasePtr getOrigin();
virtual CaptureBaseConstPtr getLast() const;
virtual CaptureBasePtr getLast();
virtual FrameBaseConstPtr getLastFrame() const;
virtual FrameBasePtr getLastFrame();
virtual CaptureBaseConstPtr getIncoming() const;
virtual CaptureBasePtr getIncoming();
protected:
/** \brief process an incoming capture
*
* Each derived processor should implement this function. It will be called if:
* - A new capture arrived and triggerInCapture() returned true.
*/
void processCapture(CaptureBasePtr) override;
/** \brief process an incoming key-frame
*
* The ProcessorTracker only processes incoming captures (it is not called).
*/
void processKeyFrame(FrameBasePtr _keyframe_ptr) override {}
/** \brief trigger in capture
*
* Returns true if processCapture() should be called after the provided capture arrived.
*/
bool triggerInCapture(CaptureBasePtr) const override { return true;}
/** \brief trigger in key-frame
*
* The ProcessorTracker only processes incoming captures, then it returns false.
*/
bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override {return false;}
/** \brief store key frame
*
* Returns true if the key frame should be stored
*/
bool storeKeyFrame(FrameBasePtr) override { return true;}
/** \brief store capture
*
* Returns true if the capture should be stored
*/
bool storeCapture(CaptureBasePtr) override { return false;}
/** Pre-process incoming Capture
*
* This is called by process() just after assigning incoming_ptr_ to a valid Capture.
*
* Overload this function to prepare stuff on derived classes.
*
* Typical uses of prePrecess() are:
* - casting base types to derived types
* - initializing counters, flags, or any derived variables
* - initializing algorithms needed for processing the derived data
*/
virtual void preProcess() { };
/** Post-process
*
* This is called by process() after finishing the processing algorithm.
*
* Overload this function to post-process stuff on derived classes.
*
* Typical uses of postPrecess() are:
* - resetting and/or clearing variables and/or algorithms at the end of processing
* - drawing / printing / logging the results of the processing
*/
virtual void postProcess() { };
/** \brief Tracker function
* \return The number of successful tracks.
*
* This is the tracker function to be implemented in derived classes.
* It operates on the \b incoming capture pointed by incoming_ptr_.
*
* This should do one of the following, depending on the design of the tracker -- see use_landmarks_:
* - Track Features against other Features in the \b origin Capture. Tips:
* - An intermediary step of matching against Features in the \b last Capture makes tracking easier.
* - Once tracked against last, then the link to Features in \b origin is provided by the Features' Factors in \b last.
* - If required, correct the drift by re-comparing against the Features in origin.
* - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
* - Track Features against Landmarks in the Map. Tips:
* - The links to the Landmarks are in the Features' Factors in last.
* - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
*
* The function must generate the necessary Features in the \b incoming Capture,
* of the correct type, derived from FeatureBase.
*
* It must also generate the factors, of the correct type, derived from FactorBase
* (through FactorAnalytic or FactorSparse).
*/
virtual unsigned int processKnown() = 0;
/** \brief Vote for KeyFrame generation
*
* If a KeyFrame criterion is validated, this function returns true,
* meaning that it wants to create a KeyFrame at the \b last Capture.
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
bool voteForKeyFrame() const override = 0;
/** \brief Advance the incoming Capture to become the last.
*
* Call this when the tracking and keyframe policy work is done and
* we need to get ready to accept a new incoming Capture.
*/
virtual void advanceDerived() = 0;
/**\brief Process new Features or Landmarks
*
*/
virtual unsigned int processNew(const int& _max_features) = 0;
/**\brief Creates and adds factors from last_ to origin_
*
*/
virtual void establishFactors() = 0;
/** \brief Reset the tracker using the \b last Capture as the new \b origin.
*/
virtual void resetDerived() = 0;
public:
FeatureBaseConstPtrList getNewFeaturesListLast() const;
FeatureBasePtrList getNewFeaturesListLast();
std::string print() const {
return this->params_tracker_->print();
}
void printHeader(int depth, //
bool constr_by, //
bool metric, //
bool state_blocks,
std::ostream& stream ,
std::string _tabs = "") const override;
protected:
void computeProcessingStep();
void addNewFeatureLast(FeatureBasePtr _feature_ptr);
FeatureBasePtrList& getNewFeaturesListIncoming();
void addNewFeatureIncoming(FeatureBasePtr _feature_ptr);
public:
typedef enum
{
FIRST_TIME_WITH_KEYFRAME,
FIRST_TIME_WITHOUT_KEYFRAME,
SECOND_TIME_WITH_KEYFRAME,
SECOND_TIME_WITHOUT_KEYFRAME,
RUNNING_WITH_KEYFRAME,
RUNNING_WITHOUT_KEYFRAME
} ProcessingStep;
protected:
ProcessingStep processing_step_; ///< State machine controlling the processing step
CaptureBasePtr origin_ptr_; ///< Pointer to the origin of the tracker.
CaptureBasePtr last_ptr_; ///< Pointer to the last tracked capture.
CaptureBasePtr incoming_ptr_; ///< Pointer to the incoming capture being processed.
FeatureBasePtrList new_features_last_; ///< List of new features in \b last for landmark initialization and new
///< key-frame creation.
FeatureBasePtrList
new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming
FeatureBasePtrList
known_features_last_; ///< list of the known features in previous captures successfully tracked in \b last
FeatureBasePtrList
known_features_incoming_; ///< list of the known features in \b last successfully tracked in \b incoming
// PARAMS
unsigned int min_features_for_keyframe_; ///< minimum nbr. of features to vote for keyframe
int max_new_features_; ///< maximum nbr. of new features to be processed when adding a keyframe (-1: unlimited. 0:
///< none.)
// profiling
ProfilingUnit preprocess_profiling_, process_known_profiling_, process_new_profiling_,
establish_factors_profiling_, postprocess_profiling_;
public:
ProcessorTracker(const std::string& _type, const TypeComposite& state_types_, const YAML::Node& _params);
~ProcessorTracker() override;
virtual CaptureBaseConstPtr getOrigin() const;
virtual CaptureBasePtr getOrigin();
virtual CaptureBaseConstPtr getLast() const;
virtual CaptureBasePtr getLast();
virtual CaptureBaseConstPtr getIncoming() const;
virtual CaptureBasePtr getIncoming();
void printProfiling(std::ostream& stream = std::cout) const override;
protected:
/** \brief process an incoming capture
*
* Each derived processor should implement this function. It will be called if:
* - A new capture arrived and triggerInCapture() returned true.
*/
void processCapture(CaptureBasePtr) override;
/** \brief process an incoming key-frame
*
* The ProcessorTracker only processes incoming captures (it is not called).
*/
void processKeyFrame(FrameBasePtr _keyframe_ptr) override {}
/** \brief trigger in capture
*
* Returns true if processCapture() should be called after the provided capture arrived.
*/
bool triggerInCapture(CaptureBasePtr) const override
{
return true;
}
/** \brief trigger in key-frame
*
* The ProcessorTracker only processes incoming captures, then it returns false.
*/
bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override
{
return false;
}
/** \brief store key frame
*
* Returns true if the key frame should be stored
*/
bool storeKeyFrame(FrameBasePtr) override
{
return true;
}
/** \brief store capture
*
* Returns true if the capture should be stored
*/
bool storeCapture(CaptureBasePtr) override
{
return false;
}
/** Pre-process incoming Capture
*
* This is called by process() just after assigning incoming_ptr_ to a valid Capture.
*
* Overload this function to prepare stuff on derived classes.
*
* Typical uses of prePrecess() are:
* - casting base types to derived types
* - initializing counters, flags, or any derived variables
* - initializing algorithms needed for processing the derived data
*/
virtual void preProcess(){};
/** Post-process
*
* This is called by process() after finishing the processing algorithm.
*
* Overload this function to post-process stuff on derived classes.
*
* Typical uses of postPrecess() are:
* - resetting and/or clearing variables and/or algorithms at the end of processing
* - drawing / printing / logging the results of the processing
*/
virtual void postProcess(){};
/** \brief Tracker function
* \return The number of successful tracks.
*
* This is the tracker function to be implemented in derived classes.
* It operates on the \b incoming capture pointed by incoming_ptr_.
*
* This should do one of the following, depending on the design of the tracker -- see use_landmarks_:
* - Track Features against other Features in the \b origin Capture. Tips:
* - An intermediary step of matching against Features in the \b last Capture makes tracking easier.
* - Once tracked against last, then the link to Features in \b origin is provided by the Features' Factors in
* \b last.
* - If required, correct the drift by re-comparing against the Features in origin.
* - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
* - Track Features against Landmarks in the Map. Tips:
* - The links to the Landmarks are in the Features' Factors in last.
* - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
*
* The function must generate the necessary Features in the \b incoming Capture,
* of the correct type, derived from FeatureBase.
*
* It must also generate the factors, of the correct type, derived from FactorBase
* (through FactorAnalytic or FactorSparse).
*/
virtual unsigned int processKnown() = 0;
/** \brief Vote for KeyFrame generation
*
* If a KeyFrame criterion is validated, this function returns true,
* meaning that it wants to create a KeyFrame at the \b last Capture.
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
bool voteForKeyFrame() const override = 0;
/** \brief Advance the incoming Capture to become the last.
*
* Call this when the tracking and keyframe policy work is done and
* we need to get ready to accept a new incoming Capture.
*/
virtual void advanceDerived() = 0;
/**\brief Process new Features or Landmarks
*
*/
virtual unsigned int processNew(const int& _max_features) = 0;
/**\brief Creates and adds factors from last_ to origin_
*
*/
virtual void establishFactors() = 0;
/** \brief Reset the tracker using the \b last Capture as the new \b origin.
*/
virtual void resetDerived() = 0;
public:
FeatureBaseConstPtrList getNewFeaturesListLast() const;
FeatureBasePtrList getNewFeaturesListLast();
void printHeader(int depth, //
bool factored_by, //
bool metric, //
bool state_blocks,
std::ostream& stream,
std::string _tabs = "") const override;
protected:
void computeProcessingStep();
void addNewFeatureLast(FeatureBasePtr _feature_ptr);
FeatureBasePtrList& getNewFeaturesListIncoming();
void addNewFeatureIncoming(FeatureBasePtr _feature_ptr);
private:
// call to derivate methods with profiling
void preProcessProfiling();
void postProcessProfiling();
unsigned int processKnownProfiling();
unsigned int processNewProfiling(const int& _max_features);
void establishFactorsProfiling();
};
inline FeatureBaseConstPtrList ProcessorTracker::getNewFeaturesListLast() const
{
FeatureBaseConstPtrList list_const;
for (auto&& obj_ptr : new_features_last_)
list_const.push_back(obj_ptr);
for (auto&& obj_ptr : new_features_last_) list_const.push_back(obj_ptr);
return list_const;
}
......@@ -315,11 +301,6 @@ inline FeatureBasePtrList& ProcessorTracker::getNewFeaturesListIncoming()
return new_features_incoming_;
}
inline const StateStructure& ProcessorTracker::getStateStructure ( ) const
{
return state_structure_;
}
inline void ProcessorTracker::addNewFeatureIncoming(FeatureBasePtr _feature_ptr)
{
new_features_incoming_.push_back(_feature_ptr);
......@@ -345,26 +326,53 @@ inline CaptureBasePtr ProcessorTracker::getLast()
return last_ptr_;
}
inline FrameBaseConstPtr ProcessorTracker::getLastFrame() const
inline CaptureBaseConstPtr ProcessorTracker::getIncoming() const
{
return last_frame_ptr_;
return incoming_ptr_;
}
inline FrameBasePtr ProcessorTracker::getLastFrame()
inline CaptureBasePtr ProcessorTracker::getIncoming()
{
return last_frame_ptr_;
return incoming_ptr_;
}
inline CaptureBaseConstPtr ProcessorTracker::getIncoming() const
inline void ProcessorTracker::preProcessProfiling()
{
return incoming_ptr_;
preprocess_profiling_.startProfiling();
preProcess();
preprocess_profiling_.stopProfiling();
}
inline CaptureBasePtr ProcessorTracker::getIncoming()
inline void ProcessorTracker::postProcessProfiling()
{
return incoming_ptr_;
postprocess_profiling_.startProfiling();
postProcess();
postprocess_profiling_.stopProfiling();
}
inline unsigned int ProcessorTracker::processKnownProfiling()
{
process_known_profiling_.startProfiling();
auto result = processKnown();
process_known_profiling_.stopProfiling();
return result;
}
} // namespace wolf
inline unsigned int ProcessorTracker::processNewProfiling(const int& _max_features)
{
process_new_profiling_.startProfiling();
auto result = processNew(_max_features);
process_new_profiling_.stopProfiling();
return result;
}
inline void ProcessorTracker::establishFactorsProfiling()
{
establish_factors_profiling_.startProfiling();
establishFactors();
establish_factors_profiling_.stopProfiling();
}
#endif /* SRC_PROCESSOR_TRACKER_H_ */
} // namespace wolf
include/core/processor/processor_tracker_feature.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/*
* \processor_tracker_feature.h
*
* Created on: 27/02/2016
* \author: jsola
*/
#ifndef PROCESSOR_TRACKER_FEATURE_H_
#define PROCESSOR_TRACKER_FEATURE_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
//wolf includes
// wolf includes
#include "core/processor/processor_tracker.h"
#include "core/capture/capture_base.h"
#include "core/feature/feature_match.h"
......@@ -38,14 +27,6 @@
namespace wolf
{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerFeature);
struct ParamsProcessorTrackerFeature : public ParamsProcessorTracker
{
using ParamsProcessorTracker::ParamsProcessorTracker;
};
WOLF_PTR_TYPEDEFS(ProcessorTrackerFeature);
/** \brief Feature tracker processor
......@@ -68,7 +49,8 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerFeature);
*
* This tracker builds on top of the ProcessorTracker by implementing some of its pure virtual functions.
* As a reminder, we sketch here the pipeline of the parent ProcessorTracker process() function.
* We highlight the functions implemented here with a sign '<--- IMPLEMENTED', and the ones to be implemented by derived classes with '<=== IMPLEMENT'
* We highlight the functions implemented here with a sign '<--- IMPLEMENTED', and the ones to be implemented by
* derived classes with '<=== IMPLEMENT'
*
* - On each incoming Capture,
* - Track known features in the \b incoming Capture: processKnown() <--- IMPLEMENTED
......@@ -101,120 +83,115 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerFeature);
*/
class ProcessorTrackerFeature : public ProcessorTracker
{
public:
/** \brief Constructor with type
*/
ProcessorTrackerFeature(const std::string& _type,
const StateStructure& _structure,
int _dim,
ParamsProcessorTrackerFeaturePtr _params_tracker_feature);
~ProcessorTrackerFeature() override;
protected:
ParamsProcessorTrackerFeaturePtr params_tracker_feature_;
TrackMatrix track_matrix_;
FeatureMatchMap matches_last_from_incoming_;
/** \brief Process known Features
* \return The number of successful matches.
*
* This function operates on the \b incoming capture pointed by incoming_ptr_.
*
* This function does:
* - Track Features against other Features in the \b origin Capture. Tips:
* - An intermediary step of matching against Features in the \b last Capture makes tracking easier.
* - Once tracked against last, then the link to Features in \b origin is provided by the Features' Factors in \b last.
* - If required, correct the drift by re-comparing against the Features in origin.
* - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
* - Create the necessary Features in the \b incoming Capture,
* of the correct type, derived from FeatureBase.
* - Create the factors, of the correct type, derived from FactorBase
* (through FactorAnalytic or FactorSparse).
*/
unsigned int processKnown() override;
/** \brief Track provided features in \b _capture
* \param _features_in input list of features in \b last to track
* \param _capture the capture in which the _features_in should be searched
* \param _features_out returned list of features found in \b _capture
* \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
*
* IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
* Then, they will be already linked to the _capture.
* If you detect all the features at once in preprocess(), you should create them (`make_shared()`) and only link the
* features that are returned in _features_out (`FeatureBase::link(_capture)`).
*
* \return the number of features tracked
*/
virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_in,
public:
/** \brief Constructor with type
*/
ProcessorTrackerFeature(const std::string& _type, const TypeComposite& state_types_, const YAML::Node& _params);
~ProcessorTrackerFeature() override;
protected:
TrackMatrix track_matrix_;
FeatureMatchMap matches_last_from_incoming_;
/** \brief Process known Features
* \return The number of successful matches.
*
* This function operates on the \b incoming capture pointed by incoming_ptr_.
*
* This function does:
* - Track Features against other Features in the \b origin Capture. Tips:
* - An intermediary step of matching against Features in the \b last Capture makes tracking easier.
* - Once tracked against last, then the link to Features in \b origin is provided by the Features' Factors in
* \b last.
* - If required, correct the drift by re-comparing against the Features in origin.
* - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
* - Create the necessary Features in the \b incoming Capture,
* of the correct type, derived from FeatureBase.
* - Create the factors, of the correct type, derived from FactorBase
* (through FactorAnalytic or FactorSparse).
*/
unsigned int processKnown() override;
/** \brief Track provided features in \b _capture
* \param _features_in input list of features in \b last to track
* \param _capture the capture in which the _features_in should be searched
* \param _features_out returned list of features found in \b _capture
* \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
*
* IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace`
* instead. Then, they will be already linked to the _capture. If you detect all the features at once in
* preprocess(), you should create them (`make_shared()`) and only link the features that are returned in
* _features_out (`FeatureBase::link(_capture)`).
*
* \return the number of features tracked
*/
virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_in,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
FeatureMatchMap& _feature_correspondences) = 0;
/** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
* \param _origin_feature input feature in origin capture tracked
* \param _incoming_feature input/output feature in incoming capture to be corrected
* \return false if the the process discards the correspondence with origin's feature
*/
virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
const FeatureBasePtr _last_feature,
FeatureBasePtr _incoming_feature) = 0;
/** \brief Vote for KeyFrame generation
*
* If a KeyFrame criterion is validated, this function returns true,
* meaning that it wants to create a KeyFrame at the \b last Capture.
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
bool voteForKeyFrame() const override = 0;
// We overload the advance and reset functions to update the lists of matches
void advanceDerived() override;
void resetDerived() override;
/**\brief Process new Features
*
*/
unsigned int processNew(const int& _max_features) override;
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
* \param _capture The capture in which the new features should be detected.
* \param _features_out The list of detected Features in _capture.
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
*
* IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace`
* instead. Then, they will be already linked to the _capture. If you detect all the features at once in
* preprocess(), you should create them (`make_shared()`) and only link the features that are returned in
* _features_out (`FeatureBase::link(_capture)`).
*
* The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
* the list of newly detected features of the capture last_ptr_.
*/
virtual unsigned int detectNewFeatures(const int& _max_new_features,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
FeatureMatchMap& _feature_correspondences) = 0;
/** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
* \param _origin_feature input feature in origin capture tracked
* \param _incoming_feature input/output feature in incoming capture to be corrected
* \return false if the the process discards the correspondence with origin's feature
*/
virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
const FeatureBasePtr _last_feature,
FeatureBasePtr _incoming_feature) = 0;
/** \brief Vote for KeyFrame generation
*
* If a KeyFrame criterion is validated, this function returns true,
* meaning that it wants to create a KeyFrame at the \b last Capture.
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
bool voteForKeyFrame() const override = 0;
// We overload the advance and reset functions to update the lists of matches
void advanceDerived() override;
void resetDerived() override;
/**\brief Process new Features
*
*/
unsigned int processNew(const int& _max_features) override;
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
* \param _capture The capture in which the new features should be detected.
* \param _features_out The list of detected Features in _capture.
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
*
* IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
* Then, they will be already linked to the _capture.
* If you detect all the features at once in preprocess(), you should create them (`make_shared()`) and only link the
* features that are returned in _features_out (`FeatureBase::link(_capture)`).
*
* The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
* the list of newly detected features of the capture last_ptr_.
*/
virtual unsigned int detectNewFeatures(const int& _max_new_features,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out) = 0;
/** \brief Emplaces a new factor
* \param _feature_ptr pointer to the parent Feature
* \param _feature_other_ptr pointer to the other feature constrained.
*
* Implement this method in derived classes.
*
* This function emplaces a factor of the appropriate type for the derived processor.
*/
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) = 0;
/** \brief Emplaces a new factor for each correspondence between a feature in Capture \b last and a feature in Capture \b origin
*/
void establishFactors() override;
FeatureBasePtrList& _features_out) = 0;
/** \brief Emplaces a new factor
* \param _feature_ptr pointer to the parent Feature
* \param _feature_other_ptr pointer to the other feature constrained.
*
* Implement this method in derived classes.
*
* This function emplaces a factor of the appropriate type for the derived processor.
*/
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) = 0;
/** \brief Emplaces a new factor for each correspondence between a feature in Capture \b last and a feature in
* Capture \b origin
*/
void establishFactors() override;
};
} // namespace wolf
#endif /* PROCESSOR_TRACKER_FEATURE_H_ */
} // namespace wolf
include/core/processor/processor_tracker_landmark.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/**
* \file processor_tracker_landmark.h
*
* Created on: Apr 7, 2016
* \author: jvallve
*/
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#ifndef PROCESSOR_TRACKER_LANDMARK_H_
#define PROCESSOR_TRACKER_LANDMARK_H_
//wolf includes
// wolf includes
#include "core/processor/processor_tracker.h"
#include "core/capture/capture_base.h"
#include "core/landmark/landmark_match.h"
namespace wolf
{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerLandmark);
struct ParamsProcessorTrackerLandmark : public ParamsProcessorTracker
{
using ParamsProcessorTracker::ParamsProcessorTracker;
//
};
WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
/** \brief Landmark tracker processor
*
* This is an abstract class.
......@@ -67,7 +47,8 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
*
* This tracker builds on top of the ProcessorTracker by implementing some of its pure virtual functions.
* As a reminder, we sketch here the pipeline of the parent ProcessorTracker process() function.
* We highlight the functions implemented here with a sign '<--- IMPLEMENTED', and the ones to be implemented by derived classes with '<=== IMPLEMENT'
* We highlight the functions implemented here with a sign '<--- IMPLEMENTED', and the ones to be implemented by
* derived classes with '<=== IMPLEMENT'
*
* - On each incoming Capture,
* - processKnown() : Track known features in the \b incoming Capture <--- IMPLEMENTED
......@@ -100,114 +81,107 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
*/
class ProcessorTrackerLandmark : public ProcessorTracker
{
public:
ProcessorTrackerLandmark(const std::string& _type,
const StateStructure& _structure,
ParamsProcessorTrackerLandmarkPtr _params_tracker_landmark);
~ProcessorTrackerLandmark() override;
protected:
ParamsProcessorTrackerLandmarkPtr params_tracker_landmark_;
LandmarkBasePtrList new_landmarks_; ///< List of new detected landmarks
LandmarkMatchMap matches_landmark_from_incoming_;
LandmarkMatchMap matches_landmark_from_last_;
/** \brief Tracker function
* \return The number of successful tracks.
*
* Find Features in \b incoming Capture corresponding to known landmarks in the \b Map.
*
* This is the tracker function to be implemented in derived classes.
* It operates on the \b incoming capture pointed by incoming_ptr_.
*
* The function must populate the list of Features in the \b incoming Capture.
* Features need to be of the correct type, derived from FeatureBase.
*/
unsigned int processKnown() override;
/** \brief Find provided landmarks as features in the provided capture
* \param _landmarks_in input list of landmarks to be found
* \param _capture the capture in which the _landmarks_in should be searched
* \param _features_out returned list of features found in \b _capture corresponding to a landmark of _landmarks_in
* \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
*
* IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
* Then, they will be already linked to the _capture.
* If you detect all the features at once in preprocess(), you should create them (`make_shared()`) and only link the
* features that are returned in _features_out (`FeatureBase::link(_capture)`).
*
* \return the number of landmarks found
*/
virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
public:
ProcessorTrackerLandmark(const std::string& _type,
const TypeComposite& _state_types,
const YAML::Node& _params_tracker_landmark);
~ProcessorTrackerLandmark() override;
protected:
LandmarkBasePtrList new_landmarks_; ///< List of new detected landmarks
LandmarkMatchMap matches_landmark_from_incoming_;
LandmarkMatchMap matches_landmark_from_last_;
/** \brief Tracker function
* \return The number of successful tracks.
*
* Find Features in \b incoming Capture corresponding to known landmarks in the \b Map.
*
* This is the tracker function to be implemented in derived classes.
* It operates on the \b incoming capture pointed by incoming_ptr_.
*
* The function must populate the list of Features in the \b incoming Capture.
* Features need to be of the correct type, derived from FeatureBase.
*/
unsigned int processKnown() override;
/** \brief Find provided landmarks as features in the provided capture
* \param _landmarks_in input list of landmarks to be found
* \param _capture the capture in which the _landmarks_in should be searched
* \param _features_out returned list of features found in \b _capture corresponding to a landmark of _landmarks_in
* \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
*
* IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace`
* instead. Then, they will be already linked to the _capture. If you detect all the features at once in
* preprocess(), you should create them (`make_shared()`) and only link the features that are returned in
* _features_out (`FeatureBase::link(_capture)`).
*
* \return the number of landmarks found
*/
virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences) = 0;
/** \brief Vote for KeyFrame generation
*
* If a KeyFrame criterion is validated, this function returns true,
* meaning that it wants to create a KeyFrame at the \b last Capture.
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
bool voteForKeyFrame() const override = 0;
// We overload the advance and reset functions to update the lists of matches
void advanceDerived() override;
void resetDerived() override;
/** \brief Process new Features
*
*/
unsigned int processNew(const int& _max_features) override;
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
* \param _capture The capture in which the new features should be detected.
* \param _features_out The list of detected Features in _capture.
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
*
* IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace`
* instead. Then, they will be already linked to the _capture. If you detect all the features at once in
* preprocess(), you should create them (`make_shared()`) and only link the features that are returned in
* _features_out (`FeatureBase::link(_capture)`).
*
* The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
* the list of newly detected features of the capture last_ptr_.
*/
virtual unsigned int detectNewFeatures(const int& _max_new_features,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences) = 0;
/** \brief Vote for KeyFrame generation
*
* If a KeyFrame criterion is validated, this function returns true,
* meaning that it wants to create a KeyFrame at the \b last Capture.
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
bool voteForKeyFrame() const override = 0;
// We overload the advance and reset functions to update the lists of matches
void advanceDerived() override;
void resetDerived() override;
/** \brief Process new Features
*
*/
unsigned int processNew(const int& _max_features) override;
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
* \param _capture The capture in which the new features should be detected.
* \param _features_out The list of detected Features in _capture.
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
*
* IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
* Then, they will be already linked to the _capture.
* If you detect all the features at once in preprocess(), you should create them (`make_shared()`) and only link the
* features that are returned in _features_out (`FeatureBase::link(_capture)`).
*
* The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
* the list of newly detected features of the capture last_ptr_.
*/
virtual unsigned int detectNewFeatures(const int& _max_new_features,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out) = 0;
/** \brief Emplaces a landmark for each new feature of new_features_last_
**/
virtual void emplaceNewLandmarks();
/** \brief Emplaces one landmark
*
* Implement in derived classes to build the type of landmark you need for this tracker.
*/
virtual LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr) = 0;
/** \brief Emplaces a new factor
* \param _feature_ptr pointer to the Feature to constrain
* \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
*
* Implement this method in derived classes.
*/
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr) = 0;
/** \brief Emplaces a new factor for each correspondence between a feature in Capture \b last and a landmark
*/
void establishFactors() override;
FeatureBasePtrList& _features_out) = 0;
/** \brief Emplaces a landmark for each new feature of new_features_last_
**/
virtual void emplaceNewLandmarks();
/** \brief Emplaces one landmark
*
* Implement in derived classes to build the type of landmark you need for this tracker.
*/
virtual LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr) = 0;
/** \brief Emplaces a new factor
* \param _feature_ptr pointer to the Feature to constrain
* \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
*
* Implement this method in derived classes.
*/
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr) = 0;
/** \brief Emplaces a new factor for each correspondence between a feature in Capture \b last and a landmark
*/
void establishFactors() override;
};
}// namespace wolf
// IMPLEMENTATION
#include "core/landmark/landmark_base.h"
#endif /* PROCESSOR_TRACKER_LANDMARK_H_ */
} // namespace wolf
include/core/processor/track_matrix.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/**
* \file track_matrix.h
*
* Created on: Mar 24, 2018
* \author: jsola
*/
#ifndef TRACK_MATRIX_H_
#define TRACK_MATRIX_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/feature/feature_base.h"
#include "core/capture/capture_base.h"
......@@ -39,18 +28,19 @@
namespace wolf
{
using std::map;
using std::vector;
using std::list;
using std::map;
using std::pair;
using std::shared_ptr;
using std::vector;
typedef map<TimeStamp, FeatureBasePtr> Track;
typedef map<TimeStamp, FeatureBaseConstPtr> TrackConst;
typedef map<SizeStd, FeatureBasePtr > Snapshot;
typedef map<SizeStd, FeatureBaseConstPtr > SnapshotConst;
typedef map<SizeStd, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // matched feature pairs indexed by track_id
typedef map<SizeStd, pair<FeatureBaseConstPtr, FeatureBaseConstPtr> > TrackMatchesConst; // matched feature pairs indexed by track_id
typedef map<TimeStamp, FeatureBasePtr> Track;
typedef map<TimeStamp, FeatureBaseConstPtr> TrackConst;
typedef map<SizeStd, FeatureBasePtr> Snapshot;
typedef map<SizeStd, FeatureBaseConstPtr> SnapshotConst;
typedef map<SizeStd, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // matched feature pairs indexed by track_id
typedef map<SizeStd, pair<FeatureBaseConstPtr, FeatureBaseConstPtr> >
TrackMatchesConst; // matched feature pairs indexed by track_id
/** \brief Matrix of tracked features, by track and by snapshot (Captures or time stamps)
* This class implements the following data structure:
......@@ -80,7 +70,8 @@ typedef map<SizeStd, pair<FeatureBaseConstPtr, FeatureBaseConstPtr> > TrackMatc
*
* The class makes sure that both accesses are consistent each time some addition or removal of features is performed.
*
* The storage is implemented as two maps of maps, so each addition and removal of single features is accomplished in logarithmic time:
* The storage is implemented as two maps of maps, so each addition and removal of single features is accomplished in
* logarithmic time:
*
* Tracks are identified with the track ID in f->trackId()
* Snapshots are identified with the Capture pointer in f->getCapture()
......@@ -98,56 +89,59 @@ typedef map<SizeStd, pair<FeatureBaseConstPtr, FeatureBaseConstPtr> > TrackMatc
class TrackMatrix
{
public:
TrackMatrix();
virtual ~TrackMatrix();
void newTrack (FeatureBasePtr _ftr);
void add (const SizeStd& _track_id, const FeatureBasePtr& _ftr);
void add (const FeatureBasePtr& _ftr_existing, const FeatureBasePtr& _ftr_new);
void remove (FeatureBasePtr _ftr);
void remove (const SizeStd& _track_id);
void remove (CaptureBasePtr _cap);
SizeStd numTracks () const;
SizeStd trackSize (const SizeStd& _track_id) const;
TrackConst track (const SizeStd& _track_id) const;
Track track (const SizeStd& _track_id);
SnapshotConst snapshot (CaptureBaseConstPtr _capture) const;
Snapshot snapshot (CaptureBasePtr _capture);
vector<FeatureBaseConstPtr> trackAsVector(const SizeStd& _track_id) const;
vector<FeatureBasePtr> trackAsVector(const SizeStd& _track_id);
FeatureBaseConstPtrList snapshotAsList(CaptureBaseConstPtr _cap) const;
FeatureBasePtrList snapshotAsList(CaptureBasePtr _cap);
TrackMatchesConst matches (CaptureBaseConstPtr _cap_1, CaptureBaseConstPtr _cap_2) const;
TrackMatches matches (CaptureBasePtr _cap_1, CaptureBasePtr _cap_2);
FeatureBaseConstPtr firstFeature(const SizeStd& _track_id) const;
FeatureBasePtr firstFeature(const SizeStd& _track_id);
FeatureBaseConstPtr lastFeature (const SizeStd& _track_id) const;
FeatureBasePtr lastFeature (const SizeStd& _track_id);
FeatureBaseConstPtr feature (const SizeStd& _track_id, CaptureBaseConstPtr _cap) const;
FeatureBasePtr feature (const SizeStd& _track_id, CaptureBasePtr _cap);
CaptureBaseConstPtr firstCapture(const SizeStd& _track_id) const;
CaptureBasePtr firstCapture(const SizeStd& _track_id);
list<SizeStd> trackIds(CaptureBaseConstPtr _capture = nullptr) const;
// tracks across captures that belong to keyframe
TrackConst trackAtKeyframes(const SizeStd& _track_id) const;
Track trackAtKeyframes(const SizeStd& _track_id);
private:
static SizeStd track_id_count_;
// tracks across all Captures
map<SizeStd, Track > tracks_; // map indexed by track_Id of ( maps indexed by TimeStamp of ( features ) )
// Across track: maps of Feature pointers indexed by track_Id.
map<CaptureBasePtr, Snapshot > snapshots_; // map indexed by capture_ptr of ( maps indexed by track_Id of ( features ) )
public:
TrackMatrix();
virtual ~TrackMatrix();
void newTrack(FeatureBasePtr _ftr);
void add(const SizeStd& _track_id, const FeatureBasePtr& _ftr);
void add(const FeatureBasePtr& _ftr_existing, const FeatureBasePtr& _ftr_new);
void remove(FeatureBasePtr _ftr);
void remove(const SizeStd& _track_id);
void remove(CaptureBasePtr _cap);
SizeStd numTracks() const;
SizeStd trackSize(const SizeStd& _track_id) const;
TrackConst track(const SizeStd& _track_id) const;
Track track(const SizeStd& _track_id);
SnapshotConst snapshot(CaptureBaseConstPtr _capture) const;
Snapshot snapshot(CaptureBasePtr _capture);
vector<FeatureBaseConstPtr> trackAsVector(const SizeStd& _track_id) const;
vector<FeatureBasePtr> trackAsVector(const SizeStd& _track_id);
FeatureBaseConstPtrList snapshotAsList(CaptureBaseConstPtr _cap) const;
FeatureBasePtrList snapshotAsList(CaptureBasePtr _cap);
TrackMatchesConst matches(CaptureBaseConstPtr _cap_1, CaptureBaseConstPtr _cap_2) const;
TrackMatches matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2);
FeatureBaseConstPtr firstFeature(const SizeStd& _track_id) const;
FeatureBasePtr firstFeature(const SizeStd& _track_id);
FeatureBaseConstPtr lastFeature(const SizeStd& _track_id) const;
FeatureBasePtr lastFeature(const SizeStd& _track_id);
FeatureBaseConstPtr feature(const SizeStd& _track_id, CaptureBaseConstPtr _cap) const;
FeatureBasePtr feature(const SizeStd& _track_id, CaptureBasePtr _cap);
CaptureBaseConstPtr firstCapture(const SizeStd& _track_id) const;
CaptureBasePtr firstCapture(const SizeStd& _track_id);
CaptureBaseConstPtr lastCapture(const SizeStd& _track_id) const;
CaptureBasePtr lastCapture(const SizeStd& _track_id);
list<SizeStd> trackIds(CaptureBaseConstPtr _capture = nullptr) const;
// tracks across captures that belong to keyframe
TrackConst trackAtKeyframes(const SizeStd& _track_id) const;
Track trackAtKeyframes(const SizeStd& _track_id);
// Remove snapshots of captures that are being removed from the problem (e.g. when a keyframe is removed)
void cleanSnapshots();
private:
static SizeStd track_id_count_;
// tracks across all Captures
map<SizeStd, Track> tracks_; // map indexed by track_Id of ( maps indexed by TimeStamp of ( features ) )
// Across track: maps of Feature pointers indexed by track_Id.
map<CaptureBasePtr, Snapshot>
snapshots_; // map indexed by capture_ptr of ( maps indexed by track_Id of ( features ) )
};
} /* namespace wolf */
#endif /* TRACK_MATRIX_H_ */
include/core/sensor/factory_sensor.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/**
* \file factory_sensor.h
*
* Created on: Apr 25, 2016
* \author: jsola
*/
#ifndef FACTORY_SENSOR_H_
#define FACTORY_SENSOR_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
namespace wolf
{
class SensorBase;
struct ParamsSensorBase;
}
#include <unordered_map>
// wolf
#include "core/common/factory.h"
#include "core/utils/params_server.h"
// yaml
#include "yaml-cpp/yaml.h"
namespace wolf
{
/** \brief Sensor factory class
/** \brief Sensor factories classes
*
* This factory can create objects of classes deriving from SensorBase.
* These factories can create objects of classes deriving from `SensorBase`.
*
* Specific object creation is invoked by `create(TYPE, params ... )`, and the TYPE of sensor is identified with a string.
* Currently, the following sensor types are implemented,
* Specific object creation is invoked by `create(TYPE, params ... )`, and the TYPE of sensor is identified with a
* string. Currently, the following sensor types are implemented,
* - "SensorOdom2d" for SensorOdom2d
* - "SensorOdom3d" for SensorOdom3d
* - "SensorDiffDrive" for SensorDiffDrive
......@@ -56,12 +42,20 @@ namespace wolf
*
* among others.
*
* There are two factories, from a YAML file and from a YAML node with the following objectives:
*
* - The creator from YAML file validates the YAML params before creating the object.
* - The creator from YAML node only validates the YAML params `_schema_folders` vector is not empty.
*
* Validation of YAML params is performed via `yaml_schema_cpp` library, the schema file (with the name of the class)
* will be recursively searched in the folders provided in `_schema_folders` vector.
*
* Find general Factory documentation in class Factory:
* - Access the factory
* - Register/unregister creators
* - Invoke object creation
*
* This documentation shows you how to use the FactorySensor specifically:
* This documentation shows you how to use the FactorySensorNode specifically:
* - Write sensor creators.
* - Create sensors
*
......@@ -69,70 +63,80 @@ namespace wolf
* Sensor creators have the following API:
*
* \code
* static SensorBasePtr create(const std::string& _name, Eigen::VectorXd& _params_extrinsics, ParamsSensorBasePtr _params_sensor);
* static SensorBasePtr create(const YAML::Node& _yaml_node,
* std::vector<std::string>& _folders_schema);
* static SensorBasePtr create(const std::string& _yaml_filepath,
* std::vector<std::string>& _folders_schema);
* \endcode
*
* They follow the general implementation shown below:
* They follow the general implementations shown below (example for class `SensorCamera`)
*
* \code
* static SensorBasePtr create(const std::string& _unique_name, Eigen::VectorXd& _params_extrinsics, ParamsSensorBasePtr _params_sensor)
* {
* // check extrinsics vector --- example: 3D pose
* assert(_params_extrinsics.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3d.");
*
* // cast sensor parameters to good type --- example: ParamsSensorCamera
* auto intrinsics_ptr = std::static_pointer_cast<ParamsSensorCamera>(_params_sensor);
*
* // Do create the Sensor object --- example: SensorCamera
* auto sen_ptr = std::make_shared<SensorCamera>(_extrinsics_pq, intrinsics_ptr);
*
* // Complete the sensor setup with a unique name identifying the sensor
* sen_ptr->setName(_unique_name);
*
* return sen_ptr;
* }
* static SensorBasePtr create(const YAML::Node& _input_node,
* std::vector<std::string>& _folders_schema={})
* {
* // validate the _input_node if provided folders where to find the schema file
* if (not _folders_schema.empty())
* {
* auto server = yaml_schema_cpp::YamlServer(_folders_schema);
* server.setYaml(_input_node);
* if (not server.applySchema("SensorCamera"))
* {
* WOLF_ERROR(server.getLog());
* return nullptr;
* }
* }
* // Do create the Sensor object
* return std::make_shared<SensorCamera>(params);
* }
* static SensorBasePtr create(const std::string& _yaml_filepath,
* std::vector<std::string>& _folders_schema)
* {
* // parse the yaml file
* auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath);
* // Check that the yaml has all necessary inforamtion
* if (not server.applySchema("SensorCamera"))
* {
* WOLF_ERROR(server.getLog());
* return nullptr;
* }
* // Do create the Sensor object (calling the previous creator)
* return create(server.getNode(), {});
* }
* \endcode
*
*
* #### Creating sensors
* Note: Prior to invoking the creation of a sensor of a particular type,
* you must register the creator for this type into the factory.
*
* To create e.g. a SensorCamera, you type:
* To create e.g. a SensorCamera in 3D, you type:
*
* \code
* auto camera_ptr = FactorySensor::create("SensorCamera", "Front-left camera", params_extrinsics, params_camera);
* auto camera_ptr = FactorySensorNode::create("SensorCamera", yaml_node);
* \endcode
*
* where ABSOLUTELY ALL input parameters are important. In particular, the sensor name "Front-left camera" will be used to identify this camera
* and to assign it the appropriate processors. DO NOT USE IT WITH DUMMY PARAMETERS!
*
* #### See also
* - FactoryParamsSensor: to create intrinsic structs deriving from ParamsSensorBase directly from YAML files.
* - FactoryProcessor: to create processors that will be bound to sensors.
* - Problem::installSensor() : to install sensors in WOLF Problem.
*
* #### Example 1: writing a SensorCamera creator
* Here is an example of SensorCamera::create() extracted from sensor_camera.cpp:
* or:
*
* \code
* static SensorBasePtr create(const std::string& _unique_name, Eigen::VectorXd& _extrinsics_pq, ParamsSensorBasePtr _intrinsics)
* {
* // check extrinsics vector
* assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3d.");
* auto camera_ptr = FactorySensorFile::create("SensorCamera", "path_of_params_file.yaml", schema_folders_vector);
* \endcode
*
* // cast instrinsics to good type
* auto intrinsics_ptr = std::static_pointer_cast<ParamsSensorCamera>(_intrinsics);
* where ABSOLUTELY ALL input parameters are important. DO NOT USE IT WITH DUMMY PARAMETERS!
*
* // Do create the SensorCamera object, and complete its setup
* auto sen_ptr = std::make_shared<SensorCamera>(_extrinsics_pq, intrinsics_ptr);
* We RECOMMEND using the macro ````WOLF_SENSOR_CREATE(SensorClass)```` to automatically
* generate the sensor creators, provided in 'sensor_base.h'.
*
* sen_ptr->setName(_unique_name);
* To do so, you should implement a constructor with the API:
*
* return sen_ptr;
* }
* \code
* SensorDerived(const YAML::Node& _params)
* \endcode
*
* #### Example 2: registering a sensor creator into the factory
* In case of sensors templated to the dimension of the problem (non-type template), there is
* also the ````WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorDerived, DIM)````. See `sensor_odom.h` for an example.
*
* #### Registering sensor creator into the factory
* Registration can be done manually or automatically. It involves the call to static functions.
* It is advisable to put these calls within unnamed namespaces.
*
......@@ -140,14 +144,18 @@ namespace wolf
* Put the code either at global scope (you must define a dummy variable for this),
* \code
* namespace {
* const bool registered_camera = FactorySensor::registerCreator("SensorCamera", SensorCamera::create);
* const bool registered_camera_node = FactorySensorNode::registerCreator("SensorCamera", SensorCamera::create);
* }
* namespace {
* const bool registered_camera_file = FactorySensorFile::registerCreator("SensorCamera", SensorCamera::create);
* }
* main () { ... }
* \endcode
* or inside your main(), where a direct call is possible:
* \code
* main () {
* FactorySensor::registerCreator("SensorCamera", SensorCamera::create);
* FactorySensorNode::registerCreator("SensorCamera", SensorCamera::create);
* FactorySensorFile::registerCreator("SensorCamera", SensorCamera::create);
* ...
* }
* \endcode
......@@ -156,17 +164,18 @@ namespace wolf
* Put the code at the last line of the sensor_xxx.cpp file,
* \code
* namespace {
* const bool registered_camera = FactorySensor::registerCreator("SensorCamera", SensorCamera::create);
* const bool registered_camera_node = FactorySensorNode::registerCreator("SensorCamera", SensorCamera::create);
* const bool registered_camera_file = FactorySensorFile::registerCreator("SensorCamera", SensorCamera::create);
* }
* \endcode
* Automatic registration is recommended in wolf, and implemented in the classes shipped with it.
* Automatic registration is recommended in wolf, and implemented in the classes shipped with it using the macro
* WOLF_REGISTER_SENSOR(SensorType).
* You are free to comment out these lines and place them wherever you consider it more convenient for your design.
*
* #### Example 2: creating sensors
* #### Creating sensors
* We finally provide the necessary steps to create a sensor of class SensorCamera in our application:
*
* \code
* #include "core/sensor/factory_sensor.h"
* #include "vision/sensor/sensor_camera.h" // provides SensorCamera
*
* // Note: SensorCamera::create() is already registered, automatically.
......@@ -177,82 +186,49 @@ namespace wolf
* // To create a camera, provide:
* // a type = "SensorCamera",
* // a name = "Front-left camera",
* // an extrinsics vector, and
* // a pointer to the intrinsics struct:
*
* Eigen::VectorXd extrinsics_1(7); // give it some values...
*
* // Create a pointer to the struct of sensor parameters stored in a YAML file ( see FactoryParamsSensor )
* ParamsSensorCameraPtr intrinsics_1 =
* FactoryParamsSensor::create("ParamsSensorCamera",
* camera_1.yaml);
* // the problem dimension dim = 3, and
* // the yaml node or a yaml file
*
* SensorBasePtr camera_1_ptr =
* FactorySensor::create ( "SensorCamera" ,
* "Front-left camera" ,
* extrinsics_1 ,
* intrinsics_1 );
*
* // A second camera... with a different name!
*
* Eigen::VectorXd extrinsics_2(7);
* FactorySensorNode::create ( "SensorCamera",
* parameter_node );
*
* ParamsSensorCameraPtr intrinsics_2 =
* FactoryParamsSensor::create("ParamsSensorCamera",
* camera_2.yaml);
* // A second camera... with a different name specified in the yaml file or the parameters!
*
* SensorBasePtr camera_2_ptr =
* FactorySensor::create( "SensorCamera" ,
* "Front-right camera" ,
* extrinsics_2 ,
* intrinsics_2 );
* FactorySensorNode::create( "SensorCamera" ,
* yaml_filename );
*
* return 0;
* }
* \endcode
*
* #### See also
* - Problem::installSensor() : to install sensors in WOLF Problem.
*/
typedef Factory<SensorBase,
const std::string&,
const Eigen::VectorXd&,
const ParamsSensorBasePtr> FactorySensor;
template<>
inline std::string FactorySensor::getClass() const
{
return "FactorySensor";
}
typedef Factory<SensorBasePtr, const YAML::Node&, const std::vector<std::string>&> FactorySensorNode;
typedef Factory<SensorBasePtr, const std::string&, const std::vector<std::string>&> FactorySensorFile;
// ParamsSensor factory
struct ParamsSensorBase;
typedef Factory<ParamsSensorBase,
const std::string&> FactoryParamsSensor;
template<>
inline std::string FactoryParamsSensor::getClass() const
template <>
inline std::string FactorySensorNode::getClass() const
{
return "FactoryParamsSensor";
return "FactorySensorNode";
}
#define WOLF_REGISTER_SENSOR(SensorType) \
namespace{ const bool WOLF_UNUSED SensorType##Registered = \
FactorySensor::registerCreator(#SensorType, SensorType::create); } \
typedef Factory<SensorBase,
const std::string&,
const ParamsServer&> AutoConfFactorySensor;
template<>
inline std::string AutoConfFactorySensor::getClass() const
template <>
inline std::string FactorySensorFile::getClass() const
{
return "AutoConfFactorySensor";
return "FactorySensorFile";
}
#define WOLF_REGISTER_SENSOR_AUTO(SensorType) \
namespace{ const bool WOLF_UNUSED SensorType##AutoConfRegistered = \
AutoConfFactorySensor::registerCreator(#SensorType, SensorType::create); } \
#define WOLF_REGISTER_SENSOR(SensorType) \
namespace \
{ \
const bool WOLF_UNUSED SensorType##NodeRegistered = \
FactorySensorNode::registerCreator(#SensorType, SensorType::create); \
const bool WOLF_UNUSED SensorType##FileRegistered = \
FactorySensorFile::registerCreator(#SensorType, SensorType::create); \
}
} /* namespace wolf */
#endif /* SENSOR_FACTORY_H_ */