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

Merge branch 'fix/windows-tests' into fix/windows3

release/4.3a0
Varun Agrawal 2023-06-30 15:25:37 -04:00
parent 9aa67b5235 a9d3a10032
commit bc51920ad8
12 changed files with 80 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -101,23 +101,31 @@ jobs:
run: |
# Since Visual Studio is a multi-generator, we need to use --config
# https://stackoverflow.com/a/24470998/1236990
cmake --build build -j 4 --config ${{ matrix.build_type }} --target gtsam
cmake --build build -j 4 --config ${{ matrix.build_type }} --target gtsam_unstable
cmake --build build -j 4 --config ${{ matrix.build_type }} --target wrap
cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam
cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam_unstable
cmake --build build -j4 --config ${{ matrix.build_type }} --target wrap
# Run GTSAM tests
cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base
cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.basis
cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.discrete
#cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.geometry
cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.inference
cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.linear
cmake --build build -j 4 --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 -j 4 --config ${{ matrix.build_type }} --target check.sfm
#cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.slam
cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.symbolic
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
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.inference
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 -j4 --config ${{ matrix.build_type }} --target check.sam
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sfm
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.symbolic
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.hybrid
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear
# 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
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry_unstable
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear_unstable
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete_unstable
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.dynamics_unstable
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear_unstable
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam_unstable
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.partition

3
gtsam/discrete/TableFactor.cpp
View File

