diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp index 0676be174..a72e3c259 100644 --- a/examples/Pose2SLAMExample_advanced.cpp +++ b/examples/Pose2SLAMExample_advanced.cpp @@ -70,7 +70,7 @@ int main(int argc, char** argv) { Optimizer::Parameters::verbosityLevel verbosity = pose2SLAM::Optimizer::Parameters::SILENT; Optimizer optimizer_result = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity); - Values result = *optimizer_result.config(); + Values result = *optimizer_result.values(); result.print("final result"); return 0; } diff --git a/examples/Pose2SLAMwSPCG_advanced.cpp b/examples/Pose2SLAMwSPCG_advanced.cpp index f35bd6097..26b6bbdb0 100644 --- a/examples/Pose2SLAMwSPCG_advanced.cpp +++ b/examples/Pose2SLAMwSPCG_advanced.cpp @@ -52,7 +52,7 @@ int main(void) { SPCGOptimizer optimizer2 = optimizer.levenbergMarquardt(parameter); cout << "after optimization, sum of error is " << optimizer2.error() << endl; - result = *optimizer2.config() ; + result = *optimizer2.values() ; result.print("final result") ; return 0 ; diff --git a/nonlinear/NonlinearOptimization-inl.h b/nonlinear/NonlinearOptimization-inl.h index 7ab816c59..be1641874 100644 --- a/nonlinear/NonlinearOptimization-inl.h +++ b/nonlinear/NonlinearOptimization-inl.h @@ -45,7 +45,7 @@ namespace gtsam { // Levenberg-Marquardt Optimizer result = optimizer.levenbergMarquardt(parameters); - return *result.config(); + return *result.values(); } /** @@ -64,7 +64,7 @@ namespace gtsam { // Levenberg-Marquardt Optimizer result = optimizer.levenbergMarquardt(parameters); - return *result.config(); + return *result.values(); } // /** @@ -83,7 +83,7 @@ namespace gtsam { // // // Levenberg-Marquardt // SPCGOptimizer result = optimizer.levenbergMarquardt(parameters); -// return *result.config(); +// return *result.values(); // } /** diff --git a/nonlinear/NonlinearOptimizationParameters.h b/nonlinear/NonlinearOptimizationParameters.h index f196e5656..be8770672 100644 --- a/nonlinear/NonlinearOptimizationParameters.h +++ b/nonlinear/NonlinearOptimizationParameters.h @@ -17,7 +17,7 @@ namespace gtsam { ERROR, LAMBDA, TRYLAMBDA, - CONFIG, + VALUES, DELTA, TRYCONFIG, TRYDELTA, diff --git a/nonlinear/NonlinearOptimizer-inl.h b/nonlinear/NonlinearOptimizer-inl.h index 4abb99e72..af46fe9a7 100644 --- a/nonlinear/NonlinearOptimizer-inl.h +++ b/nonlinear/NonlinearOptimizer-inl.h @@ -74,13 +74,13 @@ namespace gtsam { /* ************************************************************************* */ template NonlinearOptimizer::NonlinearOptimizer(shared_graph graph, - shared_values config, shared_ordering ordering, double lambda) : - graph_(graph), config_(config), error_(graph->error(*config)), - ordering_(ordering), lambda_(lambda), dimensions_(new vector(config->dims(*ordering))) { + shared_values values, shared_ordering ordering, double lambda) : + graph_(graph), values_(values), error_(graph->error(*values)), + ordering_(ordering), lambda_(lambda), dimensions_(new vector(values->dims(*ordering))) { if (!graph) throw std::invalid_argument( "NonlinearOptimizer constructor: graph = NULL"); - if (!config) throw std::invalid_argument( - "NonlinearOptimizer constructor: config = NULL"); + if (!values) throw std::invalid_argument( + "NonlinearOptimizer constructor: values = NULL"); if (!ordering) throw std::invalid_argument( "NonlinearOptimizer constructor: ordering = NULL"); } @@ -90,8 +90,8 @@ namespace gtsam { /* ************************************************************************* */ template VectorValues NonlinearOptimizer::linearizeAndOptimizeForDelta() const { - boost::shared_ptr linearized = graph_->linearize(*config_, *ordering_); -// NonlinearOptimizer prepared(graph_, config_, ordering_, error_, lambda_); + boost::shared_ptr linearized = graph_->linearize(*values_, *ordering_); +// NonlinearOptimizer prepared(graph_, values_, ordering_, error_, lambda_); return *S(*linearized).optimize(); } @@ -108,11 +108,11 @@ namespace gtsam { if (verbosity >= Parameters::DELTA) delta.print("delta"); - // take old config and update it - shared_values newValues(new C(config_->expmap(delta, *ordering_))); + // take old values and update it + shared_values newValues(new C(values_->expmap(delta, *ordering_))); // maybe show output - if (verbosity >= Parameters::CONFIG) + if (verbosity >= Parameters::VALUES) newValues->print("newValues"); NonlinearOptimizer newOptimizer = newValues_(newValues); @@ -181,10 +181,10 @@ namespace gtsam { if (verbosity >= Parameters::TRYDELTA) delta.print("delta"); - // update config - shared_values newValues(new C(config_->expmap(delta, *ordering_))); // TODO: updateValues -// if (verbosity >= TRYCONFIG) -// newValues->print("config"); + // update values + shared_values newValues(new C(values_->expmap(delta, *ordering_))); // TODO: updateValues +// if (verbosity >= TRYvalues) +// newValues->print("values"); // create new optimization state with more adventurous lambda NonlinearOptimizer next(newValuesNewLambda_(newValues, lambda_ / factor)); @@ -219,9 +219,9 @@ namespace gtsam { // Either we're not cautious, or the same lambda was worse than the current error. // The more adventerous lambda was worse too, so make lambda more conservative - // and keep the same config. + // and keep the same values. - // TODO: can we avoid copying the config ? + // TODO: can we avoid copying the values ? if(lambdaMode >= Parameters::BOUNDED && lambda_ >= 1.0e5) { return NonlinearOptimizer(newValues_(newValues)); } else { @@ -240,15 +240,15 @@ namespace gtsam { Parameters::verbosityLevel verbosity, double lambdaFactor, Parameters::LambdaMode lambdaMode) const { // show output - if (verbosity >= Parameters::CONFIG) - config_->print("config"); + if (verbosity >= Parameters::VALUES) + values_->print("values"); if (verbosity >= Parameters::ERROR) cout << "error: " << error_ << endl; if (verbosity >= Parameters::LAMBDA) cout << "lambda = " << lambda_ << endl; // linearize all factors once - boost::shared_ptr linear = graph_->linearize(*config_, *ordering_); + boost::shared_ptr linear = graph_->linearize(*values_, *ordering_); if (verbosity >= Parameters::LINEAR) linear->print("linear"); @@ -298,8 +298,8 @@ namespace gtsam { // return converged state or iterate if (converged || para.maxIterations_ <= 1) { // maybe show output - if (para.verbosity_ >= Parameters::CONFIG) - next.config_->print("final config"); + if (para.verbosity_ >= Parameters::VALUES) + next.values_->print("final values"); if (para.verbosity_ >= Parameters::ERROR) cout << "final error: " << next.error_ << endl; if (para.verbosity_ >= Parameters::LAMBDA) diff --git a/nonlinear/NonlinearOptimizer.h b/nonlinear/NonlinearOptimizer.h index b30e6d5d1..feeecc831 100644 --- a/nonlinear/NonlinearOptimizer.h +++ b/nonlinear/NonlinearOptimizer.h @@ -84,7 +84,7 @@ namespace gtsam { // keep a values structure and its error // These typically change once per iteration (in a functional way) - const shared_values config_; + const shared_values values_; const double error_; const shared_ordering ordering_; @@ -106,12 +106,12 @@ namespace gtsam { * Constructor that does not do any computation */ NonlinearOptimizer(shared_graph graph, shared_values config, shared_ordering ordering, - const double error, const double lambda, shared_dimensions dimensions): graph_(graph), config_(config), + const double error, const double lambda, shared_dimensions dimensions): graph_(graph), values_(config), error_(error), ordering_(ordering), lambda_(lambda), dimensions_(dimensions) {} /** Create a new NonlinearOptimizer with a different lambda */ This newLambda_(double newLambda) const { - return NonlinearOptimizer(graph_, config_, ordering_, error_, newLambda, dimensions_); } + return NonlinearOptimizer(graph_, values_, ordering_, error_, newLambda, dimensions_); } This newValues_(shared_values newValues) const { return NonlinearOptimizer(graph_, newValues, ordering_, graph_->error(*newValues), lambda_, dimensions_); } @@ -131,7 +131,7 @@ namespace gtsam { * Copy constructor */ NonlinearOptimizer(const NonlinearOptimizer &optimizer) : - graph_(optimizer.graph_), config_(optimizer.config_), error_(optimizer.error_), + graph_(optimizer.graph_), values_(optimizer.values_), error_(optimizer.error_), ordering_(optimizer.ordering_), lambda_(optimizer.lambda_), dimensions_(optimizer.dimensions_) {} /** @@ -149,10 +149,10 @@ namespace gtsam { } /** - * Return the config + * Return the values */ - shared_values config() const{ - return config_; + shared_values values() const{ + return values_; } /** @@ -227,7 +227,7 @@ namespace gtsam { // Levenberg-Marquardt NonlinearOptimizer result = optimizer.levenbergMarquardt(relativeThreshold, absoluteThreshold, verbosity); - return result.config(); + return result.values(); } /** @@ -258,7 +258,7 @@ namespace gtsam { // Gauss-Newton NonlinearOptimizer result = optimizer.gaussNewton(relativeThreshold, absoluteThreshold, verbosity); - return result.config(); + return result.values(); } /** diff --git a/nonlinear/TupleValues-inl.h b/nonlinear/TupleValues-inl.h index e6699b0c1..aad3327dd 100644 --- a/nonlinear/TupleValues-inl.h +++ b/nonlinear/TupleValues-inl.h @@ -50,22 +50,22 @@ namespace gtsam { /* ************************************************************************* */ /** TupleValues 1 */ template -TupleValues1::TupleValues1(const TupleValues1& config) : - TupleValuesEnd (config) {} +TupleValues1::TupleValues1(const TupleValues1& values) : + TupleValuesEnd (values) {} template TupleValues1::TupleValues1(const VALUES1& cfg1) : TupleValuesEnd (cfg1) {} template -TupleValues1::TupleValues1(const TupleValuesEnd& config) : - TupleValuesEnd(config) {} +TupleValues1::TupleValues1(const TupleValuesEnd& values) : + TupleValuesEnd(values) {} /* ************************************************************************* */ /** TupleValues 2 */ template -TupleValues2::TupleValues2(const TupleValues2& config) : - TupleValues >(config) {} +TupleValues2::TupleValues2(const TupleValues2& values) : + TupleValues >(values) {} template TupleValues2::TupleValues2(const VALUES1& cfg1, const VALUES2& cfg2) : @@ -73,15 +73,15 @@ TupleValues2::TupleValues2(const VALUES1& cfg1, const VALUES2& cfg1, TupleValuesEnd(cfg2)) {} template -TupleValues2::TupleValues2(const TupleValues >& config) : - TupleValues >(config) {} +TupleValues2::TupleValues2(const TupleValues >& values) : + TupleValues >(values) {} /* ************************************************************************* */ /** TupleValues 3 */ template TupleValues3::TupleValues3( - const TupleValues3& config) : - TupleValues > >(config) {} + const TupleValues3& values) : + TupleValues > >(values) {} template TupleValues3::TupleValues3( @@ -92,16 +92,16 @@ TupleValues3::TupleValues3( template TupleValues3::TupleValues3( - const TupleValues > >& config) : - TupleValues > >(config) {} + const TupleValues > >& values) : + TupleValues > >(values) {} /* ************************************************************************* */ /** TupleValues 4 */ template TupleValues4::TupleValues4( - const TupleValues4& config) : + const TupleValues4& values) : TupleValues > > >(config) {} + TupleValues > > >(values) {} template TupleValues4::TupleValues4( @@ -116,17 +116,17 @@ TupleValues4::TupleValues4( template TupleValues4::TupleValues4( const TupleValues > > >& config) : + TupleValues > > >& values) : TupleValues > > >(config) {} + TupleValuesEnd > > >(values) {} /* ************************************************************************* */ /** TupleValues 5 */ template TupleValues5::TupleValues5( - const TupleValues5& config) : + const TupleValues5& values) : TupleValues > > > >(config) {} + TupleValues > > > >(values) {} template TupleValues5::TupleValues5( @@ -144,9 +144,9 @@ TupleValues5::TupleValues5( template TupleValues5::TupleValues5( const TupleValues > > > >& config) : + TupleValues > > > >& values) : TupleValues > > > >(config) {} + TupleValues > > > >(values) {} /* ************************************************************************* */ /** TupleValues 6 */ @@ -154,10 +154,10 @@ template TupleValues6::TupleValues6( const TupleValues6& config) : + VALUES4, VALUES5, VALUES6>& values) : TupleValues > > > > >(config) {} + TupleValuesEnd > > > > >(values) {} template @@ -180,9 +180,9 @@ template::TupleValues6( const TupleValues > > > > >& config) : + TupleValuesEnd > > > > >& values) : TupleValues > > > > >(config) {} + TupleValuesEnd > > > > >(values) {} } diff --git a/nonlinear/TupleValues.h b/nonlinear/TupleValues.h index f87eebfa9..a232af622 100644 --- a/nonlinear/TupleValues.h +++ b/nonlinear/TupleValues.h @@ -30,8 +30,8 @@ namespace gtsam { * reduce run-time overhead and keep static type checking. The interface * mimics that of a single LieValues. * - * This uses a recursive structure of config pairs to form a lisp-like - * list, with a special case (TupleValuesEnd) that contains only one config + * This uses a recursive structure of values pairs to form a lisp-like + * list, with a special case (TupleValuesEnd) that contains only one values * at the end. Because this recursion is done at compile time, there is no * runtime performance hit to using this structure. * @@ -43,19 +43,19 @@ namespace gtsam { * Design and extension note: * To implement a recursively templated data structure, note that most operations * have two versions: one with templates and one without. The templated one allows - * for the arguments to be passed to the next config, while the specialized one - * operates on the "first" config. TupleValuesEnd contains only the specialized version. + * for the arguments to be passed to the next values, while the specialized one + * operates on the "first" values. TupleValuesEnd contains only the specialized version. */ template class TupleValues : public Testable > { protected: - // Data for internal configs - VALUES1 first_; /// Arbitrary config - VALUES2 second_; /// A TupleValues or TupleValuesEnd, which wraps an arbitrary config + // Data for internal valuess + VALUES1 first_; /// Arbitrary values + VALUES2 second_; /// A TupleValues or TupleValuesEnd, which wraps an arbitrary values public: - // typedefs for config subtypes + // typedefs for values subtypes typedef class VALUES1::Key Key1; typedef class VALUES1::Value Value1; @@ -63,10 +63,10 @@ namespace gtsam { TupleValues() {} /** Copy constructor */ - TupleValues(const TupleValues& config) : - first_(config.first_), second_(config.second_) {} + TupleValues(const TupleValues& values) : + first_(values.first_), second_(values.second_) {} - /** Construct from configs */ + /** Construct from valuess */ TupleValues(const VALUES1& cfg1, const VALUES2& cfg2) : first_(cfg1), second_(cfg2) {} @@ -82,8 +82,8 @@ namespace gtsam { } /** - * Insert a key/value pair to the config. - * Note: if the key is already in the config, the config will not be changed. + * Insert a key/value pair to the values. + * Note: if the key is already in the values, the values will not be changed. * Use update() to allow for changing existing values. * @param key is the key - can be an int (second version) if the can can be initialized from an int * @param value is the value to insert @@ -94,27 +94,27 @@ namespace gtsam { void insert(const Key1& key, const Value1& value) {first_.insert(key, value);} /** - * Insert a complete config at a time. - * Note: if the key is already in the config, the config will not be changed. + * Insert a complete values at a time. + * Note: if the key is already in the values, the values will not be changed. * Use update() to allow for changing existing values. - * @param config is a full config to add + * @param values is a full values to add */ template - void insert(const TupleValues& config) { second_.insert(config); } - void insert(const TupleValues& config) { - first_.insert(config.first_); - second_.insert(config.second_); + void insert(const TupleValues& values) { second_.insert(values); } + void insert(const TupleValues& values) { + first_.insert(values.first_); + second_.insert(values.second_); } /** - * Update function for whole configs - this will change existing values - * @param config is a config to add + * Update function for whole valuess - this will change existing values + * @param values is a values to add */ template - void update(const TupleValues& config) { second_.update(config); } - void update(const TupleValues& config) { - first_.update(config.first_); - second_.update(config.second_); + void update(const TupleValues& values) { second_.update(values); } + void update(const TupleValues& values) { + first_.update(values.first_); + second_.update(values.second_); } /** @@ -127,19 +127,19 @@ namespace gtsam { void update(const Key1& key, const Value1& value) { first_.update(key, value); } /** - * Insert a subconfig - * @param config is the config to insert + * Insert a subvalues + * @param values is the values to insert */ template - void insertSub(const CFG& config) { second_.insertSub(config); } - void insertSub(const VALUES1& config) { first_.insert(config); } + void insertSub(const CFG& values) { second_.insertSub(values); } + void insertSub(const VALUES1& values) { first_.insert(values); } /** erase an element by key */ template void erase(const KEY& j) { second_.erase(j); } void erase(const Key1& j) { first_.erase(j); } - /** clears the config */ + /** clears the values */ void clear() { first_.clear(); second_.clear(); } /** determine whether an element exists */ @@ -162,8 +162,8 @@ namespace gtsam { const typename KEY::Value & at(const KEY& j) const { return second_.at(j); } const Value1& at(const Key1& j) const { return first_.at(j); } - /** direct config access */ - const VALUES1& config() const { return first_; } + /** direct values access */ + const VALUES1& values() const { return first_; } const VALUES2& rest() const { return second_; } /** zero: create VectorValues of appropriate structure */ @@ -176,7 +176,7 @@ namespace gtsam { /** @return number of key/value pairs stored */ size_t size() const { return first_.size() + second_.size(); } - /** @return true if config is empty */ + /** @return true if values is empty */ bool empty() const { return first_.empty() && second_.empty(); } /** @return The dimensionality of the tangent space */ @@ -248,7 +248,7 @@ namespace gtsam { }; /** - * End of a recursive TupleValues - contains only one config + * End of a recursive TupleValues - contains only one values * * Do not use this class directly - it should only be used as a part * of a recursive structure @@ -257,7 +257,7 @@ namespace gtsam { class TupleValuesEnd : public Testable > { protected: - // Data for internal configs + // Data for internal valuess VALUES first_; public: @@ -267,8 +267,8 @@ namespace gtsam { TupleValuesEnd() {} - TupleValuesEnd(const TupleValuesEnd& config) : - first_(config.first_) {} + TupleValuesEnd(const TupleValuesEnd& values) : + first_(values.first_) {} TupleValuesEnd(const VALUES& cfg) : first_(cfg) {} @@ -284,17 +284,17 @@ namespace gtsam { void insert(const Key1& key, const Value1& value) {first_.insert(key, value); } void insert(int key, const Value1& value) {first_.insert(Key1(key), value);} - void insert(const TupleValuesEnd& config) {first_.insert(config.first_); } + void insert(const TupleValuesEnd& values) {first_.insert(values.first_); } - void update(const TupleValuesEnd& config) {first_.update(config.first_); } + void update(const TupleValuesEnd& values) {first_.update(values.first_); } void update(const Key1& key, const Value1& value) { first_.update(key, value); } - void insertSub(const VALUES& config) {first_.insert(config); } + void insertSub(const VALUES& values) {first_.insert(values); } const Value1& operator[](const Key1& j) const { return first_[j]; } - const VALUES& config() const { return first_; } + const VALUES& values() const { return first_; } void erase(const Key1& j) { first_.erase(j); } @@ -355,11 +355,11 @@ namespace gtsam { }; /** - * Wrapper classes to act as containers for configs. Note that these can be cascaded - * recursively, as they are TupleValuess, and are primarily a short form of the config + * Wrapper classes to act as containers for valuess. Note that these can be cascaded + * recursively, as they are TupleValuess, and are primarily a short form of the values * structure to make use of the TupleValuess easier. * - * The interface is designed to mimic PairValues, but for 2-6 config types. + * The interface is designed to mimic PairValues, but for 2-6 values types. */ template @@ -372,12 +372,12 @@ namespace gtsam { typedef TupleValues1 This; TupleValues1() {} - TupleValues1(const This& config); - TupleValues1(const Base& config); + TupleValues1(const This& values); + TupleValues1(const Base& values); TupleValues1(const Values1& cfg1); // access functions - inline const Values1& first() const { return this->config(); } + inline const Values1& first() const { return this->values(); } }; template @@ -391,13 +391,13 @@ namespace gtsam { typedef TupleValues2 This; TupleValues2() {} - TupleValues2(const This& config); - TupleValues2(const Base& config); + TupleValues2(const This& values); + TupleValues2(const Base& values); TupleValues2(const Values1& cfg1, const Values2& cfg2); // access functions - inline const Values1& first() const { return this->config(); } - inline const Values2& second() const { return this->rest().config(); } + inline const Values1& first() const { return this->values(); } + inline const Values2& second() const { return this->rest().values(); } }; template @@ -409,14 +409,14 @@ namespace gtsam { typedef C3 Values3; TupleValues3() {} - TupleValues3(const TupleValues > >& config); - TupleValues3(const TupleValues3& config); + TupleValues3(const TupleValues > >& values); + TupleValues3(const TupleValues3& values); TupleValues3(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3); // access functions - inline const Values1& first() const { return this->config(); } - inline const Values2& second() const { return this->rest().config(); } - inline const Values3& third() const { return this->rest().rest().config(); } + inline const Values1& first() const { return this->values(); } + inline const Values2& second() const { return this->rest().values(); } + inline const Values3& third() const { return this->rest().rest().values(); } }; template @@ -432,15 +432,15 @@ namespace gtsam { typedef TupleValues4 This; TupleValues4() {} - TupleValues4(const This& config); - TupleValues4(const Base& config); + TupleValues4(const This& values); + TupleValues4(const Base& values); TupleValues4(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3,const Values4& cfg4); // access functions - inline const Values1& first() const { return this->config(); } - inline const Values2& second() const { return this->rest().config(); } - inline const Values3& third() const { return this->rest().rest().config(); } - inline const Values4& fourth() const { return this->rest().rest().rest().config(); } + inline const Values1& first() const { return this->values(); } + inline const Values2& second() const { return this->rest().values(); } + inline const Values3& third() const { return this->rest().rest().values(); } + inline const Values4& fourth() const { return this->rest().rest().rest().values(); } }; template @@ -454,17 +454,17 @@ namespace gtsam { typedef C5 Values5; TupleValues5() {} - TupleValues5(const TupleValues5& config); - TupleValues5(const TupleValues > > > >& config); + TupleValues5(const TupleValues5& values); + TupleValues5(const TupleValues > > > >& values); TupleValues5(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3, const Values4& cfg4, const Values5& cfg5); // access functions - inline const Values1& first() const { return this->config(); } - inline const Values2& second() const { return this->rest().config(); } - inline const Values3& third() const { return this->rest().rest().config(); } - inline const Values4& fourth() const { return this->rest().rest().rest().config(); } - inline const Values5& fifth() const { return this->rest().rest().rest().rest().config(); } + inline const Values1& first() const { return this->values(); } + inline const Values2& second() const { return this->rest().values(); } + inline const Values3& third() const { return this->rest().rest().values(); } + inline const Values4& fourth() const { return this->rest().rest().rest().values(); } + inline const Values5& fifth() const { return this->rest().rest().rest().rest().values(); } }; template @@ -479,17 +479,17 @@ namespace gtsam { typedef C6 Values6; TupleValues6() {} - TupleValues6(const TupleValues6& config); - TupleValues6(const TupleValues > > > > >& config); + TupleValues6(const TupleValues6& values); + TupleValues6(const TupleValues > > > > >& values); TupleValues6(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3, const Values4& cfg4, const Values5& cfg5, const Values6& cfg6); // access functions - inline const Values1& first() const { return this->config(); } - inline const Values2& second() const { return this->rest().config(); } - inline const Values3& third() const { return this->rest().rest().config(); } - inline const Values4& fourth() const { return this->rest().rest().rest().config(); } - inline const Values5& fifth() const { return this->rest().rest().rest().rest().config(); } - inline const Values6& sixth() const { return this->rest().rest().rest().rest().rest().config(); } + inline const Values1& first() const { return this->values(); } + inline const Values2& second() const { return this->rest().values(); } + inline const Values3& third() const { return this->rest().rest().values(); } + inline const Values4& fourth() const { return this->rest().rest().rest().values(); } + inline const Values5& fifth() const { return this->rest().rest().rest().rest().values(); } + inline const Values6& sixth() const { return this->rest().rest().rest().rest().rest().values(); } }; } diff --git a/slam/tests/testPose2SLAM.cpp b/slam/tests/testPose2SLAM.cpp index cf47f01d3..eb7bde567 100644 --- a/slam/tests/testPose2SLAM.cpp +++ b/slam/tests/testPose2SLAM.cpp @@ -122,7 +122,7 @@ TEST(Pose2Graph, optimize) { Pose2Values expected; expected.insert(0, Pose2(0,0,0)); expected.insert(1, Pose2(1,2,M_PI_2)); - CHECK(assert_equal(expected, *optimizer.config())); + CHECK(assert_equal(expected, *optimizer.values())); } /* ************************************************************************* */ @@ -156,7 +156,7 @@ TEST(Pose2Graph, optimizeThreePoses) { pose2SLAM::Optimizer::Parameters::verbosityLevel verbosity = pose2SLAM::Optimizer::Parameters::SILENT; pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity); - Pose2Values actual = *optimizer.config(); + Pose2Values actual = *optimizer.values(); // Check with ground truth CHECK(assert_equal(hexagon, actual)); @@ -199,7 +199,7 @@ TEST(Pose2Graph, optimizeCircle) { pose2SLAM::Optimizer::Parameters::verbosityLevel verbosity = pose2SLAM::Optimizer::Parameters::SILENT; pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity); - Pose2Values actual = *optimizer.config(); + Pose2Values actual = *optimizer.values(); // Check with ground truth CHECK(assert_equal(hexagon, actual)); diff --git a/slam/tests/testPose3SLAM.cpp b/slam/tests/testPose3SLAM.cpp index 3f4b4b3b9..6c8aceaff 100644 --- a/slam/tests/testPose3SLAM.cpp +++ b/slam/tests/testPose3SLAM.cpp @@ -76,7 +76,7 @@ TEST(Pose3Graph, optimizeCircle) { // Optimizer::verbosityLevel verbosity = Optimizer::ERROR; Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity); - Pose3Values actual = *optimizer.config(); + Pose3Values actual = *optimizer.values(); // Check with ground truth CHECK(assert_equal(hexagon, actual,1e-4)); diff --git a/slam/tests/testVSLAMGraph.cpp b/slam/tests/testVSLAMGraph.cpp index 273d8a691..d081deae6 100644 --- a/slam/tests/testVSLAMGraph.cpp +++ b/slam/tests/testVSLAMGraph.cpp @@ -114,7 +114,7 @@ TEST( Graph, optimizeLM) DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // check if correct - CHECK(assert_equal(*initialEstimate,*(afterOneIteration.config()))); + CHECK(assert_equal(*initialEstimate,*(afterOneIteration.values()))); } @@ -151,7 +151,7 @@ TEST( Graph, optimizeLM2) DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // check if correct - CHECK(assert_equal(*initialEstimate,*(afterOneIteration.config()))); + CHECK(assert_equal(*initialEstimate,*(afterOneIteration.values()))); } @@ -186,7 +186,7 @@ TEST( Graph, CHECK_ORDERING) DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // check if correct - CHECK(assert_equal(*initialEstimate,*(afterOneIteration.config()))); + CHECK(assert_equal(*initialEstimate,*(afterOneIteration.values()))); } /* ************************************************************************* */ diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 43354cd56..d1acbe244 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -204,7 +204,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) { // verify PoseValues expected; expected.insert(key1, feasible1); - EXPECT(assert_equal(expected, *result.config())); + EXPECT(assert_equal(expected, *result.values())); } /* ************************************************************************* */ @@ -240,7 +240,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { // verify PoseValues expected; expected.insert(key1, feasible1); - EXPECT(assert_equal(expected, *result.config())); + EXPECT(assert_equal(expected, *result.values())); } /* ************************************************************************* */ diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 8358da7ad..118f07a9e 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -137,7 +137,7 @@ TEST( NonlinearOptimizer, iterateLM ) } delete(pointer); - CHECK(assert_equal(*iterated1.config(), *iterated2.config(), 1e-9)); + CHECK(assert_equal(*iterated1.values(), *iterated2.values(), 1e-9)); } /* ************************************************************************* */ @@ -170,12 +170,12 @@ TEST( NonlinearOptimizer, optimize ) // Gauss-Newton Optimizer actual1 = optimizer.gaussNewton(relativeThreshold, absoluteThreshold); - DOUBLES_EQUAL(0,fg->error(*(actual1.config())),tol); + DOUBLES_EQUAL(0,fg->error(*(actual1.values())),tol); // Levenberg-Marquardt Optimizer actual2 = optimizer.levenbergMarquardt(relativeThreshold, absoluteThreshold, Optimizer::Parameters::SILENT); - DOUBLES_EQUAL(0,fg->error(*(actual2.config())),tol); + DOUBLES_EQUAL(0,fg->error(*(actual2.values())),tol); } /* ************************************************************************* */ @@ -255,7 +255,7 @@ TEST( NonlinearOptimizer, Factorization ) Pose2Values expected; expected.insert(1, Pose2(0.,0.,0.)); expected.insert(2, Pose2(1.,0.,0.)); - CHECK(assert_equal(expected, *optimized.config(), 1e-5)); + CHECK(assert_equal(expected, *optimized.values(), 1e-5)); } ///* ************************************************************************* */ @@ -290,7 +290,7 @@ TEST( NonlinearOptimizer, Factorization ) // Values expected; // expected.insert(1, Pose2(0., 0., 0.)); // expected.insert(2, Pose2(1., 0., 0.)); -// CHECK(assert_equal(expected, *optimized.config(), 1e-5)); +// CHECK(assert_equal(expected, *optimized.values(), 1e-5)); //} /* ************************************************************************* */