35 using namespace gtsam;
38 void 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;
52 myfile.open(
"shonan_result_of_" + name +
".dat");
54 myfile <<
"#Type SO3 Number " << nrSO3 <<
"\n";
55 for (
size_t i = 0;
i < nrSO3; ++
i) {
60 for (
int m = 0;
m < 3; ++
m) {
61 for (
int n = 0;
n < 3; ++
n) {
62 myfile <<
" " <<
R(
m,
n);
68 cout <<
"Saved shonan_result.dat file" << endl;
73 myfile.open(
"shonan_result_of_" + name +
".g2o");
75 for (
size_t i = 0;
i < nrSO3; ++
i) {
79 myfile <<
"VERTEX_SE3:QUAT" <<
" ";
81 myfile << poses[
i].x() <<
" " << poses[
i].y() <<
" " << poses[
i].z() <<
" ";
83 myfile << quaternion.
x() <<
" " << quaternion.
y() <<
" " << quaternion.
z()
84 <<
" " << quaternion.
w();
88 cout <<
"Saved shonan_result.g2o file" << endl;
93 myfile.open(
"shonan_result.dat");
95 for (
size_t i = 0;
i < nrSOn; ++
i) {
102 myfile <<
"QuatSO3 " <<
i;
103 myfile <<
"QuatSO3 " << i <<
" " << w <<
" " << x <<
" " << y <<
" " << z <<
"\n";
108 int main(
int argc,
char* argv[]) {
111 throw runtime_error(
"Usage: timeShonanAveraging [g2oFile]");
117 g2oFile = argv[argc - 1];
121 }
catch (
const exception&
e) {
122 cerr << e.what() <<
'\n';
127 size_t pos1 = g2oFile.find(
"data/");
128 size_t pos2 = g2oFile.find(
".g2o");
129 string name = g2oFile.substr(pos1 + 5, pos2 - pos1 - 5);
130 cout << name << endl;
131 ofstream csvFile(
"shonan_timing_of_" + name +
".csv");
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;
150 for (
size_t p = pMin;
p <= 7;
p++) {
155 : ShonanAveraging3::LiftTo<Rot3>(pMin, randomRotations);
156 chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
159 chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
160 chrono::duration<double> timeUsed1 =
161 chrono::duration_cast<chrono::duration<double>>(t2 - t1);
163 chrono::steady_clock::time_point t3 = chrono::steady_clock::now();
164 chrono::duration<double> timeUsed2 =
165 chrono::duration_cast<chrono::duration<double>>(t3 - t1);
167 CostP = kShonan.
costAt(
p, result);
169 Cost3 = kShonan.
cost(SO3Values);
170 suBound = (Cost3 - CostP) / CostP;
172 saveData(
p, timeUsed1.count(), CostP, Cost3, timeUsed2.count(),
173 lambdaMin, suBound, &cout);
174 saveData(
p, timeUsed1.count(), CostP, Cost3, timeUsed2.count(),
175 lambdaMin, suBound, &csvFile);
EIGEN_DEVICE_FUNC CoeffReturnType x() const
Values initializeRandomly(std::mt19937 &rng) const
const ValueType at(Key j) const
void saveResultQuat(const Values &values)
EIGEN_DEVICE_FUNC CoeffReturnType z() const
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
double costAt(size_t p, const Values &values) const
Rot2 R(Rot2::fromAngle(0.1))
Values roundSolution(const Values &values) const
EIGEN_DEVICE_FUNC CoeffReturnType w() const
double computeMinEigenValue(const Values &values, Vector *minEigenVector=nullptr) const
Values tryOptimizingAt(size_t p, const Values &initial) const
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
double cost(const Values &values) const
static const ShonanAveraging3 kShonan
const MatrixNN & matrix() const
Return matrix.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EIGEN_DEVICE_FUNC CoeffReturnType y() const
void saveG2oResult(string name, const Values &values, std::map< Key, Pose3 > poses)
typedef and functions to augment Eigen's VectorXd
gtsam::Quaternion toQuaternion() const
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
ofstream os("timeSchurFactors.csv")
The quaternion class used to represent 3D orientations and rotations.
void saveResult(string name, const Values &values)
int main(int argc, char *argv[])
void checkR(const Matrix &R)
Annotation for function names.
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
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 x
void saveData(size_t p, double time1, double costP, double cost3, double time2, double min_eigenvalue, double suBound, std::ostream *os)
Shonan Averaging algorithm.
3D rotation represented as a rotation matrix or quaternion
Values initializeWithDescent(size_t p, const Values &values, const Vector &minEigenVector, double minEigenValue, double gradienTolerance=1e-2, double preconditionedGradNormTolerance=1e-4) const