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

Merge branch 'borglab:develop' into patch-4

release/4.3a0
roderick-koehle 2023-04-18 12:17:51 +02:00 committed by GitHub
parent db6792c894 98d36323c5
commit 5bf11de6cf
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
17 changed files with 65 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.clang-format Normal file
View File

@ -0,0 +1,8 @@
BasedOnStyle: Google
BinPackArguments: false
BinPackParameters: false
ColumnLimit: 100
DerivePointerAlignment: false
IncludeBlocks: Preserve
PointerAlignment: Left

2
cmake/GtsamBuildTypes.cmake
View File

@ -150,7 +150,7 @@ if (NOT CMAKE_VERSION VERSION_LESS 3.8)
set(CMAKE_CXX_EXTENSIONS OFF)
if (MSVC)
# NOTE(jlblanco): seems to be required in addition to the cxx_std_17 above?
list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC /std:c++latest)
list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC /std:c++17)
endif()
else()
# Old cmake versions:

3
gtsam/base/std_optional_serialization.h
View File

@ -76,8 +76,7 @@ void save(Archive& ar, const std::optional<T>& t, const unsigned int /*version*/
}
template <class Archive, class T>
void load(Archive& ar, std::optional<T>& t, const unsigned int /*version*/
) {
void load(Archive& ar, std::optional<T>& t, const unsigned int /*version*/) {
bool tflag;
ar >> boost::serialization::make_nvp("initialized", tflag);
if (!tflag) {

7
gtsam/base/timing.cpp
View File

@ -272,20 +272,21 @@ void tic(size_t id, const char *labelC) {
}
/* ************************************************************************* */
void toc(size_t id, const char *label) {
void toc(size_t id, const char *labelC) {
// disable anything which refers to TimingOutline as well, for good measure
#ifdef GTSAM_USE_BOOST_FEATURES
const std::string label(labelC);
std::shared_ptr<TimingOutline> current(gCurrentTimer.lock());
if (id != current->id_) {
gTimingRoot->print();
throw std::invalid_argument(
"gtsam timing: Mismatched tic/toc: gttoc(\"" + std::string(label) +
"gtsam timing: Mismatched tic/toc: gttoc(\"" + label +
"\") called when last tic was \"" + current->label_ + "\".");
}
if (!current->parent_.lock()) {
gTimingRoot->print();
throw std::invalid_argument(
"gtsam timing: Mismatched tic/toc: extra gttoc(\"" + std::string(label) +
"gtsam timing: Mismatched tic/toc: extra gttoc(\"" + label +
"\"), already at the root");
}
current->toc();

5
gtsam/discrete/DecisionTreeFactor.cpp
View File

@ -94,7 +94,10 @@ namespace gtsam {
for (Key j : f.keys()) cs[j] = f.cardinality(j);
// Convert map into keys
DiscreteKeys keys;
for (const std::pair<const Key, size_t>& key : cs) keys.push_back(key);
keys.reserve(cs.size());
for (const auto& key : cs) {
keys.emplace_back(key);
}
// apply operand
ADT result = ADT::apply(f, op);
// Make a new factor

4
gtsam/geometry/Line3.cpp
View File

@ -111,8 +111,8 @@ Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
}
if (Dline) {
Dline->setIdentity();
(*Dline)(0, 3) = -t[2];
(*Dline)(1, 2) = t[2];
(*Dline)(3, 0) = -t[2];
(*Dline)(2, 1) = t[2];
}
return Line3(cRl, c_ab[0], c_ab[1]);
}

4
gtsam/geometry/tests/testLine3.cpp
View File

@ -123,10 +123,10 @@ TEST(Line3, localCoordinatesOfRetract) {
// transform from world to camera test
TEST(Line3, transformToExpressionJacobians) {
Rot3 r = Rot3::Expmap(Vector3(0, M_PI / 3, 0));
Vector3 t(0, 0, 0);
Vector3 t(-2.0, 2.0, 3.0);
Pose3 p(r, t);
Line3 l_c(r.inverse(), 1, 1);
Line3 l_c(r.inverse(), 3, -1);
Line3 l_w(Rot3(), 1, 1);
EXPECT(l_c.equals(transformTo(p, l_w)));

4
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -248,7 +248,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
#ifdef HYBRID_TIMING
tictoc_print_();
tictoc_reset_();
#endif
// Separate out decision tree into conditionals and remaining factors.
@ -416,9 +415,6 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
return continuousElimination(factors, frontalKeys);
} else {
// Case 3: We are now in the hybrid land!
#ifdef HYBRID_TIMING
tictoc_reset_();
#endif
return hybridElimination(factors, frontalKeys, continuousSeparator,
discreteSeparatorSet);
}

12
gtsam/hybrid/HybridSmoother.cpp
View File

@ -57,8 +57,16 @@ Ordering HybridSmoother::getOrdering(
/* ************************************************************************* */
void HybridSmoother::update(HybridGaussianFactorGraph graph,
const Ordering &ordering,
std::optional<size_t> maxNrLeaves) {
std::optional<size_t> maxNrLeaves,
const std::optional<Ordering> given_ordering) {
Ordering ordering;
// If no ordering provided, then we compute one
if (!given_ordering.has_value()) {
ordering = this->getOrdering(graph);
} else {
ordering = *given_ordering;
}
// Add the necessary conditionals from the previous timestep(s).
std::tie(graph, hybridBayesNet_) =
addConditionals(graph, hybridBayesNet_, ordering);

11
gtsam/hybrid/HybridSmoother.h
View File

@ -44,13 +44,14 @@ class HybridSmoother {
* corresponding to the pruned choices.
*
* @param graph The new factors, should be linear only
* @param ordering The ordering for elimination, only continuous vars are
* allowed
* @param maxNrLeaves The maximum number of leaves in the new discrete factor,
* if applicable
* @param given_ordering The (optional) ordering for elimination, only
* continuous variables are allowed
*/
void update(HybridGaussianFactorGraph graph, const Ordering& ordering,
std::optional<size_t> maxNrLeaves = {});
void update(HybridGaussianFactorGraph graph,
std::optional<size_t> maxNrLeaves = {},
const std::optional<Ordering> given_ordering = {});
Ordering getOrdering(const HybridGaussianFactorGraph& newFactors);
@ -74,4 +75,4 @@ class HybridSmoother {
const HybridBayesNet& hybridBayesNet() const;
};
}; // namespace gtsam
} // namespace gtsam

37
gtsam/hybrid/tests/testHybridEstimation.cpp
View File

@ -46,35 +46,6 @@ using namespace gtsam;
using symbol_shorthand::X;
using symbol_shorthand::Z;
Ordering getOrdering(HybridGaussianFactorGraph& factors,
const HybridGaussianFactorGraph& newFactors) {
factors.push_back(newFactors);
// 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)) {
newKeysDiscreteLast.push_back(k);
}
}
// Insert discrete keys at the end
std::copy(allDiscrete.begin(), allDiscrete.end(),
std::back_inserter(newKeysDiscreteLast));
const VariableIndex index(factors);
// Get an ordering where the new keys are eliminated last
Ordering ordering = Ordering::ColamdConstrainedLast(
index, KeyVector(newKeysDiscreteLast.begin(), newKeysDiscreteLast.end()),
true);
return ordering;
}
TEST(HybridEstimation, Full) {
size_t K = 6;
std::vector<double> measurements = {0, 1, 2, 2, 2, 3};
@ -117,7 +88,7 @@ TEST(HybridEstimation, Full) {
/****************************************************************************/
// Test approximate inference with an additional pruning step.
TEST(HybridEstimation, Incremental) {
TEST(HybridEstimation, IncrementalSmoother) {
size_t K = 15;
std::vector<double> measurements = {0, 1, 2, 2, 2, 2, 3, 4, 5, 6, 6,
7, 8, 9, 9, 9, 10, 11, 11, 11, 11};
@ -136,7 +107,6 @@ TEST(HybridEstimation, Incremental) {
initial.insert(X(0), switching.linearizationPoint.at<double>(X(0)));
HybridGaussianFactorGraph linearized;
HybridGaussianFactorGraph bayesNet;
for (size_t k = 1; k < K; k++) {
// Motion Model
@ -146,11 +116,10 @@ TEST(HybridEstimation, Incremental) {
initial.insert(X(k), switching.linearizationPoint.at<double>(X(k)));
bayesNet = smoother.hybridBayesNet();
linearized = *graph.linearize(initial);
Ordering ordering = getOrdering(bayesNet, linearized);
Ordering ordering = smoother.getOrdering(linearized);
smoother.update(linearized, ordering, 3);
smoother.update(linearized, 3, ordering);
graph.resize(0);
}

21
gtsam/linear/VectorValues.cpp
View File

@ -41,7 +41,7 @@ namespace gtsam {
/* ************************************************************************ */
VectorValues::VectorValues(const Vector& x, const Dims& dims) {
size_t j = 0;
for (const auto& [key,n] : dims) {
for (const auto& [key, n] : dims) {
#ifdef TBB_GREATER_EQUAL_2020
values_.emplace(key, x.segment(j, n));
#else
@ -68,7 +68,7 @@ namespace gtsam {
VectorValues VectorValues::Zero(const VectorValues& other)
{
VectorValues result;
for(const auto& [key,value]: other)
for (const auto& [key, value] : other)
#ifdef TBB_GREATER_EQUAL_2020
result.values_.emplace(key, Vector::Zero(value.size()));
#else
@ -79,7 +79,7 @@ namespace gtsam {
/* ************************************************************************ */
VectorValues::iterator VectorValues::insert(const std::pair<Key, Vector>& key_value) {
std::pair<iterator, bool> result = values_.insert(key_value);
const std::pair<iterator, bool> result = values_.insert(key_value);
if(!result.second)
throw std::invalid_argument(
"Requested to insert variable '" + DefaultKeyFormatter(key_value.first)
@ -90,7 +90,7 @@ namespace gtsam {
/* ************************************************************************ */
VectorValues& VectorValues::update(const VectorValues& values) {
iterator hint = begin();
for (const auto& [key,value] : values) {
for (const auto& [key, value] : values) {
// Use this trick to find the value using a hint, since we are inserting
// from another sorted map
size_t oldSize = values_.size();
@ -131,10 +131,10 @@ namespace gtsam {
// Change print depending on whether we are using TBB
#ifdef GTSAM_USE_TBB
std::map<Key, Vector> sorted;
for (const auto& [key,value] : v) {
for (const auto& [key, value] : v) {
sorted.emplace(key, value);
}
for (const auto& [key,value] : sorted)
for (const auto& [key, value] : sorted)
#else
for (const auto& [key,value] : v)
#endif
@ -344,14 +344,13 @@ namespace gtsam {
}
/* ************************************************************************ */
VectorValues operator*(const double a, const VectorValues &v)
{
VectorValues operator*(const double a, const VectorValues& c) {
VectorValues result;
for(const VectorValues::KeyValuePair& key_v: v)
for (const auto& [key, value] : c)
#ifdef TBB_GREATER_EQUAL_2020
result.values_.emplace(key_v.first, a * key_v.second);
result.values_.emplace(key, a * value);
#else
result.values_.insert({key_v.first, a * key_v.second});
result.values_.insert({key, a * value});
#endif
return result;
}

2
gtsam/navigation/ConstantVelocityFactor.h
View File

@ -38,7 +38,7 @@ class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
public:
ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model)
: NoiseModelFactorN<NavState, NavState>(model, i, j), dt_(dt) {}
~ConstantVelocityFactor() override{};
~ConstantVelocityFactor() override {}
/**
* @brief Caclulate error: (x2 - x1.update(dt)))

8
gtsam/navigation/ManifoldPreintegration.cpp
View File

@ -67,9 +67,11 @@ void ManifoldPreintegration::update(const Vector3& measuredAcc,
// Possibly correct for sensor pose
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
if (p().body_P_sensor)
std::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
if (p().body_P_sensor) {
std::tie(acc, omega) = correctMeasurementsBySensorPose(
acc, omega, D_correctedAcc_acc, D_correctedAcc_omega,
D_correctedOmega_omega);
}
// Save current rotation for updating Jacobians
const Rot3 oldRij = deltaXij_.attitude();

2
gtsam/navigation/ManifoldPreintegration.h
View File

@ -27,7 +27,7 @@
namespace gtsam {
/**
* IMU pre-integration on NavSatet manifold.
* IMU pre-integration on NavState manifold.
* This corresponds to the original RSS paper (with one difference: V is rotated)
*/
class GTSAM_EXPORT ManifoldPreintegration : public PreintegrationBase {

8
gtsam/navigation/TangentPreintegration.cpp
View File

@ -111,9 +111,11 @@ void TangentPreintegration::update(const Vector3& measuredAcc,
// Possibly correct for sensor pose by converting to body frame
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
if (p().body_P_sensor)
std::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
if (p().body_P_sensor) {
std::tie(acc, omega) = correctMeasurementsBySensorPose(
acc, omega, D_correctedAcc_acc, D_correctedAcc_omega,
D_correctedOmega_omega);
}
// Do update
deltaTij_ += dt;

4
python/CMakeLists.txt
View File

@ -198,9 +198,9 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
"${GTSAM_UNSTABLE_MODULE_PATH}")
# Hack to get python test files copied every time they are modified
file(GLOB GTSAM_UNSTABLE_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/tests/*.py")
file(GLOB GTSAM_UNSTABLE_PYTHON_TEST_FILES RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/" "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/tests/*.py")
foreach(test_file ${GTSAM_UNSTABLE_PYTHON_TEST_FILES})
configure_file(${test_file} "${GTSAM_UNSTABLE_MODULE_PATH}/tests/${test_file}" COPYONLY)
configure_file("${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/${test_file}" "${GTSAM_UNSTABLE_MODULE_PATH}/${test_file}" COPYONLY)
endforeach()
# Add gtsam_unstable to the install target