121 lines
3.6 KiB
C++
121 lines
3.6 KiB
C++
/**********************************************************
|
|
Written by Frank Dellaert, Dec 2009
|
|
**********************************************************/
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#include "UrbanConfig.h"
|
|
#include "UrbanMeasurement.h"
|
|
#include "Vector.h"
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
|
|
/*
|
|
* This unit test cross checks UrbanMeasurement with the
|
|
* results from Justin's thesis codebase.
|
|
*/
|
|
|
|
static boost::shared_ptr<Matrix> sensorMatrix(new Matrix(4, 4));
|
|
|
|
static bool _data_inited = false;
|
|
|
|
static void _init() {
|
|
// Not MT-safe. Don't think that's an issue for CppUnitLite
|
|
if (_data_inited) {
|
|
return;
|
|
}
|
|
(*sensorMatrix)(0, 0) = 0.0251;
|
|
(*sensorMatrix)(0, 1) = 0.9996;
|
|
(*sensorMatrix)(0, 2) = 0.0091;
|
|
(*sensorMatrix)(0, 3) = 1.5574;
|
|
(*sensorMatrix)(1, 0) = -0.9996;
|
|
(*sensorMatrix)(1, 1) = 0.0253;
|
|
(*sensorMatrix)(1, 2) = -0.016;
|
|
(*sensorMatrix)(1, 3) = -0.86;
|
|
(*sensorMatrix)(2, 0) = -0.0163;
|
|
(*sensorMatrix)(2, 1) = -0.0087;
|
|
(*sensorMatrix)(2, 2) = 0.9998;
|
|
(*sensorMatrix)(2, 3) = -0.6478;
|
|
(*sensorMatrix)(3, 0) = 0;
|
|
(*sensorMatrix)(3, 1) = 0;
|
|
(*sensorMatrix)(3, 2) = 0;
|
|
(*sensorMatrix)(3, 3) = 1;
|
|
_data_inited = true;
|
|
|
|
}
|
|
/* Robot is at (0,0,0) looking in global "y" direction.
|
|
* For the local frame we used Navlab convention,
|
|
* x-looks forward, y-looks right, z- down*/
|
|
static const Pose3 robotPose(Matrix_(4, 4,
|
|
-0.0848808908061426, 0.993833753552749900, 0.0713421661796702400, 377.7509309858,
|
|
-0.9963883079644135, -0.08449313834814816, -0.008440931458908668, 1922.5523404841,
|
|
-0.002360959078213238, -0.07180097402774338, 0.9974161849503438, -6369417.591562273,
|
|
0.0, 0.0, 0.0, 1));
|
|
|
|
static const Point2 landmark(362.707949418, 1936.2843137418);
|
|
|
|
|
|
|
|
|
|
/* ************************************************************************* */
|
|
TEST( UrbanMeasurement, transform_to )
|
|
{
|
|
_init();
|
|
UrbanMeasurement::Transform tr = transform_to(sensorMatrix, robotPose, landmark, true);
|
|
|
|
// Check the transformation from global to sensor coordinates
|
|
CHECK(assert_equal(Point2(-12.40679724375481, -16.14944758846734),*(tr.get<0>())));
|
|
|
|
Point2 &foo = *(tr.get<0>());
|
|
|
|
// Check the jacobian w.r.t. the pose
|
|
CHECK(assert_equal(Matrix_(2, 6,
|
|
0.08471201853652961, 0.9964082882836194, 0.0, 0.03822695627705205, -0.5427133154140961, -16.15221468780303,
|
|
-0.998969460290058, 0.0851007754705408, 0.0, 0.5147498807397776, 0.0, 12.43765251715327),
|
|
*(tr.get<1>())));
|
|
|
|
// Check the jacobian w.r.t the landmark
|
|
CHECK(assert_equal(Matrix_(2, 2,
|
|
-0.08471201853652961, -0.9964082882836194,
|
|
0.998969460290058, -0.0851007754705408),
|
|
*(tr.get<2>())));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( UrbanMeasurement, error_vector )
|
|
{
|
|
_init();
|
|
|
|
Point2 z(-12.42876033350471, -16.10191021850896);
|
|
double sigma = 0.2;
|
|
UrbanMeasurement factor(sensorMatrix, z,sigma,44,66);
|
|
|
|
// Create a config
|
|
UrbanConfig config;
|
|
config.addRobotPose(44,robotPose);
|
|
config.addLandmark(66,landmark);
|
|
|
|
// test error_vector
|
|
Vector expected = Vector_(2,
|
|
-.02196308974990, .04753736995838);
|
|
Vector actual = factor.error_vector(config);
|
|
CHECK(assert_equal(expected,actual));
|
|
|
|
// test error. Expected was generated via manual calculation from
|
|
// error vector.
|
|
double expected2 = .03427723560;
|
|
double actual2 = factor.error(config);
|
|
DOUBLES_EQUAL(expected2,actual2,1e-9);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() {
|
|
TestResult tr;
|
|
TestRegistry::runAllTests(tr);
|
|
return 0;
|
|
}
|
|
/* ************************************************************************* */
|
|
|