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