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

Merge branch 'develop' into fix-1496

release/4.3a0
Varun Agrawal 2023-07-28 15:43:40 -04:00
parent ea24a2c7e8 19c3939766
commit 369d08bc92
27 changed files with 282 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
.github/workflows/build-windows.yml vendored
View File

@ -99,26 +99,67 @@ jobs:
cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib"
- name: Build
shell: bash
run: |
# Since Visual Studio is a multi-generator, we need to use --config
# https://stackoverflow.com/a/24470998/1236990
cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam
cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam_unstable
cmake --build build -j 4 --config ${{ matrix.build_type }} --target wrap
# Target doesn't exist
# cmake --build build -j4 --config ${{ matrix.build_type }} --target wrap
- name: Test
shell: bash
run: |
# Run GTSAM tests
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.basis
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete
# Compilation error
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.inference
# Compile. Fail with exception
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.navigation
#cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.nonlinear
#cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.sam
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sam
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sfm
#cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.slam
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.symbolic
# Compile. Fail with exception
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.hybrid
# Compile. Fail with exception
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear
# Compilation error
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam
# Run GTSAM_UNSTABLE tests
#cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base_unstable
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base_unstable
# Compile. Fail with exception
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry_unstable
# Compile. Fail with exception
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear_unstable
# Compile. Fail with exception
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete_unstable
# Compile. Fail with exception
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.dynamics_unstable
# Compile. Fail with exception
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear_unstable
# Compilation error
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam_unstable
# Compilation error
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.partition

8
CMakeLists.txt
View File

@ -32,6 +32,14 @@ set (CMAKE_PROJECT_VERSION_PATCH ${GTSAM_VERSION_PATCH})
###############################################################################
# Gather information, perform checks, set defaults
if(MSVC)
set(MSVC_LINKER_FLAGS "/FORCE:MULTIPLE")
set(CMAKE_EXE_LINKER_FLAGS ${MSVC_LINKER_FLAGS})
set(CMAKE_MODULE_LINKER_FLAGS ${MSVC_LINKER_FLAGS})
set(CMAKE_SHARED_LINKER_FLAGS ${MSVC_LINKER_FLAGS})
set(CMAKE_STATIC_LINKER_FLAGS ${MSVC_LINKER_FLAGS})
endif()
set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
include(GtsamMakeConfigFile)
include(GNUInstallDirs)

4
cmake/Config.cmake.in
View File

