diff --git a/.cproject b/.cproject index 10b16f91c..7aae42663 100644 --- a/.cproject +++ b/.cproject @@ -484,54 +484,6 @@ - - make - -j5 - SmartProjectionFactorExample_kitti_nonbatch.run - true - true - true - - - make - -j5 - SmartProjectionFactorExample_kitti.run - true - true - true - - - make - -j5 - SmartProjectionFactorTesting.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testSPQRUtil.run - true - true - true - - - make - -j2 - clean - true - true - true - make -j4 @@ -564,151 +516,7 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -k - check - true - false - true - - - make - - tests/testBayesTree.run - true - false - true - - - make - - testBinaryBayesNet.run - true - false - true - - - make - -j2 - testFactorGraph.run - true - true - true - - - make - -j2 - testISAM.run - true - true - true - - - make - -j2 - testJunctionTree.run - true - true - true - - - make - -j2 - testKey.run - true - true - true - - - make - -j2 - testOrdering.run - true - true - true - - - make - - testSymbolicBayesNet.run - true - false - true - - - make - - tests/testSymbolicFactor.run - true - false - true - - - make - - testSymbolicFactorGraph.run - true - false - true - - - make - -j2 - timeSymbolMaps.run - true - true - true - - - make - - tests/testBayesTree - true - false - true - - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - - - make - -j2 - all - true - true - true - - + make -j2 check @@ -716,7 +524,15 @@ true true - + + make + -j2 + tests/testSPQRUtil.run + true + true + true + + make -j2 clean @@ -724,6 +540,526 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + timeCalibratedCamera.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + testGaussianFactorGraphUnordered.run + true + true + true + + + make + -j5 + testGaussianBayesNetUnordered.run + true + true + true + + + make + -j5 + testGaussianConditional.run + true + true + true + + + make + -j5 + testGaussianDensity.run + true + true + true + + + make + -j5 + testGaussianJunctionTree.run + true + true + true + + + make + -j5 + testHessianFactor.run + true + true + true + + + make + -j5 + testJacobianFactor.run + true + true + true + + + make + -j5 + testKalmanFilter.run + true + true + true + + + make + -j5 + testNoiseModel.run + true + true + true + + + make + -j5 + testSampler.run + true + true + true + + + make + -j5 + testSerializationLinear.run + true + true + true + + + make + -j5 + testVectorValues.run + true + true + true + + + make + -j5 + testGaussianBayesTree.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + testMethod.run + true + true + true + + + make + -j5 + testClass.run + true + true + true + + + make + -j4 + testType.run + true + true + true + + + make + -j4 + testArgument.run + true + true + true + + + make + -j4 + testReturnValue.run + true + true + true + + + make + -j4 + testTemplate.run + true + true + true + + + make + -j4 + testGlobalFunction.run + true + true + true + + + make + -j5 + schedulingExample.run + true + true + true + + + make + -j5 + schedulingQuals12.run + true + true + true + + + make + -j5 + schedulingQuals13.run + true + true + true + + + make + -j5 + testCSP.run + true + true + true + + + make + -j5 + testScheduler.run + true + true + true + + + make + -j5 + testSudoku.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + + + make + -j5 + testImuFactor.run + true + true + true + + + make + -j5 + testAHRSFactor.run + true + true + true + + + make + -j8 + testAttitudeFactor.run + true + true + true + + + make + -j4 + testAggregateImuReadings.run + true + true + true + + + make + -j4 + testScenarioRunner.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testVSLAMGraph + true + true + true + + + make + -j5 + testMatrix.run + true + true + true + + + make + -j5 + testVector.run + true + true + true + + + make + -j5 + testNumericalDerivative.run + true + true + true + + + make + -j5 + testVerticalBlockMatrix.run + true + true + true + + + make + -j4 + testOptionalJacobian.run + true + true + true + + + make + -j4 + testGroup.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + all + true + true + true + + + make + -j1 + testDiscreteBayesTree.run + true + false + true + + + make + -j5 + testDiscreteConditional.run + true + true + true + + + make + -j5 + testDiscreteFactor.run + true + true + true + + + make + -j5 + testDiscreteFactorGraph.run + true + true + true + + + make + -j5 + testDiscreteMarginals.run + true + true + true + + + make + -j5 + testIMUSystem.run + true + true + true + + + make + -j5 + testPoseRTV.run + true + true + true + + + make + -j5 + testVelocityConstraint.run + true + true + true + + + make + -j5 + testVelocityConstraint3.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + clean all + true + true + true + + + make + -j5 + SmartProjectionFactorExample_kitti_nonbatch.run + true + true + true + + + make + -j5 + SmartProjectionFactorExample_kitti.run + true + true + true + + + make + -j5 + SmartProjectionFactorTesting.run + true + true + true + make -j5 @@ -868,191 +1204,7 @@ true true - - make - -j5 - testGaussianFactorGraphUnordered.run - true - true - true - - - make - -j5 - testGaussianBayesNetUnordered.run - true - true - true - - - make - -j5 - testGaussianConditional.run - true - true - true - - - make - -j5 - testGaussianDensity.run - true - true - true - - - make - -j5 - testGaussianJunctionTree.run - true - true - true - - - make - -j5 - testHessianFactor.run - true - true - true - - - make - -j5 - testJacobianFactor.run - true - true - true - - - make - -j5 - testKalmanFilter.run - true - true - true - - - make - -j5 - testNoiseModel.run - true - true - true - - - make - -j5 - testSampler.run - true - true - true - - - make - -j5 - testSerializationLinear.run - true - true - true - - - make - -j5 - testVectorValues.run - true - true - true - - - make - -j5 - testGaussianBayesTree.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testAHRSFactor.run - true - true - true - - - make - -j8 - testAttitudeFactor.run - true - true - true - - - make - -j5 - clean - true - true - true - - - make - -j5 - all - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - testGaussianConditional.run - true - true - true - - + make -j2 testGaussianFactor.run @@ -1060,175 +1212,7 @@ true true - - make - -j2 - timeGaussianFactor.run - true - true - true - - - make - -j2 - timeVectorConfig.run - true - true - true - - - make - -j2 - testVectorBTree.run - true - true - true - - - make - -j2 - testVectorMap.run - true - true - true - - - make - -j2 - testNoiseModel.run - true - true - true - - - make - -j2 - testBayesNetPreconditioner.run - true - true - true - - - make - - testErrors.run - true - false - true - - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testGaussianJunctionTree.run - true - true - true - - - make - -j2 - tests/testGaussianFactor.run - true - true - true - - - make - -j2 - tests/testGaussianConditional.run - true - true - true - - - make - -j2 - tests/timeSLAMlike.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testBTree.run - true - true - true - - - make - -j2 - testDSF.run - true - true - true - - - make - -j2 - testDSFVector.run - true - true - true - - - make - -j2 - testMatrix.run - true - true - true - - - make - -j2 - testSPQRUtil.run - true - true - true - - - make - -j2 - testVector.run - true - true - true - - - make - -j2 - timeMatrix.run - true - true - true - - + make -j2 all @@ -1236,23 +1220,53 @@ true true - + make -j2 + clean + true + true + true + + + make + -k check true - true + false true - + + make + tests/testBayesTree.run + true + false + true + + + make + testBinaryBayesNet.run + true + false + true + + make -j2 - testClusterTree.run + testFactorGraph.run true true true - + + make + -j2 + testISAM.run + true + true + true + + make -j2 testJunctionTree.run @@ -1260,793 +1274,58 @@ true true - + make -j2 - tests/testEliminationTree.run - true - true - true - - - make - -j2 - tests/testSymbolicFactor.run - true - true - true - - - make - -j2 - tests/testVariableSlots.run - true - true - true - - - make - -j2 - tests/testConditional.run - true - true - true - - - make - -j2 - tests/testSymbolicFactorGraph.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - testNonlinearConstraint.run - true - true - true - - - make - -j2 - testLieConfig.run - true - true - true - - - make - -j2 - testConstraintOptimizer.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPlanarSLAM.run - true - true - true - - - make - -j2 - testPose2Config.run - true - true - true - - - make - -j2 - testPose2Factor.run - true - true - true - - - make - -j2 - testPose2Prior.run - true - true - true - - - make - -j2 - testPose2SLAM.run - true - true - true - - - make - -j2 - testPose3Config.run - true - true - true - - - make - -j2 - testPose3SLAM.run - true - true - true - - - make - testSimulated2DOriented.run - true - false - true - - - make - -j2 - testVSLAMConfig.run - true - true - true - - - make - -j2 - testVSLAMFactor.run - true - true - true - - - make - -j2 - testVSLAMGraph.run - true - true - true - - - make - -j2 - testPose3Factor.run - true - true - true - - - make - testSimulated2D.run - true - false - true - - - make - testSimulated3D.run - true - false - true - - - make - -j2 - tests/testGaussianISAM2 - true - 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 - testEliminationTree.run - true - true - true - - - make - -j5 - testInference.run - true - true - true - - - make - -j5 testKey.run true true true - - make - -j1 - testSymbolicBayesTree.run - true - false - true - - - make - -j1 - testSymbolicSequentialSolver.run - true - false - true - - - make - -j4 - testLabeledSymbol.run - true - true - true - - + make -j2 - check + testOrdering.run true true true - + + make + testSymbolicBayesNet.run + true + false + true + + + make + tests/testSymbolicFactor.run + true + false + true + + + make + testSymbolicFactorGraph.run + true + false + true + + make -j2 - tests/testLieConfig.run + timeSymbolMaps.run true true true - + make - -j3 - install + tests/testBayesTree true false true - - make - -j2 - clean - true - true - true - - - make - -j1 - check - true - false - true - - - make - -j5 - all - true - true - true - - - cmake - .. - true - false - true - - - make - -j5 - gtsam-shared - true - true - true - - - make - -j5 - gtsam-static - true - true - true - - - make - -j5 - timing - true - true - true - - - make - -j5 - examples - true - true - true - - - make - -j5 - VERBOSE=1 all - true - true - true - - - make - -j5 - VERBOSE=1 check - true - true - true - - - make - -j5 - check.base - true - true - true - - - make - -j5 - timing.base - true - true - true - - - make - -j2 VERBOSE=1 - check.geometry - true - false - true - - - make - -j5 - timing.geometry - true - true - true - - - make - -j2 VERBOSE=1 - check.inference - true - false - true - - - make - -j5 - timing.inference - true - true - true - - - make - -j2 VERBOSE=1 - check.linear - true - false - true - - - make - -j5 - timing.linear - true - true - true - - - make - -j2 VERBOSE=1 - check.nonlinear - true - false - true - - - make - -j5 - timing.nonlinear - true - true - true - - - make - -j2 VERBOSE=1 - check.slam - true - false - true - - - make - -j5 - timing.slam - true - true - true - - - make - -j5 - wrap_gtsam - true - true - true - - - make - VERBOSE=1 - wrap_gtsam - true - false - true - - - cpack - - -G DEB - true - false - true - - - cpack - - -G RPM - true - false - true - - - cpack - - -G TGZ - true - false - true - - - cpack - - --config CPackSourceConfig.cmake - true - false - true - - - make - -j5 - check.discrete - true - true - true - - - make - -j5 - check.discrete_unstable - true - true - true - - - make - -j5 - check.base_unstable - true - true - true - - - make - -j5 - check.dynamics_unstable - true - true - true - - - make - -j5 - check.slam_unstable - true - true - true - - - make - -j5 - check.unstable - true - true - true - - - make - -j5 - wrap_gtsam_build - true - true - true - - - make - -j5 - wrap_gtsam_unstable_build - true - true - true - - - make - -j5 - wrap_gtsam_unstable - true - true - true - - - make - -j5 - wrap - true - true - true - - - make - -j5 - wrap_gtsam_distclean - true - true - true - - - make - -j5 - wrap_gtsam_unstable_distclean - true - true - true - - - make - -j5 - doc - true - true - true - - - make - -j5 - doc_clean - true - true - true - - - make - -j5 - check - true - true - true - - - make - -j5 - check.geometry_unstable - true - true - true - - - make - -j5 - check.linear_unstable - true - true - true - - - make - -j6 -j8 - gtsam_unstable-shared - true - true - true - - - make - -j6 -j8 - gtsam_unstable-static - true - true - true - - - make - -j6 -j8 - check.nonlinear_unstable - true - true - true - - - make - -j5 - check.tests - true - true - true - - - make - -j2 VERBOSE=1 - check.navigation - true - false - true - - - make - -j4 - check.sam - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - all - true - true - true - - - cmake - .. - true - false - true - - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -2279,278 +1558,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - clean all - true - true - true - - - make - -j1 - testDiscreteBayesTree.run - true - false - true - - - make - -j5 - testDiscreteConditional.run - true - true - true - - - make - -j5 - testDiscreteFactor.run - true - true - true - - - make - -j5 - testDiscreteFactorGraph.run - true - true - true - - - make - -j5 - testDiscreteMarginals.run - true - true - true - - - make - -j5 - testIMUSystem.run - true - true - true - - - make - -j5 - testPoseRTV.run - true - true - true - - - make - -j5 - testVelocityConstraint.run - true - true - true - - - make - -j5 - testVelocityConstraint3.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - timeCalibratedCamera.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testWrap.run - true - true - true - - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - - - make - -j5 - testMethod.run - true - true - true - - - make - -j5 - testClass.run - true - true - true - - - make - -j4 - testType.run - true - true - true - - - make - -j4 - testArgument.run - true - true - true - - - make - -j4 - testReturnValue.run - true - true - true - - - make - -j4 - testTemplate.run - true - true - true - - - make - -j4 - testGlobalFunction.run - true - true - true - - - make - -j5 - schedulingExample.run - true - true - true - - - make - -j5 - schedulingQuals12.run - true - true - true - - - make - -j5 - schedulingQuals13.run - true - true - true - - - make - -j5 - testCSP.run - true - true - true - - - make - -j5 - testScheduler.run - true - true - true - - - make - -j5 - testSudoku.run - true - true - true - make -j2 @@ -2559,58 +1566,50 @@ true true - + make -j2 - testVSLAMGraph + tests/testPose2.run true true true - + + make + -j2 + tests/testPose3.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + make -j5 - testMatrix.run - true - true - true - - - make - -j5 - testVector.run - true - true - true - - - make - -j5 - testNumericalDerivative.run - true - true - true - - - make - -j5 - testVerticalBlockMatrix.run - true - true - true - - - make - -j4 - testOptionalJacobian.run - true - true - true - - - make - -j4 - testGroup.run + testParticleFactor.run true true true @@ -2753,7 +1752,6 @@ make - testGraph.run true false @@ -2761,7 +1759,6 @@ make - testJunctionTree.run true false @@ -2769,7 +1766,6 @@ make - testSymbolicBayesNetB.run true false @@ -2855,170 +1851,42 @@ true true - - make - -j5 - testParticleFactor.run - true - true - true - - + make -j2 - testGaussianFactor.run + check true true true - + make -j2 - install + tests/testGaussianJunctionTree.run true true true - + make -j2 - clean + tests/testGaussianFactor.run true true true - + make -j2 - all + tests/testGaussianConditional.run true true true - + make -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testAntiFactor.run - true - true - true - - - make - -j5 - testPriorFactor.run - true - true - true - - - make - -j5 - testDataset.run - true - true - true - - - make - -j5 - testEssentialMatrixFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testRotateFactor.run - true - true - true - - - make - -j5 - testPoseRotationPrior.run - true - true - true - - - make - -j5 - testImplicitSchurFactor.run - true - true - true - - - make - -j4 - testOrientedPlane3Factor.run - true - true - true - - - make - -j4 - testSmartProjectionPoseFactor.run - true - true - true - - - make - -j4 - testInitializePose3.run + tests/timeSLAMlike.run true true true @@ -3223,6 +2091,70 @@ true true + + make + -j5 + testEliminationTree.run + true + true + true + + + make + -j5 + testInference.run + true + true + true + + + make + -j5 + testKey.run + true + true + true + + + make + -j1 + testSymbolicBayesTree.run + true + false + true + + + make + -j1 + testSymbolicSequentialSolver.run + true + false + true + + + make + -j4 + testLabeledSymbol.run + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + make -j5 @@ -3311,29 +2243,6 @@ true true - - make - -j4 - testImuFactor.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - tests/testGaussianISAM2 - true - false - true - make -j5 @@ -3430,6 +2339,983 @@ true true + + make + -j4 + testImuFactor.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + tests/testLieConfig.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + all + true + true + true + + + cmake + .. + true + false + 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 + -j4 + testBearingFactor.run + true + true + true + + + make + -j4 + testRangeFactor.run + true + true + true + + + make + -j4 + testBearingRangeFactor.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + testGaussianConditional.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + timeGaussianFactor.run + true + true + true + + + make + -j2 + timeVectorConfig.run + true + true + true + + + make + -j2 + testVectorBTree.run + true + true + true + + + make + -j2 + testVectorMap.run + true + true + true + + + make + -j2 + testNoiseModel.run + true + true + true + + + make + -j2 + testBayesNetPreconditioner.run + true + true + true + + + make + testErrors.run + true + false + true + + + make + -j2 + check + true + true + true + + + make + -j2 + testClusterTree.run + true + true + true + + + make + -j2 + testJunctionTree.run + true + true + true + + + make + -j2 + tests/testEliminationTree.run + true + true + true + + + make + -j2 + tests/testSymbolicFactor.run + true + true + true + + + make + -j2 + tests/testVariableSlots.run + true + true + true + + + make + -j2 + tests/testConditional.run + true + true + true + + + make + -j2 + tests/testSymbolicFactorGraph.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testBTree.run + true + true + true + + + make + -j2 + testDSF.run + true + true + true + + + make + -j2 + testDSFVector.run + true + true + true + + + make + -j2 + testMatrix.run + true + true + true + + + make + -j2 + testSPQRUtil.run + true + true + true + + + make + -j2 + testVector.run + true + true + true + + + make + -j2 + timeMatrix.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + testNonlinearConstraint.run + true + true + true + + + make + -j2 + testLieConfig.run + true + true + true + + + make + -j2 + testConstraintOptimizer.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + + tests/testGaussianISAM2 + true + false + true + + + make + -j3 + install + true + false + true + + + make + -j2 + clean + true + true + true + + + make + -j1 + check + true + false + true + + + make + -j5 + all + true + true + true + + + cmake + .. + true + false + true + + + make + -j5 + gtsam-shared + true + true + true + + + make + -j5 + gtsam-static + true + true + true + + + make + -j5 + timing + true + true + true + + + make + -j5 + examples + true + true + true + + + make + -j5 + VERBOSE=1 all + true + true + true + + + make + -j5 + VERBOSE=1 check + true + true + true + + + make + -j5 + check.base + true + true + true + + + make + -j5 + timing.base + true + true + true + + + make + -j2 VERBOSE=1 + check.geometry + true + false + true + + + make + -j5 + timing.geometry + true + true + true + + + make + -j2 VERBOSE=1 + check.inference + true + false + true + + + make + -j5 + timing.inference + true + true + true + + + make + -j2 VERBOSE=1 + check.linear + true + false + true + + + make + -j5 + timing.linear + true + true + true + + + make + -j2 VERBOSE=1 + check.nonlinear + true + false + true + + + make + -j5 + timing.nonlinear + true + true + true + + + make + -j2 VERBOSE=1 + check.slam + true + false + true + + + make + -j5 + timing.slam + true + true + true + + + make + -j5 + wrap_gtsam + true + true + true + + + make + VERBOSE=1 + wrap_gtsam + true + false + true + + + cpack + -G DEB + true + false + true + + + cpack + -G RPM + true + false + true + + + cpack + -G TGZ + true + false + true + + + cpack + --config CPackSourceConfig.cmake + true + false + true + + + make + -j5 + check.discrete + true + true + true + + + make + -j5 + check.discrete_unstable + true + true + true + + + make + -j5 + check.base_unstable + true + true + true + + + make + -j5 + check.dynamics_unstable + true + true + true + + + make + -j5 + check.slam_unstable + true + true + true + + + make + -j5 + check.unstable + true + true + true + + + make + -j5 + wrap_gtsam_build + true + true + true + + + make + -j5 + wrap_gtsam_unstable_build + true + true + true + + + make + -j5 + wrap_gtsam_unstable + true + true + true + + + make + -j5 + wrap + true + true + true + + + make + -j5 + wrap_gtsam_distclean + true + true + true + + + make + -j5 + wrap_gtsam_unstable_distclean + true + true + true + + + make + -j5 + doc + true + true + true + + + make + -j5 + doc_clean + true + true + true + + + make + -j5 + check + true + true + true + + + make + -j5 + check.geometry_unstable + true + true + true + + + make + -j5 + check.linear_unstable + true + true + true + + + make + -j6 -j8 + gtsam_unstable-shared + true + true + true + + + make + -j6 -j8 + gtsam_unstable-static + true + true + true + + + make + -j6 -j8 + check.nonlinear_unstable + true + true + true + + + make + -j5 + check.tests + true + true + true + + + make + -j2 VERBOSE=1 + check.navigation + true + false + true + + + make + -j4 + check.sam + true + true + true + + + make + -j5 + testAntiFactor.run + true + true + true + + + make + -j5 + testPriorFactor.run + true + true + true + + + make + -j5 + testDataset.run + true + true + true + + + make + -j5 + testEssentialMatrixFactor.run + true + true + true + + + make + -j5 + testGeneralSFMFactor_Cal3Bundler.run + true + true + true + + + make + -j5 + testGeneralSFMFactor.run + true + true + true + + + make + -j5 + testProjectionFactor.run + true + true + true + + + make + -j5 + testRotateFactor.run + true + true + true + + + make + -j5 + testPoseRotationPrior.run + true + true + true + + + make + -j5 + testImplicitSchurFactor.run + true + true + true + + + make + -j4 + testOrientedPlane3Factor.run + true + true + true + + + make + -j4 + testSmartProjectionPoseFactor.run + true + true + true + + + make + -j4 + testInitializePose3.run + true + true + true + make -j2 @@ -3526,26 +3412,146 @@ true true - + make - -j4 - testBearingFactor.run + -j2 + all true true true - + make - -j4 - testRangeFactor.run + -j2 + check true true true - + make - -j4 - testBearingRangeFactor.run + -j2 + clean + true + true + true + + + make + -j2 + testPlanarSLAM.run + true + true + true + + + make + -j2 + testPose2Config.run + true + true + true + + + make + -j2 + testPose2Factor.run + true + true + true + + + make + -j2 + testPose2Prior.run + true + true + true + + + make + -j2 + testPose2SLAM.run + true + true + true + + + make + -j2 + testPose3Config.run + true + true + true + + + make + -j2 + testPose3SLAM.run + true + true + true + + + make + + testSimulated2DOriented.run + true + false + true + + + make + -j2 + testVSLAMConfig.run + true + true + true + + + make + -j2 + testVSLAMFactor.run + true + true + true + + + make + -j2 + testVSLAMGraph.run + true + true + true + + + make + -j2 + testPose3Factor.run + true + true + true + + + make + + testSimulated2D.run + true + false + true + + + make + + testSimulated3D.run + true + false + true + + + make + -j2 + tests/testGaussianISAM2 true true true diff --git a/CMakeLists.txt b/CMakeLists.txt index 0380b8a2f..e8da98ffd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,6 +64,7 @@ option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TB option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" ON) option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON) option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) +option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" ON) # Options relating to MATLAB wrapper # TODO: Check for matlab mex binary before handling building of binaries @@ -344,6 +345,20 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX) add_subdirectory(matlab) endif() +# Python wrap +if (GTSAM_BUILD_PYTHON) + include(GtsamPythonWrap) + + # NOTE: The automatic generation of python wrapper from the gtsampy.h interface is + # not working yet, so we're using a handwritten wrapper files on python/handwritten. + # Once the python wrapping from the interface file is working, you can _swap_ the + # comments on the next lines + + # wrap_and_install_python(gtsampy.h "${GTSAM_ADDITIONAL_LIBRARIES}" "") + add_subdirectory(python) + +endif() + # Build gtsam_unstable if (GTSAM_BUILD_UNSTABLE) add_subdirectory(gtsam_unstable) @@ -460,6 +475,17 @@ print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full Ex message(STATUS "MATLAB toolbox flags ") print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ") print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ") + +message(STATUS "Python module flags ") + +if(GTSAM_PYTHON_WARNINGS) + message(STATUS " Build python module : No - dependencies missing") +else() + print_config_flag(${GTSAM_BUILD_PYTHON} "Build python module ") +endif() +if(GTSAM_BUILD_PYTHON) + message(STATUS " Python version : ${GTSAM_PYTHON_VERSION}") +endif() message(STATUS "===============================================================") # Print warnings at the end @@ -472,6 +498,9 @@ endif() if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND) message(WARNING "Your compiler does not support OpenMP - this is ok, but performance may be improved with OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning.") endif() +if(GTSAM_BUILD_PYTHON AND GTSAM_PYTHON_WARNINGS) + message(WARNING "${GTSAM_PYTHON_WARNINGS}") +endif() # Include CPack *after* all flags include(CPack) diff --git a/THANKS b/THANKS index 9c06a5d28..f84cfa185 100644 --- a/THANKS +++ b/THANKS @@ -38,6 +38,10 @@ at Uni Zurich: * Christian Forster +at LAAS-CNRS + +* Ellon Paiva + Many thanks for your hard work!!!! Frank Dellaert diff --git a/cmake/FindNumPy.cmake b/cmake/FindNumPy.cmake new file mode 100644 index 000000000..eafed165e --- /dev/null +++ b/cmake/FindNumPy.cmake @@ -0,0 +1,102 @@ +# - Find the NumPy libraries +# This module finds if NumPy is installed, and sets the following variables +# indicating where it is. +# +# TODO: Update to provide the libraries and paths for linking npymath lib. +# +# NUMPY_FOUND - was NumPy found +# NUMPY_VERSION - the version of NumPy found as a string +# NUMPY_VERSION_MAJOR - the major version number of NumPy +# NUMPY_VERSION_MINOR - the minor version number of NumPy +# NUMPY_VERSION_PATCH - the patch version number of NumPy +# NUMPY_VERSION_DECIMAL - e.g. version 1.6.1 is 10601 +# NUMPY_INCLUDE_DIRS - path to the NumPy include files + +#============================================================================ +# Copyright 2012 Continuum Analytics, Inc. +# +# MIT License +# +# Permission is hereby granted, free of charge, to any person obtaining +# a copy of this software and associated documentation files +# (the "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to +# the following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR +# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, +# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR +# OTHER DEALINGS IN THE SOFTWARE. +# +#============================================================================ + +# Finding NumPy involves calling the Python interpreter +if(NumPy_FIND_REQUIRED) + find_package(PythonInterp REQUIRED) +else() + find_package(PythonInterp) +endif() + +if(NOT PYTHONINTERP_FOUND) + set(NUMPY_FOUND FALSE) + return() +endif() + +execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c" + "import numpy as n; print(n.__version__); print(n.get_include());" + RESULT_VARIABLE _NUMPY_SEARCH_SUCCESS + OUTPUT_VARIABLE _NUMPY_VALUES_OUTPUT + ERROR_VARIABLE _NUMPY_ERROR_VALUE + OUTPUT_STRIP_TRAILING_WHITESPACE) + +if(NOT _NUMPY_SEARCH_SUCCESS MATCHES 0) + if(NumPy_FIND_REQUIRED) + message(FATAL_ERROR + "NumPy import failure:\n${_NUMPY_ERROR_VALUE}") + endif() + set(NUMPY_FOUND FALSE) + return() +endif() + +# Convert the process output into a list +string(REGEX REPLACE ";" "\\\\;" _NUMPY_VALUES ${_NUMPY_VALUES_OUTPUT}) +string(REGEX REPLACE "\n" ";" _NUMPY_VALUES ${_NUMPY_VALUES}) +# Just in case there is unexpected output from the Python command. +list(GET _NUMPY_VALUES -2 NUMPY_VERSION) +list(GET _NUMPY_VALUES -1 NUMPY_INCLUDE_DIRS) + +string(REGEX MATCH "^[0-9]+\\.[0-9]+\\.[0-9]+" _VER_CHECK "${NUMPY_VERSION}") +if("${_VER_CHECK}" STREQUAL "") + # The output from Python was unexpected. Raise an error always + # here, because we found NumPy, but it appears to be corrupted somehow. + message(FATAL_ERROR + "Requested version and include path from NumPy, got instead:\n${_NUMPY_VALUES_OUTPUT}\n") + return() +endif() + +# Make sure all directory separators are '/' +string(REGEX REPLACE "\\\\" "/" NUMPY_INCLUDE_DIRS ${NUMPY_INCLUDE_DIRS}) + +# Get the major and minor version numbers +string(REGEX REPLACE "\\." ";" _NUMPY_VERSION_LIST ${NUMPY_VERSION}) +list(GET _NUMPY_VERSION_LIST 0 NUMPY_VERSION_MAJOR) +list(GET _NUMPY_VERSION_LIST 1 NUMPY_VERSION_MINOR) +list(GET _NUMPY_VERSION_LIST 2 NUMPY_VERSION_PATCH) +string(REGEX MATCH "[0-9]*" NUMPY_VERSION_PATCH ${NUMPY_VERSION_PATCH}) +math(EXPR NUMPY_VERSION_DECIMAL + "(${NUMPY_VERSION_MAJOR} * 10000) + (${NUMPY_VERSION_MINOR} * 100) + ${NUMPY_VERSION_PATCH}") + +find_package_message(NUMPY + "Found NumPy: version \"${NUMPY_VERSION}\" ${NUMPY_INCLUDE_DIRS}" + "${NUMPY_INCLUDE_DIRS}${NUMPY_VERSION}") + +set(NUMPY_FOUND TRUE) + diff --git a/cmake/GtsamPythonWrap.cmake b/cmake/GtsamPythonWrap.cmake index d065a7828..714e37488 100644 --- a/cmake/GtsamPythonWrap.cmake +++ b/cmake/GtsamPythonWrap.cmake @@ -1,5 +1,5 @@ #Setup cache options -option(GTSAM_BUILD_PYTHON "Build Python wrapper statically (increases build time)" OFF) +set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "Target python version for GTSAM python module. Use 'Default' to chose the default version") set(GTSAM_BUILD_PYTHON_FLAGS "" CACHE STRING "Extra flags for running Matlab PYTHON compilation") set(GTSAM_PYTHON_INSTALL_PATH "" CACHE PATH "Python toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/borg/python") if(NOT GTSAM_PYTHON_INSTALL_PATH) @@ -8,13 +8,13 @@ endif() #Author: Paul Furgale Modified by Andrew Melim function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY) - # Boost - find_package(Boost COMPONENTS python filesystem system REQUIRED) - include_directories(${Boost_INCLUDE_DIRS}) + # # Boost + # find_package(Boost COMPONENTS python filesystem system REQUIRED) + # include_directories(${Boost_INCLUDE_DIRS}) - # Find Python - FIND_PACKAGE(PythonLibs 2.7 REQUIRED) - INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS}) + # # Find Python + # FIND_PACKAGE(PythonLibs 2.7 REQUIRED) + # INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS}) IF(APPLE) # The apple framework headers don't include the numpy headers for some reason. @@ -36,23 +36,46 @@ function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY) ENDIF() ENDIF(APPLE) + if(MSVC) + add_library(${moduleName}_python MODULE ${ARGN}) + set_target_properties(${moduleName}_python PROPERTIES + OUTPUT_NAME ${moduleName}_python + CLEAN_DIRECT_OUTPUT 1 + VERSION 1 + SOVERSION 0 + SUFFIX ".pyd") + target_link_libraries(${moduleName}_python ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} ${gtsamLib}) #temp - # Create a static library version - add_library(${TARGET_NAME} SHARED ${ARGN}) + set(PYLIB_OUTPUT_FILE $) + message(${PYLIB_OUTPUT_FILE}) + get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE) + set(PYLIB_SO_NAME ${PYLIB_OUTPUT_NAME}.pyd) - target_link_libraries(${TARGET_NAME} ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} gtsam-shared) - set_target_properties(${TARGET_NAME} PROPERTIES - OUTPUT_NAME ${TARGET_NAME} - CLEAN_DIRECT_OUTPUT 1 - VERSION 1 - SOVERSION 0) + ELSE() + # Create a shared library + add_library(${moduleName}_python SHARED ${generated_cpp_file}) + set_target_properties(${moduleName}_python PROPERTIES + OUTPUT_NAME ${moduleName}_python + CLEAN_DIRECT_OUTPUT 1) + target_link_libraries(${moduleName}_python ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} ${gtsamLib}) #temp + # On OSX and Linux, the python library must end in the extension .so. Build this + # filename here. + get_property(PYLIB_OUTPUT_FILE TARGET ${moduleName}_python PROPERTY LOCATION) + set(PYLIB_OUTPUT_FILE $) + message(${PYLIB_OUTPUT_FILE}) + get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE) + set(PYLIB_SO_NAME lib${moduleName}_python.so) + ENDIF(MSVC) - # On OSX and Linux, the python library must end in the extension .so. Build this - # filename here. - get_property(PYLIB_OUTPUT_FILE TARGET ${TARGET_NAME} PROPERTY LOCATION) - get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE) - set(PYLIB_SO_NAME ${PYLIB_OUTPUT_NAME}.so) + # Installs the library in the gtsam folder, which is used by setup.py to create the gtsam package + set(PYTHON_MODULE_DIRECTORY ${CMAKE_SOURCE_DIR}/python/gtsam) + # Cause the library to be output in the correct directory. + add_custom_command(TARGET ${moduleName}_python + POST_BUILD + COMMAND cp -v ${PYLIB_OUTPUT_FILE} ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME} + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} + COMMENT "Copying library files to python directory" ) # Cause the library to be output in the correct directory. add_custom_command(TARGET ${TARGET_NAME} @@ -64,4 +87,16 @@ function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY) get_directory_property(AMCF ADDITIONAL_MAKE_CLEAN_FILES) list(APPEND AMCF ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME}) set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES "${AMCF}") -endfunction(wrap_python) \ No newline at end of file +endfunction(wrap_python) + +# Macro to get list of subdirectories +macro(SUBDIRLIST result curdir) + file(GLOB children RELATIVE ${curdir} ${curdir}/*) + set(dirlist "") + foreach(child ${children}) + if(IS_DIRECTORY ${curdir}/${child}) + list(APPEND dirlist ${child}) + endif() + endforeach() + set(${result} ${dirlist}) +endmacro() diff --git a/gtsam/3rdparty/numpy_eigen/README.md b/gtsam/3rdparty/numpy_eigen/README.md new file mode 100644 index 000000000..bd5a3f44d --- /dev/null +++ b/gtsam/3rdparty/numpy_eigen/README.md @@ -0,0 +1,6 @@ +numpy_eigen +=========== + +A boost python converter that handles the copy of matrices from the Eigen linear algebra library in C++ to numpy in Python. + +This is a minimal version based on the original from Paul Furgale (https://github.com/ethz-asl/Schweizer-Messer/tree/master/numpy_eigen) \ No newline at end of file diff --git a/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/NumpyEigenConverter.hpp b/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/NumpyEigenConverter.hpp new file mode 100755 index 000000000..4cf16a974 --- /dev/null +++ b/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/NumpyEigenConverter.hpp @@ -0,0 +1,334 @@ +/** + * @file NumpyEigenConverter.hpp + * @author Paul Furgale + * @date Fri Feb 4 11:17:25 2011 + * + * @brief Classes to support conversion from numpy arrays in Python + * to Eigen3 matrices in c++ + * + * + */ + +#ifndef NUMPY_EIGEN_CONVERTER_HPP +#define NUMPY_EIGEN_CONVERTER_HPP + +#include +//#include + +#include "numpy/numpyconfig.h" +#ifdef NPY_1_7_API_VERSION +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#define NPE_PY_ARRAY_OBJECT PyArrayObject +#else +//TODO Remove this as soon as support for Numpy version before 1.7 is dropped +#define NPE_PY_ARRAY_OBJECT PyObject +#endif + +#define PY_ARRAY_UNIQUE_SYMBOL NP_Eigen_AS +#include + +#include "type_traits.hpp" +#include +#include "copy_routines.hpp" + + + +/** + * @class NumpyEigenConverter + * @tparam the Eigen3 matrix type this class is specialized for + * + * adapted from http://misspent.wordpress.com/2009/09/27/how-to-write-boost-python-converters/ + * General help available http://docs.scipy.org/doc/numpy/reference/c-api.array.html + * + * To use: + * + * #include + * + * + * BOOST_PYTHON_MODULE(libmy_module_python) + * { + * // The converters will cause a segfault unless import_array() is called before the first one + * import_array(); + * NumpyEigenConverter >::register_converter(); + * NumpyEigenConverter >::register_converter(); + * } + * + */ +template +struct NumpyEigenConverter +{ + + typedef EIGEN_MATRIX_T matrix_t; + typedef typename matrix_t::Scalar scalar_t; + + enum { + RowsAtCompileTime = matrix_t::RowsAtCompileTime, + ColsAtCompileTime = matrix_t::ColsAtCompileTime, + MaxRowsAtCompileTime = matrix_t::MaxRowsAtCompileTime, + MaxColsAtCompileTime = matrix_t::MaxColsAtCompileTime, + NpyType = TypeToNumPy::NpyType, + //Flags = ei_compute_matrix_flags<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::ret, + //CoeffReadCost = NumTraits::ReadCost, + Options = matrix_t::Options + //InnerStrideAtCompileTime = 1, + //OuterStrideAtCompileTime = (Options&RowMajor) ? ColsAtCompileTime : RowsAtCompileTime + }; + + static std::string castSizeOption(int option) + { + if(option == Eigen::Dynamic) + return "Dynamic"; + else + return boost::lexical_cast(option); + } + + static std::string toString() + { + return std::string() + "Eigen::Matrix<" + TypeToNumPy::typeString() + ", " + + castSizeOption(RowsAtCompileTime) + ", " + + castSizeOption(ColsAtCompileTime) + ", " + + boost::lexical_cast((int)Options) + ", " + + castSizeOption(MaxRowsAtCompileTime) + ", " + + castSizeOption(MaxColsAtCompileTime) + ">"; + } + + // The "Convert from C to Python" API + static PyObject * convert(const matrix_t & M) + { + PyObject * P = NULL; + if(RowsAtCompileTime == 1 || ColsAtCompileTime == 1) + { + // Create a 1D array + npy_intp dimensions[1]; + dimensions[0] = M.size(); + P = PyArray_SimpleNew(1, dimensions, TypeToNumPy::NpyType); + numpyTypeDemuxer< CopyEigenToNumpyVector >(&M, reinterpret_cast(P)); + } + else + { + // create a 2D array. + npy_intp dimensions[2]; + dimensions[0] = M.rows(); + dimensions[1] = M.cols(); + P = PyArray_SimpleNew(2, dimensions, TypeToNumPy::NpyType); + numpyTypeDemuxer< CopyEigenToNumpyMatrix >(&M, reinterpret_cast(P)); + } + + // incrementing the reference seems to cause a memory leak. + // boost::python::incref(P); + // This agrees with the sample code found here: + // http://mail.python.org/pipermail/cplusplus-sig/2008-October/013825.html + return P; + } + + static bool isDimensionValid(int requestedSize, int sizeAtCompileTime, int maxSizeAtCompileTime) + { + bool valid = true; + if(sizeAtCompileTime == Eigen::Dynamic) + { + // Check for dynamic fixed size + // http://eigen.tuxfamily.org/dox-devel/TutorialMatrixClass.html#TutorialMatrixOptTemplParams + if(!(maxSizeAtCompileTime == Eigen::Dynamic || requestedSize <= maxSizeAtCompileTime)) + { + valid = false; + } + } + else if(sizeAtCompileTime != requestedSize) + { + valid = false; + } + return valid; + } + + static void checkMatrixSizes(NPE_PY_ARRAY_OBJECT * obj_ptr) + { + int rows = PyArray_DIM(obj_ptr, 0); + int cols = PyArray_DIM(obj_ptr, 1); + + bool rowsValid = isDimensionValid(rows, RowsAtCompileTime, MaxRowsAtCompileTime); + bool colsValid = isDimensionValid(cols, ColsAtCompileTime, MaxColsAtCompileTime); + if(!rowsValid || !colsValid) + { + THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() + << ". Mismatched sizes."); + } + } + + static void checkRowVectorSizes(NPE_PY_ARRAY_OBJECT * obj_ptr, int cols) + { + if(!isDimensionValid(cols, ColsAtCompileTime, MaxColsAtCompileTime)) + { + THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() + << ". Mismatched sizes."); + } + } + + static void checkColumnVectorSizes(NPE_PY_ARRAY_OBJECT * obj_ptr, int rows) + { + // Check if the type can accomidate one column. + if(ColsAtCompileTime == Eigen::Dynamic || ColsAtCompileTime == 1) + { + if(!isDimensionValid(rows, RowsAtCompileTime, MaxRowsAtCompileTime)) + { + THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() + << ". Mismatched sizes."); + } + } + else + { + THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() + << ". Mismatched sizes."); + } + + } + + static void checkVectorSizes(NPE_PY_ARRAY_OBJECT * obj_ptr) + { + int size = PyArray_DIM(obj_ptr, 0); + + // If the number of rows is fixed at 1, assume that is the sense of the vector. + // Otherwise, assume it is a column. + if(RowsAtCompileTime == 1) + { + checkRowVectorSizes(obj_ptr, size); + } + else + { + checkColumnVectorSizes(obj_ptr, size); + } + } + + + static void* convertible(PyObject *obj_ptr) + { + // Check for a null pointer. + if(!obj_ptr) + { + //THROW_TYPE_ERROR("PyObject pointer was null"); + return 0; + } + + // Make sure this is a numpy array. + if (!PyArray_Check(obj_ptr)) + { + //THROW_TYPE_ERROR("Conversion is only defined for numpy array and matrix types"); + return 0; + } + + NPE_PY_ARRAY_OBJECT * array_ptr = reinterpret_cast(obj_ptr); + + // Check the type of the array. + int npyType = getNpyType(array_ptr); + + if(!TypeToNumPy::canConvert(npyType)) + { + //THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() + // << ". Mismatched types."); + return 0; + } + + + + // Check the array dimensions. + int nd = PyArray_NDIM(array_ptr); + + if(nd != 1 && nd != 2) + { + THROW_TYPE_ERROR("Conversion is only valid for arrays with 1 or 2 dimensions. Argument has " << nd << " dimensions"); + } + + if(nd == 1) + { + checkVectorSizes(array_ptr); + } + else + { + // Two-dimensional matrix type. + checkMatrixSizes(array_ptr); + } + + + return obj_ptr; + } + + + static void construct(PyObject *obj_ptr, boost::python::converter::rvalue_from_python_stage1_data *data) + { + boost::python::converter::rvalue_from_python_storage * matData = reinterpret_cast * >(data); + void* storage = matData->storage.bytes; + + // Make sure storage is 16byte aligned. With help from code from Memory.h + void * aligned = reinterpret_cast((reinterpret_cast(storage) & ~(size_t(15))) + 16); + + matrix_t * Mp = new (aligned) matrix_t(); + // Stash the memory chunk pointer for later use by boost.python + // This signals boost::python that the new value must be deleted eventually + data->convertible = storage; + + + // std::cout << "Creating aligned pointer " << aligned << " from storage " << storage << std::endl; + // std::cout << "matrix size: " << sizeof(matrix_t) << std::endl; + // std::cout << "referent size: " << boost::python::detail::referent_size< matrix_t & >::value << std::endl; + // std::cout << "sizeof(storage): " << sizeof(matData->storage) << std::endl; + // std::cout << "sizeof(bytes): " << sizeof(matData->storage.bytes) << std::endl; + + + + matrix_t & M = *Mp; + + if (!PyArray_Check(obj_ptr)) + { + THROW_TYPE_ERROR("construct is only defined for numpy array and matrix types"); + } + + NPE_PY_ARRAY_OBJECT * array_ptr = reinterpret_cast(obj_ptr); + + int nd = PyArray_NDIM(array_ptr); + if(nd == 1) + { + int size = PyArray_DIM(array_ptr, 0); + // This is a vector type + if(RowsAtCompileTime == 1) + { + // Row Vector + M.resize(1,size); + } + else + { + // Column Vector + M.resize(size,1); + } + numpyTypeDemuxer< CopyNumpyToEigenVector >(&M, array_ptr); + } + else + { + int rows = PyArray_DIM(array_ptr, 0); + int cols = PyArray_DIM(array_ptr, 1); + + M.resize(rows,cols); + numpyTypeDemuxer< CopyNumpyToEigenMatrix >(&M, array_ptr); + } + + + + + } + + + // The registration function. + static void register_converter() + { + boost::python::to_python_converter(); + boost::python::converter::registry::push_back( + &NumpyEigenConverter::convertible, + &NumpyEigenConverter::construct, + boost::python::type_id()); + + } + +}; + + + + +#endif /* NUMPY_EIGEN_CONVERTER_HPP */ diff --git a/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/boost_python_headers.hpp b/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/boost_python_headers.hpp new file mode 100755 index 000000000..5499d2917 --- /dev/null +++ b/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/boost_python_headers.hpp @@ -0,0 +1,250 @@ +/** + * @file boost_python_headers.hpp + * @author Paul Furgale + * @date Mon Dec 12 10:36:03 2011 + * + * @brief A header that specializes boost-python to work with fixed-sized Eigen types. + * + * The original version of this library did not include these specializations and this caused + * assert failures when running on Ubuntu 10.04 32-bit. More information about fixed-size + * vectorizable types in Eigen is available here: + * http://eigen.tuxfamily.org/dox-devel/TopicFixedSizeVectorizable.html + * + * This code has been tested on Ubunutu 10.04 64 and 32 bit, OSX Snow Leopard and OSX Lion. + * + * This code is derived from boost/python/converter/arg_from_python.hpp + * Copyright David Abrahams 2002. + * Distributed under the Boost Software License, Version 1.0. (See http://www.boost.org/LICENSE_1_0.txt) + * + */ +#ifndef NUMPY_EIGEN_CONVERTERS_HPP +#define NUMPY_EIGEN_CONVERTERS_HPP + +#include +#include +#include +#include +#include +#include + + +namespace boost { namespace python { namespace detail { + template + struct referent_size; + + // This bit of code makes sure we have 16 extra bytes to do the pointer alignment for fixed-sized Eigen types + template + struct referent_size< Eigen::Matrix& > + { + // Add 16 bytes so we can get alignment + BOOST_STATIC_CONSTANT( std::size_t, value = sizeof(Eigen::Matrix) + 16); + }; + + // This bit of code makes sure we have 16 extra bytes to do the pointer alignment for fixed-sized Eigen types + template + struct referent_size< Eigen::Matrix const & > + { + // Add 16 bytes so we can get alignment + BOOST_STATIC_CONSTANT( std::size_t, value = sizeof(Eigen::Matrix) + 16); + }; + + // This bit of code makes sure we have 16 extra bytes to do the pointer alignment for fixed-sized Eigen types + template + struct referent_size< Eigen::Matrix > + { + // Add 16 bytes so we can get alignment + BOOST_STATIC_CONSTANT( std::size_t, value = sizeof(Eigen::Matrix) + 16); + }; + + + }}} + +namespace boost { namespace python { namespace converter { + + + template + struct rvalue_from_python_data< Eigen::Matrix const &> : rvalue_from_python_storage< Eigen::Matrix const & > + { + typedef typename Eigen::Matrix T; +# if (!defined(__MWERKS__) || __MWERKS__ >= 0x3000) \ + && (!defined(__EDG_VERSION__) || __EDG_VERSION__ >= 245) \ + && (!defined(__DECCXX_VER) || __DECCXX_VER > 60590014) \ + && !defined(BOOST_PYTHON_SYNOPSIS) /* Synopsis' OpenCXX has trouble parsing this */ + // This must always be a POD struct with m_data its first member. + BOOST_STATIC_ASSERT(BOOST_PYTHON_OFFSETOF(rvalue_from_python_storage,stage1) == 0); +# endif + + // The usual constructor + rvalue_from_python_data(rvalue_from_python_stage1_data const & _stage1) + { + this->stage1 = _stage1; + } + + + // This constructor just sets m_convertible -- used by + // implicitly_convertible<> to perform the final step of the + // conversion, where the construct() function is already known. + rvalue_from_python_data(void* convertible) + { + this->stage1.convertible = convertible; + } + + // Destroys any object constructed in the storage. + ~rvalue_from_python_data() + { + // Realign the pointer and destroy + if (this->stage1.convertible == this->storage.bytes) + { + void * storage = reinterpret_cast(this->storage.bytes); + T * aligned = reinterpret_cast(reinterpret_cast((reinterpret_cast(storage) & ~(size_t(15))) + 16)); + + //std::cout << "Destroying " << (void*)aligned << std::endl; + aligned->T::~T(); + } + } + private: + typedef typename add_reference::type>::type ref_type; + }; + + + + + + // Used when T is a plain value (non-pointer, non-reference) type or + // a (non-volatile) const reference to a plain value type. + template + struct arg_rvalue_from_python< Eigen::Matrix > + { + typedef Eigen::Matrix const & T; + typedef typename boost::add_reference< + T + // We can't add_const here, or it would be impossible to pass + // auto_ptr args from Python to C++ + >::type result_type; + + arg_rvalue_from_python(PyObject * obj) : m_data(converter::rvalue_from_python_stage1(obj, registered::converters)) + , m_source(obj) + { + + } + bool convertible() const + { + return m_data.stage1.convertible != 0; + } + + +# if BOOST_MSVC < 1301 || _MSC_FULL_VER > 13102196 + typename arg_rvalue_from_python:: +# endif + result_type operator()() + { + if (m_data.stage1.construct != 0) + m_data.stage1.construct(m_source, &m_data.stage1); + + // Here is the magic... + // Realign the pointer + void * storage = reinterpret_cast(m_data.storage.bytes); + void * aligned = reinterpret_cast((reinterpret_cast(storage) & ~(size_t(15))) + 16); + + return python::detail::void_ptr_to_reference(aligned, (result_type(*)())0); + } + + private: + rvalue_from_python_data m_data; + PyObject* m_source; + }; + + + // Used when T is a plain value (non-pointer, non-reference) type or + // a (non-volatile) const reference to a plain value type. + template + struct arg_rvalue_from_python< Eigen::Matrix const & > + { + typedef Eigen::Matrix const & T; + typedef typename boost::add_reference< + T + // We can't add_const here, or it would be impossible to pass + // auto_ptr args from Python to C++ + >::type result_type; + + arg_rvalue_from_python(PyObject * obj) : m_data(converter::rvalue_from_python_stage1(obj, registered::converters)) + , m_source(obj) + { + + } + bool convertible() const + { + return m_data.stage1.convertible != 0; + } + + +# if BOOST_MSVC < 1301 || _MSC_FULL_VER > 13102196 + typename arg_rvalue_from_python:: +# endif + result_type operator()() + { + if (m_data.stage1.construct != 0) + m_data.stage1.construct(m_source, &m_data.stage1); + + // Here is the magic... + // Realign the pointer + void * storage = reinterpret_cast(m_data.storage.bytes); + void * aligned = reinterpret_cast((reinterpret_cast(storage) & ~(size_t(15))) + 16); + + return python::detail::void_ptr_to_reference(aligned, (result_type(*)())0); + } + + private: + rvalue_from_python_data m_data; + PyObject* m_source; + }; + + // Used when T is a plain value (non-pointer, non-reference) type or + // a (non-volatile) const reference to a plain value type. + template + struct arg_rvalue_from_python< Eigen::Matrix const > + { + typedef Eigen::Matrix const & T; + typedef typename boost::add_reference< + T + // We can't add_const here, or it would be impossible to pass + // auto_ptr args from Python to C++ + >::type result_type; + + arg_rvalue_from_python(PyObject * obj) : m_data(converter::rvalue_from_python_stage1(obj, registered::converters)) + , m_source(obj) + { + + } + bool convertible() const + { + return m_data.stage1.convertible != 0; + } + + +# if BOOST_MSVC < 1301 || _MSC_FULL_VER > 13102196 + typename arg_rvalue_from_python:: +# endif + result_type operator()() + { + if (m_data.stage1.construct != 0) + m_data.stage1.construct(m_source, &m_data.stage1); + + // Here is the magic... + // Realign the pointer + void * storage = reinterpret_cast(m_data.storage.bytes); + void * aligned = reinterpret_cast((reinterpret_cast(storage) & ~(size_t(15))) + 16); + + return python::detail::void_ptr_to_reference(aligned, (result_type(*)())0); + } + + private: + rvalue_from_python_data m_data; + PyObject* m_source; + }; + + + }}} + + +#endif /* NUMPY_EIGEN_CONVERTERS_HPP */ diff --git a/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/copy_routines.hpp b/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/copy_routines.hpp new file mode 100755 index 000000000..b112a7426 --- /dev/null +++ b/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/copy_routines.hpp @@ -0,0 +1,149 @@ +#ifndef NUMPY_EIGEN_COPY_ROUTINES_HPP +#define NUMPY_EIGEN_COPY_ROUTINES_HPP + + +template +struct CopyNumpyToEigenMatrix +{ + typedef EIGEN_T matrix_t; + typedef typename matrix_t::Scalar scalar_t; + + template + void exec(EIGEN_T * M_, NPE_PY_ARRAY_OBJECT * P_) + { + // Assumes M is already initialized. + for(int r = 0; r < M_->rows(); r++) + { + for(int c = 0; c < M_->cols(); c++) + { + T * p = static_cast(PyArray_GETPTR2(P_, r, c)); + (*M_)(r,c) = static_cast(*p); + } + } + } + +}; + +template +struct CopyEigenToNumpyMatrix +{ + typedef EIGEN_T matrix_t; + typedef typename matrix_t::Scalar scalar_t; + + template + void exec(EIGEN_T * M_, NPE_PY_ARRAY_OBJECT * P_) + { + // Assumes M is already initialized. + for(int r = 0; r < M_->rows(); r++) + { + for(int c = 0; c < M_->cols(); c++) + { + T * p = static_cast(PyArray_GETPTR2(P_, r, c)); + *p = static_cast((*M_)(r,c)); + } + } + } + +}; + +template +struct CopyEigenToNumpyVector +{ + typedef EIGEN_T matrix_t; + typedef typename matrix_t::Scalar scalar_t; + + template + void exec(EIGEN_T * M_, NPE_PY_ARRAY_OBJECT * P_) + { + // Assumes M is already initialized. + for(int i = 0; i < M_->size(); i++) + { + T * p = static_cast(PyArray_GETPTR1(P_, i)); + *p = static_cast((*M_)(i)); + } + } + +}; + + +template +struct CopyNumpyToEigenVector +{ + typedef EIGEN_T matrix_t; + typedef typename matrix_t::Scalar scalar_t; + + template + void exec(EIGEN_T * M_, NPE_PY_ARRAY_OBJECT * P_) + { + // Assumes M is already initialized. + for(int i = 0; i < M_->size(); i++) + { + T * p = static_cast(PyArray_GETPTR1(P_, i)); + (*M_)(i) = static_cast(*p); + } + } + +}; + + + + +// Crazy syntax in this function was found here: +// http://stackoverflow.com/questions/1840253/c-template-member-function-of-template-class-called-from-template-function/1840318#1840318 +template< typename FUNCTOR_T> +inline void numpyTypeDemuxer(typename FUNCTOR_T::matrix_t * M, NPE_PY_ARRAY_OBJECT * P) +{ + FUNCTOR_T f; + + int npyType = getNpyType(P); + switch(npyType) + { + case NPY_BOOL: + f.template exec(M,P); + break; + case NPY_BYTE: + f.template exec(M,P); + break; + case NPY_UBYTE: + f.template exec(M,P); + break; + case NPY_SHORT: + f.template exec(M,P); + break; + case NPY_USHORT: + f.template exec(M,P); + break; + case NPY_INT: + f.template exec(M,P); + break; + case NPY_UINT: + f.template exec(M,P); + break; + case NPY_LONG: + f.template exec(M,P); + break; + case NPY_ULONG: + f.template exec(M,P); + break; + case NPY_LONGLONG: + f.template exec(M,P); + break; + case NPY_ULONGLONG: + f.template exec(M,P); + break; + case NPY_FLOAT: + f.template exec(M,P); + break; + case NPY_DOUBLE: + f.template exec(M,P); + break; + case NPY_LONGDOUBLE: + f.template exec(M,P); + break; + default: + THROW_TYPE_ERROR("Unsupported type: " << npyTypeToString(npyType)); + } +} + + +#endif /* NUMPY_EIGEN_COPY_ROUTINES_HPP */ diff --git a/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/type_traits.hpp b/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/type_traits.hpp new file mode 100755 index 000000000..97e4bb5a0 --- /dev/null +++ b/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/type_traits.hpp @@ -0,0 +1,191 @@ +#ifndef NUMPY_EIGEN_TYPE_TRAITS_HPP +#define NUMPY_EIGEN_TYPE_TRAITS_HPP + +#define THROW_TYPE_ERROR(msg) \ + { \ + std::stringstream type_error_ss; \ + type_error_ss << msg; \ + PyErr_SetString(PyExc_TypeError, type_error_ss.str().c_str()); \ + throw boost::python::error_already_set(); \ + } + + +//////////////////////////////////////////////// +// TypeToNumPy +// Defines helper functions based on the Eigen3 matrix type that +// decide what conversions can happen Eigen3 --> NumPy +// Also, converts a type to a NumPy enum. +template struct TypeToNumPy; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_INT }; + static const char * npyString() { return "NPY_INT"; } + static const char * typeString() { return "int"; } + static bool canConvert(int type) + { + return type == NPY_INT || type == NPY_LONG; + } +}; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_LONG }; + static const char * npyString() { return "NPY_LONG"; } + static const char * typeString() { return "long"; } + static bool canConvert(int type) + { + return type == NPY_INT || type == NPY_LONG; + } +}; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_UINT64 }; + static const char * npyString() { return "NPY_UINT64"; } + static const char * typeString() { return "uint64"; } + static bool canConvert(int type) + { + return type == NPY_UINT8 || type == NPY_USHORT || + type == NPY_UINT16 || type == NPY_UINT32 || + type == NPY_ULONG || type == NPY_ULONGLONG || + type == NPY_UINT64; + } +}; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_UBYTE }; + static const char * npyString() { return "NPY_UBYTE"; } + static const char * typeString() { return "unsigned char"; } + static bool canConvert(int type) + { + return type == NPY_UBYTE || type == NPY_BYTE || type == NPY_CHAR; + } +}; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_BYTE }; + static const char * npyString() { return "NPY_BYTE"; } + static const char * typeString() { return "char"; } + static bool canConvert(int type) + { + return type == NPY_UBYTE || type == NPY_BYTE || type == NPY_CHAR; + } +}; + + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_FLOAT }; + static const char * npyString() { return "NPY_FLOAT"; } + static const char * typeString() { return "float"; } + static bool canConvert(int type) + { + return type == NPY_INT || type == NPY_FLOAT || type == NPY_LONG; + } +}; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_DOUBLE }; + static const char * npyString() { return "NPY_DOUBLE"; } + static const char * typeString() { return "double"; } + static bool canConvert(int type) + { + return type == NPY_INT || type == NPY_FLOAT || type == NPY_DOUBLE || type == NPY_LONG; + } +}; + + +inline int getNpyType(PyObject * obj_ptr){ + return PyArray_ObjectType(obj_ptr, 0); +} + +#ifdef NPY_1_7_API_VERSION +inline int getNpyType(PyArrayObject * obj_ptr){ + PyArray_Descr * descr = PyArray_MinScalarType(obj_ptr); + if (descr == NULL){ + THROW_TYPE_ERROR("Unsupported type: PyArray_MinScalarType returned null!"); + } + return descr->type_num; +} +#endif + +inline const char * npyTypeToString(int npyType) +{ + switch(npyType) + { + case NPY_BOOL: + return "NPY_BOOL"; + case NPY_BYTE: + return "NPY_BYTE"; + case NPY_UBYTE: + return "NPY_UBYTE"; + case NPY_SHORT: + return "NPY_SHORT"; + case NPY_USHORT: + return "NPY_USHORT"; + case NPY_INT: + return "NPY_INT"; + case NPY_UINT: + return "NPY_UINT"; + case NPY_LONG: + return "NPY_LONG"; + case NPY_ULONG: + return "NPY_ULONG"; + case NPY_LONGLONG: + return "NPY_LONGLONG"; + case NPY_ULONGLONG: + return "NPY_ULONGLONG"; + case NPY_FLOAT: + return "NPY_FLOAT"; + case NPY_DOUBLE: + return "NPY_DOUBLE"; + case NPY_LONGDOUBLE: + return "NPY_LONGDOUBLE"; + case NPY_CFLOAT: + return "NPY_CFLOAT"; + case NPY_CDOUBLE: + return "NPY_CDOUBLE"; + case NPY_CLONGDOUBLE: + return "NPY_CLONGDOUBLE"; + case NPY_OBJECT: + return "NPY_OBJECT"; + case NPY_STRING: + return "NPY_STRING"; + case NPY_UNICODE: + return "NPY_UNICODE"; + case NPY_VOID: + return "NPY_VOID"; + case NPY_NTYPES: + return "NPY_NTYPES"; + case NPY_NOTYPE: + return "NPY_NOTYPE"; + case NPY_CHAR: + return "NPY_CHAR"; + default: + return "Unknown type"; + } +} + +inline std::string npyArrayTypeString(NPE_PY_ARRAY_OBJECT * obj_ptr) +{ + std::stringstream ss; + int nd = PyArray_NDIM(obj_ptr); + ss << "numpy.array<" << npyTypeToString(getNpyType(obj_ptr)) << ">["; + if(nd > 0) + { + ss << PyArray_DIM(obj_ptr, 0); + for(int i = 1; i < nd; i++) + { + ss << ", " << PyArray_DIM(obj_ptr, i); + } + } + ss << "]"; + return ss.str(); +} + + +#endif /* NUMPY_EIGEN_TYPE_TRAITS_HPP */ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index e548f8eea..d8b8a4682 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -310,7 +310,7 @@ namespace gtsam { * Exponential map at identity - create a rotation from canonical coordinates * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula */ - static Rot3 Expmap(const Vector& v, OptionalJacobian<3,3> H = boost::none) { + static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = boost::none) { if(H) *H = Rot3::ExpmapDerivative(v); #ifdef GTSAM_USE_QUATERNIONS return traits::Expmap(v); diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index b3b8b9a41..6db13adb6 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -141,7 +141,20 @@ namespace gtsam { /* ************************************************************************* */ pair GaussianBayesNet::matrix() const { - return GaussianFactorGraph(*this).jacobian(); + GaussianFactorGraph factorGraph(*this); + KeySet keys = factorGraph.keys(); + // add frontal keys in order + Ordering ordering; + BOOST_FOREACH (const sharedConditional& cg, *this) + BOOST_FOREACH (Key key, cg->frontals()) { + ordering.push_back(key); + keys.erase(key); + } + // add remaining keys in case Bayes net is incomplete + BOOST_FOREACH (Key key, keys) + ordering.push_back(key); + // return matrix and RHS + return factorGraph.jacobian(ordering); } ///* ************************************************************************* */ diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index ace9aa09a..bab83a0fb 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -67,52 +67,55 @@ void PreintegratedCombinedMeasurements::resetIntegration() { //------------------------------------------------------------------------------ void PreintegratedCombinedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT) { - - const Matrix3 dRij = deltaXij_.R(); // expensive when quaternion + const Matrix3 dRij = deltaRij().matrix(); // expensive when quaternion // Update preintegrated measurements. - Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr - Matrix9 F_9x9; // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix93 G1,G2; + Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr + Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix93 B, C; PreintegrationBase::update(measuredAcc, measuredOmega, deltaT, - &D_incrR_integratedOmega, &F_9x9, &G1, &G2); + &D_incrR_integratedOmega, &A, &B, &C); - // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that - // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we - // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ + // Update preintegrated measurements covariance: as in [2] we consider a first + // order propagation that can be seen as a prediction phase in an EKF + // framework. In this implementation, contrarily to [2] we consider the + // uncertainty of the bias selection and we keep correlation between biases + // and preintegrated measurements // Single Jacobians to propagate covariance Matrix3 H_vel_biasacc = -dRij * deltaT; Matrix3 H_angles_biasomega = -D_incrR_integratedOmega * deltaT; // overall Jacobian wrt preintegrated measurements (df/dx) - Eigen::Matrix F; + Eigen::Matrix F; F.setZero(); - F.block<9, 9>(0, 0) = F_9x9; + F.block<9, 9>(0, 0) = A; F.block<3, 3>(0, 12) = H_angles_biasomega; F.block<3, 3>(6, 9) = H_vel_biasacc; F.block<6, 6>(9, 9) = I_6x6; // first order uncertainty propagation - // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() - Eigen::Matrix G_measCov_Gt; + // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * + // G.transpose() + Eigen::Matrix G_measCov_Gt; G_measCov_Gt.setZero(15, 15); // BLOCK DIAGONAL TERMS D_t_t(&G_measCov_Gt) = deltaT * p().integrationCovariance; - D_v_v(&G_measCov_Gt) = (1 / deltaT) * (H_vel_biasacc) - * (p().accelerometerCovariance + p().biasAccOmegaInit.block<3, 3>(0, 0)) - * (H_vel_biasacc.transpose()); - D_R_R(&G_measCov_Gt) = (1 / deltaT) * (H_angles_biasomega) - * (p().gyroscopeCovariance + p().biasAccOmegaInit.block<3, 3>(3, 3)) - * (H_angles_biasomega.transpose()); + D_v_v(&G_measCov_Gt) = + (1 / deltaT) * (H_vel_biasacc) * + (p().accelerometerCovariance + p().biasAccOmegaInit.block<3, 3>(0, 0)) * + (H_vel_biasacc.transpose()); + D_R_R(&G_measCov_Gt) = + (1 / deltaT) * (H_angles_biasomega) * + (p().gyroscopeCovariance + p().biasAccOmegaInit.block<3, 3>(3, 3)) * + (H_angles_biasomega.transpose()); D_a_a(&G_measCov_Gt) = deltaT * p().biasAccCovariance; D_g_g(&G_measCov_Gt) = deltaT * p().biasOmegaCovariance; // OFF BLOCK DIAGONAL TERMS - Matrix3 temp = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0) - * H_angles_biasomega.transpose(); + Matrix3 temp = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0) * + H_angles_biasomega.transpose(); D_v_R(&G_measCov_Gt) = temp; D_R_v(&G_measCov_Gt) = temp.transpose(); preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 7664c7fd5..929b7ac22 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -124,17 +124,21 @@ public: //// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G; // } -/// @} -/// @name Testable -/// @{ + /// @} + /// @name Testable + /// @{ -/// print with optional string - void print(const std::string& s = "") const { - // explicit printing for now. - std::cout << s + ".Acc [" << biasAcc_.transpose() << "]" << std::endl; - std::cout << s + ".Gyro [" << biasGyro_.transpose() << "]" << std::endl; + /// ostream operator + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const ConstantBias& bias) { + os << "acc = " << Point3(bias.accelerometer()); + os << " gyro = " << Point3(bias.gyroscope()); + return os; } + /// print with optional string + void print(const std::string& s = "") const { std::cout << s << *this << std::endl; } + /** equality up to tolerance */ inline bool equals(const ConstantBias& expected, double tol = 1e-5) const { return equal_with_abs_tol(biasAcc_, expected.biasAcc_, tol) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 692154c9d..a03dd92f8 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -52,36 +52,33 @@ void PreintegratedImuMeasurements::resetIntegration() { //------------------------------------------------------------------------------ void PreintegratedImuMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { - - static const Matrix93 Gi = (Matrix93() << Z_3x3, I_3x3, Z_3x3).finished(); - // Update preintegrated measurements (also get Jacobian) - Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix93 G1, G2; + Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix93 B, C; Matrix3 D_incrR_integratedOmega; PreintegrationBase::update(measuredAcc, measuredOmega, dt, - &D_incrR_integratedOmega, &F, &G1, &G2); + &D_incrR_integratedOmega, &A, &B, &C); // first order covariance propagation: - // as in [2] we consider a first order propagation that can be seen as a prediction phase in EKF - /* --------------------------------------------------------------------------------------------*/ - // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G' - // NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise - // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) -#ifdef OLD_JACOBIAN_CALCULATION - Matrix9 G; - G << G1, Gi, G2; - Matrix9 Cov; - Cov << p().accelerometerCovariance / dt, Z_3x3, Z_3x3, - Z_3x3, p().integrationCovariance * dt, Z_3x3, - Z_3x3, Z_3x3, p().gyroscopeCovariance / dt; - preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G * Cov * G.transpose(); -#else - preintMeasCov_ = F * preintMeasCov_ * F.transpose() - + Gi * (p().integrationCovariance * dt) * Gi.transpose() // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt) - + G1 * (p().accelerometerCovariance / dt) * G1.transpose() - + G2 * (p().gyroscopeCovariance / dt) * G2.transpose(); -#endif + // as in [2] we consider a first order propagation that can be seen as a + // prediction phase in EKF + + // propagate uncertainty + // TODO(frank): use noiseModel routine so we can have arbitrary noise models. + const Matrix3& aCov = p().accelerometerCovariance; + const Matrix3& wCov = p().gyroscopeCovariance; + const Matrix3& iCov = p().integrationCovariance; + + // NOTE(luca): (1/dt) allows to pass from continuous time noise to discrete + // time noise + // measurementCovariance_discrete = measurementCovariance_contTime/dt + preintMeasCov_ = A * preintMeasCov_ * A.transpose(); + preintMeasCov_.noalias() += B * (aCov / dt) * B.transpose(); + preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose(); + + // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt) + static const Matrix93 Gi = (Matrix93() << Z_3x3, I_3x3, Z_3x3).finished(); + preintMeasCov_.noalias() += Gi * (iCov * dt) * Gi.transpose(); } //------------------------------------------------------------------------------ @@ -125,17 +122,22 @@ gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { gtsam::NonlinearFactor::shared_ptr(new This(*this))); } +//------------------------------------------------------------------------------ +std::ostream& operator<<(std::ostream& os, const ImuFactor& f) { + os << " preintegrated measurements:\n" << f._PIM_; + ; + // Print standard deviations on covariance only + os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose(); + return os; +} + //------------------------------------------------------------------------------ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << "ImuFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << ")\n"; - Base::print(""); - _PIM_.print(" preintegrated measurements:"); - // Print standard deviations on covariance only - cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose() - << endl; + cout << *this << endl; } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index d94789d4a..b0b37d73b 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -201,14 +201,13 @@ public: /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const; - /** implement functions needed for Testable */ - - /// print + /// @name Testable + /// @{ + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&); virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - - /// equals virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + /// @} /** Access the preintegrated measurements. */ diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 6266c328f..860eaa85c 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -74,11 +74,17 @@ Matrix7 NavState::matrix() const { return T; } +//------------------------------------------------------------------------------ +ostream& operator<<(ostream& os, const NavState& state) { + os << "R:" << state.attitude(); + os << "p:" << state.position() << endl; + os << "v:" << Point3(state.velocity()) << endl; + return os; +} + //------------------------------------------------------------------------------ void NavState::print(const string& s) const { - attitude().print(s + ".R"); - position().print(s + ".p"); - gtsam::print((Vector) v_, s + ".v"); + cout << s << *this << endl; } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 9561aa77b..4c8b50776 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -75,18 +75,9 @@ public: /// @name Component Access /// @{ - inline const Rot3& attitude() const { - return R_; - } - inline const Point3& position() const { - return t_; - } - inline const Velocity3& velocity() const { - return v_; - } - const Rot3& attitude(OptionalJacobian<3, 9> H) const; - const Point3& position(OptionalJacobian<3, 9> H) const; - const Velocity3& velocity(OptionalJacobian<3, 9> H) const; + const Rot3& attitude(OptionalJacobian<3, 9> H = boost::none) const; + const Point3& position(OptionalJacobian<3, 9> H = boost::none) const; + const Velocity3& velocity(OptionalJacobian<3, 9> H = boost::none) const; const Pose3 pose() const { return Pose3(attitude(), position()); @@ -124,6 +115,9 @@ public: /// @name Testable /// @{ + /// Output stream operator + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const NavState& state); + /// print void print(const std::string& s = "") const; @@ -229,6 +223,8 @@ public: false, OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 = boost::none) const; + /// @} + private: /// @{ /// serialization diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 5552a952e..a26498500 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -28,10 +28,17 @@ using namespace std; namespace gtsam { +//------------------------------------------------------------------------------ +PreintegrationBase::PreintegrationBase(const boost::shared_ptr& p, + const Bias& biasHat) + : p_(p), biasHat_(biasHat), deltaTij_(0.0) { + resetIntegration(); +} + //------------------------------------------------------------------------------ void PreintegrationBase::resetIntegration() { deltaTij_ = 0.0; - deltaXij_ = NavState(); + deltaXij_ = TangentVector(); delRdelBiasOmega_ = Z_3x3; delPdelBiasAcc_ = Z_3x3; delPdelBiasOmega_ = Z_3x3; @@ -39,14 +46,20 @@ void PreintegrationBase::resetIntegration() { delVdelBiasOmega_ = Z_3x3; } +//------------------------------------------------------------------------------ +ostream& operator<<(ostream& os, const PreintegrationBase& pim) { + os << " deltaTij " << pim.deltaTij_ << endl; + os << " deltaRij " << Point3(pim.theta()) << endl; + os << " deltaPij " << Point3(pim.deltaPij()) << endl; + os << " deltaVij " << Point3(pim.deltaVij()) << endl; + os << " gyrobias " << Point3(pim.biasHat_.gyroscope()) << endl; + os << " acc_bias " << Point3(pim.biasHat_.accelerometer()) << endl; + return os; +} + //------------------------------------------------------------------------------ void PreintegrationBase::print(const string& s) const { - cout << s << endl; - cout << " deltaTij [" << deltaTij_ << "]" << endl; - cout << " deltaRij.ypr = (" << deltaRij().ypr().transpose() << ")" << endl; - cout << " deltaPij [ " << deltaPij().transpose() << " ]" << endl; - cout << " deltaVij [ " << deltaVij().transpose() << " ]" << endl; - biasHat_.print(" biasHat"); + cout << s << *this << endl; } //------------------------------------------------------------------------------ @@ -54,8 +67,8 @@ bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) const { const bool params_match = p_->equals(*other.p_, tol); return params_match && fabs(deltaTij_ - other.deltaTij_) < tol - && deltaXij_.equals(other.deltaXij_, tol) && biasHat_.equals(other.biasHat_, tol) + && equal_with_abs_tol(deltaXij_.vector(), other.deltaXij_.vector(), tol) && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) @@ -70,28 +83,25 @@ pair PreintegrationBase::correctMeasurementsByBiasAndSensorPos OptionalJacobian<3, 3> D_correctedAcc_measuredOmega, OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const { -// Correct for bias in the sensor frame + // Correct for bias in the sensor frame Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc); Vector3 j_correctedOmega = biasHat_.correctGyroscope(j_measuredOmega); -// Compensate for sensor-body displacement if needed: we express the quantities -// (originally in the IMU frame) into the body frame -// Equations below assume the "body" frame is the CG + // Compensate for sensor-body displacement if needed: we express the quantities + // (originally in the IMU frame) into the body frame + // Equations below assume the "body" frame is the CG if (p().body_P_sensor) { - // Correct omega to rotation rate vector in the body frame + // Get sensor to body rotation matrix const Matrix3 bRs = p().body_P_sensor->rotation().matrix(); - j_correctedOmega = bRs * j_correctedOmega; - // Correct acceleration + // Convert angular velocity and acceleration from sensor to body frame + j_correctedOmega = bRs * j_correctedOmega; j_correctedAcc = bRs * j_correctedAcc; // Jacobians - if (D_correctedAcc_measuredAcc) - *D_correctedAcc_measuredAcc = bRs; - if (D_correctedAcc_measuredOmega) - *D_correctedAcc_measuredOmega = Matrix3::Zero(); - if (D_correctedOmega_measuredOmega) - *D_correctedOmega_measuredOmega = bRs; + if (D_correctedAcc_measuredAcc) *D_correctedAcc_measuredAcc = bRs; + if (D_correctedAcc_measuredOmega) *D_correctedAcc_measuredOmega = Z_3x3; + if (D_correctedOmega_measuredOmega) *D_correctedOmega_measuredOmega = bRs; // Centrifugal acceleration const Vector3 b_arm = p().body_P_sensor->translation().vector(); @@ -101,6 +111,7 @@ pair PreintegrationBase::correctMeasurementsByBiasAndSensorPos const Matrix3 body_Omega_body = skewSymmetric(j_correctedOmega); const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm j_correctedAcc -= body_Omega_body * b_velocity_bs; + // Update derivative: centrifugal causes the correlation between acc and omega!!! if (D_correctedAcc_measuredOmega) { double wdp = j_correctedOmega.dot(b_arm); @@ -111,63 +122,107 @@ pair PreintegrationBase::correctMeasurementsByBiasAndSensorPos } } -// Do update in one fell swoop return make_pair(j_correctedAcc, j_correctedOmega); } //------------------------------------------------------------------------------ -NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc, - const Vector3& j_measuredOmega, const double dt, - OptionalJacobian<9, 9> D_updated_current, - OptionalJacobian<9, 3> D_updated_measuredAcc, - OptionalJacobian<9, 3> D_updated_measuredOmega) const { +// See extensive discussion in ImuFactor.lyx +PreintegrationBase::TangentVector PreintegrationBase::UpdateEstimate( + const Vector3& a_body, const Vector3& w_body, double dt, + const TangentVector& zeta, OptionalJacobian<9, 9> A, + OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) { + // Calculate exact mean propagation + Matrix3 H; + const Matrix3 R = Rot3::Expmap(zeta.theta(), H).matrix(); + const Matrix3 invH = H.inverse(); + const Vector3 a_nav = R * a_body; + const double dt22 = 0.5 * dt * dt; - Vector3 j_correctedAcc, j_correctedOmega; - Matrix3 D_correctedAcc_measuredAcc, // - D_correctedAcc_measuredOmega, // - D_correctedOmega_measuredOmega; - bool needDerivs = D_updated_measuredAcc && D_updated_measuredOmega - && p().body_P_sensor; - boost::tie(j_correctedAcc, j_correctedOmega) = - correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega, - (needDerivs ? &D_correctedAcc_measuredAcc : 0), - (needDerivs ? &D_correctedAcc_measuredOmega : 0), - (needDerivs ? &D_correctedOmega_measuredOmega : 0)); -// Do update in one fell swoop - Matrix93 D_updated_correctedAcc, D_updated_correctedOmega; - NavState updated = deltaXij_.update(j_correctedAcc, j_correctedOmega, dt, - D_updated_current, - (needDerivs ? D_updated_correctedAcc : D_updated_measuredAcc), - (needDerivs ? D_updated_correctedOmega : D_updated_measuredOmega)); - if (needDerivs) { - *D_updated_measuredAcc = D_updated_correctedAcc - * D_correctedAcc_measuredAcc; - *D_updated_measuredOmega = D_updated_correctedOmega - * D_correctedOmega_measuredOmega; - if (!p().body_P_sensor->translation().vector().isZero()) { - *D_updated_measuredOmega += D_updated_correctedAcc - * D_correctedAcc_measuredOmega; - } + TangentVector zetaPlus(zeta.theta() + invH * w_body * dt, + zeta.position() + zeta.velocity() * dt + a_nav * dt22, + zeta.velocity() + a_nav * dt); + + if (A) { + // First order (small angle) approximation of derivative of invH*w: + const Matrix3 invHw_H_theta = skewSymmetric(-0.5 * w_body); + + // Exact derivative of R*a with respect to theta: + const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * H; + + A->setIdentity(); + A->block<3, 3>(0, 0).noalias() += invHw_H_theta * dt; + A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; + A->block<3, 3>(3, 6) = I_3x3 * dt; + A->block<3, 3>(6, 0) = a_nav_H_theta * dt; + } + if (B) { + B->block<3, 3>(0, 0) = Z_3x3; + B->block<3, 3>(3, 0) = R * dt22; + B->block<3, 3>(6, 0) = R * dt; + } + if (C) { + C->block<3, 3>(0, 0) = invH * dt; + C->block<3, 3>(3, 0) = Z_3x3; + C->block<3, 3>(6, 0) = Z_3x3; + } + + return zetaPlus; +} + +//------------------------------------------------------------------------------ +PreintegrationBase::TangentVector PreintegrationBase::updatedDeltaXij( + const Vector3& measuredAcc, const Vector3& measuredOmega, double dt, + OptionalJacobian<9, 9> A, OptionalJacobian<9, 3> B, + OptionalJacobian<9, 3> C) const { + if (!p().body_P_sensor) { + // Correct for bias in the sensor frame + Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); + + // Do update in one fell swoop + return UpdateEstimate(correctedAcc, correctedOmega, dt, deltaXij_, A, B, C); + } else { + // More complicated derivatives in case of sensor displacement + Vector3 correctedAcc, correctedOmega; + Matrix3 D_correctedAcc_measuredAcc, D_correctedAcc_measuredOmega, + D_correctedOmega_measuredOmega; + boost::tie(correctedAcc, correctedOmega) = + correctMeasurementsByBiasAndSensorPose( + measuredAcc, measuredOmega, (B ? &D_correctedAcc_measuredAcc : 0), + (C ? &D_correctedAcc_measuredOmega : 0), + (C ? &D_correctedOmega_measuredOmega : 0)); + + // Do update in one fell swoop + Matrix93 D_updated_correctedAcc, D_updated_correctedOmega; + const PreintegrationBase::TangentVector updated = + UpdateEstimate(correctedAcc, correctedOmega, dt, deltaXij_, A, + ((B || C) ? &D_updated_correctedAcc : 0), + (C ? &D_updated_correctedOmega : 0)); + if (B) *B = D_updated_correctedAcc* D_correctedAcc_measuredAcc; + if (C) { + *C = D_updated_correctedOmega* D_correctedOmega_measuredOmega; + if (!p().body_P_sensor->translation().vector().isZero()) + *C += D_updated_correctedAcc* D_correctedAcc_measuredOmega; + } + return updated; } - return updated; } //------------------------------------------------------------------------------ void PreintegrationBase::update(const Vector3& j_measuredAcc, - const Vector3& j_measuredOmega, const double dt, - Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current, - Matrix93* D_updated_measuredAcc, Matrix93* D_updated_measuredOmega) { + const Vector3& j_measuredOmega, double dt, + Matrix3* D_incrR_integratedOmega, Matrix9* A, + Matrix93* B, Matrix93* C) { + // Save current rotation for updating Jacobians + const Rot3 oldRij = deltaRij(); -// Save current rotation for updating Jacobians - const Rot3 oldRij = deltaXij_.attitude(); - -// Do update + // Do update deltaTij_ += dt; - deltaXij_ = updatedDeltaXij(j_measuredAcc, j_measuredOmega, dt, - D_updated_current, D_updated_measuredAcc, D_updated_measuredOmega); // functional + deltaXij_ = updatedDeltaXij(j_measuredAcc, j_measuredOmega, dt, A, B, C); -// Update Jacobians -// TODO(frank): we are repeating some computation here: accessible in F ? + // Update Jacobians + // TODO(frank): we are repeating some computation here: accessible in A ? + // Possibly: derivatives are just -B and -C ?? Vector3 j_correctedAcc, j_correctedOmega; boost::tie(j_correctedAcc, j_correctedOmega) = correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega); @@ -204,8 +259,8 @@ Vector9 PreintegrationBase::biasCorrectedDelta( Vector9 xi; Matrix3 D_dR_correctedRij; -// TODO(frank): could line below be simplified? It is equivalent to -// LogMap(deltaRij_.compose(Expmap(biasInducedOmega))) + // TODO(frank): could line below be simplified? It is equivalent to + // LogMap(deltaRij_.compose(Expmap(biasInducedOmega))) NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0); NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer() + delPdelBiasOmega_ * biasIncr.gyroscope(); @@ -224,29 +279,55 @@ Vector9 PreintegrationBase::biasCorrectedDelta( //------------------------------------------------------------------------------ NavState PreintegrationBase::predict(const NavState& state_i, - const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, - OptionalJacobian<9, 6> H2) const { -// correct for bias + const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 6> H2) const { + // TODO(frank): make sure this stuff is still correct Matrix96 D_biasCorrected_bias; - Vector9 biasCorrected = biasCorrectedDelta(bias_i, - H2 ? &D_biasCorrected_bias : 0); + Vector9 biasCorrected = + biasCorrectedDelta(bias_i, H2 ? &D_biasCorrected_bias : 0); -// integrate on tangent space + // Correct for initial velocity and gravity Matrix9 D_delta_state, D_delta_biasCorrected; Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity, - p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0, - H2 ? &D_delta_biasCorrected : 0); + p().omegaCoriolis, p().use2ndOrderCoriolis, + H1 ? &D_delta_state : 0, + H2 ? &D_delta_biasCorrected : 0); -// Use retract to get back to NavState manifold + // Use retract to get back to NavState manifold Matrix9 D_predict_state, D_predict_delta; NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta); - if (H1) - *H1 = D_predict_state + D_predict_delta * D_delta_state; - if (H2) - *H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias; + if (H1) *H1 = D_predict_state + D_predict_delta* D_delta_state; + if (H2) *H2 = D_predict_delta* D_delta_biasCorrected * D_biasCorrected_bias; return state_j; } +//------------------------------------------------------------------------------ +Vector9 PreintegrationBase::computeError(const NavState& state_i, + const NavState& state_j, + const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 9> H2, + OptionalJacobian<9, 6> H3) const { + // Predict state at time j + Matrix9 D_predict_state_i; + Matrix96 D_predict_bias_i; + NavState predictedState_j = predict( + state_i, bias_i, H1 ? &D_predict_state_i : 0, H3 ? &D_predict_bias_i : 0); + + // Calculate error + Matrix9 D_error_state_j, D_error_predict; + Vector9 error = + state_j.localCoordinates(predictedState_j, H2 ? &D_error_state_j : 0, + H1 || H3 ? &D_error_predict : 0); + + if (H1) *H1 << D_error_predict* D_predict_state_i; + if (H2) *H2 << D_error_state_j; + if (H3) *H3 << D_error_predict* D_predict_bias_i; + + return error; +} + //------------------------------------------------------------------------------ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, @@ -254,38 +335,25 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, OptionalJacobian<9, 3> H2, OptionalJacobian<9, 6> H3, OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5) const { -// Note that derivative of constructors below is not identity for velocity, but -// a 9*3 matrix == Z_3x3, Z_3x3, state.R().transpose() + // Note that derivative of constructors below is not identity for velocity, but + // a 9*3 matrix == Z_3x3, Z_3x3, state.R().transpose() NavState state_i(pose_i, vel_i); NavState state_j(pose_j, vel_j); -/// Predict state at time j - Matrix99 D_predict_state_i; - Matrix96 D_predict_bias_i; - NavState predictedState_j = predict(state_i, bias_i, - H1 || H2 ? &D_predict_state_i : 0, H5 ? &D_predict_bias_i : 0); + // Predict state at time j + Matrix9 D_error_state_i, D_error_state_j; + Vector9 error = computeError(state_i, state_j, bias_i, + H1 || H2 ? &D_error_state_i : 0, H3 || H4 ? &D_error_state_j : 0, H5); - Matrix9 D_error_state_j, D_error_predict; - Vector9 error = state_j.localCoordinates(predictedState_j, - H3 || H4 ? &D_error_state_j : 0, H1 || H2 || H5 ? &D_error_predict : 0); - -// Separate out derivatives in terms of 5 arguments -// Note that doing so requires special treatment of velocities, as when treated as -// separate variables the retract applied will not be the semi-direct product in NavState -// Instead, the velocities in nav are updated using a straight addition -// This is difference is accounted for by the R().transpose calls below - if (H1) - *H1 << D_error_predict * D_predict_state_i.leftCols<6>(); - if (H2) - *H2 - << D_error_predict * D_predict_state_i.rightCols<3>() - * state_i.R().transpose(); - if (H3) - *H3 << D_error_state_j.leftCols<6>(); - if (H4) - *H4 << D_error_state_j.rightCols<3>() * state_j.R().transpose(); - if (H5) - *H5 << D_error_predict * D_predict_bias_i; + // Separate out derivatives in terms of 5 arguments + // Note that doing so requires special treatment of velocities, as when treated as + // separate variables the retract applied will not be the semi-direct product in NavState + // Instead, the velocities in nav are updated using a straight addition + // This is difference is accounted for by the R().transpose calls below + if (H1) *H1 << D_error_state_i.leftCols<6>(); + if (H2) *H2 << D_error_state_i.rightCols<3>() * state_i.R().transpose(); + if (H3) *H3 << D_error_state_j.leftCols<6>(); + if (H4) *H4 << D_error_state_j.rightCols<3>() * state_j.R().transpose(); return error; } @@ -307,5 +375,4 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, #endif //------------------------------------------------------------------------------ -} - /// namespace gtsam +} // namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 857157114..92f1db8aa 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -24,6 +24,9 @@ #include #include #include +#include + +#include namespace gtsam { @@ -53,15 +56,56 @@ struct PoseVelocityBias { * to access, print, and compare them. */ class PreintegrationBase { + public: + typedef imuBias::ConstantBias Bias; + typedef PreintegrationParams Params; -protected: + /// The IMU is integrated in the tangent space, represented by a Vector9 + /// This small inner class provides some convenient constructors and efficient + /// access to the orientation, position, and velocity components + class TangentVector { + Vector9 v_; + typedef const Vector9 constV9; + + public: + TangentVector() { v_.setZero(); } + TangentVector(const Vector9& v) : v_(v) {} + TangentVector(const Vector3& theta, const Vector3& pos, + const Vector3& vel) { + this->theta() = theta; + this->position() = pos; + this->velocity() = vel; + } + + const Vector9& vector() const { return v_; } + + Eigen::Block theta() { return v_.segment<3>(0); } + Eigen::Block theta() const { return v_.segment<3>(0); } + + Eigen::Block position() { return v_.segment<3>(3); } + Eigen::Block position() const { return v_.segment<3>(3); } + + Eigen::Block velocity() { return v_.segment<3>(6); } + Eigen::Block velocity() const { return v_.segment<3>(6); } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & bs::make_nvp("v_", bs::make_array(v_.data(), v_.size())); + } + }; + + protected: /// Parameters. Declared mutable only for deprecated predict method. /// TODO(frank): make const once deprecated method is removed #ifdef ALLOW_DEPRECATED_IN_GTSAM4 mutable #endif - boost::shared_ptr p_; + boost::shared_ptr p_; /// Acceleration and gyro bias used for preintegration imuBias::ConstantBias biasHat_; @@ -74,7 +118,8 @@ protected: * Note: relative position does not take into account velocity at time i, see deltap+, in [2] * Note: velocity is now also in frame i, as opposed to deltaVij in [2] */ - NavState deltaXij_; + /// Current estimate of deltaXij_k + TangentVector deltaXij_; Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias @@ -88,7 +133,6 @@ protected: } public: - /// @name Constructors /// @{ @@ -97,23 +141,20 @@ public: * @param p Parameters, typically fixed in a single application * @param bias Current estimate of acceleration and rotation rate biases */ - PreintegrationBase(const boost::shared_ptr& p, - const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : - p_(p), biasHat_(biasHat) { - resetIntegration(); - } + PreintegrationBase(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()); /** * Constructor which takes in all members for generic construction */ - PreintegrationBase(const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat, - double deltaTij, const NavState& deltaXij, const Matrix3& delPdelBiasAcc, + PreintegrationBase(const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat, + double deltaTij, const TangentVector& zeta, const Matrix3& delPdelBiasAcc, const Matrix3& delPdelBiasOmega, const Matrix3& delVdelBiasAcc, const Matrix3& delVdelBiasOmega) : p_(p), biasHat_(biasHat), deltaTij_(deltaTij), - deltaXij_(deltaXij), + deltaXij_(zeta), delPdelBiasAcc_(delPdelBiasAcc), delPdelBiasOmega_(delPdelBiasOmega), delVdelBiasAcc_(delVdelBiasAcc), @@ -125,69 +166,55 @@ public: /// Re-initialize PreintegratedMeasurements void resetIntegration(); - /// check parameters equality: checks whether shared pointer points to same PreintegrationParams object. + /// check parameters equality: checks whether shared pointer points to same Params object. bool matchesParamsWith(const PreintegrationBase& other) const { return p_ == other.p_; } /// shared pointer to params - const boost::shared_ptr& params() const { + const boost::shared_ptr& params() const { return p_; } /// const reference to params - const PreintegrationParams& p() const { - return *boost::static_pointer_cast(p_); + const Params& p() const { + return *boost::static_pointer_cast(p_); } +#ifdef ALLOW_DEPRECATED_IN_GTSAM4 void set_body_P_sensor(const Pose3& body_P_sensor) { p_->body_P_sensor = body_P_sensor; } - /// @} +#endif +/// @} /// @name Instance variables access /// @{ - const NavState& deltaXij() const { - return deltaXij_; - } - const double& deltaTij() const { - return deltaTij_; - } - const Rot3& deltaRij() const { - return deltaXij_.attitude(); - } - Vector3 deltaPij() const { - return deltaXij_.position().vector(); - } - Vector3 deltaVij() const { - return deltaXij_.velocity(); - } - const imuBias::ConstantBias& biasHat() const { - return biasHat_; - } - const Matrix3& delRdelBiasOmega() const { - return delRdelBiasOmega_; - } - const Matrix3& delPdelBiasAcc() const { - return delPdelBiasAcc_; - } - const Matrix3& delPdelBiasOmega() const { - return delPdelBiasOmega_; - } - const Matrix3& delVdelBiasAcc() const { - return delVdelBiasAcc_; - } - const Matrix3& delVdelBiasOmega() const { - return delVdelBiasOmega_; - } + const imuBias::ConstantBias& biasHat() const { return biasHat_; } + const double& deltaTij() const { return deltaTij_; } + + const Vector9& zeta() const { return deltaXij_.vector(); } + + Vector3 theta() const { return deltaXij_.theta(); } + Vector3 deltaPij() const { return deltaXij_.position(); } + Vector3 deltaVij() const { return deltaXij_.velocity(); } + + Rot3 deltaRij() const { return Rot3::Expmap(deltaXij_.theta()); } + NavState deltaXij() const { return NavState::Retract(deltaXij_.vector()); } + + const Matrix3& delRdelBiasOmega() const { return delRdelBiasOmega_; } + const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_; } + const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_; } + const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_; } + const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_; } + // Exposed for MATLAB - Vector6 biasHatVector() const { - return biasHat_.vector(); - } + Vector6 biasHatVector() const { return biasHat_.vector(); } /// @} /// @name Testable /// @{ + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim); void print(const std::string& s) const; bool equals(const PreintegrationBase& other, double tol) const; /// @} @@ -204,19 +231,28 @@ public: OptionalJacobian<3, 3> D_correctedAcc_measuredOmega = boost::none, OptionalJacobian<3, 3> D_correctedOmega_measuredOmega = boost::none) const; + // Update integrated vector on tangent manifold zeta with acceleration + // readings a_body and gyro readings w_body, bias-corrected in body frame. + static TangentVector UpdateEstimate(const Vector3& a_body, + const Vector3& w_body, double dt, + const TangentVector& zeta, + OptionalJacobian<9, 9> A = boost::none, + OptionalJacobian<9, 3> B = boost::none, + OptionalJacobian<9, 3> C = boost::none); + /// Calculate the updated preintegrated measurement, does not modify /// It takes measured quantities in the j frame - NavState updatedDeltaXij(const Vector3& j_measuredAcc, - const Vector3& j_measuredOmega, const double dt, - OptionalJacobian<9, 9> D_updated_current = boost::none, - OptionalJacobian<9, 3> D_updated_measuredAcc = boost::none, - OptionalJacobian<9, 3> D_updated_measuredOmega = boost::none) const; + PreintegrationBase::TangentVector updatedDeltaXij( + const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, double dt, + OptionalJacobian<9, 9> A = boost::none, + OptionalJacobian<9, 3> B = boost::none, + OptionalJacobian<9, 3> C = boost::none) const; /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame void update(const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, - const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current, - Matrix93* D_udpated_measuredAcc, Matrix93* D_updated_measuredOmega); + const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* A, + Matrix93* B, Matrix93* C); /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far @@ -225,8 +261,14 @@ public: /// Predict state at time j NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, - OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 = - boost::none) const; + OptionalJacobian<9, 9> H1 = boost::none, + OptionalJacobian<9, 6> H2 = boost::none) const; + + /// Calculate error given navStates + Vector9 computeError(const NavState& state_i, const NavState& state_j, + const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2, + OptionalJacobian<9, 6> H3) const; /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index 2b2b3fddf..ad684f5f8 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -17,7 +17,7 @@ #pragma once #include -#include +#include namespace gtsam { @@ -34,6 +34,7 @@ class Scenario { // Derived quantities: Rot3 rotation(double t) const { return pose(t).rotation(); } + NavState navState(double t) const { return NavState(pose(t), velocity_n(t)); } Vector3 velocity_b(double t) const { const Rot3 nRb = rotation(t); @@ -46,13 +47,18 @@ class Scenario { } }; -/// Scenario with constant twist 3D trajectory. +/** + * Scenario with constant twist 3D trajectory. + * Note that a plane flying level does not feel any acceleration: gravity is + * being perfectly compensated by the lift generated by the wings, and drag is + * compensated by thrust. The accelerometer will add the gravity component back + * in, as modeled by the specificForce method in ScenarioRunner. + */ class ConstantTwistScenario : public Scenario { public: /// Construct scenario with constant twist [w,v] ConstantTwistScenario(const Vector3& w, const Vector3& v) - : twist_((Vector6() << w, v).finished()), - a_b_(twist_.head<3>().cross(twist_.tail<3>())) {} + : twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)) {} Pose3 pose(double t) const override { return Pose3::Expmap(twist_ * t); } Vector3 omega_b(double t) const override { return twist_.head<3>(); } diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index e485e37a3..79f3f42cc 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -21,14 +21,16 @@ #include #include +using namespace std; +using namespace boost::assign; + namespace gtsam { static double intNoiseVar = 0.0000001; static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; PreintegratedImuMeasurements ScenarioRunner::integrate( - double T, const imuBias::ConstantBias& estimatedBias, - bool corrupted) const { + double T, const Bias& estimatedBias, bool corrupted) const { PreintegratedImuMeasurements pim(p_, estimatedBias); const double dt = imuSampleTime(); @@ -45,37 +47,57 @@ PreintegratedImuMeasurements ScenarioRunner::integrate( return pim; } -NavState ScenarioRunner::predict( - const PreintegratedImuMeasurements& pim, - const imuBias::ConstantBias& estimatedBias) const { +NavState ScenarioRunner::predict(const PreintegratedImuMeasurements& pim, + const Bias& estimatedBias) const { const NavState state_i(scenario_->pose(0), scenario_->velocity_n(0)); return pim.predict(state_i, estimatedBias); } -Matrix6 ScenarioRunner::estimatePoseCovariance( - double T, size_t N, const imuBias::ConstantBias& estimatedBias) const { - gttic_(estimatePoseCovariance); +Matrix9 ScenarioRunner::estimateCovariance(double T, size_t N, + const Bias& estimatedBias) const { + gttic_(estimateCovariance); // Get predict prediction from ground truth measurements - Pose3 prediction = predict(integrate(T)).pose(); + NavState prediction = predict(integrate(T)); // Sample ! Matrix samples(9, N); - Vector6 sum = Vector6::Zero(); + Vector9 sum = Vector9::Zero(); for (size_t i = 0; i < N; i++) { - Pose3 sampled = predict(integrate(T, estimatedBias, true)).pose(); - Vector6 xi = sampled.localCoordinates(prediction); + auto pim = integrate(T, estimatedBias, true); + NavState sampled = predict(pim); + Vector9 xi = sampled.localCoordinates(prediction); samples.col(i) = xi; sum += xi; } + // Compute MC covariance + Vector9 sampleMean = sum / N; + Matrix9 Q; + Q.setZero(); + for (size_t i = 0; i < N; i++) { + Vector9 xi = samples.col(i) - sampleMean; + Q += xi * xi.transpose(); + } + + return Q / (N - 1); +} + +Matrix6 ScenarioRunner::estimateNoiseCovariance(size_t N) const { + Matrix samples(6, N); + Vector6 sum = Vector6::Zero(); + for (size_t i = 0; i < N; i++) { + samples.col(i) << accSampler_.sample() / sqrt_dt_, + gyroSampler_.sample() / sqrt_dt_; + sum += samples.col(i); + } + // Compute MC covariance Vector6 sampleMean = sum / N; Matrix6 Q; Q.setZero(); for (size_t i = 0; i < N; i++) { - Vector6 xi = samples.col(i); - xi -= sampleMean; + Vector6 xi = samples.col(i) - sampleMean; Q += xi * xi.transpose(); } diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 7cb98379a..b038a831b 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -22,20 +22,42 @@ namespace gtsam { +// Convert covariance to diagonal noise model, if possible, otherwise throw +static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) { + bool smart = true; + auto model = noiseModel::Gaussian::Covariance(covariance, smart); + auto diagonal = boost::dynamic_pointer_cast(model); + if (!diagonal) + throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal"); + return diagonal; +} + /* * Simple class to test navigation scenarios. * Takes a trajectory scenario as input, and can generate IMU measurements */ class ScenarioRunner { public: - typedef boost::shared_ptr SharedParams; + typedef imuBias::ConstantBias Bias; + typedef boost::shared_ptr SharedParams; + + private: + const Scenario* scenario_; + const SharedParams p_; + const double imuSampleTime_, sqrt_dt_; + const Bias estimatedBias_; + + // Create two samplers for acceleration and omega noise + mutable Sampler gyroSampler_, accSampler_; + + public: ScenarioRunner(const Scenario* scenario, const SharedParams& p, - double imuSampleTime = 1.0 / 100.0, - const imuBias::ConstantBias& bias = imuBias::ConstantBias()) + double imuSampleTime = 1.0 / 100.0, const Bias& bias = Bias()) : scenario_(scenario), p_(p), imuSampleTime_(imuSampleTime), - bias_(bias), + sqrt_dt_(std::sqrt(imuSampleTime)), + estimatedBias_(bias), // NOTE(duy): random seeds that work well: gyroSampler_(Diagonal(p->gyroscopeCovariance), 10), accSampler_(Diagonal(p->accelerometerCovariance), 29284) {} @@ -57,59 +79,31 @@ class ScenarioRunner { // versions corrupted by bias and noise Vector3 measuredAngularVelocity(double t) const { - return actualAngularVelocity(t) + bias_.gyroscope() + - gyroSampler_.sample() / std::sqrt(imuSampleTime_); + return actualAngularVelocity(t) + estimatedBias_.gyroscope() + + gyroSampler_.sample() / sqrt_dt_; } Vector3 measuredSpecificForce(double t) const { - return actualSpecificForce(t) + bias_.accelerometer() + - accSampler_.sample() / std::sqrt(imuSampleTime_); + return actualSpecificForce(t) + estimatedBias_.accelerometer() + + accSampler_.sample() / sqrt_dt_; } const double& imuSampleTime() const { return imuSampleTime_; } /// Integrate measurements for T seconds into a PIM - PreintegratedImuMeasurements integrate( - double T, - const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias(), - bool corrupted = false) const; + PreintegratedImuMeasurements integrate(double T, + const Bias& estimatedBias = Bias(), + bool corrupted = false) const; /// Predict predict given a PIM NavState predict(const PreintegratedImuMeasurements& pim, - const imuBias::ConstantBias& estimatedBias = - imuBias::ConstantBias()) const; + const Bias& estimatedBias = Bias()) const; - /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately - Matrix6 poseCovariance(const PreintegratedImuMeasurements& pim) const { - Matrix9 cov = pim.preintMeasCov(); - Matrix6 poseCov; - poseCov << cov.block<3, 3>(0, 0), cov.block<3, 3>(0, 3), // - cov.block<3, 3>(3, 0), cov.block<3, 3>(3, 3); - return poseCov; - } + /// Compute a Monte Carlo estimate of the predict covariance using N samples + Matrix9 estimateCovariance(double T, size_t N = 1000, + const Bias& estimatedBias = Bias()) const; - /// Compute a Monte Carlo estimate of the PIM pose covariance using N samples - Matrix6 estimatePoseCovariance(double T, size_t N = 1000, - const imuBias::ConstantBias& estimatedBias = - imuBias::ConstantBias()) const; - - private: - // Convert covariance to diagonal noise model, if possible, otherwise throw - static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) { - bool smart = true; - auto model = noiseModel::Gaussian::Covariance(covariance, smart); - auto diagonal = boost::dynamic_pointer_cast(model); - if (!diagonal) - throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal"); - return diagonal; - } - - const Scenario* scenario_; - const SharedParams p_; - const double imuSampleTime_; - const imuBias::ConstantBias bias_; - - // Create two samplers for acceleration and omega noise - mutable Sampler gyroSampler_, accSampler_; + /// Estimate covariance of sampled noise for sanity-check + Matrix6 estimateNoiseCovariance(size_t N = 1000) const; }; } // namespace gtsam diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 3d2a20915..e79bf174e 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -221,9 +221,9 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) { // Compare Jacobians EXPECT(assert_equal(expectedDelPdelBiasAcc, pim.delPdelBiasAcc())); - EXPECT(assert_equal(expectedDelPdelBiasOmega, pim.delPdelBiasOmega())); + EXPECT(assert_equal(expectedDelPdelBiasOmega, pim.delPdelBiasOmega(),1e-8)); EXPECT(assert_equal(expectedDelVdelBiasAcc, pim.delVdelBiasAcc())); - EXPECT(assert_equal(expectedDelVdelBiasOmega, pim.delVdelBiasOmega())); + EXPECT(assert_equal(expectedDelVdelBiasOmega, pim.delVdelBiasOmega(),1e-8)); EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); EXPECT(assert_equal(expectedDelRdelBiasOmega, pim.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 99f5ec056..e40c9adea 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -71,7 +71,7 @@ static boost::shared_ptr defaultParams() { return p; } -// Auxiliary functions to test preintegrated Jacobians +// Auxiliary functions to test pre-integrated Jacobians // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ /* ************************************************************************* */ PreintegratedImuMeasurements evaluatePreintegratedMeasurements( @@ -140,24 +140,8 @@ TEST(ImuFactor, Accelerating) { PreintegratedImuMeasurements pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - Matrix6 estimatedCov = runner.estimatePoseCovariance(T); - EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); - - // Check G1 and G2 derivatives of pim.update - Matrix93 aG1, aG2; - boost::function f = boost::bind( - &PreintegrationBase::updatedDeltaXij, pim, _1, _2, T / 10, boost::none, - boost::none, boost::none); - const Vector3 measuredAcc = runner.actualSpecificForce(T); - const Vector3 measuredOmega = runner.actualAngularVelocity(T); - pim.updatedDeltaXij(measuredAcc, measuredOmega, T / 10, boost::none, aG1, - aG2); - EXPECT( - assert_equal(numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), - aG1, 1e-7)); - EXPECT( - assert_equal(numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), - aG2, 1e-7)); + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ @@ -167,14 +151,14 @@ TEST(ImuFactor, PreintegratedMeasurements) { Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); double deltaT = 0.5; - // Expected preintegrated values + // Expected pre-integrated values Vector3 expectedDeltaP1; expectedDeltaP1 << 0.5 * 0.1 * 0.5 * 0.5, 0, 0; Vector3 expectedDeltaV1(0.05, 0.0, 0.0); Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0); double expectedDeltaT1(0.5); - // Actual preintegrated values + // Actual pre-integrated values PreintegratedImuMeasurements actual1(defaultParams()); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); @@ -183,6 +167,24 @@ TEST(ImuFactor, PreintegratedMeasurements) { EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()))); DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-9); + // Check derivatives of computeError + imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) + NavState x1, x2 = actual1.predict(x1, bias); + + { + Matrix9 aH1, aH2; + Matrix96 aH3; + actual1.computeError(x1, x2, bias, aH1, aH2, aH3); + boost::function f = + boost::bind(&PreintegrationBase::computeError, actual1, _1, _2, _3, + boost::none, boost::none, boost::none); + // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 + EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); + EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); + EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9)); + } + // Integrate again Vector3 expectedDeltaP2; expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5 * 0.1 * 0.5 * 0.5, 0, 0; @@ -191,7 +193,7 @@ TEST(ImuFactor, PreintegratedMeasurements) { Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0); double expectedDeltaT2(1); - // Actual preintegrated values + // Actual pre-integrated values PreintegratedImuMeasurements actual2 = actual1; actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT); @@ -200,7 +202,6 @@ TEST(ImuFactor, PreintegratedMeasurements) { EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()))); DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-9); } - /* ************************************************************************* */ // Common linearization point and measurements for tests namespace common { @@ -498,7 +499,7 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { deltaTs.push_back(0.01); } - // Actual preintegrated values + // Actual pre-integrated values PreintegratedImuMeasurements preintegrated = evaluatePreintegratedMeasurements(kZeroBias, measuredAccs, measuredOmegas, deltaTs); @@ -528,13 +529,13 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { // Compare Jacobians EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc())); EXPECT( - assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); + assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega(),1e-8)); EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc())); EXPECT( - assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); + assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega(),1e-8)); EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); EXPECT( - assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega())); + assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(),1e-7)); } /* ************************************************************************* */ @@ -581,34 +582,33 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { PreintegratedImuMeasurements pim(p, biasHat); // Check updatedDeltaXij derivatives - Matrix3 D_correctedAcc_measuredOmega = Matrix3::Zero(); + Matrix3 D_correctedAcc_measuredOmega = Z_3x3; pim.correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, boost::none, D_correctedAcc_measuredOmega, boost::none); Matrix3 expectedD = numericalDerivative11( boost::bind(correctedAcc, pim, measuredAcc, _1), measuredOmega, 1e-6); EXPECT(assert_equal(expectedD, D_correctedAcc_measuredOmega, 1e-5)); - Matrix93 G1, G2; double dt = 0.1; - NavState preint = pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, - boost::none, G1, G2); -// Matrix9 preintCov = G1*((accNoiseVar2/dt).asDiagonal())*G1.transpose() + G2*((omegaNoiseVar2/dt).asDiagonal())*G2.transpose(); - Matrix93 expectedG1 = numericalDerivative21( - boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2, - dt, boost::none, boost::none, boost::none), measuredAcc, - measuredOmega, 1e-6); - EXPECT(assert_equal(expectedG1, G1, 1e-5)); - - Matrix93 expectedG2 = numericalDerivative22( - boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2, - dt, boost::none, boost::none, boost::none), measuredAcc, - measuredOmega, 1e-6); - EXPECT(assert_equal(expectedG2, G2, 1e-5)); +// TODO(frank): revive derivative tests +// Matrix93 G1, G2; +// PreintegrationBase::TangentVector preint = +// pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2); +// +// Matrix93 expectedG1 = numericalDerivative21( +// boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2, +// dt, boost::none, boost::none, boost::none), measuredAcc, +// measuredOmega, 1e-6); +// EXPECT(assert_equal(expectedG1, G1, 1e-5)); +// +// Matrix93 expectedG2 = numericalDerivative22( +// boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2, +// dt, boost::none, boost::none, boost::none), measuredAcc, +// measuredOmega, 1e-6); +// EXPECT(assert_equal(expectedG2, G2, 1e-5)); imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) -// EXPECT(MonteCarlo(pim, NavState(x1, initial_velocity), bias, dt, body_P_sensor, -// measuredAcc, measuredOmega, accNoiseVar2, omegaNoiseVar2, 100000)); // integrate at least twice to get position information // otherwise factor cov noise from preint_cov is not positive definite @@ -629,8 +629,6 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { values.insert(V(2), v2); values.insert(B(1), bias); -// factor.get_noiseModel()->print("noise: "); // Make sure the noise is valid - // Make sure linearization is correct double diffDelta = 1e-8; EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); diff --git a/gtsam/navigation/tests/testPreintegrationBase.cpp b/gtsam/navigation/tests/testPreintegrationBase.cpp new file mode 100644 index 000000000..5363f6b9f --- /dev/null +++ b/gtsam/navigation/tests/testPreintegrationBase.cpp @@ -0,0 +1,99 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testPreintegrationBase.cpp + * @brief Unit test for the InertialNavFactor + * @author Frank Dellaert + */ + +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +static const double kDt = 0.1; + +static const double kGyroSigma = 0.02; +static const double kAccelSigma = 0.1; + +// Create default parameters with Z-down and above noise parameters +static boost::shared_ptr defaultParams() { + auto p = PreintegrationParams::MakeSharedD(10); + p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; + p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; + p->integrationCovariance = 0.0000001 * I_3x3; + return p; +} + +Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) { + PreintegrationBase::TangentVector zeta_plus = + PreintegrationBase::UpdateEstimate(a, w, kDt, zeta); + return zeta_plus.vector(); +} + +/* ************************************************************************* */ +TEST(PreintegrationBase, UpdateEstimate1) { + PreintegrationBase pim(defaultParams()); + const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); + Vector9 zeta; + zeta.setZero(); + Matrix9 aH1; + Matrix93 aH2, aH3; + pim.UpdateEstimate(acc, omega, kDt, zeta, aH1, aH2, aH3); + EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-9)); + EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-9)); + EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9)); +} + +/* ************************************************************************* */ +TEST(PreintegrationBase, UpdateEstimate2) { + PreintegrationBase pim(defaultParams()); + const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); + Vector9 zeta; + zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3; + Matrix9 aH1; + Matrix93 aH2, aH3; + pim.UpdateEstimate(acc, omega, kDt, zeta, aH1, aH2, aH3); + // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 + EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3)); + EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-7)); + EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9)); +} + +/* ************************************************************************* */ +TEST(PreintegrationBase, computeError) { + PreintegrationBase pim(defaultParams()); + NavState x1, x2; + imuBias::ConstantBias bias; + Matrix9 aH1, aH2; + Matrix96 aH3; + pim.computeError(x1, x2, bias, aH1, aH2, aH3); + boost::function f = + boost::bind(&PreintegrationBase::computeError, pim, _1, _2, _3, + boost::none, boost::none, boost::none); + // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 + EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); + EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); + EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index afb62935e..fb8aa9534 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -25,7 +25,24 @@ using namespace std; using namespace gtsam; -static const double degree = M_PI / 180.0; +static const double kDegree = M_PI / 180.0; + +/* ************************************************************************* */ +TEST(Scenario, Spin) { + // angular velocity 6 kDegree/sec + const double w = 6 * kDegree; + const Vector3 W(0, 0, w), V(0, 0, 0); + const ConstantTwistScenario scenario(W, V); + + const double T = 10; + EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9)); + EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9)); + EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9)); + + const Pose3 T10 = scenario.pose(T); + EXPECT(assert_equal(Vector3(0, 0, 60 * kDegree), T10.rotation().xyz(), 1e-9)); + EXPECT(assert_equal(Point3(0, 0, 0), T10.translation(), 1e-9)); +} /* ************************************************************************* */ TEST(Scenario, Forward) { @@ -45,8 +62,8 @@ TEST(Scenario, Forward) { /* ************************************************************************* */ TEST(Scenario, Circle) { - // Forward velocity 2m/s, angular velocity 6 degree/sec around Z - const double v = 2, w = 6 * degree; + // Forward velocity 2m/s, angular velocity 6 kDegree/sec around Z + const double v = 2, w = 6 * kDegree; const Vector3 W(0, 0, w), V(v, 0, 0); const ConstantTwistScenario scenario(W, V); @@ -58,15 +75,15 @@ TEST(Scenario, Circle) { // R = v/w, so test if circle is of right size const double R = v / w; const Pose3 T15 = scenario.pose(T); - EXPECT(assert_equal(Vector3(0, 0, 90 * degree), T15.rotation().xyz(), 1e-9)); + EXPECT(assert_equal(Vector3(0, 0, 90 * kDegree), T15.rotation().xyz(), 1e-9)); EXPECT(assert_equal(Point3(R, R, 0), T15.translation(), 1e-9)); } /* ************************************************************************* */ TEST(Scenario, Loop) { // Forward velocity 2m/s - // Pitch up with angular velocity 6 degree/sec (negative in FLU) - const double v = 2, w = 6 * degree; + // Pitch up with angular velocity 6 kDegree/sec (negative in FLU) + const double v = 2, w = 6 * kDegree; const Vector3 W(0, -w, 0), V(v, 0, 0); const ConstantTwistScenario scenario(W, V); @@ -100,10 +117,10 @@ TEST(Scenario, Accelerating) { EXPECT(assert_equal(A, scenario.acceleration_n(T), 1e-9)); { - // Check acceleration in nav - Matrix expected = numericalDerivative11( - boost::bind(&Scenario::velocity_n, scenario, _1), T); - EXPECT(assert_equal(Vector3(expected), scenario.acceleration_n(T), 1e-9)); + // Check acceleration in nav + Matrix expected = numericalDerivative11( + boost::bind(&Scenario::velocity_n, scenario, _1), T); + EXPECT(assert_equal(Vector3(expected), scenario.acceleration_n(T), 1e-9)); } const Pose3 T3 = scenario.pose(3); diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp deleted file mode 100644 index 7fd5b6637..000000000 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testScenarioRunner.cpp - * @brief test ImuFacor with ScenarioRunner class - * @author Frank Dellaert - */ - -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -static const double kDegree = M_PI / 180.0; -static const double kDeltaT = 1e-2; -static const double kGyroSigma = 0.02; -static const double kAccelSigma = 0.1; - -static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3); -static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias); - -// Create default parameters with Z-down and above noise parameters -static boost::shared_ptr defaultParams() { - auto p = PreintegrationParams::MakeSharedD(10); - p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; - p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; - p->integrationCovariance = 0.0000001 * I_3x3; - return p; -} - -/* ************************************************************************* */ -namespace forward { -const double v = 2; // m/s -ConstantTwistScenario scenario(Vector3::Zero(), Vector3(v, 0, 0)); -} -/* ************************************************************************* */ -TEST(ScenarioRunner, Forward) { - using namespace forward; - ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); - const double T = 0.1; // seconds - - PreintegratedImuMeasurements pim = runner.integrate(T); - EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - - Matrix6 estimatedCov = runner.estimatePoseCovariance(T); - EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); -} - -/* ************************************************************************* */ -TEST(ScenarioRunner, ForwardWithBias) { - using namespace forward; - ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); - const double T = 0.1; // seconds - - PreintegratedImuMeasurements pim = runner.integrate(T, kNonZeroBias); - Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, kNonZeroBias); - EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); -} - -/* ************************************************************************* */ -TEST(ScenarioRunner, Circle) { - // Forward velocity 2m/s, angular velocity 6 kDegree/sec - const double v = 2, w = 6 * kDegree; - ConstantTwistScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0)); - - ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); - const double T = 0.1; // seconds - - PreintegratedImuMeasurements pim = runner.integrate(T); - EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1)); - - Matrix6 estimatedCov = runner.estimatePoseCovariance(T); - EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); -} - -/* ************************************************************************* */ -TEST(ScenarioRunner, Loop) { - // Forward velocity 2m/s - // Pitch up with angular velocity 6 kDegree/sec (negative in FLU) - const double v = 2, w = 6 * kDegree; - ConstantTwistScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0)); - - ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); - const double T = 0.1; // seconds - - PreintegratedImuMeasurements pim = runner.integrate(T); - EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1)); - - Matrix6 estimatedCov = runner.estimatePoseCovariance(T); - EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); -} - -/* ************************************************************************* */ -namespace initial { -// Set up body pointing towards y axis, and start at 10,20,0 with velocity -// going in X. The body itself has Z axis pointing down -const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); -const Point3 P0(10, 20, 0); -const Vector3 V0(50, 0, 0); -} - -/* ************************************************************************* */ -namespace accelerating { -using namespace initial; -const double a = 0.2; // m/s^2 -const Vector3 A(0, a, 0); -const AcceleratingScenario scenario(nRb, P0, V0, A); - -const double T = 3; // seconds -} - -/* ************************************************************************* */ -TEST(ScenarioRunner, Accelerating) { - using namespace accelerating; - ScenarioRunner runner(&scenario, defaultParams(), T / 10); - - PreintegratedImuMeasurements pim = runner.integrate(T); - EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - - Matrix6 estimatedCov = runner.estimatePoseCovariance(T); - EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); -} - -/* ************************************************************************* */ -// TODO(frank):Fails ! -// TEST(ScenarioRunner, AcceleratingWithBias) { -// using namespace accelerating; -// ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma, -// kNonZeroBias); -// -// PreintegratedImuMeasurements pim = runner.integrate(T, -// kNonZeroBias); -// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000, -// kNonZeroBias); -// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); -//} - -/* ************************************************************************* */ -TEST(ScenarioRunner, AcceleratingAndRotating) { - using namespace initial; - const double a = 0.2; // m/s^2 - const Vector3 A(0, a, 0), W(0, 0.1, 0); - const AcceleratingScenario scenario(nRb, P0, V0, A, W); - - const double T = 3; // seconds - ScenarioRunner runner(&scenario, defaultParams(), T / 10); - - PreintegratedImuMeasurements pim = runner.integrate(T); - EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - - Matrix6 estimatedCov = runner.estimatePoseCovariance(T); - EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - auto result = TestRegistry::runAllTests(tr); - tictoc_print_(); - return result; -} -/* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testScenarios.cpp b/gtsam/navigation/tests/testScenarios.cpp new file mode 100644 index 000000000..c2fdb963d --- /dev/null +++ b/gtsam/navigation/tests/testScenarios.cpp @@ -0,0 +1,385 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testScenarioRunner.cpp + * @brief test ImuFacor with ScenarioRunner class + * @author Frank Dellaert + */ + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +static const double kDegree = M_PI / 180.0; +static const double kDt = 1e-2; + +// realistic white noise strengths are 0.5 deg/sqrt(hr) and 0.1 (m/s)/sqrt(h) +static const double kGyroSigma = 0.5 * kDegree / 60; +static const double kAccelSigma = 0.1 / 60.0; + +static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3); +static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias); + +// Create default parameters with Z-up and above noise parameters +static boost::shared_ptr defaultParams() { + auto p = PreintegrationParams::MakeSharedU(10); + p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; + p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; + p->integrationCovariance = 0.0000001 * I_3x3; + return p; +} + +#define EXPECT_NEAR(a, b, c) EXPECT(assert_equal(Vector(a), Vector(b), c)); + +/* ************************************************************************* */ +TEST(ScenarioRunner, Spin) { + // angular velocity 6 degree/sec + const double w = 6 * kDegree; + const Vector3 W(0, 0, w), V(0, 0, 0); + const ConstantTwistScenario scenario(W, V); + + auto p = defaultParams(); + ScenarioRunner runner(&scenario, p, kDt); + const double T = 2 * kDt; // seconds + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); + +#if 0 + // Check sampled noise is kosher + Matrix6 expected; + expected << p->accelerometerCovariance / kDt, Z_3x3, // + Z_3x3, p->gyroscopeCovariance / kDt; + Matrix6 actual = runner.estimateNoiseCovariance(10000); + EXPECT(assert_equal(expected, actual, 1e-2)); +#endif + + // Check calculated covariance against Monte Carlo estimate + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); +} + +/* ************************************************************************* */ +namespace forward { +const double v = 2; // m/s +ConstantTwistScenario scenario(Vector3::Zero(), Vector3(v, 0, 0)); +} +/* ************************************************************************* */ +TEST(ScenarioRunner, Forward) { + using namespace forward; + ScenarioRunner runner(&scenario, defaultParams(), kDt); + const double T = 0.1; // seconds + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, ForwardWithBias) { + using namespace forward; + ScenarioRunner runner(&scenario, defaultParams(), kDt); + const double T = 0.1; // seconds + + auto pim = runner.integrate(T, kNonZeroBias); + Matrix9 estimatedCov = runner.estimateCovariance(T, 1000, kNonZeroBias); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, Circle) { + // Forward velocity 2m/s, angular velocity 6 degree/sec + const double v = 2, w = 6 * kDegree; + ConstantTwistScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0)); + + ScenarioRunner runner(&scenario, defaultParams(), kDt); + const double T = 0.1; // seconds + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, Loop) { + // Forward velocity 2m/s + // Pitch up with angular velocity 6 degree/sec (negative in FLU) + const double v = 2, w = 6 * kDegree; + ConstantTwistScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0)); + + ScenarioRunner runner(&scenario, defaultParams(), kDt); + const double T = 0.1; // seconds + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); +} + +/* ************************************************************************* */ +namespace initial { +const Rot3 nRb; +const Point3 P0; +const Vector3 V0(0, 0, 0); +} + +/* ************************************************************************* */ +namespace accelerating { +using namespace initial; +const double a = 0.2; // m/s^2 +const Vector3 A(0, a, 0); +const AcceleratingScenario scenario(nRb, P0, V0, A); + +const double T = 3; // seconds +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, Accelerating) { + using namespace accelerating; + ScenarioRunner runner(&scenario, defaultParams(), T / 10); + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingWithBias) { + using namespace accelerating; + ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); + + auto pim = runner.integrate(T, kNonZeroBias); + Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingAndRotating) { + using namespace initial; + const double a = 0.2; // m/s^2 + const Vector3 A(0, a, 0), W(0, 0.1, 0); + const AcceleratingScenario scenario(nRb, P0, V0, A, W); + + const double T = 10; // seconds + ScenarioRunner runner(&scenario, defaultParams(), T / 10); + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +namespace initial2 { +// No rotation, but non-zero position and velocities +const Rot3 nRb; +const Point3 P0(10, 20, 0); +const Vector3 V0(50, 0, 0); +} + +/* ************************************************************************* */ +namespace accelerating2 { +using namespace initial2; +const double a = 0.2; // m/s^2 +const Vector3 A(0, a, 0); +const AcceleratingScenario scenario(nRb, P0, V0, A); + +const double T = 3; // seconds +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, Accelerating2) { + using namespace accelerating2; + ScenarioRunner runner(&scenario, defaultParams(), T / 10); + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingWithBias2) { + using namespace accelerating2; + ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); + + auto pim = runner.integrate(T, kNonZeroBias); + Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingAndRotating2) { + using namespace initial2; + const double a = 0.2; // m/s^2 + const Vector3 A(0, a, 0), W(0, 0.1, 0); + const AcceleratingScenario scenario(nRb, P0, V0, A, W); + + const double T = 10; // seconds + ScenarioRunner runner(&scenario, defaultParams(), T / 10); + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +namespace initial3 { +// Rotation only +// Set up body pointing towards y axis. The body itself has Z axis pointing down +const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); +const Point3 P0; +const Vector3 V0(0, 0, 0); +} + +/* ************************************************************************* */ +namespace accelerating3 { +using namespace initial3; +const double a = 0.2; // m/s^2 +const Vector3 A(0, a, 0); +const AcceleratingScenario scenario(nRb, P0, V0, A); + +const double T = 3; // seconds +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, Accelerating3) { + using namespace accelerating3; + ScenarioRunner runner(&scenario, defaultParams(), T / 10); + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingWithBias3) { + using namespace accelerating3; + ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); + + auto pim = runner.integrate(T, kNonZeroBias); + Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingAndRotating3) { + using namespace initial3; + const double a = 0.2; // m/s^2 + const Vector3 A(0, a, 0), W(0, 0.1, 0); + const AcceleratingScenario scenario(nRb, P0, V0, A, W); + + const double T = 10; // seconds + ScenarioRunner runner(&scenario, defaultParams(), T / 10); + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +namespace initial4 { +// Both rotation and translation +// Set up body pointing towards y axis, and start at 10,20,0 with velocity +// going in X. The body itself has Z axis pointing down +const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); +const Point3 P0(10, 20, 0); +const Vector3 V0(50, 0, 0); +} + +/* ************************************************************************* */ +namespace accelerating4 { +using namespace initial4; +const double a = 0.2; // m/s^2 +const Vector3 A(0, a, 0); +const AcceleratingScenario scenario(nRb, P0, V0, A); + +const double T = 3; // seconds +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, Accelerating4) { + using namespace accelerating4; + ScenarioRunner runner(&scenario, defaultParams(), T / 10); + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingWithBias4) { + using namespace accelerating4; + ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); + + auto pim = runner.integrate(T, kNonZeroBias); + Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingAndRotating4) { + using namespace initial4; + const double a = 0.2; // m/s^2 + const Vector3 A(0, a, 0), W(0, 0.1, 0); + const AcceleratingScenario scenario(nRb, P0, V0, A, W); + + const double T = 10; // seconds + ScenarioRunner runner(&scenario, defaultParams(), T / 10); + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + auto result = TestRegistry::runAllTests(tr); + tictoc_print_(); + return result; +} +/* ************************************************************************* */ diff --git a/python/.gitignore b/python/.gitignore new file mode 100644 index 000000000..d16386367 --- /dev/null +++ b/python/.gitignore @@ -0,0 +1 @@ +build/ \ No newline at end of file diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt new file mode 100644 index 000000000..01141973a --- /dev/null +++ b/python/CMakeLists.txt @@ -0,0 +1,96 @@ +# Guard to avoid breaking this code in ccmake if by accident GTSAM_PYTHON_VERSION is set to an empty string +if(GTSAM_PYTHON_VERSION STREQUAL "") + set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "Target python version for GTSAM python module. Use 'Default' to chose the default version" FORCE) +endif() + +# The code below allows to clear the PythonLibs cache if we change GTSAM_PYTHON_VERSION +# Inspired from the solution found here: http://blog.bethcodes.com/cmake-tips-tricks-drop-down-list +if(NOT DEFINED GTSAM_LAST_PYTHON_VERSION) + set(GTSAM_LAST_PYTHON_VERSION ${GTSAM_PYTHON_VERSION} CACHE STRING "Python version used in the last build") + mark_as_advanced(FORCE GTSAM_LAST_PYTHON_VERSION) +endif() +if(NOT (${GTSAM_PYTHON_VERSION} MATCHES ${GTSAM_LAST_PYTHON_VERSION})) + unset(PYTHON_INCLUDE_DIR CACHE) + unset(PYTHON_INCLUDE_DIR2 CACHE) + unset(PYTHON_LIBRARY CACHE) + unset(PYTHON_LIBRARY_DEBUG CACHE) + set(GTSAM_LAST_PYTHON_VERSION ${GTSAM_PYTHON_VERSION} CACHE STRING "Updating python version used in the last build" FORCE) +endif() + +if(GTSAM_PYTHON_VERSION STREQUAL "Default") + # Search the default version. + find_package(PythonInterp) + find_package(PythonLibs) +else() + find_package(PythonInterp ${GTSAM_PYTHON_VERSION}) + find_package(PythonLibs ${GTSAM_PYTHON_VERSION}) +endif() + +# Find NumPy C-API -- this is part of the numpy package +find_package(NumPy) + +# Compose strings used to specify the boost python version. They will be empty if we want to use the defaut +if(NOT GTSAM_PYTHON_VERSION STREQUAL "Default") + string(REPLACE "." "" BOOST_PYTHON_VERSION_SUFFIX ${GTSAM_PYTHON_VERSION}) # Remove '.' from version + string(SUBSTRING ${BOOST_PYTHON_VERSION_SUFFIX} 0 2 BOOST_PYTHON_VERSION_SUFFIX) # Trim version number to 2 digits + set(BOOST_PYTHON_VERSION_SUFFIX "-py${BOOST_PYTHON_VERSION_SUFFIX}") # Append '-py' prefix + string(TOUPPER ${BOOST_PYTHON_VERSION_SUFFIX} BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE) # Get uppercase string +else() + set(BOOST_PYTHON_VERSION_SUFFIX "") + set(BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE "") +endif() + +# Find Boost Python +find_package(Boost COMPONENTS python${BOOST_PYTHON_VERSION_SUFFIX}) + +if(Boost_PYTHON${BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE}_FOUND AND PYTHONLIBS_FOUND AND NUMPY_FOUND) + # Build library + include_directories(${NUMPY_INCLUDE_DIRS}) + include_directories(${PYTHON_INCLUDE_DIRS}) + include_directories(${Boost_INCLUDE_DIRS}) + include_directories(${CMAKE_SOURCE_DIR}/gtsam/3rdparty/numpy_eigen/include/) + + # Build the python module library + add_subdirectory(handwritten) + + # Create and invoke setup.py, see https://bloerg.net/2012/11/10/cmake-and-distutils.html + set(SETUP_PY_IN "${CMAKE_CURRENT_SOURCE_DIR}/setup.py.in") + set(SETUP_PY "${CMAKE_CURRENT_BINARY_DIR}/setup.py") + + # Hacky way to figure out install folder - valid for Linux & Mac + # default pattern: prefix/lib/pythonX.Y/site-packages from https://docs.python.org/2/install/ + SET(PY_INSTALL_FOLDER "${CMAKE_INSTALL_PREFIX}/lib/python${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}/site-packages") + + configure_file(${SETUP_PY_IN} ${SETUP_PY}) + + # TODO(frank): possibly support a different prefix a la matlab wrapper + install(CODE "execute_process(WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} COMMAND ${PYTHON_EXECUTABLE} setup.py -v install --prefix ${CMAKE_INSTALL_PREFIX})") +else() + # Disable python module if we didn't find required libraries + # message will print at end of main CMakeLists.txt + SET(GTSAM_PYTHON_WARNINGS "Python dependencies not found - Python module will not be built. Set GTSAM_BUILD_PYTHON to 'Off' to disable this warning. Details:") + + if(NOT PYTHONLIBS_FOUND) + if(GTSAM_PYTHON_VERSION STREQUAL "Default") + SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- Default PythonLibs not found") + else() + SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- PythonLibs version ${GTSAM_PYTHON_VERSION} not found") + endif() + endif() + + if(NOT NUMPY_FOUND) + SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- Numpy not found") + endif() + + if(NOT Boost_PYTHON${BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE}_FOUND) + if(GTSAM_PYTHON_VERSION STREQUAL "Default") + SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- Default Boost python not found") + else() + SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- Boost Python for python ${GTSAM_PYTHON_VERSION} not found") + endif() + endif() + + # make available at top-level + SET(GTSAM_PYTHON_WARNINGS ${GTSAM_PYTHON_WARNINGS} PARENT_SCOPE) + +endif() diff --git a/python/README.md b/python/README.md new file mode 100644 index 000000000..f1c218cfb --- /dev/null +++ b/python/README.md @@ -0,0 +1,18 @@ +Python Wrapper and Packaging +============================ + +This directory contains the basic setup script and directory structure for the gtsam python module. +During the build of gtsam, when GTSAM_BUILD_PYTHON is enabled, the following instructions will run. + +* The handwritten module source files are compiled and linked with Boost Python, generating a shared + library which can then be imported by python +* A setup.py script is configured from setup.py.in +* The gtsam packages 'gtsam', 'gtsam_utils', 'gtsam_examples', and 'gtsam_tests' are installed into + the site-packages folder within the (possibly non-default) installation prefix folder. If + installing to a non-standard prefix, make sure that _prefix_/lib/pythonX.Y/site-packages is + present in your $PYTHONPATH + +The target version of Python to create the module can be set by defining GTSAM_PYTHON_VERSION to 'X.Y' (Example: 2.7 or 3.4), or 'Default' if you want to use the default python installed in your system. Note that if you specify a target version of python, you should also have the correspondening Boost +Python version installed (Example: libboost_python-py27.so or libboost_python-py34.so on Linux). +If you're using the default version, your default Boost Python library (Example: libboost_python.so on Linux) should correspond to the default python version in your system. + diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py new file mode 100644 index 000000000..f9e40517d --- /dev/null +++ b/python/gtsam/__init__.py @@ -0,0 +1 @@ +from ._libgtsam_python import * diff --git a/python/gtsam_examples/ImuFactorExample.py b/python/gtsam_examples/ImuFactorExample.py new file mode 100644 index 000000000..ca5e524ee --- /dev/null +++ b/python/gtsam_examples/ImuFactorExample.py @@ -0,0 +1,105 @@ +""" +A script validating the ImuFactor inference. +""" + +from __future__ import print_function +import math +import matplotlib.pyplot as plt +import numpy as np + +from mpl_toolkits.mplot3d import Axes3D + +import gtsam +from gtsam_utils import plotPose3 +from PreintegrationExample import PreintegrationExample, POSES_FIG + +# shorthand symbols: +BIAS_KEY = int(gtsam.Symbol('b', 0)) +V = lambda j: int(gtsam.Symbol('v', j)) +X = lambda i: int(gtsam.Symbol('x', i)) + +class ImuFactorExample(PreintegrationExample): + + def __init__(self): + self.velocity = np.array([2, 0, 0]) + self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) + self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + + forward_twist = (np.zeros(3), self.velocity) + loop_twist = (np.array([0, -math.radians(30), 0]), self.velocity) + + accBias = np.array([-0.3, 0.1, 0.2]) + gyroBias = np.array([0.1, 0.3, -0.1]) + bias = gtsam.ConstantBias(accBias, gyroBias) + + super(ImuFactorExample, self).__init__(loop_twist, bias) + + def addPrior(self, i, graph): + state = self.scenario.navState(i) + graph.push_back(gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise)) + graph.push_back(gtsam.PriorFactorVector3(V(i), state.velocity(), self.velNoise)) + + def run(self): + graph = gtsam.NonlinearFactorGraph() + + i = 0 # state index + + # initialize data structure for pre-integrated IMU measurements + pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias) + + # simulate the loop + T = 12 + actual_state_i = self.scenario.navState(0) + for k, t in enumerate(np.arange(0, T, self.dt)): + # get measurements and add them to PIM + measuredOmega = self.runner.measuredAngularVelocity(t) + measuredAcc = self.runner.measuredSpecificForce(t) + pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt) + + # Plot IMU many times + if k % 10 == 0: + self.plotImu(t, measuredOmega, measuredAcc) + + # Plot every second + if k % 100 == 0: + self.plotGroundTruthPose(t) + + # create IMU factor every second + if (k + 1) % 100 == 0: + factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1), BIAS_KEY, pim) + graph.push_back(factor) + pim.resetIntegration() + actual_state_i = self.scenario.navState(t + self.dt) + i += 1 + + # add priors on beginning and end + num_poses = i + 1 + self.addPrior(0, graph) + self.addPrior(num_poses - 1, graph) + + initial = gtsam.Values() + initial.insert(BIAS_KEY, self.actualBias) + for i in range(num_poses): + state_i = self.scenario.navState(float(i)) + initial.insert(X(i), state_i.pose()) + initial.insert(V(i), state_i.velocity()) + + # optimize using Levenberg-Marquardt optimization + params = gtsam.LevenbergMarquardtParams() + params.setVerbosityLM("SUMMARY") + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) + result = optimizer.optimize() + + # Plot resulting poses + i = 0 + while result.exists(X(i)): + pose_i = result.atPose3(X(i)) + plotPose3(POSES_FIG, pose_i, 0.1) + i += 1 + print(result.atConstantBias(BIAS_KEY)) + + plt.ioff() + plt.show() + +if __name__ == '__main__': + ImuFactorExample().run() diff --git a/python/gtsam_examples/OdometryExample.py b/python/gtsam_examples/OdometryExample.py new file mode 100644 index 000000000..0800bab4e --- /dev/null +++ b/python/gtsam_examples/OdometryExample.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python +from __future__ import print_function +import gtsam +import numpy as np + +# Create an empty nonlinear factor graph +graph = gtsam.NonlinearFactorGraph() + +# Add a prior on the first pose, setting it to the origin +# A prior factor consists of a mean and a noise model (covariance matrix) +priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin +priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) +graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise)) + +# Add odometry factors +odometry = gtsam.Pose2(2.0, 0.0, 0.0) +# For simplicity, we will use the same noise model for each odometry factor +odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) +# Create odometry (Between) factors between consecutive poses +graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise)) +graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise)) +graph.print("\nFactor Graph:\n") + +# Create the data structure to hold the initialEstimate estimate to the solution +# For illustrative purposes, these have been deliberately set to incorrect values +initial = gtsam.Values() +initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) +initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) +initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1)) +initial.print("\nInitial Estimate:\n") + +# optimize using Levenberg-Marquardt optimization +params = gtsam.LevenbergMarquardtParams() +optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) +result = optimizer.optimize() +result.print("\nFinal Result:\n") diff --git a/python/gtsam_examples/PreintegrationExample.py b/python/gtsam_examples/PreintegrationExample.py new file mode 100644 index 000000000..7095dc59e --- /dev/null +++ b/python/gtsam_examples/PreintegrationExample.py @@ -0,0 +1,129 @@ +""" +A script validating the Preintegration of IMU measurements +""" + +import math +import matplotlib.pyplot as plt +import numpy as np + +from mpl_toolkits.mplot3d import Axes3D + +import gtsam +from gtsam_utils import plotPose3 + +IMU_FIG = 1 +POSES_FIG = 2 + +class PreintegrationExample(object): + + @staticmethod + def defaultParams(g): + """Create default parameters with Z *up* and realistic noise parameters""" + params = gtsam.PreintegrationParams.MakeSharedU(g) + kGyroSigma = math.radians(0.5) / 60 # 0.5 degree ARW + kAccelSigma = 0.1 / 60 # 10 cm VRW + params.gyroscopeCovariance = kGyroSigma ** 2 * np.identity(3, np.float) + params.accelerometerCovariance = kAccelSigma ** 2 * np.identity(3, np.float) + params.integrationCovariance = 0.0000001 ** 2 * np.identity(3, np.float) + return params + + def __init__(self, twist=None, bias=None): + """Initialize with given twist, a pair(angularVelocityVector, velocityVector).""" + + # setup interactive plotting + plt.ion() + + # Setup loop as default scenario + if twist is not None: + (W, V) = twist + else: + # default = loop with forward velocity 2m/s, while pitching up + # with angular velocity 30 degree/sec (negative in FLU) + W = np.array([0, -math.radians(30), 0]) + V = np.array([2, 0, 0]) + + self.scenario = gtsam.ConstantTwistScenario(W, V) + self.dt = 1e-2 + + self.maxDim = 5 + self.labels = list('xyz') + self.colors = list('rgb') + + # Create runner + self.g = 10 # simple gravity constant + self.params = self.defaultParams(self.g) + ptr = gtsam.ScenarioPointer(self.scenario) + + if bias is not None: + self.actualBias = bias + else: + accBias = np.array([0, 0.1, 0]) + gyroBias = np.array([0, 0, 0]) + self.actualBias = gtsam.ConstantBias(accBias, gyroBias) + + self.runner = gtsam.ScenarioRunner(ptr, self.params, self.dt, self.actualBias) + + def plotImu(self, t, measuredOmega, measuredAcc): + plt.figure(IMU_FIG) + + # plot angular velocity + omega_b = self.scenario.omega_b(t) + for i, (label, color) in enumerate(zip(self.labels, self.colors)): + plt.subplot(4, 3, i + 1) + plt.scatter(t, omega_b[i], color='k', marker='.') + plt.scatter(t, measuredOmega[i], color=color, marker='.') + plt.xlabel('angular velocity ' + label) + + # plot acceleration in nav + acceleration_n = self.scenario.acceleration_n(t) + for i, (label, color) in enumerate(zip(self.labels, self.colors)): + plt.subplot(4, 3, i + 4) + plt.scatter(t, acceleration_n[i], color=color, marker='.') + plt.xlabel('acceleration in nav ' + label) + + # plot acceleration in body + acceleration_b = self.scenario.acceleration_b(t) + for i, (label, color) in enumerate(zip(self.labels, self.colors)): + plt.subplot(4, 3, i + 7) + plt.scatter(t, acceleration_b[i], color=color, marker='.') + plt.xlabel('acceleration in body ' + label) + + # plot actual specific force, as well as corrupted + actual = self.runner.actualSpecificForce(t) + for i, (label, color) in enumerate(zip(self.labels, self.colors)): + plt.subplot(4, 3, i + 10) + plt.scatter(t, actual[i], color='k', marker='.') + plt.scatter(t, measuredAcc[i], color=color, marker='.') + plt.xlabel('specific force ' + label) + + def plotGroundTruthPose(self, t): + # plot ground truth pose, as well as prediction from integrated IMU measurements + actualPose = self.scenario.pose(t) + plotPose3(POSES_FIG, actualPose, 0.3) + t = actualPose.translation() + self.maxDim = max([abs(t.x()), abs(t.y()), abs(t.z()), self.maxDim]) + ax = plt.gca() + ax.set_xlim3d(-self.maxDim, self.maxDim) + ax.set_ylim3d(-self.maxDim, self.maxDim) + ax.set_zlim3d(-self.maxDim, self.maxDim) + + plt.pause(0.01) + + def run(self): + # simulate the loop + T = 12 + for i, t in enumerate(np.arange(0, T, self.dt)): + measuredOmega = self.runner.measuredAngularVelocity(t) + measuredAcc = self.runner.measuredSpecificForce(t) + if i % 25 == 0: + self.plotImu(t, measuredOmega, measuredAcc) + self.plotGroundTruthPose(t) + pim = self.runner.integrate(t, self.actualBias, True) + predictedNavState = self.runner.predict(pim, self.actualBias) + plotPose3(POSES_FIG, predictedNavState.pose(), 0.1) + + plt.ioff() + plt.show() + +if __name__ == '__main__': + PreintegrationExample().run() diff --git a/python/gtsam_examples/SFMdata.py b/python/gtsam_examples/SFMdata.py new file mode 100644 index 000000000..21a438226 --- /dev/null +++ b/python/gtsam_examples/SFMdata.py @@ -0,0 +1,32 @@ + + # A structure-from-motion example with landmarks + # - The landmarks form a 10 meter cube + # - The robot rotates around the landmarks, always facing towards the cube + +import gtsam +import numpy as np + +def createPoints(): + # Create the set of ground-truth landmarks + points = [gtsam.Point3(10.0,10.0,10.0), + gtsam.Point3(-10.0,10.0,10.0), + gtsam.Point3(-10.0,-10.0,10.0), + gtsam.Point3(10.0,-10.0,10.0), + gtsam.Point3(10.0,10.0,-10.0), + gtsam.Point3(-10.0,10.0,-10.0), + gtsam.Point3(-10.0,-10.0,-10.0), + gtsam.Point3(10.0,-10.0,-10.0)] + return points + +def createPoses(): + # Create the set of ground-truth poses + radius = 30.0 + angles = np.linspace(0,2*np.pi,8,endpoint=False) + up = gtsam.Point3(0,0,1) + target = gtsam.Point3(0,0,0) + poses = [] + for theta in angles: + position = gtsam.Point3(radius*np.cos(theta), radius*np.sin(theta), 0.0) + camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up) + poses.append(camera.pose()) + return poses diff --git a/python/gtsam_examples/VisualISAM2Example.py b/python/gtsam_examples/VisualISAM2Example.py new file mode 100644 index 000000000..9dafa13e7 --- /dev/null +++ b/python/gtsam_examples/VisualISAM2Example.py @@ -0,0 +1,132 @@ +from __future__ import print_function + +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +import numpy as np +import time # for sleep() + +import gtsam +from gtsam_examples import SFMdata +import gtsam_utils + +# shorthand symbols: +X = lambda i: int(gtsam.Symbol('x', i)) +L = lambda j: int(gtsam.Symbol('l', j)) + +def visual_ISAM2_plot(poses, points, result): + # VisualISAMPlot plots current state of ISAM2 object + # Author: Ellon Paiva + # Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert + + # Declare an id for the figure + fignum = 0 + + fig = plt.figure(fignum) + ax = fig.gca(projection='3d') + plt.cla() + + # Plot points + # Can't use data because current frame might not see all points + # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()) # TODO - this is slow + # gtsam.plot3DPoints(result, [], marginals) + gtsam_utils.plot3DPoints(fignum, result, 'rx') + + # Plot cameras + i = 0 + while result.exists(X(i)): + pose_i = result.atPose3(X(i)) + gtsam_utils.plotPose3(fignum, pose_i, 10) + i += 1 + + # draw + ax.set_xlim3d(-40, 40) + ax.set_ylim3d(-40, 40) + ax.set_zlim3d(-40, 40) + plt.pause(1) + +def visual_ISAM2_example(): + plt.ion() + + # Define the camera calibration parameters + K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) + + # Define the camera observation noise model + measurementNoise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v + + # Create the set of ground-truth landmarks + points = SFMdata.createPoints() + + # Create the set of ground-truth poses + poses = SFMdata.createPoses() + + # Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps to maintain proper linearization + # and efficient variable ordering, iSAM2 performs partial relinearization/reordering at each step. A parameter + # structure is available that allows the user to set various properties, such as the relinearization threshold + # and type of linear solver. For this example, we we set the relinearization threshold small so the iSAM2 result + # will approach the batch result. + parameters = gtsam.ISAM2Params() + parameters.relinearize_threshold = 0.01 + parameters.relinearize_skip = 1 + isam = gtsam.ISAM2(parameters) + + # Create a Factor Graph and Values to hold the new data + graph = gtsam.NonlinearFactorGraph() + initialEstimate = gtsam.Values() + + # Loop over the different poses, adding the observations to iSAM incrementally + for i, pose in enumerate(poses): + + # Add factors for each landmark observation + for j, point in enumerate(points): + camera = gtsam.PinholeCameraCal3_S2(pose, K) + measurement = camera.project(point) + graph.push_back(gtsam.GenericProjectionFactorCal3_S2(measurement, measurementNoise, X(i), L(j), K)) + + # Add an initial guess for the current pose + # Intentionally initialize the variables off from the ground truth + initialEstimate.insert(X(i), pose.compose(gtsam.Pose3(gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) + + # If this is the first iteration, add a prior on the first pose to set the coordinate frame + # and a prior on the first landmark to set the scale + # Also, as iSAM solves incrementally, we must wait until each is observed at least twice before + # adding it to iSAM. + if(i == 0): + # Add a prior on pose x0 + poseNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], poseNoise)) + + # Add a prior on landmark l0 + pointNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + graph.push_back(gtsam.PriorFactorPoint3(L(0), points[0], pointNoise)) # add directly to graph + + # Add initial guesses to all observed landmarks + # Intentionally initialize the variables off from the ground truth + for j, point in enumerate(points): + initialEstimate.insert(L(j), point + gtsam.Point3(-0.25, 0.20, 0.15)) + else: + # Update iSAM with the new factors + isam.update(graph, initialEstimate) + # Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver. + # If accuracy is desired at the expense of time, update(*) can be called additional times + # to perform multiple optimizer iterations every step. + isam.update() + currentEstimate = isam.calculate_estimate() + print("****************************************************") + print("Frame", i, ":") + for j in range(i + 1): + print(X(j), ":", currentEstimate.atPose3(X(j))) + + for j in range(len(points)): + print(L(j), ":", currentEstimate.atPoint3(L(j))) + + visual_ISAM2_plot(poses, points, currentEstimate) + + # Clear the factor graph and values for the next iteration + graph.resize(0) + initialEstimate.clear() + + plt.ioff() + plt.show() + +if __name__ == '__main__': + visual_ISAM2_example() diff --git a/python/gtsam_examples/__init__.py b/python/gtsam_examples/__init__.py new file mode 100644 index 000000000..f9d032d54 --- /dev/null +++ b/python/gtsam_examples/__init__.py @@ -0,0 +1,2 @@ +from . import SFMdata +from . import VisualISAM2Example diff --git a/python/gtsam_tests/__init__.py b/python/gtsam_tests/__init__.py new file mode 100644 index 000000000..8b1378917 --- /dev/null +++ b/python/gtsam_tests/__init__.py @@ -0,0 +1 @@ + diff --git a/python/gtsam_tests/testPoint2.py b/python/gtsam_tests/testPoint2.py new file mode 100644 index 000000000..80a6f6cbf --- /dev/null +++ b/python/gtsam_tests/testPoint2.py @@ -0,0 +1,13 @@ +import unittest +from gtsam import * + +#https://docs.python.org/2/library/unittest.html +class TestPoint2(unittest.TestCase): + def setUp(self): + self.point = Point2() + + def test_constructor(self): + pass + +if __name__ == '__main__': + unittest.main() diff --git a/python/gtsam_tests/testScenario.py b/python/gtsam_tests/testScenario.py new file mode 100644 index 000000000..e78121241 --- /dev/null +++ b/python/gtsam_tests/testScenario.py @@ -0,0 +1,32 @@ +import math +import unittest +import numpy as np + +import gtsam + +class TestScenario(unittest.TestCase): + def setUp(self): + pass + + def test_loop(self): + # Forward velocity 2m/s + # Pitch up with angular velocity 6 degree/sec (negative in FLU) + v = 2 + w = math.radians(6) + W = np.array([0, -w, 0]) + V = np.array([v, 0, 0]) + scenario = gtsam.ConstantTwistScenario(W, V) + + T = 30 + np.testing.assert_almost_equal(W, scenario.omega_b(T)) + np.testing.assert_almost_equal(V, scenario.velocity_b(T)) + np.testing.assert_almost_equal(np.cross(W, V), scenario.acceleration_b(T)) + + # R = v/w, so test if loop crests at 2*R + R = v / w + T30 = scenario.pose(T) + np.testing.assert_almost_equal(np.array([-math.pi, 0, -math.pi]), T30.rotation().xyz()) + self.assert_(gtsam.Point3(0, 0, 2 * R).equals(T30.translation())) + +if __name__ == '__main__': + unittest.main() diff --git a/python/gtsam_tests/testScenarioRunner.py b/python/gtsam_tests/testScenarioRunner.py new file mode 100644 index 000000000..2e1b47202 --- /dev/null +++ b/python/gtsam_tests/testScenarioRunner.py @@ -0,0 +1,30 @@ +import math +import unittest +import numpy as np + +import gtsam + +class TestScenarioRunner(unittest.TestCase): + def setUp(self): + self.g = 10 # simple gravity constant + + def test_loop_runner(self): + # Forward velocity 2m/s + # Pitch up with angular velocity 6 degree/sec (negative in FLU) + v = 2 + w = math.radians(6) + W = np.array([0, -w, 0]) + V = np.array([v, 0, 0]) + scenario = gtsam.ConstantTwistScenario(W, V) + + dt = 0.1 + params = gtsam.PreintegrationParams.MakeSharedU(self.g) + runner = gtsam.ScenarioRunner(gtsam.ScenarioPointer(scenario), params, dt) + + # Test specific force at time 0: a is pointing up + t = 0.0 + a = w * v + np.testing.assert_almost_equal(np.array([0, 0, a + self.g]), runner.actualSpecificForce(t)) + +if __name__ == '__main__': + unittest.main() diff --git a/python/gtsam_utils/__init__.py b/python/gtsam_utils/__init__.py new file mode 100644 index 000000000..56c55aa94 --- /dev/null +++ b/python/gtsam_utils/__init__.py @@ -0,0 +1 @@ +from .plot import * diff --git a/python/gtsam_utils/plot.py b/python/gtsam_utils/plot.py new file mode 100644 index 000000000..84a388bbb --- /dev/null +++ b/python/gtsam_utils/plot.py @@ -0,0 +1,59 @@ +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D as _Axes3D + +def plotPoint3(fignum, point, linespec): + fig = plt.figure(fignum) + ax = fig.gca(projection='3d') + ax.plot([point.x()], [point.y()], [point.z()], linespec) + + +def plot3DPoints(fignum, values, linespec, marginals=None): + # PLOT3DPOINTS Plots the Point3's in a values, with optional covariances + # Finds all the Point3 objects in the given Values object and plots them. + # If a Marginals object is given, this function will also plot marginal + # covariance ellipses for each point. + + keys = values.keys() + + # Plot points and covariance matrices + for key in keys: + try: + p = values.atPoint3(key); + # if haveMarginals + # P = marginals.marginalCovariance(key); + # gtsam.plotPoint3(p, linespec, P); + # else + plotPoint3(fignum, p, linespec); + except RuntimeError: + continue + # I guess it's not a Point3 + +def plotPose3(fignum, pose, axisLength=0.1): + # get figure object + fig = plt.figure(fignum) + ax = fig.gca(projection='3d') + + # get rotation and translation (center) + gRp = pose.rotation().matrix() # rotation from pose to global + C = pose.translation().vector() + + # draw the camera axes + xAxis = C + gRp[:, 0] * axisLength + L = np.append(C[np.newaxis], xAxis[np.newaxis], axis=0) + ax.plot(L[:, 0], L[:, 1], L[:, 2], 'r-') + + yAxis = C + gRp[:, 1] * axisLength + L = np.append(C[np.newaxis], yAxis[np.newaxis], axis=0) + ax.plot(L[:, 0], L[:, 1], L[:, 2], 'g-') + + zAxis = C + gRp[:, 2] * axisLength + L = np.append(C[np.newaxis], zAxis[np.newaxis], axis=0) + ax.plot(L[:, 0], L[:, 1], L[:, 2], 'b-') + + # # plot the covariance + # if (nargin>2) && (~isempty(P)) + # pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame + # gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame + # gtsam.covarianceEllipse3D(C,gPp); + # end diff --git a/python/handwritten/CMakeLists.txt b/python/handwritten/CMakeLists.txt new file mode 100644 index 000000000..9d4f9151a --- /dev/null +++ b/python/handwritten/CMakeLists.txt @@ -0,0 +1,30 @@ +# get subdirectories list +subdirlist(SUBDIRS ${CMAKE_CURRENT_SOURCE_DIR}) + +# get the sources needed to compile gtsam python module +set(gtsam_python_srcs "") +foreach(subdir ${SUBDIRS}) + file(GLOB ${subdir}_src "${subdir}/*.cpp") + list(APPEND gtsam_python_srcs ${${subdir}_src}) +endforeach() + +# Create the library +add_library(gtsam_python SHARED exportgtsam.cpp ${gtsam_python_srcs}) +set_target_properties(gtsam_python PROPERTIES + OUTPUT_NAME gtsam_python + SKIP_BUILD_RPATH TRUE + CLEAN_DIRECT_OUTPUT 1 +) +target_link_libraries(gtsam_python + ${Boost_PYTHON${BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE}_LIBRARY} + ${PYTHON_LIBRARY} gtsam) + +# Cause the library to be output in the correct directory. +# TODO: Change below to work on different systems (currently works only with Linux) +add_custom_command( + OUTPUT ${CMAKE_BINARY_DIR}/python/gtsam/_libgtsam_python.so + DEPENDS gtsam_python + COMMAND ${CMAKE_COMMAND} -E copy $ ${CMAKE_BINARY_DIR}/python/gtsam/_libgtsam_python.so + COMMENT "Copying extension module to python/gtsam/_libgtsam_python.so" +) +add_custom_target(copy_gtsam_python_module ALL DEPENDS ${CMAKE_BINARY_DIR}/python/gtsam/_libgtsam_python.so) \ No newline at end of file diff --git a/python/handwritten/base/FastVector.cpp b/python/handwritten/base/FastVector.cpp new file mode 100644 index 000000000..1c3582813 --- /dev/null +++ b/python/handwritten/base/FastVector.cpp @@ -0,0 +1,37 @@ + /* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps FastVector instances to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/base/FastVector.h" +#include "gtsam/base/types.h" // for Key definition + +using namespace boost::python; +using namespace gtsam; + +void exportFastVectors(){ + + typedef FastVector KeyVector; + + class_("KeyVector") + .def(vector_indexing_suite()) + ; + +} \ No newline at end of file diff --git a/python/handwritten/exportgtsam.cpp b/python/handwritten/exportgtsam.cpp new file mode 100644 index 000000000..7e8c22a82 --- /dev/null +++ b/python/handwritten/exportgtsam.cpp @@ -0,0 +1,105 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief exports the python module + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include +#include + +#include + +// base +void exportFastVectors(); + +// geometry +void exportPoint2(); +void exportPoint3(); +void exportRot2(); +void exportRot3(); +void exportPose2(); +void exportPose3(); +void exportPinholeBaseK(); +void exportPinholeCamera(); +void exportCal3_S2(); + +// inference +void exportSymbol(); + +// linear +void exportNoiseModels(); + +// nonlinear +void exportValues(); +void exportNonlinearFactor(); +void exportNonlinearFactorGraph(); +void exportLevenbergMarquardtOptimizer(); +void exportISAM2(); + +// slam +void exportPriorFactors(); +void exportBetweenFactors(); +void exportGenericProjectionFactor(); + +// navigation +void exportImuFactor(); +void exportScenario(); + + +// Utils (or Python wrapper specific functions) +void registerNumpyEigenConversions(); + +//-----------------------------------// + +BOOST_PYTHON_MODULE(_libgtsam_python){ + + // NOTE: We need to call import_array1() instead of import_array() to support both python 2 + // and 3. The reason is that BOOST_PYTHON_MODULE puts all its contents in a function + // returning void, and import_array() is a macro that when expanded for python 3, adds + // a 'return __null' statement to that function. For more info check files: + // boost/python/module_init.hpp and numpy/__multiarray_api.h (bottom of the file). + // Should be the first thing to be done + import_array1(); + + registerNumpyEigenConversions(); + + exportFastVectors(); + + exportPoint2(); + exportPoint3(); + exportRot2(); + exportRot3(); + exportPose2(); + exportPose3(); + exportPinholeBaseK(); + exportPinholeCamera(); + exportCal3_S2(); + + exportSymbol(); + + exportNoiseModels(); + + exportValues(); + exportNonlinearFactor(); + exportNonlinearFactorGraph(); + exportLevenbergMarquardtOptimizer(); + exportISAM2(); + + exportPriorFactors(); + exportBetweenFactors(); + exportGenericProjectionFactor(); + + exportImuFactor(); + exportScenario(); +} diff --git a/python/handwritten/geometry/Cal3_S2.cpp b/python/handwritten/geometry/Cal3_S2.cpp new file mode 100644 index 000000000..339cd6a3c --- /dev/null +++ b/python/handwritten/geometry/Cal3_S2.cpp @@ -0,0 +1,62 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Cal3_S2 class to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Cal3_S2.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Cal3_S2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Cal3_S2::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(uncalibrate_overloads, Cal3_S2::uncalibrate, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(calibrate_overloads, Cal3_S2::calibrate, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Cal3_S2::between, 1, 3) + +// Function pointers to desambiguate Cal3_S2::calibrate calls +Point2 (Cal3_S2::*calibrate1)(const Point2 &, OptionalJacobian< 2, 5 > Dcal, OptionalJacobian< 2, 2 > Dp) const = &Cal3_S2::calibrate; +Vector3 (Cal3_S2::*calibrate2)(const Vector3 &) const = &Cal3_S2::calibrate; + +void exportCal3_S2(){ + +class_("Cal3_S2", init<>()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def("print", &Cal3_S2::print, print_overloads(args("s"))) + .def("equals", &Cal3_S2::equals, equals_overloads(args("q","tol"))) + .def("fx",&Cal3_S2::fx) + .def("fy",&Cal3_S2::fy) + .def("skew",&Cal3_S2::skew) + .def("px",&Cal3_S2::px) + .def("py",&Cal3_S2::py) + .def("principal_point",&Cal3_S2::principalPoint) + .def("vector",&Cal3_S2::vector) + .def("k",&Cal3_S2::K) + .def("matrix",&Cal3_S2::matrix) + .def("matrix_inverse",&Cal3_S2::matrix_inverse) + .def("uncalibrate",&Cal3_S2::uncalibrate, uncalibrate_overloads()) + .def("calibrate",calibrate1, calibrate_overloads()) + .def("calibrate",calibrate2) + .def("between",&Cal3_S2::between, between_overloads()) +; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/PinholeBaseK.cpp b/python/handwritten/geometry/PinholeBaseK.cpp new file mode 100644 index 000000000..da98783e2 --- /dev/null +++ b/python/handwritten/geometry/PinholeBaseK.cpp @@ -0,0 +1,66 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps PinholeCamera classes to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/PinholeCamera.h" +#include "gtsam/geometry/Cal3_S2.h" + + +using namespace boost::python; +using namespace gtsam; + +typedef PinholeBaseK PinholeBaseKCal3_S2; + +// Wrapper on PinholeBaseK because it has pure virtual method calibration() +struct PinholeBaseKCal3_S2Callback : PinholeBaseKCal3_S2, wrapper +{ + const Cal3_S2 & calibration () const { + return this->get_override("calibration")(); + } +}; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(project_overloads, PinholeBaseKCal3_S2::project, 2, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, PinholeBaseKCal3_S2::range, 1, 3) + +// Function pointers to desambiguate project() calls +Point2 (PinholeBaseKCal3_S2::*project1) (const Point3 &pw) const = &PinholeBaseKCal3_S2::project; +Point2 (PinholeBaseKCal3_S2::*project2) (const Point3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 3 > Dpoint, OptionalJacobian< 2, FixedDimension::value > Dcal) const = &PinholeBaseKCal3_S2::project; +Point2 (PinholeBaseKCal3_S2::*project3) (const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 2 > Dpoint, OptionalJacobian< 2, FixedDimension::value > Dcal) const = &PinholeBaseKCal3_S2::project; + +// function pointers to desambiguate range() calls +double (PinholeBaseKCal3_S2::*range1) (const Point3 &point, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 3 > Dpoint) const = &PinholeBaseKCal3_S2::range; +double (PinholeBaseKCal3_S2::*range2) (const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dpose) const = &PinholeBaseKCal3_S2::range; +double (PinholeBaseKCal3_S2::*range3) (const CalibratedCamera &camera, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dother) const = &PinholeBaseKCal3_S2::range; + +void exportPinholeBaseK(){ + + class_("PinholeBaseKCal3_S2", no_init) + .def("calibration", pure_virtual(&PinholeBaseKCal3_S2::calibration), return_value_policy()) + .def("project", project1) + .def("project", project2, project_overloads()) + .def("project", project3, project_overloads()) + .def("backproject", &PinholeBaseKCal3_S2::backproject) + .def("backproject_point_at_infinity", &PinholeBaseKCal3_S2::backprojectPointAtInfinity) + .def("range", range1, range_overloads()) + .def("range", range2, range_overloads()) + .def("range", range3, range_overloads()) + ; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/PinholeCamera.cpp b/python/handwritten/geometry/PinholeCamera.cpp new file mode 100644 index 000000000..21ffcdeb7 --- /dev/null +++ b/python/handwritten/geometry/PinholeCamera.cpp @@ -0,0 +1,51 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps PinholeCamera classes to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/PinholeCamera.h" +#include "gtsam/geometry/Cal3_S2.h" + +using namespace boost::python; +using namespace gtsam; + +typedef PinholeBaseK PinholeBaseKCal3_S2; +typedef PinholeCamera PinholeCameraCal3_S2; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, PinholeCameraCal3_S2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, PinholeCameraCal3_S2::equals, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Lookat_overloads, PinholeCameraCal3_S2::Lookat, 3, 4) + +void exportPinholeCamera(){ + +class_ >("PinholeCameraCal3_S2", init<>()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def("print", &PinholeCameraCal3_S2::print, print_overloads(args("s"))) + .def("equals", &PinholeCameraCal3_S2::equals, equals_overloads(args("q","tol"))) + .def("pose", &PinholeCameraCal3_S2::pose, return_value_policy()) + // We don't need to define calibration() here because it's already defined as virtual in the base class PinholeBaseKCal3_S2 + // .def("calibration", &PinholeCameraCal3_S2::calibration, return_value_policy()) + .def("Lookat", &PinholeCameraCal3_S2::Lookat, Lookat_overloads()) + .staticmethod("Lookat") +; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/Point2.cpp b/python/handwritten/geometry/Point2.cpp new file mode 100644 index 000000000..7af3f8cf6 --- /dev/null +++ b/python/handwritten/geometry/Point2.cpp @@ -0,0 +1,58 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Point2 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Point2.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point2::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Point2::compose, 1, 3) + +void exportPoint2(){ + + class_("Point2", init<>()) + .def(init()) + .def(init()) + .def("identity", &Point2::identity) + .def("dist", &Point2::dist) + .def("distance", &Point2::distance) + .def("equals", &Point2::equals, equals_overloads(args("q","tol"))) + .def("norm", &Point2::norm) + .def("print", &Point2::print, print_overloads(args("s"))) + .def("unit", &Point2::unit) + .def("vector", &Point2::vector) + .def("x", &Point2::x) + .def("y", &Point2::y) + .def(self * other()) // __mult__ + .def(other() * self) // __mult__ + .def(self + self) + .def(-self) + .def(self - self) + .def(self / other()) + .def(self_ns::str(self)) + .def(repr(self)) + .def(self == self) + ; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp new file mode 100644 index 000000000..664b4ffda --- /dev/null +++ b/python/handwritten/geometry/Point3.cpp @@ -0,0 +1,64 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Point3 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Point3.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point3::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point3::equals, 1, 2) + +void exportPoint3(){ + +class_("Point3") + .def(init<>()) + .def(init()) + .def(init()) + .def("identity", &Point3::identity) + .staticmethod("identity") + .def("add", &Point3::add) + .def("cross", &Point3::cross) + .def("dist", &Point3::dist) + .def("distance", &Point3::distance) + .def("dot", &Point3::dot) + .def("equals", &Point3::equals, equals_overloads(args("q","tol"))) + .def("norm", &Point3::norm) + .def("normalize", &Point3::normalize) + .def("print", &Point3::print, print_overloads(args("s"))) + .def("sub", &Point3::sub) + .def("vector", &Point3::vector) + .def("x", &Point3::x) + .def("y", &Point3::y) + .def("z", &Point3::z) + .def(self * other()) + .def(other() * self) + .def(self + self) + .def(-self) + .def(self - self) + .def(self / other()) + .def(self_ns::str(self)) + .def(repr(self)) + .def(self == self) +; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/Pose2.cpp b/python/handwritten/geometry/Pose2.cpp new file mode 100644 index 000000000..4f402df7e --- /dev/null +++ b/python/handwritten/geometry/Pose2.cpp @@ -0,0 +1,93 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Pose2 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Pose2.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Pose2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Pose2::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Pose2::compose, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Pose2::between, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose2::transform_to, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose2::transform_from, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(bearing_overloads, Pose2::bearing, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, Pose2::range, 1, 3) + +// Manually wrap + +void exportPose2(){ + + // double (Pose2::*range1)(const Pose2&, boost::optional, boost::optional) const + // = &Pose2::range; + // double (Pose2::*range2)(const Point2&, boost::optional, boost::optional) const + // = &Pose2::range; + + // Rot2 (Pose2::*bearing1)(const Pose2&, boost::optional, boost::optional) const + // = &Pose2::bearing; + // Rot2 (Pose2::*bearing2)(const Point2&, boost::optional, boost::optional) const + // = &Pose2::bearing; + + class_("Pose2", init<>()) + .def(init()) + .def(init()) + .def(init()) + .def("print", &Pose2::print, print_overloads(args("s"))) + + .def("equals", &Pose2::equals, equals_overloads(args("pose","tol"))) + // .def("inverse", &Pose2::inverse) + // .def("compose", &Pose2::compose, compose_overloads(args("p2", "H1", "H2"))) + // .def("between", &Pose2::between, between_overloads(args("p2", "H1", "H2"))) + // .def("dim", &Pose2::dim) + // .def("retract", &Pose2::retract) + + .def("transform_to", &Pose2::transform_to, + transform_to_overloads(args("point", "H1", "H2"))) + .def("transform_from", &Pose2::transform_from, + transform_to_overloads(args("point", "H1", "H2"))) + + .def("x", &Pose2::x) + .def("y", &Pose2::y) + .def("theta", &Pose2::theta) + // See documentation on call policy for more information + // https://wiki.python.org/moin/boost.python/CallPolicy + .def("t", &Pose2::t, return_value_policy()) + .def("r", &Pose2::r, return_value_policy()) + .def("translation", &Pose2::translation, return_value_policy()) + .def("rotation", &Pose2::rotation, return_value_policy()) + + // .def("bearing", bearing1, bearing_overloads()) + // .def("bearing", bearing2, bearing_overloads()) + + // Function overload example + // .def("range", range1, range_overloads()) + // .def("range", range2, range_overloads()) + + + .def("Expmap", &Pose2::Expmap) + .staticmethod("Expmap") + + .def(self * self) // __mult__ + ; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/Pose3.cpp b/python/handwritten/geometry/Pose3.cpp new file mode 100644 index 000000000..dfdfe8ad1 --- /dev/null +++ b/python/handwritten/geometry/Pose3.cpp @@ -0,0 +1,92 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Pose3 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Pose3.h" +#include "gtsam/geometry/Pose2.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Rot3.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Pose3::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Pose3::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose3::transform_to, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose3::transform_from, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(translation_overloads, Pose3::translation, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Pose3::compose, 2, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Pose3::between, 2, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(bearing_overloads, Pose3::bearing, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, Pose3::range, 1, 3) + +void exportPose3(){ + + // function pointers to desambiguate transform_to() calls + Point3 (Pose3::*transform_to1)(const Point3&, OptionalJacobian< 3, 6 >, OptionalJacobian< 3, 3 > ) const = &Pose3::transform_to; + Pose3 (Pose3::*transform_to2)(const Pose3&) const = &Pose3::transform_to; + // function pointers to desambiguate compose() calls + Pose3 (Pose3::*compose1)(const Pose3 &g) const = &Pose3::compose; + Pose3 (Pose3::*compose2)(const Pose3 &g, typename Pose3::ChartJacobian, typename Pose3::ChartJacobian) const = &Pose3::compose; + // function pointers to desambiguate between() calls + Pose3 (Pose3::*between1)(const Pose3 &g) const = &Pose3::between; + Pose3 (Pose3::*between2)(const Pose3 &g, typename Pose3::ChartJacobian, typename Pose3::ChartJacobian) const = &Pose3::between; + // function pointers to desambiguate range() calls + double (Pose3::*range1)(const Point3 &, OptionalJacobian<1,6>, OptionalJacobian<1,3>) const = &Pose3::range; + double (Pose3::*range2)(const Pose3 &, OptionalJacobian<1,6>, OptionalJacobian<1,6>) const = &Pose3::range; + + class_("Pose3") + .def(init<>()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def("print", &Pose3::print, print_overloads(args("s"))) + .def("equals", &Pose3::equals, equals_overloads(args("pose","tol"))) + .def("identity", &Pose3::identity) + .staticmethod("identity") + .def("bearing", &Pose3::bearing) + .def("matrix", &Pose3::matrix) + .def("transform_from", &Pose3::transform_from, + transform_from_overloads(args("point", "H1", "H2"))) + .def("transform_to", transform_to1, + transform_to_overloads(args("point", "H1", "H2"))) + .def("transform_to", transform_to2) + .def("x", &Pose3::x) + .def("y", &Pose3::y) + .def("z", &Pose3::z) + .def("translation", &Pose3::translation, + translation_overloads()[return_value_policy()]) + .def("rotation", &Pose3::rotation, return_value_policy()) + .def(self * self) // __mult__ + .def(self * other()) // __mult__ + .def(self_ns::str(self)) // __str__ + .def(repr(self)) // __repr__ + .def("compose", compose1) + .def("compose", compose2, compose_overloads()) + .def("between", between1) + .def("between", between2, between_overloads()) + .def("range", range1, range_overloads()) + .def("range", range2, range_overloads()) + .def("bearing", &Pose3::bearing, bearing_overloads()) + ; +} \ No newline at end of file diff --git a/python/handwritten/geometry/Rot2.cpp b/python/handwritten/geometry/Rot2.cpp new file mode 100644 index 000000000..59b4ce4e8 --- /dev/null +++ b/python/handwritten/geometry/Rot2.cpp @@ -0,0 +1,65 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Rot2 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Rot2.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Rot2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Rot2::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Rot2::compose, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(relativeBearing_overloads, Rot2::relativeBearing, 1, 3) + +void exportRot2(){ + + class_("Rot2", init<>()) + .def(init()) + .def("Expmap", &Rot2::Expmap) + .staticmethod("Expmap") + .def("Logmap", &Rot2::Logmap) + .staticmethod("Logmap") + .def("atan2", &Rot2::atan2) + .staticmethod("atan2") + .def("fromAngle", &Rot2::fromAngle) + .staticmethod("fromAngle") + .def("fromCosSin", &Rot2::fromCosSin) + .staticmethod("fromCosSin") + .def("fromDegrees", &Rot2::fromDegrees) + .staticmethod("fromDegrees") + .def("identity", &Rot2::identity) + .staticmethod("identity") + .def("relativeBearing", &Rot2::relativeBearing) + .staticmethod("relativeBearing") + .def("c", &Rot2::c) + .def("degrees", &Rot2::degrees) + .def("equals", &Rot2::equals, equals_overloads(args("q","tol"))) + .def("matrix", &Rot2::matrix) + .def("print", &Rot2::print, print_overloads(args("s"))) + .def("rotate", &Rot2::rotate) + .def("s", &Rot2::s) + .def("theta", &Rot2::theta) + .def("unrotate", &Rot2::unrotate) + .def(self * self) // __mult__ + ; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/Rot3.cpp b/python/handwritten/geometry/Rot3.cpp new file mode 100644 index 000000000..b4551ca5c --- /dev/null +++ b/python/handwritten/geometry/Rot3.cpp @@ -0,0 +1,111 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Rot3 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Rot3.h" + +using namespace boost::python; +using namespace gtsam; + +static Rot3 Quaternion_0(const Vector4& q) +{ + return Rot3::quaternion(q[0],q[1],q[2],q[3]); +} + +static Rot3 Quaternion_1(double w, double x, double y, double z) +{ + return Rot3::quaternion(w,x,y,z); +} + +// Prototypes used to perform overloading +// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/functions.html +gtsam::Rot3 (*AxisAngle_0)(const Vector3&, double) = &Rot3::AxisAngle; +gtsam::Rot3 (*AxisAngle_1)(const gtsam::Point3&, double) = &Rot3::AxisAngle; +gtsam::Rot3 (*Rodrigues_0)(const Vector3&) = &Rot3::Rodrigues; +gtsam::Rot3 (*Rodrigues_1)(double, double, double) = &Rot3::Rodrigues; +gtsam::Rot3 (*RzRyRx_0)(double, double, double) = &Rot3::RzRyRx; +gtsam::Rot3 (*RzRyRx_1)(const Vector&) = &Rot3::RzRyRx; +Vector (Rot3::*quaternion_0)() const = &Rot3::quaternion; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Rot3::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Rot3::equals, 1, 2) + +void exportRot3(){ + + class_("Rot3") + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def("Quaternion", Quaternion_0, arg("q"), "Creates a Rot3 from an array [w,x,y,z] representing a quaternion") + .def("Quaternion", Quaternion_1, (arg("w"),arg("x"),arg("y"),arg("z")) ) + .staticmethod("Quaternion") + .def("AxisAngle", AxisAngle_0) + .def("AxisAngle", AxisAngle_1) + .staticmethod("AxisAngle") + .def("Expmap", &Rot3::Expmap) + .staticmethod("Expmap") + .def("ExpmapDerivative", &Rot3::ExpmapDerivative) + .staticmethod("ExpmapDerivative") + .def("Logmap", &Rot3::Logmap) + .staticmethod("Logmap") + .def("LogmapDerivative", &Rot3::LogmapDerivative) + .staticmethod("LogmapDerivative") + .def("Rodrigues", Rodrigues_0) + .def("Rodrigues", Rodrigues_1) + .staticmethod("Rodrigues") + .def("Rx", &Rot3::Rx) + .staticmethod("Rx") + .def("Ry", &Rot3::Ry) + .staticmethod("Ry") + .def("Rz", &Rot3::Rz) + .staticmethod("Rz") + .def("RzRyRx", RzRyRx_0, (arg("x"),arg("y"),arg("z")), "Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis" ) + .def("RzRyRx", RzRyRx_1, arg("xyz"), "Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis" ) + .staticmethod("RzRyRx") + .def("identity", &Rot3::identity) + .staticmethod("identity") + .def("AdjointMap", &Rot3::AdjointMap) + .def("column", &Rot3::column) + .def("conjugate", &Rot3::conjugate) + .def("equals", &Rot3::equals, equals_overloads(args("q","tol"))) +#ifndef GTSAM_USE_QUATERNIONS + .def("localCayley", &Rot3::localCayley) + .def("retractCayley", &Rot3::retractCayley) +#endif + .def("matrix", &Rot3::matrix) + .def("print", &Rot3::print, print_overloads(args("s"))) + .def("r1", &Rot3::r1) + .def("r2", &Rot3::r2) + .def("r3", &Rot3::r3) + .def("rpy", &Rot3::rpy) + .def("slerp", &Rot3::slerp) + .def("transpose", &Rot3::transpose) + .def("xyz", &Rot3::xyz) + .def("quaternion", quaternion_0) + .def(self * self) + .def(self * other()) + .def(self * other()) + .def(self_ns::str(self)) // __str__ + .def(repr(self)) // __repr__ + ; + +} \ No newline at end of file diff --git a/python/handwritten/inference/Symbol.cpp b/python/handwritten/inference/Symbol.cpp new file mode 100644 index 000000000..9fc5b74e7 --- /dev/null +++ b/python/handwritten/inference/Symbol.cpp @@ -0,0 +1,83 @@ + /* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Symbol class to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include +#include + +#define NO_IMPORT_ARRAY +#include + +#include // for stringstream + +#include "gtsam/inference/Symbol.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Symbol::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Symbol::equals, 1, 2) + +// Helper function to allow building a symbol from a python string and a index. +static boost::shared_ptr makeSymbol(const std::string &str, size_t j) +{ + if(str.size() > 1) + throw std::runtime_error("string argument must have one character only"); + + return boost::make_shared(str.at(0),j); +} + +// Helper function to print the symbol as "char-and-index" in python +std::string selfToString(const Symbol & self) +{ + return (std::string)self; +} + +// Helper function to convert a Symbol to int using int() cast in python +size_t selfToKey(const Symbol & self) +{ + return self.key(); +} + +// Helper function to recover symbol's unsigned char as string +std::string chrFromSelf(const Symbol & self) +{ + std::stringstream ss; + ss << self.chr(); + return ss.str(); +} + +void exportSymbol(){ + +class_ >("Symbol") + .def(init<>()) + .def(init()) + .def("__init__", make_constructor(makeSymbol)) + .def(init()) + .def("print", &Symbol::print, print_overloads(args("s"))) + .def("equals", &Symbol::equals, equals_overloads(args("q","tol"))) + .def("key", &Symbol::key) + .def("index", &Symbol::index) + .def(self < self) + .def(self == self) + .def(self == other()) + .def(self != self) + .def(self != other()) + .def("__repr__", &selfToString) + .def("__int__", &selfToKey) + .def("chr", &chrFromSelf) +; + +} \ No newline at end of file diff --git a/python/handwritten/linear/NoiseModel.cpp b/python/handwritten/linear/NoiseModel.cpp new file mode 100644 index 000000000..3453184bd --- /dev/null +++ b/python/handwritten/linear/NoiseModel.cpp @@ -0,0 +1,137 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps the noise model classes into the noiseModel module + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + + /** TODOs Summary: + * + * TODO(Ellon): Don't know yet it it's worth/needed to add 'Wrap structs' for each of the noise models. + * I think it's only worthy if we want to access virtual the virtual functions from python. + * TODO(Ellon): Wrap non-pure virtual methods of Base on BaseWrap + */ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/linear/NoiseModel.h" + +using namespace boost::python; +using namespace gtsam; +using namespace gtsam::noiseModel; + +// Wrap around pure virtual class Base. +// All pure virtual methods should be wrapped. Non-pure may be wrapped if we want to mimic the +// overloading through inheritance in Python. +// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.class_virtual_functions +struct BaseCallback : Base, wrapper +{ + void print (const std::string & name="") const { + this->get_override("print")(); + } + bool equals (const Base & expected, double tol=1e-9) const { + return this->get_override("equals")(); + } + Vector whiten (const Vector & v) const { + return this->get_override("whiten")(); + } + Matrix Whiten (const Matrix & v) const { + return this->get_override("Whiten")(); + } + Vector unwhiten (const Vector & v) const { + return this->get_override("unwhiten")(); + } + double distance (const Vector & v) const { + return this->get_override("distance")(); + } + void WhitenSystem (std::vector< Matrix > &A, Vector &b) const { + this->get_override("WhitenSystem")(); + } + void WhitenSystem (Matrix &A, Vector &b) const { + this->get_override("WhitenSystem")(); + } + void WhitenSystem (Matrix &A1, Matrix &A2, Vector &b) const { + this->get_override("WhitenSystem")(); + } + void WhitenSystem (Matrix &A1, Matrix &A2, Matrix &A3, Vector &b) const { + this->get_override("WhitenSystem")(); + } + + // TODO(Ellon): Wrap non-pure virtual methods should go here. + // See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.virtual_functions_with_default_implementations + +}; + +// Overloads for named constructors. Named constructors are static, so we declare them +// using BOOST_PYTHON_FUNCTION_OVERLOADS instead of BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS +// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/functions.html#python.default_arguments +BOOST_PYTHON_FUNCTION_OVERLOADS(Gaussian_SqrtInformation_overloads, Gaussian::SqrtInformation, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Gaussian_Information_overloads, Gaussian::Information, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Gaussian_Covariance_overloads, Gaussian::Covariance, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Diagonal_Sigmas_overloads, Diagonal::Sigmas, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Diagonal_Variances_overloads, Diagonal::Variances, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Diagonal_Precisions_overloads, Diagonal::Precisions, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Isotropic_Sigma_overloads, Isotropic::Sigma, 2, 3) +BOOST_PYTHON_FUNCTION_OVERLOADS(Isotropic_Variance_overloads, Isotropic::Variance, 2, 3) +BOOST_PYTHON_FUNCTION_OVERLOADS(Isotropic_Precision_overloads, Isotropic::Precision, 2, 3) + + +void exportNoiseModels(){ + + // Create a scope "noiseModel". See: http://isolation-nation.blogspot.fr/2008/09/packages-in-python-extension-modules.html + std::string noiseModel_name = extract(scope().attr("__name__") + ".noiseModel"); + object noiseModel_module(handle<>(borrowed(PyImport_AddModule(noiseModel_name.c_str())))); + scope().attr("noiseModel") = noiseModel_module; + scope noiseModel_scope = noiseModel_module; + + // Then export our classes in the noiseModel scope + class_("Base") + .def("print", pure_virtual(&Base::print)) + ; + + // NOTE: We should use "Base" in "bases<...>", and not "BaseCallback" (it was not clear at the begining) + class_, bases >("Gaussian", no_init) + .def("SqrtInformation",&Gaussian::SqrtInformation, Gaussian_SqrtInformation_overloads()) + .staticmethod("SqrtInformation") + .def("Information",&Gaussian::Information, Gaussian_Information_overloads()) + .staticmethod("Information") + .def("Covariance",&Gaussian::Covariance, Gaussian_Covariance_overloads()) + .staticmethod("Covariance") + ; + + class_, bases >("Diagonal", no_init) + .def("Sigmas",&Diagonal::Sigmas, Diagonal_Sigmas_overloads()) + .staticmethod("Sigmas") + .def("Variances",&Diagonal::Variances, Diagonal_Variances_overloads()) + .staticmethod("Variances") + .def("Precisions",&Diagonal::Precisions, Diagonal_Precisions_overloads()) + .staticmethod("Precisions") + ; + + class_, bases >("Isotropic", no_init) + .def("Sigma",&Isotropic::Sigma, Isotropic_Sigma_overloads()) + .staticmethod("Sigma") + .def("Variance",&Isotropic::Variance, Isotropic_Variance_overloads()) + .staticmethod("Variance") + .def("Precision",&Isotropic::Precision, Isotropic_Precision_overloads()) + .staticmethod("Precision") + ; + + class_, bases >("Unit", no_init) + .def("Create",&Unit::Create) + .staticmethod("Create") + ; + +} \ No newline at end of file diff --git a/python/handwritten/navigation/ImuFactor.cpp b/python/handwritten/navigation/ImuFactor.cpp new file mode 100644 index 000000000..c947ae9ee --- /dev/null +++ b/python/handwritten/navigation/ImuFactor.cpp @@ -0,0 +1,94 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps ConstantTwistScenario class to python + * @author Frank Dellaert + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/navigation/ImuFactor.h" + +using namespace boost::python; +using namespace gtsam; + +typedef gtsam::OptionalJacobian<3, 9> OptionalJacobian39; +typedef gtsam::OptionalJacobian<9, 6> OptionalJacobian96; +typedef gtsam::OptionalJacobian<9, 9> OptionalJacobian9; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(attitude_overloads, attitude, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(position_overloads, position, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(velocity_overloads, velocity, 0, 1) + +void exportImuFactor() { + class_("OptionalJacobian39", init<>()); + class_("OptionalJacobian96", init<>()); + class_("OptionalJacobian9", init<>()); + + class_("NavState", init<>()) + // TODO(frank): overload with jacobians + .def("print", &NavState::print, print_overloads()) + .def("attitude", &NavState::attitude, + attitude_overloads()[return_value_policy()]) + .def("position", &NavState::position, + position_overloads()[return_value_policy()]) + .def("velocity", &NavState::velocity, + velocity_overloads()[return_value_policy()]) + .def(repr(self)) + .def("pose", &NavState::pose); + + class_("ConstantBias", init<>()) + .def(init()) + .def(repr(self)); + + class_>( + "PreintegrationParams", init()) + .def_readwrite("gyroscopeCovariance", + &PreintegrationParams::gyroscopeCovariance) + .def_readwrite("omegaCoriolis", &PreintegrationParams::omegaCoriolis) + .def_readwrite("body_P_sensor", &PreintegrationParams::body_P_sensor) + .def_readwrite("accelerometerCovariance", + &PreintegrationParams::accelerometerCovariance) + .def_readwrite("integrationCovariance", + &PreintegrationParams::integrationCovariance) + .def_readwrite("use2ndOrderCoriolis", + &PreintegrationParams::use2ndOrderCoriolis) + .def_readwrite("n_gravity", &PreintegrationParams::n_gravity) + + .def("MakeSharedD", &PreintegrationParams::MakeSharedD) + .staticmethod("MakeSharedD") + .def("MakeSharedU", &PreintegrationParams::MakeSharedU) + .staticmethod("MakeSharedU"); + + class_( + "PreintegratedImuMeasurements", + init&, + const imuBias::ConstantBias&>()) + .def(repr(self)) + .def("predict", &PreintegratedImuMeasurements::predict) + .def("computeError", &PreintegratedImuMeasurements::computeError) + .def("resetIntegration", &PreintegratedImuMeasurements::resetIntegration) + .def("integrateMeasurement", + &PreintegratedImuMeasurements::integrateMeasurement) + .def("preintMeasCov", &PreintegratedImuMeasurements::preintMeasCov); + + // NOTE(frank): Abstract classes need boost::noncopyable + class_, boost::shared_ptr>( + "ImuFactor") + .def("error", &ImuFactor::error) + .def(init()) + .def(repr(self)); +} diff --git a/python/handwritten/navigation/Scenario.cpp b/python/handwritten/navigation/Scenario.cpp new file mode 100644 index 000000000..e11f04a57 --- /dev/null +++ b/python/handwritten/navigation/Scenario.cpp @@ -0,0 +1,64 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps ConstantTwistScenario class to python + * @author Frank Dellaert + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/navigation/ScenarioRunner.h" + +using namespace boost::python; +using namespace gtsam; + +// Create const Scenario pointer from ConstantTwistScenario +static const Scenario* ScenarioPointer(const ConstantTwistScenario& scenario) { + return static_cast(&scenario); +} + +void exportScenario() { + // NOTE(frank): Abstract classes need boost::noncopyable + class_("Scenario", no_init); + + // TODO(frank): figure out how to do inheritance + class_("ConstantTwistScenario", + init()) + .def("pose", &Scenario::pose) + .def("omega_b", &Scenario::omega_b) + .def("velocity_n", &Scenario::velocity_n) + .def("acceleration_n", &Scenario::acceleration_n) + .def("navState", &Scenario::navState) + .def("rotation", &Scenario::rotation) + .def("velocity_b", &Scenario::velocity_b) + .def("acceleration_b", &Scenario::acceleration_b); + + // NOTE(frank): https://wiki.python.org/moin/boost.python/CallPolicy + def("ScenarioPointer", &ScenarioPointer, + return_value_policy()); + + class_( + "ScenarioRunner", + init&, + double, const imuBias::ConstantBias&>()) + .def("actualSpecificForce", &ScenarioRunner::actualSpecificForce) + .def("measuredAngularVelocity", &ScenarioRunner::measuredAngularVelocity) + .def("measuredSpecificForce", &ScenarioRunner::measuredSpecificForce) + .def("imuSampleTime", &ScenarioRunner::imuSampleTime, + return_value_policy()) + .def("integrate", &ScenarioRunner::integrate) + .def("predict", &ScenarioRunner::predict) + .def("estimateCovariance", &ScenarioRunner::estimateCovariance); +} diff --git a/python/handwritten/nonlinear/ISAM2.cpp b/python/handwritten/nonlinear/ISAM2.cpp new file mode 100644 index 000000000..f80c5df99 --- /dev/null +++ b/python/handwritten/nonlinear/ISAM2.cpp @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief exports ISAM2 class to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/nonlinear/ISAM2.h" +#include "gtsam/geometry/Pose3.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(update_overloads, ISAM2::update, 0, 7) + +void exportISAM2(){ + +// TODO(Ellon): Export all properties of ISAM2Params +class_("ISAM2Params") + .add_property("relinearize_skip", &ISAM2Params::getRelinearizeSkip, &ISAM2Params::setRelinearizeSkip) + .add_property("enable_relinearization", &ISAM2Params::isEnableRelinearization, &ISAM2Params::setEnableRelinearization) + .add_property("evaluate_non_linear_error", &ISAM2Params::isEvaluateNonlinearError, &ISAM2Params::setEvaluateNonlinearError) + .add_property("factorization", &ISAM2Params::getFactorization, &ISAM2Params::setFactorization) + .add_property("cache_linearized_factors", &ISAM2Params::isCacheLinearizedFactors, &ISAM2Params::setCacheLinearizedFactors) + .add_property("enable_detailed_results", &ISAM2Params::isEnableDetailedResults, &ISAM2Params::setEnableDetailedResults) + .add_property("enable_partial_linearization_check", &ISAM2Params::isEnablePartialRelinearizationCheck, &ISAM2Params::setEnablePartialRelinearizationCheck) + // TODO(Ellon): Check if it works with FastMap; Implement properly if it doesn't. + .add_property("relinearization_threshold", &ISAM2Params::getRelinearizeThreshold, &ISAM2Params::setRelinearizeThreshold) + // TODO(Ellon): Wrap the following setters/getters: + // void setOptimizationParams (OptimizationParams optimizationParams) + // OptimizationParams getOptimizationParams () const + // void setKeyFormatter (KeyFormatter keyFormatter) + // KeyFormatter getKeyFormatter () const + // GaussianFactorGraph::Eliminate getEliminationFunction () const +; + +// TODO(Ellon): Export useful methods/properties of ISAM2Result +class_("ISAM2Result") +; + +// Function pointers for overloads in ISAM2 +Values (ISAM2::*calculateEstimate_0)() const = &ISAM2::calculateEstimate; + +class_("ISAM2") + .def(init()) + // TODO(Ellon): wrap all optional values of update + .def("update",&ISAM2::update, update_overloads()) + .def("calculate_estimate", calculateEstimate_0) + .def("calculate_pose3_estimate", &ISAM2::calculateEstimate, (arg("self"), arg("key")) ) + .def("value_exists", &ISAM2::valueExists) +; + +} \ No newline at end of file diff --git a/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp b/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp new file mode 100644 index 000000000..7c7cdf3cc --- /dev/null +++ b/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -0,0 +1,27 @@ +#include + +#define NO_IMPORT_ARRAY +#include + +#include + +using namespace boost::python; +using namespace gtsam; + +void exportLevenbergMarquardtOptimizer(){ + class_("LevenbergMarquardtParams", init<>()) + .def("setDiagonalDamping", &LevenbergMarquardtParams::setDiagonalDamping) + .def("setlambdaFactor", &LevenbergMarquardtParams::setlambdaFactor) + .def("setlambdaInitial", &LevenbergMarquardtParams::setlambdaInitial) + .def("setlambdaLowerBound", &LevenbergMarquardtParams::setlambdaLowerBound) + .def("setlambdaUpperBound", &LevenbergMarquardtParams::setlambdaUpperBound) + .def("setLogFile", &LevenbergMarquardtParams::setLogFile) + .def("setUseFixedLambdaFactor", &LevenbergMarquardtParams::setUseFixedLambdaFactor) + .def("setVerbosityLM", &LevenbergMarquardtParams::setVerbosityLM) + ; + + class_("LevenbergMarquardtOptimizer", + init()) + .def("optimize", &LevenbergMarquardtOptimizer::optimize, return_value_policy()) + ; +} diff --git a/python/handwritten/nonlinear/NonlinearFactor.cpp b/python/handwritten/nonlinear/NonlinearFactor.cpp new file mode 100644 index 000000000..9a130d8e9 --- /dev/null +++ b/python/handwritten/nonlinear/NonlinearFactor.cpp @@ -0,0 +1,49 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief exports virtual class NonlinearFactor to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/nonlinear/NonlinearFactor.h" + +using namespace boost::python; +using namespace gtsam; + +// Wrap around pure virtual class NonlinearFactor. +// All pure virtual methods should be wrapped. Non-pure may be wrapped if we want to mimic the +// overloading through inheritance in Python. +// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.class_virtual_functions +struct NonlinearFactorCallback : NonlinearFactor, wrapper +{ + double error (const Values & values) const { + return this->get_override("error")(values); + } + size_t dim () const { + return this->get_override("dim")(); + } + boost::shared_ptr linearize(const Values & values) const { + return this->get_override("linearize")(values); + } +}; + +void exportNonlinearFactor(){ + + class_("NonlinearFactor") + ; + +} \ No newline at end of file diff --git a/python/handwritten/nonlinear/NonlinearFactorGraph.cpp b/python/handwritten/nonlinear/NonlinearFactorGraph.cpp new file mode 100644 index 000000000..4ba4ba008 --- /dev/null +++ b/python/handwritten/nonlinear/NonlinearFactorGraph.cpp @@ -0,0 +1,56 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief exports NonlinearFactorGraph class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/nonlinear/NonlinearFactorGraph.h" +#include "gtsam/nonlinear/NonlinearFactor.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, NonlinearFactorGraph::print, 0, 1); + +boost::shared_ptr getNonlinearFactor( + const NonlinearFactorGraph& graph, size_t idx) { + auto p = boost::dynamic_pointer_cast(graph.at(idx)); + if (!p) throw std::runtime_error("No NonlinearFactor at requested index"); + return p; +}; + +void exportNonlinearFactorGraph(){ + + typedef NonlinearFactorGraph::sharedFactor sharedFactor; + + void (NonlinearFactorGraph::*push_back1)(const sharedFactor&) = &NonlinearFactorGraph::push_back; + void (NonlinearFactorGraph::*add1)(const sharedFactor&) = &NonlinearFactorGraph::add; + + class_("NonlinearFactorGraph", init<>()) + .def("size",&NonlinearFactorGraph::size) + .def("push_back", push_back1) + .def("add", add1) + .def("resize", &NonlinearFactorGraph::resize) + .def("empty", &NonlinearFactorGraph::empty) + .def("print", &NonlinearFactorGraph::print, print_overloads(args("s"))) + ; + + def("getNonlinearFactor", getNonlinearFactor); + +} diff --git a/python/handwritten/nonlinear/Values.cpp b/python/handwritten/nonlinear/Values.cpp new file mode 100644 index 000000000..84e82f376 --- /dev/null +++ b/python/handwritten/nonlinear/Values.cpp @@ -0,0 +1,80 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Values class to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/nonlinear/Values.h" +#include "gtsam/geometry/Point2.h" +#include "gtsam/geometry/Rot2.h" +#include "gtsam/geometry/Pose2.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Rot3.h" +#include "gtsam/geometry/Pose3.h" +#include "gtsam/navigation/ImuBias.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Values::print, 0, 1); + +void exportValues(){ + + typedef imuBias::ConstantBias Bias; + + bool (Values::*exists1)(Key) const = &Values::exists; + void (Values::*insert_point2)(Key, const gtsam::Point2&) = &Values::insert; + void (Values::*insert_rot2) (Key, const gtsam::Rot2&) = &Values::insert; + void (Values::*insert_pose2) (Key, const gtsam::Pose2&) = &Values::insert; + void (Values::*insert_point3)(Key, const gtsam::Point3&) = &Values::insert; + void (Values::*insert_rot3) (Key, const gtsam::Rot3&) = &Values::insert; + void (Values::*insert_pose3) (Key, const gtsam::Pose3&) = &Values::insert; + void (Values::*insert_bias) (Key, const Bias&) = &Values::insert; + void (Values::*insert_vector3) (Key, const gtsam::Vector3&) = &Values::insert; + + class_("Values", init<>()) + .def(init()) + .def("clear", &Values::clear) + .def("dim", &Values::dim) + .def("empty", &Values::empty) + .def("equals", &Values::equals) + .def("erase", &Values::erase) + .def("insert_fixed", &Values::insertFixed) + .def("print", &Values::print, print_overloads(args("s"))) + .def("size", &Values::size) + .def("swap", &Values::swap) + .def("insert", insert_point2) + .def("insert", insert_rot2) + .def("insert", insert_pose2) + .def("insert", insert_point3) + .def("insert", insert_rot3) + .def("insert", insert_pose3) + .def("insert", insert_bias) + .def("insert", insert_vector3) + .def("atPoint2", &Values::at, return_value_policy()) + .def("atRot2", &Values::at, return_value_policy()) + .def("atPose2", &Values::at, return_value_policy()) + .def("atPoint3", &Values::at, return_value_policy()) + .def("atRot3", &Values::at, return_value_policy()) + .def("atPose3", &Values::at, return_value_policy()) + .def("atConstantBias", &Values::at, return_value_policy()) + .def("atVector3", &Values::at, return_value_policy()) + .def("exists", exists1) + .def("keys", &Values::keys) + ; +} diff --git a/python/handwritten/slam/BearingFactor.cpp b/python/handwritten/slam/BearingFactor.cpp new file mode 100644 index 000000000..84c67d522 --- /dev/null +++ b/python/handwritten/slam/BearingFactor.cpp @@ -0,0 +1,17 @@ +#include + +#define NO_IMPORT_ARRAY +#include + +#include + +using namespace boost::python; +using namespace gtsam; + +using namespace std; + +template +void exportBearingFactor(const std::string& name){ + class_(name, init<>()) + ; +} \ No newline at end of file diff --git a/python/handwritten/slam/BetweenFactor.cpp b/python/handwritten/slam/BetweenFactor.cpp new file mode 100644 index 000000000..0e0949fb5 --- /dev/null +++ b/python/handwritten/slam/BetweenFactor.cpp @@ -0,0 +1,57 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps BetweenFactor for several values to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/slam/BetweenFactor.h" +#include "gtsam/geometry/Point2.h" +#include "gtsam/geometry/Rot2.h" +#include "gtsam/geometry/Pose2.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Rot3.h" +#include "gtsam/geometry/Pose3.h" + +using namespace boost::python; +using namespace gtsam; + +using namespace std; + +// template +// void exportBetweenFactor(const std::string& name){ +// class_(name, init<>()) +// .def(init()) +// ; +// } + +#define BETWEENFACTOR(T) \ + class_< BetweenFactor, bases, boost::shared_ptr< BetweenFactor > >("BetweenFactor"#T) \ + .def(init()) \ + .def("measured", &BetweenFactor::measured, return_internal_reference<>()) \ +; + +void exportBetweenFactors() +{ + BETWEENFACTOR(Point2) + BETWEENFACTOR(Rot2) + BETWEENFACTOR(Pose2) + BETWEENFACTOR(Point3) + BETWEENFACTOR(Rot3) + BETWEENFACTOR(Pose3) +} diff --git a/python/handwritten/slam/GenericProjectionFactor.cpp b/python/handwritten/slam/GenericProjectionFactor.cpp new file mode 100644 index 000000000..dd932ccd4 --- /dev/null +++ b/python/handwritten/slam/GenericProjectionFactor.cpp @@ -0,0 +1,54 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps GenericProjectionFactor for several values to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/slam/ProjectionFactor.h" +#include "gtsam/geometry/Pose3.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Cal3_S2.h" + +using namespace boost::python; +using namespace gtsam; + +using namespace std; + +typedef GenericProjectionFactor GenericProjectionFactorCal3_S2; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, GenericProjectionFactorCal3_S2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, GenericProjectionFactorCal3_S2::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(evaluateError_overloads, GenericProjectionFactorCal3_S2::evaluateError, 2, 4) + +void exportGenericProjectionFactor() +{ + + class_ >("GenericProjectionFactorCal3_S2", init<>()) + .def(init &, optional >()) + .def(init &, bool, bool, optional >()) + .def("print", &GenericProjectionFactorCal3_S2::print, print_overloads(args("s"))) + .def("equals", &GenericProjectionFactorCal3_S2::equals, equals_overloads(args("q","tol"))) + .def("evaluate_error", &GenericProjectionFactorCal3_S2::evaluateError, evaluateError_overloads()) + .def("measured", &GenericProjectionFactorCal3_S2::measured, return_value_policy()) + // TODO(Ellon): Find the right return policy when returning a 'const shared_ptr<...> &' + // .def("calibration", &GenericProjectionFactorCal3_S2::calibration, return_value_policy()) + .def("verbose_cheirality", &GenericProjectionFactorCal3_S2::verboseCheirality) + .def("throw_cheirality", &GenericProjectionFactorCal3_S2::throwCheirality) + ; + +} diff --git a/python/handwritten/slam/PriorFactor.cpp b/python/handwritten/slam/PriorFactor.cpp new file mode 100644 index 000000000..3ada91f43 --- /dev/null +++ b/python/handwritten/slam/PriorFactor.cpp @@ -0,0 +1,58 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps PriorFactor for several values to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/slam/PriorFactor.h" +#include "gtsam/geometry/Point2.h" +#include "gtsam/geometry/Rot2.h" +#include "gtsam/geometry/Pose2.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Rot3.h" +#include "gtsam/geometry/Pose3.h" + +using namespace boost::python; +using namespace gtsam; + +using namespace std; + +// template< class FACTOR, class VALUE > +// void exportPriorFactor(const std::string& name){ +// class_< FACTOR >(name.c_str(), init<>()) +// .def(init< Key, VALUE&, SharedNoiseModel >()) +// ; +// } + +#define PRIORFACTOR(VALUE) \ + class_< PriorFactor, bases, boost::shared_ptr< PriorFactor > >("PriorFactor"#VALUE) \ + .def(init()) \ + .def("prior", &PriorFactor::prior, return_internal_reference<>()) \ +; + +void exportPriorFactors() +{ + PRIORFACTOR(Point2) + PRIORFACTOR(Rot2) + PRIORFACTOR(Pose2) + PRIORFACTOR(Point3) + PRIORFACTOR(Rot3) + PRIORFACTOR(Pose3) + PRIORFACTOR(Vector3) +} diff --git a/python/handwritten/utils/NumpyEigen.cpp b/python/handwritten/utils/NumpyEigen.cpp new file mode 100644 index 000000000..d7cebe7ad --- /dev/null +++ b/python/handwritten/utils/NumpyEigen.cpp @@ -0,0 +1,54 @@ + /* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief register conversion matrix between numpy and Eigen + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/base/Matrix.h" +#include "gtsam/base/Vector.h" + +using namespace boost::python; +using namespace gtsam; + +void registerNumpyEigenConversions() +{ + // NOTE: import array should be called only in the cpp defining the module + // import_array(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + +} diff --git a/python/setup.py.in b/python/setup.py.in new file mode 100644 index 000000000..d3b5fcde4 --- /dev/null +++ b/python/setup.py.in @@ -0,0 +1,15 @@ +from distutils.core import setup + +setup(name='gtsam', + version='${GTSAM_VERSION_STRING}', + description='GTSAM Python wrapper', + license = "BSD", + author='Frank Dellaert et. al', + author_email='frank.dellaert@gatech.edu', + maintainer_email='gtsam@lists.gatech.edu', + url='https://collab.cc.gatech.edu/borg/gtsam', + package_dir={ '': '${CMAKE_CURRENT_SOURCE_DIR}' }, + packages=['gtsam', 'gtsam_utils', 'gtsam_examples', 'gtsam_tests'], + #package_data={'gtsam' : ['_libgtsam_python.so']}, # location of .so file is relative to package_dir + data_files=[('${PY_INSTALL_FOLDER}/gtsam/', ['gtsam/_libgtsam_python.so'])], # location of .so file relative to setup.py + )