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

@@ -148,6 +148,23 @@ GaussianConditional::GaussianConditional(const std::list<std::pair<Index, Matrix
   get_d_() = d;
 }
 
+/* ************************************************************************* */
+GaussianConditional::GaussianConditional(const GaussianConditional& rhs) :
+  rsd_(matrix_) {
+  *this = rhs;
+}
+
+/* ************************************************************************* */
+GaussianConditional& GaussianConditional::operator=(const GaussianConditional& rhs) {
+  if(this != &rhs) {
+    this->Base::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
 {

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<MATRIX>& 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;
 

gtsam/linear/HessianFactor.cpp

@@ -224,6 +224,13 @@ HessianFactor::HessianFactor(const FactorGraph<GaussianFactor>& 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";

gtsam/linear/HessianFactor.h

@@ -116,6 +116,7 @@ namespace gtsam {
   protected:
     typedef Matrix InfoMatrix; ///< The full augmented Hessian
     typedef SymmetricBlockView<InfoMatrix> 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<GaussianFactor>(

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";

gtsam/linear/JacobianFactor.h

@@ -88,6 +88,7 @@ namespace gtsam {
     std::vector<size_t> 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<JacobianFactor> 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<GaussianFactor>(

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);

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());
+  LONGS_EQUAL(4, copy1.rows());
-  DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10);
+  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(G11, copy1.info(copy1.begin(), copy1.begin())));
-  EXPECT(assert_equal(G12, factor.info(factor.begin(), factor.begin()+1)));
+  EXPECT(assert_equal(G12, copy1.info(copy1.begin(), copy1.begin()+1)));
-  EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.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)));
 }
 
 /* ************************************************************************* */

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) <<