Main MRPT website > C++ reference for MRPT 1.5.6
CPosePDF.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "base-precomp.h" // Precompiled headers
11 
12 #include <mrpt/poses/CPosePDF.h>
14 #include <mrpt/utils/CStream.h>
15 
16 using namespace mrpt::poses;
17 using namespace mrpt::math;
18 using namespace std;
19 
20 IMPLEMENTS_VIRTUAL_SERIALIZABLE( CPosePDF, CSerializable, mrpt::poses )
21 
22 
23 void CPosePDF::jacobiansPoseComposition(
24  const CPosePDFGaussian &x,
25  const CPosePDFGaussian &u,
26  CMatrixDouble33 &df_dx,
27  CMatrixDouble33 &df_du)
28 {
29  CPosePDF::jacobiansPoseComposition(x.mean,u.mean,df_dx,df_du);
30 }
31 
32 /*---------------------------------------------------------------
33  jacobiansPoseComposition
34  ---------------------------------------------------------------*/
36  const CPose2D &x,
37  const CPose2D &u,
38  CMatrixDouble33 &df_dx,
39  CMatrixDouble33 &df_du,
40  const bool compute_df_dx,
41  const bool compute_df_du )
42 
43 {
44  const double spx = sin(x.phi());
45  const double cpx = cos(x.phi());
46  if (compute_df_dx)
47  {
48  /*
49  df_dx =
50  [ 1, 0, -sin(phi_x)*x_u-cos(phi_x)*y_u ]
51  [ 0, 1, cos(phi_x)*x_u-sin(phi_x)*y_u ]
52  [ 0, 0, 1 ]
53  */
54  df_dx.unit(3,1.0);
55 
56  const double xu = u.x();
57  const double yu = u.y();
58 
59  df_dx.get_unsafe(0,2) = -spx*xu-cpx*yu;
60  df_dx.get_unsafe(1,2) = cpx*xu-spx*yu;
61  }
62 
63  if (compute_df_du)
64  {
65  /*
66  df_du =
67  [ cos(phi_x) , -sin(phi_x) , 0 ]
68  [ sin(phi_x) , cos(phi_x) , 0 ]
69  [ 0 , 0 , 1 ]
70  */
71  // This is the homogeneous matrix of "x":
72  df_du.get_unsafe(0,2) =
73  df_du.get_unsafe(1,2) =
74  df_du.get_unsafe(2,0) =
75  df_du.get_unsafe(2,1) = 0;
76  df_du.get_unsafe(2,2) = 1;
77 
78  df_du.get_unsafe(0,0) = cpx;
79  df_du.get_unsafe(0,1) = -spx;
80  df_du.get_unsafe(1,0) = spx;
81  df_du.get_unsafe(1,1) = cpx;
82  }
83 }
84 
85 
GLvoid *typedef void(GLAPIENTRY *PFNGLGETVERTEXATTRIBDVPROC)(GLuint
Definition: glew.h:1745
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
#define IMPLEMENTS_VIRTUAL_SERIALIZABLE(class_name, base_class_name, NameSpace)
This must be inserted as implementation of some required members for virtual CSerializable classes: ...
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:39
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
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:35



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018