testTransformBtwRobotsUnaryFactor.cpp
Go to the documentation of this file.
1 
7 #include <CppUnitLite/TestHarness.h>
8 
9 
10 #include <gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h>
11 #include <gtsam/geometry/Pose2.h>
12 #include <gtsam/nonlinear/Values.h>
13 #include <gtsam/base/numericalDerivative.h>
14 
15 #include <gtsam/slam/BetweenFactor.h>
16 
17 #include <gtsam/nonlinear/NonlinearOptimizer.h>
18 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
19 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
20 
21 using namespace std::placeholders;
22 using namespace std;
23 using namespace gtsam;
24 
25 /* ************************************************************************* */
26 Vector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactor<gtsam::Pose2>& factor){
27  gtsam::Values values;
28  values.insert(key, org1_T_org2);
29  return factor.whitenedError(values);
30 }
31 
32 /* ************************************************************************* */
33 //Vector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor<gtsam::Pose2>& factor){
34 // gtsam::Values values;
35 // values.insert(keyA, p1);
36 // values.insert(keyB, p2);
37 // // Vector err = factor.whitenedError(values);
38 // // return err;
39 // return Vector::Expmap(factor.whitenedError(values));
40 //}
41 
42 /* ************************************************************************* */
43 TEST( TransformBtwRobotsUnaryFactor, ConstructorAndEquals)
44 {
45  gtsam::Key key(0);
46  gtsam::Key keyA(1);
47  gtsam::Key keyB(2);
48 
49  gtsam::Pose2 p1(10.0, 15.0, 0.1);
50  gtsam::Pose2 p2(15.0, 15.0, 0.3);
51  gtsam::Pose2 noise(0.5, 0.4, 0.01);
52  gtsam::Pose2 rel_pose_ideal = p1.between(p2);
53  gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
54 
55  SharedGaussian model(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05)));
56 
57  gtsam::Values valA, valB;
58  valA.insert(keyA, p1);
59  valB.insert(keyB, p2);
60 
61  // Constructor
62  TransformBtwRobotsUnaryFactor<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB, model);
63  TransformBtwRobotsUnaryFactor<gtsam::Pose2> h(key, rel_pose_msr, keyA, keyB, valA, valB, model);
64 
65  // Equals
66  CHECK(assert_equal(g, h, 1e-5));
67 }
68 
69 /* ************************************************************************* */
70 TEST( TransformBtwRobotsUnaryFactor, unwhitenedError)
71 {
72  gtsam::Key key(0);
73  gtsam::Key keyA(1);
74  gtsam::Key keyB(2);
75 
76  gtsam::Pose2 orgA_T_1(10.0, 15.0, 0.1);
77  gtsam::Pose2 orgB_T_2(15.0, 15.0, 0.3);
78 
79  gtsam::Pose2 orgA_T_orgB(100.0, 45.0, 1.8);
80 
81  gtsam::Pose2 orgA_T_2 = orgA_T_orgB.compose(orgB_T_2);
82 
83  gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2);
84  gtsam::Pose2 rel_pose_msr = rel_pose_ideal;
85 
86  SharedGaussian model(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05)));
87 
88  gtsam::Values valA, valB;
89  valA.insert(keyA, orgA_T_1);
90  valB.insert(keyB, orgB_T_2);
91 
92  // Constructor
93  TransformBtwRobotsUnaryFactor<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB, model);
94 
95  gtsam::Values values;
96  values.insert(key, orgA_T_orgB);
97  Vector err = g.unwhitenedError(values);
98 
99  // Equals
100  CHECK(assert_equal(err, (Vector) Z_3x1, 1e-5));
101 }
102 
103 /* ************************************************************************* */
104 TEST( TransformBtwRobotsUnaryFactor, unwhitenedError2)
105 {
106  gtsam::Key key(0);
107  gtsam::Key keyA(1);
108  gtsam::Key keyB(2);
109 
110  gtsam::Pose2 orgA_T_currA(0.0, 0.0, 0.0);
111  gtsam::Pose2 orgB_T_currB(-10.0, 15.0, 0.1);
112 
113  gtsam::Pose2 orgA_T_orgB(0.0, 0.0, 0.0);
114 
115  gtsam::Pose2 orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB);
116 
117  gtsam::Pose2 rel_pose_ideal = orgA_T_currA.between(orgA_T_currB);
118  gtsam::Pose2 rel_pose_msr = rel_pose_ideal;
119 
120  SharedGaussian model(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05)));
121 
122  gtsam::Values valA, valB;
123  valA.insert(keyA, orgA_T_currA);
124  valB.insert(keyB, orgB_T_currB);
125 
126  // Constructor
127  TransformBtwRobotsUnaryFactor<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB, model);
128 
129  gtsam::Values values;
130  values.insert(key, orgA_T_orgB);
131  Vector err = g.unwhitenedError(values);
132 
133  // Equals
134  CHECK(assert_equal(err, Z_3x1, 1e-5));
135 }
136 
137 /* ************************************************************************* */
138 TEST( TransformBtwRobotsUnaryFactor, Optimize)
139 {
140  gtsam::Key key(0);
141  gtsam::Key keyA(1);
142  gtsam::Key keyB(2);
143 
144  gtsam::Pose2 orgA_T_currA(0.0, 0.0, 0.0);
145  gtsam::Pose2 orgB_T_currB(1.0, 2.0, 0.05);
146 
147  gtsam::Pose2 orgA_T_orgB_tr(10.0, -15.0, 0.0);
148  gtsam::Pose2 orgA_T_currB_tr = orgA_T_orgB_tr.compose(orgB_T_currB);
149  gtsam::Pose2 currA_T_currB_tr = orgA_T_currA.between(orgA_T_currB_tr);
150 
151  // some error in measurements
152  // gtsam::Pose2 currA_Tmsr_currB1 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, 0.02, 0.01));
153  // gtsam::Pose2 currA_Tmsr_currB2 = currA_T_currB_tr.compose(gtsam::Pose2(-0.1, 0.02, 0.01));
154  // gtsam::Pose2 currA_Tmsr_currB3 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, -0.02, 0.01));
155  // gtsam::Pose2 currA_Tmsr_currB4 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, 0.02, -0.01));
156 
157  // ideal measurements
158  gtsam::Pose2 currA_Tmsr_currB1 = currA_T_currB_tr.compose(gtsam::Pose2(0.0, 0.0, 0.0));
159  gtsam::Pose2 currA_Tmsr_currB2 = currA_Tmsr_currB1;
160  gtsam::Pose2 currA_Tmsr_currB3 = currA_Tmsr_currB1;
161  gtsam::Pose2 currA_Tmsr_currB4 = currA_Tmsr_currB1;
162 
163  SharedGaussian model(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05)));
164 
165  gtsam::Values valA, valB;
166  valA.insert(keyA, orgA_T_currA);
167  valB.insert(keyB, orgB_T_currB);
168 
169  // Constructor
170  TransformBtwRobotsUnaryFactor<gtsam::Pose2> g1(key, currA_Tmsr_currB1, keyA, keyB, valA, valB, model);
171 
172  TransformBtwRobotsUnaryFactor<gtsam::Pose2> g2(key, currA_Tmsr_currB2, keyA, keyB, valA, valB, model);
173 
174  TransformBtwRobotsUnaryFactor<gtsam::Pose2> g3(key, currA_Tmsr_currB3, keyA, keyB, valA, valB, model);
175 
176  TransformBtwRobotsUnaryFactor<gtsam::Pose2> g4(key, currA_Tmsr_currB4, keyA, keyB, valA, valB, model);
177 
178  gtsam::Values values;
179  values.insert(key, gtsam::Pose2());
180 
181  gtsam::NonlinearFactorGraph graph;
182  graph.push_back(g1);
183  graph.push_back(g2);
184  graph.push_back(g3);
185  graph.push_back(g4);
186 
187  gtsam::GaussNewtonParams params;
188  gtsam::GaussNewtonOptimizer optimizer(graph, values, params);
189  gtsam::Values result = optimizer.optimize();
190 
191  gtsam::Pose2 orgA_T_orgB_opt = result.at<gtsam::Pose2>(key);
192 
193  CHECK(assert_equal(orgA_T_orgB_opt, orgA_T_orgB_tr, 1e-5));
194 }
195 
196 
197 /* ************************************************************************* */
198 TEST( TransformBtwRobotsUnaryFactor, Jacobian)
199 {
200  gtsam::Key key(0);
201  gtsam::Key keyA(1);
202  gtsam::Key keyB(2);
203 
204  gtsam::Pose2 orgA_T_1(10.0, 15.0, 0.1);
205  gtsam::Pose2 orgB_T_2(15.0, 15.0, 0.3);
206 
207  gtsam::Pose2 orgA_T_orgB(100.0, 45.0, 1.8);
208 
209  gtsam::Pose2 orgA_T_2 = orgA_T_orgB.compose(orgB_T_2);
210 
211  gtsam::Pose2 noise(0.5, 0.4, 0.01);
212 
213  gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2);
214  gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
215 
216  SharedGaussian model(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05)));
217 
218  gtsam::Values valA, valB;
219  valA.insert(keyA, orgA_T_1);
220  valB.insert(keyB, orgB_T_2);
221 
222  // Constructor
223  TransformBtwRobotsUnaryFactor<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB, model);
224 
225  gtsam::Values values;
226  values.insert(key, orgA_T_orgB);
227 
228  std::vector<gtsam::Matrix> H_actual(1);
229  Vector actual_err_wh = g.whitenedError(values, H_actual);
230 
231  Matrix H1_actual = H_actual[0];
232 
233  double stepsize = 1.0e-9;
234  Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, std::placeholders::_1, key, g), orgA_T_orgB, stepsize);
235 // CHECK( assert_equal(H1_expected, H1_actual, 1e-5));
236 }
237 
238 
239 
240 
242 //TEST (TransformBtwRobotsUnaryFactor, jacobian ) {
243 //
244 // gtsam::Key keyA(1);
245 // gtsam::Key keyB(2);
246 //
247 // // Inlier test
248 // gtsam::Pose2 p1(10.0, 15.0, 0.1);
249 // gtsam::Pose2 p2(15.0, 15.0, 0.3);
250 // gtsam::Pose2 noise(0.5, 0.4, 0.01);
251 // gtsam::Pose2 rel_pose_ideal = p1.between(p2);
252 // gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
253 //
254 // SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.5, 0.5, 0.05)));
255 // SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(50.0, 50.0, 10.0)));
256 //
257 // gtsam::Values values;
258 // values.insert(keyA, p1);
259 // values.insert(keyB, p2);
260 //
261 // double prior_outlier = 0.0;
262 // double prior_inlier = 1.0;
263 //
264 // TransformBtwRobotsUnaryFactor<gtsam::Pose2> f(keyA, keyB, rel_pose_msr, model_inlier, model_outlier,
265 // prior_inlier, prior_outlier);
266 //
267 // std::vector<gtsam::Matrix> H_actual(2);
268 // Vector actual_err_wh = f.whitenedError(values, H_actual);
269 //
270 // Matrix H1_actual = H_actual[0];
271 // Matrix H2_actual = H_actual[1];
272 //
273 // // compare to standard between factor
274 // BetweenFactor<gtsam::Pose2> h(keyA, keyB, rel_pose_msr, model_inlier );
275 // Vector actual_err_wh_stnd = h.whitenedError(values);
276 // Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
277 // CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8));
278 // std::vector<gtsam::Matrix> H_actual_stnd_unwh(2);
279 // (void)h.unwhitenedError(values, H_actual_stnd_unwh);
280 // Matrix H1_actual_stnd_unwh = H_actual_stnd_unwh[0];
281 // Matrix H2_actual_stnd_unwh = H_actual_stnd_unwh[1];
282 // Matrix H1_actual_stnd = model_inlier->Whiten(H1_actual_stnd_unwh);
283 // Matrix H2_actual_stnd = model_inlier->Whiten(H2_actual_stnd_unwh);
286 //
287 // double stepsize = 1.0e-9;
288 // Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, std::placeholders::_1, p2, keyA, keyB, f), p1, stepsize);
289 // Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, p1, std::placeholders::_1, keyA, keyB, f), p2, stepsize);
290 //
291 //
292 // // try to check numerical derivatives of a standard between factor
293 // Matrix H1_expected_stnd = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError_standard, std::placeholders::_1, p2, keyA, keyB, h), p1, stepsize);
294 // CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
295 //
296 //
297 // CHECK( assert_equal(H1_expected, H1_actual, 1e-8));
298 // CHECK( assert_equal(H2_expected, H2_actual, 1e-8));
299 //
300 //}
301 
302 /* ************************************************************************* */
303  int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
304 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::LieGroup::between
Class between(const Class &g) const
Definition: Lie.h:52
Pose2.h
2D Pose
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
predictionError
Vector predictionError(const Pose2 &org1_T_org2, const gtsam::Key &key, const TransformBtwRobotsUnaryFactor< gtsam::Pose2 > &factor)
Definition: testTransformBtwRobotsUnaryFactor.cpp:26
gtsam::GaussNewtonOptimizer
Definition: GaussNewtonOptimizer.h:38
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::Z_3x1
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:47
TestHarness.h
g1
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
NonlinearOptimizer.h
Base class and parameters for nonlinear optimization algorithms.
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:247
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
h
const double h
Definition: testSimpleHelicopter.cpp:19
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
result
Values result
Definition: OdometryOptimize.cpp:8
pruning_fixture::factor
DecisionTreeFactor factor(D &C &B &A, "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0")
GaussNewtonOptimizer.h
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
main
int main()
Definition: testTransformBtwRobotsUnaryFactor.cpp:303
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:259
numericalDerivative.h
Some functions to compute numerical derivatives.
BetweenFactor.h
g2
Pose3 g2(g1.expmap(h *V1_g1))
gtsam::GaussNewtonParams
Definition: GaussNewtonOptimizer.h:30
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::SharedGaussian
noiseModel::Gaussian::shared_ptr SharedGaussian
Definition: NoiseModel.h:763
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
TestResult
Definition: TestResult.h:26
key
const gtsam::Symbol key('X', 0)
gtsam
traits
Definition: ABC.h:17
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
keyA
Key keyA
Definition: testFunctorizedFactor.cpp:36
gtsam::Values
Definition: Values.h:65
TransformBtwRobotsUnaryFactor.h
Unary factor for determining transformation between given trajectories of two robots.
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:156
gtsam::TransformBtwRobotsUnaryFactor
Definition: TransformBtwRobotsUnaryFactor.h:35
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:41
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
TEST
TEST(TransformBtwRobotsUnaryFactor, ConstructorAndEquals)
Definition: testTransformBtwRobotsUnaryFactor.cpp:43
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam::Pose2
Definition: Pose2.h:39

gtsam
Author(s):
autogenerated on Wed May 28 2025 03:07:47