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