testSmartRangeFactor.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
23 
24 using namespace std;
25 using namespace gtsam;
26 
27 static const double sigma = 2.0;
28 
29 // Test situation:
30 static const Point2 p(0, 10);
31 static const Pose2 pose1(0, 0, 0), pose2(5, 0, 0), pose3(5, 5, 0);
32 static const double r1 = pose1.range(p), r2 = pose2.range(p), r3 = pose3.range(
33  p);
34 
35 /* ************************************************************************* */
36 
39  LONGS_EQUAL(0, f1.size())
41  LONGS_EQUAL(0, f2.size())
42 }
43 /* ************************************************************************* */
44 
45 TEST( SmartRangeFactor, addRange ) {
47  f.addRange(1, 10);
48  f.addRange(2, 12);
49  LONGS_EQUAL(2, f.size())
50 }
51 /* ************************************************************************* */
52 
54  DOUBLES_EQUAL(10, r1, 1e-9);
55  DOUBLES_EQUAL(sqrt(100.0+25.0), r2, 1e-9);
56  DOUBLES_EQUAL(sqrt(50.0), r3, 1e-9);
57 }
58 /* ************************************************************************* */
59 
60 TEST( SmartRangeFactor, unwhitenedError ) {
61  Values values; // all correct
62  values.insert(1, pose1);
63  values.insert(2, pose2);
64  values.insert(3, pose3);
65 
67  f.addRange(1, r1);
68 
69  // Check Jacobian for n==1
70  vector<Matrix> H1(1);
71  f.unwhitenedError(values, H1); // with H now !
72  CHECK(assert_equal(Matrix::Zero(3,1), H1.front()));
73 
74  // Whenever there are two ranges or less, error should be zero
75  Vector actual1 = f.unwhitenedError(values);
76  EXPECT(assert_equal((Vector(1) << 0.0).finished(), actual1));
77  f.addRange(2, r2);
78  Vector actual2 = f.unwhitenedError(values);
79  EXPECT(assert_equal((Vector(1) << 0.0).finished(), actual2));
80 
81  f.addRange(3, r3);
82  vector<Matrix> H(3);
83  Vector actual3 = f.unwhitenedError(values);
84  EXPECT_LONGS_EQUAL(3, f.keys().size());
85  EXPECT(assert_equal((Vector(1) << 0.0).finished(), actual3));
86 
87  // Check keys and Jacobian
88  Vector actual4 = f.unwhitenedError(values, H); // with H now !
89  EXPECT(assert_equal((Vector(1) << 0.0).finished(), actual4));
90  CHECK(assert_equal((Matrix(1, 3) << 0.0,-1.0,0.0).finished(), H.front()));
91  CHECK(assert_equal((Matrix(1, 3) << sqrt(2.0)/2,-sqrt(2.0)/2,0.0).finished(), H.back()));
92 
93  // Test clone
94  NonlinearFactor::shared_ptr clone = f.clone();
95  EXPECT_LONGS_EQUAL(3, clone->keys().size());
96 }
97 /* ************************************************************************* */
98 
99 TEST( SmartRangeFactor, optimization ) {
101  f.addRange(1, r1);
102  f.addRange(2, r2);
103  f.addRange(3, r3);
104 
105  // Create initial value for optimization
106  Values initial;
107  initial.insert(1, pose1);
108  initial.insert(2, pose2);
109  initial.insert(3, Pose2(5, 6, 0)); // does not satisfy range measurement
110  Vector actual5 = f.unwhitenedError(initial);
111  EXPECT(assert_equal((Vector(1) << sqrt(25.0+16.0)-sqrt(50.0)).finished(), actual5));
112 
113  // Create Factor graph
115  graph.push_back(f);
117  priorNoise = noiseModel::Diagonal::Sigmas(Vector3(1, 1, M_PI));
120 
121  // Try optimizing
123  // params.setVerbosity("ERROR");
125  Values result = optimizer.optimize();
128  // only the third pose will be changed, converges on following:
129  EXPECT(assert_equal(Pose2(5.52159, 5.582727, 0), result.at<Pose2>(3),1e-5));
130 }
131 
132 /* ************************************************************************* */
133 int main() {
134  TestResult tr;
135  return TestRegistry::runAllTests(tr);
136 }
137 /* ************************************************************************* */
138 
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
pose1
static const Pose2 pose1(0, 0, 0)
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::noiseModel::Base::shared_ptr
std::shared_ptr< Base > shared_ptr
Definition: NoiseModel.h:60
TestHarness.h
r2
static const double r2
Definition: testSmartRangeFactor.cpp:32
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
initial
Values initial
Definition: OdometryOptimize.cpp:2
f2
double f2(const Vector2 &x)
Definition: testNumericalDerivative.cpp:56
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
r1
static const double r1
Definition: testSmartRangeFactor.cpp:32
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
gtsam::SmartRangeFactor
Definition: SmartRangeFactor.h:30
r3
static const double r3
Definition: testSmartRangeFactor.cpp:32
sigma
static const double sigma
Definition: testSmartRangeFactor.cpp:27
forward::scenario
ConstantTwistScenario scenario(Z_3x1, Vector3(v, 0, 0))
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
priorNoise
auto priorNoise
Definition: doc/Code/OdometryExample.cpp:6
TestResult
Definition: TestResult.h:26
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
TEST
TEST(SmartRangeFactor, constructor)
Definition: testSmartRangeFactor.cpp:37
p
static const Point2 p(0, 10)
gtsam
traits
Definition: chartTesting.h:28
SmartRangeFactor.h
A smart factor for range-only SLAM that does initialization and marginalization.
constructor
Definition: init.h:200
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
leaf::values
leaf::MyValues values
gtsam::Values
Definition: Values.h:65
CHECK
#define CHECK(condition)
Definition: Test.h:108
std
Definition: BFloat16.h:88
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
pose2
static const Pose2 pose2(5, 0, 0)
main
int main()
Definition: testSmartRangeFactor.cpp:133
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
gtsam::Pose2::range
double range(const Point2 &point, OptionalJacobian< 1, 3 > H1={}, OptionalJacobian< 1, 2 > H2={}) const
Definition: Pose2.cpp:270
initial
Definition: testScenarioRunner.cpp:148
unary::f1
Point2 f1(const Point3 &p, OptionalJacobian< 2, 3 > H)
Definition: testExpression.cpp:79
M_PI
#define M_PI
Definition: mconf.h:117
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
pose3
static const Pose2 pose3(5, 5, 0)
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
gtsam::Pose2
Definition: Pose2.h:39


gtsam
Author(s):
autogenerated on Thu Jun 13 2024 03:10:28