support augmented matrices in Matrix's vector_scale_in_place, Constrained's Whiten, WhitenInPlace, and unit(...). These are used in constraints SQP solver with Lagrange multipliers.

gtsam/base/Matrix.cpp

@@ -586,7 +586,8 @@ Matrix collect(size_t nrMatrices, ...)
 void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) {
   const size_t m = A.rows();
   if (inf_mask) {
-    for (size_t i=0; i<m; ++i) {
+    // only scale the first v.size() rows of A to support augmented Matrix
+    for (size_t i=0; i<v.size(); ++i) {
       const double& vi = v(i);
       if (std::isfinite(vi))
         A.row(i) *= vi;

gtsam/base/Matrix.h

@@ -423,7 +423,8 @@ GTSAM_EXPORT Matrix collect(size_t nrMatrices, ...);
  * scales a matrix row or column by the values in a vector
  * Arguments (Matrix, Vector) scales the columns,
  * (Vector, Matrix) scales the rows
- * @param inf_mask when true, will not scale with a NaN or inf value
+ * @param inf_mask when true, will not scale with a NaN or inf value.
+ * The inplace version also allows v.size()<A.rows() and only scales the first v.size() rows of A.
  */
 GTSAM_EXPORT void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask = false); // row
 GTSAM_EXPORT Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask = false); // row

gtsam/linear/NoiseModel.cpp

@@ -261,11 +261,13 @@ Vector Constrained::whiten(const Vector& v) const {
   const Vector& a = v;
   const Vector& b = sigmas_;
   size_t n = a.size();
-  assert (b.size()==a.size());
-  Vector c(n);
-  for( size_t i = 0; i < n; i++ ) {
+  // Now allow for whiten augmented vector with a new additional part coming
+  // from the Lagrange multiplier. So a.size() >= b.size()
+//  assert (b.size()==a.size());
+  Vector c = a;
+  for( size_t i = 0; i < b.size(); i++ ) {
     const double& ai = a(i), &bi = b(i);
-    c(i) = (bi==0.0) ? ai : ai/bi; // NOTE: not ediv_()
+    if (bi!=0) c(i) = ai/bi;
   }
   return c;
 }
@@ -286,7 +288,9 @@ double Constrained::distance(const Vector& v) const {
 /* ************************************************************************* */
 Matrix Constrained::Whiten(const Matrix& H) const {
   // selective scaling
-  return vector_scale(invsigmas(), H, true);
+  // Now allow augmented Matrix with a new additional part coming
+  // from the Lagrange multiplier.
+  return vector_scale(invsigmas(), H.block(0, 0, dim(), H.cols()), true);
 }
 
 /* ************************************************************************* */
@@ -295,16 +299,20 @@ void Constrained::WhitenInPlace(Matrix& H) const {
   // Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas)
   // Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0,
   // indicating a hard constraint, we leave H's row i in place.
+  // Now allow augmented Matrix with a new additional part coming
+  // from the Lagrange multiplier.
   vector_scale_inplace(invsigmas(), H, true);
 }
 
 /* ************************************************************************* */
-Constrained::shared_ptr Constrained::unit() const {
-  Vector sigmas = ones(dim());
+Constrained::shared_ptr Constrained::unit(size_t augmentedDim) const {
+  Vector sigmas = ones(dim()+augmentedDim);
   for (size_t i=0; i<dim(); ++i)
     if (this->sigmas_(i) == 0.0)
       sigmas(i) = 0.0;
-  return MixedSigmas(mu_, sigmas);
+  Vector augmentedMu = zero(dim()+augmentedDim);
+  subInsert(augmentedMu, mu_, 0);
+  return MixedSigmas(augmentedMu, sigmas);
 }
 
 /* ************************************************************************* */

gtsam/linear/NoiseModel.h

@@ -446,8 +446,9 @@ namespace gtsam {
       /**
        * Returns a Unit version of a constrained noisemodel in which
        * constrained sigmas remain constrained and the rest are unit scaled
+       * Now support augmented part from the Lagrange multiplier.
        */
-      shared_ptr unit() const;
+      shared_ptr unit(size_t augmentedDim = 0) const;
 
     private:
       /** Serialization function */
@@ -815,6 +816,7 @@ namespace gtsam {
   typedef noiseModel::Base::shared_ptr SharedNoiseModel;
   typedef noiseModel::Gaussian::shared_ptr SharedGaussian;
   typedef noiseModel::Diagonal::shared_ptr SharedDiagonal;
+  typedef noiseModel::Constrained::shared_ptr SharedConstrained;
 
 } // namespace gtsam