53 lines
1.8 KiB
C++
53 lines
1.8 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file HybridFactor.cpp
|
|
* @date Mar 11, 2022
|
|
* @author Fan Jiang
|
|
*/
|
|
|
|
#include <gtsam/hybrid/HybridFactor.h>
|
|
|
|
namespace gtsam {
|
|
|
|
KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) {
|
|
KeyVector allKeys;
|
|
std::copy(continuousKeys.begin(), continuousKeys.end(), std::back_inserter(allKeys));
|
|
std::transform(discreteKeys.begin(),
|
|
discreteKeys.end(),
|
|
std::back_inserter(allKeys),
|
|
[](const DiscreteKey &k) { return k.first; });
|
|
return allKeys;
|
|
}
|
|
|
|
KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2) {
|
|
KeyVector allKeys;
|
|
std::copy(keys1.begin(), keys1.end(), std::back_inserter(allKeys));
|
|
std::copy(keys2.begin(), keys2.end(), std::back_inserter(allKeys));
|
|
return allKeys;
|
|
}
|
|
|
|
HybridFactor::HybridFactor() = default;
|
|
|
|
HybridFactor::HybridFactor(const KeyVector &keys) : Base(keys), isContinuous_(true) {}
|
|
|
|
HybridFactor::HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys)
|
|
: Base(
|
|
CollectKeys(continuousKeys, discreteKeys)), isHybrid_(true), discreteKeys_(discreteKeys) {}
|
|
|
|
HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) : Base(CollectKeys({}, discreteKeys)),
|
|
isDiscrete_(true),
|
|
discreteKeys_(discreteKeys) {}
|
|
|
|
HybridFactor::~HybridFactor() = default;
|
|
|
|
} |