12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Move shared code into a header file

release/4.3a0
Peter Mullen 2019-12-15 22:59:11 -08:00
parent f33a77234f
commit 1f4297a0ec
3 changed files with 92 additions and 121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
gtsam/geometry/tests/testPose2.cpp
View File

@ -836,67 +836,12 @@ TEST(Pose2 , ChartDerivatives) {
}
//******************************************************************************
namespace transform_covariance3 {
const double degree = M_PI / 180;
#include "testPoseAdjointMap.h"
// Create a covariance matrix for type T. Use sigma_values^2 on the diagonal
// and fill in non-diagonal entries with a correlation coefficient of 1. Note:
// a covariance matrix for T has the same dimensions as a Jacobian for T.
template<class T>
static typename T::Jacobian GenerateFullCovariance(
std::array<double, T::dimension> sigma_values)
{
typename T::TangentVector sigmas(&sigma_values.front());
return typename T::Jacobian{sigmas * sigmas.transpose()};
}
// Create a covariance matrix with one non-zero element on the diagonal.
template<class T>
static typename T::Jacobian GenerateOneVariableCovariance(int idx, double sigma)
{
typename T::Jacobian cov = T::Jacobian::Zero();
cov(idx, idx) = sigma * sigma;
return cov;
}
// Create a covariance matrix with two non-zero elements on the diagonal with
// a correlation of 1.0
template<class T>
static typename T::Jacobian GenerateTwoVariableCovariance(int idx0, int idx1, double sigma0, double sigma1)
{
typename T::Jacobian cov = T::Jacobian::Zero();
cov(idx0, idx0) = sigma0 * sigma0;
cov(idx1, idx1) = sigma1 * sigma1;
cov(idx0, idx1) = cov(idx1, idx0) = sigma0 * sigma1;
return cov;
}
// Overloaded function to create a Rot2 from one angle.
static Rot2 RotFromArray(const std::array<double, Rot2::dimension> &r)
{
return Rot2{r[0] * degree};
}
// Transform a covariance matrix with a rotation and a translation
template<class TPose>
static typename TPose::Jacobian RotateTranslate(
const std::array<double, TPose::Rotation::dimension> &r,
const std::array<double, TPose::Translation::dimension> &t,
const typename TPose::Jacobian &cov)
{
// Construct a pose object
typename TPose::Rotation rot{RotFromArray(r)};
TPose wTb{rot, typename TPose::Translation{&t.front()}};
// transform the covariance with the AdjointMap
auto adjointMap = wTb.AdjointMap();
return adjointMap * cov * adjointMap.transpose();
}
}
TEST(Pose2 , TransformCovariance3) {
// Use simple covariance matrices and transforms to create tests that can be
// validated with simple computations.
using namespace transform_covariance3;
using namespace test_pose_adjoint_map;
// rotate
{

67
gtsam/geometry/tests/testPose3.cpp
View File

@ -879,72 +879,11 @@ TEST(Pose3 , ChartDerivatives) {
}
//******************************************************************************
namespace transform_covariance6 {
const double degree = M_PI / 180;
#include "testPoseAdjointMap.h"
// Create a covariance matrix for type T. Use sigma_values^2 on the diagonal
// and fill in non-diagonal entries with a correlation coefficient of 1. Note:
// a covariance matrix for T has the same dimensions as a Jacobian for T.
template<class T>
static typename T::Jacobian GenerateFullCovariance(
std::array<double, T::dimension> sigma_values)
{
typename T::TangentVector sigmas(&sigma_values.front());
return typename T::Jacobian{sigmas * sigmas.transpose()};
}
// Create a covariance matrix with one non-zero element on the diagonal.
template<class T>
static typename T::Jacobian GenerateOneVariableCovariance(int idx, double sigma)
{
typename T::Jacobian cov = T::Jacobian::Zero();
cov(idx, idx) = sigma * sigma;
return cov;
}
// Create a covariance matrix with two non-zero elements on the diagonal with
// a correlation of 1.0
template<class T>
static typename T::Jacobian GenerateTwoVariableCovariance(int idx0, int idx1, double sigma0, double sigma1)
{
typename T::Jacobian cov = T::Jacobian::Zero();
cov(idx0, idx0) = sigma0 * sigma0;
cov(idx1, idx1) = sigma1 * sigma1;
cov(idx0, idx1) = cov(idx1, idx0) = sigma0 * sigma1;
return cov;
}
// Overloaded function to create a Rot2 from one angle.
static Rot2 RotFromArray(const std::array<double, Rot2::dimension> &r)
{
return Rot2{r[0] * degree};
}
// Overloaded function to create a Rot3 from three angles.
static Rot3 RotFromArray(const std::array<double, Rot3::dimension> &r)
{
return Rot3::RzRyRx(r[0] * degree, r[1] * degree, r[2] * degree);
}
// Transform a covariance matrix with a rotation and a translation
template<class TPose>
static typename TPose::Jacobian RotateTranslate(
const std::array<double, TPose::Rotation::dimension> &r,
const std::array<double, TPose::Translation::dimension> &t,
const typename TPose::Jacobian &cov)
{
// Construct a pose object
typename TPose::Rotation rot{RotFromArray(r)};
TPose wTb{rot, typename TPose::Translation{&t.front()}};
// transform the covariance with the AdjointMap
auto adjointMap = wTb.AdjointMap();
return adjointMap * cov * adjointMap.transpose();
}
}
TEST(Pose3, TransformCovariance6MapTo2d) {
// Create 3d scenarios that map to 2d configurations and compare with Pose2 results.
using namespace transform_covariance6;
using namespace test_pose_adjoint_map;
std::array<double, Pose2::dimension> s{{0.1, 0.3, 0.7}};
std::array<double, Pose2::Rotation::dimension> r{{31.}};
@ -990,7 +929,7 @@ TEST(Pose3, TransformCovariance6MapTo2d) {
TEST(Pose3, TransformCovariance6) {
// Use simple covariance matrices and transforms to create tests that can be
// validated with simple computations.
using namespace transform_covariance6;
using namespace test_pose_adjoint_map;
// rotate 90 around z axis and then 90 around y axis
{

87
gtsam/geometry/tests/testPoseAdjointMap.h Normal file
View File

@ -0,0 +1,87 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testPoseAdjointMap.h
* @brief Support utilities for using AdjointMap for transforming Pose2 and Pose3 covariance matrices
* @author Peter Mulllen
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
namespace test_pose_adjoint_map
{
const double degree = M_PI / 180;
// Create a covariance matrix for type T. Use sigma_values^2 on the diagonal
// and fill in non-diagonal entries with correlation coefficient of 1. Note:
// a covariance matrix for T has the same dimensions as a Jacobian for T.
template<class T>
typename T::Jacobian GenerateFullCovariance(
std::array<double, T::dimension> sigma_values)
{
typename T::TangentVector sigmas(&sigma_values.front());
return typename T::Jacobian{sigmas * sigmas.transpose()};
}
// Create a covariance matrix with one non-zero element on the diagonal.
template<class T>
typename T::Jacobian GenerateOneVariableCovariance(int idx, double sigma)
{
typename T::Jacobian cov = T::Jacobian::Zero();
cov(idx, idx) = sigma * sigma;
return cov;
}
// Create a covariance matrix with two non-zero elements on the diagonal with
// a correlation of 1.0
template<class T>
typename T::Jacobian GenerateTwoVariableCovariance(int idx0, int idx1, double sigma0, double sigma1)
{
typename T::Jacobian cov = T::Jacobian::Zero();
cov(idx0, idx0) = sigma0 * sigma0;
cov(idx1, idx1) = sigma1 * sigma1;
cov(idx0, idx1) = cov(idx1, idx0) = sigma0 * sigma1;
return cov;
}
// Overloaded function to create a Rot2 from one angle.
Rot2 RotFromArray(const std::array<double, Rot2::dimension> &r)
{
return Rot2{r[0] * degree};
}
// Overloaded function to create a Rot3 from three angles.
Rot3 RotFromArray(const std::array<double, Rot3::dimension> &r)
{
return Rot3::RzRyRx(r[0] * degree, r[1] * degree, r[2] * degree);
}
// Transform a covariance matrix with a rotation and a translation
template<class Pose>
typename Pose::Jacobian RotateTranslate(
std::array<double, Pose::Rotation::dimension> r,
std::array<double, Pose::Translation::dimension> t,
const typename Pose::Jacobian &cov)
{
// Construct a pose object
typename Pose::Rotation rot{RotFromArray(r)};
Pose wTb{rot, typename Pose::Translation{&t.front()}};
// transform the covariance with the AdjointMap
auto adjointMap = wTb.AdjointMap();
return adjointMap * cov * adjointMap.transpose();
}
}