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

Feature/shonan averaging (#473) 

Shonan Rotation Averaging.

199 commit messages below, many are obsolete as design has changed quite a bit over time, especially from the earlier period where I thought we only needed SO(4).

* prototyping weighted sampler

* Moved WeightedSampler into its own header

* Random now uses std header <random>.

* Removed boost/random usage from linear and discrete directories

* Made into class

* Now using new WeightedSampler class

* Inlined random direction generation

* eradicated last vestiges of boost/random in gtsam_unstable

* Added 3D example g2o file

* Added Frobenius norm factors

* Shonan averaging algorithm, using SOn class

* Wrapping Frobenius and Shonan

* Fixed issues with <<

* Use Builder parameters

* Refactored Shonan interface

* Fixed << issues as well as MATLAB segfault, using eval(), as discussed in issue #451

* ShonanAveragingParameters

* New factor FrobeniusWormholeFactorP computes |Rj*P - Ri*P*Rij|

* Fixed broken GetDimension for Lie groups with variable dimension.

* Removed all but Shonan averaging factor and made everything work with new SOn

* Just a single WormholeFactor, wrapped noise model

* Use std <random>

* comments/todos

* added timing script

* add script to process ShonanAveraging timing results

* Now producing a CSV file

* Parse csv file and make combined plot

* Fixed range

* change p value and set two flags on

* input file path, all the csv files proceeses at the same time

* add check convergence rate part

* csv file have name according to input  data name

* correct one mistake in initialization

* generate the convergence rate for each p value

* add yticks for the bar plot

* add noises to the measurements

* test add noise

* Basic structure for checkOptimalityAt

* change optimizer method to cholesky

* buildQ now working. Tests should be better but visually inspected.

* multiple test with cholesky

* back

* computeLambda now works

* make combined plots while make bar plot

* Calculate minimum eigenvalue - the very expensive version

* Exposed computeMinEigenValue

* make plots and bar  togenter

* method change to jacobi

* add time for check optimality, min_eigen_value, sub_bound

* updated plot min_eigen value and subounds

* Adding Spectra headers

* David's min eigenvalue code inserted and made to compile.

* Made it work

* Made "run" method work.

* add rim.g2o name

* Fixed bug in shifting eigenvalues

* roundSolution which replaces projectFrom

* removed extra arguments

* Added to wrapper

* Add SOn to template lists

* roundSolution delete the extra arguement p

* only calculate p=5 and change to the correct way computing f_R

* Fixed conflict and made Google-style name changes

* prototype descent code and unit test for initializeWithDescent

* add averaging cost/time part in processing data

* initializewithDescent success in test

* Formatting and find example rather than hardcode

* Removed accidentally checked in cmake files

* give value to xi by block

* correct gradient descent

* correct xi

* }

* Fix wrapper

* Make Hat/Vee have alternating signs

* MakeATangentVector helpder function

* Fixed cmake files

* changed sign

* add line search

* unit test for line search

* test real data with line search

* correct comment

* Fix boost::uniform_real

* add save .dat file

* correct test case

* add explanation

* delete redundant cout

* add name to .dat output file

* correct checkR

* add get poses_  in shonan

* add Vector Point type for savig data

* Remove cmake file which magically re-appeared??

* Switched to std random library.

* Prepare Klaus test

* Add klaus3.g2o data.

* fix comment

* Fix derivatives

* Fixed broken GetDimension for Lie groups with variable dimension.

* Fix SOn tests to report correct dimension

* Added tests for Klaus3 data

* Add runWithRandomKlaus test for shonan.

* Finish runWithRandomKlaus unittest.

* Correct datafile.

* Correct the format.

* Added measured and keys methods

* Shonan works on Klaus data

* Create dense versions for wrappers, for testing

* Now store D, Q, and L

* Remove another cmake file incorrectly checked in.

* Found and fixed the bug in ComputeLambda !

* Now using Q in Lambdas calculation, so Lambdas agree with Eriksson18cvpr.

* Make FrobeniusFactor not use deprecated methods

* FrobeniusWormholeFactor takes Rot3 as argument

* Wrapped some more methods.

* Wrapped more methods

* Allow creating and populating BetweenFactorPose3s in python

* New constructors for ShonanAveraging

* add function of get measurements number

* Remove option not to use noise model

* wrap Use nrMeasurements

* Made Logmap a bit more tolerant of slightly degenerate rotations (with trace < -1)

* Allow for Anchor index

* Fix anchor bug

* Change outside view to Rot3 rather than SO3

* Add Lift in SOn class

* Make comet working

* Small fixes

* Delete extra function

* Add SOn::Lift

* Removed hardcoded flag

* Moved Frobenius factor to gtsam from unstable

* Added new tests and made an old regression pass again

* Cleaned up formatting and some comments, added EXPORT directives

* Throw exception if wrongly dimensioned values are given

* static_cast and other throw

* Fixed run-time dimension

* Added gauge-constraining factor

* LM parameters now passed in, added Gauge fixing

* 2D test scaffold

* Comments

* Pre-allocated generators

* Document API

* Add optional weight

* New prior weeights infrastructure

* Made d a template parameter

* Recursive Hat and RetractJacobian test

* Added Spectra 0.9.0 to 3rdparty

* Enabling 2D averaging

* Templatized Wormhole factor

* ignore xcode folder

* Fixed vec and VectorizedGenerators templates for fixed N!=3 or 4

* Simplifying constructors
Moved file loading to tests (for now)
All unit tests pass for d==3!

* Templated some methods internally

* Very generic parseToVector

* refactored load2d

* Very much improved FrobeniusWormholeFactor (Shonan) Jacobians

* SO(2) averaging works !

* Templated parse methods

* Switched to new Dataset paradigm

* Moved Shonan to gtsam

* Checked noise model is correctly gotten from file

* Fixed covariance bug

* Making Shonan wrapper work

* Renamed FrobeniusWormholeFactor to ShonanFactor and moved into its own compilation unit in gtsam/sfm

* Fixed wrong include

* Simplified interface (removed irrelevant random inits) and fixed eigenvector test

* Removed stray boost::none

* Added citation as suggested by Jose

* Made descent test deterministic

* Fixed some comments, commented out flaky test

Co-authored-by: Jing Wu <jingwu@gatech.edu>
Co-authored-by: jingwuOUO <wujing2951@gmail.com>
Co-authored-by: swang <swang736@gatech.edu>
Co-authored-by: ss <ss>
Co-authored-by: Fan Jiang <prof.fan@foxmail.com>
release/4.3a0
Frank Dellaert 2020-08-17 07:43:10 -04:00 committed by GitHub
parent ae4968328d
commit 84afc94458
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
28 changed files with 2880 additions and 297 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -21,3 +21,4 @@ cython/gtsam_wrapper.pxd
/CMakeSettings.json
# for QtCreator:
CMakeLists.txt.user*
xcode/

4
cython/gtsam/tests/test_FrobeniusFactor.py
View File

@ -13,7 +13,7 @@ import unittest
import numpy as np
from gtsam import (Rot3, SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4,
FrobeniusWormholeFactor, SOn)
ShonanFactor3, SOn)
id = SO4()
v1 = np.array([0, 0, 0, 0.1, 0, 0])
@ -43,7 +43,7 @@ class TestFrobeniusFactorSO4(unittest.TestCase):
"""Test creation of a factor that calculates Shonan error."""
R1 = SO3.Expmap(v1[3:])
R2 = SO3.Expmap(v2[3:])
factor = FrobeniusWormholeFactor(1, 2, Rot3(R1.between(R2).matrix()), p=4)
factor = ShonanFactor3(1, 2, Rot3(R1.between(R2).matrix()), p=4)
I4 = SOn(4)
Q1 = I4.retract(v1)
Q2 = I4.retract(v2)

12
examples/Data/Klaus3.g2o
View File

@ -1,6 +1,6 @@
VERTEX_SE3:QUAT 0 -3.865747774038187 0.06639337702667497 -0.16064874691945374 0.024595211709139555 0.49179523413089893 -0.06279232989379242 0.8680954132776109
VERTEX_SE3:QUAT 1 -3.614793159814815 0.04774490041587656 -0.2837650367985949 0.00991721787943912 0.4854918961891193 -0.042343290945895576 0.8731588132957809
VERTEX_SE3:QUAT 2 -3.255096913553434 0.013296754286114112 -0.5339792269680574 -0.027851108010665374 0.585478168397957 -0.05088341463532465 0.8086102325762403
EDGE_SE3:QUAT 0 1 0.2509546142233723 -0.01864847661079841 -0.12311628987914114 -0.022048798853273946 -0.01796327847857683 0.010210006313668573 0.9995433591728293 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0
EDGE_SE3:QUAT 0 2 0.6106508604847534 -0.05309662274056086 -0.3733304800486037 -0.054972994022992064 0.10432547598981769 -0.02221474884651081 0.9927742290779572 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0
EDGE_SE3:QUAT 1 2 0.3596962462613811 -0.03444814612976245 -0.25021419016946256 -0.03174661848656213 0.11646825423134777 -0.02951742735854383 0.9922479626852876 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0
VERTEX_SE3:QUAT 0 -1.6618596980158338 -0.5736497760548741 -3.3319774096611026 -0.02676080288219576 -0.024497002638379624 -0.015064701622500615 0.9992281076190063
VERTEX_SE3:QUAT 1 -1.431820463019384 -0.549139761976065 -3.160677992237872 -0.049543805396343954 -0.03232420352077356 -0.004386230477751116 0.998239108728862
VERTEX_SE3:QUAT 2 -1.0394840214436651 -0.5268841046291037 -2.972143862665523 -0.07993768981394891 0.0825062894866454 -0.04088089479075661 0.9925378735259738
EDGE_SE3:QUAT 0 1 0.23003923499644974 0.02451001407880915 0.17129941742323052 -0.022048798853273946 -0.01796327847857683 0.010210006313668573 0.9995433591728293 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0
EDGE_SE3:QUAT 0 2 0.6223756765721686 0.04676567142577037 0.35983354699557957 -0.054972994022992064 0.10432547598981769 -0.02221474884651081 0.9927742290779572 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0
EDGE_SE3:QUAT 1 2 0.3923364415757189 0.022255657346961222 0.18853412957234905 -0.03174661848656213 0.11646825423134777 -0.02951742735854383 0.9922479626852876 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0

131
gtsam.h
View File

@ -2855,7 +2855,7 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
};
#include <gtsam/slam/FrobeniusFactor.h>
gtsam::noiseModel::Isotropic* ConvertPose3NoiseModel(
gtsam::noiseModel::Isotropic* ConvertNoiseModel(
gtsam::noiseModel::Base* model, size_t d);
template<T = {gtsam::SO3, gtsam::SO4}>
@ -2874,12 +2874,14 @@ virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {
Vector evaluateError(const T& R1, const T& R2);
};
virtual class FrobeniusWormholeFactor : gtsam::NoiseModelFactor {
FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12,
#include <gtsam/sfm/ShonanFactor.h>
virtual class ShonanFactor3 : gtsam::NoiseModelFactor {
ShonanFactor(size_t key1, size_t key2, const gtsam::Rot3 &R12,
size_t p);
FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12,
size_t p, gtsam::noiseModel::Base* model);
Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2);
ShonanFactor(size_t key1, size_t key2, const gtsam::Rot3 &R12,
size_t p, gtsam::noiseModel::Base *model);
Vector evaluateError(const gtsam::SOn &Q1, const gtsam::SOn &Q2);
};
#include <gtsam/sfm/BinaryMeasurement.h>
@ -2895,6 +2897,123 @@ class BinaryMeasurement {
typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3;
#include <gtsam/sfm/ShonanAveraging.h>
// TODO(frank): copy/pasta below until we have integer template paremeters in wrap!
class ShonanAveragingParameters2 {
ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm);
ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm, string method);
gtsam::LevenbergMarquardtParams getLMParams() const;
void setOptimalityThreshold(double value);
double getOptimalityThreshold() const;
void setAnchor(size_t index, const gtsam::Rot2& value);
void setAnchorWeight(double value);
double getAnchorWeight() const;
void setKarcherWeight(double value);
double getKarcherWeight();
void setGaugesWeight(double value);
double getGaugesWeight();
};
class ShonanAveragingParameters3 {
ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm);
ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm, string method);
gtsam::LevenbergMarquardtParams getLMParams() const;
void setOptimalityThreshold(double value);
double getOptimalityThreshold() const;
void setAnchor(size_t index, const gtsam::Rot3& value);
void setAnchorWeight(double value);
double getAnchorWeight() const;
void setKarcherWeight(double value);
double getKarcherWeight();
void setGaugesWeight(double value);
double getGaugesWeight();
};
class ShonanAveraging2 {
ShonanAveraging2(string g2oFile);
ShonanAveraging2(string g2oFile,
const gtsam::ShonanAveragingParameters2 &parameters);
// Query properties
size_t nrUnknowns() const;
size_t nrMeasurements() const;
gtsam::Rot2 measured(size_t i);
gtsam::KeyVector keys(size_t i);
// Matrix API (advanced use, debugging)
Matrix denseD() const;
Matrix denseQ() const;
Matrix denseL() const;
// Matrix computeLambda_(Matrix S) const;
Matrix computeLambda_(const gtsam::Values& values) const;
Matrix computeA_(const gtsam::Values& values) const;
double computeMinEigenValue(const gtsam::Values& values) const;
gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values,
const Vector& minEigenVector, double minEigenValue) const;
// Advanced API
gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const;
double costAt(size_t p, const gtsam::Values& values) const;
pair<double, Vector> computeMinEigenVector(const gtsam::Values& values) const;
bool checkOptimality(const gtsam::Values& values) const;
gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(size_t p, const gtsam::Values& initial);
// gtsam::Values tryOptimizingAt(size_t p) const;
gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const;
gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const;
gtsam::Values roundSolution(const gtsam::Values& values) const;
// Basic API
double cost(const gtsam::Values& values) const;
gtsam::Values initializeRandomly() const;
pair<gtsam::Values, double> run(const gtsam::Values& initial, size_t min_p, size_t max_p) const;
};
class ShonanAveraging3 {
ShonanAveraging3(string g2oFile);
ShonanAveraging3(string g2oFile,
const gtsam::ShonanAveragingParameters3 &parameters);
// TODO(frank): deprecate once we land pybind wrapper
ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors);
ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors,
const gtsam::ShonanAveragingParameters3 &parameters);
// Query properties
size_t nrUnknowns() const;
size_t nrMeasurements() const;
gtsam::Rot3 measured(size_t i);
gtsam::KeyVector keys(size_t i);
// Matrix API (advanced use, debugging)
Matrix denseD() const;
Matrix denseQ() const;
Matrix denseL() const;
// Matrix computeLambda_(Matrix S) const;
Matrix computeLambda_(const gtsam::Values& values) const;
Matrix computeA_(const gtsam::Values& values) const;
double computeMinEigenValue(const gtsam::Values& values) const;
gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values,
const Vector& minEigenVector, double minEigenValue) const;
// Advanced API
gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const;
double costAt(size_t p, const gtsam::Values& values) const;
pair<double, Vector> computeMinEigenVector(const gtsam::Values& values) const;
bool checkOptimality(const gtsam::Values& values) const;
gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(size_t p, const gtsam::Values& initial);
// gtsam::Values tryOptimizingAt(size_t p) const;
gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const;
gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const;
gtsam::Values roundSolution(const gtsam::Values& values) const;
// Basic API
double cost(const gtsam::Values& values) const;
gtsam::Values initializeRandomly() const;
pair<gtsam::Values, double> run(const gtsam::Values& initial, size_t min_p, size_t max_p) const;
};
//*************************************************************************
// Navigation
//*************************************************************************

7
gtsam/geometry/Rot2.cpp
View File

