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

Merge pull request #547 from borglab/feature/RobustShonan 

Feature/robust shonan
release/4.3a0
Varun Agrawal 2021-01-11 21:58:42 -05:00 committed by GitHub
parent d50f9755a9 d89dd7d874
commit fb44e56dc8
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
9 changed files with 256 additions and 3630 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3563
.cproject
View File

File diff suppressed because it is too large Load Diff

25
examples/ShonanAveragingCLI.cpp
View File

@ -25,6 +25,9 @@
* Read 3D dataset sphere25000.txt and output to shonan.g2o (default)
* ./ShonanAveragingCLI -i spere2500.txt
*
* If you prefer using a robust Huber loss, you can add the option "-h true",
* for instance
* ./ShonanAveragingCLI -i spere2500.txt -h true
*/
#include <gtsam/base/timing.h>
@ -43,7 +46,8 @@ int main(int argc, char* argv[]) {
string datasetName;
string inputFile;
string outputFile;
int d, seed;
int d, seed, pMin;
bool useHuberLoss;
po::options_description desc(
"Shonan Rotation Averaging CLI reads a *pose* graph, extracts the "
"rotation constraints, and runs the Shonan algorithm.");
@ -58,6 +62,10 @@ int main(int argc, char* argv[]) {
"Write solution to the specified file")(
"dimension,d", po::value<int>(&d)->default_value(3),
"Optimize over 2D or 3D rotations")(
"useHuberLoss,h", po::value<bool>(&useHuberLoss)->default_value(false),
"set True to use Huber loss")("pMin,p",
po::value<int>(&pMin)->default_value(3),
"set to use desired rank pMin")(
"seed,s", po::value<int>(&seed)->default_value(42),
"Random seed for initial estimate");
po::variables_map vm;
@ -85,11 +93,14 @@ int main(int argc, char* argv[]) {
NonlinearFactorGraph::shared_ptr inputGraph;
Values::shared_ptr posesInFile;
Values poses;
auto lmParams = LevenbergMarquardtParams::CeresDefaults();
if (d == 2) {
cout << "Running Shonan averaging for SO(2) on " << inputFile << endl;
ShonanAveraging2 shonan(inputFile);
ShonanAveraging2::Parameters parameters(lmParams);
parameters.setUseHuber(useHuberLoss);
ShonanAveraging2 shonan(inputFile, parameters);
auto initial = shonan.initializeRandomly(rng);
auto result = shonan.run(initial);
auto result = shonan.run(initial, pMin);
// Parse file again to set up translation problem, adding a prior
boost::tie(inputGraph, posesInFile) = load2D(inputFile);
@ -101,9 +112,11 @@ int main(int argc, char* argv[]) {
poses = initialize::computePoses<Pose2>(result.first, &poseGraph);
} else if (d == 3) {
cout << "Running Shonan averaging for SO(3) on " << inputFile << endl;
ShonanAveraging3 shonan(inputFile);
ShonanAveraging3::Parameters parameters(lmParams);
parameters.setUseHuber(useHuberLoss);
ShonanAveraging3 shonan(inputFile, parameters);
auto initial = shonan.initializeRandomly(rng);
auto result = shonan.run(initial);
auto result = shonan.run(initial, pMin);
// Parse file again to set up translation problem, adding a prior
boost::tie(inputGraph, posesInFile) = load3D(inputFile);
@ -118,7 +131,7 @@ int main(int argc, char* argv[]) {
return 1;
}
cout << "Writing result to " << outputFile << endl;
writeG2o(NonlinearFactorGraph(), poses, outputFile);
writeG2o(*inputGraph, poses, outputFile);
return 0;
}

5
gtsam/sfm/BinaryMeasurement.h
View File

@ -45,10 +45,11 @@ private:
T measured_; ///< The measurement
SharedNoiseModel noiseModel_; ///< Noise model
public:
public:
BinaryMeasurement(Key key1, Key key2, const T &measured,
const SharedNoiseModel &model = nullptr)
: Factor(std::vector<Key>({key1, key2})), measured_(measured),
: Factor(std::vector<Key>({key1, key2})),
measured_(measured),
noiseModel_(model) {}
/// @name Standard Interface

105
gtsam/sfm/ShonanAveraging.cpp
View File

@ -53,7 +53,9 @@ ShonanAveragingParameters<d>::ShonanAveragingParameters(
optimalityThreshold(optimalityThreshold),
alpha(alpha),
beta(beta),
gamma(gamma) {
gamma(gamma),
useHuber(false),
certifyOptimality(true) {
// By default, we will do conjugate gradient
lm.linearSolverType = LevenbergMarquardtParams::Iterative;
@ -140,25 +142,23 @@ 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;
}
@ -186,7 +186,6 @@ ShonanAveraging<d>::createOptimizerAt(size_t p, const Values &initial) const {
graph.emplace_shared<PriorFactor<SOn>>(i, SOn::Lift(p, value.matrix()),
model);
}
// Optimize
return boost::make_shared<LevenbergMarquardtOptimizer>(graph, initial,
parameters_.lm);
@ -334,15 +333,33 @@ double ShonanAveraging<d>::cost(const Values &values) const {
/* ************************************************************************* */
// Get kappa from noise model
template <typename T>
static double Kappa(const BinaryMeasurement<T> &measurement) {
template <typename T, size_t d>
static double Kappa(const BinaryMeasurement<T> &measurement,
const ShonanAveragingParameters<d> &parameters) {
const auto &isotropic = boost::dynamic_pointer_cast<noiseModel::Isotropic>(
measurement.noiseModel());
if (!isotropic) {
throw std::invalid_argument(
"Shonan averaging noise models must be isotropic.");
double sigma;
if (isotropic) {
sigma = isotropic->sigma();
} else {
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(
measurement.noiseModel());
// Check if noise model is robust
if (robust) {
// If robust, check if optimality certificate is expected
if (parameters.getCertifyOptimality()) {
throw std::invalid_argument(
"Certification of optimality does not work with robust cost.");
} else {
// Optimality certificate not required, so setting default sigma
sigma = 1;
}
} else {
throw std::invalid_argument(
"Shonan averaging noise models must be isotropic (but robust losses "
"are allowed).");
}
}
const double sigma = isotropic->sigma();
return 1.0 / (sigma * sigma);
}
@ -362,7 +379,7 @@ Sparse ShonanAveraging<d>::buildD() const {
const auto &keys = measurement.keys();
// Get kappa from noise model
double kappa = Kappa<Rot>(measurement);
double kappa = Kappa<Rot, d>(measurement, parameters_);
const size_t di = d * keys[0], dj = d * keys[1];
for (size_t k = 0; k < d; k++) {
@ -400,7 +417,7 @@ Sparse ShonanAveraging<d>::buildQ() const {
const auto Rij = measurement.measured().matrix();
// Get kappa from noise model
double kappa = Kappa<Rot>(measurement);
double kappa = Kappa<Rot, d>(measurement, parameters_);
const size_t di = d * keys[0], dj = d * keys[1];
for (size_t r = 0; r < d; r++) {
@ -793,21 +810,30 @@ std::pair<Values, double> ShonanAveraging<d>::run(const Values &initialEstimate,
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
if (parameters_.getUseHuber() || !parameters_.getCertifyOptimality()) {
// in this case, there is no optimality certification
if (pMin != pMax) {
throw std::runtime_error(
"When using robust norm, Shonan only tests a single rank. Set pMin = pMax");
}
const Values SO3Values = roundSolution(Qstar);
return std::make_pair(SO3Values, minEigenValue);
}
return std::make_pair(SO3Values, 0);
} else {
// Check certificate of global optimality
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);
// 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");
@ -819,9 +845,13 @@ template class ShonanAveraging<2>;
ShonanAveraging2::ShonanAveraging2(const Measurements &measurements,
const Parameters &parameters)
: ShonanAveraging<2>(measurements, parameters) {}
: ShonanAveraging<2>(maybeRobust(measurements, parameters.getUseHuber()),
parameters) {}
ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters &parameters)
: ShonanAveraging<2>(parseMeasurements<Rot2>(g2oFile), parameters) {}
: ShonanAveraging<2>(maybeRobust(parseMeasurements<Rot2>(g2oFile),
parameters.getUseHuber()),
parameters) {}
/* ************************************************************************* */
// Explicit instantiation for d=3
@ -829,10 +859,13 @@ template class ShonanAveraging<3>;
ShonanAveraging3::ShonanAveraging3(const Measurements &measurements,
const Parameters &parameters)
: ShonanAveraging<3>(measurements, parameters) {}
: ShonanAveraging<3>(maybeRobust(measurements, parameters.getUseHuber()),
parameters) {}
ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters &parameters)
: ShonanAveraging<3>(parseMeasurements<Rot3>(g2oFile), parameters) {}
: ShonanAveraging<3>(maybeRobust(parseMeasurements<Rot3>(g2oFile),
parameters.getUseHuber()),
parameters) {}
// TODO(frank): Deprecate after we land pybind wrapper
@ -847,9 +880,11 @@ static BinaryMeasurement<Rot3> convert(
"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));
auto model = noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345),
noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3)));
return BinaryMeasurement<Rot3>(f->key1(), f->key2(), f->measured().rotation(),
model);
}
static ShonanAveraging3::Measurements extractRot3Measurements(
@ -862,7 +897,9 @@ static ShonanAveraging3::Measurements extractRot3Measurements(
ShonanAveraging3::ShonanAveraging3(const BetweenFactorPose3s &factors,
const Parameters &parameters)
: ShonanAveraging<3>(extractRot3Measurements(factors), parameters) {}
: ShonanAveraging<3>(maybeRobust(extractRot3Measurements(factors),
parameters.getUseHuber()),
parameters) {}
/* ************************************************************************* */
} // namespace gtsam

88
gtsam/sfm/ShonanAveraging.h
View File

@ -46,13 +46,17 @@ struct GTSAM_EXPORT ShonanAveragingParameters {
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)
// Parameters 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)
/// if enabled, the Huber loss is used (default false)
bool useHuber;
/// if enabled solution optimality is certified (default true)
bool certifyOptimality;
ShonanAveragingParameters(const LevenbergMarquardtParams &lm =
LevenbergMarquardtParams::CeresDefaults(),
@ -67,16 +71,31 @@ 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; }
std::pair<size_t, Rot> getAnchor() const { return anchor; }
void setAnchorWeight(double value) { alpha = value; }
double getAnchorWeight() { return alpha; }
double getAnchorWeight() const { return alpha; }
void setKarcherWeight(double value) { beta = value; }
double getKarcherWeight() { return beta; }
double getKarcherWeight() const { return beta; }
void setGaugesWeight(double value) { gamma = value; }
double getGaugesWeight() { return gamma; }
double getGaugesWeight() const { return gamma; }
void setUseHuber(bool value) { useHuber = value; }
bool getUseHuber() const { return useHuber; }
void setCertifyOptimality(bool value) { certifyOptimality = value; }
bool getCertifyOptimality() const { return certifyOptimality; }
/// Print the parameters and flags used for rotation averaging.
void print() const {
std::cout << " ShonanAveragingParameters: " << std::endl;
std::cout << " alpha: " << alpha << std::endl;
std::cout << " beta: " << beta << std::endl;
std::cout << " gamma: " << gamma << std::endl;
std::cout << " useHuber: " << useHuber << std::endl;
}
};
using ShonanAveragingParameters2 = ShonanAveragingParameters<2>;
@ -107,7 +126,6 @@ class GTSAM_EXPORT ShonanAveraging {
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:
@ -151,6 +169,36 @@ class GTSAM_EXPORT ShonanAveraging {
return measurements_[k];
}
/**
* Update factors to use robust Huber loss.
*
* @param measurements Vector of BinaryMeasurements.
* @param k Huber noise model threshold.
*/
Measurements makeNoiseModelRobust(const Measurements &measurements,
double k = 1.345) const {
Measurements robustMeasurements;
for (auto &measurement : measurements) {
auto model = measurement.noiseModel();
const auto &robust =
boost::dynamic_pointer_cast<noiseModel::Robust>(model);
SharedNoiseModel robust_model;
// Check if the noise model is already robust
if (robust) {
robust_model = model;
} else {
// make robust
robust_model = noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(k), model);
}
BinaryMeasurement<Rot> meas(measurement.key1(), measurement.key2(),
measurement.measured(), robust_model);
robustMeasurements.push_back(meas);
}
return robustMeasurements;
}
/// k^th measurement, as a Rot.
const Rot &measured(size_t k) const { return measurements_[k].measured(); }
@ -345,6 +393,22 @@ class GTSAM_EXPORT ShonanAveraging {
std::pair<Values, double> run(const Values &initialEstimate, size_t pMin = d,
size_t pMax = 10) const;
/// @}
/**
* Helper function to convert measurements to robust noise model
* if flag is set.
*
* @tparam T the type of measurement, e.g. Rot3.
* @param measurements vector of BinaryMeasurements of type T.
* @param useRobustModel flag indicating whether use robust noise model
* instead.
*/
template <typename T>
inline std::vector<BinaryMeasurement<T>> maybeRobust(
const std::vector<BinaryMeasurement<T>> &measurements,
bool useRobustModel = false) const {
return useRobustModel ? makeNoiseModelRobust(measurements) : measurements;
}
};
// Subclasses for d=2 and d=3 that explicitly instantiate, as well as provide a

17
gtsam/sfm/tests/testBinaryMeasurement.cpp
View File

@ -36,6 +36,7 @@ static SharedNoiseModel rot3_model(noiseModel::Isotropic::Sigma(3, 0.05));
const Unit3 unit3Measured(Vector3(1, 1, 1));
const Rot3 rot3Measured;
/* ************************************************************************* */
TEST(BinaryMeasurement, Unit3) {
BinaryMeasurement<Unit3> unit3Measurement(kKey1, kKey2, unit3Measured,
unit3_model);
@ -48,6 +49,7 @@ TEST(BinaryMeasurement, Unit3) {
EXPECT(unit3Measurement.equals(unit3MeasurementCopy));
}
/* ************************************************************************* */
TEST(BinaryMeasurement, Rot3) {
// testing the accessors
BinaryMeasurement<Rot3> rot3Measurement(kKey1, kKey2, rot3Measured,
@ -62,6 +64,21 @@ TEST(BinaryMeasurement, Rot3) {
EXPECT(rot3Measurement.equals(rot3MeasurementCopy));
}
/* ************************************************************************* */
TEST(BinaryMeasurement, Rot3MakeRobust) {
auto huber_model = noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), rot3_model);
BinaryMeasurement<Rot3> rot3Measurement(kKey1, kKey2, rot3Measured,
huber_model);
EXPECT_LONGS_EQUAL(rot3Measurement.key1(), kKey1);
EXPECT_LONGS_EQUAL(rot3Measurement.key2(), kKey2);
EXPECT(rot3Measurement.measured().equals(rot3Measured));
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(
rot3Measurement.noiseModel());
EXPECT(robust);
}
/* ************************************************************************* */
int main() {
TestResult tr;

37
gtsam/sfm/tests/testShonanAveraging.cpp
View File

@ -17,6 +17,7 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/sfm/ShonanAveraging.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/FrobeniusFactor.h>
@ -342,6 +343,42 @@ TEST(ShonanAveraging2, noisyToyGraph) {
EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate!
}
/* ************************************************************************* */
TEST(ShonanAveraging2, noisyToyGraphWithHuber) {
// Load 2D toy example
auto lmParams = LevenbergMarquardtParams::CeresDefaults();
string g2oFile = findExampleDataFile("noisyToyGraph.txt");
ShonanAveraging2::Parameters parameters(lmParams);
auto measurements = parseMeasurements<Rot2>(g2oFile);
parameters.setUseHuber(true);
parameters.setCertifyOptimality(false);
string parameters_print =
" ShonanAveragingParameters: \n alpha: 0\n beta: 1\n gamma: 0\n "
"useHuber: 1\n";
assert_print_equal(parameters_print, parameters);
ShonanAveraging2 shonan(measurements, parameters);
EXPECT_LONGS_EQUAL(4, shonan.nrUnknowns());
// Check graph building
NonlinearFactorGraph graph = shonan.buildGraphAt(2);
EXPECT_LONGS_EQUAL(6, graph.size());
// test that each factor is actually robust
for (size_t i=0; i<=4; i++) { // note: last is the Gauge factor and is not robust
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(
boost::dynamic_pointer_cast<NoiseModelFactor>(graph[i])->noiseModel());
EXPECT(robust); // we expect the factors to be use a robust noise model (in particular, Huber)
}
// test result
auto initial = shonan.initializeRandomly(kRandomNumberGenerator);
auto result = shonan.run(initial, 2,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) {

25
gtsam/slam/FrobeniusFactor.cpp
View File

@ -23,17 +23,25 @@ using namespace std;
namespace gtsam {
//******************************************************************************
boost::shared_ptr<noiseModel::Isotropic>
SharedNoiseModel
ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) {
double sigma = 1.0;
if (model != nullptr) {
auto sigmas = model->sigmas();
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(model);
Vector sigmas;
if (robust) {
sigmas = robust->noise()->sigmas();
} else {
sigmas = model->sigmas();
}
size_t n = sigmas.size();
if (n == 1) {
sigma = sigmas(0); // Rot2
goto exit;
}
if (n == 3 || n == 6) {
else if (n == 3 || n == 6) {
sigma = sigmas(2); // Pose2, Rot3, or Pose3
if (sigmas(0) != sigma || sigmas(1) != sigma) {
if (!defaultToUnit) {
@ -46,8 +54,15 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) {
throw std::runtime_error("Can only convert Pose2/Pose3 noise models");
}
}
exit:
return noiseModel::Isotropic::Sigma(d, sigma);
exit:
auto isoModel = noiseModel::Isotropic::Sigma(d, sigma);
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(model);
if (robust) {
return noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), isoModel);
} else {
return isoModel;
}
}
//******************************************************************************

19
gtsam/slam/FrobeniusFactor.h
View File

@ -26,15 +26,20 @@
namespace gtsam {
/**
* When creating (any) FrobeniusFactor we can convert a Rot/Pose
* BetweenFactor noise model into a n-dimensional isotropic noise
* model used to weight the Frobenius norm. If the noise model passed is
* null we return a n-dimensional isotropic noise model with sigma=1.0. If
* not, we we check if the d-dimensional noise model on rotations is
* When creating (any) FrobeniusFactor we can convert a Rot/Pose BetweenFactor
* noise model into a n-dimensional isotropic noise
* model used to weight the Frobenius norm.
* If the noise model passed is null we return a n-dimensional isotropic noise
* model with sigma=1.0.
* If not, we we check if the d-dimensional noise model on rotations is
* isotropic. If it is, we extend to 'n' dimensions, otherwise we throw an
* error. If defaultToUnit == false throws an exception on unexepcted input.
* error.
* If the noise model is a robust error model, we use the sigmas of the
* underlying noise model.
*
* If defaultToUnit == false throws an exception on unexepcted input.
*/
GTSAM_EXPORT boost::shared_ptr<noiseModel::Isotropic>
GTSAM_EXPORT SharedNoiseModel
ConvertNoiseModel(const SharedNoiseModel &model, size_t n,
bool defaultToUnit = true);