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 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;
}

2
gtsam/hybrid/HybridNonlinearFactorGraph.h
View File

@ -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;
/// @}
};

11
gtsam/hybrid/MixtureFactor.h
View File

@ -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());
}
/**

2
gtsam/linear/GaussianConditional.cpp
View File

@ -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: ");