Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Some Hybrid Improvements #1387

Merged
merged 6 commits into from
Jan 17, 2023
Merged

Some Hybrid Improvements #1387

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
abe9b28
return DiscreteKeys as a vector instead of a set
varunagrawal Jan 12, 2023
b010a24
STL-based efficient transformation
varunagrawal Jan 15, 2023
35d560f
implement MixtureFactor::error
varunagrawal Jan 15, 2023
86bfdf7
add formatter to GaussianConditional mean printing
varunagrawal Jan 15, 2023
64743ef
switch to boost::shared_ptr
varunagrawal Jan 16, 2023
5e1de8c
switch from DiscreteKeys back to std::set<DiscreteKey>
varunagrawal Jan 16, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 4 additions & 3 deletions gtsam/hybrid/HybridFactorGraph.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down
2 changes: 1 addition & 1 deletion gtsam/hybrid/HybridNonlinearFactorGraph.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
/// @}
};
Expand Down
11 changes: 8 additions & 3 deletions gtsam/hybrid/MixtureFactor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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());
}

/**
Expand Down
2 changes: 1 addition & 1 deletion gtsam/linear/GaussianConditional.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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: ");
Expand Down