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

added flag to enable optimality certification, some formatting

release/4.3a0
Varun Agrawal 2020-12-05 16:06:27 -05:00
parent 553f569038
commit de5adb495f
2 changed files with 41 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gtsam/sfm/ShonanAveraging.cpp
View File

@ -54,7 +54,8 @@ ShonanAveragingParameters<d>::ShonanAveragingParameters(
alpha(alpha), alpha(alpha),
beta(beta), beta(beta),
gamma(gamma), gamma(gamma),
useHuber(false) { useHuber(false),
certifyOptimality(true) {
// By default, we will do conjugate gradient // By default, we will do conjugate gradient
lm.linearSolverType = LevenbergMarquardtParams::Iterative; lm.linearSolverType = LevenbergMarquardtParams::Iterative;
@ -343,13 +344,15 @@ static double Kappa(const BinaryMeasurement<T> &measurement,
} else { } else {
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>( const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(
measurement.noiseModel()); measurement.noiseModel());
// Check if noise model is robust
if (robust) { if (robust) {
if (parameters.getUseHuber()) { // If robust, check if optimality certificate is expected
// Cannot verify optimality, setting arbitrary value if (parameters.getCertifyOptimality()) {
sigma = 1;
} else {
throw std::invalid_argument( throw std::invalid_argument(
"Robust cost function is invalid unless useHuber is set."); "Verification of optimality does not work with robust cost.");
} else {
// Optimality certificate not required, so setting default sigma
sigma = 1;
} }
} else { } else {
throw std::invalid_argument( throw std::invalid_argument(
@ -807,16 +810,15 @@ std::pair<Values, double> ShonanAveraging<d>::run(const Values &initialEstimate,
for (size_t p = pMin; p <= pMax; p++) { for (size_t p = pMin; p <= pMax; p++) {
// Optimize until convergence at this level // Optimize until convergence at this level
Qstar = tryOptimizingAt(p, initialSOp); Qstar = tryOptimizingAt(p, initialSOp);
if (parameters_ if (parameters_.getUseHuber() || parameters_.getCertifyOptimality()) {
.useHuber) { // in this case, there is no optimality verification // in this case, there is no optimality verification
if (pMin != pMax) { if (pMin != pMax) {
throw std::runtime_error( throw std::runtime_error(
"When using robust norm, Shonan only tests a single rank. Set pMin = pMax"); "When using robust norm, Shonan only tests a single rank. Set pMin = pMax");
} }
const Values SO3Values = roundSolution(Qstar); const Values SO3Values = roundSolution(Qstar);
return std::make_pair(SO3Values, 0); return std::make_pair(SO3Values, 0);
} } else {
// Check certificate of global optimality // Check certificate of global optimality
Vector minEigenVector; Vector minEigenVector;
double minEigenValue = computeMinEigenValue(Qstar, &minEigenVector); double minEigenValue = computeMinEigenValue(Qstar, &minEigenVector);
@ -833,6 +835,7 @@ std::pair<Values, double> ShonanAveraging<d>::run(const Values &initialEstimate,
initializeWithDescent(p + 1, Qstar, minEigenVector, minEigenValue); initializeWithDescent(p + 1, Qstar, minEigenVector, minEigenValue);
} }
} }
}
throw std::runtime_error("Shonan::run did not converge for given pMax"); throw std::runtime_error("Shonan::run did not converge for given pMax");
} }

22
gtsam/sfm/ShonanAveraging.h
View File

@ -47,14 +47,16 @@ struct GTSAM_EXPORT ShonanAveragingParameters {
using Anchor = std::pair<size_t, Rot>; using Anchor = std::pair<size_t, Rot>;
// Paremeters themselves: // Paremeters themselves:
LevenbergMarquardtParams lm; // LM parameters LevenbergMarquardtParams lm; ///< LM parameters
double optimalityThreshold; // threshold used in checkOptimality double optimalityThreshold; ///< threshold used in checkOptimality
Anchor anchor; // pose to use as anchor if not Karcher Anchor anchor; ///< pose to use as anchor if not Karcher
double alpha; // weight of anchor-based prior (default 0) double alpha; ///< weight of anchor-based prior (default 0)
double beta; // weight of Karcher-based prior (default 1) double beta; ///< weight of Karcher-based prior (default 1)
double gamma; // weight of gauge-fixing factors (default 0) double gamma; ///< weight of gauge-fixing factors (default 0)
bool useHuber; // if enabled, the Huber loss is used in the optimization ///< if enabled, the Huber loss is used (default false)
// (default is false) bool useHuber;
///< if enabled solution optimality is certified (default true)
bool certifyOptimality;
ShonanAveragingParameters(const LevenbergMarquardtParams &lm = ShonanAveragingParameters(const LevenbergMarquardtParams &lm =
LevenbergMarquardtParams::CeresDefaults(), LevenbergMarquardtParams::CeresDefaults(),
@ -83,6 +85,9 @@ struct GTSAM_EXPORT ShonanAveragingParameters {
void setUseHuber(bool value) { useHuber = value; } void setUseHuber(bool value) { useHuber = value; }
bool getUseHuber() const { return useHuber; } 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. /// Print the parameters and flags used for rotation averaging.
void print() const { void print() const {
std::cout << " ShonanAveragingParameters: " << std::endl; std::cout << " ShonanAveragingParameters: " << std::endl;
@ -174,7 +179,6 @@ class GTSAM_EXPORT ShonanAveraging {
double k = 1.345) const { double k = 1.345) const {
Measurements robustMeasurements; Measurements robustMeasurements;
for (auto &measurement : measurements) { for (auto &measurement : measurements) {
auto model = measurement.noiseModel(); auto model = measurement.noiseModel();
const auto &robust = const auto &robust =
boost::dynamic_pointer_cast<noiseModel::Robust>(model); boost::dynamic_pointer_cast<noiseModel::Robust>(model);