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

Merge branch 'develop' into discrete-elimination-refactor

release/4.3a0
Varun Agrawal 2025-01-05 08:51:11 -05:00
parent 90d8486224 e9e52ad21f
commit d3901be1c1
35 changed files with 214 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
cmake/HandleGeneralOptions.cmake
View File

@ -21,12 +21,13 @@ if (NOT MSVC)
endif()
# Configurable Options
option(BUILD_SHARED_LIBS "Build shared libraries" ON)
if(GTSAM_UNSTABLE_AVAILABLE)
option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON)
option(GTSAM_UNSTABLE_BUILD_PYTHON "Enable/Disable Python wrapper for libgtsam_unstable" ON)
option(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX "Enable/Disable MATLAB wrapper for libgtsam_unstable" OFF)
endif()
option(GTSAM_FORCE_SHARED_LIB "Force gtsam to be a shared library, overriding BUILD_SHARED_LIBS" ON)
option(GTSAM_FORCE_SHARED_LIB "Force gtsam to be a shared library, overriding BUILD_SHARED_LIBS" OFF)
option(GTSAM_FORCE_STATIC_LIB "Force gtsam to be a static library, overriding BUILD_SHARED_LIBS" OFF)
option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON)
@ -46,7 +47,9 @@ option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration
option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF)
option(GTSAM_SLOW_BUT_CORRECT_EXPMAP "Use slower but correct expmap for Pose2" OFF)
if (GTSAM_FORCE_SHARED_LIB)
if (GTSAM_FORCE_SHARED_LIB AND GTSAM_FORCE_STATIC_LIB)
message(FATAL_ERROR "GTSAM_FORCE_SHARED_LIB and GTSAM_FORCE_STATIC_LIB are both true. Please, to unambiguously select the desired library type to use to build GTSAM, set one of GTSAM_FORCE_SHARED_LIB=ON, GTSAM_FORCE_STATIC_LIB=ON, or BUILD_SHARED_LIBS={ON/OFF}")
elseif (GTSAM_FORCE_SHARED_LIB)
message(STATUS "GTSAM is a shared library due to GTSAM_FORCE_SHARED_LIB")
set(GTSAM_LIBRARY_TYPE SHARED CACHE STRING "" FORCE)
set(GTSAM_SHARED_LIB 1 CACHE BOOL "" FORCE)
@ -55,10 +58,9 @@ elseif (GTSAM_FORCE_STATIC_LIB)
set(GTSAM_LIBRARY_TYPE STATIC CACHE STRING "" FORCE)
set(GTSAM_SHARED_LIB 0 CACHE BOOL "" FORCE)
elseif (BUILD_SHARED_LIBS)
message(STATUS "GTSAM is a shared library due to BUILD_SHARED_LIBS is ON")
set(GTSAM_LIBRARY_TYPE SHARED CACHE STRING "" FORCE)
set(GTSAM_SHARED_LIB 1 CACHE BOOL "" FORCE)
elseif((DEFINED BUILD_SHARED_LIBS) AND (NOT BUILD_SHARED_LIBS))
elseif(NOT BUILD_SHARED_LIBS)
message(STATUS "GTSAM is a static library due to BUILD_SHARED_LIBS is OFF")
set(GTSAM_LIBRARY_TYPE STATIC CACHE STRING "" FORCE)
set(GTSAM_SHARED_LIB 0 CACHE BOOL "" FORCE)

3
gtsam/3rdparty/metis/libmetis/CMakeLists.txt vendored
View File

@ -13,7 +13,8 @@ if(WIN32)
set_target_properties(metis-gtsam PROPERTIES
PREFIX ""
COMPILE_FLAGS /w
RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin")
RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin"
WINDOWS_EXPORT_ALL_SYMBOLS ON)
endif()
if (APPLE)

2
gtsam/base/ConcurrentMap.h
View File

@ -85,6 +85,8 @@ public:
/** Copy constructor from the base map class */
ConcurrentMap(const Base& x) : Base(x) {}
ConcurrentMap& operator=(const ConcurrentMap& other) = default;
/** Handy 'exists' function */
bool exists(const KEY& e) const { return this->count(e); }

