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

throw error if robust model used but not specified in parameters

release/4.3a0
Varun Agrawal 2020-12-03 20:27:59 -05:00
parent 636d5f4f1c
commit 06aa4ef780
1 changed files with 14 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gtsam/sfm/ShonanAveraging.cpp
View File

@ -332,8 +332,9 @@ double ShonanAveraging<d>::cost(const Values &values) const {
/* ************************************************************************* */ /* ************************************************************************* */
// Get kappa from noise model // Get kappa from noise model
template <typename T> template <typename T, size_t d>
static double Kappa(const BinaryMeasurement<T> &measurement) { static double Kappa(const BinaryMeasurement<T> &measurement,
const ShonanAveragingParameters<d> &parameters) {
const auto &isotropic = boost::dynamic_pointer_cast<noiseModel::Isotropic>( const auto &isotropic = boost::dynamic_pointer_cast<noiseModel::Isotropic>(
measurement.noiseModel()); measurement.noiseModel());
double sigma; double sigma;
@ -343,10 +344,13 @@ static double Kappa(const BinaryMeasurement<T> &measurement) {
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>( const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(
measurement.noiseModel()); measurement.noiseModel());
if (robust) { if (robust) {
std::cout << "Verification of optimality does not work with robust cost " if (parameters.getUseHuber()) {
"function" // Cannot verify optimality, setting arbitrary value
<< std::endl; sigma = 1;
sigma = 1; // setting arbitrary value } else {
throw std::invalid_argument(
"Robust cost function is invalid unless useHuber is set.");
}
} else { } else {
throw std::invalid_argument( throw std::invalid_argument(
"Shonan averaging noise models must be isotropic (but robust losses " "Shonan averaging noise models must be isotropic (but robust losses "
@ -372,7 +376,7 @@ Sparse ShonanAveraging<d>::buildD() const {
const auto &keys = measurement.keys(); const auto &keys = measurement.keys();
// Get kappa from noise model // 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]; const size_t di = d * keys[0], dj = d * keys[1];
for (size_t k = 0; k < d; k++) { for (size_t k = 0; k < d; k++) {
@ -410,7 +414,7 @@ Sparse ShonanAveraging<d>::buildQ() const {
const auto Rij = measurement.measured().matrix(); const auto Rij = measurement.measured().matrix();
// Get kappa from noise model // 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]; const size_t di = d * keys[0], dj = d * keys[1];
for (size_t r = 0; r < d; r++) { for (size_t r = 0; r < d; r++) {
@ -803,7 +807,8 @@ 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_.useHuber){ // in this case, there is no optimality verification if (parameters_
.useHuber) { // 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"); "When using robust norm, Shonan only tests a single rank");