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

Address comments on PR #1843 

(cherry picked from commit 7e587e4eb396cadf67799e968cb0cf153af9750a)
release/4.3a0
Frank Dellaert 2024-09-24 11:23:50 -07:00
parent 74c40556e3
commit 82c25d887c
2 changed files with 4 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/hybrid/HybridNonlinearFactor.cpp
View File

@ -22,7 +22,7 @@ namespace gtsam {
/* *******************************************************************************/ /* *******************************************************************************/
static void checkKeys(const KeyVector& continuousKeys, static void checkKeys(const KeyVector& continuousKeys,
std::vector<NonlinearFactorValuePair>& pairs) { const std::vector<NonlinearFactorValuePair>& pairs) {
KeySet factor_keys_set; KeySet factor_keys_set;
for (const auto& pair : pairs) { for (const auto& pair : pairs) {
auto f = pair.first; auto f = pair.first;
@ -55,14 +55,9 @@ HybridNonlinearFactor::HybridNonlinearFactor(
/* *******************************************************************************/ /* *******************************************************************************/
HybridNonlinearFactor::HybridNonlinearFactor( HybridNonlinearFactor::HybridNonlinearFactor(
const KeyVector& continuousKeys, const DiscreteKey& discreteKey, const KeyVector& continuousKeys, const DiscreteKey& discreteKey,
const std::vector<NonlinearFactorValuePair>& factors) const std::vector<NonlinearFactorValuePair>& pairs)
: Base(continuousKeys, {discreteKey}) { : Base(continuousKeys, {discreteKey}) {
std::vector<NonlinearFactorValuePair> pairs;
KeySet continuous_keys_set(continuousKeys.begin(), continuousKeys.end()); KeySet continuous_keys_set(continuousKeys.begin(), continuousKeys.end());
KeySet factor_keys_set;
for (auto&& [f, val] : factors) {
pairs.emplace_back(f, val);
}
checkKeys(continuousKeys, pairs); checkKeys(continuousKeys, pairs);
factors_ = FactorValuePairs({discreteKey}, pairs); factors_ = FactorValuePairs({discreteKey}, pairs);
} }

4
gtsam/hybrid/HybridNonlinearFactor.h
View File

@ -106,11 +106,11 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
* *
* @param continuousKeys Vector of keys for continuous factors. * @param continuousKeys Vector of keys for continuous factors.
* @param discreteKey The discrete key for the "mode", indexing components. * @param discreteKey The discrete key for the "mode", indexing components.
* @param factors Vector of gaussian factor-scalar pairs, one per mode. * @param pairs Vector of gaussian factor-scalar pairs, one per mode.
*/ */
HybridNonlinearFactor(const KeyVector& continuousKeys, HybridNonlinearFactor(const KeyVector& continuousKeys,
const DiscreteKey& discreteKey, const DiscreteKey& discreteKey,
const std::vector<NonlinearFactorValuePair>& factors); const std::vector<NonlinearFactorValuePair>& pairs);
/** /**
* @brief Construct a new HybridNonlinearFactor on a several discrete keys M, * @brief Construct a new HybridNonlinearFactor on a several discrete keys M,