@ -39,6 +39,13 @@ Rot2 Rot2::atan2(double y, double x) {
return R.normalize();
}
/* ************************************************************************* */
Rot2 Rot2::Random(std::mt19937& rng) {
uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
double angle = randomAngle(rng);
return fromAngle(angle);
}
/* ************************************************************************* */
void Rot2::print(const string& s) const {
cout << s << ": " << theta() << endl;

10
gtsam/geometry/Rot2.h
View File

@ -22,6 +22,8 @@
#include <gtsam/base/Lie.h>
#include <boost/optional.hpp>
#include <random>
namespace gtsam {
/**
@ -79,6 +81,14 @@ namespace gtsam {
/** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
static Rot2 atan2(double y, double x);
/**
* Random, generates random angle \in [-p,pi]
* Example:
* std::mt19937 engine(42);
* Unit3 unit = Unit3::Random(engine);
*/
static Rot2 Random(std::mt19937 & rng);
/// @}
/// @name Testable
/// @{

19
gtsam/geometry/SOn-inl.h
View File

@ -60,8 +60,9 @@ typename SO<N>::TangentVector SO<N>::ChartAtOrigin::Local(const SO& R,
template <int N>
typename SO<N>::MatrixDD SO<N>::AdjointMap() const {
if (N==2) return I_1x1; // SO(2) case
throw std::runtime_error(
"SO<N>::AdjointMap only implemented for SO3 and SO4.");
"SO<N>::AdjointMap only implemented for SO2, SO3 and SO4.");
}
template <int N>
@ -84,26 +85,22 @@ typename SO<N>::MatrixDD SO<N>::LogmapDerivative(const TangentVector& omega) {
throw std::runtime_error("O<N>::LogmapDerivative only implemented for SO3.");
}
// Default fixed size version (but specialized elsewehere for N=2,3,4)
template <int N>
typename SO<N>::VectorN2 SO<N>::vec(
OptionalJacobian<internal::NSquaredSO(N), dimension> H) const {
const size_t n = rows();
const size_t n2 = n * n;
// Vectorize
VectorN2 X(n2);
X << Eigen::Map<const Matrix>(matrix_.data(), n2, 1);
VectorN2 X = Eigen::Map<const VectorN2>(matrix_.data());
// If requested, calculate H as (I \oplus Q) * P,
// where Q is the N*N rotation matrix, and P is calculated below.
if (H) {
// Calculate P matrix of vectorized generators
// TODO(duy): Should we refactor this as the jacobian of Hat?
Matrix P = VectorizedGenerators(n);
const size_t d = dim();
H->resize(n2, d);
for (size_t i = 0; i < n; i++) {
H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d);
Matrix P = SO<N>::VectorizedGenerators();
for (size_t i = 0; i < N; i++) {
H->block(i * N, 0, N, dimension) =
matrix_ * P.block(i * N, 0, N, dimension);
}
}
return X;

43
gtsam/geometry/SOn.cpp
View File

@ -22,21 +22,18 @@
namespace gtsam {
template <>
GTSAM_EXPORT
Matrix SOn::Hat(const Vector& xi) {
GTSAM_EXPORT void SOn::Hat(const Vector &xi, Eigen::Ref<Matrix> X) {
size_t n = AmbientDim(xi.size());
if (n < 2) throw std::invalid_argument("SO<N>::Hat: n<2 not supported");
Matrix X(n, n); // allocate space for n*n skew-symmetric matrix
X.setZero();
if (n == 2) {
if (n < 2)
throw std::invalid_argument("SO<N>::Hat: n<2 not supported");
else if (n == 2) {
// Handle SO(2) case as recursion bottom
assert(xi.size() == 1);
X << 0, -xi(0), xi(0), 0;
} else {
// Recursively call SO(n-1) call for top-left block
const size_t dmin = (n - 1) * (n - 2) / 2;
X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin));
Hat(xi.tail(dmin), X.topLeftCorner(n - 1, n - 1));
// determine sign of last element (signs alternate)
double sign = pow(-1.0, xi.size());
@ -47,7 +44,14 @@ Matrix SOn::Hat(const Vector& xi) {
X(j, n - 1) = -X(n - 1, j);
sign = -sign;
}
X(n - 1, n - 1) = 0; // bottom-right
}
}
template <> GTSAM_EXPORT Matrix SOn::Hat(const Vector &xi) {
size_t n = AmbientDim(xi.size());
Matrix X(n, n); // allocate space for n*n skew-symmetric matrix
SOn::Hat(xi, X);
return X;
}
@ -99,4 +103,27 @@ SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
return result;
}
// Dynamic version of vec
template <> typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const {
const size_t n = rows(), n2 = n * n;
// Vectorize
VectorN2 X(n2);
X << Eigen::Map<const Matrix>(matrix_.data(), n2, 1);
// If requested, calculate H as (I \oplus Q) * P,
// where Q is the N*N rotation matrix, and P is calculated below.
if (H) {
// Calculate P matrix of vectorized generators
// TODO(duy): Should we refactor this as the jacobian of Hat?
Matrix P = SOn::VectorizedGenerators(n);
const size_t d = dim();
H->resize(n2, d);
for (size_t i = 0; i < n; i++) {
H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d);
}
}
return X;
}
} // namespace gtsam

22
gtsam/geometry/SOn.h
View File

@ -98,8 +98,8 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
template <typename Derived, int N_ = N, typename = IsDynamic<N_>>
static SO Lift(size_t n, const Eigen::MatrixBase<Derived> &R) {
Matrix Q = Matrix::Identity(n, n);
size_t p = R.rows();
assert(p < n && R.cols() == p);
const int p = R.rows();
assert(p >= 0 && p <= static_cast<int>(n) && R.cols() == p);
Q.topLeftCorner(p, p) = R;
return SO(Q);
}
@ -208,7 +208,7 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
// Calculate run-time dimensionality of manifold.
// Available as dimension or Dim() for fixed N.
size_t dim() const { return Dimension(matrix_.rows()); }
size_t dim() const { return Dimension(static_cast<size_t>(matrix_.rows())); }
/**
* Hat operator creates Lie algebra element corresponding to d-vector, where d
@ -227,9 +227,10 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
*/
static MatrixNN Hat(const TangentVector& xi);
/**
* Inverse of Hat. See note about xi element order in Hat.
*/
/// In-place version of Hat (see details there), implements recursion.
static void Hat(const Vector &xi, Eigen::Ref<MatrixNN> X);
/// Inverse of Hat. See note about xi element order in Hat.
static TangentVector Vee(const MatrixNN& X);
// Chart at origin
@ -295,10 +296,10 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
template <int N_ = N, typename = IsFixed<N_>>
static Matrix VectorizedGenerators() {
constexpr size_t N2 = static_cast<size_t>(N * N);
Matrix G(N2, dimension);
Eigen::Matrix<double, N2, dimension> G;
for (size_t j = 0; j < dimension; j++) {
const auto X = Hat(Vector::Unit(dimension, j));
G.col(j) = Eigen::Map<const Matrix>(X.data(), N2, 1);
G.col(j) = Eigen::Map<const VectorN2>(X.data());
}
return G;
}
@ -362,6 +363,11 @@ template <>
SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
DynamicJacobian H2) const;
/*
* Specialize dynamic vec.
*/
template <> typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const;
/** Serialization function */
template<class Archive>
void serialize(

88
gtsam/geometry/tests/testSOn.cpp
View File

@ -39,8 +39,8 @@ using namespace std;
using namespace gtsam;
//******************************************************************************
// Test dhynamic with n=0
TEST(SOn, SO0) {
// Test dynamic with n=0
TEST(SOn, SOn0) {
const auto R = SOn(0);
EXPECT_LONGS_EQUAL(0, R.rows());
EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension);
@ -50,7 +50,8 @@ TEST(SOn, SO0) {
}
//******************************************************************************
TEST(SOn, SO5) {
// Test dynamic with n=5
TEST(SOn, SOn5) {
const auto R = SOn(5);
EXPECT_LONGS_EQUAL(5, R.rows());
EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension);
@ -59,6 +60,28 @@ TEST(SOn, SO5) {
EXPECT_LONGS_EQUAL(10, traits<SOn>::GetDimension(R));
}
//******************************************************************************
// Test fixed with n=2
TEST(SOn, SO0) {
const auto R = SO<2>();
EXPECT_LONGS_EQUAL(2, R.rows());
EXPECT_LONGS_EQUAL(1, SO<2>::dimension);
EXPECT_LONGS_EQUAL(1, SO<2>::Dim());
EXPECT_LONGS_EQUAL(1, R.dim());
EXPECT_LONGS_EQUAL(1, traits<SO<2>>::GetDimension(R));
}
//******************************************************************************
// Test fixed with n=5
TEST(SOn, SO5) {
const auto R = SO<5>();
EXPECT_LONGS_EQUAL(5, R.rows());
EXPECT_LONGS_EQUAL(10, SO<5>::dimension);
EXPECT_LONGS_EQUAL(10, SO<5>::Dim());
EXPECT_LONGS_EQUAL(10, R.dim());
EXPECT_LONGS_EQUAL(10, traits<SO<5>>::GetDimension(R));
}
//******************************************************************************
TEST(SOn, Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<SOn>));
@ -105,29 +128,29 @@ TEST(SOn, HatVee) {
EXPECT(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2)));
Matrix expected3(3, 3);
expected3 << 0, -3, 2, //
3, 0, -1, //
-2, 1, 0;
expected3 << 0, -3, 2, //
3, 0, -1, //
-2, 1, 0;
const auto actual3 = SOn::Hat(v.head<3>());
EXPECT(assert_equal(expected3, actual3));
EXPECT(assert_equal(skewSymmetric(1, 2, 3), actual3));
EXPECT(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3)));
Matrix expected4(4, 4);
expected4 << 0, -6, 5, 3, //
6, 0, -4, -2, //
-5, 4, 0, 1, //
-3, 2, -1, 0;
expected4 << 0, -6, 5, 3, //
6, 0, -4, -2, //
-5, 4, 0, 1, //
-3, 2, -1, 0;
const auto actual4 = SOn::Hat(v.head<6>());
EXPECT(assert_equal(expected4, actual4));
EXPECT(assert_equal((Vector)v.head<6>(), SOn::Vee(actual4)));
Matrix expected5(5, 5);
expected5 << 0,-10, 9, 7, -4, //
10, 0, -8, -6, 3, //
-9, 8, 0, 5, -2, //
-7, 6, -5, 0, 1, //
4, -3, 2, -1, 0;
expected5 << 0, -10, 9, 7, -4, //
10, 0, -8, -6, 3, //
-9, 8, 0, 5, -2, //
-7, 6, -5, 0, 1, //
4, -3, 2, -1, 0;
const auto actual5 = SOn::Hat(v);
EXPECT(assert_equal(expected5, actual5));
EXPECT(assert_equal((Vector)v, SOn::Vee(actual5)));
@ -159,6 +182,22 @@ TEST(SOn, RetractLocal) {
CHECK(assert_equal(v1, SOn::ChartAtOrigin::Local(Q1), 1e-7));
}
//******************************************************************************
Matrix RetractJacobian(size_t n) { return SOn::VectorizedGenerators(n); }
/// Test Jacobian of Retract at origin
TEST(SOn, RetractJacobian) {
Matrix actualH = RetractJacobian(3);
boost::function<Matrix(const Vector &)> h = [](const Vector &v) {
return SOn::ChartAtOrigin::Retract(v).matrix();
};
Vector3 v;
v.setZero();
const Matrix expectedH = numericalDerivative11<Matrix, Vector, 3>(h, v, 1e-5);
CHECK(assert_equal(expectedH, actualH));
}
//******************************************************************************
TEST(SOn, vec) {
Vector10 v;
@ -166,11 +205,28 @@ TEST(SOn, vec) {
SOn Q = SOn::ChartAtOrigin::Retract(v);
Matrix actualH;
const Vector actual = Q.vec(actualH);
boost::function<Vector(const SOn&)> h = [](const SOn& Q) { return Q.vec(); };
boost::function<Vector(const SOn &)> h = [](const SOn &Q) { return Q.vec(); };
const Matrix H = numericalDerivative11<Vector, SOn, 10>(h, Q, 1e-5);
CHECK(assert_equal(H, actualH));
}
//******************************************************************************
TEST(SOn, VectorizedGenerators) {
// Default fixed
auto actual2 = SO<2>::VectorizedGenerators();
CHECK(actual2.rows()==4 && actual2.cols()==1)
// Specialized
auto actual3 = SO<3>::VectorizedGenerators();
CHECK(actual3.rows()==9 && actual3.cols()==3)
auto actual4 = SO<4>::VectorizedGenerators();
CHECK(actual4.rows()==16 && actual4.cols()==6)
// Dynamic
auto actual5 = SOn::VectorizedGenerators(5);
CHECK(actual5.rows()==25 && actual5.cols()==10)
}
//******************************************************************************
int main() {
TestResult tr;

2
gtsam/nonlinear/NonlinearOptimizer.cpp
View File

@ -90,7 +90,7 @@ void NonlinearOptimizer::defaultOptimize() {
// Iterative loop
do {
// Do next iteration
currentError = error();
currentError = error(); // TODO(frank): don't do this twice at first !? Computed above!
iterate();
tictoc_finishedIteration();

841
gtsam/sfm/ShonanAveraging.cpp Normal file
View File

@ -0,0 +1,841 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ShonanAveraging.cpp
* @date March 2019 - August 2020
* @author Frank Dellaert, David Rosen, and Jing Wu
* @brief Shonan Averaging algorithm
*/
#include <gtsam/sfm/ShonanAveraging.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/sfm/ShonanGaugeFactor.h>
#include <gtsam/slam/FrobeniusFactor.h>
#include <gtsam/slam/KarcherMeanFactor-inl.h>
#include <gtsam/sfm/ShonanFactor.h>
#include <Eigen/Eigenvalues>
#include <SymEigsSolver.h>
#include <algorithm>
#include <complex>
#include <iostream>
#include <map>
#include <random>
#include <vector>
namespace gtsam {
// In Wrappers we have no access to this so have a default ready
static std::mt19937 kRandomNumberGenerator(42);
using Sparse = Eigen::SparseMatrix<double>;
/* ************************************************************************* */
template <size_t d>
ShonanAveragingParameters<d>::ShonanAveragingParameters(
const LevenbergMarquardtParams &_lm, const std::string &method,
double optimalityThreshold, double alpha, double beta, double gamma)
: lm(_lm), optimalityThreshold(optimalityThreshold), alpha(alpha),
beta(beta), gamma(gamma) {
// By default, we will do conjugate gradient
lm.linearSolverType = LevenbergMarquardtParams::Iterative;
// Create subgraph builder parameters
SubgraphBuilderParameters builderParameters;
builderParameters.skeletonType = SubgraphBuilderParameters::KRUSKAL;
builderParameters.skeletonWeight = SubgraphBuilderParameters::EQUAL;
builderParameters.augmentationWeight = SubgraphBuilderParameters::SKELETON;
builderParameters.augmentationFactor = 0.0;
auto pcg = boost::make_shared<PCGSolverParameters>();
// Choose optimization method
if (method == "SUBGRAPH") {
lm.iterativeParams =
boost::make_shared<SubgraphSolverParameters>(builderParameters);
} else if (method == "SGPC") {
pcg->preconditioner_ =
boost::make_shared<SubgraphPreconditionerParameters>(builderParameters);
lm.iterativeParams = pcg;
} else if (method == "JACOBI") {
pcg->preconditioner_ =
boost::make_shared<BlockJacobiPreconditionerParameters>();
lm.iterativeParams = pcg;
} else if (method == "QR") {
lm.setLinearSolverType("MULTIFRONTAL_QR");
} else if (method == "CHOLESKY") {
lm.setLinearSolverType("MULTIFRONTAL_CHOLESKY");
} else {
throw std::invalid_argument("ShonanAveragingParameters: unknown method");
}
}
/* ************************************************************************* */
// Explicit instantiation for d=2 and d=3
template struct ShonanAveragingParameters<2>;
template struct ShonanAveragingParameters<3>;
/* ************************************************************************* */
// Calculate number of unknown rotations referenced by measurements
template <size_t d>
static size_t
NrUnknowns(const typename ShonanAveraging<d>::Measurements &measurements) {
std::set<Key> keys;
for (const auto &measurement : measurements) {
keys.insert(measurement.key1());
keys.insert(measurement.key2());
}
return keys.size();
}
/* ************************************************************************* */
template <size_t d>
ShonanAveraging<d>::ShonanAveraging(const Measurements &measurements,
const Parameters &parameters)
: parameters_(parameters), measurements_(measurements),
nrUnknowns_(NrUnknowns<d>(measurements)) {
for (const auto &measurement : measurements_) {
const auto &model = measurement.noiseModel();
if (model && model->dim() != SO<d>::dimension) {
measurement.print("Factor with incorrect noise model:\n");
throw std::invalid_argument("ShonanAveraging: measurements passed to "
"constructor have incorrect dimension.");
}
}
Q_ = buildQ();
D_ = buildD();
L_ = D_ - Q_;
}
/* ************************************************************************* */
template <size_t d>
NonlinearFactorGraph ShonanAveraging<d>::buildGraphAt(size_t p) const {
NonlinearFactorGraph graph;
auto G = boost::make_shared<Matrix>(SO<-1>::VectorizedGenerators(p));
for (const auto &measurement : measurements_) {
const auto &keys = measurement.keys();
const auto &Rij = measurement.measured();
const auto &model = measurement.noiseModel();
graph.emplace_shared<ShonanFactor<d>>(keys[0], keys[1], Rij, p, model, G);
}
// Possibly add Karcher prior
if (parameters_.beta > 0) {
const size_t dim = SOn::Dimension(p);
graph.emplace_shared<KarcherMeanFactor<SOn>>(graph.keys(), dim);
}
// Possibly add gauge factors - they are probably useless as gradient is zero
if (parameters_.gamma > 0 && p > d + 1) {
for (auto key : graph.keys())
graph.emplace_shared<ShonanGaugeFactor>(key, p, d, parameters_.gamma);
}
return graph;
}
/* ************************************************************************* */
template <size_t d>
double ShonanAveraging<d>::costAt(size_t p, const Values &values) const {
const NonlinearFactorGraph graph = buildGraphAt(p);
return graph.error(values);
}
/* ************************************************************************* */
template <size_t d>
boost::shared_ptr<LevenbergMarquardtOptimizer>
ShonanAveraging<d>::createOptimizerAt(size_t p, const Values &initial) const {
// Build graph
NonlinearFactorGraph graph = buildGraphAt(p);
// Anchor prior is added here as depends on initial value (and cost is zero)
if (parameters_.alpha > 0) {
size_t i;
Rot value;
const size_t dim = SOn::Dimension(p);
std::tie(i, value) = parameters_.anchor;
auto model = noiseModel::Isotropic::Precision(dim, parameters_.alpha);
graph.emplace_shared<PriorFactor<SOn>>(i, SOn::Lift(p, value.matrix()),
model);
}
// Optimize
return boost::make_shared<LevenbergMarquardtOptimizer>(graph, initial,
parameters_.lm);
}
/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::tryOptimizingAt(size_t p,
const Values &initial) const {
auto lm = createOptimizerAt(p, initial);
return lm->optimize();
}
/* ************************************************************************* */
// Project to pxdN Stiefel manifold
template <size_t d>
Matrix ShonanAveraging<d>::StiefelElementMatrix(const Values &values) {
const size_t N = values.size();
const size_t p = values.at<SOn>(0).rows();
Matrix S(p, N * d);
for (const auto it : values.filter<SOn>()) {
S.middleCols<d>(it.key * d) =
it.value.matrix().leftCols<d>(); // project Qj to Stiefel
}
return S;
}
/* ************************************************************************* */
template <>
Values ShonanAveraging<2>::projectFrom(size_t p, const Values &values) const {
Values result;
for (const auto it : values.filter<SOn>()) {
assert(it.value.rows() == p);
const auto &M = it.value.matrix();
const Rot2 R = Rot2::atan2(M(1, 0), M(0, 0));
result.insert(it.key, R);
}
return result;
}
template <>
Values ShonanAveraging<3>::projectFrom(size_t p, const Values &values) const {
Values result;
for (const auto it : values.filter<SOn>()) {
assert(it.value.rows() == p);
const auto &M = it.value.matrix();
const Rot3 R = Rot3::ClosestTo(M.topLeftCorner<3, 3>());
result.insert(it.key, R);
}
return result;
}
/* ************************************************************************* */
template <size_t d> static Matrix RoundSolutionS(const Matrix &S) {
const size_t N = S.cols() / d;
// First, compute a thin SVD of S
Eigen::JacobiSVD<Matrix> svd(S, Eigen::ComputeThinV);
const Vector sigmas = svd.singularValues();
// Construct a diagonal matrix comprised of the first d singular values
using DiagonalMatrix = Eigen::DiagonalMatrix<double, d>;
DiagonalMatrix Sigma_d;
Sigma_d.diagonal() = sigmas.head<d>();
// Now, construct a rank-d truncated singular value decomposition for S
Matrix R = Sigma_d * svd.matrixV().leftCols<d>().transpose();
// Count the number of blocks whose determinants have positive sign
size_t numPositiveBlocks = 0;
for (size_t i = 0; i < N; ++i) {
// Compute the determinant of the ith dxd block of R
double determinant = R.middleCols<d>(d * i).determinant();
if (determinant > 0)
++numPositiveBlocks;
}
if (numPositiveBlocks < N / 2) {
// Less than half of the total number of blocks have the correct sign.
// To reverse their orientations, multiply with a reflection matrix.
DiagonalMatrix reflector;
reflector.setIdentity();
reflector.diagonal()(d - 1) = -1;
R = reflector * R;
}
return R;
}
/* ************************************************************************* */
template <> Values ShonanAveraging<2>::roundSolutionS(const Matrix &S) const {
// Round to a 2*2N matrix
Matrix R = RoundSolutionS<2>(S);
// Finally, project each dxd rotation block to SO(2)
Values values;
for (size_t j = 0; j < nrUnknowns(); ++j) {
const Rot2 Ri = Rot2::atan2(R(1, 2 * j), R(0, 2 * j));
values.insert(j, Ri);
}
return values;
}
template <> Values ShonanAveraging<3>::roundSolutionS(const Matrix &S) const {
// Round to a 3*3N matrix
Matrix R = RoundSolutionS<3>(S);
// Finally, project each dxd rotation block to SO(3)
Values values;
for (size_t j = 0; j < nrUnknowns(); ++j) {
const Rot3 Ri = Rot3::ClosestTo(R.middleCols<3>(3 * j));
values.insert(j, Ri);
}
return values;
}
/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::roundSolution(const Values &values) const {
// Project to pxdN Stiefel manifold...
Matrix S = StiefelElementMatrix(values);
// ...and call version above.
return roundSolutionS(S);
}
/* ************************************************************************* */
template <size_t d>
double ShonanAveraging<d>::cost(const Values &values) const {
NonlinearFactorGraph graph;
for (const auto &measurement : measurements_) {
const auto &keys = measurement.keys();
const auto &Rij = measurement.measured();
const auto &model = measurement.noiseModel();
graph.emplace_shared<FrobeniusBetweenFactor<SO<d>>>(
keys[0], keys[1], SO<d>(Rij.matrix()), model);
}
// Finally, project each dxd rotation block to SO(d)
Values result;
for (const auto it : values.filter<Rot>()) {
result.insert(it.key, SO<d>(it.value.matrix()));
}
return graph.error(result);
}
/* ************************************************************************* */
// Get kappa from noise model
template <typename T>
static double Kappa(const BinaryMeasurement<T> &measurement) {
const auto &isotropic = boost::dynamic_pointer_cast<noiseModel::Isotropic>(
measurement.noiseModel());
if (!isotropic) {
throw std::invalid_argument(
"Shonan averaging noise models must be isotropic.");
}
const double sigma = isotropic->sigma();
return 1.0 / (sigma * sigma);
}
/* ************************************************************************* */
template <size_t d> Sparse ShonanAveraging<d>::buildD() const {
// Each measurement contributes 2*d elements along the diagonal of the
// degree matrix.
static constexpr size_t stride = 2 * d;
// Reserve space for triplets
std::vector<Eigen::Triplet<double>> triplets;
triplets.reserve(stride * measurements_.size());
for (const auto &measurement : measurements_) {
// Get pose keys
const auto &keys = measurement.keys();
// Get kappa from noise model
double kappa = Kappa<Rot>(measurement);
const size_t di = d * keys[0], dj = d * keys[1];
for (size_t k = 0; k < d; k++) {
// Elements of ith block-diagonal
triplets.emplace_back(di + k, di + k, kappa);
// Elements of jth block-diagonal
triplets.emplace_back(dj + k, dj + k, kappa);
}
}
// Construct and return a sparse matrix from these triplets
const size_t dN = d * nrUnknowns();
Sparse D(dN, dN);
D.setFromTriplets(triplets.begin(), triplets.end());
return D;
}
/* ************************************************************************* */
template <size_t d> Sparse ShonanAveraging<d>::buildQ() const {
// Each measurement contributes 2*d^2 elements on a pair of symmetric
// off-diagonal blocks
static constexpr size_t stride = 2 * d * d;
// Reserve space for triplets
std::vector<Eigen::Triplet<double>> triplets;
triplets.reserve(stride * measurements_.size());
for (const auto &measurement : measurements_) {
// Get pose keys
const auto &keys = measurement.keys();
// Extract rotation measurement
const auto Rij = measurement.measured().matrix();
// Get kappa from noise model
double kappa = Kappa<Rot>(measurement);
const size_t di = d * keys[0], dj = d * keys[1];
for (size_t r = 0; r < d; r++) {
for (size_t c = 0; c < d; c++) {
// Elements of ij block
triplets.emplace_back(di + r, dj + c, kappa * Rij(r, c));
// Elements of ji block
triplets.emplace_back(dj + r, di + c, kappa * Rij(c, r));
}
}
}
// Construct and return a sparse matrix from these triplets
const size_t dN = d * nrUnknowns();
Sparse Q(dN, dN);
Q.setFromTriplets(triplets.begin(), triplets.end());
return Q;
}
/* ************************************************************************* */
template <size_t d>
Sparse ShonanAveraging<d>::computeLambda(const Matrix &S) const {
// Each pose contributes 2*d elements along the diagonal of Lambda
static constexpr size_t stride = d * d;
// Reserve space for triplets
const size_t N = nrUnknowns();
std::vector<Eigen::Triplet<double>> triplets;
triplets.reserve(stride * N);
// Do sparse-dense multiply to get Q*S'
auto QSt = Q_ * S.transpose();
for (size_t j = 0; j < N; j++) {
// Compute B, the building block for the j^th diagonal block of Lambda
const size_t dj = d * j;
Matrix B = QSt.middleRows(dj, d) * S.middleCols<d>(dj);
// Elements of jth block-diagonal
for (size_t r = 0; r < d; r++)
for (size_t c = 0; c < d; c++)
triplets.emplace_back(dj + r, dj + c, 0.5 * (B(r, c) + B(c, r)));
}
// Construct and return a sparse matrix from these triplets
Sparse Lambda(d * N, d * N);
Lambda.setFromTriplets(triplets.begin(), triplets.end());
return Lambda;
}
/* ************************************************************************* */
template <size_t d>
Sparse ShonanAveraging<d>::computeLambda(const Values &values) const {
// Project to pxdN Stiefel manifold...
Matrix S = StiefelElementMatrix(values);
// ...and call version above.
return computeLambda(S);
}
/* ************************************************************************* */
template <size_t d>
Sparse ShonanAveraging<d>::computeA(const Values &values) const {
assert(values.size() == nrUnknowns());
const Matrix S = StiefelElementMatrix(values);
auto Lambda = computeLambda(S);
return Lambda - Q_;
}
/* ************************************************************************* */
/// MINIMUM EIGENVALUE COMPUTATIONS
/** This is a lightweight struct used in conjunction with Spectra to compute
* the minimum eigenvalue and eigenvector of a sparse matrix A; it has a single
* nontrivial function, perform_op(x,y), that computes and returns the product
* y = (A + sigma*I) x */
struct MatrixProdFunctor {
// Const reference to an externally-held matrix whose minimum-eigenvalue we
// want to compute
const Sparse &A_;
// Spectral shift
double sigma_;
// Constructor
explicit MatrixProdFunctor(const Sparse &A, double sigma = 0)
: A_(A), sigma_(sigma) {}
int rows() const { return A_.rows(); }
int cols() const { return A_.cols(); }
// Matrix-vector multiplication operation
void perform_op(const double *x, double *y) const {
// Wrap the raw arrays as Eigen Vector types
Eigen::Map<const Vector> X(x, rows());
Eigen::Map<Vector> Y(y, rows());
// Do the multiplication using wrapped Eigen vectors
Y = A_ * X + sigma_ * X;
}
};
/// Function to compute the minimum eigenvalue of A using Lanczos in Spectra.
/// This does 2 things:
///
/// (1) Quick (coarse) eigenvalue computation to estimate the largest-magnitude
/// eigenvalue (2) A second eigenvalue computation applied to A-sigma*I, where
/// sigma is chosen to make the minimum eigenvalue of A the extremal eigenvalue
/// of A-sigma*I
///
/// Upon completion, this returns a boolean value indicating whether the minimum
/// eigenvalue was computed to the required precision -- if so, its sets the
/// values of minEigenValue and minEigenVector appropriately
/// Note that in the following function signature, S is supposed to be the
/// block-row-matrix that is a critical point for the optimization algorithm;
/// either S (Stiefel manifold) or R (block rotations). We use this to
/// construct a starting vector v for the Lanczos process that will be close to
/// the minimum eigenvector we're looking for whenever the relaxation is exact
/// -- this is a key feature that helps to make this method fast. Note that
/// instead of passing in all of S, it would be enough to pass in one of S's
/// *rows*, if that's more convenient.
// For the defaults, David Rosen says:
// - maxIterations refers to the max number of Lanczos iterations to run;
// ~1000 should be sufficiently large
// - We've been using 10^-4 for the nonnegativity tolerance
// - for numLanczosVectors, 20 is a good default value
static bool
SparseMinimumEigenValue(const Sparse &A, const Matrix &S, double *minEigenValue,
Vector *minEigenVector = 0, size_t *numIterations = 0,
size_t maxIterations = 1000,
double minEigenvalueNonnegativityTolerance = 10e-4,
Eigen::Index numLanczosVectors = 20) {
// a. Estimate the largest-magnitude eigenvalue of this matrix using Lanczos
MatrixProdFunctor lmOperator(A);
Spectra::SymEigsSolver<double, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN,
MatrixProdFunctor>
lmEigenValueSolver(&lmOperator, 1, std::min(numLanczosVectors, A.rows()));
lmEigenValueSolver.init();
const int lmConverged = lmEigenValueSolver.compute(
maxIterations, 1e-4, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN);
// Check convergence and bail out if necessary
if (lmConverged != 1)
return false;
const double lmEigenValue = lmEigenValueSolver.eigenvalues()(0);
if (lmEigenValue < 0) {
// The largest-magnitude eigenvalue is negative, and therefore also the
// minimum eigenvalue, so just return this solution
*minEigenValue = lmEigenValue;
if (minEigenVector) {
*minEigenVector = lmEigenValueSolver.eigenvectors(1).col(0);
minEigenVector->normalize(); // Ensure that this is a unit vector
}
return true;
}
// The largest-magnitude eigenvalue is positive, and is therefore the
// maximum eigenvalue. Therefore, after shifting the spectrum of A
// by -2*lmEigenValue (by forming A - 2*lambda_max*I), the shifted
// spectrum will lie in the interval [minEigenValue(A) - 2* lambda_max(A),
// -lambda_max*A]; in particular, the largest-magnitude eigenvalue of
// A - 2*lambda_max*I is minEigenValue - 2*lambda_max, with corresponding
// eigenvector v_min
MatrixProdFunctor minShiftedOperator(A, -2 * lmEigenValue);
Spectra::SymEigsSolver<double, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN,
MatrixProdFunctor>
minEigenValueSolver(&minShiftedOperator, 1,
std::min(numLanczosVectors, A.rows()));
// If S is a critical point of F, then S^T is also in the null space of S -
// Lambda(S) (cf. Lemma 6 of the tech report), and therefore its rows are
// eigenvectors corresponding to the eigenvalue 0. In the case that the
// relaxation is exact, this is the *minimum* eigenvalue, and therefore the
// rows of S are exactly the eigenvectors that we're looking for. On the
// other hand, if the relaxation is *not* exact, then S - Lambda(S) has at
// least one strictly negative eigenvalue, and the rows of S are *unstable
// fixed points* for the Lanczos iterations. Thus, we will take a slightly
// "fuzzed" version of the first row of S as an initialization for the
// Lanczos iterations; this allows for rapid convergence in the case that
// the relaxation is exact (since are starting close to a solution), while
// simultaneously allowing the iterations to escape from this fixed point in
// the case that the relaxation is not exact.
Vector v0 = S.row(0).transpose();
Vector perturbation(v0.size());
perturbation.setRandom();
perturbation.normalize();
Vector xinit = v0 + (.03 * v0.norm()) * perturbation; // Perturb v0 by ~3%
// Use this to initialize the eigensolver
minEigenValueSolver.init(xinit.data());
// Now determine the relative precision required in the Lanczos method in
// order to be able to estimate the smallest eigenvalue within an *absolute*
// tolerance of 'minEigenvalueNonnegativityTolerance'
const int minConverged = minEigenValueSolver.compute(
maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue,
Spectra::SELECT_EIGENVALUE::LARGEST_MAGN);
if (minConverged != 1)
return false;
*minEigenValue = minEigenValueSolver.eigenvalues()(0) + 2 * lmEigenValue;
if (minEigenVector) {
*minEigenVector = minEigenValueSolver.eigenvectors(1).col(0);
minEigenVector->normalize(); // Ensure that this is a unit vector
}
if (numIterations)
*numIterations = minEigenValueSolver.num_iterations();
return true;
}
/* ************************************************************************* */
template <size_t d> Sparse ShonanAveraging<d>::computeA(const Matrix &S) const {
auto Lambda = computeLambda(S);
return Lambda - Q_;
}
/* ************************************************************************* */
template <size_t d>
double ShonanAveraging<d>::computeMinEigenValue(const Values &values,
Vector *minEigenVector) const {
assert(values.size() == nrUnknowns());
const Matrix S = StiefelElementMatrix(values);
auto A = computeA(S);
double minEigenValue;
bool success = SparseMinimumEigenValue(A, S, &minEigenValue, minEigenVector);
if (!success) {
throw std::runtime_error(
"SparseMinimumEigenValue failed to compute minimum eigenvalue.");
}
return minEigenValue;
}
/* ************************************************************************* */
template <size_t d>
std::pair<double, Vector>
ShonanAveraging<d>::computeMinEigenVector(const Values &values) const {
Vector minEigenVector;
double minEigenValue = computeMinEigenValue(values, &minEigenVector);
return std::make_pair(minEigenValue, minEigenVector);
}
/* ************************************************************************* */
template <size_t d>
bool ShonanAveraging<d>::checkOptimality(const Values &values) const {
double minEigenValue = computeMinEigenValue(values);
return minEigenValue > parameters_.optimalityThreshold;
}
/* ************************************************************************* */
/// Create a tangent direction xi with eigenvector segment v_i
template <size_t d>
Vector ShonanAveraging<d>::MakeATangentVector(size_t p, const Vector &v,
size_t i) {
// Create a tangent direction xi with eigenvector segment v_i
const size_t dimension = SOn::Dimension(p);
const auto v_i = v.segment<d>(d * i);
Vector xi = Vector::Zero(dimension);
double sign = pow(-1.0, round((p + 1) / 2) + 1);
for (size_t j = 0; j < d; j++) {
xi(j + p - d - 1) = sign * v_i(d - j - 1);
sign = -sign;
}
return xi;
}
/* ************************************************************************* */
template <size_t d>
Matrix ShonanAveraging<d>::riemannianGradient(size_t p,
const Values &values) const {
Matrix S_dot = StiefelElementMatrix(values);
// calculate the gradient of F(Q_dot) at Q_dot
Matrix euclideanGradient = 2 * (L_ * (S_dot.transpose())).transpose();
// cout << "euclidean gradient rows and cols" << euclideanGradient.rows() <<
// "\t" << euclideanGradient.cols() << endl;
// project the gradient onto the entire euclidean space
Matrix symBlockDiagProduct(p, d * nrUnknowns());
for (size_t i = 0; i < nrUnknowns(); i++) {
// Compute block product
const size_t di = d * i;
const Matrix P = S_dot.middleCols<d>(di).transpose() *
euclideanGradient.middleCols<d>(di);
// Symmetrize this block
Matrix S = .5 * (P + P.transpose());
// Compute S_dot * S and set corresponding block
symBlockDiagProduct.middleCols<d>(di) = S_dot.middleCols<d>(di) * S;
}
Matrix riemannianGradient = euclideanGradient - symBlockDiagProduct;
return riemannianGradient;
}
/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::LiftwithDescent(size_t p, const Values &values,
const Vector &minEigenVector) {
Values lifted = LiftTo<SOn>(p, values);
for (auto it : lifted.filter<SOn>()) {
// Create a tangent direction xi with eigenvector segment v_i
// Assumes key is 0-based integer
const Vector xi = MakeATangentVector(p, minEigenVector, it.key);
// Move the old value in the descent direction
it.value = it.value.retract(xi);
}
return lifted;
}
/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::initializeWithDescent(
size_t p, const Values &values, const Vector &minEigenVector,
double minEigenValue, double gradienTolerance,
double preconditionedGradNormTolerance) const {
double funcVal = costAt(p - 1, values);
double alphaMin = 1e-2;
double alpha =
std::max(1024 * alphaMin, 10 * gradienTolerance / fabs(minEigenValue));
vector<double> alphas;
vector<double> fvals;
// line search
while ((alpha >= alphaMin)) {
Values Qplus = LiftwithDescent(p, values, alpha * minEigenVector);
double funcValTest = costAt(p, Qplus);
Matrix gradTest = riemannianGradient(p, Qplus);
double gradTestNorm = gradTest.norm();
// Record alpha and funcVal
alphas.push_back(alpha);
fvals.push_back(funcValTest);
if ((funcVal > funcValTest) && (gradTestNorm > gradienTolerance)) {
return Qplus;
}
alpha /= 2;
}
auto fminIter = min_element(fvals.begin(), fvals.end());
auto minIdx = distance(fvals.begin(), fminIter);
double fMin = fvals[minIdx];
double aMin = alphas[minIdx];
if (fMin < funcVal) {
Values Qplus = LiftwithDescent(p, values, aMin * minEigenVector);
return Qplus;
}
return LiftwithDescent(p, values, alpha * minEigenVector);
}
/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::initializeRandomly(std::mt19937 &rng) const {
Values initial;
for (size_t j = 0; j < nrUnknowns(); j++) {
initial.insert(j, Rot::Random(rng));
}
return initial;
}
/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::initializeRandomly() const {
return ShonanAveraging<d>::initializeRandomly(kRandomNumberGenerator);
}
/* ************************************************************************* */
template <size_t d>
std::pair<Values, double> ShonanAveraging<d>::run(const Values &initialEstimate,
size_t pMin,
size_t pMax) const {
Values Qstar;
Values initialSOp = LiftTo<Rot>(pMin, initialEstimate); // lift to pMin!
for (size_t p = pMin; p <= pMax; p++) {
// Optimize until convergence at this level
Qstar = tryOptimizingAt(p, initialSOp);
// Check certificate of global optimzality
Vector minEigenVector;
double minEigenValue = computeMinEigenValue(Qstar, &minEigenVector);
if (minEigenValue > parameters_.optimalityThreshold) {
// If at global optimum, round and return solution
const Values SO3Values = roundSolution(Qstar);
return std::make_pair(SO3Values, minEigenValue);
}
// Not at global optimimum yet, so check whether we will go to next level
if (p != pMax) {
// Calculate initial estimate for next level by following minEigenVector
initialSOp =
initializeWithDescent(p + 1, Qstar, minEigenVector, minEigenValue);
}
}
throw std::runtime_error("Shonan::run did not converge for given pMax");
}
/* ************************************************************************* */
// Explicit instantiation for d=2
template class ShonanAveraging<2>;
ShonanAveraging2::ShonanAveraging2(const Measurements &measurements,
const Parameters &parameters)
: ShonanAveraging<2>(measurements, parameters) {}
ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters &parameters)
: ShonanAveraging<2>(parseMeasurements<Rot2>(g2oFile), parameters) {}
/* ************************************************************************* */
// Explicit instantiation for d=3
template class ShonanAveraging<3>;
ShonanAveraging3::ShonanAveraging3(const Measurements &measurements,
const Parameters &parameters)
: ShonanAveraging<3>(measurements, parameters) {}
ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters &parameters)
: ShonanAveraging<3>(parseMeasurements<Rot3>(g2oFile), parameters) {}
// TODO(frank): Deprecate after we land pybind wrapper
// Extract Rot3 measurement from Pose3 betweenfactors
// Modeled after similar function in dataset.cpp
static BinaryMeasurement<Rot3>
convert(const BetweenFactor<Pose3>::shared_ptr &f) {
auto gaussian =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
if (!gaussian)
throw std::invalid_argument(
"parseMeasurements<Rot3> can only convert Pose3 measurements "
"with Gaussian noise models.");
const Matrix6 M = gaussian->covariance();
return BinaryMeasurement<Rot3>(
f->key1(), f->key2(), f->measured().rotation(),
noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true));
}
static ShonanAveraging3::Measurements
extractRot3Measurements(const BetweenFactorPose3s &factors) {
ShonanAveraging3::Measurements result;
result.reserve(factors.size());
for (auto f : factors)
result.push_back(convert(f));
return result;
}
ShonanAveraging3::ShonanAveraging3(const BetweenFactorPose3s &factors,
const Parameters &parameters)
: ShonanAveraging<3>(extractRot3Measurements(factors), parameters) {}
/* ************************************************************************* */
} // namespace gtsam

356
gtsam/sfm/ShonanAveraging.h Normal file
View File

@ -0,0 +1,356 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ShonanAveraging.h
* @date March 2019 - August 2020
* @author Frank Dellaert, David Rosen, and Jing Wu
* @brief Shonan Averaging algorithm
*/
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
#include <gtsam/sfm/BinaryMeasurement.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/dllexport.h>
#include <Eigen/Sparse>
#include <map>
#include <string>
#include <type_traits>
#include <utility>
namespace gtsam {
class NonlinearFactorGraph;
class LevenbergMarquardtOptimizer;
/// Parameters governing optimization etc.
template <size_t d> struct GTSAM_EXPORT ShonanAveragingParameters {
// Select Rot2 or Rot3 interface based template parameter d
using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
using Anchor = std::pair<size_t, Rot>;
// Paremeters themselves:
LevenbergMarquardtParams lm; // LM parameters
double optimalityThreshold; // threshold used in checkOptimality
Anchor anchor; // pose to use as anchor if not Karcher
double alpha; // weight of anchor-based prior (default 0)
double beta; // weight of Karcher-based prior (default 1)
double gamma; // weight of gauge-fixing factors (default 0)
ShonanAveragingParameters(const LevenbergMarquardtParams &lm =
LevenbergMarquardtParams::CeresDefaults(),
const std::string &method = "JACOBI",
double optimalityThreshold = -1e-4,
double alpha = 0.0, double beta = 1.0,
double gamma = 0.0);
LevenbergMarquardtParams getLMParams() const { return lm; }
void setOptimalityThreshold(double value) { optimalityThreshold = value; }
double getOptimalityThreshold() const { return optimalityThreshold; }
void setAnchor(size_t index, const Rot &value) { anchor = {index, value}; }
void setAnchorWeight(double value) { alpha = value; }
double getAnchorWeight() { return alpha; }
void setKarcherWeight(double value) { beta = value; }
double getKarcherWeight() { return beta; }
void setGaugesWeight(double value) { gamma = value; }
double getGaugesWeight() { return gamma; }
};
using ShonanAveragingParameters2 = ShonanAveragingParameters<2>;
using ShonanAveragingParameters3 = ShonanAveragingParameters<3>;
/**
* Class that implements Shonan Averaging from our ECCV'20 paper.
* Note: The "basic" API uses all Rot values (Rot2 or Rot3, depending on value
* of d), whereas the different levels and "advanced" API at SO(p) needs Values
* of type SOn<Dynamic>.
*
* The template parameter d can be 2 or 3.
* Both are specialized in the .cpp file.
*
* If you use this code in your work, please consider citing our paper:
* Shonan Rotation Averaging, Global Optimality by Surfing SO(p)^n
* Frank Dellaert, David M. Rosen, Jing Wu, Robert Mahony, and Luca Carlone,
* European Computer Vision Conference, 2020.
* You can view our ECCV spotlight video at https://youtu.be/5ppaqMyHtE0
*/
template <size_t d> class GTSAM_EXPORT ShonanAveraging {
public:
using Sparse = Eigen::SparseMatrix<double>;
// Define the Parameters type and use its typedef of the rotation type:
using Parameters = ShonanAveragingParameters<d>;
using Rot = typename Parameters::Rot;
// We store SO(d) BetweenFactors to get noise model
// TODO(frank): use BinaryMeasurement?
using Measurements = std::vector<BinaryMeasurement<Rot>>;
private:
Parameters parameters_;
Measurements measurements_;
size_t nrUnknowns_;
Sparse D_; // Sparse (diagonal) degree matrix
Sparse Q_; // Sparse measurement matrix, == \tilde{R} in Eriksson18cvpr
Sparse L_; // connection Laplacian L = D - Q, needed for optimality check
/**
* Build 3Nx3N sparse matrix consisting of rotation measurements, arranged as
* (i,j) and (j,i) blocks within a sparse matrix.
*/
Sparse buildQ() const;
/// Build 3Nx3N sparse degree matrix D
Sparse buildD() const;
public:
/// @name Standard Constructors
/// @{
/// Construct from set of relative measurements (given as BetweenFactor<Rot3>
/// for now) NoiseModel *must* be isotropic.
ShonanAveraging(const Measurements &measurements,
const Parameters &parameters = Parameters());
/// @}
/// @name Query properties
/// @{
/// Return number of unknowns
size_t nrUnknowns() const { return nrUnknowns_; }
/// Return number of measurements
size_t nrMeasurements() const { return measurements_.size(); }
/// k^th binary measurement
const BinaryMeasurement<Rot> &measurement(size_t k) const {
return measurements_[k];
}
/// k^th measurement, as a Rot.
const Rot &measured(size_t k) const { return measurements_[k].measured(); }
/// Keys for k^th measurement, as a vector of Key values.
const KeyVector &keys(size_t k) const { return measurements_[k].keys(); }
/// @}
/// @name Matrix API (advanced use, debugging)
/// @{
Sparse D() const { return D_; } ///< Sparse version of D
Matrix denseD() const { return Matrix(D_); } ///< Dense version of D
Sparse Q() const { return Q_; } ///< Sparse version of Q
Matrix denseQ() const { return Matrix(Q_); } ///< Dense version of Q
Sparse L() const { return L_; } ///< Sparse version of L
Matrix denseL() const { return Matrix(L_); } ///< Dense version of L
/// Version that takes pxdN Stiefel manifold elements
Sparse computeLambda(const Matrix &S) const;
/// Dense versions of computeLambda for wrapper/testing
Matrix computeLambda_(const Values &values) const {
return Matrix(computeLambda(values));
}
/// Dense versions of computeLambda for wrapper/testing
Matrix computeLambda_(const Matrix &S) const {
return Matrix(computeLambda(S));
}
/// Compute A matrix whose Eigenvalues we will examine
Sparse computeA(const Values &values) const;
/// Version that takes pxdN Stiefel manifold elements
Sparse computeA(const Matrix &S) const;
/// Dense version of computeA for wrapper/testing
Matrix computeA_(const Values &values) const {
return Matrix(computeA(values));
}
/// Project to pxdN Stiefel manifold
static Matrix StiefelElementMatrix(const Values &values);
/**
* Compute minimum eigenvalue for optimality check.
* @param values: should be of type SOn
*/
double computeMinEigenValue(const Values &values,
Vector *minEigenVector = nullptr) const;
/// Project pxdN Stiefel manifold matrix S to Rot3^N
Values roundSolutionS(const Matrix &S) const;
/// Create a tangent direction xi with eigenvector segment v_i
static Vector MakeATangentVector(size_t p, const Vector &v, size_t i);
/// Calculate the riemannian gradient of F(values) at values
Matrix riemannianGradient(size_t p, const Values &values) const;
/**
* Lift up the dimension of values in type SO(p-1) with descent direction
* provided by minEigenVector and return new values in type SO(p)
*/
static Values LiftwithDescent(size_t p, const Values &values,
const Vector &minEigenVector);
/**
* Given some values at p-1, return new values at p, by doing a line search
* along the descent direction, computed from the minimum eigenvector at p-1.
* @param values should be of type SO(p-1)
* @param minEigenVector corresponding to minEigenValue at level p-1
* @return values of type SO(p)
*/
Values
initializeWithDescent(size_t p, const Values &values,
const Vector &minEigenVector, double minEigenValue,
double gradienTolerance = 1e-2,
double preconditionedGradNormTolerance = 1e-4) const;
/// @}
/// @name Advanced API
/// @{
/**
* Build graph for SO(p)
* @param p the dimensionality of the rotation manifold to optimize over
*/
NonlinearFactorGraph buildGraphAt(size_t p) const;
/**
* Calculate cost for SO(p)
* Values should be of type SO(p)
*/
double costAt(size_t p, const Values &values) const;
/**
* Given an estimated local minimum Yopt for the (possibly lifted)
* relaxation, this function computes and returns the block-diagonal elements
* of the corresponding Lagrange multiplier.
*/
Sparse computeLambda(const Values &values) const;
/**
* Compute minimum eigenvalue for optimality check.
* @param values: should be of type SOn
* @return minEigenVector and minEigenValue
*/
std::pair<double, Vector> computeMinEigenVector(const Values &values) const;
/**
* Check optimality
* @param values: should be of type SOn
*/
bool checkOptimality(const Values &values) const;
/**
* Try to create optimizer at SO(p)
* @param p the dimensionality of the rotation manifold to optimize over
* @param initial initial SO(p) values
* @return lm optimizer
*/
boost::shared_ptr<LevenbergMarquardtOptimizer> createOptimizerAt(
size_t p, const Values &initial) const;
/**
* Try to optimize at SO(p)
* @param p the dimensionality of the rotation manifold to optimize over
* @param initial initial SO(p) values
* @return SO(p) values
*/
Values tryOptimizingAt(size_t p, const Values &initial) const;
/**
* Project from SO(p) to Rot2 or Rot3
* Values should be of type SO(p)
*/
Values projectFrom(size_t p, const Values &values) const;
/**
* Project from SO(p)^N to Rot2^N or Rot3^N
* Values should be of type SO(p)
*/
Values roundSolution(const Values &values) const;
/// Lift Values of type T to SO(p)
template <class T> static Values LiftTo(size_t p, const Values &values) {
Values result;
for (const auto it : values.filter<T>()) {
result.insert(it.key, SOn::Lift(p, it.value.matrix()));
}
return result;
}
/// @}
/// @name Basic API
/// @{
/**
* Calculate cost for SO(3)
* Values should be of type Rot3
*/
double cost(const Values &values) const;
/**
* Initialize randomly at SO(d)
* @param rng random number generator
* Example:
* std::mt19937 rng(42);
* Values initial = initializeRandomly(rng, p);
*/
Values initializeRandomly(std::mt19937 &rng) const;
/// Random initialization for wrapper, fixed random seed.
Values initializeRandomly() const;
/**
* Optimize at different values of p until convergence.
* @param initial initial Rot3 values
* @param pMin value of p to start Riemanian staircase at (default: d).
* @param pMax maximum value of p to try (default: 10)
* @return (Rot3 values, minimum eigenvalue)
*/
std::pair<Values, double> run(const Values &initialEstimate, size_t pMin = d,
size_t pMax = 10) const;
/// @}
};
// Subclasses for d=2 and d=3 that explicitly instantiate, as well as provide a
// convenience interface with file access.
class ShonanAveraging2 : public ShonanAveraging<2> {
public:
ShonanAveraging2(const Measurements &measurements,
const Parameters &parameters = Parameters());
ShonanAveraging2(string g2oFile, const Parameters &parameters = Parameters());
};
class ShonanAveraging3 : public ShonanAveraging<3> {
public:
ShonanAveraging3(const Measurements &measurements,
const Parameters &parameters = Parameters());
ShonanAveraging3(string g2oFile, const Parameters &parameters = Parameters());
// TODO(frank): Deprecate after we land pybind wrapper
ShonanAveraging3(const BetweenFactorPose3s &factors,
const Parameters &parameters = Parameters());
};
} // namespace gtsam

141
gtsam/sfm/ShonanFactor.cpp Normal file
View File

@ -0,0 +1,141 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file FrobeniusFactor.cpp
* @date March 2019
* @author Frank Dellaert
* @brief Various factors that minimize some Frobenius norm
*/
#include <gtsam/sfm/ShonanFactor.h>
#include <gtsam/base/timing.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/FrobeniusFactor.h>
#include <cmath>
#include <iostream>
#include <vector>
using namespace std;
namespace gtsam {
//******************************************************************************
template <size_t d>
ShonanFactor<d>::ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p,
const SharedNoiseModel &model,
const boost::shared_ptr<Matrix> &G)
: NoiseModelFactor2<SOn, SOn>(ConvertNoiseModel(model, p * d), j1, j2),
M_(R12.matrix()), // d*d in all cases
p_(p), // 4 for SO(4)
pp_(p * p), // 16 for SO(4)
G_(G) {
if (noiseModel()->dim() != d * p_)
throw std::invalid_argument(
"ShonanFactor: model with incorrect dimension.");
if (!G) {
G_ = boost::make_shared<Matrix>();
*G_ = SOn::VectorizedGenerators(p); // expensive!
}
if (static_cast<size_t>(G_->rows()) != pp_ ||
static_cast<size_t>(G_->cols()) != SOn::Dimension(p))
throw std::invalid_argument("ShonanFactor: passed in generators "
"of incorrect dimension.");
}
//******************************************************************************
template <size_t d>
void ShonanFactor<d>::print(const std::string &s,
const KeyFormatter &keyFormatter) const {
std::cout << s << "ShonanFactor<" << p_ << ">(" << keyFormatter(key1()) << ","
<< keyFormatter(key2()) << ")\n";
traits<Matrix>::Print(M_, " M: ");
noiseModel_->print(" noise model: ");
}
//******************************************************************************
template <size_t d>
bool ShonanFactor<d>::equals(const NonlinearFactor &expected,
double tol) const {
auto e = dynamic_cast<const ShonanFactor *>(&expected);
return e != nullptr && NoiseModelFactor2<SOn, SOn>::equals(*e, tol) &&
p_ == e->p_ && M_ == e->M_;
}
//******************************************************************************
template <size_t d>
void ShonanFactor<d>::fillJacobians(const Matrix &M1, const Matrix &M2,
boost::optional<Matrix &> H1,
boost::optional<Matrix &> H2) const {
gttic(ShonanFactor_Jacobians);
const size_t dim = p_ * d; // Stiefel manifold dimension
if (H1) {
// If asked, calculate Jacobian H1 as as (M' \otimes M1) * G
// M' = dxd, M1 = pxp, G = (p*p)xDim(p), result should be dim x Dim(p)
// (M' \otimes M1) is dim*dim, but last pp-dim columns are zero
*H1 = Matrix::Zero(dim, G_->cols());
for (size_t j = 0; j < d; j++) {
auto MG_j = M1 * G_->middleRows(j * p_, p_); // p_ * Dim(p)
for (size_t i = 0; i < d; i++) {
H1->middleRows(i * p_, p_) -= M_(j, i) * MG_j;
}
}
}
if (H2) {
// If asked, calculate Jacobian H2 as as (I_d \otimes M2) * G
// I_d = dxd, M2 = pxp, G = (p*p)xDim(p), result should be dim x Dim(p)
// (I_d \otimes M2) is dim*dim, but again last pp-dim columns are zero
H2->resize(dim, G_->cols());
for (size_t i = 0; i < d; i++) {
H2->middleRows(i * p_, p_) = M2 * G_->middleRows(i * p_, p_);
}
}
}
//******************************************************************************
template <size_t d>
Vector ShonanFactor<d>::evaluateError(const SOn &Q1, const SOn &Q2,
boost::optional<Matrix &> H1,
boost::optional<Matrix &> H2) const {
gttic(ShonanFactor_evaluateError);
const Matrix &M1 = Q1.matrix();
const Matrix &M2 = Q2.matrix();
if (M1.rows() != static_cast<int>(p_) || M2.rows() != static_cast<int>(p_))
throw std::invalid_argument("Invalid dimension SOn values passed to "
"ShonanFactor<d>::evaluateError");
const size_t dim = p_ * d; // Stiefel manifold dimension
Vector fQ2(dim), hQ1(dim);
// Vectorize and extract only d leftmost columns, i.e. vec(M2*P)
fQ2 << Eigen::Map<const Matrix>(M2.data(), dim, 1);
// Vectorize M1*P*R12
const Matrix Q1PR12 = M1.leftCols<d>() * M_;
hQ1 << Eigen::Map<const Matrix>(Q1PR12.data(), dim, 1);
this->fillJacobians(M1, M2, H1, H2);
return fQ2 - hQ1;
}
/* ************************************************************************* */
// Explicit instantiation for d=2 and d=3
template class ShonanFactor<2>;
template class ShonanFactor<3>;
//******************************************************************************
} // namespace gtsam

91
gtsam/sfm/ShonanFactor.h Normal file
View File

@ -0,0 +1,91 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ShonanFactor.h
* @date March 2019
* @author Frank Dellaert
* @brief Main factor type in Shonan averaging, on SO(n) pairs
*/
#pragma once
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SOn.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <type_traits>
namespace gtsam {
/**
* ShonanFactor is a BetweenFactor that moves in SO(p), but will
* land on the SO(d) sub-manifold of SO(p) at the global minimum. It projects
* the SO(p) matrices down to a Stiefel manifold of p*d matrices.
*/
template <size_t d>
class GTSAM_EXPORT ShonanFactor : public NoiseModelFactor2<SOn, SOn> {
Matrix M_; ///< measured rotation between R1 and R2
size_t p_, pp_; ///< dimensionality constants
boost::shared_ptr<Matrix> G_; ///< matrix of vectorized generators
// Select Rot2 or Rot3 interface based template parameter d
using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
public:
/// @name Constructor
/// @{
/// Constructor. Note we convert to d*p-dimensional noise model.
/// To save memory and mallocs, pass in the vectorized Lie algebra generators:
/// G = boost::make_shared<Matrix>(SOn::VectorizedGenerators(p));
ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p,
const SharedNoiseModel &model = nullptr,
const boost::shared_ptr<Matrix> &G = nullptr);
/// @}
/// @name Testable
/// @{
/// print with optional string
void
print(const std::string &s,
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
/// assert equality up to a tolerance
bool equals(const NonlinearFactor &expected,
double tol = 1e-9) const override;
/// @}
/// @name NoiseModelFactor2 methods
/// @{
/// Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0]
/// projects down from SO(p) to the Stiefel manifold of px3 matrices.
Vector
evaluateError(const SOn &Q1, const SOn &Q2,
boost::optional<Matrix &> H1 = boost::none,
boost::optional<Matrix &> H2 = boost::none) const override;
/// @}
private:
/// Calculate Jacobians if asked, Only implemented for d=2 and 3 in .cpp
void fillJacobians(const Matrix &M1, const Matrix &M2,
boost::optional<Matrix &> H1,
boost::optional<Matrix &> H2) const;
};
// Explicit instantiation for d=2 and d=3 in .cpp file:
using ShonanFactor2 = ShonanFactor<2>;
using ShonanFactor3 = ShonanFactor<3>;
} // namespace gtsam

108
gtsam/sfm/ShonanGaugeFactor.h Normal file
View File

@ -0,0 +1,108 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ShonanGaugeFactor.h
* @date March 2019
* @author Frank Dellaert
* @brief Factor used in Shonan Averaging to clamp down gauge freedom
*/
#pragma once
#include <gtsam/geometry/SOn.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <boost/assign/list_of.hpp>
namespace gtsam {
/**
* The ShonanGaugeFactor creates a constraint on a single SO(n) to avoid moving
* in the stabilizer.
*
* Details: SO(p) contains the p*3 Stiefel matrices of orthogonal frames: we
* take those to be the 3 columns on the left.
* The P*P skew-symmetric matrices associated with so(p) can be partitioned as
* (Appendix B in the ECCV paper):
* | [w] -K' |
* | K [g] |
* where w is the SO(3) space, K are the extra Stiefel diemnsions (wormhole!)
* and (g)amma are extra dimensions in SO(p) that do not modify the cost
* function. The latter corresponds to rotations SO(p-d), and so the first few
* values of p-d for d==3 with their corresponding dimensionality are {0:0, 1:0,
* 2:1, 3:3, ...}
*
* The ShonanGaugeFactor adds a unit Jacobian to these extra dimensions,
* essentially restricting the optimization to the Stiefel manifold.
*/
class GTSAM_EXPORT ShonanGaugeFactor : public NonlinearFactor {
// Row dimension, equal to the dimensionality of SO(p-d)
size_t rows_;
/// Constant Jacobian
boost::shared_ptr<JacobianFactor> whitenedJacobian_;
public:
/**
* Construct from key for an SO(p) matrix, for base dimension d (2 or 3)
* If parameter gamma is given, it acts as a precision = 1/sigma^2, and
* the Jacobian will be multiplied with 1/sigma = sqrt(gamma).
*/
ShonanGaugeFactor(Key key, size_t p, size_t d = 3,
boost::optional<double> gamma = boost::none)
: NonlinearFactor(boost::assign::cref_list_of<1>(key)) {
if (p < d) {
throw std::invalid_argument("ShonanGaugeFactor must have p>=d.");
}
// Calculate dimensions
size_t q = p - d;
size_t P = SOn::Dimension(p); // dimensionality of SO(p)
rows_ = SOn::Dimension(q); // dimensionality of SO(q), the gauge
// Create constant Jacobian as a rows_*P matrix: there are rows_ penalized
// dimensions, but it is a bit tricky to find them among the P columns.
// The key is to look at how skew-symmetric matrices are laid out in SOn.h:
// the first tangent dimension will always be included, but beyond that we
// have to be careful. We always need to skip the d top-rows of the skew-
// symmetric matrix as they below to K, part of the Stiefel manifold.
Matrix A(rows_, P);
A.setZero();
double invSigma = gamma ? std::sqrt(*gamma) : 1.0;
size_t i = 0, j = 0, n = p - 1 - d;
while (i < rows_) {
A.block(i, j, n, n) = invSigma * Matrix::Identity(n, n);
i += n;
j += n + d; // skip d columns
n -= 1;
}
// TODO(frank): assign the right one in the right columns
whitenedJacobian_ =
boost::make_shared<JacobianFactor>(key, A, Vector::Zero(rows_));
}
/// Destructor
virtual ~ShonanGaugeFactor() {}
/// Calculate the error of the factor: always zero
double error(const Values &c) const override { return 0; }
/// get the dimension of the factor (number of rows on linearization)
size_t dim() const override { return rows_; }
/// linearize to a GaussianFactor
boost::shared_ptr<GaussianFactor> linearize(const Values &c) const override {
return whitenedJacobian_;
}
};
// \ShonanGaugeFactor
} // namespace gtsam

