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

Attempt to fix clang compile warnings and execution failures. Change function names. Add a few comments.

release/4.3a0
Peter Mullen 2019-12-15 20:17:56 -08:00
parent 323a9ab9f2
commit 46eb35ecff
2 changed files with 104 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -839,59 +839,59 @@ TEST(Pose2 , ChartDerivatives) {
namespace transform_covariance3 {
const double degree = M_PI / 180;
template<class TPose>
static typename TPose::Jacobian generate_full_covariance(
std::array<double, TPose::dimension> sigma_values)
// 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 TPose::TangentVector sigmas(&sigma_values.front());
return typename TPose::Jacobian{sigmas * sigmas.transpose()};
typename T::TangentVector sigmas(&sigma_values.front());
return typename T::Jacobian{sigmas * sigmas.transpose()};
}
template<class TPose>
static typename TPose::Jacobian generate_one_variable_covariance(int idx, double sigma)
// 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 TPose::Jacobian cov = TPose::Jacobian::Zero();
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 transform_covariance(
const TPose &wTb,
static typename TPose::Jacobian RotateTranslate(
std::array<double, TPose::Rotation::dimension> r,
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();
}
static typename Pose2::Jacobian rotate_only(
const std::array<double, Pose2::Rotation::dimension> r,
const typename Pose2::Jacobian &cov)
{
// Create a rotation only transform
Pose2 wTb{Pose2::Rotation{r[0] * degree}, Pose2::Translation{}};
return transform_covariance(wTb, cov);
}
static Pose2::Jacobian translate_only(
const std::array<double, Pose2::Translation::dimension> t,
const Pose2::Jacobian &cov)
{
// Create a translation only transform
Pose2 wTb{Pose2::Rotation{}, Pose2::Translation{t[0], t[1]}};
return transform_covariance(wTb, cov);
}
static Pose2::Jacobian rotate_translate(
const std::array<double, Pose2::Rotation::dimension> r,
const std::array<double, Pose2::Translation::dimension> t,
const Pose2::Jacobian &cov)
{
// Create a translation only transform
Pose2 wTb{Pose2::Rotation{r[0] * degree}, Pose2::Translation{t[0], t[1]}};
return transform_covariance(wTb, cov);
}
}
TEST(Pose2 , TransformCovariance3) {
// Use simple covariance matrices and transforms to create tests that can be
@ -900,8 +900,8 @@ TEST(Pose2 , TransformCovariance3) {
// rotate
{
auto cov = generate_full_covariance<Pose2>({0.1, 0.3, 0.7});
auto cov_trans = rotate_only({90.}, cov);
auto cov = GenerateFullCovariance<Pose2>({0.1, 0.3, 0.7});
auto cov_trans = RotateTranslate<Pose2>({{90.}}, {{}}, cov);
// interchange x and y axes
EXPECT_DOUBLES_EQUAL(cov_trans(1, 1), cov(0, 0), 1e-9);
EXPECT_DOUBLES_EQUAL(cov_trans(0, 0), cov(1, 1), 1e-9);
@ -914,8 +914,8 @@ TEST(Pose2 , TransformCovariance3) {
// translate along x with uncertainty in x
{
auto cov = generate_one_variable_covariance<Pose2>(0, 0.1);
auto cov_trans = translate_only({20., 0.}, cov);
auto cov = GenerateOneVariableCovariance<Pose2>(0, 0.1);
auto cov_trans = RotateTranslate<Pose2>({{}}, {{20., 0.}}, cov);
EXPECT_DOUBLES_EQUAL(cov_trans(0, 0), 0.1 * 0.1, 1e-9);
EXPECT_DOUBLES_EQUAL(cov_trans(1, 1), 0., 1e-9);
EXPECT_DOUBLES_EQUAL(cov_trans(2, 2), 0., 1e-9);
@ -927,8 +927,8 @@ TEST(Pose2 , TransformCovariance3) {
// translate along x with uncertainty in y
{
auto cov = generate_one_variable_covariance<Pose2>(1, 0.1);
auto cov_trans = translate_only({20., 0.}, cov);
auto cov = GenerateOneVariableCovariance<Pose2>(1, 0.1);
auto cov_trans = RotateTranslate<Pose2>({{}}, {{20., 0.}}, cov);
EXPECT_DOUBLES_EQUAL(cov_trans(0, 0), 0., 1e-9);
EXPECT_DOUBLES_EQUAL(cov_trans(1, 1), 0.1 * 0.1, 1e-9);
EXPECT_DOUBLES_EQUAL(cov_trans(2, 2), 0., 1e-9);
@ -940,24 +940,24 @@ TEST(Pose2 , TransformCovariance3) {
// translate along x with uncertainty in theta
{
auto cov = generate_one_variable_covariance<Pose2>(2, 0.1);
auto cov_trans = translate_only({20., 0.}, cov);
auto cov = GenerateOneVariableCovariance<Pose2>(2, 0.1);
auto cov_trans = RotateTranslate<Pose2>({{}}, {{20., 0.}}, cov);
EXPECT_DOUBLES_EQUAL(cov_trans(2, 1), -0.1 * 0.1 * 20., 1e-9);
EXPECT_DOUBLES_EQUAL(cov_trans(1, 1), 0.1 * 0.1 * 20. * 20., 1e-9);
}
// rotate and translate along x with uncertainty in x
{
auto cov = generate_one_variable_covariance<Pose2>(0, 0.1);
auto cov_trans = rotate_translate({90.}, {20., 0.}, cov);
auto cov = GenerateOneVariableCovariance<Pose2>(0, 0.1);
auto cov_trans = RotateTranslate<Pose2>({{90.}}, {{20., 0.}}, cov);
EXPECT_DOUBLES_EQUAL(cov_trans(0, 0), 0., 1e-9);
EXPECT_DOUBLES_EQUAL(cov_trans(1, 1), 0.1 * 0.1, 1e-9);
}
// rotate and translate along x with uncertainty in theta
{
auto cov = generate_one_variable_covariance<Pose2>(2, 0.1);
auto cov_trans = rotate_translate({90.}, {20., 0.}, cov);
auto cov = GenerateOneVariableCovariance<Pose2>(2, 0.1);
auto cov_trans = RotateTranslate<Pose2>({{90.}}, {{20., 0.}}, cov);
EXPECT_DOUBLES_EQUAL(cov_trans(0, 0), 0., 1e-9);
EXPECT_DOUBLES_EQUAL(cov_trans(1, 1), 0.1 * 0.1 * 20. * 20., 1e-9);
EXPECT_DOUBLES_EQUAL(cov_trans(2, 2), 0.1 * 0.1, 1e-9);

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

@ -882,90 +882,75 @@ TEST(Pose3 , ChartDerivatives) {
namespace transform_covariance6 {
const double degree = M_PI / 180;
template<class TPose>
static typename TPose::Jacobian generate_full_covariance(
std::array<double, TPose::dimension> sigma_values)
// 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 TPose::TangentVector sigmas(&sigma_values.front());
return typename TPose::Jacobian{sigmas * sigmas.transpose()};
typename T::TangentVector sigmas(&sigma_values.front());
return typename T::Jacobian{sigmas * sigmas.transpose()};
}
template<class TPose>
static typename TPose::Jacobian generate_one_variable_covariance(int idx, double sigma)
// 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 TPose::Jacobian cov = TPose::Jacobian::Zero();
typename T::Jacobian cov = T::Jacobian::Zero();
cov(idx, idx) = sigma * sigma;
return cov;
}
template<class TPose>
static typename TPose::Jacobian generate_two_variable_covariance(int idx0, int idx1, double sigma0, double sigma1)
// 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 TPose::Jacobian cov = TPose::Jacobian::Zero();
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 transform_covariance(
const TPose &wTb,
static typename TPose::Jacobian RotateTranslate(
std::array<double, TPose::Rotation::dimension> r,
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();
}
static Pose2::Jacobian rotate_translate(
const std::array<double, Pose2::Rotation::dimension> r,
const std::array<double, Pose2::Translation::dimension> t,
const Pose2::Jacobian &cov)
{
// Create a translation only transform
Pose2 wTb{Pose2::Rotation{r[0] * degree}, Pose2::Translation{t[0], t[1]}};
return transform_covariance(wTb, cov);
}
static typename Pose3::Jacobian rotate_only(
const std::array<double, Pose3::Rotation::dimension> r,
const typename Pose3::Jacobian &cov)
{
// Create a rotation only transform
Pose3 wTb{Pose3::Rotation::RzRyRx(r[0] * degree, r[1] * degree, r[2] * degree),
Pose3::Translation{}};
return transform_covariance(wTb, cov);
}
static Pose3::Jacobian translate_only(
const std::array<double, Pose3::Translation::dimension> t,
const Pose3::Jacobian &cov)
{
// Create a translation only transform
Pose3 wTb{Pose3::Rotation{}, Pose3::Translation{t[0], t[1], t[2]}};
return transform_covariance(wTb, cov);
}
static Pose3::Jacobian rotate_translate(
const std::array<double, Pose3::Rotation::dimension> r,
const std::array<double, Pose3::Translation::dimension> t,
const Pose3::Jacobian &cov)
{
// Create a translation only transform
Pose3 wTb{Pose3::Rotation::RzRyRx(r[0] * degree, r[1] * degree, r[2] * degree),
Pose3::Translation{t[0], t[1], t[2]}};
return transform_covariance(wTb, cov);
}
}
TEST(Pose3, TransformCovariance6MapTo2d) {
// Create 3d scenarios that map to 2d configurations and compare with Pose2 results.
using namespace transform_covariance6;
std::array<double, Pose2::dimension> s{{0.1, 0.3, 0.7}};
std::array<double, Pose2::Rotation::dimension> r{{31.}};
std::array<double, Pose2::Translation::dimension> t{{1.1, 1.5}};
auto cov2 = generate_full_covariance<Pose2>({0.1, 0.3, 0.7});
auto cov2_trans = rotate_translate(r, t, cov2);
auto cov2 = GenerateFullCovariance<Pose2>({{0.1, 0.3, 0.7}});
auto cov2_trans = RotateTranslate<Pose2>(r, t, cov2);
auto match_cov3_to_cov2 = [&](int r_axis, int spatial_axis0, int spatial_axis1,
const Pose2::Jacobian &cov2, const Pose3::Jacobian &cov3) -> void
@ -981,22 +966,22 @@ TEST(Pose3, TransformCovariance6MapTo2d) {
// rotate around x axis
{
auto cov3 = generate_full_covariance<Pose3>({s[2], 0., 0., 0., s[0], s[1]});
auto cov3_trans = rotate_translate({r[0], 0., 0.}, {0., t[0], t[1]}, cov3);
auto cov3 = GenerateFullCovariance<Pose3>({{s[2], 0., 0., 0., s[0], s[1]}});
auto cov3_trans = RotateTranslate<Pose3>({{r[0], 0., 0.}}, {{0., t[0], t[1]}}, cov3);
match_cov3_to_cov2(0, 4, 5, cov2_trans, cov3_trans);
}
// rotate around y axis
{
auto cov3 = generate_full_covariance<Pose3>({0., s[2], 0., s[1], 0., s[0]});
auto cov3_trans = rotate_translate({0., r[0], 0.}, {t[1], 0., t[0]}, cov3);
auto cov3 = GenerateFullCovariance<Pose3>({{0., s[2], 0., s[1], 0., s[0]}});
auto cov3_trans = RotateTranslate<Pose3>({{0., r[0], 0.}}, {{t[1], 0., t[0]}}, cov3);
match_cov3_to_cov2(1, 5, 3, cov2_trans, cov3_trans);
}
// rotate around z axis
{
auto cov3 = generate_full_covariance<Pose3>({0., 0., s[2], s[0], s[1], 0.});
auto cov3_trans = rotate_translate({0., 0., r[0]}, {t[0], t[1], 0.}, cov3);
auto cov3 = GenerateFullCovariance<Pose3>({{0., 0., s[2], s[0], s[1], 0.}});
auto cov3_trans = RotateTranslate<Pose3>({{0., 0., r[0]}}, {{t[0], t[1], 0.}}, cov3);
match_cov3_to_cov2(2, 3, 4, cov2_trans, cov3_trans);
}
}
@ -1009,8 +994,8 @@ TEST(Pose3, TransformCovariance6) {
// rotate 90 around z axis and then 90 around y axis
{
auto cov = generate_full_covariance<Pose3>({0.1, 0.2, 0.3, 0.5, 0.7, 1.1});
auto cov_trans = rotate_only({0., 90., 90.}, cov);
auto cov = GenerateFullCovariance<Pose3>({{0.1, 0.2, 0.3, 0.5, 0.7, 1.1}});
auto cov_trans = RotateTranslate<Pose3>({{0., 90., 90.}}, {{}}, cov);
// x from y, y from z, z from x
EXPECT_DOUBLES_EQUAL(cov_trans(0, 0), cov(1, 1), 1e-9);
EXPECT_DOUBLES_EQUAL(cov_trans(1, 1), cov(2, 2), 1e-9);
@ -1029,8 +1014,8 @@ TEST(Pose3, TransformCovariance6) {
// translate along the x axis with uncertainty in roty and rotz
{
auto cov = generate_two_variable_covariance<Pose3>(1, 2, 0.7, 0.3);
auto cov_trans = translate_only({20., 0., 0.}, cov);
auto cov = GenerateTwoVariableCovariance<Pose3>(1, 2, 0.7, 0.3);
auto cov_trans = RotateTranslate<Pose3>({{}}, {{20., 0., 0.}}, cov);
// The variance in roty and rotz causes off-diagonal covariances
EXPECT_DOUBLES_EQUAL(cov_trans(5, 1), 0.7 * 0.7 * 20., 1e-9);
EXPECT_DOUBLES_EQUAL(cov_trans(5, 5), 0.7 * 0.7 * 20. * 20., 1e-9);
@ -1043,16 +1028,16 @@ TEST(Pose3, TransformCovariance6) {
// rotate around x axis and translate along the x axis with uncertainty in rotx
{
auto cov = generate_one_variable_covariance<Pose3>(0, 0.1);
auto cov_trans = rotate_translate({90., 0., 0.}, {20., 0., 0.}, cov);
auto cov = GenerateOneVariableCovariance<Pose3>(0, 0.1);
auto cov_trans = RotateTranslate<Pose3>({{90., 0., 0.}}, {{20., 0., 0.}}, cov);
EXPECT_DOUBLES_EQUAL(cov_trans(1, 0), 0., 1e-9);
EXPECT_DOUBLES_EQUAL(cov_trans(2, 0), 0., 1e-9);
}
// rotate around x axis and translate along the x axis with uncertainty in roty
{
auto cov = generate_one_variable_covariance<Pose3>(1, 0.1);
auto cov_trans = rotate_translate({90., 0., 0.}, {20., 0., 0.}, cov);
auto cov = GenerateOneVariableCovariance<Pose3>(1, 0.1);
auto cov_trans = RotateTranslate<Pose3>({{90., 0., 0.}}, {{20., 0., 0.}}, cov);
// interchange the y and z axes.
EXPECT_DOUBLES_EQUAL(cov_trans(2, 2), 0.1 * 0.1, 1e-9);
EXPECT_DOUBLES_EQUAL(cov_trans(4, 2), -0.1 * 0.1 * 20., 1e-9);