delete SQP
Duy-Nguyen Ta
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