Added in-place sub insert function to Vector

cpp/Vector.cpp

@@ -23,12 +23,15 @@
 #include <gsl/gsl_linalg.h>
 #endif
 
+#include <boost/numeric/ublas/io.hpp>
+#include <boost/numeric/ublas/vector_proxy.hpp>
 #include <boost/random/normal_distribution.hpp>
 #include <boost/random/variate_generator.hpp>
 
 #include "Vector.h"
 
 using namespace std;
+namespace ublas = boost::numeric::ublas;
 
 namespace gtsam {
   
@@ -159,6 +162,12 @@ namespace gtsam {
     return v_return;
   }
   
+  /* ************************************************************************* */
+  void subInsert(Vector& big, const Vector& small, size_t i) {
+	  ublas::vector_range<Vector> colsubproxy(big, ublas::range (i, i+small.size()));
+	  colsubproxy = small;
+  }
+
   /* ************************************************************************* */
   Vector emul(const Vector &a, const Vector &b) {
   	size_t n = a.size();

cpp/Vector.h

@@ -139,6 +139,14 @@ bool assert_equal(const Vector& vec1, const Vector& vec2, double tol=1e-9);
  */
 Vector sub(const Vector &v, size_t i1, size_t i2);
 
+/**
+ * Inserts a subvector into a vector IN PLACE
+ * @param big is the vector to be changed
+ * @param small is the vector to insert
+ * @param i is the index where the subvector should be inserted
+ */
+void subInsert(Vector& big, const Vector& small, size_t i);
+
 /**
  * elementwise multiplication
  * @param a first vector

cpp/testVector.cpp

@@ -84,6 +84,20 @@ TEST( TestVector, sub )
   CHECK(b==result);
 }
 
+/* ************************************************************************* */
+TEST( TestVector, subInsert )
+{
+	Vector big = zero(6),
+		   small = ones(3);
+
+	size_t i = 2;
+	subInsert(big, small, i);
+
+	Vector expected = Vector_(6, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0);
+
+	CHECK(assert_equal(expected, big));
+}
+
 /* ************************************************************************* */
 TEST( TestVector, householder )
 {