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

Merge pull request #1640 from borglab/fixes 

Various Fixes and TODOs
release/4.3a0
Frank Dellaert 2023-10-17 14:18:25 -07:00 committed by GitHub
parent 3a1fe57468 320ac1bc6a
commit 7f511837fe
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
15 changed files with 42 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/CMakeLists.txt
View File

@ -145,10 +145,6 @@ if (GTSAM_USE_EIGEN_MKL)
target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR})
endif()
if(GTSAM_USE_TBB)
target_include_directories(gtsam PUBLIC ${TBB_INCLUDE_DIRS})
endif()
# Add includes for source directories 'BEFORE' boost and any system include
# paths so that the compiler uses GTSAM headers in our source directory instead
# of any previously installed GTSAM headers.

24
gtsam/discrete/DecisionTreeFactor.cpp
View File

@ -164,15 +164,23 @@ namespace gtsam {
}
// create new factor, note we collect keys that are not in frontalKeys
// TODO(frank): why do we need this??? result should contain correct keys!!!
/*
Due to branch merging, the labels in `result` may be missing some keys
E.g. After branch merging, we may get a ADT like:
Leaf [2] 1.0204082
This is missing the key values used for branching.
*/
KeyVector difference, frontalKeys_(frontalKeys), keys_(keys());
// Get the difference of the frontalKeys and the factor keys using set_difference
std::sort(keys_.begin(), keys_.end());
std::sort(frontalKeys_.begin(), frontalKeys_.end());
std::set_difference(keys_.begin(), keys_.end(), frontalKeys_.begin(),
frontalKeys_.end(), back_inserter(difference));
DiscreteKeys dkeys;
for (i = 0; i < keys().size(); i++) {
Key j = keys()[i];
// TODO(frank): inefficient!
if (std::find(frontalKeys.begin(), frontalKeys.end(), j) !=
frontalKeys.end())
continue;
dkeys.push_back(DiscreteKey(j, cardinality(j)));
for (Key key : difference) {
dkeys.push_back(DiscreteKey(key, cardinality(key)));
}
return std::make_shared<DecisionTreeFactor>(dkeys, result);
}

8
gtsam/discrete/DiscreteBayesTree.h
View File

@ -110,4 +110,12 @@ class GTSAM_EXPORT DiscreteBayesTree
/// @}
};
/// traits
template <>
struct traits<DiscreteBayesTreeClique>
: public Testable<DiscreteBayesTreeClique> {};
template <>
struct traits<DiscreteBayesTree> : public Testable<DiscreteBayesTree> {};
} // namespace gtsam

1
gtsam/discrete/DiscreteKey.cpp
View File

