1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010-2019, 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
12/**
13 * @file testShonanAveraging.cpp
14 * @date September 2019
15 * @author Jing Wu
16 * @author Frank Dellaert
17 * @brief Timing script for Shonan Averaging algorithm
18 */
19
20#include "gtsam/base/Matrix.h"
21#include "gtsam/base/Vector.h"
22#include "gtsam/geometry/Point3.h"
23#include "gtsam/geometry/Rot3.h"
24#include <gtsam/base/timing.h>
25#include <gtsam/sfm/ShonanAveraging.h>
26
27#include <CppUnitLite/TestHarness.h>
28
29#include <chrono>
30#include <fstream>
31#include <iostream>
32#include <map>
33
34using namespace std;
35using namespace gtsam;
36
37// save a single line of timing info to an output stream
38void saveData(size_t p, double time1, double costP, double cost3, double time2,
39 double min_eigenvalue, double suBound, std::ostream* os) {
40 *os << static_cast<int>(p) << "\t" << time1 << "\t" << costP << "\t" << cost3
41 << "\t" << time2 << "\t" << min_eigenvalue << "\t" << suBound << endl;
42}
43
44void checkR(const Matrix& R) {
45 Matrix R2 = R.transpose();
46 Matrix actual_R = R2 * R;
47 assert_equal(expected: Rot3(), actual: Rot3(actual_R));
48}
49
50void saveResult(string name, const Values& values) {
51 ofstream myfile;
52 myfile.open(s: "shonan_result_of_" + name + ".dat");
53 size_t nrSO3 = values.count<SO3>();
54 myfile << "#Type SO3 Number " << nrSO3 << "\n";
55 for (size_t i = 0; i < nrSO3; ++i) {
56 Matrix R = values.at<SO3>(j: i).matrix();
57 // Check if the result of R.Transpose*R satisfy orthogonal constraint
58 checkR(R);
59 myfile << i;
60 for (int m = 0; m < 3; ++m) {
61 for (int n = 0; n < 3; ++n) {
62 myfile << " " << R(m, n);
63 }
64 }
65 myfile << "\n";
66 }
67 myfile.close();
68 cout << "Saved shonan_result.dat file" << endl;
69}
70
71void saveG2oResult(string name, const Values& values, std::map<Key, Pose3> poses) {
72 ofstream myfile;
73 myfile.open(s: "shonan_result_of_" + name + ".g2o");
74 size_t nrSO3 = values.count<SO3>();
75 for (size_t i = 0; i < nrSO3; ++i) {
76 Matrix R = values.at<SO3>(j: i).matrix();
77 // Check if the result of R.Transpose*R satisfy orthogonal constraint
78 checkR(R);
79 myfile << "VERTEX_SE3:QUAT" << " ";
80 myfile << i << " ";
81 myfile << poses[i].x() << " " << poses[i].y() << " " << poses[i].z() << " ";
82 Quaternion quaternion = Rot3(R).toQuaternion();
83 myfile << quaternion.x() << " " << quaternion.y() << " " << quaternion.z()
84 << " " << quaternion.w();
85 myfile << "\n";
86 }
87 myfile.close();
88 cout << "Saved shonan_result.g2o file" << endl;
89}
90
91void saveResultQuat(const Values& values) {
92 ofstream myfile;
93 myfile.open(s: "shonan_result.dat");
94 size_t nrSOn = values.count<SOn>();
95 for (size_t i = 0; i < nrSOn; ++i) {
96 GTSAM_PRINT(values.at<SOn>(i));
97 Rot3 R = Rot3(values.at<SOn>(j: i).matrix());
98 float x = R.toQuaternion().x();
99 float y = R.toQuaternion().y();
100 float z = R.toQuaternion().z();
101 float w = R.toQuaternion().w();
102 myfile << "QuatSO3 " << i;
103 myfile << "QuatSO3 " << i << " " << w << " " << x << " " << y << " " << z << "\n";
104 myfile.close();
105 }
106}
107
108int main(int argc, char* argv[]) {
109 // primitive argument parsing:
110 if (argc > 3) {
111 throw runtime_error("Usage: timeShonanAveraging [g2oFile]");
112 }
113
114 string g2oFile;
115 try {
116 if (argc > 1)
117 g2oFile = argv[argc - 1];
118 else
119 g2oFile = findExampleDataFile(name: "sphere2500");
120
121 } catch (const exception& e) {
122 cerr << e.what() << '\n';
123 exit(status: 1);
124 }
125
126 // Create a csv output file
127 size_t pos1 = g2oFile.find(s: "data/");
128 size_t pos2 = g2oFile.find(s: ".g2o");
129 string name = g2oFile.substr(pos: pos1 + 5, n: pos2 - pos1 - 5);
130 cout << name << endl;
131 ofstream csvFile("shonan_timing_of_" + name + ".csv");
132
133 // Create Shonan averaging instance from the file.
134 // ShonanAveragingParameters parameters;
135 // double sigmaNoiseInRadians = 0 * M_PI / 180;
136 // parameters.setNoiseSigma(sigmaNoiseInRadians);
137 static const ShonanAveraging3 kShonan(g2oFile);
138
139 // increase p value and try optimize using Shonan Algorithm. use chrono for
140 // timing
141 size_t pMin = 3;
142 Values Qstar;
143 Vector minEigenVector;
144 double CostP = 0, Cost3 = 0, lambdaMin = 0, suBound = 0;
145 cout << "(int)p" << "\t" << "time1" << "\t" << "costP" << "\t" << "cost3" << "\t"
146 << "time2" << "\t" << "MinEigenvalue" << "\t" << "SuBound" << endl;
147
148 const Values randomRotations = kShonan.initializeRandomly();
149
150 for (size_t p = pMin; p <= 7; p++) {
151 // Randomly initialize at lowest level, initialize by line search after that
152 const Values initial =
153 (p > pMin) ? kShonan.initializeWithDescent(p, values: Qstar, minEigenVector,
154 minEigenValue: lambdaMin)
155 : ShonanAveraging3::LiftTo<Rot3>(p: pMin, values: randomRotations);
156 chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
157 // optimizing
158 const Values result = kShonan.tryOptimizingAt(p, initial);
159 chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
160 chrono::duration<double> timeUsed1 =
161 chrono::duration_cast<chrono::duration<double>>(d: t2 - t1);
162 lambdaMin = kShonan.computeMinEigenValue(values: result, minEigenVector: &minEigenVector);
163 chrono::steady_clock::time_point t3 = chrono::steady_clock::now();
164 chrono::duration<double> timeUsed2 =
165 chrono::duration_cast<chrono::duration<double>>(d: t3 - t1);
166 Qstar = result;
167 CostP = kShonan.costAt(p, values: result);
168 const Values SO3Values = kShonan.roundSolution(values: result);
169 Cost3 = kShonan.cost(values: SO3Values);
170 suBound = (Cost3 - CostP) / CostP;
171
172 saveData(p, time1: timeUsed1.count(), costP: CostP, cost3: Cost3, time2: timeUsed2.count(),
173 min_eigenvalue: lambdaMin, suBound, os: &cout);
174 saveData(p, time1: timeUsed1.count(), costP: CostP, cost3: Cost3, time2: timeUsed2.count(),
175 min_eigenvalue: lambdaMin, suBound, os: &csvFile);
176 }
177 saveResult(name, values: kShonan.roundSolution(values: Qstar));
178 // saveG2oResult(name, kShonan.roundSolution(Qstar), kShonan.Poses());
179 return 0;
180}
181