90 lines
2.6 KiB
C++
90 lines
2.6 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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 testSmartRangeFactor.cpp
|
|
* @brief Unit tests for SmartRangeFactor Class
|
|
* @author Frank Dellaert
|
|
* @date Nov 2013
|
|
*/
|
|
|
|
#include <gtsam_unstable/slam/SmartRangeFactor.h>
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
const noiseModel::Base::shared_ptr gaussian = noiseModel::Isotropic::Sigma(1,
|
|
2.0);
|
|
|
|
/* ************************************************************************* */
|
|
|
|
TEST( SmartRangeFactor, constructor ) {
|
|
SmartRangeFactor f1;
|
|
LONGS_EQUAL(0, f1.nrMeasurements())
|
|
SmartRangeFactor f2(gaussian);
|
|
LONGS_EQUAL(0, f2.nrMeasurements())
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
TEST( SmartRangeFactor, addRange ) {
|
|
SmartRangeFactor f(gaussian);
|
|
f.addRange(1, 10);
|
|
f.addRange(1, 12);
|
|
LONGS_EQUAL(2, f.nrMeasurements())
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
TEST( SmartRangeFactor, unwhitenedError ) {
|
|
// Test situation:
|
|
Point2 p(0, 10);
|
|
Pose2 pose1(0, 0, 0), pose2(5, 0, 0), pose3(5, 5, 0);
|
|
double r1 = pose1.range(p), r2 = pose2.range(p), r3 = pose3.range(p);
|
|
DOUBLES_EQUAL(10, r1, 1e-9);
|
|
DOUBLES_EQUAL(sqrt(100+25), r2, 1e-9);
|
|
DOUBLES_EQUAL(sqrt(50), r3, 1e-9);
|
|
|
|
Values values; // all correct
|
|
values.insert(1, pose1);
|
|
values.insert(2, pose2);
|
|
values.insert(3, pose3);
|
|
|
|
SmartRangeFactor f(gaussian);
|
|
f.addRange(1, r1);
|
|
|
|
// Whenever there are two ranges or less, error should be zero
|
|
Vector actual1 = f.unwhitenedError(values);
|
|
EXPECT(assert_equal(Vector_(1,0.0), actual1));
|
|
f.addRange(2, r2);
|
|
Vector actual2 = f.unwhitenedError(values);
|
|
EXPECT(assert_equal(Vector2(0,0), actual2));
|
|
|
|
f.addRange(3, r3);
|
|
vector<Matrix> H;
|
|
Vector actual3 = f.unwhitenedError(values,H);
|
|
EXPECT(assert_equal(Vector3(0,0,0), actual3));
|
|
|
|
// Check keys and Jacobian
|
|
EXPECT_LONGS_EQUAL(3,f.keys().size());
|
|
EXPECT(assert_equal(Matrix_(1,3,0.0,-1.0,0.0), H.front()));
|
|
EXPECT(assert_equal(Matrix_(1,3,sqrt(2)/2,-sqrt(2)/2,0.0), H.back()));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() {
|
|
TestResult tr;
|
|
return TestRegistry::runAllTests(tr);
|
|
}
|
|
/* ************************************************************************* */
|
|
|