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

Recreated initializeRandomlyAt

release/4.3a0
Frank Dellaert 2020-08-18 22:28:04 -04:00
parent 5a6bfed42c
commit fc7944b42d
3 changed files with 26 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/gtsam.i
View File

@ -2960,6 +2960,7 @@ class ShonanAveraging2 {
// Advanced API // Advanced API
gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const;
gtsam::Values initializeRandomlyAt(size_t p) const;
double costAt(size_t p, const gtsam::Values& values) const; double costAt(size_t p, const gtsam::Values& values) const;
pair<double, Vector> computeMinEigenVector(const gtsam::Values& values) const; pair<double, Vector> computeMinEigenVector(const gtsam::Values& values) const;
bool checkOptimality(const gtsam::Values& values) const; bool checkOptimality(const gtsam::Values& values) const;
@ -3004,6 +3005,7 @@ class ShonanAveraging3 {
// Advanced API // Advanced API
gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const;
gtsam::Values initializeRandomlyAt(size_t p) const;
double costAt(size_t p, const gtsam::Values& values) const; double costAt(size_t p, const gtsam::Values& values) const;
pair<double, Vector> computeMinEigenVector(const gtsam::Values& values) const; pair<double, Vector> computeMinEigenVector(const gtsam::Values& values) const;
bool checkOptimality(const gtsam::Values& values) const; bool checkOptimality(const gtsam::Values& values) const;

17
gtsam/sfm/ShonanAveraging.cpp
View File

@ -749,10 +749,23 @@ Values ShonanAveraging<d>::initializeRandomly(std::mt19937 &rng) const {
return initial; return initial;
} }
/* ************************************************************************* */
template <size_t d> Values ShonanAveraging<d>::initializeRandomly() const {
return initializeRandomly(kRandomNumberGenerator);
}
/* ************************************************************************* */ /* ************************************************************************* */
template <size_t d> template <size_t d>
Values ShonanAveraging<d>::initializeRandomly() const { Values ShonanAveraging<d>::initializeRandomlyAt(size_t p,
return ShonanAveraging<d>::initializeRandomly(kRandomNumberGenerator); std::mt19937 &rng) const {
const Values randomRotations = initializeRandomly(rng);
return LiftTo<Rot3>(p, randomRotations); // lift to p!
}
/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::initializeRandomlyAt(size_t p) const {
return initializeRandomlyAt(p, kRandomNumberGenerator);
} }
/* ************************************************************************* */ /* ************************************************************************* */

9
gtsam/sfm/ShonanAveraging.h
View File

@ -235,6 +235,15 @@ public:
*/ */
NonlinearFactorGraph buildGraphAt(size_t p) const; NonlinearFactorGraph buildGraphAt(size_t p) const;
/**
* Create initial Values of type SO(p)
* @param p the dimensionality of the rotation manifold
*/
Values initializeRandomlyAt(size_t p, std::mt19937 &rng) const;
/// Version of initializeRandomlyAt with fixed random seed.
Values initializeRandomlyAt(size_t p) const;
/** /**
* Calculate cost for SO(p) * Calculate cost for SO(p)
* Values should be of type SO(p) * Values should be of type SO(p)