66 lines
1.7 KiB
C++
66 lines
1.7 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;
|
|
|
|
Point2 landmark(2,5);
|
|
|
|
/* 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*/
|
|
Pose3 robotPose(Matrix_(3,3,
|
|
0., 1., 0.,
|
|
1., 0., 0.,
|
|
0., 0.,-1.
|
|
),
|
|
Point3(0,0,0));
|
|
|
|
/* ************************************************************************* */
|
|
TEST( UrbanMeasurement, transform_to )
|
|
{
|
|
Point2 expected(5,2);
|
|
Point2 actual = transform_to(robotPose, landmark);
|
|
CHECK(assert_equal(expected,actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( UrbanMeasurement, error_vector )
|
|
{
|
|
// Create a measurement, no-noise measurement would yield 5,2
|
|
Point2 z(4,2);
|
|
double sigma = 0.2;
|
|
UrbanMeasurement factor(z,sigma,44,66);
|
|
|
|
// Create a config
|
|
UrbanConfig config;
|
|
config.addRobotPose(44,robotPose);
|
|
config.addLandmark(66,landmark);
|
|
|
|
// test error_vector
|
|
Vector expected = Vector_(2, -1.0, 0.0);
|
|
Vector actual = factor.error_vector(config);
|
|
CHECK(assert_equal(expected,actual));
|
|
|
|
// test error
|
|
double expected2 = 12.5; // 0.5 * 5^2
|
|
double actual2 = factor.error(config);
|
|
DOUBLES_EQUAL(expected2,actual2,1e-9);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() {
|
|
TestResult tr;
|
|
TestRegistry::runAllTests(tr);
|
|
return 0;
|
|
}
|
|
/* ************************************************************************* */
|
|
|