@ -49,6 +49,7 @@ namespace gtsam {
void DiscreteKeys::print(const std::string& s,
const KeyFormatter& keyFormatter) const {
std::cout << (s.empty() ? "" : s + " ") << std::endl;
for (auto&& dkey : *this) {
std::cout << DefaultKeyFormatter(dkey.first) << " " << dkey.second
<< std::endl;

2
gtsam/discrete/tests/testDiscreteMarginals.cpp
View File

@ -121,7 +121,7 @@ TEST_UNSAFE( DiscreteMarginals, truss ) {
Clique expected0(std::make_shared<DiscreteConditional>((key[0] | key[2], key[4]) = "2/1 2/1 2/1 2/1"));
Clique::shared_ptr actual0 = (*bayesTree)[0];
// EXPECT(assert_equal(expected0, *actual0)); // TODO, correct but fails
EXPECT(assert_equal(expected0, *actual0));
Clique expected1(std::make_shared<DiscreteConditional>((key[1] | key[3], key[4]) = "1/2 1/2 1/2 1/2"));
Clique::shared_ptr actual1 = (*bayesTree)[1];

3
gtsam/geometry/CameraSet.h
View File

@ -438,8 +438,7 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
// (DxD) += (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
// add contribution of current factor
// TODO(gareth): Eigen doesn't let us pass the expression. Call eval() for
// now...
// Eigen doesn't let us pass the expression so we call eval()
augmentedHessian.updateDiagonalBlock(
aug_i,
((FiT *

2
gtsam/geometry/Similarity2.h
View File

@ -74,7 +74,7 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
bool operator==(const Similarity2& other) const;
/// Print with optional string
void print(const std::string& s) const;
void print(const std::string& s = "") const;
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Similarity2& p);

2
gtsam/geometry/Similarity3.cpp
View File

@ -171,7 +171,7 @@ Similarity3 Similarity3::Align(const Point3Pairs &abPointPairs) {
return internal::align(d_abPointPairs, aRb, centroids);
}
Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
Similarity3 Similarity3::Align(const Pose3Pairs &abPosePairs) {
const size_t n = abPosePairs.size();
if (n < 2)
throw std::runtime_error("input should have at least 2 pairs of poses");

4
gtsam/geometry/Similarity3.h
View File

@ -75,7 +75,7 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
bool operator==(const Similarity3& other) const;
/// Print with optional string
void print(const std::string& s) const;
void print(const std::string& s = "") const;
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Similarity3& p);
@ -133,7 +133,7 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
* computed using the algorithm described here:
* http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
*/
static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
static Similarity3 Align(const Pose3Pairs& abPosePairs);
/// @}
/// @name Lie Group

2
gtsam/geometry/geometry.i
View File

@ -1082,6 +1082,7 @@ class Similarity2 {
// Standard Interface
bool equals(const gtsam::Similarity2& sim, double tol) const;
void print(const std::string& s = "") const;
Matrix matrix() const;
gtsam::Rot2& rotation();
gtsam::Point2& translation();
@ -1105,6 +1106,7 @@ class Similarity3 {
// Standard Interface
bool equals(const gtsam::Similarity3& sim, double tol) const;
void print(const std::string& s = "") const;
Matrix matrix() const;
gtsam::Rot3& rotation();
gtsam::Point3& translation();

4
gtsam/hybrid/HybridBayesTree.h
View File

@ -123,6 +123,10 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree<HybridBayesTreeClique> {
};
/// traits
template <>
struct traits<HybridBayesTreeClique> : public Testable<HybridBayesTreeClique> {
};
template <>
struct traits<HybridBayesTree> : public Testable<HybridBayesTree> {};

3
gtsam/hybrid/HybridGaussianISAM.h
View File

@ -29,7 +29,8 @@
namespace gtsam {
/**
* @brief
* @brief Incremental Smoothing and Mapping (ISAM) algorithm
* for hybrid factor graphs.
*
* @ingroup hybrid
*/

2
gtsam/hybrid/HybridNonlinearFactorGraph.cpp
View File

@ -56,8 +56,6 @@ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize(
for (auto& f : factors_) {
// First check if it is a valid factor
if (!f) {
// TODO(dellaert): why?
linearFG->push_back(GaussianFactor::shared_ptr());
continue;
}
// Check if it is a nonlinear mixture factor

4
gtsam_unstable/partition/FindSeparator-inl.h
View File

@ -39,7 +39,7 @@ namespace gtsam { namespace partition {
* whether node j is in the left part of the graph, the right part, or the
* separator, respectively
*/
std::pair<int, sharedInts> separatorMetis(idx_t n, const sharedInts& xadj,
std::pair<idx_t, sharedInts> separatorMetis(idx_t n, const sharedInts& xadj,
const sharedInts& adjncy, const sharedInts& adjwgt, bool verbose) {
// control parameters
@ -277,7 +277,7 @@ namespace gtsam { namespace partition {
//throw runtime_error("separatorPartitionByMetis:stop for debug");
}
if(result.C.size() != sepsize) {
if(result.C.size() != size_t(sepsize)) {
std::cout << "total key: " << keys.size()
<< " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " << result.C.size()
<< "; sepsize from Metis = " << sepsize << std::endl;

2
python/gtsam/tests/test_backwards_compatibility.py
View File

@ -29,7 +29,7 @@ UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2)
class TestBackwardsCompatibility(GtsamTestCase):
"""Tests for the backwards compatibility for the Python wrapper."""
"""Tests for backwards compatibility of the Python wrapper."""
def setUp(self):
"""Setup test fixtures"""