diff --git a/.cproject b/.cproject index 18175bec1..52571de92 100644 --- a/.cproject +++ b/.cproject @@ -592,7 +592,6 @@ make - tests/testBayesTree.run true false @@ -600,7 +599,6 @@ make - testBinaryBayesNet.run true false @@ -648,7 +646,6 @@ make - testSymbolicBayesNet.run true false @@ -656,7 +653,6 @@ make - tests/testSymbolicFactor.run true false @@ -664,7 +660,6 @@ make - testSymbolicFactorGraph.run true false @@ -680,7 +675,6 @@ make - tests/testBayesTree true false @@ -966,38 +960,6 @@ true true - - make - -j5 - timeCameraExpression.run - true - true - true - - - make - -j5 - timeOneCameraExpression.run - true - true - true - - - make - -j5 - timeSFMExpressions.run - true - true - true - - - make - -j5 - timeAdaptAutoDiff.run - true - true - true - make -j5 @@ -1136,7 +1098,6 @@ make - testErrors.run true false @@ -1366,46 +1327,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j2 @@ -1488,6 +1409,7 @@ make + testSimulated2DOriented.run true false @@ -1527,6 +1449,7 @@ make + testSimulated2D.run true false @@ -1534,6 +1457,7 @@ make + testSimulated3D.run true false @@ -1547,6 +1471,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j5 @@ -1804,7 +1768,6 @@ cpack - -G DEB true false @@ -1812,7 +1775,6 @@ cpack - -G RPM true false @@ -1820,7 +1782,6 @@ cpack - -G TGZ true false @@ -1828,7 +1789,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2699,7 +2659,6 @@ make - testGraph.run true false @@ -2707,7 +2666,6 @@ make - testJunctionTree.run true false @@ -2715,7 +2673,6 @@ make - testSymbolicBayesNetB.run true false @@ -3249,6 +3206,46 @@ true true + + make + -j4 + timeAdaptAutoDiff.run + true + true + true + + + make + -j4 + timeCameraExpression.run + true + true + true + + + make + -j4 + timeOneCameraExpression.run + true + true + true + + + make + -j4 + timeSFMExpressions.run + true + true + true + + + make + -j4 + timeIncremental.run + true + true + true + make -j2 @@ -3259,6 +3256,7 @@ make + tests/testGaussianISAM2 true false diff --git a/gtsam_unstable/timing/timeAdaptAutoDiff.cpp b/timing/timeAdaptAutoDiff.cpp similarity index 97% rename from gtsam_unstable/timing/timeAdaptAutoDiff.cpp rename to timing/timeAdaptAutoDiff.cpp index 437067697..edfd420ef 100644 --- a/gtsam_unstable/timing/timeAdaptAutoDiff.cpp +++ b/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/timing/timeCameraExpression.cpp similarity index 94% rename from gtsam_unstable/timing/timeCameraExpression.cpp rename to timing/timeCameraExpression.cpp index 92522c440..208c991fd 100644 --- a/gtsam_unstable/timing/timeCameraExpression.cpp +++ b/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/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/gtsam_unstable/timing/timeLinearize.h b/timing/timeLinearize.h similarity index 100% rename from gtsam_unstable/timing/timeLinearize.h rename to timing/timeLinearize.h diff --git a/gtsam_unstable/timing/timeOneCameraExpression.cpp b/timing/timeOneCameraExpression.cpp similarity index 92% rename from gtsam_unstable/timing/timeOneCameraExpression.cpp rename to timing/timeOneCameraExpression.cpp index 4cb655825..962e7ee21 100644 --- a/gtsam_unstable/timing/timeOneCameraExpression.cpp +++ b/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/timing/timeSFMExpressions.cpp similarity index 95% rename from gtsam_unstable/timing/timeSFMExpressions.cpp rename to timing/timeSFMExpressions.cpp index 678e4db60..cfb8e0dc6 100644 --- a/gtsam_unstable/timing/timeSFMExpressions.cpp +++ b/timing/timeSFMExpressions.cpp @@ -16,8 +16,8 @@ * @date October 3, 2014 */ -#include -#include +#include +#include #include #include 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));