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

Merge branch 'hybrid-smoother' into city10000

release/4.3a0
Varun Agrawal 2025-01-23 21:27:30 -05:00
parent 17836314ab 8ad3cb6ba1
commit 0796ee293e
9 changed files with 84 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
examples/TriangulationLOSTExample.cpp
View File

@ -123,9 +123,9 @@ int main(int argc, char* argv[]) {
double rank_tol = 1e-9;
std::shared_ptr<Cal3_S2> calib = std::make_shared<Cal3_S2>();
std::chrono::nanoseconds durationDLT;
std::chrono::nanoseconds durationDLTOpt;
std::chrono::nanoseconds durationLOST;
std::chrono::nanoseconds durationDLT{};
std::chrono::nanoseconds durationDLTOpt{};
std::chrono::nanoseconds durationLOST{};
for (int i = 0; i < nrTrials; i++) {
Point2Vector noisyMeasurements =

4
gtsam/basis/tests/testFourier.cpp
View File

@ -22,6 +22,10 @@
#include <gtsam/basis/Fourier.h>
#include <gtsam/nonlinear/factorTesting.h>
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic warning "-Wstringop-overread"
#pragma GCC diagnostic warning "-Warray-bounds"
#endif
using namespace std;
using namespace gtsam;

4
gtsam/geometry/BearingRange.h
View File

@ -138,8 +138,10 @@ public:
TangentVector localCoordinates(const BearingRange& other) const {
typename traits<B>::TangentVector v1 = traits<B>::Local(bearing_, other.bearing_);
typename traits<R>::TangentVector v2 = traits<R>::Local(range_, other.range_);
// Set the first dimB elements to v1, and the next dimR elements to v2
TangentVector v;
v << v1, v2;
v.template head<dimB>() = v1;
v.template tail<dimR>() = v2;
return v;
}

4
gtsam/geometry/FundamentalMatrix.cpp
View File

@ -9,6 +9,10 @@
#include <gtsam/geometry/FundamentalMatrix.h>
#include <gtsam/geometry/Point2.h>
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif
namespace gtsam {
//*************************************************************************

59
gtsam/hybrid/HybridSmoother.cpp
View File

@ -24,17 +24,14 @@
namespace gtsam {
/* ************************************************************************* */
Ordering HybridSmoother::getOrdering(
const HybridGaussianFactorGraph &newFactors) {
HybridGaussianFactorGraph factors(hybridBayesNet());
factors.push_back(newFactors);
Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors,
const KeySet &newFactorKeys) {
// Get all the discrete keys from the factors
KeySet allDiscrete = factors.discreteKeySet();
// Create KeyVector with continuous keys followed by discrete keys.
KeyVector newKeysDiscreteLast;
const KeySet newFactorKeys = newFactors.keys();
// Insert continuous keys first.
for (auto &k : newFactorKeys) {
if (!allDiscrete.exists(k)) {
@ -56,23 +53,29 @@ Ordering HybridSmoother::getOrdering(
}
/* ************************************************************************* */
void HybridSmoother::update(HybridGaussianFactorGraph graph,
void HybridSmoother::update(const HybridGaussianFactorGraph &graph,
std::optional<size_t> maxNrLeaves,
const std::optional<Ordering> given_ordering) {
HybridGaussianFactorGraph updatedGraph;
// Add the necessary conditionals from the previous timestep(s).
std::tie(updatedGraph, hybridBayesNet_) =
addConditionals(graph, hybridBayesNet_);
Ordering ordering;
// If no ordering provided, then we compute one
if (!given_ordering.has_value()) {
ordering = this->getOrdering(graph);
// Get the keys from the new factors
const KeySet newFactorKeys = graph.keys();
// Since updatedGraph now has all the connected conditionals,
// we can get the correct ordering.
ordering = this->getOrdering(updatedGraph, newFactorKeys);
} else {
ordering = *given_ordering;
}
// Add the necessary conditionals from the previous timestep(s).
std::tie(graph, hybridBayesNet_) =
addConditionals(graph, hybridBayesNet_, ordering);
// Eliminate.
HybridBayesNet bayesNetFragment = *graph.eliminateSequential(ordering);
HybridBayesNet bayesNetFragment = *updatedGraph.eliminateSequential(ordering);
/// Prune
if (maxNrLeaves) {
@ -88,10 +91,11 @@ void HybridSmoother::update(HybridGaussianFactorGraph graph,
/* ************************************************************************* */
std::pair<HybridGaussianFactorGraph, HybridBayesNet>
HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph,
const HybridBayesNet &originalHybridBayesNet,
const Ordering &ordering) const {
const HybridBayesNet &hybridBayesNet) const {
HybridGaussianFactorGraph graph(originalGraph);
HybridBayesNet hybridBayesNet(originalHybridBayesNet);
HybridBayesNet updatedHybridBayesNet(hybridBayesNet);
KeySet factorKeys = graph.keys();
// If hybridBayesNet is not empty,
// it means we have conditionals to add to the factor graph.
@ -99,10 +103,6 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph,
// We add all relevant hybrid conditionals on the last continuous variable
// in the previous `hybridBayesNet` to the graph
// Conditionals to remove from the bayes net
// since the conditional will be updated.
std::vector<HybridConditional::shared_ptr> conditionals_to_erase;
// New conditionals to add to the graph
gtsam::HybridBayesNet newConditionals;
@ -112,25 +112,24 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph,
auto conditional = hybridBayesNet.at(i);
for (auto &key : conditional->frontals()) {
if (std::find(ordering.begin(), ordering.end(), key) !=
ordering.end()) {
if (std::find(factorKeys.begin(), factorKeys.end(), key) !=
factorKeys.end()) {
newConditionals.push_back(conditional);
conditionals_to_erase.push_back(conditional);
// Remove the conditional from the updated Bayes net
auto it = find(updatedHybridBayesNet.begin(),
updatedHybridBayesNet.end(), conditional);
updatedHybridBayesNet.erase(it);
break;
}
}
}
// Remove conditionals at the end so we don't affect the order in the
// original bayes net.
for (auto &&conditional : conditionals_to_erase) {
auto it = find(hybridBayesNet.begin(), hybridBayesNet.end(), conditional);
hybridBayesNet.erase(it);
}
graph.push_back(newConditionals);
}
return {graph, hybridBayesNet};
return {graph, updatedHybridBayesNet};
}
/* ************************************************************************* */

30
gtsam/hybrid/HybridSmoother.h
View File

@ -49,11 +49,35 @@ class GTSAM_EXPORT HybridSmoother {
* @param given_ordering The (optional) ordering for elimination, only
* continuous variables are allowed
*/
void update(HybridGaussianFactorGraph graph,
void update(const HybridGaussianFactorGraph& graph,
std::optional<size_t> maxNrLeaves = {},
const std::optional<Ordering> given_ordering = {});
Ordering getOrdering(const HybridGaussianFactorGraph& newFactors);
/**
* @brief Get an elimination ordering which eliminates continuous and then
* discrete.
*
* Expects `newFactors` to already have the necessary conditionals connected
* to the
*
* @param factors
* @return Ordering
*/
/**
* @brief Get an elimination ordering which eliminates continuous
* and then discrete.
*
* Expects `factors` to already have the necessary conditionals
* which were connected to the variables in the newly added factors.
* Those variables should be in `newFactorKeys`.
*
* @param factors All the new factors and connected conditionals.
* @param newFactorKeys The keys/variables in the newly added factors.
* @return Ordering
*/
Ordering getOrdering(const HybridGaussianFactorGraph& factors,
const KeySet& newFactorKeys);
/**
* @brief Add conditionals from previous timestep as part of liquefication.
@ -66,7 +90,7 @@ class GTSAM_EXPORT HybridSmoother {
*/
std::pair<HybridGaussianFactorGraph, HybridBayesNet> addConditionals(
const HybridGaussianFactorGraph& graph,
const HybridBayesNet& hybridBayesNet, const Ordering& ordering) const;
const HybridBayesNet& hybridBayesNet) const;
/**
* @brief Get the hybrid Gaussian conditional from

10
gtsam/hybrid/tests/testHybridSmoother.cpp
View File

@ -95,16 +95,15 @@ TEST(HybridSmoother, IncrementalSmoother) {
initial.insert(X(k), switching.linearizationPoint.at<double>(X(k)));
HybridGaussianFactorGraph linearized = *graph.linearize(initial);
Ordering ordering = smoother.getOrdering(linearized);
smoother.update(linearized, maxNrLeaves, ordering);
smoother.update(linearized, maxNrLeaves);
// Clear all the factors from the graph
graph.resize(0);
}
EXPECT_LONGS_EQUAL(11,
smoother.hybridBayesNet().at(0)->asDiscrete()->nrValues());
smoother.hybridBayesNet().at(3)->asDiscrete()->nrValues());
// Get the continuous delta update as well as
// the optimal discrete assignment.
@ -150,16 +149,15 @@ TEST(HybridSmoother, ValidPruningError) {
initial.insert(X(k), switching.linearizationPoint.at<double>(X(k)));
HybridGaussianFactorGraph linearized = *graph.linearize(initial);
Ordering ordering = smoother.getOrdering(linearized);
smoother.update(linearized, maxNrLeaves, ordering);
smoother.update(linearized, maxNrLeaves);
// Clear all the factors from the graph
graph.resize(0);
}
EXPECT_LONGS_EQUAL(14,
smoother.hybridBayesNet().at(0)->asDiscrete()->nrValues());
smoother.hybridBayesNet().at(6)->asDiscrete()->nrValues());
// Get the continuous delta update as well as
// the optimal discrete assignment.

15
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -38,18 +38,17 @@ static const auto& kWGS84 = Geocentric::WGS84();
// *************************************************************************
namespace example {
// ENU Origin is where the plane was in hold next to runway
const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
static constexpr double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
// Convert from GPS to ENU
LocalCartesian origin_ENU(lat0, lon0, h0, kWGS84);
static const LocalCartesian origin_ENU(lat0, lon0, h0, kWGS84);
// Dekalb-Peachtree Airport runway 2L
const double lat = 33.87071, lon = -84.30482, h = 274;\
static constexpr double lat = 33.87071, lon = -84.30482, h = 274;
// Random lever arm
const Point3 leverArm(0.1, 0.2, 0.3);
}
static const Point3 leverArm(0.1, 0.2, 0.3);
} // namespace example
// *************************************************************************
TEST( GPSFactor, Constructor ) {
@ -135,7 +134,7 @@ TEST( GPSFactorArmCalib, Constructor ) {
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
[&factor, &leverArm](const Pose3& pose_arg) {
[&factor](const Pose3& pose_arg) {
return factor.evaluateError(pose_arg, leverArm);
},
T);
@ -235,7 +234,7 @@ TEST( GPSFactor2ArmCalib, Constructor ) {
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector, NavState>(
[&factor, &leverArm](const NavState& nav_arg) {
[&factor](const NavState& nav_arg) {
return factor.evaluateError(nav_arg, leverArm);
},
T);

3
gtsam/slam/tests/testRotateFactor.cpp
View File

@ -14,6 +14,9 @@
#include <vector>
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif
using namespace std;
using namespace std::placeholders;
using namespace gtsam;