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

Helper method to robustify measurements

release/4.3a0
Varun Agrawal 2020-12-03 14:34:18 -05:00
parent b788fb14c0
commit 636d5f4f1c
2 changed files with 29 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
gtsam/sfm/ShonanAveraging.cpp
View File

@ -812,7 +812,7 @@ std::pair<Values, double> ShonanAveraging<d>::run(const Values &initialEstimate,
return std::make_pair(SO3Values, 0); return std::make_pair(SO3Values, 0);
} }
// Check certificate of global optimzality // Check certificate of global optimality
Vector minEigenVector; Vector minEigenVector;
double minEigenValue = computeMinEigenValue(Qstar, &minEigenVector); double minEigenValue = computeMinEigenValue(Qstar, &minEigenVector);
if (minEigenValue > parameters_.optimalityThreshold) { if (minEigenValue > parameters_.optimalityThreshold) {
@ -837,17 +837,13 @@ template class ShonanAveraging<2>;
ShonanAveraging2::ShonanAveraging2(const Measurements &measurements, ShonanAveraging2::ShonanAveraging2(const Measurements &measurements,
const Parameters &parameters) const Parameters &parameters)
: ShonanAveraging<2>(parameters.useHuber : ShonanAveraging<2>(maybeRobust(measurements, parameters.getUseHuber()),
? makeNoiseModelRobust(measurements)
: measurements,
parameters) {} parameters) {}
ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters &parameters) ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters &parameters)
: ShonanAveraging<2>( : ShonanAveraging<2>(maybeRobust(parseMeasurements<Rot2>(g2oFile),
parameters.useHuber parameters.getUseHuber()),
? makeNoiseModelRobust(parseMeasurements<Rot2>(g2oFile)) parameters) {}
: parseMeasurements<Rot2>(g2oFile),
parameters) {}
/* ************************************************************************* */ /* ************************************************************************* */
// Explicit instantiation for d=3 // Explicit instantiation for d=3
@ -855,15 +851,13 @@ template class ShonanAveraging<3>;
ShonanAveraging3::ShonanAveraging3(const Measurements &measurements, ShonanAveraging3::ShonanAveraging3(const Measurements &measurements,
const Parameters &parameters) const Parameters &parameters)
: ShonanAveraging<3>(parameters.useHuber? : ShonanAveraging<3>(maybeRobust(measurements, parameters.getUseHuber()),
makeNoiseModelRobust(measurements) : measurements, parameters) {} parameters) {}
ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters &parameters) ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters &parameters)
: ShonanAveraging<3>( : ShonanAveraging<3>(maybeRobust(parseMeasurements<Rot3>(g2oFile),
parameters.useHuber parameters.getUseHuber()),
? makeNoiseModelRobust(parseMeasurements<Rot3>(g2oFile)) parameters) {}
: parseMeasurements<Rot3>(g2oFile),
parameters) {}
// TODO(frank): Deprecate after we land pybind wrapper // TODO(frank): Deprecate after we land pybind wrapper
@ -895,11 +889,9 @@ static ShonanAveraging3::Measurements extractRot3Measurements(
ShonanAveraging3::ShonanAveraging3(const BetweenFactorPose3s &factors, ShonanAveraging3::ShonanAveraging3(const BetweenFactorPose3s &factors,
const Parameters &parameters) const Parameters &parameters)
: ShonanAveraging<3>( : ShonanAveraging<3>(maybeRobust(extractRot3Measurements(factors),
parameters.useHuber parameters.getUseHuber()),
? makeNoiseModelRobust(extractRot3Measurements(factors)) parameters) {}
: extractRot3Measurements(factors),
parameters) {}
/* ************************************************************************* */ /* ************************************************************************* */
} // namespace gtsam } // namespace gtsam

16
gtsam/sfm/ShonanAveraging.h
View File

@ -389,6 +389,22 @@ class GTSAM_EXPORT ShonanAveraging {
std::pair<Values, double> run(const Values &initialEstimate, size_t pMin = d, std::pair<Values, double> run(const Values &initialEstimate, size_t pMin = d,
size_t pMax = 10) const; size_t pMax = 10) const;
/// @} /// @}
/**
* Helper function to convert measurements to robust noise model
* if flag is set.
*
* @tparam T the type of measurement, e.g. Rot3.
* @param measurements vector of BinaryMeasurements of type T.
* @param useRobustModel flag indicating whether use robust noise model
* instead.
*/
template <typename T>
inline std::vector<BinaryMeasurement<T>> maybeRobust(
const std::vector<BinaryMeasurement<T>> &measurements,
bool useRobustModel = false) const {
return useRobustModel ? makeNoiseModelRobust(measurements) : measurements;
}
}; };
// Subclasses for d=2 and d=3 that explicitly instantiate, as well as provide a // Subclasses for d=2 and d=3 that explicitly instantiate, as well as provide a