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

93
gtsam/sfm/ShonanAveraging.cpp
View File

@ -16,26 +16,25 @@
* @brief Shonan Averaging algorithm
*/
#include <gtsam/sfm/ShonanAveraging.h>
#include <SymEigsSolver.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/ShonanAveraging.h>
#include <gtsam/sfm/ShonanFactor.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 <set>
#include <vector>
namespace gtsam {
@ -50,8 +49,11 @@ 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) {
: lm(_lm),
optimalityThreshold(optimalityThreshold),
alpha(alpha),
beta(beta),
gamma(gamma) {
// By default, we will do conjugate gradient
lm.linearSolverType = LevenbergMarquardtParams::Iterative;
@ -92,28 +94,39 @@ template struct ShonanAveragingParameters<3>;
/* ************************************************************************* */
// 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>
static size_t
NrUnknowns(const typename ShonanAveraging<d>::Measurements &measurements) {
static size_t NrUnknowns(
const typename ShonanAveraging<d>::Measurements &measurements) {
Key maxKey = 0;
std::set<Key> keys;
for (const auto &measurement : measurements) {
keys.insert(measurement.key1());
keys.insert(measurement.key2());
for (const Key &key : measurement.keys()) {
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>
ShonanAveraging<d>::ShonanAveraging(const Measurements &measurements,
const Parameters &parameters)
: parameters_(parameters), measurements_(measurements),
: 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 "
throw std::invalid_argument(
"ShonanAveraging: measurements passed to "
"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;
// First, compute a thin SVD of S
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) {
// Compute the determinant of the ith dxd block of R
double determinant = R.middleCols<d>(d * i).determinant();
if (determinant > 0)
++numPositiveBlocks;
if (determinant > 0) ++numPositiveBlocks;
}
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
Matrix R = RoundSolutionS<2>(S);
@ -276,7 +290,8 @@ template <> Values ShonanAveraging<2>::roundSolutionS(const Matrix &S) const {
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
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
// degree matrix.
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
// off-diagonal blocks
static constexpr size_t stride = 2 * d * d;
@ -513,8 +530,8 @@ struct MatrixProdFunctor {
// - 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,
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,
@ -530,8 +547,7 @@ SparseMinimumEigenValue(const Sparse &A, const Matrix &S, double *minEigenValue,
maxIterations, 1e-4, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN);
// Check convergence and bail out if necessary
if (lmConverged != 1)
return false;
if (lmConverged != 1) return false;
const double lmEigenValue = lmEigenValueSolver.eigenvalues()(0);
@ -590,21 +606,20 @@ SparseMinimumEigenValue(const Sparse &A, const Matrix &S, double *minEigenValue,
maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue,
Spectra::SELECT_EIGENVALUE::LARGEST_MAGN);
if (minConverged != 1)
return false;
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();
if (numIterations) *numIterations = minEigenValueSolver.num_iterations();
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);
return Lambda - Q_;
}
@ -628,8 +643,8 @@ double ShonanAveraging<d>::computeMinEigenValue(const Values &values,
/* ************************************************************************* */
template <size_t d>
std::pair<double, Vector>
ShonanAveraging<d>::computeMinEigenVector(const Values &values) const {
std::pair<double, Vector> ShonanAveraging<d>::computeMinEigenVector(
const Values &values) const {
Vector minEigenVector;
double minEigenValue = computeMinEigenValue(values, &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);
}
@ -823,8 +839,8 @@ ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters &parameters)
// Extract Rot3 measurement from Pose3 betweenfactors
// Modeled after similar function in dataset.cpp
static BinaryMeasurement<Rot3>
convert(const BetweenFactor<Pose3>::shared_ptr &f) {
static BinaryMeasurement<Rot3> convert(
const BetweenFactor<Pose3>::shared_ptr &f) {
auto gaussian =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
if (!gaussian)
@ -837,12 +853,11 @@ convert(const BetweenFactor<Pose3>::shared_ptr &f) {
noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true));
}
static ShonanAveraging3::Measurements
extractRot3Measurements(const BetweenFactorPose3s &factors) {
static ShonanAveraging3::Measurements extractRot3Measurements(
const BetweenFactorPose3s &factors) {
ShonanAveraging3::Measurements result;
result.reserve(factors.size());
for (auto f : factors)
result.push_back(convert(f));
for (auto f : factors) result.push_back(convert(f));
return result;
}

26
gtsam/sfm/ShonanAveraging.h
View File

@ -20,25 +20,27 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/dllexport.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>
#include <vector>
namespace gtsam {
class NonlinearFactorGraph;
class LevenbergMarquardtOptimizer;
/// 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
using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
using Anchor = std::pair<size_t, Rot>;
@ -64,6 +66,7 @@ template <size_t d> struct GTSAM_EXPORT ShonanAveragingParameters {
double getOptimalityThreshold() const { return optimalityThreshold; }
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; }
double getAnchorWeight() { return alpha; }
@ -93,7 +96,8 @@ using ShonanAveragingParameters3 = ShonanAveragingParameters<3>;
* 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 {
template <size_t d>
class GTSAM_EXPORT ShonanAveraging {
public:
using Sparse = Eigen::SparseMatrix<double>;
@ -220,10 +224,9 @@ public:
* @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,
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
@ -300,7 +303,8 @@ public:
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) {
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()));
@ -349,14 +353,16 @@ class ShonanAveraging2 : public ShonanAveraging<2> {
public:
ShonanAveraging2(const Measurements &measurements,
const Parameters &parameters = Parameters());
ShonanAveraging2(string g2oFile, const Parameters &parameters = Parameters());
explicit ShonanAveraging2(string g2oFile,
const Parameters &parameters = Parameters());
};
class ShonanAveraging3 : public ShonanAveraging<3> {
public:
ShonanAveraging3(const Measurements &measurements,
const Parameters &parameters = Parameters());
ShonanAveraging3(string g2oFile, const Parameters &parameters = Parameters());
explicit ShonanAveraging3(string g2oFile,
const Parameters &parameters = Parameters());
// TODO(frank): Deprecate after we land pybind wrapper
ShonanAveraging3(const BetweenFactorPose3s &factors,