Skip to content

Commit 00f9fca

Browse files
koide3koide3
authored
Merge pull request #91 from koide3/gtsam4.2
Gtsam4.2
2 parents 7d095aa + 438bdd5 commit 00f9fca

File tree

48 files changed

+2324
-82
lines changed

Some content is hidden

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

48 files changed

+2324
-82
lines changed

‎CMakeLists.txt‎

Lines changed: 15 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -30,10 +30,19 @@ if(BUILD_WITH_MARCH_NATIVE)
3030
endif()
3131

3232
find_package(Boost REQUIRED COMPONENTS graph filesystem)
33-
find_package(GTSAM 4.3 REQUIRED)
34-
find_package(GTSAM_UNSTABLE 4.3 REQUIRED)
33+
find_package(GTSAM 4.2 REQUIRED)
34+
find_package(GTSAM_UNSTABLE 4.2 REQUIRED)
3535
find_package(Eigen3 REQUIRED)
3636

37+
# Set optimizer source paths based on GTSAM version
38+
if(GTSAM_VERSION VERSION_LESS "4.3")
39+
set(GTSAM_POINTS_ISAM2_SRC_DIR "gtsam4.2/")
40+
message(STATUS "Using ISAM2 sources for GTSAM4.2")
41+
else()
42+
set(GTSAM_POINTS_ISAM2_SRC_DIR "")
43+
message(STATUS "Using default ISAM2 sources")
44+
endif()
45+
3746
if(BUILD_WITH_TBB)
3847
find_package(TBB REQUIRED)
3948
set(GTSAM_POINTS_USE_TBB 1)
@@ -140,6 +149,7 @@ configure_file(include/gtsam_points/config.hpp.in include/gtsam_points/config.hp
140149
## Build ##
141150
###########
142151

152+
143153
add_library(gtsam_points SHARED
144154
# util
145155
src/gtsam_points/util/bspline.cpp
@@ -194,9 +204,9 @@ add_library(gtsam_points SHARED
194204
src/gtsam_points/optimizers/gaussian_factor_graph_solver.cpp
195205
src/gtsam_points/optimizers/levenberg_marquardt_ext.cpp
196206
src/gtsam_points/optimizers/levenberg_marquardt_optimization_status.cpp
197-
src/gtsam_points/optimizers/isam2_ext.cpp
198-
src/gtsam_points/optimizers/isam2_ext_impl.cpp
199-
src/gtsam_points/optimizers/incremental_fixed_lag_smoother_ext.cpp
207+
src/gtsam_points/optimizers/${GTSAM_POINTS_ISAM2_SRC_DIR}isam2_ext.cpp
208+
src/gtsam_points/optimizers/${GTSAM_POINTS_ISAM2_SRC_DIR}isam2_ext_impl.cpp
209+
src/gtsam_points/optimizers/${GTSAM_POINTS_ISAM2_SRC_DIR}incremental_fixed_lag_smoother_ext.cpp
200210
src/gtsam_points/optimizers/incremental_fixed_lag_smoother_ext_with_fallback.cpp
201211
src/gtsam_points/optimizers/dogleg_optimizer_ext_impl.cpp
202212
)

‎include/gtsam_points/cuda/nonlinear_factor_set_gpu.hpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -55,7 +55,7 @@ class NonlinearFactorSetGPU : public NonlinearFactorSet {
5555
* @param factor Nonlinear factor
5656
* @return True if the factor is GPU-based one and added to the set
5757
*/
58-
bool add(std::shared_ptr<gtsam::NonlinearFactor> factor) override;
58+
bool add(gtsam::NonlinearFactor::shared_ptr factor) override;
5959

6060
/**
6161
* @brief Add all GPU-based factors in a factor graph to the GPU factor set
@@ -105,7 +105,7 @@ class NonlinearFactorSetGPU : public NonlinearFactorSet {
105105
int num_linearizations;
106106
int num_evaluations;
107107

108-
std::vector<std::shared_ptr<NonlinearFactorGPU>> factors;
108+
std::vector<NonlinearFactorGPU::shared_ptr> factors;
109109

110110
std::vector<unsigned char, Eigen::aligned_allocator<unsigned char>> linearization_input_buffer_cpu;
111111
std::vector<unsigned char, Eigen::aligned_allocator<unsigned char>> linearization_output_buffer_cpu;

‎include/gtsam_points/factors/bundle_adjustment_factor.hpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55

66
#include <gtsam/geometry/Point3.h>
77
#include <gtsam/nonlinear/NonlinearFactor.h>
8+
#include <gtsam_points/util/gtsam_migration.hpp>
89

910
namespace gtsam_points {
1011

@@ -13,7 +14,7 @@ namespace gtsam_points {
1314
*/
1415
class BundleAdjustmentFactorBase : public gtsam::NonlinearFactor {
1516
public:
16-
using shared_ptr = std::shared_ptr<BundleAdjustmentFactorBase>;
17+
using shared_ptr = gtsam_points::shared_ptr<BundleAdjustmentFactorBase>;
1718

1819
virtual ~BundleAdjustmentFactorBase() {}
1920

‎include/gtsam_points/factors/bundle_adjustment_factor_evm.hpp‎

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77
#include <unordered_map>
88
#include <gtsam/geometry/Point3.h>
99
#include <gtsam/nonlinear/NonlinearFactor.h>
10+
#include <gtsam_points/util/gtsam_migration.hpp>
1011
#include <gtsam_points/factors/bundle_adjustment_factor.hpp>
1112

1213
namespace gtsam_points {
@@ -24,7 +25,7 @@ struct BALMFeature;
2425
*/
2526
class EVMBundleAdjustmentFactorBase : public BundleAdjustmentFactorBase {
2627
public:
27-
using shared_ptr = std::shared_ptr<EVMBundleAdjustmentFactorBase>;
28+
using shared_ptr = gtsam_points::shared_ptr<EVMBundleAdjustmentFactorBase>;
2829

2930
EVMBundleAdjustmentFactorBase();
3031
virtual ~EVMBundleAdjustmentFactorBase() override;
@@ -72,7 +73,7 @@ class EVMBundleAdjustmentFactorBase : public BundleAdjustmentFactorBase {
7273
*/
7374
class PlaneEVMFactor : public EVMBundleAdjustmentFactorBase {
7475
public:
75-
using shared_ptr = std::shared_ptr<PlaneEVMFactor>;
76+
using shared_ptr = gtsam_points::shared_ptr<PlaneEVMFactor>;
7677

7778
PlaneEVMFactor();
7879
virtual ~PlaneEVMFactor() override;
@@ -81,7 +82,7 @@ class PlaneEVMFactor : public EVMBundleAdjustmentFactorBase {
8182
virtual void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
8283

8384
virtual double error(const gtsam::Values& c) const override;
84-
virtual std::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& c) const override;
85+
virtual gtsam::GaussianFactor::shared_ptr linearize(const gtsam::Values& c) const override;
8586

8687
// TODO: Add feature parameter extraction method
8788
};
@@ -91,7 +92,7 @@ class PlaneEVMFactor : public EVMBundleAdjustmentFactorBase {
9192
*/
9293
class EdgeEVMFactor : public EVMBundleAdjustmentFactorBase {
9394
public:
94-
using shared_ptr = std::shared_ptr<EdgeEVMFactor>;
95+
using shared_ptr = gtsam_points::shared_ptr<EdgeEVMFactor>;
9596

9697
EdgeEVMFactor();
9798
virtual ~EdgeEVMFactor() override;
@@ -100,6 +101,6 @@ class EdgeEVMFactor : public EVMBundleAdjustmentFactorBase {
100101
virtual void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
101102

102103
virtual double error(const gtsam::Values& c) const override;
103-
virtual std::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& c) const override;
104+
virtual gtsam::GaussianFactor::shared_ptr linearize(const gtsam::Values& c) const override;
104105
};
105106
} // namespace gtsam_points

‎include/gtsam_points/factors/bundle_adjustment_factor_lsq.hpp‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,8 @@
33

44
#pragma once
55

6+
#include <gtsam/linear/GaussianFactor.h>
7+
#include <gtsam_points/util/gtsam_migration.hpp>
68
#include <gtsam_points/factors/bundle_adjustment_factor.hpp>
79

810
namespace gtsam_points {
@@ -20,7 +22,7 @@ namespace gtsam_points {
2022
class LsqBundleAdjustmentFactor : public BundleAdjustmentFactorBase {
2123
public:
2224
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23-
using shared_ptr = std::shared_ptr<LsqBundleAdjustmentFactor>;
25+
using shared_ptr = gtsam_points::shared_ptr<LsqBundleAdjustmentFactor>;
2426

2527
LsqBundleAdjustmentFactor();
2628
virtual ~LsqBundleAdjustmentFactor() override;
@@ -30,7 +32,7 @@ class LsqBundleAdjustmentFactor : public BundleAdjustmentFactorBase {
3032

3133
virtual size_t dim() const override { return 6; }
3234
virtual double error(const gtsam::Values& c) const override;
33-
virtual std::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& c) const override;
35+
virtual gtsam::GaussianFactor::shared_ptr linearize(const gtsam::Values& c) const override;
3436

3537
virtual void add(const gtsam::Point3& pt, const gtsam::Key& key) override;
3638
virtual int num_points() const override { return global_num_points; }

‎include/gtsam_points/factors/experimental/between_sim3_se3_factor.hpp‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
#include <gtsam/geometry/Pose3.h>
77
#include <gtsam/geometry/Similarity3.h>
88
#include <gtsam/nonlinear/NonlinearFactor.h>
9+
#include <gtsam_points/util/gtsam_migration.hpp>
910

1011
namespace gtsam_points {
1112

@@ -28,8 +29,8 @@ class BetweenSim3SE3Factor : public gtsam::NoiseModelFactor2<gtsam::Similarity3,
2829
virtual gtsam::Vector evaluateError(
2930
const gtsam::Similarity3& x1,
3031
const gtsam::Pose3& x2,
31-
gtsam::OptionalMatrixType H1 = nullptr,
32-
gtsam::OptionalMatrixType H2 = nullptr) const override {
32+
OptionalMatrixType H1 = NoneValue,
33+
OptionalMatrixType H2 = NoneValue) const override {
3334
//
3435
if (!H1 || !H2) {
3536
gtsam::Pose3 delta = scaled_transform(x1).between(x2);

‎include/gtsam_points/factors/experimental/continuous_time_icp_factor.hpp‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55

66
#include <gtsam/slam/expressions.h>
77
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
8+
#include <gtsam_points/util/gtsam_migration.hpp>
89
#include <gtsam_points/types/point_cloud.hpp>
910

1011
namespace gtsam_points {
@@ -41,7 +42,7 @@ class CTICPFactorExpr : public gtsam::NoiseModelFactor {
4142

4243
~CTICPFactorExpr();
4344

44-
virtual gtsam::Vector unwhitenedError(const gtsam::Values& values, gtsam::OptionalMatrixVecType H = nullptr) const;
45+
virtual gtsam::Vector unwhitenedError(const gtsam::Values& values, OptionalMatrixVecType H = NoneValue) const;
4546

4647
void update_correspondence(const gtsam::Values& values) const;
4748

@@ -65,15 +66,15 @@ class CTICPFactorExpr : public gtsam::NoiseModelFactor {
6566
*/
6667
class IntegratedCTICPFactorExpr : public gtsam::NonlinearFactor {
6768
public:
68-
using shared_ptr = std::shared_ptr<IntegratedCTICPFactorExpr>;
69+
using shared_ptr = gtsam_points::shared_ptr<IntegratedCTICPFactorExpr>;
6970

7071
IntegratedCTICPFactorExpr(const gtsam::NonlinearFactorGraph::shared_ptr& graph);
7172
~IntegratedCTICPFactorExpr();
7273

7374
virtual size_t dim() const override { return 6; }
7475

7576
virtual double error(const gtsam::Values& values) const override;
76-
virtual std::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& values) const override;
77+
virtual gtsam::GaussianFactor::shared_ptr linearize(const gtsam::Values& values) const override;
7778

7879
std::vector<Eigen::Vector3d> deskewed_source_points(const gtsam::Values& values) const;
7980

‎include/gtsam_points/factors/experimental/expression_icp_factor.hpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
#include <gtsam/slam/expressions.h>
77
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
88
#include <gtsam_points/types/point_cloud.hpp>
9+
#include <gtsam_points/util/gtsam_migration.hpp>
910

1011
namespace gtsam_points {
1112

@@ -29,7 +30,7 @@ class ICPFactorExpr : public gtsam::NoiseModelFactor {
2930

3031
~ICPFactorExpr();
3132

32-
virtual gtsam::Vector unwhitenedError(const gtsam::Values& values, gtsam::OptionalMatrixType H = nullptr) const;
33+
virtual gtsam::Vector unwhitenedError(const gtsam::Values& values, OptionalMatrixType H = NoneValue) const;
3334

3435
void update_correspondence(const gtsam::Values& values) const;
3536

‎include/gtsam_points/factors/impl/integrated_ct_gicp_factor_impl.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -100,7 +100,7 @@ double IntegratedCT_GICPFactor_<TargetFrame, SourceFrame>::error(const gtsam::Va
100100
}
101101

102102
template <typename TargetFrame, typename SourceFrame>
103-
std::shared_ptr<gtsam::GaussianFactor> IntegratedCT_GICPFactor_<TargetFrame, SourceFrame>::linearize(const gtsam::Values& values) const {
103+
gtsam::GaussianFactor::shared_ptr IntegratedCT_GICPFactor_<TargetFrame, SourceFrame>::linearize(const gtsam::Values& values) const {
104104
this->update_poses(values);
105105
this->update_correspondences();
106106

‎include/gtsam_points/factors/impl/integrated_ct_icp_factor_impl.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -129,7 +129,7 @@ double IntegratedCT_ICPFactor_<TargetFrame, SourceFrame>::error(const gtsam::Val
129129
}
130130

131131
template <typename TargetFrame, typename SourceFrame>
132-
std::shared_ptr<gtsam::GaussianFactor> IntegratedCT_ICPFactor_<TargetFrame, SourceFrame>::linearize(const gtsam::Values& values) const {
132+
gtsam::GaussianFactor::shared_ptr IntegratedCT_ICPFactor_<TargetFrame, SourceFrame>::linearize(const gtsam::Values& values) const {
133133
if (!frame::has_normals(*target)) {
134134
std::cerr << "error: target cloud doesn't have normals!!" << std::endl;
135135
abort();

0 commit comments

Comments
 (0)