From 3ebcfc5ce040adecd272b29198e7a3722d3c16ff Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 28 Dec 2014 16:37:54 +0100 Subject: [PATCH] make timing compiles --- gtsam_unstable/timing/timeAdaptAutoDiff.cpp | 2 +- .../timing/timeCameraExpression.cpp | 6 ++-- .../timing/timeOneCameraExpression.cpp | 4 +-- gtsam_unstable/timing/timeSFMExpressions.cpp | 4 +-- timing/timeIncremental.cpp | 34 +++++++++---------- timing/timeiSAM2Chain.cpp | 6 ++-- 6 files changed, 28 insertions(+), 28 deletions(-) diff --git a/gtsam_unstable/timing/timeAdaptAutoDiff.cpp b/gtsam_unstable/timing/timeAdaptAutoDiff.cpp index 437067697..edfd420ef 100644 --- a/gtsam_unstable/timing/timeAdaptAutoDiff.cpp +++ b/gtsam_unstable/timing/timeAdaptAutoDiff.cpp @@ -19,7 +19,7 @@ #include "timeLinearize.h" #include #include -#include +#include #include #include #include diff --git a/gtsam_unstable/timing/timeCameraExpression.cpp b/gtsam_unstable/timing/timeCameraExpression.cpp index 92522c440..208c991fd 100644 --- a/gtsam_unstable/timing/timeCameraExpression.cpp +++ b/gtsam_unstable/timing/timeCameraExpression.cpp @@ -16,8 +16,8 @@ * @date October 3, 2014 */ -#include -#include +#include +#include #include #include #include @@ -32,7 +32,7 @@ using namespace gtsam; boost::shared_ptr fixedK(new Cal3_S2()); Point2 myProject(const Pose3& pose, const Point3& point, - boost::optional H1, boost::optional H2) { + OptionalJacobian<2,6> H1, OptionalJacobian<2,3> H2) { PinholeCamera camera(pose, *fixedK); return camera.project(point, H1, H2, boost::none); } diff --git a/gtsam_unstable/timing/timeOneCameraExpression.cpp b/gtsam_unstable/timing/timeOneCameraExpression.cpp index 4cb655825..962e7ee21 100644 --- a/gtsam_unstable/timing/timeOneCameraExpression.cpp +++ b/gtsam_unstable/timing/timeOneCameraExpression.cpp @@ -16,8 +16,8 @@ * @date October 3, 2014 */ -#include -#include +#include +#include #include "timeLinearize.h" using namespace std; diff --git a/gtsam_unstable/timing/timeSFMExpressions.cpp b/gtsam_unstable/timing/timeSFMExpressions.cpp index 678e4db60..cfb8e0dc6 100644 --- a/gtsam_unstable/timing/timeSFMExpressions.cpp +++ b/gtsam_unstable/timing/timeSFMExpressions.cpp @@ -16,8 +16,8 @@ * @date October 3, 2014 */ -#include -#include +#include +#include #include #include diff --git a/timing/timeIncremental.cpp b/timing/timeIncremental.cpp index 6377649e2..d82ef9442 100644 --- a/timing/timeIncremental.cpp +++ b/timing/timeIncremental.cpp @@ -14,15 +14,15 @@ * @author Richard Roberts */ -#include #include -#include #include #include #include +#include #include #include #include +#include #include #include @@ -39,20 +39,20 @@ typedef NoiseModelFactor1 NM1; typedef NoiseModelFactor2 NM2; typedef BearingRangeFactor BR; -GTSAM_VALUE_EXPORT(Value); -GTSAM_VALUE_EXPORT(Pose); -GTSAM_VALUE_EXPORT(NonlinearFactor); -GTSAM_VALUE_EXPORT(NoiseModelFactor); -GTSAM_VALUE_EXPORT(NM1); -GTSAM_VALUE_EXPORT(NM2); -GTSAM_VALUE_EXPORT(BetweenFactor); -GTSAM_VALUE_EXPORT(PriorFactor); -GTSAM_VALUE_EXPORT(BR); -GTSAM_VALUE_EXPORT(noiseModel::Base); -GTSAM_VALUE_EXPORT(noiseModel::Isotropic); -GTSAM_VALUE_EXPORT(noiseModel::Gaussian); -GTSAM_VALUE_EXPORT(noiseModel::Diagonal); -GTSAM_VALUE_EXPORT(noiseModel::Unit); +//GTSAM_VALUE_EXPORT(Value); +//GTSAM_VALUE_EXPORT(Pose); +//GTSAM_VALUE_EXPORT(NonlinearFactor); +//GTSAM_VALUE_EXPORT(NoiseModelFactor); +//GTSAM_VALUE_EXPORT(NM1); +//GTSAM_VALUE_EXPORT(NM2); +//GTSAM_VALUE_EXPORT(BetweenFactor); +//GTSAM_VALUE_EXPORT(PriorFactor); +//GTSAM_VALUE_EXPORT(BR); +//GTSAM_VALUE_EXPORT(noiseModel::Base); +//GTSAM_VALUE_EXPORT(noiseModel::Isotropic); +//GTSAM_VALUE_EXPORT(noiseModel::Gaussian); +//GTSAM_VALUE_EXPORT(noiseModel::Diagonal); +//GTSAM_VALUE_EXPORT(noiseModel::Unit); double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) { // Compute degrees of freedom (observations - variables) @@ -97,7 +97,7 @@ int main(int argc, char *argv[]) { // cout << "Initializing " << 0 << endl; newVariables.insert(0, Pose()); // Add prior - newFactors.add(PriorFactor(0, Pose(), noiseModel::Unit::Create(Pose::Dim()))); + newFactors.add(PriorFactor(0, Pose(), noiseModel::Unit::Create(3))); } while(nextMeasurement < measurements.size()) { diff --git a/timing/timeiSAM2Chain.cpp b/timing/timeiSAM2Chain.cpp index d15ccfaf4..c4196f414 100644 --- a/timing/timeiSAM2Chain.cpp +++ b/timing/timeiSAM2Chain.cpp @@ -39,7 +39,7 @@ typedef Pose2 Pose; typedef NoiseModelFactor1 NM1; typedef NoiseModelFactor2 NM2; -noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(Pose::Dim()); +noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(3); int main(int argc, char *argv[]) { @@ -61,10 +61,10 @@ int main(int argc, char *argv[]) { gttic_(Create_measurements); if(step == 0) { // Add prior - newFactors.add(PriorFactor(0, Pose(), noiseModel::Unit::Create(Pose::Dim()))); + newFactors.add(PriorFactor(0, Pose(), noiseModel::Unit::Create(3))); newVariables.insert(0, Pose()); } else { - Vector eta = Vector::Random(Pose::Dim()) * 0.1; + Vector eta = Vector::Random(3) * 0.1; Pose2 between = Pose().retract(eta); // Add between newFactors.add(BetweenFactor(step-1, step, between, model));