@ -21,7 +21,6 @@
#include <gtsam/discrete/TableFactor.h>
#include <gtsam/hybrid/HybridValues.h>
#include <boost/format.hpp>
#include <utility>
using namespace std;
@ -203,7 +202,7 @@ void TableFactor::print(const string& s, const KeyFormatter& formatter) const {
cout << s;
cout << " f[";
for (auto&& key : keys())
cout << boost::format(" (%1%,%2%),") % formatter(key) % cardinality(key);
cout << " (" << formatter(key) << "," << cardinality(key) << "),";
cout << " ]" << endl;
for (SparseIt it(sparse_table_); it; ++it) {
DiscreteValues assignment = findAssignments(it.index());

11
gtsam/geometry/CameraSet.h
View File

@ -131,12 +131,15 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
return z;
}
/** An overload o the project2 function to accept
/** An overload of the project2 function to accept
* full matrices and vectors and pass it to the pointer
* version of the function
* version of the function.
*
* Use SFINAE to resolve overload ambiguity.
*/
template <class POINT, class... OptArgs>
ZVector project2(const POINT& point, OptArgs&... args) const {
typename std::enable_if<(sizeof...(OptArgs) != 0), ZVector>::type project2(
const POINT& point, OptArgs&... args) const {
// pass it to the pointer version of the function
return project2(point, (&args)...);
}
@ -270,7 +273,7 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
// Get map from key to location in the new augmented Hessian matrix (the
// one including only unique keys).
std::map<Key, size_t> keyToSlotMap;
std::map<Key, uint32_t> keyToSlotMap;
for (size_t k = 0; k < nrUniqueKeys; k++) {
keyToSlotMap[hessianKeys[k]] = k;
}

34
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,10 +192,10 @@ 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 GTSAM_EXPORT ChartAtOrigin {
@ -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;

4
gtsam/geometry/StereoPoint2.h
View File

@ -46,7 +46,9 @@ public:
uL_(0), uR_(0), v_(0) {
}
/** constructor */
/** uL and uR represent the x-axis value of left and right frame coordinates respectively.
v represents the y coordinate value. The y-axis value should be the same under the
stereo constraint. */
StereoPoint2(double uL, double uR, double v) :
uL_(uL), uR_(uR), v_(v) {
}

2
gtsam/inference/Ordering.h
View File

@ -77,9 +77,11 @@ public:
* @param keys The key vector to append to this ordering.
* @return The ordering variable with appended keys.
*/
GTSAM_EXPORT
This& operator+=(KeyVector& keys);
/// Check if key exists in ordering.
GTSAM_EXPORT
bool contains(const Key& key) const;
/**

2
gtsam_unstable/linear/LP.h
View File

@ -29,7 +29,7 @@ namespace gtsam {
using namespace std;
/// Mapping between variable's key and its corresponding dimensionality
using KeyDimMap = std::map<Key, size_t>;
using KeyDimMap = std::map<Key, uint32_t>;
/*
* Iterates through every factor in a linear graph and generates a
* mapping between every factor key and it's corresponding dimensionality.

4
gtsam_unstable/linear/LPInitSolver.cpp
View File

@ -65,9 +65,7 @@ GaussianFactorGraph::shared_ptr LPInitSolver::buildInitOfInitGraph() const {
new GaussianFactorGraph(lp_.equalities));
// create factor ||x||^2 and add to the graph
const KeyDimMap& constrainedKeyDim = lp_.constrainedKeyDimMap();
for (const auto& [key, _] : constrainedKeyDim) {
size_t dim = constrainedKeyDim.at(key);
for (const auto& [key, dim] : lp_.constrainedKeyDimMap()) {
initGraph->push_back(
JacobianFactor(key, Matrix::Identity(dim, dim), Vector::Zero(dim)));
}

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_;

38
gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
View File

@ -201,10 +201,9 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP
}
/// linearize and return a Hessianfactor that is an approximation of error(p)
std::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor(
const Values& values, const double lambda = 0.0, bool diagonalDamping =
false) const {
std::shared_ptr<RegularHessianFactor<DimPose>> createHessianFactor(
const Values& values, const double lambda = 0.0,
bool diagonalDamping = false) const {
// we may have multiple cameras sharing the same extrinsic cals, hence the number
// of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we
// have a body key and an extrinsic calibration key for each measurement)
@ -212,23 +211,22 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP
// Create structures for Hessian Factors
KeyVector js;
std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
std::vector<Vector> gs(nrUniqueKeys);
if (this->measured_.size() != cameras(values).size())
throw std::runtime_error("SmartStereoProjectionHessianFactor: this->"
"measured_.size() inconsistent with input");
"measured_.size() inconsistent with input");
// triangulate 3D point at given linearization point
triangulateSafe(cameras(values));
if (!result_) { // failed: return "empty/zero" Hessian
for (Matrix& m : Gs)
m = Matrix::Zero(DimPose, DimPose);
for (Vector& v : gs)
v = Vector::Zero(DimPose);
return std::make_shared < RegularHessianFactor<DimPose>
> (keys_, Gs, gs, 0.0);
// failed: return "empty/zero" Hessian
if (!result_) {
for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose);
for (Vector& v : gs) v = Vector::Zero(DimPose);
return std::make_shared<RegularHessianFactor<DimPose>>(keys_, Gs, gs,
0.0);
}
// compute Jacobian given triangulated 3D Point
@ -239,12 +237,13 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP
// Whiten using noise model
noiseModel_->WhitenSystem(E, b);
for (size_t i = 0; i < Fs.size(); i++)
for (size_t i = 0; i < Fs.size(); i++) {
Fs[i] = noiseModel_->Whiten(Fs[i]);
}
// build augmented Hessian (with last row/column being the information vector)
Matrix3 P;
Cameras::ComputePointCovariance <3> (P, E, lambda, diagonalDamping);
Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
// these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement)
KeyVector nonuniqueKeys;
@ -254,11 +253,12 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP
}
// but we need to get the augumented hessian wrt the unique keys in key_
SymmetricBlockMatrix augmentedHessianUniqueKeys =
Cameras::SchurComplementAndRearrangeBlocks<3,DimBlock,DimPose>(Fs,E,P,b,
nonuniqueKeys, keys_);
Base::Cameras::template SchurComplementAndRearrangeBlocks<3, DimBlock,
DimPose>(
Fs, E, P, b, nonuniqueKeys, keys_);
return std::make_shared < RegularHessianFactor<DimPose>
> (keys_, augmentedHessianUniqueKeys);
return std::make_shared<RegularHessianFactor<DimPose>>(
keys_, augmentedHessianUniqueKeys);
}
/**

2
tests/CMakeLists.txt
View File

@ -13,7 +13,7 @@ if (NOT GTSAM_USE_BOOST_FEATURES)
endif()
if (NOT GTSAM_ENABLE_BOOST_SERIALIZATION)
list(APPEND excluded_tests "testSerializationSLAM.cpp")
list(APPEND excluded_tests "testSerializationSlam.cpp")
endif()
# Build tests