diff --git a/gtsam_unstable/linear/NP.h b/gtsam_unstable/linear/NP.h deleted file mode 100644 index 309b4c2f8..000000000 --- a/gtsam_unstable/linear/NP.h +++ /dev/null @@ -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 - -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; -}; - -} diff --git a/gtsam_unstable/slam/tests/testPositionConstraints.cpp b/gtsam_unstable/slam/tests/testPositionConstraints.cpp deleted file mode 100644 index bacf5dcbf..000000000 --- a/gtsam_unstable/slam/tests/testPositionConstraints.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include - -namespace gtsam { - -template -class LinearInequalityManifold1: public NoiseModelFactor1 { - typedef NoiseModelFactor1 Base; - typedef LinearInequalityManifold1 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(&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 H = - boost::none) const = 0; - -}; - -/** - * This class defines an inequality upper bound on x for a Pose3 - * x <= upperBound - */ -class PositionUpperBoundX: public LinearInequalityManifold1 { - double upperBound_; - typedef LinearInequalityManifold1 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( - 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(&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 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( - 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( - boost::bind(&PositionUpperBoundX::computeError, ineq, _1), - pose, 1e-5); - cout << "Hessian: \n" << hessian << endl; -} - -//****************************************************************************** -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -//****************************************************************************** -