|  | @@ -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 @@ class ParameterBlock {
 | 
	
		
			
				|  |  |      }
 | 
	
		
			
				|  |  |    }
 | 
	
		
			
				|  |  |  
 | 
	
		
			
				|  |  | +  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 @@ class ParameterBlock {
 | 
	
		
			
				|  |  |      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 @@ class ParameterBlock {
 | 
	
		
			
				|  |  |        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 @@ class ParameterBlock {
 | 
	
		
			
				|  |  |    // 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;
 | 
	
		
			
				|  |  |  };
 |