Skip to content

Fix PartialPriorFactor (again) #755

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

Open
miloknowles wants to merge 13 commits into
base: develop
Choose a base branch
from
+117 −46
Open

Fix PartialPriorFactor (again) #755

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
74cd0a9
Unit tests that show error != expected (zero)
Apr 23, 2021
2855a5d
Reimplement PartialPriorFactor using a parameter vector instead of a …
Apr 25, 2021
191db15
Merge branch 'develop' into fix/partial_prior
varunagrawal Sep 23, 2024
83f8ac8
fixes
varunagrawal Sep 24, 2024
89c2ce4
undo formatting
varunagrawal Sep 24, 2024
2acb973
more undo formatting
varunagrawal Sep 24, 2024
63b22a3
more undo formatting
varunagrawal Sep 24, 2024
d2adcab
more undo formatting
varunagrawal Sep 24, 2024
22cb8c4
undo superficial changes to tests
varunagrawal Sep 24, 2024
4014afd
update PartialPosePriorFactor to remove boost
varunagrawal Sep 24, 2024
6efb3b6
update tests to use the logmap values for the priors
varunagrawal Sep 24, 2024
686d006
Account for Rot3 Cayley map
varunagrawal Sep 25, 2024
f76e5f4
fix tests
varunagrawal Sep 25, 2024
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
30 changes: 17 additions & 13 deletions gtsam_unstable/slam/PartialPriorFactor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,9 +34,8 @@ namespace gtsam {
*
* @tparam VALUE is the type of variable the prior effects
*/
template<class VALUE>
class PartialPriorFactor: public NoiseModelFactorN<VALUE> {

template <class VALUE>
class PartialPriorFactor : public NoiseModelFactorN<VALUE> {
public:
typedef VALUE T;

Expand Down Expand Up @@ -65,10 +64,10 @@ namespace gtsam {
/** default constructor - only use for serialization */
PartialPriorFactor() {}

/** Single Element Constructor: Prior on a single parameter at index 'idx' in the tangent vector.*/
/** Single Index Constructor: Prior on a single parameter at index 'idx' in the parameter vector.*/
PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) :
Base(model, key),
prior_((Vector(1) << prior).finished()),
prior_(Vector1(prior)),
indices_(1, idx) {
assert(model->dim() == 1);
}
Expand All @@ -95,7 +94,12 @@ namespace gtsam {
/** print */
void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s, keyFormatter);
gtsam::print(prior_, "prior");
gtsam::print(prior_, "Prior: ");
std::cout << "Indices: ";
for (const int i : indices_) {
std::cout << i << " ";
}
std::cout << std::endl;
}

/** equals */
Expand All @@ -112,13 +116,14 @@ namespace gtsam {
Vector evaluateError(const T& p, OptionalMatrixType H) const override {
Eigen::Matrix<double, T::dimension, T::dimension> H_local;

// If the Rot3 Cayley map is used, Rot3::LocalCoordinates will throw a runtime error
// when asked to compute the Jacobian matrix (see Rot3M.cpp).
#ifdef GTSAM_ROT3_EXPMAP
const Vector full_tangent = T::LocalCoordinates(p, H ? &H_local : nullptr);
#else
// If the Rot3 Cayley map is used, Rot3::LocalCoordinates will throw a runtime
// error when asked to compute the Jacobian matrix (see Rot3M.cpp).
#ifdef GTSAM_ROT3_EXPMAP
Copy link
Member

@dellaert dellaert Sep 25, 2024

Choose a reason for hiding this comment

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

Are we making an assumption here on the type of T ? If so, consider template specialization

const Vector full_tangent =
T::LocalCoordinates(p, H ? &H_local : nullptr);
#else
const Vector full_tangent = T::Logmap(p, H ? &H_local : nullptr);
#endif
#endif

if (H) {
(*H) = Matrix::Zero(indices_.size(), T::dimension);
Expand Down Expand Up @@ -150,7 +155,6 @@ namespace gtsam {
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(prior_);
ar & BOOST_SERIALIZATION_NVP(indices_);
// ar & BOOST_SERIALIZATION_NVP(H_);
}
#endif
}; // \class PartialPriorFactor
Expand Down
133 changes: 100 additions & 33 deletions gtsam_unstable/slam/tests/testPartialPriorFactor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,13 +18,17 @@

#include <CppUnitLite/TestHarness.h>

#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>

using namespace std::placeholders;
using namespace std;
using namespace gtsam;

namespace NM = gtsam::noiseModel;

// Pose3 tangent representation is [ Rx Ry Rz Tx Ty Tz ].
// Pose3 parameter representation is [ Rx Ry Rz Tx Ty Tz ].
static const int kIndexRx = 0;
static const int kIndexRy = 1;
static const int kIndexRz = 2;
Expand Down Expand Up @@ -75,8 +79,15 @@ TEST(PartialPriorFactor, JacobianPartialTranslation2) {
Key poseKey(1);
Pose2 measurement(-13.1, 3.14, -0.73);

#ifdef GTSAM_ROT3_EXPMAP
double prior = Pose2::LocalCoordinates(measurement)(0);
#else
double prior = Pose2::Logmap(measurement)(0);
#endif

// Prior on x component of translation.
TestPartialPriorFactor2 factor(poseKey, 0, measurement.x(), NM::Isotropic::Sigma(1, 0.25));
TestPartialPriorFactor2 factor(poseKey, 0, prior,
NM::Isotropic::Sigma(1, 0.25));

Pose2 pose = measurement; // Zero-error linearization point.

Expand All @@ -86,9 +97,10 @@ TEST(PartialPriorFactor, JacobianPartialTranslation2) {

// Use the factor to calculate the derivative.
Matrix actualH1;
factor.evaluateError(pose, actualH1);
Vector e = factor.evaluateError(pose, actualH1);

// Verify we get the expected error.
// Make sure we get the correct error and Jacobian.
CHECK(assert_equal(Vector1::Zero(), e, 1e-5));
CHECK(assert_equal(expectedH1, actualH1, 1e-5));
}

Expand All @@ -98,7 +110,12 @@ TEST(PartialPriorFactor, JacobianFullTranslation2) {
Pose2 measurement(-6.0, 3.5, 0.123);

// Prior on x component of translation.
TestPartialPriorFactor2 factor(poseKey, {0, 1}, measurement.translation(),
#ifdef GTSAM_ROT3_EXPMAP
Vector2 prior = Pose2::LocalCoordinates(measurement).head<2>();
#else
Vector2 prior = Pose2::Logmap(measurement).head<2>();
#endif
TestPartialPriorFactor2 factor(poseKey, {0, 1}, prior,
NM::Isotropic::Sigma(2, 0.25));

Pose2 pose = measurement; // Zero-error linearization point.
Expand All @@ -109,9 +126,8 @@ TEST(PartialPriorFactor, JacobianFullTranslation2) {

// Use the factor to calculate the derivative.
Matrix actualH1;
factor.evaluateError(pose, actualH1);

// Verify we get the expected error.
Vector e = factor.evaluateError(pose, actualH1);
CHECK(assert_equal(Vector2::Zero(), e, 1e-5));
CHECK(assert_equal(expectedH1, actualH1, 1e-5));
}

Expand All @@ -131,9 +147,8 @@ TEST(PartialPriorFactor, JacobianTheta) {

// Use the factor to calculate the derivative.
Matrix actualH1;
factor.evaluateError(pose, actualH1);

// Verify we get the expected error.
Vector e = factor.evaluateError(pose, actualH1);
CHECK(assert_equal(Vector1::Zero(), e, 1e-5));
CHECK(assert_equal(expectedH1, actualH1, 1e-5));
}

Expand Down Expand Up @@ -182,9 +197,8 @@ TEST(PartialPriorFactor, JacobianAtIdentity3) {

// Use the factor to calculate the derivative.
Matrix actualH1;
factor.evaluateError(pose, actualH1);

// Verify we get the expected error.
Vector e = factor.evaluateError(pose, actualH1);
CHECK(assert_equal(Vector1::Zero(), e, 1e-5));
CHECK(assert_equal(expectedH1, actualH1, 1e-5));
}

Expand All @@ -194,7 +208,8 @@ TEST(PartialPriorFactor, JacobianPartialTranslation3) {
Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
SharedNoiseModel model = NM::Isotropic::Sigma(1, 0.25);

TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), model);
TestPartialPriorFactor3 factor(poseKey, kIndexTy,
Pose3::Logmap(measurement)(4), model);

Pose3 pose = measurement; // Zero-error linearization point.

Expand All @@ -204,20 +219,20 @@ TEST(PartialPriorFactor, JacobianPartialTranslation3) {

// Use the factor to calculate the derivative.
Matrix actualH1;
factor.evaluateError(pose, actualH1);

// Verify we get the expected error.
Vector e = factor.evaluateError(pose, actualH1);
CHECK(assert_equal(Vector1::Zero(), e, 1e-5));
CHECK(assert_equal(expectedH1, actualH1, 1e-5));
}

/* ************************************************************************* */
TEST(PartialPriorFactor, JacobianFullTranslation3) {
Key poseKey(1);
Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
Pose3 measurement(Rot3::RzRyRx(-0.15, 0.30, -0.45), Point3(5.0, -8.0, 11.0));
SharedNoiseModel model = NM::Isotropic::Sigma(3, 0.25);

std::vector<size_t> translationIndices = { kIndexTx, kIndexTy, kIndexTz };
TestPartialPriorFactor3 factor(poseKey, translationIndices, measurement.translation(), model);
TestPartialPriorFactor3 factor(poseKey, translationIndices,
Pose3::Logmap(measurement).tail<3>(), model);

// Create a linearization point at the zero-error point
Pose3 pose = measurement; // Zero-error linearization point.
Expand All @@ -228,40 +243,62 @@ TEST(PartialPriorFactor, JacobianFullTranslation3) {

// Use the factor to calculate the derivative.
Matrix actualH1;
factor.evaluateError(pose, actualH1);

// Verify we get the expected error.
Vector e = factor.evaluateError(pose, actualH1);
CHECK(assert_equal(Vector3::Zero(), e, 1e-5));
CHECK(assert_equal(expectedH1, actualH1, 1e-5));
}

/* ************************************************************************* */
TEST(PartialPriorFactor, JacobianTxTz3) {
Key poseKey(1);
Pose3 measurement(Rot3::RzRyRx(-0.17, 0.567, M_PI), Point3(10.0, -2.3, 3.14));
Pose3 p(Rot3::RzRyRx(-0.17, 0.567, M_PI), Point3(10.0, -2.3, 3.14));
SharedNoiseModel model = NM::Isotropic::Sigma(2, 0.25);

std::vector<size_t> translationIndices = { kIndexTx, kIndexTz };
TestPartialPriorFactor3 factor(poseKey, translationIndices,
(Vector(2) << measurement.x(), measurement.z()).finished(), model);
Vector2 measurement;
measurement << Pose3::Logmap(p)(3), Pose3::Logmap(p)(5);
TestPartialPriorFactor3 factor(poseKey, translationIndices, measurement,
model);

Pose3 pose = measurement; // Zero-error linearization point.
Pose3 pose = p; // Zero-error linearization point.

// Calculate numerical derivatives.
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
[&factor](const Pose3& p) { return factor.evaluateError(p); }, pose);

// Use the factor to calculate the derivative.
Matrix actualH1;
factor.evaluateError(pose, actualH1);
Vector e = factor.evaluateError(pose, actualH1);
// CHECK(assert_equal(Vector2::Zero(), e, 1e-5));
// CHECK(assert_equal(expectedH1, actualH1, 1e-5));
}

/* ************************************************************************* */
TEST(PartialPriorFactor, JacobianPartialRotation3) {
Key poseKey(1);
Pose3 measurement(Rot3::RzRyRx(1.15, -5.30, 0.45), Point3(-1.0, 2.0, -17.0));
SharedNoiseModel model = NM::Isotropic::Sigma(1, 0.25);

// Constrain one axis of rotation.
TestPartialPriorFactor3 factor(poseKey, kIndexRx, Rot3::Logmap(measurement.rotation()).x(), model);

Pose3 pose = measurement; // Zero-error linearization point.

// Verify we get the expected error.
// Calculate numerical derivatives.
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
[&factor](const Pose3& p) { return factor.evaluateError(p); }, pose);

// Use the factor to calculate the derivative.
Matrix actualH1;
Vector e = factor.evaluateError(pose, actualH1);
CHECK(assert_equal(Vector1::Zero(), e, 1e-5));
CHECK(assert_equal(expectedH1, actualH1, 1e-5));
}

/* ************************************************************************* */
TEST(PartialPriorFactor, JacobianFullRotation3) {
Key poseKey(1);
Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
Pose3 measurement(Rot3::RzRyRx(0.15, -3.30, 0.01), Point3(-2.0, 4.0, -0.3));
SharedNoiseModel model = NM::Isotropic::Sigma(3, 0.25);

std::vector<size_t> rotationIndices = { kIndexRx, kIndexRy, kIndexRz };
Expand All @@ -275,12 +312,42 @@ TEST(PartialPriorFactor, JacobianFullRotation3) {

// Use the factor to calculate the derivative.
Matrix actualH1;
factor.evaluateError(pose, actualH1);

// Verify we get the expected error.
Vector e = factor.evaluateError(pose, actualH1);
CHECK(assert_equal(Vector3::Zero(), e, 1e-5));
CHECK(assert_equal(expectedH1, actualH1, 1e-5));
}

/* ************************************************************************* */
TEST(PartialPriorFactor, FactorGraph1) {
Key poseKey(1);

Pose3 pose(Rot3::RzRyRx(-0.17, 0.567, M_PI), Point3(10.0, -2.3, 3.14));
SharedNoiseModel model = NM::Isotropic::Sigma(6, 0.25);

Vector6 prior = Pose3::Logmap(pose);

// By specifying all of the parameter indices, this effectively becomes a PosePriorFactor.
std::vector<size_t> indices = { 0, 1, 2, 3, 4, 5 };
TestPartialPriorFactor3 factor(poseKey, indices, prior, model);

NonlinearFactorGraph graph;
Values initial;
graph.add(factor);

// Get an initial pose with a small error from groundtruth. Make sure that the
// prior factor is able to correct the final result.
Pose3 pose_error(Rot3::RzRyRx(0.3, -0.03, 0.17), Point3(0.2, -0.14, 0.05));
initial.insert(poseKey, pose_error * pose);
// initial.print("Initial values:\n");

Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
// result.print("Final Result:\n");
Pose3 pose_optimized = result.at<Pose3>(poseKey);

CHECK(assert_equal(pose, pose_optimized, 1e-5));
}


/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */