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

Linting and getAnchor wrap

release/4.3a0
Frank dellaert 2020-08-24 14:40:29 -04:00
parent 02cd45d4b8
commit 4dd9ee5f1f
3 changed files with 108 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/gtsam.i
View File

@ -2925,6 +2925,7 @@ class ShonanAveragingParameters2 {
void setOptimalityThreshold(double value); void setOptimalityThreshold(double value);
double getOptimalityThreshold() const; double getOptimalityThreshold() const;
void setAnchor(size_t index, const gtsam::Rot2& value); void setAnchor(size_t index, const gtsam::Rot2& value);
pair<size_t, Rot2> getAnchor();
void setAnchorWeight(double value); void setAnchorWeight(double value);
double getAnchorWeight() const; double getAnchorWeight() const;
void setKarcherWeight(double value); void setKarcherWeight(double value);
@ -2940,6 +2941,7 @@ class ShonanAveragingParameters3 {
void setOptimalityThreshold(double value); void setOptimalityThreshold(double value);
double getOptimalityThreshold() const; double getOptimalityThreshold() const;
void setAnchor(size_t index, const gtsam::Rot3& value); void setAnchor(size_t index, const gtsam::Rot3& value);
pair<size_t, Rot3> getAnchor();
void setAnchorWeight(double value); void setAnchorWeight(double value);
double getAnchorWeight() const; double getAnchorWeight() const;
void setKarcherWeight(double value); void setKarcherWeight(double value);

93
gtsam/sfm/ShonanAveraging.cpp
View File

@ -16,26 +16,25 @@
* @brief Shonan Averaging algorithm * @brief Shonan Averaging algorithm
*/ */
#include <gtsam/sfm/ShonanAveraging.h> #include <SymEigsSolver.h>
#include <gtsam/linear/PCGSolver.h> #include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/SubgraphPreconditioner.h> #include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearEquality.h> #include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/sfm/ShonanAveraging.h>
#include <gtsam/sfm/ShonanFactor.h>
#include <gtsam/sfm/ShonanGaugeFactor.h> #include <gtsam/sfm/ShonanGaugeFactor.h>
#include <gtsam/slam/FrobeniusFactor.h> #include <gtsam/slam/FrobeniusFactor.h>
#include <gtsam/slam/KarcherMeanFactor-inl.h> #include <gtsam/slam/KarcherMeanFactor-inl.h>
#include <gtsam/sfm/ShonanFactor.h>
#include <Eigen/Eigenvalues> #include <Eigen/Eigenvalues>
#include <SymEigsSolver.h>
#include <algorithm> #include <algorithm>
#include <complex> #include <complex>
#include <iostream> #include <iostream>
#include <map> #include <map>
#include <random> #include <random>
#include <set>
#include <vector> #include <vector>
namespace gtsam { namespace gtsam {
@ -50,8 +49,11 @@ template <size_t d>
ShonanAveragingParameters<d>::ShonanAveragingParameters( ShonanAveragingParameters<d>::ShonanAveragingParameters(
const LevenbergMarquardtParams &_lm, const std::string &method, const LevenbergMarquardtParams &_lm, const std::string &method,
double optimalityThreshold, double alpha, double beta, double gamma) double optimalityThreshold, double alpha, double beta, double gamma)
: lm(_lm), optimalityThreshold(optimalityThreshold), alpha(alpha), : lm(_lm),
beta(beta), gamma(gamma) { optimalityThreshold(optimalityThreshold),
alpha(alpha),
beta(beta),
gamma(gamma) {
// By default, we will do conjugate gradient // By default, we will do conjugate gradient
lm.linearSolverType = LevenbergMarquardtParams::Iterative; lm.linearSolverType = LevenbergMarquardtParams::Iterative;
@ -92,28 +94,39 @@ template struct ShonanAveragingParameters<3>;
/* ************************************************************************* */ /* ************************************************************************* */
// Calculate number of unknown rotations referenced by measurements // Calculate number of unknown rotations referenced by measurements
// Throws exception of keys are not numbered as expected (may change in future).
template <size_t d> template <size_t d>
static size_t static size_t NrUnknowns(
NrUnknowns(const typename ShonanAveraging<d>::Measurements &measurements) { const typename ShonanAveraging<d>::Measurements &measurements) {
Key maxKey = 0;
std::set<Key> keys; std::set<Key> keys;
for (const auto &measurement : measurements) { for (const auto &measurement : measurements) {
keys.insert(measurement.key1()); for (const Key &key : measurement.keys()) {
keys.insert(measurement.key2()); maxKey = std::max(key, maxKey);
keys.insert(key);
} }
return keys.size(); }
size_t N = keys.size();
if (maxKey != N - 1) {
throw std::invalid_argument(
"As of now, ShonanAveraging expects keys numbered 0..N-1");
}
return N;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <size_t d> template <size_t d>
ShonanAveraging<d>::ShonanAveraging(const Measurements &measurements, ShonanAveraging<d>::ShonanAveraging(const Measurements &measurements,
const Parameters &parameters) const Parameters &parameters)
: parameters_(parameters), measurements_(measurements), : parameters_(parameters),
measurements_(measurements),
nrUnknowns_(NrUnknowns<d>(measurements)) { nrUnknowns_(NrUnknowns<d>(measurements)) {
for (const auto &measurement : measurements_) { for (const auto &measurement : measurements_) {
const auto &model = measurement.noiseModel(); const auto &model = measurement.noiseModel();
if (model && model->dim() != SO<d>::dimension) { if (model && model->dim() != SO<d>::dimension) {
measurement.print("Factor with incorrect noise model:\n"); measurement.print("Factor with incorrect noise model:\n");
throw std::invalid_argument("ShonanAveraging: measurements passed to " throw std::invalid_argument(
"ShonanAveraging: measurements passed to "
"constructor have incorrect dimension."); "constructor have incorrect dimension.");
} }
} }
@ -227,7 +240,8 @@ Values ShonanAveraging<3>::projectFrom(size_t p, const Values &values) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <size_t d> static Matrix RoundSolutionS(const Matrix &S) { template <size_t d>
static Matrix RoundSolutionS(const Matrix &S) {
const size_t N = S.cols() / d; const size_t N = S.cols() / d;
// First, compute a thin SVD of S // First, compute a thin SVD of S
Eigen::JacobiSVD<Matrix> svd(S, Eigen::ComputeThinV); Eigen::JacobiSVD<Matrix> svd(S, Eigen::ComputeThinV);
@ -246,8 +260,7 @@ template <size_t d> static Matrix RoundSolutionS(const Matrix &S) {
for (size_t i = 0; i < N; ++i) { for (size_t i = 0; i < N; ++i) {
// Compute the determinant of the ith dxd block of R // Compute the determinant of the ith dxd block of R
double determinant = R.middleCols<d>(d * i).determinant(); double determinant = R.middleCols<d>(d * i).determinant();
if (determinant > 0) if (determinant > 0) ++numPositiveBlocks;
++numPositiveBlocks;
} }
if (numPositiveBlocks < N / 2) { if (numPositiveBlocks < N / 2) {
@ -263,7 +276,8 @@ template <size_t d> static Matrix RoundSolutionS(const Matrix &S) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <> Values ShonanAveraging<2>::roundSolutionS(const Matrix &S) const { template <>
Values ShonanAveraging<2>::roundSolutionS(const Matrix &S) const {
// Round to a 2*2N matrix // Round to a 2*2N matrix
Matrix R = RoundSolutionS<2>(S); Matrix R = RoundSolutionS<2>(S);
@ -276,7 +290,8 @@ template <> Values ShonanAveraging<2>::roundSolutionS(const Matrix &S) const {
return values; return values;
} }
template <> Values ShonanAveraging<3>::roundSolutionS(const Matrix &S) const { template <>
Values ShonanAveraging<3>::roundSolutionS(const Matrix &S) const {
// Round to a 3*3N matrix // Round to a 3*3N matrix
Matrix R = RoundSolutionS<3>(S); Matrix R = RoundSolutionS<3>(S);
@ -332,7 +347,8 @@ static double Kappa(const BinaryMeasurement<T> &measurement) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <size_t d> Sparse ShonanAveraging<d>::buildD() const { template <size_t d>
Sparse ShonanAveraging<d>::buildD() const {
// Each measurement contributes 2*d elements along the diagonal of the // Each measurement contributes 2*d elements along the diagonal of the
// degree matrix. // degree matrix.
static constexpr size_t stride = 2 * d; static constexpr size_t stride = 2 * d;
@ -366,7 +382,8 @@ template <size_t d> Sparse ShonanAveraging<d>::buildD() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <size_t d> Sparse ShonanAveraging<d>::buildQ() const { template <size_t d>
Sparse ShonanAveraging<d>::buildQ() const {
// Each measurement contributes 2*d^2 elements on a pair of symmetric // Each measurement contributes 2*d^2 elements on a pair of symmetric
// off-diagonal blocks // off-diagonal blocks
static constexpr size_t stride = 2 * d * d; static constexpr size_t stride = 2 * d * d;
@ -513,8 +530,8 @@ struct MatrixProdFunctor {
// - We've been using 10^-4 for the nonnegativity tolerance // - We've been using 10^-4 for the nonnegativity tolerance
// - for numLanczosVectors, 20 is a good default value // - for numLanczosVectors, 20 is a good default value
static bool static bool SparseMinimumEigenValue(
SparseMinimumEigenValue(const Sparse &A, const Matrix &S, double *minEigenValue, const Sparse &A, const Matrix &S, double *minEigenValue,
Vector *minEigenVector = 0, size_t *numIterations = 0, Vector *minEigenVector = 0, size_t *numIterations = 0,
size_t maxIterations = 1000, size_t maxIterations = 1000,
double minEigenvalueNonnegativityTolerance = 10e-4, double minEigenvalueNonnegativityTolerance = 10e-4,
@ -530,8 +547,7 @@ SparseMinimumEigenValue(const Sparse &A, const Matrix &S, double *minEigenValue,
maxIterations, 1e-4, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN); maxIterations, 1e-4, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN);
// Check convergence and bail out if necessary // Check convergence and bail out if necessary
if (lmConverged != 1) if (lmConverged != 1) return false;
return false;
const double lmEigenValue = lmEigenValueSolver.eigenvalues()(0); const double lmEigenValue = lmEigenValueSolver.eigenvalues()(0);
@ -590,21 +606,20 @@ SparseMinimumEigenValue(const Sparse &A, const Matrix &S, double *minEigenValue,
maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue, maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue,
Spectra::SELECT_EIGENVALUE::LARGEST_MAGN); Spectra::SELECT_EIGENVALUE::LARGEST_MAGN);
if (minConverged != 1) if (minConverged != 1) return false;
return false;
*minEigenValue = minEigenValueSolver.eigenvalues()(0) + 2 * lmEigenValue; *minEigenValue = minEigenValueSolver.eigenvalues()(0) + 2 * lmEigenValue;
if (minEigenVector) { if (minEigenVector) {
*minEigenVector = minEigenValueSolver.eigenvectors(1).col(0); *minEigenVector = minEigenValueSolver.eigenvectors(1).col(0);
minEigenVector->normalize(); // Ensure that this is a unit vector minEigenVector->normalize(); // Ensure that this is a unit vector
} }
if (numIterations) if (numIterations) *numIterations = minEigenValueSolver.num_iterations();
*numIterations = minEigenValueSolver.num_iterations();
return true; return true;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <size_t d> Sparse ShonanAveraging<d>::computeA(const Matrix &S) const { template <size_t d>
Sparse ShonanAveraging<d>::computeA(const Matrix &S) const {
auto Lambda = computeLambda(S); auto Lambda = computeLambda(S);
return Lambda - Q_; return Lambda - Q_;
} }
@ -628,8 +643,8 @@ double ShonanAveraging<d>::computeMinEigenValue(const Values &values,
/* ************************************************************************* */ /* ************************************************************************* */
template <size_t d> template <size_t d>
std::pair<double, Vector> std::pair<double, Vector> ShonanAveraging<d>::computeMinEigenVector(
ShonanAveraging<d>::computeMinEigenVector(const Values &values) const { const Values &values) const {
Vector minEigenVector; Vector minEigenVector;
double minEigenValue = computeMinEigenValue(values, &minEigenVector); double minEigenValue = computeMinEigenValue(values, &minEigenVector);
return std::make_pair(minEigenValue, minEigenVector); return std::make_pair(minEigenValue, minEigenVector);
@ -750,7 +765,8 @@ Values ShonanAveraging<d>::initializeRandomly(std::mt19937 &rng) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <size_t d> Values ShonanAveraging<d>::initializeRandomly() const { template <size_t d>
Values ShonanAveraging<d>::initializeRandomly() const {
return initializeRandomly(kRandomNumberGenerator); return initializeRandomly(kRandomNumberGenerator);
} }
@ -823,8 +839,8 @@ ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters &parameters)
// Extract Rot3 measurement from Pose3 betweenfactors // Extract Rot3 measurement from Pose3 betweenfactors
// Modeled after similar function in dataset.cpp // Modeled after similar function in dataset.cpp
static BinaryMeasurement<Rot3> static BinaryMeasurement<Rot3> convert(
convert(const BetweenFactor<Pose3>::shared_ptr &f) { const BetweenFactor<Pose3>::shared_ptr &f) {
auto gaussian = auto gaussian =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel()); boost::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
if (!gaussian) if (!gaussian)
@ -837,12 +853,11 @@ convert(const BetweenFactor<Pose3>::shared_ptr &f) {
noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true)); noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true));
} }
static ShonanAveraging3::Measurements static ShonanAveraging3::Measurements extractRot3Measurements(
extractRot3Measurements(const BetweenFactorPose3s &factors) { const BetweenFactorPose3s &factors) {
ShonanAveraging3::Measurements result; ShonanAveraging3::Measurements result;
result.reserve(factors.size()); result.reserve(factors.size());
for (auto f : factors) for (auto f : factors) result.push_back(convert(f));
result.push_back(convert(f));
return result; return result;
} }

26
gtsam/sfm/ShonanAveraging.h
View File

@ -20,25 +20,27 @@
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <gtsam/dllexport.h>
#include <gtsam/geometry/Rot2.h> #include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/nonlinear/LevenbergMarquardtParams.h> #include <gtsam/nonlinear/LevenbergMarquardtParams.h>
#include <gtsam/sfm/BinaryMeasurement.h> #include <gtsam/sfm/BinaryMeasurement.h>
#include <gtsam/slam/dataset.h> #include <gtsam/slam/dataset.h>
#include <gtsam/dllexport.h>
#include <Eigen/Sparse> #include <Eigen/Sparse>
#include <map> #include <map>
#include <string> #include <string>
#include <type_traits> #include <type_traits>
#include <utility> #include <utility>
#include <vector>
namespace gtsam { namespace gtsam {
class NonlinearFactorGraph; class NonlinearFactorGraph;
class LevenbergMarquardtOptimizer; class LevenbergMarquardtOptimizer;
/// Parameters governing optimization etc. /// Parameters governing optimization etc.
template <size_t d> struct GTSAM_EXPORT ShonanAveragingParameters { template <size_t d>
struct GTSAM_EXPORT ShonanAveragingParameters {
// Select Rot2 or Rot3 interface based template parameter d // Select Rot2 or Rot3 interface based template parameter d
using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type; using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
using Anchor = std::pair<size_t, Rot>; using Anchor = std::pair<size_t, Rot>;
@ -64,6 +66,7 @@ template <size_t d> struct GTSAM_EXPORT ShonanAveragingParameters {
double getOptimalityThreshold() const { return optimalityThreshold; } double getOptimalityThreshold() const { return optimalityThreshold; }
void setAnchor(size_t index, const Rot &value) { anchor = {index, value}; } void setAnchor(size_t index, const Rot &value) { anchor = {index, value}; }
std::pair<size_t, Rot> getAnchor() { return anchor; }
void setAnchorWeight(double value) { alpha = value; } void setAnchorWeight(double value) { alpha = value; }
double getAnchorWeight() { return alpha; } double getAnchorWeight() { return alpha; }
@ -93,7 +96,8 @@ using ShonanAveragingParameters3 = ShonanAveragingParameters<3>;
* European Computer Vision Conference, 2020. * European Computer Vision Conference, 2020.
* You can view our ECCV spotlight video at https://youtu.be/5ppaqMyHtE0 * You can view our ECCV spotlight video at https://youtu.be/5ppaqMyHtE0
*/ */
template <size_t d> class GTSAM_EXPORT ShonanAveraging { template <size_t d>
class GTSAM_EXPORT ShonanAveraging {
public: public:
using Sparse = Eigen::SparseMatrix<double>; using Sparse = Eigen::SparseMatrix<double>;
@ -220,10 +224,9 @@ public:
* @param minEigenVector corresponding to minEigenValue at level p-1 * @param minEigenVector corresponding to minEigenValue at level p-1
* @return values of type SO(p) * @return values of type SO(p)
*/ */
Values Values initializeWithDescent(
initializeWithDescent(size_t p, const Values &values, size_t p, const Values &values, const Vector &minEigenVector,
const Vector &minEigenVector, double minEigenValue, double minEigenValue, double gradienTolerance = 1e-2,
double gradienTolerance = 1e-2,
double preconditionedGradNormTolerance = 1e-4) const; double preconditionedGradNormTolerance = 1e-4) const;
/// @} /// @}
/// @name Advanced API /// @name Advanced API
@ -300,7 +303,8 @@ public:
Values roundSolution(const Values &values) const; Values roundSolution(const Values &values) const;
/// Lift Values of type T to SO(p) /// Lift Values of type T to SO(p)
template <class T> static Values LiftTo(size_t p, const Values &values) { template <class T>
static Values LiftTo(size_t p, const Values &values) {
Values result; Values result;
for (const auto it : values.filter<T>()) { for (const auto it : values.filter<T>()) {
result.insert(it.key, SOn::Lift(p, it.value.matrix())); result.insert(it.key, SOn::Lift(p, it.value.matrix()));
@ -349,14 +353,16 @@ class ShonanAveraging2 : public ShonanAveraging<2> {
public: public:
ShonanAveraging2(const Measurements &measurements, ShonanAveraging2(const Measurements &measurements,
const Parameters &parameters = Parameters()); const Parameters &parameters = Parameters());
ShonanAveraging2(string g2oFile, const Parameters &parameters = Parameters()); explicit ShonanAveraging2(string g2oFile,
const Parameters &parameters = Parameters());
}; };
class ShonanAveraging3 : public ShonanAveraging<3> { class ShonanAveraging3 : public ShonanAveraging<3> {
public: public:
ShonanAveraging3(const Measurements &measurements, ShonanAveraging3(const Measurements &measurements,
const Parameters &parameters = Parameters()); const Parameters &parameters = Parameters());
ShonanAveraging3(string g2oFile, const Parameters &parameters = Parameters()); explicit ShonanAveraging3(string g2oFile,
const Parameters &parameters = Parameters());
// TODO(frank): Deprecate after we land pybind wrapper // TODO(frank): Deprecate after we land pybind wrapper
ShonanAveraging3(const BetweenFactorPose3s &factors, ShonanAveraging3(const BetweenFactorPose3s &factors,