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

solving issue with robust model

release/4.3a0
lcarlone 2020-09-24 17:06:20 -04:00
parent 001a55ad3a
commit 73600c8faa
2 changed files with 19 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam/sfm/ShonanAveraging.cpp
View File

@ -337,14 +337,24 @@ 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());
if (!isotropic) {
throw std::invalid_argument(
"Shonan averaging noise models must be isotropic.");
}
const double sigma = isotropic->sigma();
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);
}
/* ************************************************************************* */

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

@ -338,7 +338,7 @@ TEST(ShonanAveraging2, noisyToyGraphWithHuber) {
graph.print();
EXPECT_LONGS_EQUAL(6, graph.size());
auto initial = shonan.initializeRandomly(kRandomNumberGenerator);
auto result = shonan.run(initial, 2);
auto result = shonan.run(initial, 2, 3);
EXPECT_DOUBLES_EQUAL(0.0008211, shonan.cost(result.first), 1e-6);
EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate!
}