360
gtsam/sfm/tests/testShonanAveraging.cpp Normal file
View File

@ -0,0 +1,360 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testShonanAveraging.cpp
* @date March 2019
* @author Frank Dellaert
* @brief Unit tests for Shonan Averaging algorithm
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/sfm/ShonanAveraging.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/FrobeniusFactor.h>
#include <algorithm>
#include <iostream>
#include <map>
#include <random>
using namespace std;
using namespace gtsam;
template <size_t d>
using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
template <size_t d>
using Pose = typename std::conditional<d == 2, Pose2, Pose3>::type;
ShonanAveraging3 fromExampleName(
const std::string &name,
ShonanAveraging3::Parameters parameters = ShonanAveraging3::Parameters()) {
string g2oFile = findExampleDataFile(name);
return ShonanAveraging3(g2oFile, parameters);
}
static const ShonanAveraging3 kShonan = fromExampleName("toyExample.g2o");
static std::mt19937 kRandomNumberGenerator(42);
/* ************************************************************************* */
TEST(ShonanAveraging3, checkConstructor) {
EXPECT_LONGS_EQUAL(5, kShonan.nrUnknowns());
EXPECT_LONGS_EQUAL(15, kShonan.D().rows());
EXPECT_LONGS_EQUAL(15, kShonan.D().cols());
auto D = kShonan.denseD();
EXPECT_LONGS_EQUAL(15, D.rows());
EXPECT_LONGS_EQUAL(15, D.cols());
EXPECT_LONGS_EQUAL(15, kShonan.Q().rows());
EXPECT_LONGS_EQUAL(15, kShonan.Q().cols());
auto Q = kShonan.denseQ();
EXPECT_LONGS_EQUAL(15, Q.rows());
EXPECT_LONGS_EQUAL(15, Q.cols());
EXPECT_LONGS_EQUAL(15, kShonan.L().rows());
EXPECT_LONGS_EQUAL(15, kShonan.L().cols());
auto L = kShonan.denseL();
EXPECT_LONGS_EQUAL(15, L.rows());
EXPECT_LONGS_EQUAL(15, L.cols());
}
/* ************************************************************************* */
TEST(ShonanAveraging3, buildGraphAt) {
auto graph = kShonan.buildGraphAt(5);
EXPECT_LONGS_EQUAL(7, graph.size());
}
/* ************************************************************************* */
TEST(ShonanAveraging3, checkOptimality) {
const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
Values random = ShonanAveraging3::LiftTo<Rot3>(4, randomRotations); // lift to 4!
auto Lambda = kShonan.computeLambda(random);
EXPECT_LONGS_EQUAL(15, Lambda.rows());
EXPECT_LONGS_EQUAL(15, Lambda.cols());
EXPECT_LONGS_EQUAL(45, Lambda.nonZeros());
auto lambdaMin = kShonan.computeMinEigenValue(random);
// EXPECT_DOUBLES_EQUAL(-5.2964625490657866, lambdaMin,
// 1e-4); // Regression test
EXPECT_DOUBLES_EQUAL(-414.87376657555996, lambdaMin,
1e-4); // Regression test
EXPECT(!kShonan.checkOptimality(random));
}
/* ************************************************************************* */
TEST(ShonanAveraging3, tryOptimizingAt3) {
const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
Values initial = ShonanAveraging3::LiftTo<Rot3>(3, randomRotations); // convert to SOn
EXPECT(!kShonan.checkOptimality(initial));
const Values result = kShonan.tryOptimizingAt(3, initial);
EXPECT(kShonan.checkOptimality(result));
auto lambdaMin = kShonan.computeMinEigenValue(result);
EXPECT_DOUBLES_EQUAL(-5.427688831332745e-07, lambdaMin,
1e-4); // Regression test
EXPECT_DOUBLES_EQUAL(0, kShonan.costAt(3, result), 1e-4);
const Values SO3Values = kShonan.roundSolution(result);
EXPECT_DOUBLES_EQUAL(0, kShonan.cost(SO3Values), 1e-4);
}
/* ************************************************************************* */
TEST(ShonanAveraging3, tryOptimizingAt4) {
const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
Values random = ShonanAveraging3::LiftTo<Rot3>(4, randomRotations); // lift to 4!
const Values result = kShonan.tryOptimizingAt(4, random);
EXPECT(kShonan.checkOptimality(result));
EXPECT_DOUBLES_EQUAL(0, kShonan.costAt(4, result), 1e-3);
auto lambdaMin = kShonan.computeMinEigenValue(result);
EXPECT_DOUBLES_EQUAL(-5.427688831332745e-07, lambdaMin,
1e-4); // Regression test
const Values SO3Values = kShonan.roundSolution(result);
EXPECT_DOUBLES_EQUAL(0, kShonan.cost(SO3Values), 1e-4);
}
/* ************************************************************************* */
TEST(ShonanAveraging3, MakeATangentVector) {
Vector9 v;
v << 1, 2, 3, 4, 5, 6, 7, 8, 9;
Matrix expected(5, 5);
expected << 0, 0, 0, 0, -4, //
0, 0, 0, 0, -5, //
0, 0, 0, 0, -6, //
0, 0, 0, 0, 0, //
4, 5, 6, 0, 0;
const Vector xi_1 = ShonanAveraging3::MakeATangentVector(5, v, 1);
const auto actual = SOn::Hat(xi_1);
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(ShonanAveraging3, LiftTo) {
auto I = genericValue(Rot3());
Values initial{{0, I}, {1, I}, {2, I}};
Values lifted = ShonanAveraging3::LiftTo<Rot3>(5, initial);
EXPECT(assert_equal(SOn(5), lifted.at<SOn>(0)));
}
/* ************************************************************************* */
TEST(ShonanAveraging3, CheckWithEigen) {
// control randomness
static std::mt19937 rng(0);
Vector descentDirection = Vector::Random(15); // for use below
const Values randomRotations = kShonan.initializeRandomly(rng);
Values random = ShonanAveraging3::LiftTo<Rot3>(3, randomRotations);
// Optimize
const Values Qstar3 = kShonan.tryOptimizingAt(3, random);
// Compute Eigenvalue with Spectra solver
double lambda = kShonan.computeMinEigenValue(Qstar3);
// Check Eigenvalue with slow Eigen version, converts matrix A to dense matrix!
const Matrix S = ShonanAveraging3::StiefelElementMatrix(Qstar3);
auto A = kShonan.computeA(S);
bool computeEigenvectors = false;
Eigen::EigenSolver<Matrix> eigenSolver(Matrix(A), computeEigenvectors);
auto lambdas = eigenSolver.eigenvalues().real();
double minEigenValue = lambdas(0);
for (int i = 1; i < lambdas.size(); i++)
minEigenValue = min(lambdas(i), minEigenValue);
// Actual check
EXPECT_DOUBLES_EQUAL(minEigenValue, lambda, 1e-12);
// Construct test descent direction (as minEigenVector is not predictable
// across platforms, being one from a basically flat 3d- subspace)
// Check descent
Values initialQ4 =
ShonanAveraging3::LiftwithDescent(4, Qstar3, descentDirection);
EXPECT_LONGS_EQUAL(5, initialQ4.size());
// TODO(frank): uncomment this regression test: currently not repeatable
// across platforms.
// Matrix expected(4, 4);
// expected << 0.0459224, -0.688689, -0.216922, 0.690321, //
// 0.92381, 0.191931, 0.255854, 0.21042, //
// -0.376669, 0.301589, 0.687953, 0.542111, //
// -0.0508588, 0.630804, -0.643587, 0.43046;
// EXPECT(assert_equal(SOn(expected), initialQ4.at<SOn>(0), 1e-5));
}
/* ************************************************************************* */
TEST(ShonanAveraging3, initializeWithDescent) {
const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
Values random = ShonanAveraging3::LiftTo<Rot3>(3, randomRotations);
const Values Qstar3 = kShonan.tryOptimizingAt(3, random);
Vector minEigenVector;
double lambdaMin = kShonan.computeMinEigenValue(Qstar3, &minEigenVector);
Values initialQ4 =
kShonan.initializeWithDescent(4, Qstar3, minEigenVector, lambdaMin);
EXPECT_LONGS_EQUAL(5, initialQ4.size());
}
/* ************************************************************************* */
TEST(ShonanAveraging3, run) {
auto initial = kShonan.initializeRandomly(kRandomNumberGenerator);
auto result = kShonan.run(initial, 5);
EXPECT_DOUBLES_EQUAL(0, kShonan.cost(result.first), 1e-3);
EXPECT_DOUBLES_EQUAL(-5.427688831332745e-07, result.second,
1e-4); // Regression test
}
/* ************************************************************************* */
namespace klaus {
// The data in the file is the Colmap solution
const Rot3 wR0(0.9992281076190063, -0.02676080288219576, -0.024497002638379624,
-0.015064701622500615);
const Rot3 wR1(0.998239108728862, -0.049543805396343954, -0.03232420352077356,
-0.004386230477751116);
const Rot3 wR2(0.9925378735259738, -0.07993768981394891, 0.0825062894866454,
-0.04088089479075661);
} // namespace klaus
TEST(ShonanAveraging3, runKlaus) {
using namespace klaus;
// Initialize a Shonan instance without the Karcher mean
ShonanAveraging3::Parameters parameters;
parameters.setKarcherWeight(0);
// Load 3 pose example taken in Klaus by Shicong
static const ShonanAveraging3 shonan =
fromExampleName("Klaus3.g2o", parameters);
// Check nr poses
EXPECT_LONGS_EQUAL(3, shonan.nrUnknowns());
// Colmap uses the Y-down vision frame, and the first 3 rotations are close to
// identity. We check that below. Note tolerance is quite high.
static const Rot3 identity;
EXPECT(assert_equal(identity, wR0, 0.2));
EXPECT(assert_equal(identity, wR1, 0.2));
EXPECT(assert_equal(identity, wR2, 0.2));
// Get measurements
const Rot3 R01 = shonan.measured(0);
const Rot3 R12 = shonan.measured(1);
const Rot3 R02 = shonan.measured(2);
// Regression test to make sure data did not change.
EXPECT(assert_equal(Rot3(0.9995433591728293, -0.022048798853273946,
-0.01796327847857683, 0.010210006313668573),
R01));
// Check Colmap solution agrees OK with relative rotation measurements.
EXPECT(assert_equal(R01, wR0.between(wR1), 0.1));
EXPECT(assert_equal(R12, wR1.between(wR2), 0.1));
EXPECT(assert_equal(R02, wR0.between(wR2), 0.1));
// Run Shonan (with prior on first rotation)
auto initial = shonan.initializeRandomly(kRandomNumberGenerator);
auto result = shonan.run(initial, 5);
EXPECT_DOUBLES_EQUAL(0, shonan.cost(result.first), 1e-2);
EXPECT_DOUBLES_EQUAL(-9.2259161494467889e-05, result.second,
1e-4); // Regression
// Get Shonan solution in new frame R (R for result)
const Rot3 rR0 = result.first.at<Rot3>(0);
const Rot3 rR1 = result.first.at<Rot3>(1);
const Rot3 rR2 = result.first.at<Rot3>(2);
// rR0 = rRw * wR0 => rRw = rR0 * wR0.inverse()
// rR1 = rRw * wR1
// rR2 = rRw * wR2
const Rot3 rRw = rR0 * wR0.inverse();
EXPECT(assert_equal(rRw * wR1, rR1, 0.1))
EXPECT(assert_equal(rRw * wR2, rR2, 0.1))
}
/* ************************************************************************* */
TEST(ShonanAveraging3, runKlausKarcher) {
using namespace klaus;
// Load 3 pose example taken in Klaus by Shicong
static const ShonanAveraging3 shonan = fromExampleName("Klaus3.g2o");
// Run Shonan (with Karcher mean prior)
auto initial = shonan.initializeRandomly(kRandomNumberGenerator);
auto result = shonan.run(initial, 5);
EXPECT_DOUBLES_EQUAL(0, shonan.cost(result.first), 1e-2);
EXPECT_DOUBLES_EQUAL(-1.361402670507772e-05, result.second,
1e-4); // Regression test
// Get Shonan solution in new frame R (R for result)
const Rot3 rR0 = result.first.at<Rot3>(0);
const Rot3 rR1 = result.first.at<Rot3>(1);
const Rot3 rR2 = result.first.at<Rot3>(2);
const Rot3 rRw = rR0 * wR0.inverse();
EXPECT(assert_equal(rRw * wR1, rR1, 0.1))
EXPECT(assert_equal(rRw * wR2, rR2, 0.1))
}
/* ************************************************************************* */
TEST(ShonanAveraging2, runKlausKarcher) {
// Load 2D toy example
auto lmParams = LevenbergMarquardtParams::CeresDefaults();
// lmParams.setVerbosityLM("SUMMARY");
string g2oFile = findExampleDataFile("noisyToyGraph.txt");
ShonanAveraging2::Parameters parameters(lmParams);
auto measurements = parseMeasurements<Rot2>(g2oFile);
ShonanAveraging2 shonan(measurements, parameters);
EXPECT_LONGS_EQUAL(4, shonan.nrUnknowns());
// Check graph building
NonlinearFactorGraph graph = shonan.buildGraphAt(2);
EXPECT_LONGS_EQUAL(6, graph.size());
auto initial = shonan.initializeRandomly(kRandomNumberGenerator);
auto result = shonan.run(initial, 2);
EXPECT_DOUBLES_EQUAL(0.0008211, shonan.cost(result.first), 1e-6);
EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate!
}
/* ************************************************************************* */
// Test alpha/beta/gamma prior weighting.
TEST(ShonanAveraging3, PriorWeights) {
auto lmParams = LevenbergMarquardtParams::CeresDefaults();
ShonanAveraging3::Parameters params(lmParams);
EXPECT_DOUBLES_EQUAL(0, params.alpha, 1e-9);
EXPECT_DOUBLES_EQUAL(1, params.beta, 1e-9);
EXPECT_DOUBLES_EQUAL(0, params.gamma, 1e-9);
double alpha = 100.0, beta = 200.0, gamma = 300.0;
params.setAnchorWeight(alpha);
params.setKarcherWeight(beta);
params.setGaugesWeight(gamma);
EXPECT_DOUBLES_EQUAL(alpha, params.alpha, 1e-9);
EXPECT_DOUBLES_EQUAL(beta, params.beta, 1e-9);
EXPECT_DOUBLES_EQUAL(gamma, params.gamma, 1e-9);
params.setKarcherWeight(0);
static const ShonanAveraging3 shonan = fromExampleName("Klaus3.g2o", params);
for (auto i : {0,1,2}) {
const auto& m = shonan.measurement(i);
auto isotropic =
boost::static_pointer_cast<noiseModel::Isotropic>(m.noiseModel());
CHECK(isotropic != nullptr);
EXPECT_LONGS_EQUAL(3, isotropic->dim());
EXPECT_DOUBLES_EQUAL(0.2, isotropic->sigma(), 1e-9);
}
auto I = genericValue(Rot3());
Values initial{{0, I}, {1, I}, {2, I}};
EXPECT_DOUBLES_EQUAL(3.0756, shonan.cost(initial), 1e-4);
auto result = shonan.run(initial, 3, 3);
EXPECT_DOUBLES_EQUAL(0.0015, shonan.cost(result.first), 1e-4);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

121
gtsam/sfm/tests/testShonanFactor.cpp Normal file
View File

@ -0,0 +1,121 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* testFrobeniusFactor.cpp
*
* @file testFrobeniusFactor.cpp
* @date March 2019
* @author Frank Dellaert
* @brief Check evaluateError for various Frobenius norm
*/
#include <gtsam/base/lieProxies.h>
#include <gtsam/base/testLie.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h>
#include <gtsam/geometry/SO4.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/sfm/ShonanFactor.h>
#include <gtsam/slam/FrobeniusFactor.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
//******************************************************************************
namespace so3 {
SO3 id;
Vector3 v1 = (Vector(3) << 0.1, 0, 0).finished();
SO3 R1 = SO3::Expmap(v1);
Vector3 v2 = (Vector(3) << 0.01, 0.02, 0.03).finished();
SO3 R2 = SO3::Expmap(v2);
SO3 R12 = R1.between(R2);
} // namespace so3
//******************************************************************************
namespace submanifold {
SO4 id;
Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished();
SO3 R1 = SO3::Expmap(v1.tail<3>());
SO4 Q1 = SO4::Expmap(v1);
Vector6 v2 = (Vector(6) << 0, 0, 0, 0.01, 0.02, 0.03).finished();
SO3 R2 = SO3::Expmap(v2.tail<3>());
SO4 Q2 = SO4::Expmap(v2);
SO3 R12 = R1.between(R2);
} // namespace submanifold
/* ************************************************************************* */
TEST(ShonanFactor3, evaluateError) {
auto model = noiseModel::Isotropic::Sigma(3, 1.2);
for (const size_t p : {5, 4, 3}) {
Matrix M = Matrix::Identity(p, p);
M.topLeftCorner(3, 3) = submanifold::R1.matrix();
SOn Q1(M);
M.topLeftCorner(3, 3) = submanifold::R2.matrix();
SOn Q2(M);
auto factor = ShonanFactor3(1, 2, Rot3(::so3::R12.matrix()), p, model);
Matrix H1, H2;
factor.evaluateError(Q1, Q2, H1, H2);
// Test derivatives
Values values;
values.insert(1, Q1);
values.insert(2, Q2);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
}
/* ************************************************************************* */
TEST(ShonanFactor3, equivalenceToSO3) {
using namespace ::submanifold;
auto R12 = ::so3::R12.retract(Vector3(0.1, 0.2, -0.1));
auto model = noiseModel::Isotropic::Sigma(6, 1.2); // wrong dimension
auto factor3 = FrobeniusBetweenFactor<SO3>(1, 2, R12, model);
auto factor4 = ShonanFactor3(1, 2, Rot3(R12.matrix()), 4, model);
const Matrix3 E3(factor3.evaluateError(R1, R2).data());
const Matrix43 E4(
factor4.evaluateError(SOn(Q1.matrix()), SOn(Q2.matrix())).data());
EXPECT(assert_equal((Matrix)E4.topLeftCorner<3, 3>(), E3, 1e-9));
EXPECT(assert_equal((Matrix)E4.row(3), Matrix13::Zero(), 1e-9));
}
/* ************************************************************************* */
TEST(ShonanFactor2, evaluateError) {
auto model = noiseModel::Isotropic::Sigma(1, 1.2);
const Rot2 R1(0.3), R2(0.5), R12(0.2);
for (const size_t p : {5, 4, 3, 2}) {
Matrix M = Matrix::Identity(p, p);
M.topLeftCorner(2, 2) = R1.matrix();
SOn Q1(M);
M.topLeftCorner(2, 2) = R2.matrix();
SOn Q2(M);
auto factor = ShonanFactor2(1, 2, R12, p, model);
Matrix H1, H2;
factor.evaluateError(Q1, Q2, H1, H2);
// Test derivatives
Values values;
values.insert(1, Q1);
values.insert(2, Q2);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

106
gtsam/sfm/tests/testShonanGaugeFactor.cpp Normal file
View File

@ -0,0 +1,106 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testShonanGaugeFactor.cpp
* @date March 2019
* @author Frank Dellaert
* @brief Unit tests for ShonanGaugeFactor class
*/
#include <gtsam/sfm/ShonanGaugeFactor.h>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
#include <map>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
// Check dimensions of all low-dim GaugeFactors
TEST(ShonanAveraging, GaugeFactorLows) {
constexpr Key key(123);
EXPECT_LONGS_EQUAL(0, ShonanGaugeFactor(key, 2, 2).dim());
EXPECT_LONGS_EQUAL(0, ShonanGaugeFactor(key, 3, 2).dim());
EXPECT_LONGS_EQUAL(1, ShonanGaugeFactor(key, 4, 2).dim()); // SO(4-2) -> 1
EXPECT_LONGS_EQUAL(3, ShonanGaugeFactor(key, 5, 2).dim()); // SO(5-2) -> 3
EXPECT_LONGS_EQUAL(0, ShonanGaugeFactor(key, 3, 3).dim());
EXPECT_LONGS_EQUAL(0, ShonanGaugeFactor(key, 4, 3).dim());
EXPECT_LONGS_EQUAL(1, ShonanGaugeFactor(key, 5, 3).dim()); // SO(5-3) -> 1
}
/* ************************************************************************* */
// Check ShonanGaugeFactor for SO(6)
TEST(ShonanAveraging, GaugeFactorSO6) {
constexpr Key key(666);
ShonanGaugeFactor factor(key, 6); // For SO(6)
Matrix A = Matrix::Zero(3, 15); // SO(6-3) = SO(3) == 3-dimensional gauge
A(0, 0) = 1; // first 2 of 6^th skew column, which has 5 non-zero entries
A(1, 1) = 1; // then we skip 3 tangent dimensions
A(2, 5) = 1; // first of 5th skew colum, which has 4 non-zero entries above
// diagonal.
JacobianFactor linearized(key, A, Vector::Zero(3));
Values values;
EXPECT_LONGS_EQUAL(3, factor.dim());
EXPECT(assert_equal(linearized, *boost::dynamic_pointer_cast<JacobianFactor>(
factor.linearize(values))));
}
/* ************************************************************************* */
// Check ShonanGaugeFactor for SO(7)
TEST(ShonanAveraging, GaugeFactorSO7) {
constexpr Key key(777);
ShonanGaugeFactor factor(key, 7); // For SO(7)
Matrix A = Matrix::Zero(6, 21); // SO(7-3) = SO(4) == 6-dimensional gauge
A(0, 0) = 1; // first 3 of 7^th skew column, which has 6 non-zero entries
A(1, 1) = 1;
A(2, 2) = 1; // then we skip 3 tangent dimensions
A(3, 6) = 1; // first 2 of 6^th skew column, which has 5 non-zero entries
A(4, 7) = 1; // then we skip 3 tangent dimensions
A(5, 11) = 1; // first of 5th skew colum, which has 4 non-zero entries above
// diagonal.
JacobianFactor linearized(key, A, Vector::Zero(6));
Values values;
EXPECT_LONGS_EQUAL(6, factor.dim());
EXPECT(assert_equal(linearized, *boost::dynamic_pointer_cast<JacobianFactor>(
factor.linearize(values))));
}
/* ************************************************************************* */
// Check ShonanGaugeFactor for SO(6), with base SO(2)
TEST(ShonanAveraging, GaugeFactorSO6over2) {
constexpr Key key(602);
double gamma = 4;
ShonanGaugeFactor factor(key, 6, 2, gamma); // For SO(6), base SO(2)
Matrix A = Matrix::Zero(6, 15); // SO(6-2) = SO(4) == 6-dimensional gauge
A(0, 0) = 2; // first 3 of 6^th skew column, which has 5 non-zero entries
A(1, 1) = 2;
A(2, 2) = 2; // then we skip only 2 tangent dimensions
A(3, 5) = 2; // first 2 of 5^th skew column, which has 4 non-zero entries
A(4, 6) = 2; // then we skip only 2 tangent dimensions
A(5, 9) = 2; // first of 4th skew colum, which has 3 non-zero entries above
// diagonal.
JacobianFactor linearized(key, A, Vector::Zero(6));
Values values;
EXPECT_LONGS_EQUAL(6, factor.dim());
EXPECT(assert_equal(linearized, *boost::dynamic_pointer_cast<JacobianFactor>(
factor.linearize(values))));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

118
gtsam/slam/FrobeniusFactor.cpp
View File

@ -18,118 +18,38 @@
#include <gtsam/slam/FrobeniusFactor.h>
#include <gtsam/base/timing.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <cmath>
#include <iostream>
#include <vector>
using namespace std;
namespace gtsam {
//******************************************************************************
boost::shared_ptr<noiseModel::Isotropic> ConvertPose3NoiseModel(
const SharedNoiseModel& model, size_t d, bool defaultToUnit) {
boost::shared_ptr<noiseModel::Isotropic>
ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) {
double sigma = 1.0;
if (model != nullptr) {
if (model->dim() != 6) {
if (!defaultToUnit)
throw std::runtime_error("Can only convert Pose3 noise models");
} else {
auto sigmas = model->sigmas().head(3).eval();
if (sigmas(1) != sigmas(0) || sigmas(2) != sigmas(0)) {
if (!defaultToUnit)
auto sigmas = model->sigmas();
size_t n = sigmas.size();
if (n == 1) {
sigma = sigmas(0); // Rot2
goto exit;
}
if (n == 3 || n == 6) {
sigma = sigmas(2); // Pose2, Rot3, or Pose3
if (sigmas(0) != sigma || sigmas(1) != sigma) {
if (!defaultToUnit) {
throw std::runtime_error("Can only convert isotropic rotation noise");
} else {
sigma = sigmas(0);
}
}
goto exit;
}
if (!defaultToUnit) {
throw std::runtime_error("Can only convert Pose2/Pose3 noise models");
}
}
exit:
return noiseModel::Isotropic::Sigma(d, sigma);
}
//******************************************************************************
FrobeniusWormholeFactor::FrobeniusWormholeFactor(
Key j1, Key j2, const Rot3 &R12, size_t p, const SharedNoiseModel &model,
const boost::shared_ptr<Matrix> &G)
: NoiseModelFactor2<SOn, SOn>(ConvertPose3NoiseModel(model, p * 3), j1, j2),
M_(R12.matrix()), // 3*3 in all cases
p_(p), // 4 for SO(4)
pp_(p * p), // 16 for SO(4)
G_(G) {
if (noiseModel()->dim() != 3 * p_)
throw std::invalid_argument(
"FrobeniusWormholeFactor: model with incorrect dimension.");
if (!G) {
G_ = boost::make_shared<Matrix>();
*G_ = SOn::VectorizedGenerators(p); // expensive!
}
if (static_cast<size_t>(G_->rows()) != pp_ ||
static_cast<size_t>(G_->cols()) != SOn::Dimension(p))
throw std::invalid_argument("FrobeniusWormholeFactor: passed in generators "
"of incorrect dimension.");
}
//******************************************************************************
void FrobeniusWormholeFactor::print(const std::string &s, const KeyFormatter &keyFormatter) const {
std::cout << s << "FrobeniusWormholeFactor<" << p_ << ">(" << keyFormatter(key1()) << ","
<< keyFormatter(key2()) << ")\n";
traits<Matrix>::Print(M_, " M: ");
noiseModel_->print(" noise model: ");
}
//******************************************************************************
bool FrobeniusWormholeFactor::equals(const NonlinearFactor &expected,
double tol) const {
auto e = dynamic_cast<const FrobeniusWormholeFactor *>(&expected);
return e != nullptr && NoiseModelFactor2<SOn, SOn>::equals(*e, tol) &&
p_ == e->p_ && M_ == e->M_;
}
//******************************************************************************
Vector FrobeniusWormholeFactor::evaluateError(
const SOn& Q1, const SOn& Q2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
gttic(FrobeniusWormholeFactorP_evaluateError);
const Matrix& M1 = Q1.matrix();
const Matrix& M2 = Q2.matrix();
assert(M1.rows() == p_ && M2.rows() == p_);
const size_t dim = 3 * p_; // Stiefel manifold dimension
Vector fQ2(dim), hQ1(dim);
// Vectorize and extract only d leftmost columns, i.e. vec(M2*P)
fQ2 << Eigen::Map<const Matrix>(M2.data(), dim, 1);
// Vectorize M1*P*R12
const Matrix Q1PR12 = M1.leftCols<3>() * M_;
hQ1 << Eigen::Map<const Matrix>(Q1PR12.data(), dim, 1);
// If asked, calculate Jacobian as (M \otimes M1) * G
if (H1) {
const size_t p2 = 2 * p_;
Matrix RPxQ = Matrix::Zero(dim, pp_);
RPxQ.block(0, 0, p_, dim) << M1 * M_(0, 0), M1 * M_(1, 0), M1 * M_(2, 0);
RPxQ.block(p_, 0, p_, dim) << M1 * M_(0, 1), M1 * M_(1, 1), M1 * M_(2, 1);
RPxQ.block(p2, 0, p_, dim) << M1 * M_(0, 2), M1 * M_(1, 2), M1 * M_(2, 2);
*H1 = -RPxQ * (*G_);
}
if (H2) {
const size_t p2 = 2 * p_;
Matrix PxQ = Matrix::Zero(dim, pp_);
PxQ.block(0, 0, p_, p_) = M2;
PxQ.block(p_, p_, p_, p_) = M2;
PxQ.block(p2, p2, p_, p_) = M2;
*H2 = PxQ * (*G_);
}
return fQ2 - hQ1;
}
//******************************************************************************
} // namespace gtsam
} // namespace gtsam

74
gtsam/slam/FrobeniusFactor.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SOn.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
@ -25,23 +26,24 @@
namespace gtsam {
/**
* When creating (any) FrobeniusFactor we convert a 6-dimensional Pose3
* BetweenFactor noise model into an 9 or 16-dimensional isotropic noise
* When creating (any) FrobeniusFactor we can convert a Rot/Pose
* BetweenFactor noise model into a n-dimensional isotropic noise
* model used to weight the Frobenius norm. If the noise model passed is
* null we return a Dim-dimensional isotropic noise model with sigma=1.0. If
* not, we we check if the 3-dimensional noise model on rotations is
* isotropic. If it is, we extend to 'Dim' dimensions, otherwise we throw an
* null we return a n-dimensional isotropic noise model with sigma=1.0. If
* not, we we check if the d-dimensional noise model on rotations is
* isotropic. If it is, we extend to 'n' dimensions, otherwise we throw an
* error. If defaultToUnit == false throws an exception on unexepcted input.
*/
GTSAM_EXPORT boost::shared_ptr<noiseModel::Isotropic> ConvertPose3NoiseModel(
const SharedNoiseModel& model, size_t d, bool defaultToUnit = true);
GTSAM_EXPORT boost::shared_ptr<noiseModel::Isotropic>
ConvertNoiseModel(const SharedNoiseModel &model, size_t n,
bool defaultToUnit = true);
/**
* FrobeniusPrior calculates the Frobenius norm between a given matrix and an
* element of SO(3) or SO(4).
*/
template <class Rot>
class FrobeniusPrior : public NoiseModelFactor1<Rot> {
class GTSAM_EXPORT FrobeniusPrior : public NoiseModelFactor1<Rot> {
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
using MatrixNN = typename Rot::MatrixNN;
Eigen::Matrix<double, Dim, 1> vecM_; ///< vectorized matrix to approximate
@ -50,7 +52,7 @@ class FrobeniusPrior : public NoiseModelFactor1<Rot> {
/// Constructor
FrobeniusPrior(Key j, const MatrixNN& M,
const SharedNoiseModel& model = nullptr)
: NoiseModelFactor1<Rot>(ConvertPose3NoiseModel(model, Dim), j) {
: NoiseModelFactor1<Rot>(ConvertNoiseModel(model, Dim), j) {
vecM_ << Eigen::Map<const Matrix>(M.data(), Dim, 1);
}
@ -66,13 +68,13 @@ class FrobeniusPrior : public NoiseModelFactor1<Rot> {
* The template argument can be any fixed-size SO<N>.
*/
template <class Rot>
class FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
class GTSAM_EXPORT FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
public:
/// Constructor
FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
: NoiseModelFactor2<Rot, Rot>(ConvertPose3NoiseModel(model, Dim), j1,
: NoiseModelFactor2<Rot, Rot>(ConvertNoiseModel(model, Dim), j1,
j2) {}
/// Error is just Frobenius norm between rotation matrices.
@ -106,7 +108,7 @@ class GTSAM_EXPORT FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12,
const SharedNoiseModel& model = nullptr)
: NoiseModelFactor2<Rot, Rot>(
ConvertPose3NoiseModel(model, Dim), j1, j2),
ConvertNoiseModel(model, Dim), j1, j2),
R12_(R12),
R2hat_H_R1_(R12.inverse().AdjointMap()) {}
@ -150,52 +152,4 @@ class GTSAM_EXPORT FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
/// @}
};
/**
* FrobeniusWormholeFactor is a BetweenFactor that moves in SO(p), but will
* land on the SO(3) sub-manifold of SO(p) at the global minimum. It projects
* the SO(p) matrices down to a Stiefel manifold of p*d matrices.
* TODO(frank): template on D=2 or 3
*/
class GTSAM_EXPORT FrobeniusWormholeFactor
: public NoiseModelFactor2<SOn, SOn> {
Matrix M_; ///< measured rotation between R1 and R2
size_t p_, pp_; ///< dimensionality constants
boost::shared_ptr<Matrix> G_; ///< matrix of vectorized generators
public:
/// @name Constructor
/// @{
/// Constructor. Note we convert to 3*p-dimensional noise model.
/// To save memory and mallocs, pass in the vectorized Lie algebra generators:
/// G = boost::make_shared<Matrix>(SOn::VectorizedGenerators(p));
FrobeniusWormholeFactor(Key j1, Key j2, const Rot3 &R12, size_t p = 4,
const SharedNoiseModel &model = nullptr,
const boost::shared_ptr<Matrix> &G = nullptr);
/// @}
/// @name Testable
/// @{
/// print with optional string
void
print(const std::string &s,
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
/// assert equality up to a tolerance
bool equals(const NonlinearFactor &expected,
double tol = 1e-9) const override;
/// @}
/// @name NoiseModelFactor2 methods
/// @{
/// Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0]
/// projects down from SO(p) to the Stiefel manifold of px3 matrices.
Vector evaluateError(const SOn& Q1, const SOn& Q2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override;
/// @}
};
} // namespace gtsam

18
gtsam/slam/KarcherMeanFactor-inl.h
View File

@ -58,20 +58,22 @@ T FindKarcherMean(std::initializer_list<T>&& rotations) {
template <class T>
template <typename CONTAINER>
KarcherMeanFactor<T>::KarcherMeanFactor(const CONTAINER& keys, int d)
: NonlinearFactor(keys) {
KarcherMeanFactor<T>::KarcherMeanFactor(const CONTAINER &keys, int d,
boost::optional<double> beta)
: NonlinearFactor(keys), d_(static_cast<size_t>(d)) {
if (d <= 0) {
throw std::invalid_argument(
"KarcherMeanFactor needs dimension for dynamic types.");
}
// Create the constant Jacobian made of D*D identity matrices,
// where D is the dimensionality of the manifold.
const auto I = Eigen::MatrixXd::Identity(d, d);
// Create the constant Jacobian made of d*d identity matrices,
// where d is the dimensionality of the manifold.
Matrix A = Matrix::Identity(d, d);
if (beta) A *= std::sqrt(*beta);
std::map<Key, Matrix> terms;
for (Key j : keys) {
terms[j] = I;
terms[j] = A;
}
jacobian_ =
boost::make_shared<JacobianFactor>(terms, Eigen::VectorXd::Zero(d));
whitenedJacobian_ =
boost::make_shared<JacobianFactor>(terms, Vector::Zero(d));
}
} // namespace gtsam

43
gtsam/slam/KarcherMeanFactor.h
View File

@ -30,44 +30,51 @@ namespace gtsam {
* PriorFactors.
*/
template <class T>
T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>>& rotations);
T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>> &rotations);
template <class T>
T FindKarcherMean(std::initializer_list<T>&& rotations);
template <class T> T FindKarcherMean(std::initializer_list<T> &&rotations);
/**
* The KarcherMeanFactor creates a constraint on all SO(n) variables with
* given keys that the Karcher mean (see above) will stay the same. Note the
* mean itself is irrelevant to the constraint and is not a parameter: the
* constraint is implemented as enforcing that the sum of local updates is
* equal to zero, hence creating a rank-dim constraint. Note it is implemented as
* a soft constraint, as typically it is used to fix a gauge freedom.
* equal to zero, hence creating a rank-dim constraint. Note it is implemented
* as a soft constraint, as typically it is used to fix a gauge freedom.
* */
template <class T>
class KarcherMeanFactor : public NonlinearFactor {
template <class T> class KarcherMeanFactor : public NonlinearFactor {
// Compile time dimension: can be -1
enum { D = traits<T>::dimension };
// Runtime dimension: always >=0
size_t d_;
/// Constant Jacobian made of d*d identity matrices
boost::shared_ptr<JacobianFactor> jacobian_;
boost::shared_ptr<JacobianFactor> whitenedJacobian_;
enum {D = traits<T>::dimension};
public:
/// Construct from given keys.
public:
/**
* Construct from given keys.
* If parameter beta is given, it acts as a precision = 1/sigma^2, and
* the Jacobian will be multiplied with 1/sigma = sqrt(beta).
*/
template <typename CONTAINER>
KarcherMeanFactor(const CONTAINER& keys, int d=D);
KarcherMeanFactor(const CONTAINER &keys, int d = D,
boost::optional<double> beta = boost::none);
/// Destructor
virtual ~KarcherMeanFactor() {}
/// Calculate the error of the factor: always zero
double error(const Values& c) const override { return 0; }
double error(const Values &c) const override { return 0; }
/// get the dimension of the factor (number of rows on linearization)
size_t dim() const override { return D; }
size_t dim() const override { return d_; }
/// linearize to a GaussianFactor
boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override {
return jacobian_;
boost::shared_ptr<GaussianFactor> linearize(const Values &c) const override {
return whitenedJacobian_;
}
};
// \KarcherMeanFactor
} // namespace gtsam
} // namespace gtsam

48
gtsam/slam/tests/testFrobeniusFactor.cpp
View File

@ -188,54 +188,6 @@ TEST(FrobeniusBetweenFactorSO4, evaluateError) {
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
//******************************************************************************
namespace submanifold {
SO4 id;
Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished();
SO3 R1 = SO3::Expmap(v1.tail<3>());
SO4 Q1 = SO4::Expmap(v1);
Vector6 v2 = (Vector(6) << 0, 0, 0, 0.01, 0.02, 0.03).finished();
SO3 R2 = SO3::Expmap(v2.tail<3>());
SO4 Q2 = SO4::Expmap(v2);
SO3 R12 = R1.between(R2);
} // namespace submanifold
/* ************************************************************************* */
TEST(FrobeniusWormholeFactor, evaluateError) {
auto model = noiseModel::Isotropic::Sigma(6, 1.2); // dimension = 6 not 16
for (const size_t p : {5, 4, 3}) {
Matrix M = Matrix::Identity(p, p);
M.topLeftCorner(3, 3) = submanifold::R1.matrix();
SOn Q1(M);
M.topLeftCorner(3, 3) = submanifold::R2.matrix();
SOn Q2(M);
auto factor =
FrobeniusWormholeFactor(1, 2, Rot3(::so3::R12.matrix()), p, model);
Matrix H1, H2;
factor.evaluateError(Q1, Q2, H1, H2);
// Test derivatives
Values values;
values.insert(1, Q1);
values.insert(2, Q2);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
}
/* ************************************************************************* */
TEST(FrobeniusWormholeFactor, equivalenceToSO3) {
using namespace ::submanifold;
auto R12 = ::so3::R12.retract(Vector3(0.1, 0.2, -0.1));
auto model = noiseModel::Isotropic::Sigma(6, 1.2); // wrong dimension
auto factor3 = FrobeniusBetweenFactor<SO3>(1, 2, R12, model);
auto factor4 = FrobeniusWormholeFactor(1, 2, Rot3(R12.matrix()), 4, model);
const Matrix3 E3(factor3.evaluateError(R1, R2).data());
const Matrix43 E4(
factor4.evaluateError(SOn(Q1.matrix()), SOn(Q2.matrix())).data());
EXPECT(assert_equal((Matrix)E4.topLeftCorner<3, 3>(), E3, 1e-9));
EXPECT(assert_equal((Matrix)E4.row(3), Matrix13::Zero(), 1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;

6
gtsam_unstable/gtsam_unstable.h
View File

@ -12,10 +12,14 @@ class gtsam::Point2Vector;
class gtsam::Rot2;
class gtsam::Pose2;
class gtsam::Point3;
class gtsam::SO3;
class gtsam::SO4;
class gtsam::SOn;
class gtsam::Rot3;
class gtsam::Pose3;
virtual class gtsam::noiseModel::Base;
virtual class gtsam::noiseModel::Gaussian;
virtual class gtsam::noiseModel::Isotropic;
virtual class gtsam::imuBias::ConstantBias;
virtual class gtsam::NonlinearFactor;
virtual class gtsam::NoiseModelFactor;
@ -39,6 +43,7 @@ class gtsam::KeyVector;
class gtsam::LevenbergMarquardtParams;
class gtsam::ISAM2Params;
class gtsam::GaussianDensity;
class gtsam::LevenbergMarquardtOptimizer;
namespace gtsam {
@ -282,7 +287,6 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
void serializable() const; // enabling serialization functionality
};
#include <gtsam/slam/BetweenFactor.h>
template<T = {gtsam::PoseRTV}>
virtual class BetweenFactor : gtsam::NoiseModelFactor {

215
gtsam_unstable/timing/process_shonan_timing_results.py Normal file
View File

@ -0,0 +1,215 @@
"""
Process timing results from timeShonanAveraging
"""
import xlrd
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.ticker import FuncFormatter
import heapq
from collections import Counter
def make_combined_plot(name, p_values, times, costs, min_cost_range=10):
""" Make a plot that combines timing and SO(3) cost.
Arguments:
name: string of the plot title
p_values: list of p-values (int)
times: list of timings (seconds)
costs: list of costs (double)
Will calculate the range of the costs, default minimum range = 10.0
"""
min_cost = min(costs)
cost_range = max(max(costs)-min_cost,min_cost_range)
fig = plt.figure()
ax1 = fig.add_subplot(111)
ax1.plot(p_values, times, label="time")
ax1.set_ylabel('Time used to optimize \ seconds')
ax1.set_xlabel('p_value')
ax2 = ax1.twinx()
ax2.plot(p_values, costs, 'r', label="cost")
ax2.set_ylabel('Cost at SO(3) form')
ax2.set_xlabel('p_value')
ax2.set_xticks(p_values)
ax2.set_ylim(min_cost, min_cost + cost_range)
plt.title(name, fontsize=12)
ax1.legend(loc="upper left")
ax2.legend(loc="upper right")
plt.interactive(False)
plt.show()
def make_convergence_plot(name, p_values, times, costs, iter=10):
""" Make a bar that show the success rate for each p_value according to whether the SO(3) cost converges
Arguments:
name: string of the plot title
p_values: list of p-values (int)
times: list of timings (seconds)
costs: list of costs (double)
iter: int of iteration number for each p_value
"""
max_cost = np.mean(np.array(heapq.nlargest(iter, costs)))
# calculate mean costs for each p value
p_values = list(dict(Counter(p_values)).keys())
# make sure the iter number
iter = int(len(times)/len(p_values))
p_mean_cost = [np.mean(np.array(costs[i*iter:(i+1)*iter])) for i in range(len(p_values))]
p_max = p_values[p_mean_cost.index(max(p_mean_cost))]
# print(p_mean_cost)
# print(p_max)
#take mean and make the combined plot
mean_times = []
mean_costs = []
for p in p_values:
costs_tmp = costs[p_values.index(p)*iter:(p_values.index(p)+1)*iter]
mean_cost = sum(costs_tmp)/len(costs_tmp)
mean_costs.append(mean_cost)
times_tmp = times[p_values.index(p)*iter:(p_values.index(p)+1)*iter]
mean_time = sum(times_tmp)/len(times_tmp)
mean_times.append(mean_time)
make_combined_plot(name, p_values,mean_times, mean_costs)
# calculate the convergence rate for each p_value
p_success_rates = []
if p_mean_cost[0] >= 0.95*np.mean(np.array(costs)) and p_mean_cost[0] <= 1.05*np.mean(np.array(costs)):
p_success_rates = [ 1.0 for p in p_values]
else:
for p in p_values:
if p > p_max:
p_costs = costs[p_values.index(p)*iter:(p_values.index(p)+1)*iter]
# print(p_costs)
converged = [ int(p_cost < 0.3*max_cost) for p_cost in p_costs]
success_rate = sum(converged)/len(converged)
p_success_rates.append(success_rate)
else:
p_success_rates.append(0)
plt.bar(p_values, p_success_rates, align='center', alpha=0.5)
plt.xticks(p_values)
plt.yticks(np.arange(0, 1.2, 0.2), ['0%', '20%', '40%', '60%', '80%', '100%'])
plt.xlabel("p_value")
plt.ylabel("success rate")
plt.title(name, fontsize=12)
plt.show()
def make_eigen_and_bound_plot(name, p_values, times1, costPs, cost3s, times2, min_eigens, subounds):
""" Make a plot that combines time for optimizing, time for optimizing and compute min_eigen,
min_eigen, subound (subound = (f_R - f_SDP) / f_SDP).
Arguments:
name: string of the plot title
p_values: list of p-values (int)
times1: list of timings (seconds)
costPs: f_SDP
cost3s: f_R
times2: list of timings (seconds)
min_eigens: list of min_eigen (double)
subounds: list of subound (double)
"""
if dict(Counter(p_values))[5] != 1:
p_values = list(dict(Counter(p_values)).keys())
iter = int(len(times1)/len(p_values))
p_mean_times1 = [np.mean(np.array(times1[i*iter:(i+1)*iter])) for i in range(len(p_values))]
p_mean_times2 = [np.mean(np.array(times2[i*iter:(i+1)*iter])) for i in range(len(p_values))]
print("p_values \n", p_values)
print("p_mean_times_opti \n", p_mean_times1)
print("p_mean_times_eig \n", p_mean_times2)
p_mean_costPs = [np.mean(np.array(costPs[i*iter:(i+1)*iter])) for i in range(len(p_values))]
p_mean_cost3s = [np.mean(np.array(cost3s[i*iter:(i+1)*iter])) for i in range(len(p_values))]
p_mean_lambdas = [np.mean(np.array(min_eigens[i*iter:(i+1)*iter])) for i in range(len(p_values))]
print("p_mean_costPs \n", p_mean_costPs)
print("p_mean_cost3s \n", p_mean_cost3s)
print("p_mean_lambdas \n", p_mean_lambdas)
print("*******************************************************************************************************************")
else:
plt.figure(1)
plt.ylabel('Time used (seconds)')
plt.xlabel('p_value')
plt.plot(p_values, times1, 'r', label="time for optimizing")
plt.plot(p_values, times2, 'blue', label="time for optimizing and check")
plt.title(name, fontsize=12)
plt.legend(loc="best")
plt.interactive(False)
plt.show()
plt.figure(2)
plt.ylabel('Min eigen_value')
plt.xlabel('p_value')
plt.plot(p_values, min_eigens, 'r', label="min_eigen values")
plt.title(name, fontsize=12)
plt.legend(loc="best")
plt.interactive(False)
plt.show()
plt.figure(3)
plt.ylabel('sub_bounds')
plt.xlabel('p_value')
plt.plot(p_values, subounds, 'blue', label="sub_bounds")
plt.title(name, fontsize=12)
plt.legend(loc="best")
plt.show()
# Process arguments and call plot function
import argparse
import csv
import os
parser = argparse.ArgumentParser()
parser.add_argument("path")
args = parser.parse_args()
file_path = []
domain = os.path.abspath(args.path)
for info in os.listdir(args.path):
file_path.append(os.path.join(domain, info))
file_path.sort()
print(file_path)
# name of all the plots
names = {}
names[0] = 'tinyGrid3D vertex = 9, edge = 11'
names[1] = 'smallGrid3D vertex = 125, edge = 297'
names[2] = 'parking-garage vertex = 1661, edge = 6275'
names[3] = 'sphere2500 vertex = 2500, edge = 4949'
# names[4] = 'sphere_bignoise vertex = 2200, edge = 8647'
names[5] = 'torus3D vertex = 5000, edge = 9048'
names[6] = 'cubicle vertex = 5750, edge = 16869'
names[7] = 'rim vertex = 10195, edge = 29743'
# Parse CSV file
for key, name in names.items():
print(key, name)
# find according file to process
name_file = None
for path in file_path:
if name[0:3] in path:
name_file = path
if name_file == None:
print("The file %s is not in the path" % name)
continue
p_values, times1, costPs, cost3s, times2, min_eigens, subounds = [],[],[],[],[],[],[]
with open(name_file) as csvfile:
reader = csv.reader(csvfile, delimiter='\t')
for row in reader:
print(row)
p_values.append(int(row[0]))
times1.append(float(row[1]))
costPs.append(float(row[2]))
cost3s.append(float(row[3]))
if len(row) > 4:
times2.append(float(row[4]))
min_eigens.append(float(row[5]))
subounds.append(float(row[6]))
#plot
# make_combined_plot(name, p_values, times1, cost3s)
# make_convergence_plot(name, p_values, times1, cost3s)
make_eigen_and_bound_plot(name, p_values, times1, costPs, cost3s, times2, min_eigens, subounds)

182
gtsam_unstable/timing/timeShonanAveraging.cpp Normal file
View File

@ -0,0 +1,182 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testShonanAveraging.cpp
* @date September 2019
* @author Jing Wu
* @author Frank Dellaert
* @brief Timing script for Shonan Averaging algorithm
*/
#include "gtsam/base/Matrix.h"
#include "gtsam/base/Vector.h"
#include "gtsam/geometry/Point3.h"
#include "gtsam/geometry/Rot3.h"
#include <gtsam/base/timing.h>
#include <gtsam/sfm/ShonanAveraging.h>
#include <CppUnitLite/TestHarness.h>
#include <chrono>
#include <fstream>
#include <iostream>
#include <map>
using namespace std;
using namespace gtsam;
// string g2oFile = findExampleDataFile("toyExample.g2o");
// save a single line of timing info to an output stream
void saveData(size_t p, double time1, double costP, double cost3, double time2,
double min_eigenvalue, double suBound, std::ostream* os) {
*os << (int)p << "\t" << time1 << "\t" << costP << "\t" << cost3 << "\t"
<< time2 << "\t" << min_eigenvalue << "\t" << suBound << endl;
}
void checkR(const Matrix& R) {
Matrix R2 = R.transpose();
Matrix actual_R = R2 * R;
assert_equal(Rot3(),Rot3(actual_R));
}
void saveResult(string name, const Values& values) {
ofstream myfile;
myfile.open("shonan_result_of_" + name + ".dat");
size_t nrSO3 = values.count<SO3>();
myfile << "#Type SO3 Number " << nrSO3 << "\n";
for (int i = 0; i < nrSO3; ++i) {
Matrix R = values.at<SO3>(i).matrix();
// Check if the result of R.Transpose*R satisfy orthogonal constraint
checkR(R);
myfile << i;
for (int m = 0; m < 3; ++m) {
for (int n = 0; n < 3; ++n) {
myfile << " " << R(m, n);
}
}
myfile << "\n";
}
myfile.close();
cout << "Saved shonan_result.dat file" << endl;
}
void saveG2oResult(string name, const Values& values, std::map<Key, Pose3> poses) {
ofstream myfile;
myfile.open("shonan_result_of_" + name + ".g2o");
size_t nrSO3 = values.count<SO3>();
for (int i = 0; i < nrSO3; ++i) {
Matrix R = values.at<SO3>(i).matrix();
// Check if the result of R.Transpose*R satisfy orthogonal constraint
checkR(R);
myfile << "VERTEX_SE3:QUAT" << " ";
myfile << i << " ";
myfile << poses[i].x() << " " << poses[i].y() << " " << poses[i].z() << " ";
Vector quaternion = Rot3(R).quaternion();
myfile << quaternion(3) << " " << quaternion(2) << " " << quaternion(1) << " " << quaternion(0);
myfile << "\n";
}
myfile.close();
cout << "Saved shonan_result.g2o file" << endl;
}
void saveResultQuat(const Values& values) {
ofstream myfile;
myfile.open("shonan_result.dat");
size_t nrSOn = values.count<SOn>();
for (int i = 0; i < nrSOn; ++i) {
GTSAM_PRINT(values.at<SOn>(i));
Rot3 R = Rot3(values.at<SOn>(i).matrix());
float x = R.toQuaternion().x();
float y = R.toQuaternion().y();
float z = R.toQuaternion().z();
float w = R.toQuaternion().w();
myfile << "QuatSO3 " << i;
myfile << "QuatSO3 " << i << " " << w << " " << x << " " << y << " " << z << "\n";
myfile.close();
}
}
int main(int argc, char* argv[]) {
// primitive argument parsing:
if (argc > 3) {
throw runtime_error("Usage: timeShonanAveraging [g2oFile]");
}
string g2oFile;
try {
if (argc > 1)
g2oFile = argv[argc - 1];
else
g2oFile = string(
"/home/jingwu/git/SESync/data/sphere2500.g2o");
} catch (const exception& e) {
cerr << e.what() << '\n';
exit(1);
}
// Create a csv output file
size_t pos1 = g2oFile.find("data/");
size_t pos2 = g2oFile.find(".g2o");
string name = g2oFile.substr(pos1 + 5, pos2 - pos1 - 5);
cout << name << endl;
ofstream csvFile("shonan_timing_of_" + name + ".csv");
// Create Shonan averaging instance from the file.
// ShonanAveragingParameters parameters;
// double sigmaNoiseInRadians = 0 * M_PI / 180;
// parameters.setNoiseSigma(sigmaNoiseInRadians);
static const ShonanAveraging3 kShonan(g2oFile);
// increase p value and try optimize using Shonan Algorithm. use chrono for
// timing
size_t pMin = 3;
Values Qstar;
Vector minEigenVector;
double CostP = 0, Cost3 = 0, lambdaMin = 0, suBound = 0;
cout << "(int)p" << "\t" << "time1" << "\t" << "costP" << "\t" << "cost3" << "\t"
<< "time2" << "\t" << "MinEigenvalue" << "\t" << "SuBound" << endl;
const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
for (size_t p = pMin; p < 6; p++) {
// Randomly initialize at lowest level, initialize by line search after that
const Values initial =
(p > pMin) ? kShonan.initializeWithDescent(p, Qstar, minEigenVector,
lambdaMin)
: ShonanAveraging::LiftTo<Rot3>(pMin, randomRotations);
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
// optimizing
const Values result = kShonan.tryOptimizingAt(p, initial);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> timeUsed1 =
chrono::duration_cast<chrono::duration<double>>(t2 - t1);
lambdaMin = kShonan.computeMinEigenValue(result, &minEigenVector);
chrono::steady_clock::time_point t3 = chrono::steady_clock::now();
chrono::duration<double> timeUsed2 =
chrono::duration_cast<chrono::duration<double>>(t3 - t1);
Qstar = result;
CostP = kShonan.costAt(p, result);
const Values SO3Values = kShonan.roundSolution(result);
Cost3 = kShonan.cost(SO3Values);
suBound = (Cost3 - CostP) / CostP;
saveData(p, timeUsed1.count(), CostP, Cost3, timeUsed2.count(),
lambdaMin, suBound, &cout);
saveData(p, timeUsed1.count(), CostP, Cost3, timeUsed2.count(),
lambdaMin, suBound, &csvFile);
}
saveResult(name, kShonan.roundSolution(Qstar));
// saveG2oResult(name, kShonan.roundSolution(Qstar), kShonan.Poses());
return 0;
}

10
timing/timeFrobeniusFactor.cpp → timing/timeShonanFactor.cpp
View File

@ -10,8 +10,8 @@
* -------------------------------------------------------------------------- */
/**
* @file timeFrobeniusFactor.cpp
* @brief time FrobeniusFactor with BAL file
* @file timeShonanFactor.cpp
* @brief time ShonanFactor with BAL file
* @author Frank Dellaert
* @date 2019
*/
@ -27,7 +27,7 @@
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/FrobeniusFactor.h>
#include <gtsam/sfm/ShonanFactor.h>
#include <iostream>
#include <random>
@ -42,7 +42,7 @@ static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2);
int main(int argc, char* argv[]) {
// primitive argument parsing:
if (argc > 3) {
throw runtime_error("Usage: timeFrobeniusFactor [g2oFile]");
throw runtime_error("Usage: timeShonanFactor [g2oFile]");
}
string g2oFile;
@ -70,7 +70,7 @@ int main(int argc, char* argv[]) {
const auto &keys = m.keys();
const Rot3 &Rij = m.measured();
const auto &model = m.noiseModel();
graph.emplace_shared<FrobeniusWormholeFactor>(
graph.emplace_shared<ShonanFactor3>(
keys[0], keys[1], Rij, 4, model, G);
}