removed obsolete tests
Frank Dellaert
parent
b0f27c5d4b
commit
2605d18152
|
|
@ -1,103 +0,0 @@
|
|||
/**
|
||||
* @file testBearingFactor.cpp
|
||||
* @brief Unit tests for BearingFactor Class
|
||||
* @authors Frank Dellaert
|
||||
**/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include "numericalDerivative.h"
|
||||
#include "BearingFactor.h"
|
||||
#include "TupleConfig.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// typedefs
|
||||
typedef Symbol<Pose2, 'x'> PoseKey;
|
||||
typedef Symbol<Point2, 'l'> PointKey;
|
||||
typedef PairConfig<PoseKey, Pose2, PointKey, Point2> Config;
|
||||
typedef BearingFactor<Config, PoseKey, PointKey> MyFactor;
|
||||
|
||||
// some shared test values
|
||||
Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
|
||||
Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( BearingFactor, relativeBearing )
|
||||
{
|
||||
Matrix expectedH, actualH;
|
||||
|
||||
// establish relativeBearing is indeed zero
|
||||
Rot2 actual1 = relativeBearing(l1, actualH);
|
||||
CHECK(assert_equal(Rot2(),actual1));
|
||||
|
||||
// Check numerical derivative
|
||||
expectedH = numericalDerivative11(relativeBearing, l1, 1e-5);
|
||||
CHECK(assert_equal(expectedH,actualH));
|
||||
|
||||
// establish relativeBearing is indeed 45 degrees
|
||||
Rot2 actual2 = relativeBearing(l2, actualH);
|
||||
CHECK(assert_equal(Rot2(M_PI_4),actual2));
|
||||
|
||||
// Check numerical derivative
|
||||
expectedH = numericalDerivative11(relativeBearing, l2, 1e-5);
|
||||
CHECK(assert_equal(expectedH,actualH));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( BearingFactor, bearing )
|
||||
{
|
||||
Matrix expectedH1, actualH1, expectedH2, actualH2;
|
||||
|
||||
// establish bearing is indeed zero
|
||||
CHECK(assert_equal(Rot2(),bearing(x1,l1)));
|
||||
|
||||
// establish bearing is indeed 45 degrees
|
||||
CHECK(assert_equal(Rot2(M_PI_4),bearing(x1,l2)));
|
||||
|
||||
// establish bearing is indeed 45 degrees even if shifted
|
||||
Rot2 actual23 = bearing(x2, l3, actualH1, actualH2);
|
||||
CHECK(assert_equal(Rot2(M_PI_4),actual23));
|
||||
|
||||
// Check numerical derivatives
|
||||
expectedH1 = numericalDerivative21(bearing, x2, l3, 1e-5);
|
||||
CHECK(assert_equal(expectedH1,actualH1));
|
||||
expectedH2 = numericalDerivative22(bearing, x2, l3, 1e-5);
|
||||
CHECK(assert_equal(expectedH1,actualH1));
|
||||
|
||||
// establish bearing is indeed 45 degrees even if rotated
|
||||
Rot2 actual34 = bearing(x3, l4, actualH1, actualH2);
|
||||
CHECK(assert_equal(Rot2(M_PI_4),actual34));
|
||||
|
||||
// Check numerical derivatives
|
||||
expectedH1 = numericalDerivative21(bearing, x3, l4, 1e-5);
|
||||
expectedH2 = numericalDerivative22(bearing, x3, l4, 1e-5);
|
||||
CHECK(assert_equal(expectedH1,actualH1));
|
||||
CHECK(assert_equal(expectedH1,actualH1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( BearingFactor, evaluateError )
|
||||
{
|
||||
// Create factor
|
||||
Rot2 z(M_PI_4+0.1); // h(x) - z = -0.1
|
||||
double sigma = 0.1;
|
||||
MyFactor factor(z, sigma, 2, 3);
|
||||
|
||||
// create config
|
||||
Config c;
|
||||
c.insert(2, x2);
|
||||
c.insert(3, l3);
|
||||
|
||||
// Check error
|
||||
Vector actual = factor.error_vector(c);
|
||||
CHECK(assert_equal(Vector_(1,-0.1),actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -1,62 +0,0 @@
|
|||
/**
|
||||
* @file testBearingRange.cpp
|
||||
* @authors Frank Dellaert
|
||||
**/
|
||||
|
||||
#include <iostream>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/BearingFactor.h>
|
||||
#include <gtsam/RangeFactor.h>
|
||||
#include <gtsam/TupleConfig.h>
|
||||
#include <gtsam/FactorGraph-inl.h>
|
||||
#include <gtsam/NonlinearFactorGraph-inl.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// typedefs
|
||||
typedef Symbol<Pose2, 'x'> PoseKey;
|
||||
typedef Symbol<Point2, 'l'> PointKey;
|
||||
typedef PairConfig<PoseKey, Pose2, PointKey, Point2> Config;
|
||||
typedef BearingFactor<Config, PoseKey, PointKey> BearingMeasurement;
|
||||
typedef RangeFactor<Config, PoseKey, PointKey> RangeMeasurement;
|
||||
typedef NonlinearFactorGraph<Config> Graph;
|
||||
|
||||
// some shared test values
|
||||
Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
|
||||
Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( BearingRange, constructor )
|
||||
{
|
||||
// create config
|
||||
Config c;
|
||||
c.insert(2, x2);
|
||||
c.insert(3, l3);
|
||||
|
||||
// create graph
|
||||
Graph G;
|
||||
|
||||
// Create bearing factor
|
||||
Rot2 z1(M_PI_4 + 0.1); // h(x) - z = -0.1
|
||||
double sigma1 = 0.1;
|
||||
Graph::sharedFactor factor1(new BearingMeasurement(z1, sigma1, 2, 3));
|
||||
CHECK(assert_equal(Vector_(1,-0.1),factor1->error_vector(c)));
|
||||
G.push_back(factor1);
|
||||
|
||||
// Create range factor
|
||||
double z2(sqrt(2) - 0.22); // h(x) - z = 0.22
|
||||
double sigma2 = 0.1;
|
||||
Graph::sharedFactor factor2(new RangeMeasurement(z2, sigma2, 2, 3));
|
||||
CHECK(assert_equal(Vector_(1,0.22),factor2->error_vector(c)));
|
||||
G.push_back(factor2);
|
||||
|
||||
CHECK(assert_equal(Vector_(2,-0.1,0.22),G.error_vector(c)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
/**
|
||||
* @file testRangeFactor.cpp
|
||||
* @brief Unit tests for RangeFactor Class
|
||||
* @authors Frank Dellaert
|
||||
**/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include "numericalDerivative.h"
|
||||
#include "RangeFactor.h"
|
||||
#include "TupleConfig.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// typedefs
|
||||
typedef Symbol<Pose2, 'x'> PoseKey;
|
||||
typedef Symbol<Point2, 'l'> PointKey;
|
||||
typedef PairConfig<PoseKey, Pose2, PointKey, Point2> Config;
|
||||
typedef RangeFactor<Config, PoseKey, PointKey> MyFactor;
|
||||
|
||||
// some shared test values
|
||||
Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
|
||||
Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( RangeFactor, range )
|
||||
{
|
||||
Matrix expectedH1, actualH1, expectedH2, actualH2;
|
||||
|
||||
// establish range is indeed zero
|
||||
DOUBLES_EQUAL(1,gtsam::range(x1,l1),1e-9);
|
||||
|
||||
// establish range is indeed 45 degrees
|
||||
DOUBLES_EQUAL(sqrt(2),gtsam::range(x1,l2),1e-9);
|
||||
|
||||
// Another pair
|
||||
double actual23 = gtsam::range(x2, l3, actualH1, actualH2);
|
||||
DOUBLES_EQUAL(sqrt(2),actual23,1e-9);
|
||||
|
||||
// Check numerical derivatives
|
||||
expectedH1 = numericalDerivative21(range, x2, l3, 1e-5);
|
||||
CHECK(assert_equal(expectedH1,actualH1));
|
||||
expectedH2 = numericalDerivative22(range, x2, l3, 1e-5);
|
||||
CHECK(assert_equal(expectedH1,actualH1));
|
||||
|
||||
// Another test
|
||||
double actual34 = gtsam::range(x3, l4, actualH1, actualH2);
|
||||
DOUBLES_EQUAL(2,actual34,1e-9);
|
||||
|
||||
// Check numerical derivatives
|
||||
expectedH1 = numericalDerivative21(range, x3, l4, 1e-5);
|
||||
expectedH2 = numericalDerivative22(range, x3, l4, 1e-5);
|
||||
CHECK(assert_equal(expectedH1,actualH1));
|
||||
CHECK(assert_equal(expectedH1,actualH1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( RangeFactor, evaluateError )
|
||||
{
|
||||
// Create factor
|
||||
double z(sqrt(2) - 0.22); // h(x) - z = 0.22
|
||||
double sigma = 0.1;
|
||||
MyFactor factor(z, sigma, 2, 3);
|
||||
|
||||
// create config
|
||||
Config c;
|
||||
c.insert(2, x2);
|
||||
c.insert(3, l3);
|
||||
|
||||
// Check error
|
||||
Vector actual = factor.error_vector(c);
|
||||
CHECK(assert_equal(Vector_(1,0.22),actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue