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);

44
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;
@ -339,16 +339,18 @@ static double Kappa(const BinaryMeasurement<T> &measurement) {
double sigma;
if (isotropic) {
sigma = isotropic->sigma();
} else{
} 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;
std::cout << "Verification of optimality does not work with robust cost "
"function"
<< std::endl;
sigma = 1; // setting arbitrary value
}else{
} else {
throw std::invalid_argument(
"Shonan averaging noise models must be isotropic (but robust losses are allowed).");
"Shonan averaging noise models must be isotropic (but robust losses "
"are allowed).");
}
}
return 1.0 / (sigma * sigma);
@ -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
@ -851,9 +857,11 @@ ShonanAveraging3::ShonanAveraging3(const Measurements &measurements,
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
@ -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

8
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(),
@ -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,10 +165,10 @@ class GTSAM_EXPORT ShonanAveraging {
}
/// wrap factors with robust Huber loss
static Measurements makeNoiseModelRobust(Measurements measurements){
Measurements makeNoiseModelRobust(const Measurements& measurements) const {
Measurements robustMeasurements = measurements;
for (auto &measurement : robustMeasurements) {
measurement.makeNoiseModelRobust();
measurement = BinaryMeasurement<Rot>(measurement, true);
}
return robustMeasurements;
}

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

@ -67,8 +67,7 @@ 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);
@ -78,7 +77,7 @@ TEST(BinaryMeasurement, Rot3MakeRobust) {
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());
EXPECT(robust2);