addressed final comments by Frank
parent
24672385b3
commit
248eec8e41
|
@ -57,39 +57,25 @@ class GncOptimizer {
|
||||||
if (graph[i]) {
|
if (graph[i]) {
|
||||||
NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast<
|
NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast<
|
||||||
NoiseModelFactor>(graph[i]);
|
NoiseModelFactor>(graph[i]);
|
||||||
noiseModel::Robust::shared_ptr robust = boost::dynamic_pointer_cast<
|
auto robust = boost::dynamic_pointer_cast<
|
||||||
noiseModel::Robust>(factor->noiseModel());
|
noiseModel::Robust>(factor->noiseModel());
|
||||||
if (robust) { // if the factor has a robust loss, we have to change it:
|
// if the factor has a robust loss, we remove the robust loss
|
||||||
SharedNoiseModel gaussianNoise = robust->noise();
|
nfg_[i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : factor;
|
||||||
NoiseModelFactor::shared_ptr gaussianFactor = factor
|
|
||||||
->cloneWithNewNoiseModel(gaussianNoise);
|
|
||||||
nfg_[i] = gaussianFactor;
|
|
||||||
} else { // else we directly push it back
|
|
||||||
nfg_[i] = factor;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Access a copy of the internal factor graph.
|
/// Access a copy of the internal factor graph.
|
||||||
NonlinearFactorGraph getFactors() const {
|
const NonlinearFactorGraph& getFactors() const { return nfg_; }
|
||||||
return NonlinearFactorGraph(nfg_);
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Access a copy of the internal values.
|
/// Access a copy of the internal values.
|
||||||
Values getState() const {
|
const Values& getState() const { return state_; }
|
||||||
return Values(state_);
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Access a copy of the parameters.
|
/// Access a copy of the parameters.
|
||||||
GncParameters getParams() const {
|
const GncParameters& getParams() const { return params_;}
|
||||||
return GncParameters(params_);
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Access a copy of the GNC weights.
|
/// Access a copy of the GNC weights.
|
||||||
Vector getWeights() const {
|
const Vector& getWeights() const { return weights_;}
|
||||||
return weights_;
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Compute optimal solution using graduated non-convexity.
|
/// Compute optimal solution using graduated non-convexity.
|
||||||
Values optimize() {
|
Values optimize() {
|
||||||
|
@ -279,15 +265,14 @@ class GncOptimizer {
|
||||||
newGraph.resize(nfg_.size());
|
newGraph.resize(nfg_.size());
|
||||||
for (size_t i = 0; i < nfg_.size(); i++) {
|
for (size_t i = 0; i < nfg_.size(); i++) {
|
||||||
if (nfg_[i]) {
|
if (nfg_[i]) {
|
||||||
NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast<
|
auto factor = boost::dynamic_pointer_cast<
|
||||||
NoiseModelFactor>(nfg_[i]);
|
NoiseModelFactor>(nfg_[i]);
|
||||||
noiseModel::Gaussian::shared_ptr noiseModel =
|
auto noiseModel =
|
||||||
boost::dynamic_pointer_cast<noiseModel::Gaussian>(
|
boost::dynamic_pointer_cast<noiseModel::Gaussian>(
|
||||||
factor->noiseModel());
|
factor->noiseModel());
|
||||||
if (noiseModel) {
|
if (noiseModel) {
|
||||||
Matrix newInfo = weights[i] * noiseModel->information();
|
Matrix newInfo = weights[i] * noiseModel->information();
|
||||||
SharedNoiseModel newNoiseModel = noiseModel::Gaussian::Information(
|
auto newNoiseModel = noiseModel::Gaussian::Information(newInfo);
|
||||||
newInfo);
|
|
||||||
newGraph[i] = factor->cloneWithNewNoiseModel(newNoiseModel);
|
newGraph[i] = factor->cloneWithNewNoiseModel(newNoiseModel);
|
||||||
} else {
|
} else {
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
|
|
|
@ -77,6 +77,7 @@ class GncParams {
|
||||||
void setLossType(const GncLossType type) {
|
void setLossType(const GncLossType type) {
|
||||||
lossType = type;
|
lossType = type;
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Set the maximum number of iterations in GNC (changing the max nr of iters might lead to less accurate solutions and is not recommended).
|
/// Set the maximum number of iterations in GNC (changing the max nr of iters might lead to less accurate solutions and is not recommended).
|
||||||
void setMaxIterations(const size_t maxIter) {
|
void setMaxIterations(const size_t maxIter) {
|
||||||
std::cout
|
std::cout
|
||||||
|
@ -84,6 +85,7 @@ class GncParams {
|
||||||
<< std::endl;
|
<< std::endl;
|
||||||
maxIterations = maxIter;
|
maxIterations = maxIter;
|
||||||
}
|
}
|
||||||
|
|
||||||
/** Set the maximum weighted residual error for an inlier. For a factor in the form f(x) = 0.5 * || r(x) ||^2_Omega,
|
/** Set the maximum weighted residual error for an inlier. For a factor in the form f(x) = 0.5 * || r(x) ||^2_Omega,
|
||||||
* the inlier threshold is the largest value of f(x) for the corresponding measurement to be considered an inlier.
|
* the inlier threshold is the largest value of f(x) for the corresponding measurement to be considered an inlier.
|
||||||
* In other words, an inlier at x is such that 0.5 * || r(x) ||^2_Omega <= barcSq.
|
* In other words, an inlier at x is such that 0.5 * || r(x) ||^2_Omega <= barcSq.
|
||||||
|
@ -93,22 +95,27 @@ class GncParams {
|
||||||
void setInlierCostThreshold(const double inth) {
|
void setInlierCostThreshold(const double inth) {
|
||||||
barcSq = inth;
|
barcSq = inth;
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Set the graduated non-convexity step: at each GNC iteration, mu is updated as mu <- mu * muStep.
|
/// Set the graduated non-convexity step: at each GNC iteration, mu is updated as mu <- mu * muStep.
|
||||||
void setMuStep(const double step) {
|
void setMuStep(const double step) {
|
||||||
muStep = step;
|
muStep = step;
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Set the maximum relative difference in mu values to stop iterating.
|
/// Set the maximum relative difference in mu values to stop iterating.
|
||||||
void setRelativeCostTol(double value) {
|
void setRelativeCostTol(double value) {
|
||||||
relativeCostTol = value;
|
relativeCostTol = value;
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Set the maximum difference between the weights and their rounding in {0,1} to stop iterating.
|
/// Set the maximum difference between the weights and their rounding in {0,1} to stop iterating.
|
||||||
void setWeightsTol(double value) {
|
void setWeightsTol(double value) {
|
||||||
weightsTol = value;
|
weightsTol = value;
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Set the verbosity level.
|
/// Set the verbosity level.
|
||||||
void setVerbosityGNC(const Verbosity value) {
|
void setVerbosityGNC(const Verbosity value) {
|
||||||
verbosity = value;
|
verbosity = value;
|
||||||
}
|
}
|
||||||
|
|
||||||
/** (Optional) Provide a vector of measurements that must be considered inliers. The enties in the vector
|
/** (Optional) Provide a vector of measurements that must be considered inliers. The enties in the vector
|
||||||
* corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg,
|
* corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg,
|
||||||
* and you provide knownIn = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15].
|
* and you provide knownIn = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15].
|
||||||
|
@ -119,6 +126,7 @@ class GncParams {
|
||||||
for (size_t i = 0; i < knownIn.size(); i++)
|
for (size_t i = 0; i < knownIn.size(); i++)
|
||||||
knownInliers.push_back(knownIn[i]);
|
knownInliers.push_back(knownIn[i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Equals.
|
/// Equals.
|
||||||
bool equals(const GncParams& other, double tol = 1e-9) const {
|
bool equals(const GncParams& other, double tol = 1e-9) const {
|
||||||
return baseOptimizerParams.equals(other.baseOptimizerParams)
|
return baseOptimizerParams.equals(other.baseOptimizerParams)
|
||||||
|
@ -127,6 +135,7 @@ class GncParams {
|
||||||
&& std::fabs(muStep - other.muStep) <= tol
|
&& std::fabs(muStep - other.muStep) <= tol
|
||||||
&& verbosity == other.verbosity && knownInliers == other.knownInliers;
|
&& verbosity == other.verbosity && knownInliers == other.knownInliers;
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Print.
|
/// Print.
|
||||||
void print(const std::string& str) const {
|
void print(const std::string& str) const {
|
||||||
std::cout << str << "\n";
|
std::cout << str << "\n";
|
||||||
|
|
Loading…
Reference in New Issue