diff --git a/gtsam/linear/GaussianISAM.cpp b/gtsam/linear/GaussianISAM.cpp new file mode 100644 index 000000000..dffc532c7 --- /dev/null +++ b/gtsam/linear/GaussianISAM.cpp @@ -0,0 +1,31 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SymbolicISAM.cpp + * @date July 29, 2013 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include + +namespace gtsam { + + /* ************************************************************************* */ + GaussianISAM::GaussianISAM() {} + + /* ************************************************************************* */ + GaussianISAM::GaussianISAM(const GaussianBayesTree& bayesTree) : + Base(bayesTree) {} + +} diff --git a/gtsam/linear/GaussianISAM.h b/gtsam/linear/GaussianISAM.h new file mode 100644 index 000000000..24cf2a95d --- /dev/null +++ b/gtsam/linear/GaussianISAM.h @@ -0,0 +1,46 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SymbolicISAM.h + * @date July 29, 2013 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#pragma once + +#include +#include + +namespace gtsam { + + class GTSAM_EXPORT GaussianISAM : public ISAM + { + public: + typedef ISAM Base; + typedef GaussianISAM This; + typedef boost::shared_ptr shared_ptr; + + /// @name Standard Constructors + /// @{ + + /** Create an empty Bayes Tree */ + GaussianISAM(); + + /** Copy constructor */ + GaussianISAM(const GaussianBayesTree& bayesTree); + + /// @} + + }; + +} diff --git a/tests/smallExample.h b/tests/smallExample.h index 1a1c17fce..05405f651 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -30,13 +30,13 @@ namespace gtsam { namespace example { namespace { -typedef NonlinearFactorGraph Graph; +typedef NonlinearFactorGraph NonlinearFactorGraph; /** * Create small example for non-linear factor graph */ -boost::shared_ptr sharedNonlinearFactorGraph(); -Graph createNonlinearFactorGraph(); +boost::shared_ptr sharedNonlinearFactorGraph(); +NonlinearFactorGraph createNonlinearFactorGraph(); /** * Create values structure to go with it @@ -45,7 +45,7 @@ Graph createNonlinearFactorGraph(); Values createValues(); /** Vector Values equivalent */ -VectorValuesOrdered createVectorValues(); +VectorValues createVectorValues(); /** * create a noisy values structure for a nonlinear factor graph @@ -56,42 +56,42 @@ Values createNoisyValues(); /** * Zero delta config */ -VectorValuesOrdered createZeroDelta(const OrderingOrdered& ordering); +VectorValues createZeroDelta(const Ordering& ordering); /** * Delta config that, when added to noisyValues, returns the ground truth */ -VectorValuesOrdered createCorrectDelta(const OrderingOrdered& ordering); +VectorValues createCorrectDelta(const Ordering& ordering); /** * create a linear factor graph * The non-linear graph above evaluated at NoisyValues */ -GaussianFactorGraphOrdered createGaussianFactorGraph(const OrderingOrdered& ordering); +GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering); /** * create small Chordal Bayes Net x <- y */ -GaussianBayesNetOrdered createSmallGaussianBayesNet(); +GaussianBayesNet createSmallGaussianBayesNet(); /** * Create really non-linear factor graph (cos/sin) */ -boost::shared_ptr +boost::shared_ptr sharedReallyNonlinearFactorGraph(); -Graph createReallyNonlinearFactorGraph(); +NonlinearFactorGraph createReallyNonlinearFactorGraph(); /** * Create a full nonlinear smoother * @param T number of time-steps */ -std::pair createNonlinearSmoother(int T); +std::pair createNonlinearSmoother(int T); /** * Create a Kalman smoother by linearizing a non-linear factor graph * @param T number of time-steps */ -std::pair, OrderingOrdered> createSmoother(int T, boost::optional ordering = boost::none); +std::pair createSmoother(int T, boost::optional ordering = boost::none); /* ******************************************************* */ // Linear Constrained Examples @@ -101,22 +101,22 @@ std::pair, OrderingOrdered> createSmoo * Creates a simple constrained graph with one linear factor and * one binary equality constraint that sets x = y */ -GaussianFactorGraphOrdered createSimpleConstraintGraph(); -VectorValuesOrdered createSimpleConstraintValues(); +GaussianFactorGraph createSimpleConstraintGraph(); +VectorValues createSimpleConstraintValues(); /** * Creates a simple constrained graph with one linear factor and * one binary constraint. */ -GaussianFactorGraphOrdered createSingleConstraintGraph(); -VectorValuesOrdered createSingleConstraintValues(); +GaussianFactorGraph createSingleConstraintGraph(); +VectorValues createSingleConstraintValues(); /** * Creates a constrained graph with a linear factor and two * binary constraints that share a node */ -GaussianFactorGraphOrdered createMultiConstraintGraph(); -VectorValuesOrdered createMultiConstraintValues(); +GaussianFactorGraph createMultiConstraintGraph(); +VectorValues createMultiConstraintValues(); /* ******************************************************* */ // Planar graph with easy subtree for SubgraphPreconditioner @@ -131,14 +131,14 @@ VectorValuesOrdered createMultiConstraintValues(); * -x11-x21-x31 * with x11 clamped at (1,1), and others related by 2D odometry. */ -boost::tuple planarGraph(size_t N); +boost::tuple planarGraph(size_t N); /* * Create canonical ordering for planar graph that also works for tree * With x11 the root, e.g. for N=3 * x33 x23 x13 x32 x22 x12 x31 x21 x11 */ -OrderingOrdered planarOrdering(size_t N); +Ordering planarOrdering(size_t N); /* * Split graph into tree and loop closing constraints, e.g., with N=3 @@ -148,8 +148,8 @@ OrderingOrdered planarOrdering(size_t N); * | * -x11-x21-x31 */ -std::pair splitOffPlanarTree( - size_t N, const GaussianFactorGraphOrdered& original); +std::pair splitOffPlanarTree( + size_t N, const GaussianFactorGraph& original); @@ -165,19 +165,19 @@ static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2); static SharedDiagonal constraintModel = noiseModel::Constrained::All(2); -static const Index _l1_=0, _x1_=1, _x2_=2; -static const Index _x_=0, _y_=1, _z_=2; +static const Key _l1_=0, _x1_=1, _x2_=2; +static const Key _x_=0, _y_=1, _z_=2; } // \namespace impl /* ************************************************************************* */ -boost::shared_ptr sharedNonlinearFactorGraph() { +boost::shared_ptr sharedNonlinearFactorGraph() { using namespace impl; using symbol_shorthand::X; using symbol_shorthand::L; // Create - boost::shared_ptr nlfg( - new Graph); + boost::shared_ptr nlfg( + new NonlinearFactorGraph); // prior on x1 Point2 mu; @@ -203,7 +203,7 @@ boost::shared_ptr sharedNonlinearFactorGraph() { } /* ************************************************************************* */ -Graph createNonlinearFactorGraph() { +NonlinearFactorGraph createNonlinearFactorGraph() { return *sharedNonlinearFactorGraph(); } @@ -219,9 +219,9 @@ Values createValues() { } /* ************************************************************************* */ -VectorValuesOrdered createVectorValues() { +VectorValues createVectorValues() { using namespace impl; - VectorValuesOrdered c(std::vector(3, 2)); + VectorValues c(std::vector(3, 2)); c[_l1_] = Vector_(2, 0.0, -1.0); c[_x1_] = Vector_(2, 0.0, 0.0); c[_x2_] = Vector_(2, 1.5, 0.0); @@ -245,47 +245,47 @@ Values createNoisyValues() { } /* ************************************************************************* */ -VectorValuesOrdered createCorrectDelta(const OrderingOrdered& ordering) { +VectorValues createCorrectDelta() { using symbol_shorthand::X; using symbol_shorthand::L; - VectorValuesOrdered c(std::vector(3,2)); - c[ordering[L(1)]] = Vector_(2, -0.1, 0.1); - c[ordering[X(1)]] = Vector_(2, -0.1, -0.1); - c[ordering[X(2)]] = Vector_(2, 0.1, -0.2); + VectorValues c; + c.insert(L(1), Vector_(2, -0.1, 0.1)); + c.insert(X(1), Vector_(2, -0.1, -0.1)); + c.insert(X(2), Vector_(2, 0.1, -0.2)); return c; } /* ************************************************************************* */ -VectorValuesOrdered createZeroDelta(const OrderingOrdered& ordering) { +VectorValues createZeroDelta() { using symbol_shorthand::X; using symbol_shorthand::L; - VectorValuesOrdered c(std::vector(3,2)); - c[ordering[L(1)]] = zero(2); - c[ordering[X(1)]] = zero(2); - c[ordering[X(2)]] = zero(2); + VectorValues c(std::vector(3,2)); + c.insert(L(1), zero(2)); + c.insert(X(1), zero(2)); + c.insert(X(2), zero(2)); return c; } /* ************************************************************************* */ -GaussianFactorGraphOrdered createGaussianFactorGraph(const OrderingOrdered& ordering) { +GaussianFactorGraph createGaussianFactorGraph() { using symbol_shorthand::X; using symbol_shorthand::L; // Create empty graph - GaussianFactorGraphOrdered fg; + GaussianFactorGraph fg; SharedDiagonal unit2 = noiseModel::Unit::Create(2); // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg.push_back(boost::make_shared(ordering[X(1)], 10*eye(2), -1.0*ones(2), unit2)); + fg += JacobianFactor(X(1), 10*eye(2), -1.0*ones(2), unit2); // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg.push_back(boost::make_shared(ordering[X(1)], -10*eye(2),ordering[X(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2)); + fg += JacobianFactor(X(1), -10*eye(2), X(2), 10*eye(2), Vector_(2, 2.0, -1.0), unit2); // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg.push_back(boost::make_shared(ordering[X(1)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2)); + fg += JacobianFactor(X(1), -5*eye(2), L(1), 5*eye(2), Vector_(2, 0.0, 1.0), unit2); // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg.push_back(boost::make_shared(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2)); + fg += JacobianFactor(X(2), -5*eye(2), L(1), 5*eye(2), Vector_(2, -1.0, 1.5), unit2); return fg; } @@ -296,20 +296,18 @@ GaussianFactorGraphOrdered createGaussianFactorGraph(const OrderingOrdered& orde * 1 1 9 * 1 5 */ -GaussianBayesNetOrdered createSmallGaussianBayesNet() { +GaussianBayesNet createSmallGaussianBayesNet() { using namespace impl; Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0); Matrix R22 = Matrix_(1, 1, 1.0); Vector d1(1), d2(1); d1(0) = 9; d2(0) = 5; - Vector tau(1); - tau(0) = 1.0; // define nodes and specify in reverse topological sort (i.e. parents last) - GaussianConditionalOrdered::shared_ptr Px_y(new GaussianConditionalOrdered(_x_, d1, R11, _y_, S12, tau)); - GaussianConditionalOrdered::shared_ptr Py(new GaussianConditionalOrdered(_y_, d2, R22, tau)); - GaussianBayesNetOrdered cbn; + GaussianConditional::shared_ptr Px_y(new GaussianConditional(_x_, d1, R11, _y_, S12)); + GaussianConditional::shared_ptr Py(new GaussianConditional(_y_, d2, R22)); + GaussianBayesNet cbn; cbn.push_back(Px_y); cbn.push_back(Py); @@ -349,10 +347,10 @@ struct UnaryFactor: public gtsam::NoiseModelFactor1 { } /* ************************************************************************* */ -boost::shared_ptr sharedReallyNonlinearFactorGraph() { +boost::shared_ptr sharedReallyNonlinearFactorGraph() { using symbol_shorthand::X; using symbol_shorthand::L; - boost::shared_ptr fg(new Graph); + boost::shared_ptr fg(new NonlinearFactorGraph); Vector z = Vector_(2, 1.0, 0.0); double sigma = 0.1; boost::shared_ptr factor( @@ -361,18 +359,18 @@ boost::shared_ptr sharedReallyNonlinearFactorGraph() { return fg; } -Graph createReallyNonlinearFactorGraph() { +NonlinearFactorGraph createReallyNonlinearFactorGraph() { return *sharedReallyNonlinearFactorGraph(); } /* ************************************************************************* */ -std::pair createNonlinearSmoother(int T) { +std::pair createNonlinearSmoother(int T) { using namespace impl; using symbol_shorthand::X; using symbol_shorthand::L; // Create - Graph nlfg; + NonlinearFactorGraph nlfg; Values poses; // prior on x1 @@ -400,8 +398,8 @@ std::pair createNonlinearSmoother(int T) { } /* ************************************************************************* */ -std::pair, OrderingOrdered> createSmoother(int T, boost::optional ordering) { - Graph nlfg; +std::pair createSmoother(int T) { + NonlinearFactorGraph nlfg; Values poses; boost::tie(nlfg, poses) = createNonlinearSmoother(T); @@ -410,7 +408,7 @@ std::pair, OrderingOrdered> createSmoo } /* ************************************************************************* */ -GaussianFactorGraphOrdered createSimpleConstraintGraph() { +GaussianFactorGraph createSimpleConstraintGraph() { using namespace impl; // create unary factor // prior on _x_, mean = [1,-1], sigma=0.1 @@ -418,7 +416,7 @@ GaussianFactorGraphOrdered createSimpleConstraintGraph() { Vector b1(2); b1(0) = 1.0; b1(1) = -1.0; - JacobianFactorOrdered::shared_ptr f1(new JacobianFactorOrdered(_x_, Ax, b1, sigma0_1)); + JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1)); // create binary constraint factor // between _x_ and _y_, that is going to be the only factor on _y_ @@ -427,11 +425,11 @@ GaussianFactorGraphOrdered createSimpleConstraintGraph() { Matrix Ax1 = eye(2); Matrix Ay1 = eye(2) * -1; Vector b2 = Vector_(2, 0.0, 0.0); - JacobianFactorOrdered::shared_ptr f2(new JacobianFactorOrdered(_x_, Ax1, _y_, Ay1, b2, + JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, constraintModel)); // construct the graph - GaussianFactorGraphOrdered fg; + GaussianFactorGraph fg; fg.push_back(f1); fg.push_back(f2); @@ -439,11 +437,11 @@ GaussianFactorGraphOrdered createSimpleConstraintGraph() { } /* ************************************************************************* */ -VectorValuesOrdered createSimpleConstraintValues() { +VectorValues createSimpleConstraintValues() { using namespace impl; using symbol_shorthand::X; using symbol_shorthand::L; - VectorValuesOrdered config(std::vector(2,2)); + VectorValues config(std::vector(2,2)); Vector v = Vector_(2, 1.0, -1.0); config[_x_] = v; config[_y_] = v; @@ -451,7 +449,7 @@ VectorValuesOrdered createSimpleConstraintValues() { } /* ************************************************************************* */ -GaussianFactorGraphOrdered createSingleConstraintGraph() { +GaussianFactorGraph createSingleConstraintGraph() { using namespace impl; // create unary factor // prior on _x_, mean = [1,-1], sigma=0.1 @@ -459,8 +457,8 @@ GaussianFactorGraphOrdered createSingleConstraintGraph() { Vector b1(2); b1(0) = 1.0; b1(1) = -1.0; - //GaussianFactorOrdered::shared_ptr f1(new JacobianFactor(_x_, sigma0_1->Whiten(Ax), sigma0_1->whiten(b1), sigma0_1)); - JacobianFactorOrdered::shared_ptr f1(new JacobianFactorOrdered(_x_, Ax, b1, sigma0_1)); + //GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, sigma0_1->Whiten(Ax), sigma0_1->whiten(b1), sigma0_1)); + JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1)); // create binary constraint factor // between _x_ and _y_, that is going to be the only factor on _y_ @@ -473,11 +471,11 @@ GaussianFactorGraphOrdered createSingleConstraintGraph() { Ax1(1, 1) = 1.0; Matrix Ay1 = eye(2) * 10; Vector b2 = Vector_(2, 1.0, 2.0); - JacobianFactorOrdered::shared_ptr f2(new JacobianFactorOrdered(_x_, Ax1, _y_, Ay1, b2, + JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, constraintModel)); // construct the graph - GaussianFactorGraphOrdered fg; + GaussianFactorGraph fg; fg.push_back(f1); fg.push_back(f2); @@ -485,21 +483,21 @@ GaussianFactorGraphOrdered createSingleConstraintGraph() { } /* ************************************************************************* */ -VectorValuesOrdered createSingleConstraintValues() { +VectorValues createSingleConstraintValues() { using namespace impl; - VectorValuesOrdered config(std::vector(2,2)); + VectorValues config(std::vector(2,2)); config[_x_] = Vector_(2, 1.0, -1.0); config[_y_] = Vector_(2, 0.2, 0.1); return config; } /* ************************************************************************* */ -GaussianFactorGraphOrdered createMultiConstraintGraph() { +GaussianFactorGraph createMultiConstraintGraph() { using namespace impl; // unary factor 1 Matrix A = eye(2); Vector b = Vector_(2, -2.0, 2.0); - JacobianFactorOrdered::shared_ptr lf1(new JacobianFactorOrdered(_x_, A, b, sigma0_1)); + JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1)); // constraint 1 Matrix A11(2, 2); @@ -517,7 +515,7 @@ GaussianFactorGraphOrdered createMultiConstraintGraph() { Vector b1(2); b1(0) = 1.0; b1(1) = 2.0; - JacobianFactorOrdered::shared_ptr lc1(new JacobianFactorOrdered(_x_, A11, _y_, A12, b1, + JacobianFactor::shared_ptr lc1(new JacobianFactor(_x_, A11, _y_, A12, b1, constraintModel)); // constraint 2 @@ -536,11 +534,11 @@ GaussianFactorGraphOrdered createMultiConstraintGraph() { Vector b2(2); b2(0) = 3.0; b2(1) = 4.0; - JacobianFactorOrdered::shared_ptr lc2(new JacobianFactorOrdered(_x_, A21, _z_, A22, b2, + JacobianFactor::shared_ptr lc2(new JacobianFactor(_x_, A21, _z_, A22, b2, constraintModel)); // construct the graph - GaussianFactorGraphOrdered fg; + GaussianFactorGraph fg; fg.push_back(lf1); fg.push_back(lc1); fg.push_back(lc2); @@ -549,9 +547,9 @@ GaussianFactorGraphOrdered createMultiConstraintGraph() { } /* ************************************************************************* */ -VectorValuesOrdered createMultiConstraintValues() { +VectorValues createMultiConstraintValues() { using namespace impl; - VectorValuesOrdered config(std::vector(3,2)); + VectorValues config(std::vector(3,2)); config[_x_] = Vector_(2, -2.0, 2.0); config[_y_] = Vector_(2, -0.1, 0.4); config[_z_] = Vector_(2, -4.0, 5.0); @@ -568,7 +566,7 @@ Symbol key(int x, int y) { } // \namespace impl /* ************************************************************************* */ -boost::tuple planarGraph(size_t N) { +boost::tuple planarGraph(size_t N) { using namespace impl; // create empty graph @@ -599,20 +597,20 @@ boost::tuple planarGraph(size_t for (size_t x = 1; x <= N; x++) for (size_t y = 1; y <= N; y++) zeros.insert(key(x, y), Point2()); - OrderingOrdered ordering(planarOrdering(N)); - VectorValuesOrdered xtrue(zeros.dims(ordering)); + Ordering ordering(planarOrdering(N)); + VectorValues xtrue(zeros.dims(ordering)); for (size_t x = 1; x <= N; x++) for (size_t y = 1; y <= N; y++) xtrue[ordering[key(x, y)]] = Point2(x,y).vector(); // linearize around zero - boost::shared_ptr gfg = nlfg.linearize(zeros, ordering); + boost::shared_ptr gfg = nlfg.linearize(zeros, ordering); return boost::make_tuple(*gfg, xtrue); } /* ************************************************************************* */ -OrderingOrdered planarOrdering(size_t N) { - OrderingOrdered ordering; +Ordering planarOrdering(size_t N) { + Ordering ordering; for (size_t y = N; y >= 1; y--) for (size_t x = N; x >= 1; x--) ordering.push_back(impl::key(x, y)); @@ -620,9 +618,9 @@ OrderingOrdered planarOrdering(size_t N) { } /* ************************************************************************* */ -std::pair splitOffPlanarTree(size_t N, - const GaussianFactorGraphOrdered& original) { - GaussianFactorGraphOrdered T, C; +std::pair splitOffPlanarTree(size_t N, + const GaussianFactorGraph& original) { + GaussianFactorGraph T, C; // Add the x11 constraint to the tree T.push_back(original[0]); diff --git a/tests/smallExampleOrdered.h b/tests/smallExampleOrdered.h new file mode 100644 index 000000000..1a1c17fce --- /dev/null +++ b/tests/smallExampleOrdered.h @@ -0,0 +1,651 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file smallExample.h + * @brief Create small example with two poses and one landmark + * @brief smallExample + * @author Carlos Nieto + */ + +// \callgraph + + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { +namespace example { + namespace { + +typedef NonlinearFactorGraph Graph; + +/** + * Create small example for non-linear factor graph + */ +boost::shared_ptr sharedNonlinearFactorGraph(); +Graph createNonlinearFactorGraph(); + +/** + * Create values structure to go with it + * The ground truth values structure for the example above + */ +Values createValues(); + +/** Vector Values equivalent */ +VectorValuesOrdered createVectorValues(); + +/** + * create a noisy values structure for a nonlinear factor graph + */ +boost::shared_ptr sharedNoisyValues(); +Values createNoisyValues(); + +/** + * Zero delta config + */ +VectorValuesOrdered createZeroDelta(const OrderingOrdered& ordering); + +/** + * Delta config that, when added to noisyValues, returns the ground truth + */ +VectorValuesOrdered createCorrectDelta(const OrderingOrdered& ordering); + +/** + * create a linear factor graph + * The non-linear graph above evaluated at NoisyValues + */ +GaussianFactorGraphOrdered createGaussianFactorGraph(const OrderingOrdered& ordering); + +/** + * create small Chordal Bayes Net x <- y + */ +GaussianBayesNetOrdered createSmallGaussianBayesNet(); + +/** + * Create really non-linear factor graph (cos/sin) + */ +boost::shared_ptr +sharedReallyNonlinearFactorGraph(); +Graph createReallyNonlinearFactorGraph(); + +/** + * Create a full nonlinear smoother + * @param T number of time-steps + */ +std::pair createNonlinearSmoother(int T); + +/** + * Create a Kalman smoother by linearizing a non-linear factor graph + * @param T number of time-steps + */ +std::pair, OrderingOrdered> createSmoother(int T, boost::optional ordering = boost::none); + +/* ******************************************************* */ +// Linear Constrained Examples +/* ******************************************************* */ + +/** + * Creates a simple constrained graph with one linear factor and + * one binary equality constraint that sets x = y + */ +GaussianFactorGraphOrdered createSimpleConstraintGraph(); +VectorValuesOrdered createSimpleConstraintValues(); + +/** + * Creates a simple constrained graph with one linear factor and + * one binary constraint. + */ +GaussianFactorGraphOrdered createSingleConstraintGraph(); +VectorValuesOrdered createSingleConstraintValues(); + +/** + * Creates a constrained graph with a linear factor and two + * binary constraints that share a node + */ +GaussianFactorGraphOrdered createMultiConstraintGraph(); +VectorValuesOrdered createMultiConstraintValues(); + +/* ******************************************************* */ +// Planar graph with easy subtree for SubgraphPreconditioner +/* ******************************************************* */ + +/* + * Create factor graph with N^2 nodes, for example for N=3 + * x13-x23-x33 + * | | | + * x12-x22-x32 + * | | | + * -x11-x21-x31 + * with x11 clamped at (1,1), and others related by 2D odometry. + */ +boost::tuple planarGraph(size_t N); + +/* + * Create canonical ordering for planar graph that also works for tree + * With x11 the root, e.g. for N=3 + * x33 x23 x13 x32 x22 x12 x31 x21 x11 + */ +OrderingOrdered planarOrdering(size_t N); + +/* + * Split graph into tree and loop closing constraints, e.g., with N=3 + * x13-x23-x33 + * | + * x12-x22-x32 + * | + * -x11-x21-x31 + */ +std::pair splitOffPlanarTree( + size_t N, const GaussianFactorGraphOrdered& original); + + + +// Implementations + +// using namespace gtsam::noiseModel; + +namespace impl { +typedef boost::shared_ptr shared_nlf; + +static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0); +static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); +static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2); +static SharedDiagonal constraintModel = noiseModel::Constrained::All(2); + +static const Index _l1_=0, _x1_=1, _x2_=2; +static const Index _x_=0, _y_=1, _z_=2; +} // \namespace impl + + +/* ************************************************************************* */ +boost::shared_ptr sharedNonlinearFactorGraph() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + // Create + boost::shared_ptr nlfg( + new Graph); + + // prior on x1 + Point2 mu; + shared_nlf f1(new simulated2D::Prior(mu, sigma0_1, X(1))); + nlfg->push_back(f1); + + // odometry between x1 and x2 + Point2 z2(1.5, 0); + shared_nlf f2(new simulated2D::Odometry(z2, sigma0_1, X(1), X(2))); + nlfg->push_back(f2); + + // measurement between x1 and l1 + Point2 z3(0, -1); + shared_nlf f3(new simulated2D::Measurement(z3, sigma0_2, X(1), L(1))); + nlfg->push_back(f3); + + // measurement between x2 and l1 + Point2 z4(-1.5, -1.); + shared_nlf f4(new simulated2D::Measurement(z4, sigma0_2, X(2), L(1))); + nlfg->push_back(f4); + + return nlfg; +} + +/* ************************************************************************* */ +Graph createNonlinearFactorGraph() { + return *sharedNonlinearFactorGraph(); +} + +/* ************************************************************************* */ +Values createValues() { + using symbol_shorthand::X; + using symbol_shorthand::L; + Values c; + c.insert(X(1), Point2(0.0, 0.0)); + c.insert(X(2), Point2(1.5, 0.0)); + c.insert(L(1), Point2(0.0, -1.0)); + return c; +} + +/* ************************************************************************* */ +VectorValuesOrdered createVectorValues() { + using namespace impl; + VectorValuesOrdered c(std::vector(3, 2)); + c[_l1_] = Vector_(2, 0.0, -1.0); + c[_x1_] = Vector_(2, 0.0, 0.0); + c[_x2_] = Vector_(2, 1.5, 0.0); + return c; +} + +/* ************************************************************************* */ +boost::shared_ptr sharedNoisyValues() { + using symbol_shorthand::X; + using symbol_shorthand::L; + boost::shared_ptr c(new Values); + c->insert(X(1), Point2(0.1, 0.1)); + c->insert(X(2), Point2(1.4, 0.2)); + c->insert(L(1), Point2(0.1, -1.1)); + return c; +} + +/* ************************************************************************* */ +Values createNoisyValues() { + return *sharedNoisyValues(); +} + +/* ************************************************************************* */ +VectorValuesOrdered createCorrectDelta(const OrderingOrdered& ordering) { + using symbol_shorthand::X; + using symbol_shorthand::L; + VectorValuesOrdered c(std::vector(3,2)); + c[ordering[L(1)]] = Vector_(2, -0.1, 0.1); + c[ordering[X(1)]] = Vector_(2, -0.1, -0.1); + c[ordering[X(2)]] = Vector_(2, 0.1, -0.2); + return c; +} + +/* ************************************************************************* */ +VectorValuesOrdered createZeroDelta(const OrderingOrdered& ordering) { + using symbol_shorthand::X; + using symbol_shorthand::L; + VectorValuesOrdered c(std::vector(3,2)); + c[ordering[L(1)]] = zero(2); + c[ordering[X(1)]] = zero(2); + c[ordering[X(2)]] = zero(2); + return c; +} + +/* ************************************************************************* */ +GaussianFactorGraphOrdered createGaussianFactorGraph(const OrderingOrdered& ordering) { + using symbol_shorthand::X; + using symbol_shorthand::L; + // Create empty graph + GaussianFactorGraphOrdered fg; + + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + + // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] + fg.push_back(boost::make_shared(ordering[X(1)], 10*eye(2), -1.0*ones(2), unit2)); + + // odometry between x1 and x2: x2-x1=[0.2;-0.1] + fg.push_back(boost::make_shared(ordering[X(1)], -10*eye(2),ordering[X(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2)); + + // measurement between x1 and l1: l1-x1=[0.0;0.2] + fg.push_back(boost::make_shared(ordering[X(1)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2)); + + // measurement between x2 and l1: l1-x2=[-0.2;0.3] + fg.push_back(boost::make_shared(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2)); + + return fg; +} + +/* ************************************************************************* */ +/** create small Chordal Bayes Net x <- y + * x y d + * 1 1 9 + * 1 5 + */ +GaussianBayesNetOrdered createSmallGaussianBayesNet() { + using namespace impl; + Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0); + Matrix R22 = Matrix_(1, 1, 1.0); + Vector d1(1), d2(1); + d1(0) = 9; + d2(0) = 5; + Vector tau(1); + tau(0) = 1.0; + + // define nodes and specify in reverse topological sort (i.e. parents last) + GaussianConditionalOrdered::shared_ptr Px_y(new GaussianConditionalOrdered(_x_, d1, R11, _y_, S12, tau)); + GaussianConditionalOrdered::shared_ptr Py(new GaussianConditionalOrdered(_y_, d2, R22, tau)); + GaussianBayesNetOrdered cbn; + cbn.push_back(Px_y); + cbn.push_back(Py); + + return cbn; +} + +/* ************************************************************************* */ +// Some nonlinear functions to optimize +/* ************************************************************************* */ +namespace smallOptimize { + +Point2 h(const Point2& v) { + return Point2(cos(v.x()), sin(v.y())); +} + +Matrix H(const Point2& v) { + return Matrix_(2, 2, + -sin(v.x()), 0.0, + 0.0, cos(v.y())); +} + +struct UnaryFactor: public gtsam::NoiseModelFactor1 { + + Point2 z_; + + UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) : + gtsam::NoiseModelFactor1(model, key), z_(z) { + } + + Vector evaluateError(const Point2& x, boost::optional A = boost::none) const { + if (A) *A = H(x); + return (h(x) - z_).vector(); + } + +}; + +} + +/* ************************************************************************* */ +boost::shared_ptr sharedReallyNonlinearFactorGraph() { + using symbol_shorthand::X; + using symbol_shorthand::L; + boost::shared_ptr fg(new Graph); + Vector z = Vector_(2, 1.0, 0.0); + double sigma = 0.1; + boost::shared_ptr factor( + new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); + fg->push_back(factor); + return fg; +} + +Graph createReallyNonlinearFactorGraph() { + return *sharedReallyNonlinearFactorGraph(); +} + +/* ************************************************************************* */ +std::pair createNonlinearSmoother(int T) { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + + // Create + Graph nlfg; + Values poses; + + // prior on x1 + Point2 x1(1.0, 0.0); + shared_nlf prior(new simulated2D::Prior(x1, sigma1_0, X(1))); + nlfg.push_back(prior); + poses.insert(X(1), x1); + + for (int t = 2; t <= T; t++) { + // odometry between x_t and x_{t-1} + Point2 odo(1.0, 0.0); + shared_nlf odometry(new simulated2D::Odometry(odo, sigma1_0, X(t - 1), X(t))); + nlfg.push_back(odometry); + + // measurement on x_t is like perfect GPS + Point2 xt(t, 0); + shared_nlf measurement(new simulated2D::Prior(xt, sigma1_0, X(t))); + nlfg.push_back(measurement); + + // initial estimate + poses.insert(X(t), xt); + } + + return std::make_pair(nlfg, poses); +} + +/* ************************************************************************* */ +std::pair, OrderingOrdered> createSmoother(int T, boost::optional ordering) { + Graph nlfg; + Values poses; + boost::tie(nlfg, poses) = createNonlinearSmoother(T); + + if(!ordering) ordering = *poses.orderingArbitrary(); + return std::make_pair(*nlfg.linearize(poses, *ordering), *ordering); +} + +/* ************************************************************************* */ +GaussianFactorGraphOrdered createSimpleConstraintGraph() { + using namespace impl; + // create unary factor + // prior on _x_, mean = [1,-1], sigma=0.1 + Matrix Ax = eye(2); + Vector b1(2); + b1(0) = 1.0; + b1(1) = -1.0; + JacobianFactorOrdered::shared_ptr f1(new JacobianFactorOrdered(_x_, Ax, b1, sigma0_1)); + + // create binary constraint factor + // between _x_ and _y_, that is going to be the only factor on _y_ + // |1 0||x_1| + |-1 0||y_1| = |0| + // |0 1||x_2| | 0 -1||y_2| |0| + Matrix Ax1 = eye(2); + Matrix Ay1 = eye(2) * -1; + Vector b2 = Vector_(2, 0.0, 0.0); + JacobianFactorOrdered::shared_ptr f2(new JacobianFactorOrdered(_x_, Ax1, _y_, Ay1, b2, + constraintModel)); + + // construct the graph + GaussianFactorGraphOrdered fg; + fg.push_back(f1); + fg.push_back(f2); + + return fg; +} + +/* ************************************************************************* */ +VectorValuesOrdered createSimpleConstraintValues() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + VectorValuesOrdered config(std::vector(2,2)); + Vector v = Vector_(2, 1.0, -1.0); + config[_x_] = v; + config[_y_] = v; + return config; +} + +/* ************************************************************************* */ +GaussianFactorGraphOrdered createSingleConstraintGraph() { + using namespace impl; + // create unary factor + // prior on _x_, mean = [1,-1], sigma=0.1 + Matrix Ax = eye(2); + Vector b1(2); + b1(0) = 1.0; + b1(1) = -1.0; + //GaussianFactorOrdered::shared_ptr f1(new JacobianFactor(_x_, sigma0_1->Whiten(Ax), sigma0_1->whiten(b1), sigma0_1)); + JacobianFactorOrdered::shared_ptr f1(new JacobianFactorOrdered(_x_, Ax, b1, sigma0_1)); + + // create binary constraint factor + // between _x_ and _y_, that is going to be the only factor on _y_ + // |1 2||x_1| + |10 0||y_1| = |1| + // |2 1||x_2| |0 10||y_2| |2| + Matrix Ax1(2, 2); + Ax1(0, 0) = 1.0; + Ax1(0, 1) = 2.0; + Ax1(1, 0) = 2.0; + Ax1(1, 1) = 1.0; + Matrix Ay1 = eye(2) * 10; + Vector b2 = Vector_(2, 1.0, 2.0); + JacobianFactorOrdered::shared_ptr f2(new JacobianFactorOrdered(_x_, Ax1, _y_, Ay1, b2, + constraintModel)); + + // construct the graph + GaussianFactorGraphOrdered fg; + fg.push_back(f1); + fg.push_back(f2); + + return fg; +} + +/* ************************************************************************* */ +VectorValuesOrdered createSingleConstraintValues() { + using namespace impl; + VectorValuesOrdered config(std::vector(2,2)); + config[_x_] = Vector_(2, 1.0, -1.0); + config[_y_] = Vector_(2, 0.2, 0.1); + return config; +} + +/* ************************************************************************* */ +GaussianFactorGraphOrdered createMultiConstraintGraph() { + using namespace impl; + // unary factor 1 + Matrix A = eye(2); + Vector b = Vector_(2, -2.0, 2.0); + JacobianFactorOrdered::shared_ptr lf1(new JacobianFactorOrdered(_x_, A, b, sigma0_1)); + + // constraint 1 + Matrix A11(2, 2); + A11(0, 0) = 1.0; + A11(0, 1) = 2.0; + A11(1, 0) = 2.0; + A11(1, 1) = 1.0; + + Matrix A12(2, 2); + A12(0, 0) = 10.0; + A12(0, 1) = 0.0; + A12(1, 0) = 0.0; + A12(1, 1) = 10.0; + + Vector b1(2); + b1(0) = 1.0; + b1(1) = 2.0; + JacobianFactorOrdered::shared_ptr lc1(new JacobianFactorOrdered(_x_, A11, _y_, A12, b1, + constraintModel)); + + // constraint 2 + Matrix A21(2, 2); + A21(0, 0) = 3.0; + A21(0, 1) = 4.0; + A21(1, 0) = -1.0; + A21(1, 1) = -2.0; + + Matrix A22(2, 2); + A22(0, 0) = 1.0; + A22(0, 1) = 1.0; + A22(1, 0) = 1.0; + A22(1, 1) = 2.0; + + Vector b2(2); + b2(0) = 3.0; + b2(1) = 4.0; + JacobianFactorOrdered::shared_ptr lc2(new JacobianFactorOrdered(_x_, A21, _z_, A22, b2, + constraintModel)); + + // construct the graph + GaussianFactorGraphOrdered fg; + fg.push_back(lf1); + fg.push_back(lc1); + fg.push_back(lc2); + + return fg; +} + +/* ************************************************************************* */ +VectorValuesOrdered createMultiConstraintValues() { + using namespace impl; + VectorValuesOrdered config(std::vector(3,2)); + config[_x_] = Vector_(2, -2.0, 2.0); + config[_y_] = Vector_(2, -0.1, 0.4); + config[_z_] = Vector_(2, -4.0, 5.0); + return config; +} + +/* ************************************************************************* */ +// Create key for simulated planar graph +namespace impl { +Symbol key(int x, int y) { + using symbol_shorthand::X; + return X(1000*x+y); +} +} // \namespace impl + +/* ************************************************************************* */ +boost::tuple planarGraph(size_t N) { + using namespace impl; + + // create empty graph + NonlinearFactorGraph nlfg; + + // Create almost hard constraint on x11, sigma=0 will work for PCG not for normal + shared_nlf constraint(new simulated2D::Prior(Point2(1.0, 1.0), noiseModel::Isotropic::Sigma(2,1e-3), key(1,1))); + nlfg.push_back(constraint); + + // Create horizontal constraints, 1...N*(N-1) + Point2 z1(1.0, 0.0); // move right + for (size_t x = 1; x < N; x++) + for (size_t y = 1; y <= N; y++) { + shared_nlf f(new simulated2D::Odometry(z1, noiseModel::Isotropic::Sigma(2,0.01), key(x, y), key(x + 1, y))); + nlfg.push_back(f); + } + + // Create vertical constraints, N*(N-1)+1..2*N*(N-1) + Point2 z2(0.0, 1.0); // move up + for (size_t x = 1; x <= N; x++) + for (size_t y = 1; y < N; y++) { + shared_nlf f(new simulated2D::Odometry(z2, noiseModel::Isotropic::Sigma(2,0.01), key(x, y), key(x, y + 1))); + nlfg.push_back(f); + } + + // Create linearization and ground xtrue config + Values zeros; + for (size_t x = 1; x <= N; x++) + for (size_t y = 1; y <= N; y++) + zeros.insert(key(x, y), Point2()); + OrderingOrdered ordering(planarOrdering(N)); + VectorValuesOrdered xtrue(zeros.dims(ordering)); + for (size_t x = 1; x <= N; x++) + for (size_t y = 1; y <= N; y++) + xtrue[ordering[key(x, y)]] = Point2(x,y).vector(); + + // linearize around zero + boost::shared_ptr gfg = nlfg.linearize(zeros, ordering); + return boost::make_tuple(*gfg, xtrue); +} + +/* ************************************************************************* */ +OrderingOrdered planarOrdering(size_t N) { + OrderingOrdered ordering; + for (size_t y = N; y >= 1; y--) + for (size_t x = N; x >= 1; x--) + ordering.push_back(impl::key(x, y)); + return ordering; +} + +/* ************************************************************************* */ +std::pair splitOffPlanarTree(size_t N, + const GaussianFactorGraphOrdered& original) { + GaussianFactorGraphOrdered T, C; + + // Add the x11 constraint to the tree + T.push_back(original[0]); + + // Add all horizontal constraints to the tree + size_t i = 1; + for (size_t x = 1; x < N; x++) + for (size_t y = 1; y <= N; y++, i++) + T.push_back(original[i]); + + // Add first vertical column of constraints to T, others to C + for (size_t x = 1; x <= N; x++) + for (size_t y = 1; y < N; y++, i++) + if (x == 1) + T.push_back(original[i]); + else + C.push_back(original[i]); + + return std::make_pair(T, C); +} + +/* ************************************************************************* */ + +} // anonymous namespace +} // \namespace example +} // \namespace gtsam diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index e6aad0864..4eca49b3c 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -16,14 +16,9 @@ */ #include -#include #include -#include -#include -#include -#include -#include -#include +#include +#include #include @@ -44,29 +39,29 @@ using symbol_shorthand::L; static const double tol = 1e-4; /* ************************************************************************* */ -TEST( ISAMOrdered, iSAM_smoother ) +TEST( ISAM, iSAM_smoother ) { - OrderingOrdered ordering; + Ordering ordering; for (int t = 1; t <= 7; t++) ordering += X(t); // Create smoother with 7 nodes - GaussianFactorGraphOrdered smoother = createSmoother(7, ordering).first; + GaussianFactorGraph smoother = createSmoother(7, ordering).first; // run iSAM for every factor - GaussianISAMOrdered actual; - BOOST_FOREACH(boost::shared_ptr factor, smoother) { - GaussianFactorGraphOrdered factorGraph; + GaussianISAM actual; + BOOST_FOREACH(boost::shared_ptr factor, smoother) { + GaussianFactorGraph factorGraph; factorGraph.push_back(factor); actual.update(factorGraph); } // Create expected Bayes Tree by solving smoother with "natural" ordering - BayesTreeOrdered::shared_ptr bayesTree = GaussianMultifrontalSolver(smoother).eliminate(); - GaussianISAMOrdered expected(*bayesTree); + BayesTree::shared_ptr bayesTree = GaussianMultifrontalSolver(smoother).eliminate(); + GaussianISAM expected(*bayesTree); // Verify sigmas in the bayes tree - BOOST_FOREACH(const GaussianBayesTreeOrdered::sharedClique& clique, bayesTree->nodes()) { - GaussianConditionalOrdered::shared_ptr conditional = clique->conditional(); + BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, bayesTree->nodes()) { + GaussianConditional::shared_ptr conditional = clique->conditional(); size_t dim = conditional->dim(); EXPECT(assert_equal(gtsam::ones(dim), conditional->get_sigmas(), tol)); } @@ -75,8 +70,8 @@ TEST( ISAMOrdered, iSAM_smoother ) EXPECT(assert_equal(expected, actual)); // obtain solution - VectorValuesOrdered e(VectorValuesOrdered::Zero(7,2)); // expected solution - VectorValuesOrdered optimized = optimize(actual); // actual solution + VectorValues e(VectorValues::Zero(7,2)); // expected solution + VectorValues optimized = optimize(actual); // actual solution EXPECT(assert_equal(e, optimized)); } diff --git a/tests/testGaussianISAMOrdered.cpp b/tests/testGaussianISAMOrdered.cpp new file mode 100644 index 000000000..d353b62a5 --- /dev/null +++ b/tests/testGaussianISAMOrdered.cpp @@ -0,0 +1,85 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testGaussianISAM.cpp + * @brief Unit tests for GaussianISAM + * @author Michael Kaess + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include // for operator += +using namespace boost::assign; + +using namespace std; +using namespace gtsam; +using namespace example; + +using symbol_shorthand::X; +using symbol_shorthand::L; + +/* ************************************************************************* */ +// Some numbers that should be consistent among all smoother tests + +static const double tol = 1e-4; + +/* ************************************************************************* */ +TEST( ISAMOrdered, iSAM_smoother ) +{ + OrderingOrdered ordering; + for (int t = 1; t <= 7; t++) ordering += X(t); + + // Create smoother with 7 nodes + GaussianFactorGraphOrdered smoother = createSmoother(7, ordering).first; + + // run iSAM for every factor + GaussianISAMOrdered actual; + BOOST_FOREACH(boost::shared_ptr factor, smoother) { + GaussianFactorGraphOrdered factorGraph; + factorGraph.push_back(factor); + actual.update(factorGraph); + } + + // Create expected Bayes Tree by solving smoother with "natural" ordering + BayesTreeOrdered::shared_ptr bayesTree = GaussianMultifrontalSolver(smoother).eliminate(); + GaussianISAMOrdered expected(*bayesTree); + + // Verify sigmas in the bayes tree + BOOST_FOREACH(const GaussianBayesTreeOrdered::sharedClique& clique, bayesTree->nodes()) { + GaussianConditionalOrdered::shared_ptr conditional = clique->conditional(); + size_t dim = conditional->dim(); + EXPECT(assert_equal(gtsam::ones(dim), conditional->get_sigmas(), tol)); + } + + // Check whether BayesTree is correct + EXPECT(assert_equal(expected, actual)); + + // obtain solution + VectorValuesOrdered e(VectorValuesOrdered::Zero(7,2)); // expected solution + VectorValuesOrdered optimized = optimize(actual); // actual solution + EXPECT(assert_equal(e, optimized)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */