delete SQP
							parent
							
								
									41684ee6e0
								
							
						
					
					
						commit
						016da71033
					
				|  | @ -1,37 +0,0 @@ | |||
| /**
 | ||||
|  * @file     NP.h | ||||
|  * @brief    Use this virtual class to represent a non-linear | ||||
|  *           problem. An implementation of this should provide | ||||
|  *           approximation and differentiation services to | ||||
|  *           allow SQP to work on the given problem. | ||||
|  * @author   Ivan Dario Jimenez | ||||
|  * @date     2/2/16 | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam_unstable/linear/QP.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| class NP { | ||||
| public: | ||||
|   /*
 | ||||
|    * This function takes the a point and returns a quadratic  | ||||
|    * problem approximation to the non-linear Problem. | ||||
|    */ | ||||
|   virtual QP approximate(const Vector& x) const = 0; | ||||
| 
 | ||||
|   /*
 | ||||
|    * Differentiates the objective function of the non-linear  | ||||
|    * problem at point x. | ||||
|    */ | ||||
|   virtual Vector objectiveDerivative(const Vector& x) const = 0; | ||||
| 
 | ||||
|   /*
 | ||||
|    * Differentiates the constraints at point x. | ||||
|    */ | ||||
|   virtual Vector constraintsDerivative(const Vector& x) const = 0; | ||||
| }; | ||||
| 
 | ||||
| } | ||||
|  | @ -1,176 +0,0 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * GTSAM Copyright 2010, Georgia Tech Research Corporation, | ||||
|  * Atlanta, Georgia 30332-0415 | ||||
|  * All Rights Reserved | ||||
|  * Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||||
| 
 | ||||
|  * See LICENSE for the license information | ||||
| 
 | ||||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /**
 | ||||
|  * @file    testPositionUpperBoundX.cpp | ||||
|  * @brief   Unit tests for testPositionUpperBoundX | ||||
|  * @author  Krunal Chande | ||||
|  * @author  Duy-Nguyen Ta | ||||
|  * @author  Luca Carlone | ||||
|  * @date    Dec 16, 2014 | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam/base/numericalDerivative.h> | ||||
| #include <gtsam/geometry/Pose3.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| #include <iostream> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| template<class T> | ||||
| class LinearInequalityManifold1: public NoiseModelFactor1<T> { | ||||
|   typedef NoiseModelFactor1<T> Base; | ||||
|   typedef LinearInequalityManifold1<T> This; | ||||
| protected: | ||||
|   bool active_; | ||||
| public: | ||||
| 
 | ||||
|   /// Constructor
 | ||||
|   LinearInequalityManifold1(Key key) : | ||||
|       Base(noiseModel::Constrained::All(1), key), active_(true) { | ||||
|   } | ||||
| 
 | ||||
|   /** implement functions needed for Testable */ | ||||
| 
 | ||||
|   /** print */ | ||||
|   virtual void print(const std::string& s, const KeyFormatter& keyFormatter = | ||||
|       DefaultKeyFormatter) const { | ||||
|     std::cout << s << " key = { " << keyFormatter(this->key()) << "}" | ||||
|         << std::endl; | ||||
|     if (active_) | ||||
|       std::cout << " Active" << std::endl; | ||||
|     else | ||||
|       std::cout << " Inactive" << std::endl; | ||||
|   } | ||||
| 
 | ||||
|   /** equals */ | ||||
|   virtual bool equals(const NonlinearFactor& expected, | ||||
|       double tol = 1e-9) const { | ||||
|     const This* e = dynamic_cast<const This*>(&expected); | ||||
|     return e != NULL && Base::equals(*e, tol) && (active_ == e->active_); | ||||
|   } | ||||
| 
 | ||||
|   /** Evaluate error MUST return a *one dimensional* vector,
 | ||||
|    * because we don't support multi-valued inequality factors | ||||
|    */ | ||||
|   virtual Vector evaluateError(const T& x, boost::optional<Matrix&> H = | ||||
|       boost::none) const = 0; | ||||
| 
 | ||||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  * This class defines an inequality upper bound on x for a Pose3 | ||||