@ -21,6 +21,10 @@ else()
find_dependency(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
endif()
if(@GTSAM_USE_TBB@)
find_dependency(TBB 4.4 COMPONENTS tbb tbbmalloc)
endif()
if(@GTSAM_USE_SYSTEM_EIGEN@)
find_dependency(Eigen3 REQUIRED)
endif()

2
gtsam/basis/Basis.h
View File

@ -80,7 +80,7 @@ using Weights = Eigen::Matrix<double, 1, -1>; /* 1xN vector */
*
* @ingroup basis
*/
Matrix kroneckerProductIdentity(size_t M, const Weights& w);
Matrix GTSAM_EXPORT kroneckerProductIdentity(size_t M, const Weights& w);
/**
* CRTP Base class for function bases

69
gtsam/discrete/DecisionTreeFactor.cpp
View File

@ -82,6 +82,22 @@ namespace gtsam {
ADT::print("", formatter);
}
/* ************************************************************************ */
DecisionTreeFactor DecisionTreeFactor::apply(ADT::Unary op) const {
// apply operand
ADT result = ADT::apply(op);
// Make a new factor
return DecisionTreeFactor(discreteKeys(), result);
}
/* ************************************************************************ */
DecisionTreeFactor DecisionTreeFactor::apply(ADT::UnaryAssignment op) const {
// apply operand
ADT result = ADT::apply(op);
// Make a new factor
return DecisionTreeFactor(discreteKeys(), result);
}
/* ************************************************************************ */
DecisionTreeFactor DecisionTreeFactor::apply(const DecisionTreeFactor& f,
ADT::Binary op) const {
@ -101,14 +117,6 @@ namespace gtsam {
return DecisionTreeFactor(keys, result);
}
/* ************************************************************************ */
DecisionTreeFactor DecisionTreeFactor::apply(ADT::UnaryAssignment op) const {
// apply operand
ADT result = ADT::apply(op);
// Make a new factor
return DecisionTreeFactor(discreteKeys(), result);
}
/* ************************************************************************ */
DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(
size_t nrFrontals, ADT::Binary op) const {
@ -188,10 +196,45 @@ namespace gtsam {
/* ************************************************************************ */
std::vector<double> DecisionTreeFactor::probabilities() const {
// Set of all keys
std::set<Key> allKeys(keys().begin(), keys().end());
std::vector<double> probs;
for (auto&& [key, value] : enumerate()) {
probs.push_back(value);
/* An operation that takes each leaf probability, and computes the
* nrAssignments by checking the difference between the keys in the factor
* and the keys in the assignment.
* The nrAssignments is then used to append
* the correct number of leaf probability values to the `probs` vector
* defined above.
*/
auto op = [&](const Assignment<Key>& a, double p) {
// Get all the keys in the current assignment
std::set<Key> assignment_keys;
for (auto&& [k, _] : a) {
assignment_keys.insert(k);
}
// Find the keys missing in the assignment
std::vector<Key> diff;
std::set_difference(allKeys.begin(), allKeys.end(),
assignment_keys.begin(), assignment_keys.end(),
std::back_inserter(diff));
// Compute the total number of assignments in the (pruned) subtree
size_t nrAssignments = 1;
for (auto&& k : diff) {
nrAssignments *= cardinalities_.at(k);
}
// Add p `nrAssignments` times to the probs vector.
probs.insert(probs.end(), nrAssignments, p);
return p;
};
// Go through the tree
this->apply(op);
return probs;
}
@ -305,11 +348,7 @@ namespace gtsam {
const size_t N = maxNrAssignments;
// Get the probabilities in the decision tree so we can threshold.
std::vector<double> probabilities;
// NOTE(Varun) this is potentially slow due to the cartesian product
for (auto&& [assignment, prob] : this->enumerate()) {
probabilities.push_back(prob);
}
std::vector<double> probabilities = this->probabilities();
// The number of probabilities can be lower than max_leaves
if (probabilities.size() <= N) {

7
gtsam/discrete/DecisionTreeFactor.h
View File

@ -186,6 +186,13 @@ namespace gtsam {
* Apply unary operator (*this) "op" f
* @param op a unary operator that operates on AlgebraicDecisionTree
*/
DecisionTreeFactor apply(ADT::Unary op) const;
/**
* Apply unary operator (*this) "op" f
* @param op a unary operator that operates on AlgebraicDecisionTree. Takes
* both the assignment and the value.
*/
DecisionTreeFactor apply(ADT::UnaryAssignment op) const;
/**

4
gtsam/discrete/SignatureParser.h
View File

@ -22,6 +22,8 @@
#include <string>
#include <vector>
#include <gtsam/dllexport.h>
namespace gtsam {
/**
* @brief A simple parser that replaces the boost spirit parser.
@ -47,7 +49,7 @@ namespace gtsam {
*
* Also fails if the rows are not of the same size.
*/
struct SignatureParser {
struct GTSAM_EXPORT SignatureParser {
using Row = std::vector<double>;
using Table = std::vector<Row>;

38
gtsam/discrete/TableFactor.cpp
View File

@ -56,9 +56,45 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys,
sort(sorted_dkeys_.begin(), sorted_dkeys_.end());
}
/* ************************************************************************ */
TableFactor::TableFactor(const DiscreteKeys& dkeys,
const DecisionTree<Key, double>& dtree)
: TableFactor(dkeys, DecisionTreeFactor(dkeys, dtree)) {}
/**
* @brief Compute the correct ordering of the leaves in the decision tree.
*
* This is done by first taking all the values which have modulo 0 value with
* the cardinality of the innermost key `n`, and we go up to modulo n.
*
* @param dt The DecisionTree
* @return std::vector<double>
*/
std::vector<double> ComputeLeafOrdering(const DiscreteKeys& dkeys,
const DecisionTreeFactor& dt) {
std::vector<double> probs = dt.probabilities();
std::vector<double> ordered;
size_t n = dkeys[0].second;
for (size_t k = 0; k < n; ++k) {
for (size_t idx = 0; idx < probs.size(); ++idx) {
if (idx % n == k) {
ordered.push_back(probs[idx]);
}
}
}
return ordered;
}
/* ************************************************************************ */
TableFactor::TableFactor(const DiscreteKeys& dkeys,
const DecisionTreeFactor& dtf)
: TableFactor(dkeys, ComputeLeafOrdering(dkeys, dtf)) {}
/* ************************************************************************ */
TableFactor::TableFactor(const DiscreteConditional& c)
: TableFactor(c.discreteKeys(), c.probabilities()) {}
: TableFactor(c.discreteKeys(), c) {}
/* ************************************************************************ */
Eigen::SparseVector<double> TableFactor::Convert(

8
gtsam/discrete/TableFactor.h
View File

@ -144,6 +144,12 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
TableFactor(const DiscreteKey& key, const std::vector<double>& row)
: TableFactor(DiscreteKeys{key}, row) {}
/// Constructor from DecisionTreeFactor
TableFactor(const DiscreteKeys& keys, const DecisionTreeFactor& dtf);
/// Constructor from DecisionTree<Key, double>/AlgebraicDecisionTree
TableFactor(const DiscreteKeys& keys, const DecisionTree<Key, double>& dtree);
/** Construct from a DiscreteConditional type */
explicit TableFactor(const DiscreteConditional& c);
@ -180,7 +186,7 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
return apply(f, Ring::mul);
};
/// multiple with DecisionTreeFactor
/// multiply with DecisionTreeFactor
DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
static double safe_div(const double& a, const double& b);

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

@ -19,6 +19,7 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/discrete/DiscreteDistribution.h>
#include <gtsam/discrete/Signature.h>
#include <gtsam/discrete/TableFactor.h>
@ -131,6 +132,16 @@ TEST(TableFactor, constructors) {
// Manually constructed via inspection and comparison to DecisionTreeFactor
TableFactor expected(X & Y, "0.5 0.4 0.2 0.5 0.6 0.8");
EXPECT(assert_equal(expected, f4));
// Test for 9=3x3 values.
DiscreteKey V(0, 3), W(1, 3);
DiscreteConditional conditional5(V | W = "1/2/3 5/6/7 9/10/11");
TableFactor f5(conditional5);
// GTSAM_PRINT(f5);
TableFactor expected_f5(
X & Y,
"0.166667 0.277778 0.3 0.333333 0.333333 0.333333 0.5 0.388889 0.366667");
EXPECT(assert_equal(expected_f5, f5, 1e-6));
}
/* ************************************************************************* */

2
gtsam/geometry/Line3.h
View File

@ -146,7 +146,7 @@ class GTSAM_EXPORT Line3 {
* @param Dline - OptionalJacobian of transformed line with respect to l
* @return Transformed line in camera frame
*/
friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
GTSAM_EXPORT friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
OptionalJacobian<4, 6> Dpose,
OptionalJacobian<4, 4> Dline);
};

40
gtsam/geometry/Pose2.h
View File

@ -36,7 +36,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Pose2: public LieGroup<Pose2, 3> {
class GTSAM_EXPORT Pose2: public LieGroup<Pose2, 3> {
public:
@ -112,10 +112,10 @@ public:
/// @{
/** print with optional string */
GTSAM_EXPORT void print(const std::string& s = "") const;
void print(const std::string& s = "") const;
/** assert equality up to a tolerance */
GTSAM_EXPORT bool equals(const Pose2& pose, double tol = 1e-9) const;
bool equals(const Pose2& pose, double tol = 1e-9) const;
/// @}
/// @name Group
@ -125,7 +125,7 @@ public:
inline static Pose2 Identity() { return Pose2(); }
/// inverse
GTSAM_EXPORT Pose2 inverse() const;
Pose2 inverse() const;
/// compose syntactic sugar
inline Pose2 operator*(const Pose2& p2) const {
@ -137,16 +137,16 @@ public:
/// @{
///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$
GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = {});
static Pose2 Expmap(const Vector3& xi, ChartJacobian H = {});
///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation
GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = {});
static Vector3 Logmap(const Pose2& p, ChartJacobian H = {});
/**
* Calculate Adjoint map
* Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi)
*/
GTSAM_EXPORT Matrix3 AdjointMap() const;
Matrix3 AdjointMap() const;
/// Apply AdjointMap to twist xi
inline Vector3 Adjoint(const Vector3& xi) const {
@ -156,7 +156,7 @@ public:
/**
* Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19
*/
GTSAM_EXPORT static Matrix3 adjointMap(const Vector3& v);
static Matrix3 adjointMap(const Vector3& v);
/**
* Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
@ -192,15 +192,15 @@ public:
}
/// Derivative of Expmap
GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& v);
static Matrix3 ExpmapDerivative(const Vector3& v);
/// Derivative of Logmap
GTSAM_EXPORT static Matrix3 LogmapDerivative(const Pose2& v);
static Matrix3 LogmapDerivative(const Pose2& v);
// Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
struct ChartAtOrigin {
GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = {});
GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = {});
struct GTSAM_EXPORT ChartAtOrigin {
static Pose2 Retract(const Vector3& v, ChartJacobian H = {});
static Vector3 Local(const Pose2& r, ChartJacobian H = {});
};
using LieGroup<Pose2, 3>::inverse; // version with derivative
@ -210,7 +210,7 @@ public:
/// @{
/** Return point coordinates in pose coordinate frame */
GTSAM_EXPORT Point2 transformTo(const Point2& point,
Point2 transformTo(const Point2& point,
OptionalJacobian<2, 3> Dpose = {},
OptionalJacobian<2, 2> Dpoint = {}) const;
@ -222,7 +222,7 @@ public:
Matrix transformTo(const Matrix& points) const;
/** Return point coordinates in global frame */
GTSAM_EXPORT Point2 transformFrom(const Point2& point,
Point2 transformFrom(const Point2& point,
OptionalJacobian<2, 3> Dpose = {},
OptionalJacobian<2, 2> Dpoint = {}) const;
@ -273,14 +273,14 @@ public:
}
//// return transformation matrix
GTSAM_EXPORT Matrix3 matrix() const;
Matrix3 matrix() const;
/**
* Calculate bearing to a landmark
* @param point 2D location of landmark
* @return 2D rotation \f$ \in SO(2) \f$
*/
GTSAM_EXPORT Rot2 bearing(const Point2& point,
Rot2 bearing(const Point2& point,
OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 2> H2={}) const;
/**
@ -288,7 +288,7 @@ public:
* @param point SO(2) location of other pose
* @return 2D rotation \f$ \in SO(2) \f$
*/
GTSAM_EXPORT Rot2 bearing(const Pose2& pose,
Rot2 bearing(const Pose2& pose,
OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 3> H2={}) const;
/**
@ -296,7 +296,7 @@ public:
* @param point 2D location of landmark
* @return range (double)
*/
GTSAM_EXPORT double range(const Point2& point,
double range(const Point2& point,
OptionalJacobian<1, 3> H1={},
OptionalJacobian<1, 2> H2={}) const;
@ -305,7 +305,7 @@ public:
* @param point 2D location of other pose
* @return range (double)
*/
GTSAM_EXPORT double range(const Pose2& point,
double range(const Pose2& point,
OptionalJacobian<1, 3> H1={},
OptionalJacobian<1, 3> H2={}) const;

2
gtsam/geometry/Pose3.h
View File

@ -204,7 +204,7 @@ public:
static Matrix6 LogmapDerivative(const Pose3& xi);
// Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
struct ChartAtOrigin {
struct GTSAM_EXPORT ChartAtOrigin {
static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {});
static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {});
};

19
gtsam/geometry/tests/testRot3.cpp
View File

@ -597,6 +597,25 @@ TEST(Rot3, quaternion) {
EXPECT(assert_equal(expected2, actual2));
}
/* ************************************************************************* */
TEST(Rot3, ConvertQuaternion) {
Eigen::Quaterniond eigenQuaternion;
eigenQuaternion.w() = 1.0;
eigenQuaternion.x() = 2.0;
eigenQuaternion.y() = 3.0;
eigenQuaternion.z() = 4.0;
EXPECT_DOUBLES_EQUAL(1, eigenQuaternion.w(), 1e-9);
EXPECT_DOUBLES_EQUAL(2, eigenQuaternion.x(), 1e-9);
EXPECT_DOUBLES_EQUAL(3, eigenQuaternion.y(), 1e-9);
EXPECT_DOUBLES_EQUAL(4, eigenQuaternion.z(), 1e-9);
Rot3 R(eigenQuaternion);
EXPECT_DOUBLES_EQUAL(1, R.toQuaternion().w(), 1e-9);
EXPECT_DOUBLES_EQUAL(2, R.toQuaternion().x(), 1e-9);
EXPECT_DOUBLES_EQUAL(3, R.toQuaternion().y(), 1e-9);
EXPECT_DOUBLES_EQUAL(4, R.toQuaternion().z(), 1e-9);
}
/* ************************************************************************* */
Matrix Cayley(const Matrix& A) {
Matrix::Index n = A.cols();

2
gtsam/hybrid/GaussianMixture.cpp
View File

@ -286,8 +286,6 @@ GaussianMixture::prunerFunc(const DecisionTreeFactor &discreteProbs) {
/* *******************************************************************************/
void GaussianMixture::prune(const DecisionTreeFactor &discreteProbs) {
auto discreteProbsKeySet = DiscreteKeysAsSet(discreteProbs.discreteKeys());
auto gmKeySet = DiscreteKeysAsSet(this->discreteKeys());
// Functional which loops over all assignments and create a set of
// GaussianConditionals
auto pruner = prunerFunc(discreteProbs);

5
gtsam/hybrid/HybridBayesNet.cpp
View File

@ -129,7 +129,6 @@ std::function<double(const Assignment<Key> &, double)> prunerFunc(
DecisionTreeFactor HybridBayesNet::pruneDiscreteConditionals(
size_t maxNrLeaves) {
// Get the joint distribution of only the discrete keys
gttic_(HybridBayesNet_PruneDiscreteConditionals);
// The joint discrete probability.
DiscreteConditional discreteProbs;
@ -163,7 +162,6 @@ DecisionTreeFactor HybridBayesNet::pruneDiscreteConditionals(
gttoc_(HybridBayesNet_PruneDiscreteConditionals);
// Eliminate joint probability back into conditionals
gttic_(HybridBayesNet_UpdateDiscreteConditionals);
DiscreteFactorGraph dfg{prunedDiscreteProbs};
DiscreteBayesNet::shared_ptr dbn = dfg.eliminateSequential(discrete_frontals);
@ -174,7 +172,6 @@ DecisionTreeFactor HybridBayesNet::pruneDiscreteConditionals(
// dbn->at(i)->print();
this->at(idx) = std::make_shared<HybridConditional>(dbn->at(i));
}
gttoc_(HybridBayesNet_UpdateDiscreteConditionals);
return prunedDiscreteProbs;
}
@ -193,7 +190,6 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) {
HybridBayesNet prunedBayesNetFragment;
gttic_(HybridBayesNet_PruneMixtures);
// Go through all the conditionals in the
// Bayes Net and prune them as per prunedDiscreteProbs.
for (auto &&conditional : *this) {
@ -210,7 +206,6 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) {
prunedBayesNetFragment.push_back(conditional);
}
}
gttoc_(HybridBayesNet_PruneMixtures);
return prunedBayesNetFragment;
}

2
gtsam/hybrid/HybridFactorGraph.h
View File

@ -35,7 +35,7 @@ using SharedFactor = std::shared_ptr<Factor>;
* Hybrid Factor Graph
* Factor graph with utilities for hybrid factors.
*/
class HybridFactorGraph : public FactorGraph<Factor> {
class GTSAM_EXPORT HybridFactorGraph : public FactorGraph<Factor> {
public:
using Base = FactorGraph<Factor>;
using This = HybridFactorGraph; ///< this class

3
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -96,7 +96,6 @@ static GaussianFactorGraphTree addGaussian(
// TODO(dellaert): it's probably more efficient to first collect the discrete
// keys, and then loop over all assignments to populate a vector.
GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const {
gttic_(assembleGraphTree);
GaussianFactorGraphTree result;
@ -129,8 +128,6 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const {
}
}
gttoc_(assembleGraphTree);
return result;
}

2
gtsam/hybrid/HybridSmoother.h
View File

@ -24,7 +24,7 @@
namespace gtsam {
class HybridSmoother {
class GTSAM_EXPORT HybridSmoother {
private:
HybridBayesNet hybridBayesNet_;
HybridGaussianFactorGraph remainingFactorGraph_;

1
gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
View File

@ -43,6 +43,7 @@
#include <iostream>
#include <iterator>
#include <vector>
#include <numeric>
#include "Switching.h"
#include "TinyHybridExample.h"

2
gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
View File

@ -420,7 +420,7 @@ TEST(HybridFactorGraph, Full_Elimination) {
DiscreteFactorGraph discrete_fg;
// TODO(Varun) Make this a function of HybridGaussianFactorGraph?
for (auto& factor : (*remainingFactorGraph_partial)) {
auto df = dynamic_pointer_cast<DecisionTreeFactor>(factor);
auto df = dynamic_pointer_cast<DiscreteFactor>(factor);
assert(df);
discrete_fg.push_back(df);
}

19
gtsam/inference/Ordering.h
View File

@ -30,7 +30,7 @@
namespace gtsam {
class Ordering: public KeyVector {
class GTSAM_EXPORT Ordering: public KeyVector {
protected:
typedef KeyVector Base;
@ -45,7 +45,6 @@ public:
typedef std::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
/// Create an empty ordering
GTSAM_EXPORT
Ordering() {
}
@ -99,7 +98,7 @@ public:
}
/// Compute a fill-reducing ordering using COLAMD from a VariableIndex.
static GTSAM_EXPORT Ordering Colamd(const VariableIndex& variableIndex);
static Ordering Colamd(const VariableIndex& variableIndex);
/// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
/// for note on performance). This internally builds a VariableIndex so if you already have a
@ -124,7 +123,7 @@ public:
/// variables in \c constrainLast will be ordered in the same order specified in the KeyVector
/// \c constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be
/// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
static GTSAM_EXPORT Ordering ColamdConstrainedLast(
static Ordering ColamdConstrainedLast(
const VariableIndex& variableIndex, const KeyVector& constrainLast,
bool forceOrder = false);
@ -152,7 +151,7 @@ public:
/// KeyVector \c constrainFirst. If \c forceOrder is false, the variables in \c
/// constrainFirst will be ordered before all the others, but will be rearranged by CCOLAMD to
/// reduce fill-in as well.
static GTSAM_EXPORT Ordering ColamdConstrainedFirst(
static Ordering ColamdConstrainedFirst(
const VariableIndex& variableIndex,
const KeyVector& constrainFirst, bool forceOrder = false);
@ -181,7 +180,7 @@ public:
/// appear in \c groups in arbitrary order. Any variables not present in \c groups will be
/// assigned to group 0. This function simply fills the \c cmember argument to CCOLAMD with the
/// supplied indices, see the CCOLAMD documentation for more information.
static GTSAM_EXPORT Ordering ColamdConstrained(
static Ordering ColamdConstrained(
const VariableIndex& variableIndex, const FastMap<Key, int>& groups);
/// Return a natural Ordering. Typically used by iterative solvers
@ -195,11 +194,11 @@ public:
/// METIS Formatting function
template<class FACTOR_GRAPH>
static GTSAM_EXPORT void CSRFormat(std::vector<int>& xadj,
static void CSRFormat(std::vector<int>& xadj,
std::vector<int>& adj, const FACTOR_GRAPH& graph);
/// Compute an ordering determined by METIS from a VariableIndex
static GTSAM_EXPORT Ordering Metis(const MetisIndex& met);
static Ordering Metis(const MetisIndex& met);
template<class FACTOR_GRAPH>
static Ordering Metis(const FACTOR_GRAPH& graph) {
@ -241,18 +240,16 @@ public:
/// @name Testable
/// @{
GTSAM_EXPORT
void print(const std::string& str = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
GTSAM_EXPORT
bool equals(const Ordering& other, double tol = 1e-9) const;
/// @}
private:
/// Internal COLAMD function
static GTSAM_EXPORT Ordering ColamdConstrained(
static Ordering ColamdConstrained(
const VariableIndex& variableIndex, std::vector<int>& cmember);
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION

3
gtsam/sfm/ShonanAveraging.cpp
View File

@ -894,6 +894,9 @@ template <size_t d>
std::pair<Values, double> ShonanAveraging<d>::run(const Values &initialEstimate,
size_t pMin,
size_t pMax) const {
if (pMin < d) {
throw std::runtime_error("pMin is smaller than the base dimension d");
}
Values Qstar;
Values initialSOp = LiftTo<Rot>(pMin, initialEstimate); // lift to pMin!
for (size_t p = pMin; p <= pMax; p++) {

14
gtsam/sfm/tests/testShonanAveraging.cpp
View File

@ -415,6 +415,20 @@ TEST(ShonanAveraging3, PriorWeights) {
auto result = shonan.run(initial, 3, 3);
EXPECT_DOUBLES_EQUAL(0.0015, shonan.cost(result.first), 1e-4);
}
/* ************************************************************************* */
// Check a small graph created using binary measurements
TEST(ShonanAveraging3, BinaryMeasurements) {
std::vector<BinaryMeasurement<Rot3>> measurements;
auto unit3 = noiseModel::Unit::Create(3);
measurements.emplace_back(0, 1, Rot3::Yaw(M_PI_2), unit3);
measurements.emplace_back(1, 2, Rot3::Yaw(M_PI_2), unit3);
ShonanAveraging3 shonan(measurements);
Values initial = shonan.initializeRandomly();
auto result = shonan.run(initial, 3, 5);
EXPECT_DOUBLES_EQUAL(0.0, shonan.cost(result.first), 1e-4);
}
/* ************************************************************************* */
int main() {
TestResult tr;

3
gtsam_unstable/linear/LPInitSolver.h
View File

@ -19,6 +19,7 @@
#pragma once
#include <gtsam_unstable/dllexport.h>
#include <gtsam_unstable/linear/LP.h>
#include <gtsam/linear/GaussianFactorGraph.h>
@ -49,7 +50,7 @@ namespace gtsam {
* inequality constraint, we can't conclude that the problem is infeasible.
* However, whether it is infeasible or unbounded, we don't have a unique solution anyway.
*/
class LPInitSolver {
class GTSAM_UNSTABLE_EXPORT LPInitSolver {
private:
const LP& lp_;

3
gtsam_unstable/linear/QPSParser.h
View File

@ -18,12 +18,13 @@
#pragma once
#include <gtsam_unstable/dllexport.h>
#include <gtsam_unstable/linear/QP.h>
#include <fstream>
namespace gtsam {
class QPSParser {
class GTSAM_UNSTABLE_EXPORT QPSParser {
private:
std::string fileName_;

19
python/gtsam/tests/test_ShonanAveraging.py
View File

@ -10,14 +10,16 @@ Author: Frank Dellaert
"""
# pylint: disable=invalid-name, no-name-in-module, no-member
import math
import unittest
import numpy as np
from gtsam.utils.test_case import GtsamTestCase
import gtsam
from gtsam import (BetweenFactorPose2, LevenbergMarquardtParams, Pose2, Rot2,
ShonanAveraging2, ShonanAveraging3,
from gtsam import (BetweenFactorPose2, BetweenFactorPose3,
BinaryMeasurementRot3, LevenbergMarquardtParams, Pose2,
Pose3, Rot2, Rot3, ShonanAveraging2, ShonanAveraging3,
ShonanAveragingParameters2, ShonanAveragingParameters3)
DEFAULT_PARAMS = ShonanAveragingParameters3(
@ -197,6 +199,19 @@ class TestShonanAveraging(GtsamTestCase):
expected_thetas_deg = np.array([0.0, 90.0, 0.0])
np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1)
def test_measurements3(self):
"""Create from Measurements."""
measurements = []
unit3 = gtsam.noiseModel.Unit.Create(3)
m01 = BinaryMeasurementRot3(0, 1, Rot3.Yaw(math.radians(90)), unit3)
m12 = BinaryMeasurementRot3(1, 2, Rot3.Yaw(math.radians(90)), unit3)
measurements.append(m01)
measurements.append(m12)
obj = ShonanAveraging3(measurements)
self.assertIsInstance(obj, ShonanAveraging3)
initial = obj.initializeRandomly()
_, cost = obj.run(initial, min_p=3, max_p=5)
self.assertAlmostEqual(cost, 0)
if __name__ == "__main__":
unittest.main()