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

Merge pull request #1387 from borglab/discreteKeys-vector 

Some Hybrid Improvements
release/4.3a0
Varun Agrawal 2023-01-17 04:00:41 -05:00 committed by GitHub
parent 618ac28f2c 5e1de8c062
commit 7cd360039f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 14 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/hybrid/HybridFactorGraph.cpp
View File

@ -45,9 +45,10 @@ std::set<DiscreteKey> HybridFactorGraph::discreteKeys() const {
/* ************************************************************************* */ /* ************************************************************************* */
KeySet HybridFactorGraph::discreteKeySet() const { KeySet HybridFactorGraph::discreteKeySet() const {
KeySet keys; KeySet keys;
for (const DiscreteKey& k : discreteKeys()) { std::set<DiscreteKey> key_set = discreteKeys();
keys.insert(k.first); std::transform(key_set.begin(), key_set.end(),
} std::inserter(keys, keys.begin()),
[](const DiscreteKey& k) { return k.first; });
return keys; return keys;
} }

2
gtsam/hybrid/HybridNonlinearFactorGraph.h
View File

@ -74,7 +74,7 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph {
* @param continuousValues: Dictionary of continuous values. * @param continuousValues: Dictionary of continuous values.
* @return HybridGaussianFactorGraph::shared_ptr * @return HybridGaussianFactorGraph::shared_ptr
*/ */
HybridGaussianFactorGraph::shared_ptr linearize( boost::shared_ptr<HybridGaussianFactorGraph> linearize(
const Values& continuousValues) const; const Values& continuousValues) const;
/// @} /// @}
}; };

11
gtsam/hybrid/MixtureFactor.h
View File

@ -21,6 +21,7 @@
#include <gtsam/discrete/DiscreteValues.h> #include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h> #include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/nonlinear/Symbol.h>
@ -160,10 +161,14 @@ class MixtureFactor : public HybridFactor {
factor, continuousValues); 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 { double error(const HybridValues& values) const override {
throw std::runtime_error( return error(values.nonlinear(), values.discrete());
"MixtureFactor::error(HybridValues) not implemented.");
} }
/** /**

2
gtsam/linear/GaussianConditional.cpp
View File

@ -124,7 +124,7 @@ namespace gtsam {
cout << formatMatrixIndented(" d = ", getb(), true) << "\n"; cout << formatMatrixIndented(" d = ", getb(), true) << "\n";
if (nrParents() == 0) { if (nrParents() == 0) {
const auto mean = solve({}); // solve for mean. const auto mean = solve({}); // solve for mean.
mean.print(" mean"); mean.print(" mean", formatter);
} }
if (model_) if (model_)
model_->print(" Noise model: "); model_->print(" Noise model: ");