Skip to content

Commit 2e353e8

Browse files
varunagrawalvarunagrawal
authored
Merge pull request #2310 from borglab/fix-matlab
2 parents 645c574 + 3524f93 commit 2e353e8

65 files changed

Lines changed: 1577 additions & 493 deletions

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

‎gtsam/geometry/SO3.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -165,8 +165,8 @@ struct GTSAM_EXPORT ExpmapFunctor {
165165
/// Math extends Ethan theme of elegant I + aW + bWW expressions.
166166
/// See https://www.ethaneade.org/lie.pdf expmap (82) and left Jacobian (83).
167167
struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
168-
const Vector3 omega; ///< The rotation vector.
169-
bool nearPi{false}; ///< Flag indicating if theta is near pi.
168+
Vector3 omega; ///< The rotation vector.
169+
bool nearPi{false}; ///< Flag indicating if theta is near pi.
170170

171171
/// Constructor with element of Lie algebra so(3)
172172
explicit DexpFunctor(const Vector3& omega);

‎gtsam/geometry/SimpleCamera.h‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,6 @@
2323
#include <gtsam/geometry/Cal3DS2.h>
2424
#include <gtsam/geometry/Cal3Fisheye.h>
2525
#include <gtsam/geometry/Cal3Unified.h>
26-
#include <gtsam/geometry/Cal3Fisheye.h>
2726
#include <gtsam/geometry/Cal3_S2.h>
2827
#include <gtsam/geometry/PinholeCamera.h>
2928

‎gtsam/geometry/geometry.i‎

Lines changed: 6 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -221,11 +221,11 @@ namespace so3 {
221221
};
222222

223223
class ExpmapFunctor {
224-
const double theta2;
225-
const double theta;
226-
const Matrix3 W;
227-
const Matrix3 WW;
228-
const bool nearZero;
224+
double theta2;
225+
double theta;
226+
gtsam::Matrix3 W;
227+
gtsam::Matrix3 WW;
228+
bool nearZero;
229229
double A; // A = sin(theta) / theta
230230
double B; // B = (1 - cos(theta))
231231
ExpmapFunctor(const gtsam::Vector3& omega);
@@ -235,7 +235,7 @@ namespace so3 {
235235
};
236236

237237
virtual class DexpFunctor : gtsam::so3::ExpmapFunctor {
238-
const gtsam::Vector3 omega;
238+
gtsam::Vector3 omega;
239239

240240
DexpFunctor(const gtsam::Vector3& omega);
241241
DexpFunctor(const gtsam::Vector3& omega, double nearZeroThresholdSq, double nearPiThresholdSq);
@@ -291,8 +291,6 @@ class SO3 {
291291
static gtsam::Matrix3 LogmapDerivative(const gtsam::Vector3& omega);
292292
gtsam::SO3 expmap(gtsam::Vector3 v);
293293
gtsam::Vector3 logmap(const gtsam::SO3& g);
294-
static gtsam::Matrix3 Hat(const gtsam::Vector3& xi);
295-
static gtsam::Vector3 Vee(const gtsam::Matrix3& xi);
296294

297295
// Matrix Lie Group
298296
gtsam::Vector vec() const;

‎gtsam/inference/inference.i‎

Lines changed: 10 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -120,15 +120,20 @@ class Ordering {
120120
Ordering(const std::vector<size_t>& keys);
121121

122122
template <
123-
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
124-
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}>
125-
123+
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph,
124+
gtsam::DiscreteFactorGraph,
125+
gtsam::SymbolicFactorGraph,
126+
gtsam::GaussianFactorGraph,
127+
gtsam::HybridGaussianFactorGraph}>
126128
static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph);
127129
static gtsam::Ordering Colamd(const gtsam::VariableIndex& variableIndex);
128130

129131
template <
130-
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
131-
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}>
132+
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph,
133+
gtsam::DiscreteFactorGraph,
134+
gtsam::SymbolicFactorGraph,
135+
gtsam::GaussianFactorGraph,
136+
gtsam::HybridGaussianFactorGraph}>
132137
static gtsam::Ordering ColamdConstrainedLast(
133138
const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainLast,
134139
bool forceOrder = false);

‎gtsam/linear/linear.i‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -684,7 +684,7 @@ virtual class GaussianBayesTree {
684684
gtsam::DefaultKeyFormatter);
685685
size_t size() const;
686686
bool empty() const;
687-
const GaussianBayesTree::Roots& roots() const;
687+
const gtsam::GaussianBayesTree::Roots& roots() const;
688688
const gtsam::GaussianBayesTreeClique* operator[](size_t j) const;
689689
size_t numCachedSeparatorMarginals() const;
690690

