MRPT  2.0.1
CPosePDF.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "poses-precomp.h" // Precompiled headers
11 
12 #include <mrpt/poses/CPosePDF.h>
15 
16 using namespace mrpt::poses;
17 using namespace mrpt::math;
18 using namespace std;
19 
21 
22 void CPosePDF::jacobiansPoseComposition(
23  const CPosePDFGaussian& x, const CPosePDFGaussian& u,
24  CMatrixDouble33& df_dx, CMatrixDouble33& df_du)
25 {
26  CPosePDF::jacobiansPoseComposition(x.mean, u.mean, df_dx, df_du);
27 }
28 
29 /*---------------------------------------------------------------
30  jacobiansPoseComposition
31  ---------------------------------------------------------------*/
33  const CPose2D& x, const CPose2D& u, CMatrixDouble33& df_dx,
34  CMatrixDouble33& df_du, const bool compute_df_dx, const bool compute_df_du)
35 
36 {
37  const double spx = sin(x.phi());
38  const double cpx = cos(x.phi());
39  if (compute_df_dx)
40  {
41  /*
42  df_dx =
43  [ 1, 0, -sin(phi_x)*x_u-cos(phi_x)*y_u ]
44  [ 0, 1, cos(phi_x)*x_u-sin(phi_x)*y_u ]
45  [ 0, 0, 1 ]
46  */
47  df_dx.setIdentity();
48 
49  const double xu = u.x();
50  const double yu = u.y();
51 
52  df_dx(0, 2) = -spx * xu - cpx * yu;
53  df_dx(1, 2) = cpx * xu - spx * yu;
54  }
55 
56  if (compute_df_du)
57  {
58  /*
59  df_du =
60  [ cos(phi_x) , -sin(phi_x) , 0 ]
61  [ sin(phi_x) , cos(phi_x) , 0 ]
62  [ 0 , 0 , 1 ]
63  */
64  // This is the homogeneous matrix of "x":
65  df_du(0, 2) = df_du(1, 2) = df_du(2, 0) = df_du(2, 1) = 0;
66  df_du(2, 2) = 1;
67 
68  df_du(0, 0) = cpx;
69  df_du(0, 1) = -spx;
70  df_du(1, 0) = spx;
71  df_du(1, 1) = cpx;
72  }
73 }
#define IMPLEMENTS_VIRTUAL_SERIALIZABLE(class_name, base_class, NS)
This must be inserted as implementation of some required members for virtual CSerializable classes: ...
STL namespace.
This base provides a set of functions for maths stuff.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:38
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
static void jacobiansPoseComposition(const CPose2D &x, const CPose2D &u, mrpt::math::CMatrixDouble33 &df_dx, mrpt::math::CMatrixDouble33 &df_du, const bool compute_df_dx=true, const bool compute_df_du=true)
This static method computes the pose composition Jacobians, with these formulas:
Definition: CPosePDF.cpp:32



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020