testPoseAdjointMap.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #pragma once
20 
21 #include <gtsam/geometry/Pose2.h>
22 #include <gtsam/geometry/Pose3.h>
23 
25 {
26  const double degree = M_PI / 180;
27 
28  // Return a covariance matrix for type T with non-zero values for every element.
29  // Use sigma_values^2 on the diagonal and fill in non-diagonal entries with
30  // correlation coefficient of 1. Note: a covariance matrix for T has the same
31  // dimensions as a Jacobian for T, the returned matrix is not a Jacobian.
32  template<class T>
33  typename T::Jacobian FullCovarianceFromSigmas(
34  const typename T::TangentVector &sigmas)
35  {
36  return sigmas * sigmas.transpose();
37  }
38 
39  // Return a covariance matrix with one non-zero element on the diagonal.
40  template<class T>
41  typename T::Jacobian SingleVariableCovarianceFromSigma(int idx, double sigma)
42  {
43  typename T::Jacobian cov = T::Jacobian::Zero();
44  cov(idx, idx) = sigma * sigma;
45  return cov;
46  }
47 
48  // Return a covariance matrix with two non-zero elements on the diagonal and
49  // a correlation of 1.0 between the two variables.
50  template<class T>
51  typename T::Jacobian TwoVariableCovarianceFromSigmas(int idx0, int idx1, double sigma0, double sigma1)
52  {
53  typename T::Jacobian cov = T::Jacobian::Zero();
54  cov(idx0, idx0) = sigma0 * sigma0;
55  cov(idx1, idx1) = sigma1 * sigma1;
56  cov(idx0, idx1) = cov(idx1, idx0) = sigma0 * sigma1;
57  return cov;
58  }
59 }
#define M_PI
Definition: main.h:106
T::Jacobian FullCovarianceFromSigmas(const typename T::TangentVector &sigmas)
static const double sigma
T::Jacobian SingleVariableCovarianceFromSigma(int idx, double sigma)
2D Pose
3D Pose
T::Jacobian TwoVariableCovarianceFromSigmas(int idx0, int idx1, double sigma0, double sigma1)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:56