Skip to content

Commit d552eef

Browse files
varunagrawalvarunagrawal
committed
Merge branch 'develop' into model-selection-integration
2 parents 538871a + a2a7a06 commit d552eef

File tree

16 files changed

+85
-22
lines changed

16 files changed

+85
-22
lines changed

‎.github/workflows/build-python.yml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -109,7 +109,7 @@ jobs:
109109
uses: ilammy/msvc-dev-cmd@v1
110110
with:
111111
arch: x${{matrix.platform}}
112-
toolset: 14.38
112+
toolset: 14.2
113113

114114
- name: cl version (Windows)
115115
if: runner.os == 'Windows'

‎.github/workflows/build-windows.yml

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,7 @@ jobs:
5050
uses: ilammy/msvc-dev-cmd@v1
5151
with:
5252
arch: x${{ matrix.platform }}
53-
toolset: 14.38
53+
toolset: 14.2
5454

5555
- name: cl version
5656
shell: cmd
@@ -124,6 +124,7 @@ jobs:
124124
# https://stackoverflow.com/a/24470998/1236990
125125
cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam
126126
cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam_unstable
127+
cmake --build build -j4 --config ${{ matrix.build_type }} --target all.tests
127128
128129
- name: Test
129130
shell: bash

‎examples/IMUKittiExampleGPS.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -241,14 +241,14 @@ int main(int argc, char* argv[]) {
241241
"-- Starting main loop: inference is performed at each time step, but we "
242242
"plot trajectory every 10 steps\n");
243243
size_t j = 0;
244-
size_t included_imu_measurement_count = 0;
245244

246245
for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
247246
// At each non=IMU measurement we initialize a new node in the graph
248247
auto current_pose_key = X(i);
249248
auto current_vel_key = V(i);
250249
auto current_bias_key = B(i);
251250
double t = gps_measurements[i].time;
251+
size_t included_imu_measurement_count = 0;
252252

253253
if (i == first_gps_pose) {
254254
// Create initial estimate and prior on initial pose, velocity, and biases

‎gtsam/3rdparty/cephes/CMakeLists.txt

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,8 @@ set(CEPHES_HEADER_FILES
1616
cephes/lanczos.h
1717
cephes/mconf.h
1818
cephes/polevl.h
19-
cephes/sf_error.h)
19+
cephes/sf_error.h
20+
dllexport.h)
2021

2122
# Add header files
2223
install(FILES ${CEPHES_HEADER_FILES} DESTINATION include/gtsam/3rdparty/cephes)

