From 4137cfe9d7b8463c2d2083479d17171cf384e5ae Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sun, 22 Jan 2012 05:16:12 +0000 Subject: [PATCH] Added missing copy constructor for GaussianConditional, and assignment operators for GaussianConditional, JacobianFactor, and HessianFactor that properly copy the block matrices (require calling a special function because they contain references) --- gtsam/linear/GaussianConditional.cpp | 17 ++++++++++ gtsam/linear/GaussianConditional.h | 9 +++++ gtsam/linear/HessianFactor.cpp | 7 ++++ gtsam/linear/HessianFactor.h | 4 +++ gtsam/linear/JacobianFactor.cpp | 10 ++++++ gtsam/linear/JacobianFactor.h | 5 +++ .../linear/tests/testGaussianConditional.cpp | 2 +- gtsam/linear/tests/testHessianFactor.cpp | 34 ++++++++++++++----- gtsam/linear/tests/testJacobianFactor.cpp | 22 ++++++++++++ 9 files changed, 100 insertions(+), 10 deletions(-) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 168bfec97..7bff355d7 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -148,6 +148,23 @@ GaussianConditional::GaussianConditional(const std::listBase::operator=(rhs); // Copy keys + rsd_.assignNoalias(rhs.rsd_); // Copy matrix and block configuration + sigmas_ = rhs.sigmas_; // Copy sigmas + permutation_ = rhs.permutation_; // Copy permutation + } + return *this; +} + /* ************************************************************************* */ void GaussianConditional::print(const string &s) const { diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 0aa6ad19a..f2a77c894 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -82,6 +82,9 @@ protected: * This is used to get back the correct ordering of x after solving by backSubstitition */ TranspositionType permutation_; + /** typedef to base class */ + typedef IndexConditional Base; + public: /** default constructor needed for serialization */ @@ -131,6 +134,12 @@ public: const VerticalBlockView& matrices, const Vector& sigmas, const TranspositionType& permutation = TranspositionType()); + /** Copy constructor */ + GaussianConditional(const GaussianConditional& rhs); + + /** Assignment operator */ + GaussianConditional& operator=(const GaussianConditional& rhs); + /** print */ void print(const std::string& = "GaussianConditional") const; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 679ea5879..a5e876b6e 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -224,6 +224,13 @@ HessianFactor::HessianFactor(const FactorGraph& factors, assertInvariants(); } +/* ************************************************************************* */ +HessianFactor& HessianFactor::operator=(const HessianFactor& rhs) { + this->Base::operator=(rhs); // Copy keys + info_.assignNoalias(rhs.info_); // Copy matrix and block structure + return *this; +} + /* ************************************************************************* */ void HessianFactor::print(const std::string& s) const { cout << s << "\n"; diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 988b45a5c..b51f0c143 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -116,6 +116,7 @@ namespace gtsam { protected: typedef Matrix InfoMatrix; ///< The full augmented Hessian typedef SymmetricBlockView BlockInfo; ///< A blockwise view of the Hessian + typedef GaussianFactor Base; ///< Typedef to base class InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1] BlockInfo info_; ///< The block view of the full information matrix. @@ -201,6 +202,9 @@ namespace gtsam { /** Destructor */ virtual ~HessianFactor() {} + /** Aassignment operator */ + HessianFactor& operator=(const HessianFactor& rhs); + /** Clone this JacobianFactor */ virtual GaussianFactor::shared_ptr clone() const { return boost::static_pointer_cast( diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 57f068fc6..0218ecc8d 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -200,6 +200,16 @@ namespace gtsam { assertInvariants(); } + /* ************************************************************************* */ + JacobianFactor& JacobianFactor::operator=(const JacobianFactor& rhs) { + this->Base::operator=(rhs); // Copy keys + model_ = rhs.model_; // Copy noise model + firstNonzeroBlocks_ = rhs.firstNonzeroBlocks_; // Copy staircase pattern + Ab_.assignNoalias(rhs.Ab_); // Copy matrix and block structure + assertInvariants(); + return *this; + } + /* ************************************************************************* */ void JacobianFactor::print(const string& s) const { cout << s << "\n"; diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 35a1bfa9d..079cb9292 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -88,6 +88,7 @@ namespace gtsam { std::vector firstNonzeroBlocks_; AbMatrix matrix_; // the full matrix corresponding to the factor BlockAb Ab_; // the block view of the full matrix + typedef GaussianFactor Base; // typedef to base public: typedef boost::shared_ptr shared_ptr; @@ -134,8 +135,12 @@ namespace gtsam { /** Convert from a HessianFactor (does Cholesky) */ JacobianFactor(const HessianFactor& factor); + /** Virtual destructor */ virtual ~JacobianFactor() {} + /** Aassignment operator */ + JacobianFactor& operator=(const JacobianFactor& rhs); + /** Clone this JacobianFactor */ virtual GaussianFactor::shared_ptr clone() const { return boost::static_pointer_cast( diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index 98164a2e3..9bac18da0 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -95,7 +95,7 @@ TEST(GaussianConditional, constructor) EXPECT(assert_equal(R, copied.get_R())); } -/* ************************************************************************* * +/* ************************************************************************* */ GaussianConditional construct() { Vector d = Vector_(2, 1.0, 2.0); diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index aefdeef99..a085a1e82 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -199,8 +199,9 @@ TEST(HessianFactor, Constructor2) EXPECT(assert_equal(G12, factor.info(factor.begin(), factor.begin()+1))); EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.begin()+1))); } + /* ************************************************************************* */ -TEST_UNSAFE(HessianFactor, CopyConstructor) +TEST_UNSAFE(HessianFactor, CopyConstructor_and_assignment) { Matrix G11 = Matrix_(1,1, 1.0); Matrix G12 = Matrix_(1,2, 2.0, 4.0); @@ -223,21 +224,36 @@ TEST_UNSAFE(HessianFactor, CopyConstructor) HessianFactor originalFactor(0, 1, G11, G12, g1, G22, g2, f); // Make a copy - HessianFactor factor(originalFactor); + HessianFactor copy1(originalFactor); double expected = 90.5; - double actual = factor.error(dx); + double actual = copy1.error(dx); DOUBLES_EQUAL(expected, actual, 1e-10); - LONGS_EQUAL(4, factor.rows()); - DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10); + LONGS_EQUAL(4, copy1.rows()); + DOUBLES_EQUAL(10.0, copy1.constantTerm(), 1e-10); Vector linearExpected(3); linearExpected << g1, g2; - EXPECT(assert_equal(linearExpected, factor.linearTerm())); + EXPECT(assert_equal(linearExpected, copy1.linearTerm())); - EXPECT(assert_equal(G11, factor.info(factor.begin(), factor.begin()))); - EXPECT(assert_equal(G12, factor.info(factor.begin(), factor.begin()+1))); - EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.begin()+1))); + EXPECT(assert_equal(G11, copy1.info(copy1.begin(), copy1.begin()))); + EXPECT(assert_equal(G12, copy1.info(copy1.begin(), copy1.begin()+1))); + EXPECT(assert_equal(G22, copy1.info(copy1.begin()+1, copy1.begin()+1))); + + // Make a copy using the assignment operator + HessianFactor copy2; + copy2 = HessianFactor(originalFactor); // Make a temporary to make sure copying does not shared references + + actual = copy2.error(dx); + DOUBLES_EQUAL(expected, actual, 1e-10); + LONGS_EQUAL(4, copy2.rows()); + DOUBLES_EQUAL(10.0, copy2.constantTerm(), 1e-10); + + EXPECT(assert_equal(linearExpected, copy2.linearTerm())); + + EXPECT(assert_equal(G11, copy2.info(copy2.begin(), copy2.begin()))); + EXPECT(assert_equal(G12, copy2.info(copy2.begin(), copy2.begin()+1))); + EXPECT(assert_equal(G22, copy2.info(copy2.begin()+1, copy2.begin()+1))); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index c78618ff7..0d46807b6 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -71,6 +71,28 @@ TEST(JacobianFactor, constructor2) EXPECT(assert_equal(b, actualb)); } +/* ************************************************************************* */ + +JacobianFactor construct() { + Matrix A = Matrix_(2,2, 1.,2.,3.,4.); + Vector b = Vector_(2, 1.0, 2.0); + SharedDiagonal s = sharedSigmas(Vector_(2, 3.0, 4.0)); + JacobianFactor::shared_ptr shared( + new JacobianFactor(0, A, b, s)); + return *shared; +} + +TEST(JacobianFactor, return_value) +{ + Matrix A = Matrix_(2,2, 1.,2.,3.,4.); + Vector b = Vector_(2, 1.0, 2.0); + SharedDiagonal s = sharedSigmas(Vector_(2, 3.0, 4.0)); + JacobianFactor copied = construct(); + EXPECT(assert_equal(b, copied.getb())); + EXPECT(assert_equal(*s, *copied.get_model())); + EXPECT(assert_equal(A, copied.getA(copied.begin()))); +} + /* ************************************************************************* */ TEST(JabobianFactor, Hessian_conversion) { HessianFactor hessian(0, (Matrix(4,4) <<