test files deprecated, might be removed later
krunalchande
thduynguyen
parent
6d76b5910c
commit
147d174a66
|
|
@ -0,0 +1,65 @@
|
|||
///* ----------------------------------------------------------------------------
|
||||
//
|
||||
// * 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 testLinearInequalityManifolds.cpp
|
||||
// * @brief Unit tests for testLinearInequalityManifolds
|
||||
// * @author Krunal Chande
|
||||
// * @author Duy-Nguyen Ta
|
||||
// * @author Luca Carlone
|
||||
// * @date Dec 15, 2014
|
||||
// */
|
||||
//
|
||||
//#include <gtsam/inference/Symbol.h>
|
||||
//#include <CppUnitLite/TestHarness.h>
|
||||
//#include <iostream>
|
||||
//
|
||||
//using namespace std;
|
||||
//using namespace gtsam::symbol_shorthand;
|
||||
//using namespace gtsam;
|
||||
//const double tol = 1e-10;
|
||||
//
|
||||
//namespace gtsam {
|
||||
//class PositionConstraint : public LinearInequalityManifold1 {
|
||||
//public:
|
||||
// PositionConstraint()
|
||||
//};
|
||||
//}
|
||||
////******************************************************************************
|
||||
//TEST(testLinearEqualityManifolds, equals ) {
|
||||
// // Instantiate a class LinearEqualityManifolds
|
||||
// LinearEqualityManifolds le1();
|
||||
//
|
||||
// // Instantiate another class LinearEqualityManifolds
|
||||
// LinearEqualityManifolds le2();
|
||||
//
|
||||
// // check equals
|
||||
// CHECK(assert_equal(le1,le1));
|
||||
// CHECK(le2.equals(le2));
|
||||
// CHECK(!le2.equals(le1));
|
||||
// CHECK(!le1.equals(le2));
|
||||
//}
|
||||
//
|
||||
//////******************************************************************************
|
||||
////TEST(testLinearEqualityManifolds, linearize ) {
|
||||
////}
|
||||
////
|
||||
//////******************************************************************************
|
||||
////TEST(testLinearEqualityManifolds, size ) {
|
||||
////}
|
||||
//
|
||||
////******************************************************************************
|
||||
//int main() {
|
||||
// TestResult tr;
|
||||
// return TestRegistry::runAllTests(tr);
|
||||
//}
|
||||
////******************************************************************************
|
||||
//
|
||||
|
|
@ -0,0 +1,176 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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