Add support for bounds to ParameterBlock.

Add setters and getters for lower and upper bounds.
Generalize the Plus operation to include projection onto the
hypercube implied by the bounds.

Change-Id: I1e4028a9886c4064f31bbc5b7c22b0341a56c15d

internal/ceres/parameter_block.h

diff --git a/internal/ceres/parameter_block.h b/internal/ceres/parameter_block.h
index 695fa6f..c3fcce3 100644
--- a/internal/ceres/parameter_block.h
+++ b/internal/ceres/parameter_block.h
@@ -31,7 +31,9 @@
 #ifndef CERES_INTERNAL_PARAMETER_BLOCK_H_
 #define CERES_INTERNAL_PARAMETER_BLOCK_H_
 
+#include <algorithm>
 #include <cstdlib>
+#include <limits>
 #include <string>
 #include "ceres/array_utils.h"
 #include "ceres/collections_port.h"
@@ -180,16 +182,37 @@
     }
   }
 
+  void SetUpperBound(int index, double upper_bound) {
+    CHECK_LT(index, size_);
+    upper_bounds_[index] = upper_bound;
+  };
+
+  void SetLowerBound(int index, double lower_bound) {
+    CHECK_LT(index, size_);
+    lower_bounds_[index] = lower_bound;
+  }
+
   // Generalization of the addition operation. This is the same as
-  // LocalParameterization::Plus() but uses the parameter's current state
-  // instead of operating on a passed in pointer.
+  // LocalParameterization::Plus() followed by projection onto the
+  // hyper cube implied by the bounds constraints.
   bool Plus(const double *x, const double* delta, double* x_plus_delta) {
-    if (local_parameterization_ == NULL) {
+    if (local_parameterization_ != NULL) {
+      if (!local_parameterization_->Plus(x, delta, x_plus_delta)) {
+        return false;
+      }
+    } else {
       VectorRef(x_plus_delta, size_) = ConstVectorRef(x, size_) +
                                        ConstVectorRef(delta,  size_);
-      return true;
     }
-    return local_parameterization_->Plus(x, delta, x_plus_delta);
+
+    // Project onto the box constraints.
+    for (int i = 0; i < size_; ++i) {
+      x_plus_delta[i] = std::min(std::max(x_plus_delta[i],
+                                          lower_bounds_[i]),
+                                 upper_bounds_[i]);
+    }
+
+    return true;
   }
 
   string ToString() const {
@@ -234,6 +257,14 @@
     return residual_blocks_.get();
   }
 
+  const double* upper_bounds() const {
+    return upper_bounds_.get();
+  }
+
+  const double* lower_bounds() const {
+    return lower_bounds_.get();
+  }
+
  private:
   void Init(double* user_state,
             int size,
@@ -250,6 +281,15 @@
       SetParameterization(local_parameterization);
     }
 
+    upper_bounds_.reset(new double[size_]);
+    std::fill(upper_bounds_.get(),
+              upper_bounds_.get() + size_,
+              std::numeric_limits<double>::max());
+    lower_bounds_.reset(new double[size_]);
+    std::fill(lower_bounds_.get(),
+              lower_bounds_.get() + size_,
+              -std::numeric_limits<double>::max());
+
     state_offset_ = -1;
     delta_offset_ = -1;
   }
@@ -312,6 +352,9 @@
   // If non-null, contains the residual blocks this parameter block is in.
   scoped_ptr<ResidualBlockSet> residual_blocks_;
 
+  scoped_array<double> upper_bounds_;
+  scoped_array<double> lower_bounds_;
+
   // Necessary so ProblemImpl can clean up the parameterizations.
   friend class ProblemImpl;
 };

internal/ceres/parameter_block_test.cc

diff --git a/internal/ceres/parameter_block_test.cc b/internal/ceres/parameter_block_test.cc
index 09156f8..a76c408 100644
--- a/internal/ceres/parameter_block_test.cc
+++ b/internal/ceres/parameter_block_test.cc
@@ -169,5 +169,42 @@
   EXPECT_FALSE(parameter_block.SetState(&y));
 }
 
+TEST(ParameterBlock, DefaultBounds) {
+  double x[2];
+  ParameterBlock parameter_block(x, 2, -1, NULL);
+  const double* upper_bounds = parameter_block.upper_bounds();
+  EXPECT_EQ(upper_bounds[0], std::numeric_limits<double>::max());
+  EXPECT_EQ(upper_bounds[1], std::numeric_limits<double>::max());
+  const double* lower_bounds = parameter_block.lower_bounds();
+  EXPECT_EQ(lower_bounds[0], -std::numeric_limits<double>::max());
+  EXPECT_EQ(lower_bounds[1], -std::numeric_limits<double>::max());
+}
+
+TEST(ParameterBlock, SetBounds) {
+  double x[2];
+  ParameterBlock parameter_block(x, 2, -1, NULL);
+  parameter_block.SetUpperBound(1, 1);
+  parameter_block.SetLowerBound(0, 1);
+
+  const double* upper_bounds = parameter_block.upper_bounds();
+  EXPECT_EQ(upper_bounds[0], std::numeric_limits<double>::max());
+  EXPECT_EQ(upper_bounds[1], 1.0);
+  const double* lower_bounds = parameter_block.lower_bounds();
+  EXPECT_EQ(lower_bounds[0], 1.0);
+  EXPECT_EQ(lower_bounds[1], -std::numeric_limits<double>::max());
+}
+
+TEST(ParameterBlock, PlusWithBoundsConstraints) {
+  double x[] = {1.0, 0.0};
+  double delta[] = {2.0, -10.0};
+  ParameterBlock parameter_block(x, 2, -1, NULL);
+  parameter_block.SetUpperBound(0, 2.0);
+  parameter_block.SetLowerBound(1, -1.0);
+  double x_plus_delta[2];
+  parameter_block.Plus(x, delta, x_plus_delta);
+  EXPECT_EQ(x_plus_delta[0], 2.0);
+  EXPECT_EQ(x_plus_delta[1], -1.0);
+}
+
 }  // namespace internal
 }  // namespace ceres