‎gtsam/nonlinear/nonlinear.i‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -710,7 +710,7 @@ template <T = {double,
710710
gtsam::Pose3,
711711
gtsam::Similarity2,
712712
gtsam::Similarity3}>
713-
class ConcentratedGaussian : gtsam::ExtendedPriorFactor<T> {
713+
virtual class ConcentratedGaussian : gtsam::ExtendedPriorFactor<T> {
714714
ConcentratedGaussian();
715715
// Constructors mirroring header (origin terminology)
716716
ConcentratedGaussian(gtsam::Key key, const T& origin, const gtsam::noiseModel::Gaussian::shared_ptr& noiseModel);
@@ -726,10 +726,10 @@ class ConcentratedGaussian : gtsam::ExtendedPriorFactor<T> {
726726
double evaluate(const T& x) const;
727727
double evaluate(const gtsam::Values& values) const;
728728
// Chart transport / reset operations
729-
ConcentratedGaussian reset() const;
730-
ConcentratedGaussian transportTo(const T& x_hat) const;
729+
This reset() const;
730+
This transportTo(const T& x_hat) const;
731731
// Fusion operator
732-
ConcentratedGaussian operator*(const ConcentratedGaussian& other) const;
732+
This operator*(const This& other) const;
733733
};
734734

735735
#include <gtsam/nonlinear/NonlinearEquality.h>

‎gtsam/slam/dataset.h‎

Lines changed: 12 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -20,25 +20,26 @@
2020

2121
#pragma once
2222

23-
#include <gtsam/sfm/BinaryMeasurement.h>
24-
#include <gtsam/slam/BetweenFactor.h>
25-
#include <gtsam/sfm/SfmData.h>
23+
#include <gtsam/base/Testable.h>
24+
#include <gtsam/base/types.h>
2625
#include <gtsam/geometry/Cal3Bundler.h>
2726
#include <gtsam/geometry/PinholeCamera.h>
2827
#include <gtsam/geometry/Pose2.h>
2928
#include <gtsam/geometry/Pose3.h>
29+
#include <gtsam/geometry/SL4.h>
30+
#include <gtsam/linear/NoiseModel.h>
3031
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
3132
#include <gtsam/nonlinear/Values.h>
32-
#include <gtsam/linear/NoiseModel.h>
33-
#include <gtsam/base/Testable.h>
34-
#include <gtsam/base/types.h>
33+
#include <gtsam/sfm/BinaryMeasurement.h>
34+
#include <gtsam/sfm/SfmData.h>
35+
#include <gtsam/slam/BetweenFactor.h>
3536

36-
#include <string>
37-
#include <utility> // for pair
38-
#include <vector>
3937
#include <iosfwd>
4038
#include <map>
4139
#include <optional>
40+
#include <string>
41+
#include <utility> // for pair
42+
#include <vector>
4243

4344
namespace gtsam {
4445

@@ -221,6 +222,8 @@ parse3DFactors(const std::string &filename,
221222
const noiseModel::Diagonal::shared_ptr &model = nullptr,
222223
size_t maxIndex = 0);
223224

225+
using BetweenFactorSL4s = std::vector<BetweenFactor<SL4>::shared_ptr>;
226+
224227
using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
225228
using BinaryMeasurementsPoint3 = std::vector<BinaryMeasurement<Point3>>;
226229
using BinaryMeasurementsRot3 = std::vector<BinaryMeasurement<Rot3>>;

‎gtsam/slam/slam.i‎

Lines changed: 16 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -4,13 +4,14 @@
44

55
namespace gtsam {
66

7-
#include <gtsam/geometry/Cal3DS2.h>
87
#include <gtsam/geometry/SO4.h>
98
#include <gtsam/geometry/SL4.h>
109
#include <gtsam/navigation/ImuBias.h>
1110
#include <gtsam/geometry/Similarity2.h>
1211
#include <gtsam/geometry/Similarity3.h>
1312
#include <gtsam/geometry/Gal3.h>
13+
// Following header defines PinholeCamera{Cal3_S2|Cal3DS2|Cal3Bundler|Cal3Fisheye|Cal3Unified}
14+
#include <gtsam/geometry/SimpleCamera.h>
1415

1516
// ######
1617

@@ -154,9 +155,6 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
154155
void serialize() const;
155156
};
156157

157-
// Following header defines PinholeCamera{Cal3_S2|Cal3DS2|Cal3Bundler|Cal3Fisheye|Cal3Unified}
158-
#include <gtsam/geometry/SimpleCamera.h>
159-
160158
#include <gtsam/slam/SmartFactorBase.h>
161159

162160
// Currently not wrapping SphericalCamera, since measurement type is not Point2 but Unit3
@@ -170,8 +168,8 @@ virtual class SmartFactorBase : gtsam::NonlinearFactor {
170168
void add(const gtsam::Point2& measured, gtsam::Key key);
171169
void add(const gtsam::Point2Vector& measurements, const gtsam::KeyVector& cameraKeys);
172170
size_t dim() const;
173-
const std::vector<gtsam::Point2>& measured() const;
174-
std::vector<CAMERA> cameras(const gtsam::Values& values) const;
171+
const gtsam::Point2Vector& measured() const;
172+
gtsam::CameraSet<CAMERA> cameras(const gtsam::Values& values) const;
175173

176174
void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter =
177175
gtsam::DefaultKeyFormatter) const;
@@ -219,27 +217,27 @@ virtual class SmartProjectionFactor : gtsam::SmartFactorBase<CAMERA> {
219217
gtsam::TriangulationResult triangulateSafe(const gtsam::CameraSet<CAMERA>& cameras) const;
220218
bool triangulateForLinearize(const gtsam::CameraSet<CAMERA>& cameras) const;
221219

222-
gtsam::HessianFactor createHessianFactor(
220+
gtsam::HessianFactor* createHessianFactor(
223221
const gtsam::CameraSet<CAMERA>& cameras, const double lambda = 0.0,
224222
bool diagonalDamping = false) const;
225-
gtsam::JacobianFactor createJacobianQFactor(
223+
gtsam::JacobianFactor* createJacobianQFactor(
226224
const gtsam::CameraSet<CAMERA>& cameras, double lambda) const;
227-
gtsam::JacobianFactor createJacobianQFactor(
225+
gtsam::JacobianFactor* createJacobianQFactor(
228226
const gtsam::Values& values, double lambda) const;
229-
gtsam::JacobianFactor createJacobianSVDFactor(
227+
gtsam::JacobianFactor* createJacobianSVDFactor(
230228
const gtsam::CameraSet<CAMERA>& cameras, double lambda) const;
231-
gtsam::HessianFactor linearizeToHessian(
229+
gtsam::HessianFactor* linearizeToHessian(
232230
const gtsam::Values& values, double lambda = 0.0) const;
233-
gtsam::JacobianFactor linearizeToJacobian(
231+
gtsam::JacobianFactor* linearizeToJacobian(
234232
const gtsam::Values& values, double lambda = 0.0) const;
235233

236-
gtsam::GaussianFactor linearizeDamped(const gtsam::CameraSet<CAMERA>& cameras,
234+
gtsam::GaussianFactor* linearizeDamped(const gtsam::CameraSet<CAMERA>& cameras,
237235
const double lambda = 0.0) const;
238236

239-
gtsam::GaussianFactor linearizeDamped(const gtsam::Values& values,
237+
gtsam::GaussianFactor* linearizeDamped(const gtsam::Values& values,
240238
const double lambda = 0.0) const;
241239

242-
gtsam::GaussianFactor linearize(
240+
gtsam::GaussianFactor* linearize(
243241
const gtsam::Values& values) const;
244242

245243
bool triangulateAndComputeE(gtsam::Matrix& E, const gtsam::CameraSet<CAMERA>& cameras) const;
@@ -312,7 +310,7 @@ virtual class SmartProjectionRigFactor : gtsam::SmartProjectionFactor<CAMERA> {
312310
const gtsam::FastVector<size_t>& cameraIds = gtsam::FastVector<size_t>());
313311

314312
const gtsam::KeyVector& nonUniqueKeys() const;
315-
const gtsam::CameraSet<CAMERA>& cameraRig() const;
313+
const gtsam::CameraSet<CAMERA>* cameraRig() const;
316314
const gtsam::FastVector<size_t>& cameraIds() const;
317315
};
318316

@@ -367,6 +365,7 @@ class RotateFactor : gtsam::NoiseModelFactor {
367365

368366
gtsam::Vector evaluateError(const gtsam::Rot3& R) const;
369367
};
368+
370369
class RotateDirectionsFactor : gtsam::NoiseModelFactor {
371370
RotateDirectionsFactor(gtsam::Key key, const gtsam::Unit3& i_p, const gtsam::Unit3& c_z,
372371
const gtsam::noiseModel::Base* model);
@@ -391,6 +390,7 @@ class OrientedPlane3Factor : gtsam::NoiseModelFactor {
391390
gtsam::Vector evaluateError(
392391
const gtsam::Pose3& pose, const gtsam::OrientedPlane3& plane) const;
393392
};
393+
394394
class OrientedPlane3DirectionPrior : gtsam::NoiseModelFactor {
395395
OrientedPlane3DirectionPrior();
396396
OrientedPlane3DirectionPrior(gtsam::Key key, const gtsam::Vector& z,

‎gtsam/symbolic/symbolic.i‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -206,7 +206,7 @@ class SymbolicBayesTree {
206206
// Standard Interface
207207
bool empty() const;
208208
size_t size() const;
209-
const SymbolicBayesTree::Roots& roots() const;
209+
const gtsam::SymbolicBayesTree::Roots& roots() const;
210210
const gtsam::SymbolicBayesTreeClique* operator[](size_t j) const;
211211

212212
void saveGraph(string s,

‎matlab/CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@ set(GTSAM_TOOLBOX_INSTALL_PATH
2323
"Matlab toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/gtsam_toolbox"
2424
)
2525
if(NOT GTSAM_TOOLBOX_INSTALL_PATH)
26-
set(GTSAM_TOOLBOX_INSTALL_PATH "gtsam_toolbox")
26+
set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/gtsam_toolbox")
2727
endif()
2828

2929
set(WRAP_MEX_BUILD_STATIC_MODULE ${GTSAM_MEX_BUILD_STATIC_MODULE})

0 commit comments

Comments
 (0)