diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 0d53dc4e0..4343b5408 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -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 - static typename T::Jacobian GenerateFullCovariance( - std::array 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 - 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 - 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 &r) - { - return Rot2{r[0] * degree}; - } - - // Transform a covariance matrix with a rotation and a translation - template - static typename TPose::Jacobian RotateTranslate( - const std::array &r, - const std::array &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 { diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index f9b388b9b..97153238a 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -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 - static typename T::Jacobian GenerateFullCovariance( - std::array 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 - 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 - 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 &r) - { - return Rot2{r[0] * degree}; - } - - // Overloaded function to create a Rot3 from three angles. - static Rot3 RotFromArray(const std::array &r) - { - return Rot3::RzRyRx(r[0] * degree, r[1] * degree, r[2] * degree); - } - - // Transform a covariance matrix with a rotation and a translation - template - static typename TPose::Jacobian RotateTranslate( - const std::array &r, - const std::array &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 s{{0.1, 0.3, 0.7}}; std::array 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 { diff --git a/gtsam/geometry/tests/testPoseAdjointMap.h b/gtsam/geometry/tests/testPoseAdjointMap.h new file mode 100644 index 000000000..1263a807c --- /dev/null +++ b/gtsam/geometry/tests/testPoseAdjointMap.h @@ -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 +#include + +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 + typename T::Jacobian GenerateFullCovariance( + std::array 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 + 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 + 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 &r) + { + return Rot2{r[0] * degree}; + } + + // Overloaded function to create a Rot3 from three angles. + Rot3 RotFromArray(const std::array &r) + { + return Rot3::RzRyRx(r[0] * degree, r[1] * degree, r[2] * degree); + } + + // Transform a covariance matrix with a rotation and a translation + template + typename Pose::Jacobian RotateTranslate( + std::array r, + std::array 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(); + } +} \ No newline at end of file