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

throw runtime errors and explicitly form robust noise model

release/4.3a0
Varun Agrawal 2020-11-30 17:31:08 -05:00
parent fba918ce96
commit fd74ae9330
1 changed files with 11 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/sfm/ShonanAveraging.cpp
View File

@ -343,10 +343,8 @@ 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 " throw std::runtime_error(
"function" "Verification of optimality does not work with robust cost function");
<< std::endl;
sigma = 1; // setting arbitrary value
} 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 "
@ -804,8 +802,10 @@ std::pair<Values, double> ShonanAveraging<d>::run(const Values &initialEstimate,
// 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) {
std::cout << "When using robust norm, Shonan only tests a single rank" << std::endl; throw std::runtime_error(
"When using robust norm, Shonan only tests a single rank");
}
const Values SO3Values = roundSolution(Qstar); const Values SO3Values = roundSolution(Qstar);
return std::make_pair(SO3Values, 0); return std::make_pair(SO3Values, 0);
} }
@ -876,9 +876,11 @@ static BinaryMeasurement<Rot3> convert(
"parseMeasurements<Rot3> can only convert Pose3 measurements " "parseMeasurements<Rot3> can only convert Pose3 measurements "
"with Gaussian noise models."); "with Gaussian noise models.");
const Matrix6 M = gaussian->covariance(); const Matrix6 M = gaussian->covariance();
return BinaryMeasurement<Rot3>( auto model = noiseModel::Robust::Create(
f->key1(), f->key2(), f->measured().rotation(), noiseModel::mEstimator::Huber::Create(1.345),
noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true)); noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3)));
return BinaryMeasurement<Rot3>(f->key1(), f->key2(), f->measured().rotation(),
model);
} }
static ShonanAveraging3::Measurements extractRot3Measurements( static ShonanAveraging3::Measurements extractRot3Measurements(