2
gtsam/base/FastList.h
View File

@ -62,6 +62,8 @@ public:
/// Construct from c++11 initializer list:
FastList(std::initializer_list<VALUE> l) : Base(l) {}
FastList& operator=(const FastList& other) = default;
#ifdef GTSAM_ALLOCATOR_BOOSTPOOL
/** Copy constructor from a standard STL container */
FastList(const std::list<VALUE>& x) {

2
gtsam/base/FastMap.h
View File

@ -54,6 +54,8 @@ public:
/** Copy constructor from another FastMap */
FastMap(const FastMap<KEY,VALUE>& x) : Base(x) {}
FastMap& operator=(const FastMap<KEY,VALUE>& x) = default;
/** Copy constructor from the base map class */
FastMap(const Base& x) : Base(x) {}

2
gtsam/base/FastSet.h
View File

@ -80,6 +80,8 @@ public:
Base(x) {
}
FastSet& operator=(const FastSet& other) = default;
#ifdef GTSAM_ALLOCATOR_BOOSTPOOL
/** Copy constructor from a standard STL container */
FastSet(const std::set<VALUE>& x) {

9
gtsam/base/GenericValue.h
View File

@ -56,9 +56,10 @@ public:
GenericValue(){}
/// Construct from value
GenericValue(const T& value) :
value_(value) {
}
GenericValue(const T& value) : Value(),
value_(value) {}
GenericValue(const GenericValue& other) = default;
/// Return a constant value
const T& value() const {
@ -112,7 +113,7 @@ public:
* Clone this value (normal clone on the heap, delete with 'delete' operator)
*/
std::shared_ptr<Value> clone() const override {
return std::allocate_shared<GenericValue>(Eigen::aligned_allocator<GenericValue>(), *this);
return std::allocate_shared<GenericValue>(Eigen::aligned_allocator<GenericValue>(), *this);
}
/// Generic Value interface version of retract

40
gtsam/base/MatrixSerialization.h
View File

@ -24,6 +24,7 @@
#include <gtsam/base/Matrix.h>
#include <Eigen/Sparse>
#include <boost/serialization/array.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_free.hpp>
@ -87,6 +88,45 @@ void serialize(Archive& ar, gtsam::Matrix& m, const unsigned int version) {
split_free(ar, m, version);
}
/******************************************************************************/
/// Customized functions for serializing Eigen::SparseVector
template <class Archive, typename _Scalar, int _Options, typename _Index>
void save(Archive& ar, const Eigen::SparseVector<_Scalar, _Options, _Index>& m,
const unsigned int /*version*/) {
_Index size = m.size();
std::vector<std::pair<Eigen::Index, _Scalar>> data;
for (typename Eigen::SparseVector<_Scalar, _Options, _Index>::InnerIterator
it(m);
it; ++it)
data.push_back({it.index(), it.value()});
ar << BOOST_SERIALIZATION_NVP(size);
ar << BOOST_SERIALIZATION_NVP(data);
}
template <class Archive, typename _Scalar, int _Options, typename _Index>
void load(Archive& ar, Eigen::SparseVector<_Scalar, _Options, _Index>& m,
const unsigned int /*version*/) {
_Index size;
ar >> BOOST_SERIALIZATION_NVP(size);
m.resize(size);
std::vector<std::pair<Eigen::Index, _Scalar>> data;
ar >> BOOST_SERIALIZATION_NVP(data);
for (auto&& d : data) {
m.coeffRef(d.first) = d.second;
}
}
template <class Archive, typename _Scalar, int _Options, typename _Index>
void serialize(Archive& ar, Eigen::SparseVector<_Scalar, _Options, _Index>& m,
const unsigned int version) {
split_free(ar, m, version);
}
/******************************************************************************/
} // namespace serialization
} // namespace boost
#endif

3
gtsam/base/Value.h
View File

@ -38,6 +38,9 @@ namespace gtsam {
*/
class GTSAM_EXPORT Value {
public:
// todo - not sure if valid
Value() = default;
Value(const Value& other) = default;
/** Clone this value in a special memory pool, must be deleted with Value::deallocate_, *not* with the 'delete' operator. */
virtual Value* clone_() const = 0;

2
gtsam/base/concepts.h
View File

@ -8,7 +8,7 @@
#pragma once
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
#include <boost/concept_check.hpp>
#include <boost/concept/assert.hpp>
#include <boost/concept/requires.hpp>

4
gtsam/base/tests/testMatrix.cpp
View File

@ -1151,7 +1151,7 @@ TEST(Matrix, Matrix24IsVectorSpace) {
}
TEST(Matrix, RowMajorIsVectorSpace) {
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
typedef Eigen::Matrix<double, 2, 3, Eigen::RowMajor> RowMajor;
GTSAM_CONCEPT_ASSERT(IsVectorSpace<RowMajor>);
#endif
@ -1166,7 +1166,7 @@ TEST(Matrix, VectorIsVectorSpace) {
}
TEST(Matrix, RowVectorIsVectorSpace) {
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
typedef Eigen::Matrix<double, 1, -1> RowVector;
GTSAM_CONCEPT_ASSERT(IsVectorSpace<RowVector>);
GTSAM_CONCEPT_ASSERT(IsVectorSpace<Vector5>);

2
gtsam/base/tests/testVector.cpp
View File

@ -272,7 +272,7 @@ TEST(Vector, VectorIsVectorSpace) {
}
TEST(Vector, RowVectorIsVectorSpace) {
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
typedef Eigen::Matrix<double,1,-1> RowVector;
GTSAM_CONCEPT_ASSERT(IsVectorSpace<RowVector>);
#endif

22
gtsam/base/timing.cpp
View File

@ -57,7 +57,7 @@ void TimingOutline::add(size_t usecs, size_t usecsWall) {
TimingOutline::TimingOutline(const std::string& label, size_t id) :
id_(id), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_(
0), lastChildOrder_(0), label_(label) {
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
#ifdef GTSAM_USING_NEW_BOOST_TIMERS
timer_.stop();
#endif
@ -66,7 +66,7 @@ TimingOutline::TimingOutline(const std::string& label, size_t id) :
/* ************************************************************************* */
size_t TimingOutline::time() const {
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
size_t time = 0;
bool hasChildren = false;
for(const ChildMap::value_type& child: children_) {
@ -84,7 +84,7 @@ size_t TimingOutline::time() const {
/* ************************************************************************* */
void TimingOutline::print(const std::string& outline) const {
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
std::string formattedLabel = label_;
std::replace(formattedLabel.begin(), formattedLabel.end(), '_', ' ');
std::cout << outline << "-" << formattedLabel << ": " << self() << " CPU ("
@ -108,7 +108,7 @@ void TimingOutline::print(const std::string& outline) const {
void TimingOutline::print2(const std::string& outline,
const double parentTotal) const {
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
const int w1 = 24, w2 = 2, w3 = 6, w4 = 8, precision = 2;
const double selfTotal = self(), selfMean = selfTotal / double(n_);
const double childTotal = secs();
@ -153,7 +153,7 @@ void TimingOutline::print2(const std::string& outline,
/* ************************************************************************* */
const std::shared_ptr<TimingOutline>& TimingOutline::child(size_t child,
const std::string& label, const std::weak_ptr<TimingOutline>& thisPtr) {
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
assert(thisPtr.lock().get() == this);
std::shared_ptr<TimingOutline>& result = children_[child];
if (!result) {
@ -172,7 +172,7 @@ const std::shared_ptr<TimingOutline>& TimingOutline::child(size_t child,
/* ************************************************************************* */
void TimingOutline::tic() {
// Disable this entire function if we are not using boost
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
#ifdef GTSAM_USING_NEW_BOOST_TIMERS
assert(timer_.is_stopped());
timer_.start();
@ -191,7 +191,7 @@ void TimingOutline::tic() {
/* ************************************************************************* */
void TimingOutline::toc() {
// Disable this entire function if we are not using boost
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
#ifdef GTSAM_USING_NEW_BOOST_TIMERS
@ -225,7 +225,7 @@ void TimingOutline::toc() {
/* ************************************************************************* */
void TimingOutline::finishedIteration() {
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
if (tIt_ > tMax_)
tMax_ = tIt_;
if (tMin_ == 0 || tIt_ < tMin_)
@ -240,7 +240,7 @@ void TimingOutline::finishedIteration() {
/* ************************************************************************* */
size_t getTicTocID(const char *descriptionC) {
// disable anything which refers to TimingOutline as well, for good measure
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
const std::string description(descriptionC);
// Global (static) map from strings to ID numbers and current next ID number
static size_t nextId = 0;
@ -263,7 +263,7 @@ size_t getTicTocID(const char *descriptionC) {
/* ************************************************************************* */
void tic(size_t id, const char *labelC) {
// disable anything which refers to TimingOutline as well, for good measure
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
const std::string label(labelC);
std::shared_ptr<TimingOutline> node = //
gCurrentTimer.lock()->child(id, label, gCurrentTimer);
@ -275,7 +275,7 @@ void tic(size_t id, const char *labelC) {
/* ************************************************************************* */
void toc(size_t id, const char *labelC) {
// disable anything which refers to TimingOutline as well, for good measure
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
const std::string label(labelC);
std::shared_ptr<TimingOutline> current(gCurrentTimer.lock());
if (id != current->id_) {

8
gtsam/base/timing.h
View File

@ -21,7 +21,7 @@
#include <gtsam/dllexport.h>
#include <gtsam/config.h> // for GTSAM_USE_TBB
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
#include <boost/version.hpp>
#endif
@ -107,7 +107,7 @@
// have matching gttic/gttoc statments. You may want to consider reorganizing your timing
// outline to match the scope of your code.
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
// Automatically use the new Boost timers if version is recent enough.
#if BOOST_VERSION >= 104800
# ifndef GTSAM_DISABLE_NEW_TIMERS
@ -165,7 +165,7 @@ namespace gtsam {
ChildMap children_; ///< subtrees
// disable all timers if not using boost
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
#ifdef GTSAM_USING_NEW_BOOST_TIMERS
boost::timer::cpu_timer timer_;
#else
@ -183,7 +183,7 @@ namespace gtsam {
GTSAM_EXPORT TimingOutline(const std::string& label, size_t myId);
GTSAM_EXPORT size_t time() const; ///< time taken, including children
double secs() const { return double(time()) / 1000000.0;} ///< time taken, in seconds, including children
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
double self() const { return double(t_) / 1000000.0;} ///< self time only, in seconds
double wall() const { return double(tWall_) / 1000000.0;} ///< wall time, in seconds
double min() const { return double(tMin_) / 1000000.0;} ///< min time, in seconds

20
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -112,13 +112,12 @@ namespace gtsam {
// }
/**
* @brief Multiply all the `factors` and normalize the
* product to prevent underflow.
* @brief Multiply all the `factors`.
*
* @param factors The factors to multiply as a DiscreteFactorGraph.
* @return DecisionTreeFactor
*/
static DecisionTreeFactor ProductAndNormalize(
static DecisionTreeFactor DiscreteProduct(
const DiscreteFactorGraph& factors) {
// PRODUCT: multiply all factors
gttic(product);
@ -126,14 +125,12 @@ namespace gtsam {
gttoc(product);
// Max over all the potentials by pretending all keys are frontal:
auto normalization = product.max(product.size());
auto denominator = product.max(product.size());
// Normalize the product factor to prevent underflow.
auto normalized_product =
product /
(*std::dynamic_pointer_cast<DecisionTreeFactor>(normalization));
product = product / (*denominator);
return normalized_product;
return product;
}
/* ************************************************************************ */
@ -141,7 +138,7 @@ namespace gtsam {
std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr> //
EliminateForMPE(const DiscreteFactorGraph& factors,
const Ordering& frontalKeys) {
DecisionTreeFactor product = ProductAndNormalize(factors);
DecisionTreeFactor product = DiscreteProduct(factors);
// max out frontals, this is the factor on the separator
gttic(max);
@ -210,8 +207,7 @@ namespace gtsam {
return dag.argmax();
}
DiscreteValues DiscreteFactorGraph::optimize(
const Ordering& ordering) const {
DiscreteValues DiscreteFactorGraph::optimize(const Ordering& ordering) const {
gttic(DiscreteFactorGraph_optimize);
DiscreteLookupDAG dag = maxProduct(ordering);
return dag.argmax();
@ -221,7 +217,7 @@ namespace gtsam {
std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr> //
EliminateDiscrete(const DiscreteFactorGraph& factors,
const Ordering& frontalKeys) {
DecisionTreeFactor product = ProductAndNormalize(factors);
DecisionTreeFactor product = DiscreteProduct(factors);
// sum out frontals, this is the factor on the separator
gttic(sum);

4
gtsam/discrete/SignatureParser.cpp
View File

@ -38,7 +38,7 @@ std::optional<Row> static ParseConditional(const std::string& token) {
} catch (...) {
return std::nullopt;
}
return std::move(row);
return row;
}
std::optional<Table> static ParseConditionalTable(
@ -62,7 +62,7 @@ std::optional<Table> static ParseConditionalTable(
}
}
}
return std::move(table);
return table;
}
std::vector<std::string> static Tokenize(const std::string& str) {

61
gtsam/discrete/TableFactor.cpp
View File

@ -87,7 +87,15 @@ static Eigen::SparseVector<double> ComputeSparseTable(
});
sparseTable.reserve(nrValues);
std::set<Key> allKeys(dt.keys().begin(), dt.keys().end());
KeySet allKeys(dt.keys().begin(), dt.keys().end());
// Compute denominators to be used in computing sparse table indices
std::map<Key, size_t> denominators;
double denom = sparseTable.size();
for (const DiscreteKey& dkey : dkeys) {
denom /= dkey.second;
denominators.insert(std::pair<Key, double>(dkey.first, denom));
}
/**
* @brief Functor which is called by the DecisionTree for each leaf.
@ -102,13 +110,13 @@ static Eigen::SparseVector<double> ComputeSparseTable(
auto op = [&](const Assignment<Key>& assignment, double p) {
if (p > 0) {
// Get all the keys involved in this assignment
std::set<Key> assignmentKeys;
KeySet assignmentKeys;
for (auto&& [k, _] : assignment) {
assignmentKeys.insert(k);
}
// Find the keys missing in the assignment
std::vector<Key> diff;
KeyVector diff;
std::set_difference(allKeys.begin(), allKeys.end(),
assignmentKeys.begin(), assignmentKeys.end(),
std::back_inserter(diff));
@ -127,12 +135,10 @@ static Eigen::SparseVector<double> ComputeSparseTable(
// Generate index and add to the sparse vector.
Eigen::Index idx = 0;
size_t previousCardinality = 1;
// We go in reverse since a DecisionTree has the highest label first
for (auto&& it = updatedAssignment.rbegin();
it != updatedAssignment.rend(); it++) {
idx += previousCardinality * it->second;
previousCardinality *= dt.cardinality(it->first);
idx += it->second * denominators.at(it->first);
}
sparseTable.coeffRef(idx) = p;
}
@ -252,41 +258,22 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const {
DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
DiscreteKeys dkeys = discreteKeys();
// Record key assignment and value pairs in pair_table.
// The assignments are stored in descending order of keys so that the order of
// the values matches what is expected by a DecisionTree.
// This is why we reverse the keys and then
// query for the key value/assignment.
DiscreteKeys rdkeys(dkeys.rbegin(), dkeys.rend());
std::vector<std::pair<uint64_t, double>> pair_table;
for (auto i = 0; i < sparse_table_.size(); i++) {
std::stringstream ss;
for (auto&& [key, _] : rdkeys) {
ss << keyValueForIndex(key, i);
// If no keys, then return empty DecisionTreeFactor
if (dkeys.size() == 0) {
AlgebraicDecisionTree<Key> tree;
// We can have an empty sparse_table_ or one with a single value.
if (sparse_table_.size() != 0) {
tree = AlgebraicDecisionTree<Key>(sparse_table_.coeff(0));
}
// k will be in reverse key order already
uint64_t k;
ss >> k;
pair_table.push_back(std::make_pair(k, sparse_table_.coeff(i)));
return DecisionTreeFactor(dkeys, tree);
}
// Sort the pair_table (of assignment-value pairs) based on assignment so we
// get values in reverse key order.
std::sort(
pair_table.begin(), pair_table.end(),
[](const std::pair<uint64_t, double>& a,
const std::pair<uint64_t, double>& b) { return a.first < b.first; });
std::vector<double> table(sparse_table_.size(), 0.0);
for (SparseIt it(sparse_table_); it; ++it) {
table[it.index()] = it.value();
}
// Create the table vector by extracting the values from pair_table.
// The pair_table has already been sorted in the desired order,
// so the values will be in descending key order.
std::vector<double> table;
std::for_each(pair_table.begin(), pair_table.end(),
[&table](const std::pair<uint64_t, double>& pair) {
table.push_back(pair.second);
});
AlgebraicDecisionTree<Key> tree(rdkeys, table);
AlgebraicDecisionTree<Key> tree(dkeys, table);
DecisionTreeFactor f(dkeys, tree);
return f;
}

19
gtsam/discrete/TableFactor.h
View File

@ -32,6 +32,12 @@
#include <utility>
#include <vector>
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <gtsam/base/MatrixSerialization.h>
#include <boost/serialization/nvp.hpp>
#endif
namespace gtsam {
class DiscreteConditional;
@ -360,6 +366,19 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
double error(const HybridValues& values) const override;
/// @}
private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar& BOOST_SERIALIZATION_NVP(sparse_table_);
ar& BOOST_SERIALIZATION_NVP(denominators_);
ar& BOOST_SERIALIZATION_NVP(sorted_dkeys_);
}
#endif
};
// traits

10
gtsam/discrete/tests/testDiscreteFactorGraph.cpp
View File

@ -117,9 +117,9 @@ TEST(DiscreteFactorGraph, test) {
*std::dynamic_pointer_cast<DecisionTreeFactor>(newFactorPtr);
// Normalize newFactor by max for comparison with expected
auto normalization = newFactor.max(newFactor.size());
auto normalizer = newFactor.max(newFactor.size());
newFactor = newFactor / normalization;
newFactor = newFactor / *normalizer;
// Check Conditional
CHECK(conditional);
@ -131,9 +131,9 @@ TEST(DiscreteFactorGraph, test) {
CHECK(&newFactor);
DecisionTreeFactor expectedFactor(B & A, "10 6 6 10");
// Normalize by max.
normalization = expectedFactor.max(expectedFactor.size());
// Ensure normalization is correct.
expectedFactor = expectedFactor / normalization;
normalizer = expectedFactor.max(expectedFactor.size());
// Ensure normalizer is correct.
expectedFactor = expectedFactor / *normalizer;
EXPECT(assert_equal(expectedFactor, newFactor));
// Test using elimination tree

15
gtsam/discrete/tests/testSerializationDiscrete.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteDistribution.h>
#include <gtsam/discrete/TableFactor.h>
#include <gtsam/inference/Symbol.h>
using namespace std;
@ -32,6 +33,7 @@ BOOST_CLASS_EXPORT_GUID(Tree::Leaf, "gtsam_DecisionTreeStringInt_Leaf")
BOOST_CLASS_EXPORT_GUID(Tree::Choice, "gtsam_DecisionTreeStringInt_Choice")
BOOST_CLASS_EXPORT_GUID(DecisionTreeFactor, "gtsam_DecisionTreeFactor");
BOOST_CLASS_EXPORT_GUID(TableFactor, "gtsam_TableFactor");
using ADT = AlgebraicDecisionTree<Key>;
BOOST_CLASS_EXPORT_GUID(ADT, "gtsam_AlgebraicDecisionTree");
@ -79,6 +81,19 @@ TEST(DiscreteSerialization, DecisionTreeFactor) {
EXPECT(equalsBinary<DecisionTreeFactor>(f));
}
/* ************************************************************************* */
// Check serialization for TableFactor
TEST(DiscreteSerialization, TableFactor) {
using namespace serializationTestHelpers;
DiscreteKey A(Symbol('x', 1), 3);
TableFactor tf(A, "1 2 2");
EXPECT(equalsObj<TableFactor>(tf));
EXPECT(equalsXML<TableFactor>(tf));
EXPECT(equalsBinary<TableFactor>(tf));
}
/* ************************************************************************* */
// Check serialization for DiscreteConditional & DiscreteDistribution
TEST(DiscreteSerialization, DiscreteConditional) {

30
gtsam/discrete/tests/testTableFactor.cpp
View File

@ -173,6 +173,36 @@ TEST(TableFactor, Conversion) {
TableFactor tf(dtf.discreteKeys(), dtf);
EXPECT(assert_equal(dtf, tf.toDecisionTreeFactor()));
// Test for correct construction when keys are not in reverse order.
// This is possible in conditionals e.g. P(x1 | x0)
DiscreteKey X(1, 2), Y(0, 2);
DiscreteConditional dtf2(
X, {Y}, std::vector<double>{0.33333333, 0.6, 0.66666667, 0.4});
TableFactor tf2(dtf2);
// GTSAM_PRINT(dtf2);
// GTSAM_PRINT(tf2);
// GTSAM_PRINT(tf2.toDecisionTreeFactor());
// Check for ADT equality since the order of keys is irrelevant
EXPECT(assert_equal<AlgebraicDecisionTree<Key>>(dtf2,
tf2.toDecisionTreeFactor()));
}
/* ************************************************************************* */
TEST(TableFactor, Empty) {
DiscreteKey X(1, 2);
TableFactor single = *TableFactor({X}, "1 1").sum(1);
// Should not throw a segfault
EXPECT(assert_equal(*DecisionTreeFactor(X, "1 1").sum(1),
single.toDecisionTreeFactor()));
TableFactor empty = *TableFactor({X}, "0 0").sum(1);
// Should not throw a segfault
EXPECT(assert_equal(*DecisionTreeFactor(X, "0 0").sum(1),
empty.toDecisionTreeFactor()));
}
/* ************************************************************************* */

5
gtsam/geometry/Pose2.h
View File

@ -60,7 +60,10 @@ public:
}
/** copy constructor */
Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {}
Pose2(const Pose2& pose) = default;
// : r_(pose.r_), t_(pose.t_) {}
Pose2& operator=(const Pose2& other) = default;
/**
* construct from (x,y,theta)

7
gtsam/geometry/Pose3.h
View File

@ -55,9 +55,10 @@ public:
Pose3() : R_(traits<Rot3>::Identity()), t_(traits<Point3>::Identity()) {}
/** Copy constructor */
Pose3(const Pose3& pose) :
R_(pose.R_), t_(pose.t_) {
}
Pose3(const Pose3& pose) = default;
// :
// R_(pose.R_), t_(pose.t_) {
// }
/** Construct from R,t */
Pose3(const Rot3& R, const Point3& t) :

5
gtsam/geometry/Rot2.h
View File

@ -52,11 +52,14 @@ namespace gtsam {
Rot2() : c_(1.0), s_(0.0) {}
/** copy constructor */
Rot2(const Rot2& r) : Rot2(r.c_, r.s_) {}
Rot2(const Rot2& r) = default;
// : Rot2(r.c_, r.s_) {}
/// Constructor from angle in radians == exponential map at identity
Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
// Rot2& operator=(const gtsam::Rot2& other) = default;
/// Named constructor from angle in radians
static Rot2 fromAngle(double theta) {
return Rot2(theta);

2
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -282,7 +282,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
} else if (auto hc = dynamic_pointer_cast<HybridConditional>(f)) {
auto dc = hc->asDiscrete();
if (!dc) throwRuntimeError("discreteElimination", dc);
dfg.push_back(hc->asDiscrete());
dfg.push_back(dc);
} else {
throwRuntimeError("discreteElimination", f);
}

2
gtsam/linear/ConjugateGradientSolver.h
View File

@ -69,6 +69,8 @@ struct GTSAM_EXPORT ConjugateGradientParameters
epsilon_abs(p.epsilon_abs),
blas_kernel(GTSAM) {}
ConjugateGradientParameters& operator=(const ConjugateGradientParameters& other) = default;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
inline size_t getMinIterations() const { return minIterations; }
inline size_t getMaxIterations() const { return maxIterations; }

2
gtsam/linear/HessianFactor.cpp
View File

@ -379,7 +379,7 @@ GaussianFactor::shared_ptr HessianFactor::negate() const {
shared_ptr result = std::make_shared<This>(*this);
// Negate the information matrix of the result
result->info_.negate();
return std::move(result);
return result;
}
/* ************************************************************************* */

2
gtsam/linear/JacobianFactor.h
View File

@ -117,6 +117,8 @@ namespace gtsam {
/** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */
explicit JacobianFactor(const HessianFactor& hf);
JacobianFactor& operator=(const JacobianFactor& jf) = default;
/** default constructor for I/O */
JacobianFactor();

2
gtsam/linear/NoiseModel.cpp
View File

@ -64,7 +64,7 @@ std::optional<Vector> checkIfDiagonal(const Matrix& M) {
Vector diagonal(n);
for (j = 0; j < n; j++)
diagonal(j) = M(j, j);
return std::move(diagonal);
return diagonal;
}
}

2
gtsam/linear/VectorValues.h
View File

@ -119,6 +119,8 @@ namespace gtsam {
/// Constructor from Vector, with Scatter
VectorValues(const Vector& c, const Scatter& scatter);
VectorValues& operator=(const VectorValues& other) = default;
/** Create a VectorValues with the same structure as \c other, but filled with zeros. */
static VectorValues Zero(const VectorValues& other);

2
gtsam/nonlinear/ExpressionFactor.h
View File

@ -149,7 +149,7 @@ protected:
noiseModel_->WhitenSystem(Ab.matrix(), b);
}
return std::move(factor);
return factor;
}
/// @return a deep copy of this factor

6
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -26,7 +26,7 @@
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/base/Vector.h>
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
#include <gtsam/base/timing.h>
#endif
@ -123,7 +123,7 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
auto currentState = static_cast<const State*>(state_.get());
bool verbose = (params_.verbosityLM >= LevenbergMarquardtParams::TRYLAMBDA);
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
#ifdef GTSAM_USING_NEW_BOOST_TIMERS
boost::timer::cpu_timer lamda_iteration_timer;
lamda_iteration_timer.start();
@ -222,7 +222,7 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
} // if (systemSolvedSuccessfully)
if (params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) {
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
// do timing
#ifdef GTSAM_USING_NEW_BOOST_TIMERS
double iterationTime = 1e-9 * lamda_iteration_timer.elapsed().wall;

2
gtsam/slam/FrobeniusFactor.cpp
View File

@ -61,7 +61,7 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) {
return noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), isoModel);
} else {
return std::move(isoModel);
return isoModel;
}
}

1
gtsam_unstable/geometry/Pose3Upright.h
View File

@ -43,6 +43,7 @@ public:
Pose3Upright(const Rot2& bearing, const Point3& t);
Pose3Upright(double x, double y, double z, double theta);
Pose3Upright(const Pose2& pose, double z);
Pose3Upright& operator=(const Pose3Upright& x) = default;
/// Down-converts from a full Pose3
Pose3Upright(const Pose3& fullpose);

4
gtsam_unstable/slam/Mechanization_bRn2.h
View File

@ -35,9 +35,7 @@ public:
}
/// Copy constructor
Mechanization_bRn2(const Mechanization_bRn2& other) :
bRn_(other.bRn_), x_g_(other.x_g_), x_a_(other.x_a_) {
}
Mechanization_bRn2(const Mechanization_bRn2& other) = default;
/// gravity in the body frame
Vector3 b_g(double g_e) const {