1/**
2 * @file timeFrobeniusFactor.cpp
3 * @brief Time FrobeniusBetweenFactor::evaluateError with derivatives for various Lie groups.
4 * @author Frank Dellaert
5 * @date April 2024
6 */
7
8#include <gtsam/slam/FrobeniusFactor.h>
9#include <gtsam/base/timing.h>
10
11 // Include all Lie Groups to be timed
12#include <gtsam/geometry/Rot2.h>
13#include <gtsam/geometry/Rot3.h>
14#include <gtsam/geometry/Pose2.h>
15#include <gtsam/geometry/Pose3.h>
16#include <gtsam/geometry/SOn.h>
17#include <gtsam/geometry/SO3.h>
18#include <gtsam/geometry/SO4.h>
19#include <gtsam/geometry/Gal3.h>
20#include <gtsam/navigation/NavState.h>
21
22#include <iostream>
23#include <string>
24
25using namespace gtsam;
26
27// Define a sufficiently large number of iterations for stable timing.
28static constexpr size_t NUM_ITERATIONS = 10000000;
29
30// Create CSV file for results
31std::ofstream os("timeFrobeniusFactor.csv");
32
33/**
34 * @brief Time evaluateError for a given Lie group type.
35 * This function creates a FrobeniusBetweenFactor for a given Lie group type `T`,
36 * and times the `evaluateError` method including Jacobian calculations.
37 * @tparam T The Lie group type (e.g., Rot3, Pose3). It must be a MatrixLieGroup.
38 * @param name A string identifier for the type, used for printing results.
39 */
40template <typename T>
41void timeOne(const std::string& name) {
42 Key key1(1), key2(2);
43
44 // Use random values to avoid any special cases (e.g. identity).
45 // Using ChartAtOrigin::Retract is the modern GTSAM way.
46 T T1 = T::ChartAtOrigin::Retract(Vector::Random(T::dimension));
47 T T2 = T::ChartAtOrigin::Retract(Vector::Random(T::dimension));
48 T T12 = T::ChartAtOrigin::Retract(Vector::Random(T::dimension));
49
50 // Create the factor. The noise model is not important for timing evaluateError.
51 FrobeniusBetweenFactor<T> factor(key1, key2, T12, nullptr);
52
53 // Get dimensions for Jacobian matrices.
54 // N is the matrix dimension (e.g., 3 for SO(3)), Dim is N*N.
55 // D is the dimension of the tangent space.
56 constexpr auto N = T::LieAlgebra::RowsAtCompileTime;
57 constexpr auto Dim = N * N;
58 constexpr auto D = T::dimension;
59 Matrix H1(Dim, D), H2(Dim, D);
60
61 // Warmup call to make sure code and data are in cache.
62 for (int i = 0; i < 100; ++i) {
63 factor.evaluateError(T1, T2, &H1, &H2);
64 }
65
66 // Timed loop.
67 static const size_t id_tic = ::gtsam::internal::getTicTocID(description: name.c_str());
68 ::gtsam::internal::AutoTicToc obj(id_tic, name.c_str());
69 for (size_t t = 0; t < NUM_ITERATIONS; t++) {
70 // We pass H1 and H2 to ensure Jacobians are computed.
71 factor.evaluateError(T1, T2, &H1, &H2);
72 }
73 obj.stop();
74 auto timer = ::gtsam::internal::gCurrentTimer.lock()->child(child: id_tic, label: name.c_str(), thisPtr: ::gtsam::internal::gCurrentTimer);
75 os << timer->secs() / NUM_ITERATIONS << ", \n";
76 std::cout << name << ":\t" << timer->secs()*1e9/NUM_ITERATIONS << " ns" << std::endl;
77}
78
79/*************************************************************************************/
80int main(void) {
81 // Announce the test and number of iterations.
82 std::cout << "Timing FrobeniusBetweenFactor::evaluateError with derivatives for " << NUM_ITERATIONS
83 << " iterations.\n" << std::endl;
84
85 // Time all the specified MatrixLieGroup classes.
86 timeOne<Rot2>(name: "Rot2");
87 timeOne<SO3>(name: "SO3");
88 timeOne<Rot3>(name: "Rot3");
89 timeOne<Pose2>(name: "Pose2");
90 timeOne<SO4>(name: "SO4");
91 timeOne<Pose3>(name: "Pose3");
92 timeOne<Gal3>(name: "Gal3");
93 timeOne<NavState>(name: "NavState");
94
95 return 0;
96}
97/*************************************************************************************/