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