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

Merge branch 'develop' into model-selection-integration

release/4.3a0
Varun Agrawal 2024-06-26 15:39:22 -04:00
parent 538871a95d a2a7a06646
commit d552eef26b
16 changed files with 85 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.github/workflows/build-python.yml vendored
View File

@ -109,7 +109,7 @@ jobs:
uses: ilammy/msvc-dev-cmd@v1
with:
arch: x${{matrix.platform}}
toolset: 14.38
toolset: 14.2
- name: cl version (Windows)
if: runner.os == 'Windows'

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

@ -50,7 +50,7 @@ jobs:
uses: ilammy/msvc-dev-cmd@v1
with:
arch: x${{ matrix.platform }}
toolset: 14.38
toolset: 14.2
- name: cl version
shell: cmd
@ -124,6 +124,7 @@ jobs:
# 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 -j4 --config ${{ matrix.build_type }} --target all.tests
- name: Test
shell: bash

2
examples/IMUKittiExampleGPS.cpp
View File

@ -241,7 +241,6 @@ int main(int argc, char* argv[]) {
"-- Starting main loop: inference is performed at each time step, but we "
"plot trajectory every 10 steps\n");
size_t j = 0;
size_t included_imu_measurement_count = 0;
for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
// At each non=IMU measurement we initialize a new node in the graph
@ -249,6 +248,7 @@ int main(int argc, char* argv[]) {
auto current_vel_key = V(i);
auto current_bias_key = B(i);
double t = gps_measurements[i].time;
size_t included_imu_measurement_count = 0;
if (i == first_gps_pose) {
// Create initial estimate and prior on initial pose, velocity, and biases

3
gtsam/3rdparty/cephes/CMakeLists.txt vendored
View File

@ -16,7 +16,8 @@ set(CEPHES_HEADER_FILES
cephes/lanczos.h
cephes/mconf.h
cephes/polevl.h
cephes/sf_error.h)
cephes/sf_error.h
dllexport.h)
# Add header files
install(FILES ${CEPHES_HEADER_FILES} DESTINATION include/gtsam/3rdparty/cephes)

10
gtsam/geometry/geometry.i
View File

@ -69,6 +69,7 @@ class StereoPoint2 {
// Standard Constructors
StereoPoint2();
StereoPoint2(double uL, double uR, double v);
StereoPoint2(const gtsam::Vector3 &v);
// Testable
void print(string s = "") const;
@ -836,6 +837,12 @@ class Cal3_S2Stereo {
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b);
Cal3_S2Stereo(Vector v);
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Cal3_S2Stereo retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3_S2Stereo& c) const;
// Testable
void print(string s = "") const;
bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const;
@ -846,8 +853,11 @@ class Cal3_S2Stereo {
double skew() const;
double px() const;
double py() const;
Matrix K() const;
gtsam::Point2 principalPoint() const;
double baseline() const;
Vector6 vector() const;
Matrix inverse() const;
};
#include <gtsam/geometry/Cal3Bundler.h>

10
gtsam/linear/HessianFactor.cpp
View File

@ -38,7 +38,7 @@ using namespace std;
namespace gtsam {
// Typedefs used in constructors below.
using Dims = std::vector<Eigen::Index>;
using Dims = std::vector<Key>;
/* ************************************************************************* */
void HessianFactor::Allocate(const Scatter& scatter) {
@ -72,7 +72,7 @@ HessianFactor::HessianFactor() :
/* ************************************************************************* */
HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f)
: GaussianFactor(KeyVector{j}), info_(Dims{G.cols(), 1}) {
: GaussianFactor(KeyVector{j}), info_(Dims{static_cast<Key>(G.cols()), 1}) {
if (G.rows() != G.cols() || G.rows() != g.size())
throw invalid_argument(
"Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
@ -85,7 +85,7 @@ HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f)
// error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu)
// where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g
HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma)
: GaussianFactor(KeyVector{j}), info_(Dims{Sigma.cols(), 1}) {
: GaussianFactor(KeyVector{j}), info_(Dims{static_cast<Key>(Sigma.cols()), 1}) {
if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size())
throw invalid_argument(
"Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
@ -99,7 +99,7 @@ HessianFactor::HessianFactor(Key j1, Key j2, const Matrix& G11,
const Matrix& G12, const Vector& g1, const Matrix& G22, const Vector& g2,
double f) :
GaussianFactor(KeyVector{j1,j2}), info_(
Dims{G11.cols(),G22.cols(),1}) {
Dims{static_cast<Key>(G11.cols()),static_cast<Key>(G22.cols()),1}) {
info_.setDiagonalBlock(0, G11);
info_.setOffDiagonalBlock(0, 1, G12);
info_.setDiagonalBlock(1, G22);
@ -113,7 +113,7 @@ HessianFactor::HessianFactor(Key j1, Key j2, Key j3, const Matrix& G11,
const Matrix& G23, const Vector& g2, const Matrix& G33, const Vector& g3,
double f) :
GaussianFactor(KeyVector{j1,j2,j3}), info_(
Dims{G11.cols(),G22.cols(),G33.cols(),1}) {
Dims{static_cast<Key>(G11.cols()),static_cast<Key>(G22.cols()),static_cast<Key>(G33.cols()),1}) {
if (G11.rows() != G11.cols() || G11.rows() != G12.rows()
|| G11.rows() != G13.rows() || G11.rows() != g1.size()
|| G22.cols() != G12.cols() || G33.cols() != G13.cols()

4
gtsam/linear/JacobianFactor.cpp
View File

@ -40,8 +40,8 @@ using namespace std;
namespace gtsam {
// Typedefs used in constructors below.
using Dims = std::vector<Eigen::Index>;
using Pairs = std::vector<std::pair<Eigen::Index, Matrix>>;
using Dims = std::vector<Key>;
using Pairs = std::vector<std::pair<Key, Matrix>>;
/* ************************************************************************* */
JacobianFactor::JacobianFactor() :

22
gtsam/linear/VectorValues.cpp
View File

@ -64,6 +64,13 @@ namespace gtsam {
}
}
/* ************************************************************************ */
std::map<Key, Vector> VectorValues::sorted() const {
std::map<Key, Vector> ordered;
for (const auto& kv : *this) ordered.emplace(kv);
return ordered;
}
/* ************************************************************************ */
VectorValues VectorValues::Zero(const VectorValues& other)
{
@ -130,11 +137,7 @@ namespace gtsam {
GTSAM_EXPORT std::ostream& operator<<(std::ostream& os, const VectorValues& v) {
// Change print depending on whether we are using TBB
#ifdef GTSAM_USE_TBB
std::map<Key, Vector> sorted;
for (const auto& [key, value] : v) {
sorted.emplace(key, value);
}
for (const auto& [key, value] : sorted)
for (const auto& [key, value] : v.sorted())
#else
for (const auto& [key,value] : v)
#endif
@ -176,7 +179,12 @@ namespace gtsam {
// Copy vectors
Vector result(totalDim);
DenseIndex pos = 0;
#ifdef GTSAM_USE_TBB
// TBB uses un-ordered map, so inefficiently order them:
for (const auto& [key, value] : sorted()) {
#else
for (const auto& [key, value] : *this) {
#endif
result.segment(pos, value.size()) = value;
pos += value.size();
}
@ -392,9 +400,7 @@ namespace gtsam {
// Print out all rows.
#ifdef GTSAM_USE_TBB
// TBB uses un-ordered map, so inefficiently order them:
std::map<Key, Vector> ordered;
for (const auto& kv : *this) ordered.emplace(kv);
for (const auto& kv : ordered) {
for (const auto& kv : sorted()) {
#else
for (const auto& kv : *this) {
#endif

3
gtsam/linear/VectorValues.h
View File

@ -77,6 +77,9 @@ namespace gtsam {
typedef ConcurrentMap<Key, Vector> Values; ///< Collection of Vectors making up a VectorValues
Values values_; ///< Vectors making up this VectorValues
/** Sort by key (primarily for use with TBB, which uses an unordered map)*/
std::map<Key, Vector> sorted() const;
public:
typedef Values::iterator iterator; ///< Iterator over vector values
typedef Values::const_iterator const_iterator; ///< Const iterator over vector values

21
gtsam/linear/linear.i
View File

@ -128,6 +128,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base {
virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
Cauchy(double k);
Cauchy(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
static gtsam::noiseModel::mEstimator::Cauchy* Create(double k);
// enabling serialization functionality
@ -139,6 +140,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
Tukey(double k);
Tukey(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
static gtsam::noiseModel::mEstimator::Tukey* Create(double k);
// enabling serialization functionality
@ -150,6 +152,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
Welsch(double k);
Welsch(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
static gtsam::noiseModel::mEstimator::Welsch* Create(double k);
// enabling serialization functionality
@ -161,6 +164,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
GemanMcClure(double c);
GemanMcClure(double c, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c);
// enabling serialization functionality
@ -172,6 +176,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
virtual class DCS: gtsam::noiseModel::mEstimator::Base {
DCS(double c);
DCS(double c, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
static gtsam::noiseModel::mEstimator::DCS* Create(double c);
// enabling serialization functionality
@ -183,6 +188,7 @@ virtual class DCS: gtsam::noiseModel::mEstimator::Base {
virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base {
L2WithDeadZone(double k);
L2WithDeadZone(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k);
// enabling serialization functionality
@ -193,6 +199,7 @@ virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base {
};
virtual class AsymmetricTukey: gtsam::noiseModel::mEstimator::Base {
AsymmetricTukey(double k);
AsymmetricTukey(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
static gtsam::noiseModel::mEstimator::AsymmetricTukey* Create(double k);
@ -203,6 +210,18 @@ virtual class AsymmetricTukey: gtsam::noiseModel::mEstimator::Base {
double loss(double error) const;
};
virtual class AsymmetricCauchy: gtsam::noiseModel::mEstimator::Base {
AsymmetricCauchy(double k);
AsymmetricCauchy(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
static gtsam::noiseModel::mEstimator::AsymmetricCauchy* Create(double k);
// enabling serialization functionality
void serializable() const;
double weight(double error) const;
double loss(double error) const;
};
virtual class Custom: gtsam::noiseModel::mEstimator::Base {
Custom(gtsam::noiseModel::mEstimator::CustomWeightFunction weight,
gtsam::noiseModel::mEstimator::CustomLossFunction loss,
@ -356,6 +375,8 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
void serialize() const;
};
pair<gtsam::GaussianConditional, gtsam::JacobianFactor*> EliminateQR(const gtsam::GaussianFactorGraph& factors, const gtsam::Ordering& keys);
#include <gtsam/linear/HessianFactor.h>
virtual class HessianFactor : gtsam::GaussianFactor {
//Constructors

2
gtsam/navigation/PreintegrationBase.cpp
View File

@ -129,7 +129,7 @@ NavState PreintegrationBase::predict(const NavState& state_i,
Matrix9 D_predict_state, D_predict_delta;
NavState state_j = state_i.retract(xi,
H1 ? &D_predict_state : nullptr,
H2 || H2 ? &D_predict_delta : nullptr);
H1 || H2 ? &D_predict_delta : nullptr);
if (H1)
*H1 = D_predict_state + D_predict_delta * D_delta_state;
if (H2)

1
gtsam/navigation/PreintegrationCombinedParams.h
View File

@ -25,7 +25,6 @@
/* GTSAM includes */
#include <gtsam/base/Matrix.h>
#include <gtsam/navigation/ManifoldPreintegration.h>
#include <gtsam/navigation/PreintegrationCombinedParams.h>
#include <gtsam/navigation/TangentPreintegration.h>
#include <gtsam/nonlinear/NonlinearFactor.h>

2
gtsam/navigation/navigation.i
View File

@ -200,7 +200,7 @@ class PreintegratedCombinedMeasurements {
const gtsam::imuBias::ConstantBias& bias) const;
};
virtual class CombinedImuFactor: gtsam::NonlinearFactor {
virtual class CombinedImuFactor: gtsam::NoiseModelFactor {
CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j,
size_t bias_i, size_t bias_j,
const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements);

3
gtsam/nonlinear/nonlinear.i
View File

@ -130,6 +130,7 @@ virtual class NonlinearFactor : gtsam::Factor {
virtual class NoiseModelFactor : gtsam::NonlinearFactor {
bool equals(const gtsam::NoiseModelFactor& other, double tol) const;
gtsam::noiseModel::Base* noiseModel() const;
gtsam::NoiseModelFactor* cloneWithNewNoiseModel(gtsam::noiseModel::Base* newNoise) const;
Vector unwhitenedError(const gtsam::Values& x) const;
Vector whitenedError(const gtsam::Values& x) const;
};
@ -320,6 +321,8 @@ virtual class GncParams {
enum Verbosity {
SILENT,
SUMMARY,
MU,
WEIGHTS,
VALUES
};
};

5
gtsam/slam/StereoFactor.h
View File

@ -169,6 +169,11 @@ public:
/** return flag for throwing cheirality exceptions */
inline bool throwCheirality() const { return throwCheirality_; }
/** return the (optional) sensor pose with respect to the vehicle frame */
const std::optional<POSE>& body_P_sensor() const {
return body_P_sensor_;
}
private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */

14
gtsam/slam/slam.i
View File

@ -171,6 +171,20 @@ virtual class GenericStereoFactor : gtsam::NoiseModelFactor {
GenericStereoFactor(const gtsam::StereoPoint2& measured,
const gtsam::noiseModel::Base* noiseModel, size_t poseKey,
size_t landmarkKey, const gtsam::Cal3_S2Stereo* K);
GenericStereoFactor(const gtsam::StereoPoint2& measured,
const gtsam::noiseModel::Base* noiseModel, size_t poseKey,
size_t landmarkKey, const gtsam::Cal3_S2Stereo* K,
POSE body_P_sensor);
GenericStereoFactor(const gtsam::StereoPoint2& measured,
const gtsam::noiseModel::Base* noiseModel, size_t poseKey,
size_t landmarkKey, const gtsam::Cal3_S2Stereo* K,
bool throwCheirality, bool verboseCheirality);
GenericStereoFactor(const gtsam::StereoPoint2& measured,
const gtsam::noiseModel::Base* noiseModel, size_t poseKey,
size_t landmarkKey, const gtsam::Cal3_S2Stereo* K,
bool throwCheirality, bool verboseCheirality,
POSE body_P_sensor);
gtsam::StereoPoint2 measured() const;
gtsam::Cal3_S2Stereo* calibration() const;