Visual Servoing Platform  version 3.6.1 under development (2024-10-09)
vpUKSigmaDrawerMerwe.h
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Display a point cloud using PCL library.
32  */
33 
34 #ifndef VP_UK_SIGMA_DRAWER_MERWE_H
35 #define VP_UK_SIGMA_DRAWER_MERWE_H
36 
37 #include <vector>
38 
39 #include <visp3/core/vpConfig.h>
40 
41 #include <visp3/core/vpUKSigmaDrawerAbstract.h>
42 #include <visp3/core/vpUnscentedKalman.h>
43 
44 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
45 BEGIN_VISP_NAMESPACE
97 {
98 public:
100 
112  vpUKSigmaDrawerMerwe(const int &n, const double &alpha, const double &beta, const double &kappa,
115 
119  virtual ~vpUKSigmaDrawerMerwe() { }
120 
129  virtual std::vector<vpColVector> drawSigmaPoints(const vpColVector &mean, const vpMatrix &covariance) override;
130 
136  virtual vpSigmaPointsWeights computeWeights() override;
137 protected:
138  inline void computeLambda()
139  {
140  m_lambda = m_alpha * m_alpha * (static_cast<double>(m_n) + m_kappa) - static_cast<double>(m_n);
141  }
142 
143  double m_alpha;
145  double m_beta;
146  double m_kappa;
147  double m_lambda;
150 };
151 END_VISP_NAMESPACE
152 #endif
153 #endif
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
virtual vpSigmaPointsWeights computeWeights()=0
Computed the weigths that correspond to the sigma poitns that have been drawn.
virtual std::vector< vpColVector > drawSigmaPoints(const vpColVector &mean, const vpMatrix &covariance)=0
Draw the sigma points according to the current mean and covariance of the state of the Unscented Kalm...
vpAddSubFunction m_resFunc
vpUnscentedKalman::vpAddSubFunction vpAddSubFunction
vpAddSubFunction m_addFunc
static vpColVector simpleAdd(const vpColVector &a, const vpColVector &toAdd)
Simple function to compute an addition, which just does .
static vpColVector simpleResidual(const vpColVector &a, const vpColVector &toSubtract)
Simple function to compute a residual, which just does .
std::function< vpColVector(const vpColVector &, const vpColVector &)> vpAddSubFunction
Function that computes either the equivalent of an addition or the equivalent of a subtraction in the...