‎gtsam/geometry/geometry.i

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -69,6 +69,7 @@ class StereoPoint2 {
6969
// Standard Constructors
7070
StereoPoint2();
7171
StereoPoint2(double uL, double uR, double v);
72+
StereoPoint2(const gtsam::Vector3 &v);
7273

7374
// Testable
7475
void print(string s = "") const;
@@ -836,6 +837,12 @@ class Cal3_S2Stereo {
836837
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b);
837838
Cal3_S2Stereo(Vector v);
838839

840+
// Manifold
841+
static size_t Dim();
842+
size_t dim() const;
843+
gtsam::Cal3_S2Stereo retract(Vector v) const;
844+
Vector localCoordinates(const gtsam::Cal3_S2Stereo& c) const;
845+
839846
// Testable
840847
void print(string s = "") const;
841848
bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const;
@@ -846,8 +853,11 @@ class Cal3_S2Stereo {
846853
double skew() const;
847854
double px() const;
848855
double py() const;
856+
Matrix K() const;
849857
gtsam::Point2 principalPoint() const;
850858
double baseline() const;
859+
Vector6 vector() const;
860+
Matrix inverse() const;
851861
};
852862

853863
#include <gtsam/geometry/Cal3Bundler.h>

‎gtsam/linear/HessianFactor.cpp

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,7 @@ using namespace std;
3838
namespace gtsam {
3939

4040
// Typedefs used in constructors below.
41-
using Dims = std::vector<Eigen::Index>;
41+
using Dims = std::vector<Key>;
4242

4343
/* ************************************************************************* */
4444
void HessianFactor::Allocate(const Scatter& scatter) {
@@ -72,7 +72,7 @@ HessianFactor::HessianFactor() :
7272

7373
/* ************************************************************************* */
7474
HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f)
75-
: GaussianFactor(KeyVector{j}), info_(Dims{G.cols(), 1}) {
75+
: GaussianFactor(KeyVector{j}), info_(Dims{static_cast<Key>(G.cols()), 1}) {
7676
if (G.rows() != G.cols() || G.rows() != g.size())
7777
throw invalid_argument(
7878
"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)
8585
// error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu)
8686
// where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g
8787
HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma)
88-
: GaussianFactor(KeyVector{j}), info_(Dims{Sigma.cols(), 1}) {
88+
: GaussianFactor(KeyVector{j}), info_(Dims{static_cast<Key>(Sigma.cols()), 1}) {
8989
if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size())
9090
throw invalid_argument(
9191
"Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
@@ -99,7 +99,7 @@ HessianFactor::HessianFactor(Key j1, Key j2, const Matrix& G11,
9999
const Matrix& G12, const Vector& g1, const Matrix& G22, const Vector& g2,
100100
double f) :
101101
GaussianFactor(KeyVector{j1,j2}), info_(
102-
Dims{G11.cols(),G22.cols(),1}) {
102+
Dims{static_cast<Key>(G11.cols()),static_cast<Key>(G22.cols()),1}) {
103103
info_.setDiagonalBlock(0, G11);
104104
info_.setOffDiagonalBlock(0, 1, G12);
105105
info_.setDiagonalBlock(1, G22);
@@ -113,7 +113,7 @@ HessianFactor::HessianFactor(Key j1, Key j2, Key j3, const Matrix& G11,
113113
const Matrix& G23, const Vector& g2, const Matrix& G33, const Vector& g3,
114114
double f) :
115115
GaussianFactor(KeyVector{j1,j2,j3}), info_(
116-
Dims{G11.cols(),G22.cols(),G33.cols(),1}) {
116+
Dims{static_cast<Key>(G11.cols()),static_cast<Key>(G22.cols()),static_cast<Key>(G33.cols()),1}) {
117117
if (G11.rows() != G11.cols() || G11.rows() != G12.rows()
118118
|| G11.rows() != G13.rows() || G11.rows() != g1.size()
119119
|| G22.cols() != G12.cols() || G33.cols() != G13.cols()

‎gtsam/linear/JacobianFactor.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -40,8 +40,8 @@ using namespace std;
4040
namespace gtsam {
4141

4242
// Typedefs used in constructors below.
43-
using Dims = std::vector<Eigen::Index>;
44-
using Pairs = std::vector<std::pair<Eigen::Index, Matrix>>;
43+
using Dims = std::vector<Key>;
44+
using Pairs = std::vector<std::pair<Key, Matrix>>;
4545

4646
/* ************************************************************************* */
4747
JacobianFactor::JacobianFactor() :

‎gtsam/linear/VectorValues.cpp

Lines changed: 14 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -64,6 +64,13 @@ namespace gtsam {
6464
}
6565
}
6666

67+
/* ************************************************************************ */
68+
std::map<Key, Vector> VectorValues::sorted() const {
69+
std::map<Key, Vector> ordered;
70+
for (const auto& kv : *this) ordered.emplace(kv);
71+
return ordered;
72+
}
73+
6774
/* ************************************************************************ */
6875
VectorValues VectorValues::Zero(const VectorValues& other)
6976
{
@@ -130,11 +137,7 @@ namespace gtsam {
130137
GTSAM_EXPORT std::ostream& operator<<(std::ostream& os, const VectorValues& v) {
131138
// Change print depending on whether we are using TBB
132139
#ifdef GTSAM_USE_TBB
133-
std::map<Key, Vector> sorted;
134-
for (const auto& [key, value] : v) {
135-
sorted.emplace(key, value);
136-
}
137-
for (const auto& [key, value] : sorted)
140+
for (const auto& [key, value] : v.sorted())
138141
#else
139142
for (const auto& [key,value] : v)
140143
#endif
@@ -176,7 +179,12 @@ namespace gtsam {
176179
// Copy vectors
177180
Vector result(totalDim);
178181
DenseIndex pos = 0;
182+
#ifdef GTSAM_USE_TBB
183+
// TBB uses un-ordered map, so inefficiently order them:
184+
for (const auto& [key, value] : sorted()) {
185+
#else
179186
for (const auto& [key, value] : *this) {
187+
#endif
180188
result.segment(pos, value.size()) = value;
181189
pos += value.size();
182190
}
@@ -392,9 +400,7 @@ namespace gtsam {
392400
// Print out all rows.
393401
#ifdef GTSAM_USE_TBB
394402
// TBB uses un-ordered map, so inefficiently order them:
395-
std::map<Key, Vector> ordered;
396-
for (const auto& kv : *this) ordered.emplace(kv);
397-
for (const auto& kv : ordered) {
403+
for (const auto& kv : sorted()) {
398404
#else
399405
for (const auto& kv : *this) {
400406
#endif

‎gtsam/linear/VectorValues.h

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -77,6 +77,9 @@ namespace gtsam {
7777
typedef ConcurrentMap<Key, Vector> Values; ///< Collection of Vectors making up a VectorValues
7878
Values values_; ///< Vectors making up this VectorValues
7979

80+
/** Sort by key (primarily for use with TBB, which uses an unordered map)*/
81+
std::map<Key, Vector> sorted() const;
82+
8083
public:
8184
typedef Values::iterator iterator; ///< Iterator over vector values
8285
typedef Values::const_iterator const_iterator; ///< Const iterator over vector values

‎gtsam/linear/linear.i

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -128,6 +128,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base {
128128

129129
virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
130130
Cauchy(double k);
131+
Cauchy(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
131132
static gtsam::noiseModel::mEstimator::Cauchy* Create(double k);
132133

133134
// enabling serialization functionality
@@ -139,6 +140,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
139140

140141
virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
141142
Tukey(double k);
143+
Tukey(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
142144
static gtsam::noiseModel::mEstimator::Tukey* Create(double k);
143145

144146
// enabling serialization functionality
@@ -150,6 +152,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
150152

151153
virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
152154
Welsch(double k);
155+
Welsch(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
153156
static gtsam::noiseModel::mEstimator::Welsch* Create(double k);
154157

155158
// enabling serialization functionality
@@ -161,6 +164,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
161164

162165
virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
163166
GemanMcClure(double c);
167+
GemanMcClure(double c, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
164168
static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c);
165169

166170
// enabling serialization functionality
@@ -172,6 +176,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
172176

173177
virtual class DCS: gtsam::noiseModel::mEstimator::Base {
174178
DCS(double c);
179+
DCS(double c, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
175180
static gtsam::noiseModel::mEstimator::DCS* Create(double c);
176181

177182
// enabling serialization functionality
@@ -183,6 +188,7 @@ virtual class DCS: gtsam::noiseModel::mEstimator::Base {
183188

184189
virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base {
185190
L2WithDeadZone(double k);
191+
L2WithDeadZone(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
186192
static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k);
187193

188194
// enabling serialization functionality
@@ -193,6 +199,7 @@ virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base {
193199
};
194200

195201
virtual class AsymmetricTukey: gtsam::noiseModel::mEstimator::Base {
202+
AsymmetricTukey(double k);
196203
AsymmetricTukey(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
197204
static gtsam::noiseModel::mEstimator::AsymmetricTukey* Create(double k);
198205

@@ -203,6 +210,18 @@ virtual class AsymmetricTukey: gtsam::noiseModel::mEstimator::Base {
203210
double loss(double error) const;
204211
};
205212

213+
virtual class AsymmetricCauchy: gtsam::noiseModel::mEstimator::Base {
214+
AsymmetricCauchy(double k);
215+
AsymmetricCauchy(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
216+
static gtsam::noiseModel::mEstimator::AsymmetricCauchy* Create(double k);
217+
218+
// enabling serialization functionality
219+
void serializable() const;
220+
221+
double weight(double error) const;
222+
double loss(double error) const;
223+
};
224+
206225
virtual class Custom: gtsam::noiseModel::mEstimator::Base {
207226
Custom(gtsam::noiseModel::mEstimator::CustomWeightFunction weight,
208227
gtsam::noiseModel::mEstimator::CustomLossFunction loss,
@@ -356,6 +375,8 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
356375
void serialize() const;
357376
};
358377

378+
pair<gtsam::GaussianConditional, gtsam::JacobianFactor*> EliminateQR(const gtsam::GaussianFactorGraph& factors, const gtsam::Ordering& keys);
379+
359380
#include <gtsam/linear/HessianFactor.h>
360381
virtual class HessianFactor : gtsam::GaussianFactor {
361382
//Constructors

‎gtsam/navigation/PreintegrationBase.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -129,7 +129,7 @@ NavState PreintegrationBase::predict(const NavState& state_i,
129129
Matrix9 D_predict_state, D_predict_delta;
130130
NavState state_j = state_i.retract(xi,
131131
H1 ? &D_predict_state : nullptr,
132-
H2 || H2 ? &D_predict_delta : nullptr);
132+
H1 || H2 ? &D_predict_delta : nullptr);
133133
if (H1)
134134
*H1 = D_predict_state + D_predict_delta * D_delta_state;
135135
if (H2)

‎gtsam/navigation/PreintegrationCombinedParams.h

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,6 @@
2525
/* GTSAM includes */
2626
#include <gtsam/base/Matrix.h>
2727
#include <gtsam/navigation/ManifoldPreintegration.h>
28-
#include <gtsam/navigation/PreintegrationCombinedParams.h>
2928
#include <gtsam/navigation/TangentPreintegration.h>
3029
#include <gtsam/nonlinear/NonlinearFactor.h>
3130

‎gtsam/navigation/navigation.i

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -200,7 +200,7 @@ class PreintegratedCombinedMeasurements {
200200
const gtsam::imuBias::ConstantBias& bias) const;
201201
};
202202

203-
virtual class CombinedImuFactor: gtsam::NonlinearFactor {
203+
virtual class CombinedImuFactor: gtsam::NoiseModelFactor {
204204
CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j,
205205
size_t bias_i, size_t bias_j,
206206
const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements);

‎gtsam/nonlinear/nonlinear.i

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -130,6 +130,7 @@ virtual class NonlinearFactor : gtsam::Factor {
130130
virtual class NoiseModelFactor : gtsam::NonlinearFactor {
131131
bool equals(const gtsam::NoiseModelFactor& other, double tol) const;
132132
gtsam::noiseModel::Base* noiseModel() const;
133+
gtsam::NoiseModelFactor* cloneWithNewNoiseModel(gtsam::noiseModel::Base* newNoise) const;
133134
Vector unwhitenedError(const gtsam::Values& x) const;
134135
Vector whitenedError(const gtsam::Values& x) const;
135136
};
@@ -320,6 +321,8 @@ virtual class GncParams {
320321
enum Verbosity {
321322
SILENT,
322323
SUMMARY,
324+
MU,
325+
WEIGHTS,
323326
VALUES
324327
};
325328
};

‎gtsam/slam/StereoFactor.h

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -169,6 +169,11 @@ class GenericStereoFactor: public NoiseModelFactorN<POSE, LANDMARK> {
169169
/** return flag for throwing cheirality exceptions */
170170
inline bool throwCheirality() const { return throwCheirality_; }
171171

172+
/** return the (optional) sensor pose with respect to the vehicle frame */
173+
const std::optional<POSE>& body_P_sensor() const {
174+
return body_P_sensor_;
175+
}
176+
172177
private:
173178
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
174179
/** Serialization function */

‎gtsam/slam/slam.i

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -171,6 +171,20 @@ virtual class GenericStereoFactor : gtsam::NoiseModelFactor {
171171
GenericStereoFactor(const gtsam::StereoPoint2& measured,
172172
const gtsam::noiseModel::Base* noiseModel, size_t poseKey,
173173
size_t landmarkKey, const gtsam::Cal3_S2Stereo* K);
174+
GenericStereoFactor(const gtsam::StereoPoint2& measured,
175+
const gtsam::noiseModel::Base* noiseModel, size_t poseKey,
176+
size_t landmarkKey, const gtsam::Cal3_S2Stereo* K,
177+
POSE body_P_sensor);
178+
179+
GenericStereoFactor(const gtsam::StereoPoint2& measured,
180+
const gtsam::noiseModel::Base* noiseModel, size_t poseKey,
181+
size_t landmarkKey, const gtsam::Cal3_S2Stereo* K,
182+
bool throwCheirality, bool verboseCheirality);
183+
GenericStereoFactor(const gtsam::StereoPoint2& measured,
184+
const gtsam::noiseModel::Base* noiseModel, size_t poseKey,
185+
size_t landmarkKey, const gtsam::Cal3_S2Stereo* K,
186+
bool throwCheirality, bool verboseCheirality,
187+
POSE body_P_sensor);
174188
gtsam::StereoPoint2 measured() const;
175189
gtsam::Cal3_S2Stereo* calibration() const;
176190

0 commit comments

Comments
 (0)