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
GitHub
parent
ae4968328d
commit
84afc94458
|
|
@ -21,3 +21,4 @@ cython/gtsam_wrapper.pxd
|
||||||
/CMakeSettings.json
|
/CMakeSettings.json
|
||||||
# for QtCreator:
|
# for QtCreator:
|
||||||
CMakeLists.txt.user*
|
CMakeLists.txt.user*
|
||||||
|
xcode/
|
||||||
|
|
|
||||||
|
|
@ -13,7 +13,7 @@ import unittest
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
from gtsam import (Rot3, SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4,
|
from gtsam import (Rot3, SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4,
|
||||||
FrobeniusWormholeFactor, SOn)
|
ShonanFactor3, SOn)
|
||||||
|
|
||||||
id = SO4()
|
id = SO4()
|
||||||
v1 = np.array([0, 0, 0, 0.1, 0, 0])
|
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."""
|
"""Test creation of a factor that calculates Shonan error."""
|
||||||
R1 = SO3.Expmap(v1[3:])
|
R1 = SO3.Expmap(v1[3:])
|
||||||
R2 = SO3.Expmap(v2[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)
|
I4 = SOn(4)
|
||||||
Q1 = I4.retract(v1)
|
Q1 = I4.retract(v1)
|
||||||
Q2 = I4.retract(v2)
|
Q2 = I4.retract(v2)
|
||||||
|
|
|
||||||
|
|
@ -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 0 -1.6618596980158338 -0.5736497760548741 -3.3319774096611026 -0.02676080288219576 -0.024497002638379624 -0.015064701622500615 0.9992281076190063
|
||||||
VERTEX_SE3:QUAT 1 -3.614793159814815 0.04774490041587656 -0.2837650367985949 0.00991721787943912 0.4854918961891193 -0.042343290945895576 0.8731588132957809
|
VERTEX_SE3:QUAT 1 -1.431820463019384 -0.549139761976065 -3.160677992237872 -0.049543805396343954 -0.03232420352077356 -0.004386230477751116 0.998239108728862
|
||||||
VERTEX_SE3:QUAT 2 -3.255096913553434 0.013296754286114112 -0.5339792269680574 -0.027851108010665374 0.585478168397957 -0.05088341463532465 0.8086102325762403
|
VERTEX_SE3:QUAT 2 -1.0394840214436651 -0.5268841046291037 -2.972143862665523 -0.07993768981394891 0.0825062894866454 -0.04088089479075661 0.9925378735259738
|
||||||
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 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.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 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.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
|
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
131
gtsam.h
|
|
@ -2855,7 +2855,7 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
|
||||||
};
|
};
|
||||||
|
|
||||||
#include <gtsam/slam/FrobeniusFactor.h>
|
#include <gtsam/slam/FrobeniusFactor.h>
|
||||||
gtsam::noiseModel::Isotropic* ConvertPose3NoiseModel(
|
gtsam::noiseModel::Isotropic* ConvertNoiseModel(
|
||||||
gtsam::noiseModel::Base* model, size_t d);
|
gtsam::noiseModel::Base* model, size_t d);
|
||||||
|
|
||||||
template<T = {gtsam::SO3, gtsam::SO4}>
|
template<T = {gtsam::SO3, gtsam::SO4}>
|
||||||
|
|
@ -2874,12 +2874,14 @@ virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {
|
||||||
Vector evaluateError(const T& R1, const T& R2);
|
Vector evaluateError(const T& R1, const T& R2);
|
||||||
};
|
};
|
||||||
|
|
||||||
virtual class FrobeniusWormholeFactor : gtsam::NoiseModelFactor {
|
#include <gtsam/sfm/ShonanFactor.h>
|
||||||
FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12,
|
|
||||||
|
virtual class ShonanFactor3 : gtsam::NoiseModelFactor {
|
||||||
|
ShonanFactor(size_t key1, size_t key2, const gtsam::Rot3 &R12,
|
||||||
size_t p);
|
size_t p);
|
||||||
FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12,
|
ShonanFactor(size_t key1, size_t key2, const gtsam::Rot3 &R12,
|
||||||
size_t p, gtsam::noiseModel::Base* model);
|
size_t p, gtsam::noiseModel::Base *model);
|
||||||
Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2);
|
Vector evaluateError(const gtsam::SOn &Q1, const gtsam::SOn &Q2);
|
||||||
};
|
};
|
||||||
|
|
||||||
#include <gtsam/sfm/BinaryMeasurement.h>
|
#include <gtsam/sfm/BinaryMeasurement.h>
|
||||||
|
|
@ -2895,6 +2897,123 @@ class BinaryMeasurement {
|
||||||
typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
|
typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
|
||||||
typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3;
|
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 ¶meters);
|
||||||
|
|
||||||
|
// 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 ¶meters);
|
||||||
|
|
||||||
|
// TODO(frank): deprecate once we land pybind wrapper
|
||||||
|
ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors);
|
||||||
|
ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors,
|
||||||
|
const gtsam::ShonanAveragingParameters3 ¶meters);
|
||||||
|
|
||||||
|
// 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
|
// Navigation
|
||||||
//*************************************************************************
|
//*************************************************************************
|
||||||
|
|
|
||||||
|
|
@ -39,6 +39,13 @@ Rot2 Rot2::atan2(double y, double x) {
|
||||||
return R.normalize();
|
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 {
|
void Rot2::print(const string& s) const {
|
||||||
cout << s << ": " << theta() << endl;
|
cout << s << ": " << theta() << endl;
|
||||||
|
|
|
||||||
|
|
@ -22,6 +22,8 @@
|
||||||
#include <gtsam/base/Lie.h>
|
#include <gtsam/base/Lie.h>
|
||||||
#include <boost/optional.hpp>
|
#include <boost/optional.hpp>
|
||||||
|
|
||||||
|
#include <random>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
@ -79,6 +81,14 @@ namespace gtsam {
|
||||||
/** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
|
/** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
|
||||||
static Rot2 atan2(double y, double x);
|
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
|
/// @name Testable
|
||||||
/// @{
|
/// @{
|
||||||
|
|
|
||||||
|
|
@ -60,8 +60,9 @@ typename SO<N>::TangentVector SO<N>::ChartAtOrigin::Local(const SO& R,
|
||||||
|
|
||||||
template <int N>
|
template <int N>
|
||||||
typename SO<N>::MatrixDD SO<N>::AdjointMap() const {
|
typename SO<N>::MatrixDD SO<N>::AdjointMap() const {
|
||||||
|
if (N==2) return I_1x1; // SO(2) case
|
||||||
throw std::runtime_error(
|
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>
|
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.");
|
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>
|
template <int N>
|
||||||
typename SO<N>::VectorN2 SO<N>::vec(
|
typename SO<N>::VectorN2 SO<N>::vec(
|
||||||
OptionalJacobian<internal::NSquaredSO(N), dimension> H) const {
|
OptionalJacobian<internal::NSquaredSO(N), dimension> H) const {
|
||||||
const size_t n = rows();
|
|
||||||
const size_t n2 = n * n;
|
|
||||||
|
|
||||||
// Vectorize
|
// Vectorize
|
||||||
VectorN2 X(n2);
|
VectorN2 X = Eigen::Map<const VectorN2>(matrix_.data());
|
||||||
X << Eigen::Map<const Matrix>(matrix_.data(), n2, 1);
|
|
||||||
|
|
||||||
// If requested, calculate H as (I \oplus Q) * P,
|
// If requested, calculate H as (I \oplus Q) * P,
|
||||||
// where Q is the N*N rotation matrix, and P is calculated below.
|
// where Q is the N*N rotation matrix, and P is calculated below.
|
||||||
if (H) {
|
if (H) {
|
||||||
// Calculate P matrix of vectorized generators
|
// Calculate P matrix of vectorized generators
|
||||||
// TODO(duy): Should we refactor this as the jacobian of Hat?
|
// TODO(duy): Should we refactor this as the jacobian of Hat?
|
||||||
Matrix P = VectorizedGenerators(n);
|
Matrix P = SO<N>::VectorizedGenerators();
|
||||||
const size_t d = dim();
|
for (size_t i = 0; i < N; i++) {
|
||||||
H->resize(n2, d);
|
H->block(i * N, 0, N, dimension) =
|
||||||
for (size_t i = 0; i < n; i++) {
|
matrix_ * P.block(i * N, 0, N, dimension);
|
||||||
H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return X;
|
return X;
|
||||||
|
|
|
||||||
|
|
@ -22,21 +22,18 @@
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
GTSAM_EXPORT
|
GTSAM_EXPORT void SOn::Hat(const Vector &xi, Eigen::Ref<Matrix> X) {
|
||||||
Matrix SOn::Hat(const Vector& xi) {
|
|
||||||
size_t n = AmbientDim(xi.size());
|
size_t n = AmbientDim(xi.size());
|
||||||
if (n < 2) throw std::invalid_argument("SO<N>::Hat: n<2 not supported");
|
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
|
else if (n == 2) {
|
||||||
X.setZero();
|
|
||||||
if (n == 2) {
|
|
||||||
// Handle SO(2) case as recursion bottom
|
// Handle SO(2) case as recursion bottom
|
||||||
assert(xi.size() == 1);
|
assert(xi.size() == 1);
|
||||||
X << 0, -xi(0), xi(0), 0;
|
X << 0, -xi(0), xi(0), 0;
|
||||||
} else {
|
} else {
|
||||||
// Recursively call SO(n-1) call for top-left block
|
// Recursively call SO(n-1) call for top-left block
|
||||||
const size_t dmin = (n - 1) * (n - 2) / 2;
|
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)
|
// determine sign of last element (signs alternate)
|
||||||
double sign = pow(-1.0, xi.size());
|
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);
|
X(j, n - 1) = -X(n - 1, j);
|
||||||
sign = -sign;
|
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;
|
return X;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -99,4 +103,27 @@ SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
|
||||||
return result;
|
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
|
} // namespace gtsam
|
||||||
|
|
|
||||||
|
|
@ -98,8 +98,8 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
|
||||||
template <typename Derived, int N_ = N, typename = IsDynamic<N_>>
|
template <typename Derived, int N_ = N, typename = IsDynamic<N_>>
|
||||||
static SO Lift(size_t n, const Eigen::MatrixBase<Derived> &R) {
|
static SO Lift(size_t n, const Eigen::MatrixBase<Derived> &R) {
|
||||||
Matrix Q = Matrix::Identity(n, n);
|
Matrix Q = Matrix::Identity(n, n);
|
||||||
size_t p = R.rows();
|
const int p = R.rows();
|
||||||
assert(p < n && R.cols() == p);
|
assert(p >= 0 && p <= static_cast<int>(n) && R.cols() == p);
|
||||||
Q.topLeftCorner(p, p) = R;
|
Q.topLeftCorner(p, p) = R;
|
||||||
return SO(Q);
|
return SO(Q);
|
||||||
}
|
}
|
||||||
|
|
@ -208,7 +208,7 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
|
||||||
|
|
||||||
// Calculate run-time dimensionality of manifold.
|
// Calculate run-time dimensionality of manifold.
|
||||||
// Available as dimension or Dim() for fixed N.
|
// 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
|
* 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);
|
static MatrixNN Hat(const TangentVector& xi);
|
||||||
|
|
||||||
/**
|
/// In-place version of Hat (see details there), implements recursion.
|
||||||
* Inverse of Hat. See note about xi element order in Hat.
|
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);
|
static TangentVector Vee(const MatrixNN& X);
|
||||||
|
|
||||||
// Chart at origin
|
// Chart at origin
|
||||||
|
|
@ -295,10 +296,10 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
|
||||||
template <int N_ = N, typename = IsFixed<N_>>
|
template <int N_ = N, typename = IsFixed<N_>>
|
||||||
static Matrix VectorizedGenerators() {
|
static Matrix VectorizedGenerators() {
|
||||||
constexpr size_t N2 = static_cast<size_t>(N * N);
|
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++) {
|
for (size_t j = 0; j < dimension; j++) {
|
||||||
const auto X = Hat(Vector::Unit(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;
|
return G;
|
||||||
}
|
}
|
||||||
|
|
@ -362,6 +363,11 @@ template <>
|
||||||
SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
|
SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
|
||||||
DynamicJacobian H2) const;
|
DynamicJacobian H2) const;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Specialize dynamic vec.
|
||||||
|
*/
|
||||||
|
template <> typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const;
|
||||||
|
|
||||||
/** Serialization function */
|
/** Serialization function */
|
||||||
template<class Archive>
|
template<class Archive>
|
||||||
void serialize(
|
void serialize(
|
||||||
|
|
|
||||||
|
|
@ -39,8 +39,8 @@ using namespace std;
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
// Test dhynamic with n=0
|
// Test dynamic with n=0
|
||||||
TEST(SOn, SO0) {
|
TEST(SOn, SOn0) {
|
||||||
const auto R = SOn(0);
|
const auto R = SOn(0);
|
||||||
EXPECT_LONGS_EQUAL(0, R.rows());
|
EXPECT_LONGS_EQUAL(0, R.rows());
|
||||||
EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension);
|
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);
|
const auto R = SOn(5);
|
||||||
EXPECT_LONGS_EQUAL(5, R.rows());
|
EXPECT_LONGS_EQUAL(5, R.rows());
|
||||||
EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension);
|
EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension);
|
||||||
|
|
@ -59,6 +60,28 @@ TEST(SOn, SO5) {
|
||||||
EXPECT_LONGS_EQUAL(10, traits<SOn>::GetDimension(R));
|
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) {
|
TEST(SOn, Concept) {
|
||||||
BOOST_CONCEPT_ASSERT((IsGroup<SOn>));
|
BOOST_CONCEPT_ASSERT((IsGroup<SOn>));
|
||||||
|
|
@ -105,29 +128,29 @@ TEST(SOn, HatVee) {
|
||||||
EXPECT(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2)));
|
EXPECT(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2)));
|
||||||
|
|
||||||
Matrix expected3(3, 3);
|
Matrix expected3(3, 3);
|
||||||
expected3 << 0, -3, 2, //
|
expected3 << 0, -3, 2, //
|
||||||
3, 0, -1, //
|
3, 0, -1, //
|
||||||
-2, 1, 0;
|
-2, 1, 0;
|
||||||
const auto actual3 = SOn::Hat(v.head<3>());
|
const auto actual3 = SOn::Hat(v.head<3>());
|
||||||
EXPECT(assert_equal(expected3, actual3));
|
EXPECT(assert_equal(expected3, actual3));
|
||||||
EXPECT(assert_equal(skewSymmetric(1, 2, 3), actual3));
|
EXPECT(assert_equal(skewSymmetric(1, 2, 3), actual3));
|
||||||
EXPECT(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3)));
|
EXPECT(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3)));
|
||||||
|
|
||||||
Matrix expected4(4, 4);
|
Matrix expected4(4, 4);
|
||||||
expected4 << 0, -6, 5, 3, //
|
expected4 << 0, -6, 5, 3, //
|
||||||
6, 0, -4, -2, //
|
6, 0, -4, -2, //
|
||||||
-5, 4, 0, 1, //
|
-5, 4, 0, 1, //
|
||||||
-3, 2, -1, 0;
|
-3, 2, -1, 0;
|
||||||
const auto actual4 = SOn::Hat(v.head<6>());
|
const auto actual4 = SOn::Hat(v.head<6>());
|
||||||
EXPECT(assert_equal(expected4, actual4));
|
EXPECT(assert_equal(expected4, actual4));
|
||||||
EXPECT(assert_equal((Vector)v.head<6>(), SOn::Vee(actual4)));
|
EXPECT(assert_equal((Vector)v.head<6>(), SOn::Vee(actual4)));
|
||||||
|
|
||||||
Matrix expected5(5, 5);
|
Matrix expected5(5, 5);
|
||||||
expected5 << 0,-10, 9, 7, -4, //
|
expected5 << 0, -10, 9, 7, -4, //
|
||||||
10, 0, -8, -6, 3, //
|
10, 0, -8, -6, 3, //
|
||||||
-9, 8, 0, 5, -2, //
|
-9, 8, 0, 5, -2, //
|
||||||
-7, 6, -5, 0, 1, //
|
-7, 6, -5, 0, 1, //
|
||||||
4, -3, 2, -1, 0;
|
4, -3, 2, -1, 0;
|
||||||
const auto actual5 = SOn::Hat(v);
|
const auto actual5 = SOn::Hat(v);
|
||||||
EXPECT(assert_equal(expected5, actual5));
|
EXPECT(assert_equal(expected5, actual5));
|
||||||
EXPECT(assert_equal((Vector)v, SOn::Vee(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));
|
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) {
|
TEST(SOn, vec) {
|
||||||
Vector10 v;
|
Vector10 v;
|
||||||
|
|
@ -166,11 +205,28 @@ TEST(SOn, vec) {
|
||||||
SOn Q = SOn::ChartAtOrigin::Retract(v);
|
SOn Q = SOn::ChartAtOrigin::Retract(v);
|
||||||
Matrix actualH;
|
Matrix actualH;
|
||||||
const Vector actual = Q.vec(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);
|
const Matrix H = numericalDerivative11<Vector, SOn, 10>(h, Q, 1e-5);
|
||||||
CHECK(assert_equal(H, actualH));
|
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() {
|
int main() {
|
||||||
TestResult tr;
|
TestResult tr;
|
||||||
|
|
|
||||||
|
|
@ -90,7 +90,7 @@ void NonlinearOptimizer::defaultOptimize() {
|
||||||
// Iterative loop
|
// Iterative loop
|
||||||
do {
|
do {
|
||||||
// Do next iteration
|
// Do next iteration
|
||||||
currentError = error();
|
currentError = error(); // TODO(frank): don't do this twice at first !? Computed above!
|
||||||
iterate();
|
iterate();
|
||||||
tictoc_finishedIteration();
|
tictoc_finishedIteration();
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -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 ¶meters)
|
||||||
|
: 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 ¶meters)
|
||||||
|
: ShonanAveraging<2>(measurements, parameters) {}
|
||||||
|
ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters ¶meters)
|
||||||
|
: ShonanAveraging<2>(parseMeasurements<Rot2>(g2oFile), parameters) {}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
// Explicit instantiation for d=3
|
||||||
|
template class ShonanAveraging<3>;
|
||||||
|
|
||||||
|
ShonanAveraging3::ShonanAveraging3(const Measurements &measurements,
|
||||||
|
const Parameters ¶meters)
|
||||||
|
: ShonanAveraging<3>(measurements, parameters) {}
|
||||||
|
|
||||||
|
ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters ¶meters)
|
||||||
|
: 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 ¶meters)
|
||||||
|
: ShonanAveraging<3>(extractRot3Measurements(factors), parameters) {}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
} // namespace gtsam
|
||||||
|
|
@ -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 ¶meters = 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 ¶meters = Parameters());
|
||||||
|
ShonanAveraging2(string g2oFile, const Parameters ¶meters = Parameters());
|
||||||
|
};
|
||||||
|
|
||||||
|
class ShonanAveraging3 : public ShonanAveraging<3> {
|
||||||
|
public:
|
||||||
|
ShonanAveraging3(const Measurements &measurements,
|
||||||
|
const Parameters ¶meters = Parameters());
|
||||||
|
ShonanAveraging3(string g2oFile, const Parameters ¶meters = Parameters());
|
||||||
|
|
||||||
|
// TODO(frank): Deprecate after we land pybind wrapper
|
||||||
|
ShonanAveraging3(const BetweenFactorPose3s &factors,
|
||||||
|
const Parameters ¶meters = Parameters());
|
||||||
|
};
|
||||||
|
} // namespace gtsam
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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);
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
|
|
@ -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);
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
|
|
@ -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);
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
|
|
@ -18,118 +18,38 @@
|
||||||
|
|
||||||
#include <gtsam/slam/FrobeniusFactor.h>
|
#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;
|
using namespace std;
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
boost::shared_ptr<noiseModel::Isotropic> ConvertPose3NoiseModel(
|
boost::shared_ptr<noiseModel::Isotropic>
|
||||||
const SharedNoiseModel& model, size_t d, bool defaultToUnit) {
|
ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) {
|
||||||
double sigma = 1.0;
|
double sigma = 1.0;
|
||||||
if (model != nullptr) {
|
if (model != nullptr) {
|
||||||
if (model->dim() != 6) {
|
auto sigmas = model->sigmas();
|
||||||
if (!defaultToUnit)
|
size_t n = sigmas.size();
|
||||||
throw std::runtime_error("Can only convert Pose3 noise models");
|
if (n == 1) {
|
||||||
} else {
|
sigma = sigmas(0); // Rot2
|
||||||
auto sigmas = model->sigmas().head(3).eval();
|
goto exit;
|
||||||
if (sigmas(1) != sigmas(0) || sigmas(2) != sigmas(0)) {
|
}
|
||||||
if (!defaultToUnit)
|
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");
|
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);
|
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.");
|
|
||||||
}
|
|
||||||
|
|
||||||
//******************************************************************************
|
} // namespace gtsam
|
||||||
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
|
|
||||||
|
|
|
||||||
|
|
@ -18,6 +18,7 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include <gtsam/geometry/Rot2.h>
|
||||||
#include <gtsam/geometry/Rot3.h>
|
#include <gtsam/geometry/Rot3.h>
|
||||||
#include <gtsam/geometry/SOn.h>
|
#include <gtsam/geometry/SOn.h>
|
||||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||||
|
|
@ -25,23 +26,24 @@
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* When creating (any) FrobeniusFactor we convert a 6-dimensional Pose3
|
* When creating (any) FrobeniusFactor we can convert a Rot/Pose
|
||||||
* BetweenFactor noise model into an 9 or 16-dimensional isotropic noise
|
* BetweenFactor noise model into a n-dimensional isotropic noise
|
||||||
* model used to weight the Frobenius norm. If the noise model passed is
|
* 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
|
* null we return a n-dimensional isotropic noise model with sigma=1.0. If
|
||||||
* not, we we check if the 3-dimensional noise model on rotations is
|
* not, we we check if the d-dimensional noise model on rotations is
|
||||||
* isotropic. If it is, we extend to 'Dim' dimensions, otherwise we throw an
|
* isotropic. If it is, we extend to 'n' dimensions, otherwise we throw an
|
||||||
* error. If defaultToUnit == false throws an exception on unexepcted input.
|
* error. If defaultToUnit == false throws an exception on unexepcted input.
|
||||||
*/
|
*/
|
||||||
GTSAM_EXPORT boost::shared_ptr<noiseModel::Isotropic> ConvertPose3NoiseModel(
|
GTSAM_EXPORT boost::shared_ptr<noiseModel::Isotropic>
|
||||||
const SharedNoiseModel& model, size_t d, bool defaultToUnit = true);
|
ConvertNoiseModel(const SharedNoiseModel &model, size_t n,
|
||||||
|
bool defaultToUnit = true);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* FrobeniusPrior calculates the Frobenius norm between a given matrix and an
|
* FrobeniusPrior calculates the Frobenius norm between a given matrix and an
|
||||||
* element of SO(3) or SO(4).
|
* element of SO(3) or SO(4).
|
||||||
*/
|
*/
|
||||||
template <class Rot>
|
template <class Rot>
|
||||||
class FrobeniusPrior : public NoiseModelFactor1<Rot> {
|
class GTSAM_EXPORT FrobeniusPrior : public NoiseModelFactor1<Rot> {
|
||||||
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
|
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
|
||||||
using MatrixNN = typename Rot::MatrixNN;
|
using MatrixNN = typename Rot::MatrixNN;
|
||||||
Eigen::Matrix<double, Dim, 1> vecM_; ///< vectorized matrix to approximate
|
Eigen::Matrix<double, Dim, 1> vecM_; ///< vectorized matrix to approximate
|
||||||
|
|
@ -50,7 +52,7 @@ class FrobeniusPrior : public NoiseModelFactor1<Rot> {
|
||||||
/// Constructor
|
/// Constructor
|
||||||
FrobeniusPrior(Key j, const MatrixNN& M,
|
FrobeniusPrior(Key j, const MatrixNN& M,
|
||||||
const SharedNoiseModel& model = nullptr)
|
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);
|
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>.
|
* The template argument can be any fixed-size SO<N>.
|
||||||
*/
|
*/
|
||||||
template <class Rot>
|
template <class Rot>
|
||||||
class FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
|
class GTSAM_EXPORT FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
|
||||||
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
|
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
|
||||||
|
|
||||||
public:
|
public:
|
||||||
/// Constructor
|
/// Constructor
|
||||||
FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
|
FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
|
||||||
: NoiseModelFactor2<Rot, Rot>(ConvertPose3NoiseModel(model, Dim), j1,
|
: NoiseModelFactor2<Rot, Rot>(ConvertNoiseModel(model, Dim), j1,
|
||||||
j2) {}
|
j2) {}
|
||||||
|
|
||||||
/// Error is just Frobenius norm between rotation matrices.
|
/// 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,
|
FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12,
|
||||||
const SharedNoiseModel& model = nullptr)
|
const SharedNoiseModel& model = nullptr)
|
||||||
: NoiseModelFactor2<Rot, Rot>(
|
: NoiseModelFactor2<Rot, Rot>(
|
||||||
ConvertPose3NoiseModel(model, Dim), j1, j2),
|
ConvertNoiseModel(model, Dim), j1, j2),
|
||||||
R12_(R12),
|
R12_(R12),
|
||||||
R2hat_H_R1_(R12.inverse().AdjointMap()) {}
|
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
|
} // namespace gtsam
|
||||||
|
|
|
||||||
|
|
@ -58,20 +58,22 @@ T FindKarcherMean(std::initializer_list<T>&& rotations) {
|
||||||
|
|
||||||
template <class T>
|
template <class T>
|
||||||
template <typename CONTAINER>
|
template <typename CONTAINER>
|
||||||
KarcherMeanFactor<T>::KarcherMeanFactor(const CONTAINER& keys, int d)
|
KarcherMeanFactor<T>::KarcherMeanFactor(const CONTAINER &keys, int d,
|
||||||
: NonlinearFactor(keys) {
|
boost::optional<double> beta)
|
||||||
|
: NonlinearFactor(keys), d_(static_cast<size_t>(d)) {
|
||||||
if (d <= 0) {
|
if (d <= 0) {
|
||||||
throw std::invalid_argument(
|
throw std::invalid_argument(
|
||||||
"KarcherMeanFactor needs dimension for dynamic types.");
|
"KarcherMeanFactor needs dimension for dynamic types.");
|
||||||
}
|
}
|
||||||
// Create the constant Jacobian made of D*D identity matrices,
|
// Create the constant Jacobian made of d*d identity matrices,
|
||||||
// where D is the dimensionality of the manifold.
|
// where d is the dimensionality of the manifold.
|
||||||
const auto I = Eigen::MatrixXd::Identity(d, d);
|
Matrix A = Matrix::Identity(d, d);
|
||||||
|
if (beta) A *= std::sqrt(*beta);
|
||||||
std::map<Key, Matrix> terms;
|
std::map<Key, Matrix> terms;
|
||||||
for (Key j : keys) {
|
for (Key j : keys) {
|
||||||
terms[j] = I;
|
terms[j] = A;
|
||||||
}
|
}
|
||||||
jacobian_ =
|
whitenedJacobian_ =
|
||||||
boost::make_shared<JacobianFactor>(terms, Eigen::VectorXd::Zero(d));
|
boost::make_shared<JacobianFactor>(terms, Vector::Zero(d));
|
||||||
}
|
}
|
||||||
} // namespace gtsam
|
} // namespace gtsam
|
||||||
|
|
@ -30,44 +30,51 @@ namespace gtsam {
|
||||||
* PriorFactors.
|
* PriorFactors.
|
||||||
*/
|
*/
|
||||||
template <class T>
|
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>
|
template <class T> T FindKarcherMean(std::initializer_list<T> &&rotations);
|
||||||
T FindKarcherMean(std::initializer_list<T>&& rotations);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* The KarcherMeanFactor creates a constraint on all SO(n) variables with
|
* 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
|
* 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
|
* 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
|
* 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
|
* equal to zero, hence creating a rank-dim constraint. Note it is implemented
|
||||||
* a soft constraint, as typically it is used to fix a gauge freedom.
|
* as a soft constraint, as typically it is used to fix a gauge freedom.
|
||||||
* */
|
* */
|
||||||
template <class T>
|
template <class T> class KarcherMeanFactor : public NonlinearFactor {
|
||||||
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
|
/// Constant Jacobian made of d*d identity matrices
|
||||||
boost::shared_ptr<JacobianFactor> jacobian_;
|
boost::shared_ptr<JacobianFactor> whitenedJacobian_;
|
||||||
|
|
||||||
enum {D = traits<T>::dimension};
|
public:
|
||||||
|
/**
|
||||||
public:
|
* Construct from given keys.
|
||||||
/// 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>
|
template <typename CONTAINER>
|
||||||
KarcherMeanFactor(const CONTAINER& keys, int d=D);
|
KarcherMeanFactor(const CONTAINER &keys, int d = D,
|
||||||
|
boost::optional<double> beta = boost::none);
|
||||||
|
|
||||||
/// Destructor
|
/// Destructor
|
||||||
virtual ~KarcherMeanFactor() {}
|
virtual ~KarcherMeanFactor() {}
|
||||||
|
|
||||||
/// Calculate the error of the factor: always zero
|
/// 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)
|
/// 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
|
/// linearize to a GaussianFactor
|
||||||
boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override {
|
boost::shared_ptr<GaussianFactor> linearize(const Values &c) const override {
|
||||||
return jacobian_;
|
return whitenedJacobian_;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
// \KarcherMeanFactor
|
// \KarcherMeanFactor
|
||||||
} // namespace gtsam
|
} // namespace gtsam
|
||||||
|
|
|
||||||
|
|
@ -188,54 +188,6 @@ TEST(FrobeniusBetweenFactorSO4, evaluateError) {
|
||||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
|
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() {
|
int main() {
|
||||||
TestResult tr;
|
TestResult tr;
|
||||||
|
|
|
||||||
|
|
@ -12,10 +12,14 @@ class gtsam::Point2Vector;
|
||||||
class gtsam::Rot2;
|
class gtsam::Rot2;
|
||||||
class gtsam::Pose2;
|
class gtsam::Pose2;
|
||||||
class gtsam::Point3;
|
class gtsam::Point3;
|
||||||
|
class gtsam::SO3;
|
||||||
|
class gtsam::SO4;
|
||||||
|
class gtsam::SOn;
|
||||||
class gtsam::Rot3;
|
class gtsam::Rot3;
|
||||||
class gtsam::Pose3;
|
class gtsam::Pose3;
|
||||||
virtual class gtsam::noiseModel::Base;
|
virtual class gtsam::noiseModel::Base;
|
||||||
virtual class gtsam::noiseModel::Gaussian;
|
virtual class gtsam::noiseModel::Gaussian;
|
||||||
|
virtual class gtsam::noiseModel::Isotropic;
|
||||||
virtual class gtsam::imuBias::ConstantBias;
|
virtual class gtsam::imuBias::ConstantBias;
|
||||||
virtual class gtsam::NonlinearFactor;
|
virtual class gtsam::NonlinearFactor;
|
||||||
virtual class gtsam::NoiseModelFactor;
|
virtual class gtsam::NoiseModelFactor;
|
||||||
|
|
@ -39,6 +43,7 @@ class gtsam::KeyVector;
|
||||||
class gtsam::LevenbergMarquardtParams;
|
class gtsam::LevenbergMarquardtParams;
|
||||||
class gtsam::ISAM2Params;
|
class gtsam::ISAM2Params;
|
||||||
class gtsam::GaussianDensity;
|
class gtsam::GaussianDensity;
|
||||||
|
class gtsam::LevenbergMarquardtOptimizer;
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
|
|
@ -282,7 +287,6 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
|
||||||
void serializable() const; // enabling serialization functionality
|
void serializable() const; // enabling serialization functionality
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
#include <gtsam/slam/BetweenFactor.h>
|
#include <gtsam/slam/BetweenFactor.h>
|
||||||
template<T = {gtsam::PoseRTV}>
|
template<T = {gtsam::PoseRTV}>
|
||||||
virtual class BetweenFactor : gtsam::NoiseModelFactor {
|
virtual class BetweenFactor : gtsam::NoiseModelFactor {
|
||||||
|
|
|
||||||
|
|
@ -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)
|
||||||
|
|
@ -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,8 +10,8 @@
|
||||||
* -------------------------------------------------------------------------- */
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file timeFrobeniusFactor.cpp
|
* @file timeShonanFactor.cpp
|
||||||
* @brief time FrobeniusFactor with BAL file
|
* @brief time ShonanFactor with BAL file
|
||||||
* @author Frank Dellaert
|
* @author Frank Dellaert
|
||||||
* @date 2019
|
* @date 2019
|
||||||
*/
|
*/
|
||||||
|
|
@ -27,7 +27,7 @@
|
||||||
#include <gtsam/nonlinear/Values.h>
|
#include <gtsam/nonlinear/Values.h>
|
||||||
#include <gtsam/slam/PriorFactor.h>
|
#include <gtsam/slam/PriorFactor.h>
|
||||||
#include <gtsam/slam/dataset.h>
|
#include <gtsam/slam/dataset.h>
|
||||||
#include <gtsam/slam/FrobeniusFactor.h>
|
#include <gtsam/sfm/ShonanFactor.h>
|
||||||
|
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <random>
|
#include <random>
|
||||||
|
|
@ -42,7 +42,7 @@ static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2);
|
||||||
int main(int argc, char* argv[]) {
|
int main(int argc, char* argv[]) {
|
||||||
// primitive argument parsing:
|
// primitive argument parsing:
|
||||||
if (argc > 3) {
|
if (argc > 3) {
|
||||||
throw runtime_error("Usage: timeFrobeniusFactor [g2oFile]");
|
throw runtime_error("Usage: timeShonanFactor [g2oFile]");
|
||||||
}
|
}
|
||||||
|
|
||||||
string g2oFile;
|
string g2oFile;
|
||||||
|
|
@ -70,7 +70,7 @@ int main(int argc, char* argv[]) {
|
||||||
const auto &keys = m.keys();
|
const auto &keys = m.keys();
|
||||||
const Rot3 &Rij = m.measured();
|
const Rot3 &Rij = m.measured();
|
||||||
const auto &model = m.noiseModel();
|
const auto &model = m.noiseModel();
|
||||||
graph.emplace_shared<FrobeniusWormholeFactor>(
|
graph.emplace_shared<ShonanFactor3>(
|
||||||
keys[0], keys[1], Rij, 4, model, G);
|
keys[0], keys[1], Rij, 4, model, G);
|
||||||
}
|
}
|
||||||
|
|
||||||
Loading…
Reference in New Issue