Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix rekey of LinearContainerFactor #777

Merged
merged 2 commits into from
May 31, 2021
Merged

Fix rekey of LinearContainerFactor #777

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
76c8710
add test for rekey of LinearContainerFactor
varunagrawal May 30, 2021
457d548
override the rekey methods so as to update the properties as well
varunagrawal May 30, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
41 changes: 41 additions & 0 deletions gtsam/nonlinear/LinearContainerFactor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -164,6 +164,47 @@ NonlinearFactor::shared_ptr LinearContainerFactor::negateToNonlinear() const {
return NonlinearFactor::shared_ptr(new LinearContainerFactor(antifactor, linearizationPoint_));
}

/* ************************************************************************* */
NonlinearFactor::shared_ptr LinearContainerFactor::rekey(
const std::map<Key, Key>& rekey_mapping) const {
auto rekeyed_base_factor = Base::rekey(rekey_mapping);
// Update the keys to the properties as well
// Downncast so we have access to members
auto new_factor = boost::static_pointer_cast<LinearContainerFactor>(rekeyed_base_factor);
// Create a new Values to assign later
Values newLinearizationPoint;
for (size_t i = 0; i < factor_->size(); ++i) {
auto mapping = rekey_mapping.find(factor_->keys()[i]);
if (mapping != rekey_mapping.end())
new_factor->factor_->keys()[i] = mapping->second;
newLinearizationPoint.insert(mapping->second, linearizationPoint_->at(mapping->first));
}
new_factor->linearizationPoint_ = newLinearizationPoint;

// upcast back and return
return boost::static_pointer_cast<NonlinearFactor>(new_factor);
}

/* ************************************************************************* */
NonlinearFactor::shared_ptr LinearContainerFactor::rekey(
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe we can just reuse the method for std::map here?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you please explain more?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Oh I understand now. I don't think that is doable, because the std::map version is doing a replacement, while the other one is doing an update. The assignment in the KeyVector version makes it cleaner and I am not sure how we would create the map without making some assumptions about what the user intended.

const KeyVector& new_keys) const {
auto rekeyed_base_factor = Base::rekey(new_keys);
// Update the keys to the properties as well
// Downncast so we have access to members
auto new_factor = boost::static_pointer_cast<LinearContainerFactor>(rekeyed_base_factor);
new_factor->factor_->keys() = new_factor->keys();
// Create a new Values to assign later
Values newLinearizationPoint;
for(size_t i=0; i<new_keys.size(); ++i) {
Key cur_key = linearizationPoint_->keys()[i];
newLinearizationPoint.insert(new_keys[i], linearizationPoint_->at(cur_key));
}
new_factor->linearizationPoint_ = newLinearizationPoint;

// upcast back and return
return boost::static_pointer_cast<NonlinearFactor>(new_factor);
}

/* ************************************************************************* */
NonlinearFactorGraph LinearContainerFactor::ConvertLinearGraph(
const GaussianFactorGraph& linear_graph, const Values& linearizationPoint) {
Expand Down
15 changes: 14 additions & 1 deletion gtsam/nonlinear/LinearContainerFactor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -120,8 +120,21 @@ class LinearContainerFactor : public NonlinearFactor {
return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_,linearizationPoint_));
}

// casting syntactic sugar
/**
* Creates a shared_ptr clone of the
* factor with different keys using
* a map from old->new keys
*/
NonlinearFactor::shared_ptr rekey(
const std::map<Key, Key>& rekey_mapping) const override;

/**
* Clones a factor and fully replaces its keys
* @param new_keys is the full replacement set of keys
*/
NonlinearFactor::shared_ptr rekey(const KeyVector& new_keys) const override;

/// Casting syntactic sugar
inline bool hasLinearizationPoint() const { return linearizationPoint_.is_initialized(); }

/**
Expand Down
4 changes: 2 additions & 2 deletions gtsam/nonlinear/NonlinearFactor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -126,13 +126,13 @@ class GTSAM_EXPORT NonlinearFactor: public Factor {
* factor with different keys using
* a map from old->new keys
*/
shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const;
virtual shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const;

/**
* Clones a factor and fully replaces its keys
* @param new_keys is the full replacement set of keys
*/
shared_ptr rekey(const KeyVector& new_keys) const;
virtual shared_ptr rekey(const KeyVector& new_keys) const;

}; // \class NonlinearFactor

Expand Down
70 changes: 57 additions & 13 deletions gtsam/nonlinear/tests/testLinearContainerFactor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,13 +6,15 @@
*/

#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/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>

using namespace std;
Expand All @@ -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);

/* ************************************************************************* */
TEST( testLinearContainerFactor, generic_jacobian_factor ) {
TEST(TestLinearContainerFactor, generic_jacobian_factor) {

Matrix A1 = (Matrix(2, 2) <<
2.74222, -0.0067457,
Expand Down Expand Up @@ -61,7 +63,7 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) {
}

/* ************************************************************************* */
TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
TEST(TestLinearContainerFactor, jacobian_factor_withlinpoints) {

Matrix A1 = (Matrix(2, 2) <<
2.74222, -0.0067457,
Expand Down Expand Up @@ -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 G12 = (Matrix(1, 2) << 2.0, 4.0).finished();
Matrix G13 = (Matrix(1, 3) << 3.0, 6.0, 9.0).finished();
Expand Down Expand Up @@ -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)
// Initial ordering: x1, l1

Expand Down Expand Up @@ -226,7 +228,7 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
}

/* ************************************************************************* */
TEST( testLinearContainerFactor, creation ) {
TEST(TestLinearContainerFactor, Creation) {
// Create a set of local keys (No robot label)
Key l1 = 11, l3 = 13, l5 = 15;

Expand All @@ -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.
gtsam::Key key1(1);
Expand Down Expand Up @@ -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.
gtsam::Key key1(1);
Expand Down Expand Up @@ -319,6 +321,48 @@ TEST( testLinearContainerFactor, hessian_relinearize )
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); }
/* ************************************************************************* */
Expand Down