diff --git a/nonlinear/NonlinearConstraint.h b/nonlinear/NonlinearConstraint.h index 5359d048c..38730f5c7 100644 --- a/nonlinear/NonlinearConstraint.h +++ b/nonlinear/NonlinearConstraint.h @@ -88,7 +88,7 @@ public: * @param config is the values structure * @return a combined linear factor containing both the constraint and the constraint factor */ - virtual boost::shared_ptr linearize(const Values& c) const=0; + virtual boost::shared_ptr linearize(const Values& c, const Ordering& ordering) const=0; }; @@ -147,22 +147,30 @@ public: } /** Linearize from config */ - boost::shared_ptr linearize(const Values& c, const Ordering& ordering) const { - if (!active(c)) { + boost::shared_ptr linearize(const Values& x, const Ordering& ordering) const { + if (!active(x)) { boost::shared_ptr factor; return factor; } - const Key& j = key_; - const X& x = c[j]; - Matrix grad; - Vector g = -1.0 * evaluateError(x, grad); + const X& xj = x[key_]; + Matrix A; + Vector b = - evaluateError(xj, A); + varid_t var = ordering[key_]; SharedDiagonal model = noiseModel::Constrained::All(this->dim()); - return GaussianFactor::shared_ptr(new GaussianFactor(ordering[this->key_], grad, g, model)); + return GaussianFactor::shared_ptr(new GaussianFactor(var, A, b, model)); } /** g(x) with optional derivative - does not depend on active */ virtual Vector evaluateError(const X& x, boost::optional H = boost::none) const = 0; + + /** + * Create a symbolic factor using the given ordering to determine the + * variable indices. + */ + virtual Factor::shared_ptr symbolic(const Ordering& ordering) const { + return Factor::shared_ptr(new Factor(ordering[key_])); + } }; /** @@ -259,13 +267,29 @@ public: Matrix grad1, grad2; Vector g = -1.0 * evaluateError(x1, x2, grad1, grad2); SharedDiagonal model = noiseModel::Constrained::All(this->dim()); - return GaussianFactor::shared_ptr(new GaussianFactor(ordering[j1], grad1, ordering[j2], grad2, g, model)); + varid_t var1 = ordering[j1], var2 = ordering[j2]; + if (var1 < var2) + GaussianFactor::shared_ptr(new GaussianFactor(var1, grad1, var2, grad2, g, model)); + else + GaussianFactor::shared_ptr(new GaussianFactor(var2, grad2, var1, grad1, g, model)); } /** g(x) with optional derivative2 - does not depend on active */ virtual Vector evaluateError(const X1& x1, const X2& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const = 0; + + /** + * Create a symbolic factor using the given ordering to determine the + * variable indices. + */ + virtual Factor::shared_ptr symbolic(const Ordering& ordering) const { + const varid_t var1 = ordering[key1_], var2 = ordering[key2_]; + if(var1 < var2) + return Factor::shared_ptr(new Factor(var1, var2)); + else + return Factor::shared_ptr(new Factor(var2, var1)); + } }; /** @@ -362,17 +386,37 @@ public: } /** Linearize from config */ - boost::shared_ptr linearize(const Values& c) const { + boost::shared_ptr linearize(const Values& c, const Ordering& ordering) const { if (!active(c)) { boost::shared_ptr factor; return factor; } const Key1& j1 = key1_; const Key2& j2 = key2_; const Key3& j3 = key3_; const X1& x1 = c[j1]; const X2& x2 = c[j2]; const X3& x3 = c[j3]; - Matrix grad1, grad2, grad3; - Vector g = -1.0 * evaluateError(x1, x2, x3, grad1, grad2, grad3); + Matrix A1, A2, A3; + Vector b = -1.0 * evaluateError(x1, x2, x3, A1, A2, A3); SharedDiagonal model = noiseModel::Constrained::All(this->dim()); - return GaussianFactor::shared_ptr(new GaussianFactor(j1, grad1, j2, grad2, j3, grad3, g, model)); + varid_t var1 = ordering[j1], var2 = ordering[j2], var3 = ordering[j3]; + + // perform sorting + if(var1 < var2 && var2 < var3) + return GaussianFactor::shared_ptr( + new GaussianFactor(var1, A1, var2, A2, var3, A3, b, model)); + else if(var2 < var1 && var1 < var3) + return GaussianFactor::shared_ptr( + new GaussianFactor(var2, A2, var1, A1, var3, A3, b, model)); + else if(var1 < var3 && var3 < var2) + return GaussianFactor::shared_ptr( + new GaussianFactor(var1, A1, var3, A3, var2, A2, b, model)); + else if(var2 < var3 && var3 < var1) + return GaussianFactor::shared_ptr( + new GaussianFactor(var2, A2, var3, A3, var1, A1, b, model)); + else if(var3 < var1 && var1 < var2) + return GaussianFactor::shared_ptr( + new GaussianFactor(var3, A3, var1, A1, var2, A2, b, model)); + else + return GaussianFactor::shared_ptr( + new GaussianFactor(var3, A3, var2, A2, var1, A1, b, model)); } /** g(x) with optional derivative3 - does not depend on active */ @@ -380,6 +424,26 @@ public: boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const = 0; + + /** + * Create a symbolic factor using the given ordering to determine the + * variable indices. + */ + virtual Factor::shared_ptr symbolic(const Ordering& ordering) const { + const varid_t var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_]; + if(var1 < var2 && var2 < var3) + return Factor::shared_ptr(new Factor(ordering[key1_], ordering[key2_], ordering[key3_])); + else if(var2 < var1 && var1 < var3) + return Factor::shared_ptr(new Factor(ordering[key2_], ordering[key2_], ordering[key3_])); + else if(var1 < var3 && var3 < var2) + return Factor::shared_ptr(new Factor(ordering[key1_], ordering[key3_], ordering[key2_])); + else if(var2 < var3 && var3 < var1) + return Factor::shared_ptr(new Factor(ordering[key2_], ordering[key3_], ordering[key1_])); + else if(var3 < var1 && var1 < var2) + return Factor::shared_ptr(new Factor(ordering[key3_], ordering[key1_], ordering[key2_])); + else + return Factor::shared_ptr(new Factor(ordering[key3_], ordering[key2_], ordering[key1_])); + } }; /** diff --git a/nonlinear/NonlinearFactor.h b/nonlinear/NonlinearFactor.h index 17404c3bf..d452816ef 100644 --- a/nonlinear/NonlinearFactor.h +++ b/nonlinear/NonlinearFactor.h @@ -22,10 +22,13 @@ #include #include -#define INSTANTIATE_NONLINEAR_FACTOR1(C,J,X) \ - template class gtsam::NonlinearFactor1; -#define INSTANTIATE_NONLINEAR_FACTOR2(C,J1,X1,J2,X2) \ - template class gtsam::NonlinearFactor2; +// FIXME: is this necessary? These don't even fit the right format +#define INSTANTIATE_NONLINEAR_FACTOR1(C,J) \ + template class gtsam::NonlinearFactor1; +#define INSTANTIATE_NONLINEAR_FACTOR2(C,J1,J2) \ + template class gtsam::NonlinearFactor2; +#define INSTANTIATE_NONLINEAR_FACTOR3(C,J1,J2,J3) \ + template class gtsam::NonlinearFactor3; namespace gtsam { diff --git a/tests/Makefile.am b/tests/Makefile.am index a004cc6db..0a8f43949 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -19,8 +19,10 @@ check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorG check_PROGRAMS += testNonlinearOptimizer check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph check_PROGRAMS += testTupleValues -#check_PROGRAMS += testNonlinearEqualityConstraint testBoundingConstraint -#check_PROGRAMS += testTransformConstraint testLinearApproxFactor +#check_PROGRAMS += testNonlinearEqualityConstraint +#check_PROGRAMS += testBoundingConstraint +check_PROGRAMS += testTransformConstraint +#check_PROGRAMS += testLinearApproxFactor # only if serialization is available if ENABLE_SERIALIZATION diff --git a/tests/testLinearApproxFactor.cpp b/tests/testLinearApproxFactor.cpp index 55e7e5281..ed3f75324 100644 --- a/tests/testLinearApproxFactor.cpp +++ b/tests/testLinearApproxFactor.cpp @@ -22,7 +22,7 @@ TEST ( LinearApproxFactor, basic ) { Matrix A1 = eye(2); Vector b = repeat(2, 1.2); SharedDiagonal model = noiseModel::Unit::Create(2); - GaussianFactor::shared_ptr lin_factor(new GaussianFactor(key1, A1, b, model)); + GaussianFactor::shared_ptr lin_factor(new GaussianFactor(1, A1, b, model)); planarSLAM::Values lin_points; ApproxFactor f1(lin_factor, lin_points); @@ -32,7 +32,9 @@ TEST ( LinearApproxFactor, basic ) { EXPECT(assert_equal(expKeyVec, f1.nonlinearKeys())); planarSLAM::Values config; // doesn't really need to have any data - GaussianFactor::shared_ptr actual = f1.linearize(config); + Ordering ordering; + ordering.push_back(key1); + GaussianFactor::shared_ptr actual = f1.linearize(config, ordering); // Check the linearization CHECK(assert_equal(*lin_factor, *actual));