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