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

add test for rekey of LinearContainerFactor

release/4.3a0
Varun Agrawal 2021-05-30 11:22:16 -04:00
parent 1011055007
commit 76c8710738
1 changed files with 57 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
gtsam/nonlinear/tests/testLinearContainerFactor.cpp
View File

@ -6,13 +6,15 @@
*/ */
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <boost/assign/std/vector.hpp> #include <boost/assign/std/vector.hpp>
using namespace std; using namespace std;
@ -28,7 +30,7 @@ Point2 landmark1(5.0, 1.5), landmark2(7.0, 1.5);
Pose2 poseA1(0.0, 0.0, 0.0), poseA2(2.0, 0.0, 0.0); Pose2 poseA1(0.0, 0.0, 0.0), poseA2(2.0, 0.0, 0.0);
/* ************************************************************************* */ /* ************************************************************************* */
TEST( testLinearContainerFactor, generic_jacobian_factor ) { TEST(TestLinearContainerFactor, generic_jacobian_factor) {
Matrix A1 = (Matrix(2, 2) << Matrix A1 = (Matrix(2, 2) <<
2.74222, -0.0067457, 2.74222, -0.0067457,
@ -61,7 +63,7 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { TEST(TestLinearContainerFactor, jacobian_factor_withlinpoints) {
Matrix A1 = (Matrix(2, 2) << Matrix A1 = (Matrix(2, 2) <<
2.74222, -0.0067457, 2.74222, -0.0067457,
@ -115,7 +117,7 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( testLinearContainerFactor, generic_hessian_factor ) { TEST(TestLinearContainerFactor, generic_hessian_factor) {
Matrix G11 = (Matrix(1, 1) << 1.0).finished(); Matrix G11 = (Matrix(1, 1) << 1.0).finished();
Matrix G12 = (Matrix(1, 2) << 2.0, 4.0).finished(); Matrix G12 = (Matrix(1, 2) << 2.0, 4.0).finished();
Matrix G13 = (Matrix(1, 3) << 3.0, 6.0, 9.0).finished(); Matrix G13 = (Matrix(1, 3) << 3.0, 6.0, 9.0).finished();
@ -153,7 +155,7 @@ TEST( testLinearContainerFactor, generic_hessian_factor ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) { TEST(TestLinearContainerFactor, hessian_factor_withlinpoints) {
// 2 variable example, one pose, one landmark (planar) // 2 variable example, one pose, one landmark (planar)
// Initial ordering: x1, l1 // Initial ordering: x1, l1
@ -226,7 +228,7 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( testLinearContainerFactor, creation ) { TEST(TestLinearContainerFactor, Creation) {
// Create a set of local keys (No robot label) // Create a set of local keys (No robot label)
Key l1 = 11, l3 = 13, l5 = 15; Key l1 = 11, l3 = 13, l5 = 15;
@ -252,7 +254,7 @@ TEST( testLinearContainerFactor, creation ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( testLinearContainerFactor, jacobian_relinearize ) TEST(TestLinearContainerFactor, jacobian_relinearize)
{ {
// Create a Between Factor from a Point3. This is actually a linear factor. // Create a Between Factor from a Point3. This is actually a linear factor.
gtsam::Key key1(1); gtsam::Key key1(1);
@ -286,7 +288,7 @@ TEST( testLinearContainerFactor, jacobian_relinearize )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( testLinearContainerFactor, hessian_relinearize ) TEST(TestLinearContainerFactor, hessian_relinearize)
{ {
// Create a Between Factor from a Point3. This is actually a linear factor. // Create a Between Factor from a Point3. This is actually a linear factor.
gtsam::Key key1(1); gtsam::Key key1(1);
@ -319,6 +321,48 @@ TEST( testLinearContainerFactor, hessian_relinearize )
CHECK(gtsam::assert_equal(*expected_factor, *actual_factor)); CHECK(gtsam::assert_equal(*expected_factor, *actual_factor));
} }
/* ************************************************************************* */
TEST(TestLinearContainerFactor, Rekey) {
// Make an example factor
auto nonlinear_factor =
boost::make_shared<gtsam::BetweenFactor<gtsam::Point3>>(
gtsam::Symbol('x', 0), gtsam::Symbol('l', 0), gtsam::Point3(),
gtsam::noiseModel::Isotropic::Sigma(3, 1));
// Linearize and create an LCF
gtsam::Values linearization_pt;
linearization_pt.insert(gtsam::Symbol('x', 0), gtsam::Point3());
linearization_pt.insert(gtsam::Symbol('l', 0), gtsam::Point3());
LinearContainerFactor lcf_factor(
nonlinear_factor->linearize(linearization_pt), linearization_pt);
// Define a key mapping
std::map<gtsam::Key, gtsam::Key> key_map;
key_map[gtsam::Symbol('x', 0)] = gtsam::Symbol('x', 4);
key_map[gtsam::Symbol('l', 0)] = gtsam::Symbol('l', 4);
// Rekey (Calls NonlinearFactor::rekey() which should probably be overriden)
// This of type boost_ptr<NonlinearFactor>
auto lcf_factor_rekeyed = lcf_factor.rekey(key_map);
// Cast back to LCF ptr
LinearContainerFactor::shared_ptr lcf_factor_rekey_ptr =
boost::static_pointer_cast<LinearContainerFactor>(lcf_factor_rekeyed);
CHECK(lcf_factor_rekey_ptr);
// For extra fun lets try linearizing this LCF
gtsam::Values linearization_pt_rekeyed;
for (auto key_val : linearization_pt) {
linearization_pt_rekeyed.insert(key_map.at(key_val.key), key_val.value);
}
// Check independent values since we don't want to unnecessarily sort
// The keys are just in the reverse order wrt the other container
CHECK(assert_equal(linearization_pt_rekeyed.keys()[1], lcf_factor_rekey_ptr->keys()[0]));
CHECK(assert_equal(linearization_pt_rekeyed.keys()[0], lcf_factor_rekey_ptr->keys()[1]));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */ /* ************************************************************************* */