|  *      x <= upperBound | ||||
|  */ | ||||
| class PositionUpperBoundX: public LinearInequalityManifold1<Pose3> { | ||||
|   double upperBound_; | ||||
|   typedef LinearInequalityManifold1<Pose3> Base; | ||||
|   typedef PositionUpperBoundX This; | ||||
| public: | ||||
|   PositionUpperBoundX(Key key, const double upperBound) : | ||||
|       Base(key), upperBound_(upperBound) { | ||||
|   } | ||||
| 
 | ||||
|   /// @return a deep copy of this factor
 | ||||
|   virtual gtsam::NonlinearFactor::shared_ptr clone() const { | ||||
|     return boost::static_pointer_cast<NonlinearFactor>( | ||||
|         gtsam::NonlinearFactor::shared_ptr(new This(*this))); | ||||
|   } | ||||
| 
 | ||||
|   /** implement functions needed for Testable */ | ||||
| 
 | ||||
|   /** print */ | ||||
|   virtual void print(const std::string& s, const KeyFormatter& keyFormatter = | ||||
|       DefaultKeyFormatter) const { | ||||
|     Base::print(s); | ||||
|     std::cout << "PositionUpperBoundX" << std::endl; | ||||
|     std::cout << "x <= " << upperBound_ << std::endl; | ||||
|   } | ||||
| 
 | ||||
|   /** equals */ | ||||
|   virtual bool equals(const NonlinearFactor& expected, | ||||
|       double tol = 1e-9) const { | ||||
|     const This* e = dynamic_cast<const This*>(&expected); | ||||
|     return e != NULL && Base::equals(*e, tol) && (upperBound_ == e->upperBound_); | ||||
|   } | ||||
| 
 | ||||
|   /**
 | ||||
|    * error = x - upperBound_ | ||||
|    */ | ||||
|   double computeError(const Pose3& pose) const { | ||||
|     return pose.x() - upperBound_; | ||||
|   } | ||||
| 
 | ||||
|   /**
 | ||||
|    * error = x - upperBound_ | ||||
|    */ | ||||
|   Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H = | ||||
|       boost::none) const { | ||||
|     if (H) | ||||
|       *H = (Matrix(1, 6) << 0, 0, 0, 1, 0, 0).finished(); | ||||
|     return (Vector(1) << computeError(pose)).finished(); | ||||
|   } | ||||
| }; | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam::symbol_shorthand; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| TEST(PositionUpperBoundX, equals ) { | ||||
|   // Instantiate a class PositionUpperBoundX
 | ||||
|   PositionUpperBoundX ineq1(X(1), 1); | ||||
| 
 | ||||
|   // Instantiate another class PositionUpperBoundX
 | ||||
|   PositionUpperBoundX ineq2(X(1), 1); | ||||
| 
 | ||||
|   // check equals
 | ||||
|   CHECK(ineq1.equals(ineq2, 1e-5)); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| TEST(PositionUpperBoundX, evaluateError ) { | ||||
|   // Instantiate a class PositionUpperBoundX
 | ||||
|   PositionUpperBoundX ineq(X(1), 45.6); | ||||
| 
 | ||||
|   Pose3 pose(Rot3::Ypr(0.1, 0.3, 0.2), Point3(43.0, 27.8, 91.1)); | ||||
|   Matrix H; | ||||
|   Vector error = ineq.evaluateError(pose, H); | ||||
| 
 | ||||
|   Matrix expectedH = numericalDerivative11<Vector, Pose3>( | ||||
|           boost::bind(&PositionUpperBoundX::evaluateError, ineq, _1, boost::none), | ||||
|       pose, 1e-5); | ||||
| 
 | ||||
|   cout << "expectedH: " << expectedH << endl; | ||||
|   cout << "H: " << H << endl; | ||||
| 
 | ||||
|   Vector expectedError = (Vector(1) << 0.0).finished(); | ||||
| //  CHECK(assert_equal(expectedError, error, 1e-100));
 | ||||
| //  CHECK(assert_equal(expectedH, H, 1e-100));
 | ||||
| 
 | ||||
|   Matrix hessian = numericalHessian<Pose3>( | ||||
|           boost::bind(&PositionUpperBoundX::computeError, ineq, _1), | ||||
|       pose, 1e-5); | ||||
|   cout << "Hessian: \n" << hessian << endl; | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| int main() { | ||||
|   TestResult tr; | ||||
|   return TestRegistry::runAllTests(tr); | ||||
| } | ||||
| //******************************************************************************
 | ||||
| 
 | ||||
		Loading…
	
		Reference in New Issue