Merge branch 'develop' into hybrid/simplify
commit
f714c4ac82
|
@ -45,9 +45,10 @@ std::set<DiscreteKey> HybridFactorGraph::discreteKeys() const {
|
|||
/* ************************************************************************* */
|
||||
KeySet HybridFactorGraph::discreteKeySet() const {
|
||||
KeySet keys;
|
||||
for (const DiscreteKey& k : discreteKeys()) {
|
||||
keys.insert(k.first);
|
||||
}
|
||||
std::set<DiscreteKey> key_set = discreteKeys();
|
||||
std::transform(key_set.begin(), key_set.end(),
|
||||
std::inserter(keys, keys.begin()),
|
||||
[](const DiscreteKey& k) { return k.first; });
|
||||
return keys;
|
||||
}
|
||||
|
||||
|
|
|
@ -74,7 +74,7 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph {
|
|||
* @param continuousValues: Dictionary of continuous values.
|
||||
* @return HybridGaussianFactorGraph::shared_ptr
|
||||
*/
|
||||
HybridGaussianFactorGraph::shared_ptr linearize(
|
||||
boost::shared_ptr<HybridGaussianFactorGraph> linearize(
|
||||
const Values& continuousValues) const;
|
||||
/// @}
|
||||
};
|
||||
|
|
|
@ -21,6 +21,7 @@
|
|||
|
||||
#include <gtsam/discrete/DiscreteValues.h>
|
||||
#include <gtsam/hybrid/GaussianMixtureFactor.h>
|
||||
#include <gtsam/hybrid/HybridValues.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
|
@ -160,10 +161,14 @@ class MixtureFactor : public HybridFactor {
|
|||
factor, continuousValues);
|
||||
}
|
||||
|
||||
/// Error for HybridValues is not provided for nonlinear hybrid factor.
|
||||
/**
|
||||
* @brief Compute error of factor given hybrid values.
|
||||
*
|
||||
* @param values The continuous Values and the discrete assignment.
|
||||
* @return double The error of this factor.
|
||||
*/
|
||||
double error(const HybridValues& values) const override {
|
||||
throw std::runtime_error(
|
||||
"MixtureFactor::error(HybridValues) not implemented.");
|
||||
return error(values.nonlinear(), values.discrete());
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
@ -124,7 +124,7 @@ namespace gtsam {
|
|||
cout << formatMatrixIndented(" d = ", getb(), true) << "\n";
|
||||
if (nrParents() == 0) {
|
||||
const auto mean = solve({}); // solve for mean.
|
||||
mean.print(" mean");
|
||||
mean.print(" mean", formatter);
|
||||
}
|
||||
if (model_)
|
||||
model_->print(" Noise model: ");
|
||||
|
|
Loading…
Reference in New Issue