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

Roustify BinaryMeasurements in a functional way, plus formatting

release/4.3a0
Varun Agrawal 2020-11-09 10:38:43 -05:00
parent fa26cf85ab
commit c99cb14b49
4 changed files with 103 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
gtsam/sfm/BinaryMeasurement.h
View File

@ -45,11 +45,43 @@ 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),
noiseModel_(model) {}
const SharedNoiseModel &model = nullptr,
bool useHuber = false)
: Factor(std::vector<Key>({key1, key2})),
measured_(measured),
noiseModel_(model) {
if (useHuber) {
const auto &robust =
boost::dynamic_pointer_cast<noiseModel::Robust>(this->noiseModel_);
if (!robust) {
// make robust
this->noiseModel_ = noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), this->noiseModel_);
}
}
}
/**
* Copy constructor to allow for making existing BinaryMeasurements as robust
* in a functional way.
*
* @param measurement BinaryMeasurement object.
* @param useHuber Boolean flag indicating whether to use Huber noise model.
*/
BinaryMeasurement(const BinaryMeasurement& measurement, bool useHuber = false) {
*this = measurement;
if (useHuber) {
const auto &robust =
boost::dynamic_pointer_cast<noiseModel::Robust>(this->noiseModel_);
if (!robust) {
// make robust
this->noiseModel_ = noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), this->noiseModel_);
}
}
}
/// @name Standard Interface
/// @{
@ -71,14 +103,6 @@ public:
this->noiseModel_->print(" noise model: ");
}
// TODO: make this more general?
void makeNoiseModelRobust(){
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(this->noiseModel_);
if(!robust) // make robust
this->noiseModel_ = noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), this->noiseModel_);
}
bool equals(const BinaryMeasurement &expected, double tol = 1e-9) const {
const BinaryMeasurement<T> *e =
dynamic_cast<const BinaryMeasurement<T> *>(&expected);

88
gtsam/sfm/ShonanAveraging.cpp
View File

@ -54,7 +54,7 @@ ShonanAveragingParameters<d>::ShonanAveragingParameters(
alpha(alpha),
beta(beta),
gamma(gamma),
useHuber(false){
useHuber(false) {
// By default, we will do conjugate gradient
lm.linearSolverType = LevenbergMarquardtParams::Iterative;
@ -139,7 +139,7 @@ ShonanAveraging<d>::ShonanAveraging(const Measurements &measurements,
/* ************************************************************************* */
template <size_t d>
NonlinearFactorGraph ShonanAveraging<d>::buildGraphAt(size_t p) const {
NonlinearFactorGraph graph;
NonlinearFactorGraph graph;
auto G = boost::make_shared<Matrix>(SO<-1>::VectorizedGenerators(p));
for (const auto &measurement : measurements_) {
@ -194,7 +194,7 @@ ShonanAveraging<d>::createOptimizerAt(size_t p, const Values &initial) const {
template <size_t d>
Values ShonanAveraging<d>::tryOptimizingAt(size_t p,
const Values &initial) const {
auto lm = createOptimizerAt(p, initial);
auto lm = createOptimizerAt(p, initial);
return lm->optimize();
}
@ -334,24 +334,26 @@ double ShonanAveraging<d>::cost(const Values &values) const {
// 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());
double sigma;
if (isotropic) {
sigma = isotropic->sigma();
} else{
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(
measurement.noiseModel());
if (robust) {
std::cout << "Verification of optimality does not work with robust cost function" << std::endl;
sigma = 1; // setting arbitrary value
}else{
throw std::invalid_argument(
"Shonan averaging noise models must be isotropic (but robust losses are allowed).");
}
}
return 1.0 / (sigma * sigma);
const auto &isotropic = boost::dynamic_pointer_cast<noiseModel::Isotropic>(
measurement.noiseModel());
double sigma;
if (isotropic) {
sigma = isotropic->sigma();
} else {
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(
measurement.noiseModel());
if (robust) {
std::cout << "Verification of optimality does not work with robust cost "
"function"
<< std::endl;
sigma = 1; // setting arbitrary value
} else {
throw std::invalid_argument(
"Shonan averaging noise models must be isotropic (but robust losses "
"are allowed).");
}
}
return 1.0 / (sigma * sigma);
}
/* ************************************************************************* */
@ -802,10 +804,10 @@ std::pair<Values, double> ShonanAveraging<d>::run(const Values &initialEstimate,
// Optimize until convergence at this level
Qstar = tryOptimizingAt(p, initialSOp);
if(parameters_.useHuber){ // in this case, there is no optimality verification
if(pMin!=pMax)
std::cout << "When using robust norm, Shonan only tests a single rank" << std::endl;
const Values SO3Values = roundSolution(Qstar);
return std::make_pair(SO3Values, 0);
if(pMin!=pMax)
std::cout << "When using robust norm, Shonan only tests a single rank" << std::endl;
const Values SO3Values = roundSolution(Qstar);
return std::make_pair(SO3Values, 0);
}
// Check certificate of global optimzality
@ -833,13 +835,17 @@ template class ShonanAveraging<2>;
ShonanAveraging2::ShonanAveraging2(const Measurements &measurements,
const Parameters &parameters)
: ShonanAveraging<2>(parameters.useHuber?
makeNoiseModelRobust(measurements) : measurements, parameters) {}
: ShonanAveraging<2>(parameters.useHuber
? makeNoiseModelRobust(measurements)
: measurements,
parameters) {}
ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters &parameters)
: ShonanAveraging<2>(parameters.useHuber?
makeNoiseModelRobust( parseMeasurements<Rot2>(g2oFile) ) :
parseMeasurements<Rot2>(g2oFile), parameters) {}
: ShonanAveraging<2>(
parameters.useHuber
? makeNoiseModelRobust(parseMeasurements<Rot2>(g2oFile))
: parseMeasurements<Rot2>(g2oFile),
parameters) {}
/* ************************************************************************* */
// Explicit instantiation for d=3
@ -848,12 +854,14 @@ template class ShonanAveraging<3>;
ShonanAveraging3::ShonanAveraging3(const Measurements &measurements,
const Parameters &parameters)
: ShonanAveraging<3>(parameters.useHuber?
makeNoiseModelRobust(measurements) : measurements, parameters) {}
makeNoiseModelRobust(measurements) : measurements, parameters) {}
ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters &parameters)
: ShonanAveraging<3>(parameters.useHuber?
makeNoiseModelRobust( parseMeasurements<Rot3>(g2oFile) ) :
parseMeasurements<Rot3>(g2oFile), parameters) {}
: ShonanAveraging<3>(
parameters.useHuber
? makeNoiseModelRobust(parseMeasurements<Rot3>(g2oFile))
: parseMeasurements<Rot3>(g2oFile),
parameters) {}
// TODO(frank): Deprecate after we land pybind wrapper
@ -869,8 +877,8 @@ static BinaryMeasurement<Rot3> convert(
"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));
f->key1(), f->key2(), f->measured().rotation(),
noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true));
}
static ShonanAveraging3::Measurements extractRot3Measurements(
@ -883,9 +891,11 @@ static ShonanAveraging3::Measurements extractRot3Measurements(
ShonanAveraging3::ShonanAveraging3(const BetweenFactorPose3s &factors,
const Parameters &parameters)
: ShonanAveraging<3>(parameters.useHuber?
makeNoiseModelRobust( extractRot3Measurements(factors) ):
extractRot3Measurements(factors), parameters) {}
: ShonanAveraging<3>(
parameters.useHuber
? makeNoiseModelRobust(extractRot3Measurements(factors))
: extractRot3Measurements(factors),
parameters) {}
/* ************************************************************************* */
} // namespace gtsam

28
gtsam/sfm/ShonanAveraging.h
View File

@ -53,7 +53,8 @@ struct GTSAM_EXPORT ShonanAveragingParameters {
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)
bool useHuber; // if enabled, the Huber loss is used in the optimization (default is false)
bool useHuber; // if enabled, the Huber loss is used in the optimization
// (default is false)
ShonanAveragingParameters(const LevenbergMarquardtParams &lm =
LevenbergMarquardtParams::CeresDefaults(),
@ -83,12 +84,12 @@ struct GTSAM_EXPORT ShonanAveragingParameters {
bool getUseHuber() { return useHuber; }
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;
std::cout << " --------------------------" << std::endl;
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;
std::cout << " --------------------------" << std::endl;
}
};
@ -120,7 +121,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:
@ -165,12 +165,12 @@ class GTSAM_EXPORT ShonanAveraging {
}
/// wrap factors with robust Huber loss
static Measurements makeNoiseModelRobust(Measurements measurements){
Measurements robustMeasurements = measurements;
for (auto &measurement : robustMeasurements) {
measurement.makeNoiseModelRobust();
}
return robustMeasurements;
Measurements makeNoiseModelRobust(const Measurements& measurements) const {
Measurements robustMeasurements = measurements;
for (auto &measurement : robustMeasurements) {
measurement = BinaryMeasurement<Rot>(measurement, true);
}
return robustMeasurements;
}
/// k^th measurement, as a Rot.

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

@ -67,20 +67,19 @@ TEST(BinaryMeasurement, Rot3) {
/* ************************************************************************* */
TEST(BinaryMeasurement, Rot3MakeRobust) {
BinaryMeasurement<Rot3> rot3Measurement(kKey1, kKey2, rot3Measured,
rot3_model);
rot3Measurement.makeNoiseModelRobust();
rot3_model, true);
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());
rot3Measurement.noiseModel());
EXPECT(robust);
// test that if we call it again nothing changes:
rot3Measurement.makeNoiseModelRobust();
rot3Measurement = BinaryMeasurement<Rot3>(rot3Measurement, true);
const auto &robust2 = boost::dynamic_pointer_cast<noiseModel::Robust>(
rot3Measurement.noiseModel());
rot3Measurement.noiseModel());
EXPECT(robust2);
}