12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed compilation and removed absolute path.

release/4.3a0
Frank dellaert 2020-08-21 11:01:28 -04:00
parent 12d8ae2f5f
commit 9f345bb703
1 changed files with 15 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
gtsam_unstable/timing/timeShonanAveraging.cpp
View File

@ -34,19 +34,17 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
// string g2oFile = findExampleDataFile("toyExample.g2o");
// save a single line of timing info to an output stream // save a single line of timing info to an output stream
void saveData(size_t p, double time1, double costP, double cost3, double time2, void saveData(size_t p, double time1, double costP, double cost3, double time2,
double min_eigenvalue, double suBound, std::ostream* os) { double min_eigenvalue, double suBound, std::ostream* os) {
*os << (int)p << "\t" << time1 << "\t" << costP << "\t" << cost3 << "\t" *os << static_cast<int>(p) << "\t" << time1 << "\t" << costP << "\t" << cost3
<< time2 << "\t" << min_eigenvalue << "\t" << suBound << endl; << "\t" << time2 << "\t" << min_eigenvalue << "\t" << suBound << endl;
} }
void checkR(const Matrix& R) { void checkR(const Matrix& R) {
Matrix R2 = R.transpose(); Matrix R2 = R.transpose();
Matrix actual_R = R2 * R; Matrix actual_R = R2 * R;
assert_equal(Rot3(),Rot3(actual_R)); assert_equal(Rot3(), Rot3(actual_R));
} }
void saveResult(string name, const Values& values) { void saveResult(string name, const Values& values) {
@ -82,7 +80,8 @@ void saveG2oResult(string name, const Values& values, std::map<Key, Pose3> poses
myfile << i << " "; myfile << i << " ";
myfile << poses[i].x() << " " << poses[i].y() << " " << poses[i].z() << " "; myfile << poses[i].x() << " " << poses[i].y() << " " << poses[i].z() << " ";
Vector quaternion = Rot3(R).quaternion(); Vector quaternion = Rot3(R).quaternion();
myfile << quaternion(3) << " " << quaternion(2) << " " << quaternion(1) << " " << quaternion(0); myfile << quaternion(3) << " " << quaternion(2) << " " << quaternion(1)
<< " " << quaternion(0);
myfile << "\n"; myfile << "\n";
} }
myfile.close(); myfile.close();
@ -109,20 +108,19 @@ void saveResultQuat(const Values& values) {
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
// primitive argument parsing: // primitive argument parsing:
if (argc > 3) { if (argc > 3) {
throw runtime_error("Usage: timeShonanAveraging [g2oFile]"); throw runtime_error("Usage: timeShonanAveraging [g2oFile]");
} }
string g2oFile; string g2oFile;
try { try {
if (argc > 1) if (argc > 1)
g2oFile = argv[argc - 1]; g2oFile = argv[argc - 1];
else else
g2oFile = string( g2oFile = findExampleDataFile("sphere2500");
"/home/jingwu/git/SESync/data/sphere2500.g2o");
} catch (const exception& e) { } catch (const exception& e) {
cerr << e.what() << '\n'; cerr << e.what() << '\n';
exit(1); exit(1);
} }
// Create a csv output file // Create a csv output file
@ -147,14 +145,14 @@ int main(int argc, char* argv[]) {
cout << "(int)p" << "\t" << "time1" << "\t" << "costP" << "\t" << "cost3" << "\t" cout << "(int)p" << "\t" << "time1" << "\t" << "costP" << "\t" << "cost3" << "\t"
<< "time2" << "\t" << "MinEigenvalue" << "\t" << "SuBound" << endl; << "time2" << "\t" << "MinEigenvalue" << "\t" << "SuBound" << endl;
const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator); const Values randomRotations = kShonan.initializeRandomly();
for (size_t p = pMin; p < 6; p++) { for (size_t p = pMin; p <= 7; p++) {
// Randomly initialize at lowest level, initialize by line search after that // Randomly initialize at lowest level, initialize by line search after that
const Values initial = const Values initial =
(p > pMin) ? kShonan.initializeWithDescent(p, Qstar, minEigenVector, (p > pMin) ? kShonan.initializeWithDescent(p, Qstar, minEigenVector,
lambdaMin) lambdaMin)
: ShonanAveraging::LiftTo<Rot3>(pMin, randomRotations); : ShonanAveraging3::LiftTo<Rot3>(pMin, randomRotations);
chrono::steady_clock::time_point t1 = chrono::steady_clock::now(); chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
// optimizing // optimizing
const Values result = kShonan.tryOptimizingAt(p, initial); const Values result = kShonan.tryOptimizingAt(p, initial);