diff --git a/.cproject b/.cproject index b3709d422..ac9b166ec 100644 --- a/.cproject +++ b/.cproject @@ -1,7 +1,5 @@ - - - + @@ -486,6 +484,341 @@ + + 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 + -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 + testGaussianFactor.run + true + true + true + + + make + -j4 + testImuFactor.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + 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 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + wrap + 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 + check + true + true + true + + + make + -j2 + tests/testLieConfig.run + true + true + true + + + make + -j4 + testSimilarity3.run + true + true + true + + + make + -j5 + testInvDepthCamera3.run + true + true + true + + + make + -j5 + testTriangulation.run + true + true + true + + + make + -j4 + testEvent.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + all + true + false + true + + + make + -j5 + check + true + false + true + make -j5 @@ -510,38 +843,6 @@ true true - - make - -j5 - SFMExampleExpressions.run - true - true - true - - - make - -j5 - Pose2SLAMExampleExpressions.run - true - true - true - - - make - -j5 - testInvDepthCamera3.run - true - true - true - - - make - -j5 - testTriangulation.run - true - true - true - make -j2 @@ -566,7 +867,183 @@ true true - + + make + -j5 + testCal3Bundler.run + true + true + true + + + make + -j5 + testCal3DS2.run + true + true + true + + + make + -j5 + testCalibratedCamera.run + true + true + true + + + make + -j5 + testEssentialMatrix.run + true + true + true + + + make + -j1 VERBOSE=1 + testHomography2.run + true + false + true + + + make + -j5 + testPinholeCamera.run + true + true + true + + + make + -j5 + testPoint2.run + true + true + true + + + make + -j5 + testPoint3.run + true + true + true + + + make + -j5 + testPose2.run + true + true + true + + + make + -j5 + testPose3.run + true + true + true + + + make + -j5 + testRot3M.run + true + true + true + + + make + -j5 + testSphere2.run + true + true + true + + + make + -j5 + testStereoCamera.run + true + true + true + + + make + -j5 + testCal3Unified.run + true + true + true + + + make + -j5 + testRot2.run + true + true + true + + + make + -j5 + testRot3Q.run + true + true + true + + + make + -j5 + testRot3.run + true + true + true + + + make + -j4 + testSO3.run + true + true + true + + + make + -j4 + testQuaternion.run + true + true + true + + + make + -j4 + testOrientedPlane3.run + true + true + true + + + make + -j4 + testPinholePose.run + true + true + true + + + make + -j4 + testCyclic.run + true + true + true + + make -j2 all @@ -574,7 +1051,7 @@ true true - + make -j2 clean @@ -582,137 +1059,23 @@ 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 + clean all true true true - + make -j2 - testISAM.run + install 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 - true - true - true - - + make -j2 clean @@ -848,186 +1211,159 @@ true true - + make - -j5 - testGaussianFactorGraphUnordered.run + -j4 + testSmartStereoProjectionPoseFactor.run true true true - + make - -j5 - testGaussianBayesNetUnordered.run + -j4 + testTOAFactor.run true true true - + make - -j5 - testGaussianConditional.run + -j2 + all true true true - + make - -j5 - testGaussianDensity.run + -j2 + check 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 - timeCameraExpression.run - true - true - true - - - make - -j5 - timeOneCameraExpression.run - true - true - true - - - make - -j5 - timeSFMExpressions.run - true - true - true - - - make - -j5 - timeAdaptAutoDiff.run - true - true - true - - - make - -j5 - 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 + -j2 clean true true true - + make - -j5 - all + -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 @@ -1122,471 +1458,112 @@ 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 - true - true - 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 - 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 + testAntiFactor.run true true true - + make -j5 - testDSF.run + testPriorFactor.run true true true - + make -j5 - testDSFMap.run + testDataset.run true true true - + make -j5 - testDSFVector.run + testEssentialMatrixFactor.run true true true - + make -j5 - testFixedVector.run + testGeneralSFMFactor_Cal3Bundler.run true true true - + make -j5 - testEliminationTree.run + testGeneralSFMFactor.run true true true - + make -j5 - testInference.run + testProjectionFactor.run true true true - + make -j5 - testKey.run + testRotateFactor.run true true true - + make - -j1 - testSymbolicBayesTree.run - true - false - true - - - make - -j1 - testSymbolicSequentialSolver.run - true - false - true - - - make - -j2 - check + -j5 + testPoseRotationPrior.run true true true - + make - -j2 - tests/testLieConfig.run + -j5 + testImplicitSchurFactor.run + true + true + true + + + make + -j4 + testRangeFactor.run + true + true + true + + + make + -j4 + testOrientedPlane3Factor.run + true + true + true + + + make + -j4 + testSmartProjectionPoseFactor.run true true true @@ -1792,6 +1769,7 @@ cpack + -G DEB true false @@ -1799,6 +1777,7 @@ cpack + -G RPM true false @@ -1806,6 +1785,7 @@ cpack + -G TGZ true false @@ -1813,6 +1793,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -1986,7 +1967,15 @@ true true - + + make + -j2 VERBOSE=1 + check.navigation + true + false + true + + make -j2 check @@ -1994,38 +1983,54 @@ true true - + make - -j2 - clean - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - all - true - true - true - - - cmake - .. + tests/testGaussianISAM2 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 testGaussianFactor.run @@ -2033,362 +2038,10 @@ true true - + make -j5 - testCal3Bundler.run - true - true - true - - - make - -j5 - testCal3DS2.run - true - true - true - - - make - -j5 - testCalibratedCamera.run - true - true - true - - - make - -j5 - testEssentialMatrix.run - true - true - true - - - make - -j1 VERBOSE=1 - testHomography2.run - true - false - true - - - make - -j5 - testPinholeCamera.run - true - true - true - - - make - -j5 - testPoint2.run - true - true - true - - - make - -j5 - testPoint3.run - true - true - true - - - make - -j5 - testPose2.run - true - true - true - - - make - -j5 - testPose3.run - true - true - true - - - make - -j5 - testRot3M.run - true - true - true - - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testStereoCamera.run - true - true - true - - - make - -j5 - testCal3Unified.run - true - true - true - - - make - -j5 - testRot2.run - true - true - true - - - make - -j5 - testRot3Q.run - true - true - true - - - make - -j5 - testRot3.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - all - true - false - true - - - make - -j5 - check - true - 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 - -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 - -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 - -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 + testParticleFactor.run true true true @@ -2441,340 +2094,151 @@ true true - + make -j2 - vSFMexample.run + all 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 - -j5 - check.tests - true - true - true - - - make - -j2 - timeGaussianFactorGraph.run - true - true - true - - - make - -j5 - testMarginals.run - true - true - true - - - make - -j5 - testGaussianISAM2.run - true - true - true - - - make - -j5 - testSymbolicFactorGraphB.run - true - true - true - - - make - -j2 - timeSequentialOnDataset.run - true - true - true - - - make - -j5 - testGradientDescentOptimizer.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - testNonlinearOptimizer.run - true - true - true - - - make - -j2 - testGaussianBayesNet.run - true - true - true - - - make - -j2 - testNonlinearISAM.run - true - true - true - - - make - -j2 - testNonlinearEquality.run - true - true - true - - - make - -j2 - testExtendedKalmanFilter.run - true - true - true - - - make - -j5 - timing.tests - true - true - true - - - make - -j5 - testNonlinearFactor.run - true - true - true - - - make - -j5 clean true true true - + make - -j5 - testGaussianJunctionTreeB.run - true - true - true - - - make - testGraph.run + -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 - false + true true - + make - testSymbolicBayesNetB.run + -j2 + testKey.run + true + true + true + + + make + -j2 + testOrdering.run + true + true + true + + + make + + testSymbolicBayesNet.run true false true - + make - -j5 - testGaussianISAM.run + + tests/testSymbolicFactor.run true - true + false true - + make - -j5 - testDoglegOptimizer.run + + testSymbolicFactorGraph.run true - true + false true - - make - -j5 - testNonlinearFactorGraph.run - true - true - true - - - make - -j5 - testIterative.run - true - true - true - - - make - -j5 - testSubgraphSolver.run - true - true - true - - - make - -j5 - testGaussianFactorGraphB.run - true - true - true - - - make - -j5 - testSummarization.run - true - true - true - - - make - -j5 - testManifold.run - true - true - true - - - make - -j5 - testParticleFactor.run - true - true - true - - - make - -j5 - testExpressionFactor.run - true - true - true - - - make - -j5 - testExpression.run - true - true - true - - - make - -j5 - testExpressionMeta.run - true - true - true - - - make - -j5 - testAdaptAutoDiff.run - true - true - true - - - make - -j5 - testCallRecord.run - true - true - true - - + make -j2 - testGaussianFactor.run + timeSymbolMaps.run true true true - + + make + + tests/testBayesTree + true + false + true + + make -j2 - install + check true true true - + + make + -j2 + timeCalibratedCamera.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + make -j2 clean @@ -2782,114 +2246,18 @@ true true - + make -j2 - all + tests/testPose2.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 + tests/testPose3.run true true true @@ -3070,110 +2438,30 @@ true true - - make - -j5 - testLago.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j5 - testOrdering.run - true - true - true - - - make - -j5 - testValues.run - true - true - true - - - make - -j5 - testWhiteNoiseFactor.run - true - true - true - - + make -j4 - testImuFactor.run + Pose2SLAMExampleExpressions.run true true true - + make - -j5 - timeCalibratedCamera.run + -j4 + SFMExampleExpressions.run true true true - + make - -j5 - timePinholeCamera.run + -j4 + SFMExampleExpressions_bal.run true true true - - make - -j5 - timeStereoCamera.run - true - true - true - - - make - -j5 - timeLago.run - true - true - true - - - make - -j5 - timePose3.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - - tests/testGaussianISAM2 - true - false - true - make -j2 @@ -3270,10 +2558,922 @@ true true - + + make + -j1 + testDiscreteBayesTree.run + true + false + true + + make -j5 - wrap + 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 + 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 + 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 + -j5 + testLago.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run + true + true + true + + + make + -j5 + testOrdering.run + true + true + true + + + make + -j5 + testValues.run + true + true + true + + + make + -j5 + testWhiteNoiseFactor.run + true + true + true + + + make + -j4 + testExpression.run + true + true + true + + + make + -j4 + testAdaptAutoDiff.run + true + true + true + + + make + -j4 + testCallRecord.run + true + true + true + + + make + -j4 + testExpressionFactor.run + true + true + true + + + make + -j4 + testExecutionTrace.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 + -j5 + timeCalibratedCamera.run + true + true + true + + + make + -j5 + timePinholeCamera.run + true + true + true + + + make + -j5 + timeStereoCamera.run + true + true + true + + + make + -j5 + timeLago.run + true + true + true + + + make + -j5 + timePose3.run + true + true + true + + + make + -j4 + timeAdaptAutoDiff.run + true + true + true + + + make + -j4 + timeCameraExpression.run + true + true + true + + + make + -j4 + timeOneCameraExpression.run + true + true + true + + + make + -j4 + timeSFMExpressions.run + true + true + true + + + make + -j4 + timeIncremental.run + true + true + true + + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + + + make + -j2 + all + 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 + -j2 + all + 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 + true + true + true + + + make + -j5 + check.tests + true + true + true + + + make + -j2 + timeGaussianFactorGraph.run + true + true + true + + + make + -j5 + testMarginals.run + true + true + true + + + make + -j5 + testGaussianISAM2.run + true + true + true + + + make + -j5 + testSymbolicFactorGraphB.run + true + true + true + + + make + -j2 + timeSequentialOnDataset.run + true + true + true + + + make + -j5 + testGradientDescentOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + testNonlinearOptimizer.run + true + true + true + + + make + -j2 + testGaussianBayesNet.run + true + true + true + + + make + -j2 + testNonlinearISAM.run + true + true + true + + + make + -j2 + testNonlinearEquality.run + true + true + true + + + make + -j2 + testExtendedKalmanFilter.run + true + true + true + + + make + -j5 + timing.tests + true + true + true + + + make + -j5 + testNonlinearFactor.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + testGaussianJunctionTreeB.run + true + true + true + + + make + + testGraph.run + true + false + true + + + make + + testJunctionTree.run + true + false + true + + + make + + testSymbolicBayesNetB.run + true + false + true + + + make + -j5 + testGaussianISAM.run + true + true + true + + + make + -j5 + testDoglegOptimizer.run + true + true + true + + + make + -j5 + testNonlinearFactorGraph.run + true + true + true + + + make + -j5 + testIterative.run + true + true + true + + + make + -j5 + testSubgraphSolver.run + true + true + true + + + make + -j5 + testGaussianFactorGraphB.run + true + true + true + + + make + -j5 + testSummarization.run + true + true + true + + + make + -j5 + testManifold.run + true + true + true + + + make + -j4 + testLie.run + true + true + true + + + make + -j2 + vSFMexample.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + testVSLAMGraph true true true diff --git a/.gitignore b/.gitignore index 60633adaf..d46bddd10 100644 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,8 @@ /build* -/doc* *.pyc *.DS_Store /examples/Data/dubrovnik-3-7-pre-rewritten.txt /examples/Data/pose2example-rewritten.txt /examples/Data/pose3example-rewritten.txt +*.txt.user +*.txt.user.6d59f0c diff --git a/.settings/.gitignore b/.settings/.gitignore new file mode 100644 index 000000000..faa6d2991 --- /dev/null +++ b/.settings/.gitignore @@ -0,0 +1 @@ +/org.eclipse.cdt.codan.core.prefs diff --git a/.settings/org.eclipse.cdt.core.prefs b/.settings/org.eclipse.cdt.core.prefs new file mode 100644 index 000000000..b700d0a81 --- /dev/null +++ b/.settings/org.eclipse.cdt.core.prefs @@ -0,0 +1,6 @@ +eclipse.preferences.version=1 +environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/delimiter=\: +environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/operation=append +environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/value=$PATH\:/opt/local/bin +environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/append=true +environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/appendContributed=true diff --git a/CMakeLists.txt b/CMakeLists.txt index bbf4ebb53..6e9aa0009 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -59,7 +59,7 @@ option(GTSAM_BUILD_STATIC_LIBRARY "Build a static gtsam library, instead option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF) option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF) -option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) +option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) 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) @@ -192,37 +192,40 @@ endif() ############################################################################### # Option for using system Eigen or GTSAM-bundled Eigen -### Disabled until our patches are included in Eigen ### +### These patches only affect usage of MKL. If you want to enable MKL, you *must* +### use our patched version of Eigen ### See: http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection) ### http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code) -### http://eigen.tuxfamily.org/bz/show_bug.cgi?id=716 (Improved comma initialization) -# option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF) -set(GTSAM_USE_SYSTEM_EIGEN OFF) +option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF) # Switch for using system Eigen or GTSAM-bundled Eigen if(GTSAM_USE_SYSTEM_EIGEN) - # Use generic Eigen include paths e.g. - set(GTSAM_EIGEN_INCLUDE_PREFIX "") - find_package(Eigen3 REQUIRED) include_directories(AFTER "${EIGEN3_INCLUDE_DIR}") -else() - # Use bundled Eigen include paths e.g. - set(GTSAM_EIGEN_INCLUDE_PREFIX "gtsam/3rdparty/Eigen/") + # Use generic Eigen include paths e.g. + set(GTSAM_EIGEN_INCLUDE_PREFIX "${EIGEN3_INCLUDE_DIR}") + + # check if MKL is also enabled - can have one or the other, but not both! + if(EIGEN_USE_MKL_ALL) + message(FATAL_ERROR "MKL cannot be used together with system-installed Eigen, as MKL support relies on patches which are not yet in the system-installed Eigen. Disable either GTSAM_USE_SYSTEM_EIGEN or GTSAM_WITH_EIGEN_MKL") + endif() +else() + # Use bundled Eigen include path. # Clear any variables set by FindEigen3 if(EIGEN3_INCLUDE_DIR) set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE) endif() + # Add the bundled version of eigen to the include path so that it can still be included + # with #include + include_directories(BEFORE "gtsam/3rdparty/Eigen/") + + # set full path to be used by external projects + # this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in + set(GTSAM_EIGEN_INCLUDE_PREFIX "${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen/") + endif() -# Write Eigen include file with the paths for either the system Eigen or the GTSAM-bundled Eigen -configure_file(gtsam/3rdparty/gtsam_eigen_includes.h.in gtsam/3rdparty/gtsam_eigen_includes.h) - -# Install the configuration file for Eigen -install(FILES ${PROJECT_BINARY_DIR}/gtsam/3rdparty/gtsam_eigen_includes.h DESTINATION include/gtsam/3rdparty) - - ############################################################################### # Global compile options @@ -269,18 +272,18 @@ include_directories(BEFORE ${Boost_INCLUDE_DIR}) # paths so that the compiler uses GTSAM headers in our source directory instead # of any previously installed GTSAM headers. include_directories(BEFORE - gtsam/3rdparty/UFconfig + gtsam/3rdparty/UFconfig gtsam/3rdparty/CCOLAMD/Include - gtsam/3rdparty/metis-5.1.0/include - gtsam/3rdparty/metis-5.1.0/libmetis - gtsam/3rdparty/metis-5.1.0/GKlib + gtsam/3rdparty/metis/include + gtsam/3rdparty/metis/libmetis + gtsam/3rdparty/metis/GKlib ${PROJECT_SOURCE_DIR} ${PROJECT_BINARY_DIR} # So we can include generated config header files CppUnitLite) if(MSVC) add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS) - add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344) # Disable non-DLL-exported base class and other warnings + add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings endif() # GCC 4.8+ complains about local typedefs which we use for shared_ptr etc. @@ -331,6 +334,7 @@ endif(GTSAM_BUILD_UNSTABLE) GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in") export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake) + # Check for doxygen availability - optional dependency find_package(Doxygen) @@ -389,6 +393,11 @@ if(NOT MSVC AND NOT XCODE_VERSION) message(STATUS " C compilation flags : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${cmake_build_type_toupper}}") message(STATUS " C++ compilation flags : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${cmake_build_type_toupper}}") endif() +if(GTSAM_USE_SYSTEM_EIGEN) + message(STATUS " Use System Eigen : Yes") +else() + message(STATUS " Use System Eigen : No") +endif() if(GTSAM_USE_TBB) message(STATUS " Use Intel TBB : Yes") elseif(TBB_FOUND) @@ -424,6 +433,7 @@ message(STATUS " CPack Generator : ${CPACK_GENERATOR}") message(STATUS "GTSAM flags ") print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ") print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") +print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") message(STATUS "MATLAB toolbox flags ") diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index 3ea6a6318..ff1f1b692 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -80,6 +80,12 @@ protected: testGroup##testName##Instance; \ void testGroup##testName##Test::run (TestResult& result_) +/** + * Use this to disable unwanted tests without commenting them out. + */ +#define TEST_DISABLED(testGroup, testName)\ + void testGroup##testName##Test(TestResult& result_, const std::string& name_) + /* * Convention for tests: * - "EXPECT" is a test that will not end execution of the series of tests diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md new file mode 100644 index 000000000..3c10451be --- /dev/null +++ b/GTSAM-Concepts.md @@ -0,0 +1,206 @@ +GTSAM Concepts +============== + +As discussed in [Generic Programming Techniques](http://www.boost.org/community/generic_programming.html), concepts define + +* associated types +* valid expressions, like functions and values +* invariants +* complexity guarantees + +Below we discuss the most important concepts use in GTSAM, and after that we discuss how they are implemented/used/enforced. + + +Manifold +-------- + +To optimize over continuous types, we assume they are manifolds. This is central to GTSAM and hence discussed in some more detail below. + +[Manifolds](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) and [charts](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) are intimately linked concepts. We are only interested here in [differentiable manifolds](http://en.wikipedia.org/wiki/Differentiable_manifold#Definition), continuous spaces that can be locally approximated *at any point* using a local vector space, called the [tangent space](http://en.wikipedia.org/wiki/Tangent_space). A *chart* is an invertible map from the manifold to that tangent space. + +In GTSAM, all properties and operations needed to use a type must be defined through template specialization of the struct `gtsam::traits`. Concept checks are used to check that all required functions are implemented. +In detail, we ask the following are defined in the traits object: + +* values: + * `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimenionality is only defined at runtime, by specifying the value -1. +* types: + * `TangentVector`, type that lives in tangent space. This will almost always be an `Eigen::Matrix`. + * `ChartJacobian`, a typedef for `OptionalJacobian`. + * `ManifoldType`, a pointer back to the type. + * `structure_category`, a tag type that defines what requirements the type fulfills, and therefore what requirements this traits class must fulfill. It should be defined to be one of the following: + * `gtsam::traits::manifold_tag` -- Everything in this list is expected + * `gtsam::traits::group_tag` -- The functions defined under **Groups** below. + * `gtsam::traits::lie_group_tag` -- Everything in this list is expected, plus the functions defined under **Groups**, and **Lie Groups** below. + * `gtsam::traits::vector_space_tag` -- Everything in this list is expected, plus the functions defined under **Groups**, and **Lie Groups** below. +* valid expressions: + * `size_t dim = traits::getDimension(p);` static function should be defined. This is mostly useful if the size is not known at compile time. + * `v = traits::Local(p,q)`, the chart, from manifold to tangent space, think of it as *q (-) p*, where *p* and *q* are elements of the manifold and the result, *v* is an element of the vector space. + * `p = traits::Retract(p,v)`, the inverse chart, from tangent space to manifold, think of it as *p (+) v*, where *p* is an element of the manifold and the result, *v* is an element of the vector space. +* invariants + * `Retract(p, Local(p,q)) == q` + * `Local(p, Retract(p, v)) == v` + +Group +----- +A [group]("http://en.wikipedia.org/wiki/Group_(mathematics)"") should be well known from grade school :-), and provides a type with a composition operation that is closed, associative, has an identity element, and an inverse for each element. The following should be added to the traits class for a group: + +* valid expressions: + * `r = traits::Compose(p,q)`, where *p*, *q*, and *r* are elements of the manifold. + * `q = traits::Inverse(p)`, where *p* and*q* are elements of the manifold. + * `r = traits::Between(p,q)`, where *p*, *q*, and *r* are elements of the manifold. +* static members: + * `traits::Identity`, a static const member that represents the group's identity element. +* invariants: + * `Compose(p,Inverse(p)) == Identity` + * `Compose(p,Between(p,q)) == q` + * `Between(p,q) == Compose(Inverse(p),q)` +The `gtsam::group::traits` namespace defines the following: +* values: + * `traits::Identity` -- The identity element for this group stored as a static const. + * `traits::group_flavor` -- the flavor of this group's `compose()` operator, either: + * `gtsam::traits::group_multiplicative_tag` for multiplicative operator syntax ,or + * `gtsam::traits::group_additive_tag` for additive operator syntax. + +We do *not* at this time support more than one composition operator per type. Although mathematically possible, it is hardly ever needed, and the machinery to support it would be burdensome and counter-intuitive. + +Also, a type should provide either multiplication or addition operators depending on the flavor of the operation. To distinguish between the two, we will use a tag (see below). + +Lie Group +--------- + +A Lie group is both a manifold *and* a group. Hence, a LIE_GROUP type should implements both MANIFOLD and GROUP concepts. +However, we now also need to be able to evaluate the derivatives of compose and inverse. +Hence, we have the following extra valid static functions defined in the struct `gtsam::traits`: + +* `r = traits::Compose(p,q,Hq,Hp)` +* `q = traits::Inverse(p,Hp)` +* `r = traits::Between(p,q,Hq,H2p)` + +where above the *H* arguments stand for optional Jacobian arguments. +That makes it possible to create factors implementing priors (PriorFactor) or relations between two instances of a Lie group type (BetweenFactor). + +In addition, a Lie group has a Lie algebra, which affords two extra valid expressions: + +* `v = traits::Logmap(p,Hp)`, the log map, with optional Jacobian +* `p = traits::Expmap(v,Hv)`, the exponential map, with optional Jacobian + +Note that in the Lie group case, the usual valid expressions for Retract and Local can be generated automatically, e.g. + +``` + T Retract(p,v,Hp,Hv) { + T q = Expmap(v,Hqv); + T r = Compose(p,q,Hrp,Hrq); + Hv = Hrq * Hqv; // chain rule + return r; + } +``` + +For Lie groups, the `exponential map` above is the most obvious mapping: it +associates straight lines in the tangent space with geodesics on the manifold +(and it's inverse, the log map). However, there are two cases in which we deviate from this: + +However, the exponential map is unnecessarily expensive for use in optimization. Hence, in GTSAM there is the option to provide a cheaper chart by means of the `ChartAtOrigin` struct in a class. This is done for *SE(2)*, *SO(3)* and *SE(3)* (see `Pose2`, `Rot3`, `Pose3`) + +Most Lie groups we care about are *Matrix groups*, continuous sub-groups of *GL(n)*, the group of *n x n* invertible matrices. In this case, a lot of the derivatives calculations needed can be standardized, and this is done by the `LieGroup` superclass. You only need to provide an `AdjointMap` method. + +Vector Space +------------ + +While vector spaces are in principle also manifolds, it is overkill to think about charts etc. Really, we should simply think about vector addition and subtraction. I.e.where + + * `Identity == 0` + * `Inverse(p) == -p` + * `Compose(p,q) == p+q` + * `Between(p,q) == q-p` + * `Local(q) == p-q` + * `Retract(v) == p+v` + +This considerably simplifies certain operations. A `VectorSpace` superclass is available to implement the traits. Types that are vector space models include `Matrix`, `Vector`, any fixed or dynamic Eigen Matrix, `Point2`, and `Point3`. + +Testable +-------- +Unit tests heavily depend on the following two functions being defined for all types that need to be tested: + +* valid expressions: + * `Print(p,s)` where s is an optional string + * `Equals(p,q,tol)` where tol is an optional (double) tolerance + +Implementation +============== + +GTSAM Types start with Uppercase, e.g., `gtsam::Point2`, and are models of the +TESTABLE, MANIFOLD, GROUP, LIE_GROUP, and VECTOR_SPACE concepts. + +`gtsam::traits` is our way to associate these concepts with types, +and we also define a limited number of `gtsam::tags` to select the correct implementation +of certain functions at compile time (tag dispatching). + +Traits +------ + +However, a base class is not a good way to implement/check the other concepts, as we would like these +to apply equally well to types that are outside GTSAM control, e.g., `Eigen::VectorXd`. This is where +[traits](http://www.boost.org/doc/libs/1_57_0/libs/type_traits/doc/html/boost_typetraits/background.html) come in. + +We use Eigen-style or STL-style traits, that define *many* properties at once. + +Note that not everything that makes a concept is defined by traits. Valid expressions such as traits::Compose are +defined simply as static functions within the traits class. +Finally, for GTSAM types, it is perfectly acceptable (and even desired) to define associated types as internal types, +rather than having to use traits internally. + +Concept Checks +-------------- + +Boost provides a nice way to check whether a given type satisfies a concept. For example, the following + + BOOST_CONCEPT_ASSERT(IsVectorSpace) + +asserts that Point2 indeed is a model for the VectorSpace concept. + +Future Concepts +=============== + + +Group Action +------------ + +Group actions are concepts in and of themselves that can be concept checked (see below). +In particular, a group can *act* on another space. +For example, the [cyclic group of order 6](http://en.wikipedia.org/wiki/Cyclic_group) can rotate 2D vectors around the origin: + + q = R(i)*p + where R(i) = R(60)^i, where R(60) rotates by 60 degrees + +Hence, we formalize by the following extension of the concept: + +* valid expressions: + * `q = traits::Act(g,p)`, for some instance, *p*, of a space *S*, that can be acted upon by the group element *g* to produce *q* in *S*. + * `q = traits::Act(g,p,Hp)`, if the space acted upon is a continuous differentiable manifold. * + +In the latter case, if *S* is an n-dimensional manifold, *Hp* is an output argument that should be +filled with the *nxn* Jacobian matrix of the action with respect to a change in *p*. It typically depends +on the group element *g*, but in most common example will *not* depend on the value of *p*. For example, in +the cyclic group example above, we simply have + + Hp = R(i) + +Note there is no derivative of the action with respect to a change in g. That will only be defined +for Lie groups, which we introduce now. + + +Lie Group Action +---------------- + +When a Lie group acts on a space, we have two derivatives to care about: + + * `gtasm::manifold::traits::act(g,p,Hg,Hp)`, if the space acted upon is a continuous differentiable manifold. + +An example is a *similarity transform* in 3D, which can act on 3D space, like + + q = s*R*p + t + +Note that again the derivative in *p*, *Hp* is simply *s R*, which depends on *g* but not on *p*. +The derivative in *g*, *Hg*, is in general more complex. + +For now, we won't care about Lie groups acting on non-manifolds. diff --git a/README.md b/README.md index 460f51bf3..679af5a2f 100644 --- a/README.md +++ b/README.md @@ -1,47 +1,51 @@ -README - Georgia Tech Smoothing and Mapping library -=================================================== +README - Georgia Tech Smoothing and Mapping library +=================================================== + +What is GTSAM? +-------------- + +GTSAM is a library of C++ classes that implement smoothing and +mapping (SAM) in robotics and vision, using factor graphs and Bayes +networks as the underlying computing paradigm rather than sparse +matrices. + +On top of the C++ library, GTSAM includes a MATLAB interface (enable +GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface +is under development. + +Quickstart +---------- + +In the root library folder execute: + +``` +#!bash +$ mkdir build +$ cd build +$ cmake .. +$ make check (optional, runs unit tests) +$ make install +``` + +Prerequisites: + +- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`) +- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`) + +Optional prerequisites - used automatically if findable by CMake: + +- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`) +- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) + +Additional Information +---------------------- + +Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. + +See the [`INSTALL`](INSTALL) file for more detailed installation instructions. + +GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files. + +Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM. -What is GTSAM? --------------- - -GTSAM is a library of C++ classes that implement smoothing and -mapping (SAM) in robotics and vision, using factor graphs and Bayes -networks as the underlying computing paradigm rather than sparse -matrices. - -On top of the C++ library, GTSAM includes a MATLAB interface (enable -GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface -is under development. - -Quickstart ----------- - -In the root library folder execute: - -``` -#!bash -$ mkdir build -$ cd build -$ cmake .. -$ make check (optional, runs unit tests) -$ make install -``` - -Prerequisites: - -- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`) -- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`) - -Optional prerequisites - used automatically if findable by CMake: - -- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`) -- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) - -Additional Information ----------------------- - -See the [`INSTALL`](https://bitbucket.org/gtborg/gtsam/src/develop/INSTALL) file for more detailed installation instructions. - -GTSAM is open source under the BSD license, see the [`LICENSE`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE) and [`LICENSE.BSD`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE.BSD) files. - -Please see the [`examples/`](https://bitbucket.org/gtborg/gtsam/src/develop/examples) directory and the [`USAGE`](https://bitbucket.org/gtborg/gtsam/src/develop/USAGE) file for examples on how to use GTSAM. +GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS). \ No newline at end of file diff --git a/THANKS b/THANKS index 0d5d9db30..9c06a5d28 100644 --- a/THANKS +++ b/THANKS @@ -1,20 +1,43 @@ -GTSAM was made possible by the efforts of many collaborators at Georgia Tech +GTSAM was made possible by the efforts of many collaborators at Georgia Tech, listed below with their current afffiliation, if they left Tech: -Doru Balcan -Chris Beall -Alex Cunningham -Alireza Fathi -Eohan George -Viorela Ila -Yong-Dian Jian -Michael Kaess -Kai Ni -Carlos Nieto -Duy-Nguyen -Manohar Paluri -Christian Potthast -Richard Roberts -Grant Schindler +* Sungtae An +* Doru Balcan, Bank of America +* Chris Beall +* Luca Carlone +* Alex Cunningham, U Michigan +* Jing Dong +* Alireza Fathi, Stanford +* Eohan George +* Alex Hagiopol +* Viorela Ila, Czeck Republic +* Vadim Indelman, the Technion +* David Jensen, GTRI +* Yong-Dian Jian, Baidu +* Michael Kaess, Carnegie Mellon +* Zhaoyang Lv +* Andrew Melim, Oculus Rift +* Kai Ni, Baidu +* Carlos Nieto +* Duy-Nguyen Ta +* Manohar Paluri, Facebook +* Christian Potthast, USC +* Richard Roberts, Google X +* Grant Schindler, Consultant +* Natesh Srinivasan +* Alex Trevor +* Stephen Williams, BossaNova + +at ETH, Zurich + +* Paul Furgale +* Mike Bosse +* Hannes Sommer +* Thomas Schneider + +at Uni Zurich: + +* Christian Forster Many thanks for your hard work!!!! + Frank Dellaert diff --git a/USAGE b/USAGE.md similarity index 57% rename from USAGE rename to USAGE.md index a41b71045..0493db680 100644 --- a/USAGE +++ b/USAGE.md @@ -1,6 +1,5 @@ USAGE - Georgia Tech Smoothing and Mapping library ---------------------------------------------------- - +=================================== What is this file? This file explains how to make use of the library for common SLAM tasks, @@ -34,18 +33,12 @@ The GTSAM library has three primary components necessary for the construction of factor graph representation and optimization which users will need to adapt to their particular problem. -FactorGraph: - A factor graph contains a set of variables to solve for (i.e., robot poses, - landmark poses, etc.) and a set of constraints between these variables, which - make up factors. -Values: - Values is a single object containing labeled values for all of the - variables. Currently, all variables are labeled with strings, but the type - or organization of the variables can change -Factors: - A nonlinear factor expresses a constraint between variables, which in the - SLAM example, is a measurement such as a visual reading on a landmark or - odometry. +* FactorGraph: + A factor graph contains a set of variables to solve for (i.e., robot poses, landmark poses, etc.) and a set of constraints between these variables, which make up factors. +* Values: + Values is a single object containing labeled values for all of the variables. Currently, all variables are labeled with strings, but the type or organization of the variables can change +* Factors: + A nonlinear factor expresses a constraint between variables, which in the SLAM example, is a measurement such as a visual reading on a landmark or odometry. The library is organized according to the following directory structure: @@ -59,23 +52,3 @@ The library is organized according to the following directory structure: -VSLAM Example ---------------------------------------------------- -The visual slam example shows a full implementation of a slam system. The example contains -derived versions of NonlinearFactor, NonlinearFactorGraph, in classes visualSLAM::ProjectionFactor, -visualSLAM::Graph, respectively. The values for the system are stored in the generic -Values structure. For definitions and interface, see gtsam/slam/visualSLAM.h. - -The clearest example of the use of the graph to find a solution is in -testVSLAM. The basic process for using graphs is as follows (and can be seen in -the test): - - Create a NonlinearFactorGraph object (visualSLAM::Graph) - - Add factors to the graph (note the use of Boost.shared_ptr here) (visualSLAM::ProjectionFactor) - - Create an initial configuration (Values) - - Create an elimination ordering of variables (this must include all variables) - - Create and initialize a NonlinearOptimizer object (Note that this is a generic - algorithm that does not need to be derived for a particular problem) - - Call optimization functions with the optimizer to optimize the graph - - Extract an updated values from the optimizer - - \ No newline at end of file diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt index 49069c57f..a6860f205 100644 --- a/cmake/CMakeLists.txt +++ b/cmake/CMakeLists.txt @@ -19,7 +19,6 @@ install(FILES GtsamMatlabWrap.cmake GtsamPythonWrap.cmake GtsamTesting.cmake - GtsamTestingObsolete.cmake README.html DESTINATION "${SCRIPT_INSTALL_DIR}/GTSAMCMakeTools") diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 9a0b297ab..c2cd7b449 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -15,8 +15,8 @@ option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build typ # Add debugging flags but only on the first pass if(NOT FIRST_PASS_DONE) if(MSVC) - set(CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) - set(CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) + set(CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) + set(CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) set(CMAKE_C_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) set(CMAKE_C_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds." FORCE) @@ -34,8 +34,8 @@ if(NOT FIRST_PASS_DONE) set(CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING CMAKE_MODULE_LINKER_FLAGS_PROFILING) else() - set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall" CACHE STRING "Flags used by the compiler during debug builds." FORCE) - set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall" CACHE STRING "Flags used by the compiler during debug builds." FORCE) + set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) + set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) set(CMAKE_C_FLAGS_RELWITHDEBINFO "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) set(CMAKE_C_FLAGS_RELEASE "-O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE) @@ -53,11 +53,12 @@ if(NOT FIRST_PASS_DONE) endif() endif() -# Clang on Mac uses a template depth that is less than standard and is too small -if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") - if(NOT "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024") - endif() +# Clang uses a template depth that is less than standard and is too small +if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") + # Apple Clang before 5.0 does not support -ftemplate-depth. + if(NOT (APPLE AND "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0")) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024") + endif() endif() # Set up build type library postfixes @@ -97,7 +98,8 @@ if( NOT cmake_build_type_tolower STREQUAL "" AND NOT cmake_build_type_tolower STREQUAL "release" AND NOT cmake_build_type_tolower STREQUAL "timing" AND NOT cmake_build_type_tolower STREQUAL "profiling" - AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo") + AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo" + AND NOT cmake_build_type_tolower STREQUAL "minsizerel") message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are None, Debug, Release, Timing, Profiling, RelWithDebInfo (case-insensitive).") endif() diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake index ba616b025..d1d3d93dd 100644 --- a/cmake/GtsamMatlabWrap.cmake +++ b/cmake/GtsamMatlabWrap.cmake @@ -270,7 +270,7 @@ function(install_wrapped_library_internal interfaceHeader) if(GTSAM_BUILD_TYPE_POSTFIXES) foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) string(TOUPPER "${build_type}" build_type_upper) - if("${build_type_upper}" STREQUAL "RELEASE") + if(${build_type_upper} STREQUAL "RELEASE") set(build_type_tag "") # Don't create release mode tag on installed directory else() set(build_type_tag "${build_type}") @@ -367,13 +367,18 @@ endfunction() # should be installed to all build type toolboxes function(install_matlab_scripts source_directory patterns) set(patterns_args "") + set(exclude_patterns "") + if(NOT GTSAM_WRAP_SERIALIZATION) + set(exclude_patterns "testSerialization.m") + endif() + foreach(pattern ${patterns}) list(APPEND patterns_args PATTERN "${pattern}") endforeach() if(GTSAM_BUILD_TYPE_POSTFIXES) foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) string(TOUPPER "${build_type}" build_type_upper) - if("${build_type_upper}" STREQUAL "RELEASE") + if(${build_type_upper} STREQUAL "RELEASE") set(build_type_tag "") # Don't create release mode tag on installed directory else() set(build_type_tag "${build_type}") @@ -381,10 +386,10 @@ function(install_matlab_scripts source_directory patterns) # Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if there is one get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH) get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME) - install(DIRECTORY "${source_directory}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING ${patterns_args} PATTERN ".svn" EXCLUDE) + install(DIRECTORY "${source_directory}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE) endforeach() else() - install(DIRECTORY "${source_directory}" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING ${patterns_args} PATTERN ".svn" EXCLUDE) + install(DIRECTORY "${source_directory}" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE) endif() endfunction() diff --git a/cmake/GtsamTesting.cmake b/cmake/GtsamTesting.cmake index 2e505c11e..4b3af9810 100644 --- a/cmake/GtsamTesting.cmake +++ b/cmake/GtsamTesting.cmake @@ -109,9 +109,8 @@ add_custom_target(examples) # Add timing target add_custom_target(timing) -# Include obsolete macros - will be removed in the near future -include(GtsamTestingObsolete) - +# Add target to build tests without running +add_custom_target(all.tests) # Implementations of this file's macros: @@ -165,6 +164,7 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) add_test(NAME ${script_name} COMMAND ${script_name}) add_dependencies(check.${groupName} ${script_name}) add_dependencies(check ${script_name}) + add_dependencies(all.tests ${script_name}) if(NOT MSVC AND NOT XCODE_VERSION) add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name}) endif() @@ -195,6 +195,7 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) add_test(NAME ${target_name} COMMAND ${target_name}) add_dependencies(check.${groupName} ${target_name}) add_dependencies(check ${target_name}) + add_dependencies(all.tests ${script_name}) # Add TOPSRCDIR set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") diff --git a/cmake/GtsamTestingObsolete.cmake b/cmake/obsolete/GtsamTestingObsolete.cmake similarity index 100% rename from cmake/GtsamTestingObsolete.cmake rename to cmake/obsolete/GtsamTestingObsolete.cmake diff --git a/doc/.gitignore b/doc/.gitignore new file mode 100644 index 000000000..ac7af2e80 --- /dev/null +++ b/doc/.gitignore @@ -0,0 +1 @@ +/html/ diff --git a/doc/Mathematica/Quaternion-Logmap.nb b/doc/Mathematica/Quaternion-Logmap.nb new file mode 100644 index 000000000..154cd7e51 --- /dev/null +++ b/doc/Mathematica/Quaternion-Logmap.nb @@ -0,0 +1,226 @@ +(* Content-type: application/vnd.wolfram.mathematica *) + +(*** Wolfram Notebook File ***) +(* http://www.wolfram.com/nb *) + +(* CreatedBy='Mathematica 10.0' *) + +(*CacheID: 234*) +(* Internal cache information: +NotebookFileLineBreakTest +NotebookFileLineBreakTest +NotebookDataPosition[ 158, 7] +NotebookDataLength[ 6004, 217] +NotebookOptionsPosition[ 5104, 179] +NotebookOutlinePosition[ 5456, 195] +CellTagsIndexPosition[ 5413, 192] +WindowFrame->Normal*) + +(* Beginning of Notebook Content *) +Notebook[{ +Cell["\<\ +In Quaternion.h we have Logmap, but we have to be careful when qw approaches \ +-1 (from above) or 1 (from below). The Taylor expansions below are the basis \ +for the code.\ +\>", "Text", + CellChangeTimes->{{3.632651837171029*^9, 3.6326518973274307`*^9}}], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{"angle", "=", + RowBox[{"2", + RowBox[{"ArcCos", "[", "qw", "]"}]}]}]], "Input", + CellChangeTimes->{{3.6326509558588057`*^9, 3.632650976842943*^9}}], + +Cell[BoxData[ + RowBox[{"2", " ", + RowBox[{"ArcCos", "[", "qw", "]"}]}]], "Output", + CellChangeTimes->{{3.6326509669341784`*^9, 3.6326509795921097`*^9}}] +}, Open ]], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{"s", "=", + RowBox[{"Sqrt", "[", + RowBox[{"1", "-", + RowBox[{"qw", "*", "qw"}]}], "]"}]}]], "Input", + CellChangeTimes->{{3.632650983796185*^9, 3.632650994132272*^9}}], + +Cell[BoxData[ + SqrtBox[ + RowBox[{"1", "-", + SuperscriptBox["qw", "2"]}]]], "Output", + CellChangeTimes->{3.63265099440246*^9}] +}, Open ]], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{"factor", " ", "=", " ", + RowBox[{"angle", "/", "s"}]}]], "Input", + CellChangeTimes->{{3.632650999925654*^9, 3.632651001339293*^9}, { + 3.632651070297429*^9, 3.632651071527272*^9}}], + +Cell[BoxData[ + FractionBox[ + RowBox[{"2", " ", + RowBox[{"ArcCos", "[", "qw", "]"}]}], + SqrtBox[ + RowBox[{"1", "-", + SuperscriptBox["qw", "2"]}]]]], "Output", + CellChangeTimes->{3.632651001671771*^9, 3.632651072007021*^9}] +}, Open ]], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{"Expand", "[", + RowBox[{"Series", "[", + RowBox[{ + FractionBox[ + RowBox[{"2", " ", + RowBox[{"ArcCos", "[", "qw", "]"}]}], + SqrtBox[ + RowBox[{"1", "-", + SuperscriptBox["qw", "2"]}]]], ",", + RowBox[{"{", + RowBox[{"qw", ",", "1", ",", "1"}], "}"}], ",", + RowBox[{"Assumptions", "->", + RowBox[{"(", + RowBox[{"qw", "<", "1"}], ")"}]}]}], "]"}], "]"}]], "Input", + CellChangeTimes->{{3.6326510739355927`*^9, 3.632651117949705*^9}, { + 3.6326511716876993`*^9, 3.632651189491748*^9}, {3.632651248821335*^9, + 3.632651267905816*^9}}], + +Cell[BoxData[ + InterpretationBox[ + RowBox[{"2", "-", + FractionBox[ + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{"qw", "-", "1"}], ")"}]}], "3"], "+", + InterpretationBox[ + SuperscriptBox[ + RowBox[{"O", "[", + RowBox[{"qw", "-", "1"}], "]"}], + RowBox[{"3", "/", "2"}]], + SeriesData[$CellContext`qw, 1, {}, 0, 3, 2], + Editable->False]}], + SeriesData[$CellContext`qw, 1, {2, 0, + Rational[-2, 3]}, 0, 3, 2], + Editable->False]], "Output", + CellChangeTimes->{{3.632651102947558*^9, 3.632651118218814*^9}, { + 3.632651179610784*^9, 3.6326511898522263`*^9}, {3.632651249719887*^9, + 3.632651268312502*^9}}] +}, Open ]], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{"ArcCos", "[", + RowBox[{"-", "1"}], "]"}]], "Input", + CellChangeTimes->{{3.632651352754121*^9, 3.63265135286866*^9}}], + +Cell[BoxData["\[Pi]"], "Output", + CellChangeTimes->{3.632651353300222*^9}] +}, Open ]], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{"Expand", "[", + RowBox[{"Series", "[", + RowBox[{ + FractionBox[ + RowBox[{ + RowBox[{"-", "2"}], + RowBox[{"ArcCos", "[", + RowBox[{"-", "qw"}], "]"}]}], + SqrtBox[ + RowBox[{"1", "-", + SuperscriptBox["qw", "2"]}]]], ",", + RowBox[{"{", + RowBox[{"qw", ",", + RowBox[{"-", "1"}], ",", "1"}], "}"}], ",", + RowBox[{"Assumptions", "->", + RowBox[{"(", + RowBox[{"qw", ">", + RowBox[{"-", "1"}]}], ")"}]}]}], "]"}], "]"}]], "Input", + CellChangeTimes->{{3.6326510739355927`*^9, 3.632651117949705*^9}, { + 3.6326511716876993`*^9, 3.6326512088422937`*^9}, {3.632651301817163*^9, + 3.6326513406015453`*^9}, {3.63265150259446*^9, 3.632651505055284*^9}, { + 3.632651744223112*^9, 3.632651772717318*^9}}], + +Cell[BoxData[ + InterpretationBox[ + RowBox[{ + RowBox[{"-", "2"}], "-", + FractionBox[ + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{"qw", "+", "1"}], ")"}]}], "3"], "+", + InterpretationBox[ + SuperscriptBox[ + RowBox[{"O", "[", + RowBox[{"qw", "+", "1"}], "]"}], + RowBox[{"3", "/", "2"}]], + SeriesData[$CellContext`qw, -1, {}, 0, 3, 2], + Editable->False]}], + SeriesData[$CellContext`qw, -1, {-2, 0, + Rational[-2, 3]}, 0, 3, 2], + Editable->False]], "Output", + CellChangeTimes->{ + 3.632651209181905*^9, 3.6326513025091133`*^9, {3.632651332608609*^9, + 3.632651341031022*^9}, 3.632651506516138*^9, {3.632651746679185*^9, + 3.632651773032124*^9}}] +}, Open ]] +}, +WindowSize->{808, 751}, +WindowMargins->{{4, Automatic}, {Automatic, 4}}, +FrontEndVersion->"10.0 for Mac OS X x86 (32-bit, 64-bit Kernel) (June 27, \ +2014)", +StyleDefinitions->"Default.nb" +] +(* End of Notebook Content *) + +(* Internal cache information *) +(*CellTagsOutline +CellTagsIndex->{} +*) +(*CellTagsIndex +CellTagsIndex->{} +*) +(*NotebookFileOutline +Notebook[{ +Cell[558, 20, 263, 5, 49, "Text"], +Cell[CellGroupData[{ +Cell[846, 29, 174, 4, 28, "Input"], +Cell[1023, 35, 154, 3, 28, "Output"] +}, Open ]], +Cell[CellGroupData[{ +Cell[1214, 43, 197, 5, 28, "Input"], +Cell[1414, 50, 129, 4, 40, "Output"] +}, Open ]], +Cell[CellGroupData[{ +Cell[1580, 59, 206, 4, 28, "Input"], +Cell[1789, 65, 233, 7, 59, "Output"] +}, Open ]], +Cell[CellGroupData[{ +Cell[2059, 77, 605, 17, 61, "Input"], +Cell[2667, 96, 645, 19, 48, "Output"] +}, Open ]], +Cell[CellGroupData[{ +Cell[3349, 120, 142, 3, 28, "Input"], +Cell[3494, 125, 74, 1, 28, "Output"] +}, Open ]], +Cell[CellGroupData[{ +Cell[3605, 131, 788, 22, 61, "Input"], +Cell[4396, 155, 692, 21, 48, "Output"] +}, Open ]] +} +] +*) + +(* End of internal cache information *) diff --git a/doc/Mathematica/dexpInvL_SE2.nb b/doc/Mathematica/dexpInvL_SE2.nb new file mode 100644 index 000000000..d15932768 --- /dev/null +++ b/doc/Mathematica/dexpInvL_SE2.nb @@ -0,0 +1,607 @@ +(* Content-type: application/vnd.wolfram.mathematica *) + +(*** Wolfram Notebook File ***) +(* http://www.wolfram.com/nb *) + +(* CreatedBy='Mathematica 8.0' *) + +(*CacheID: 234*) +(* Internal cache information: +NotebookFileLineBreakTest +NotebookFileLineBreakTest +NotebookDataPosition[ 157, 7] +NotebookDataLength[ 18933, 598] +NotebookOptionsPosition[ 18110, 565] +NotebookOutlinePosition[ 18464, 581] +CellTagsIndexPosition[ 18421, 578] +WindowFrame->Normal*) + +(* Beginning of Notebook Content *) +Notebook[{ +Cell[TextData[{ + "The \[OpenCurlyQuote]right trivialised\[CloseCurlyQuote] tangent of the \ +exponential map, ", + Cell[BoxData[ + FormBox["dexpR", TraditionalForm]], + FormatType->"TraditionalForm"], + ", according to Iserles05an, formula 2.42, pg. 32 can be written as\n\t", + Cell[BoxData[ + FormBox[GridBox[{ + {"\t"}, + { + RowBox[{ + RowBox[{ + RowBox[{"g", "'"}], + SuperscriptBox["g", + RowBox[{"-", "1"}]]}], "=", + RowBox[{ + SubscriptBox["dexpR", "\[Omega]"], "(", + RowBox[{"\[Omega]", "'"}], ")"}]}]} + }], TraditionalForm]], + FormatType->"TraditionalForm"], + "\nwhere ", + Cell[BoxData[ + FormBox[ + RowBox[{"g", "=", + RowBox[{"exp", "(", "\[Omega]", ")"}]}], TraditionalForm]], + FormatType->"TraditionalForm"], + ", and ", + Cell[BoxData[ + FormBox[ + RowBox[{ + RowBox[{"g", "'"}], "=", + RowBox[{ + RowBox[{"exp", "'"}], + RowBox[{"(", "\[Omega]", ")"}]}]}], TraditionalForm]], + FormatType->"TraditionalForm"], + ".\nCompare this to the left Jacobian matrix ", + Cell[BoxData[ + FormBox[ + SubscriptBox["J", "l"], TraditionalForm]], + FormatType->"TraditionalForm"], + " in Chirikjian11book2, pg. 26, we see that ", + Cell[BoxData[ + FormBox["dexpR", TraditionalForm]], + FormatType->"TraditionalForm"], + " and ", + Cell[BoxData[ + FormBox[ + SubscriptBox["J", "l"], TraditionalForm]], + FormatType->"TraditionalForm"], + " are the same.\n\nHence, be careful, Iserles\[CloseCurlyQuote]s \ +\[OpenCurlyQuote]", + StyleBox["right", + FontWeight->"Bold"], + " trivialised\[CloseCurlyQuote] tangent of the exponential map ", + Cell[BoxData[ + FormBox["dexpR", TraditionalForm]], + FormatType->"TraditionalForm"], + " is in fact Chirikjian\[CloseCurlyQuote]s ", + StyleBox["left", + FontWeight->"Bold"], + " Jacobian matrix ", + Cell[BoxData[ + FormBox[ + SubscriptBox["J", "l"], TraditionalForm]], + FormatType->"TraditionalForm"], + "!!!\n\nWe want to compute the s \[OpenCurlyQuote]", + StyleBox["left", + FontWeight->"Bold"], + " trivialised\[CloseCurlyQuote] tangent of the exponential map, ", + Cell[BoxData[ + FormBox["dexpL", TraditionalForm]], + FormatType->"TraditionalForm"], + ", for SE2, hence, we need to use Chirikjian\[CloseCurlyQuote]s ", + StyleBox["right", + FontWeight->"Bold"], + " Jacobian matrix ", + Cell[BoxData[ + FormBox[ + SubscriptBox["J", "r"], TraditionalForm]], + FormatType->"TraditionalForm"], + " formula in Chirikjian11book2, pg. 36." +}], "Text", + CellChangeTimes->{{3.6279967389044943`*^9, 3.6279968865058002`*^9}, { + 3.6279969695759087`*^9, 3.6279974871811028`*^9}, 3.62799757389325*^9}], + +Cell[BoxData[{ + RowBox[{"Clear", "[", "J", "]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"J", "[", "\[Alpha]_", "]"}], ":=", + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], "/", "\[Alpha]"}], ",", + RowBox[{ + RowBox[{"(", + RowBox[{"1", "-", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}], "/", "\[Alpha]"}], + ",", " ", + RowBox[{ + RowBox[{"(", + RowBox[{ + RowBox[{"\[Alpha]", " ", + SubscriptBox["v", "1"]}], "-", + SubscriptBox["v", "2"], "+", + RowBox[{ + SubscriptBox["v", "2"], + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "-", + RowBox[{ + SubscriptBox["v", "1"], + RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], "/", + SuperscriptBox["\[Alpha]", "2"]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{ + RowBox[{"-", + RowBox[{"(", + RowBox[{"1", "-", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}]}], "/", "\[Alpha]"}], + ",", + RowBox[{ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], "/", "\[Alpha]"}], ",", " ", + RowBox[{ + RowBox[{"(", + RowBox[{ + SubscriptBox["v", "1"], "+", + RowBox[{"\[Alpha]", " ", + SubscriptBox["v", "2"]}], "-", + RowBox[{ + SubscriptBox["v", "1"], + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "-", + RowBox[{ + SubscriptBox["v", "2"], + RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], "/", + SuperscriptBox["\[Alpha]", "2"]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "}"}]}]}], "Input", + CellChangeTimes->{{3.627993817228732*^9, 3.6279939547434673`*^9}, { + 3.627993986274671*^9, 3.6279940386007967`*^9}, {3.627995391081044*^9, + 3.627995412846286*^9}, 3.6279954452391644`*^9, {3.627995531089571*^9, + 3.6279955341932592`*^9}, {3.627996429604282*^9, 3.62799643077184*^9}}], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{ + RowBox[{ + RowBox[{"Jinv", "[", "\[Alpha]_", "]"}], "=", + RowBox[{"Inverse", "[", + RowBox[{"J", "[", "\[Alpha]", "]"}], "]"}]}], + "\[IndentingNewLine]"}]], "Input", + CellChangeTimes->{ + 3.627995475343099*^9, {3.627995548533964*^9, 3.627995559455151*^9}, { + 3.627996438504909*^9, 3.6279964431657553`*^9}}], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{ + FractionBox[ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], + RowBox[{"\[Alpha]", " ", + RowBox[{"(", + RowBox[{ + FractionBox["1", + SuperscriptBox["\[Alpha]", "2"]], "-", + FractionBox[ + RowBox[{"2", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]]}], ")"}]}]], ",", + FractionBox[ + RowBox[{ + RowBox[{"-", + FractionBox["1", "\[Alpha]"]}], "+", + FractionBox[ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], "\[Alpha]"]}], + RowBox[{ + FractionBox["1", + SuperscriptBox["\[Alpha]", "2"]], "-", + FractionBox[ + RowBox[{"2", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]]}]], ",", + FractionBox[ + RowBox[{ + FractionBox[ + SubscriptBox["v", "1"], + SuperscriptBox["\[Alpha]", "3"]], "-", + FractionBox[ + RowBox[{"2", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ", + SubscriptBox["v", "1"]}], + SuperscriptBox["\[Alpha]", "3"]], "+", + FractionBox[ + RowBox[{ + SuperscriptBox[ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], " ", + SubscriptBox["v", "1"]}], + SuperscriptBox["\[Alpha]", "3"]], "-", + FractionBox[ + RowBox[{ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], " ", + SubscriptBox["v", "1"]}], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + RowBox[{ + SuperscriptBox[ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], " ", + SubscriptBox["v", "1"]}], + SuperscriptBox["\[Alpha]", "3"]], "+", + FractionBox[ + SubscriptBox["v", "2"], + SuperscriptBox["\[Alpha]", "2"]], "-", + FractionBox[ + RowBox[{ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ", + SubscriptBox["v", "2"]}], + SuperscriptBox["\[Alpha]", "2"]]}], + RowBox[{ + FractionBox["1", + SuperscriptBox["\[Alpha]", "2"]], "-", + FractionBox[ + RowBox[{"2", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]]}]]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + FractionBox[ + RowBox[{ + FractionBox["1", "\[Alpha]"], "-", + FractionBox[ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], "\[Alpha]"]}], + RowBox[{ + FractionBox["1", + SuperscriptBox["\[Alpha]", "2"]], "-", + FractionBox[ + RowBox[{"2", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]]}]], ",", + FractionBox[ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], + RowBox[{"\[Alpha]", " ", + RowBox[{"(", + RowBox[{ + FractionBox["1", + SuperscriptBox["\[Alpha]", "2"]], "-", + FractionBox[ + RowBox[{"2", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]]}], ")"}]}]], ",", + FractionBox[ + RowBox[{ + RowBox[{"-", + FractionBox[ + SubscriptBox["v", "1"], + SuperscriptBox["\[Alpha]", "2"]]}], "+", + FractionBox[ + RowBox[{ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ", + SubscriptBox["v", "1"]}], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SubscriptBox["v", "2"], + SuperscriptBox["\[Alpha]", "3"]], "-", + FractionBox[ + RowBox[{"2", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ", + SubscriptBox["v", "2"]}], + SuperscriptBox["\[Alpha]", "3"]], "+", + FractionBox[ + RowBox[{ + SuperscriptBox[ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], " ", + SubscriptBox["v", "2"]}], + SuperscriptBox["\[Alpha]", "3"]], "-", + FractionBox[ + RowBox[{ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], " ", + SubscriptBox["v", "2"]}], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + RowBox[{ + SuperscriptBox[ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], " ", + SubscriptBox["v", "2"]}], + SuperscriptBox["\[Alpha]", "3"]]}], + RowBox[{ + FractionBox["1", + SuperscriptBox["\[Alpha]", "2"]], "-", + FractionBox[ + RowBox[{"2", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]]}]]}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "}"}]], "Output", + CellChangeTimes->{ + 3.627995560030972*^9, {3.627996412919798*^9, 3.627996444306521*^9}}] +}, Open ]], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{ + RowBox[{ + RowBox[{"Jinv", "[", "\[Alpha]", "]"}], "//", "Simplify"}], "//", + "MatrixForm"}]], "Input", + CellChangeTimes->{{3.627993835637863*^9, 3.627993839233502*^9}, { + 3.627994046108457*^9, 3.627994058781851*^9}, {3.627995546842499*^9, + 3.6279955664940767`*^9}}], + +Cell[BoxData[ + TagBox[ + RowBox[{"(", "\[NoBreak]", GridBox[{ + { + RowBox[{ + FractionBox["1", "2"], " ", "\[Alpha]", " ", + RowBox[{"Cot", "[", + FractionBox["\[Alpha]", "2"], "]"}]}], + RowBox[{"-", + FractionBox["\[Alpha]", "2"]}], + FractionBox[ + RowBox[{ + RowBox[{ + RowBox[{"(", + RowBox[{ + RowBox[{"-", "2"}], "+", + RowBox[{"2", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "+", + RowBox[{"\[Alpha]", " ", + RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], " ", + SubscriptBox["v", "1"]}], "+", + RowBox[{"\[Alpha]", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "1"}], "+", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}], " ", + SubscriptBox["v", "2"]}]}], + RowBox[{"2", " ", "\[Alpha]", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "1"}], "+", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}]}]]}, + { + FractionBox["\[Alpha]", "2"], + RowBox[{ + FractionBox["1", "2"], " ", "\[Alpha]", " ", + RowBox[{"Cot", "[", + FractionBox["\[Alpha]", "2"], "]"}]}], + FractionBox[ + RowBox[{ + RowBox[{ + RowBox[{"(", + RowBox[{"\[Alpha]", "-", + RowBox[{"\[Alpha]", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}]}], ")"}], " ", + SubscriptBox["v", "1"]}], "+", + RowBox[{ + RowBox[{"(", + RowBox[{ + RowBox[{"-", "2"}], "+", + RowBox[{"2", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "+", + RowBox[{"\[Alpha]", " ", + RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], " ", + SubscriptBox["v", "2"]}]}], + RowBox[{"2", " ", "\[Alpha]", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "1"}], "+", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}]}]]}, + {"0", "0", "1"} + }, + GridBoxAlignment->{ + "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, + "RowsIndexed" -> {}}, + GridBoxSpacings->{"Columns" -> { + Offset[0.27999999999999997`], { + Offset[0.7]}, + Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { + Offset[0.2], { + Offset[0.4]}, + Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], + Function[BoxForm`e$, + MatrixForm[BoxForm`e$]]]], "Output", + CellChangeTimes->{ + 3.627993840513033*^9, {3.62799404156531*^9, 3.6279940592345743`*^9}, + 3.627995567356995*^9, 3.627996415136314*^9, 3.6279964490074778`*^9}] +}, Open ]], + +Cell[TextData[{ + "In case ", + Cell[BoxData[ + FormBox[ + RowBox[{"\[Alpha]", "=", "0"}], TraditionalForm]], + FormatType->"TraditionalForm"], + ", we compute the limits of ", + Cell[BoxData[ + FormBox[ + SubscriptBox["J", "r"], TraditionalForm]], + FormatType->"TraditionalForm"], + " and ", + Cell[BoxData[ + FormBox[ + SuperscriptBox[ + SubscriptBox["J", "r"], + RowBox[{"-", "1"}]], TraditionalForm]], + FormatType->"TraditionalForm"], + " as follows" +}], "Text", + CellChangeTimes->{{3.627997495449997*^9, 3.627997524522543*^9}}], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{ + RowBox[{ + RowBox[{"Limit", "[", + RowBox[{ + RowBox[{"Jinv", "[", "\[Alpha]", "]"}], ",", + RowBox[{"\[Alpha]", "\[Rule]", "0"}]}], "]"}], "//", "Simplify"}], "//", + "MatrixForm"}]], "Input", + CellChangeTimes->{{3.627995572179821*^9, 3.627995606373824*^9}}], + +Cell[BoxData[ + TagBox[ + RowBox[{"(", "\[NoBreak]", GridBox[{ + {"1", "0", + FractionBox[ + SubscriptBox["v", "2"], "2"]}, + {"0", "1", + RowBox[{"-", + FractionBox[ + SubscriptBox["v", "1"], "2"]}]}, + {"0", "0", "1"} + }, + GridBoxAlignment->{ + "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, + "RowsIndexed" -> {}}, + GridBoxSpacings->{"Columns" -> { + Offset[0.27999999999999997`], { + Offset[0.7]}, + Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { + Offset[0.2], { + Offset[0.4]}, + Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], + Function[BoxForm`e$, + MatrixForm[BoxForm`e$]]]], "Output", + CellChangeTimes->{{3.627995585954463*^9, 3.627995606858135*^9}, + 3.6279964178087473`*^9, 3.6279964634008904`*^9}] +}, Open ]], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{ + RowBox[{ + RowBox[{"Limit", "[", + RowBox[{ + RowBox[{"J", "[", "\[Alpha]", "]"}], ",", + RowBox[{"\[Alpha]", "\[Rule]", "0"}]}], "]"}], "//", "Simplify"}], "//", + "MatrixForm"}]], "Input", + CellChangeTimes->{{3.6279956527343893`*^9, 3.627995660697241*^9}}], + +Cell[BoxData[ + TagBox[ + RowBox[{"(", "\[NoBreak]", GridBox[{ + {"1", "0", + RowBox[{"-", + FractionBox[ + SubscriptBox["v", "2"], "2"]}]}, + {"0", "1", + FractionBox[ + SubscriptBox["v", "1"], "2"]}, + {"0", "0", "1"} + }, + GridBoxAlignment->{ + "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, + "RowsIndexed" -> {}}, + GridBoxSpacings->{"Columns" -> { + Offset[0.27999999999999997`], { + Offset[0.7]}, + Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { + Offset[0.2], { + Offset[0.4]}, + Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], + Function[BoxForm`e$, + MatrixForm[BoxForm`e$]]]], "Output", + CellChangeTimes->{{3.627995653969245*^9, 3.627995661346282*^9}, + 3.627996419383007*^9, 3.627996465705708*^9}] +}, Open ]], + +Cell[BoxData[""], "Input", + CellChangeTimes->{{3.627995694633294*^9, 3.627995695945466*^9}}] +}, +WindowSize->{654, 852}, +WindowMargins->{{Automatic, 27}, {Automatic, 0}}, +FrontEndVersion->"8.0 for Mac OS X x86 (32-bit, 64-bit Kernel) (October 5, \ +2011)", +StyleDefinitions->"Default.nb" +] +(* End of Notebook Content *) + +(* Internal cache information *) +(*CellTagsOutline +CellTagsIndex->{} +*) +(*CellTagsIndex +CellTagsIndex->{} +*) +(*NotebookFileOutline +Notebook[{ +Cell[557, 20, 2591, 84, 197, "Text"], +Cell[3151, 106, 2022, 56, 68, "Input"], +Cell[CellGroupData[{ +Cell[5198, 166, 343, 9, 43, "Input"], +Cell[5544, 177, 6519, 190, 290, "Output"] +}, Open ]], +Cell[CellGroupData[{ +Cell[12100, 372, 298, 7, 27, "Input"], +Cell[12401, 381, 2665, 76, 99, "Output"] +}, Open ]], +Cell[15081, 460, 535, 20, 29, "Text"], +Cell[CellGroupData[{ +Cell[15641, 484, 297, 8, 27, "Input"], +Cell[15941, 494, 863, 25, 91, "Output"] +}, Open ]], +Cell[CellGroupData[{ +Cell[16841, 524, 296, 8, 27, "Input"], +Cell[17140, 534, 859, 25, 91, "Output"] +}, Open ]], +Cell[18014, 562, 92, 1, 27, "Input"] +} +] +*) + +(* End of internal cache information *) diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp new file mode 100644 index 000000000..eb6bdd49d --- /dev/null +++ b/examples/METISOrderingExample.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 + + * -------------------------------------------------------------------------- */ + +/** + * @file METISOrdering.cpp + * @brief Simple robot motion example, with prior and two odometry measurements, using a METIS ordering + * @author Frank Dellaert + * @author Andrew Melim + */ + +/** + * Example of a simple 2D localization example optimized using METIS ordering + * - For more details on the full optimization pipeline, see OdometryExample.cpp + */ + +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main(int argc, char** argv) { + NonlinearFactorGraph graph; + + Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); + graph.add(PriorFactor(1, priorMean, priorNoise)); + + Pose2 odometry(2.0, 0.0, 0.0); + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); + graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); + graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); + graph.print("\nFactor Graph:\n"); // print + + Values initial; + initial.insert(1, Pose2(0.5, 0.0, 0.2)); + initial.insert(2, Pose2(2.3, 0.1, -0.2)); + initial.insert(3, Pose2(4.1, 0.1, 0.1)); + initial.print("\nInitial Estimate:\n"); // print + + // optimize using Levenberg-Marquardt optimization + LevenbergMarquardtParams params; + // In order to specify the ordering type, we need to se the NonlinearOptimizerParameter "orderingType" + // By default this parameter is set to OrderingType::COLAMD + params.orderingType = Ordering::METIS; + LevenbergMarquardtOptimizer optimizer(graph, initial, params); + Values result = optimizer.optimize(); + result.print("Final Result:\n"); + + return 0; +} diff --git a/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp b/examples/Pose2SLAMExampleExpressions.cpp similarity index 77% rename from gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp rename to examples/Pose2SLAMExampleExpressions.cpp index 3a3b97b77..1f6de6cb1 100644 --- a/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp +++ b/examples/Pose2SLAMExampleExpressions.cpp @@ -14,20 +14,18 @@ * @brief Expressions version of Pose2SLAMExample.cpp * @date Oct 2, 2014 * @author Frank Dellaert - * @author Yong Dian Jian */ // The two new headers that allow using our Automatic Differentiation Expression framework -#include -#include +#include +#include -// Header order is close to far -#include +// For an explanation of headers below, please see Pose2SLAMExample.cpp +#include +#include +#include #include #include -#include -#include -#include using namespace std; using namespace gtsam; @@ -35,26 +33,26 @@ using namespace gtsam; int main(int argc, char** argv) { // 1. Create a factor graph container and add factors to it - NonlinearFactorGraph graph; + ExpressionFactorGraph graph; // Create Expressions for unknowns Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5); // 2a. Add a prior on the first pose, setting it to the origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.push_back(ExpressionFactor(priorNoise, Pose2(0, 0, 0), x1)); + graph.addExpressionFactor(x1, Pose2(0, 0, 0), priorNoise); // For simplicity, we will use the same noise model for odometry and loop closures noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 2b. Add odometry factors - graph.push_back(ExpressionFactor(model, Pose2(2, 0, 0 ), between(x1,x2))); - graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x2,x3))); - graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x3,x4))); - graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x4,x5))); + graph.addExpressionFactor(between(x1,x2), Pose2(2, 0, 0 ), model); + graph.addExpressionFactor(between(x2,x3), Pose2(2, 0, M_PI_2), model); + graph.addExpressionFactor(between(x3,x4), Pose2(2, 0, M_PI_2), model); + graph.addExpressionFactor(between(x4,x5), Pose2(2, 0, M_PI_2), model); // 2c. Add the loop closure constraint - graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x5,x2))); + graph.addExpressionFactor(between(x5,x2), Pose2(2, 0, M_PI_2), model); graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index 564930d74..35f9884e1 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index 46ca02ee4..c3d901507 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -16,11 +16,15 @@ * @author Frank Dellaert */ -#include +// For an explanation of headers below, please see Pose2SLAMExample.cpp #include -#include -#include +#include #include +#include +#include + +// This new header allows us to read examples easily from .graph files +#include using namespace std; using namespace gtsam; diff --git a/examples/Pose2SLAMExample_graphviz.cpp b/examples/Pose2SLAMExample_graphviz.cpp index 818a9e577..314ccbdb4 100644 --- a/examples/Pose2SLAMExample_graphviz.cpp +++ b/examples/Pose2SLAMExample_graphviz.cpp @@ -16,11 +16,11 @@ * @author Frank Dellaert */ +// For an explanation of headers below, please see Pose2SLAMExample.cpp #include #include -#include -#include #include +#include #include using namespace std; diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index 9507797eb..b83dfa1db 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include using namespace std; diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 4422586b0..2c25d2f8e 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -16,47 +16,15 @@ * @date June 2, 2012 */ -/** - * A simple 2D pose slam example solved using a Conjugate-Gradient method - * - The robot moves in a 2 meter square - * - The robot moves 2 meters each step, turning 90 degrees after each step - * - The robot initially faces along the X axis (horizontal, to the right in 2D) - * - We have full odometry between pose - * - We have a loop closure constraint when the robot returns to the first position - */ - -// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent -// the robot positions -#include -#include - -// Each variable in the system (poses) must be identified with a unique key. -// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). -// Here we will use simple integer keys -#include - -// In GTSAM, measurement functions are represented as 'factors'. Several common factors -// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. -// Here we will use Between factors for the relative motion described by odometry measurements. -// We will also use a Between Factor to encode the loop closure constraint -// Also, we will initialize the robot at the origin using a Prior factor. +// For an explanation of headers below, please see Pose2SLAMExample.cpp #include #include - -// When the factors are created, we will add them to a Factor Graph. As the factors we are using -// are nonlinear factors, we will need a Nonlinear Factor Graph. -#include - -// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the -// nonlinear functions around an initial linearization point, then solve the linear system -// to update the linearization point. This happens repeatedly until the solver converges -// to a consistent set of variable values. This requires us to specify an initial guess -// for each variable, held in a Values container. -#include - -#include +#include #include +// In contrast to that example, however, we will use a PCG solver here +#include + using namespace std; using namespace gtsam; @@ -66,32 +34,24 @@ int main(int argc, char** argv) { NonlinearFactorGraph graph; // 2a. 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) Pose2 prior(0.0, 0.0, 0.0); // prior at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.push_back(PriorFactor(1, prior, priorNoise)); // 2b. Add odometry factors - // For simplicity, we will use the same noise model for each odometry factor noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - // Create odometry (Between) factors between consecutive poses graph.push_back(BetweenFactor(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.push_back(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.push_back(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.push_back(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); // 2c. Add the loop closure constraint - // This factor encodes the fact that we have returned to the same pose. In real systems, - // these constraints may be identified in many ways, such as appearance-based techniques - // with camera images. - // We will use another Between Factor to enforce this constraint, with the distance set to zero, noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); graph.push_back(BetweenFactor(5, 1, Pose2(0.0, 0.0, 0.0), model)); graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution - // For illustrative purposes, these have been deliberately set to incorrect values Values initialEstimate; initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(2, Pose2(2.3, 0.1, 1.1)); @@ -104,15 +64,18 @@ int main(int argc, char** argv) { LevenbergMarquardtParams parameters; parameters.verbosity = NonlinearOptimizerParams::ERROR; parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; - parameters.linearSolverType = NonlinearOptimizerParams::Iterative; - { - parameters.iterativeParams = boost::make_shared(); - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); - Values result = optimizer.optimize(); - result.print("Final Result:\n"); - cout << "subgraph solver final error = " << graph.error(result) << endl; - } + // LM is still the outer optimization loop, but by specifying "Iterative" below + // We indicate that an iterative linear solver should be used. + // In addition, the *type* of the iterativeParams decides on the type of + // iterative solver, in this case the SPCG (subgraph PCG) + parameters.linearSolverType = NonlinearOptimizerParams::Iterative; + parameters.iterativeParams = boost::make_shared(); + + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); + Values result = optimizer.optimize(); + result.print("Final Result:\n"); + cout << "subgraph solver final error = " << graph.error(result) << endl; return 0; } diff --git a/examples/RangeISAMExample_plaza2.cpp b/examples/RangeISAMExample_plaza2.cpp index 252372f39..04632e9e3 100644 --- a/examples/RangeISAMExample_plaza2.cpp +++ b/examples/RangeISAMExample_plaza2.cpp @@ -82,7 +82,8 @@ vector readTriples() { ifstream is(data_file.c_str()); while (is) { - double t, sender, receiver, range; + double t, sender, range; + size_t receiver; is >> t >> sender >> receiver >> range; triples.push_back(RangeTriple(t, receiver, range)); } diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 5be266d11..38dd1ca81 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -15,13 +15,7 @@ * @author Duy-Nguyen Ta */ -/** - * 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 - */ - -// For loading the data +// For loading the data, see the comments therein for scenario (camera rotates around cube) #include "SFMdata.h" // Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). diff --git a/gtsam_unstable/examples/SFMExampleExpressions.cpp b/examples/SFMExampleExpressions.cpp similarity index 64% rename from gtsam_unstable/examples/SFMExampleExpressions.cpp rename to examples/SFMExampleExpressions.cpp index 941f3dcb8..e9c9e920d 100644 --- a/gtsam_unstable/examples/SFMExampleExpressions.cpp +++ b/examples/SFMExampleExpressions.cpp @@ -23,15 +23,12 @@ */ // The two new headers that allow using our Automatic Differentiation Expression framework -#include -#include +#include +#include // Header order is close to far #include -#include -#include #include -#include #include #include #include @@ -40,27 +37,29 @@ using namespace std; using namespace gtsam; +using namespace noiseModel; /* ************************************************************************* */ int main(int argc, char* argv[]) { Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0); - noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + Isotropic::shared_ptr measurementNoise = Isotropic::Sigma(2, 1.0); // one pixel in u and v // Create the set of ground-truth landmarks and poses vector points = createPoints(); vector poses = createPoses(); // Create a factor graph - NonlinearFactorGraph graph; + ExpressionFactorGraph graph; // Specify uncertainty on first pose prior - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); + Vector6 sigmas; sigmas << Vector3(0.3,0.3,0.3), Vector3(0.1,0.1,0.1); + Diagonal::shared_ptr poseNoise = Diagonal::Sigmas(sigmas); // Here we don't use a PriorFactor but directly the ExpressionFactor class - // The object x0 is an Expression, and we create a factor wanting it to be equal to poses[0] + // x0 is an Expression, and we create a factor wanting it to be equal to poses[0] Pose3_ x0('x',0); - graph.push_back(ExpressionFactor(poseNoise, poses[0], x0)); + graph.addExpressionFactor(x0, poses[0], poseNoise); // We create a constant Expression for the calibration here Cal3_S2_ cK(K); @@ -74,25 +73,26 @@ int main(int argc, char* argv[]) { // Below an expression for the prediction of the measurement: Point3_ p('l', j); Point2_ prediction = uncalibrate(cK, project(transform_to(x, p))); - // Again, here we use a ExpressionFactor - graph.push_back(ExpressionFactor(measurementNoise, measurement, prediction)); + // Again, here we use an ExpressionFactor + graph.addExpressionFactor(prediction, measurement, measurementNoise); } } // Add prior on first point to constrain scale, again with ExpressionFactor - noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.push_back(ExpressionFactor(pointNoise, points[0], Point3_('l', 0))); + Isotropic::shared_ptr pointNoise = Isotropic::Sigma(3, 0.1); + graph.addExpressionFactor(Point3_('l', 0), points[0], pointNoise); // Create perturbed initial - Values initialEstimate; + Values initial; + Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); for (size_t i = 0; i < poses.size(); ++i) - initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + initial.insert(Symbol('x', i), poses[i].compose(delta)); for (size_t j = 0; j < points.size(); ++j) - initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); - cout << "initial error = " << graph.error(initialEstimate) << endl; + initial.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); + cout << "initial error = " << graph.error(initial) << endl; /* Optimize the graph and print results */ - Values result = DoglegOptimizer(graph, initialEstimate).optimize(); + Values result = DoglegOptimizer(graph, initial).optimize(); cout << "final error = " << graph.error(result) << endl; return 0; diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp new file mode 100644 index 000000000..66beefb35 --- /dev/null +++ b/examples/SFMExampleExpressions_bal.cpp @@ -0,0 +1,121 @@ +/* ---------------------------------------------------------------------------- + + * 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 SFMExampleExpressions_bal.cpp + * @brief A structure-from-motion example done with Expressions + * @author Frank Dellaert + * @date January 2015 + */ + +/** + * This is the Expression version of SFMExample + * See detailed description of headers there, this focuses on explaining the AD part + */ + +// The two new headers that allow using our Automatic Differentiation Expression framework +#include +#include + +// Header order is close to far +#include +#include +#include // for loading BAL datasets ! +#include + +using namespace std; +using namespace gtsam; +using namespace noiseModel; +using symbol_shorthand::C; +using symbol_shorthand::P; + +// An SfM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration +// and has a total of 9 free parameters + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + + // Find default file, but if an argument is given, try loading a file + string filename = findExampleDataFile("dubrovnik-3-7-pre"); + if (argc > 1) + filename = string(argv[1]); + + // Load the SfM data from file + SfM_data mydata; + readBAL(filename, mydata); + cout + << boost::format("read %1% tracks on %2% cameras\n") + % mydata.number_tracks() % mydata.number_cameras(); + + // Create a factor graph + ExpressionFactorGraph graph; + + // Here we don't use a PriorFactor but directly the ExpressionFactor class + // First, we create an expression to the pose from the first camera + Expression camera0_(C(0)); + // Then, to get its pose: + Pose3_ pose0_(&SfM_Camera::getPose, camera0_); + // Finally, we say it should be equal to first guess + graph.addExpressionFactor(pose0_, mydata.cameras[0].pose(), + noiseModel::Isotropic::Sigma(6, 0.1)); + + // similarly, we create a prior on the first point + Point3_ point0_(P(0)); + graph.addExpressionFactor(point0_, mydata.tracks[0].p, + noiseModel::Isotropic::Sigma(3, 0.1)); + + // We share *one* noiseModel between all projection factors + noiseModel::Isotropic::shared_ptr noise = noiseModel::Isotropic::Sigma(2, + 1.0); // one pixel in u and v + + // Simulated measurements from each camera pose, adding them to the factor graph + size_t j = 0; + BOOST_FOREACH(const SfM_Track& track, mydata.tracks) { + // Leaf expression for j^th point + Point3_ point_('p', j); + BOOST_FOREACH(const SfM_Measurement& m, track.measurements) { + size_t i = m.first; + Point2 uv = m.second; + // Leaf expression for i^th camera + Expression camera_(C(i)); + // Below an expression for the prediction of the measurement: + Point2_ predict_ = project2(camera_, point_); + // Again, here we use an ExpressionFactor + graph.addExpressionFactor(predict_, uv, noise); + } + j += 1; + } + + // Create initial estimate + Values initial; + size_t i = 0; + j = 0; + BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) + initial.insert(C(i++), camera); + BOOST_FOREACH(const SfM_Track& track, mydata.tracks) + initial.insert(P(j++), track.p); + + /* Optimize the graph and print results */ + Values result; + try { + LevenbergMarquardtParams params; + params.setVerbosity("ERROR"); + LevenbergMarquardtOptimizer lm(graph, initial, params); + result = lm.optimize(); + } catch (exception& e) { + cout << e.what(); + } + cout << "final error: " << graph.error(result) << endl; + + return 0; +} +/* ************************************************************************* */ + diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index 9e9c74edc..fce046a59 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -17,52 +17,22 @@ * @author Frank Dellaert */ -/** - * 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 - */ - -// For loading the data -#include "SFMdata.h" - -// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). -#include - // In GTSAM, measurement functions are represented as 'factors'. -// The factor we used here is SmartProjectionPoseFactor. Every smart factor represent a single landmark, -// The SmartProjectionPoseFactor only optimize the pose of camera, not the calibration, -// The calibration should be known. +// The factor we used here is SmartProjectionPoseFactor. +// Every smart factor represent a single landmark, seen from multiple cameras. +// The SmartProjectionPoseFactor only optimizes for the poses of a camera, +// not the calibration, which is assumed known. #include -// Also, we will initialize the robot at some location using a Prior factor. -#include - -// When the factors are created, we will add them to a Factor Graph. As the factors we are using -// are nonlinear factors, we will need a Nonlinear Factor Graph. -#include - -// Finally, once all of the factors have been added to our factor graph, we will want to -// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. -// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a -// trust-region method known as Powell's Degleg -#include - -// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the -// nonlinear functions around an initial linearization point, then solve the linear system -// to update the linearization point. This happens repeatedly until the solver converges -// to a consistent set of variable values. This requires us to specify an initial guess -// for each variable, held in a Values container. -#include - -#include +// For an explanation of these headers, see SFMExample.cpp +#include "SFMdata.h" +#include using namespace std; using namespace gtsam; // Make the typename short so it looks much cleaner -typedef gtsam::SmartProjectionPoseFactor SmartFactor; +typedef SmartProjectionPoseFactor SmartFactor; /* ************************************************************************* */ int main(int argc, char* argv[]) { @@ -128,7 +98,8 @@ int main(int argc, char* argv[]) { initialEstimate.print("Initial Estimates:\n"); // Optimize the graph and print results - Values result = DoglegOptimizer(graph, initialEstimate).optimize(); + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); + Values result = optimizer.optimize(); result.print("Final results:\n"); // A smart factor represent the 3D point inside the factor, not as a variable. @@ -137,20 +108,20 @@ int main(int argc, char* argv[]) { Values landmark_result; for (size_t j = 0; j < points.size(); ++j) { - // The output of point() is in boost::optional, as sometimes - // the triangulation operation inside smart factor will encounter degeneracy. - boost::optional point; - // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast(graph[j]); if (smart) { - point = smart->point(result); + // The output of point() is in boost::optional, as sometimes + // the triangulation operation inside smart factor will encounter degeneracy. + boost::optional point = smart->point(result); if (point) // ignore if boost::optional return NULL landmark_result.insert(j, *point); } } landmark_result.print("Landmark results:\n"); + cout << "final error: " << graph.error(result) << endl; + cout << "number of iterations: " << optimizer.iterations() << endl; return 0; } diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp new file mode 100644 index 000000000..49482292f --- /dev/null +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -0,0 +1,126 @@ +/* ---------------------------------------------------------------------------- + + * 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 SFMExample_SmartFactorPCG.cpp + * @brief Version of SFMExample_SmartFactor that uses Preconditioned Conjugate Gradient + * @author Frank Dellaert + */ + +// For an explanation of these headers, see SFMExample_SmartFactor.cpp +#include "SFMdata.h" +#include + +// These extra headers allow us a LM outer loop with PCG linear solver (inner loop) +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// Make the typename short so it looks much cleaner +typedef SmartProjectionPoseFactor SmartFactor; + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + + // Define the camera calibration parameters + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + + // Define the camera observation noise model + noiseModel::Isotropic::shared_ptr measurementNoise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + // Create the set of ground-truth landmarks and poses + vector points = createPoints(); + vector poses = createPoses(); + + // Create a factor graph + NonlinearFactorGraph graph; + + // Simulated measurements from each camera pose, adding them to the factor graph + for (size_t j = 0; j < points.size(); ++j) { + + // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements. + SmartFactor::shared_ptr smartfactor(new SmartFactor()); + + for (size_t i = 0; i < poses.size(); ++i) { + + // generate the 2D measurement + SimpleCamera camera(poses[i], *K); + Point2 measurement = camera.project(points[j]); + + // call add() function to add measurement into a single factor, here we need to add: + smartfactor->add(measurement, i, measurementNoise, K); + } + + // insert the smart factor in the graph + graph.push_back(smartfactor); + } + + // Add a prior on pose x0. This indirectly specifies where the origin is. + // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); + graph.push_back(PriorFactor(0, poses[0], poseNoise)); + + // Fix the scale ambiguity by adding a prior + graph.push_back(PriorFactor(1, poses[1], poseNoise)); + + // Create the initial estimate to the solution + Values initialEstimate; + Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); + for (size_t i = 0; i < poses.size(); ++i) + initialEstimate.insert(i, poses[i].compose(delta)); + + // We will use LM in the outer optimization loop, but by specifying "Iterative" below + // We indicate that an iterative linear solver should be used. + // In addition, the *type* of the iterativeParams decides on the type of + // iterative solver, in this case the SPCG (subgraph PCG) + LevenbergMarquardtParams parameters; + parameters.linearSolverType = NonlinearOptimizerParams::Iterative; + parameters.absoluteErrorTol = 1e-10; + parameters.relativeErrorTol = 1e-10; + parameters.maxIterations = 500; + PCGSolverParameters::shared_ptr pcg = + boost::make_shared(); + pcg->preconditioner_ = + boost::make_shared(); + // Following is crucial: + pcg->setEpsilon_abs(1e-10); + pcg->setEpsilon_rel(1e-10); + parameters.iterativeParams = pcg; + + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); + Values result = optimizer.optimize(); + + // Display result as in SFMExample_SmartFactor.run + result.print("Final results:\n"); + Values landmark_result; + for (size_t j = 0; j < points.size(); ++j) { + SmartFactor::shared_ptr smart = // + boost::dynamic_pointer_cast(graph[j]); + if (smart) { + boost::optional point = smart->point(result); + if (point) // ignore if boost::optional return NULL + landmark_result.insert(j, *point); + } + } + + landmark_result.print("Landmark results:\n"); + cout << "final error: " << graph.error(result) << endl; + cout << "number of iterations: " << optimizer.iterations() << endl; + + return 0; +} +/* ************************************************************************* */ + diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index 43177dc42..0e11adaed 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -43,7 +43,7 @@ int main (int argc, char* argv[]) { // Load the SfM data from file SfM_data mydata; - assert(readBAL(filename, mydata)); + readBAL(filename, mydata); cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras(); // Create a factor graph diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 26abfff85..0393affe1 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -31,28 +31,29 @@ * */ -#include -#include -#include -#include -#include #include #include -#include -#include -#include +#include +#include +#include #include #include #include +#include +#include +#include +#include +#include -#include -#include -#include #include -#include +#include #include #include #include +#include + +#include +#include #ifdef GTSAM_USE_TBB #include @@ -72,23 +73,6 @@ typedef NoiseModelFactor1 NM1; typedef NoiseModelFactor2 NM2; typedef BearingRangeFactor BR; -BOOST_CLASS_EXPORT(Value); -BOOST_CLASS_EXPORT(Pose); -BOOST_CLASS_EXPORT(Rot2); -BOOST_CLASS_EXPORT(Point2); -BOOST_CLASS_EXPORT(NonlinearFactor); -BOOST_CLASS_EXPORT(NoiseModelFactor); -BOOST_CLASS_EXPORT(NM1); -BOOST_CLASS_EXPORT(NM2); -BOOST_CLASS_EXPORT(BetweenFactor); -BOOST_CLASS_EXPORT(PriorFactor); -BOOST_CLASS_EXPORT(BR); -BOOST_CLASS_EXPORT(noiseModel::Base); -BOOST_CLASS_EXPORT(noiseModel::Isotropic); -BOOST_CLASS_EXPORT(noiseModel::Gaussian); -BOOST_CLASS_EXPORT(noiseModel::Diagonal); -BOOST_CLASS_EXPORT(noiseModel::Unit); - double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) { // Compute degrees of freedom (observations - variables) // In ocaml, +1 was added to the observations to account for the prior, but @@ -269,12 +253,12 @@ void runIncremental() boost::dynamic_pointer_cast >(datasetMeasurements[nextMeasurement])) { Key key1 = measurement->key1(), key2 = measurement->key2(); - if((key1 >= firstStep && key1 < key2) || (key2 >= firstStep && key2 < key1)) { + if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) { // We found an odometry starting at firstStep firstPose = std::min(key1, key2); break; } - if((key2 >= firstStep && key1 < key2) || (key1 >= firstStep && key2 < key1)) { + if(((int)key2 >= firstStep && key1 < key2) || ((int)key1 >= firstStep && key2 < key1)) { // We found an odometry joining firstStep with a previous pose havePreviousPose = true; firstPose = std::max(key1, key2); @@ -295,7 +279,7 @@ void runIncremental() NonlinearFactorGraph newFactors; Values newVariables; - newFactors.push_back(boost::make_shared >(firstPose, Pose(), noiseModel::Unit::Create(Pose::Dim()))); + newFactors.push_back(boost::make_shared >(firstPose, Pose(), noiseModel::Unit::Create(3))); newVariables.insert(firstPose, Pose()); isam2.update(newFactors, newVariables); @@ -303,7 +287,9 @@ void runIncremental() cout << "Playing forward time steps..." << endl; - for(size_t step = firstPose; nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || step <= lastStep); ++step) + for (size_t step = firstPose; + nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || (int)step <= lastStep); + ++step) { Values newVariables; NonlinearFactorGraph newFactors; @@ -474,7 +460,7 @@ void runBatch() cout << "Creating batch optimizer..." << endl; NonlinearFactorGraph measurements = datasetMeasurements; - measurements.push_back(boost::make_shared >(0, Pose(), noiseModel::Unit::Create(Pose::Dim()))); + measurements.push_back(boost::make_shared >(0, Pose(), noiseModel::Unit::Create(3))); gttic_(Create_optimizer); GaussNewtonParams params; @@ -589,7 +575,7 @@ void runStats() { cout << "Gathering statistics..." << endl; GaussianFactorGraph linear = *datasetMeasurements.linearize(initial); - GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::COLAMD(linear))); + GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::colamd(linear))); treeTraversal::ForestStatistics statistics = treeTraversal::GatherStatistics(jt); ofstream file; diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp index b9ab663d9..e357745be 100644 --- a/examples/StereoVOExample_large.cpp +++ b/examples/StereoVOExample_large.cpp @@ -103,7 +103,9 @@ int main(int argc, char** argv){ cout << "Optimizing" << endl; //create Levenberg-Marquardt optimizer to optimize the factor graph - LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate); + LevenbergMarquardtParams params; + params.orderingType = Ordering::METIS; + LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params); Values result = optimizer.optimize(); cout << "Final result sample:" << endl; diff --git a/examples/TimeTBB.cpp b/examples/TimeTBB.cpp index 077331848..a35980836 100644 --- a/examples/TimeTBB.cpp +++ b/examples/TimeTBB.cpp @@ -134,7 +134,7 @@ map testWithMemoryAllocation() tbb::parallel_for(tbb::blocked_range(0, numberOfProblems), WorkerWithAllocation(results)); tbb::tick_count t1 = tbb::tick_count::now(); cout << "With memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl; - timingResults[grainSize] = (t1 - t0).seconds(); + timingResults[(int)grainSize] = (t1 - t0).seconds(); } return timingResults; @@ -152,9 +152,9 @@ int main(int argc, char* argv[]) BOOST_FOREACH(size_t n, numThreads) { cout << "With " << n << " threads:" << endl; - tbb::task_scheduler_init init(n); - results[n].grainSizesWithoutAllocation = testWithoutMemoryAllocation(); - results[n].grainSizesWithAllocation = testWithMemoryAllocation(); + tbb::task_scheduler_init init((int)n); + results[(int)n].grainSizesWithoutAllocation = testWithoutMemoryAllocation(); + results[(int)n].grainSizesWithAllocation = testWithMemoryAllocation(); cout << endl; } diff --git a/gtsam.h b/gtsam.h index 9a44c7aa2..70f3b566f 100644 --- a/gtsam.h +++ b/gtsam.h @@ -156,13 +156,7 @@ virtual class Value { size_t dim() const; }; -class Vector3 { - Vector3(Vector v); -}; -class Vector6 { - Vector6(Vector v); -}; -#include +#include class LieScalar { // Standard constructors LieScalar(); @@ -191,7 +185,7 @@ class LieScalar { static Vector Logmap(const gtsam::LieScalar& p); }; -#include +#include class LieVector { // Standard constructors LieVector(); @@ -223,7 +217,7 @@ class LieVector { void serialize() const; }; -#include +#include class LieMatrix { // Standard constructors LieMatrix(); @@ -276,8 +270,6 @@ class Point2 { gtsam::Point2 between(const gtsam::Point2& p2) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::Point2 retract(Vector v) const; Vector localCoordinates(const gtsam::Point2& p) const; @@ -296,6 +288,32 @@ class Point2 { void serialize() const; }; +// std::vector +class Point2Vector +{ + // Constructors + Point2Vector(); + Point2Vector(const gtsam::Point2Vector& v); + + //Capacity + size_t size() const; + size_t max_size() const; + void resize(size_t sz); + size_t capacity() const; + bool empty() const; + void reserve(size_t n); + + //Element access + gtsam::Point2 at(size_t n) const; + gtsam::Point2 front() const; + gtsam::Point2 back() const; + + //Modifiers + void assign(size_t n, const gtsam::Point2& u); + void push_back(const gtsam::Point2& x); + void pop_back(); +}; + class StereoPoint2 { // Standard Constructors StereoPoint2(); @@ -312,8 +330,6 @@ class StereoPoint2 { gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::StereoPoint2 retract(Vector v) const; Vector localCoordinates(const gtsam::StereoPoint2& p) const; @@ -348,8 +364,6 @@ class Point3 { gtsam::Point3 between(const gtsam::Point3& p2) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::Point3 retract(Vector v) const; Vector localCoordinates(const gtsam::Point3& p) const; @@ -386,8 +400,6 @@ class Rot2 { gtsam::Rot2 between(const gtsam::Rot2& p2) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::Rot2 retract(Vector v) const; Vector localCoordinates(const gtsam::Rot2& p) const; @@ -439,8 +451,6 @@ class Rot3 { gtsam::Rot3 between(const gtsam::Rot3& p2) const; // Manifold - static size_t Dim(); - size_t dim() const; //gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options gtsam::Rot3 retract(Vector v) const; Vector localCoordinates(const gtsam::Rot3& p) const; @@ -488,8 +498,6 @@ class Pose2 { gtsam::Pose2 between(const gtsam::Pose2& p2) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::Pose2 retract(Vector v) const; Vector localCoordinates(const gtsam::Pose2& p) const; @@ -537,10 +545,7 @@ class Pose3 { gtsam::Pose3 between(const gtsam::Pose3& p2) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::Pose3 retract(Vector v) const; - gtsam::Pose3 retractFirstOrder(Vector v) const; Vector localCoordinates(const gtsam::Pose3& T2) const; // Lie Group @@ -569,6 +574,16 @@ class Pose3 { void serialize() const; }; +// std::vector +class Pose3Vector +{ + Pose3Vector(); + size_t size() const; + bool empty() const; + gtsam::Pose3 at(size_t n) const; + void push_back(const gtsam::Pose3& x); +}; + #include class Unit3 { // Standard Constructors @@ -648,28 +663,13 @@ class Cal3_S2 { void serialize() const; }; -#include -class Cal3DS2 { +#include +virtual class Cal3DS2_Base { // Standard Constructors - Cal3DS2(); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4); - Cal3DS2(Vector v); + Cal3DS2_Base(); // Testable void print(string s) const; - bool equals(const gtsam::Cal3DS2& rhs, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3DS2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3DS2& c) const; - - // Action on Point2 - gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; - gtsam::Point2 calibrate(const gtsam::Point2& p) const; - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - // TODO: D2d functions that start with an uppercase letter // Standard Interface double fx() const; @@ -677,14 +677,66 @@ class Cal3DS2 { double skew() const; double px() const; double py() const; - Vector vector() const; - Vector k() const; - //Matrix K() const; //FIXME: Uppercase + double k1() const; + double k2() const; + + // Action on Point2 + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; // enabling serialization functionality void serialize() const; }; +#include +virtual class Cal3DS2 : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3DS2(); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2); + Cal3DS2(Vector v); + + // Testable + bool equals(const gtsam::Cal3DS2& rhs, double tol) const; + + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3DS2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3DS2& c) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class Cal3Unified : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3Unified(); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi); + Cal3Unified(Vector v); + + // Testable + bool equals(const gtsam::Cal3Unified& rhs, double tol) const; + + // Standard Interface + double xi() const; + gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const; + gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const; + + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3Unified retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Unified& c) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include class Cal3_S2Stereo { // Standard Constructors Cal3_S2Stereo(); @@ -758,10 +810,6 @@ class CalibratedCamera { gtsam::CalibratedCamera retract(const Vector& d) const; Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; - // Group - gtsam::CalibratedCamera compose(const gtsam::CalibratedCamera& c) const; - gtsam::CalibratedCamera inverse() const; - // Action on Point3 gtsam::Point2 project(const gtsam::Point3& point) const; static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); @@ -774,56 +822,16 @@ class CalibratedCamera { void serialize() const; }; -class SimpleCamera { - // Standard Constructors and Named Constructors - SimpleCamera(); - SimpleCamera(const gtsam::Pose3& pose); - SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K); - static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, - const gtsam::Pose2& pose, double height); - static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height); - static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, - const gtsam::Point3& target, const gtsam::Point3& upVector, - const gtsam::Cal3_S2& K); - - // Testable - void print(string s) const; - bool equals(const gtsam::SimpleCamera& camera, double tol) const; - - // Standard Interface - gtsam::Pose3 pose() const; - gtsam::Cal3_S2 calibration(); - - // Manifold - gtsam::SimpleCamera retract(const Vector& d) const; - Vector localCoordinates(const gtsam::SimpleCamera& T2) const; - size_t dim() const; - static size_t Dim(); - - // Transformations and measurement functions - static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); - pair projectSafe(const gtsam::Point3& pw) const; - gtsam::Point2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& point); - - // enabling serialization functionality - void serialize() const; -}; - -template +template class PinholeCamera { // Standard Constructors and Named Constructors PinholeCamera(); PinholeCamera(const gtsam::Pose3& pose); - PinholeCamera(const gtsam::Pose3& pose, const gtsam::Cal3DS2& K); - static This Level(const gtsam::Cal3DS2& K, - const gtsam::Pose2& pose, double height); + PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); + static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, double height); static This Level(const gtsam::Pose2& pose, double height); - static This Lookat(const gtsam::Point3& eye, - const gtsam::Point3& target, const gtsam::Point3& upVector, - const gtsam::Cal3DS2& K); + static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector, const CALIBRATION& K); // Testable void print(string s) const; @@ -851,6 +859,50 @@ class PinholeCamera { void serialize() const; }; +virtual class SimpleCamera { + // Standard Constructors and Named Constructors + SimpleCamera(); + SimpleCamera(const gtsam::Pose3& pose); + SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K); + static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height); + static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height); + static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector, const gtsam::Cal3_S2& K); + + // Testable + void print(string s) const; + bool equals(const gtsam::SimpleCamera& camera, double tol) const; + + // Standard Interface + gtsam::Pose3 pose() const; + gtsam::Cal3_S2 calibration() const; + + // Manifold + gtsam::SimpleCamera retract(const Vector& d) const; + Vector localCoordinates(const gtsam::SimpleCamera& T2) const; + size_t dim() const; + static size_t Dim(); + + // Transformations and measurement functions + static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); + pair projectSafe(const gtsam::Point3& pw) const; + gtsam::Point2 project(const gtsam::Point3& point); + gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& point); + + // enabling serialization functionality + void serialize() const; + +}; + +// Some typedefs for common camera types +// PinholeCameraCal3_S2 is the same as SimpleCamera above +typedef gtsam::PinholeCamera PinholeCameraCal3_S2; +typedef gtsam::PinholeCamera PinholeCameraCal3DS2; +typedef gtsam::PinholeCamera PinholeCameraCal3Unified; +typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; + class StereoCamera { // Standard Constructors and Named Constructors StereoCamera(); @@ -879,6 +931,16 @@ class StereoCamera { void serialize() const; }; +#include + +// Templates appear not yet supported for free functions +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); + //************************************************************************* // Symbolic //************************************************************************* @@ -1234,6 +1296,7 @@ class VectorValues { #include virtual class GaussianFactor { + gtsam::KeyVector keys() const; void print(string s) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const; double error(const gtsam::VectorValues& c) const; @@ -1657,6 +1720,7 @@ class NonlinearFactorGraph { void push_back(gtsam::NonlinearFactor* factor); void add(gtsam::NonlinearFactor* factor); bool exists(size_t idx) const; + gtsam::KeySet keys() const; // NonlinearFactorGraph double error(const gtsam::Values& values) const; @@ -1727,6 +1791,7 @@ class Values { // void insert(size_t j, const gtsam::Value& value); // void update(size_t j, const gtsam::Value& val); // gtsam::Value at(size_t j) const; + void insert(size_t j, const gtsam::Point2& t); void insert(size_t j, const gtsam::Point3& t); void insert(size_t j, const gtsam::Rot2& t); @@ -1737,7 +1802,13 @@ class Values { void insert(size_t j, const gtsam::Cal3DS2& t); void insert(size_t j, const gtsam::Cal3Bundler& t); void insert(size_t j, const gtsam::EssentialMatrix& t); + void insert(size_t j, const gtsam::SimpleCamera& t); void insert(size_t j, const gtsam::imuBias::ConstantBias& t); + void insert(size_t j, Vector t); + void insert(size_t j, Matrix t); + + // Fixed size version + void insertFixed(size_t j, Vector t, size_t n); void update(size_t j, const gtsam::Point2& t); void update(size_t j, const gtsam::Point3& t); @@ -1750,10 +1821,21 @@ class Values { void update(size_t j, const gtsam::Cal3Bundler& t); void update(size_t j, const gtsam::EssentialMatrix& t); void update(size_t j, const gtsam::imuBias::ConstantBias& t); + void update(size_t j, Vector t); + void update(size_t j, Matrix t); - template + template T at(size_t j); + + /// Fixed size versions, for n in 1..9 + void insertFixed(size_t j, Vector t, size_t n); + + /// Fixed size versions, for n in 1..9 + Vector atFixed(size_t j, size_t n); + + /// version for double + void insertDouble(size_t j, double c); + double atDouble(size_t j) const; }; // Actually a FastList @@ -2142,15 +2224,12 @@ class NonlinearISAM { //************************************************************************* // Nonlinear factor types //************************************************************************* -#include -#include -#include #include #include #include #include -template +template virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2195,10 +2274,14 @@ typedef gtsam::RangeFactor RangeFactorPosePoint2; typedef gtsam::RangeFactor RangeFactorPosePoint3; typedef gtsam::RangeFactor RangeFactorPose2; typedef gtsam::RangeFactor RangeFactorPose3; -typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; -typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; -typedef gtsam::RangeFactor RangeFactorCalibratedCamera; -typedef gtsam::RangeFactor RangeFactorSimpleCamera; + +// Commented out by Frank Dec 2014: not poses! +// If needed, we need a RangeFactor that takes a camera, extracts the pose +// Should be easy with Expressions +//typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; +//typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; +//typedef gtsam::RangeFactor RangeFactorCalibratedCamera; +//typedef gtsam::RangeFactor RangeFactorSimpleCamera; #include @@ -2272,23 +2355,23 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { }; #include -template +template virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { SmartProjectionPoseFactor(double rankTol, double linThreshold, - bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor); + bool manageDegeneracy, bool enableEPI, const gtsam::Pose3& body_P_sensor); SmartProjectionPoseFactor(double rankTol); SmartProjectionPoseFactor(); - void add(const gtsam::Point2& measured_i, size_t poseKey_i, const gtsam::noiseModel::Base* noise_i, - const CALIBRATION* K_i); + void add(const gtsam::Point2& measured_i, size_t poseKey_i, + const gtsam::noiseModel::Base* noise_i, const CALIBRATION* K_i); // enabling serialization functionality //void serialize() const; }; -typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; +typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; #include @@ -2370,8 +2453,6 @@ class ConstantBias { gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::imuBias::ConstantBias retract(Vector v) const; Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; @@ -2390,25 +2471,24 @@ class ConstantBias { }///\namespace imuBias #include -class PoseVelocity{ - PoseVelocity(const gtsam::Pose3& pose, const gtsam::Vector3 velocity); +class PoseVelocityBias{ + PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias); }; class ImuFactorPreintegratedMeasurements { // Standard Constructor ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration); ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); - ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs); + // ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs); // Testable void print(string s) const; bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol); - Matrix measurementCovariance() const; - Matrix deltaRij() const; double deltaTij() const; + Matrix deltaRij() const; Vector deltaPij() const; Vector deltaVij() const; - Vector biasHat() const; + Vector biasHatVector() const; Matrix delPdelBiasAcc() const; Matrix delPdelBiasOmega() const; Matrix delVdelBiasAcc() const; @@ -2416,10 +2496,11 @@ class ImuFactorPreintegratedMeasurements { Matrix delRdelBiasOmega() const; Matrix preintMeasCov() const; - // Standard Interface void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); + gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, + Vector gravity, Vector omegaCoriolis) const; }; virtual class ImuFactor : gtsam::NonlinearFactor { @@ -2430,16 +2511,65 @@ virtual class ImuFactor : gtsam::NonlinearFactor { const gtsam::Pose3& body_P_sensor); // Standard Interface gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias, - const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, +}; + +#include +class CombinedImuFactorPreintegratedMeasurements { + // Standard Constructor + CombinedImuFactorPreintegratedMeasurements( + const gtsam::imuBias::ConstantBias& bias, + Matrix measuredAccCovariance, + Matrix measuredOmegaCovariance, + Matrix integrationErrorCovariance, + Matrix biasAccCovariance, + Matrix biasOmegaCovariance, + Matrix biasAccOmegaInit); + CombinedImuFactorPreintegratedMeasurements( + const gtsam::imuBias::ConstantBias& bias, + Matrix measuredAccCovariance, + Matrix measuredOmegaCovariance, + Matrix integrationErrorCovariance, + Matrix biasAccCovariance, + Matrix biasOmegaCovariance, + Matrix biasAccOmegaInit, + bool use2ndOrderIntegration); + // CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs); + + // Testable + void print(string s) const; + bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol); + + double deltaTij() const; + Matrix deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + Vector biasHatVector() const; + Matrix delPdelBiasAcc() const; + Matrix delPdelBiasOmega() const; + Matrix delVdelBiasAcc() const; + Matrix delVdelBiasOmega() const; + Matrix delRdelBiasOmega() const; + Matrix preintMeasCov() const; + + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); + gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, Vector gravity, Vector omegaCoriolis) const; }; +virtual class CombinedImuFactor : gtsam::NonlinearFactor { + CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, + const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); + // Standard Interface + gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; +}; + #include class AHRSFactorPreintegratedMeasurements { // Standard Constructor - AHRSFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredOmegaCovariance); - AHRSFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredOmegaCovariance); + AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance); + AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance); AHRSFactorPreintegratedMeasurements(const gtsam::AHRSFactorPreintegratedMeasurements& rhs); // Testable @@ -2447,7 +2577,6 @@ class AHRSFactorPreintegratedMeasurements { bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol); // get Data - Matrix measurementCovariance() const; Matrix deltaRij() const; double deltaTij() const; Vector biasHat() const; @@ -2468,70 +2597,12 @@ virtual class AHRSFactor : gtsam::NonlinearFactor { // Standard Interface gtsam::AHRSFactorPreintegratedMeasurements preintegratedMeasurements() const; Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, - const gtsam::imuBias::ConstantBias& bias) const; - gtsam::Rot3 predict(const gtsam::Rot3& rot_i, const gtsam::imuBias::ConstantBias& bias, + Vector bias) const; + gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis) const; }; -#include -class PoseVelocityBias{ - PoseVelocityBias(const gtsam::Pose3& pose, const gtsam::Vector3 velocity, const gtsam::imuBias::ConstantBias& bias); - }; -class CombinedImuFactorPreintegratedMeasurements { - // Standard Constructor - CombinedImuFactorPreintegratedMeasurements( - const gtsam::imuBias::ConstantBias& bias, - Matrix measuredAccCovariance, - Matrix measuredOmegaCovariance, - Matrix integrationErrorCovariance, - Matrix biasAccCovariance, - Matrix biasOmegaCovariance, - Matrix biasAccOmegaInit); - CombinedImuFactorPreintegratedMeasurements( - const gtsam::imuBias::ConstantBias& bias, - Matrix measuredAccCovariance, - Matrix measuredOmegaCovariance, - Matrix integrationErrorCovariance, - Matrix biasAccCovariance, - Matrix biasOmegaCovariance, - Matrix biasAccOmegaInit, - bool use2ndOrderIntegration); - CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs); - - // Testable - void print(string s) const; - bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol); - - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); - - Matrix measurementCovariance() const; - Matrix deltaRij() const; - double deltaTij() const; - Vector deltaPij() const; - Vector deltaVij() const; - Vector biasHat() const; - Matrix delPdelBiasAcc() const; - Matrix delPdelBiasOmega() const; - Matrix delVdelBiasAcc() const; - Matrix delVdelBiasOmega() const; - Matrix delRdelBiasOmega() const; - Matrix PreintMeasCov() const; -}; - -virtual class CombinedImuFactor : gtsam::NonlinearFactor { - CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, - const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); - - // Standard Interface - gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias_i, - const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, - Vector gravity, Vector omegaCoriolis); -}; - #include //virtual class AttitudeFactor : gtsam::NonlinearFactor { // AttitudeFactor(const Unit3& nZ, const Unit3& bRef); diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 576da93bd..4adbfb250 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -18,7 +18,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) # do the same for the unsupported eigen folder file(GLOB_RECURSE unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h") - + file(GLOB unsupported_eigen_dir_headers_all "Eigen/unsupported/Eigen/*") foreach(unsupported_eigen_dir ${unsupported_eigen_dir_headers_all}) get_filename_component(filename ${unsupported_eigen_dir} NAME) @@ -36,17 +36,17 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) install(DIRECTORY Eigen/Eigen DESTINATION include/gtsam/3rdparty/Eigen FILES_MATCHING PATTERN "*.h") - + install(DIRECTORY Eigen/unsupported/Eigen DESTINATION include/gtsam/3rdparty/Eigen/unsupported/ FILES_MATCHING PATTERN "*.h") endif() -option(GTSAM_BUILD_METIS "Build metis library" ON) option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF) -if(GTSAM_BUILD_METIS) - add_subdirectory(metis-5.1.0) -endif(GTSAM_BUILD_METIS) +add_subdirectory(metis) + +add_subdirectory(ceres) + ############ NOTE: When updating GeographicLib be sure to disable building their examples ############ and unit tests by commenting out their lines: # add_subdirectory (examples) @@ -58,10 +58,10 @@ endif(GTSAM_BUILD_METIS) include(GeographicLib/cmake/FindGeographicLib.cmake) # Set up the option to install GeographicLib -if(GEOGRAPHICLIB_FOUND) - set(install_geographiclib_default OFF) -else() +if(GEOGRAPHICLIB-NOTFOUND) set(install_geographiclib_default ON) +else() + set(install_geographiclib_default OFF) endif() option(GTSAM_INSTALL_GEOGRAPHICLIB "Build and install the 3rd-party library GeographicLib" ${install_geographiclib_default}) @@ -73,3 +73,5 @@ endif() if(GTSAM_INSTALL_GEOGRAPHICLIB) add_subdirectory(GeographicLib) endif() + +set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) diff --git a/gtsam/3rdparty/Eigen/.hg_archival.txt b/gtsam/3rdparty/Eigen/.hg_archival.txt index 9fceca658..aea5a5515 100644 --- a/gtsam/3rdparty/Eigen/.hg_archival.txt +++ b/gtsam/3rdparty/Eigen/.hg_archival.txt @@ -1,4 +1,4 @@ repo: 8a21fd850624c931e448cbcfb38168cb2717c790 -node: ffa86ffb557094721ca71dcea6aed2651b9fd610 +node: 10219c95fe653d4962aa9db4946f6fbea96dd740 branch: 3.2 -tag: 3.2.0 +tag: 3.2.4 diff --git a/gtsam/3rdparty/Eigen/.hgtags b/gtsam/3rdparty/Eigen/.hgtags index 1b9b1142e..7a6e19411 100644 --- a/gtsam/3rdparty/Eigen/.hgtags +++ b/gtsam/3rdparty/Eigen/.hgtags @@ -23,3 +23,7 @@ bf4cb8c934fa3a79f45f1e629610f0225e93e493 3.1.0-rc2 da195914abcc1d739027cbee7c52077aab30b336 3.2-beta1 4b687cad1d23066f66863f4f87298447298443df 3.2-rc1 1eeda7b1258bcd306018c0738e2b6a8543661141 3.2-rc2 +ffa86ffb557094721ca71dcea6aed2651b9fd610 3.2.0 +6b38706d90a9fe182e66ab88477b3dbde34b9f66 3.2.1 +1306d75b4a21891e59ff9bd96678882cf831e39f 3.2.2 +36fd1ba04c120cfdd90f3e4cede47f43b21d19ad 3.2.3 diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h index c52b7d1a6..02ab93880 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h @@ -442,6 +442,7 @@ LDLT& LDLT::compute(const MatrixType& a) m_transpositions.resize(size); m_isInitialized = false; m_temporary.resize(size); + m_sign = internal::ZeroSign; internal::ldlt_inplace::unblocked(m_matrix, m_transpositions, m_temporary, m_sign); @@ -502,7 +503,6 @@ struct solve_retval, Rhs> using std::abs; using std::max; typedef typename LDLTType::MatrixType MatrixType; - typedef typename LDLTType::Scalar Scalar; typedef typename LDLTType::RealScalar RealScalar; const typename Diagonal::RealReturnType vectorD(dec().vectorD()); // In some previous versions, tolerance was set to the max of 1/highest and the maximal diagonal entry * epsilon diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h index a791bc358..b4641e2a0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h @@ -29,6 +29,11 @@ struct traits > : public traits::type > { typedef ArrayXpr XprKind; + // Let's remove NestByRefBit + enum { + Flags0 = traits::type >::Flags, + Flags = Flags0 & ~NestByRefBit + }; }; } @@ -149,6 +154,11 @@ struct traits > : public traits::type > { typedef MatrixXpr XprKind; + // Let's remove NestByRefBit + enum { + Flags0 = traits::type >::Flags, + Flags = Flags0 & ~NestByRefBit + }; }; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h index c5800f6c8..04862f374 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h @@ -462,8 +462,10 @@ template class DenseBase template RealScalar lpNorm() const; template - const Replicate replicate() const; - const Replicate replicate(Index rowFacor,Index colFactor) const; + inline const Replicate replicate() const; + + typedef Replicate ReplicateReturnType; + inline const ReplicateReturnType replicate(Index rowFacor,Index colFactor) const; typedef Reverse ReverseReturnType; typedef const Reverse ConstReverseReturnType; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h index aab8007b3..68cf6d4b0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h @@ -190,18 +190,18 @@ MatrixBase::diagonal() const * * \sa MatrixBase::diagonal(), class Diagonal */ template -inline typename MatrixBase::template DiagonalIndexReturnType::Type +inline typename MatrixBase::DiagonalDynamicIndexReturnType MatrixBase::diagonal(Index index) { - return typename DiagonalIndexReturnType::Type(derived(), index); + return DiagonalDynamicIndexReturnType(derived(), index); } /** This is the const version of diagonal(Index). */ template -inline typename MatrixBase::template ConstDiagonalIndexReturnType::Type +inline typename MatrixBase::ConstDiagonalDynamicIndexReturnType MatrixBase::diagonal(Index index) const { - return typename ConstDiagonalIndexReturnType::Type(derived(), index); + return ConstDiagonalDynamicIndexReturnType(derived(), index); } /** \returns an expression of the \a DiagIndex-th sub or super diagonal of the matrix \c *this diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h index 2a59d9464..9e805a80f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h @@ -232,7 +232,7 @@ EIGEN_DONT_INLINE void outer_product_selector_run(const ProductType& prod, Dest& // FIXME not very good if rhs is real and lhs complex while alpha is real too const Index cols = dest.cols(); for (Index j=0; j diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h index ab50c9b81..cebed2bb6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h @@ -168,6 +168,7 @@ template class MapBase template class MapBase : public MapBase { + typedef MapBase ReadOnlyMapBase; public: typedef MapBase Base; @@ -230,11 +231,13 @@ template class MapBase Derived& operator=(const MapBase& other) { - Base::Base::operator=(other); + ReadOnlyMapBase::Base::operator=(other); return derived(); } - using Base::Base::operator=; + // In theory we could simply refer to Base:Base::operator=, but MSVC does not like Base::Base, + // see bugs 821 and 920. + using ReadOnlyMapBase::Base::operator=; }; #undef EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h index 344b38f2f..cc3279746 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h @@ -215,7 +215,7 @@ template class MatrixBase typedef Diagonal DiagonalReturnType; DiagonalReturnType diagonal(); - typedef typename internal::add_const >::type ConstDiagonalReturnType; + typedef typename internal::add_const >::type ConstDiagonalReturnType; ConstDiagonalReturnType diagonal() const; template struct DiagonalIndexReturnType { typedef Diagonal Type; }; @@ -223,16 +223,12 @@ template class MatrixBase template typename DiagonalIndexReturnType::Type diagonal(); template typename ConstDiagonalIndexReturnType::Type diagonal() const; + + typedef Diagonal DiagonalDynamicIndexReturnType; + typedef typename internal::add_const >::type ConstDiagonalDynamicIndexReturnType; - // Note: The "MatrixBase::" prefixes are added to help MSVC9 to match these declarations with the later implementations. - // On the other hand they confuse MSVC8... - #if (defined _MSC_VER) && (_MSC_VER >= 1500) // 2008 or later - typename MatrixBase::template DiagonalIndexReturnType::Type diagonal(Index index); - typename MatrixBase::template ConstDiagonalIndexReturnType::Type diagonal(Index index) const; - #else - typename DiagonalIndexReturnType::Type diagonal(Index index); - typename ConstDiagonalIndexReturnType::Type diagonal(Index index) const; - #endif + DiagonalDynamicIndexReturnType diagonal(Index index); + ConstDiagonalDynamicIndexReturnType diagonal(Index index) const; #ifdef EIGEN2_SUPPORT template typename internal::eigen2_part_return_type::type part(); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h index 1297b8413..f26f3e5cc 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h @@ -555,7 +555,10 @@ struct permut_matrix_product_retval const Index n = Side==OnTheLeft ? rows() : cols(); // FIXME we need an is_same for expression that is not sensitive to constness. For instance // is_same_xpr, Block >::value should be true. - if(is_same::value && extract_data(dst) == extract_data(m_matrix)) + if( is_same::value + && blas_traits::HasUsableDirectAccess + && blas_traits::HasUsableDirectAccess + && extract_data(dst) == extract_data(m_matrix)) { // apply the permutation inplace Matrix mask(m_permutation.size()); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h index a494b5f87..cf74470a9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h @@ -85,7 +85,14 @@ class ProductBase : public MatrixBase public: +#ifndef EIGEN_NO_MALLOC + typedef typename Base::PlainObject BasePlainObject; + typedef Matrix DynPlainObject; + typedef typename internal::conditional<(BasePlainObject::SizeAtCompileTime==Dynamic) || (BasePlainObject::SizeAtCompileTime*int(sizeof(Scalar)) < int(EIGEN_STACK_ALLOCATION_LIMIT)), + BasePlainObject, DynPlainObject>::type PlainObject; +#else typedef typename Base::PlainObject PlainObject; +#endif ProductBase(const Lhs& a_lhs, const Rhs& a_rhs) : m_lhs(a_lhs), m_rhs(a_rhs) @@ -180,7 +187,12 @@ namespace internal { template struct nested, N, PlainObject> { - typedef PlainObject const& type; + typedef typename GeneralProduct::PlainObject const& type; +}; +template +struct nested, N, PlainObject> +{ + typedef typename GeneralProduct::PlainObject const& type; }; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h index cd6d949c4..df87b1af4 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h @@ -188,6 +188,8 @@ template class Ref : public RefBase > { typedef internal::traits Traits; + template + inline Ref(const PlainObjectBase& expr); public: typedef RefBase Base; @@ -196,20 +198,21 @@ template class Ref #ifndef EIGEN_PARSED_BY_DOXYGEN template - inline Ref(PlainObjectBase& expr, - typename internal::enable_if::MatchAtCompileTime),Derived>::type* = 0) + inline Ref(PlainObjectBase& expr) { - Base::construct(expr); + EIGEN_STATIC_ASSERT(static_cast(Traits::template match::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH); + Base::construct(expr.derived()); } template - inline Ref(const DenseBase& expr, - typename internal::enable_if::value&&bool(Traits::template match::MatchAtCompileTime)),Derived>::type* = 0, - int = Derived::ThisConstantIsPrivateInPlainObjectBase) + inline Ref(const DenseBase& expr) #else template inline Ref(DenseBase& expr) #endif { + EIGEN_STATIC_ASSERT(static_cast(internal::is_lvalue::value), THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY); + EIGEN_STATIC_ASSERT(static_cast(Traits::template match::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH); + enum { THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY = Derived::ThisConstantIsPrivateInPlainObjectBase}; Base::construct(expr.const_cast_derived()); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h index dde86a834..ac4537c14 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h @@ -135,7 +135,7 @@ template class Replicate */ template template -inline const Replicate +const Replicate DenseBase::replicate() const { return Replicate(derived()); @@ -150,7 +150,7 @@ DenseBase::replicate() const * \sa VectorwiseOp::replicate(), DenseBase::replicate(), class Replicate */ template -inline const Replicate +const typename DenseBase::ReplicateReturnType DenseBase::replicate(Index rowFactor,Index colFactor) const { return Replicate(derived(),rowFactor,colFactor); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h index 845ae1aec..4d65392c6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h @@ -380,19 +380,19 @@ template class TriangularView EIGEN_STRONG_INLINE TriangularView& operator=(const ProductBase& other) { setZero(); - return assignProduct(other,1); + return assignProduct(other.derived(),1); } template EIGEN_STRONG_INLINE TriangularView& operator+=(const ProductBase& other) { - return assignProduct(other,1); + return assignProduct(other.derived(),1); } template EIGEN_STRONG_INLINE TriangularView& operator-=(const ProductBase& other) { - return assignProduct(other,-1); + return assignProduct(other.derived(),-1); } @@ -400,25 +400,34 @@ template class TriangularView EIGEN_STRONG_INLINE TriangularView& operator=(const ScaledProduct& other) { setZero(); - return assignProduct(other,other.alpha()); + return assignProduct(other.derived(),other.alpha()); } template EIGEN_STRONG_INLINE TriangularView& operator+=(const ScaledProduct& other) { - return assignProduct(other,other.alpha()); + return assignProduct(other.derived(),other.alpha()); } template EIGEN_STRONG_INLINE TriangularView& operator-=(const ScaledProduct& other) { - return assignProduct(other,-other.alpha()); + return assignProduct(other.derived(),-other.alpha()); } protected: template EIGEN_STRONG_INLINE TriangularView& assignProduct(const ProductBase& prod, const Scalar& alpha); + + template + EIGEN_STRONG_INLINE TriangularView& assignProduct(const TriangularProduct& prod, const Scalar& alpha) + { + lazyAssign(alpha*prod.eval()); + return *this; + } MatrixTypeNested m_matrix; }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h index f183d31de..8d9255eef 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h @@ -110,7 +110,7 @@ template<> EIGEN_STRONG_INLINE Packet2cf ploaddup(const std::complex< template<> EIGEN_STRONG_INLINE void pstore >(std::complex * to, const Packet2cf& from) { EIGEN_DEBUG_ALIGNED_STORE pstore((float*)to, from.v); } template<> EIGEN_STRONG_INLINE void pstoreu >(std::complex * to, const Packet2cf& from) { EIGEN_DEBUG_UNALIGNED_STORE pstoreu((float*)to, from.v); } -template<> EIGEN_STRONG_INLINE void prefetch >(const std::complex * addr) { __pld((float *)addr); } +template<> EIGEN_STRONG_INLINE void prefetch >(const std::complex * addr) { EIGEN_ARM_PREFETCH((float *)addr); } template<> EIGEN_STRONG_INLINE std::complex pfirst(const Packet2cf& a) { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h index 163bac215..94dfab330 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h @@ -48,9 +48,18 @@ typedef uint32x4_t Packet4ui; #define EIGEN_INIT_NEON_PACKET2(X, Y) {X, Y} #define EIGEN_INIT_NEON_PACKET4(X, Y, Z, W) {X, Y, Z, W} #endif - -#ifndef __pld -#define __pld(x) asm volatile ( " pld [%[addr]]\n" :: [addr] "r" (x) : "cc" ); + +// arm64 does have the pld instruction. If available, let's trust the __builtin_prefetch built-in function +// which available on LLVM and GCC (at least) +#if EIGEN_HAS_BUILTIN(__builtin_prefetch) || defined(__GNUC__) + #define EIGEN_ARM_PREFETCH(ADDR) __builtin_prefetch(ADDR); +#elif defined __pld + #define EIGEN_ARM_PREFETCH(ADDR) __pld(ADDR) +#elif !defined(__aarch64__) + #define EIGEN_ARM_PREFETCH(ADDR) __asm__ __volatile__ ( " pld [%[addr]]\n" :: [addr] "r" (ADDR) : "cc" ); +#else + // by default no explicit prefetching + #define EIGEN_ARM_PREFETCH(ADDR) #endif template<> struct packet_traits : default_packet_traits @@ -209,8 +218,8 @@ template<> EIGEN_STRONG_INLINE void pstore(int* to, const Packet4i& f template<> EIGEN_STRONG_INLINE void pstoreu(float* to, const Packet4f& from) { EIGEN_DEBUG_UNALIGNED_STORE vst1q_f32(to, from); } template<> EIGEN_STRONG_INLINE void pstoreu(int* to, const Packet4i& from) { EIGEN_DEBUG_UNALIGNED_STORE vst1q_s32(to, from); } -template<> EIGEN_STRONG_INLINE void prefetch(const float* addr) { __pld(addr); } -template<> EIGEN_STRONG_INLINE void prefetch(const int* addr) { __pld(addr); } +template<> EIGEN_STRONG_INLINE void prefetch(const float* addr) { EIGEN_ARM_PREFETCH(addr); } +template<> EIGEN_STRONG_INLINE void prefetch(const int* addr) { EIGEN_ARM_PREFETCH(addr); } // FIXME only store the 2 first elements ? template<> EIGEN_STRONG_INLINE float pfirst(const Packet4f& a) { float EIGEN_ALIGN16 x[4]; vst1q_f32(x, a); return x[0]; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h index 99cbd0d95..d16f30bb0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h @@ -52,7 +52,7 @@ Packet4f plog(const Packet4f& _x) Packet4i emm0; - Packet4f invalid_mask = _mm_cmplt_ps(x, _mm_setzero_ps()); + Packet4f invalid_mask = _mm_cmpnge_ps(x, _mm_setzero_ps()); // not greater equal is true if x is NaN Packet4f iszero_mask = _mm_cmpeq_ps(x, _mm_setzero_ps()); x = pmax(x, p4f_min_norm_pos); /* cut off denormalized stuff */ @@ -166,7 +166,7 @@ Packet4f pexp(const Packet4f& _x) emm0 = _mm_cvttps_epi32(fx); emm0 = _mm_add_epi32(emm0, p4i_0x7f); emm0 = _mm_slli_epi32(emm0, 23); - return pmul(y, _mm_castsi128_ps(emm0)); + return pmax(pmul(y, Packet4f(_mm_castsi128_ps(emm0))), _x); } template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2d pexp(const Packet2d& _x) @@ -239,7 +239,7 @@ Packet2d pexp(const Packet2d& _x) emm0 = _mm_add_epi32(emm0, p4i_1023_0); emm0 = _mm_slli_epi32(emm0, 20); emm0 = _mm_shuffle_epi32(emm0, _MM_SHUFFLE(1,2,0,3)); - return pmul(x, _mm_castsi128_pd(emm0)); + return pmax(pmul(x, Packet2d(_mm_castsi128_pd(emm0))), _x); } /* evaluation of 4 sines at onces, using SSE2 intrinsics. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h index c06a0df1c..421f925e1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h @@ -90,6 +90,7 @@ struct traits > | (SameType && (CanVectorizeLhs || CanVectorizeRhs) ? PacketAccessBit : 0), CoeffReadCost = InnerSize == Dynamic ? Dynamic + : InnerSize == 0 ? 0 : InnerSize * (NumTraits::MulCost + LhsCoeffReadCost + RhsCoeffReadCost) + (InnerSize - 1) * NumTraits::AddCost, @@ -133,7 +134,7 @@ class CoeffBasedProduct }; typedef internal::product_coeff_impl ScalarCoeffImpl; typedef CoeffBasedProduct LazyCoeffBasedProductType; @@ -184,7 +185,7 @@ class CoeffBasedProduct { PacketScalar res; internal::product_packet_impl ::run(row, col, m_lhs, m_rhs, res); return res; @@ -262,10 +263,7 @@ struct product_coeff_impl typedef typename Lhs::Index Index; static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, RetScalar& res) { - eigen_assert(lhs.cols()>0 && "you are using a non initialized matrix"); - res = lhs.coeff(row, 0) * rhs.coeff(0, col); - for(Index i = 1; i < lhs.cols(); ++i) - res += lhs.coeff(row, i) * rhs.coeff(i, col); + res = (lhs.row(row).transpose().cwiseProduct( rhs.col(col) )).sum(); } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h index 3a010ec6a..6d1e6c133 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h @@ -13,7 +13,7 @@ #define EIGEN_WORLD_VERSION 3 #define EIGEN_MAJOR_VERSION 2 -#define EIGEN_MINOR_VERSION 2 +#define EIGEN_MINOR_VERSION 4 #define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \ (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \ @@ -96,6 +96,13 @@ #define EIGEN_DEFAULT_DENSE_INDEX_TYPE std::ptrdiff_t #endif +// Cross compiler wrapper around LLVM's __has_builtin +#ifdef __has_builtin +# define EIGEN_HAS_BUILTIN(x) __has_builtin(x) +#else +# define EIGEN_HAS_BUILTIN(x) 0 +#endif + /** Allows to disable some optimizations which might affect the accuracy of the result. * Such optimization are enabled by default, and set EIGEN_FAST_MATH to 0 to disable them. * They currently include: @@ -247,7 +254,7 @@ namespace Eigen { #if !defined(EIGEN_ASM_COMMENT) #if (defined __GNUC__) && ( defined(__i386__) || defined(__x86_64__) ) - #define EIGEN_ASM_COMMENT(X) asm("#" X) + #define EIGEN_ASM_COMMENT(X) __asm__("#" X) #else #define EIGEN_ASM_COMMENT(X) #endif @@ -306,7 +313,7 @@ namespace Eigen { // just an empty macro ! #define EIGEN_EMPTY -#if defined(_MSC_VER) && (!defined(__INTEL_COMPILER)) +#if defined(_MSC_VER) && (_MSC_VER < 1900) && (!defined(__INTEL_COMPILER)) #define EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \ using Base::operator =; #elif defined(__clang__) // workaround clang bug (see http://forum.kde.org/viewtopic.php?f=74&t=102653) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h index 6c2461725..41dd7db06 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h @@ -63,7 +63,7 @@ // Currently, let's include it only on unix systems: #if defined(__unix__) || defined(__unix) #include - #if ((defined __QNXNTO__) || (defined _GNU_SOURCE) || ((defined _XOPEN_SOURCE) && (_XOPEN_SOURCE >= 600))) && (defined _POSIX_ADVISORY_INFO) && (_POSIX_ADVISORY_INFO > 0) + #if ((defined __QNXNTO__) || (defined _GNU_SOURCE) || (defined __PGI) || ((defined _XOPEN_SOURCE) && (_XOPEN_SOURCE >= 600))) && (defined _POSIX_ADVISORY_INFO) && (_POSIX_ADVISORY_INFO > 0) #define EIGEN_HAS_POSIX_MEMALIGN 1 #endif #endif @@ -417,6 +417,8 @@ template inline T* conditional_aligned_realloc_new(T* pt template inline T* conditional_aligned_new_auto(size_t size) { + if(size==0) + return 0; // short-cut. Also fixes Bug 884 check_size_for_overflow(size); T *result = reinterpret_cast(conditional_aligned_malloc(sizeof(T)*size)); if(NumTraits::RequireInitialization) @@ -464,9 +466,8 @@ template inline void conditional_aligned_delete_auto(T * template static inline Index first_aligned(const Scalar* array, Index size) { - enum { PacketSize = packet_traits::size, - PacketAlignedMask = PacketSize-1 - }; + static const Index PacketSize = packet_traits::size; + static const Index PacketAlignedMask = PacketSize-1; if(PacketSize==1) { @@ -612,7 +613,6 @@ template class aligned_stack_memory_handler void* operator new(size_t size, const std::nothrow_t&) throw() { \ try { return Eigen::internal::conditional_aligned_malloc(size); } \ catch (...) { return 0; } \ - return 0; \ } #else #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign) \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h index 8872c5b64..bac5d9fe9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h @@ -90,7 +90,9 @@ YOU_PASSED_A_COLUMN_VECTOR_BUT_A_ROW_VECTOR_WAS_EXPECTED, THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE, THE_STORAGE_ORDER_OF_BOTH_SIDES_MUST_MATCH, - OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG + OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG, + IMPLICIT_CONVERSION_TO_SCALAR_IS_FOR_INNER_PRODUCT_ONLY, + STORAGE_LAYOUT_DOES_NOT_MATCH }; }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h index 3c4773054..781965d2c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h @@ -341,7 +341,7 @@ template::type> str }; template -T* const_cast_ptr(const T* ptr) +inline T* const_cast_ptr(const T* ptr) { return const_cast(ptr); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/LeastSquares.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/LeastSquares.h index 0e6fdb488..7992d4944 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/LeastSquares.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/LeastSquares.h @@ -147,7 +147,6 @@ void fitHyperplane(int numPoints, // compute the covariance matrix CovMatrixType covMat = CovMatrixType::Zero(size, size); - VectorType remean = VectorType::Zero(size); for(int i = 0; i < numPoints; ++i) { VectorType diff = (*(points[i]) - mean).conjugate(); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h index 5706eeebe..4f36091db 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h @@ -313,7 +313,7 @@ namespace Eigen { using std::abs; using std::sqrt; const Index dim=m_S.cols(); - if (abs(m_S.coeff(i+1,i)==Scalar(0))) + if (abs(m_S.coeff(i+1,i))==Scalar(0)) return; Index z = findSmallDiagEntry(i,i+1); if (z==i-1) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h index 3993046a8..be89de4a9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h @@ -563,7 +563,6 @@ template struct direct_selfadjoint_eigenvalues::epsilon(); - safeNorm2 *= safeNorm2; if((eivals(2)-eivals(0))<=Eigen::NumTraits::epsilon()) { eivecs.setIdentity(); @@ -577,7 +576,7 @@ template struct direct_selfadjoint_eigenvalues d1 ? 2 : 0; - d0 = d0 > d1 ? d1 : d0; + d0 = d0 > d1 ? d0 : d1; tmp.diagonal().array () -= eivals(k); VectorType cross; @@ -585,19 +584,25 @@ template struct direct_selfadjoint_eigenvaluessafeNorm2) + { eivecs.col(k) = cross / sqrt(n); + } else { n = (cross = tmp.row(0).cross(tmp.row(2))).squaredNorm(); if(n>safeNorm2) + { eivecs.col(k) = cross / sqrt(n); + } else { n = (cross = tmp.row(1).cross(tmp.row(2))).squaredNorm(); if(n>safeNorm2) + { eivecs.col(k) = cross / sqrt(n); + } else { // the input matrix and/or the eigenvaues probably contains some inf/NaN, @@ -617,12 +622,16 @@ template struct direct_selfadjoint_eigenvalues::epsilon()) + { eivecs.col(1) = eivecs.col(k).unitOrthogonal(); + } else { - n = (cross = eivecs.col(k).cross(tmp.row(0).normalized())).squaredNorm(); + n = (cross = eivecs.col(k).cross(tmp.row(0))).squaredNorm(); if(n>safeNorm2) + { eivecs.col(1) = cross / sqrt(n); + } else { n = (cross = eivecs.col(k).cross(tmp.row(1))).squaredNorm(); @@ -636,13 +645,14 @@ template struct direct_selfadjoint_eigenvalues::epsilon()) + { + Matrix m; m << v0.transpose(), v1.transpose(); + JacobiSVD > svd(m, ComputeFullV); + result.normal() = svd.matrixV().col(2); + } + else + result.normal() /= norm; result.offset() = -p0.dot(result.normal()); return result; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Rotation2D.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Rotation2D.h index 1cac343a5..a2d59fce1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Rotation2D.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Rotation2D.h @@ -60,6 +60,9 @@ public: /** Construct a 2D counter clock wise rotation from the angle \a a in radian. */ inline Rotation2D(const Scalar& a) : m_angle(a) {} + + /** Default constructor wihtout initialization. The represented rotation is undefined. */ + Rotation2D() {} /** \returns the rotation angle */ inline Scalar angle() const { return m_angle; } @@ -81,10 +84,10 @@ public: /** Applies the rotation to a 2D vector */ Vector2 operator* (const Vector2& vec) const { return toRotationMatrix() * vec; } - + template Rotation2D& fromRotationMatrix(const MatrixBase& m); - Matrix2 toRotationMatrix(void) const; + Matrix2 toRotationMatrix() const; /** \returns the spherical interpolation between \c *this and \a other using * parameter \a t. It is in fact equivalent to a linear interpolation. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h index 56f361566..e786e5356 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h @@ -62,6 +62,8 @@ struct transform_construct_from_matrix; template struct transform_take_affine_part; +template struct transform_make_affine; + } // end namespace internal /** \geometry_module \ingroup Geometry_Module @@ -230,8 +232,7 @@ public: inline Transform() { check_template_params(); - if (int(Mode)==Affine) - makeAffine(); + internal::transform_make_affine<(int(Mode)==Affine) ? Affine : AffineCompact>::run(m_matrix); } inline Transform(const Transform& other) @@ -591,11 +592,7 @@ public: */ void makeAffine() { - if(int(Mode)!=int(AffineCompact)) - { - matrix().template block<1,Dim>(Dim,0).setZero(); - matrix().coeffRef(Dim,Dim) = Scalar(1); - } + internal::transform_make_affine::run(m_matrix); } /** \internal @@ -1079,6 +1076,24 @@ Transform::fromPositionOrientationScale(const MatrixBas namespace internal { +template +struct transform_make_affine +{ + template + static void run(MatrixType &mat) + { + static const int Dim = MatrixType::ColsAtCompileTime-1; + mat.template block<1,Dim>(Dim,0).setZero(); + mat.coeffRef(Dim,Dim) = typename MatrixType::Scalar(1); + } +}; + +template<> +struct transform_make_affine +{ + template static void run(MatrixType &) { } +}; + // selector needed to avoid taking the inverse of a 3x4 matrix template struct projective_transform_inverse diff --git a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h index 2b9fb7f88..dd135c21f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h @@ -39,7 +39,6 @@ bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x, int maxIters = iters; int n = mat.cols(); - x = precond.solve(x); VectorType r = rhs - mat * x; VectorType r0 = r; @@ -143,7 +142,7 @@ struct traits > * SparseMatrix A(n,n); * // fill A and b * BiCGSTAB > solver; - * solver(A); + * solver.compute(A); * x = solver.solve(b); * std::cout << "#iterations: " << solver.iterations() << std::endl; * std::cout << "estimated error: " << solver.error() << std::endl; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h index 1c48f0df7..18cd7d88a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h @@ -219,7 +219,7 @@ class PardisoImpl void pardisoInit(int type) { m_type = type; - bool symmetric = abs(m_type) < 10; + bool symmetric = std::abs(m_type) < 10; m_iparm[0] = 1; // No solver default m_iparm[1] = 3; // use Metis for the ordering m_iparm[2] = 1; // Numbers of processors, value of OMP_NUM_THREADS diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h index dff9e44eb..c57f2974c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h @@ -762,6 +762,7 @@ template class JacobiSVD internal::qr_preconditioner_impl m_qr_precond_morecols; internal::qr_preconditioner_impl m_qr_precond_morerows; + MatrixType m_scaledMatrix; }; template @@ -808,8 +809,9 @@ void JacobiSVD::allocate(Index rows, Index cols, u : 0); m_workMatrix.resize(m_diagSize, m_diagSize); - if(m_cols>m_rows) m_qr_precond_morecols.allocate(*this); - if(m_rows>m_cols) m_qr_precond_morerows.allocate(*this); + if(m_cols>m_rows) m_qr_precond_morecols.allocate(*this); + if(m_rows>m_cols) m_qr_precond_morerows.allocate(*this); + if(m_cols!=m_cols) m_scaledMatrix.resize(rows,cols); } template @@ -826,21 +828,26 @@ JacobiSVD::compute(const MatrixType& matrix, unsig // limit for very small denormal numbers to be considered zero in order to avoid infinite loops (see bug 286) const RealScalar considerAsZero = RealScalar(2) * std::numeric_limits::denorm_min(); + // Scaling factor to reduce over/under-flows + RealScalar scale = matrix.cwiseAbs().maxCoeff(); + if(scale==RealScalar(0)) scale = RealScalar(1); + /*** step 1. The R-SVD step: we use a QR decomposition to reduce to the case of a square matrix */ - if(!m_qr_precond_morecols.run(*this, matrix) && !m_qr_precond_morerows.run(*this, matrix)) + if(m_rows!=m_cols) { - m_workMatrix = matrix.block(0,0,m_diagSize,m_diagSize); + m_scaledMatrix = matrix / scale; + m_qr_precond_morecols.run(*this, m_scaledMatrix); + m_qr_precond_morerows.run(*this, m_scaledMatrix); + } + else + { + m_workMatrix = matrix.block(0,0,m_diagSize,m_diagSize) / scale; if(m_computeFullU) m_matrixU.setIdentity(m_rows,m_rows); if(m_computeThinU) m_matrixU.setIdentity(m_rows,m_diagSize); if(m_computeFullV) m_matrixV.setIdentity(m_cols,m_cols); if(m_computeThinV) m_matrixV.setIdentity(m_cols, m_diagSize); } - - // Scaling factor to reduce over/under-flows - RealScalar scale = m_workMatrix.cwiseAbs().maxCoeff(); - if(scale==RealScalar(0)) scale = RealScalar(1); - m_workMatrix /= scale; /*** step 2. The main Jacobi SVD iteration. ***/ @@ -861,7 +868,8 @@ JacobiSVD::compute(const MatrixType& matrix, unsig using std::max; RealScalar threshold = (max)(considerAsZero, precision * (max)(abs(m_workMatrix.coeff(p,p)), abs(m_workMatrix.coeff(q,q)))); - if((max)(abs(m_workMatrix.coeff(p,q)),abs(m_workMatrix.coeff(q,p))) > threshold) + // We compare both values to threshold instead of calling max to be robust to NaN (See bug 791) + if(abs(m_workMatrix.coeff(p,q))>threshold || abs(m_workMatrix.coeff(q,p)) > threshold) { finished = false; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h index 17fff96a7..220c6451c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h @@ -69,7 +69,7 @@ class AmbiVector delete[] m_buffer; if (size<1000) { - Index allocSize = (size * sizeof(ListEl))/sizeof(Scalar); + Index allocSize = (size * sizeof(ListEl) + sizeof(Scalar) - 1)/sizeof(Scalar); m_allocatedElements = (allocSize*sizeof(Scalar))/sizeof(ListEl); m_buffer = new Scalar[allocSize]; } @@ -88,7 +88,7 @@ class AmbiVector Index copyElements = m_allocatedElements; m_allocatedElements = (std::min)(Index(m_allocatedElements*1.5),m_size); Index allocSize = m_allocatedElements * sizeof(ListEl); - allocSize = allocSize/sizeof(Scalar) + (allocSize%sizeof(Scalar)>0?1:0); + allocSize = (allocSize + sizeof(Scalar) - 1)/sizeof(Scalar); Scalar* newBuffer = new Scalar[allocSize]; memcpy(newBuffer, m_buffer, copyElements * sizeof(ListEl)); delete[] m_buffer; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h index 16a20a574..0ede034ba 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h @@ -68,6 +68,8 @@ public: const internal::variable_if_dynamic m_outerSize; EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) + private: + Index nonZeros() const; }; @@ -82,6 +84,7 @@ class BlockImpl,BlockRows,BlockCols,true typedef SparseMatrix<_Scalar, _Options, _Index> SparseMatrixType; typedef typename internal::remove_all::type _MatrixTypeNested; typedef Block BlockType; + typedef Block ConstBlockType; public: enum { IsRowMajor = internal::traits::IsRowMajor }; EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) @@ -245,6 +248,93 @@ public: }; + +template +class BlockImpl,BlockRows,BlockCols,true,Sparse> + : public SparseMatrixBase,BlockRows,BlockCols,true> > +{ + typedef SparseMatrix<_Scalar, _Options, _Index> SparseMatrixType; + typedef typename internal::remove_all::type _MatrixTypeNested; + typedef Block BlockType; +public: + enum { IsRowMajor = internal::traits::IsRowMajor }; + EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) +protected: + enum { OuterSize = IsRowMajor ? BlockRows : BlockCols }; +public: + + class InnerIterator: public SparseMatrixType::InnerIterator + { + public: + inline InnerIterator(const BlockType& xpr, Index outer) + : SparseMatrixType::InnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) + {} + inline Index row() const { return IsRowMajor ? m_outer : this->index(); } + inline Index col() const { return IsRowMajor ? this->index() : m_outer; } + protected: + Index m_outer; + }; + class ReverseInnerIterator: public SparseMatrixType::ReverseInnerIterator + { + public: + inline ReverseInnerIterator(const BlockType& xpr, Index outer) + : SparseMatrixType::ReverseInnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) + {} + inline Index row() const { return IsRowMajor ? m_outer : this->index(); } + inline Index col() const { return IsRowMajor ? this->index() : m_outer; } + protected: + Index m_outer; + }; + + inline BlockImpl(const SparseMatrixType& xpr, int i) + : m_matrix(xpr), m_outerStart(i), m_outerSize(OuterSize) + {} + + inline BlockImpl(const SparseMatrixType& xpr, int startRow, int startCol, int blockRows, int blockCols) + : m_matrix(xpr), m_outerStart(IsRowMajor ? startRow : startCol), m_outerSize(IsRowMajor ? blockRows : blockCols) + {} + + inline const Scalar* valuePtr() const + { return m_matrix.valuePtr() + m_matrix.outerIndexPtr()[m_outerStart]; } + + inline const Index* innerIndexPtr() const + { return m_matrix.innerIndexPtr() + m_matrix.outerIndexPtr()[m_outerStart]; } + + inline const Index* outerIndexPtr() const + { return m_matrix.outerIndexPtr() + m_outerStart; } + + Index nonZeros() const + { + if(m_matrix.isCompressed()) + return std::size_t(m_matrix.outerIndexPtr()[m_outerStart+m_outerSize.value()]) + - std::size_t(m_matrix.outerIndexPtr()[m_outerStart]); + else if(m_outerSize.value()==0) + return 0; + else + return Map >(m_matrix.innerNonZeroPtr()+m_outerStart, m_outerSize.value()).sum(); + } + + const Scalar& lastCoeff() const + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(BlockImpl); + eigen_assert(nonZeros()>0); + if(m_matrix.isCompressed()) + return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart+1]-1]; + else + return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart]+m_matrix.innerNonZeroPtr()[m_outerStart]-1]; + } + + EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); } + EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); } + + protected: + + typename SparseMatrixType::Nested m_matrix; + Index m_outerStart; + const internal::variable_if_dynamic m_outerSize; + +}; + //---------- /** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h index 78411db98..a9084192e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h @@ -306,15 +306,6 @@ class DenseTimeSparseProduct DenseTimeSparseProduct& operator=(const DenseTimeSparseProduct&); }; -// sparse * dense -template -template -inline const typename SparseDenseProductReturnType::Type -SparseMatrixBase::operator*(const MatrixBase &other) const -{ - return typename SparseDenseProductReturnType::Type(derived(), other.derived()); -} - } // end namespace Eigen #endif // EIGEN_SPARSEDENSEPRODUCT_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h index bbcf7fb1c..485e85bec 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h @@ -358,7 +358,8 @@ template class SparseMatrixBase : public EigenBase /** sparse * dense (returns a dense object unless it is an outer product) */ template const typename SparseDenseProductReturnType::Type - operator*(const MatrixBase &other) const; + operator*(const MatrixBase &other) const + { return typename SparseDenseProductReturnType::Type(derived(), other.derived()); } /** \returns an expression of P H P^-1 where H is the matrix represented by \c *this */ SparseSymmetricPermutationProduct twistedBy(const PermutationMatrix& perm) const diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h index b85be93f6..75e210009 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h @@ -61,7 +61,7 @@ struct permut_sparsematrix_product_retval for(Index j=0; jcols(); ++j) { for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it) { - if(it.row() < j) continue; - if(it.row() == j) + if(it.index() == j) { det *= (std::abs)(it.value()); break; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h index ad6f2183f..b16afd6a4 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h @@ -189,8 +189,8 @@ class MappedSuperNodalMatrix::InnerIterator m_idval(mat.colIndexPtr()[outer]), m_startidval(m_idval), m_endidval(mat.colIndexPtr()[outer+1]), - m_idrow(mat.rowIndexPtr()[outer]), - m_endidrow(mat.rowIndexPtr()[outer+1]) + m_idrow(mat.rowIndexPtr()[mat.supToCol()[mat.colToSup()[outer]]]), + m_endidrow(mat.rowIndexPtr()[mat.supToCol()[mat.colToSup()[outer]]+1]) {} inline InnerIterator& operator++() { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h index 4c6553bf2..a00bd5db1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h @@ -75,7 +75,7 @@ class SparseQR typedef Matrix ScalarVector; typedef PermutationMatrix PermutationType; public: - SparseQR () : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false) + SparseQR () : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false),m_isEtreeOk(false) { } /** Construct a QR factorization of the matrix \a mat. @@ -84,7 +84,7 @@ class SparseQR * * \sa compute() */ - SparseQR(const MatrixType& mat) : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false) + SparseQR(const MatrixType& mat) : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false),m_isEtreeOk(false) { compute(mat); } @@ -262,6 +262,7 @@ class SparseQR IndexVector m_etree; // Column elimination tree IndexVector m_firstRowElt; // First element in each row bool m_isQSorted; // whether Q is sorted or not + bool m_isEtreeOk; // whether the elimination tree match the initial input matrix template friend struct SparseQR_QProduct; template friend struct SparseQRMatrixQReturnType; @@ -281,9 +282,11 @@ template void SparseQR::analyzePattern(const MatrixType& mat) { eigen_assert(mat.isCompressed() && "SparseQR requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it to SparseQR"); + // Copy to a column major matrix if the input is rowmajor + typename internal::conditional::type matCpy(mat); // Compute the column fill reducing ordering OrderingType ord; - ord(mat, m_perm_c); + ord(matCpy, m_perm_c); Index n = mat.cols(); Index m = mat.rows(); Index diagSize = (std::min)(m,n); @@ -296,7 +299,8 @@ void SparseQR::analyzePattern(const MatrixType& mat) // Compute the column elimination tree of the permuted matrix m_outputPerm_c = m_perm_c.inverse(); - internal::coletree(mat, m_etree, m_firstRowElt, m_outputPerm_c.indices().data()); + internal::coletree(matCpy, m_etree, m_firstRowElt, m_outputPerm_c.indices().data()); + m_isEtreeOk = true; m_R.resize(m, n); m_Q.resize(m, diagSize); @@ -330,15 +334,38 @@ void SparseQR::factorize(const MatrixType& mat) Index nzcolR, nzcolQ; // Number of nonzero for the current column of R and Q ScalarVector tval(m); // The dense vector used to compute the current column RealScalar pivotThreshold = m_threshold; - + + m_R.setZero(); + m_Q.setZero(); m_pmat = mat; - m_pmat.uncompress(); // To have the innerNonZeroPtr allocated - // Apply the fill-in reducing permutation lazily: - for (int i = 0; i < n; i++) + if(!m_isEtreeOk) { - Index p = m_perm_c.size() ? m_perm_c.indices()(i) : i; - m_pmat.outerIndexPtr()[p] = mat.outerIndexPtr()[i]; - m_pmat.innerNonZeroPtr()[p] = mat.outerIndexPtr()[i+1] - mat.outerIndexPtr()[i]; + m_outputPerm_c = m_perm_c.inverse(); + internal::coletree(m_pmat, m_etree, m_firstRowElt, m_outputPerm_c.indices().data()); + m_isEtreeOk = true; + } + + m_pmat.uncompress(); // To have the innerNonZeroPtr allocated + + // Apply the fill-in reducing permutation lazily: + { + // If the input is row major, copy the original column indices, + // otherwise directly use the input matrix + // + IndexVector originalOuterIndicesCpy; + const Index *originalOuterIndices = mat.outerIndexPtr(); + if(MatrixType::IsRowMajor) + { + originalOuterIndicesCpy = IndexVector::Map(m_pmat.outerIndexPtr(),n+1); + originalOuterIndices = originalOuterIndicesCpy.data(); + } + + for (int i = 0; i < n; i++) + { + Index p = m_perm_c.size() ? m_perm_c.indices()(i) : i; + m_pmat.outerIndexPtr()[p] = originalOuterIndices[i]; + m_pmat.innerNonZeroPtr()[p] = originalOuterIndices[i+1] - originalOuterIndices[i]; + } } /* Compute the default threshold as in MatLab, see: @@ -349,6 +376,8 @@ void SparseQR::factorize(const MatrixType& mat) { RealScalar max2Norm = 0.0; for (int j = 0; j < n; j++) max2Norm = (max)(max2Norm, m_pmat.col(j).norm()); + if(max2Norm==RealScalar(0)) + max2Norm = RealScalar(1); pivotThreshold = 20 * (m + n) * max2Norm * NumTraits::epsilon(); } @@ -357,7 +386,7 @@ void SparseQR::factorize(const MatrixType& mat) Index nonzeroCol = 0; // Record the number of valid pivots m_Q.startVec(0); - + // Left looking rank-revealing QR factorization: compute a column of R and Q at a time for (Index col = 0; col < n; ++col) { @@ -373,7 +402,7 @@ void SparseQR::factorize(const MatrixType& mat) // all the nodes (with indexes lower than rank) reachable through the column elimination tree (etree) rooted at node k. // Note: if the diagonal entry does not exist, then its contribution must be explicitly added, // thus the trick with found_diag that permits to do one more iteration on the diagonal element if this one has not been found. - for (typename MatrixType::InnerIterator itp(m_pmat, col); itp || !found_diag; ++itp) + for (typename QRMatrixType::InnerIterator itp(m_pmat, col); itp || !found_diag; ++itp) { Index curIdx = nonzeroCol; if(itp) curIdx = itp.row(); @@ -447,7 +476,7 @@ void SparseQR::factorize(const MatrixType& mat) } } // End update current column - Scalar tau; + Scalar tau = 0; RealScalar beta = 0; if(nonzeroCol < diagSize) @@ -461,7 +490,6 @@ void SparseQR::factorize(const MatrixType& mat) for (Index itq = 1; itq < nzcolQ; ++itq) sqrNorm += numext::abs2(tval(Qidx(itq))); if(sqrNorm == RealScalar(0) && numext::imag(c0) == RealScalar(0)) { - tau = RealScalar(0); beta = numext::real(c0); tval(Qidx(0)) = 1; } @@ -514,6 +542,7 @@ void SparseQR::factorize(const MatrixType& mat) // Recompute the column elimination tree internal::coletree(m_pmat, m_etree, m_firstRowElt, m_pivotperm.indices().data()); + m_isEtreeOk = false; } } @@ -525,13 +554,13 @@ void SparseQR::factorize(const MatrixType& mat) m_R.finalize(); m_R.makeCompressed(); m_isQSorted = false; - + m_nonzeropivots = nonzeroCol; if(nonzeroCol *Mx, double *Ex, void *N return umfpack_zi_get_determinant(&mx_real,0,Ex,NumericHandle,User_Info); } +namespace internal { + template struct umfpack_helper_is_sparse_plain : false_type {}; + template + struct umfpack_helper_is_sparse_plain > + : true_type {}; + template + struct umfpack_helper_is_sparse_plain > + : true_type {}; +} + /** \ingroup UmfPackSupport_Module * \brief A sparse LU factorization and solver based on UmfPack * @@ -192,10 +202,14 @@ class UmfPackLU : internal::noncopyable * Note that the matrix should be column-major, and in compressed format for best performance. * \sa SparseMatrix::makeCompressed(). */ - void compute(const MatrixType& matrix) + template + void compute(const InputMatrixType& matrix) { - analyzePattern(matrix); - factorize(matrix); + if(m_symbolic) umfpack_free_symbolic(&m_symbolic,Scalar()); + if(m_numeric) umfpack_free_numeric(&m_numeric,Scalar()); + grapInput(matrix.derived()); + analyzePattern_impl(); + factorize_impl(); } /** \returns the solution x of \f$ A x = b \f$ using the current decomposition of A. @@ -230,23 +244,15 @@ class UmfPackLU : internal::noncopyable * * \sa factorize(), compute() */ - void analyzePattern(const MatrixType& matrix) + template + void analyzePattern(const InputMatrixType& matrix) { - if(m_symbolic) - umfpack_free_symbolic(&m_symbolic,Scalar()); - if(m_numeric) - umfpack_free_numeric(&m_numeric,Scalar()); + if(m_symbolic) umfpack_free_symbolic(&m_symbolic,Scalar()); + if(m_numeric) umfpack_free_numeric(&m_numeric,Scalar()); - grapInput(matrix); + grapInput(matrix.derived()); - int errorCode = 0; - errorCode = umfpack_symbolic(matrix.rows(), matrix.cols(), m_outerIndexPtr, m_innerIndexPtr, m_valuePtr, - &m_symbolic, 0, 0); - - m_isInitialized = true; - m_info = errorCode ? InvalidInput : Success; - m_analysisIsOk = true; - m_factorizationIsOk = false; + analyzePattern_impl(); } /** Performs a numeric decomposition of \a matrix @@ -255,20 +261,16 @@ class UmfPackLU : internal::noncopyable * * \sa analyzePattern(), compute() */ - void factorize(const MatrixType& matrix) + template + void factorize(const InputMatrixType& matrix) { eigen_assert(m_analysisIsOk && "UmfPackLU: you must first call analyzePattern()"); if(m_numeric) umfpack_free_numeric(&m_numeric,Scalar()); - grapInput(matrix); - - int errorCode; - errorCode = umfpack_numeric(m_outerIndexPtr, m_innerIndexPtr, m_valuePtr, - m_symbolic, &m_numeric, 0, 0); - - m_info = errorCode ? NumericalIssue : Success; - m_factorizationIsOk = true; + grapInput(matrix.derived()); + + factorize_impl(); } #ifndef EIGEN_PARSED_BY_DOXYGEN @@ -283,19 +285,20 @@ class UmfPackLU : internal::noncopyable protected: - void init() { - m_info = InvalidInput; - m_isInitialized = false; - m_numeric = 0; - m_symbolic = 0; - m_outerIndexPtr = 0; - m_innerIndexPtr = 0; - m_valuePtr = 0; + m_info = InvalidInput; + m_isInitialized = false; + m_numeric = 0; + m_symbolic = 0; + m_outerIndexPtr = 0; + m_innerIndexPtr = 0; + m_valuePtr = 0; + m_extractedDataAreDirty = true; } - void grapInput(const MatrixType& mat) + template + void grapInput_impl(const InputMatrixType& mat, internal::true_type) { m_copyMatrix.resize(mat.rows(), mat.cols()); if( ((MatrixType::Flags&RowMajorBit)==RowMajorBit) || sizeof(typename MatrixType::Index)!=sizeof(int) || !mat.isCompressed() ) @@ -313,6 +316,45 @@ class UmfPackLU : internal::noncopyable m_valuePtr = mat.valuePtr(); } } + + template + void grapInput_impl(const InputMatrixType& mat, internal::false_type) + { + m_copyMatrix = mat; + m_outerIndexPtr = m_copyMatrix.outerIndexPtr(); + m_innerIndexPtr = m_copyMatrix.innerIndexPtr(); + m_valuePtr = m_copyMatrix.valuePtr(); + } + + template + void grapInput(const InputMatrixType& mat) + { + grapInput_impl(mat, internal::umfpack_helper_is_sparse_plain()); + } + + void analyzePattern_impl() + { + int errorCode = 0; + errorCode = umfpack_symbolic(m_copyMatrix.rows(), m_copyMatrix.cols(), m_outerIndexPtr, m_innerIndexPtr, m_valuePtr, + &m_symbolic, 0, 0); + + m_isInitialized = true; + m_info = errorCode ? InvalidInput : Success; + m_analysisIsOk = true; + m_factorizationIsOk = false; + m_extractedDataAreDirty = true; + } + + void factorize_impl() + { + int errorCode; + errorCode = umfpack_numeric(m_outerIndexPtr, m_innerIndexPtr, m_valuePtr, + m_symbolic, &m_numeric, 0, 0); + + m_info = errorCode ? NumericalIssue : Success; + m_factorizationIsOk = true; + m_extractedDataAreDirty = true; + } // cached data to reduce reallocation, etc. mutable LUMatrixType m_l; diff --git a/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake b/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake index d9e22ab1a..80b2841df 100644 --- a/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake +++ b/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake @@ -452,20 +452,12 @@ macro(ei_set_build_string) endmacro(ei_set_build_string) macro(ei_is_64bit_env VAR) - - file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/is64.cpp" - "int main() { return (sizeof(int*) == 8 ? 1 : 0); } - ") - try_run(run_res compile_res - ${CMAKE_CURRENT_BINARY_DIR} "${CMAKE_CURRENT_BINARY_DIR}/is64.cpp" - RUN_OUTPUT_VARIABLE run_output) - - if(compile_res AND run_res) - set(${VAR} ${run_res}) - elseif(CMAKE_CL_64) - set(${VAR} 1) - elseif("$ENV{Platform}" STREQUAL "X64") # nmake 64 bit + if(CMAKE_SIZEOF_VOID_P EQUAL 8) set(${VAR} 1) + elseif(CMAKE_SIZEOF_VOID_P EQUAL 4) + set(${VAR} 0) + else() + message(WARNING "Unsupported pointer size. Please contact the authors.") endif() endmacro(ei_is_64bit_env) diff --git a/gtsam/3rdparty/Eigen/cmake/FindCholmod.cmake b/gtsam/3rdparty/Eigen/cmake/FindCholmod.cmake index 7b3046d45..23239c300 100644 --- a/gtsam/3rdparty/Eigen/cmake/FindCholmod.cmake +++ b/gtsam/3rdparty/Eigen/cmake/FindCholmod.cmake @@ -86,4 +86,4 @@ include(FindPackageHandleStandardArgs) find_package_handle_standard_args(CHOLMOD DEFAULT_MSG CHOLMOD_INCLUDES CHOLMOD_LIBRARIES) -mark_as_advanced(CHOLMOD_INCLUDES CHOLMOD_LIBRARIES AMD_LIBRARY COLAMD_LIBRARY SUITESPARSE_LIBRARY) +mark_as_advanced(CHOLMOD_INCLUDES CHOLMOD_LIBRARIES AMD_LIBRARY COLAMD_LIBRARY SUITESPARSE_LIBRARY CAMD_LIBRARY CCOLAMD_LIBRARY CHOLMOD_METIS_LIBRARY) diff --git a/gtsam/3rdparty/Eigen/cmake/FindFFTW.cmake b/gtsam/3rdparty/Eigen/cmake/FindFFTW.cmake index a9e9925e7..6c4dc9ab4 100644 --- a/gtsam/3rdparty/Eigen/cmake/FindFFTW.cmake +++ b/gtsam/3rdparty/Eigen/cmake/FindFFTW.cmake @@ -115,5 +115,5 @@ include(FindPackageHandleStandardArgs) find_package_handle_standard_args(FFTW DEFAULT_MSG FFTW_INCLUDES FFTW_LIBRARIES) -mark_as_advanced(FFTW_INCLUDES FFTW_LIBRARIES) +mark_as_advanced(FFTW_INCLUDES FFTW_LIBRARIES FFTW_LIB FFTWF_LIB FFTWL_LIB) diff --git a/gtsam/3rdparty/Eigen/cmake/FindMetis.cmake b/gtsam/3rdparty/Eigen/cmake/FindMetis.cmake index 627c3e9ae..e0040d320 100644 --- a/gtsam/3rdparty/Eigen/cmake/FindMetis.cmake +++ b/gtsam/3rdparty/Eigen/cmake/FindMetis.cmake @@ -10,16 +10,50 @@ find_path(METIS_INCLUDES PATHS $ENV{METISDIR} ${INCLUDE_INSTALL_DIR} - PATH_SUFFIXES + PATH_SUFFIXES + . metis include ) +macro(_metis_check_version) + file(READ "${METIS_INCLUDES}/metis.h" _metis_version_header) + + string(REGEX MATCH "define[ \t]+METIS_VER_MAJOR[ \t]+([0-9]+)" _metis_major_version_match "${_metis_version_header}") + set(METIS_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+METIS_VER_MINOR[ \t]+([0-9]+)" _metis_minor_version_match "${_metis_version_header}") + set(METIS_MINOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+METIS_VER_SUBMINOR[ \t]+([0-9]+)" _metis_subminor_version_match "${_metis_version_header}") + set(METIS_SUBMINOR_VERSION "${CMAKE_MATCH_1}") + if(NOT METIS_MAJOR_VERSION) + message(WARNING "Could not determine Metis version. Assuming version 4.0.0") + set(METIS_VERSION 4.0.0) + else() + set(METIS_VERSION ${METIS_MAJOR_VERSION}.${METIS_MINOR_VERSION}.${METIS_SUBMINOR_VERSION}) + endif() + if(${METIS_VERSION} VERSION_LESS ${Metis_FIND_VERSION}) + set(METIS_VERSION_OK FALSE) + else() + set(METIS_VERSION_OK TRUE) + endif() + + if(NOT METIS_VERSION_OK) + message(STATUS "Metis version ${METIS_VERSION} found in ${METIS_INCLUDES}, " + "but at least version ${Metis_FIND_VERSION} is required") + endif(NOT METIS_VERSION_OK) +endmacro(_metis_check_version) + + if(METIS_INCLUDES AND Metis_FIND_VERSION) + _metis_check_version() + else() + set(METIS_VERSION_OK TRUE) + endif() + find_library(METIS_LIBRARIES metis PATHS $ENV{METISDIR} ${LIB_INSTALL_DIR} PATH_SUFFIXES lib) include(FindPackageHandleStandardArgs) find_package_handle_standard_args(METIS DEFAULT_MSG - METIS_INCLUDES METIS_LIBRARIES) + METIS_INCLUDES METIS_LIBRARIES METIS_VERSION_OK) mark_as_advanced(METIS_INCLUDES METIS_LIBRARIES) diff --git a/gtsam/3rdparty/Eigen/cmake/language_support.cmake b/gtsam/3rdparty/Eigen/cmake/language_support.cmake index d687b71f6..231f7ff70 100644 --- a/gtsam/3rdparty/Eigen/cmake/language_support.cmake +++ b/gtsam/3rdparty/Eigen/cmake/language_support.cmake @@ -33,7 +33,7 @@ function(workaround_9220 language language_works) file(WRITE ${CMAKE_BINARY_DIR}/language_tests/${language}/CMakeLists.txt ${text}) execute_process( - COMMAND ${CMAKE_COMMAND} . + COMMAND ${CMAKE_COMMAND} . -G "${CMAKE_GENERATOR}" WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/language_tests/${language} RESULT_VARIABLE return_code OUTPUT_QUIET diff --git a/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt b/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt index 1e74e0528..b9f497f87 100644 --- a/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt +++ b/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt @@ -67,10 +67,10 @@ P.rightCols() // P(:, end-cols+1:end) P.rightCols(cols) // P(:, end-cols+1:end) P.topRows() // P(1:rows, :) P.topRows(rows) // P(1:rows, :) -P.middleRows(i) // P(:, i+1:i+rows) -P.middleRows(i, rows) // P(:, i+1:i+rows) -P.bottomRows() // P(:, end-rows+1:end) -P.bottomRows(rows) // P(:, end-rows+1:end) +P.middleRows(i) // P(i+1:i+rows, :) +P.middleRows(i, rows) // P(i+1:i+rows, :) +P.bottomRows() // P(end-rows+1:end, :) +P.bottomRows(rows) // P(end-rows+1:end, :) P.topLeftCorner(rows, cols) // P(1:rows, 1:cols) P.topRightCorner(rows, cols) // P(1:rows, end-cols+1:end) P.bottomLeftCorner(rows, cols) // P(end-rows+1:end, 1:cols) diff --git a/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox b/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox index 4a33d0cc9..d04ac35c5 100644 --- a/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox +++ b/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox @@ -71,11 +71,10 @@ i.e either row major or column major. The default is column major. Most arithmet Constant or Random Insertion \code -sm1.setZero(); // Set the matrix with zero elements -sm1.setConstant(val); //Replace all the nonzero values with val +sm1.setZero(); \endcode - The matrix sm1 should have been created before ??? +Remove all non-zero coefficients diff --git a/gtsam/3rdparty/Eigen/doc/examples/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/examples/CMakeLists.txt index 71b272a15..08cf8efd7 100644 --- a/gtsam/3rdparty/Eigen/doc/examples/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/doc/examples/CMakeLists.txt @@ -6,12 +6,10 @@ foreach(example_src ${examples_SRCS}) if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO) target_link_libraries(${example} ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO}) endif() - get_target_property(example_executable - ${example} LOCATION) add_custom_command( TARGET ${example} POST_BUILD - COMMAND ${example_executable} + COMMAND ${example} ARGS >${CMAKE_CURRENT_BINARY_DIR}/${example}.out ) add_dependencies(all_examples ${example}) diff --git a/gtsam/3rdparty/Eigen/doc/snippets/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/snippets/CMakeLists.txt index 92a22ea61..1135900cf 100644 --- a/gtsam/3rdparty/Eigen/doc/snippets/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/doc/snippets/CMakeLists.txt @@ -14,12 +14,10 @@ foreach(snippet_src ${snippets_SRCS}) if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO) target_link_libraries(${compile_snippet_target} ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO}) endif() - get_target_property(compile_snippet_executable - ${compile_snippet_target} LOCATION) add_custom_command( TARGET ${compile_snippet_target} POST_BUILD - COMMAND ${compile_snippet_executable} + COMMAND ${compile_snippet_target} ARGS >${CMAKE_CURRENT_BINARY_DIR}/${snippet}.out ) add_dependencies(all_snippets ${compile_snippet_target}) @@ -27,4 +25,4 @@ foreach(snippet_src ${snippets_SRCS}) PROPERTIES OBJECT_DEPENDS ${snippet_src}) endforeach(snippet_src) -ei_add_target_property(compile_tut_arithmetic_transpose_aliasing COMPILE_FLAGS -DEIGEN_NO_DEBUG) \ No newline at end of file +ei_add_target_property(compile_tut_arithmetic_transpose_aliasing COMPILE_FLAGS -DEIGEN_NO_DEBUG) diff --git a/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt index 0c9b3c3ba..f8a0148c8 100644 --- a/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt @@ -1,4 +1,3 @@ - if(NOT EIGEN_TEST_NOQT) find_package(Qt4) if(QT4_FOUND) @@ -6,16 +5,16 @@ if(NOT EIGEN_TEST_NOQT) endif() endif(NOT EIGEN_TEST_NOQT) - if(QT4_FOUND) add_executable(Tutorial_sparse_example Tutorial_sparse_example.cpp Tutorial_sparse_example_details.cpp) target_link_libraries(Tutorial_sparse_example ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO} ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY}) - + add_custom_command( TARGET Tutorial_sparse_example POST_BUILD COMMAND Tutorial_sparse_example ARGS ${CMAKE_CURRENT_BINARY_DIR}/../html/Tutorial_sparse_example.jpeg ) - + add_dependencies(all_examples Tutorial_sparse_example) endif(QT4_FOUND) + diff --git a/gtsam/3rdparty/Eigen/failtest/CMakeLists.txt b/gtsam/3rdparty/Eigen/failtest/CMakeLists.txt index 5afa2ac82..5c4860237 100644 --- a/gtsam/3rdparty/Eigen/failtest/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/failtest/CMakeLists.txt @@ -26,6 +26,12 @@ ei_add_failtest("block_on_const_type_actually_const_1") ei_add_failtest("transpose_on_const_type_actually_const") ei_add_failtest("diagonal_on_const_type_actually_const") +ei_add_failtest("ref_1") +ei_add_failtest("ref_2") +ei_add_failtest("ref_3") +ei_add_failtest("ref_4") +ei_add_failtest("ref_5") + if (EIGEN_FAILTEST_FAILURE_COUNT) message(FATAL_ERROR "${EIGEN_FAILTEST_FAILURE_COUNT} out of ${EIGEN_FAILTEST_COUNT} failtests FAILED. " diff --git a/gtsam/3rdparty/Eigen/test/CMakeLists.txt b/gtsam/3rdparty/Eigen/test/CMakeLists.txt index ccb0fc798..d32451df6 100644 --- a/gtsam/3rdparty/Eigen/test/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/test/CMakeLists.txt @@ -66,7 +66,7 @@ endif() find_package(Pastix) find_package(Scotch) -find_package(Metis) +find_package(Metis 5.0 REQUIRED) if(PASTIX_FOUND AND BLAS_FOUND) add_definitions("-DEIGEN_PASTIX_SUPPORT") include_directories(${PASTIX_INCLUDES}) @@ -279,6 +279,7 @@ ei_add_property(EIGEN_TESTING_SUMMARY "CXX_FLAGS: ${CMAKE_CXX_FLAGS}\n") ei_add_property(EIGEN_TESTING_SUMMARY "Sparse lib flags: ${SPARSE_LIBS}\n") option(EIGEN_TEST_EIGEN2 "Run whole Eigen2 test suite against EIGEN2_SUPPORT" OFF) +mark_as_advanced(EIGEN_TEST_EIGEN2) if(EIGEN_TEST_EIGEN2) add_subdirectory(eigen2) endif() diff --git a/gtsam/3rdparty/Eigen/test/cholesky.cpp b/gtsam/3rdparty/Eigen/test/cholesky.cpp index 64bcbccc4..56885deb7 100644 --- a/gtsam/3rdparty/Eigen/test/cholesky.cpp +++ b/gtsam/3rdparty/Eigen/test/cholesky.cpp @@ -320,33 +320,35 @@ template void cholesky_definiteness(const MatrixType& m) { eigen_assert(m.rows() == 2 && m.cols() == 2); MatrixType mat; + LDLT ldlt(2); + { mat << 1, 0, 0, -1; - LDLT ldlt(mat); + ldlt.compute(mat); VERIFY(!ldlt.isNegative()); VERIFY(!ldlt.isPositive()); } { mat << 1, 2, 2, 1; - LDLT ldlt(mat); + ldlt.compute(mat); VERIFY(!ldlt.isNegative()); VERIFY(!ldlt.isPositive()); } { mat << 0, 0, 0, 0; - LDLT ldlt(mat); + ldlt.compute(mat); VERIFY(ldlt.isNegative()); VERIFY(ldlt.isPositive()); } { mat << 0, 0, 0, 1; - LDLT ldlt(mat); + ldlt.compute(mat); VERIFY(!ldlt.isNegative()); VERIFY(ldlt.isPositive()); } { mat << -1, 0, 0, 0; - LDLT ldlt(mat); + ldlt.compute(mat); VERIFY(ldlt.isNegative()); VERIFY(!ldlt.isPositive()); } diff --git a/gtsam/3rdparty/Eigen/test/cwiseop.cpp b/gtsam/3rdparty/Eigen/test/cwiseop.cpp index 247fa2a09..e3361da17 100644 --- a/gtsam/3rdparty/Eigen/test/cwiseop.cpp +++ b/gtsam/3rdparty/Eigen/test/cwiseop.cpp @@ -9,6 +9,8 @@ // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. #define EIGEN2_SUPPORT +#define EIGEN_NO_EIGEN2_DEPRECATED_WARNING + #define EIGEN_NO_STATIC_ASSERT #include "main.h" #include diff --git a/gtsam/3rdparty/Eigen/test/dynalloc.cpp b/gtsam/3rdparty/Eigen/test/dynalloc.cpp index 9a6a9d140..7e41bfa97 100644 --- a/gtsam/3rdparty/Eigen/test/dynalloc.cpp +++ b/gtsam/3rdparty/Eigen/test/dynalloc.cpp @@ -87,32 +87,6 @@ template void check_dynaligned() delete obj; } -template void check_custom_new_delete() -{ - { - T* t = new T; - delete t; - } - - { - std::size_t N = internal::random(1,10); - T* t = new T[N]; - delete[] t; - } - -#ifdef EIGEN_ALIGN - { - T* t = static_cast((T::operator new)(sizeof(T))); - (T::operator delete)(t, sizeof(T)); - } - - { - T* t = static_cast((T::operator new)(sizeof(T))); - (T::operator delete)(t); - } -#endif -} - void test_dynalloc() { // low level dynamic memory allocation @@ -128,12 +102,6 @@ void test_dynalloc() CALL_SUBTEST(check_dynaligned() ); CALL_SUBTEST(check_dynaligned() ); CALL_SUBTEST(check_dynaligned() ); - CALL_SUBTEST(check_dynaligned() ); - - CALL_SUBTEST( check_custom_new_delete() ); - CALL_SUBTEST( check_custom_new_delete() ); - CALL_SUBTEST( check_custom_new_delete() ); - CALL_SUBTEST( check_custom_new_delete() ); } // check static allocation, who knows ? diff --git a/gtsam/3rdparty/Eigen/test/eigen2/CMakeLists.txt b/gtsam/3rdparty/Eigen/test/eigen2/CMakeLists.txt index 84931e037..9615a6026 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/test/eigen2/CMakeLists.txt @@ -4,6 +4,7 @@ add_dependencies(eigen2_check eigen2_buildtests) add_dependencies(buildtests eigen2_buildtests) add_definitions("-DEIGEN2_SUPPORT_STAGE10_FULL_EIGEN2_API") +add_definitions("-DEIGEN_NO_EIGEN2_DEPRECATED_WARNING") ei_add_test(eigen2_meta) ei_add_test(eigen2_sizeof) diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_adjoint.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_adjoint.cpp index 8ec9c9202..c0f811459 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_adjoint.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_adjoint.cpp @@ -29,8 +29,6 @@ template void adjoint(const MatrixType& m) MatrixType m1 = MatrixType::Random(rows, cols), m2 = MatrixType::Random(rows, cols), m3(rows, cols), - mzero = MatrixType::Zero(rows, cols), - identity = SquareMatrixType::Identity(rows, rows), square = SquareMatrixType::Random(rows, rows); VectorType v1 = VectorType::Random(rows), v2 = VectorType::Random(rows), diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_basicstuff.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_basicstuff.cpp index 4fa16d5ae..dd2dec1ef 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_basicstuff.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_basicstuff.cpp @@ -23,11 +23,8 @@ template void basicStuff(const MatrixType& m) m2 = MatrixType::Random(rows, cols), m3(rows, cols), mzero = MatrixType::Zero(rows, cols), - identity = Matrix - ::Identity(rows, rows), square = Matrix::Random(rows, rows); VectorType v1 = VectorType::Random(rows), - v2 = VectorType::Random(rows), vzero = VectorType::Zero(rows); Scalar x = ei_random(); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp index 4391d19b4..22e1cc342 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp @@ -35,11 +35,8 @@ template void cwiseops(const MatrixType& m) mzero = MatrixType::Zero(rows, cols), mones = MatrixType::Ones(rows, cols), identity = Matrix - ::Identity(rows, rows), - square = Matrix::Random(rows, rows); - VectorType v1 = VectorType::Random(rows), - v2 = VectorType::Random(rows), - vzero = VectorType::Zero(rows), + ::Identity(rows, rows); + VectorType vzero = VectorType::Zero(rows), vones = VectorType::Ones(rows), v3(rows); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry.cpp index 70b4ab5f1..514040774 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry.cpp @@ -392,6 +392,7 @@ template void geometry(void) #define VERIFY_EULER(I,J,K, X,Y,Z) { \ Vector3 ea = m.eulerAngles(I,J,K); \ Matrix3 m1 = Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \ + VERIFY_IS_APPROX(m, m1); \ VERIFY_IS_APPROX(m, Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z()))); \ } VERIFY_EULER(0,1,2, X,Y,Z); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry_with_eigen2_prefix.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry_with_eigen2_prefix.cpp index f83b57249..12d4a71c3 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry_with_eigen2_prefix.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry_with_eigen2_prefix.cpp @@ -394,6 +394,7 @@ template void geometry(void) #define VERIFY_EULER(I,J,K, X,Y,Z) { \ Vector3 ea = m.eulerAngles(I,J,K); \ Matrix3 m1 = Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \ + VERIFY_IS_APPROX(m, m1); \ VERIFY_IS_APPROX(m, Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z()))); \ } VERIFY_EULER(0,1,2, X,Y,Z); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_inverse.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_inverse.cpp index 5de1dfefa..ccd24a194 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_inverse.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_inverse.cpp @@ -25,7 +25,6 @@ template void inverse(const MatrixType& m) MatrixType m1 = MatrixType::Random(rows, cols), m2(rows, cols), - mzero = MatrixType::Zero(rows, cols), identity = MatrixType::Identity(rows, rows); while(ei_abs(m1.determinant()) < RealScalar(0.1) && rows <= 8) diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_linearstructure.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_linearstructure.cpp index 22f02c396..488f4c485 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_linearstructure.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_linearstructure.cpp @@ -25,8 +25,7 @@ template void linearStructure(const MatrixType& m) // to test it, hence I consider that we will have tested Random.h MatrixType m1 = MatrixType::Random(rows, cols), m2 = MatrixType::Random(rows, cols), - m3(rows, cols), - mzero = MatrixType::Zero(rows, cols); + m3(rows, cols); Scalar s1 = ei_random(); while (ei_abs(s1)<1e-3) s1 = ei_random(); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_nomalloc.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_nomalloc.cpp index e234abe4b..d34a69999 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_nomalloc.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_nomalloc.cpp @@ -25,22 +25,12 @@ template void nomalloc(const MatrixType& m) */ typedef typename MatrixType::Scalar Scalar; - typedef Matrix VectorType; int rows = m.rows(); int cols = m.cols(); MatrixType m1 = MatrixType::Random(rows, cols), - m2 = MatrixType::Random(rows, cols), - m3(rows, cols), - mzero = MatrixType::Zero(rows, cols), - identity = Matrix - ::Identity(rows, rows), - square = Matrix - ::Random(rows, rows); - VectorType v1 = VectorType::Random(rows), - v2 = VectorType::Random(rows), - vzero = VectorType::Zero(rows); + m2 = MatrixType::Random(rows, cols); Scalar s1 = ei_random(); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_submatrices.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_submatrices.cpp index c5d3f243d..dee970b63 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_submatrices.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_submatrices.cpp @@ -51,16 +51,10 @@ template void submatrices(const MatrixType& m) MatrixType m1 = MatrixType::Random(rows, cols), m2 = MatrixType::Random(rows, cols), m3(rows, cols), - mzero = MatrixType::Zero(rows, cols), ones = MatrixType::Ones(rows, cols), - identity = Matrix - ::Identity(rows, rows), square = Matrix ::Random(rows, rows); - VectorType v1 = VectorType::Random(rows), - v2 = VectorType::Random(rows), - v3 = VectorType::Random(rows), - vzero = VectorType::Zero(rows); + VectorType v1 = VectorType::Random(rows); Scalar s1 = ei_random(); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_triangular.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_triangular.cpp index 3748c7d71..6f17b7dff 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_triangular.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_triangular.cpp @@ -13,7 +13,6 @@ template void triangular(const MatrixType& m) { typedef typename MatrixType::Scalar Scalar; typedef typename NumTraits::Real RealScalar; - typedef Matrix VectorType; RealScalar largerEps = 10*test_precision(); @@ -25,16 +24,7 @@ template void triangular(const MatrixType& m) m3(rows, cols), m4(rows, cols), r1(rows, cols), - r2(rows, cols), - mzero = MatrixType::Zero(rows, cols), - mones = MatrixType::Ones(rows, cols), - identity = Matrix - ::Identity(rows, rows), - square = Matrix - ::Random(rows, rows); - VectorType v1 = VectorType::Random(rows), - v2 = VectorType::Random(rows), - vzero = VectorType::Zero(rows); + r2(rows, cols); MatrixType m1up = m1.template part(); MatrixType m2up = m2.template part(); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/product.h b/gtsam/3rdparty/Eigen/test/eigen2/product.h index 2c9604d9a..ae1b4bae4 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/product.h +++ b/gtsam/3rdparty/Eigen/test/eigen2/product.h @@ -40,8 +40,7 @@ template void product(const MatrixType& m) // to test it, hence I consider that we will have tested Random.h MatrixType m1 = MatrixType::Random(rows, cols), m2 = MatrixType::Random(rows, cols), - m3(rows, cols), - mzero = MatrixType::Zero(rows, cols); + m3(rows, cols); RowSquareMatrixType identity = RowSquareMatrixType::Identity(rows, rows), square = RowSquareMatrixType::Random(rows, rows), @@ -49,9 +48,7 @@ template void product(const MatrixType& m) ColSquareMatrixType square2 = ColSquareMatrixType::Random(cols, cols), res2 = ColSquareMatrixType::Random(cols, cols); - RowVectorType v1 = RowVectorType::Random(rows), - v2 = RowVectorType::Random(rows), - vzero = RowVectorType::Zero(rows); + RowVectorType v1 = RowVectorType::Random(rows); ColVectorType vc2 = ColVectorType::Random(cols), vcres(cols); OtherMajorMatrixType tm1 = m1; diff --git a/gtsam/3rdparty/Eigen/test/eigen2support.cpp b/gtsam/3rdparty/Eigen/test/eigen2support.cpp index ad1d98091..1fa49a8c8 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2support.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2support.cpp @@ -8,6 +8,7 @@ // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. #define EIGEN2_SUPPORT +#define EIGEN_NO_EIGEN2_DEPRECATED_WARNING #include "main.h" diff --git a/gtsam/3rdparty/Eigen/test/eigensolver_selfadjoint.cpp b/gtsam/3rdparty/Eigen/test/eigensolver_selfadjoint.cpp index 5c6ecd875..38689cfbf 100644 --- a/gtsam/3rdparty/Eigen/test/eigensolver_selfadjoint.cpp +++ b/gtsam/3rdparty/Eigen/test/eigensolver_selfadjoint.cpp @@ -29,7 +29,21 @@ template void selfadjointeigensolver(const MatrixType& m) MatrixType a = MatrixType::Random(rows,cols); MatrixType a1 = MatrixType::Random(rows,cols); MatrixType symmA = a.adjoint() * a + a1.adjoint() * a1; + MatrixType symmC = symmA; + + // randomly nullify some rows/columns + { + Index count = 1;//internal::random(-cols,cols); + for(Index k=0; k(0,cols-1); + symmA.row(i).setZero(); + symmA.col(i).setZero(); + } + } + symmA.template triangularView().setZero(); + symmC.template triangularView().setZero(); MatrixType b = MatrixType::Random(rows,cols); MatrixType b1 = MatrixType::Random(rows,cols); @@ -40,7 +54,7 @@ template void selfadjointeigensolver(const MatrixType& m) SelfAdjointEigenSolver eiDirect; eiDirect.computeDirect(symmA); // generalized eigen pb - GeneralizedSelfAdjointEigenSolver eiSymmGen(symmA, symmB); + GeneralizedSelfAdjointEigenSolver eiSymmGen(symmC, symmB); VERIFY_IS_EQUAL(eiSymm.info(), Success); VERIFY((symmA.template selfadjointView() * eiSymm.eigenvectors()).isApprox( @@ -57,27 +71,28 @@ template void selfadjointeigensolver(const MatrixType& m) VERIFY_IS_APPROX(eiSymm.eigenvalues(), eiSymmNoEivecs.eigenvalues()); // generalized eigen problem Ax = lBx - eiSymmGen.compute(symmA, symmB,Ax_lBx); + eiSymmGen.compute(symmC, symmB,Ax_lBx); VERIFY_IS_EQUAL(eiSymmGen.info(), Success); - VERIFY((symmA.template selfadjointView() * eiSymmGen.eigenvectors()).isApprox( + VERIFY((symmC.template selfadjointView() * eiSymmGen.eigenvectors()).isApprox( symmB.template selfadjointView() * (eiSymmGen.eigenvectors() * eiSymmGen.eigenvalues().asDiagonal()), largerEps)); // generalized eigen problem BAx = lx - eiSymmGen.compute(symmA, symmB,BAx_lx); + eiSymmGen.compute(symmC, symmB,BAx_lx); VERIFY_IS_EQUAL(eiSymmGen.info(), Success); - VERIFY((symmB.template selfadjointView() * (symmA.template selfadjointView() * eiSymmGen.eigenvectors())).isApprox( + VERIFY((symmB.template selfadjointView() * (symmC.template selfadjointView() * eiSymmGen.eigenvectors())).isApprox( (eiSymmGen.eigenvectors() * eiSymmGen.eigenvalues().asDiagonal()), largerEps)); // generalized eigen problem ABx = lx - eiSymmGen.compute(symmA, symmB,ABx_lx); + eiSymmGen.compute(symmC, symmB,ABx_lx); VERIFY_IS_EQUAL(eiSymmGen.info(), Success); - VERIFY((symmA.template selfadjointView() * (symmB.template selfadjointView() * eiSymmGen.eigenvectors())).isApprox( + VERIFY((symmC.template selfadjointView() * (symmB.template selfadjointView() * eiSymmGen.eigenvectors())).isApprox( (eiSymmGen.eigenvectors() * eiSymmGen.eigenvalues().asDiagonal()), largerEps)); + eiSymm.compute(symmC); MatrixType sqrtSymmA = eiSymm.operatorSqrt(); - VERIFY_IS_APPROX(MatrixType(symmA.template selfadjointView()), sqrtSymmA*sqrtSymmA); - VERIFY_IS_APPROX(sqrtSymmA, symmA.template selfadjointView()*eiSymm.operatorInverseSqrt()); + VERIFY_IS_APPROX(MatrixType(symmC.template selfadjointView()), sqrtSymmA*sqrtSymmA); + VERIFY_IS_APPROX(sqrtSymmA, symmC.template selfadjointView()*eiSymm.operatorInverseSqrt()); MatrixType id = MatrixType::Identity(rows, cols); VERIFY_IS_APPROX(id.template selfadjointView().operatorNorm(), RealScalar(1)); @@ -95,15 +110,15 @@ template void selfadjointeigensolver(const MatrixType& m) VERIFY_RAISES_ASSERT(eiSymmUninitialized.operatorInverseSqrt()); // test Tridiagonalization's methods - Tridiagonalization tridiag(symmA); + Tridiagonalization tridiag(symmC); // FIXME tridiag.matrixQ().adjoint() does not work - VERIFY_IS_APPROX(MatrixType(symmA.template selfadjointView()), tridiag.matrixQ() * tridiag.matrixT().eval() * MatrixType(tridiag.matrixQ()).adjoint()); + VERIFY_IS_APPROX(MatrixType(symmC.template selfadjointView()), tridiag.matrixQ() * tridiag.matrixT().eval() * MatrixType(tridiag.matrixQ()).adjoint()); if (rows > 1) { // Test matrix with NaN - symmA(0,0) = std::numeric_limits::quiet_NaN(); - SelfAdjointEigenSolver eiSymmNaN(symmA); + symmC(0,0) = std::numeric_limits::quiet_NaN(); + SelfAdjointEigenSolver eiSymmNaN(symmC); VERIFY_IS_EQUAL(eiSymmNaN.info(), NoConvergence); } } @@ -113,8 +128,10 @@ void test_eigensolver_selfadjoint() int s = 0; for(int i = 0; i < g_repeat; i++) { // very important to test 3x3 and 2x2 matrices since we provide special paths for them + CALL_SUBTEST_1( selfadjointeigensolver(Matrix2f()) ); CALL_SUBTEST_1( selfadjointeigensolver(Matrix2d()) ); CALL_SUBTEST_1( selfadjointeigensolver(Matrix3f()) ); + CALL_SUBTEST_1( selfadjointeigensolver(Matrix3d()) ); CALL_SUBTEST_2( selfadjointeigensolver(Matrix4d()) ); s = internal::random(1,EIGEN_TEST_MAX_SIZE/4); CALL_SUBTEST_3( selfadjointeigensolver(MatrixXf(s,s)) ); diff --git a/gtsam/3rdparty/Eigen/test/geo_hyperplane.cpp b/gtsam/3rdparty/Eigen/test/geo_hyperplane.cpp index f26fc1329..327537801 100644 --- a/gtsam/3rdparty/Eigen/test/geo_hyperplane.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_hyperplane.cpp @@ -114,6 +114,32 @@ template void lines() } } +template void planes() +{ + using std::abs; + typedef Hyperplane Plane; + typedef Matrix Vector; + + for(int i = 0; i < 10; i++) + { + Vector v0 = Vector::Random(); + Vector v1(v0), v2(v0); + if(internal::random(0,1)>0.25) + v1 += Vector::Random(); + if(internal::random(0,1)>0.25) + v2 += v1 * std::pow(internal::random(0,1),internal::random(1,16)); + if(internal::random(0,1)>0.25) + v2 += Vector::Random() * std::pow(internal::random(0,1),internal::random(1,16)); + + Plane p0 = Plane::Through(v0, v1, v2); + + VERIFY_IS_APPROX(p0.normal().norm(), Scalar(1)); + VERIFY_IS_MUCH_SMALLER_THAN(p0.absDistance(v0), Scalar(1)); + VERIFY_IS_MUCH_SMALLER_THAN(p0.absDistance(v1), Scalar(1)); + VERIFY_IS_MUCH_SMALLER_THAN(p0.absDistance(v2), Scalar(1)); + } +} + template void hyperplane_alignment() { typedef Hyperplane Plane3a; @@ -153,5 +179,7 @@ void test_geo_hyperplane() CALL_SUBTEST_4( hyperplane(Hyperplane,5>()) ); CALL_SUBTEST_1( lines() ); CALL_SUBTEST_3( lines() ); + CALL_SUBTEST_2( planes() ); + CALL_SUBTEST_5( planes() ); } } diff --git a/gtsam/3rdparty/Eigen/test/geo_transformations.cpp b/gtsam/3rdparty/Eigen/test/geo_transformations.cpp index ee3030b5d..547765714 100644 --- a/gtsam/3rdparty/Eigen/test/geo_transformations.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_transformations.cpp @@ -98,11 +98,19 @@ template void transformations() Matrix3 matrot1, m; Scalar a = internal::random(-Scalar(M_PI), Scalar(M_PI)); - Scalar s0 = internal::random(); + Scalar s0 = internal::random(), + s1 = internal::random(); + + while(v0.norm() < test_precision()) v0 = Vector3::Random(); + while(v1.norm() < test_precision()) v1 = Vector3::Random(); + VERIFY_IS_APPROX(v0, AngleAxisx(a, v0.normalized()) * v0); VERIFY_IS_APPROX(-v0, AngleAxisx(Scalar(M_PI), v0.unitOrthogonal()) * v0); - VERIFY_IS_APPROX(cos(a)*v0.squaredNorm(), v0.dot(AngleAxisx(a, v0.unitOrthogonal()) * v0)); + if(abs(cos(a)) > test_precision()) + { + VERIFY_IS_APPROX(cos(a)*v0.squaredNorm(), v0.dot(AngleAxisx(a, v0.unitOrthogonal()) * v0)); + } m = AngleAxisx(a, v0.normalized()).toRotationMatrix().adjoint(); VERIFY_IS_APPROX(Matrix3::Identity(), m * AngleAxisx(a, v0.normalized())); VERIFY_IS_APPROX(Matrix3::Identity(), AngleAxisx(a, v0.normalized()) * m); @@ -123,11 +131,18 @@ template void transformations() // angle-axis conversion AngleAxisx aa = AngleAxisx(q1); VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1); - VERIFY_IS_NOT_APPROX(q1 * v1, Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1); + + if(abs(aa.angle()) > NumTraits::dummy_precision()) + { + VERIFY( !(q1 * v1).isApprox(Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1) ); + } aa.fromRotationMatrix(aa.toRotationMatrix()); VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1); - VERIFY_IS_NOT_APPROX(q1 * v1, Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1); + if(abs(aa.angle()) > NumTraits::dummy_precision()) + { + VERIFY( !(q1 * v1).isApprox(Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1) ); + } // AngleAxis VERIFY_IS_APPROX(AngleAxisx(a,v1.normalized()).toRotationMatrix(), @@ -347,7 +362,9 @@ template void transformations() // test transform inversion t0.setIdentity(); t0.translate(v0); - t0.linear().setRandom(); + do { + t0.linear().setRandom(); + } while(t0.linear().jacobiSvd().singularValues()(2)()); Matrix4 t044 = Matrix4::Zero(); t044(3,3) = 1; t044.block(0,0,t0.matrix().rows(),4) = t0.matrix(); @@ -397,6 +414,29 @@ template void transformations() t20 = Translation2(v20) * (Rotation2D(s0) * Eigen::Scaling(s0)); t21 = Translation2(v20) * Rotation2D(s0) * Eigen::Scaling(s0); VERIFY_IS_APPROX(t20,t21); + + Rotation2D R0(s0), R1(s1); + t20 = Translation2(v20) * (R0 * Eigen::Scaling(s0)); + t21 = Translation2(v20) * R0 * Eigen::Scaling(s0); + VERIFY_IS_APPROX(t20,t21); + + t20 = Translation2(v20) * (R0 * R0.inverse() * Eigen::Scaling(s0)); + t21 = Translation2(v20) * Eigen::Scaling(s0); + VERIFY_IS_APPROX(t20,t21); + + VERIFY_IS_APPROX(s0, (R0.slerp(0, R1)).angle()); + VERIFY_IS_APPROX(s1, (R0.slerp(1, R1)).angle()); + VERIFY_IS_APPROX(s0, (R0.slerp(0.5, R0)).angle()); + VERIFY_IS_APPROX(Scalar(0), (R0.slerp(0.5, R0.inverse())).angle()); + + // check basic features + { + Rotation2D r1; // default ctor + r1 = Rotation2D(s0); // copy assignment + VERIFY_IS_APPROX(r1.angle(),s0); + Rotation2D r2(r1); // copy ctor + VERIFY_IS_APPROX(r2.angle(),s0); + } } template void transform_alignment() diff --git a/gtsam/3rdparty/Eigen/test/jacobisvd.cpp b/gtsam/3rdparty/Eigen/test/jacobisvd.cpp index 7c21f0ab3..12c556e59 100644 --- a/gtsam/3rdparty/Eigen/test/jacobisvd.cpp +++ b/gtsam/3rdparty/Eigen/test/jacobisvd.cpp @@ -321,16 +321,23 @@ void jacobisvd_inf_nan() VERIFY(sub(some_inf, some_inf) != sub(some_inf, some_inf)); svd.compute(MatrixType::Constant(10,10,some_inf), ComputeFullU | ComputeFullV); - Scalar some_nan = zero() / zero(); - VERIFY(some_nan != some_nan); - svd.compute(MatrixType::Constant(10,10,some_nan), ComputeFullU | ComputeFullV); + Scalar nan = std::numeric_limits::quiet_NaN(); + VERIFY(nan != nan); + svd.compute(MatrixType::Constant(10,10,nan), ComputeFullU | ComputeFullV); MatrixType m = MatrixType::Zero(10,10); m(internal::random(0,9), internal::random(0,9)) = some_inf; svd.compute(m, ComputeFullU | ComputeFullV); m = MatrixType::Zero(10,10); - m(internal::random(0,9), internal::random(0,9)) = some_nan; + m(internal::random(0,9), internal::random(0,9)) = nan; + svd.compute(m, ComputeFullU | ComputeFullV); + + // regression test for bug 791 + m.resize(3,3); + m << 0, 2*NumTraits::epsilon(), 0.5, + 0, -0.5, 0, + nan, 0, 0; svd.compute(m, ComputeFullU | ComputeFullV); } @@ -434,6 +441,7 @@ void test_jacobisvd() // Test on inf/nan matrix CALL_SUBTEST_7( jacobisvd_inf_nan() ); + CALL_SUBTEST_10( jacobisvd_inf_nan() ); } CALL_SUBTEST_7(( jacobisvd(MatrixXf(internal::random(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2), internal::random(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2))) )); diff --git a/gtsam/3rdparty/Eigen/test/main.h b/gtsam/3rdparty/Eigen/test/main.h index 14f0d2f78..664204866 100644 --- a/gtsam/3rdparty/Eigen/test/main.h +++ b/gtsam/3rdparty/Eigen/test/main.h @@ -17,13 +17,36 @@ #include #include #include + +// The following includes of STL headers have to be done _before_ the +// definition of macros min() and max(). The reason is that many STL +// implementations will not work properly as the min and max symbols collide +// with the STL functions std:min() and std::max(). The STL headers may check +// for the macro definition of min/max and issue a warning or undefine the +// macros. +// +// Still, Windows defines min() and max() in windef.h as part of the regular +// Windows system interfaces and many other Windows APIs depend on these +// macros being available. To prevent the macro expansion of min/max and to +// make Eigen compatible with the Windows environment all function calls of +// std::min() and std::max() have to be written with parenthesis around the +// function name. +// +// All STL headers used by Eigen should be included here. Because main.h is +// included before any Eigen header and because the STL headers are guarded +// against multiple inclusions, no STL header will see our own min/max macro +// definitions. #include #include -#include #include #include #include +#include +// To test that all calls from Eigen code to std::min() and std::max() are +// protected by parenthesis against macro expansion, the min()/max() macros +// are defined here and any not-parenthesized min/max call will cause a +// compiler error. #define min(A,B) please_protect_your_min_with_parentheses #define max(A,B) please_protect_your_max_with_parentheses @@ -383,6 +406,26 @@ void randomPermutationVector(PermutationVectorType& v, typename PermutationVecto } } +template bool isNotNaN(const T& x) +{ + return x==x; +} + +template bool isNaN(const T& x) +{ + return x!=x; +} + +template bool isInf(const T& x) +{ + return x > NumTraits::highest(); +} + +template bool isMinusInf(const T& x) +{ + return x < NumTraits::lowest(); +} + } // end namespace Eigen template struct GetDifferentType; diff --git a/gtsam/3rdparty/Eigen/test/nomalloc.cpp b/gtsam/3rdparty/Eigen/test/nomalloc.cpp index cbd02dd21..8e0402358 100644 --- a/gtsam/3rdparty/Eigen/test/nomalloc.cpp +++ b/gtsam/3rdparty/Eigen/test/nomalloc.cpp @@ -165,6 +165,38 @@ void ctms_decompositions() Eigen::JacobiSVD jSVD; jSVD.compute(A, ComputeFullU | ComputeFullV); } +void test_zerosized() { + // default constructors: + Eigen::MatrixXd A; + Eigen::VectorXd v; + // explicit zero-sized: + Eigen::ArrayXXd A0(0,0); + Eigen::ArrayXd v0(std::ptrdiff_t(0)); // FIXME ArrayXd(0) is ambiguous + + // assigning empty objects to each other: + A=A0; + v=v0; +} + +template void test_reference(const MatrixType& m) { + typedef typename MatrixType::Scalar Scalar; + enum { Flag = MatrixType::IsRowMajor ? Eigen::RowMajor : Eigen::ColMajor}; + enum { TransposeFlag = !MatrixType::IsRowMajor ? Eigen::RowMajor : Eigen::ColMajor}; + typename MatrixType::Index rows = m.rows(), cols=m.cols(); + // Dynamic reference: + typedef Eigen::Ref > Ref; + typedef Eigen::Ref > RefT; + + Ref r1(m); + Ref r2(m.block(rows/3, cols/4, rows/2, cols/2)); + RefT r3(m.transpose()); + RefT r4(m.topLeftCorner(rows/2, cols/2).transpose()); + + VERIFY_RAISES_ASSERT(RefT r5(m)); + VERIFY_RAISES_ASSERT(Ref r6(m.transpose())); + VERIFY_RAISES_ASSERT(Ref r7(Scalar(2) * m)); +} + void test_nomalloc() { // check that our operator new is indeed called: @@ -175,5 +207,6 @@ void test_nomalloc() // Check decomposition modules with dynamic matrices that have a known compile-time max size (ctms) CALL_SUBTEST_4(ctms_decompositions()); - + CALL_SUBTEST_5(test_zerosized()); + CALL_SUBTEST_6(test_reference(Matrix())); } diff --git a/gtsam/3rdparty/Eigen/test/nullary.cpp b/gtsam/3rdparty/Eigen/test/nullary.cpp index 5408d88b2..fbc721a1a 100644 --- a/gtsam/3rdparty/Eigen/test/nullary.cpp +++ b/gtsam/3rdparty/Eigen/test/nullary.cpp @@ -80,7 +80,9 @@ void testVectorType(const VectorType& base) Matrix col_vector(size); row_vector.setLinSpaced(size,low,high); col_vector.setLinSpaced(size,low,high); - VERIFY( row_vector.isApprox(col_vector.transpose(), NumTraits::epsilon())); + // when using the extended precision (e.g., FPU) the relative error might exceed 1 bit + // when computing the squared sum in isApprox, thus the 2x factor. + VERIFY( row_vector.isApprox(col_vector.transpose(), Scalar(2)*NumTraits::epsilon())); Matrix size_changer(size+50); size_changer.setLinSpaced(size,low,high); diff --git a/gtsam/3rdparty/Eigen/test/packetmath.cpp b/gtsam/3rdparty/Eigen/test/packetmath.cpp index 2c0519c41..38aa256ce 100644 --- a/gtsam/3rdparty/Eigen/test/packetmath.cpp +++ b/gtsam/3rdparty/Eigen/test/packetmath.cpp @@ -239,6 +239,12 @@ template void packetmath_real() data2[i] = internal::random(-87,88); } CHECK_CWISE1_IF(internal::packet_traits::HasExp, std::exp, internal::pexp); + { + data1[0] = std::numeric_limits::quiet_NaN(); + packet_helper::HasExp,Packet> h; + h.store(data2, internal::pexp(h.load(data1))); + VERIFY(isNaN(data2[0])); + } for (int i=0; i void packetmath_real() } if(internal::random(0,1)<0.1) data1[internal::random(0, PacketSize)] = 0; - CHECK_CWISE1_IF(internal::packet_traits::HasLog, std::log, internal::plog); CHECK_CWISE1_IF(internal::packet_traits::HasSqrt, std::sqrt, internal::psqrt); + CHECK_CWISE1_IF(internal::packet_traits::HasLog, std::log, internal::plog); + { + data1[0] = std::numeric_limits::quiet_NaN(); + packet_helper::HasLog,Packet> h; + h.store(data2, internal::plog(h.load(data1))); + VERIFY(isNaN(data2[0])); + data1[0] = -1.0f; + h.store(data2, internal::plog(h.load(data1))); + VERIFY(isNaN(data2[0])); +#if !EIGEN_FAST_MATH + h.store(data2, internal::psqrt(h.load(data1))); + VERIFY(isNaN(data2[0])); + VERIFY(isNaN(data2[1])); +#endif + } } template void packetmath_notcomplex() diff --git a/gtsam/3rdparty/Eigen/test/product.h b/gtsam/3rdparty/Eigen/test/product.h index 856b234ac..0b3abe402 100644 --- a/gtsam/3rdparty/Eigen/test/product.h +++ b/gtsam/3rdparty/Eigen/test/product.h @@ -139,4 +139,12 @@ template void product(const MatrixType& m) // inner product Scalar x = square2.row(c) * square2.col(c2); VERIFY_IS_APPROX(x, square2.row(c).transpose().cwiseProduct(square2.col(c2)).sum()); + + // outer product + VERIFY_IS_APPROX(m1.col(c) * m1.row(r), m1.block(0,c,rows,1) * m1.block(r,0,1,cols)); + VERIFY_IS_APPROX(m1.row(r).transpose() * m1.col(c).transpose(), m1.block(r,0,1,cols).transpose() * m1.block(0,c,rows,1).transpose()); + VERIFY_IS_APPROX(m1.block(0,c,rows,1) * m1.row(r), m1.block(0,c,rows,1) * m1.block(r,0,1,cols)); + VERIFY_IS_APPROX(m1.col(c) * m1.block(r,0,1,cols), m1.block(0,c,rows,1) * m1.block(r,0,1,cols)); + VERIFY_IS_APPROX(m1.leftCols(1) * m1.row(r), m1.block(0,0,rows,1) * m1.block(r,0,1,cols)); + VERIFY_IS_APPROX(m1.col(c) * m1.topRows(1), m1.block(0,c,rows,1) * m1.block(0,0,1,cols)); } diff --git a/gtsam/3rdparty/Eigen/test/ref.cpp b/gtsam/3rdparty/Eigen/test/ref.cpp index 19e81549c..32eb31048 100644 --- a/gtsam/3rdparty/Eigen/test/ref.cpp +++ b/gtsam/3rdparty/Eigen/test/ref.cpp @@ -183,15 +183,15 @@ void call_ref() VERIFY_EVALUATION_COUNT( call_ref_1(a,a), 0); VERIFY_EVALUATION_COUNT( call_ref_1(b,b.transpose()), 0); -// call_ref_1(ac); // does not compile because ac is const +// call_ref_1(ac,a +void check_sparse_abs_determinant(Solver& solver, const typename Solver::MatrixType& A, const DenseMat& dA) +{ + using std::abs; + typedef typename Solver::MatrixType Mat; + typedef typename Mat::Scalar Scalar; + + solver.compute(A); + if (solver.info() != Success) + { + std::cerr << "sparse solver testing: factorization failed (check_sparse_abs_determinant)\n"; + return; + } + Scalar refDet = abs(dA.determinant()); + VERIFY_IS_APPROX(refDet,solver.absDeterminant()); +} template int generate_sparse_spd_problem(Solver& , typename Solver::MatrixType& A, typename Solver::MatrixType& halfA, DenseMat& dA, int maxSize = 300) @@ -324,3 +340,20 @@ template void check_sparse_square_determinant(Solver& solver) check_sparse_determinant(solver, A, dA); } } + +template void check_sparse_square_abs_determinant(Solver& solver) +{ + typedef typename Solver::MatrixType Mat; + typedef typename Mat::Scalar Scalar; + typedef Matrix DenseMatrix; + + // generate the problem + Mat A; + DenseMatrix dA; + generate_sparse_square_problem(solver, A, dA, 30); + A.makeCompressed(); + for (int i = 0; i < g_repeat; i++) { + check_sparse_abs_determinant(solver, A, dA); + } +} + diff --git a/gtsam/3rdparty/Eigen/test/sparselu.cpp b/gtsam/3rdparty/Eigen/test/sparselu.cpp index 37980defc..52371cb12 100644 --- a/gtsam/3rdparty/Eigen/test/sparselu.cpp +++ b/gtsam/3rdparty/Eigen/test/sparselu.cpp @@ -44,6 +44,9 @@ template void test_sparselu_T() check_sparse_square_solving(sparselu_colamd); check_sparse_square_solving(sparselu_amd); check_sparse_square_solving(sparselu_natural); + + check_sparse_square_abs_determinant(sparselu_colamd); + check_sparse_square_abs_determinant(sparselu_amd); } void test_sparselu() diff --git a/gtsam/3rdparty/Eigen/test/sparseqr.cpp b/gtsam/3rdparty/Eigen/test/sparseqr.cpp index 1fe4a98ee..451c0e7f8 100644 --- a/gtsam/3rdparty/Eigen/test/sparseqr.cpp +++ b/gtsam/3rdparty/Eigen/test/sparseqr.cpp @@ -10,12 +10,11 @@ #include template -int generate_sparse_rectangular_problem(MatrixType& A, DenseMat& dA, int maxRows = 300, int maxCols = 150) +int generate_sparse_rectangular_problem(MatrixType& A, DenseMat& dA, int maxRows = 300) { - eigen_assert(maxRows >= maxCols); typedef typename MatrixType::Scalar Scalar; int rows = internal::random(1,maxRows); - int cols = internal::random(1,maxCols); + int cols = internal::random(1,rows); double density = (std::max)(8./(rows*cols), 0.01); A.resize(rows,cols); @@ -54,6 +53,8 @@ template void test_sparseqr_scalar() b = dA * DenseVector::Random(A.cols()); solver.compute(A); + if(internal::random(0,1)>0.5) + solver.factorize(A); // this checks that calling analyzePattern is not needed if the pattern do not change. if (solver.info() != Success) { std::cerr << "sparse QR factorization failed\n"; diff --git a/gtsam/3rdparty/Eigen/test/stable_norm.cpp b/gtsam/3rdparty/Eigen/test/stable_norm.cpp index 549f91fbf..231dd9189 100644 --- a/gtsam/3rdparty/Eigen/test/stable_norm.cpp +++ b/gtsam/3rdparty/Eigen/test/stable_norm.cpp @@ -9,11 +9,6 @@ #include "main.h" -template bool isNotNaN(const T& x) -{ - return x==x; -} - // workaround aggressive optimization in ICC template EIGEN_DONT_INLINE T sub(T a, T b) { return a - b; } diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport b/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport index c4090ab11..e2769449c 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport @@ -178,11 +178,11 @@ template void glLoadMatrix(const Transform& t) template void glLoadMatrix(const Transform& t) { glLoadMatrix(t.matrix()); } template void glLoadMatrix(const Transform& t) { glLoadMatrix(Transform(t).matrix()); } -static void glRotate(const Rotation2D& rot) +inline void glRotate(const Rotation2D& rot) { glRotatef(rot.angle()*180.f/float(M_PI), 0.f, 0.f, 1.f); } -static void glRotate(const Rotation2D& rot) +inline void glRotate(const Rotation2D& rot) { glRotated(rot.angle()*180.0/M_PI, 0.0, 0.0, 1.0); } @@ -246,18 +246,18 @@ EIGEN_GL_FUNC1_SPECIALIZATION_MAT(glGet,GLenum,_,double, 4,4,Doublev) #ifdef GL_VERSION_2_0 -static void glUniform2fv_ei (GLint loc, const float* v) { glUniform2fv(loc,1,v); } -static void glUniform2iv_ei (GLint loc, const int* v) { glUniform2iv(loc,1,v); } +inline void glUniform2fv_ei (GLint loc, const float* v) { glUniform2fv(loc,1,v); } +inline void glUniform2iv_ei (GLint loc, const int* v) { glUniform2iv(loc,1,v); } -static void glUniform3fv_ei (GLint loc, const float* v) { glUniform3fv(loc,1,v); } -static void glUniform3iv_ei (GLint loc, const int* v) { glUniform3iv(loc,1,v); } +inline void glUniform3fv_ei (GLint loc, const float* v) { glUniform3fv(loc,1,v); } +inline void glUniform3iv_ei (GLint loc, const int* v) { glUniform3iv(loc,1,v); } -static void glUniform4fv_ei (GLint loc, const float* v) { glUniform4fv(loc,1,v); } -static void glUniform4iv_ei (GLint loc, const int* v) { glUniform4iv(loc,1,v); } +inline void glUniform4fv_ei (GLint loc, const float* v) { glUniform4fv(loc,1,v); } +inline void glUniform4iv_ei (GLint loc, const int* v) { glUniform4iv(loc,1,v); } -static void glUniformMatrix2fv_ei (GLint loc, const float* v) { glUniformMatrix2fv(loc,1,false,v); } -static void glUniformMatrix3fv_ei (GLint loc, const float* v) { glUniformMatrix3fv(loc,1,false,v); } -static void glUniformMatrix4fv_ei (GLint loc, const float* v) { glUniformMatrix4fv(loc,1,false,v); } +inline void glUniformMatrix2fv_ei (GLint loc, const float* v) { glUniformMatrix2fv(loc,1,false,v); } +inline void glUniformMatrix3fv_ei (GLint loc, const float* v) { glUniformMatrix3fv(loc,1,false,v); } +inline void glUniformMatrix4fv_ei (GLint loc, const float* v) { glUniformMatrix4fv(loc,1,false,v); } EIGEN_GL_FUNC1_DECLARATION (glUniform,GLint,const) @@ -294,9 +294,9 @@ EIGEN_GL_FUNC1_SPECIALIZATION_MAT(glUniform,GLint,const,float, 4,3,Matrix #ifdef GL_VERSION_3_0 -static void glUniform2uiv_ei (GLint loc, const unsigned int* v) { glUniform2uiv(loc,1,v); } -static void glUniform3uiv_ei (GLint loc, const unsigned int* v) { glUniform3uiv(loc,1,v); } -static void glUniform4uiv_ei (GLint loc, const unsigned int* v) { glUniform4uiv(loc,1,v); } +inline void glUniform2uiv_ei (GLint loc, const unsigned int* v) { glUniform2uiv(loc,1,v); } +inline void glUniform3uiv_ei (GLint loc, const unsigned int* v) { glUniform3uiv(loc,1,v); } +inline void glUniform4uiv_ei (GLint loc, const unsigned int* v) { glUniform4uiv(loc,1,v); } EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,unsigned int, 2,2uiv_ei) EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,unsigned int, 3,3uiv_ei) @@ -305,9 +305,9 @@ EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,unsigned int, 4,4uiv_ei) #endif #ifdef GL_ARB_gpu_shader_fp64 -static void glUniform2dv_ei (GLint loc, const double* v) { glUniform2dv(loc,1,v); } -static void glUniform3dv_ei (GLint loc, const double* v) { glUniform3dv(loc,1,v); } -static void glUniform4dv_ei (GLint loc, const double* v) { glUniform4dv(loc,1,v); } +inline void glUniform2dv_ei (GLint loc, const double* v) { glUniform2dv(loc,1,v); } +inline void glUniform3dv_ei (GLint loc, const double* v) { glUniform3dv(loc,1,v); } +inline void glUniform4dv_ei (GLint loc, const double* v) { glUniform4dv(loc,1,v); } EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,double, 2,2dv_ei) EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,double, 3,3dv_ei) diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h index c32437281..78a307e96 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h @@ -110,7 +110,6 @@ void MatrixPowerAtomic::compute2x2(MatrixType& res, RealScalar p) co using std::abs; using std::pow; - ArrayType logTdiag = m_A.diagonal().array().log(); res.coeffRef(0,0) = pow(m_A.coeff(0,0), p); for (Index i=1; i < m_A.cols(); ++i) { diff --git a/gtsam/3rdparty/Eigen/unsupported/doc/examples/CMakeLists.txt b/gtsam/3rdparty/Eigen/unsupported/doc/examples/CMakeLists.txt index 978f9afd8..c47646dfc 100644 --- a/gtsam/3rdparty/Eigen/unsupported/doc/examples/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/unsupported/doc/examples/CMakeLists.txt @@ -10,12 +10,10 @@ FOREACH(example_src ${examples_SRCS}) if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO) target_link_libraries(example_${example} ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO}) endif() - GET_TARGET_PROPERTY(example_executable - example_${example} LOCATION) ADD_CUSTOM_COMMAND( TARGET example_${example} POST_BUILD - COMMAND ${example_executable} + COMMAND example_${example} ARGS >${CMAKE_CURRENT_BINARY_DIR}/${example}.out ) ADD_DEPENDENCIES(unsupported_examples example_${example}) diff --git a/gtsam/3rdparty/Eigen/unsupported/doc/snippets/CMakeLists.txt b/gtsam/3rdparty/Eigen/unsupported/doc/snippets/CMakeLists.txt index 4a4157933..f0c5cc2a8 100644 --- a/gtsam/3rdparty/Eigen/unsupported/doc/snippets/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/unsupported/doc/snippets/CMakeLists.txt @@ -14,12 +14,10 @@ FOREACH(snippet_src ${snippets_SRCS}) if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO) target_link_libraries(${compile_snippet_target} ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO}) endif() - GET_TARGET_PROPERTY(compile_snippet_executable - ${compile_snippet_target} LOCATION) ADD_CUSTOM_COMMAND( TARGET ${compile_snippet_target} POST_BUILD - COMMAND ${compile_snippet_executable} + COMMAND ${compile_snippet_target} ARGS >${CMAKE_CURRENT_BINARY_DIR}/${snippet}.out ) ADD_DEPENDENCIES(unsupported_snippets ${compile_snippet_target}) diff --git a/gtsam/3rdparty/Eigen/unsupported/test/CMakeLists.txt b/gtsam/3rdparty/Eigen/unsupported/test/CMakeLists.txt index a94a3b5e5..2e4cfdb2e 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/unsupported/test/CMakeLists.txt @@ -29,11 +29,7 @@ ei_add_test(NonLinearOptimization) ei_add_test(NumericalDiff) ei_add_test(autodiff) - -if (NOT CMAKE_CXX_COMPILER MATCHES "clang\\+\\+$") ei_add_test(BVH) -endif() - ei_add_test(matrix_exponential) ei_add_test(matrix_function) ei_add_test(matrix_power) @@ -73,8 +69,9 @@ if(NOT EIGEN_TEST_NO_OPENGL) find_package(GLUT) find_package(GLEW) if(OPENGL_FOUND AND GLUT_FOUND AND GLEW_FOUND) + include_directories(${OPENGL_INCLUDE_DIR} ${GLUT_INCLUDE_DIR} ${GLEW_INCLUDE_DIRS}) ei_add_property(EIGEN_TESTED_BACKENDS "OpenGL, ") - set(EIGEN_GL_LIB ${GLUT_LIBRARIES} ${GLEW_LIBRARIES}) + set(EIGEN_GL_LIB ${GLUT_LIBRARIES} ${GLEW_LIBRARIES} ${OPENGL_LIBRARIES}) ei_add_test(openglsupport "" "${EIGEN_GL_LIB}" ) else() ei_add_property(EIGEN_MISSING_BACKENDS "OpenGL, ") diff --git a/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h b/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h index ef0a6a9f0..dddda7dd9 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h +++ b/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h @@ -1,59 +1,47 @@ /* - Multi-precision floating point number class for C++. + MPFR C++: Multi-precision floating point number class for C++. Based on MPFR library: http://mpfr.org Project homepage: http://www.holoborodko.com/pavel/mpfr Contact e-mail: pavel@holoborodko.com - Copyright (c) 2008-2012 Pavel Holoborodko + Copyright (c) 2008-2014 Pavel Holoborodko Contributors: Dmitriy Gubanov, Konstantin Holoborodko, Brian Gladman, Helmut Jarausch, Fokko Beekhof, Ulrich Mutze, Heinz van Saanen, Pere Constans, Peter van Hoof, Gael Guennebaud, Tsai Chia Cheng, - Alexei Zubanov, Jauhien Piatlicki, Victor Berger, John Westwood. + Alexei Zubanov, Jauhien Piatlicki, Victor Berger, John Westwood, + Petr Aleksandrov, Orion Poplawski, Charles Karney. - **************************************************************************** - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. + Licensing: + (A) MPFR C++ is under GNU General Public License ("GPL"). + + (B) Non-free licenses may also be purchased from the author, for users who + do not want their programs protected by the GPL. - This library is distributed in the hope that it will be useful, + The non-free licenses are for users that wish to use MPFR C++ in + their products but are unwilling to release their software + under the GPL (which would require them to release source code + and allow free redistribution). + + Such users can purchase an unlimited-use license from the author. + Contact us for more details. + + GNU General Public License ("GPL") copyright permissions statement: + ************************************************************************** + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - - **************************************************************************** - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions - are met: - - 1. Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - - 2. Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - - 3. The name of the author may not be used to endorse or promote products - derived from this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND - ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE - FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS - OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) - HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY - OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF - SUCH DAMAGE. + You should have received a copy of the GNU General Public License + along with this program. If not, see . */ #ifndef __MPREAL_H__ @@ -65,11 +53,21 @@ #include #include #include +#include #include // Options #define MPREAL_HAVE_INT64_SUPPORT // Enable int64_t support if possible. Available only for MSVC 2010 & GCC. #define MPREAL_HAVE_MSVC_DEBUGVIEW // Enable Debugger Visualizer for "Debug" builds in MSVC. +#define MPREAL_HAVE_DYNAMIC_STD_NUMERIC_LIMITS // Enable extended std::numeric_limits specialization. + // Meaning that "digits", "round_style" and similar members are defined as functions, not constants. + // See std::numeric_limits at the end of the file for more information. + +// Library version +#define MPREAL_VERSION_MAJOR 3 +#define MPREAL_VERSION_MINOR 5 +#define MPREAL_VERSION_PATCHLEVEL 9 +#define MPREAL_VERSION_STRING "3.5.9" // Detect compiler using signatures from http://predef.sourceforge.net/ #if defined(__GNUC__) && defined(__INTEL_COMPILER) @@ -82,6 +80,32 @@ #define IsInf(x) std::isinf(x) // GNU C/C++ (and/or other compilers), just hope for C99 conformance #endif +// A Clang feature extension to determine compiler features. +#ifndef __has_feature + #define __has_feature(x) 0 +#endif + +// Detect support for r-value references (move semantic). Borrowed from Eigen. +#if (__has_feature(cxx_rvalue_references) || \ + defined(__GXX_EXPERIMENTAL_CXX0X__) || __cplusplus >= 201103L || \ + (defined(_MSC_VER) && _MSC_VER >= 1600)) + + #define MPREAL_HAVE_MOVE_SUPPORT + + // Use fields in mpfr_t structure to check if it was initialized / set dummy initialization + #define mpfr_is_initialized(x) (0 != (x)->_mpfr_d) + #define mpfr_set_uninitialized(x) ((x)->_mpfr_d = 0 ) +#endif + +// Detect support for explicit converters. +#if (__has_feature(cxx_explicit_conversions) || \ + defined(__GXX_EXPERIMENTAL_CXX0X__) || __cplusplus >= 201103L || \ + (defined(_MSC_VER) && _MSC_VER >= 1800)) + + #define MPREAL_HAVE_EXPLICIT_CONVERTERS +#endif + +// Detect available 64-bit capabilities #if defined(MPREAL_HAVE_INT64_SUPPORT) #define MPFR_USE_INTMAX_T // Should be defined before mpfr.h @@ -97,7 +121,7 @@ #endif #elif defined (__GNUC__) && defined(__linux__) - #if defined(__amd64__) || defined(__amd64) || defined(__x86_64__) || defined(__x86_64) || defined(__ia64) || defined(__itanium__) || defined(_M_IA64) + #if defined(__amd64__) || defined(__amd64) || defined(__x86_64__) || defined(__x86_64) || defined(__ia64) || defined(__itanium__) || defined(_M_IA64) || defined (__PPC64__) #undef MPREAL_HAVE_INT64_SUPPORT // Remove all shaman dances for x64 builds since #undef MPFR_USE_INTMAX_T // GCC already supports x64 as of "long int" is 64-bit integer, nothing left to do #else @@ -111,7 +135,7 @@ #endif #if defined(MPREAL_HAVE_MSVC_DEBUGVIEW) && defined(_MSC_VER) && defined(_DEBUG) -#define MPREAL_MSVC_DEBUGVIEW_CODE DebugView = toString(); + #define MPREAL_MSVC_DEBUGVIEW_CODE DebugView = toString(); #define MPREAL_MSVC_DEBUGVIEW_DATA std::string DebugView; #else #define MPREAL_MSVC_DEBUGVIEW_CODE @@ -149,7 +173,6 @@ public: // Constructors && type conversions mpreal(); mpreal(const mpreal& u); - mpreal(const mpfr_t u); mpreal(const mpf_t u); mpreal(const mpz_t u, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t mode = mpreal::get_default_rnd()); mpreal(const mpq_t u, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t mode = mpreal::get_default_rnd()); @@ -159,6 +182,10 @@ public: mpreal(const unsigned int u, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t mode = mpreal::get_default_rnd()); mpreal(const long int u, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t mode = mpreal::get_default_rnd()); mpreal(const int u, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t mode = mpreal::get_default_rnd()); + + // Construct mpreal from mpfr_t structure. + // shared = true allows to avoid deep copy, so that mpreal and 'u' share the same data & pointers. + mpreal(const mpfr_t u, bool shared = false); #if defined (MPREAL_HAVE_INT64_SUPPORT) mpreal(const uint64_t u, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t mode = mpreal::get_default_rnd()); @@ -170,6 +197,11 @@ public: ~mpreal(); +#ifdef MPREAL_HAVE_MOVE_SUPPORT + mpreal& operator=(mpreal&& v); + mpreal(mpreal&& u); +#endif + // Operations // = // +, -, *, /, ++, --, <<, >> @@ -292,14 +324,34 @@ public: friend bool operator == (const mpreal& a, const double b); // Type Conversion operators + bool toBool (mp_rnd_t mode = GMP_RNDZ) const; long toLong (mp_rnd_t mode = GMP_RNDZ) const; unsigned long toULong (mp_rnd_t mode = GMP_RNDZ) const; + float toFloat (mp_rnd_t mode = GMP_RNDN) const; double toDouble (mp_rnd_t mode = GMP_RNDN) const; long double toLDouble (mp_rnd_t mode = GMP_RNDN) const; +#if defined (MPREAL_HAVE_EXPLICIT_CONVERTERS) + explicit operator bool () const { return toBool(); } + explicit operator int () const { return toLong(); } + explicit operator long () const { return toLong(); } + explicit operator long long () const { return toLong(); } + explicit operator unsigned () const { return toULong(); } + explicit operator unsigned long () const { return toULong(); } + explicit operator unsigned long long () const { return toULong(); } + explicit operator float () const { return toFloat(); } + explicit operator double () const { return toDouble(); } + explicit operator long double () const { return toLDouble(); } +#endif + #if defined (MPREAL_HAVE_INT64_SUPPORT) int64_t toInt64 (mp_rnd_t mode = GMP_RNDZ) const; uint64_t toUInt64 (mp_rnd_t mode = GMP_RNDZ) const; + + #if defined (MPREAL_HAVE_EXPLICIT_CONVERTERS) + explicit operator int64_t () const { return toInt64(); } + explicit operator uint64_t () const { return toUInt64(); } + #endif #endif // Get raw pointers so that mpreal can be directly used in raw mpfr_* functions @@ -308,121 +360,125 @@ public: ::mpfr_srcptr mpfr_srcptr() const; // Convert mpreal to string with n significant digits in base b - // n = 0 -> convert with the maximum available digits - std::string toString(int n = 0, int b = 10, mp_rnd_t mode = mpreal::get_default_rnd()) const; + // n = -1 -> convert with the maximum available digits + std::string toString(int n = -1, int b = 10, mp_rnd_t mode = mpreal::get_default_rnd()) const; #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - std::string toString(const std::string& format) const; + std::string toString(const std::string& format) const; #endif - // Math Functions - friend const mpreal sqr (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal sqrt(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal sqrt(const unsigned long int v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal cbrt(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal root(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal pow (const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal pow (const mpreal& a, const mpz_t b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal pow (const mpreal& a, const unsigned long int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal pow (const mpreal& a, const long int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal pow (const unsigned long int a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal pow (const unsigned long int a, const unsigned long int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal fabs(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + std::ostream& output(std::ostream& os) const; - friend const mpreal abs(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal dim(const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend inline const mpreal mul_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend inline const mpreal mul_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend inline const mpreal div_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend inline const mpreal div_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + // Math Functions + friend const mpreal sqr (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal sqrt(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal sqrt(const unsigned long int v, mp_rnd_t rnd_mode); + friend const mpreal cbrt(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal root(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode); + friend const mpreal pow (const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode); + friend const mpreal pow (const mpreal& a, const mpz_t b, mp_rnd_t rnd_mode); + friend const mpreal pow (const mpreal& a, const unsigned long int b, mp_rnd_t rnd_mode); + friend const mpreal pow (const mpreal& a, const long int b, mp_rnd_t rnd_mode); + friend const mpreal pow (const unsigned long int a, const mpreal& b, mp_rnd_t rnd_mode); + friend const mpreal pow (const unsigned long int a, const unsigned long int b, mp_rnd_t rnd_mode); + friend const mpreal fabs(const mpreal& v, mp_rnd_t rnd_mode); + + friend const mpreal abs(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal dim(const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode); + friend inline const mpreal mul_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode); + friend inline const mpreal mul_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode); + friend inline const mpreal div_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode); + friend inline const mpreal div_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode); friend int cmpabs(const mpreal& a,const mpreal& b); - friend const mpreal log (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal log2 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal log10(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal exp (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal exp2 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal exp10(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal log1p(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal expm1(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal log (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal log2 (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal log10(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal exp (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal exp2 (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal exp10(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal log1p(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal expm1(const mpreal& v, mp_rnd_t rnd_mode); - friend const mpreal cos(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal sin(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal tan(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal sec(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal csc(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal cot(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend int sin_cos(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal cos(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal sin(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal tan(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal sec(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal csc(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal cot(const mpreal& v, mp_rnd_t rnd_mode); + friend int sin_cos(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode); - friend const mpreal acos (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal asin (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal atan (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal atan2 (const mpreal& y, const mpreal& x, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal acot (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal asec (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal acsc (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal acos (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal asin (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal atan (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal atan2 (const mpreal& y, const mpreal& x, mp_rnd_t rnd_mode); + friend const mpreal acot (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal asec (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal acsc (const mpreal& v, mp_rnd_t rnd_mode); - friend const mpreal cosh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal sinh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal tanh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal sech (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal csch (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal coth (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal acosh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal asinh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal atanh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal acoth (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal asech (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal acsch (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal cosh (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal sinh (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal tanh (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal sech (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal csch (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal coth (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal acosh (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal asinh (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal atanh (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal acoth (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal asech (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal acsch (const mpreal& v, mp_rnd_t rnd_mode); - friend const mpreal hypot (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal hypot (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); - friend const mpreal fac_ui (unsigned long int v, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal eint (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal fac_ui (unsigned long int v, mp_prec_t prec, mp_rnd_t rnd_mode); + friend const mpreal eint (const mpreal& v, mp_rnd_t rnd_mode); - friend const mpreal gamma (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal lngamma (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal lgamma (const mpreal& v, int *signp = 0, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal zeta (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal erf (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal erfc (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal besselj0 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal besselj1 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal besseljn (long n, const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal bessely0 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal bessely1 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal besselyn (long n, const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal fma (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal fms (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal agm (const mpreal& v1, const mpreal& v2, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal sum (const mpreal tab[], unsigned long int n, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal gamma (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal lngamma (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal lgamma (const mpreal& v, int *signp, mp_rnd_t rnd_mode); + friend const mpreal zeta (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal erf (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal erfc (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal besselj0 (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal besselj1 (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal besseljn (long n, const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal bessely0 (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal bessely1 (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal besselyn (long n, const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal fma (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode); + friend const mpreal fms (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode); + friend const mpreal agm (const mpreal& v1, const mpreal& v2, mp_rnd_t rnd_mode); + friend const mpreal sum (const mpreal tab[], unsigned long int n, mp_rnd_t rnd_mode); friend int sgn(const mpreal& v); // returns -1 or +1 // MPFR 2.4.0 Specifics #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - friend int sinh_cosh (mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal li2 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal fmod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal rec_sqrt (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend int sinh_cosh (mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal li2 (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal fmod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); + friend const mpreal rec_sqrt (const mpreal& v, mp_rnd_t rnd_mode); // MATLAB's semantic equivalents - friend const mpreal rem (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); // Remainder after division - friend const mpreal mod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); // Modulus after division + friend const mpreal rem (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); // Remainder after division + friend const mpreal mod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); // Modulus after division #endif // MPFR 3.0.0 Specifics #if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) - friend const mpreal digamma (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal ai (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal urandom (gmp_randstate_t& state, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); // use gmp_randinit_default() to init state, gmp_randclear() to clear + friend const mpreal digamma (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal ai (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal urandom (gmp_randstate_t& state, mp_rnd_t rnd_mode); // use gmp_randinit_default() to init state, gmp_randclear() to clear + friend const mpreal grandom (gmp_randstate_t& state, mp_rnd_t rnd_mode); // use gmp_randinit_default() to init state, gmp_randclear() to clear + friend const mpreal grandom (unsigned int seed); #endif // Uniformly distributed random number generation in [0,1] using // Mersenne-Twister algorithm by default. // Use parameter to setup seed, e.g.: random((unsigned)time(NULL)) // Check urandom() for more precise control. - friend const mpreal random(unsigned int seed = 0); - + friend const mpreal random(unsigned int seed); + // Exponent and mantissa manipulation friend const mpreal frexp(const mpreal& v, mp_exp_t* exp); friend const mpreal ldexp(const mpreal& v, mp_exp_t exp); @@ -433,31 +489,31 @@ public: // Constants // don't forget to call mpfr_free_cache() for every thread where you are using const-functions - friend const mpreal const_log2 (mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal const_pi (mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal const_euler (mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal const_catalan (mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal const_log2 (mp_prec_t prec, mp_rnd_t rnd_mode); + friend const mpreal const_pi (mp_prec_t prec, mp_rnd_t rnd_mode); + friend const mpreal const_euler (mp_prec_t prec, mp_rnd_t rnd_mode); + friend const mpreal const_catalan (mp_prec_t prec, mp_rnd_t rnd_mode); // returns +inf iff sign>=0 otherwise -inf - friend const mpreal const_infinity(int sign = 1, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal const_infinity(int sign, mp_prec_t prec); // Output/ Input friend std::ostream& operator<<(std::ostream& os, const mpreal& v); friend std::istream& operator>>(std::istream& is, mpreal& v); // Integer Related Functions - friend const mpreal rint (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal rint (const mpreal& v, mp_rnd_t rnd_mode); friend const mpreal ceil (const mpreal& v); friend const mpreal floor(const mpreal& v); friend const mpreal round(const mpreal& v); friend const mpreal trunc(const mpreal& v); - friend const mpreal rint_ceil (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal rint_floor (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal rint_round (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal rint_trunc (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal frac (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal remainder ( const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal remquo (long* q, const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal rint_ceil (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal rint_floor (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal rint_round (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal rint_trunc (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal frac (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal remainder ( const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); + friend const mpreal remquo (long* q, const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); // Miscellaneous Functions friend const mpreal nexttoward (const mpreal& x, const mpreal& y); @@ -524,19 +580,22 @@ public: // Efficient swapping of two mpreal values - needed for std algorithms friend void swap(mpreal& x, mpreal& y); - friend const mpreal fmax(const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal fmin(const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal fmax(const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); + friend const mpreal fmin(const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); private: // Human friendly Debug Preview in Visual Studio. // Put one of these lines: // - // mpfr::mpreal= ; Show value only + // mpfr::mpreal= ; Show value only // mpfr::mpreal=, bits ; Show value & precision // // at the beginning of // [Visual Studio Installation Folder]\Common7\Packages\Debugger\autoexp.dat MPREAL_MSVC_DEBUGVIEW_DATA + + // "Smart" resources deallocation. Checks if instance initialized before deletion. + void clear(::mpfr_ptr); }; ////////////////////////////////////////////////////////////////////////// @@ -551,64 +610,89 @@ public: // Default constructor: creates mp number and initializes it to 0. inline mpreal::mpreal() { - mpfr_init2(mp,mpreal::get_default_prec()); - mpfr_set_ui(mp,0,mpreal::get_default_rnd()); + mpfr_init2 (mpfr_ptr(), mpreal::get_default_prec()); + mpfr_set_ui(mpfr_ptr(), 0, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const mpreal& u) { - mpfr_init2(mp,mpfr_get_prec(u.mp)); - mpfr_set(mp,u.mp,mpreal::get_default_rnd()); + mpfr_init2(mpfr_ptr(),mpfr_get_prec(u.mpfr_srcptr())); + mpfr_set (mpfr_ptr(),u.mpfr_srcptr(),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; } -inline mpreal::mpreal(const mpfr_t u) +#ifdef MPREAL_HAVE_MOVE_SUPPORT +inline mpreal::mpreal(mpreal&& other) { - mpfr_init2(mp,mpfr_get_prec(u)); - mpfr_set(mp,u,mpreal::get_default_rnd()); + mpfr_set_uninitialized(mpfr_ptr()); // make sure "other" holds no pinter to actual data + mpfr_swap(mpfr_ptr(), other.mpfr_ptr()); + + MPREAL_MSVC_DEBUGVIEW_CODE; +} + +inline mpreal& mpreal::operator=(mpreal&& other) +{ + mpfr_swap(mpfr_ptr(), other.mpfr_ptr()); + + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; +} +#endif + +inline mpreal::mpreal(const mpfr_t u, bool shared) +{ + if(shared) + { + std::memcpy(mpfr_ptr(), u, sizeof(mpfr_t)); + } + else + { + mpfr_init2(mpfr_ptr(), mpfr_get_prec(u)); + mpfr_set (mpfr_ptr(), u, mpreal::get_default_rnd()); + } MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const mpf_t u) { - mpfr_init2(mp,(mp_prec_t) mpf_get_prec(u)); // (gmp: mp_bitcnt_t) unsigned long -> long (mpfr: mp_prec_t) - mpfr_set_f(mp,u,mpreal::get_default_rnd()); + mpfr_init2(mpfr_ptr(),(mp_prec_t) mpf_get_prec(u)); // (gmp: mp_bitcnt_t) unsigned long -> long (mpfr: mp_prec_t) + mpfr_set_f(mpfr_ptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const mpz_t u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_z(mp,u,mode); + mpfr_init2(mpfr_ptr(), prec); + mpfr_set_z(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const mpq_t u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_q(mp,u,mode); + mpfr_init2(mpfr_ptr(), prec); + mpfr_set_q(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const double u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp, prec); + mpfr_init2(mpfr_ptr(), prec); #if (MPREAL_DOUBLE_BITS_OVERFLOW > -1) - if(fits_in_bits(u, MPREAL_DOUBLE_BITS_OVERFLOW)) - { - mpfr_set_d(mp, u, mode); - }else - throw conversion_overflow(); + if(fits_in_bits(u, MPREAL_DOUBLE_BITS_OVERFLOW)) + { + mpfr_set_d(mpfr_ptr(), u, mode); + }else + throw conversion_overflow(); #else - mpfr_set_d(mp, u, mode); + mpfr_set_d(mpfr_ptr(), u, mode); #endif MPREAL_MSVC_DEBUGVIEW_CODE; @@ -616,40 +700,40 @@ inline mpreal::mpreal(const double u, mp_prec_t prec, mp_rnd_t mode) inline mpreal::mpreal(const long double u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_ld(mp,u,mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_ld(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const unsigned long int u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_ui(mp,u,mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_ui(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const unsigned int u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_ui(mp,u,mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_ui(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const long int u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_si(mp,u,mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_si(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const int u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_si(mp,u,mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_si(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } @@ -657,16 +741,16 @@ inline mpreal::mpreal(const int u, mp_prec_t prec, mp_rnd_t mode) #if defined (MPREAL_HAVE_INT64_SUPPORT) inline mpreal::mpreal(const uint64_t u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_uj(mp, u, mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_uj(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const int64_t u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_sj(mp, u, mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_sj(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } @@ -674,23 +758,31 @@ inline mpreal::mpreal(const int64_t u, mp_prec_t prec, mp_rnd_t mode) inline mpreal::mpreal(const char* s, mp_prec_t prec, int base, mp_rnd_t mode) { - mpfr_init2(mp, prec); - mpfr_set_str(mp, s, base, mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_str(mpfr_ptr(), s, base, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const std::string& s, mp_prec_t prec, int base, mp_rnd_t mode) { - mpfr_init2(mp, prec); - mpfr_set_str(mp, s.c_str(), base, mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_str(mpfr_ptr(), s.c_str(), base, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } +inline void mpreal::clear(::mpfr_ptr x) +{ +#ifdef MPREAL_HAVE_MOVE_SUPPORT + if(mpfr_is_initialized(x)) +#endif + mpfr_clear(x); +} + inline mpreal::~mpreal() { - mpfr_clear(mp); + clear(mpfr_ptr()); } // internal namespace needed for template magic @@ -761,6 +853,9 @@ const mpreal sqrt(const int v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); const mpreal sqrt(const long double v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); const mpreal sqrt(const double v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); +// abs +inline const mpreal abs(const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()); + ////////////////////////////////////////////////////////////////////////// // pow const mpreal pow(const mpreal& a, const unsigned int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); @@ -813,6 +908,11 @@ const mpreal pow(const double a, const unsigned int b, mp_rnd_t rnd_mode = mprea const mpreal pow(const double a, const long int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); const mpreal pow(const double a, const int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); +inline const mpreal mul_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); +inline const mpreal mul_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); +inline const mpreal div_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); +inline const mpreal div_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + ////////////////////////////////////////////////////////////////////////// // Estimate machine epsilon for the given precision // Returns smallest eps such that 1.0 + eps != 1.0 @@ -860,15 +960,15 @@ inline mpreal& mpreal::operator=(const mpreal& v) { if (this != &v) { - mp_prec_t tp = mpfr_get_prec(mp); - mp_prec_t vp = mpfr_get_prec(v.mp); + mp_prec_t tp = mpfr_get_prec( mpfr_srcptr()); + mp_prec_t vp = mpfr_get_prec(v.mpfr_srcptr()); - if(tp != vp){ - mpfr_clear(mp); - mpfr_init2(mp, vp); - } + if(tp != vp){ + clear(mpfr_ptr()); + mpfr_init2(mpfr_ptr(), vp); + } - mpfr_set(mp, v.mp, mpreal::get_default_rnd()); + mpfr_set(mpfr_ptr(), v.mpfr_srcptr(), mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; } @@ -877,7 +977,7 @@ inline mpreal& mpreal::operator=(const mpreal& v) inline mpreal& mpreal::operator=(const mpf_t v) { - mpfr_set_f(mp, v, mpreal::get_default_rnd()); + mpfr_set_f(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -885,7 +985,7 @@ inline mpreal& mpreal::operator=(const mpf_t v) inline mpreal& mpreal::operator=(const mpz_t v) { - mpfr_set_z(mp, v, mpreal::get_default_rnd()); + mpfr_set_z(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -893,7 +993,7 @@ inline mpreal& mpreal::operator=(const mpz_t v) inline mpreal& mpreal::operator=(const mpq_t v) { - mpfr_set_q(mp, v, mpreal::get_default_rnd()); + mpfr_set_q(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -901,7 +1001,7 @@ inline mpreal& mpreal::operator=(const mpq_t v) inline mpreal& mpreal::operator=(const long double v) { - mpfr_set_ld(mp, v, mpreal::get_default_rnd()); + mpfr_set_ld(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -910,22 +1010,22 @@ inline mpreal& mpreal::operator=(const long double v) inline mpreal& mpreal::operator=(const double v) { #if (MPREAL_DOUBLE_BITS_OVERFLOW > -1) - if(fits_in_bits(v, MPREAL_DOUBLE_BITS_OVERFLOW)) - { - mpfr_set_d(mp,v,mpreal::get_default_rnd()); - }else - throw conversion_overflow(); + if(fits_in_bits(v, MPREAL_DOUBLE_BITS_OVERFLOW)) + { + mpfr_set_d(mpfr_ptr(),v,mpreal::get_default_rnd()); + }else + throw conversion_overflow(); #else - mpfr_set_d(mp,v,mpreal::get_default_rnd()); + mpfr_set_d(mpfr_ptr(),v,mpreal::get_default_rnd()); #endif - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator=(const unsigned long int v) { - mpfr_set_ui(mp, v, mpreal::get_default_rnd()); + mpfr_set_ui(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -933,7 +1033,7 @@ inline mpreal& mpreal::operator=(const unsigned long int v) inline mpreal& mpreal::operator=(const unsigned int v) { - mpfr_set_ui(mp, v, mpreal::get_default_rnd()); + mpfr_set_ui(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -941,7 +1041,7 @@ inline mpreal& mpreal::operator=(const unsigned int v) inline mpreal& mpreal::operator=(const long int v) { - mpfr_set_si(mp, v, mpreal::get_default_rnd()); + mpfr_set_si(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -949,7 +1049,7 @@ inline mpreal& mpreal::operator=(const long int v) inline mpreal& mpreal::operator=(const int v) { - mpfr_set_si(mp, v, mpreal::get_default_rnd()); + mpfr_set_si(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -966,15 +1066,15 @@ inline mpreal& mpreal::operator=(const char* s) mpfr_t t; - mpfr_init2(t, mpfr_get_prec(mp)); + mpfr_init2(t, mpfr_get_prec(mpfr_srcptr())); if(0 == mpfr_set_str(t, s, 10, mpreal::get_default_rnd())) { - mpfr_set(mp, t, mpreal::get_default_rnd()); + mpfr_set(mpfr_ptr(), t, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; } - mpfr_clear(t); + clear(t); return *this; } @@ -989,15 +1089,15 @@ inline mpreal& mpreal::operator=(const std::string& s) mpfr_t t; - mpfr_init2(t, mpfr_get_prec(mp)); + mpfr_init2(t, mpfr_get_prec(mpfr_srcptr())); if(0 == mpfr_set_str(t, s.c_str(), 10, mpreal::get_default_rnd())) { - mpfr_set(mp, t, mpreal::get_default_rnd()); + mpfr_set(mpfr_ptr(), t, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; } - mpfr_clear(t); + clear(t); return *this; } @@ -1006,7 +1106,7 @@ inline mpreal& mpreal::operator=(const std::string& s) // + Addition inline mpreal& mpreal::operator+=(const mpreal& v) { - mpfr_add(mp,mp,v.mp,mpreal::get_default_rnd()); + mpfr_add(mpfr_ptr(), mpfr_srcptr(), v.mpfr_srcptr(), mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1020,14 +1120,14 @@ inline mpreal& mpreal::operator+=(const mpf_t u) inline mpreal& mpreal::operator+=(const mpz_t u) { - mpfr_add_z(mp,mp,u,mpreal::get_default_rnd()); + mpfr_add_z(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator+=(const mpq_t u) { - mpfr_add_q(mp,mp,u,mpreal::get_default_rnd()); + mpfr_add_q(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1042,7 +1142,7 @@ inline mpreal& mpreal::operator+= (const long double u) inline mpreal& mpreal::operator+= (const double u) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpfr_add_d(mp,mp,u,mpreal::get_default_rnd()); + mpfr_add_d(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); #else *this += mpreal(u); #endif @@ -1053,28 +1153,28 @@ inline mpreal& mpreal::operator+= (const double u) inline mpreal& mpreal::operator+=(const unsigned long int u) { - mpfr_add_ui(mp,mp,u,mpreal::get_default_rnd()); + mpfr_add_ui(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator+=(const unsigned int u) { - mpfr_add_ui(mp,mp,u,mpreal::get_default_rnd()); + mpfr_add_ui(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator+=(const long int u) { - mpfr_add_si(mp,mp,u,mpreal::get_default_rnd()); + mpfr_add_si(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator+=(const int u) { - mpfr_add_si(mp,mp,u,mpreal::get_default_rnd()); + mpfr_add_si(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1094,9 +1194,9 @@ inline const mpreal mpreal::operator+()const { return mpreal(*this); } inline const mpreal operator+(const mpreal& a, const mpreal& b) { - mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_ptr()), mpfr_get_prec(b.mpfr_ptr()))); - mpfr_add(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); - return c; + mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_ptr()), mpfr_get_prec(b.mpfr_ptr()))); + mpfr_add(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); + return c; } inline mpreal& mpreal::operator++() @@ -1127,21 +1227,21 @@ inline const mpreal mpreal::operator-- (int) // - Subtraction inline mpreal& mpreal::operator-=(const mpreal& v) { - mpfr_sub(mp,mp,v.mp,mpreal::get_default_rnd()); + mpfr_sub(mpfr_ptr(),mpfr_srcptr(),v.mpfr_srcptr(),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator-=(const mpz_t v) { - mpfr_sub_z(mp,mp,v,mpreal::get_default_rnd()); + mpfr_sub_z(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator-=(const mpq_t v) { - mpfr_sub_q(mp,mp,v,mpreal::get_default_rnd()); + mpfr_sub_q(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1156,7 +1256,7 @@ inline mpreal& mpreal::operator-=(const long double v) inline mpreal& mpreal::operator-=(const double v) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpfr_sub_d(mp,mp,v,mpreal::get_default_rnd()); + mpfr_sub_d(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); #else *this -= mpreal(v); #endif @@ -1167,28 +1267,28 @@ inline mpreal& mpreal::operator-=(const double v) inline mpreal& mpreal::operator-=(const unsigned long int v) { - mpfr_sub_ui(mp,mp,v,mpreal::get_default_rnd()); + mpfr_sub_ui(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator-=(const unsigned int v) { - mpfr_sub_ui(mp,mp,v,mpreal::get_default_rnd()); + mpfr_sub_ui(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator-=(const long int v) { - mpfr_sub_si(mp,mp,v,mpreal::get_default_rnd()); + mpfr_sub_si(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator-=(const int v) { - mpfr_sub_si(mp,mp,v,mpreal::get_default_rnd()); + mpfr_sub_si(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1196,15 +1296,15 @@ inline mpreal& mpreal::operator-=(const int v) inline const mpreal mpreal::operator-()const { mpreal u(*this); - mpfr_neg(u.mp,u.mp,mpreal::get_default_rnd()); + mpfr_neg(u.mpfr_ptr(),u.mpfr_srcptr(),mpreal::get_default_rnd()); return u; } inline const mpreal operator-(const mpreal& a, const mpreal& b) { - mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_ptr()), mpfr_get_prec(b.mpfr_ptr()))); - mpfr_sub(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); - return c; + mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_ptr()), mpfr_get_prec(b.mpfr_ptr()))); + mpfr_sub(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); + return c; } inline const mpreal operator-(const double b, const mpreal& a) @@ -1252,21 +1352,21 @@ inline const mpreal operator-(const int b, const mpreal& a) // * Multiplication inline mpreal& mpreal::operator*= (const mpreal& v) { - mpfr_mul(mp,mp,v.mp,mpreal::get_default_rnd()); + mpfr_mul(mpfr_ptr(),mpfr_srcptr(),v.mpfr_srcptr(),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator*=(const mpz_t v) { - mpfr_mul_z(mp,mp,v,mpreal::get_default_rnd()); + mpfr_mul_z(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator*=(const mpq_t v) { - mpfr_mul_q(mp,mp,v,mpreal::get_default_rnd()); + mpfr_mul_q(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1281,7 +1381,7 @@ inline mpreal& mpreal::operator*=(const long double v) inline mpreal& mpreal::operator*=(const double v) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpfr_mul_d(mp,mp,v,mpreal::get_default_rnd()); + mpfr_mul_d(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); #else *this *= mpreal(v); #endif @@ -1291,58 +1391,58 @@ inline mpreal& mpreal::operator*=(const double v) inline mpreal& mpreal::operator*=(const unsigned long int v) { - mpfr_mul_ui(mp,mp,v,mpreal::get_default_rnd()); + mpfr_mul_ui(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator*=(const unsigned int v) { - mpfr_mul_ui(mp,mp,v,mpreal::get_default_rnd()); + mpfr_mul_ui(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator*=(const long int v) { - mpfr_mul_si(mp,mp,v,mpreal::get_default_rnd()); + mpfr_mul_si(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator*=(const int v) { - mpfr_mul_si(mp,mp,v,mpreal::get_default_rnd()); + mpfr_mul_si(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline const mpreal operator*(const mpreal& a, const mpreal& b) { - mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_ptr()), mpfr_get_prec(b.mpfr_ptr()))); - mpfr_mul(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); - return c; + mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_ptr()), mpfr_get_prec(b.mpfr_ptr()))); + mpfr_mul(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); + return c; } ////////////////////////////////////////////////////////////////////////// // / Division inline mpreal& mpreal::operator/=(const mpreal& v) { - mpfr_div(mp,mp,v.mp,mpreal::get_default_rnd()); + mpfr_div(mpfr_ptr(),mpfr_srcptr(),v.mpfr_srcptr(),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator/=(const mpz_t v) { - mpfr_div_z(mp,mp,v,mpreal::get_default_rnd()); + mpfr_div_z(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator/=(const mpq_t v) { - mpfr_div_q(mp,mp,v,mpreal::get_default_rnd()); + mpfr_div_q(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1357,7 +1457,7 @@ inline mpreal& mpreal::operator/=(const long double v) inline mpreal& mpreal::operator/=(const double v) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpfr_div_d(mp,mp,v,mpreal::get_default_rnd()); + mpfr_div_d(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); #else *this /= mpreal(v); #endif @@ -1367,72 +1467,72 @@ inline mpreal& mpreal::operator/=(const double v) inline mpreal& mpreal::operator/=(const unsigned long int v) { - mpfr_div_ui(mp,mp,v,mpreal::get_default_rnd()); + mpfr_div_ui(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator/=(const unsigned int v) { - mpfr_div_ui(mp,mp,v,mpreal::get_default_rnd()); + mpfr_div_ui(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator/=(const long int v) { - mpfr_div_si(mp,mp,v,mpreal::get_default_rnd()); + mpfr_div_si(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator/=(const int v) { - mpfr_div_si(mp,mp,v,mpreal::get_default_rnd()); + mpfr_div_si(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline const mpreal operator/(const mpreal& a, const mpreal& b) { - mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_ptr()), mpfr_get_prec(b.mpfr_ptr()))); - mpfr_div(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); - return c; + mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_srcptr()), mpfr_get_prec(b.mpfr_srcptr()))); + mpfr_div(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); + return c; } inline const mpreal operator/(const unsigned long int b, const mpreal& a) { - mpreal x(0, mpfr_get_prec(a.mpfr_ptr())); + mpreal x(0, mpfr_get_prec(a.mpfr_srcptr())); mpfr_ui_div(x.mpfr_ptr(), b, a.mpfr_srcptr(), mpreal::get_default_rnd()); return x; } inline const mpreal operator/(const unsigned int b, const mpreal& a) { - mpreal x(0, mpfr_get_prec(a.mpfr_ptr())); + mpreal x(0, mpfr_get_prec(a.mpfr_srcptr())); mpfr_ui_div(x.mpfr_ptr(), b, a.mpfr_srcptr(), mpreal::get_default_rnd()); return x; } inline const mpreal operator/(const long int b, const mpreal& a) { - mpreal x(0, mpfr_get_prec(a.mpfr_ptr())); - mpfr_si_div(x.mpfr_ptr(), b, a.mpfr_srcptr(),mpreal::get_default_rnd()); + mpreal x(0, mpfr_get_prec(a.mpfr_srcptr())); + mpfr_si_div(x.mpfr_ptr(), b, a.mpfr_srcptr(), mpreal::get_default_rnd()); return x; } inline const mpreal operator/(const int b, const mpreal& a) { - mpreal x(0, mpfr_get_prec(a.mpfr_ptr())); - mpfr_si_div(x.mpfr_ptr(), b, a.mpfr_srcptr(),mpreal::get_default_rnd()); + mpreal x(0, mpfr_get_prec(a.mpfr_srcptr())); + mpfr_si_div(x.mpfr_ptr(), b, a.mpfr_srcptr(), mpreal::get_default_rnd()); return x; } inline const mpreal operator/(const double b, const mpreal& a) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpreal x(0, mpfr_get_prec(a.mpfr_ptr())); - mpfr_d_div(x.mpfr_ptr(), b, a.mpfr_srcptr(),mpreal::get_default_rnd()); + mpreal x(0, mpfr_get_prec(a.mpfr_srcptr())); + mpfr_d_div(x.mpfr_ptr(), b, a.mpfr_srcptr(), mpreal::get_default_rnd()); return x; #else mpreal x(0, mpfr_get_prec(a.mpfr_ptr())); @@ -1445,56 +1545,56 @@ inline const mpreal operator/(const double b, const mpreal& a) // Shifts operators - Multiplication/Division by power of 2 inline mpreal& mpreal::operator<<=(const unsigned long int u) { - mpfr_mul_2ui(mp,mp,u,mpreal::get_default_rnd()); + mpfr_mul_2ui(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator<<=(const unsigned int u) { - mpfr_mul_2ui(mp,mp,static_cast(u),mpreal::get_default_rnd()); + mpfr_mul_2ui(mpfr_ptr(),mpfr_srcptr(),static_cast(u),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator<<=(const long int u) { - mpfr_mul_2si(mp,mp,u,mpreal::get_default_rnd()); + mpfr_mul_2si(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator<<=(const int u) { - mpfr_mul_2si(mp,mp,static_cast(u),mpreal::get_default_rnd()); + mpfr_mul_2si(mpfr_ptr(),mpfr_srcptr(),static_cast(u),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator>>=(const unsigned long int u) { - mpfr_div_2ui(mp,mp,u,mpreal::get_default_rnd()); + mpfr_div_2ui(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator>>=(const unsigned int u) { - mpfr_div_2ui(mp,mp,static_cast(u),mpreal::get_default_rnd()); + mpfr_div_2ui(mpfr_ptr(),mpfr_srcptr(),static_cast(u),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator>>=(const long int u) { - mpfr_div_2si(mp,mp,u,mpreal::get_default_rnd()); + mpfr_div_2si(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator>>=(const int u) { - mpfr_div_2si(mp,mp,static_cast(u),mpreal::get_default_rnd()); + mpfr_div_2si(mpfr_ptr(),mpfr_srcptr(),static_cast(u),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1543,7 +1643,7 @@ inline const mpreal operator>>(const mpreal& v, const int k) inline const mpreal mul_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode) { mpreal x(v); - mpfr_mul_2ui(x.mp,v.mp,k,rnd_mode); + mpfr_mul_2ui(x.mpfr_ptr(),v.mpfr_srcptr(),k,rnd_mode); return x; } @@ -1551,61 +1651,63 @@ inline const mpreal mul_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_m inline const mpreal mul_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode) { mpreal x(v); - mpfr_mul_2si(x.mp,v.mp,k,rnd_mode); + mpfr_mul_2si(x.mpfr_ptr(),v.mpfr_srcptr(),k,rnd_mode); return x; } inline const mpreal div_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode) { mpreal x(v); - mpfr_div_2ui(x.mp,v.mp,k,rnd_mode); + mpfr_div_2ui(x.mpfr_ptr(),v.mpfr_srcptr(),k,rnd_mode); return x; } inline const mpreal div_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode) { mpreal x(v); - mpfr_div_2si(x.mp,v.mp,k,rnd_mode); + mpfr_div_2si(x.mpfr_ptr(),v.mpfr_srcptr(),k,rnd_mode); return x; } ////////////////////////////////////////////////////////////////////////// //Boolean operators -inline bool operator > (const mpreal& a, const mpreal& b){ return (mpfr_greater_p(a.mp,b.mp) !=0); } -inline bool operator >= (const mpreal& a, const mpreal& b){ return (mpfr_greaterequal_p(a.mp,b.mp) !=0); } -inline bool operator < (const mpreal& a, const mpreal& b){ return (mpfr_less_p(a.mp,b.mp) !=0); } -inline bool operator <= (const mpreal& a, const mpreal& b){ return (mpfr_lessequal_p(a.mp,b.mp) !=0); } -inline bool operator == (const mpreal& a, const mpreal& b){ return (mpfr_equal_p(a.mp,b.mp) !=0); } -inline bool operator != (const mpreal& a, const mpreal& b){ return (mpfr_lessgreater_p(a.mp,b.mp) !=0); } +inline bool operator > (const mpreal& a, const mpreal& b){ return (mpfr_greater_p (a.mpfr_srcptr(),b.mpfr_srcptr()) !=0 ); } +inline bool operator >= (const mpreal& a, const mpreal& b){ return (mpfr_greaterequal_p (a.mpfr_srcptr(),b.mpfr_srcptr()) !=0 ); } +inline bool operator < (const mpreal& a, const mpreal& b){ return (mpfr_less_p (a.mpfr_srcptr(),b.mpfr_srcptr()) !=0 ); } +inline bool operator <= (const mpreal& a, const mpreal& b){ return (mpfr_lessequal_p (a.mpfr_srcptr(),b.mpfr_srcptr()) !=0 ); } +inline bool operator == (const mpreal& a, const mpreal& b){ return (mpfr_equal_p (a.mpfr_srcptr(),b.mpfr_srcptr()) !=0 ); } +inline bool operator != (const mpreal& a, const mpreal& b){ return (mpfr_lessgreater_p (a.mpfr_srcptr(),b.mpfr_srcptr()) !=0 ); } -inline bool operator == (const mpreal& a, const unsigned long int b ){ return (mpfr_cmp_ui(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const unsigned int b ){ return (mpfr_cmp_ui(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const long int b ){ return (mpfr_cmp_si(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const int b ){ return (mpfr_cmp_si(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const long double b ){ return (mpfr_cmp_ld(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const double b ){ return (mpfr_cmp_d(a.mp,b) == 0); } +inline bool operator == (const mpreal& a, const unsigned long int b ){ return (mpfr_cmp_ui(a.mpfr_srcptr(),b) == 0 ); } +inline bool operator == (const mpreal& a, const unsigned int b ){ return (mpfr_cmp_ui(a.mpfr_srcptr(),b) == 0 ); } +inline bool operator == (const mpreal& a, const long int b ){ return (mpfr_cmp_si(a.mpfr_srcptr(),b) == 0 ); } +inline bool operator == (const mpreal& a, const int b ){ return (mpfr_cmp_si(a.mpfr_srcptr(),b) == 0 ); } +inline bool operator == (const mpreal& a, const long double b ){ return (mpfr_cmp_ld(a.mpfr_srcptr(),b) == 0 ); } +inline bool operator == (const mpreal& a, const double b ){ return (mpfr_cmp_d (a.mpfr_srcptr(),b) == 0 ); } -inline bool isnan (const mpreal& v){ return (mpfr_nan_p(v.mp) != 0); } -inline bool isinf (const mpreal& v){ return (mpfr_inf_p(v.mp) != 0); } -inline bool isfinite (const mpreal& v){ return (mpfr_number_p(v.mp) != 0); } -inline bool iszero (const mpreal& v){ return (mpfr_zero_p(v.mp) != 0); } -inline bool isint (const mpreal& v){ return (mpfr_integer_p(v.mp) != 0); } +inline bool isnan (const mpreal& op){ return (mpfr_nan_p (op.mpfr_srcptr()) != 0 ); } +inline bool isinf (const mpreal& op){ return (mpfr_inf_p (op.mpfr_srcptr()) != 0 ); } +inline bool isfinite (const mpreal& op){ return (mpfr_number_p (op.mpfr_srcptr()) != 0 ); } +inline bool iszero (const mpreal& op){ return (mpfr_zero_p (op.mpfr_srcptr()) != 0 ); } +inline bool isint (const mpreal& op){ return (mpfr_integer_p(op.mpfr_srcptr()) != 0 ); } #if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) -inline bool isregular(const mpreal& v){ return (mpfr_regular_p(v.mp));} +inline bool isregular(const mpreal& op){ return (mpfr_regular_p(op.mpfr_srcptr()));} #endif ////////////////////////////////////////////////////////////////////////// // Type Converters -inline long mpreal::toLong (mp_rnd_t mode) const { return mpfr_get_si(mp, mode); } -inline unsigned long mpreal::toULong (mp_rnd_t mode) const { return mpfr_get_ui(mp, mode); } -inline double mpreal::toDouble (mp_rnd_t mode) const { return mpfr_get_d (mp, mode); } -inline long double mpreal::toLDouble(mp_rnd_t mode) const { return mpfr_get_ld(mp, mode); } +inline bool mpreal::toBool (mp_rnd_t /*mode*/) const { return mpfr_zero_p (mpfr_srcptr()) == 0; } +inline long mpreal::toLong (mp_rnd_t mode) const { return mpfr_get_si (mpfr_srcptr(), mode); } +inline unsigned long mpreal::toULong (mp_rnd_t mode) const { return mpfr_get_ui (mpfr_srcptr(), mode); } +inline float mpreal::toFloat (mp_rnd_t mode) const { return mpfr_get_flt(mpfr_srcptr(), mode); } +inline double mpreal::toDouble (mp_rnd_t mode) const { return mpfr_get_d (mpfr_srcptr(), mode); } +inline long double mpreal::toLDouble(mp_rnd_t mode) const { return mpfr_get_ld (mpfr_srcptr(), mode); } #if defined (MPREAL_HAVE_INT64_SUPPORT) -inline int64_t mpreal::toInt64 (mp_rnd_t mode) const{ return mpfr_get_sj(mp, mode); } -inline uint64_t mpreal::toUInt64(mp_rnd_t mode) const{ return mpfr_get_uj(mp, mode); } +inline int64_t mpreal::toInt64 (mp_rnd_t mode) const{ return mpfr_get_sj(mpfr_srcptr(), mode); } +inline uint64_t mpreal::toUInt64(mp_rnd_t mode) const{ return mpfr_get_uj(mpfr_srcptr(), mode); } #endif inline ::mpfr_ptr mpreal::mpfr_ptr() { return mp; } @@ -1629,7 +1731,7 @@ inline std::string mpreal::toString(const std::string& format) const if( !format.empty() ) { - if(!(mpfr_asprintf(&s,format.c_str(),mp) < 0)) + if(!(mpfr_asprintf(&s, format.c_str(), mpfr_srcptr()) < 0)) { out = std::string(s); @@ -1644,20 +1746,19 @@ inline std::string mpreal::toString(const std::string& format) const inline std::string mpreal::toString(int n, int b, mp_rnd_t mode) const { + // TODO: Add extended format specification (f, e, rounding mode) as it done in output operator (void)b; (void)mode; #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - // Use MPFR native function for output - char format[128]; - int digits; + std::ostringstream format; - digits = n > 0 ? n : bits2digits(mpfr_get_prec(mp)); + int digits = (n >= 0) ? n : bits2digits(mpfr_get_prec(mpfr_srcptr())); + + format << "%." << digits << "RNg"; - sprintf(format,"%%.%dRNg",digits); // Default format - - return toString(std::string(format)); + return toString(format.str()); #else @@ -1675,8 +1776,8 @@ inline std::string mpreal::toString(int n, int b, mp_rnd_t mode) const if(mpfr_zero_p(mp)) return "0"; if(mpfr_nan_p(mp)) return "NaN"; - s = mpfr_get_str(NULL,&exp,b,0,mp,mode); - ns = mpfr_get_str(NULL,&exp,b,n,mp,mode); + s = mpfr_get_str(NULL, &exp, b, 0, mp, mode); + ns = mpfr_get_str(NULL, &exp, b, (std::max)(0,n), mp, mode); if(s!=NULL && ns!=NULL) { @@ -1761,17 +1862,43 @@ inline std::string mpreal::toString(int n, int b, mp_rnd_t mode) const ////////////////////////////////////////////////////////////////////////// // I/O +inline std::ostream& mpreal::output(std::ostream& os) const +{ + std::ostringstream format; + const std::ios::fmtflags flags = os.flags(); + + format << ((flags & std::ios::showpos) ? "%+" : "%"); + if (os.precision() >= 0) + format << '.' << os.precision() << "R*" + << ((flags & std::ios::floatfield) == std::ios::fixed ? 'f' : + (flags & std::ios::floatfield) == std::ios::scientific ? 'e' : + 'g'); + else + format << "R*e"; + + char *s = NULL; + if(!(mpfr_asprintf(&s, format.str().c_str(), + mpfr::mpreal::get_default_rnd(), + mpfr_srcptr()) + < 0)) + { + os << std::string(s); + mpfr_free_str(s); + } + return os; +} + inline std::ostream& operator<<(std::ostream& os, const mpreal& v) { - return os<(os.precision())); + return v.output(os); } inline std::istream& operator>>(std::istream &is, mpreal& v) { - // ToDo, use cout::hexfloat and other flags to setup base + // TODO: use cout::hexfloat and other flags to setup base std::string tmp; is >> tmp; - mpfr_set_str(v.mp, tmp.c_str(), 10, mpreal::get_default_rnd()); + mpfr_set_str(v.mpfr_ptr(), tmp.c_str(), 10, mpreal::get_default_rnd()); return is; } @@ -1784,53 +1911,53 @@ inline mp_prec_t digits2bits(int d) { const double LOG2_10 = 3.3219280948873624; - return (mp_prec_t) std::ceil( d * LOG2_10 ); + return mp_prec_t(std::ceil( d * LOG2_10 )); } inline int bits2digits(mp_prec_t b) { const double LOG10_2 = 0.30102999566398119; - return (int) std::floor( b * LOG10_2 ); + return int(std::floor( b * LOG10_2 )); } ////////////////////////////////////////////////////////////////////////// // Set/Get number properties -inline int sgn(const mpreal& v) +inline int sgn(const mpreal& op) { - int r = mpfr_signbit(v.mp); - return (r>0?-1:1); + int r = mpfr_signbit(op.mpfr_srcptr()); + return (r > 0? -1 : 1); } inline mpreal& mpreal::setSign(int sign, mp_rnd_t RoundingMode) { - mpfr_setsign(mp,mp,(sign < 0 ? 1 : 0),RoundingMode); + mpfr_setsign(mpfr_ptr(), mpfr_srcptr(), (sign < 0 ? 1 : 0), RoundingMode); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline int mpreal::getPrecision() const { - return mpfr_get_prec(mp); + return int(mpfr_get_prec(mpfr_srcptr())); } inline mpreal& mpreal::setPrecision(int Precision, mp_rnd_t RoundingMode) { - mpfr_prec_round(mp, Precision, RoundingMode); + mpfr_prec_round(mpfr_ptr(), Precision, RoundingMode); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::setInf(int sign) { - mpfr_set_inf(mp,sign); + mpfr_set_inf(mpfr_ptr(), sign); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::setNan() { - mpfr_set_nan(mp); + mpfr_set_nan(mpfr_ptr()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1839,9 +1966,9 @@ inline mpreal& mpreal::setZero(int sign) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) - mpfr_set_zero(mp, sign); + mpfr_set_zero(mpfr_ptr(), sign); #else - mpfr_set_si(mp, 0, (mpfr_get_default_rounding_mode)()); + mpfr_set_si(mpfr_ptr(), 0, (mpfr_get_default_rounding_mode)()); setSign(sign); #endif @@ -1851,23 +1978,23 @@ inline mpreal& mpreal::setZero(int sign) inline mp_prec_t mpreal::get_prec() const { - return mpfr_get_prec(mp); + return mpfr_get_prec(mpfr_srcptr()); } inline void mpreal::set_prec(mp_prec_t prec, mp_rnd_t rnd_mode) { - mpfr_prec_round(mp,prec,rnd_mode); + mpfr_prec_round(mpfr_ptr(),prec,rnd_mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mp_exp_t mpreal::get_exp () { - return mpfr_get_exp(mp); + return mpfr_get_exp(mpfr_srcptr()); } inline int mpreal::set_exp (mp_exp_t e) { - int x = mpfr_set_exp(mp, e); + int x = mpfr_set_exp(mpfr_ptr(), e); MPREAL_MSVC_DEBUGVIEW_CODE; return x; } @@ -1885,7 +2012,7 @@ inline const mpreal ldexp(const mpreal& v, mp_exp_t exp) mpreal x(v); // rounding is not important since we just increasing the exponent - mpfr_mul_2si(x.mp,x.mp,exp,mpreal::get_default_rnd()); + mpfr_mul_2si(x.mpfr_ptr(), x.mpfr_srcptr(), exp, mpreal::get_default_rnd()); return x; } @@ -1900,9 +2027,9 @@ inline mpreal machine_epsilon(const mpreal& x) /* the smallest eps such that x + eps != x */ if( x < 0) { - return nextabove(-x)+x; + return nextabove(-x) + x; }else{ - return nextabove(x)-x; + return nextabove( x) - x; } } @@ -1922,37 +2049,37 @@ inline mpreal maxval(mp_prec_t prec) inline bool isEqualUlps(const mpreal& a, const mpreal& b, int maxUlps) { - return abs(a - b) <= machine_epsilon((max)(abs(a), abs(b))) * maxUlps; + return abs(a - b) <= machine_epsilon((max)(abs(a), abs(b))) * maxUlps; } inline bool isEqualFuzzy(const mpreal& a, const mpreal& b, const mpreal& eps) { - return abs(a - b) <= (min)(abs(a), abs(b)) * eps; + return abs(a - b) <= eps; } inline bool isEqualFuzzy(const mpreal& a, const mpreal& b) { - return isEqualFuzzy(a, b, machine_epsilon((min)(abs(a), abs(b)))); + return isEqualFuzzy(a, b, machine_epsilon((max)(1, (min)(abs(a), abs(b))))); } inline const mpreal modf(const mpreal& v, mpreal& n) { - mpreal frac(v); + mpreal f(v); // rounding is not important since we are using the same number - mpfr_frac(frac.mp,frac.mp,mpreal::get_default_rnd()); - mpfr_trunc(n.mp,v.mp); - return frac; + mpfr_frac (f.mpfr_ptr(),f.mpfr_srcptr(),mpreal::get_default_rnd()); + mpfr_trunc(n.mpfr_ptr(),v.mpfr_srcptr()); + return f; } inline int mpreal::check_range (int t, mp_rnd_t rnd_mode) { - return mpfr_check_range(mp,t,rnd_mode); + return mpfr_check_range(mpfr_ptr(),t,rnd_mode); } inline int mpreal::subnormalize (int t,mp_rnd_t rnd_mode) { - int r = mpfr_subnormalize(mp,t,rnd_mode); + int r = mpfr_subnormalize(mpfr_ptr(),t,rnd_mode); MPREAL_MSVC_DEBUGVIEW_CODE; return r; } @@ -2005,8 +2132,11 @@ inline mp_exp_t mpreal::get_emax_max (void) mpfr_##f(y.mpfr_ptr(), x.mpfr_srcptr(), r); \ return y; -inline const mpreal sqr (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(sqr ); } -inline const mpreal sqrt (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(sqrt); } +inline const mpreal sqr (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) +{ MPREAL_UNARY_MATH_FUNCTION_BODY(sqr ); } + +inline const mpreal sqrt (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) +{ MPREAL_UNARY_MATH_FUNCTION_BODY(sqrt); } inline const mpreal sqrt(const unsigned long int x, mp_rnd_t r) { @@ -2032,14 +2162,14 @@ inline const mpreal sqrt(const int v, mp_rnd_t rnd_mode) else return mpreal().setNan(); // NaN } -inline const mpreal root(const mpreal& x, unsigned long int k, mp_rnd_t r) +inline const mpreal root(const mpreal& x, unsigned long int k, mp_rnd_t r = mpreal::get_default_rnd()) { mpreal y(0, mpfr_get_prec(x.mpfr_srcptr())); mpfr_root(y.mpfr_ptr(), x.mpfr_srcptr(), k, r); return y; } -inline const mpreal dim(const mpreal& a, const mpreal& b, mp_rnd_t r) +inline const mpreal dim(const mpreal& a, const mpreal& b, mp_rnd_t r = mpreal::get_default_rnd()) { mpreal y(0, mpfr_get_prec(a.mpfr_srcptr())); mpfr_dim(y.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), r); @@ -2048,161 +2178,130 @@ inline const mpreal dim(const mpreal& a, const mpreal& b, mp_rnd_t r) inline int cmpabs(const mpreal& a,const mpreal& b) { - return mpfr_cmpabs(a.mp,b.mp); + return mpfr_cmpabs(a.mpfr_ptr(), b.mpfr_srcptr()); } -inline int sin_cos(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode) +inline int sin_cos(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { - return mpfr_sin_cos(s.mp,c.mp,v.mp,rnd_mode); + return mpfr_sin_cos(s.mpfr_ptr(), c.mpfr_ptr(), v.mpfr_srcptr(), rnd_mode); } inline const mpreal sqrt (const long double v, mp_rnd_t rnd_mode) { return sqrt(mpreal(v),rnd_mode); } inline const mpreal sqrt (const double v, mp_rnd_t rnd_mode) { return sqrt(mpreal(v),rnd_mode); } -inline const mpreal cbrt (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(cbrt ); } -inline const mpreal fabs (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(abs ); } -inline const mpreal abs (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(abs ); } -inline const mpreal log (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(log ); } -inline const mpreal log2 (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(log2 ); } -inline const mpreal log10 (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(log10); } -inline const mpreal exp (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(exp ); } -inline const mpreal exp2 (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(exp2 ); } -inline const mpreal exp10 (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(exp10); } -inline const mpreal cos (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(cos ); } -inline const mpreal sin (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(sin ); } -inline const mpreal tan (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(tan ); } -inline const mpreal sec (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(sec ); } -inline const mpreal csc (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(csc ); } -inline const mpreal cot (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(cot ); } -inline const mpreal acos (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(acos); } -inline const mpreal asin (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(asin); } -inline const mpreal atan (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(atan); } +inline const mpreal cbrt (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(cbrt ); } +inline const mpreal fabs (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(abs ); } +inline const mpreal abs (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(abs ); } +inline const mpreal log (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(log ); } +inline const mpreal log2 (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(log2 ); } +inline const mpreal log10 (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(log10); } +inline const mpreal exp (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(exp ); } +inline const mpreal exp2 (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(exp2 ); } +inline const mpreal exp10 (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(exp10); } +inline const mpreal cos (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(cos ); } +inline const mpreal sin (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(sin ); } +inline const mpreal tan (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(tan ); } +inline const mpreal sec (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(sec ); } +inline const mpreal csc (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(csc ); } +inline const mpreal cot (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(cot ); } +inline const mpreal acos (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(acos ); } +inline const mpreal asin (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(asin ); } +inline const mpreal atan (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(atan ); } -inline const mpreal acot (const mpreal& v, mp_rnd_t r) { return atan (1/v, r); } -inline const mpreal asec (const mpreal& v, mp_rnd_t r) { return acos (1/v, r); } -inline const mpreal acsc (const mpreal& v, mp_rnd_t r) { return asin (1/v, r); } -inline const mpreal acoth (const mpreal& v, mp_rnd_t r) { return atanh(1/v, r); } -inline const mpreal asech (const mpreal& v, mp_rnd_t r) { return acosh(1/v, r); } -inline const mpreal acsch (const mpreal& v, mp_rnd_t r) { return asinh(1/v, r); } +inline const mpreal acot (const mpreal& v, mp_rnd_t r = mpreal::get_default_rnd()) { return atan (1/v, r); } +inline const mpreal asec (const mpreal& v, mp_rnd_t r = mpreal::get_default_rnd()) { return acos (1/v, r); } +inline const mpreal acsc (const mpreal& v, mp_rnd_t r = mpreal::get_default_rnd()) { return asin (1/v, r); } +inline const mpreal acoth (const mpreal& v, mp_rnd_t r = mpreal::get_default_rnd()) { return atanh(1/v, r); } +inline const mpreal asech (const mpreal& v, mp_rnd_t r = mpreal::get_default_rnd()) { return acosh(1/v, r); } +inline const mpreal acsch (const mpreal& v, mp_rnd_t r = mpreal::get_default_rnd()) { return asinh(1/v, r); } -inline const mpreal cosh (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(cosh ); } -inline const mpreal sinh (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(sinh ); } -inline const mpreal tanh (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(tanh ); } -inline const mpreal sech (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(sech ); } -inline const mpreal csch (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(csch ); } -inline const mpreal coth (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(coth ); } -inline const mpreal acosh (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(acosh); } -inline const mpreal asinh (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(asinh); } -inline const mpreal atanh (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(atanh); } +inline const mpreal cosh (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(cosh ); } +inline const mpreal sinh (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(sinh ); } +inline const mpreal tanh (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(tanh ); } +inline const mpreal sech (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(sech ); } +inline const mpreal csch (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(csch ); } +inline const mpreal coth (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(coth ); } +inline const mpreal acosh (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(acosh); } +inline const mpreal asinh (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(asinh); } +inline const mpreal atanh (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(atanh); } -inline const mpreal log1p (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(log1p ); } -inline const mpreal expm1 (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(expm1 ); } -inline const mpreal eint (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(eint ); } -inline const mpreal gamma (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(gamma ); } -inline const mpreal lngamma (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(lngamma); } -inline const mpreal zeta (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(zeta ); } -inline const mpreal erf (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(erf ); } -inline const mpreal erfc (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(erfc ); } -inline const mpreal besselj0(const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(j0 ); } -inline const mpreal besselj1(const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(j1 ); } -inline const mpreal bessely0(const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(y0 ); } -inline const mpreal bessely1(const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(y1 ); } +inline const mpreal log1p (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(log1p ); } +inline const mpreal expm1 (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(expm1 ); } +inline const mpreal eint (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(eint ); } +inline const mpreal gamma (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(gamma ); } +inline const mpreal lngamma (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(lngamma); } +inline const mpreal zeta (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(zeta ); } +inline const mpreal erf (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(erf ); } +inline const mpreal erfc (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(erfc ); } +inline const mpreal besselj0(const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(j0 ); } +inline const mpreal besselj1(const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(j1 ); } +inline const mpreal bessely0(const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(y0 ); } +inline const mpreal bessely1(const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(y1 ); } -inline const mpreal atan2 (const mpreal& y, const mpreal& x, mp_rnd_t rnd_mode) +inline const mpreal atan2 (const mpreal& y, const mpreal& x, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { - mpreal a; - mp_prec_t yp, xp; - - yp = y.get_prec(); - xp = x.get_prec(); - - a.set_prec(yp>xp?yp:xp); - - mpfr_atan2(a.mp, y.mp, x.mp, rnd_mode); - + mpreal a(0,(std::max)(y.getPrecision(), x.getPrecision())); + mpfr_atan2(a.mpfr_ptr(), y.mpfr_srcptr(), x.mpfr_srcptr(), rnd_mode); return a; } -inline const mpreal hypot (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) +inline const mpreal hypot (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { - mpreal a; - mp_prec_t yp, xp; - - yp = y.get_prec(); - xp = x.get_prec(); - - a.set_prec(yp>xp?yp:xp); - - mpfr_hypot(a.mp, x.mp, y.mp, rnd_mode); - + mpreal a(0,(std::max)(y.getPrecision(), x.getPrecision())); + mpfr_hypot(a.mpfr_ptr(), x.mpfr_srcptr(), y.mpfr_srcptr(), rnd_mode); return a; } -inline const mpreal remainder (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) +inline const mpreal remainder (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { - mpreal a; - mp_prec_t yp, xp; - - yp = y.get_prec(); - xp = x.get_prec(); - - a.set_prec(yp>xp?yp:xp); - - mpfr_remainder(a.mp, x.mp, y.mp, rnd_mode); - + mpreal a(0,(std::max)(y.getPrecision(), x.getPrecision())); + mpfr_remainder(a.mpfr_ptr(), x.mpfr_srcptr(), y.mpfr_srcptr(), rnd_mode); return a; } -inline const mpreal remquo (long* q, const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) +inline const mpreal remquo (long* q, const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { - mpreal a; - mp_prec_t yp, xp; - - yp = y.get_prec(); - xp = x.get_prec(); - - a.set_prec(yp>xp?yp:xp); - - mpfr_remquo(a.mp,q, x.mp, y.mp, rnd_mode); - + mpreal a(0,(std::max)(y.getPrecision(), x.getPrecision())); + mpfr_remquo(a.mpfr_ptr(),q, x.mpfr_srcptr(), y.mpfr_srcptr(), rnd_mode); return a; } -inline const mpreal fac_ui (unsigned long int v, mp_prec_t prec, mp_rnd_t rnd_mode) +inline const mpreal fac_ui (unsigned long int v, mp_prec_t prec = mpreal::get_default_prec(), + mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(0, prec); - mpfr_fac_ui(x.mp,v,rnd_mode); + mpfr_fac_ui(x.mpfr_ptr(),v,rnd_mode); return x; } -inline const mpreal lgamma (const mpreal& v, int *signp, mp_rnd_t rnd_mode) +inline const mpreal lgamma (const mpreal& v, int *signp = 0, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(v); int tsignp; - if(signp) mpfr_lgamma(x.mp,signp,v.mp,rnd_mode); - else mpfr_lgamma(x.mp,&tsignp,v.mp,rnd_mode); + if(signp) mpfr_lgamma(x.mpfr_ptr(), signp,v.mpfr_srcptr(),rnd_mode); + else mpfr_lgamma(x.mpfr_ptr(),&tsignp,v.mpfr_srcptr(),rnd_mode); return x; } -inline const mpreal besseljn (long n, const mpreal& x, mp_rnd_t r) +inline const mpreal besseljn (long n, const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { - mpreal y(0, mpfr_get_prec(x.mpfr_srcptr())); + mpreal y(0, x.getPrecision()); mpfr_jn(y.mpfr_ptr(), n, x.mpfr_srcptr(), r); return y; } -inline const mpreal besselyn (long n, const mpreal& x, mp_rnd_t r) +inline const mpreal besselyn (long n, const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { - mpreal y(0, mpfr_get_prec(x.mpfr_srcptr())); + mpreal y(0, x.getPrecision()); mpfr_yn(y.mpfr_ptr(), n, x.mpfr_srcptr(), r); return y; } -inline const mpreal fma (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode) +inline const mpreal fma (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal a; mp_prec_t p1, p2, p3; @@ -2217,7 +2316,7 @@ inline const mpreal fma (const mpreal& v1, const mpreal& v2, const mpreal& v3, m return a; } -inline const mpreal fms (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode) +inline const mpreal fms (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal a; mp_prec_t p1, p2, p3; @@ -2232,7 +2331,7 @@ inline const mpreal fms (const mpreal& v1, const mpreal& v2, const mpreal& v3, m return a; } -inline const mpreal agm (const mpreal& v1, const mpreal& v2, mp_rnd_t rnd_mode) +inline const mpreal agm (const mpreal& v1, const mpreal& v2, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal a; mp_prec_t p1, p2; @@ -2247,7 +2346,7 @@ inline const mpreal agm (const mpreal& v1, const mpreal& v2, mp_rnd_t rnd_mode) return a; } -inline const mpreal sum (const mpreal tab[], unsigned long int n, mp_rnd_t rnd_mode) +inline const mpreal sum (const mpreal tab[], unsigned long int n, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x; mpfr_ptr* t; @@ -2264,23 +2363,23 @@ inline const mpreal sum (const mpreal tab[], unsigned long int n, mp_rnd_t rnd_m // MPFR 2.4.0 Specifics #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) -inline int sinh_cosh(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode) +inline int sinh_cosh(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { return mpfr_sinh_cosh(s.mp,c.mp,v.mp,rnd_mode); } -inline const mpreal li2 (const mpreal& x, mp_rnd_t r) +inline const mpreal li2 (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(li2); } -inline const mpreal rem (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) +inline const mpreal rem (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { /* R = rem(X,Y) if Y != 0, returns X - n * Y where n = trunc(X/Y). */ return fmod(x, y, rnd_mode); } -inline const mpreal mod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) +inline const mpreal mod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { (void)rnd_mode; @@ -2305,7 +2404,7 @@ inline const mpreal mod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) return m; } -inline const mpreal fmod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) +inline const mpreal fmod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal a; mp_prec_t yp, xp; @@ -2320,7 +2419,7 @@ inline const mpreal fmod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) return a; } -inline const mpreal rec_sqrt(const mpreal& v, mp_rnd_t rnd_mode) +inline const mpreal rec_sqrt(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(v); mpfr_rec_sqrt(x.mp,v.mp,rnd_mode); @@ -2331,41 +2430,41 @@ inline const mpreal rec_sqrt(const mpreal& v, mp_rnd_t rnd_mode) ////////////////////////////////////////////////////////////////////////// // MPFR 3.0.0 Specifics #if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) -inline const mpreal digamma (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(digamma); } -inline const mpreal ai (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(ai); } +inline const mpreal digamma (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(digamma); } +inline const mpreal ai (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(ai); } #endif // MPFR 3.0.0 Specifics ////////////////////////////////////////////////////////////////////////// // Constants -inline const mpreal const_log2 (mp_prec_t p, mp_rnd_t r) +inline const mpreal const_log2 (mp_prec_t p = mpreal::get_default_prec(), mp_rnd_t r = mpreal::get_default_rnd()) { mpreal x(0, p); mpfr_const_log2(x.mpfr_ptr(), r); return x; } -inline const mpreal const_pi (mp_prec_t p, mp_rnd_t r) +inline const mpreal const_pi (mp_prec_t p = mpreal::get_default_prec(), mp_rnd_t r = mpreal::get_default_rnd()) { mpreal x(0, p); mpfr_const_pi(x.mpfr_ptr(), r); return x; } -inline const mpreal const_euler (mp_prec_t p, mp_rnd_t r) +inline const mpreal const_euler (mp_prec_t p = mpreal::get_default_prec(), mp_rnd_t r = mpreal::get_default_rnd()) { mpreal x(0, p); mpfr_const_euler(x.mpfr_ptr(), r); return x; } -inline const mpreal const_catalan (mp_prec_t p, mp_rnd_t r) +inline const mpreal const_catalan (mp_prec_t p = mpreal::get_default_prec(), mp_rnd_t r = mpreal::get_default_rnd()) { mpreal x(0, p); mpfr_const_catalan(x.mpfr_ptr(), r); return x; } -inline const mpreal const_infinity (int sign, mp_prec_t p, mp_rnd_t /*r*/) +inline const mpreal const_infinity (int sign = 1, mp_prec_t p = mpreal::get_default_prec()) { mpreal x(0, p); mpfr_set_inf(x.mpfr_ptr(), sign); @@ -2402,12 +2501,12 @@ inline const mpreal trunc(const mpreal& v) return x; } -inline const mpreal rint (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint ); } -inline const mpreal rint_ceil (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_ceil ); } -inline const mpreal rint_floor (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_floor); } -inline const mpreal rint_round (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_round); } -inline const mpreal rint_trunc (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_trunc); } -inline const mpreal frac (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(frac ); } +inline const mpreal rint (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint ); } +inline const mpreal rint_ceil (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_ceil ); } +inline const mpreal rint_floor (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_floor); } +inline const mpreal rint_round (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_round); } +inline const mpreal rint_trunc (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_trunc); } +inline const mpreal frac (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(frac ); } ////////////////////////////////////////////////////////////////////////// // Miscellaneous Functions @@ -2415,14 +2514,14 @@ inline void swap (mpreal& a, mpreal& b) { mpfr_swap(a.mp,b inline const mpreal (max)(const mpreal& x, const mpreal& y){ return (x>y?x:y); } inline const mpreal (min)(const mpreal& x, const mpreal& y){ return (x= MPFR_VERSION_NUM(3,0,0)) +#if (MPFR_VERSION >= MPFR_VERSION_NUM(3,1,0)) // use gmp_randinit_default() to init state, gmp_randclear() to clear -inline const mpreal urandom (gmp_randstate_t& state, mp_rnd_t rnd_mode) +inline const mpreal urandom (gmp_randstate_t& state, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x; mpfr_urandom(x.mp,state,rnd_mode); return x; } + +inline const mpreal grandom (gmp_randstate_t& state, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) +{ + mpreal x; + mpfr_grandom(x.mp, NULL, state, rnd_mode); + return x; +} + #endif #if (MPFR_VERSION <= MPFR_VERSION_NUM(2,4,2)) @@ -2480,7 +2587,7 @@ inline const mpreal random2 (mp_size_t size, mp_exp_t exp) // a = random(seed); <- initialization & first random number generation // a = random(); <- next random numbers generation // seed != 0 -inline const mpreal random(unsigned int seed) +inline const mpreal random(unsigned int seed = 0) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) @@ -2504,6 +2611,25 @@ inline const mpreal random(unsigned int seed) } +#if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) +inline const mpreal grandom(unsigned int seed = 0) +{ + static gmp_randstate_t state; + static bool isFirstTime = true; + + if(isFirstTime) + { + gmp_randinit_default(state); + gmp_randseed_ui(state,0); + isFirstTime = false; + } + + if(seed != 0) gmp_randseed_ui(state,seed); + + return mpfr::grandom(state); +} +#endif + ////////////////////////////////////////////////////////////////////////// // Set/Get global properties inline void mpreal::set_default_prec(mp_prec_t prec) @@ -2523,21 +2649,21 @@ inline bool mpreal::fits_in_bits(double x, int n) return IsInf(x) || (std::modf ( std::ldexp ( std::frexp ( x, &i ), n ), &t ) == 0.0); } -inline const mpreal pow(const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode) +inline const mpreal pow(const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(a); mpfr_pow(x.mp,x.mp,b.mp,rnd_mode); return x; } -inline const mpreal pow(const mpreal& a, const mpz_t b, mp_rnd_t rnd_mode) +inline const mpreal pow(const mpreal& a, const mpz_t b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(a); mpfr_pow_z(x.mp,x.mp,b,rnd_mode); return x; } -inline const mpreal pow(const mpreal& a, const unsigned long int b, mp_rnd_t rnd_mode) +inline const mpreal pow(const mpreal& a, const unsigned long int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(a); mpfr_pow_ui(x.mp,x.mp,b,rnd_mode); @@ -2549,7 +2675,7 @@ inline const mpreal pow(const mpreal& a, const unsigned int b, mp_rnd_t rnd_mode return pow(a,static_cast(b),rnd_mode); } -inline const mpreal pow(const mpreal& a, const long int b, mp_rnd_t rnd_mode) +inline const mpreal pow(const mpreal& a, const long int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(a); mpfr_pow_si(x.mp,x.mp,b,rnd_mode); @@ -2571,7 +2697,7 @@ inline const mpreal pow(const mpreal& a, const double b, mp_rnd_t rnd_mode) return pow(a,mpreal(b),rnd_mode); } -inline const mpreal pow(const unsigned long int a, const mpreal& b, mp_rnd_t rnd_mode) +inline const mpreal pow(const unsigned long int a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(a); mpfr_ui_pow(x.mp,a,b.mp,rnd_mode); @@ -2586,13 +2712,13 @@ inline const mpreal pow(const unsigned int a, const mpreal& b, mp_rnd_t rnd_mode inline const mpreal pow(const long int a, const mpreal& b, mp_rnd_t rnd_mode) { if (a>=0) return pow(static_cast(a),b,rnd_mode); - else return pow(mpreal(a),b,rnd_mode); + else return pow(mpreal(a),b,rnd_mode); } inline const mpreal pow(const int a, const mpreal& b, mp_rnd_t rnd_mode) { if (a>=0) return pow(static_cast(a),b,rnd_mode); - else return pow(mpreal(a),b,rnd_mode); + else return pow(mpreal(a),b,rnd_mode); } inline const mpreal pow(const long double a, const mpreal& b, mp_rnd_t rnd_mode) @@ -2621,13 +2747,13 @@ inline const mpreal pow(const unsigned long int a, const unsigned int b, mp_rnd_ inline const mpreal pow(const unsigned long int a, const long int b, mp_rnd_t rnd_mode) { if(b>0) return pow(a,static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow + else return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow } inline const mpreal pow(const unsigned long int a, const int b, mp_rnd_t rnd_mode) { if(b>0) return pow(a,static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow + else return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow } inline const mpreal pow(const unsigned long int a, const long double b, mp_rnd_t rnd_mode) @@ -2824,7 +2950,7 @@ inline const mpreal pow(const double a, const int b, mp_rnd_t rnd_mode) // Non-throwing swap C++ idiom: http://en.wikibooks.org/wiki/More_C%2B%2B_Idioms/Non-throwing_swap namespace std { - // only allowed to extend namespace std with specializations + // we are allowed to extend namespace std with specializations only template <> inline void swap(mpfr::mpreal& x, mpfr::mpreal& y) { @@ -2852,20 +2978,6 @@ namespace std static const bool tinyness_before = true; static const float_denorm_style has_denorm = denorm_absent; - - inline static float_round_style round_style() - { - mp_rnd_t r = mpfr::mpreal::get_default_rnd(); - - switch (r) - { - case MPFR_RNDN: return round_to_nearest; - case MPFR_RNDZ: return round_toward_zero; - case MPFR_RNDU: return round_toward_infinity; - case MPFR_RNDD: return round_toward_neg_infinity; - default: return round_indeterminate; - } - } inline static mpfr::mpreal (min) (mp_prec_t precision = mpfr::mpreal::get_default_prec()) { return mpfr::minval(precision); } inline static mpfr::mpreal (max) (mp_prec_t precision = mpfr::mpreal::get_default_prec()) { return mpfr::maxval(precision); } @@ -2873,7 +2985,7 @@ namespace std // Returns smallest eps such that 1 + eps != 1 (classic machine epsilon) inline static mpfr::mpreal epsilon(mp_prec_t precision = mpfr::mpreal::get_default_prec()) { return mpfr::machine_epsilon(precision); } - + // Returns smallest eps such that x + eps != x (relative machine epsilon) inline static mpfr::mpreal epsilon(const mpfr::mpreal& x) { return mpfr::machine_epsilon(x); } @@ -2881,7 +2993,7 @@ namespace std { mp_rnd_t r = mpfr::mpreal::get_default_rnd(); - if(r == MPFR_RNDN) return mpfr::mpreal(0.5, precision); + if(r == GMP_RNDN) return mpfr::mpreal(0.5, precision); else return mpfr::mpreal(1.0, precision); } @@ -2896,10 +3008,30 @@ namespace std MPREAL_PERMISSIVE_EXPR static const int min_exponent10 = (int) (MPFR_EMIN_DEFAULT * 0.3010299956639811); MPREAL_PERMISSIVE_EXPR static const int max_exponent10 = (int) (MPFR_EMAX_DEFAULT * 0.3010299956639811); - // Should be constant according to standard, but 'digits' depends on precision in MPFR +#ifdef MPREAL_HAVE_DYNAMIC_STD_NUMERIC_LIMITS - inline static int digits() { return mpfr::mpreal::get_default_prec(); } - inline static int digits(const mpfr::mpreal& x) { return x.getPrecision(); } + // Following members should be constant according to standard, but they can be variable in MPFR + // So we define them as functions here. + // + // This is preferable way for std::numeric_limits specialization. + // But it is incompatible with standard std::numeric_limits and might not work with other libraries, e.g. boost. + // See below for compatible implementation. + inline static float_round_style round_style() + { + mp_rnd_t r = mpfr::mpreal::get_default_rnd(); + + switch (r) + { + case GMP_RNDN: return round_to_nearest; + case GMP_RNDZ: return round_toward_zero; + case GMP_RNDU: return round_toward_infinity; + case GMP_RNDD: return round_toward_neg_infinity; + default: return round_indeterminate; + } + } + + inline static int digits() { return int(mpfr::mpreal::get_default_prec()); } + inline static int digits(const mpfr::mpreal& x) { return x.getPrecision(); } inline static int digits10(mp_prec_t precision = mpfr::mpreal::get_default_prec()) { @@ -2915,6 +3047,25 @@ namespace std { return digits10(precision); } +#else + // Digits and round_style are NOT constants when it comes to mpreal. + // If possible, please use functions digits() and round_style() defined above. + // + // These (default) values are preserved for compatibility with existing libraries, e.g. boost. + // Change them accordingly to your application. + // + // For example, if you use 256 bits of precision uniformly in your program, then: + // digits = 256 + // digits10 = 77 + // max_digits10 = 78 + // + // Approximate formula for decimal digits is: digits10 = floor(log10(2) * digits). See bits2digits() for more details. + + static const std::float_round_style round_style = round_to_nearest; + static const int digits = 53; + static const int digits10 = 15; + static const int max_digits10 = 16; +#endif }; } diff --git a/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp b/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp index 13f92169e..de79f1538 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp +++ b/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp @@ -104,9 +104,7 @@ void evalSolverSugarFunction( const POLYNOMIAL& pols, const ROOTS& roots, const // 1) the roots found are correct // 2) the roots have distinct moduli - typedef typename POLYNOMIAL::Scalar Scalar; typedef typename REAL_ROOTS::Scalar Real; - typedef PolynomialSolver PolynomialSolverType; //Test realRoots std::vector< Real > calc_realRoots; diff --git a/gtsam/3rdparty/ceres/CMakeLists.txt b/gtsam/3rdparty/ceres/CMakeLists.txt new file mode 100644 index 000000000..98b2cffce --- /dev/null +++ b/gtsam/3rdparty/ceres/CMakeLists.txt @@ -0,0 +1,2 @@ +file(GLOB ceres_headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h") +install(FILES ${ceres_headers} DESTINATION include/gtsam/3rdparty/ceres) \ No newline at end of file diff --git a/gtsam_unstable/nonlinear/ceres_autodiff.h b/gtsam/3rdparty/ceres/autodiff.h similarity index 98% rename from gtsam_unstable/nonlinear/ceres_autodiff.h rename to gtsam/3rdparty/ceres/autodiff.h index 2a0ac8987..f124425cc 100644 --- a/gtsam_unstable/nonlinear/ceres_autodiff.h +++ b/gtsam/3rdparty/ceres/autodiff.h @@ -142,10 +142,10 @@ #include -#include -#include -#include -#include +#include +#include +#include +#include #define DCHECK assert #define DCHECK_GT(a,b) assert((a)>(b)) diff --git a/gtsam_unstable/nonlinear/ceres_eigen.h b/gtsam/3rdparty/ceres/eigen.h similarity index 98% rename from gtsam_unstable/nonlinear/ceres_eigen.h rename to gtsam/3rdparty/ceres/eigen.h index 18a602cf4..a25fde97f 100644 --- a/gtsam_unstable/nonlinear/ceres_eigen.h +++ b/gtsam/3rdparty/ceres/eigen.h @@ -31,7 +31,7 @@ #ifndef CERES_INTERNAL_EIGEN_H_ #define CERES_INTERNAL_EIGEN_H_ -#include +#include namespace ceres { diff --git a/gtsam_unstable/nonlinear/ceres_example.h b/gtsam/3rdparty/ceres/example.h similarity index 98% rename from gtsam_unstable/nonlinear/ceres_example.h rename to gtsam/3rdparty/ceres/example.h index 45ec3428e..6b051fce0 100644 --- a/gtsam_unstable/nonlinear/ceres_example.h +++ b/gtsam/3rdparty/ceres/example.h @@ -33,7 +33,7 @@ #pragma once -#include +#include // Templated pinhole camera model for used with Ceres. The camera is // parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for diff --git a/gtsam_unstable/nonlinear/ceres_fixed_array.h b/gtsam/3rdparty/ceres/fixed_array.h similarity index 97% rename from gtsam_unstable/nonlinear/ceres_fixed_array.h rename to gtsam/3rdparty/ceres/fixed_array.h index 69a426379..455fce383 100644 --- a/gtsam_unstable/nonlinear/ceres_fixed_array.h +++ b/gtsam/3rdparty/ceres/fixed_array.h @@ -33,9 +33,9 @@ #define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ #include -#include -#include -#include +#include +#include +#include namespace ceres { namespace internal { diff --git a/gtsam_unstable/nonlinear/ceres_fpclassify.h b/gtsam/3rdparty/ceres/fpclassify.h similarity index 100% rename from gtsam_unstable/nonlinear/ceres_fpclassify.h rename to gtsam/3rdparty/ceres/fpclassify.h diff --git a/gtsam_unstable/nonlinear/ceres_jet.h b/gtsam/3rdparty/ceres/jet.h similarity index 99% rename from gtsam_unstable/nonlinear/ceres_jet.h rename to gtsam/3rdparty/ceres/jet.h index ed4834caf..4a7683f50 100644 --- a/gtsam_unstable/nonlinear/ceres_jet.h +++ b/gtsam/3rdparty/ceres/jet.h @@ -162,8 +162,8 @@ #include #include -#include -#include +#include +#include namespace ceres { diff --git a/gtsam_unstable/nonlinear/ceres_macros.h b/gtsam/3rdparty/ceres/macros.h similarity index 100% rename from gtsam_unstable/nonlinear/ceres_macros.h rename to gtsam/3rdparty/ceres/macros.h diff --git a/gtsam_unstable/nonlinear/ceres_manual_constructor.h b/gtsam/3rdparty/ceres/manual_constructor.h similarity index 100% rename from gtsam_unstable/nonlinear/ceres_manual_constructor.h rename to gtsam/3rdparty/ceres/manual_constructor.h diff --git a/gtsam_unstable/nonlinear/ceres_rotation.h b/gtsam/3rdparty/ceres/rotation.h similarity index 99% rename from gtsam_unstable/nonlinear/ceres_rotation.h rename to gtsam/3rdparty/ceres/rotation.h index 83627291c..6ed3b88cb 100644 --- a/gtsam_unstable/nonlinear/ceres_rotation.h +++ b/gtsam/3rdparty/ceres/rotation.h @@ -47,6 +47,8 @@ #include #include +#include +#include #define DCHECK assert namespace ceres { diff --git a/gtsam_unstable/nonlinear/ceres_variadic_evaluate.h b/gtsam/3rdparty/ceres/variadic_evaluate.h similarity index 97% rename from gtsam_unstable/nonlinear/ceres_variadic_evaluate.h rename to gtsam/3rdparty/ceres/variadic_evaluate.h index 7d22fe22e..86aec4829 100644 --- a/gtsam_unstable/nonlinear/ceres_variadic_evaluate.h +++ b/gtsam/3rdparty/ceres/variadic_evaluate.h @@ -34,9 +34,9 @@ #include -#include -#include -#include +#include +#include +#include namespace ceres { namespace internal { diff --git a/gtsam/3rdparty/gtsam_eigen_includes.h.in b/gtsam/3rdparty/gtsam_eigen_includes.h.in deleted file mode 100644 index f53e37f07..000000000 --- a/gtsam/3rdparty/gtsam_eigen_includes.h.in +++ /dev/null @@ -1,33 +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 gtsam_eigen_includes.h - * @brief File to include the Eigen headers that we use - generated by CMake - * @author Richard Roberts - */ - -#pragma once - -#ifndef MKL_BLAS -#define MKL_BLAS MKL_DOMAIN_BLAS -#endif - -#cmakedefine EIGEN_USE_MKL_ALL // This is also defined in config.h -#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/Dense> -#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/QR> -#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/LU> -#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/SVD> -#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/Geometry> - - - - diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt b/gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt deleted file mode 100644 index 66cfcba4a..000000000 --- a/gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -# Add this directory for internal users. -include_directories(.) -# Find sources. -file(GLOB metis_sources *.c) -# Build libmetis. -add_library(metis ${METIS_LIBRARY_TYPE} ${GKlib_sources} ${metis_sources}) -if(UNIX) - target_link_libraries(metis m) -endif() - - -install(TARGETS metis - LIBRARY DESTINATION include/gtsam/3rdparty/metis/lib - RUNTIME DESTINATION include/gtsam/3rdparty/metis/lib - ARCHIVE DESTINATION include/gtsam/3rdparty/metis/lib) - diff --git a/gtsam/3rdparty/metis-5.1.0/BUILD-Windows.txt b/gtsam/3rdparty/metis/BUILD-Windows.txt similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/BUILD-Windows.txt rename to gtsam/3rdparty/metis/BUILD-Windows.txt diff --git a/gtsam/3rdparty/metis-5.1.0/BUILD.txt b/gtsam/3rdparty/metis/BUILD.txt similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/BUILD.txt rename to gtsam/3rdparty/metis/BUILD.txt diff --git a/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt b/gtsam/3rdparty/metis/CMakeLists.txt similarity index 85% rename from gtsam/3rdparty/metis-5.1.0/CMakeLists.txt rename to gtsam/3rdparty/metis/CMakeLists.txt index 93c546be8..6ba17787f 100644 --- a/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt +++ b/gtsam/3rdparty/metis/CMakeLists.txt @@ -2,14 +2,14 @@ cmake_minimum_required(VERSION 2.8) project(METIS) # Add flags for currect directory and below -if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") +if (${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") add_definitions(-Wno-unused-variable) if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 5.0 OR CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 5.0) add_definitions(-Wno-sometimes-uninitialized) endif() endif() -if(NOT ("${CMAKE_C_COMPILER_ID}" MATCHES "MSVC" OR "${CMAKE_CXX_COMPILER_ID}" MATCHES "MSVC")) +if(NOT (${CMAKE_C_COMPILER_ID} MATCHES "MSVC" OR ${CMAKE_CXX_COMPILER_ID} MATCHES "MSVC")) #add_definitions(-Wno-unknown-pragmas) endif() @@ -46,3 +46,6 @@ add_subdirectory("libmetis") if(GTSAM_BUILD_METIS_EXECUTABLES) add_subdirectory("programs") endif() + +set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) + diff --git a/gtsam/3rdparty/metis-5.1.0/Changelog b/gtsam/3rdparty/metis/Changelog similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/Changelog rename to gtsam/3rdparty/metis/Changelog diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/BUILD.txt b/gtsam/3rdparty/metis/GKlib/BUILD.txt similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/BUILD.txt rename to gtsam/3rdparty/metis/GKlib/BUILD.txt diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/CMakeLists.txt b/gtsam/3rdparty/metis/GKlib/CMakeLists.txt similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/CMakeLists.txt rename to gtsam/3rdparty/metis/GKlib/CMakeLists.txt diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/GKlib.h b/gtsam/3rdparty/metis/GKlib/GKlib.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/GKlib.h rename to gtsam/3rdparty/metis/GKlib/GKlib.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/GKlibSystem.cmake b/gtsam/3rdparty/metis/GKlib/GKlibSystem.cmake similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/GKlibSystem.cmake rename to gtsam/3rdparty/metis/GKlib/GKlibSystem.cmake diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/Makefile b/gtsam/3rdparty/metis/GKlib/Makefile similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/Makefile rename to gtsam/3rdparty/metis/GKlib/Makefile diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/b64.c b/gtsam/3rdparty/metis/GKlib/b64.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/b64.c rename to gtsam/3rdparty/metis/GKlib/b64.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/blas.c b/gtsam/3rdparty/metis/GKlib/blas.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/blas.c rename to gtsam/3rdparty/metis/GKlib/blas.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/conf/check_thread_storage.c b/gtsam/3rdparty/metis/GKlib/conf/check_thread_storage.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/conf/check_thread_storage.c rename to gtsam/3rdparty/metis/GKlib/conf/check_thread_storage.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/csr.c b/gtsam/3rdparty/metis/GKlib/csr.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/csr.c rename to gtsam/3rdparty/metis/GKlib/csr.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/error.c b/gtsam/3rdparty/metis/GKlib/error.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/error.c rename to gtsam/3rdparty/metis/GKlib/error.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/evaluate.c b/gtsam/3rdparty/metis/GKlib/evaluate.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/evaluate.c rename to gtsam/3rdparty/metis/GKlib/evaluate.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/fkvkselect.c b/gtsam/3rdparty/metis/GKlib/fkvkselect.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/fkvkselect.c rename to gtsam/3rdparty/metis/GKlib/fkvkselect.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/fs.c b/gtsam/3rdparty/metis/GKlib/fs.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/fs.c rename to gtsam/3rdparty/metis/GKlib/fs.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/getopt.c b/gtsam/3rdparty/metis/GKlib/getopt.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/getopt.c rename to gtsam/3rdparty/metis/GKlib/getopt.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_arch.h b/gtsam/3rdparty/metis/GKlib/gk_arch.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_arch.h rename to gtsam/3rdparty/metis/GKlib/gk_arch.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_defs.h b/gtsam/3rdparty/metis/GKlib/gk_defs.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_defs.h rename to gtsam/3rdparty/metis/GKlib/gk_defs.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_externs.h b/gtsam/3rdparty/metis/GKlib/gk_externs.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_externs.h rename to gtsam/3rdparty/metis/GKlib/gk_externs.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_getopt.h b/gtsam/3rdparty/metis/GKlib/gk_getopt.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_getopt.h rename to gtsam/3rdparty/metis/GKlib/gk_getopt.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_macros.h b/gtsam/3rdparty/metis/GKlib/gk_macros.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_macros.h rename to gtsam/3rdparty/metis/GKlib/gk_macros.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkblas.h b/gtsam/3rdparty/metis/GKlib/gk_mkblas.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkblas.h rename to gtsam/3rdparty/metis/GKlib/gk_mkblas.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkmemory.h b/gtsam/3rdparty/metis/GKlib/gk_mkmemory.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkmemory.h rename to gtsam/3rdparty/metis/GKlib/gk_mkmemory.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkpqueue.h b/gtsam/3rdparty/metis/GKlib/gk_mkpqueue.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkpqueue.h rename to gtsam/3rdparty/metis/GKlib/gk_mkpqueue.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkpqueue2.h b/gtsam/3rdparty/metis/GKlib/gk_mkpqueue2.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkpqueue2.h rename to gtsam/3rdparty/metis/GKlib/gk_mkpqueue2.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkrandom.h b/gtsam/3rdparty/metis/GKlib/gk_mkrandom.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkrandom.h rename to gtsam/3rdparty/metis/GKlib/gk_mkrandom.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_mksort.h b/gtsam/3rdparty/metis/GKlib/gk_mksort.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_mksort.h rename to gtsam/3rdparty/metis/GKlib/gk_mksort.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkutils.h b/gtsam/3rdparty/metis/GKlib/gk_mkutils.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkutils.h rename to gtsam/3rdparty/metis/GKlib/gk_mkutils.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_proto.h b/gtsam/3rdparty/metis/GKlib/gk_proto.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_proto.h rename to gtsam/3rdparty/metis/GKlib/gk_proto.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_struct.h b/gtsam/3rdparty/metis/GKlib/gk_struct.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_struct.h rename to gtsam/3rdparty/metis/GKlib/gk_struct.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_types.h b/gtsam/3rdparty/metis/GKlib/gk_types.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_types.h rename to gtsam/3rdparty/metis/GKlib/gk_types.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gkregex.c b/gtsam/3rdparty/metis/GKlib/gkregex.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gkregex.c rename to gtsam/3rdparty/metis/GKlib/gkregex.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gkregex.h b/gtsam/3rdparty/metis/GKlib/gkregex.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gkregex.h rename to gtsam/3rdparty/metis/GKlib/gkregex.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/graph.c b/gtsam/3rdparty/metis/GKlib/graph.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/graph.c rename to gtsam/3rdparty/metis/GKlib/graph.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/htable.c b/gtsam/3rdparty/metis/GKlib/htable.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/htable.c rename to gtsam/3rdparty/metis/GKlib/htable.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/io.c b/gtsam/3rdparty/metis/GKlib/io.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/io.c rename to gtsam/3rdparty/metis/GKlib/io.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/itemsets.c b/gtsam/3rdparty/metis/GKlib/itemsets.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/itemsets.c rename to gtsam/3rdparty/metis/GKlib/itemsets.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/mcore.c b/gtsam/3rdparty/metis/GKlib/mcore.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/mcore.c rename to gtsam/3rdparty/metis/GKlib/mcore.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/memory.c b/gtsam/3rdparty/metis/GKlib/memory.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/memory.c rename to gtsam/3rdparty/metis/GKlib/memory.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/ms_inttypes.h b/gtsam/3rdparty/metis/GKlib/ms_inttypes.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/ms_inttypes.h rename to gtsam/3rdparty/metis/GKlib/ms_inttypes.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/ms_stat.h b/gtsam/3rdparty/metis/GKlib/ms_stat.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/ms_stat.h rename to gtsam/3rdparty/metis/GKlib/ms_stat.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/ms_stdint.h b/gtsam/3rdparty/metis/GKlib/ms_stdint.h similarity index 77% rename from gtsam/3rdparty/metis-5.1.0/GKlib/ms_stdint.h rename to gtsam/3rdparty/metis/GKlib/ms_stdint.h index 7e200dc6f..39b8aed9d 100644 --- a/gtsam/3rdparty/metis-5.1.0/GKlib/ms_stdint.h +++ b/gtsam/3rdparty/metis/GKlib/ms_stdint.h @@ -1,7 +1,7 @@ // ISO C9x compliant stdint.h for Microsoft Visual Studio // Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124 // -// Copyright (c) 2006 Alexander Chemeris +// Copyright (c) 2006-2013 Alexander Chemeris // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -13,8 +13,9 @@ // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // -// 3. The name of the author may be used to endorse or promote products -// derived from this software without specific prior written permission. +// 3. Neither the name of the product nor the names of its contributors may +// be used to endorse or promote products derived from this software +// without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED // WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF @@ -40,30 +41,59 @@ #pragma once #endif +#if _MSC_VER >= 1600 // [ +#include +#else // ] _MSC_VER >= 1600 [ + #include -// For Visual Studio 6 in C++ mode wrap include with 'extern "C++" {}' +// For Visual Studio 6 in C++ mode and for many Visual Studio versions when +// compiling for ARM we should wrap include with 'extern "C++" {}' // or compiler give many errors like this: // error C2733: second C linkage of overloaded function 'wmemchr' not allowed -#if (_MSC_VER < 1300) && defined(__cplusplus) - extern "C++" { -#endif -# include -#if (_MSC_VER < 1300) && defined(__cplusplus) - } +#ifdef __cplusplus +extern "C" { #endif +# include +#ifdef __cplusplus +} +#endif + +// Define _W64 macros to mark types changing their size, like intptr_t. +#ifndef _W64 +# if !defined(__midl) && (defined(_X86_) || defined(_M_IX86)) && _MSC_VER >= 1300 +# define _W64 __w64 +# else +# define _W64 +# endif +#endif + // 7.18.1 Integer types // 7.18.1.1 Exact-width integer types -typedef __int8 int8_t; -typedef __int16 int16_t; -typedef __int32 int32_t; -typedef __int64 int64_t; + +// Visual Studio 6 and Embedded Visual C++ 4 doesn't +// realize that, e.g. char has the same size as __int8 +// so we give up on __intX for them. +#if (_MSC_VER < 1300) +typedef signed char int8_t; +typedef signed short int16_t; +typedef signed int int32_t; +typedef unsigned char uint8_t; +typedef unsigned short uint16_t; +typedef unsigned int uint32_t; +#else +typedef signed __int8 int8_t; +typedef signed __int16 int16_t; +typedef signed __int32 int32_t; typedef unsigned __int8 uint8_t; typedef unsigned __int16 uint16_t; typedef unsigned __int32 uint32_t; -typedef unsigned __int64 uint64_t; +#endif +typedef signed __int64 int64_t; +typedef unsigned __int64 uint64_t; + // 7.18.1.2 Minimum-width integer types typedef int8_t int_least8_t; @@ -87,11 +117,11 @@ typedef uint64_t uint_fast64_t; // 7.18.1.4 Integer types capable of holding object pointers #ifdef _WIN64 // [ - typedef __int64 intptr_t; - typedef unsigned __int64 uintptr_t; +typedef signed __int64 intptr_t; +typedef unsigned __int64 uintptr_t; #else // _WIN64 ][ - typedef int intptr_t; - typedef unsigned int uintptr_t; +typedef _W64 signed int intptr_t; +typedef _W64 unsigned int uintptr_t; #endif // _WIN64 ] // 7.18.1.5 Greatest-width integer types @@ -213,10 +243,17 @@ typedef uint64_t uintmax_t; #define UINT64_C(val) val##ui64 // 7.18.4.2 Macros for greatest-width integer constants -#define INTMAX_C INT64_C -#define UINTMAX_C UINT64_C +// These #ifndef's are needed to prevent collisions with . +// Check out Issue 9 for the details. +#ifndef INTMAX_C // [ +# define INTMAX_C INT64_C +#endif // INTMAX_C ] +#ifndef UINTMAX_C // [ +# define UINTMAX_C UINT64_C +#endif // UINTMAX_C ] #endif // __STDC_CONSTANT_MACROS ] +#endif // _MSC_VER >= 1600 ] -#endif // _MSC_STDINT_H_ ] +#endif // _MSC_STDINT_H_ ] \ No newline at end of file diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/omp.c b/gtsam/3rdparty/metis/GKlib/omp.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/omp.c rename to gtsam/3rdparty/metis/GKlib/omp.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/pdb.c b/gtsam/3rdparty/metis/GKlib/pdb.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/pdb.c rename to gtsam/3rdparty/metis/GKlib/pdb.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/pqueue.c b/gtsam/3rdparty/metis/GKlib/pqueue.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/pqueue.c rename to gtsam/3rdparty/metis/GKlib/pqueue.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/random.c b/gtsam/3rdparty/metis/GKlib/random.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/random.c rename to gtsam/3rdparty/metis/GKlib/random.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/rw.c b/gtsam/3rdparty/metis/GKlib/rw.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/rw.c rename to gtsam/3rdparty/metis/GKlib/rw.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/seq.c b/gtsam/3rdparty/metis/GKlib/seq.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/seq.c rename to gtsam/3rdparty/metis/GKlib/seq.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/sort.c b/gtsam/3rdparty/metis/GKlib/sort.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/sort.c rename to gtsam/3rdparty/metis/GKlib/sort.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/string.c b/gtsam/3rdparty/metis/GKlib/string.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/string.c rename to gtsam/3rdparty/metis/GKlib/string.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/test/CMakeLists.txt b/gtsam/3rdparty/metis/GKlib/test/CMakeLists.txt similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/test/CMakeLists.txt rename to gtsam/3rdparty/metis/GKlib/test/CMakeLists.txt diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/test/Makefile.in.old b/gtsam/3rdparty/metis/GKlib/test/Makefile.in.old similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/test/Makefile.in.old rename to gtsam/3rdparty/metis/GKlib/test/Makefile.in.old diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/test/Makefile.old b/gtsam/3rdparty/metis/GKlib/test/Makefile.old similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/test/Makefile.old rename to gtsam/3rdparty/metis/GKlib/test/Makefile.old diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/test/fis.c b/gtsam/3rdparty/metis/GKlib/test/fis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/test/fis.c rename to gtsam/3rdparty/metis/GKlib/test/fis.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/test/gkgraph.c b/gtsam/3rdparty/metis/GKlib/test/gkgraph.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/test/gkgraph.c rename to gtsam/3rdparty/metis/GKlib/test/gkgraph.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/test/gksort.c b/gtsam/3rdparty/metis/GKlib/test/gksort.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/test/gksort.c rename to gtsam/3rdparty/metis/GKlib/test/gksort.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/test/rw.c b/gtsam/3rdparty/metis/GKlib/test/rw.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/test/rw.c rename to gtsam/3rdparty/metis/GKlib/test/rw.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/test/strings.c b/gtsam/3rdparty/metis/GKlib/test/strings.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/test/strings.c rename to gtsam/3rdparty/metis/GKlib/test/strings.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/timers.c b/gtsam/3rdparty/metis/GKlib/timers.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/timers.c rename to gtsam/3rdparty/metis/GKlib/timers.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/tokenizer.c b/gtsam/3rdparty/metis/GKlib/tokenizer.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/tokenizer.c rename to gtsam/3rdparty/metis/GKlib/tokenizer.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/util.c b/gtsam/3rdparty/metis/GKlib/util.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/util.c rename to gtsam/3rdparty/metis/GKlib/util.c diff --git a/gtsam/3rdparty/metis-5.1.0/Install.txt b/gtsam/3rdparty/metis/Install.txt similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/Install.txt rename to gtsam/3rdparty/metis/Install.txt diff --git a/gtsam/3rdparty/metis-5.1.0/LICENSE.txt b/gtsam/3rdparty/metis/LICENSE.txt similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/LICENSE.txt rename to gtsam/3rdparty/metis/LICENSE.txt diff --git a/gtsam/3rdparty/metis-5.1.0/Makefile b/gtsam/3rdparty/metis/Makefile similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/Makefile rename to gtsam/3rdparty/metis/Makefile diff --git a/gtsam/3rdparty/metis-5.1.0/graphs/4elt.graph b/gtsam/3rdparty/metis/graphs/4elt.graph similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/graphs/4elt.graph rename to gtsam/3rdparty/metis/graphs/4elt.graph diff --git a/gtsam/3rdparty/metis-5.1.0/graphs/README b/gtsam/3rdparty/metis/graphs/README similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/graphs/README rename to gtsam/3rdparty/metis/graphs/README diff --git a/gtsam/3rdparty/metis-5.1.0/graphs/copter2.graph b/gtsam/3rdparty/metis/graphs/copter2.graph similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/graphs/copter2.graph rename to gtsam/3rdparty/metis/graphs/copter2.graph diff --git a/gtsam/3rdparty/metis-5.1.0/graphs/mdual.graph b/gtsam/3rdparty/metis/graphs/mdual.graph similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/graphs/mdual.graph rename to gtsam/3rdparty/metis/graphs/mdual.graph diff --git a/gtsam/3rdparty/metis-5.1.0/graphs/metis.mesh b/gtsam/3rdparty/metis/graphs/metis.mesh similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/graphs/metis.mesh rename to gtsam/3rdparty/metis/graphs/metis.mesh diff --git a/gtsam/3rdparty/metis-5.1.0/graphs/test.mgraph b/gtsam/3rdparty/metis/graphs/test.mgraph similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/graphs/test.mgraph rename to gtsam/3rdparty/metis/graphs/test.mgraph diff --git a/gtsam/3rdparty/metis-5.1.0/include/CMakeLists.txt b/gtsam/3rdparty/metis/include/CMakeLists.txt similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/include/CMakeLists.txt rename to gtsam/3rdparty/metis/include/CMakeLists.txt diff --git a/gtsam/3rdparty/metis-5.1.0/include/metis.h b/gtsam/3rdparty/metis/include/metis.h similarity index 98% rename from gtsam/3rdparty/metis-5.1.0/include/metis.h rename to gtsam/3rdparty/metis/include/metis.h index dc5406ae5..2866a946b 100644 --- a/gtsam/3rdparty/metis-5.1.0/include/metis.h +++ b/gtsam/3rdparty/metis/include/metis.h @@ -65,17 +65,29 @@ #ifndef _GKLIB_H_ #ifdef COMPILER_MSC #include - typedef __int32 int32_t; typedef __int64 int64_t; #define PRId32 "I32d" #define PRId64 "I64d" #define SCNd32 "ld" #define SCNd64 "I64d" + +#ifndef INT32_MIN #define INT32_MIN ((int32_t)_I32_MIN) +#endif + +#ifndef INT32_MAX #define INT32_MAX _I32_MAX +#endif + +#ifndef INT64_MIN #define INT64_MIN ((int64_t)_I64_MIN) +#endif + +#ifndef INT64_MAX #define INT64_MAX _I64_MAX +#endif + #else #include #endif diff --git a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt new file mode 100644 index 000000000..cc02a6ed2 --- /dev/null +++ b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt @@ -0,0 +1,20 @@ +# Add this directory for internal users. +include_directories(.) +# Find sources. +file(GLOB metis_sources *.c) +# Build libmetis. +add_definitions(-fPIC) +add_library(metis ${METIS_LIBRARY_TYPE} ${GKlib_sources} ${metis_sources}) +if(UNIX) + target_link_libraries(metis m) +endif() + +if(WIN32) + set_target_properties(metis PROPERTIES + PREFIX "" + RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin") +endif() + +install(TARGETS metis EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) +list(APPEND GTSAM_EXPORTED_TARGETS metis) +set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/auxapi.c b/gtsam/3rdparty/metis/libmetis/auxapi.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/auxapi.c rename to gtsam/3rdparty/metis/libmetis/auxapi.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/balance.c b/gtsam/3rdparty/metis/libmetis/balance.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/balance.c rename to gtsam/3rdparty/metis/libmetis/balance.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/bucketsort.c b/gtsam/3rdparty/metis/libmetis/bucketsort.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/bucketsort.c rename to gtsam/3rdparty/metis/libmetis/bucketsort.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/checkgraph.c b/gtsam/3rdparty/metis/libmetis/checkgraph.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/checkgraph.c rename to gtsam/3rdparty/metis/libmetis/checkgraph.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/coarsen.c b/gtsam/3rdparty/metis/libmetis/coarsen.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/coarsen.c rename to gtsam/3rdparty/metis/libmetis/coarsen.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/compress.c b/gtsam/3rdparty/metis/libmetis/compress.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/compress.c rename to gtsam/3rdparty/metis/libmetis/compress.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/contig.c b/gtsam/3rdparty/metis/libmetis/contig.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/contig.c rename to gtsam/3rdparty/metis/libmetis/contig.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/debug.c b/gtsam/3rdparty/metis/libmetis/debug.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/debug.c rename to gtsam/3rdparty/metis/libmetis/debug.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/defs.h b/gtsam/3rdparty/metis/libmetis/defs.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/defs.h rename to gtsam/3rdparty/metis/libmetis/defs.h diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/fm.c b/gtsam/3rdparty/metis/libmetis/fm.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/fm.c rename to gtsam/3rdparty/metis/libmetis/fm.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/fortran.c b/gtsam/3rdparty/metis/libmetis/fortran.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/fortran.c rename to gtsam/3rdparty/metis/libmetis/fortran.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/frename.c b/gtsam/3rdparty/metis/libmetis/frename.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/frename.c rename to gtsam/3rdparty/metis/libmetis/frename.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/gklib.c b/gtsam/3rdparty/metis/libmetis/gklib.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/gklib.c rename to gtsam/3rdparty/metis/libmetis/gklib.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/gklib_defs.h b/gtsam/3rdparty/metis/libmetis/gklib_defs.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/gklib_defs.h rename to gtsam/3rdparty/metis/libmetis/gklib_defs.h diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/gklib_rename.h b/gtsam/3rdparty/metis/libmetis/gklib_rename.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/gklib_rename.h rename to gtsam/3rdparty/metis/libmetis/gklib_rename.h diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/graph.c b/gtsam/3rdparty/metis/libmetis/graph.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/graph.c rename to gtsam/3rdparty/metis/libmetis/graph.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/initpart.c b/gtsam/3rdparty/metis/libmetis/initpart.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/initpart.c rename to gtsam/3rdparty/metis/libmetis/initpart.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/kmetis.c b/gtsam/3rdparty/metis/libmetis/kmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/kmetis.c rename to gtsam/3rdparty/metis/libmetis/kmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/kwayfm.c b/gtsam/3rdparty/metis/libmetis/kwayfm.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/kwayfm.c rename to gtsam/3rdparty/metis/libmetis/kwayfm.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/kwayrefine.c b/gtsam/3rdparty/metis/libmetis/kwayrefine.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/kwayrefine.c rename to gtsam/3rdparty/metis/libmetis/kwayrefine.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/macros.h b/gtsam/3rdparty/metis/libmetis/macros.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/macros.h rename to gtsam/3rdparty/metis/libmetis/macros.h diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/mcutil.c b/gtsam/3rdparty/metis/libmetis/mcutil.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/mcutil.c rename to gtsam/3rdparty/metis/libmetis/mcutil.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/mesh.c b/gtsam/3rdparty/metis/libmetis/mesh.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/mesh.c rename to gtsam/3rdparty/metis/libmetis/mesh.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/meshpart.c b/gtsam/3rdparty/metis/libmetis/meshpart.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/meshpart.c rename to gtsam/3rdparty/metis/libmetis/meshpart.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/metislib.h b/gtsam/3rdparty/metis/libmetis/metislib.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/metislib.h rename to gtsam/3rdparty/metis/libmetis/metislib.h diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/minconn.c b/gtsam/3rdparty/metis/libmetis/minconn.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/minconn.c rename to gtsam/3rdparty/metis/libmetis/minconn.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/mincover.c b/gtsam/3rdparty/metis/libmetis/mincover.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/mincover.c rename to gtsam/3rdparty/metis/libmetis/mincover.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/mmd.c b/gtsam/3rdparty/metis/libmetis/mmd.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/mmd.c rename to gtsam/3rdparty/metis/libmetis/mmd.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/ometis.c b/gtsam/3rdparty/metis/libmetis/ometis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/ometis.c rename to gtsam/3rdparty/metis/libmetis/ometis.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/options.c b/gtsam/3rdparty/metis/libmetis/options.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/options.c rename to gtsam/3rdparty/metis/libmetis/options.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/parmetis.c b/gtsam/3rdparty/metis/libmetis/parmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/parmetis.c rename to gtsam/3rdparty/metis/libmetis/parmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/pmetis.c b/gtsam/3rdparty/metis/libmetis/pmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/pmetis.c rename to gtsam/3rdparty/metis/libmetis/pmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/proto.h b/gtsam/3rdparty/metis/libmetis/proto.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/proto.h rename to gtsam/3rdparty/metis/libmetis/proto.h diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/refine.c b/gtsam/3rdparty/metis/libmetis/refine.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/refine.c rename to gtsam/3rdparty/metis/libmetis/refine.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/rename.h b/gtsam/3rdparty/metis/libmetis/rename.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/rename.h rename to gtsam/3rdparty/metis/libmetis/rename.h diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/separator.c b/gtsam/3rdparty/metis/libmetis/separator.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/separator.c rename to gtsam/3rdparty/metis/libmetis/separator.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/sfm.c b/gtsam/3rdparty/metis/libmetis/sfm.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/sfm.c rename to gtsam/3rdparty/metis/libmetis/sfm.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/srefine.c b/gtsam/3rdparty/metis/libmetis/srefine.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/srefine.c rename to gtsam/3rdparty/metis/libmetis/srefine.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/stat.c b/gtsam/3rdparty/metis/libmetis/stat.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/stat.c rename to gtsam/3rdparty/metis/libmetis/stat.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/stdheaders.h b/gtsam/3rdparty/metis/libmetis/stdheaders.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/stdheaders.h rename to gtsam/3rdparty/metis/libmetis/stdheaders.h diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/struct.h b/gtsam/3rdparty/metis/libmetis/struct.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/struct.h rename to gtsam/3rdparty/metis/libmetis/struct.h diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/timing.c b/gtsam/3rdparty/metis/libmetis/timing.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/timing.c rename to gtsam/3rdparty/metis/libmetis/timing.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/util.c b/gtsam/3rdparty/metis/libmetis/util.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/util.c rename to gtsam/3rdparty/metis/libmetis/util.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/wspace.c b/gtsam/3rdparty/metis/libmetis/wspace.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/wspace.c rename to gtsam/3rdparty/metis/libmetis/wspace.c diff --git a/gtsam/3rdparty/metis-5.1.0/manual/manual.pdf b/gtsam/3rdparty/metis/manual/manual.pdf similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/manual/manual.pdf rename to gtsam/3rdparty/metis/manual/manual.pdf diff --git a/gtsam/3rdparty/metis/metis.h b/gtsam/3rdparty/metis/metis.h new file mode 100644 index 000000000..a1ba08684 --- /dev/null +++ b/gtsam/3rdparty/metis/metis.h @@ -0,0 +1,13 @@ +/* + * metis.h + * Dummy header, not installed! + * Created on: Nov 24, 2014 + * Author: cbeall3 + */ + +#ifndef GTSAM_3RDPARTY_METIS_METIS_H_ +#define GTSAM_3RDPARTY_METIS_METIS_H_ + +#include + +#endif /* GTSAM_3RDPARTY_METIS_METIS_H_ */ diff --git a/gtsam/3rdparty/metis-5.1.0/programs/CMakeLists.txt b/gtsam/3rdparty/metis/programs/CMakeLists.txt similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/CMakeLists.txt rename to gtsam/3rdparty/metis/programs/CMakeLists.txt diff --git a/gtsam/3rdparty/metis-5.1.0/programs/cmdline_gpmetis.c b/gtsam/3rdparty/metis/programs/cmdline_gpmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/cmdline_gpmetis.c rename to gtsam/3rdparty/metis/programs/cmdline_gpmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/cmdline_m2gmetis.c b/gtsam/3rdparty/metis/programs/cmdline_m2gmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/cmdline_m2gmetis.c rename to gtsam/3rdparty/metis/programs/cmdline_m2gmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/cmdline_mpmetis.c b/gtsam/3rdparty/metis/programs/cmdline_mpmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/cmdline_mpmetis.c rename to gtsam/3rdparty/metis/programs/cmdline_mpmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/cmdline_ndmetis.c b/gtsam/3rdparty/metis/programs/cmdline_ndmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/cmdline_ndmetis.c rename to gtsam/3rdparty/metis/programs/cmdline_ndmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/cmpfillin.c b/gtsam/3rdparty/metis/programs/cmpfillin.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/cmpfillin.c rename to gtsam/3rdparty/metis/programs/cmpfillin.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/defs.h b/gtsam/3rdparty/metis/programs/defs.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/defs.h rename to gtsam/3rdparty/metis/programs/defs.h diff --git a/gtsam/3rdparty/metis-5.1.0/programs/gpmetis.c b/gtsam/3rdparty/metis/programs/gpmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/gpmetis.c rename to gtsam/3rdparty/metis/programs/gpmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/graphchk.c b/gtsam/3rdparty/metis/programs/graphchk.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/graphchk.c rename to gtsam/3rdparty/metis/programs/graphchk.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/io.c b/gtsam/3rdparty/metis/programs/io.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/io.c rename to gtsam/3rdparty/metis/programs/io.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/m2gmetis.c b/gtsam/3rdparty/metis/programs/m2gmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/m2gmetis.c rename to gtsam/3rdparty/metis/programs/m2gmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/metisbin.h b/gtsam/3rdparty/metis/programs/metisbin.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/metisbin.h rename to gtsam/3rdparty/metis/programs/metisbin.h diff --git a/gtsam/3rdparty/metis-5.1.0/programs/mpmetis.c b/gtsam/3rdparty/metis/programs/mpmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/mpmetis.c rename to gtsam/3rdparty/metis/programs/mpmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/ndmetis.c b/gtsam/3rdparty/metis/programs/ndmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/ndmetis.c rename to gtsam/3rdparty/metis/programs/ndmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/proto.h b/gtsam/3rdparty/metis/programs/proto.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/proto.h rename to gtsam/3rdparty/metis/programs/proto.h diff --git a/gtsam/3rdparty/metis-5.1.0/programs/smbfactor.c b/gtsam/3rdparty/metis/programs/smbfactor.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/smbfactor.c rename to gtsam/3rdparty/metis/programs/smbfactor.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/stat.c b/gtsam/3rdparty/metis/programs/stat.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/stat.c rename to gtsam/3rdparty/metis/programs/stat.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/struct.h b/gtsam/3rdparty/metis/programs/struct.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/struct.h rename to gtsam/3rdparty/metis/programs/struct.h diff --git a/gtsam/3rdparty/metis-5.1.0/vsgen.bat b/gtsam/3rdparty/metis/vsgen.bat similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/vsgen.bat rename to gtsam/3rdparty/metis/vsgen.bat diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 90241faa0..cab5e8639 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -1,16 +1,16 @@ # We split the library in to separate subfolders, each containing # tests, timing, and an optional convenience library. # The following variable is the master list of subdirs to add -set (gtsam_subdirs - base - geometry - inference - symbolic - discrete - linear +set (gtsam_subdirs + base + geometry + inference + symbolic + discrete + linear nonlinear sam - sfm + sfm slam smart navigation @@ -19,13 +19,12 @@ set (gtsam_subdirs set(gtsam_srcs) # Build 3rdparty separately -message(STATUS "Building 3rdparty") +message(STATUS "Building 3rdparty") add_subdirectory(3rdparty) -# build convenience library -set (3rdparty_srcs +set (3rdparty_srcs ${eigen_headers} # Set by 3rdparty/CMakeLists.txt - ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Source/ccolamd.c + ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Source/ccolamd.c ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Source/ccolamd_global.c ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/UFconfig/UFconfig.c) gtsam_assign_source_folders("${3rdparty_srcs}") # Create MSVC structure @@ -40,7 +39,7 @@ set (excluded_sources #"") set (excluded_headers #"") "${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.h" ) - + if(GTSAM_USE_QUATERNIONS) set(excluded_sources ${excluded_sources} "${CMAKE_CURRENT_SOURCE_DIR}/geometry/Rot3M.cpp") else() @@ -62,10 +61,10 @@ foreach(subdir ${gtsam_subdirs}) set(${subdir}_srcs ${subdir_srcs}) # Build local library and tests - message(STATUS "Building ${subdir}") + message(STATUS "Building ${subdir}") add_subdirectory(${subdir}) endforeach(subdir) - + # To add additional sources to gtsam when building the full library (static or shared) # Add the subfolder with _srcs appended to the end to this list set(gtsam_srcs @@ -73,15 +72,15 @@ set(gtsam_srcs ${base_srcs} ${geometry_srcs} ${inference_srcs} - ${symbolic_srcs} + ${symbolic_srcs} ${discrete_srcs} ${linear_srcs} ${nonlinear_srcs} ${slam_srcs} - ${navigation_srcs} + ${navigation_srcs} ${gtsam_core_headers} ) - + # Generate and install config and dllexport files configure_file(config.h.in config.h) set(library_name GTSAM) # For substitution in dllexport.h.in @@ -89,18 +88,20 @@ configure_file("${PROJECT_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h") list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h") install(FILES "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h" DESTINATION include/gtsam) +list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis) + # Versions set(gtsam_version ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}) set(gtsam_soversion ${GTSAM_VERSION_MAJOR}) message(STATUS "GTSAM Version: ${gtsam_version}") message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}") - + # build shared and static versions of the library if (GTSAM_BUILD_STATIC_LIBRARY) message(STATUS "Building GTSAM - static") add_library(gtsam STATIC ${gtsam_srcs}) target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES}) - set_target_properties(gtsam PROPERTIES + set_target_properties(gtsam PROPERTIES OUTPUT_NAME gtsam CLEAN_DIRECT_OUTPUT 1 VERSION ${gtsam_version} @@ -117,7 +118,7 @@ else() message(STATUS "Building GTSAM - shared") add_library(gtsam SHARED ${gtsam_srcs}) target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES}) - set_target_properties(gtsam PROPERTIES + set_target_properties(gtsam PROPERTIES OUTPUT_NAME gtsam CLEAN_DIRECT_OUTPUT 1 VERSION ${gtsam_version} @@ -133,12 +134,6 @@ else() set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) endif() -# Set dataset paths -set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/slam/dataset.cpp" - APPEND PROPERTY COMPILE_DEFINITIONS - "SOURCE_TREE_DATASET_DIR=\"${PROJECT_SOURCE_DIR}/examples/Data\"" - "INSTALLED_DATASET_DIR=\"${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data\"") - # Special cases if(MSVC) set_property(SOURCE @@ -151,7 +146,7 @@ endif() if (GTSAM_INSTALL_MATLAB_TOOLBOX) # Set up codegen include(GtsamMatlabWrap) - + # Generate, build and install toolbox set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}") if(GTSAM_BUILD_STATIC_LIBRARY) diff --git a/gtsam/base/ChartValue.h b/gtsam/base/ChartValue.h deleted file mode 100644 index 3cc6a041c..000000000 --- a/gtsam/base/ChartValue.h +++ /dev/null @@ -1,221 +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 ChartValue.h - * @brief - * @date October, 2014 - * @author Michael Bosse, Abel Gawel, Renaud Dube - * based on DerivedValue.h by Duy Nguyen Ta - */ - -#pragma once - -#include -#include -#include - -////////////////// -// The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR -#include - -#ifdef min -#undef min -#endif - -#ifdef max -#undef max -#endif - -#ifdef ERROR -#undef ERROR -#endif -////////////////// - -namespace gtsam { - -/** - * ChartValue is derived from GenericValue and Chart so that - * Chart can be zero sized (as in DefaultChart) - * if the Chart is a member variable then it won't ever be zero sized. - */ -template > -class ChartValue: public GenericValue, public Chart_ { - - BOOST_CONCEPT_ASSERT((ChartConcept)); - -public: - - typedef T type; - typedef Chart_ Chart; - -public: - - /// Default Constructor. TODO might not make sense for some types - ChartValue() : - GenericValue(T()) { - } - - /// Construct froma value - ChartValue(const T& value) : - GenericValue(value) { - } - - /// Construct from a value and initialize the chart - template - ChartValue(const T& value, C chart_initializer) : - GenericValue(value), Chart(chart_initializer) { - } - - /// Destructor - virtual ~ChartValue() { - } - - /** - * Create a duplicate object returned as a pointer to the generic Value interface. - * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator. - * The result must be deleted with Value::deallocate_, not with the 'delete' operator. - */ - virtual Value* clone_() const { - void *place = boost::singleton_pool::malloc(); - ChartValue* ptr = new (place) ChartValue(*this); // calls copy constructor to fill in - return ptr; - } - - /** - * Destroy and deallocate this object, only if it was originally allocated using clone_(). - */ - virtual void deallocate_() const { - this->~ChartValue(); // Virtual destructor cleans up the derived object - boost::singleton_pool::free((void*) this); // Release memory from pool - } - - /** - * Clone this value (normal clone on the heap, delete with 'delete' operator) - */ - virtual boost::shared_ptr clone() const { - return boost::make_shared(*this); - } - - /// Chart Value interface version of retract - virtual Value* retract_(const Vector& delta) const { - // Call retract on the derived class using the retract trait function - const T retractResult = Chart::retract(GenericValue::value(), delta); - - // Create a Value pointer copy of the result - void* resultAsValuePlace = - boost::singleton_pool::malloc(); - Value* resultAsValue = new (resultAsValuePlace) ChartValue(retractResult, - static_cast(*this)); - - // Return the pointer to the Value base class - return resultAsValue; - } - - /// Generic Value interface version of localCoordinates - virtual Vector localCoordinates_(const Value& value2) const { - // Cast the base class Value pointer to a templated generic class pointer - const GenericValue& genericValue2 = - static_cast&>(value2); - - // Return the result of calling localCoordinates trait on the derived class - return Chart::local(GenericValue::value(), genericValue2.value()); - } - - /// Non-virtual version of retract - ChartValue retract(const Vector& delta) const { - return ChartValue(Chart::retract(GenericValue::value(), delta), - static_cast(*this)); - } - - /// Non-virtual version of localCoordinates - Vector localCoordinates(const ChartValue& value2) const { - return localCoordinates_(value2); - } - - /// Return run-time dimensionality - virtual size_t dim() const { - // need functional form here since the dimension may be dynamic - return Chart::getDimension(GenericValue::value()); - } - - /// Assignment operator - virtual Value& operator=(const Value& rhs) { - // Cast the base class Value pointer to a derived class pointer - const ChartValue& derivedRhs = static_cast(rhs); - - // Do the assignment and return the result - *this = ChartValue(derivedRhs); // calls copy constructor - return *this; - } - -protected: - - // implicit assignment operator for (const ChartValue& rhs) works fine here - /// Assignment operator, protected because only the Value or DERIVED - /// assignment operators should be used. - // DerivedValue& operator=(const DerivedValue& rhs) { - // // Nothing to do, do not call base class assignment operator - // return *this; - // } - -private: - - /// Fake Tag struct for singleton pool allocator. In fact, it is never used! - struct PoolTag { - }; - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - // ar & boost::serialization::make_nvp("value",); - // todo: implement a serialization for charts - ar - & boost::serialization::make_nvp("GenericValue", - boost::serialization::base_object >(*this)); - } - -}; - -// Define -namespace traits { - -/// The dimension of a ChartValue is the dimension of the chart -template -struct dimension > : public dimension { - // TODO Frank thinks dimension is a property of type, chart should conform -}; - -} // \ traits - -/// Get the chart from a Value -template -const Chart& Value::getChart() const { - return dynamic_cast(*this); -} - -/// Convenience function that can be used to make an expression to convert a value to a chart -template -ChartValue convertToChartValue(const T& value, - boost::optional< - Eigen::Matrix::value, - traits::dimension::value>&> H = boost::none) { - if (H) { - *H = Eigen::Matrix::value, - traits::dimension::value>::Identity(); - } - return ChartValue(value); -} - -} /* namespace gtsam */ diff --git a/gtsam/base/ConcurrentMap.h b/gtsam/base/ConcurrentMap.h index 336ea7e05..b8388057d 100644 --- a/gtsam/base/ConcurrentMap.h +++ b/gtsam/base/ConcurrentMap.h @@ -95,7 +95,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void save(Archive& ar, const unsigned int version) const + void save(Archive& ar, const unsigned int /*version*/) const { // Copy to an STL container and serialize that FastVector > map(this->size()); @@ -103,7 +103,7 @@ private: ar & BOOST_SERIALIZATION_NVP(map); } template - void load(Archive& ar, const unsigned int version) + void load(Archive& ar, const unsigned int /*version*/) { // Load into STL container and then fill our map FastVector > map; diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h index 78155d308..f01156bd6 100644 --- a/gtsam/base/DerivedValue.h +++ b/gtsam/base/DerivedValue.h @@ -129,7 +129,7 @@ public: protected: /// Assignment operator, protected because only the Value or DERIVED /// assignment operators should be used. - DerivedValue& operator=(const DerivedValue& rhs) { + DerivedValue& operator=(const DerivedValue& /*rhs*/) { // Nothing to do, do not call base class assignment operator return *this; } diff --git a/gtsam/base/FastList.h b/gtsam/base/FastList.h index 4b5d1caf1..380836d1d 100644 --- a/gtsam/base/FastList.h +++ b/gtsam/base/FastList.h @@ -74,7 +74,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } diff --git a/gtsam/base/FastMap.h b/gtsam/base/FastMap.h index 0a76c08b0..7cd6e28b8 100644 --- a/gtsam/base/FastMap.h +++ b/gtsam/base/FastMap.h @@ -70,7 +70,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index 69e841e57..73df17b0d 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -19,15 +19,15 @@ #pragma once #include - +#include +#include +#include +#include +#include #include #include #include #include -#include -#include -#include -#include BOOST_MPL_HAS_XXX_TRAIT_DEF(print) @@ -112,7 +112,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h index be3eaa981..d154ea52a 100644 --- a/gtsam/base/FastVector.h +++ b/gtsam/base/FastVector.h @@ -95,7 +95,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 6c109675d..e83a64ba9 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -19,78 +19,18 @@ #pragma once -#include +#include #include + +#include +#include + #include -#include +#include +#include // operator typeid namespace gtsam { -// To play as a GenericValue, we need the following traits -namespace traits { - -// trait to wrap the default equals of types -template -struct equals { - typedef T type; - typedef bool result_type; - bool operator()(const T& a, const T& b, double tol) { - return a.equals(b, tol); - } -}; - -// trait to wrap the default print of types -template -struct print { - typedef T type; - typedef void result_type; - void operator()(const T& obj, const std::string& str) { - obj.print(str); - } -}; - -// equals for scalars -template<> -struct equals { - typedef double type; - typedef bool result_type; - bool operator()(double a, double b, double tol) { - return std::abs(a - b) <= tol; - } -}; - -// print for scalars -template<> -struct print { - typedef double type; - typedef void result_type; - void operator()(double a, const std::string& str) { - std::cout << str << ": " << a << std::endl; - } -}; - -// equals for Matrix types -template -struct equals > { - typedef Eigen::Matrix type; - typedef bool result_type; - bool operator()(const type& A, const type& B, double tol) { - return equal_with_abs_tol(A, B, tol); - } -}; - -// print for Matrix types -template -struct print > { - typedef Eigen::Matrix type; - typedef void result_type; - void operator()(const type& A, const std::string& str) { - std::cout << str << ": " << A << std::endl; - } -}; - -} - /** * Wraps any type T so it can play as a Value */ @@ -106,6 +46,8 @@ protected: T value_; ///< The wrapped value public: + // Only needed for serialization. + GenericValue(){} /// Construct from value GenericValue(const T& value) : @@ -131,31 +73,124 @@ public: // Cast the base class Value pointer to a templated generic class pointer const GenericValue& genericValue2 = static_cast(p); // Return the result of using the equals traits for the derived class - return traits::equals()(this->value_, genericValue2.value_, tol); + return traits::Equals(this->value_, genericValue2.value_, tol); } /// non virtual equals function, uses traits bool equals(const GenericValue &other, double tol = 1e-9) const { - return traits::equals()(this->value(), other.value(), tol); + return traits::Equals(this->value(), other.value(), tol); } /// Virtual print function, uses traits virtual void print(const std::string& str) const { - traits::print()(value_, str); + std::cout << "(" << typeid(T).name() << ") "; + traits::Print(value_, str); } - // Serialization below: + /** + * Create a duplicate object returned as a pointer to the generic Value interface. + * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator. + * The result must be deleted with Value::deallocate_, not with the 'delete' operator. + */ + virtual Value* clone_() const { + void *place = boost::singleton_pool::malloc(); + GenericValue* ptr = new (place) GenericValue(*this); // calls copy constructor to fill in + return ptr; + } - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value); - ar & BOOST_SERIALIZATION_NVP(value_); - } + /** + * Destroy and deallocate this object, only if it was originally allocated using clone_(). + */ + virtual void deallocate_() const { + this->~GenericValue(); // Virtual destructor cleans up the derived object + boost::singleton_pool::free((void*) this); // Release memory from pool + } -protected: + /** + * Clone this value (normal clone on the heap, delete with 'delete' operator) + */ + virtual boost::shared_ptr clone() const { + return boost::make_shared(*this); + } - // Assignment operator for this class not needed since GenericValue is an abstract class + /// Generic Value interface version of retract + virtual Value* retract_(const Vector& delta) const { + // Call retract on the derived class using the retract trait function + const T retractResult = traits::Retract(GenericValue::value(), delta); + + // Create a Value pointer copy of the result + void* resultAsValuePlace = + boost::singleton_pool::malloc(); + Value* resultAsValue = new (resultAsValuePlace) GenericValue(retractResult); + + // Return the pointer to the Value base class + return resultAsValue; + } + + /// Generic Value interface version of localCoordinates + virtual Vector localCoordinates_(const Value& value2) const { + // Cast the base class Value pointer to a templated generic class pointer + const GenericValue& genericValue2 = + static_cast&>(value2); + + // Return the result of calling localCoordinates trait on the derived class + return traits::Local(GenericValue::value(), genericValue2.value()); + } + + /// Non-virtual version of retract + GenericValue retract(const Vector& delta) const { + return GenericValue(traits::Retract(GenericValue::value(), delta)); + } + + /// Non-virtual version of localCoordinates + Vector localCoordinates(const GenericValue& value2) const { + return localCoordinates_(value2); + } + + /// Return run-time dimensionality + virtual size_t dim() const { + return traits::GetDimension(value_); + } + + /// Assignment operator + virtual Value& operator=(const Value& rhs) { + // Cast the base class Value pointer to a derived class pointer + const GenericValue& derivedRhs = static_cast(rhs); + + // Do the assignment and return the result + *this = GenericValue(derivedRhs); // calls copy constructor + return *this; + } + + protected: + + // implicit assignment operator for (const GenericValue& rhs) works fine here + /// Assignment operator, protected because only the Value or DERIVED + /// assignment operators should be used. + // DerivedValue& operator=(const DerivedValue& rhs) { + // // Nothing to do, do not call base class assignment operator + // return *this; + // } + + private: + + /// Fake Tag struct for singleton pool allocator. In fact, it is never used! + struct PoolTag { + }; + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("GenericValue", + boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("value", value_); + } + +/// use this macro instead of BOOST_CLASS_EXPORT for GenericValues +#define GTSAM_VALUE_EXPORT(Type) BOOST_CLASS_EXPORT(GenericValue) }; diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index 5b2574ee3..39541416e 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -13,49 +13,201 @@ * @file Group.h * * @brief Concept check class for variable types with Group properties - * @date Nov 5, 2011 + * @date November, 2011 * @author Alex Cunningham + * @author Frank Dellaert */ #pragma once -#include +#include + +#include +#include +#include +#include +#include namespace gtsam { +/// tag to assert a type is a group +struct group_tag {}; + +/// Group operator syntax flavors +struct multiplicative_group_tag {}; +struct additive_group_tag {}; + +template struct traits; + /** - * This concept check enforces a Group structure on a variable type, - * in which we require the existence of basic algebraic operations. + * Group Concept */ -template -class GroupConcept { +template +class IsGroup { +public: + typedef typename traits::structure_category structure_category_tag; + typedef typename traits::group_flavor flavor_tag; + //typedef typename traits::identity::value_type identity_value_type; + + BOOST_CONCEPT_USAGE(IsGroup) { + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "This type's structure_category trait does not assert it as a group (or derived)"); + e = traits::Identity(); + e = traits::Compose(g, h); + e = traits::Between(g, h); + e = traits::Inverse(g); + operator_usage(flavor); + // todo: how do we test the act concept? or do we even need to? + } + private: - static T concept_check(const T& t) { - /** assignment */ - T t2 = t; + void operator_usage(multiplicative_group_tag) { + e = g * h; + //e = -g; // todo this should work, but it is failing for Quaternions + } + void operator_usage(additive_group_tag) { + e = g + h; + e = h - g; + e = -g; + } - /** identity */ - T identity = T::identity(); + flavor_tag flavor; + G e, g, h; + bool b; +}; - /** compose with another object */ - T compose_ret = identity.compose(t2); +/// Check invariants +template +BOOST_CONCEPT_REQUIRES(((IsGroup)),(bool)) // +check_group_invariants(const G& a, const G& b, double tol = 1e-9) { + G e = traits::Identity(); + return traits::Equals(traits::Compose(a, traits::Inverse(a)), e, tol) + && traits::Equals(traits::Between(a, b), traits::Compose(traits::Inverse(a), b), tol) + && traits::Equals(traits::Compose(a, traits::Between(a, b)), b, tol); +} - /** invert the object and yield a new one */ - T inverse_ret = compose_ret.inverse(); +namespace internal { - return inverse_ret; +/// A helper class that implements the traits interface for multiplicative groups. +/// Assumes existence of identity, operator* and inverse method +template +struct MultiplicativeGroupTraits { + typedef group_tag structure_category; + typedef multiplicative_group_tag group_flavor; + static Class Identity() { return Class::identity(); } + static Class Compose(const Class &g, const Class & h) { return g * h;} + static Class Between(const Class &g, const Class & h) { return g.inverse() * h;} + static Class Inverse(const Class &g) { return g.inverse();} +}; + +/// Both multiplicative group traits and Testable +template +struct MultiplicativeGroup : MultiplicativeGroupTraits, Testable {}; + +/// A helper class that implements the traits interface for additive groups. +/// Assumes existence of identity and three additive operators +template +struct AdditiveGroupTraits { + typedef group_tag structure_category; + typedef additive_group_tag group_flavor; + static Class Identity() { return Class::identity(); } + static Class Compose(const Class &g, const Class & h) { return g + h;} + static Class Between(const Class &g, const Class & h) { return h - g;} + static Class Inverse(const Class &g) { return -g;} +}; + +/// Both additive group traits and Testable +template +struct AdditiveGroup : AdditiveGroupTraits, Testable {}; + +} // namespace internal + +/// compose multiple times +template +BOOST_CONCEPT_REQUIRES(((IsGroup)),(G)) // +compose_pow(const G& g, size_t n) { + if (n == 0) return traits::Identity(); + else if (n == 1) return g; + else return traits::Compose(compose_pow(g, n - 1), g); +} + +/// Template to construct the direct product of two arbitrary groups +/// Assumes nothing except group structure and Testable from G and H +template +class DirectProduct: public std::pair { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsGroup)); + +public: + /// Default constructor yields identity + DirectProduct():std::pair(traits::Identity(),traits::Identity()) {} + + // Construct from two subgroup elements + DirectProduct(const G& g, const H& h):std::pair(g,h) {} + + // identity + static DirectProduct identity() { return DirectProduct(); } + + DirectProduct operator*(const DirectProduct& other) const { + return DirectProduct(traits::Compose(this->first, other.first), + traits::Compose(this->second, other.second)); + } + DirectProduct inverse() const { + return DirectProduct(this->first.inverse(), this->second.inverse()); } }; -} // \namespace gtsam +// Define any direct product group to be a model of the multiplicative Group concept +template +struct traits > : + internal::MultiplicativeGroupTraits > {}; + +/// Template to construct the direct sum of two additive groups +/// Assumes existence of three additive operators for both groups +template +class DirectSum: public std::pair { + BOOST_CONCEPT_ASSERT((IsGroup)); // TODO(frank): check additive + BOOST_CONCEPT_ASSERT((IsGroup)); // TODO(frank): check additive + + const G& g() const { return this->first; } + const H& h() const { return this->second;} + +public: + /// Default constructor yields identity + DirectSum():std::pair(traits::Identity(),traits::Identity()) {} + + // Construct from two subgroup elements + DirectSum(const G& g, const H& h):std::pair(g,h) {} + + // identity + static DirectSum identity() { return DirectSum(); } + + DirectSum operator+(const DirectSum& other) const { + return DirectSum(g()+other.g(), h()+other.h()); + } + DirectSum operator-(const DirectSum& other) const { + return DirectSum(g()-other.g(), h()-other.h()); + } + DirectSum operator-() const { + return DirectSum(- g(), - h()); + } +}; + +// Define direct sums to be a model of the Additive Group concept +template +struct traits > : + internal::AdditiveGroupTraits > {}; + +} // namespace gtsam /** - * Macros for using the GroupConcept + * Macros for using the IsGroup * - An instantiation for use inside unit tests * - A typedef for use inside generic algorithms * * NOTE: intentionally not in the gtsam namespace to allow for classes not in * the gtsam namespace to be more easily enforced as testable */ -#define GTSAM_CONCEPT_GROUP_INST(T) template class gtsam::GroupConcept; -#define GTSAM_CONCEPT_GROUP_TYPE(T) typedef gtsam::GroupConcept _gtsam_GroupConcept_##T; +#define GTSAM_CONCEPT_GROUP_INST(T) template class gtsam::IsGroup; +#define GTSAM_CONCEPT_GROUP_TYPE(T) typedef gtsam::IsGroup _gtsam_IsGroup_##T; diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 7a9d32249..05c7bc20f 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -14,6 +14,9 @@ * @brief Base class and basic functions for Lie types * @author Richard Roberts * @author Alex Cunningham + * @author Frank Dellaert + * @author Mike Bosse + * @author Duy Nguyen Ta */ @@ -24,82 +27,236 @@ namespace gtsam { +/// A CRTP helper class that implements Lie group methods +/// Prerequisites: methods operator*, inverse, and AdjointMap, as well as a +/// ChartAtOrigin struct that will be used to define the manifold Chart +/// To use, simply derive, but also say "using LieGroup::inverse" +/// For derivative math, see doc/math.pdf +template +struct LieGroup { + + BOOST_STATIC_ASSERT_MSG(N != Eigen::Dynamic, + "LieGroup not yet specialized for dynamically sized types."); + + enum { dimension = N }; + typedef OptionalJacobian ChartJacobian; + typedef Eigen::Matrix Jacobian; + typedef Eigen::Matrix TangentVector; + + const Class & derived() const { + return static_cast(*this); + } + + Class compose(const Class& g) const { + return derived() * g; + } + + Class between(const Class& g) const { + return derived().inverse() * g; + } + + Class compose(const Class& g, ChartJacobian H1, + ChartJacobian H2 = boost::none) const { + if (H1) *H1 = g.inverse().AdjointMap(); + if (H2) *H2 = Eigen::Matrix::Identity(); + return derived() * g; + } + + Class between(const Class& g, ChartJacobian H1, + ChartJacobian H2 = boost::none) const { + Class result = derived().inverse() * g; + if (H1) *H1 = - result.inverse().AdjointMap(); + if (H2) *H2 = Eigen::Matrix::Identity(); + return result; + } + + Class inverse(ChartJacobian H) const { + if (H) *H = - derived().AdjointMap(); + return derived().inverse(); + } + + Class expmap(const TangentVector& v) const { + return compose(Class::Expmap(v)); + } + + TangentVector logmap(const Class& g) const { + return Class::Logmap(between(g)); + } + + static Class Retract(const TangentVector& v) { + return Class::ChartAtOrigin::Retract(v); + } + + static TangentVector LocalCoordinates(const Class& g) { + return Class::ChartAtOrigin::Local(g); + } + + static Class Retract(const TangentVector& v, ChartJacobian H) { + return Class::ChartAtOrigin::Retract(v,H); + } + + static TangentVector LocalCoordinates(const Class& g, ChartJacobian H) { + return Class::ChartAtOrigin::Local(g,H); + } + + Class retract(const TangentVector& v) const { + return compose(Class::ChartAtOrigin::Retract(v)); + } + + TangentVector localCoordinates(const Class& g) const { + return Class::ChartAtOrigin::Local(between(g)); + } + + Class retract(const TangentVector& v, // + ChartJacobian H1, ChartJacobian H2 = boost::none) const { + Jacobian D_g_v; + Class g = Class::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0); + Class h = compose(g,H1,H2); + if (H2) *H2 = (*H2) * D_g_v; + return h; + } + + TangentVector localCoordinates(const Class& g, // + ChartJacobian H1, ChartJacobian H2 = boost::none) const { + Class h = between(g,H1,H2); + Jacobian D_v_h; + TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0); + if (H1) *H1 = D_v_h * (*H1); + if (H2) *H2 = D_v_h * (*H2); + return v; + } + +}; + +/// tag to assert a type is a Lie group +struct lie_group_tag: public manifold_tag, public group_tag {}; + +namespace internal { + +/// A helper class that implements the traits interface for GTSAM lie groups. +/// To use this for your gtsam type, define: +/// template<> struct traits : public internal::LieGroupTraits {}; +/// Assumes existence of: identity, dimension, localCoordinates, retract, +/// and additionally Logmap, Expmap, compose, between, and inverse +template +struct LieGroupTraits { + typedef lie_group_tag structure_category; + + /// @name Group + /// @{ + typedef multiplicative_group_tag group_flavor; + static Class Identity() { return Class::identity();} + /// @} + + /// @name Manifold + /// @{ + typedef Class ManifoldType; + enum { dimension = Class::dimension }; + typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian ChartJacobian; + + BOOST_STATIC_ASSERT_MSG(dimension != Eigen::Dynamic, + "LieGroupTraits not yet specialized for dynamically sized types."); + + static int GetDimension(const Class&) {return dimension;} + + static TangentVector Local(const Class& origin, const Class& other, + ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { + return origin.localCoordinates(other, Horigin, Hother); + } + + static Class Retract(const Class& origin, const TangentVector& v, + ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) { + return origin.retract(v, Horigin, Hv); + } + /// @} + + /// @name Lie Group + /// @{ + static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) { + return Class::Logmap(m, Hm); + } + + static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + return Class::Expmap(v, Hv); + } + + static Class Compose(const Class& m1, const Class& m2, // + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + return m1.compose(m2, H1, H2); + } + + static Class Between(const Class& m1, const Class& m2, // + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + return m1.between(m2, H1, H2); + } + + static Class Inverse(const Class& m, // + ChartJacobian H = boost::none) { + return m.inverse(H); + } + /// @} +}; + +/// Both LieGroupTraits and Testable +template struct LieGroup: LieGroupTraits, Testable {}; + +} // \ namepsace internal + /** * These core global functions can be specialized by new Lie types * for better performance. */ /** Compute l0 s.t. l2=l1*l0 */ -template -inline T between_default(const T& l1, const T& l2) {return l1.inverse().compose(l2);} +template +inline Class between_default(const Class& l1, const Class& l2) { + return l1.inverse().compose(l2); +} /** Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp */ -template -inline Vector logmap_default(const T& l0, const T& lp) {return T::Logmap(l0.between(lp));} +template +inline Vector logmap_default(const Class& l0, const Class& lp) { + return Class::Logmap(l0.between(lp)); +} /** Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */ -template -inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap(d));} +template +inline Class expmap_default(const Class& t, const Vector& d) { + return t.compose(Class::Expmap(d)); +} /** - * Concept check class for Lie group type - * - * This concept check provides a specialization on the Manifold type, - * in which the Manifolds represented require an algebra and group structure. - * All Lie types must also be a Manifold. - * - * The necessary functions to implement for Lie are defined - * below with additional details as to the interface. The - * concept checking function in class Lie will check whether or not - * the function exists and throw compile-time errors. - * - * The exponential map is a specific retraction for Lie groups, - * which maps the tangent space in canonical coordinates back to - * the underlying manifold. Because we can enforce a group structure, - * a retraction of delta v, with tangent space centered at x1 can be performed - * as x2 = x1.compose(Expmap(v)). - * - * Expmap around identity - * static T Expmap(const Vector& v); - * - * Logmap around identity - * static Vector Logmap(const T& p); - * - * Compute l0 s.t. l2=l1*l0, where (*this) is l1 - * A default implementation of between(*this, lp) is available: - * between_default() - * T between(const T& l2) const; - * - * By convention, we use capital letters to designate a static function - * - * @tparam T is a Lie type, like Point2, Pose3, etc. + * Lie Group Concept */ -template -class LieConcept { -private: - /** concept checking function - implement the functions this demands */ - static T concept_check(const T& t) { +template +class IsLieGroup: public IsGroup, public IsManifold { +public: + typedef typename traits::structure_category structure_category_tag; + typedef typename traits::ManifoldType ManifoldType; + typedef typename traits::TangentVector TangentVector; + typedef typename traits::ChartJacobian ChartJacobian; - /** assignment */ - T t2 = t; + BOOST_CONCEPT_USAGE(IsLieGroup) { + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "This type's trait does not assert it is a Lie group (or derived)"); - /** - * Returns dimensionality of the tangent space - */ - size_t dim_ret = t.dim(); - - /** expmap around identity */ - T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret)); - - /** Logmap around identity */ - Vector logmap_identity_ret = T::Logmap(t); - - /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */ - T between_ret = expmap_identity_ret.between(t2); - - return between_ret; + // group opertations with Jacobians + g = traits::Compose(g, h, Hg, Hh); + g = traits::Between(g, h, Hg, Hh); + g = traits::Inverse(g, Hg); + // log and exp map without Jacobians + g = traits::Expmap(v); + v = traits::Logmap(g); + // log and exponential map with Jacobians + g = traits::Expmap(v, Hg); + v = traits::Logmap(g, Hg); } - +private: + T g, h; + TangentVector v; + ChartJacobian Hg, Hh; }; /** @@ -146,12 +303,5 @@ T expm(const Vector& x, int K=7) { * NOTE: intentionally not in the gtsam namespace to allow for classes not in * the gtsam namespace to be more easily enforced as testable */ -#define GTSAM_CONCEPT_LIE_INST(T) \ - template class gtsam::ManifoldConcept; \ - template class gtsam::GroupConcept; \ - template class gtsam::LieConcept; - -#define GTSAM_CONCEPT_LIE_TYPE(T) \ - typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; \ - typedef gtsam::GroupConcept _gtsam_GroupConcept_##T; \ - typedef gtsam::LieConcept _gtsam_LieConcept_##T; +#define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::IsLieGroup; +#define GTSAM_CONCEPT_LIE_TYPE(T) typedef gtsam::IsLieGroup _gtsam_IsLieGroup_##T; diff --git a/gtsam/base/LieMatrix.cpp b/gtsam/base/LieMatrix.cpp index 69054fc1c..cf5b57536 100644 --- a/gtsam/base/LieMatrix.cpp +++ b/gtsam/base/LieMatrix.cpp @@ -15,7 +15,7 @@ * @author Richard Roberts and Alex Cunningham */ -#include +#include namespace gtsam { diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index f1fc69e71..e24f339e4 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -11,179 +11,16 @@ /** * @file LieMatrix.h - * @brief A wrapper around Matrix providing Lie compatibility - * @author Richard Roberts and Alex Cunningham + * @brief External deprecation warning, see LieMatrix_Deprecated.h for details + * @author Paul Drews */ #pragma once -#include - -#include -#include -#include -#include - -namespace gtsam { - -/** - * LieVector is a wrapper around vector to allow it to be a Lie type - */ -struct LieMatrix : public Matrix { - - /// @name Constructors - /// @{ - - /** default constructor - should be unnecessary */ - LieMatrix() {} - - /** initialize from a normal matrix */ - LieMatrix(const Matrix& v) : Matrix(v) {} - -// Currently TMP constructor causes ICE on MSVS 2013 -#if (_MSC_VER < 1800) - /** initialize from a fixed size normal vector */ - template - LieMatrix(const Eigen::Matrix& v) : Matrix(v) {} +#ifdef _MSC_VER +#pragma message("LieMatrix.h is deprecated. Please use Eigen::Matrix instead.") +#else +#warning "LieMatrix.h is deprecated. Please use Eigen::Matrix instead." #endif - /** constructor with size and initial data, row order ! */ - LieMatrix(size_t m, size_t n, const double* const data) : - Matrix(Eigen::Map(data, m, n)) {} - - /// @} - /// @name Testable interface - /// @{ - - /** print @param s optional string naming the object */ - GTSAM_EXPORT void print(const std::string& name="") const; - - /** equality up to tolerance */ - inline bool equals(const LieMatrix& expected, double tol=1e-5) const { - return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol); - } - - /// @} - /// @name Standard Interface - /// @{ - - /** get the underlying vector */ - inline Matrix matrix() const { - return static_cast(*this); - } - - /// @} - /// @name Manifold interface - /// @{ - - /** Returns dimensionality of the tangent space */ - inline size_t dim() const { return this->size(); } - - /** Update the LieMatrix with a tangent space update. The elements of the - * tangent space vector correspond to the matrix entries arranged in - * *row-major* order. */ - inline LieMatrix retract(const Vector& v) const { - if(v.size() != this->size()) - throw std::invalid_argument("LieMatrix::retract called with Vector of incorrect size"); - - return LieMatrix(*this + - Eigen::Map >( - &v(0), this->rows(), this->cols())); - } - - /** @return the local coordinates of another object. The elements of the - * tangent space vector correspond to the matrix entries arranged in - * *row-major* order. */ - inline Vector localCoordinates(const LieMatrix& t2) const { - Vector result(this->size()); - Eigen::Map >( - &result(0), this->rows(), this->cols()) = t2 - *this; - return result; - } - - /// @} - /// @name Group interface - /// @{ - - /** identity - NOTE: no known size at compile time - so zero length */ - inline static LieMatrix identity() { - throw std::runtime_error("LieMatrix::identity(): Don't use this function"); - return LieMatrix(); - } - - // Note: Manually specifying the 'gtsam' namespace for the optional Matrix arguments - // This is a work-around for linux g++ 4.6.1 that incorrectly selects the Eigen::Matrix class - // instead of the gtsam::Matrix class. This is related to deriving this class from an Eigen Vector - // as the other geometry objects (Point3, Rot3, etc.) have this problem - /** compose with another object */ - inline LieMatrix compose(const LieMatrix& p, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = eye(dim()); - if(H2) *H2 = eye(p.dim()); - - return LieMatrix(*this + p); - } - - /** between operation */ - inline LieMatrix between(const LieMatrix& l2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(dim()); - if(H2) *H2 = eye(l2.dim()); - return LieMatrix(l2 - *this); - } - - /** invert the object and yield a new one */ - inline LieMatrix inverse(boost::optional H=boost::none) const { - if(H) *H = -eye(dim()); - - return LieMatrix(-(*this)); - } - - /// @} - /// @name Lie group interface - /// @{ - - /** Expmap around identity */ - static inline LieMatrix Expmap(const Vector& v) { - throw std::runtime_error("LieMatrix::Expmap(): Don't use this function"); - return LieMatrix(v); } - - /** Logmap around identity */ - static inline Vector Logmap(const LieMatrix& p) { - Vector result(p.size()); - Eigen::Map >( - result.data(), p.rows(), p.cols()) = p; - return result; - } - - /// @} - -private: - - // Serialization function - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Matrix", - boost::serialization::base_object(*this)); - - } - -}; - -// Define GTSAM traits -namespace traits { - -template<> -struct is_manifold : public boost::true_type { -}; - -template<> -struct dimension : public Dynamic { -}; - -} - -} // \namespace gtsam +#include "gtsam/base/LieMatrix_Deprecated.h" diff --git a/gtsam/base/LieMatrix_Deprecated.h b/gtsam/base/LieMatrix_Deprecated.h new file mode 100644 index 000000000..15b4401f2 --- /dev/null +++ b/gtsam/base/LieMatrix_Deprecated.h @@ -0,0 +1,151 @@ +/* ---------------------------------------------------------------------------- + + * 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 LieMatrix.h + * @brief A wrapper around Matrix providing Lie compatibility + * @author Richard Roberts and Alex Cunningham + */ + +#pragma once + +#include + +#include +#include + +namespace gtsam { + +/** + * @deprecated: LieMatrix, LieVector and LieMatrix are obsolete in GTSAM 4.0 as + * we can directly add double, Vector, and Matrix into values now, because of + * gtsam::traits. + */ +struct LieMatrix : public Matrix { + + /// @name Constructors + /// @{ + enum { dimension = Eigen::Dynamic }; + + /** default constructor - only for serialize */ + LieMatrix() {} + + /** initialize from a normal matrix */ + LieMatrix(const Matrix& v) : Matrix(v) {} + + template + LieMatrix(const M& v) : Matrix(v) {} + +// Currently TMP constructor causes ICE on MSVS 2013 +#if (_MSC_VER < 1800) + /** initialize from a fixed size normal vector */ + template + LieMatrix(const Eigen::Matrix& v) : Matrix(v) {} +#endif + + /** constructor with size and initial data, row order ! */ + LieMatrix(size_t m, size_t n, const double* const data) : + Matrix(Eigen::Map(data, m, n)) {} + + /// @} + /// @name Testable interface + /// @{ + + /** print @param s optional string naming the object */ + GTSAM_EXPORT void print(const std::string& name="") const; + + /** equality up to tolerance */ + inline bool equals(const LieMatrix& expected, double tol=1e-5) const { + return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol); + } + + /// @} + /// @name Standard Interface + /// @{ + + /** get the underlying matrix */ + inline Matrix matrix() const { + return static_cast(*this); + } + + /// @} + + /// @name Group + /// @{ + LieMatrix compose(const LieMatrix& q) { return (*this)+q;} + LieMatrix between(const LieMatrix& q) { return q-(*this);} + LieMatrix inverse() { return -(*this);} + /// @} + + /// @name Manifold + /// @{ + Vector localCoordinates(const LieMatrix& q) { return between(q).vector();} + LieMatrix retract(const Vector& v) {return compose(LieMatrix(v));} + /// @} + + /// @name Lie Group + /// @{ + static Vector Logmap(const LieMatrix& p) {return p.vector();} + static LieMatrix Expmap(const Vector& v) { return LieMatrix(v);} + /// @} + + /// @name VectorSpace requirements + /// @{ + + /** Returns dimensionality of the tangent space */ + inline size_t dim() const { return size(); } + + /** Convert to vector, is done row-wise - TODO why? */ + inline Vector vector() const { + Vector result(size()); + typedef Eigen::Matrix RowMajor; + Eigen::Map(&result(0), rows(), cols()) = *this; + return result; + } + + /** identity - NOTE: no known size at compile time - so zero length */ + inline static LieMatrix identity() { + throw std::runtime_error("LieMatrix::identity(): Don't use this function"); + return LieMatrix(); + } + /// @} + +private: + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("Matrix", + boost::serialization::base_object(*this)); + + } + +}; + + +template<> +struct traits : public internal::VectorSpace { + + // Override Retract, as the default version does not know how to initialize + static LieMatrix Retract(const LieMatrix& origin, const TangentVector& v, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = Eye(origin); + if (H2) *H2 = Eye(origin); + typedef const Eigen::Matrix RowMajor; + return origin + Eigen::Map(&v(0), origin.rows(), origin.cols()); + } + +}; + +} // \namespace gtsam diff --git a/gtsam/base/LieScalar.cpp b/gtsam/base/LieScalar.cpp index 5f59c29bc..4c5a6a257 100644 --- a/gtsam/base/LieScalar.cpp +++ b/gtsam/base/LieScalar.cpp @@ -8,7 +8,7 @@ -#include +#include namespace gtsam { void LieScalar::print(const std::string& name) const { diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 5125340be..650e2bb21 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -11,122 +11,16 @@ /** * @file LieScalar.h - * @brief A wrapper around scalar providing Lie compatibility + * @brief External deprecation warning, see LieScalar_Deprecated.h for details * @author Kai Ni */ #pragma once -#include -#include -#include +#ifdef _MSC_VER +#pragma message("LieScalar.h is deprecated. Please use double/float instead.") +#else + #warning "LieScalar.h is deprecated. Please use double/float instead." +#endif -namespace gtsam { - - /** - * LieScalar is a wrapper around double to allow it to be a Lie type - */ - struct GTSAM_EXPORT LieScalar { - - /** default constructor */ - LieScalar() : d_(0.0) {} - - /** wrap a double */ - explicit LieScalar(double d) : d_(d) {} - - /** access the underlying value */ - double value() const { return d_; } - - /** Automatic conversion to underlying value */ - operator double() const { return d_; } - - /** print @param name optional string naming the object */ - void print(const std::string& name="") const; - - /** equality up to tolerance */ - bool equals(const LieScalar& expected, double tol=1e-5) const { - return fabs(expected.d_ - d_) <= tol; - } - - // Manifold requirements - - /** Returns dimensionality of the tangent space */ - size_t dim() const { return 1; } - static size_t Dim() { return 1; } - - /** Update the LieScalar with a tangent space update */ - LieScalar retract(const Vector& v) const { return LieScalar(value() + v(0)); } - - /** @return the local coordinates of another object */ - Vector localCoordinates(const LieScalar& t2) const { return (Vector(1) << (t2.value() - value())).finished(); } - - // Group requirements - - /** identity */ - static LieScalar identity() { - return LieScalar(); - } - - /** compose with another object */ - LieScalar compose(const LieScalar& p, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = eye(1); - if(H2) *H2 = eye(1); - return LieScalar(d_ + p.d_); - } - - /** between operation */ - LieScalar between(const LieScalar& l2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(1); - if(H2) *H2 = eye(1); - return LieScalar(l2.value() - value()); - } - - /** invert the object and yield a new one */ - LieScalar inverse() const { - return LieScalar(-1.0 * value()); - } - - // Lie functions - - /** Expmap around identity */ - static LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); } - - /** Logmap around identity - just returns with default cast back */ - static Vector Logmap(const LieScalar& p) { return (Vector(1) << p.value()).finished(); } - - /// Left-trivialized derivative of the exponential map - static Matrix dexpL(const Vector& v) { - return eye(1); - } - - /// Left-trivialized derivative inverse of the exponential map - static Matrix dexpInvL(const Vector& v) { - return eye(1); - } - - private: - double d_; - }; - - // Define GTSAM traits - namespace traits { - - template<> - struct is_group : public boost::true_type { - }; - - template<> - struct is_manifold : public boost::true_type { - }; - - template<> - struct dimension : public boost::integral_constant { - }; - - } - -} // \namespace gtsam +#include diff --git a/gtsam/base/LieScalar_Deprecated.h b/gtsam/base/LieScalar_Deprecated.h new file mode 100644 index 000000000..6ffc76d37 --- /dev/null +++ b/gtsam/base/LieScalar_Deprecated.h @@ -0,0 +1,85 @@ +/* ---------------------------------------------------------------------------- + + * 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 LieScalar.h + * @brief A wrapper around scalar providing Lie compatibility + * @author Kai Ni + */ + +#pragma once + +#include +#include + +namespace gtsam { + + /** + * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as + * we can directly add double, Vector, and Matrix into values now, because of + * gtsam::traits. + */ + struct GTSAM_EXPORT LieScalar { + + enum { dimension = 1 }; + + /** default constructor */ + LieScalar() : d_(0.0) {} + + /** wrap a double */ + /*explicit*/ LieScalar(double d) : d_(d) {} + + /** access the underlying value */ + double value() const { return d_; } + + /** Automatic conversion to underlying value */ + operator double() const { return d_; } + + /** convert vector */ + Vector1 vector() const { Vector1 v; v< + struct traits : public internal::ScalarTraits {}; + +} // \namespace gtsam diff --git a/gtsam/base/LieVector.cpp b/gtsam/base/LieVector.cpp index f6cae28e2..3e4eeecf2 100644 --- a/gtsam/base/LieVector.cpp +++ b/gtsam/base/LieVector.cpp @@ -15,8 +15,8 @@ * @author Alex Cunningham */ +#include #include -#include using namespace std; @@ -35,4 +35,6 @@ void LieVector::print(const std::string& name) const { gtsam::print(vector(), name); } +// Does not compile because LieVector is not fixed size. +// GTSAM_CONCEPT_LIE_INST(LieVector) } // \namespace gtsam diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index d1b2ad78c..813431775 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -11,134 +11,16 @@ /** * @file LieVector.h - * @brief A wrapper around vector providing Lie compatibility - * @author Alex Cunningham + * @brief Deprecation warning for LieVector_Deprecated.h, see LieVector_Deprecated.h for details. + * @author Paul Drews */ #pragma once -#include -#include -#include - -namespace gtsam { - -/** - * LieVector is a wrapper around vector to allow it to be a Lie type - */ -struct LieVector : public Vector { - - /** default constructor - should be unnecessary */ - LieVector() {} - - /** initialize from a normal vector */ - LieVector(const Vector& v) : Vector(v) {} - -// Currently TMP constructor causes ICE on MSVS 2013 -#if (_MSC_VER < 1800) - /** initialize from a fixed size normal vector */ - template - LieVector(const Eigen::Matrix& v) : Vector(v) {} +#ifdef _MSC_VER +#pragma message("LieVector.h is deprecated. Please use Eigen::Vector instead.") +#else +#warning "LieVector.h is deprecated. Please use Eigen::Vector instead." #endif - /** wrap a double */ - LieVector(double d) : Vector((Vector(1) << d).finished()) {} - - /** constructor with size and initial data, row order ! */ - GTSAM_EXPORT LieVector(size_t m, const double* const data); - - /** get the underlying vector */ - Vector vector() const { - return static_cast(*this); - } - - /** print @param name optional string naming the object */ - GTSAM_EXPORT void print(const std::string& name="") const; - - /** equality up to tolerance */ - bool equals(const LieVector& expected, double tol=1e-5) const { - return gtsam::equal(vector(), expected.vector(), tol); - } - - // Manifold requirements - - /** Returns dimensionality of the tangent space */ - size_t dim() const { return this->size(); } - - /** Update the LieVector with a tangent space update */ - LieVector retract(const Vector& v) const { return LieVector(vector() + v); } - - /** @return the local coordinates of another object */ - Vector localCoordinates(const LieVector& t2) const { return LieVector(t2 - vector()); } - - // Group requirements - - /** identity - NOTE: no known size at compile time - so zero length */ - static LieVector identity() { - throw std::runtime_error("LieVector::identity(): Don't use this function"); - return LieVector(); - } - - // Note: Manually specifying the 'gtsam' namespace for the optional Matrix arguments - // This is a work-around for linux g++ 4.6.1 that incorrectly selects the Eigen::Matrix class - // instead of the gtsam::Matrix class. This is related to deriving this class from an Eigen Vector - // as the other geometry objects (Point3, Rot3, etc.) have this problem - /** compose with another object */ - LieVector compose(const LieVector& p, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = eye(dim()); - if(H2) *H2 = eye(p.dim()); - - return LieVector(vector() + p); - } - - /** between operation */ - LieVector between(const LieVector& l2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(dim()); - if(H2) *H2 = eye(l2.dim()); - return LieVector(l2.vector() - vector()); - } - - /** invert the object and yield a new one */ - LieVector inverse(boost::optional H=boost::none) const { - if(H) *H = -eye(dim()); - - return LieVector(-1.0 * vector()); - } - - // Lie functions - - /** Expmap around identity */ - static LieVector Expmap(const Vector& v) { return LieVector(v); } - - /** Logmap around identity - just returns with default cast back */ - static Vector Logmap(const LieVector& p) { return p; } - -private: - - // Serialization function - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Vector", - boost::serialization::base_object(*this)); - } -}; - -// Define GTSAM traits -namespace traits { - -template<> -struct is_manifold : public boost::true_type { -}; - -template<> -struct dimension : public Dynamic { -}; - -} - -} // \namespace gtsam +#include diff --git a/gtsam/base/LieVector_Deprecated.h b/gtsam/base/LieVector_Deprecated.h new file mode 100644 index 000000000..67c42c748 --- /dev/null +++ b/gtsam/base/LieVector_Deprecated.h @@ -0,0 +1,116 @@ +/* ---------------------------------------------------------------------------- + + * 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 LieVector.h + * @brief A wrapper around vector providing Lie compatibility + * @author Alex Cunningham + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * @deprecated: LieVector, LieVector and LieMatrix are obsolete in GTSAM 4.0 as + * we can directly add double, Vector, and Matrix into values now, because of + * gtsam::traits. + */ +struct LieVector : public Vector { + + enum { dimension = Eigen::Dynamic }; + + /** default constructor - should be unnecessary */ + LieVector() {} + + /** initialize from a normal vector */ + LieVector(const Vector& v) : Vector(v) {} + + template + LieVector(const V& v) : Vector(v) {} + +// Currently TMP constructor causes ICE on MSVS 2013 +#if (_MSC_VER < 1800) + /** initialize from a fixed size normal vector */ + template + LieVector(const Eigen::Matrix& v) : Vector(v) {} +#endif + + /** wrap a double */ + LieVector(double d) : Vector((Vector(1) << d).finished()) {} + + /** constructor with size and initial data, row order ! */ + GTSAM_EXPORT LieVector(size_t m, const double* const data); + + /// @name Testable + /// @{ + GTSAM_EXPORT void print(const std::string& name="") const; + bool equals(const LieVector& expected, double tol=1e-5) const { + return gtsam::equal(vector(), expected.vector(), tol); + } + /// @} + + /// @name Group + /// @{ + LieVector compose(const LieVector& q) { return (*this)+q;} + LieVector between(const LieVector& q) { return q-(*this);} + LieVector inverse() { return -(*this);} + /// @} + + /// @name Manifold + /// @{ + Vector localCoordinates(const LieVector& q) { return between(q).vector();} + LieVector retract(const Vector& v) {return compose(LieVector(v));} + /// @} + + /// @name Lie Group + /// @{ + static Vector Logmap(const LieVector& p) {return p.vector();} + static LieVector Expmap(const Vector& v) { return LieVector(v);} + /// @} + + /// @name VectorSpace requirements + /// @{ + + /** get the underlying vector */ + Vector vector() const { + return static_cast(*this); + } + + /** Returns dimensionality of the tangent space */ + size_t dim() const { return this->size(); } + + /** identity - NOTE: no known size at compile time - so zero length */ + static LieVector identity() { + throw std::runtime_error("LieVector::identity(): Don't use this function"); + return LieVector(); + } + + /// @} + +private: + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("Vector", + boost::serialization::base_object(*this)); + } +}; + + +template<> +struct traits : public internal::VectorSpace {}; + +} // \namespace gtsam diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 0839e8fb5..d7ea9ea4c 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -14,17 +14,24 @@ * @brief Base class and basic functions for Manifold types * @author Alex Cunningham * @author Frank Dellaert + * @author Mike Bosse */ #pragma once #include -#include -#include -#include +#include +#include + +#include +#include +#include namespace gtsam { +/// tag to assert a type is a manifold +struct manifold_tag {}; + /** * A manifold defines a space in which there is a notion of a linear tangent space * that can be centered around a given point on the manifold. These nonlinear @@ -39,308 +46,184 @@ namespace gtsam { * There may be multiple possible retractions for a given manifold, which can be chosen * between depending on the computational complexity. The important criteria for * the creation for the retract and localCoordinates functions is that they be - * inverse operations. The new notion of a Chart guarantees that. + * inverse operations. * */ -// Traits, same style as Boost.TypeTraits -// All meta-functions below ever only declare a single type -// or a type/value/value_type -namespace traits { +template struct traits; -// is group, by default this is false -template -struct is_group: public boost::false_type { -}; +namespace internal { -// identity, no default provided, by default given by default constructor -template -struct identity { - static T value() { - return T(); +/// Requirements on type to pass it to Manifold template below +template +struct HasManifoldPrereqs { + + enum { dim = Class::dimension }; + + Class p, q; + Eigen::Matrix v; + OptionalJacobian Hp, Hq, Hv; + + BOOST_CONCEPT_USAGE(HasManifoldPrereqs) { + v = p.localCoordinates(q); + q = p.retract(v); } }; -// is manifold, by default this is false -template -struct is_manifold: public boost::false_type { -}; - -// dimension, can return Eigen::Dynamic (-1) if not known at compile time -// defaults to dynamic, TODO makes sense ? -typedef boost::integral_constant Dynamic; -template -struct dimension: public Dynamic { -}; - - -/** - * zero::value is intended to be the origin of a canonical coordinate system - * with canonical(t) == DefaultChart::local(zero::value, t) - * Below we provide the group identity as zero *in case* it is a group - */ -template struct zero: public identity { - BOOST_STATIC_ASSERT(is_group::value); -}; - -// double - -template<> -struct is_group : public boost::true_type { -}; - -template<> -struct is_manifold : public boost::true_type { -}; - -template<> -struct dimension : public boost::integral_constant { -}; - -template<> -struct zero { - static double value() { - return 0; +/// Extra manifold traits for fixed-dimension types +template +struct ManifoldImpl { + // Compile-time dimensionality + static int GetDimension(const Class&) { + return N; } }; -// Fixed size Eigen::Matrix type - -template -struct is_group > : public boost::true_type { -}; - -template -struct is_manifold > : public boost::true_type{ -}; - -template -struct dimension > : public boost::integral_constant { - //TODO after switch to c++11 : the above should should be extracted to a constexpr function - // for readability and to reduce code duplication -}; - -template -struct zero > { - BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), - "traits::zero is only supported for fixed-size matrices"); - static Eigen::Matrix value() { - return Eigen::Matrix::Zero(); +/// Extra manifold traits for variable-dimension types +template +struct ManifoldImpl { + // Run-time dimensionality + static int GetDimension(const Class& m) { + return m.dim(); } }; -template -struct identity > : public zero > { -}; +/// A helper that implements the traits interface for GTSAM manifolds. +/// To use this for your class type, define: +/// template<> struct traits : public internal::ManifoldTraits { }; +template +struct ManifoldTraits: ManifoldImpl { + // Check that Class has the necessary machinery + BOOST_CONCEPT_ASSERT((HasManifoldPrereqs)); -template struct is_chart: public boost::false_type { -}; + // Dimension of the manifold + enum { dimension = Class::dimension }; -} // \ namespace traits + // Typedefs required by all manifold types. + typedef Class ManifoldType; + typedef manifold_tag structure_category; + typedef Eigen::Matrix TangentVector; -// Chart is a map from T -> vector, retract is its inverse -template -struct DefaultChart { - //BOOST_STATIC_ASSERT(traits::is_manifold::value); - typedef T type; - typedef Eigen::Matrix::value, 1> vector; - - static vector local(const T& origin, const T& other) { + // Local coordinates + static TangentVector Local(const Class& origin, const Class& other) { return origin.localCoordinates(other); } - static T retract(const T& origin, const vector& d) { - return origin.retract(d); - } - static int getDimension(const T& origin) { - return origin.dim(); + + // Retraction back to manifold + static Class Retract(const Class& origin, const TangentVector& v) { + return origin.retract(v); } }; -namespace traits { -// populate default traits -template struct is_chart > : public boost::true_type { -}; -template struct dimension > : public dimension { -}; +/// Both ManifoldTraits and Testable +template struct Manifold: ManifoldTraits, Testable {}; + +} // \ namespace internal + +/// Check invariants for Manifold type +template +BOOST_CONCEPT_REQUIRES(((IsTestable)),(bool)) // +check_manifold_invariants(const T& a, const T& b, double tol=1e-9) { + typename traits::TangentVector v0 = traits::Local(a,a); + typename traits::TangentVector v = traits::Local(a,b); + T c = traits::Retract(a,v); + return v0.norm() < tol && traits::Equals(b,c,tol); } -template -struct ChartConcept { +/// Manifold concept +template +class IsManifold { + public: - typedef typename C::type type; - typedef typename C::vector vector; - BOOST_CONCEPT_USAGE(ChartConcept) { - // is_chart trait should be true - BOOST_STATIC_ASSERT((traits::is_chart::value)); + typedef typename traits::structure_category structure_category_tag; + static const int dim = traits::dimension; + typedef typename traits::ManifoldType ManifoldType; + typedef typename traits::TangentVector TangentVector; - /** - * Returns Retraction update of val_ - */ - type retract_ret = C::retract(val_, vec_); + BOOST_CONCEPT_USAGE(IsManifold) { + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "This type's structure_category trait does not assert it as a manifold (or derived)"); + BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); - /** - * Returns local coordinates of another object - */ - vec_ = C::local(val_, retract_ret); - - // a way to get the dimension that is compatible with dynamically sized types - dim_ = C::getDimension(val_); + // make sure Chart methods are defined + v = traits::Local(p, q); + q = traits::Retract(p, v); } private: - type val_; - vector vec_; - int dim_; + + TangentVector v; + ManifoldType p, q; }; -/** - * CanonicalChart > is a chart around zero::value - * Canonical is CanonicalChart > - * An example is Canonical - */ -template struct CanonicalChart { - BOOST_CONCEPT_ASSERT((ChartConcept)); - - typedef C Chart; - typedef typename Chart::type type; - typedef typename Chart::vector vector; - - // Convert t of type T into canonical coordinates - vector local(const type& t) { - return Chart::local(traits::zero::value(), t); - } - // Convert back from canonical coordinates to T - type retract(const vector& v) { - return Chart::retract(traits::zero::value(), v); - } -}; -template struct Canonical: public CanonicalChart > { +/// Give fixed size dimension of a type, fails at compile time if dynamic +template +struct FixedDimension { + typedef const int value_type; + static const int value = traits::dimension; + BOOST_STATIC_ASSERT_MSG(value != Eigen::Dynamic, + "FixedDimension instantiated for dymanically-sized type."); }; -// double +/// Helper class to construct the product manifold of two other manifolds, M1 and M2 +/// Assumes nothing except manifold structure from M1 and M2 +template +class ProductManifold: public std::pair { + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsManifold)); -template<> -struct DefaultChart { - typedef double type; - typedef Eigen::Matrix vector; +protected: + enum { dimension1 = traits::dimension }; + enum { dimension2 = traits::dimension }; - static vector local(double origin, double other) { - vector d; - d << other - origin; - return d; +public: + enum { dimension = dimension1 + dimension2 }; + inline static size_t Dim() { return dimension;} + inline size_t dim() const { return dimension;} + + typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian ChartJacobian; + + /// Default constructor yields identity + ProductManifold():std::pair(traits::Identity(),traits::Identity()) {} + + // Construct from two original manifold values + ProductManifold(const M1& m1, const M2& m2):std::pair(m1,m2) {} + + /// Retract delta to manifold + ProductManifold retract(const TangentVector& xi) const { + M1 m1 = traits::Retract(this->first, xi.template head()); + M2 m2 = traits::Retract(this->second, xi.template tail()); + return ProductManifold(m1,m2); } - static double retract(double origin, const vector& d) { - return origin + d[0]; - } - static const int getDimension(double) { - return 1; + + /// Compute the coordinates in the tangent space + TangentVector localCoordinates(const ProductManifold& other) const { + typename traits::TangentVector v1 = traits::Local(this->first, other.first); + typename traits::TangentVector v2 = traits::Local(this->second, other.second); + TangentVector v; + v << v1, v2; + return v; } }; -// Fixed size Eigen::Matrix type - -template -struct DefaultChart > { - /** - * This chart for the vector space of M x N matrices (represented by Eigen matrices) chooses as basis the one with respect to which the coordinates are exactly the matrix entries as laid out in memory (as determined by Options). - * Computing coordinates for a matrix is then simply a reshape to the row vector of appropriate size. - */ - typedef Eigen::Matrix type; - typedef type T; - typedef Eigen::Matrix::value, 1> vector;BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), - "DefaultChart has not been implemented yet for dynamically sized matrices"); - static vector local(const T& origin, const T& other) { - return reshape(other) - reshape(origin); - } - static T retract(const T& origin, const vector& d) { - return origin + reshape(d); - } - static int getDimension(const T&origin) { - return origin.rows() * origin.cols(); - } -}; - -// Dynamically sized Vector -template<> -struct DefaultChart { - typedef Vector T; - typedef T type; - typedef T vector; - static vector local(const T& origin, const T& other) { - return other - origin; - } - static T retract(const T& origin, const vector& d) { - return origin + d; - } - static int getDimension(const T& origin) { - return origin.size(); - } -}; - -/** - * Old Concept check class for Manifold types - * Requires a mapping between a linear tangent space and the underlying - * manifold, of which Lie is a specialization. - * - * The necessary functions to implement for Manifold are defined - * below with additional details as to the interface. The - * concept checking function in class Manifold will check whether or not - * the function exists and throw compile-time errors. - * - * Returns dimensionality of the tangent space, which may be smaller than the number - * of nonlinear parameters. - * size_t dim() const; - * - * Returns a new T that is a result of updating *this with the delta v after pulling - * the updated value back to the manifold T. - * T retract(const Vector& v) const; - * - * Returns the linear coordinates of lp in the tangent space centered around *this. - * Vector localCoordinates(const T& lp) const; - * - * By convention, we use capital letters to designate a static function - * @tparam T is a Lie type, like Point2, Pose3, etc. - */ -template -class ManifoldConcept { -private: - /** concept checking function - implement the functions this demands */ - static T concept_check(const T& t) { - - /** assignment */ - T t2 = t; - - /** - * Returns dimensionality of the tangent space - */ - size_t dim_ret = t.dim(); - - /** - * Returns Retraction update of T - */ - T retract_ret = t.retract(gtsam::zero(dim_ret)); - - /** - * Returns local coordinates of another object - */ - Vector localCoords_ret = t.localCoordinates(t2); - - return retract_ret; - } +// Define any direct product group to be a model of the multiplicative Group concept +template +struct traits > : internal::Manifold > { }; } // \ namespace gtsam -/** - * Macros for using the ManifoldConcept - * - An instantiation for use inside unit tests - * - A typedef for use inside generic algorithms - * - * NOTE: intentionally not in the gtsam namespace to allow for classes not in - * the gtsam namespace to be more easily enforced as testable - */ -#define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::ManifoldConcept; -#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; +///** +// * Macros for using the ManifoldConcept +// * - An instantiation for use inside unit tests +// * - A typedef for use inside generic algorithms +// * +// * NOTE: intentionally not in the gtsam namespace to allow for classes not in +// * the gtsam namespace to be more easily enforced as testable +// */ +#define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::IsManifold; +#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::IsManifold _gtsam_IsManifold_##T; diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 1469e265d..9e56dfb6c 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -20,6 +20,8 @@ #include #include #include +#include +#include #include #include @@ -180,6 +182,7 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVec } /* ************************************************************************* */ +//3 argument call void print(const Matrix& A, const string &s, ostream& stream) { size_t m = A.rows(), n = A.cols(); @@ -198,6 +201,12 @@ void print(const Matrix& A, const string &s, ostream& stream) { stream << "];" << endl; } +/* ************************************************************************* */ +//1 or 2 argument call +void print(const Matrix& A, const string &s){ + print(A, s, cout); +} + /* ************************************************************************* */ void save(const Matrix& A, const string &s, const string& filename) { fstream stream(filename.c_str(), fstream::out | fstream::app); @@ -238,11 +247,6 @@ istream& operator>>(istream& inputStream, Matrix& destinationMatrix) { return inputStream; } -/* ************************************************************************* */ -void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j) { - fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix; -} - /* ************************************************************************* */ Matrix diag(const std::vector& Hs) { size_t rows = 0, cols = 0; @@ -665,19 +669,6 @@ Matrix expm(const Matrix& A, size_t K) { return E; } -/* ************************************************************************* */ -Matrix Cayley(const Matrix& A) { - Matrix::Index n = A.cols(); - assert(A.rows() == n); - - // original -// const Matrix I = eye(n); -// return (I-A)*inverse(I+A); - - // inlined to let Eigen do more optimization - return (Matrix::Identity(n, n) - A)*(Matrix::Identity(n, n) + A).inverse(); -} - /* ************************************************************************* */ std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal) { @@ -715,6 +706,26 @@ std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, return ss.str(); } +/* ************************************************************************* */ +void inplace_QR(Matrix& A){ + size_t rows = A.rows(); + size_t cols = A.cols(); + size_t size = std::min(rows,cols); + typedef Eigen::internal::plain_diag_type::type HCoeffsType; + typedef Eigen::internal::plain_row_type::type RowVectorType; + HCoeffsType hCoeffs(size); + RowVectorType temp(cols); + +#ifdef GTSAM_USE_SYSTEM_EIGEN + // System-Eigen is used, and MKL is off + Eigen::internal::householder_qr_inplace_blocked(A, hCoeffs, 48, temp.data()); +#else + // Patched Eigen is used, and MKL is either on or off + Eigen::internal::householder_qr_inplace_blocked::run(A, hCoeffs, 48, temp.data()); +#endif + + zeroBelowDiagonal(A); +} } // namespace gtsam diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 132bf79ad..caee2071c 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -21,11 +21,14 @@ // \callgraph #pragma once - #include +#include +#include +#include +#include #include #include -#include + /** * Matrix is a typedef in the gtsam namespace @@ -37,27 +40,31 @@ namespace gtsam { typedef Eigen::MatrixXd Matrix; typedef Eigen::Matrix MatrixRowMajor; -typedef Eigen::Matrix2d Matrix2; -typedef Eigen::Matrix3d Matrix3; -typedef Eigen::Matrix4d Matrix4; -typedef Eigen::Matrix Matrix5; -typedef Eigen::Matrix Matrix6; +// Create handy typedefs and constants for square-size matrices +// MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9 +#define GTSAM_MAKE_TYPEDEFS(SIZE, SUFFIX) \ +typedef Eigen::Matrix Matrix##SUFFIX; \ +typedef Eigen::Matrix Matrix1##SUFFIX; \ +typedef Eigen::Matrix Matrix2##SUFFIX; \ +typedef Eigen::Matrix Matrix3##SUFFIX; \ +typedef Eigen::Matrix Matrix4##SUFFIX; \ +typedef Eigen::Matrix Matrix5##SUFFIX; \ +typedef Eigen::Matrix Matrix6##SUFFIX; \ +typedef Eigen::Matrix Matrix7##SUFFIX; \ +typedef Eigen::Matrix Matrix8##SUFFIX; \ +typedef Eigen::Matrix Matrix9##SUFFIX; \ +static const Eigen::MatrixBase::IdentityReturnType I_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Identity(); \ +static const Eigen::MatrixBase::ConstantReturnType Z_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Zero(); -typedef Eigen::Matrix Matrix23; -typedef Eigen::Matrix Matrix24; -typedef Eigen::Matrix Matrix25; -typedef Eigen::Matrix Matrix26; -typedef Eigen::Matrix Matrix27; -typedef Eigen::Matrix Matrix28; -typedef Eigen::Matrix Matrix29; - -typedef Eigen::Matrix Matrix32; -typedef Eigen::Matrix Matrix34; -typedef Eigen::Matrix Matrix35; -typedef Eigen::Matrix Matrix36; -typedef Eigen::Matrix Matrix37; -typedef Eigen::Matrix Matrix38; -typedef Eigen::Matrix Matrix39; +GTSAM_MAKE_TYPEDEFS(1,1); +GTSAM_MAKE_TYPEDEFS(2,2); +GTSAM_MAKE_TYPEDEFS(3,3); +GTSAM_MAKE_TYPEDEFS(4,4); +GTSAM_MAKE_TYPEDEFS(5,5); +GTSAM_MAKE_TYPEDEFS(6,6); +GTSAM_MAKE_TYPEDEFS(7,7); +GTSAM_MAKE_TYPEDEFS(8,8); +GTSAM_MAKE_TYPEDEFS(9,9); // Matrix expressions for accessing parts of matrices typedef Eigen::Block SubMatrix; @@ -194,9 +201,14 @@ inline MATRIX prod(const MATRIX& A, const MATRIX&B) { } /** - * print a matrix + * print without optional string, must specify cout yourself */ -GTSAM_EXPORT void print(const Matrix& A, const std::string& s = "", std::ostream& stream = std::cout); +GTSAM_EXPORT void print(const Matrix& A, const std::string& s, std::ostream& stream); + +/** + * print with optional string to cout + */ +GTSAM_EXPORT void print(const Matrix& A, const std::string& s = ""); /** * save a matrix to file, which can be loaded by matlab @@ -233,7 +245,10 @@ Eigen::Block sub(const MATRIX& A, size_t i1, size_t i2, size_t j1, * @param i is the row of the upper left corner insert location * @param j is the column of the upper left corner insert location */ -GTSAM_EXPORT void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j); +template +void insertSub(Eigen::MatrixBase& fullMatrix, const Eigen::MatrixBase& subMatrix, size_t i, size_t j) { + fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix; +} /** * Create a matrix with submatrices along its diagonal @@ -360,21 +375,7 @@ GTSAM_EXPORT std::pair qr(const Matrix& A); * @param A is the input matrix, and is the output * @param clear_below_diagonal enables zeroing out below diagonal */ -template -void inplace_QR(MATRIX& A) { - size_t rows = A.rows(); - size_t cols = A.cols(); - size_t size = std::min(rows,cols); - - typedef Eigen::internal::plain_diag_type::type HCoeffsType; - typedef Eigen::internal::plain_row_type::type RowVectorType; - HCoeffsType hCoeffs(size); - RowVectorType temp(cols); - - Eigen::internal::householder_qr_inplace_blocked::run(A, hCoeffs, 48, temp.data()); - - zeroBelowDiagonal(A); -} +void inplace_QR(Matrix& A); /** * Imperative algorithm for in-place full elimination with @@ -521,17 +522,6 @@ DLT(const Matrix& A, double rank_tol = 1e-9); */ GTSAM_EXPORT Matrix expm(const Matrix& A, size_t K=7); -/// Cayley transform -GTSAM_EXPORT Matrix Cayley(const Matrix& A); - -/// Implementation of Cayley transform using fixed size matrices to let -/// Eigen do more optimization -template -Eigen::Matrix CayleyFixed(const Eigen::Matrix& A) { - typedef Eigen::Matrix FMat; - return (FMat::Identity() - A)*(FMat::Identity() + A).inverse(); -} - std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false); } // namespace gtsam @@ -545,7 +535,7 @@ namespace boost { // split version - sends sizes ahead template - void save(Archive & ar, const gtsam::Matrix & m, unsigned int version) { + void save(Archive & ar, const gtsam::Matrix & m, unsigned int /*version*/) { const size_t rows = m.rows(), cols = m.cols(); ar << BOOST_SERIALIZATION_NVP(rows); ar << BOOST_SERIALIZATION_NVP(cols); @@ -553,7 +543,7 @@ namespace boost { } template - void load(Archive & ar, gtsam::Matrix & m, unsigned int version) { + void load(Archive & ar, gtsam::Matrix & m, unsigned int /*version*/) { size_t rows, cols; ar >> BOOST_SERIALIZATION_NVP(rows); ar >> BOOST_SERIALIZATION_NVP(cols); @@ -564,4 +554,5 @@ namespace boost { } // namespace serialization } // namespace boost -BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Matrix) +BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Matrix); + diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h new file mode 100644 index 000000000..7055a287a --- /dev/null +++ b/gtsam/base/OptionalJacobian.h @@ -0,0 +1,187 @@ +/* ---------------------------------------------------------------------------- + + * 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 OptionalJacobian.h + * @brief Special class for optional Jacobian arguments + * @author Frank Dellaert + * @author Natesh Srinivasan + * @date Nov 28, 2014 + */ + +#pragma once + +#include + +#ifndef OPTIONALJACOBIAN_NOBOOST +#include +#endif + +namespace gtsam { + +/** + * OptionalJacobian is an Eigen::Ref like class that can take be constructed using + * either a fixed size or dynamic Eigen matrix. In the latter case, the dynamic + * matrix will be resized. Finally, there is a constructor that takes + * boost::none, the default constructor acts like boost::none, and + * boost::optional is also supported for backwards compatibility. + * Below this class, a dynamic version is also implemented. + */ +template +class OptionalJacobian { + +public: + + /// ::Jacobian size type + typedef Eigen::Matrix Jacobian; + +private: + + Eigen::Map map_; /// View on constructor argument, if given + + // Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html + // uses "placement new" to make map_ usurp the memory of the fixed size matrix + void usurp(double* data) { + new (&map_) Eigen::Map(data); + } + +public: + + /// Default constructor acts like boost::none + OptionalJacobian() : + map_(NULL) { + } + + /// Constructor that will usurp data of a fixed-size matrix + OptionalJacobian(Jacobian& fixed) : + map_(NULL) { + usurp(fixed.data()); + } + + /// Constructor that will usurp data of a fixed-size matrix, pointer version + OptionalJacobian(Jacobian* fixedPtr) : + map_(NULL) { + if (fixedPtr) + usurp(fixedPtr->data()); + } + + /// Constructor that will resize a dynamic matrix (unless already correct) + OptionalJacobian(Eigen::MatrixXd& dynamic) : + map_(NULL) { + dynamic.resize(Rows, Cols); // no malloc if correct size + usurp(dynamic.data()); + } + +#ifndef OPTIONALJACOBIAN_NOBOOST + + /// Constructor with boost::none just makes empty + OptionalJacobian(boost::none_t /*none*/) : + map_(NULL) { + } + + /// Constructor compatible with old-style derivatives + OptionalJacobian(const boost::optional optional) : + map_(NULL) { + if (optional) { + optional->resize(Rows, Cols); + usurp(optional->data()); + } + } + +#endif + + /// Return true is allocated, false if default constructor was used + operator bool() const { + return map_.data() != NULL; + } + + /// De-reference, like boost optional + Eigen::Map& operator*() { + return map_; + } + + /// TODO: operator->() + Eigen::Map* operator->(){ return &map_; } +}; + +// The pure dynamic specialization of this is needed to support +// variable-sized types. Note that this is designed to work like the +// boost optional scheme from GTSAM 3. +template<> +class OptionalJacobian { + +public: + + /// Jacobian size type + typedef Eigen::MatrixXd Jacobian; + +private: + + Jacobian* pointer_; /// View on constructor argument, if given + +public: + + /// Default constructor acts like boost::none + OptionalJacobian() : + pointer_(NULL) { + } + + /// Constructor that will resize a dynamic matrix (unless already correct) + OptionalJacobian(Eigen::MatrixXd& dynamic) : + pointer_(&dynamic) { + } + +#ifndef OPTIONALJACOBIAN_NOBOOST + + /// Constructor with boost::none just makes empty + OptionalJacobian(boost::none_t /*none*/) : + pointer_(NULL) { + } + + /// Constructor compatible with old-style derivatives + OptionalJacobian(const boost::optional optional) : + pointer_(NULL) { + if (optional) pointer_ = &(*optional); + } + +#endif + + /// Return true is allocated, false if default constructor was used + operator bool() const { + return pointer_!=NULL; + } + + /// De-reference, like boost optional + Jacobian& operator*() { + return *pointer_; + } + + /// TODO: operator->() + Jacobian* operator->(){ return pointer_; } +}; + +// forward declare +template struct traits; + +/** + * @brief: meta-function to generate JacobianTA optional reference + * Used mainly by Expressions + * @param T return type + * @param A argument type + */ +template +struct MakeOptionalJacobian { + typedef OptionalJacobian::dimension, + traits::dimension> type; +}; + +} // namespace gtsam + diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h new file mode 100644 index 000000000..90aeb54d1 --- /dev/null +++ b/gtsam/base/ProductLieGroup.h @@ -0,0 +1,197 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------1------------------------------------------- */ + +/** + * @file ProductLieGroup.h + * @date May, 2015 + * @author Frank Dellaert + * @brief Group product of two Lie Groups + */ + +#pragma once + +#include +#include // pair + +namespace gtsam { + +/// Template to construct the product Lie group of two other Lie groups +/// Assumes Lie group structure for G and H +template +class ProductLieGroup: public std::pair { + BOOST_CONCEPT_ASSERT((IsLieGroup)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); + typedef std::pair Base; + +protected: + enum {dimension1 = traits::dimension}; + enum {dimension2 = traits::dimension}; + +public: + /// Default constructor yields identity + ProductLieGroup():Base(traits::Identity(),traits::Identity()) {} + + // Construct from two subgroup elements + ProductLieGroup(const G& g, const H& h):Base(g,h) {} + + // Construct from base + ProductLieGroup(const Base& base):Base(base) {} + + /// @name Group + /// @{ + typedef multiplicative_group_tag group_flavor; + static ProductLieGroup identity() {return ProductLieGroup();} + + ProductLieGroup operator*(const ProductLieGroup& other) const { + return ProductLieGroup(traits::Compose(this->first,other.first), + traits::Compose(this->second,other.second)); + } + ProductLieGroup inverse() const { + return ProductLieGroup(this->first.inverse(), this->second.inverse()); + } + ProductLieGroup compose(const ProductLieGroup& g) const { + return (*this) * g; + } + ProductLieGroup between(const ProductLieGroup& g) const { + return this->inverse() * g; + } + /// @} + + /// @name Manifold + /// @{ + enum {dimension = dimension1 + dimension2}; + inline static size_t Dim() {return dimension;} + inline size_t dim() const {return dimension;} + + typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian ChartJacobian; + + static ProductLieGroup Retract(const TangentVector& v) { + return ProductLieGroup::ChartAtOrigin::Retract(v); + } + static TangentVector LocalCoordinates(const ProductLieGroup& g) { + return ProductLieGroup::ChartAtOrigin::Local(g); + } + ProductLieGroup retract(const TangentVector& v) const { + return compose(ProductLieGroup::ChartAtOrigin::Retract(v)); + } + TangentVector localCoordinates(const ProductLieGroup& g) const { + return ProductLieGroup::ChartAtOrigin::Local(between(g)); + } + /// @} + + /// @name Lie Group + /// @{ +protected: + typedef Eigen::Matrix Jacobian; + typedef Eigen::Matrix Jacobian1; + typedef Eigen::Matrix Jacobian2; + +public: + ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1, + ChartJacobian H2 = boost::none) const { + Jacobian1 D_g_first; Jacobian2 D_h_second; + G g = traits::Compose(this->first,other.first, H1 ? &D_g_first : 0); + H h = traits::Compose(this->second,other.second, H1 ? &D_h_second : 0); + if (H1) { + H1->setZero(); + H1->template topLeftCorner() = D_g_first; + H1->template bottomRightCorner() = D_h_second; + } + if (H2) *H2 = Jacobian::Identity(); + return ProductLieGroup(g,h); + } + ProductLieGroup between(const ProductLieGroup& other, ChartJacobian H1, + ChartJacobian H2 = boost::none) const { + Jacobian1 D_g_first; Jacobian2 D_h_second; + G g = traits::Between(this->first,other.first, H1 ? &D_g_first : 0); + H h = traits::Between(this->second,other.second, H1 ? &D_h_second : 0); + if (H1) { + H1->setZero(); + H1->template topLeftCorner() = D_g_first; + H1->template bottomRightCorner() = D_h_second; + } + if (H2) *H2 = Jacobian::Identity(); + return ProductLieGroup(g,h); + } + ProductLieGroup inverse(ChartJacobian D) const { + Jacobian1 D_g_first; Jacobian2 D_h_second; + G g = traits::Inverse(this->first, D ? &D_g_first : 0); + H h = traits::Inverse(this->second, D ? &D_h_second : 0); + if (D) { + D->setZero(); + D->template topLeftCorner() = D_g_first; + D->template bottomRightCorner() = D_h_second; + } + return ProductLieGroup(g,h); + } + static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + if (Hv) throw std::runtime_error("ProductLieGroup::Expmap derivatives not implemented yet"); + G g = traits::Expmap(v.template head()); + H h = traits::Expmap(v.template tail()); + return ProductLieGroup(g,h); + } + static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) { + if (Hp) throw std::runtime_error("ProductLieGroup::Logmap derivatives not implemented yet"); + typename traits::TangentVector v1 = traits::Logmap(p.first); + typename traits::TangentVector v2 = traits::Logmap(p.second); + TangentVector v; + v << v1, v2; + return v; + } + struct ChartAtOrigin { + static TangentVector Local(const ProductLieGroup& m, ChartJacobian Hm = boost::none) { + return Logmap(m, Hm); + } + static ProductLieGroup Retract(const TangentVector& v, ChartJacobian Hv = boost::none) { + return Expmap(v, Hv); + } + }; + ProductLieGroup expmap(const TangentVector& v) const { + return compose(ProductLieGroup::Expmap(v)); + } + TangentVector logmap(const ProductLieGroup& g) const { + return ProductLieGroup::Logmap(between(g)); + } + static ProductLieGroup Retract(const TangentVector& v, ChartJacobian H1) { + return ProductLieGroup::ChartAtOrigin::Retract(v,H1); + } + static TangentVector LocalCoordinates(const ProductLieGroup& g, ChartJacobian H1) { + return ProductLieGroup::ChartAtOrigin::Local(g,H1); + } + ProductLieGroup retract(const TangentVector& v, // + ChartJacobian H1, ChartJacobian H2 = boost::none) const { + Jacobian D_g_v; + ProductLieGroup g = ProductLieGroup::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0); + ProductLieGroup h = compose(g,H1,H2); + if (H2) *H2 = (*H2) * D_g_v; + return h; + } + TangentVector localCoordinates(const ProductLieGroup& g, // + ChartJacobian H1, ChartJacobian H2 = boost::none) const { + ProductLieGroup h = between(g,H1,H2); + Jacobian D_v_h; + TangentVector v = ProductLieGroup::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0); + if (H1) *H1 = D_v_h * (*H1); + if (H2) *H2 = D_v_h * (*H2); + return v; + } + /// @} + +}; + +// Define any direct product group to be a model of the multiplicative Group concept +template +struct traits > : internal::LieGroupTraits< + ProductLieGroup > { +}; +} // namespace gtsam + diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 5dcc79a68..41584c7f9 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -199,6 +199,7 @@ namespace gtsam { void checkBlock(DenseIndex block) const { + static_cast(block); //Disable unused varibale warnings. assert(matrix_.rows() == matrix_.cols()); assert(matrix_.cols() == variableColOffsets_.back()); assert(block >= 0); @@ -235,7 +236,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { // Fill in the lower triangle part of the matrix, so boost::serialization won't // complain about uninitialized data with an input_stream_error exception // http://www.boost.org/doc/libs/1_37_0/libs/serialization/doc/exceptions.html#stream_error diff --git a/gtsam/base/SymmetricBlockMatrixBlockExpr.h b/gtsam/base/SymmetricBlockMatrixBlockExpr.h index 6a65b2748..dd999ae6c 100644 --- a/gtsam/base/SymmetricBlockMatrixBlockExpr.h +++ b/gtsam/base/SymmetricBlockMatrixBlockExpr.h @@ -93,7 +93,7 @@ namespace gtsam /// Create a SymmetricBlockMatrixBlockExpr from the specified range of blocks of a /// SymmetricBlockMatrix. - SymmetricBlockMatrixBlockExpr(SymmetricBlockMatrixType& blockMatrix, Index firstBlock, Index blocks, char dummy) : + SymmetricBlockMatrixBlockExpr(SymmetricBlockMatrixType& blockMatrix, Index firstBlock, Index blocks, char /*dummy*/) : xpr_(blockMatrix), myBlock_(blockMatrix.matrix_.block(0, 0, 0, 0)) { initIndices(firstBlock, firstBlock, blocks, blocks); diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index a308c50a1..4790530ab 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -34,6 +34,7 @@ #pragma once #include +#include #include #include @@ -41,45 +42,52 @@ namespace gtsam { + // Forward declaration + template struct traits; + /** * A testable concept check that should be placed in applicable unit * tests and in generic algorithms. * * See macros for details on using this structure * @addtogroup base - * @tparam T is the type this constrains to be testable - assumes print() and equals() + * @tparam T is the objectype this constrains to be testable - assumes print() and equals() */ template - class TestableConcept { - static bool checkTestableConcept(const T& d) { + class IsTestable { + T t; + bool r1,r2; + public: + + BOOST_CONCEPT_USAGE(IsTestable) { // check print function, with optional string - d.print(std::string()); - d.print(); + traits::Print(t, std::string()); + traits::Print(t); // check print, with optional threshold double tol = 1.0; - bool r1 = d.equals(d, tol); - bool r2 = d.equals(d); - return r1 && r2; + r1 = traits::Equals(t,t,tol); + r2 = traits::Equals(t,t); } - }; + }; // \ Testable - /** Call print on the object */ - template - inline void print(const T& object, const std::string& s = "") { - object.print(s); + inline void print(float v, const std::string& s = "") { + printf("%s%f\n",s.c_str(),v); + } + inline void print(double v, const std::string& s = "") { + printf("%s%lf\n",s.c_str(),v); } /** Call equal on the object */ template inline bool equal(const T& obj1, const T& obj2, double tol) { - return obj1.equals(obj2, tol); + return traits::Equals(obj1,obj2, tol); } - /** Call equal on the object without tolerance (use default tolerance) */ + /** Call equal without tolerance (use default tolerance) */ template inline bool equal(const T& obj1, const T& obj2) { - return obj1.equals(obj2); + return traits::Equals(obj1,obj2); } /** @@ -87,11 +95,11 @@ namespace gtsam { */ template bool assert_equal(const V& expected, const V& actual, double tol = 1e-9) { - if (actual.equals(expected, tol)) + if (traits::Equals(actual,expected, tol)) return true; printf("Not equal:\n"); - expected.print("expected:\n"); - actual.print("actual:\n"); + traits::Print(expected,"expected:\n"); + traits::Print(actual,"actual:\n"); return false; } @@ -103,7 +111,7 @@ namespace gtsam { double tol_; equals(double tol = 1e-9) : tol_(tol) {} bool operator()(const V& expected, const V& actual) { - return (actual.equals(expected, tol_)); + return (traits::Equals(actual, expected, tol_)); } }; @@ -116,7 +124,39 @@ namespace gtsam { equals_star(double tol = 1e-9) : tol_(tol) {} bool operator()(const boost::shared_ptr& expected, const boost::shared_ptr& actual) { if (!actual || !expected) return false; - return (actual->equals(*expected, tol_)); + return (traits::Equals(*actual,*expected, tol_)); + } + }; + + /// Requirements on type to pass it to Testable template below + template + struct HasTestablePrereqs { + + BOOST_CONCEPT_USAGE(HasTestablePrereqs) { + t->print(str); + b = t->equals(*s,tol); + } + + T *t, *s; // Pointer is to allow abstract classes + bool b; + double tol; + std::string str; + }; + + /// A helper that implements the traits interface for GTSAM types. + /// To use this for your gtsam type, define: + /// template<> struct traits : public Testable { }; + template + struct Testable { + + // Check that T has the necessary methods + BOOST_CONCEPT_ASSERT((HasTestablePrereqs)); + + static void Print(const T& m, const std::string& str = "") { + m.print(str); + } + static bool Equals(const T& m1, const T& m2, double tol = 1e-8) { + return m1.equals(m2, tol); } }; @@ -129,6 +169,7 @@ namespace gtsam { * * NOTE: intentionally not in the gtsam namespace to allow for classes not in * the gtsam namespace to be more easily enforced as testable + * @deprecated please use BOOST_CONCEPT_ASSERT and */ -#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::TestableConcept; -#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::TestableConcept _gtsam_TestableConcept_##T; +#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::IsTestable; +#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::IsTestable _gtsam_Testable_##T; diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index afc244d6c..9537baa31 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -18,9 +18,10 @@ #pragma once -#include -#include #include +#include +#include + namespace gtsam { @@ -120,7 +121,7 @@ namespace gtsam { virtual Vector localCoordinates_(const Value& value) const = 0; /** Assignment operator */ - virtual Value& operator=(const Value& rhs) { + virtual Value& operator=(const Value& /*rhs*/) { //needs a empty definition so recursion in implicit derived assignment operators work return *this; } @@ -129,9 +130,6 @@ namespace gtsam { template const ValueType& cast() const; - template - const Chart& getChart() const; - /** Virutal destructor */ virtual ~Value() {} @@ -168,7 +166,8 @@ namespace gtsam { * */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) {} + void serialize(ARCHIVE & /*ar*/, const unsigned int /*version*/) { + } }; diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index b872aa08b..ed6373f5b 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -16,20 +16,18 @@ * @author Frank Dellaert */ +#include +#include +#include +#include #include #include -#include #include #include #include #include -#include -#include -#include #include - -#include - +#include using namespace std; @@ -55,6 +53,7 @@ Vector delta(size_t n, size_t i, double value) { } /* ************************************************************************* */ +//3 argument call void print(const Vector& v, const string& s, ostream& stream) { size_t n = v.size(); @@ -65,6 +64,12 @@ void print(const Vector& v, const string& s, ostream& stream) { stream << "];" << endl; } +/* ************************************************************************* */ +//1 or 2 argument call +void print(const Vector& v, const string& s) { + print(v, s, cout); +} + /* ************************************************************************* */ void save(const Vector& v, const string &s, const string& filename) { fstream stream(filename.c_str(), fstream::out | fstream::app); @@ -217,11 +222,7 @@ double norm_2(const Vector& v) { /* ************************************************************************* */ Vector reciprocal(const Vector &a) { - size_t n = a.size(); - Vector b(n); - for( size_t i = 0; i < n; i++ ) - b(i) = 1.0/a(i); - return b; + return a.array().inverse(); } /* ************************************************************************* */ @@ -286,11 +287,15 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights, for (size_t i = 0; i < m; ++i) isZero.push_back(fabs(a[i]) < 1e-9); // If there is a valid (a!=0) constraint (sigma==0) return the first one - for (size_t i = 0; i < m; ++i) + for (size_t i = 0; i < m; ++i) { if (weights[i] == inf && !isZero[i]) { + // Basically, instead of doing a normal QR step with the weighted + // pseudoinverse, we enforce the constraint by turning + // ax + AS = b into x + (A/a)S = b/a, for the first row where a!=0 pseudo = delta(m, i, 1 / a[i]); return inf; } + } // Form psuedo-inverse inv(a'inv(Sigma)a)a'inv(Sigma) // For diagonal Sigma, inv(Sigma) = diag(precisions) diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 1be8408d2..20477a205 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -18,13 +18,17 @@ // \callgraph + #pragma once -#include -#include -#include +#ifndef MKL_BLAS +#define MKL_BLAS MKL_DOMAIN_BLAS +#endif + #include -#include +#include +#include +#include namespace gtsam { @@ -34,6 +38,7 @@ namespace gtsam { typedef Eigen::VectorXd Vector; // Commonly used fixed size vectors +typedef Eigen::Matrix Vector1; typedef Eigen::Vector2d Vector2; typedef Eigen::Vector3d Vector3; typedef Eigen::Matrix Vector4; @@ -42,6 +47,7 @@ typedef Eigen::Matrix Vector6; typedef Eigen::Matrix Vector7; typedef Eigen::Matrix Vector8; typedef Eigen::Matrix Vector9; +typedef Eigen::Matrix Vector10; typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; @@ -95,9 +101,14 @@ GTSAM_EXPORT bool zero(const Vector& v); inline size_t dim(const Vector& v) { return v.size(); } /** - * print with optional string + * print without optional string, must specify cout yourself */ -GTSAM_EXPORT void print(const Vector& v, const std::string& s = "", std::ostream& stream = std::cout); +GTSAM_EXPORT void print(const Vector& v, const std::string& s, std::ostream& stream); + +/** + * print with optional string to cout + */ +GTSAM_EXPORT void print(const Vector& v, const std::string& s = ""); /** * save a vector to file, which can be loaded by matlab @@ -344,14 +355,14 @@ namespace boost { // split version - copies into an STL vector for serialization template - void save(Archive & ar, const gtsam::Vector & v, unsigned int version) { + void save(Archive & ar, const gtsam::Vector & v, unsigned int /*version*/) { const size_t size = v.size(); ar << BOOST_SERIALIZATION_NVP(size); ar << make_nvp("data", make_array(v.data(), v.size())); } template - void load(Archive & ar, gtsam::Vector & v, unsigned int version) { + void load(Archive & ar, gtsam::Vector & v, unsigned int /*version*/) { size_t size; ar >> BOOST_SERIALIZATION_NVP(size); v.resize(size); @@ -360,12 +371,12 @@ namespace boost { // split version - copies into an STL vector for serialization template - void save(Archive & ar, const Eigen::Matrix & v, unsigned int version) { + void save(Archive & ar, const Eigen::Matrix & v, unsigned int /*version*/) { ar << make_nvp("data", make_array(v.data(), v.RowsAtCompileTime)); } template - void load(Archive & ar, Eigen::Matrix & v, unsigned int version) { + void load(Archive & ar, Eigen::Matrix & v, unsigned int /*version*/) { ar >> make_nvp("data", make_array(v.data(), v.RowsAtCompileTime)); } diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h new file mode 100644 index 000000000..c356dcc07 --- /dev/null +++ b/gtsam/base/VectorSpace.h @@ -0,0 +1,468 @@ +/* + * VectorSpace.h + * + * @date December 21, 2014 + * @author Mike Bosse + * @author Frank Dellaert + */ + +#pragma once + +#include + +namespace gtsam { + +/// tag to assert a type is a vector space +struct vector_space_tag: public lie_group_tag { +}; + +template struct traits; + +namespace internal { + +/// VectorSpaceTraits Implementation for Fixed sizes +template +struct VectorSpaceImpl { + + /// @name Manifold + /// @{ + typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian ChartJacobian; + typedef Eigen::Matrix Jacobian; + static int GetDimension(const Class&) { return N;} + + static TangentVector Local(const Class& origin, const Class& other, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = - Jacobian::Identity(); + if (H2) *H2 = Jacobian::Identity(); + Class v = other-origin; + return v.vector(); + } + + static Class Retract(const Class& origin, const TangentVector& v, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = Jacobian::Identity(); + if (H2) *H2 = Jacobian::Identity(); + return origin + (Class)v; + } + + /// @} + + /// @name Lie Group + /// @{ + + static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) { + if (Hm) *Hm = Jacobian::Identity(); + return m.vector(); + } + + static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + if (Hv) *Hv = Jacobian::Identity(); + return Class(v); + } + + static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none, + ChartJacobian H2 = boost::none) { + if (H1) *H1 = Jacobian::Identity(); + if (H2) *H2 = Jacobian::Identity(); + return v1 + v2; + } + + static Class Between(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none, + ChartJacobian H2 = boost::none) { + if (H1) *H1 = - Jacobian::Identity(); + if (H2) *H2 = Jacobian::Identity(); + return v2 - v1; + } + + static Class Inverse(const Class& v, ChartJacobian H = boost::none) { + if (H) *H = - Jacobian::Identity(); + return -v; + } + + /// @} +}; + +/// VectorSpaceTraits implementation for dynamic types. +template +struct VectorSpaceImpl { + + /// @name Group + /// @{ + static Class Compose(const Class& v1, const Class& v2) { return v1+v2;} + static Class Between(const Class& v1, const Class& v2) { return v2-v1;} + static Class Inverse(const Class& m) { return -m;} + /// @} + + /// @name Manifold + /// @{ + typedef Eigen::VectorXd TangentVector; + typedef OptionalJacobian ChartJacobian; + static int GetDimension(const Class& m) { return m.dim();} + + static Eigen::MatrixXd Eye(const Class& m) { + int dim = GetDimension(m); + return Eigen::MatrixXd::Identity(dim, dim); + } + + static TangentVector Local(const Class& origin, const Class& other, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = - Eye(origin); + if (H2) *H2 = Eye(other); + Class v = other-origin; + return v.vector(); + } + + static Class Retract(const Class& origin, const TangentVector& v, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = Eye(origin); + if (H2) *H2 = Eye(origin); + return origin + Class(v); + } + + /// @} + + /// @name Lie Group + /// @{ + + static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) { + if (Hm) *Hm = Eye(m); + return m.vector(); + } + + static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + Class result(v); + if (Hv) + *Hv = Eye(v); + return result; + } + + static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1, + ChartJacobian H2) { + if (H1) *H1 = Eye(v1); + if (H2) *H2 = Eye(v2); + return v1 + v2; + } + + static Class Between(const Class& v1, const Class& v2, ChartJacobian H1, + ChartJacobian H2) { + if (H1) *H1 = - Eye(v1); + if (H2) *H2 = Eye(v2); + return v2 - v1; + } + + static Class Inverse(const Class& v, ChartJacobian H) { + if (H) *H = -Eye(v); + return -v; + } + + /// @} +}; + +/// A helper that implements the traits interface for GTSAM vector space types. +/// To use this for your gtsam type, define: +/// template<> struct traits : public VectorSpaceTraits { }; +template +struct VectorSpaceTraits: VectorSpaceImpl { + + typedef vector_space_tag structure_category; + + /// @name Group + /// @{ + typedef additive_group_tag group_flavor; + static Class Identity() { return Class::identity();} + /// @} + + /// @name Manifold + /// @{ + enum { dimension = Class::dimension}; + typedef Class ManifoldType; + /// @} +}; + +/// Both VectorSpaceTRaits and Testable +template +struct VectorSpace: Testable, VectorSpaceTraits {}; + +/// A helper that implements the traits interface for GTSAM lie groups. +/// To use this for your gtsam type, define: +/// template<> struct traits : public ScalarTraits { }; +template +struct ScalarTraits : VectorSpaceImpl { + + typedef vector_space_tag structure_category; + + /// @name Testable + /// @{ + static void Print(Scalar m, const std::string& str = "") { + gtsam::print(m, str); + } + static bool Equals(Scalar v1, Scalar v2, double tol = 1e-8) { + return std::abs(v1 - v2) < tol; + } + /// @} + + /// @name Group + /// @{ + typedef additive_group_tag group_flavor; + static Scalar Identity() { return 0;} + /// @} + + /// @name Manifold + /// @{ + typedef Scalar ManifoldType; + enum { dimension = 1 }; + typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian<1, 1> ChartJacobian; + + static TangentVector Local(Scalar origin, Scalar other, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) (*H1)[0] = -1.0; + if (H2) (*H2)[0] = 1.0; + TangentVector result; + result(0) = other - origin; + return result; + } + + static Scalar Retract(Scalar origin, const TangentVector& v, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) (*H1)[0] = 1.0; + if (H2) (*H2)[0] = 1.0; + return origin + v[0]; + } + /// @} + + /// @name Lie Group + /// @{ + static TangentVector Logmap(Scalar m, ChartJacobian H = boost::none) { + if (H) (*H)[0] = 1.0; + return Local(0, m); + } + + static Scalar Expmap(const TangentVector& v, ChartJacobian H = boost::none) { + if (H) (*H)[0] = 1.0; + return v[0]; + } + /// @} + +}; + +} // namespace internal + +/// double +template<> struct traits : public internal::ScalarTraits { +}; + +/// float +template<> struct traits : public internal::ScalarTraits { +}; + +// traits for any fixed double Eigen matrix +template +struct traits > : + internal::VectorSpaceImpl< + Eigen::Matrix, M * N> { + + typedef vector_space_tag structure_category; + typedef Eigen::Matrix Fixed; + + /// @name Testable + /// @{ + static void Print(const Fixed& m, const std::string& str = "") { + gtsam::print(Eigen::MatrixXd(m), str); + } + static bool Equals(const Fixed& v1, const Fixed& v2, double tol = 1e-8) { + return equal_with_abs_tol(v1, v2, tol); + } + /// @} + + /// @name Group + /// @{ + typedef additive_group_tag group_flavor; + static Fixed Identity() { return Fixed::Zero();} + /// @} + + /// @name Manifold + /// @{ + enum { dimension = M*N}; + typedef Fixed ManifoldType; + typedef Eigen::Matrix TangentVector; + typedef Eigen::Matrix Jacobian; + typedef OptionalJacobian ChartJacobian; + + static TangentVector Local(Fixed origin, Fixed other, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) (*H1) = -Jacobian::Identity(); + if (H2) (*H2) = Jacobian::Identity(); + TangentVector result; + Eigen::Map(result.data()) = other - origin; + return result; + } + + static Fixed Retract(Fixed origin, const TangentVector& v, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) (*H1) = Jacobian::Identity(); + if (H2) (*H2) = Jacobian::Identity(); + return origin + Eigen::Map(v.data()); + } + /// @} + + /// @name Lie Group + /// @{ + static TangentVector Logmap(const Fixed& m, ChartJacobian H = boost::none) { + if (H) *H = Jacobian::Identity(); + TangentVector result; + Eigen::Map(result.data()) = m; + return result; + } + + static Fixed Expmap(const TangentVector& v, ChartJacobian H = boost::none) { + Fixed m; + m.setZero(); + if (H) *H = Jacobian::Identity(); + return m + Eigen::Map(v.data()); + } + /// @} +}; + + +namespace internal { + +// traits for dynamic Eigen matrices +template +struct DynamicTraits { + + typedef vector_space_tag structure_category; + typedef Eigen::Matrix Dynamic; + + /// @name Testable + /// @{ + static void Print(const Dynamic& m, const std::string& str = "") { + gtsam::print(Eigen::MatrixXd(m), str); + } + static bool Equals(const Dynamic& v1, const Dynamic& v2, + double tol = 1e-8) { + return equal_with_abs_tol(v1, v2, tol); + } + /// @} + + /// @name Group + /// @{ + typedef additive_group_tag group_flavor; + static Dynamic Identity() { + throw std::runtime_error("Identity not defined for dynamic types"); + } + /// @} + + /// @name Manifold + /// @{ + enum { dimension = Eigen::Dynamic }; + typedef Eigen::VectorXd TangentVector; + typedef Eigen::MatrixXd Jacobian; + typedef OptionalJacobian ChartJacobian; + typedef Dynamic ManifoldType; + + static int GetDimension(const Dynamic& m) { + return m.rows() * m.cols(); + } + + static Jacobian Eye(const Dynamic& m) { + int dim = GetDimension(m); + return Eigen::Matrix::Identity(dim, dim); + } + + static TangentVector Local(const Dynamic& m, const Dynamic& other, // + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = -Eye(m); + if (H2) *H2 = Eye(m); + TangentVector v(GetDimension(m)); + Eigen::Map(v.data(), m.rows(), m.cols()) = other - m; + return v; + } + + static Dynamic Retract(const Dynamic& m, const TangentVector& v, // + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = Eye(m); + if (H2) *H2 = Eye(m); + return m + Eigen::Map(v.data(), m.rows(), m.cols()); + } + /// @} + + /// @name Lie Group + /// @{ + static TangentVector Logmap(const Dynamic& m, ChartJacobian H = boost::none) { + if (H) *H = Eye(m); + TangentVector result(GetDimension(m)); + Eigen::Map(result.data(), m.cols(), m.rows()) = m; + return result; + } + + static Dynamic Expmap(const TangentVector& /*v*/, ChartJacobian H = boost::none) { + static_cast(H); + throw std::runtime_error("Expmap not defined for dynamic types"); + } + + static Dynamic Inverse(const Dynamic& m, ChartJacobian H = boost::none) { + if (H) *H = -Eye(m); + return -m; + } + + static Dynamic Compose(const Dynamic& v1, const Dynamic& v2, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = Eye(v1); + if (H2) *H2 = Eye(v1); + return v1 + v2; + } + + static Dynamic Between(const Dynamic& v1, const Dynamic& v2, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = -Eye(v1); + if (H2) *H2 = Eye(v1); + return v2 - v1; + } + /// @} + +}; + +} // \ internal + +// traits for fully dynamic matrix +template +struct traits > : + public internal::DynamicTraits<-1, -1, Options, MaxRows, MaxCols> { +}; + +// traits for dynamic column vector +template +struct traits > : + public internal::DynamicTraits<-1, 1, Options, MaxRows, MaxCols> { +}; + +// traits for dynamic row vector +template +struct traits > : + public internal::DynamicTraits<1, -1, Options, MaxRows, MaxCols> { +}; + +/// Vector Space concept +template +class IsVectorSpace: public IsLieGroup { +public: + + typedef typename traits::structure_category structure_category_tag; + + BOOST_CONCEPT_USAGE(IsVectorSpace) { + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "This type's trait does not assert it as a vector space (or derived)"); + r = p + q; + r = -p; + r = p - q; + } + +private: + T p, q, r; +}; + +} // namespace gtsam + diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index b075d73b3..c6a3eb034 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -199,6 +199,7 @@ namespace gtsam { } void checkBlock(DenseIndex block) const { + static_cast(block); //Disable unused varibale warnings. assert(matrix_.cols() == variableColOffsets_.back()); assert(block < (DenseIndex)variableColOffsets_.size() - 1); assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols()); @@ -220,7 +221,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(matrix_); ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); ar & BOOST_SERIALIZATION_NVP(rowStart_); diff --git a/gtsam/base/chartTesting.h b/gtsam/base/chartTesting.h new file mode 100644 index 000000000..f63054a5b --- /dev/null +++ b/gtsam/base/chartTesting.h @@ -0,0 +1,69 @@ +/* ---------------------------------------------------------------------------- + + * 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 chartTesting.h + * @brief + * @date November, 2014 + * @author Paul Furgale + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam { +// Do a full concept check and test the invertibility of local() vs. retract(). +template +void testDefaultChart(TestResult& result_, + const std::string& name_, + const T& value) { + + GTSAM_CONCEPT_TESTABLE_TYPE(T); + + typedef typename gtsam::DefaultChart Chart; + typedef typename Chart::vector Vector; + + // First, check the basic chart concept. This checks that the interface is satisfied. + // The rest of the function is even more detailed, checking the correctness of the chart. + BOOST_CONCEPT_ASSERT((ChartConcept)); + + T other = value; + + // Check that the dimension of the local value matches the chart dimension. + Vector dx = Chart::local(value, other); + EXPECT_LONGS_EQUAL(Chart::getDimension(value), dx.size()); + // And that the "local" of a value vs. itself is zero. + EXPECT(assert_equal(Matrix(dx), Matrix(Eigen::VectorXd::Zero(dx.size())))); + + // Test the invertibility of retract/local + dx.setRandom(); + T updated = Chart::retract(value, dx); + Vector invdx = Chart::local(value, updated); + EXPECT(assert_equal(Matrix(dx), Matrix(invdx), 1e-9)); + + // And test that negative steps work as well. + dx = -dx; + updated = Chart::retract(value, dx); + invdx = Chart::local(value, updated); + EXPECT(assert_equal(Matrix(dx), Matrix(invdx), 1e-9)); +} +} // namespace gtsam + +/// \brief Perform a concept check on the default chart for a type. +/// \param value An instantiation of the type to be tested. +#define CHECK_CHART_CONCEPT(value) \ + { gtsam::testDefaultChart(result_, name_, value); } diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h new file mode 100644 index 000000000..da872d006 --- /dev/null +++ b/gtsam/base/concepts.h @@ -0,0 +1,32 @@ +/* + * concepts.h + * + * @date Dec 4, 2014 + * @author Mike Bosse + * @author Frank Dellaert + */ + +#pragma once + +// This is a helper to ease the transition to the new traits defined in this file. +// Uncomment this if you want all methods that are tagged as not implemented +// to cause compilation errors. +#ifdef COMPILE_ERROR_NOT_IMPLEMENTED + +#include +#define CONCEPT_NOT_IMPLEMENTED BOOST_STATIC_ASSERT_MSG(boost::false_type, \ +"This method is required by the new concepts framework but has not been implemented yet."); + +#else + +#include +#define CONCEPT_NOT_IMPLEMENTED \ + throw std::runtime_error("This method is required by the new concepts framework but has not been implemented yet."); + +#endif + +namespace gtsam { + +template struct traits; + +} diff --git a/gtsam/base/debug.cpp b/gtsam/base/debug.cpp index e50e3af35..6abc98642 100644 --- a/gtsam/base/debug.cpp +++ b/gtsam/base/debug.cpp @@ -17,9 +17,32 @@ */ #include +#ifdef GTSAM_USE_TBB +#include +#endif namespace gtsam { - GTSAM_EXPORT FastMap > debugFlags; +GTSAM_EXPORT FastMap > debugFlags; + +#ifdef GTSAM_USE_TBB +tbb::mutex debugFlagsMutex; +#endif + +/* ************************************************************************* */ +bool guardedIsDebug(const std::string& s) { +#ifdef GTSAM_USE_TBB + tbb::mutex::scoped_lock lock(debugFlagsMutex); +#endif + return gtsam::debugFlags[s]; +} + +/* ************************************************************************* */ +void guardedSetDebug(const std::string& s, const bool v) { +#ifdef GTSAM_USE_TBB + tbb::mutex::scoped_lock lock(debugFlagsMutex); +#endif + gtsam::debugFlags[s] = v; +} } diff --git a/gtsam/base/debug.h b/gtsam/base/debug.h index f416bd826..e21efcc83 100644 --- a/gtsam/base/debug.h +++ b/gtsam/base/debug.h @@ -43,6 +43,10 @@ namespace gtsam { GTSAM_EXTERN_EXPORT FastMap > debugFlags; + + // Non-guarded use led to crashes, and solved in commit cd35db2 + bool GTSAM_EXPORT guardedIsDebug(const std::string& s); + void GTSAM_EXPORT guardedSetDebug(const std::string& s, const bool v); } #undef ISDEBUG @@ -50,8 +54,8 @@ namespace gtsam { #ifdef GTSAM_ENABLE_DEBUG -#define ISDEBUG(S) (gtsam::debugFlags[S]) -#define SETDEBUG(S,V) ((void)(gtsam::debugFlags[S] = (V))) +#define ISDEBUG(S) (gtsam::guardedIsDebug(S)) +#define SETDEBUG(S,V) ((void)(gtsam::guardedSetDebug(S,V))) #else diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 9339c9c7b..a7dc37d55 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -28,8 +28,10 @@ #pragma GCC diagnostic pop #endif -#include -#include +#include +#include +#include +#include namespace gtsam { @@ -55,25 +57,32 @@ namespace gtsam { * http://www.boost.org/doc/libs/release/libs/bind/bind.html */ + +// a quick helper struct to get the appropriate fixed sized matrix from two value types +namespace internal { +template +struct FixedSizeMatrix { + typedef Eigen::Matrix::dimension, traits::dimension> type; +}; +} + /** * Numerically compute gradient of scalar function * Class X is the input argument * The class X needs to have dim, expmap, logmap */ template -Vector numericalGradient(boost::function h, const X& x, +typename internal::FixedSizeMatrix::type numericalGradient(boost::function h, const X& x, double delta = 1e-5) { double factor = 1.0 / (2.0 * delta); - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::structure_category>::value), "Template argument X must be a manifold type."); - static const int N = traits::dimension::value; + static const int N = traits::dimension; BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type."); - typedef DefaultChart ChartX; - typedef typename ChartX::vector TangentX; - // get chart at x - ChartX chartX; + typedef typename traits::TangentVector TangentX; // Prepare a tangent vector to perturb x with, only works for fixed size TangentX d; @@ -82,9 +91,9 @@ Vector numericalGradient(boost::function h, const X& x, Vector g = zero(N); // Can be fixed size for (int j = 0; j < N; j++) { d(j) = delta; - double hxplus = h(chartX.retract(x, d)); + double hxplus = h(traits::Retract(x, d)); d(j) = -delta; - double hxmin = h(chartX.retract(x, d)); + double hxmin = h(traits::Retract(x, d)); d(j) = 0; g(j) = (hxplus - hxmin) * factor; } @@ -101,35 +110,33 @@ Vector numericalGradient(boost::function h, const X& x, * Class X is the input argument * @return m*n Jacobian computed via central differencing */ + template // TODO Should compute fixed-size matrix -Matrix numericalDerivative11(boost::function h, const X& x, +typename internal::FixedSizeMatrix::type numericalDerivative11(boost::function h, const X& x, double delta = 1e-5) { - using namespace traits; - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + typedef typename internal::FixedSizeMatrix::type Matrix; + + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - typedef DefaultChart ChartY; - typedef typename ChartY::vector TangentY; + typedef traits TraitsY; + typedef typename TraitsY::TangentVector TangentY; - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X must be a manifold type."); - static const int N = traits::dimension::value; + static const int N = traits::dimension; BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type."); - typedef DefaultChart ChartX; - typedef typename ChartX::vector TangentX; + typedef traits TraitsX; + typedef typename TraitsX::TangentVector TangentX; // get value at x, and corresponding chart Y hx = h(x); - ChartY chartY; // Bit of a hack for now to find number of rows - TangentY zeroY = chartY.local(hx, hx); + TangentY zeroY = TraitsY::Local(hx, hx); size_t m = zeroY.size(); - // get chart at x - ChartX chartX; - // Prepare a tangent vector to perturb x with, only works for fixed size TangentX dx; dx.setZero(); @@ -139,9 +146,9 @@ Matrix numericalDerivative11(boost::function h, const X& x, double factor = 1.0 / (2.0 * delta); for (int j = 0; j < N; j++) { dx(j) = delta; - TangentY dy1 = chartY.local(hx, h(chartX.retract(x, dx))); + TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); dx(j) = -delta; - TangentY dy2 = chartY.local(hx, h(chartX.retract(x, dx))); + TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); dx(j) = 0; H.col(j) << (dy1 - dy2) * factor; } @@ -150,7 +157,7 @@ Matrix numericalDerivative11(boost::function h, const X& x, /** use a raw C++ function pointer */ template -Matrix numericalDerivative11(Y (*h)(const X&), const X& x, +typename internal::FixedSizeMatrix::type numericalDerivative11(Y (*h)(const X&), const X& x, double delta = 1e-5) { return numericalDerivative11(boost::bind(h, _1), x, delta); } @@ -164,18 +171,18 @@ Matrix numericalDerivative11(Y (*h)(const X&), const X& x, * @return m*n Jacobian computed via central differencing */ template -Matrix numericalDerivative21(const boost::function& h, +typename internal::FixedSizeMatrix::type numericalDerivative21(const boost::function& h, const X1& x1, const X2& x2, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11(boost::bind(h, _1, x2), x1, delta); } /** use a raw C++ function pointer */ template -inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1, +typename internal::FixedSizeMatrix::type numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { return numericalDerivative21(boost::bind(h, _1, _2), x1, x2, delta); } @@ -189,18 +196,18 @@ inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1, * @return m*n Jacobian computed via central differencing */ template -Matrix numericalDerivative22(boost::function h, +typename internal::FixedSizeMatrix::type numericalDerivative22(boost::function h, const X1& x1, const X2& x2, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, - "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, - "Template argument X2 must be a manifold type."); +// BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), +// "Template argument X1 must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X2 must be a manifold type."); return numericalDerivative11(boost::bind(h, x1, _1), x2, delta); } /** use a raw C++ function pointer */ template -inline Matrix numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1, +typename internal::FixedSizeMatrix::type numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { return numericalDerivative22(boost::bind(h, _1, _2), x1, x2, delta); } @@ -216,18 +223,18 @@ inline Matrix numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1, * All classes Y,X1,X2,X3 need dim, expmap, logmap */ template -Matrix numericalDerivative31( +typename internal::FixedSizeMatrix::type numericalDerivative31( boost::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11(boost::bind(h, _1, x2, x3), x1, delta); } template -inline Matrix numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&), +typename internal::FixedSizeMatrix::type numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalDerivative31(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); @@ -244,18 +251,18 @@ inline Matrix numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&), * All classes Y,X1,X2,X3 need dim, expmap, logmap */ template -Matrix numericalDerivative32( +typename internal::FixedSizeMatrix::type numericalDerivative32( boost::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); return numericalDerivative11(boost::bind(h, x1, _1, x3), x2, delta); } template -inline Matrix numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&), +inline typename internal::FixedSizeMatrix::type numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalDerivative32(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); @@ -272,23 +279,65 @@ inline Matrix numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&), * All classes Y,X1,X2,X3 need dim, expmap, logmap */ template -Matrix numericalDerivative33( +typename internal::FixedSizeMatrix::type numericalDerivative33( boost::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X3 must be a manifold type."); return numericalDerivative11(boost::bind(h, x1, x2, _1), x3, delta); } template -inline Matrix numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&), +inline typename internal::FixedSizeMatrix::type numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalDerivative33(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); } +/** + * Compute numerical derivative in argument 1 of 4-argument function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative41( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, _1, x2, x3, x4), x1, delta); +} + +/** + * Compute numerical derivative in argument 2 of 4-argument function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative42( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, _1, x3, x4), x2, delta); +} + /** * Compute numerical Hessian matrix. Requires a single-argument Lie->scalar * function. This is implemented simply as the derivative of the gradient. @@ -298,19 +347,20 @@ inline Matrix numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&), * @return n*n Hessian matrix computed via central differencing */ template -inline Matrix numericalHessian(boost::function f, const X& x, +inline typename internal::FixedSizeMatrix::type numericalHessian(boost::function f, const X& x, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X must be a manifold type."); + typedef Eigen::Matrix::dimension, 1> VectorD; typedef boost::function F; - typedef boost::function G; + typedef boost::function G; G ng = static_cast(numericalGradient ); - return numericalDerivative11(boost::bind(ng, f, _1, delta), x, + return numericalDerivative11(boost::bind(ng, f, _1, delta), x, delta); } template -inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta = +inline typename internal::FixedSizeMatrix::type numericalHessian(double (*f)(const X&), const X& x, double delta = 1e-5) { return numericalHessian(boost::function(f), x, delta); } @@ -324,6 +374,8 @@ class G_x1 { X1 x1_; double delta_; public: + typedef typename internal::FixedSizeMatrix::type Vector; + G_x1(const boost::function& f, const X1& x1, double delta) : f_(f), x1_(x1), delta_(delta) { @@ -334,9 +386,10 @@ public: }; template -inline Matrix numericalHessian212( +inline typename internal::FixedSizeMatrix::type numericalHessian212( boost::function f, const X1& x1, const X2& x2, double delta = 1e-5) { + typedef typename internal::FixedSizeMatrix::type Vector; G_x1 g_x1(f, x1, delta); return numericalDerivative11( boost::function( @@ -344,17 +397,19 @@ inline Matrix numericalHessian212( } template -inline Matrix numericalHessian212(double (*f)(const X1&, const X2&), +inline typename internal::FixedSizeMatrix::type numericalHessian212(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { return numericalHessian212(boost::function(f), x1, x2, delta); } template -inline Matrix numericalHessian211( +inline typename internal::FixedSizeMatrix::type numericalHessian211( boost::function f, const X1& x1, const X2& x2, double delta = 1e-5) { + typedef typename internal::FixedSizeMatrix::type Vector; + Vector (*numGrad)(boost::function, const X1&, double) = &numericalGradient; boost::function f2(boost::bind(f, _1, x2)); @@ -365,17 +420,17 @@ inline Matrix numericalHessian211( } template -inline Matrix numericalHessian211(double (*f)(const X1&, const X2&), +inline typename internal::FixedSizeMatrix::type numericalHessian211(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { return numericalHessian211(boost::function(f), x1, x2, delta); } template -inline Matrix numericalHessian222( +inline typename internal::FixedSizeMatrix::type numericalHessian222( boost::function f, const X1& x1, const X2& x2, double delta = 1e-5) { - + typedef typename internal::FixedSizeMatrix::type Vector; Vector (*numGrad)(boost::function, const X2&, double) = &numericalGradient; boost::function f2(boost::bind(f, x1, _1)); @@ -386,7 +441,7 @@ inline Matrix numericalHessian222( } template -inline Matrix numericalHessian222(double (*f)(const X1&, const X2&), +inline typename internal::FixedSizeMatrix::type numericalHessian222(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { return numericalHessian222(boost::function(f), x1, x2, delta); @@ -397,10 +452,10 @@ inline Matrix numericalHessian222(double (*f)(const X1&, const X2&), */ /* **************************************************************** */ template -inline Matrix numericalHessian311( +inline typename internal::FixedSizeMatrix::type numericalHessian311( boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { - + typedef typename internal::FixedSizeMatrix::type Vector; Vector (*numGrad)(boost::function, const X1&, double) = &numericalGradient; boost::function f2(boost::bind(f, _1, x2, x3)); @@ -411,7 +466,7 @@ inline Matrix numericalHessian311( } template -inline Matrix numericalHessian311(double (*f)(const X1&, const X2&, const X3&), +inline typename internal::FixedSizeMatrix::type numericalHessian311(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian311( boost::function(f), x1, x2, x3, @@ -420,10 +475,10 @@ inline Matrix numericalHessian311(double (*f)(const X1&, const X2&, const X3&), /* **************************************************************** */ template -inline Matrix numericalHessian322( +inline typename internal::FixedSizeMatrix::type numericalHessian322( boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { - + typedef typename internal::FixedSizeMatrix::type Vector; Vector (*numGrad)(boost::function, const X2&, double) = &numericalGradient; boost::function f2(boost::bind(f, x1, _1, x3)); @@ -434,7 +489,7 @@ inline Matrix numericalHessian322( } template -inline Matrix numericalHessian322(double (*f)(const X1&, const X2&, const X3&), +inline typename internal::FixedSizeMatrix::type numericalHessian322(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian322( boost::function(f), x1, x2, x3, @@ -443,10 +498,10 @@ inline Matrix numericalHessian322(double (*f)(const X1&, const X2&, const X3&), /* **************************************************************** */ template -inline Matrix numericalHessian333( +inline typename internal::FixedSizeMatrix::type numericalHessian333( boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { - + typedef typename internal::FixedSizeMatrix::type Vector; Vector (*numGrad)(boost::function, const X3&, double) = &numericalGradient; boost::function f2(boost::bind(f, x1, x2, _1)); @@ -457,7 +512,7 @@ inline Matrix numericalHessian333( } template -inline Matrix numericalHessian333(double (*f)(const X1&, const X2&, const X3&), +inline typename internal::FixedSizeMatrix::type numericalHessian333(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian333( boost::function(f), x1, x2, x3, @@ -466,7 +521,7 @@ inline Matrix numericalHessian333(double (*f)(const X1&, const X2&, const X3&), /* **************************************************************** */ template -inline Matrix numericalHessian312( +inline typename internal::FixedSizeMatrix::type numericalHessian312( boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian212( @@ -475,7 +530,7 @@ inline Matrix numericalHessian312( } template -inline Matrix numericalHessian313( +inline typename internal::FixedSizeMatrix::type numericalHessian313( boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian212( @@ -484,7 +539,7 @@ inline Matrix numericalHessian313( } template -inline Matrix numericalHessian323( +inline typename internal::FixedSizeMatrix::type numericalHessian323( boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian212( @@ -494,7 +549,7 @@ inline Matrix numericalHessian323( /* **************************************************************** */ template -inline Matrix numericalHessian312(double (*f)(const X1&, const X2&, const X3&), +inline typename internal::FixedSizeMatrix::type numericalHessian312(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian312( boost::function(f), x1, x2, x3, @@ -502,7 +557,7 @@ inline Matrix numericalHessian312(double (*f)(const X1&, const X2&, const X3&), } template -inline Matrix numericalHessian313(double (*f)(const X1&, const X2&, const X3&), +inline typename internal::FixedSizeMatrix::type numericalHessian313(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian313( boost::function(f), x1, x2, x3, @@ -510,10 +565,12 @@ inline Matrix numericalHessian313(double (*f)(const X1&, const X2&, const X3&), } template -inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const X3&), +inline typename internal::FixedSizeMatrix::type numericalHessian323(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian323( boost::function(f), x1, x2, x3, delta); } -} + +} // namespace gtsam + diff --git a/gtsam/base/testLie.h b/gtsam/base/testLie.h new file mode 100644 index 000000000..1906ec439 --- /dev/null +++ b/gtsam/base/testLie.h @@ -0,0 +1,86 @@ +/* ---------------------------------------------------------------------------- + + * 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 chartTesting.h + * @brief + * @date November, 2014 + * @author Paul Furgale + */ + +#pragma once + +#include +#include + +#include +#include +#include + +namespace gtsam { + +// Do a comprehensive test of Lie Group derivatives +template +void testLieGroupDerivatives(TestResult& result_, const std::string& name_, + const G& t1, const G& t2) { + + Matrix H1, H2; + typedef traits T; + typedef OptionalJacobian OJ; + + // Inverse + OJ none; + EXPECT(assert_equal(t1.inverse(),T::Inverse(t1, H1))); + EXPECT(assert_equal(numericalDerivative21(T::Inverse, t1, none),H1)); + + EXPECT(assert_equal(t2.inverse(),T::Inverse(t2, H1))); + EXPECT(assert_equal(numericalDerivative21(T::Inverse, t2, none),H1)); + + // Compose + EXPECT(assert_equal(t1 * t2,T::Compose(t1, t2, H1, H2))); + EXPECT(assert_equal(numericalDerivative41(T::Compose, t1, t2, none, none), H1)); + EXPECT(assert_equal(numericalDerivative42(T::Compose, t1, t2, none, none), H2)); + + // Between + EXPECT(assert_equal(t1.inverse() * t2,T::Between(t1, t2, H1, H2))); + EXPECT(assert_equal(numericalDerivative41(T::Between, t1, t2, none, none), H1)); + EXPECT(assert_equal(numericalDerivative42(T::Between, t1, t2, none, none), H2)); +} + +// Do a comprehensive test of Lie Group Chart derivatives +template +void testChartDerivatives(TestResult& result_, const std::string& name_, + const G& t1, const G& t2) { + + Matrix H1, H2; + typedef traits T; + typedef typename T::TangentVector V; + typedef OptionalJacobian OJ; + + // Retract + OJ none; + V w12 = T::Local(t1, t2); + EXPECT(assert_equal(t2, T::Retract(t1,w12, H1, H2))); + EXPECT(assert_equal(numericalDerivative41(T::Retract, t1, w12, none, none), H1)); + EXPECT(assert_equal(numericalDerivative42(T::Retract, t1, w12, none, none), H2)); + + // Local + EXPECT(assert_equal(w12, T::Local(t1, t2, H1, H2))); + EXPECT(assert_equal(numericalDerivative41(T::Local, t1, t2, none, none), H1)); + EXPECT(assert_equal(numericalDerivative42(T::Local, t1, t2, none, none), H2)); +} +} // namespace gtsam + +#define CHECK_LIE_GROUP_DERIVATIVES(t1,t2) \ + { gtsam::testLieGroupDerivatives(result_, name_, t1, t2); } + +#define CHECK_CHART_DERIVATIVES(t1,t2) \ + { gtsam::testChartDerivatives(result_, name_, t1, t2); } diff --git a/gtsam/base/tests/testGroup.cpp b/gtsam/base/tests/testGroup.cpp new file mode 100644 index 000000000..bce9a6036 --- /dev/null +++ b/gtsam/base/tests/testGroup.cpp @@ -0,0 +1,144 @@ +/* ---------------------------------------------------------------------------- + + * 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 testGroup.cpp + * @brief Unit tests for groups + * @author Frank Dellaert + **/ + +#include +#include +#include +#include +#include + +namespace gtsam { + +/// Symmetric group +template +class Symmetric: private Eigen::PermutationMatrix { + Symmetric(const Eigen::PermutationMatrix& P) : + Eigen::PermutationMatrix(P) { + } +public: + static Symmetric identity() { return Symmetric(); } + Symmetric() { + Eigen::PermutationMatrix::setIdentity(); + } + static Symmetric Transposition(int i, int j) { + Symmetric g; + return g.applyTranspositionOnTheRight(i, j); + } + Symmetric operator*(const Symmetric& other) const { + return Eigen::PermutationMatrix::operator*(other); + } + bool operator==(const Symmetric& other) const { + for (size_t i = 0; i < N; i++) + if (this->indices()[i] != other.indices()[i]) + return false; + return true; + } + Symmetric inverse() const { + return Eigen::PermutationMatrix(Eigen::PermutationMatrix::inverse()); + } + friend std::ostream &operator<<(std::ostream &os, const Symmetric& m) { + for (size_t i = 0; i < N; i++) + os << m.indices()[i] << " "; + return os; + } + void print(const std::string& s = "") const { + std::cout << s << *this << std::endl; + } + bool equals(const Symmetric& other, double tol = 0) const { + return this->indices() == other.indices(); + } +}; + +/// Define permutation group traits to be a model of the Multiplicative Group concept +template +struct traits > : internal::MultiplicativeGroupTraits >, + Testable > { +}; + +} // namespace gtsam + +#include +#include + +using namespace std; +using namespace gtsam; + +//****************************************************************************** +typedef Symmetric<2> S2; +TEST(Group, S2) { + S2 e, s1 = S2::Transposition(0, 1); + BOOST_CONCEPT_ASSERT((IsGroup)); + EXPECT(check_group_invariants(e, s1)); +} + +//****************************************************************************** +typedef Symmetric<3> S3; +TEST(Group, S3) { + S3 e, s1 = S3::Transposition(0, 1), s2 = S3::Transposition(1, 2); + BOOST_CONCEPT_ASSERT((IsGroup)); + EXPECT(check_group_invariants(e, s1)); + EXPECT(assert_equal(s1, s1 * e)); + EXPECT(assert_equal(s1, e * s1)); + EXPECT(assert_equal(e, s1 * s1)); + S3 g = s1 * s2; // 1 2 0 + EXPECT(assert_equal(s1, g * s2)); + EXPECT(assert_equal(e, compose_pow(g, 0))); + EXPECT(assert_equal(g, compose_pow(g, 1))); + EXPECT(assert_equal(e, compose_pow(g, 3))); // g is generator of Z3 subgroup +} + +//****************************************************************************** +// The direct product of S2=Z2 and S3 is the symmetry group of a hexagon, +// i.e., the dihedral group of order 12 (denoted Dih6 because 6-sided polygon) +namespace gtsam { +typedef DirectProduct Dih6; + +std::ostream &operator<<(std::ostream &os, const Dih6& m) { + os << "( " << m.first << ", " << m.second << ")"; + return os; +} + +// Provide traits with Testable + +template<> +struct traits : internal::MultiplicativeGroupTraits { + static void Print(const Dih6& m, const string& s = "") { + cout << s << m << endl; + } + static bool Equals(const Dih6& m1, const Dih6& m2, double tol = 1e-8) { + return m1 == m2; + } +}; +} // namespace gtsam + +TEST(Group, Dih6) { + Dih6 e, g(S2::Transposition(0, 1), + S3::Transposition(0, 1) * S3::Transposition(1, 2)); + BOOST_CONCEPT_ASSERT((IsGroup)); + EXPECT(check_group_invariants(e, g)); + EXPECT(assert_equal(e, compose_pow(g, 0))); + EXPECT(assert_equal(g, compose_pow(g, 1))); + EXPECT(assert_equal(e, compose_pow(g, 6))); // g is generator of Z6 subgroup +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp index 53dd6d4d5..09caadabd 100644 --- a/gtsam/base/tests/testLieMatrix.cpp +++ b/gtsam/base/tests/testLieMatrix.cpp @@ -15,10 +15,10 @@ */ #include +#include #include #include -#include using namespace gtsam; @@ -30,7 +30,7 @@ TEST( LieMatrix, construction ) { Matrix m = (Matrix(2,2) << 1.0,2.0, 3.0,4.0).finished(); LieMatrix lie1(m), lie2(m); - EXPECT(lie1.dim() == 4); + EXPECT(traits::GetDimension(m) == 4); EXPECT(assert_equal(m, lie1.matrix())); EXPECT(assert_equal(lie1, lie2)); } @@ -50,17 +50,17 @@ TEST(LieMatrix, retract) { Vector update = (Vector(4) << 3.0, 4.0, 6.0, 7.0).finished(); LieMatrix expected((Matrix(2,2) << 4.0, 6.0, 9.0, 11.0).finished()); - LieMatrix actual = init.retract(update); + LieMatrix actual = traits::Retract(init,update); EXPECT(assert_equal(expected, actual)); Vector expectedUpdate = update; - Vector actualUpdate = init.localCoordinates(actual); + Vector actualUpdate = traits::Local(init,actual); EXPECT(assert_equal(expectedUpdate, actualUpdate)); Vector expectedLogmap = (Vector(4) << 1, 2, 3, 4).finished(); - Vector actualLogmap = LieMatrix::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0).finished())); + Vector actualLogmap = traits::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0).finished())); EXPECT(assert_equal(expectedLogmap, actualLogmap)); } diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp index 946a342fc..07e666fbe 100644 --- a/gtsam/base/tests/testLieScalar.cpp +++ b/gtsam/base/tests/testLieScalar.cpp @@ -15,10 +15,10 @@ */ #include +#include #include #include -#include using namespace gtsam; @@ -27,6 +27,20 @@ GTSAM_CONCEPT_LIE_INST(LieScalar) const double tol=1e-9; +//****************************************************************************** +TEST(LieScalar , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +//****************************************************************************** +TEST(LieScalar , Invariants) { + LieScalar lie1(2), lie2(3); + CHECK(check_group_invariants(lie1, lie2)); + CHECK(check_manifold_invariants(lie1, lie2)); +} + /* ************************************************************************* */ TEST( testLieScalar, construction ) { double d = 2.; @@ -34,7 +48,7 @@ TEST( testLieScalar, construction ) { EXPECT_DOUBLES_EQUAL(2., lie1.value(),tol); EXPECT_DOUBLES_EQUAL(2., lie2.value(),tol); - EXPECT(lie1.dim() == 1); + EXPECT(traits::dimension == 1); EXPECT(assert_equal(lie1, lie2)); } @@ -42,7 +56,8 @@ TEST( testLieScalar, construction ) { TEST( testLieScalar, localCoordinates ) { LieScalar lie1(1.), lie2(3.); - EXPECT(assert_equal((Vector)(Vector(1) << 2).finished(), lie1.localCoordinates(lie2))); + Vector1 actual = traits::Local(lie1, lie2); + EXPECT( assert_equal((Vector)(Vector(1) << 2).finished(), actual)); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp index c1dface2e..ed3afac8c 100644 --- a/gtsam/base/tests/testLieVector.cpp +++ b/gtsam/base/tests/testLieVector.cpp @@ -15,17 +15,31 @@ */ #include +#include #include #include -#include using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(LieVector) GTSAM_CONCEPT_LIE_INST(LieVector) -/* ************************************************************************* */ +//****************************************************************************** +TEST(LieVector , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +//****************************************************************************** +TEST(LieVector , Invariants) { + Vector v = Vector3(1.0, 2.0, 3.0); + LieVector lie1(v), lie2(v); + check_manifold_invariants(lie1, lie2); +} + +//****************************************************************************** TEST( testLieVector, construction ) { Vector v = Vector3(1.0, 2.0, 3.0); LieVector lie1(v), lie2(v); @@ -35,17 +49,19 @@ TEST( testLieVector, construction ) { EXPECT(assert_equal(lie1, lie2)); } -/* ************************************************************************* */ +//****************************************************************************** TEST( testLieVector, other_constructors ) { Vector init = Vector2(10.0, 20.0); LieVector exp(init); - double data[] = {10,20}; - LieVector b(2,data); + double data[] = { 10, 20 }; + LieVector b(2, data); EXPECT(assert_equal(exp, b)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ - diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 5b36d2b02..13d32794e 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -16,12 +16,14 @@ * @author Carlos Nieto **/ -#include -#include +#include +#include +#include #include #include #include -#include +#include +#include using namespace std; using namespace gtsam; @@ -30,7 +32,7 @@ static double inf = std::numeric_limits::infinity(); static const double tol = 1e-9; /* ************************************************************************* */ -TEST( matrix, constructor_data ) +TEST(Matrix, constructor_data ) { Matrix A = (Matrix(2, 2) << -5, 3, 0, -5).finished(); @@ -44,7 +46,7 @@ TEST( matrix, constructor_data ) } /* ************************************************************************* */ -TEST( matrix, Matrix_ ) +TEST(Matrix, Matrix_ ) { Matrix A = (Matrix(2, 2) << -5.0, 3.0, 0.0, -5.0).finished(); Matrix B(2, 2); @@ -74,7 +76,7 @@ namespace { } /* ************************************************************************* */ -TEST( matrix, special_comma_initializer) +TEST(Matrix, special_comma_initializer) { Matrix expected(2,2); expected(0,0) = 1; @@ -103,7 +105,7 @@ TEST( matrix, special_comma_initializer) } /* ************************************************************************* */ -TEST( matrix, col_major ) +TEST(Matrix, col_major ) { Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); const double * const a = &A(0, 0); @@ -114,7 +116,7 @@ TEST( matrix, col_major ) } /* ************************************************************************* */ -TEST( matrix, collect1 ) +TEST(Matrix, collect1 ) { Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished(); Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished(); @@ -131,7 +133,7 @@ TEST( matrix, collect1 ) } /* ************************************************************************* */ -TEST( matrix, collect2 ) +TEST(Matrix, collect2 ) { Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished(); Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished(); @@ -151,7 +153,7 @@ TEST( matrix, collect2 ) } /* ************************************************************************* */ -TEST( matrix, collect3 ) +TEST(Matrix, collect3 ) { Matrix A, B; A = eye(2, 3); @@ -168,7 +170,7 @@ TEST( matrix, collect3 ) } /* ************************************************************************* */ -TEST( matrix, stack ) +TEST(Matrix, stack ) { Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished(); Matrix B = (Matrix(3, 2) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished(); @@ -191,7 +193,7 @@ TEST( matrix, stack ) } /* ************************************************************************* */ -TEST( matrix, column ) +TEST(Matrix, column ) { Matrix A = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., @@ -210,7 +212,7 @@ TEST( matrix, column ) } /* ************************************************************************* */ -TEST( matrix, insert_column ) +TEST(Matrix, insert_column ) { Matrix big = zeros(5, 6); Vector col = ones(5); @@ -229,7 +231,7 @@ TEST( matrix, insert_column ) } /* ************************************************************************* */ -TEST( matrix, insert_subcolumn ) +TEST(Matrix, insert_subcolumn ) { Matrix big = zeros(5, 6); Vector col1 = ones(2); @@ -252,7 +254,7 @@ TEST( matrix, insert_subcolumn ) } /* ************************************************************************* */ -TEST( matrix, row ) +TEST(Matrix, row ) { Matrix A = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., @@ -271,7 +273,7 @@ TEST( matrix, row ) } /* ************************************************************************* */ -TEST( matrix, zeros ) +TEST(Matrix, zeros ) { Matrix A(2, 3); A(0, 0) = 0; @@ -287,7 +289,7 @@ TEST( matrix, zeros ) } /* ************************************************************************* */ -TEST( matrix, insert_sub ) +TEST(Matrix, insert_sub ) { Matrix big = zeros(5, 6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0).finished(); @@ -302,7 +304,7 @@ TEST( matrix, insert_sub ) } /* ************************************************************************* */ -TEST( matrix, diagMatrices ) +TEST(Matrix, diagMatrices ) { std::vector Hs; Hs.push_back(ones(3,3)); @@ -326,7 +328,7 @@ TEST( matrix, diagMatrices ) } /* ************************************************************************* */ -TEST( matrix, stream_read ) { +TEST(Matrix, stream_read ) { Matrix expected = (Matrix(3,4) << 1.1, 2.3, 4.2, 7.6, -0.3, -8e-2, 5.1, 9.0, @@ -346,7 +348,7 @@ TEST( matrix, stream_read ) { } /* ************************************************************************* */ -TEST( matrix, scale_columns ) +TEST(Matrix, scale_columns ) { Matrix A(3, 4); A(0, 0) = 1.; @@ -384,7 +386,7 @@ TEST( matrix, scale_columns ) } /* ************************************************************************* */ -TEST( matrix, scale_rows ) +TEST(Matrix, scale_rows ) { Matrix A(3, 4); A(0, 0) = 1.; @@ -422,7 +424,7 @@ TEST( matrix, scale_rows ) } /* ************************************************************************* */ -TEST( matrix, scale_rows_mask ) +TEST(Matrix, scale_rows_mask ) { Matrix A(3, 4); A(0, 0) = 1.; @@ -460,7 +462,7 @@ TEST( matrix, scale_rows_mask ) } /* ************************************************************************* */ -TEST( matrix, skewSymmetric ) +TEST(Matrix, skewSymmetric ) { double wx = 1, wy = 2, wz = 3; Matrix3 actual = skewSymmetric(wx,wy,wz); @@ -476,7 +478,7 @@ TEST( matrix, skewSymmetric ) /* ************************************************************************* */ -TEST( matrix, equal ) +TEST(Matrix, equal ) { Matrix A(4, 4); A(0, 0) = -1; @@ -506,7 +508,7 @@ TEST( matrix, equal ) } /* ************************************************************************* */ -TEST( matrix, equal_nan ) +TEST(Matrix, equal_nan ) { Matrix A(4, 4); A(0, 0) = -1; @@ -535,7 +537,7 @@ TEST( matrix, equal_nan ) } /* ************************************************************************* */ -TEST( matrix, addition ) +TEST(Matrix, addition ) { Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished(); @@ -544,7 +546,7 @@ TEST( matrix, addition ) } /* ************************************************************************* */ -TEST( matrix, addition_in_place ) +TEST(Matrix, addition_in_place ) { Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished(); @@ -554,7 +556,7 @@ TEST( matrix, addition_in_place ) } /* ************************************************************************* */ -TEST( matrix, subtraction ) +TEST(Matrix, subtraction ) { Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished(); @@ -563,7 +565,7 @@ TEST( matrix, subtraction ) } /* ************************************************************************* */ -TEST( matrix, subtraction_in_place ) +TEST(Matrix, subtraction_in_place ) { Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished(); @@ -573,7 +575,7 @@ TEST( matrix, subtraction_in_place ) } /* ************************************************************************* */ -TEST( matrix, multiplication ) +TEST(Matrix, multiplication ) { Matrix A(2, 2); A(0, 0) = -1; @@ -593,7 +595,7 @@ TEST( matrix, multiplication ) } /* ************************************************************************* */ -TEST( matrix, scalar_matrix_multiplication ) +TEST(Matrix, scalar_matrix_multiplication ) { Vector result(2); @@ -613,7 +615,7 @@ TEST( matrix, scalar_matrix_multiplication ) } /* ************************************************************************* */ -TEST( matrix, matrix_vector_multiplication ) +TEST(Matrix, matrix_vector_multiplication ) { Vector result(2); @@ -627,7 +629,7 @@ TEST( matrix, matrix_vector_multiplication ) } /* ************************************************************************* */ -TEST( matrix, nrRowsAndnrCols ) +TEST(Matrix, nrRowsAndnrCols ) { Matrix A(3, 6); LONGS_EQUAL( A.rows() , 3 ); @@ -635,7 +637,7 @@ TEST( matrix, nrRowsAndnrCols ) } /* ************************************************************************* */ -TEST( matrix, scalar_divide ) +TEST(Matrix, scalar_divide ) { Matrix A(2, 2); A(0, 0) = 10; @@ -653,7 +655,7 @@ TEST( matrix, scalar_divide ) } /* ************************************************************************* */ -TEST( matrix, zero_below_diagonal ) { +TEST(Matrix, zero_below_diagonal ) { Matrix A1 = (Matrix(3, 4) << 1.0, 2.0, 3.0, 4.0, 1.0, 2.0, 3.0, 4.0, @@ -708,7 +710,7 @@ TEST( matrix, zero_below_diagonal ) { } /* ************************************************************************* */ -TEST( matrix, inverse ) +TEST(Matrix, inverse ) { Matrix A(3, 3); A(0, 0) = 1; @@ -754,7 +756,7 @@ TEST( matrix, inverse ) } /* ************************************************************************* */ -TEST( matrix, inverse2 ) +TEST(Matrix, inverse2 ) { Matrix A(3, 3); A(0, 0) = 0; @@ -784,7 +786,7 @@ TEST( matrix, inverse2 ) } /* ************************************************************************* */ -TEST( matrix, backsubtitution ) +TEST(Matrix, backsubtitution ) { // TEST ONE 2x2 matrix U1*x=b1 Vector expected1 = Vector2(3.6250, -0.75); @@ -809,7 +811,7 @@ TEST( matrix, backsubtitution ) } /* ************************************************************************* */ -TEST( matrix, householder ) +TEST(Matrix, householder ) { // check in-place householder, with v vectors below diagonal @@ -838,7 +840,7 @@ TEST( matrix, householder ) } /* ************************************************************************* */ -TEST( matrix, householder_colMajor ) +TEST(Matrix, householder_colMajor ) { // check in-place householder, with v vectors below diagonal @@ -867,7 +869,7 @@ TEST( matrix, householder_colMajor ) } /* ************************************************************************* */ -TEST( matrix, eigen_QR ) +TEST(Matrix, eigen_QR ) { // use standard Eigen function to yield a non-in-place QR factorization @@ -898,7 +900,7 @@ TEST( matrix, eigen_QR ) // unit test for qr factorization (and hence householder) // This behaves the same as QR in matlab: [Q,R] = qr(A), except for signs /* ************************************************************************* */ -TEST( matrix, qr ) +TEST(Matrix, qr ) { Matrix A = (Matrix(6, 4) << -5, 0, 5, 0, 00, -5, 0, 5, 10, 0, 0, 0, 00, 10, 0, 0, 00, @@ -921,7 +923,7 @@ TEST( matrix, qr ) } /* ************************************************************************* */ -TEST( matrix, sub ) +TEST(Matrix, sub ) { Matrix A = (Matrix(4, 6) << -5, 0, 5, 0, 0, 0, 00, -5, 0, 5, 0, 0, 10, 0, 0, 0, -10, 0, 00, 10, 0, 0, 0, -10).finished(); @@ -933,7 +935,7 @@ TEST( matrix, sub ) } /* ************************************************************************* */ -TEST( matrix, trans ) +TEST(Matrix, trans ) { Matrix A = (Matrix(2, 2) << 1.0, 3.0, 2.0, 4.0).finished(); Matrix B = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); @@ -941,7 +943,7 @@ TEST( matrix, trans ) } /* ************************************************************************* */ -TEST( matrix, col_major_access ) +TEST(Matrix, col_major_access ) { Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); const double* a = &A(0, 0); @@ -949,7 +951,7 @@ TEST( matrix, col_major_access ) } /* ************************************************************************* */ -TEST( matrix, weighted_elimination ) +TEST(Matrix, weighted_elimination ) { // create a matrix to eliminate Matrix A = (Matrix(4, 6) << -1., 0., 1., 0., 0., 0., 0., -1., 0., 1., 0., 0., @@ -984,7 +986,7 @@ TEST( matrix, weighted_elimination ) } /* ************************************************************************* */ -TEST( matrix, inverse_square_root ) +TEST(Matrix, inverse_square_root ) { Matrix measurement_covariance = (Matrix(3, 3) << 0.25, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.01).finished(); @@ -1036,22 +1038,22 @@ Matrix expected = (Matrix(5, 5) << 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247).finished(); } -TEST( matrix, LLt ) +TEST(Matrix, LLt ) { EQUALITY(cholesky::expected, LLt(cholesky::M)); } -TEST( matrix, RtR ) +TEST(Matrix, RtR ) { EQUALITY(cholesky::expected.transpose(), RtR(cholesky::M)); } -TEST( matrix, cholesky_inverse ) +TEST(Matrix, cholesky_inverse ) { EQUALITY(cholesky::M.inverse(), cholesky_inverse(cholesky::M)); } /* ************************************************************************* */ -TEST( matrix, multiplyAdd ) +TEST(Matrix, multiplyAdd ) { Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished(); Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.), @@ -1062,7 +1064,7 @@ TEST( matrix, multiplyAdd ) } /* ************************************************************************* */ -TEST( matrix, transposeMultiplyAdd ) +TEST(Matrix, transposeMultiplyAdd ) { Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished(); Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.), @@ -1073,7 +1075,7 @@ TEST( matrix, transposeMultiplyAdd ) } /* ************************************************************************* */ -TEST( matrix, linear_dependent ) +TEST(Matrix, linear_dependent ) { Matrix A = (Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished(); Matrix B = (Matrix(2, 3) << -1.0, -2.0, -3.0, 8.0, 10.0, 12.0).finished(); @@ -1081,7 +1083,7 @@ TEST( matrix, linear_dependent ) } /* ************************************************************************* */ -TEST( matrix, linear_dependent2 ) +TEST(Matrix, linear_dependent2 ) { Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished(); Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.0, 10.0, 12.0).finished(); @@ -1089,7 +1091,7 @@ TEST( matrix, linear_dependent2 ) } /* ************************************************************************* */ -TEST( matrix, linear_dependent3 ) +TEST(Matrix, linear_dependent3 ) { Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished(); Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.1, 10.0, 12.0).finished(); @@ -1097,7 +1099,7 @@ TEST( matrix, linear_dependent3 ) } /* ************************************************************************* */ -TEST( matrix, svd1 ) +TEST(Matrix, svd1 ) { Vector v = Vector3(2., 1., 0.); Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1) @@ -1116,7 +1118,7 @@ static Matrix sampleA = (Matrix(3, 2) << 0.,-2., 0., 0., 3., 0.).finished(); static Matrix sampleAt = trans(sampleA); /* ************************************************************************* */ -TEST( matrix, svd2 ) +TEST(Matrix, svd2 ) { Matrix U, V; Vector s; @@ -1139,7 +1141,7 @@ TEST( matrix, svd2 ) } /* ************************************************************************* */ -TEST( matrix, svd3 ) +TEST(Matrix, svd3 ) { Matrix U, V; Vector s; @@ -1167,7 +1169,7 @@ TEST( matrix, svd3 ) } /* ************************************************************************* */ -TEST( matrix, svd4 ) +TEST(Matrix, svd4 ) { Matrix U, V; Vector s; @@ -1209,7 +1211,7 @@ TEST( matrix, svd4 ) } /* ************************************************************************* */ -TEST( matrix, DLT ) +TEST(Matrix, DLT ) { Matrix A = (Matrix(8, 9) << 0.21, -0.42, -10.71, 0.18, -0.36, -9.18, -0.61, 1.22, 31.11, @@ -1231,6 +1233,18 @@ TEST( matrix, DLT ) EXPECT(assert_equal(expected, actual, 1e-4)); } +//****************************************************************************** +TEST(Matrix , IsVectorSpace) { + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + typedef Eigen::Matrix RowMajor; + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + typedef Eigen::Matrix RowVector; + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index d3a2b929f..1cbf71e1f 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -95,6 +95,12 @@ TEST(testNumericalDerivative, numericalHessian211) { EXPECT(assert_equal(expected22, actual22, 1e-5)); } +TEST(testNumericalDerivative, numericalHessian212) { + // TODO should implement test for all the variants of numerical Hessian, for mixed dimension types, + // like Point3 y = Project(Camera, Point3); + // I'm not sure how numericalHessian212 is different from 211 or 222 -Mike B. +} + /* ************************************************************************* */ double f4(double x, double y, double z) { return sin(x) * cos(y) * z * z; diff --git a/gtsam/base/tests/testOptionalJacobian.cpp b/gtsam/base/tests/testOptionalJacobian.cpp new file mode 100644 index 000000000..331fb49eb --- /dev/null +++ b/gtsam/base/tests/testOptionalJacobian.cpp @@ -0,0 +1,147 @@ +/* ---------------------------------------------------------------------------- + + * 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 testOptionalJacobian.cpp + * @brief Unit test for OptionalJacobian + * @author Frank Dellaert + * @date Nov 28, 2014 + **/ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +//****************************************************************************** +TEST( OptionalJacobian, Constructors ) { + Matrix23 fixed; + + OptionalJacobian<2, 3> H1; + EXPECT(!H1); + + OptionalJacobian<2, 3> H2(fixed); + EXPECT(H2); + + OptionalJacobian<2, 3> H3(&fixed); + EXPECT(H3); + + Matrix dynamic; + OptionalJacobian<2, 3> H4(dynamic); + EXPECT(H4); + + OptionalJacobian<2, 3> H5(boost::none); + EXPECT(!H5); + + boost::optional optional(dynamic); + OptionalJacobian<2, 3> H6(optional); + EXPECT(H6); + + OptionalJacobian<-1, -1> H7; + EXPECT(!H7); + + OptionalJacobian<-1, -1> H8(dynamic); + EXPECT(H8); + + OptionalJacobian<-1, -1> H9(boost::none); + EXPECT(!H9); + + OptionalJacobian<-1, -1> H10(optional); + EXPECT(H10); +} + +//****************************************************************************** +void test(OptionalJacobian<2, 3> H = boost::none) { + if (H) + *H = Matrix23::Zero(); +} + +void testPtr(Matrix23* H = NULL) { + if (H) + *H = Matrix23::Zero(); +} + +TEST( OptionalJacobian, Fixed) { + + Matrix expected = Matrix23::Zero(); + + // Default argument does nothing + test(); + + // Fixed size, no copy + Matrix23 fixed1; + fixed1.setOnes(); + test(fixed1); + EXPECT(assert_equal(expected,fixed1)); + + // Fixed size, no copy, pointer style + Matrix23 fixed2; + fixed2.setOnes(); + test(&fixed2); + EXPECT(assert_equal(expected,fixed2)); + + // Empty is no longer a sign we don't want a matrix, we want it resized + Matrix dynamic0; + test(dynamic0); + EXPECT(assert_equal(expected,dynamic0)); + + // Dynamic wrong size + Matrix dynamic1(3, 5); + dynamic1.setOnes(); + test(dynamic1); + EXPECT(assert_equal(expected,dynamic1)); + + // Dynamic right size + Matrix dynamic2(2, 5); + dynamic2.setOnes(); + test(dynamic2); + EXPECT(assert_equal(expected, dynamic2)); +} + +//****************************************************************************** +void test2(OptionalJacobian<-1,-1> H = boost::none) { + if (H) + *H = Matrix23::Zero(); // resizes +} + +TEST( OptionalJacobian, Dynamic) { + + Matrix expected = Matrix23::Zero(); + + // Default argument does nothing + test2(); + + // Empty is no longer a sign we don't want a matrix, we want it resized + Matrix dynamic0; + test2(dynamic0); + EXPECT(assert_equal(expected,dynamic0)); + + // Dynamic wrong size + Matrix dynamic1(3, 5); + dynamic1.setOnes(); + test2(dynamic1); + EXPECT(assert_equal(expected,dynamic1)); + + // Dynamic right size + Matrix dynamic2(2, 5); + dynamic2.setOnes(); + test2(dynamic2); + EXPECT(assert_equal(expected, dynamic2)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/base/tests/testTestableAssertions.cpp b/gtsam/base/tests/testTestableAssertions.cpp index d8c62b121..364834e2a 100644 --- a/gtsam/base/tests/testTestableAssertions.cpp +++ b/gtsam/base/tests/testTestableAssertions.cpp @@ -15,8 +15,8 @@ */ #include +#include -#include #include using namespace gtsam; diff --git a/gtsam/base/tests/testTreeTraversal.cpp b/gtsam/base/tests/testTreeTraversal.cpp index c9083f781..8fe5e53ef 100644 --- a/gtsam/base/tests/testTreeTraversal.cpp +++ b/gtsam/base/tests/testTreeTraversal.cpp @@ -45,11 +45,11 @@ struct TestForest { }; TestForest makeTestForest() { - // 0 1 - // / \ - // 2 3 - // | - // 4 + // 0 1 + // / | + // 2 3 + // | + // 4 TestForest forest; forest.roots_.push_back(boost::make_shared(0)); forest.roots_.push_back(boost::make_shared(1)); diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 460ff6cd6..00e40039b 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -15,10 +15,12 @@ * @author Frank Dellaert **/ -#include +#include +#include +#include #include #include -#include +#include using namespace std; using namespace gtsam; @@ -40,7 +42,7 @@ namespace { } /* ************************************************************************* */ -TEST( TestVector, special_comma_initializer) +TEST(Vector, special_comma_initializer) { Vector expected(3); expected(0) = 1; @@ -68,7 +70,7 @@ TEST( TestVector, special_comma_initializer) } /* ************************************************************************* */ -TEST( TestVector, copy ) +TEST(Vector, copy ) { Vector a(2); a(0) = 10; a(1) = 20; double data[] = {10,20}; @@ -78,14 +80,14 @@ TEST( TestVector, copy ) } /* ************************************************************************* */ -TEST( TestVector, zero1 ) +TEST(Vector, zero1 ) { Vector v = Vector::Zero(2); EXPECT(zero(v)); } /* ************************************************************************* */ -TEST( TestVector, zero2 ) +TEST(Vector, zero2 ) { Vector a = zero(2); Vector b = Vector::Zero(2); @@ -94,7 +96,7 @@ TEST( TestVector, zero2 ) } /* ************************************************************************* */ -TEST( TestVector, scalar_multiply ) +TEST(Vector, scalar_multiply ) { Vector a(2); a(0) = 10; a(1) = 20; Vector b(2); b(0) = 1; b(1) = 2; @@ -102,7 +104,7 @@ TEST( TestVector, scalar_multiply ) } /* ************************************************************************* */ -TEST( TestVector, scalar_divide ) +TEST(Vector, scalar_divide ) { Vector a(2); a(0) = 10; a(1) = 20; Vector b(2); b(0) = 1; b(1) = 2; @@ -110,7 +112,7 @@ TEST( TestVector, scalar_divide ) } /* ************************************************************************* */ -TEST( TestVector, negate ) +TEST(Vector, negate ) { Vector a(2); a(0) = 10; a(1) = 20; Vector b(2); b(0) = -10; b(1) = -20; @@ -118,7 +120,7 @@ TEST( TestVector, negate ) } /* ************************************************************************* */ -TEST( TestVector, sub ) +TEST(Vector, sub ) { Vector a(6); a(0) = 10; a(1) = 20; a(2) = 3; @@ -134,7 +136,7 @@ TEST( TestVector, sub ) } /* ************************************************************************* */ -TEST( TestVector, subInsert ) +TEST(Vector, subInsert ) { Vector big = zero(6), small = ones(3); @@ -148,7 +150,7 @@ TEST( TestVector, subInsert ) } /* ************************************************************************* */ -TEST( TestVector, householder ) +TEST(Vector, householder ) { Vector x(4); x(0) = 3; x(1) = 1; x(2) = 5; x(3) = 1; @@ -163,7 +165,7 @@ TEST( TestVector, householder ) } /* ************************************************************************* */ -TEST( TestVector, concatVectors) +TEST(Vector, concatVectors) { Vector A(2); for(int i = 0; i < 2; i++) @@ -187,7 +189,7 @@ TEST( TestVector, concatVectors) } /* ************************************************************************* */ -TEST( TestVector, weightedPseudoinverse ) +TEST(Vector, weightedPseudoinverse ) { // column from a matrix Vector x(2); @@ -213,7 +215,7 @@ TEST( TestVector, weightedPseudoinverse ) } /* ************************************************************************* */ -TEST( TestVector, weightedPseudoinverse_constraint ) +TEST(Vector, weightedPseudoinverse_constraint ) { // column from a matrix Vector x(2); @@ -238,7 +240,7 @@ TEST( TestVector, weightedPseudoinverse_constraint ) } /* ************************************************************************* */ -TEST( TestVector, weightedPseudoinverse_nan ) +TEST(Vector, weightedPseudoinverse_nan ) { Vector a = (Vector(4) << 1., 0., 0., 0.).finished(); Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished(); @@ -252,7 +254,7 @@ TEST( TestVector, weightedPseudoinverse_nan ) } /* ************************************************************************* */ -TEST( TestVector, ediv ) +TEST(Vector, ediv ) { Vector a = Vector3(10., 20., 30.); Vector b = Vector3(2.0, 5.0, 6.0); @@ -263,7 +265,7 @@ TEST( TestVector, ediv ) } /* ************************************************************************* */ -TEST( TestVector, dot ) +TEST(Vector, dot ) { Vector a = Vector3(10., 20., 30.); Vector b = Vector3(2.0, 5.0, 6.0); @@ -271,7 +273,7 @@ TEST( TestVector, dot ) } /* ************************************************************************* */ -TEST( TestVector, axpy ) +TEST(Vector, axpy ) { Vector x = Vector3(10., 20., 30.); Vector y0 = Vector3(2.0, 5.0, 6.0); @@ -284,7 +286,7 @@ TEST( TestVector, axpy ) } /* ************************************************************************* */ -TEST( TestVector, equals ) +TEST(Vector, equals ) { Vector v1 = (Vector(1) << 0.0/std::numeric_limits::quiet_NaN()).finished(); //testing nan Vector v2 = (Vector(1) << 1.0).finished(); @@ -293,7 +295,7 @@ TEST( TestVector, equals ) } /* ************************************************************************* */ -TEST( TestVector, greater_than ) +TEST(Vector, greater_than ) { Vector v1 = Vector3(1.0, 2.0, 3.0), v2 = zero(3); @@ -302,14 +304,14 @@ TEST( TestVector, greater_than ) } /* ************************************************************************* */ -TEST( TestVector, reciprocal ) +TEST(Vector, reciprocal ) { Vector v = Vector3(1.0, 2.0, 4.0); EXPECT(assert_equal(Vector3(1.0, 0.5, 0.25),reciprocal(v))); } /* ************************************************************************* */ -TEST( TestVector, linear_dependent ) +TEST(Vector, linear_dependent ) { Vector v1 = Vector3(1.0, 2.0, 3.0); Vector v2 = Vector3(-2.0, -4.0, -6.0); @@ -317,7 +319,7 @@ TEST( TestVector, linear_dependent ) } /* ************************************************************************* */ -TEST( TestVector, linear_dependent2 ) +TEST(Vector, linear_dependent2 ) { Vector v1 = Vector3(0.0, 2.0, 0.0); Vector v2 = Vector3(0.0, -4.0, 0.0); @@ -325,13 +327,21 @@ TEST( TestVector, linear_dependent2 ) } /* ************************************************************************* */ -TEST( TestVector, linear_dependent3 ) +TEST(Vector, linear_dependent3 ) { Vector v1 = Vector3(0.0, 2.0, 0.0); Vector v2 = Vector3(0.1, -4.1, 0.0); EXPECT(!linear_dependent(v1, v2)); } +//****************************************************************************** +TEST(Vector, IsVectorSpace) { + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + typedef Eigen::Matrix RowVector; + BOOST_CONCEPT_ASSERT((IsVectorSpace)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/base/types.cpp b/gtsam/base/types.cpp index 3f86dc0c1..03e7fd120 100644 --- a/gtsam/base/types.cpp +++ b/gtsam/base/types.cpp @@ -17,10 +17,9 @@ * @addtogroup base */ -#include - #include #include +#include namespace gtsam { diff --git a/gtsam/base/types.h b/gtsam/base/types.h index aca04a14b..a66db13c8 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -21,18 +21,18 @@ #include #include - -#include -#include -#include #include #include #include +#include +#include +#include #ifdef GTSAM_USE_TBB #include #include #include +#include #endif #ifdef GTSAM_USE_EIGEN_MKL_OPENMP diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 64136511d..20073152e 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -25,7 +25,7 @@ #define GTSAM_VERSION_STRING "@GTSAM_VERSION_STRING@" // Paths to example datasets distributed with GTSAM -#define GTSAM_SOURCE_TREE_DATASET_DIR "@CMAKE_SOURCE_DIR@/examples/Data" +#define GTSAM_SOURCE_TREE_DATASET_DIR "@PROJECT_SOURCE_DIR@/examples/Data" #define GTSAM_INSTALLED_DATASET_DIR "@GTSAM_TOOLBOX_INSTALL_PATH@/gtsam_examples/Data" // Whether GTSAM is compiled to use quaternions for Rot3 (otherwise uses rotation matrices) @@ -42,6 +42,9 @@ // Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake) #cmakedefine GTSAM_USE_TBB +// Whether we are using system-Eigen or our own patched version +#cmakedefine GTSAM_USE_SYSTEM_EIGEN + // Whether Eigen will use MKL (if MKL was found and GTSAM_WITH_EIGEN_MKL is enabled in CMake) #cmakedefine GTSAM_USE_EIGEN_MKL #cmakedefine EIGEN_USE_MKL_ALL // This is also defined in gtsam_eigen_includes.h diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 9319a1541..c1648888e 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -462,46 +462,48 @@ namespace gtsam { // cannot just create a root Choice node on the label: if the label is not the // highest label, we need to do a complicated and expensive recursive call. template template - typename DecisionTree::NodePtr DecisionTree::compose( - Iterator begin, Iterator end, const L& label) const { + typename DecisionTree::NodePtr DecisionTree::compose(Iterator begin, + Iterator end, const L& label) const { // find highest label among branches boost::optional highestLabel; - boost::optional nrChoices; + size_t nrChoices = 0; for (Iterator it = begin; it != end; it++) { - if (it->root_->isLeaf()) continue; - boost::shared_ptr c = boost::dynamic_pointer_cast (it->root_); + if (it->root_->isLeaf()) + continue; + boost::shared_ptr c = + boost::dynamic_pointer_cast(it->root_); if (!highestLabel || c->label() > *highestLabel) { highestLabel.reset(c->label()); - nrChoices.reset(c->nrChoices()); + nrChoices = c->nrChoices(); } } // if label is already in correct order, just put together a choice on label - if (!highestLabel || label > *highestLabel) { + if (!nrChoices || !highestLabel || label > *highestLabel) { boost::shared_ptr choiceOnLabel(new Choice(label, end - begin)); for (Iterator it = begin; it != end; it++) choiceOnLabel->push_back(it->root_); return Choice::Unique(choiceOnLabel); - } - - // Set up a new choice on the highest label - boost::shared_ptr choiceOnHighestLabel(new Choice(*highestLabel, *nrChoices)); - // now, for all possible values of highestLabel - for (size_t index = 0; index < *nrChoices; index++) { - // make a new set of functions for composing by iterating over the given - // functions, and selecting the appropriate branch. - std::vector functions; - for (Iterator it = begin; it != end; it++) { - // by restricting the input functions to value i for labelBelow - DecisionTree chosen = it->choose(*highestLabel, index); - functions.push_back(chosen); + } else { + // Set up a new choice on the highest label + boost::shared_ptr choiceOnHighestLabel(new Choice(*highestLabel, nrChoices)); + // now, for all possible values of highestLabel + for (size_t index = 0; index < nrChoices; index++) { + // make a new set of functions for composing by iterating over the given + // functions, and selecting the appropriate branch. + std::vector functions; + for (Iterator it = begin; it != end; it++) { + // by restricting the input functions to value i for labelBelow + DecisionTree chosen = it->choose(*highestLabel, index); + functions.push_back(chosen); + } + // We then recurse, for all values of the highest label + NodePtr fi = compose(functions.begin(), functions.end(), label); + choiceOnHighestLabel->push_back(fi); } - // We then recurse, for all values of the highest label - NodePtr fi = compose(functions.begin(), functions.end(), label); - choiceOnHighestLabel->push_back(fi); + return Choice::Unique(choiceOnHighestLabel); } - return Choice::Unique(choiceOnHighestLabel); } /*********************************************************************************/ @@ -667,9 +669,10 @@ namespace gtsam { void DecisionTree::dot(const std::string& name, bool showZero) const { std::ofstream os((name + ".dot").c_str()); dot(os, showZero); - system( + int result = system( ("dot -Tpdf " + name + ".dot -o " + name + ".pdf >& /dev/null").c_str()); - } + if (result==-1) throw std::runtime_error("DecisionTree::dot system call failed"); +} /*********************************************************************************/ diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index dc7466199..f7253ec24 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -167,4 +167,7 @@ namespace gtsam { }; // DecisionTreeFactor +// traits +template<> struct traits : public Testable {}; + }// namespace gtsam diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index 8cb2db182..dcc336f89 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -90,10 +90,13 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; -} // namespace +// traits +template<> struct traits : public Testable {}; + +} // \ namespace gtsam diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 1ba97444f..88c3e5620 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -130,6 +130,9 @@ public: }; // DiscreteConditional +// traits +template<> struct traits : public Testable {}; + /* ************************************************************************* */ template DiscreteConditional::shared_ptr DiscreteConditional::Combine( diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 8351b310b..e24dfdf2a 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -20,6 +20,7 @@ #include #include +#include namespace gtsam { @@ -101,4 +102,8 @@ public: }; // DiscreteFactor +// traits +template<> struct traits : public Testable {}; +template<> struct traits : public Testable {}; + }// namespace gtsam diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index 27eb722d9..c5b11adf1 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -144,7 +144,10 @@ public: // // /** Apply a reduction, which is a remapping of variable indices. */ // GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction); -}; -// DiscreteFactorGraph -} // namespace gtsam +}; // \ DiscreteFactorGraph + +/// traits +template<> struct traits : public Testable {}; + +} // \ namespace gtsam diff --git a/gtsam/discrete/Potentials.h b/gtsam/discrete/Potentials.h index 51475e07d..978781d63 100644 --- a/gtsam/discrete/Potentials.h +++ b/gtsam/discrete/Potentials.h @@ -88,4 +88,9 @@ namespace gtsam { }; // Potentials +// traits +template<> struct traits : public Testable {}; +template<> struct traits : public Testable {}; + + } // namespace gtsam diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp index f8bcb45c2..970a18b42 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -40,6 +40,11 @@ using namespace gtsam; /* ******************************************************************************** */ typedef AlgebraicDecisionTree ADT; +// traits +namespace gtsam { +template<> struct traits : public Testable {}; +} + #define DISABLE_DOT template diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index 1dfd56eec..05223c67a 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -44,12 +44,22 @@ void dot(const T&f, const string& filename) { struct Crazy { int a; double b; }; typedef DecisionTree CrazyDecisionTree; // check that DecisionTree is actually generic (as it pretends to be) +// traits +namespace gtsam { +template<> struct traits : public Testable {}; +} + /* ******************************************************************************** */ // Test string labels and int range /* ******************************************************************************** */ typedef DecisionTree DT; +// traits +namespace gtsam { +template<> struct traits
: public Testable
{}; +} + struct Ring { static inline int zero() { return 0; diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 8ff728d26..368ae6c98 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -34,20 +34,22 @@ Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) : } /* ************************************************************************* */ -Matrix Cal3Bundler::K() const { +Matrix3 Cal3Bundler::K() const { Matrix3 K; K << f_, 0, u0_, 0, f_, v0_, 0, 0, 1; return K; } /* ************************************************************************* */ -Vector Cal3Bundler::k() const { - return (Vector(4) << k1_, k2_, 0, 0).finished(); +Vector4 Cal3Bundler::k() const { + Vector4 rvalue_; + rvalue_ << k1_, k2_, 0, 0; + return rvalue_; } /* ************************************************************************* */ Vector3 Cal3Bundler::vector() const { - return (Vector(3) << f_, k1_, k2_).finished(); + return Vector3(f_, k1_, k2_); } /* ************************************************************************* */ @@ -64,21 +66,9 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const { return true; } -/* ************************************************************************* */ -Point2 Cal3Bundler::uncalibrate(const Point2& p) const { - // r = x^2 + y^2; - // g = (1 + k(1)*r + k(2)*r^2); - // pi(:,i) = g * pn(:,i) - const double x = p.x(), y = p.y(); - const double r = x * x + y * y; - const double g = 1. + (k1_ + k2_ * r) * r; - const double u = g * x, v = g * y; - return Point2(u0_ + f_ * u, v0_ + f_ * v); -} - /* ************************************************************************* */ Point2 Cal3Bundler::uncalibrate(const Point2& p, // - boost::optional Dcal, boost::optional Dp) const { + OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const { // r = x^2 + y^2; // g = (1 + k(1)*r + k(2)*r^2); // pi(:,i) = g * pn(:,i) @@ -103,35 +93,6 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, // return Point2(u0_ + f_ * u, v0_ + f_ * v); } -/* ************************************************************************* */ -Point2 Cal3Bundler::uncalibrate(const Point2& p, // - boost::optional Dcal, boost::optional Dp) const { - // r = x^2 + y^2; - // g = (1 + k(1)*r + k(2)*r^2); - // pi(:,i) = g * pn(:,i) - const double x = p.x(), y = p.y(); - const double r = x * x + y * y; - const double g = 1. + (k1_ + k2_ * r) * r; - const double u = g * x, v = g * y; - - // Derivatives make use of intermediate variables above - if (Dcal) { - double rx = r * x, ry = r * y; - Dcal->resize(2, 3); - *Dcal << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry; - } - - if (Dp) { - const double a = 2. * (k1_ + 2. * k2_ * r); - const double axx = a * x * x, axy = a * x * y, ayy = a * y * y; - Dp->resize(2,2); - *Dp << g + axx, axy, axy, g + ayy; - *Dp *= f_; - } - - return Point2(u0_ + f_ * u, v0_ + f_ * v); -} - /* ************************************************************************* */ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { // Copied from Cal3DS2 :-( @@ -161,24 +122,25 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { } /* ************************************************************************* */ -Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const { - Matrix Dp; +Matrix2 Cal3Bundler::D2d_intrinsic(const Point2& p) const { + Matrix2 Dp; uncalibrate(p, boost::none, Dp); return Dp; } /* ************************************************************************* */ -Matrix Cal3Bundler::D2d_calibration(const Point2& p) const { - Matrix Dcal; +Matrix23 Cal3Bundler::D2d_calibration(const Point2& p) const { + Matrix23 Dcal; uncalibrate(p, Dcal, boost::none); return Dcal; } /* ************************************************************************* */ -Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const { - Matrix Dcal, Dp; +Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const { + Matrix23 Dcal; + Matrix2 Dp; uncalibrate(p, Dcal, Dp); - Matrix H(2, 5); + Matrix25 H; H << Dp, Dcal; return H; } diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 4c77fdf23..234ee261a 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -37,6 +37,8 @@ private: public: + enum { dimension = 3 }; + /// @name Standard Constructors /// @{ @@ -69,8 +71,8 @@ public: /// @name Standard Interface /// @{ - Matrix K() const; ///< Standard 3*3 calibration matrix - Vector k() const; ///< Radial distortion parameters (4 of them, 2 0) + Matrix3 K() const; ///< Standard 3*3 calibration matrix + Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0) Vector3 vector() const; @@ -106,43 +108,27 @@ public: /** - * convert intrinsic coordinates xy to image coordinates uv - * @param p point in intrinsic coordinates - * @return point in image coordinates - */ - Point2 uncalibrate(const Point2& p) const; - - /** - * Version of uncalibrate with fixed derivatives + * @brief: convert intrinsic coordinates xy to image coordinates uv + * Version of uncalibrate with derivatives * @param p point in intrinsic coordinates * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const; - - /** - * Version of uncalibrate with dynamic derivatives - * @param p point in intrinsic coordinates - * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters - * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates - * @return point in image coordinates - */ - Point2 uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const; + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; /// Conver a pixel coordinate to ideal coordinate Point2 calibrate(const Point2& pi, const double tol = 1e-5) const; /// @deprecated might be removed in next release, use uncalibrate - Matrix D2d_intrinsic(const Point2& p) const; + Matrix2 D2d_intrinsic(const Point2& p) const; /// @deprecated might be removed in next release, use uncalibrate - Matrix D2d_calibration(const Point2& p) const; + Matrix23 D2d_calibration(const Point2& p) const; /// @deprecated might be removed in next release, use uncalibrate - Matrix D2d_intrinsic_calibration(const Point2& p) const; + Matrix25 D2d_intrinsic_calibration(const Point2& p) const; /// @} /// @name Manifold @@ -173,7 +159,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(f_); ar & BOOST_SERIALIZATION_NVP(k1_); ar & BOOST_SERIALIZATION_NVP(k2_); @@ -185,24 +171,10 @@ private: }; -// Define GTSAM traits -namespace traits { +template<> +struct traits : public internal::Manifold {}; template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ -}; - -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ -}; - -template<> -struct GTSAM_EXPORT zero { - static Cal3Bundler value() { - return Cal3Bundler(0, 0, 0); - } -}; - -} +struct traits : public internal::Manifold {}; } // namespace gtsam diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index fb338431a..9f4641f71 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -43,6 +43,8 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base { public: + enum { dimension = 9 }; + /// @name Standard Constructors /// @{ @@ -66,7 +68,7 @@ public: /// @{ /// print with optional string - void print(const std::string& s = "") const ; + virtual void print(const std::string& s = "") const ; /// assert equality up to a tolerance bool equals(const Cal3DS2& K, double tol = 10e-9) const; @@ -87,17 +89,27 @@ public: /// Return dimensions of calibration manifold object static size_t Dim() { return 9; } //TODO: make a final dimension variable + /// @} + /// @name Clone + /// @{ + + /// @return a deep copy of this object + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(new Cal3DS2(*this)); + } + + /// @} + private: - /// @} /// @name Advanced Interface /// @{ /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) + void serialize(Archive & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("Cal3DS2", boost::serialization::base_object(*this)); @@ -107,18 +119,11 @@ private: }; -// Define GTSAM traits -namespace traits { +template<> +struct traits : public internal::Manifold {}; template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ -}; - -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ -}; - -} +struct traits : public internal::Manifold {}; } diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index 1105fecfb..12060c12d 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -28,18 +28,22 @@ Cal3DS2_Base::Cal3DS2_Base(const Vector &v): fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){} /* ************************************************************************* */ -Matrix Cal3DS2_Base::K() const { - return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0).finished(); +Matrix3 Cal3DS2_Base::K() const { + Matrix3 K; + K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0; + return K; } /* ************************************************************************* */ -Vector Cal3DS2_Base::vector() const { - return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_).finished(); +Vector9 Cal3DS2_Base::vector() const { + Vector9 v; + v << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_; + return v; } /* ************************************************************************* */ void Cal3DS2_Base::print(const std::string& s_) const { - gtsam::print(K(), s_ + ".K"); + gtsam::print((Matrix)K(), s_ + ".K"); gtsam::print(Vector(k()), s_ + ".k"); } @@ -91,8 +95,7 @@ static Matrix2 D2dintrinsic(double x, double y, double rr, /* ************************************************************************* */ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, - boost::optional H1, - boost::optional H2) const { + OptionalJacobian<2,9> H1, OptionalJacobian<2,2> H2) const { // rr = x^2 + y^2; // g = (1 + k(1)*rr + k(2)*rr^2); @@ -126,26 +129,6 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); } -/* ************************************************************************* */ -Point2 Cal3DS2_Base::uncalibrate(const Point2& p, - boost::optional H1, - boost::optional H2) const { - - if (H1 || H2) { - Matrix29 D1; - Matrix2 D2; - Point2 pu = uncalibrate(p, D1, D2); - if (H1) - *H1 = D1; - if (H2) - *H2 = D2; - return pu; - - } else { - return uncalibrate(p); - } -} - /* ************************************************************************* */ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const { // Use the following fixed point iteration to invert the radial distortion. @@ -177,7 +160,7 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const { } /* ************************************************************************* */ -Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const { +Matrix2 Cal3DS2_Base::D2d_intrinsic(const Point2& p) const { const double x = p.x(), y = p.y(), xx = x * x, yy = y * y; const double rr = xx + yy; const double r4 = rr * rr; @@ -188,7 +171,7 @@ Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const { } /* ************************************************************************* */ -Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const { +Matrix29 Cal3DS2_Base::D2d_calibration(const Point2& p) const { const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y; const double rr = xx + yy; const double r4 = rr * rr; diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index 61c01e349..cfbdde07c 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -45,9 +45,6 @@ protected: double p1_, p2_ ; // tangential distortion public: - Matrix K() const ; - Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); } - Vector vector() const ; /// @name Standard Constructors /// @{ @@ -59,6 +56,8 @@ public: double k1, double k2, double p1 = 0.0, double p2 = 0.0) : fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {} + virtual ~Cal3DS2_Base() {} + /// @} /// @name Advanced Constructors /// @{ @@ -70,7 +69,7 @@ public: /// @{ /// print with optional string - void print(const std::string& s = "") const ; + virtual void print(const std::string& s = "") const ; /// assert equality up to a tolerance bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const; @@ -106,6 +105,15 @@ public: /// Second tangential distortion coefficient inline double p2() const { return p2_;} + /// return calibration matrix -- not really applicable + Matrix3 K() const; + + /// return distortion parameter vector + Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); } + + /// Return all parameters as a vector + Vector9 vector() const; + /** * convert intrinsic coordinates xy to (distorted) image coordinates uv * @param p point in intrinsic coordinates @@ -113,34 +121,39 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in (distorted) image coordinates */ - Point2 uncalibrate(const Point2& p, - boost::optional Dcal = boost::none, - boost::optional Dp = boost::none) const ; - - Point2 uncalibrate(const Point2& p, - boost::optional Dcal, - boost::optional Dp) const ; + OptionalJacobian<2,9> Dcal = boost::none, + OptionalJacobian<2,2> Dp = boost::none) const ; /// Convert (distorted) image coordinates uv to intrinsic coordinates xy Point2 calibrate(const Point2& p, const double tol=1e-5) const; /// Derivative of uncalibrate wrpt intrinsic coordinates - Matrix D2d_intrinsic(const Point2& p) const ; + Matrix2 D2d_intrinsic(const Point2& p) const ; /// Derivative of uncalibrate wrpt the calibration parameters - Matrix D2d_calibration(const Point2& p) const ; + Matrix29 D2d_calibration(const Point2& p) const ; + + /// @} + /// @name Clone + /// @{ + + /// @return a deep copy of this object + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(new Cal3DS2_Base(*this)); + } + + /// @} private: - /// @} /// @name Advanced Interface /// @{ /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fy_); diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index 6bc4c4bb3..8b7c1abf4 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -29,8 +29,10 @@ Cal3Unified::Cal3Unified(const Vector &v): Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {} /* ************************************************************************* */ -Vector Cal3Unified::vector() const { - return (Vector(10) << Base::vector(), xi_).finished(); +Vector10 Cal3Unified::vector() const { + Vector10 v; + v << Base::vector(), xi_; + return v; } /* ************************************************************************* */ @@ -52,8 +54,8 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const { /* ************************************************************************* */ // todo: make a fixed sized jacobian version of this Point2 Cal3Unified::uncalibrate(const Point2& p, - boost::optional H1, - boost::optional H2) const { + OptionalJacobian<2,10> H1, + OptionalJacobian<2,2> H2) const { // this part of code is modified from Cal3DS2, // since the second part of this model (after project to normalized plane) @@ -81,10 +83,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p, Vector2 DU; DU << -xs * sqrt_nx * xi_sqrt_nx2, // -ys * sqrt_nx * xi_sqrt_nx2; - - H1->resize(2,10); - H1->block<2,9>(0,0) = H1base; - H1->block<2,1>(0,9) = H2base * DU; + *H1 << H1base, H2base * DU; } // Inlined derivative for points @@ -96,7 +95,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p, DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, // mid, (sqrt_nx + xi*(xs*xs + 1)) * denom; - *H2 = H2base * DU; + *H2 << H2base * DU; } return puncalib; @@ -136,7 +135,7 @@ Cal3Unified Cal3Unified::retract(const Vector& d) const { } /* ************************************************************************* */ -Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const { +Vector10 Cal3Unified::localCoordinates(const Cal3Unified& T2) const { return T2.vector() - vector(); } diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index d65093847..99bd04fb1 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -51,7 +51,7 @@ private: public: - Vector vector() const ; + enum { dimension = 10 }; /// @name Standard Constructors /// @{ @@ -63,6 +63,8 @@ public: double k1, double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0) : Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {} + virtual ~Cal3Unified() {} + /// @} /// @name Advanced Constructors /// @{ @@ -74,7 +76,7 @@ public: /// @{ /// print with optional string - void print(const std::string& s = "") const ; + virtual void print(const std::string& s = "") const ; /// assert equality up to a tolerance bool equals(const Cal3Unified& K, double tol = 10e-9) const; @@ -94,8 +96,8 @@ public: * @return point in image coordinates */ Point2 uncalibrate(const Point2& p, - boost::optional Dcal = boost::none, - boost::optional Dp = boost::none) const ; + OptionalJacobian<2,10> Dcal = boost::none, + OptionalJacobian<2,2> Dp = boost::none) const ; /// Conver a pixel coordinate to ideal coordinate Point2 calibrate(const Point2& p, const double tol=1e-5) const; @@ -114,7 +116,7 @@ public: Cal3Unified retract(const Vector& d) const ; /// Given a different calibration, calculate update to obtain it - Vector localCoordinates(const Cal3Unified& T2) const ; + Vector10 localCoordinates(const Cal3Unified& T2) const ; /// Return dimensions of calibration manifold object virtual size_t dim() const { return 10 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) @@ -122,12 +124,17 @@ public: /// Return dimensions of calibration manifold object static size_t Dim() { return 10; } //TODO: make a final dimension variable + /// Return all parameters as a vector + Vector10 vector() const ; + + /// @} + private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) + void serialize(Archive & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("Cal3Unified", boost::serialization::base_object(*this)); @@ -136,23 +143,11 @@ private: }; -// Define GTSAM traits -namespace traits { +template<> +struct traits : public internal::Manifold {}; template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ -}; - -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ -}; - -template<> -struct GTSAM_EXPORT zero { - static Cal3Unified value() { return Cal3Unified();} -}; - -} +struct traits : public internal::Manifold {}; } diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index aece1ded1..3ec29bbd2 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -53,7 +53,7 @@ Cal3_S2::Cal3_S2(const std::string &path) : /* ************************************************************************* */ void Cal3_S2::print(const std::string& s) const { - gtsam::print(matrix(), s); + gtsam::print((Matrix)matrix(), s); } /* ************************************************************************* */ @@ -72,32 +72,13 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { } /* ************************************************************************* */ -Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const { +Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal, + OptionalJacobian<2, 2> Dp) const { const double x = p.x(), y = p.y(); - if (Dcal) { - Dcal->resize(2,5); + if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; - } - if (Dp) { - Dp->resize(2,2); + if (Dp) *Dp << fx_, s_, 0.0, fy_; - } - return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); -} - -/* ************************************************************************* */ -Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const { - const double x = p.x(), y = p.y(); - if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; - if (Dp) *Dp << fx_, s_, 0.0, fy_; - return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); -} - -/* ************************************************************************* */ -Point2 Cal3_S2::uncalibrate(const Point2& p) const { - const double x = p.x(), y = p.y(); return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); } diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 6317d251d..3d5342c92 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -21,7 +21,6 @@ #pragma once -#include #include namespace gtsam { @@ -36,7 +35,7 @@ private: double fx_, fy_, s_, u0_, v0_; public: - + enum { dimension = 5 }; typedef boost::shared_ptr shared_ptr; ///< shared pointer to calibration object /// @name Standard Constructors @@ -117,37 +116,33 @@ public: } /// vectorized form (column-wise) - Vector vector() const { - double r[] = { fx_, fy_, s_, u0_, v0_ }; - Vector v(5); - std::copy(r, r + 5, v.data()); + Vector5 vector() const { + Vector5 v; + v << fx_, fy_, s_, u0_, v0_; return v; } /// return calibration matrix K - Matrix K() const { - return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0).finished(); + Matrix3 K() const { + Matrix3 K; + K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0; + return K; } /** @deprecated The following function has been deprecated, use K above */ - Matrix matrix() const { + Matrix3 matrix() const { return K(); } /// return inverted calibration matrix inv(K) - Matrix matrix_inverse() const { + Matrix3 matrix_inverse() const { const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_; - return (Matrix(3, 3) << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, - 1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0).finished(); + Matrix3 K_inverse; + K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, + 1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0; + return K_inverse; } - /** - * convert intrinsic coordinates xy to image coordinates uv - * @param p point in intrinsic coordinates - * @return point in image coordinates - */ - Point2 uncalibrate(const Point2& p) const; - /** * convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves * @param p point in intrinsic coordinates @@ -155,18 +150,8 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const; - - /** - * convert intrinsic coordinates xy to image coordinates uv, dynamic derivaitves - * @param p point in intrinsic coordinates - * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters - * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates - * @return point in image coordinates - */ - Point2 uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const; + Point2 uncalibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, + OptionalJacobian<2,2> Dp = boost::none) const; /** * convert image coordinates uv to intrinsic coordinates xy @@ -184,10 +169,10 @@ public: /// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q) inline Cal3_S2 between(const Cal3_S2& q, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(5); - if(H2) *H2 = eye(5); + OptionalJacobian<5,5> H1=boost::none, + OptionalJacobian<5,5> H2=boost::none) const { + if(H1) *H1 = -I_5x5; + if(H2) *H2 = I_5x5; return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_); } @@ -212,7 +197,7 @@ public: } /// Unretraction for the calibration - Vector localCoordinates(const Cal3_S2& T2) const { + Vector5 localCoordinates(const Cal3_S2& T2) const { return T2.vector() - vector(); } @@ -225,7 +210,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fy_); ar & BOOST_SERIALIZATION_NVP(s_); @@ -237,22 +222,10 @@ private: }; -// Define GTSAM traits -namespace traits { +template<> +struct traits : public internal::Manifold {}; template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ -}; - -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ -}; - -template<> -struct GTSAM_EXPORT zero { - static Cal3_S2 value() { return Cal3_S2();} -}; - -} +struct traits : public internal::Manifold {}; } // \ namespace gtsam diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 811264967..365a6c40e 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -34,6 +34,7 @@ namespace gtsam { public: + enum { dimension = 6 }; typedef boost::shared_ptr shared_ptr; ///< shared pointer to stereo calibration object /// @name Standard Constructors @@ -52,6 +53,11 @@ namespace gtsam { /// constructor from vector Cal3_S2Stereo(const Vector &d): K_(d(0), d(1), d(2), d(3), d(4)), b_(d(5)){} + /// easy constructor; field-of-view in degrees, assumes zero skew + Cal3_S2Stereo(double fov, int w, int h, double b) : + K_(fov, w, h), b_(b) { + } + /// @} /// @name Testable /// @{ @@ -98,6 +104,38 @@ namespace gtsam { /// return baseline inline double baseline() const { return b_; } + /// vectorized form (column-wise) + Vector6 vector() const { + Vector6 v; + v << K_.vector(), b_; + return v; + } + + /// @} + /// @name Manifold + /// @{ + + /// return DOF, dimensionality of tangent space + inline size_t dim() const { + return 6; + } + + /// return DOF, dimensionality of tangent space + static size_t Dim() { + return 6; + } + + /// Given 6-dim tangent vector, create new calibration + inline Cal3_S2Stereo retract(const Vector& d) const { + return Cal3_S2Stereo(K_.fx() + d(0), K_.fy() + d(1), K_.skew() + d(2), K_.px() + d(3), K_.py() + d(4), b_ + d(5)); + } + + /// Unretraction for the calibration + Vector6 localCoordinates(const Cal3_S2Stereo& T2) const { + return T2.vector() - vector(); + } + + /// @} /// @name Advanced Interface /// @{ @@ -106,7 +144,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(K_); ar & BOOST_SERIALIZATION_NVP(b_); @@ -114,4 +152,14 @@ namespace gtsam { /// @} }; + + // Define GTSAM traits + template<> + struct traits : public internal::Manifold { + }; + + template<> + struct traits : public internal::Manifold { + }; + } // \ namespace gtsam diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 392a53858..33f2c84eb 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -19,83 +19,131 @@ #include #include +using namespace std; + namespace gtsam { /* ************************************************************************* */ -CalibratedCamera::CalibratedCamera(const Pose3& pose) : - pose_(pose) { +Matrix26 PinholeBase::Dpose(const Point2& pn, double d) { + // optimized version of derivatives, see CalibratedCamera.nb + const double u = pn.x(), v = pn.y(); + double uv = u * v, uu = u * u, vv = v * v; + Matrix26 Dpn_pose; + Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v; + return Dpn_pose; } /* ************************************************************************* */ -CalibratedCamera::CalibratedCamera(const Vector &v) : - pose_(Pose3::Expmap(v)) { +Matrix23 PinholeBase::Dpoint(const Point2& pn, double d, const Matrix3& Rt) { + // optimized version of derivatives, see CalibratedCamera.nb + const double u = pn.x(), v = pn.y(); + Matrix23 Dpn_point; + Dpn_point << // + Rt(0, 0) - u * Rt(2, 0), Rt(0, 1) - u * Rt(2, 1), Rt(0, 2) - u * Rt(2, 2), // + /**/Rt(1, 0) - v * Rt(2, 0), Rt(1, 1) - v * Rt(2, 1), Rt(1, 2) - v * Rt(2, 2); + Dpn_point *= d; + return Dpn_point; } /* ************************************************************************* */ -Point2 CalibratedCamera::project_to_camera(const Point3& P, - boost::optional H1) { - if (H1) { - double d = 1.0 / P.z(), d2 = d * d; - *H1 = (Matrix(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2).finished(); +Pose3 PinholeBase::LevelPose(const Pose2& pose2, double height) { + const double st = sin(pose2.theta()), ct = cos(pose2.theta()); + const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); + const Rot3 wRc(x, y, z); + const Point3 t(pose2.x(), pose2.y(), height); + return Pose3(wRc, t); +} + +/* ************************************************************************* */ +Pose3 PinholeBase::LookatPose(const Point3& eye, const Point3& target, + const Point3& upVector) { + Point3 zc = target - eye; + zc = zc / zc.norm(); + Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down + xc = xc / xc.norm(); + Point3 yc = zc.cross(xc); + return Pose3(Rot3(xc, yc, zc), eye); +} + +/* ************************************************************************* */ +bool PinholeBase::equals(const PinholeBase &camera, double tol) const { + return pose_.equals(camera.pose(), tol); +} + +/* ************************************************************************* */ +void PinholeBase::print(const string& s) const { + pose_.print(s + ".pose"); +} + +/* ************************************************************************* */ +const Pose3& PinholeBase::getPose(OptionalJacobian<6, 6> H) const { + if (H) { + H->setZero(); + H->block(0, 0, 6, 6) = I_6x6; } - return Point2(P.x() / P.z(), P.y() / P.z()); + return pose_; } /* ************************************************************************* */ -Point3 CalibratedCamera::backproject_from_camera(const Point2& p, - const double scale) { - return Point3(p.x() * scale, p.y() * scale, scale); +Point2 PinholeBase::project_to_camera(const Point3& pc, + OptionalJacobian<2, 3> Dpoint) { + double d = 1.0 / pc.z(); + const double u = pc.x() * d, v = pc.y() * d; + if (Dpoint) + *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; + return Point2(u, v); +} + +/* ************************************************************************* */ +pair PinholeBase::projectSafe(const Point3& pw) const { + const Point3 pc = pose().transform_to(pw); + const Point2 pn = project_to_camera(pc); + return make_pair(pn, pc.z() > 0); +} + +/* ************************************************************************* */ +Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, 3> Dpoint) const { + + Matrix3 Rt; // calculated by transform_to if needed + const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + if (q.z() <= 0) + throw CheiralityException(); +#endif + const Point2 pn = project_to_camera(q); + + if (Dpose || Dpoint) { + const double d = 1.0 / q.z(); + if (Dpose) + *Dpose = PinholeBase::Dpose(pn, d); + if (Dpoint) + *Dpoint = PinholeBase::Dpoint(pn, d, Rt); + } + return pn; +} + +/* ************************************************************************* */ +Point3 PinholeBase::backproject_from_camera(const Point2& p, + const double depth) { + return Point3(p.x() * depth, p.y() * depth, depth); } /* ************************************************************************* */ CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) { - double st = sin(pose2.theta()), ct = cos(pose2.theta()); - Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); - Rot3 wRc(x, y, z); - Point3 t(pose2.x(), pose2.y(), height); - Pose3 pose3(wRc, t); - return CalibratedCamera(pose3); + return CalibratedCamera(LevelPose(pose2, height)); +} + +/* ************************************************************************* */ +CalibratedCamera CalibratedCamera::Lookat(const Point3& eye, + const Point3& target, const Point3& upVector) { + return CalibratedCamera(LookatPose(eye, target, upVector)); } /* ************************************************************************* */ Point2 CalibratedCamera::project(const Point3& point, - boost::optional Dpose, boost::optional Dpoint) const { - -#ifdef CALIBRATEDCAMERA_CHAIN_RULE - Point3 q = pose_.transform_to(point, Dpose, Dpoint); -#else - Point3 q = pose_.transform_to(point); -#endif - Point2 intrinsic = project_to_camera(q); - - // Check if point is in front of camera - if (q.z() <= 0) - throw CheiralityException(); - - if (Dpose || Dpoint) { -#ifdef CALIBRATEDCAMERA_CHAIN_RULE - // just implement chain rule - Matrix H; - project_to_camera(q,H); - if (Dpose) *Dpose = H * (*Dpose); - if (Dpoint) *Dpoint = H * (*Dpoint); -#else - // optimized version, see CalibratedCamera.nb - const double z = q.z(), d = 1.0 / z; - const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v; - if (Dpose) - *Dpose = (Matrix(2, 6) << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v), - -uv, -u, 0., -d, d * v).finished(); - if (Dpoint) { - const Matrix R(pose_.rotation().matrix()); - *Dpoint = d - * (Matrix(2, 3) << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), - R(2, 0) - u * R(2, 2), R(0, 1) - v * R(0, 2), - R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2)).finished(); - } -#endif - } - return intrinsic; + OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const { + return project2(point, Dcamera, Dpoint); } /* ************************************************************************* */ diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index c66941091..f17cc6441 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -18,12 +18,12 @@ #pragma once -#include -#include #include namespace gtsam { +class Point2; + class GTSAM_EXPORT CheiralityException: public ThreadsafeException< CheiralityException> { public: @@ -32,6 +32,146 @@ public: } }; +/** + * A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT PinholeBase { + +private: + + Pose3 pose_; ///< 3D pose of camera + +protected: + + /// @name Derivatives + /// @{ + + /** + * Calculate Jacobian with respect to pose + * @param pn projection in normalized coordinates + * @param d disparity (inverse depth) + */ + static Matrix26 Dpose(const Point2& pn, double d); + + /** + * Calculate Jacobian with respect to point + * @param pn projection in normalized coordinates + * @param d disparity (inverse depth) + * @param Rt transposed rotation matrix + */ + static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt); + + /// @} + +public: + + /// @name Static functions + /// @{ + + /** + * Create a level pose at the given 2D pose and height + * @param K the calibration + * @param pose2 specifies the location and viewing direction + * (theta 0 = looking in direction of positive X axis) + * @param height camera height + */ + static Pose3 LevelPose(const Pose2& pose2, double height); + + /** + * Create a camera pose at the given eye position looking at a target point in the scene + * with the specified up direction vector. + * @param eye specifies the camera position + * @param target the point to look at + * @param upVector specifies the camera up direction vector, + * doesn't need to be on the image plane nor orthogonal to the viewing axis + */ + static Pose3 LookatPose(const Point3& eye, const Point3& target, + const Point3& upVector); + + /// @} + /// @name Standard Constructors + /// @{ + + /** default constructor */ + PinholeBase() { + } + + /** constructor with pose */ + explicit PinholeBase(const Pose3& pose) : + pose_(pose) { + } + + /// @} + /// @name Advanced Constructors + /// @{ + + explicit PinholeBase(const Vector &v) : + pose_(Pose3::Expmap(v)) { + } + + /// @} + /// @name Testable + /// @{ + + /// assert equality up to a tolerance + bool equals(const PinholeBase &camera, double tol = 1e-9) const; + + /// print + void print(const std::string& s = "PinholeBase") const; + + /// @} + /// @name Standard Interface + /// @{ + + /// return pose, constant version + const Pose3& pose() const { + return pose_; + } + + /// return pose, with derivative + const Pose3& getPose(OptionalJacobian<6, 6> H) const; + + /// @} + /// @name Transformations and measurement functions + /// @{ + + /** + * Project from 3D point in camera coordinates into image + * Does *not* throw a CheiralityException, even if pc behind image plane + * @param pc point in camera coordinates + */ + static Point2 project_to_camera(const Point3& pc, // + OptionalJacobian<2, 3> Dpoint = boost::none); + + /// Project a point into the image and check depth + std::pair projectSafe(const Point3& pw) const; + + /** + * Project point into the image + * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION + * @param point 3D point in world coordinates + * @return the intrinsic coordinates of the projected point + */ + Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose = + boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; + + /// backproject a 2-dimensional point to a 3-dimensional point at given depth + static Point3 backproject_from_camera(const Point2& p, const double depth); + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(pose_); + } + +}; +// end of class PinholeBase + /** * A Calibrated camera class [R|-R't], calibration K=I. * If calibration is known, it is more computationally efficient @@ -39,12 +179,14 @@ public: * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT CalibratedCamera { -private: - Pose3 pose_; // 6DOF pose +class GTSAM_EXPORT CalibratedCamera: public PinholeBase { public: + enum { + dimension = 6 + }; + /// @name Standard Constructors /// @{ @@ -53,26 +195,40 @@ public: } /// construct with pose - explicit CalibratedCamera(const Pose3& pose); + explicit CalibratedCamera(const Pose3& pose) : + PinholeBase(pose) { + } + + /// @} + /// @name Named Constructors + /// @{ + + /** + * Create a level camera at the given 2D pose and height + * @param pose2 specifies the location and viewing direction + * @param height specifies the height of the camera (along the positive Z-axis) + * (theta 0 = looking in direction of positive X axis) + */ + static CalibratedCamera Level(const Pose2& pose2, double height); + + /** + * Create a camera at the given eye position looking at a target point in the scene + * with the specified up direction vector. + * @param eye specifies the camera position + * @param target the point to look at + * @param upVector specifies the camera up direction vector, + * doesn't need to be on the image plane nor orthogonal to the viewing axis + */ + static CalibratedCamera Lookat(const Point3& eye, const Point3& target, + const Point3& upVector); /// @} /// @name Advanced Constructors /// @{ /// construct from vector - explicit CalibratedCamera(const Vector &v); - - /// @} - /// @name Testable - /// @{ - - virtual void print(const std::string& s = "") const { - pose_.print(s); - } - - /// check equality to another camera - bool equals(const CalibratedCamera &camera, double tol = 1e-9) const { - return pose_.equals(camera.pose(), tol); + explicit CalibratedCamera(const Vector &v) : + PinholeBase(v) { } /// @} @@ -83,39 +239,6 @@ public: virtual ~CalibratedCamera() { } - /// return pose - inline const Pose3& pose() const { - return pose_; - } - - /// compose the two camera poses: TODO Frank says this might not make sense - inline const CalibratedCamera compose(const CalibratedCamera &c, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - return CalibratedCamera(pose_.compose(c.pose(), H1, H2)); - } - - /// between the two camera poses: TODO Frank says this might not make sense - inline const CalibratedCamera between(const CalibratedCamera& c, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - return CalibratedCamera(pose_.between(c.pose(), H1, H2)); - } - - /// invert the camera pose: TODO Frank says this might not make sense - inline const CalibratedCamera inverse(boost::optional H1 = - boost::none) const { - return CalibratedCamera(pose_.inverse(H1)); - } - - /** - * Create a level camera at the given 2D pose and height - * @param pose2 specifies the location and viewing direction - * @param height specifies the height of the camera (along the positive Z-axis) - * (theta 0 = looking in direction of positive X axis) - */ - static CalibratedCamera Level(const Pose2& pose2, double height); - /// @} /// @name Manifold /// @{ @@ -126,116 +249,91 @@ public: /// Return canonical coordinate Vector localCoordinates(const CalibratedCamera& T2) const; - /// Lie group dimensionality + /// @deprecated inline size_t dim() const { return 6; } - /// Lie group dimensionality + /// @deprecated inline static size_t Dim() { return 6; } - /* ************************************************************************* */ - // measurement functions and derivatives - /* ************************************************************************* */ - /// @} /// @name Transformations and mesaurement functions /// @{ - /** - * This function receives the camera pose and the landmark location and - * returns the location the point is supposed to appear in the image - * @param point a 3D point to be projected - * @param Dpose the optionally computed Jacobian with respect to pose - * @param Dpoint the optionally computed Jacobian with respect to the 3D point - * @return the intrinsic coordinates of the projected point - */ - Point2 project(const Point3& point, - boost::optional Dpose = boost::none, - boost::optional Dpoint = boost::none) const; /** - * projects a 3-dimensional point in camera coordinates into the - * camera and returns a 2-dimensional point, no calibration applied - * With optional 2by3 derivative + * @deprecated + * Use project2, which is more consistently named across Pinhole cameras */ - static Point2 project_to_camera(const Point3& cameraPoint, - boost::optional H1 = boost::none); + Point2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera = + boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; - /** - * backproject a 2-dimensional point to a 3-dimension point - */ - static Point3 backproject_from_camera(const Point2& p, const double scale); + /// backproject a 2-dimensional point to a 3-dimensional point at given depth + Point3 backproject(const Point2& pn, double depth) const { + return pose().transform_from(backproject_from_camera(pn, depth)); + } /** * Calculate range to a landmark * @param point 3D location of landmark - * @param H1 optionally computed Jacobian with respect to pose - * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const Point3& point, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { - return pose_.range(point, H1, H2); + double range(const Point3& point, + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 3> Dpoint = boost::none) const { + return pose().range(point, Dcamera, Dpoint); } /** * Calculate range to another pose * @param pose Other SO(3) pose - * @param H1 optionally computed Jacobian with respect to pose - * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const Pose3& pose, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { - return pose_.range(pose, H1, H2); + double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 6> Dpose = boost::none) const { + return this->pose().range(pose, Dcamera, Dpose); } /** * Calculate range to another camera * @param camera Other camera - * @param H1 optionally computed Jacobian with respect to pose - * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const CalibratedCamera& camera, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const { - return pose_.range(camera.pose_, H1, H2); + double range(const CalibratedCamera& camera, // + OptionalJacobian<1, 6> H1 = boost::none, // + OptionalJacobian<1, 6> H2 = boost::none) const { + return pose().range(camera.pose(), H1, H2); } + /// @} + private: - /// @} /// @name Advanced Interface /// @{ /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(pose_); + void serialize(Archive & ar, const unsigned int /*version*/) { + ar + & boost::serialization::make_nvp("PinholeBase", + boost::serialization::base_object(*this)); } /// @} }; -// Define GTSAM traits -namespace traits { - template<> -struct GTSAM_EXPORT is_group : public boost::true_type{ +struct traits : public internal::Manifold { }; template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ -}; - -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ +struct traits : public internal::Manifold< + CalibratedCamera> { }; } -} - diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h new file mode 100644 index 000000000..3e40d11a0 --- /dev/null +++ b/gtsam/geometry/CameraSet.h @@ -0,0 +1,123 @@ +/* ---------------------------------------------------------------------------- + + * 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 CameraSet.h + * @brief Base class to create smart factors on poses or cameras + * @author Frank Dellaert + * @date Feb 19, 2015 + */ + +#pragma once + +#include +#include // for Cheirality exception +#include +#include + +namespace gtsam { + +/** + * @brief A set of cameras, all with their own calibration + * Assumes that a camera is laid out as 6 Pose3 parameters then calibration + */ +template +class CameraSet: public std::vector { + +protected: + + /** + * 2D measurement and noise model for each of the m views + * The order is kept the same as the keys that we use to create the factor. + */ + typedef typename CAMERA::Measurement Z; + + static const int ZDim = traits::dimension; ///< Measurement dimension + static const int Dim = traits::dimension; ///< Camera dimension + +public: + + /// Definitions for blocks of F + typedef Eigen::Matrix MatrixZD; // F + typedef std::pair FBlock; // Fblocks + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "") const { + std::cout << s << "CameraSet, cameras = \n"; + for (size_t k = 0; k < this->size(); ++k) + this->at(k).print(); + } + + /// equals + virtual bool equals(const CameraSet& p, double tol = 1e-9) const { + if (this->size() != p.size()) + return false; + bool camerasAreEqual = true; + for (size_t i = 0; i < this->size(); i++) { + if (this->at(i).equals(p.at(i), tol) == false) + camerasAreEqual = false; + break; + } + return camerasAreEqual; + } + + /** + * Project a point, with derivatives in this, point, and calibration + * throws CheiralityException + */ + std::vector project(const Point3& point, boost::optional F = + boost::none, boost::optional E = boost::none, + boost::optional H = boost::none) const { + + size_t nrCameras = this->size(); + if (F) F->resize(ZDim * nrCameras, 6); + if (E) E->resize(ZDim * nrCameras, 3); + if (H && Dim > 6) H->resize(ZDim * nrCameras, Dim - 6); + std::vector z(nrCameras); + + for (size_t i = 0; i < nrCameras; i++) { + Eigen::Matrix Fi; + Eigen::Matrix Ei; + Eigen::Matrix Hi; + z[i] = this->at(i).project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0); + if (F) F->block(ZDim * i, 0) = Fi; + if (E) E->block(ZDim * i, 0) = Ei; + if (H) H->block(ZDim * i, 0) = Hi; + } + return z; + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & (*this); + } +}; + +template +const int CameraSet::ZDim; + +template +struct traits > : public Testable > { +}; + +template +struct traits > : public Testable > { +}; + +} // \ namespace gtsam diff --git a/gtsam/base/Lie-inl.h b/gtsam/geometry/Cyclic.cpp similarity index 53% rename from gtsam/base/Lie-inl.h rename to gtsam/geometry/Cyclic.cpp index f1bb947a2..7242459a6 100644 --- a/gtsam/base/Lie-inl.h +++ b/gtsam/geometry/Cyclic.cpp @@ -10,19 +10,13 @@ * -------------------------------------------------------------------------- */ /** - * @file Lie-inl.h - * @date Jan 9, 2010 - * @author Richard Roberts - * @brief Instantiate macro for Lie type - */ + * @file Cyclic.cpp + * @brief Cyclic group implementation + * @author Frank Dellaert + **/ -#pragma once - -#include - -#define INSTANTIATE_LIE(T) \ - template T between_default(const T&, const T&); \ - template Vector logmap_default(const T&, const T&); \ - template T expmap_default(const T&, const Vector&); +#include +namespace gtsam { +} // \namespace gtsam diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h new file mode 100644 index 000000000..15d8154b8 --- /dev/null +++ b/gtsam/geometry/Cyclic.h @@ -0,0 +1,72 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cyclic.h + * @brief Cyclic group, i.e., the integers modulo N + * @author Frank Dellaert + **/ + +#include +#include +#include // for cout :-( + +namespace gtsam { + +/// Cyclic group of order N +template +class Cyclic { + size_t i_; ///< we just use an unsigned int +public: + /// Constructor + Cyclic(size_t i) : + i_(i) { + assert(i < N); + } + /// Default constructor yields identity + Cyclic():i_(0) { + } + static Cyclic identity() { return Cyclic();} + + /// Cast to size_t + operator size_t() const { + return i_; + } + /// Addition modulo N + Cyclic operator+(const Cyclic& h) const { + return (i_ + h.i_) % N; + } + /// Subtraction modulo N + Cyclic operator-(const Cyclic& h) const { + return (N + i_ - h.i_) % N; + } + /// Inverse + Cyclic operator-() const { + return (N - i_) % N; + } + /// print with optional string + void print(const std::string& s = "") const { + std::cout << s << i_ << std::endl; + } + /// equals with an tolerance, prints out message if unequal + bool equals(const Cyclic& other, double tol = 1e-9) const { + return other.i_ == i_; + } +}; + +/// Define cyclic group to be a model of the Additive Group concept +template +struct traits > : internal::AdditiveGroupTraits >, // + Testable > { +}; + +} // \namespace gtsam + diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index e65e5d097..7b90edc39 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -13,64 +13,46 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, - boost::optional H) { - const Rot3& _1R2_ = _1P2_.rotation(); - const Point3& _1T2_ = _1P2_.translation(); +EssentialMatrix EssentialMatrix::FromPose3(const Pose3& aPb, + OptionalJacobian<5, 6> H) { + const Rot3& aRb = aPb.rotation(); + const Point3& aTb = aPb.translation(); if (!H) { // just make a direction out of translation and create E - Unit3 direction(_1T2_); - return EssentialMatrix(_1R2_, direction); + Unit3 direction(aTb); + return EssentialMatrix(aRb, direction); } else { // Calculate the 5*6 Jacobian H = D_E_1P2 // D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation // First get 2*3 derivative from Unit3::FromPoint3 - Matrix D_direction_1T2; - Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2); - H->resize(5, 6); - H->block<3, 3>(0, 0) << Matrix::Identity(3, 3); // upper left - H->block<2, 3>(3, 0) << Matrix::Zero(2, 3); // lower left - H->block<3, 3>(0, 3) << Matrix::Zero(3, 3); // upper right - H->block<2, 3>(3, 3) << D_direction_1T2 * _1R2_.matrix(); // lower right - return EssentialMatrix(_1R2_, direction); + Matrix23 D_direction_1T2; + Unit3 direction = Unit3::FromPoint3(aTb, D_direction_1T2); + *H << I_3x3, Z_3x3, // + Matrix23::Zero(), D_direction_1T2 * aRb.matrix(); + return EssentialMatrix(aRb, direction); } } /* ************************************************************************* */ void EssentialMatrix::print(const string& s) const { cout << s; - aRb_.print("R:\n"); - aTb_.print("d: "); + rotation().print("R:\n"); + direction().print("d: "); } /* ************************************************************************* */ -EssentialMatrix EssentialMatrix::retract(const Vector& xi) const { - assert(xi.size() == 5); - Vector3 omega(sub(xi, 0, 3)); - Vector2 z(sub(xi, 3, 5)); - Rot3 R = aRb_.retract(omega); - Unit3 t = aTb_.retract(z); - return EssentialMatrix(R, t); -} - -/* ************************************************************************* */ -Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const { - return (Vector(5) << - aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_)).finished(); -} - -/* ************************************************************************* */ -Point3 EssentialMatrix::transform_to(const Point3& p, - boost::optional DE, boost::optional Dpoint) const { - Pose3 pose(aRb_, aTb_.point3()); - Point3 q = pose.transform_to(p, DE, Dpoint); +Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE, + OptionalJacobian<3, 3> Dpoint) const { + Pose3 pose(rotation(), direction().point3()); + Matrix36 DE_; + Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint); if (DE) { // DE returned by pose.transform_to is 3*6, but we need it to be 3*5 // The last 3 columns are derivative with respect to change in translation - // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis() + // The derivative of translation with respect to a 2D sphere delta is 3*2 direction().basis() // Duy made an educated guess that this needs to be rotated to the local frame - Matrix H(3, 5); - H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis(); + Matrix35 H; + H << DE_.block < 3, 3 > (0, 0), -rotation().transpose() * direction().basis(); *DE = H; } return q; @@ -78,29 +60,27 @@ Point3 EssentialMatrix::transform_to(const Point3& p, /* ************************************************************************* */ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, - boost::optional HE, boost::optional HR) const { + OptionalJacobian<5, 5> HE, OptionalJacobian<5, 3> HR) const { // The rotation must be conjugated to act in the camera frame - Rot3 c1Rc2 = aRb_.conjugate(cRb); + Rot3 c1Rc2 = rotation().conjugate(cRb); if (!HE && !HR) { // Rotate translation direction and return - Unit3 c1Tc2 = cRb * aTb_; + Unit3 c1Tc2 = cRb * direction(); return EssentialMatrix(c1Rc2, c1Tc2); } else { // Calculate derivatives - Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2 - Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); - if (HE) { - *HE = zeros(5, 5); - HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2 - HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2) - } + Matrix23 D_c1Tc2_cRb; // 2*3 + Matrix2 D_c1Tc2_aTb; // 2*2 + Unit3 c1Tc2 = cRb.rotate(direction(), D_c1Tc2_cRb, D_c1Tc2_aTb); + if (HE) + *HE << cRb.matrix(), Matrix32::Zero(), // + Matrix23::Zero(), D_c1Tc2_aTb; if (HR) { throw runtime_error( "EssentialMatrix::rotate: derivative HR not implemented yet"); /* - HR->resize(5, 3); HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ? HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ? */ @@ -110,14 +90,13 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, } /* ************************************************************************* */ -double EssentialMatrix::error(const Vector& vA, const Vector& vB, // - boost::optional H) const { +double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // + OptionalJacobian<1, 5> H) const { if (H) { - H->resize(1, 5); // See math.lyx - Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB); - Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) - * aTb_.basis(); + Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB); + Matrix12 HD = vA.transpose() * skewSymmetric(-rotation().matrix() * vB) + * direction().basis(); *H << HR, HD; } return dot(vA, E_ * vB); diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index fd5f45160..697bd462d 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -10,7 +10,8 @@ #include #include #include -#include +#include +#include namespace gtsam { @@ -20,19 +21,22 @@ namespace gtsam { * but here we choose instead to parameterize it as a (Rot3,Unit3) pair. * We can then non-linearly optimize immediately on this 5-dimensional manifold. */ -class GTSAM_EXPORT EssentialMatrix { +class GTSAM_EXPORT EssentialMatrix : private ProductManifold { private: - - Rot3 aRb_; ///< Rotation between a and b - Unit3 aTb_; ///< translation direction from a to b + typedef ProductManifold Base; Matrix3 E_; ///< Essential matrix + /// Construct from Base + EssentialMatrix(const Base& base) : + Base(base), E_(direction().skew() * rotation().matrix()) { + } + public: /// Static function to convert Point2 to homogeneous coordinates - static Vector Homogeneous(const Point2& p) { - return (Vector(3) << p.x(), p.y(), 1).finished(); + static Vector3 Homogeneous(const Point2& p) { + return Vector3(p.x(), p.y(), 1); } /// @name Constructors and named constructors @@ -40,17 +44,17 @@ public: /// Default constructor EssentialMatrix() : - aTb_(1, 0, 0), E_(aTb_.skew()) { + Base(Rot3(), Unit3(1, 0, 0)), E_(direction().skew()) { } /// Construct from rotation and translation EssentialMatrix(const Rot3& aRb, const Unit3& aTb) : - aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) { + Base(aRb, aTb), E_(direction().skew() * rotation().matrix()) { } /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale) static EssentialMatrix FromPose3(const Pose3& _1P2_, - boost::optional H = boost::none); + OptionalJacobian<5, 6> H = boost::none); /// Random, using Rot3::Random and Unit3::Random template @@ -70,7 +74,8 @@ public: /// assert equality up to a tolerance bool equals(const EssentialMatrix& other, double tol = 1e-8) const { - return aRb_.equals(other.aRb_, tol) && aTb_.equals(other.aTb_, tol); + return rotation().equals(other.rotation(), tol) + && direction().equals(other.direction(), tol); } /// @} @@ -78,22 +83,19 @@ public: /// @name Manifold /// @{ - /// Dimensionality of tangent space = 5 DOF - inline static size_t Dim() { - return 5; - } - - /// Return the dimensionality of the tangent space - virtual size_t dim() const { - return 5; - } + using Base::dimension; + using Base::dim; + using Base::Dim; /// Retract delta to manifold - virtual EssentialMatrix retract(const Vector& xi) const; + EssentialMatrix retract(const TangentVector& v) const { + return Base::retract(v); + } /// Compute the coordinates in the tangent space - virtual Vector localCoordinates(const EssentialMatrix& other) const; - + TangentVector localCoordinates(const EssentialMatrix& other) const { + return Base::localCoordinates(other); + } /// @} /// @name Essential matrix methods @@ -101,12 +103,12 @@ public: /// Rotation inline const Rot3& rotation() const { - return aRb_; + return this->first; } /// Direction inline const Unit3& direction() const { - return aTb_; + return this->second; } /// Return 3*3 matrix representation @@ -116,12 +118,12 @@ public: /// Return epipole in image_a , as Unit3 to allow for infinity inline const Unit3& epipole_a() const { - return aTb_; // == direction() + return direction(); } /// Return epipole in image_b, as Unit3 to allow for infinity inline Unit3 epipole_b() const { - return aRb_.unrotate(aTb_); // == rotation.unrotate(direction()) + return rotation().unrotate(direction()); } /** @@ -132,16 +134,16 @@ public: * @return point in pose coordinates */ Point3 transform_to(const Point3& p, - boost::optional DE = boost::none, - boost::optional Dpoint = boost::none) const; + OptionalJacobian<3,5> DE = boost::none, + OptionalJacobian<3,3> Dpoint = boost::none) const; /** * Given essential matrix E in camera frame B, convert to body frame C * @param cRb rotation from body frame to camera frame * @param E essential matrix E in camera frame C */ - EssentialMatrix rotate(const Rot3& cRb, boost::optional HE = - boost::none, boost::optional HR = boost::none) const; + EssentialMatrix rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE = + boost::none, OptionalJacobian<5, 3> HR = boost::none) const; /** * Given essential matrix E in camera frame B, convert to body frame C @@ -153,8 +155,8 @@ public: } /// epipolar error, algebraic - double error(const Vector& vA, const Vector& vB, // - boost::optional H = boost::none) const; + double error(const Vector3& vA, const Vector3& vB, // + OptionalJacobian<1,5> H = boost::none) const; /// @} @@ -177,9 +179,9 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(aRb_); - ar & BOOST_SERIALIZATION_NVP(aTb_); + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(first); + ar & BOOST_SERIALIZATION_NVP(second); ar & boost::serialization::make_nvp("E11", E_(0,0)); ar & boost::serialization::make_nvp("E12", E_(0,1)); @@ -193,26 +195,13 @@ private: } /// @} - -}; - -// Define GTSAM traits -namespace traits { - -template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ }; template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ -}; +struct traits : public internal::Manifold {}; template<> -struct GTSAM_EXPORT zero { - static EssentialMatrix value() { return EssentialMatrix();} -}; - -} +struct traits : public internal::Manifold {}; } // gtsam diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp new file mode 100644 index 000000000..e96708942 --- /dev/null +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -0,0 +1,103 @@ +/* ---------------------------------------------------------------------------- + + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file OrientedPlane3.cpp + * @date Dec 19, 2013 + * @author Alex Trevor + * @brief A plane, represented by a normal direction and perpendicular distance + */ + +#include +#include +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +/// The print fuction +void OrientedPlane3::print(const std::string& s) const { + Vector coeffs = planeCoefficients(); + cout << s << " : " << coeffs << endl; +} + +/* ************************************************************************* */ +OrientedPlane3 OrientedPlane3::Transform(const gtsam::OrientedPlane3& plane, + const gtsam::Pose3& xr, OptionalJacobian<3, 6> Hr, + OptionalJacobian<3, 3> Hp) { + Matrix n_hr; + Matrix n_hp; + Unit3 n_rotated = xr.rotation().unrotate(plane.n_, n_hr, n_hp); + + Vector n_unit = plane.n_.unitVector(); + Vector unit_vec = n_rotated.unitVector(); + double pred_d = n_unit.dot(xr.translation().vector()) + plane.d_; + OrientedPlane3 transformed_plane(unit_vec(0), unit_vec(1), unit_vec(2), + pred_d); + + if (Hr) { + *Hr = gtsam::zeros(3, 6); + (*Hr).block<2, 3>(0, 0) = n_hr; + (*Hr).block<1, 3>(2, 3) = unit_vec; + } + if (Hp) { + Vector xrp = xr.translation().vector(); + Matrix n_basis = plane.n_.basis(); + Vector hpp = n_basis.transpose() * xrp; + *Hp = gtsam::zeros(3, 3); + (*Hp).block<2, 2>(0, 0) = n_hp; + (*Hp).block<1, 2>(2, 0) = hpp; + (*Hp)(2, 2) = 1; + } + + return (transformed_plane); +} + +/* ************************************************************************* */ +Vector3 OrientedPlane3::error(const gtsam::OrientedPlane3& plane) const { + Vector2 n_error = -n_.localCoordinates(plane.n_); + double d_error = d_ - plane.d_; + Vector3 e; + e << n_error(0), n_error(1), d_error; + return (e); +} + +/* ************************************************************************* */ +OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const { + // Retract the Unit3 + Vector2 n_v(v(0), v(1)); + Unit3 n_retracted = n_.retract(n_v); + double d_retracted = d_ + v(2); + return OrientedPlane3(n_retracted, d_retracted); +} + +/* ************************************************************************* */ +Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const { + Vector2 n_local = n_.localCoordinates(y.n_); + double d_local = d_ - y.d_; + Vector3 e; + e << n_local(0), n_local(1), -d_local; + return e; +} + +/* ************************************************************************* */ +Vector3 OrientedPlane3::planeCoefficients() const { + Vector unit_vec = n_.unitVector(); + Vector3 a; + a << unit_vec[0], unit_vec[1], unit_vec[2], d_; + return a; +} + +/* ************************************************************************* */ + +} diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h new file mode 100644 index 000000000..d987ad7b3 --- /dev/null +++ b/gtsam/geometry/OrientedPlane3.h @@ -0,0 +1,116 @@ +/* ---------------------------------------------------------------------------- + + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file OrientedPlane3.h + * @date Dec 19, 2013 + * @author Alex Trevor + * @author Frank Dellaert + * @brief An infinite plane, represented by a normal direction and perpendicular distance + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/// Represents an infinite plane in 3D. +class OrientedPlane3: public DerivedValue { + +private: + + Unit3 n_; /// The direction of the planar normal + double d_; /// The perpendicular distance to this plane + +public: + enum { + dimension = 3 + }; + /// @name Constructors + /// @{ + + /// Default constructor + OrientedPlane3() : + n_(), d_(0.0) { + } + + /// Construct from a Unit3 and a distance + OrientedPlane3(const Unit3& s, double d) : + n_(s), d_(d) { + } + + OrientedPlane3(const Vector4& vec) : + n_(vec(0), vec(1), vec(2)), d_(vec(3)) { + } + + /// Construct from a, b, c, d + OrientedPlane3(double a, double b, double c, double d) { + Point3 p(a, b, c); + n_ = Unit3(p); + d_ = d; + } + + /// The print function + void print(const std::string& s = std::string()) const; + + /// The equals function with tolerance + bool equals(const OrientedPlane3& s, double tol = 1e-9) const { + return (n_.equals(s.n_, tol) && (fabs(d_ - s.d_) < tol)); + } + + /// Transforms a plane to the specified pose + static OrientedPlane3 Transform(const gtsam::OrientedPlane3& plane, + const gtsam::Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none, + OptionalJacobian<3, 3> Hp = boost::none); + + /// Computes the error between two poses + Vector3 error(const gtsam::OrientedPlane3& plane) const; + + /// Dimensionality of tangent space = 3 DOF + inline static size_t Dim() { + return 3; + } + + /// Dimensionality of tangent space = 3 DOF + inline size_t dim() const { + return 3; + } + + /// The retract function + OrientedPlane3 retract(const Vector3& v) const; + + /// The local coordinates function + Vector3 localCoordinates(const OrientedPlane3& s) const; + + /// Returns the plane coefficients (a, b, c, d) + Vector3 planeCoefficients() const; + + inline Unit3 normal() const { + return n_; + } + + /// @} +}; + +template<> struct traits : public internal::Manifold< + OrientedPlane3> { +}; + +template<> struct traits : public internal::Manifold< + OrientedPlane3> { +}; + +} // namespace gtsam + diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index c5174ae65..fb57b0a69 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -18,34 +18,41 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include namespace gtsam { /** * A pinhole camera class that has a Pose3 and a Calibration. + * Use PinholePose if you will not be optimizing for Calibration * @addtogroup geometry * \nosubgrouping */ template -class PinholeCamera { -private: - Pose3 pose_; - Calibration K_; - - static const int DimK = traits::dimension::value; +class GTSAM_EXPORT PinholeCamera: public PinholeBaseK { public: + /** + * Some classes template on either PinholeCamera or StereoCamera, + * and this typedef informs those classes what "project" returns. + */ + typedef Point2 Measurement; + +private: + + typedef PinholeBaseK Base; ///< base class has 3D pose as private member + Calibration K_; ///< Calibration, part of class now + + // Get dimensions of calibration type at compile time + static const int DimK = FixedDimension::value; + +public: + + enum { + dimension = 6 + DimK + }; ///< Dimension depends on calibration + /// @name Standard Constructors /// @{ @@ -55,12 +62,12 @@ public: /** constructor with pose */ explicit PinholeCamera(const Pose3& pose) : - pose_(pose) { + Base(pose) { } /** constructor with pose and calibration */ PinholeCamera(const Pose3& pose, const Calibration& K) : - pose_(pose), K_(K) { + Base(pose), K_(K) { } /// @} @@ -76,12 +83,7 @@ public: */ static PinholeCamera Level(const Calibration &K, const Pose2& pose2, double height) { - const double st = sin(pose2.theta()), ct = cos(pose2.theta()); - const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); - const Rot3 wRc(x, y, z); - const Point3 t(pose2.x(), pose2.y(), height); - const Pose3 pose3(wRc, t); - return PinholeCamera(pose3, K); + return PinholeCamera(Base::LevelPose(pose2, height), K); } /// PinholeCamera::level with default calibration @@ -100,28 +102,23 @@ public: */ static PinholeCamera Lookat(const Point3& eye, const Point3& target, const Point3& upVector, const Calibration& K = Calibration()) { - Point3 zc = target - eye; - zc = zc / zc.norm(); - Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down - xc = xc / xc.norm(); - Point3 yc = zc.cross(xc); - Pose3 pose3(Rot3(xc, yc, zc), eye); - return PinholeCamera(pose3, K); + return PinholeCamera(Base::LookatPose(eye, target, upVector), K); } /// @} /// @name Advanced Constructors /// @{ - explicit PinholeCamera(const Vector &v) { - pose_ = Pose3::Expmap(v.head(Pose3::Dim())); - if (v.size() > Pose3::Dim()) { - K_ = Calibration(v.tail(Calibration::Dim())); - } + /// Init from vector, can be 6D (default calibration) or dim + explicit PinholeCamera(const Vector &v) : + Base(v.head<6>()) { + if (v.size() > 6) + K_ = Calibration(v.tail()); } + /// Init from Vector and calibration PinholeCamera(const Vector &v, const Vector &K) : - pose_(Pose3::Expmap(v)), K_(K) { + Base(v), K_(K) { } /// @} @@ -129,14 +126,14 @@ public: /// @{ /// assert equality up to a tolerance - bool equals(const PinholeCamera &camera, double tol = 1e-9) const { - return pose_.equals(camera.pose(), tol) - && K_.equals(camera.calibration(), tol); + bool equals(const Base &camera, double tol = 1e-9) const { + const PinholeCamera* e = dynamic_cast(&camera); + return Base::equals(camera, tol) && K_.equals(e->calibration(), tol); } /// print void print(const std::string& s = "PinholeCamera") const { - pose_.print(s + ".pose"); + Base::print(s); K_.print(s + ".calibration"); } @@ -148,238 +145,94 @@ public: } /// return pose - inline Pose3& pose() { - return pose_; + const Pose3& pose() const { + return Base::pose(); } - /// return pose - inline const Pose3& pose() const { - return pose_; + /// return pose, with derivative + const Pose3& getPose(OptionalJacobian<6, dimension> H) const { + if (H) { + H->setZero(); + H->block(0, 0, 6, 6) = I_6x6; + } + return Base::pose(); } /// return calibration - inline Calibration& calibration() { + const Calibration& calibration() const { return K_; } - /// return calibration - inline const Calibration& calibration() const { - return K_; - } - - /// @} - /// @name Group ?? Frank says this might not make sense - /// @{ - - /// compose two cameras: TODO Frank says this might not make sense - inline const PinholeCamera compose(const PinholeCamera &c, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - PinholeCamera result(pose_.compose(c.pose(), H1, H2), K_); - if (H1) { - H1->conservativeResize(Dim(), Dim()); - H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), - Calibration::Dim()); - H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - if (H2) { - H2->conservativeResize(Dim(), Dim()); - H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), - Calibration::Dim()); - H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - return result; - } - - /// between two cameras: TODO Frank says this might not make sense - inline const PinholeCamera between(const PinholeCamera& c, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - PinholeCamera result(pose_.between(c.pose(), H1, H2), K_); - if (H1) { - H1->conservativeResize(Dim(), Dim()); - H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), - Calibration::Dim()); - H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - if (H2) { - H2->conservativeResize(Dim(), Dim()); - H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), - Calibration::Dim()); - H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - return result; - } - - /// inverse camera: TODO Frank says this might not make sense - inline const PinholeCamera inverse( - boost::optional H1 = boost::none) const { - PinholeCamera result(pose_.inverse(H1), K_); - if (H1) { - H1->conservativeResize(Dim(), Dim()); - H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), - Calibration::Dim()); - H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - return result; - } - - /// compose two cameras: TODO Frank says this might not make sense - inline const PinholeCamera compose(const Pose3 &c) const { - return PinholeCamera(pose_.compose(c), K_); - } - /// @} /// @name Manifold /// @{ + /// @deprecated + size_t dim() const { + return dimension; + } + + /// @deprecated + static size_t Dim() { + return dimension; + } + + typedef Eigen::Matrix VectorK6; + /// move a cameras according to d PinholeCamera retract(const Vector& d) const { - if ((size_t) d.size() == pose_.dim()) - return PinholeCamera(pose().retract(d), calibration()); + if ((size_t) d.size() == 6) + return PinholeCamera(this->pose().retract(d), calibration()); else - return PinholeCamera(pose().retract(d.head(pose().dim())), + return PinholeCamera(this->pose().retract(d.head(6)), calibration().retract(d.tail(calibration().dim()))); } - typedef Eigen::Matrix VectorK6; - /// return canonical coordinate VectorK6 localCoordinates(const PinholeCamera& T2) const { - VectorK6 d; // TODO: why does d.head<6>() not compile?? - d.head(6) = pose().localCoordinates(T2.pose()); + VectorK6 d; + // TODO: why does d.head<6>() not compile?? + d.head(6) = this->pose().localCoordinates(T2.pose()); d.tail(DimK) = calibration().localCoordinates(T2.calibration()); return d; } - /// Manifold dimension - inline size_t dim() const { - return pose_.dim() + K_.dim(); - } - - /// Manifold dimension - inline static size_t Dim() { - return Pose3::Dim() + Calibration::Dim(); + /// for Canonical + static PinholeCamera identity() { + return PinholeCamera(); // assumes that the default constructor is valid } /// @} /// @name Transformations and measurement functions /// @{ - /** - * projects a 3-dimensional point in camera coordinates into the - * camera and returns a 2-dimensional point, no calibration applied - * @param P A point in camera coordinates - * @param Dpoint is the 2*3 Jacobian w.r.t. P + typedef Eigen::Matrix Matrix2K; + + /** project a point from world coordinate to the image + * @param pw is a point in world coordinates + * @param Dpose is the Jacobian w.r.t. pose3 + * @param Dpoint is the Jacobian w.r.t. point3 + * @param Dcal is the Jacobian w.r.t. calibration */ - inline static Point2 project_to_camera(const Point3& P, - boost::optional Dpoint = boost::none) { -#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - if (P.z() <= 0) - throw CheiralityException(); -#endif - double d = 1.0 / P.z(); - const double u = P.x() * d, v = P.y() * d; + Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 3> Dpoint = boost::none, + OptionalJacobian<2, DimK> Dcal = boost::none) const { + + // project to normalized coordinates + const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint); + + // uncalibrate to pixel coordinates + Matrix2 Dpi_pn; + const Point2 pi = calibration().uncalibrate(pn, Dcal, + Dpose || Dpoint ? &Dpi_pn : 0); + + // If needed, apply chain rule + if (Dpose) + *Dpose = Dpi_pn * *Dpose; if (Dpoint) - *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; - return Point2(u, v); - } + *Dpoint = Dpi_pn * *Dpoint; - /// Project a point into the image and check depth - inline std::pair projectSafe(const Point3& pw) const { - const Point3 pc = pose_.transform_to(pw); - const Point2 pn = project_to_camera(pc); - return std::make_pair(K_.uncalibrate(pn), pc.z() > 0); - } - - /** project a point from world coordinate to the image - * @param pw is a point in world coordinates - */ - inline Point2 project(const Point3& pw) const { - - // Transform to camera coordinates and check cheirality - const Point3 pc = pose_.transform_to(pw); - - // Project to normalized image coordinates - const Point2 pn = project_to_camera(pc); - - return K_.uncalibrate(pn); - } - - typedef Eigen::Matrix Matrix2K; - - /** project a point from world coordinate to the image - * @param pw is a point in world coordinates - * @param Dpose is the Jacobian w.r.t. pose3 - * @param Dpoint is the Jacobian w.r.t. point3 - * @param Dcal is the Jacobian w.r.t. calibration - */ - inline Point2 project( - const Point3& pw, // - boost::optional Dpose, - boost::optional Dpoint, - boost::optional Dcal) const { - - // Transform to camera coordinates and check cheirality - const Point3 pc = pose_.transform_to(pw); - - // Project to normalized image coordinates - const Point2 pn = project_to_camera(pc); - - if (Dpose || Dpoint) { - const double z = pc.z(), d = 1.0 / z; - - // uncalibration - Matrix2 Dpi_pn; - const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); - - // chain the Jacobian matrices - if (Dpose) - calculateDpose(pn, d, Dpi_pn, *Dpose); - if (Dpoint) - calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); - return pi; - } else - return K_.uncalibrate(pn, Dcal, boost::none); - } - - /** project a point from world coordinate to the image - * @param pw is a point in world coordinates - * @param Dpose is the Jacobian w.r.t. pose3 - * @param Dpoint is the Jacobian w.r.t. point3 - * @param Dcal is the Jacobian w.r.t. calibration - */ - inline Point2 project( - const Point3& pw, // - boost::optional Dpose, - boost::optional Dpoint, - boost::optional Dcal) const { - - // Transform to camera coordinates and check cheirality - const Point3 pc = pose_.transform_to(pw); - - // Project to normalized image coordinates - const Point2 pn = project_to_camera(pc); - - if (Dpose || Dpoint) { - const double z = pc.z(), d = 1.0 / z; - - // uncalibration - Matrix Dpi_pn(2, 2); - const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); - - // chain the Jacobian matrices - if (Dpose) { - Dpose->resize(2, 6); - calculateDpose(pn, d, Dpi_pn, *Dpose); - } - if (Dpoint) { - Dpoint->resize(2, 3); - calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); - } - return pi; - } else - return K_.uncalibrate(pn, Dcal, boost::none); + return pi; } /** project a point at infinity from world coordinate to the image @@ -388,203 +241,116 @@ public: * @param Dpoint is the Jacobian w.r.t. point3 * @param Dcal is the Jacobian w.r.t. calibration */ - inline Point2 projectPointAtInfinity( - const Point3& pw, // - boost::optional Dpose = boost::none, - boost::optional Dpoint = boost::none, - boost::optional Dcal = boost::none) const { + Point2 projectPointAtInfinity(const Point3& pw, OptionalJacobian<2, 6> Dpose = + boost::none, OptionalJacobian<2, 2> Dpoint = boost::none, + OptionalJacobian<2, DimK> Dcal = boost::none) const { if (!Dpose && !Dpoint && !Dcal) { - const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter) - const Point2 pn = project_to_camera(pc); // project the point to the camera + const Point3 pc = this->pose().rotation().unrotate(pw); // get direction in camera frame (translation does not matter) + const Point2 pn = Base::project_to_camera(pc); // project the point to the camera return K_.uncalibrate(pn); } // world to camera coordinate - Matrix Dpc_rot /* 3*3 */, Dpc_point /* 3*3 */; - const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point); + Matrix3 Dpc_rot, Dpc_point; + const Point3 pc = this->pose().rotation().unrotate(pw, Dpc_rot, Dpc_point); - Matrix Dpc_pose = Matrix::Zero(3, 6); - Dpc_pose.block(0, 0, 3, 3) = Dpc_rot; + Matrix36 Dpc_pose; + Dpc_pose.setZero(); + Dpc_pose.leftCols<3>() = Dpc_rot; // camera to normalized image coordinate Matrix23 Dpn_pc; // 2*3 - const Point2 pn = project_to_camera(pc, Dpn_pc); + const Point2 pn = Base::project_to_camera(pc, Dpn_pc); // uncalibration - Matrix Dpi_pn; // 2*2 + Matrix2 Dpi_pn; // 2*2 const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); // chain the Jacobian matrices - const Matrix Dpi_pc = Dpi_pn * Dpn_pc; + const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc; if (Dpose) *Dpose = Dpi_pc * Dpc_pose; if (Dpoint) - *Dpoint = (Dpi_pc * Dpc_point).block(0, 0, 2, 2); // only 2dof are important for the point (direction-only) + *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only) return pi; } - typedef Eigen::Matrix Matrix2K6; - - /** project a point from world coordinate to the image - * @param pw is a point in the world coordinate - */ - Point2 project2(const Point3& pw) const { - const Point3 pc = pose_.transform_to(pw); - const Point2 pn = project_to_camera(pc); - return K_.uncalibrate(pn); - } - /** project a point from world coordinate to the image, fixed Jacobians * @param pw is a point in the world coordinate - * @param Dcamera is the Jacobian w.r.t. [pose3 calibration] - * @param Dpoint is the Jacobian w.r.t. point3 */ Point2 project2( const Point3& pw, // - boost::optional Dcamera, - boost::optional Dpoint) const { + OptionalJacobian<2, dimension> Dcamera = boost::none, + OptionalJacobian<2, 3> Dpoint = boost::none) const { - const Point3 pc = pose_.transform_to(pw); - const Point2 pn = project_to_camera(pc); + // project to normalized coordinates + Matrix26 Dpose; + const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint); - if (!Dcamera && !Dpoint) { - return K_.uncalibrate(pn); - } else { - const double z = pc.z(), d = 1.0 / z; + // uncalibrate to pixel coordinates + Matrix2K Dcal; + Matrix2 Dpi_pn; + const Point2 pi = calibration().uncalibrate(pn, Dcamera ? &Dcal : 0, + Dcamera || Dpoint ? &Dpi_pn : 0); - // uncalibration - Matrix2K Dcal; - Matrix2 Dpi_pn; - const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); + // If needed, calculate derivatives + if (Dcamera) + *Dcamera << Dpi_pn * Dpose, Dcal; + if (Dpoint) + *Dpoint = Dpi_pn * (*Dpoint); - if (Dcamera) { // TODO why does leftCols<6>() not compile ?? - calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols(6)); - Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib - } - if (Dpoint) { - calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); - } - return pi; - } - } - - /** project a point from world coordinate to the image - * @param pw is a point in the world coordinate - * @param Dcamera is the Jacobian w.r.t. [pose3 calibration] - * @param Dpoint is the Jacobian w.r.t. point3 - */ - Point2 project2(const Point3& pw, // - boost::optional Dcamera, boost::optional Dpoint) const { - - const Point3 pc = pose_.transform_to(pw); - const Point2 pn = project_to_camera(pc); - - if (!Dcamera && !Dpoint) { - return K_.uncalibrate(pn); - } else { - const double z = pc.z(), d = 1.0 / z; - - // uncalibration - Matrix2K Dcal; - Matrix2 Dpi_pn; - const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); - - if (Dcamera) { - Dcamera->resize(2, this->dim()); - calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols<6>()); - Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib - } - if (Dpoint) { - Dpoint->resize(2, 3); - calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); - } - return pi; - } - } - - /// backproject a 2-dimensional point to a 3-dimensional point at given depth - inline Point3 backproject(const Point2& p, double depth) const { - const Point2 pn = K_.calibrate(p); - const Point3 pc(pn.x() * depth, pn.y() * depth, depth); - return pose_.transform_from(pc); - } - - /// backproject a 2-dimensional point to a 3-dimensional point at infinity - inline Point3 backprojectPointAtInfinity(const Point2& p) const { - const Point2 pn = K_.calibrate(p); - const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1 - return pose_.rotation().rotate(pc); + return pi; } /** * Calculate range to a landmark * @param point 3D location of landmark - * @param Dpose the optionally computed Jacobian with respect to pose - * @param Dpoint the optionally computed Jacobian with respect to the landmark * @return range (double) */ - double range( - const Point3& point, // - boost::optional Dpose = boost::none, - boost::optional Dpoint = boost::none) const { - double result = pose_.range(point, Dpose, Dpoint); - if (Dpose) { - // Add columns of zeros to Jacobian for calibration - Matrix& H1r(*Dpose); - H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim()); - H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim()); - } + double range(const Point3& point, OptionalJacobian<1, dimension> Dcamera = + boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const { + Matrix16 Dpose_; + double result = this->pose().range(point, Dcamera ? &Dpose_ : 0, Dpoint); + if (Dcamera) + *Dcamera << Dpose_, Eigen::Matrix::Zero(); return result; } /** * Calculate range to another pose * @param pose Other SO(3) pose - * @param Dpose the optionally computed Jacobian with respect to pose - * @param Dpose2 the optionally computed Jacobian with respect to the other pose * @return range (double) */ - double range( - const Pose3& pose, // - boost::optional Dpose = boost::none, - boost::optional Dpose2 = boost::none) const { - double result = pose_.range(pose, Dpose, Dpose2); - if (Dpose) { - // Add columns of zeros to Jacobian for calibration - Matrix& H1r(*Dpose); - H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim()); - H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim()); - } + double range(const Pose3& pose, OptionalJacobian<1, dimension> Dcamera = + boost::none, OptionalJacobian<1, 6> Dpose = boost::none) const { + Matrix16 Dpose_; + double result = this->pose().range(pose, Dcamera ? &Dpose_ : 0, Dpose); + if (Dcamera) + *Dcamera << Dpose_, Eigen::Matrix::Zero(); return result; } /** * Calculate range to another camera * @param camera Other camera - * @param Dpose the optionally computed Jacobian with respect to pose - * @param Dother the optionally computed Jacobian with respect to the other camera * @return range (double) */ template - double range( - const PinholeCamera& camera, // - boost::optional Dpose = boost::none, + double range(const PinholeCamera& camera, + OptionalJacobian<1, dimension> Dcamera = boost::none, boost::optional Dother = boost::none) const { - double result = pose_.range(camera.pose_, Dpose, Dother); - if (Dpose) { - // Add columns of zeros to Jacobian for calibration - Matrix& H1r(*Dpose); - H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim()); - H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim()); + Matrix16 Dcamera_, Dother_; + double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0, + Dother ? &Dother_ : 0); + if (Dcamera) { + Dcamera->resize(1, 6 + DimK); + *Dcamera << Dcamera_, Eigen::Matrix::Zero(); } if (Dother) { - // Add columns of zeros to Jacobian for calibration - Matrix& H2r(*Dother); - H2r.conservativeResize(Eigen::NoChange, - camera.pose().dim() + camera.calibration().dim()); - H2r.block(0, camera.pose().dim(), 1, camera.calibration().dim()) = - Matrix::Zero(1, camera.calibration().dim()); + Dother->resize(1, 6 + CalibrationB::dimension); + Dother->setZero(); + Dother->block(0, 0, 1, 6) = Dother_; } return result; } @@ -592,93 +358,36 @@ public: /** * Calculate range to another camera * @param camera Other camera - * @param Dpose the optionally computed Jacobian with respect to pose - * @param Dother the optionally computed Jacobian with respect to the other camera * @return range (double) */ - double range( - const CalibratedCamera& camera, // - boost::optional Dpose = boost::none, - boost::optional Dother = boost::none) const { - return pose_.range(camera.pose_, Dpose, Dother); + double range(const CalibratedCamera& camera, + OptionalJacobian<1, dimension> Dcamera = boost::none, + OptionalJacobian<1, 6> Dother = boost::none) const { + return range(camera.pose(), Dcamera, Dother); } private: - /** - * Calculate Jacobian with respect to pose - * @param pn projection in normalized coordinates - * @param d disparity (inverse depth) - * @param Dpi_pn derivative of uncalibrate with respect to pn - * @param Dpose Output argument, can be matrix or block, assumed right size ! - * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html - */ - template - static void calculateDpose(const Point2& pn, double d, const Matrix2& Dpi_pn, - Eigen::MatrixBase const & Dpose) { - // optimized version of derivatives, see CalibratedCamera.nb - const double u = pn.x(), v = pn.y(); - double uv = u * v, uu = u * u, vv = v * v; - Matrix26 Dpn_pose; - Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v; - assert(Dpose.rows()==2 && Dpose.cols()==6); - const_cast&>(Dpose) = // - Dpi_pn * Dpn_pose; - } - - /** - * Calculate Jacobian with respect to point - * @param pn projection in normalized coordinates - * @param d disparity (inverse depth) - * @param Dpi_pn derivative of uncalibrate with respect to pn - * @param Dpoint Output argument, can be matrix or block, assumed right size ! - * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html - */ - template - static void calculateDpoint(const Point2& pn, double d, const Matrix3& R, - const Matrix2& Dpi_pn, Eigen::MatrixBase const & Dpoint) { - // optimized version of derivatives, see CalibratedCamera.nb - const double u = pn.x(), v = pn.y(); - Matrix23 Dpn_point; - Dpn_point << // - R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), // - /**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2); - Dpn_point *= d; - assert(Dpoint.rows()==2 && Dpoint.cols()==3); - const_cast&>(Dpoint) = // - Dpi_pn * Dpn_point; - } - /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(pose_); + void serialize(Archive & ar, const unsigned int /*version*/) { + ar + & boost::serialization::make_nvp("PinholeBaseK", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(K_); } }; -// Define GTSAM traits -namespace traits { - template -struct GTSAM_EXPORT is_manifold > : public boost::true_type{ +struct traits > : public internal::Manifold< + PinholeCamera > { }; template -struct GTSAM_EXPORT dimension > : public boost::integral_constant< - int, dimension::value + dimension::value> { +struct traits > : public internal::Manifold< + PinholeCamera > { }; -template -struct GTSAM_EXPORT zero > { - static PinholeCamera value() { - return PinholeCamera(zero::value(), - zero::value()); - } -}; - -} - } // \ gtsam diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h new file mode 100644 index 000000000..bfb336f9a --- /dev/null +++ b/gtsam/geometry/PinholePose.h @@ -0,0 +1,349 @@ +/* ---------------------------------------------------------------------------- + + * 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 PinholePose.h + * @brief Pinhole camera with known calibration + * @author Yong-Dian Jian + * @author Frank Dellaert + * @date Feb 20, 2015 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * A pinhole camera class that has a Pose3 and a *fixed* Calibration. + * @addtogroup geometry + * \nosubgrouping + */ +template +class GTSAM_EXPORT PinholeBaseK: public PinholeBase { + + GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration) + +public : + + typedef Calibration CalibrationType; + + /// @name Standard Constructors + /// @{ + + /** default constructor */ + PinholeBaseK() { + } + + /** constructor with pose */ + explicit PinholeBaseK(const Pose3& pose) : + PinholeBase(pose) { + } + + /// @} + /// @name Advanced Constructors + /// @{ + + explicit PinholeBaseK(const Vector &v) : + PinholeBase(v) { + } + + /// @} + /// @name Standard Interface + /// @{ + + virtual ~PinholeBaseK() { + } + + /// return calibration + virtual const Calibration& calibration() const = 0; + + /// @} + /// @name Transformations and measurement functions + /// @{ + + /// Project a point into the image and check depth + std::pair projectSafe(const Point3& pw) const { + std::pair pn = PinholeBase::projectSafe(pw); + pn.first = calibration().uncalibrate(pn.first); + return pn; + } + + /** project a point from world coordinate to the image, fixed Jacobians + * @param pw is a point in the world coordinates + */ + Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 3> Dpoint = boost::none) const { + + // project to normalized coordinates + const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint); + + // uncalibrate to pixel coordinates + Matrix2 Dpi_pn; + const Point2 pi = calibration().uncalibrate(pn, boost::none, + Dpose || Dpoint ? &Dpi_pn : 0); + + // If needed, apply chain rule + if (Dpose) *Dpose = Dpi_pn * (*Dpose); + if (Dpoint) *Dpoint = Dpi_pn * (*Dpoint); + + return pi; + } + + /// backproject a 2-dimensional point to a 3-dimensional point at given depth + Point3 backproject(const Point2& p, double depth) const { + const Point2 pn = calibration().calibrate(p); + return pose().transform_from(backproject_from_camera(pn, depth)); + } + + /// backproject a 2-dimensional point to a 3-dimensional point at infinity + Point3 backprojectPointAtInfinity(const Point2& p) const { + const Point2 pn = calibration().calibrate(p); + const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1 + return pose().rotation().rotate(pc); + } + + /** + * Calculate range to a landmark + * @param point 3D location of landmark + * @return range (double) + */ + double range(const Point3& point, + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 3> Dpoint = boost::none) const { + return pose().range(point, Dcamera, Dpoint); + } + + /** + * Calculate range to another pose + * @param pose Other SO(3) pose + * @return range (double) + */ + double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 6> Dpose = boost::none) const { + return this->pose().range(pose, Dcamera, Dpose); + } + + /** + * Calculate range to a CalibratedCamera + * @param camera Other camera + * @return range (double) + */ + double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> Dcamera = + boost::none, OptionalJacobian<1, 6> Dother = boost::none) const { + return pose().range(camera.pose(), Dcamera, Dother); + } + + /** + * Calculate range to a PinholePoseK derived class + * @param camera Other camera + * @return range (double) + */ + template + double range(const PinholeBaseK& camera, + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 6> Dother = boost::none) const { + return pose().range(camera.pose(), Dcamera, Dother); + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar + & boost::serialization::make_nvp("PinholeBase", + boost::serialization::base_object(*this)); + } + +}; +// end of class PinholeBaseK + +/** + * A pinhole camera class that has a Pose3 and a *fixed* Calibration. + * Instead of using this class, one might consider calibrating the measurements + * and using CalibratedCamera, which would then be faster. + * @addtogroup geometry + * \nosubgrouping + */ +template +class GTSAM_EXPORT PinholePose: public PinholeBaseK { + +private: + + typedef PinholeBaseK Base; ///< base class has 3D pose as private member + boost::shared_ptr K_; ///< shared pointer to fixed calibration + +public: + + enum { + dimension = 6 + }; ///< There are 6 DOF to optimize for + + /// @name Standard Constructors + /// @{ + + /** default constructor */ + PinholePose() { + } + + /** constructor with pose, uses default calibration */ + explicit PinholePose(const Pose3& pose) : + Base(pose), K_(new Calibration()) { + } + + /** constructor with pose and calibration */ + PinholePose(const Pose3& pose, const boost::shared_ptr& K) : + Base(pose), K_(K) { + } + + /// @} + /// @name Named Constructors + /// @{ + + /** + * Create a level camera at the given 2D pose and height + * @param K the calibration + * @param pose2 specifies the location and viewing direction + * (theta 0 = looking in direction of positive X axis) + * @param height camera height + */ + static PinholePose Level(const boost::shared_ptr& K, + const Pose2& pose2, double height) { + return PinholePose(Base::LevelPose(pose2, height), K); + } + + /// PinholePose::level with default calibration + static PinholePose Level(const Pose2& pose2, double height) { + return PinholePose::Level(boost::make_shared(), pose2, height); + } + + /** + * Create a camera at the given eye position looking at a target point in the scene + * with the specified up direction vector. + * @param eye specifies the camera position + * @param target the point to look at + * @param upVector specifies the camera up direction vector, + * doesn't need to be on the image plane nor orthogonal to the viewing axis + * @param K optional calibration parameter + */ + static PinholePose Lookat(const Point3& eye, const Point3& target, + const Point3& upVector, const boost::shared_ptr& K = + boost::make_shared()) { + return PinholePose(Base::LookatPose(eye, target, upVector), K); + } + + /// @} + /// @name Advanced Constructors + /// @{ + + /// Init from 6D vector + explicit PinholePose(const Vector &v) : + Base(v), K_(new Calibration()) { + } + + /// Init from Vector and calibration + PinholePose(const Vector &v, const Vector &K) : + Base(v), K_(new Calibration(K)) { + } + + /// @} + /// @name Testable + /// @{ + + /// assert equality up to a tolerance + bool equals(const Base &camera, double tol = 1e-9) const { + const PinholePose* e = dynamic_cast(&camera); + return Base::equals(camera, tol) && K_->equals(e->calibration(), tol); + } + + /// print + void print(const std::string& s = "PinholePose") const { + Base::print(s); + if (!K_) + std::cout << "s No calibration given" << std::endl; + else + K_->print(s + ".calibration"); + } + + /// @} + /// @name Standard Interface + /// @{ + + virtual ~PinholePose() { + } + + /// return calibration + virtual const Calibration& calibration() const { + return *K_; + } + + /// @} + /// @name Manifold + /// @{ + + /// @deprecated + size_t dim() const { + return 6; + } + + /// @deprecated + static size_t Dim() { + return 6; + } + + /// move a cameras according to d + PinholePose retract(const Vector6& d) const { + return PinholePose(Base::pose().retract(d), K_); + } + + /// return canonical coordinate + Vector6 localCoordinates(const PinholePose& p) const { + return Base::pose().localCoordinates(p.Base::pose()); + } + + /// for Canonical + static PinholePose identity() { + return PinholePose(); // assumes that the default constructor is valid + } + + /// @} + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar + & boost::serialization::make_nvp("PinholeBaseK", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(K_); + } + +}; +// end of class PinholePose + +template +struct traits > : public internal::Manifold< + PinholePose > { +}; + +template +struct traits > : public internal::Manifold< + PinholePose > { +}; + +} // \ gtsam diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 8b90aed63..4b2111efc 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -16,7 +16,6 @@ */ #include -#include #include #include @@ -24,9 +23,6 @@ using namespace std; namespace gtsam { -/** Explicit instantiation of base class to export members */ -INSTANTIATE_LIE(Point2); - /* ************************************************************************* */ void Point2::print(const string& s) const { cout << s << *this << endl; @@ -38,15 +34,9 @@ bool Point2::equals(const Point2& q, double tol) const { } /* ************************************************************************* */ -double Point2::norm() const { - return sqrt(x_ * x_ + y_ * y_); -} - -/* ************************************************************************* */ -double Point2::norm(boost::optional H) const { - double r = norm(); +double Point2::norm(OptionalJacobian<1,2> H) const { + double r = sqrt(x_ * x_ + y_ * y_); if (H) { - H->resize(1,2); if (fabs(r) > 1e-10) *H << x_ / r, y_ / r; else @@ -56,12 +46,11 @@ double Point2::norm(boost::optional H) const { } /* ************************************************************************* */ -static const Matrix I2 = eye(2); -double Point2::distance(const Point2& point, boost::optional H1, - boost::optional H2) const { +double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1, + OptionalJacobian<1,2> H2) const { Point2 d = point - *this; if (H1 || H2) { - Matrix H; + Matrix12 H; double r = d.norm(H); if (H1) *H1 = -H; if (H2) *H2 = H; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index a4e0cc296..56809ae53 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -17,12 +17,9 @@ #pragma once +#include #include -#include -#include -#include - namespace gtsam { /** @@ -33,13 +30,12 @@ namespace gtsam { * \nosubgrouping */ class GTSAM_EXPORT Point2 { - private: double x_, y_; public: - + enum { dimension = 2 }; /// @name Standard Constructors /// @{ @@ -54,9 +50,7 @@ public: /// @{ /// construct from 2D vector - Point2(const Vector& v) { - if(v.size() != 2) - throw std::invalid_argument("Point2 constructor from Vector requires that the Vector have dimension 2"); + Point2(const Vector2& v) { x_ = v(0); y_ = v(1); } @@ -113,66 +107,17 @@ public: /// @{ /// identity - inline static Point2 identity() { - return Point2(); - } + inline static Point2 identity() {return Point2();} - /// "Inverse" - negates each coordinate such that compose(p,inverse(p)) == identity() - inline Point2 inverse() const { return Point2(-x_, -y_); } - - /// syntactic sugar for inverse, i.e., -p == inverse(p) + /// inverse inline Point2 operator- () const {return Point2(-x_,-y_);} - /// "Compose", just adds the coordinates of two points. With optional derivatives - inline Point2 compose(const Point2& q, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = eye(2); - if(H2) *H2 = eye(2); - return *this + q; - } - - /// syntactic sugar for adding two points, i.e., p+q == compose(p,q) + /// add inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} - /// "Between", subtracts point coordinates. between(p,q) == compose(inverse(p),q) - inline Point2 between(const Point2& q, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(2); - if(H2) *H2 = eye(2); - return q - (*this); - } - - /// syntactic sugar for subtracting points, i.e., q-p == between(p,q) + /// subtract inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);} - /// @} - /// @name Manifold - /// @{ - - /// dimension of the variable - used to autodetect sizes - inline static size_t Dim() { return 2; } - - /// Dimensionality of tangent space = 2 DOF - inline size_t dim() const { return 2; } - - /// Updates a with tangent space delta - inline Point2 retract(const Vector& v) const { return *this + Point2(v); } - - /// Local coordinates of manifold neighborhood around current value - inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); } - - /// @} - /// @name Lie Group - /// @{ - - /// Exponential map around identity - just create a Point2 from a vector - static inline Point2 Expmap(const Vector& v) { return Point2(v); } - - /// Log map around identity - just return the Point2 as a vector - static inline Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()).finished(); } - /// @} /// @name Vector Space /// @{ @@ -180,15 +125,12 @@ public: /** creates a unit vector */ Point2 unit() const { return *this/norm(); } - /** norm of point */ - double norm() const; - /** norm of point, with derivative */ - double norm(boost::optional H) const; + double norm(OptionalJacobian<1,2> H = boost::none) const; /** distance between two points */ - double distance(const Point2& p2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none, + OptionalJacobian<1,2> H2 = boost::none) const; /** @deprecated The following function has been deprecated, use distance above */ inline double dist(const Point2& p2) const { @@ -206,7 +148,7 @@ public: /// @{ /// equality - inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;} + inline bool operator ==(const Point2& q) const {return x_==q.x_ && y_==q.y_;} /// get x double x() const {return x_;} @@ -218,10 +160,18 @@ public: Vector2 vector() const { return Vector2(x_, y_); } /// @} - /// @name Deprecated (non-const, non-functional style. Do not use). + + /// @name Deprecated /// @{ inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;} inline void operator *= (double s) {x_*=s;y_*=s;} + Point2 inverse() const { return -(*this);} + Point2 compose(const Point2& q) const { return (*this)+q;} + Point2 between(const Point2& q) const { return q-(*this);} + Vector2 localCoordinates(const Point2& q) const { return between(q).vector();} + Point2 retract(const Vector2& v) const { return compose(Point2(v));} + static Vector2 Logmap(const Point2& p) { return p.vector();} + static Point2 Expmap(const Vector2& v) { return Point2(v);} /// @} /// Streaming @@ -235,35 +185,23 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(x_); ar & BOOST_SERIALIZATION_NVP(y_); } /// @} - }; +// For MATLAB wrapper +typedef std::vector Point2Vector; + /// multiply with scalar inline Point2 operator*(double s, const Point2& p) {return p*s;} -// Define GTSAM traits -namespace traits { - template<> -struct GTSAM_EXPORT is_group : public boost::true_type{ -}; +struct traits : public internal::VectorSpace {}; -template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ -}; - -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ -}; - -} - -} +} // \ namespace gtsam diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 42ecd8c74..a87aeb650 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -15,15 +15,11 @@ */ #include -#include using namespace std; namespace gtsam { -/** Explicit instantiation of base class to export members */ -INSTANTIATE_LIE(Point3); - /* ************************************************************************* */ bool Point3::equals(const Point3 & q, double tol) const { return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol @@ -63,22 +59,18 @@ Point3 Point3::operator/(double s) const { } /* ************************************************************************* */ -Point3 Point3::add(const Point3 &q, boost::optional H1, - boost::optional H2) const { - if (H1) - *H1 = eye(3, 3); - if (H2) - *H2 = eye(3, 3); +Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, + OptionalJacobian<3,3> H2) const { + if (H1) *H1 = I_3x3; + if (H2) *H2 = I_3x3; return *this + q; } /* ************************************************************************* */ -Point3 Point3::sub(const Point3 &q, boost::optional H1, - boost::optional H2) const { - if (H1) - *H1 = eye(3, 3); - if (H2) - *H2 = -eye(3, 3); +Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, + OptionalJacobian<3,3> H2) const { + if (H1) *H1 = I_3x3; + if (H2) *H2 = I_3x3; return *this - q; } @@ -94,15 +86,9 @@ double Point3::dot(const Point3 &q) const { } /* ************************************************************************* */ -double Point3::norm() const { - return sqrt(x_ * x_ + y_ * y_ + z_ * z_); -} - -/* ************************************************************************* */ -double Point3::norm(boost::optional H) const { - double r = norm(); +double Point3::norm(OptionalJacobian<1,3> H) const { + double r = sqrt(x_ * x_ + y_ * y_ + z_ * z_); if (H) { - H->resize(1,3); if (fabs(r) > 1e-10) *H << x_ / r, y_ / r, z_ / r; else @@ -112,13 +98,12 @@ double Point3::norm(boost::optional H) const { } /* ************************************************************************* */ -Point3 Point3::normalize(boost::optional H) const { +Point3 Point3::normalize(OptionalJacobian<3,3> H) const { Point3 normalized = *this / norm(); if (H) { // 3*3 Derivative double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_; double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_; - H->resize(3, 3); *H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2; *H /= pow(x2 + y2 + z2, 1.5); } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index d2dc8d892..883e5fb62 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -21,12 +21,8 @@ #pragma once -#include -#include -#include - +#include #include - #include namespace gtsam { @@ -43,6 +39,7 @@ namespace gtsam { double x_, y_, z_; public: + enum { dimension = 3 }; /// @name Standard Constructors /// @{ @@ -58,9 +55,7 @@ namespace gtsam { /// @{ /// Construct from 3-element vector - Point3(const Vector& v) { - if(v.size() != 3) - throw std::invalid_argument("Point3 constructor from Vector requires that the Vector have dimension 3"); + Point3(const Vector3& v) { x_ = v(0); y_ = v(1); z_ = v(2); @@ -81,76 +76,17 @@ namespace gtsam { /// @{ /// identity for group operation - inline static Point3 identity() { - return Point3(); - } + inline static Point3 identity() { return Point3();} - /// "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() - inline Point3 inverse() const { return Point3(-x_, -y_, -z_); } - - /// syntactic sugar for inverse, i.e., -p == inverse(p) + /// inverse Point3 operator - () const { return Point3(-x_,-y_,-z_);} - /// "Compose" - just adds coordinates of two points - inline Point3 compose(const Point3& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if (H1) *H1 = eye(3); - if (H2) *H2 = eye(3); - return *this + p2; - } - - ///syntactic sugar for adding two points, i.e., p+q == compose(p,q) + /// add Point3 operator + (const Point3& q) const; - /** Between using the default implementation */ - inline Point3 between(const Point3& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(3); - if(H2) *H2 = eye(3); - return p2 - *this; - } - - /// syntactic sugar for subtracting points, i.e., q-p == between(p,q) + /// subtract Point3 operator - (const Point3& q) const; - /// @} - /// @name Manifold - /// @{ - - /// dimension of the variable - used to autodetect sizes - inline static size_t Dim() { return 3; } - - /// return dimensionality of tangent space, DOF = 3 - inline size_t dim() const { return 3; } - - /// Updates a with tangent space delta - inline Point3 retract(const Vector& v) const { return Point3(*this + v); } - - /// Returns inverse retraction - inline Vector3 localCoordinates(const Point3& q) const { return (q -*this).vector(); } - - /// @} - /// @name Lie Group - /// @{ - - /** Exponential map at identity - just create a Point3 from x,y,z */ - static inline Point3 Expmap(const Vector& v) { return Point3(v); } - - /** Log map at identity - return the x,y,z of this point */ - static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); } - - /// Left-trivialized derivative of the exponential map - static Matrix dexpL(const Vector& v) { - return eye(3); - } - - /// Left-trivialized derivative inverse of the exponential map - static Matrix dexpInvL(const Vector& v) { - return eye(3); - } - /// @} /// @name Vector Space /// @{ @@ -163,14 +99,16 @@ namespace gtsam { /** distance between two points */ inline double distance(const Point3& p2, - boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { + OptionalJacobian<1,3> H1 = boost::none, OptionalJacobian<1,3> H2 = boost::none) const { double d = (p2 - *this).norm(); if (H1) { - *H1 = (Matrix(1, 3) << x_-p2.x(), y_-p2.y(), z_-p2.z()).finished()*(1./d); + *H1 << x_-p2.x(), y_-p2.y(), z_-p2.z(); + *H1 = *H1 *(1./d); } if (H2) { - *H2 = (Matrix(1, 3) << -x_+p2.x(), -y_+p2.y(), -z_+p2.z()).finished()*(1./d); + *H2 << -x_+p2.x(), -y_+p2.y(), -z_+p2.z(); + *H2 << *H2 *(1./d); } return d; } @@ -180,14 +118,11 @@ namespace gtsam { return (p2 - *this).norm(); } - /** Distance of the point from the origin */ - double norm() const; - /** Distance of the point from the origin, with Jacobian */ - double norm(boost::optional H) const; + double norm(OptionalJacobian<1,3> H = boost::none) const; /** normalize, with optional Jacobian */ - Point3 normalize(boost::optional H = boost::none) const; + Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const; /** cross product @return this x q */ Point3 cross(const Point3 &q) const; @@ -216,17 +151,28 @@ namespace gtsam { /** add two points, add(this,q) is same as this + q */ Point3 add (const Point3 &q, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + OptionalJacobian<3, 3> H1=boost::none, OptionalJacobian<3, 3> H2=boost::none) const; /** subtract two points, sub(this,q) is same as this - q */ Point3 sub (const Point3 &q, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const; /// @} /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p); + /// @name Deprecated + /// @{ + Point3 inverse() const { return -(*this);} + Point3 compose(const Point3& q) const { return (*this)+q;} + Point3 between(const Point3& q) const { return q-(*this);} + Vector3 localCoordinates(const Point3& q) const { return between(q).vector();} + Point3 retract(const Vector3& v) const { return compose(Point3(v));} + static Vector3 Logmap(const Point3& p) { return p.vector();} + static Point3 Expmap(const Vector3& v) { return Point3(v);} + /// @} + private: /// @name Advanced Interface @@ -235,7 +181,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(x_); ar & BOOST_SERIALIZATION_NVP(y_); @@ -243,26 +189,14 @@ namespace gtsam { } /// @} - }; - /// Syntactic sugar for multiplying coordinates by a scalar s*p - inline Point3 operator*(double s, const Point3& p) { return p*s;} +/// Syntactic sugar for multiplying coordinates by a scalar s*p +inline Point3 operator*(double s, const Point3& p) { return p*s;} - // Define GTSAM traits - namespace traits { +template<> +struct traits : public internal::VectorSpace {}; - template<> - struct GTSAM_EXPORT is_group : public boost::true_type{ - }; - - template<> - struct GTSAM_EXPORT is_manifold : public boost::true_type{ - }; - - template<> - struct GTSAM_EXPORT dimension : public boost::integral_constant{ - }; - - } +template<> +struct traits : public internal::VectorSpace {}; } diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 7a693ed3d..9e87407f4 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -16,9 +16,11 @@ #include #include -#include #include +#include + #include + #include #include #include @@ -27,21 +29,23 @@ using namespace std; namespace gtsam { -/** Explicit instantiation of base class to export members */ -INSTANTIATE_LIE(Pose2); - /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose2); -static const Matrix I3 = eye(3), Z12 = zeros(1,2); static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); /* ************************************************************************* */ -Matrix Pose2::matrix() const { - Matrix R = r_.matrix(); - R = stack(2, &R, &Z12); - Matrix T = (Matrix(3, 1) << t_.x(), t_.y(), 1.0).finished(); - return collect(2, &R, &T); +Matrix3 Pose2::matrix() const { + Matrix2 R = r_.matrix(); + Matrix32 R0; + R0.block<2,2>(0,0) = R; + R0.block<1,2>(2,0).setZero(); + Matrix31 T; + T << t_.x(), t_.y(), 1.0; + Matrix3 RT_; + RT_.block<3,2>(0,0) = R0; + RT_.block<3,1>(0,2) = T; + return RT_; } /* ************************************************************************* */ @@ -55,7 +59,8 @@ bool Pose2::equals(const Pose2& q, double tol) const { } /* ************************************************************************* */ -Pose2 Pose2::Expmap(const Vector& xi) { +Pose2 Pose2::Expmap(const Vector3& xi, OptionalJacobian<3, 3> H) { + if (H) *H = Pose2::ExpmapDerivative(xi); assert(xi.size() == 3); Point2 v(xi(0),xi(1)); double w = xi(2); @@ -70,260 +75,222 @@ Pose2 Pose2::Expmap(const Vector& xi) { } /* ************************************************************************* */ -Vector Pose2::Logmap(const Pose2& p) { +Vector3 Pose2::Logmap(const Pose2& p, OptionalJacobian<3, 3> H) { + if (H) *H = Pose2::LogmapDerivative(p); const Rot2& R = p.r(); const Point2& t = p.t(); double w = R.theta(); if (std::abs(w) < 1e-10) - return (Vector(3) << t.x(), t.y(), w).finished(); + return Vector3(t.x(), t.y(), w); else { double c_1 = R.c()-1.0, s = R.s(); double det = c_1*c_1 + s*s; Point2 p = R_PI_2 * (R.unrotate(t) - t); Point2 v = (w/det) * p; - return (Vector(3) << v.x(), v.y(), w).finished(); + return Vector3(v.x(), v.y(), w); } } /* ************************************************************************* */ -Pose2 Pose2::retract(const Vector& v) const { +Pose2 Pose2::ChartAtOrigin::Retract(const Vector3& v, ChartJacobian H) { #ifdef SLOW_BUT_CORRECT_EXPMAP - return compose(Expmap(v)); + return Expmap(v, H); #else - assert(v.size() == 3); - return compose(Pose2(v[0], v[1], v[2])); + if (H) { + *H = I_3x3; + H->topLeftCorner<2,2>() = Rot2(-v[2]).matrix(); + } + return Pose2(v[0], v[1], v[2]); #endif } - /* ************************************************************************* */ -Vector Pose2::localCoordinates(const Pose2& p2) const { +Vector3 Pose2::ChartAtOrigin::Local(const Pose2& r, ChartJacobian H) { #ifdef SLOW_BUT_CORRECT_EXPMAP - return Logmap(between(p2)); + return Logmap(r, H); #else - Pose2 r = between(p2); - return (Vector(3) << r.x(), r.y(), r.theta()).finished(); + if (H) { + *H = I_3x3; + H->topLeftCorner<2,2>() = r.rotation().matrix(); + } + return Vector3(r.x(), r.y(), r.theta()); #endif } /* ************************************************************************* */ // Calculate Adjoint map // Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) -Matrix Pose2::AdjointMap() const { +Matrix3 Pose2::AdjointMap() const { double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y(); - return (Matrix(3, 3) << + Matrix3 rvalue; + rvalue << c, -s, y, s, c, -x, - 0.0, 0.0, 1.0 - ).finished(); + 0.0, 0.0, 1.0; + return rvalue; } /* ************************************************************************* */ -Pose2 Pose2::inverse(boost::optional H1) const { - if (H1) *H1 = -AdjointMap(); +Matrix3 Pose2::adjointMap(const Vector3& v) { + // See Chirikjian12book2, vol.2, pg. 36 + Matrix3 ad = zeros(3,3); + ad(0,1) = -v[2]; + ad(1,0) = v[2]; + ad(0,2) = v[1]; + ad(1,2) = -v[0]; + return ad; +} + +/* ************************************************************************* */ +Matrix3 Pose2::ExpmapDerivative(const Vector3& v) { + double alpha = v[2]; + Matrix3 J; + if (fabs(alpha) > 1e-5) { + // Chirikjian11book2, pg. 36 + /* !!!Warning!!! Compare Iserles05an, formula 2.42 and Chirikjian11book2 pg.26 + * Iserles' right-trivialization dexpR is actually the left Jacobian J_l in Chirikjian's notation + * In fact, Iserles 2.42 can be written as: + * \dot{g} g^{-1} = dexpR_{q}\dot{q} + * where q = A, and g = exp(A) + * and the LHS is in the definition of J_l in Chirikjian11book2, pg. 26. + * Hence, to compute ExpmapDerivative, we have to use the formula of J_r Chirikjian11book2, pg.36 + */ + double sZalpha = sin(alpha)/alpha, c_1Zalpha = (cos(alpha)-1)/alpha; + double v1Zalpha = v[0]/alpha, v2Zalpha = v[1]/alpha; + J << sZalpha, -c_1Zalpha, v1Zalpha + v2Zalpha*c_1Zalpha - v1Zalpha*sZalpha, + c_1Zalpha, sZalpha, -v1Zalpha*c_1Zalpha + v2Zalpha - v2Zalpha*sZalpha, + 0, 0, 1; + } + else { + // Thanks to Krunal: Apply L'Hospital rule to several times to + // compute the limits when alpha -> 0 + J << 1,0,-0.5*v[1], + 0,1, 0.5*v[0], + 0,0, 1; + } + + return J; +} + +/* ************************************************************************* */ +Matrix3 Pose2::LogmapDerivative(const Pose2& p) { + Vector3 v = Logmap(p); + double alpha = v[2]; + Matrix3 J; + if (fabs(alpha) > 1e-5) { + double alphaInv = 1/alpha; + double halfCotHalfAlpha = 0.5*sin(alpha)/(1-cos(alpha)); + double v1 = v[0], v2 = v[1]; + + J << alpha*halfCotHalfAlpha, -0.5*alpha, v1*alphaInv - v1*halfCotHalfAlpha + 0.5*v2, + 0.5*alpha, alpha*halfCotHalfAlpha, v2*alphaInv - 0.5*v1 - v2*halfCotHalfAlpha, + 0, 0, 1; + } + else { + J << 1,0, 0.5*v[1], + 0,1, -0.5*v[0], + 0,0, 1; + } + return J; +} + +/* ************************************************************************* */ +Pose2 Pose2::inverse() const { return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); } -/* ************************************************************************* */ -// see doc/math.lyx, SE(2) section -Point2 Pose2::transform_to(const Point2& point) const { - Point2 d = point - t_; - return r_.unrotate(d); -} - /* ************************************************************************* */ // see doc/math.lyx, SE(2) section Point2 Pose2::transform_to(const Point2& point, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const { Point2 d = point - t_; Point2 q = r_.unrotate(d); if (!H1 && !H2) return q; if (H1) *H1 << -1.0, 0.0, q.y(), 0.0, -1.0, -q.x(); - if (H2) *H2 = r_.transpose(); + if (H2) *H2 << r_.transpose(); return q; } -/* ************************************************************************* */ -// see doc/math.lyx, SE(2) section -Point2 Pose2::transform_to(const Point2& point, - boost::optional H1, boost::optional H2) const { - Point2 d = point - t_; - Point2 q = r_.unrotate(d); - if (!H1 && !H2) return q; - if (H1) *H1 = (Matrix(2, 3) << - -1.0, 0.0, q.y(), - 0.0, -1.0, -q.x()).finished(); - if (H2) *H2 = r_.transpose(); - return q; -} - -/* ************************************************************************* */ -// see doc/math.lyx, SE(2) section -Pose2 Pose2::compose(const Pose2& p2, boost::optional H1, - boost::optional H2) const { - // TODO: inline and reuse? - if(H1) *H1 = p2.inverse().AdjointMap(); - if(H2) *H2 = I3; - return (*this)*p2; -} - /* ************************************************************************* */ // see doc/math.lyx, SE(2) section Point2 Pose2::transform_from(const Point2& p, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const { const Point2 q = r_ * p; if (H1 || H2) { - const Matrix R = r_.matrix(); - const Matrix Drotate1 = (Matrix(2, 1) << -q.y(), q.x()).finished(); - if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q] - if (H2) *H2 = R; // R + const Matrix2 R = r_.matrix(); + Matrix21 Drotate1; + Drotate1 << -q.y(), q.x(); + if (H1) *H1 << R, Drotate1; + if (H2) *H2 = R; // R } return q + t_; } -/* ************************************************************************* */ -Pose2 Pose2::between(const Pose2& p2) const { - // get cosines and sines from rotation matrices - const Rot2& R1 = r_, R2 = p2.r(); - double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); - - // Assert that R1 and R2 are normalized - assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); - - // Calculate delta rotation = between(R1,R2) - double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; - Rot2 R(Rot2::atan2(s,c)); // normalizes - - // Calculate delta translation = unrotate(R1, dt); - Point2 dt = p2.t() - t_; - double x = dt.x(), y = dt.y(); - // t = R1' * (t2-t1) - Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); - - return Pose2(R,t); -} - -/* ************************************************************************* */ -Pose2 Pose2::between(const Pose2& p2, boost::optional H1, - boost::optional H2) const { - // get cosines and sines from rotation matrices - const Rot2& R1 = r_, R2 = p2.r(); - double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); - - // Assert that R1 and R2 are normalized - assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); - - // Calculate delta rotation = between(R1,R2) - double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; - Rot2 R(Rot2::atan2(s,c)); // normalizes - - // Calculate delta translation = unrotate(R1, dt); - Point2 dt = p2.t() - t_; - double x = dt.x(), y = dt.y(); - // t = R1' * (t2-t1) - Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); - - // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above - if (H1) { - double dt1 = -s2 * x + c2 * y; - double dt2 = -c2 * x - s2 * y; - *H1 << - -c, -s, dt1, - s, -c, dt2, - 0.0, 0.0,-1.0; - } - if (H2) *H2 = I3; - - return Pose2(R,t); -} - -/* ************************************************************************* */ -Pose2 Pose2::between(const Pose2& p2, boost::optional H1, - boost::optional H2) const { - // get cosines and sines from rotation matrices - const Rot2& R1 = r_, R2 = p2.r(); - double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); - - // Assert that R1 and R2 are normalized - assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); - - // Calculate delta rotation = between(R1,R2) - double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; - Rot2 R(Rot2::atan2(s,c)); // normalizes - - // Calculate delta translation = unrotate(R1, dt); - Point2 dt = p2.t() - t_; - double x = dt.x(), y = dt.y(); - // t = R1' * (t2-t1) - Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); - - // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above - if (H1) { - double dt1 = -s2 * x + c2 * y; - double dt2 = -c2 * x - s2 * y; - *H1 = (Matrix(3, 3) << - -c, -s, dt1, - s, -c, dt2, - 0.0, 0.0,-1.0).finished(); - } - if (H2) *H2 = I3; - - return Pose2(R,t); -} - /* ************************************************************************* */ Rot2 Pose2::bearing(const Point2& point, - boost::optional H1, boost::optional H2) const { - Point2 d = transform_to(point, H1, H2); + OptionalJacobian<1, 3> H1, OptionalJacobian<1, 2> H2) const { + // make temporary matrices + Matrix23 D1; Matrix2 D2; + Point2 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); // uses pointer version if (!H1 && !H2) return Rot2::relativeBearing(d); - Matrix D_result_d; + Matrix12 D_result_d; Rot2 result = Rot2::relativeBearing(d, D_result_d); - if (H1) *H1 = D_result_d * (*H1); - if (H2) *H2 = D_result_d * (*H2); + if (H1) *H1 = D_result_d * (D1); + if (H2) *H2 = D_result_d * (D2); return result; } /* ************************************************************************* */ -Rot2 Pose2::bearing(const Pose2& point, - boost::optional H1, boost::optional H2) const { - Rot2 result = bearing(point.t(), H1, H2); +Rot2 Pose2::bearing(const Pose2& pose, + OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const { + Matrix12 D2; + Rot2 result = bearing(pose.t(), H1, H2 ? &D2 : 0); if (H2) { - Matrix H2_ = *H2 * point.r().matrix(); - *H2 = zeros(1, 3); - insertSub(*H2, H2_, 0, 0); + Matrix12 H2_ = D2 * pose.r().matrix(); + *H2 << H2_, Z_1x1; } return result; } - /* ************************************************************************* */ double Pose2::range(const Point2& point, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<1,3> H1, OptionalJacobian<1,2> H2) const { Point2 d = point - t_; if (!H1 && !H2) return d.norm(); - Matrix H; + Matrix12 H; double r = d.norm(H); - if (H1) *H1 = H * (Matrix(2, 3) << - -r_.c(), r_.s(), 0.0, - -r_.s(), -r_.c(), 0.0).finished(); + if (H1) { + Matrix23 H1_; + H1_ << -r_.c(), r_.s(), 0.0, + -r_.s(), -r_.c(), 0.0; + *H1 = H * H1_; + } if (H2) *H2 = H; return r; } /* ************************************************************************* */ -double Pose2::range(const Pose2& pose2, - boost::optional H1, - boost::optional H2) const { - Point2 d = pose2.t() - t_; +double Pose2::range(const Pose2& pose, + OptionalJacobian<1,3> H1, + OptionalJacobian<1,3> H2) const { + Point2 d = pose.t() - t_; if (!H1 && !H2) return d.norm(); - Matrix H; + Matrix12 H; double r = d.norm(H); - if (H1) *H1 = H * (Matrix(2, 3) << + if (H1) { + Matrix23 H1_; + H1_ << -r_.c(), r_.s(), 0.0, - -r_.s(), -r_.c(), 0.0).finished(); - if (H2) *H2 = H * (Matrix(2, 3) << - pose2.r_.c(), -pose2.r_.s(), 0.0, - pose2.r_.s(), pose2.r_.c(), 0.0).finished(); + -r_.s(), -r_.c(), 0.0; + *H1 = H * H1_; + } + if (H2) { + Matrix23 H2_; + H2_ << + pose.r_.c(), -pose.r_.s(), 0.0, + pose.r_.s(), pose.r_.c(), 0.0; + *H2 = H * H2_; + } return r; } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index d43be6afb..f3cb9e2a1 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -20,11 +20,9 @@ #pragma once -#include -#include -#include #include #include +#include namespace gtsam { @@ -33,7 +31,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Pose2 { +class GTSAM_EXPORT Pose2: public LieGroup { public: @@ -42,6 +40,7 @@ public: typedef Point2 Translation; private: + Rot2 r_; Point2 t_; @@ -106,72 +105,38 @@ public: /// identity for group operation inline static Pose2 identity() { return Pose2(); } - /// inverse transformation with derivatives - Pose2 inverse(boost::optional H1=boost::none) const; - - /// compose this transformation onto another (first *this and then p2) - Pose2 compose(const Pose2& p2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + /// inverse + Pose2 inverse() const; /// compose syntactic sugar inline Pose2 operator*(const Pose2& p2) const { return Pose2(r_*p2.r(), t_ + r_*p2.t()); } - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - */ - Pose2 between(const Pose2& p2) const; - - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - */ - Pose2 between(const Pose2& p2, boost::optional H1, - boost::optional H2) const; - - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - */ - Pose2 between(const Pose2& p2, boost::optional H1, - boost::optional H2) const; - - /// @} - /// @name Manifold - /// @{ - - /// Dimensionality of tangent space = 3 DOF - used to autodetect sizes - inline static size_t Dim() { return 3; } - - /// Dimensionality of tangent space = 3 DOF - inline size_t dim() const { return 3; } - - /// Retraction from R^3 \f$ [T_x,T_y,\theta] \f$ to Pose2 manifold neighborhood around current pose - Pose2 retract(const Vector& v) const; - - /// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose - Vector localCoordinates(const Pose2& p2) const; - /// @} /// @name Lie Group /// @{ ///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$ - static Pose2 Expmap(const Vector& xi); + static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none); ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation - static Vector Logmap(const Pose2& p); + static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none); /** * Calculate Adjoint map * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi) */ - Matrix AdjointMap() const; - inline Vector Adjoint(const Vector& xi) const { - assert(xi.size() == 3); + Matrix3 AdjointMap() const; + inline Vector3 Adjoint(const Vector3& xi) const { return AdjointMap()*xi; } + /** + * Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19 + */ + static Matrix3 adjointMap(const Vector3& v); + /** * wedge for SE(2): * @param xi 3-dim twist (v,omega) where @@ -179,34 +144,41 @@ public: * v (vx,vy) = 2D velocity * @return xihat, 3*3 element of Lie algebra that can be exponentiated */ - static inline Matrix wedge(double vx, double vy, double w) { - return (Matrix(3,3) << - 0.,-w, vx, - w, 0., vy, - 0., 0., 0.).finished(); + static inline Matrix3 wedge(double vx, double vy, double w) { + Matrix3 m; + m << 0.,-w, vx, + w, 0., vy, + 0., 0., 0.; + return m; } + /// Derivative of Expmap + static Matrix3 ExpmapDerivative(const Vector3& v); + + /// Derivative of Logmap + static Matrix3 LogmapDerivative(const Pose2& v); + + // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP + struct ChartAtOrigin { + static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none); + static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none); + }; + + using LieGroup::inverse; // version with derivative + /// @} /// @name Group Action on Point2 /// @{ - /** Return point coordinates in pose coordinate frame */ - Point2 transform_to(const Point2& point) const; - /** Return point coordinates in pose coordinate frame */ Point2 transform_to(const Point2& point, - boost::optional H1, - boost::optional H2) const; - - /** Return point coordinates in pose coordinate frame */ - Point2 transform_to(const Point2& point, - boost::optional H1, - boost::optional H2) const; + OptionalJacobian<2, 3> H1 = boost::none, + OptionalJacobian<2, 2> H2 = boost::none) const; /** Return point coordinates in global frame */ Point2 transform_from(const Point2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<2, 3> H1 = boost::none, + OptionalJacobian<2, 2> H2 = boost::none) const; /** syntactic sugar for transform_from */ inline Point2 operator*(const Point2& point) const { return transform_from(point);} @@ -237,7 +209,7 @@ public: inline const Rot2& rotation() const { return r_; } //// return transformation matrix - Matrix matrix() const; + Matrix3 matrix() const; /** * Calculate bearing to a landmark @@ -245,17 +217,15 @@ public: * @return 2D rotation \f$ \in SO(2) \f$ */ Rot2 bearing(const Point2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const; /** * Calculate bearing to another pose * @param point SO(2) location of other pose * @return 2D rotation \f$ \in SO(2) \f$ */ - Rot2 bearing(const Pose2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + Rot2 bearing(const Pose2& pose, + OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const; /** * Calculate range to a landmark @@ -263,8 +233,8 @@ public: * @return range (double) */ double range(const Point2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<1, 3> H1=boost::none, + OptionalJacobian<1, 2> H2=boost::none) const; /** * Calculate range to another pose @@ -272,8 +242,8 @@ public: * @return range (double) */ double range(const Pose2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<1, 3> H1=boost::none, + OptionalJacobian<1, 3> H2=boost::none) const; /// @} /// @name Advanced Interface @@ -300,7 +270,7 @@ private: // Serialization function friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(t_); ar & BOOST_SERIALIZATION_NVP(r_); } @@ -319,18 +289,11 @@ inline Matrix wedge(const Vector& xi) { typedef std::pair Point2Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); -// Define GTSAM traits -namespace traits { +template<> +struct traits : public internal::LieGroup {}; template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ -}; - -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ -}; - -} +struct traits : public internal::LieGroup {}; } // namespace gtsam diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index b134553d9..32fd75eb8 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -17,7 +17,8 @@ #include #include #include -#include +#include + #include #include #include @@ -26,21 +27,21 @@ using namespace std; namespace gtsam { -/** Explicit instantiation of base class to export members */ -INSTANTIATE_LIE(Pose3); - /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose3); -static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3 = -I3; -static const Matrix6 I6 = eye(6); - /* ************************************************************************* */ Pose3::Pose3(const Pose2& pose2) : R_(Rot3::rodriguez(0, 0, pose2.theta())), t_( Point3(pose2.x(), pose2.y(), 0)) { } +/* ************************************************************************* */ +Pose3 Pose3::inverse() const { + Rot3 Rt = R_.inverse(); + return Pose3(Rt, Rt * (-t_)); +} + /* ************************************************************************* */ // Calculate Adjoint map // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) @@ -50,69 +51,52 @@ Matrix6 Pose3::AdjointMap() const { const Vector3 t = t_.vector(); Matrix3 A = skewSymmetric(t) * R; Matrix6 adj; - adj << R, Z3, A, R; + adj << R, Z_3x3, A, R; return adj; } /* ************************************************************************* */ -Matrix6 Pose3::adjointMap(const Vector& xi) { +Matrix6 Pose3::adjointMap(const Vector6& xi) { Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); Matrix6 adj; - adj << w_hat, Z3, v_hat, w_hat; + adj << w_hat, Z_3x3, v_hat, w_hat; return adj; } /* ************************************************************************* */ -Vector Pose3::adjoint(const Vector& xi, const Vector& y, - boost::optional H) { +Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, + OptionalJacobian<6,6> H) { if (H) { - *H = zeros(6, 6); + H->setZero(); for (int i = 0; i < 6; ++i) { - Vector dxi = zero(6); + Vector6 dxi; + dxi.setZero(); dxi(i) = 1.0; - Matrix Gi = adjointMap(dxi); - (*H).col(i) = Gi * y; + Matrix6 Gi = adjointMap(dxi); + H->col(i) = Gi * y; } } return adjointMap(xi) * y; } /* ************************************************************************* */ -Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y, - boost::optional H) { +Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, + OptionalJacobian<6,6> H) { if (H) { - *H = zeros(6, 6); + H->setZero(); for (int i = 0; i < 6; ++i) { - Vector dxi = zero(6); + Vector6 dxi; + dxi.setZero(); dxi(i) = 1.0; - Matrix GTi = adjointMap(dxi).transpose(); - (*H).col(i) = GTi * y; + Matrix6 GTi = adjointMap(dxi).transpose(); + H->col(i) = GTi * y; } } - Matrix adjT = adjointMap(xi).transpose(); return adjointMap(xi).transpose() * y; } -/* ************************************************************************* */ -Matrix6 Pose3::dExpInv_exp(const Vector& xi) { - // Bernoulli numbers, from Wikipedia - static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0, - 0.0, 1.0 / 42.0, 0.0, -1.0 / 30).finished(); - static const int N = 5; // order of approximation - Matrix res = I6; - Matrix6 ad_i = I6; - Matrix6 ad_xi = adjointMap(xi); - double fac = 1.0; - for (int i = 1; i < N; ++i) { - ad_i = ad_xi * ad_i; - fac = fac * i; - res = res + B(i) / fac * ad_i; - } - return res; -} - /* ************************************************************************* */ void Pose3::print(const string& s) const { cout << s; @@ -127,7 +111,10 @@ bool Pose3::equals(const Pose3& pose, double tol) const { /* ************************************************************************* */ /** Modified from Murray94book version (which assumes w and v normalized?) */ -Pose3 Pose3::Expmap(const Vector& xi) { +Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) { + if (H) { + *H = ExpmapDerivative(xi); + } // get angular velocity omega and translational velocity v from twist xi Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); @@ -147,7 +134,8 @@ Pose3 Pose3::Expmap(const Vector& xi) { } /* ************************************************************************* */ -Vector6 Pose3::Logmap(const Pose3& p) { +Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) { + if (H) *H = LogmapDerivative(p); Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); double t = w.norm(); if (t < 1e-10) { @@ -168,64 +156,119 @@ Vector6 Pose3::Logmap(const Pose3& p) { } /* ************************************************************************* */ -Pose3 Pose3::retractFirstOrder(const Vector& xi) const { - Vector3 omega(sub(xi, 0, 3)); - Point3 v(sub(xi, 3, 6)); - Rot3 R = R_.retract(omega); // R is done exactly - Point3 t = t_ + R_ * v; // First order t approximation - return Pose3(R, t); +Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) { +#ifdef GTSAM_POSE3_EXPMAP + return Expmap(xi, H); +#else + Matrix3 DR; + Rot3 R = Rot3::Retract(xi.head<3>(), H ? &DR : 0); + if (H) { + *H = I_6x6; + H->topLeftCorner<3,3>() = DR; + } + return Pose3(R, Point3(xi.tail<3>())); +#endif } /* ************************************************************************* */ -// Different versions of retract -Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const { - if (mode == Pose3::EXPMAP) { - // Lie group exponential map, traces out geodesic - return compose(Expmap(xi)); - } else if (mode == Pose3::FIRST_ORDER) { - // First order - return retractFirstOrder(xi); - } else { - // Point3 t = t_.retract(v.vector()); // Incorrect version retracts t independently - // Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); // Second order t approximation - assert(false); - exit(1); +Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) { +#ifdef GTSAM_POSE3_EXPMAP + return Logmap(T, H); +#else + Matrix3 DR; + Vector3 omega = Rot3::LocalCoordinates(T.rotation(), H ? &DR : 0); + if (H) { + *H = I_6x6; + H->topLeftCorner<3,3>() = DR; } + Vector6 xi; + xi << omega, T.translation().vector(); + return xi; +#endif } /* ************************************************************************* */ -// different versions of localCoordinates -Vector6 Pose3::localCoordinates(const Pose3& T, - Pose3::CoordinatesMode mode) const { - if (mode == Pose3::EXPMAP) { - // Lie group logarithm map, exact inverse of exponential map - return Logmap(between(T)); - } else if (mode == Pose3::FIRST_ORDER) { - // R is always done exactly in all three retract versions below - Vector3 omega = R_.localCoordinates(T.rotation()); +/** + * Compute the 3x3 bottom-left block Q of the SE3 Expmap derivative matrix + * J(xi) = [J_(w) Z_3x3; + * Q J_(w)] + * where J_(w) is the SO3 Expmap derivative. + * (see Chirikjian11book2, pg 44, eq 10.95. + * The closed-form formula is similar to formula 102 in Barfoot14tro) + */ +static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { + Vector3 w(sub(xi, 0, 3)); + Vector3 v(sub(xi, 3, 6)); + Matrix3 V = skewSymmetric(v); + Matrix3 W = skewSymmetric(w); - // Incorrect version - // Independently computes the logmap of the translation and rotation - // Vector v = t_.localCoordinates(T.translation()); + Matrix3 Q; - // Correct first order t inverse - Point3 d = R_.unrotate(T.translation() - t_); - - // TODO: correct second order t inverse - Vector6 local; - local << omega(0), omega(1), omega(2), d.x(), d.y(), d.z(); - return local; - } else { - assert(false); - exit(1); +#ifdef NUMERICAL_EXPMAP_DERIV + Matrix3 Qj = Z_3x3; + double invFac = 1.0; + Q = Z_3x3; + Matrix3 Wj = I_3x3; + for (size_t j=1; j<10; ++j) { + Qj = Qj*W + Wj*V; + invFac = -invFac/(j+1); + Q = Q + invFac*Qj; + Wj = Wj*W; } +#else + // The closed-form formula in Barfoot14tro eq. (102) + double phi = w.norm(); + if (fabs(phi)>1e-5) { + double sinPhi = sin(phi), cosPhi = cos(phi); + double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; + // Invert the sign of odd-order terms to have the right Jacobian + Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W) + + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W) + - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6.)/phi5)*(W*V*W*W + W*W*V*W); + } + else { + Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W) + + 1./24.*(W*W*V + V*W*W - 3*W*V*W) + - 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W); + } +#endif + + return Q; +} + +/* ************************************************************************* */ +Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { + Vector3 w(sub(xi, 0, 3)); + Matrix3 Jw = Rot3::ExpmapDerivative(w); + Matrix3 Q = computeQforExpmapDerivative(xi); + Matrix6 J = (Matrix(6,6) << Jw, Z_3x3, Q, Jw).finished(); + return J; +} + +/* ************************************************************************* */ +Matrix6 Pose3::LogmapDerivative(const Pose3& pose) { + Vector6 xi = Logmap(pose); + Vector3 w(sub(xi, 0, 3)); + Matrix3 Jw = Rot3::LogmapDerivative(w); + Matrix3 Q = computeQforExpmapDerivative(xi); + Matrix3 Q2 = -Jw*Q*Jw; + Matrix6 J = (Matrix(6,6) << Jw, Z_3x3, Q2, Jw).finished(); + return J; +} + +/* ************************************************************************* */ +const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const { + if (H) { + *H << Z_3x3, rotation().matrix(); + } + return t_; } /* ************************************************************************* */ Matrix4 Pose3::matrix() const { const Matrix3 R = R_.matrix(); const Vector3 T = t_.vector(); - Eigen::Matrix A14; + Matrix14 A14; A14 << 0.0, 0.0, 0.0, 1.0; Matrix4 mat; mat << R, T, A14; @@ -240,27 +283,22 @@ Pose3 Pose3::transform_to(const Pose3& pose) const { } /* ************************************************************************* */ -Point3 Pose3::transform_from(const Point3& p, boost::optional Dpose, - boost::optional Dpoint) const { +Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose, + OptionalJacobian<3,3> Dpoint) const { if (Dpose) { const Matrix3 R = R_.matrix(); Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); - Dpose->resize(3, 6); (*Dpose) << DR, R; } - if (Dpoint) + if (Dpoint) { *Dpoint = R_.matrix(); + } return R_ * p + t_; } /* ************************************************************************* */ -Point3 Pose3::transform_to(const Point3& p) const { - return R_.unrotate(p - t_); -} - -/* ************************************************************************* */ -Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, - boost::optional Dpoint) const { +Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose, + OptionalJacobian<3,3> Dpoint) const { // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); @@ -272,83 +310,39 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, +wz, 0.0, -wx, 0.0,-1.0, 0.0, -wy, +wx, 0.0, 0.0, 0.0,-1.0; } - if (Dpoint) + if (Dpoint) { *Dpoint = Rt; - return q; -} - -/* ************************************************************************* */ -Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, - boost::optional Dpoint) const { - const Matrix3 Rt = R_.transpose(); - const Point3 q(Rt*(p - t_).vector()); - if (Dpose) { - const double wx = q.x(), wy = q.y(), wz = q.z(); - Dpose->resize(3, 6); - (*Dpose) << - 0.0, -wz, +wy,-1.0, 0.0, 0.0, - +wz, 0.0, -wx, 0.0,-1.0, 0.0, - -wy, +wx, 0.0, 0.0, 0.0,-1.0; } - if (Dpoint) - *Dpoint = Rt; return q; } /* ************************************************************************* */ -Pose3 Pose3::compose(const Pose3& p2, boost::optional H1, - boost::optional H2) const { - if (H1) - *H1 = p2.inverse().AdjointMap(); - if (H2) - *H2 = I6; - return (*this) * p2; -} - -/* ************************************************************************* */ -Pose3 Pose3::inverse(boost::optional H1) const { - if (H1) - *H1 = -AdjointMap(); - Rot3 Rt = R_.inverse(); - return Pose3(Rt, Rt * (-t_)); -} - -/* ************************************************************************* */ -// between = compose(p2,inverse(p1)); -Pose3 Pose3::between(const Pose3& p2, boost::optional H1, - boost::optional H2) const { - Pose3 result = inverse() * p2; - if (H1) - *H1 = -result.inverse().AdjointMap(); - if (H2) - *H2 = I6; - return result; -} - -/* ************************************************************************* */ -double Pose3::range(const Point3& point, boost::optional H1, - boost::optional H2) const { - if (!H1 && !H2) +double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, + OptionalJacobian<1, 3> H2) const { + if (!H1 && !H2) { return transform_to(point).norm(); - Point3 d = transform_to(point, H1, H2); - double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt( - d2); - Matrix D_result_d = (Matrix(1, 3) << x / n, y / n, z / n).finished(); - if (H1) - *H1 = D_result_d * (*H1); - if (H2) - *H2 = D_result_d * (*H2); - return n; + } else { + Matrix36 D1; + Matrix3 D2; + Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); + const double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, + n = sqrt(d2); + Matrix13 D_result_d; + D_result_d << x / n, y / n, z / n; + if (H1) *H1 = D_result_d * D1; + if (H2) *H2 = D_result_d * D2; + return n; + } } /* ************************************************************************* */ -double Pose3::range(const Pose3& point, boost::optional H1, - boost::optional H2) const { - double r = range(point.translation(), H1, H2); +double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1, + OptionalJacobian<1,6> H2) const { + Matrix13 D2; + double r = range(pose.translation(), H1, H2? &D2 : 0); if (H2) { - Matrix H2_ = *H2 * point.rotation().matrix(); - *H2 = zeros(1, 6); - insertSub(*H2, H2_, 0, 3); + Matrix13 H2_ = D2 * pose.rotation().matrix(); + *H2 << Matrix13::Zero(), H2_; } return r; } @@ -360,22 +354,22 @@ boost::optional align(const vector& pairs) { return boost::none; // we need at least three pairs // calculate centroids - Vector cp = zero(3), cq = zero(3); - BOOST_FOREACH(const Point3Pair& pair, pairs){ - cp += pair.first.vector(); - cq += pair.second.vector(); -} + Vector3 cp = Vector3::Zero(), cq = Vector3::Zero(); + BOOST_FOREACH(const Point3Pair& pair, pairs) { + cp += pair.first.vector(); + cq += pair.second.vector(); + } double f = 1.0 / n; cp *= f; cq *= f; // Add to form H matrix - Matrix H = zeros(3, 3); - BOOST_FOREACH(const Point3Pair& pair, pairs){ - Vector dp = pair.first.vector() - cp; - Vector dq = pair.second.vector() - cq; - H += dp * dq.transpose(); -} + Matrix3 H = Z_3x3; + BOOST_FOREACH(const Point3Pair& pair, pairs) { + Vector3 dp = pair.first.vector() - cp; + Vector3 dq = pair.second.vector() - cq; + H += dp * dq.transpose(); + } // Compute SVD Matrix U, V; @@ -383,8 +377,8 @@ boost::optional align(const vector& pairs) { svd(H, U, S, V); // Recover transform with correction from Eggert97machinevisionandapplications - Matrix UVtranspose = U * V.transpose(); - Matrix detWeighting = eye(3, 3); + Matrix3 UVtranspose = U * V.transpose(); + Matrix3 detWeighting = I_3x3; detWeighting(2, 2) = UVtranspose.determinant(); Rot3 R(Matrix(V * detWeighting * U.transpose())); Point3 t = Point3(cq) - R * Point3(cp); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 2090558a6..8a0f23404 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -19,15 +19,9 @@ #include -#ifndef GTSAM_POSE3_EXPMAP -#define POSE3_DEFAULT_COORDINATES_MODE Pose3::FIRST_ORDER -#else -#define POSE3_DEFAULT_COORDINATES_MODE Pose3::EXPMAP -#endif - -#include #include #include +#include namespace gtsam { @@ -39,7 +33,7 @@ class Pose2; * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Pose3{ +class GTSAM_EXPORT Pose3: public LieGroup { public: /** Pose Concept requirements */ @@ -48,7 +42,7 @@ public: private: - Rot3 R_; ///< Rotation gRp, between global and pose frame + Rot3 R_; ///< Rotation gRp, between global and pose frame Point3 t_; ///< Translation gTp, from global origin to pose frame origin public: @@ -70,6 +64,11 @@ public: R_(R), t_(t) { } + /** Construct from R,t, where t \in vector3 */ + Pose3(const Rot3& R, const Vector3& t) : + R_(R), t_(t) { + } + /** Construct from Pose2 */ explicit Pose3(const Pose2& pose2); @@ -99,247 +98,216 @@ public: } /// inverse transformation with derivatives - Pose3 inverse(boost::optional H1 = boost::none) const; - - ///compose this transformation onto another (first *this and then p2) - Pose3 compose(const Pose3& p2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + Pose3 inverse() const; /// compose syntactic sugar Pose3 operator*(const Pose3& T) const { return Pose3(R_ * T.R_, t_ + R_ * T.t_); } - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - * as well as optionally the derivatives - */ - Pose3 between(const Pose3& p2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; - /// @} - /// @name Manifold + /// @name Lie Group /// @{ - /** Enum to indicate which method should be used in Pose3::retract() and - * Pose3::localCoordinates() + /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ + static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> H = boost::none); + + /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation + static Vector6 Logmap(const Pose3& p, OptionalJacobian<6, 6> H = boost::none); + + /** + * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame + * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi) */ - enum CoordinatesMode { - EXPMAP, ///< The correct exponential map, computationally expensive. - FIRST_ORDER ///< A fast first-order approximation to the exponential map. + Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect + + /** + * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame + * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ + */ + Vector6 Adjoint(const Vector6& xi_b) const { + return AdjointMap() * xi_b; + } /// FIXME Not tested - marked as incorrect + + /** + * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 + * [ad(w,v)] = [w^, zero3; v^, w^] + * Note that this is the matrix representation of the adjoint operator for se3 Lie algebra, + * aka the Lie bracket, and also the derivative of Adjoint map for the Lie group SE3. + * + * Let \f$ \hat{\xi}_i \f$ be the se3 Lie algebra, and \f$ \hat{\xi}_i^\vee = \xi_i = [\omega_i,v_i] \in \mathbb{R}^6\f$ be its + * vector representation. + * We have the following relationship: + * \f$ [\hat{\xi}_1,\hat{\xi}_2]^\vee = ad_{\xi_1}(\xi_2) = [ad_{(\omega_1,v_1)}]*\xi_2 \f$ + * + * We use this to compute the discrete version of the inverse right-trivialized tangent map, + * and its inverse transpose in the discrete Euler Poincare' (DEP) operator. + * + */ + static Matrix6 adjointMap(const Vector6 &xi); + + /** + * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives + */ + static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, + OptionalJacobian<6, 6> = boost::none); + + /** + * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. + */ + static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, + OptionalJacobian<6, 6> H = boost::none); + + /// Derivative of Expmap + static Matrix6 ExpmapDerivative(const Vector6& xi); + + /// Derivative of Logmap + static Matrix6 LogmapDerivative(const Pose3& xi); + + // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP + struct ChartAtOrigin { + static Pose3 Retract(const Vector6& v, ChartJacobian H = boost::none); + static Vector6 Local(const Pose3& r, ChartJacobian H = boost::none); }; - /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes - static size_t Dim() { - return 6; - } - - /// Dimensionality of the tangent space = 6 DOF - size_t dim() const { - return 6; - } - - /// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ from R^ with fast first-order approximation to the exponential map - Pose3 retractFirstOrder(const Vector& d) const; - - /// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ to Pose3 manifold neighborhood around current pose - Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode = - POSE3_DEFAULT_COORDINATES_MODE) const; - - /// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose - Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode =POSE3_DEFAULT_COORDINATES_MODE) const; - - /// @} - /// @name Lie Group - /// @{ - - /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ - static Pose3 Expmap(const Vector& xi); - - /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation - static Vector6 Logmap(const Pose3& p); - - /** - * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame - * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi) - */ - Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect - - /** - * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame - * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ - */ - Vector Adjoint(const Vector& xi_b) const {return AdjointMap()*xi_b; } /// FIXME Not tested - marked as incorrect - - /** - * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 - * [ad(w,v)] = [w^, zero3; v^, w^] - * Note that this is the matrix representation of the adjoint operator for se3 Lie algebra, - * aka the Lie bracket, and also the derivative of Adjoint map for the Lie group SE3. - * - * Let \f$ \hat{\xi}_i \f$ be the se3 Lie algebra, and \f$ \hat{\xi}_i^\vee = \xi_i = [\omega_i,v_i] \in \mathbb{R}^6\f$ be its - * vector representation. - * We have the following relationship: - * \f$ [\hat{\xi}_1,\hat{\xi}_2]^\vee = ad_{\xi_1}(\xi_2) = [ad_{(\omega_1,v_1)}]*\xi_2 \f$ - * - * We use this to compute the discrete version of the inverse right-trivialized tangent map, - * and its inverse transpose in the discrete Euler Poincare' (DEP) operator. - * - */ - static Matrix6 adjointMap(const Vector& xi); - - /** - * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives - */ - static Vector adjoint(const Vector& xi, const Vector& y, boost::optional H = boost::none); - - /** - * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. - */ - static Vector adjointTranspose(const Vector& xi, const Vector& y, boost::optional H = boost::none); - - /** - * Compute the inverse right-trivialized tangent (derivative) map of the exponential map, - * as detailed in [Kobilarov09siggraph] eq. (15) - * The full formula is documented in [Celledoni99cmame] - * Elena Celledoni and Brynjulf Owren. Lie group methods for rigid body dynamics and - * time integration on manifolds. Comput. meth. in Appl. Mech. and Eng., 19(3,4):421-438, 2003. - * and in [Hairer06book] in formula (4.5), pg. 84, Lemma 4.2 - * Ernst Hairer, et al., Geometric Numerical Integration, - * Structure-Preserving Algorithms for Ordinary Differential Equations, 2nd edition, Springer-Verlag, 2006. - */ - static Matrix6 dExpInv_exp(const Vector& xi); - - /** - * wedge for Pose3: - * @param xi 6-dim twist (omega,v) where - * omega = (wx,wy,wz) 3D angular velocity - * v (vx,vy,vz) = 3D velocity - * @return xihat, 4*4 element of Lie algebra that can be exponentiated - */ - static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) { - return (Matrix(4,4) << - 0.,-wz, wy, vx, - wz, 0.,-wx, vy, - -wy, wx, 0., vz, - 0., 0., 0., 0.).finished(); - } - - /// @} - /// @name Group Action on Point3 - /// @{ - - /** - * @brief takes point in Pose coordinates and transforms it to world coordinates - * @param p point in Pose coordinates - * @param Dpose optional 3*6 Jacobian wrpt this pose - * @param Dpoint optional 3*3 Jacobian wrpt point - * @return point in world coordinates - */ - Point3 transform_from(const Point3& p, - boost::optional Dpose=boost::none, boost::optional Dpoint=boost::none) const; - - /** syntactic sugar for transform_from */ - inline Point3 operator*(const Point3& p) const { return transform_from(p); } - - /** - * @brief takes point in world coordinates and transforms it to Pose coordinates - * @param p point in world coordinates - * @param Dpose optional 3*6 Jacobian wrpt this pose - * @param Dpoint optional 3*3 Jacobian wrpt point - * @return point in Pose coordinates - */ - Point3 transform_to(const Point3& p) const; - - Point3 transform_to(const Point3& p, - boost::optional Dpose, boost::optional Dpoint) const; - - Point3 transform_to(const Point3& p, - boost::optional Dpose, boost::optional Dpoint) const; - - /// @} - /// @name Standard Interface - /// @{ - - /// get rotation - const Rot3& rotation() const { return R_; } - - /// get translation - const Point3& translation() const { return t_; } - - /// get x - double x() const { return t_.x(); } - - /// get y - double y() const { return t_.y(); } - - /// get z - double z() const { return t_.z(); } - - /** convert to 4*4 matrix */ - Matrix4 matrix() const; - - /** receives a pose in world coordinates and transforms it to local coordinates */ - Pose3 transform_to(const Pose3& pose) const; - - /** - * Calculate range to a landmark - * @param point 3D location of landmark - * @return range (double) - */ - double range(const Point3& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; - - /** - * Calculate range to another pose - * @param pose Other SO(3) pose - * @return range (double) - */ - double range(const Pose3& pose, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; - - /// @} - /// @name Advanced Interface - /// @{ - - /** - * Return the start and end indices (inclusive) of the translation component of the - * exponential map parameterization - * @return a pair of [start, end] indices into the tangent space vector - */ - inline static std::pair translationInterval() { return std::make_pair(3, 5); } - - /** - * Return the start and end indices (inclusive) of the rotation component of the - * exponential map parameterization - * @return a pair of [start, end] indices into the tangent space vector - */ - static std::pair rotationInterval() { return std::make_pair(0, 2); } - - /// Output stream operator - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose3& p); - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(R_); - ar & BOOST_SERIALIZATION_NVP(t_); - } - /// @} - - };// Pose3 class + using LieGroup::inverse; // version with derivative /** * wedge for Pose3: * @param xi 6-dim twist (omega,v) where - * omega = 3D angular velocity - * v = 3D velocity + * omega = (wx,wy,wz) 3D angular velocity + * v (vx,vy,vz) = 3D velocity * @return xihat, 4*4 element of Lie algebra that can be exponentiated */ + static Matrix wedge(double wx, double wy, double wz, double vx, double vy, + double vz) { + return (Matrix(4, 4) << 0., -wz, wy, vx, wz, 0., -wx, vy, -wy, wx, 0., vz, 0., 0., 0., 0.).finished(); + } + + /// @} + /// @name Group Action on Point3 + /// @{ + + /** + * @brief takes point in Pose coordinates and transforms it to world coordinates + * @param p point in Pose coordinates + * @param Dpose optional 3*6 Jacobian wrpt this pose + * @param Dpoint optional 3*3 Jacobian wrpt point + * @return point in world coordinates + */ + Point3 transform_from(const Point3& p, OptionalJacobian<3, 6> Dpose = + boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const; + + /** syntactic sugar for transform_from */ + inline Point3 operator*(const Point3& p) const { + return transform_from(p); + } + + /** + * @brief takes point in world coordinates and transforms it to Pose coordinates + * @param p point in world coordinates + * @param Dpose optional 3*6 Jacobian wrpt this pose + * @param Dpoint optional 3*3 Jacobian wrpt point + * @return point in Pose coordinates + */ + Point3 transform_to(const Point3& p, OptionalJacobian<3, 6> Dpose = + boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const; + + /// @} + /// @name Standard Interface + /// @{ + + /// get rotation + const Rot3& rotation() const { + return R_; + } + + /// get translation + const Point3& translation(OptionalJacobian<3, 6> H = boost::none) const; + + /// get x + double x() const { + return t_.x(); + } + + /// get y + double y() const { + return t_.y(); + } + + /// get z + double z() const { + return t_.z(); + } + + /** convert to 4*4 matrix */ + Matrix4 matrix() const; + + /** receives a pose in world coordinates and transforms it to local coordinates */ + Pose3 transform_to(const Pose3& pose) const; + + /** + * Calculate range to a landmark + * @param point 3D location of landmark + * @return range (double) + */ + double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none, + OptionalJacobian<1, 3> H2 = boost::none) const; + + /** + * Calculate range to another pose + * @param pose Other SO(3) pose + * @return range (double) + */ + double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none, + OptionalJacobian<1, 6> H2 = boost::none) const; + + /// @} + /// @name Advanced Interface + /// @{ + + /** + * Return the start and end indices (inclusive) of the translation component of the + * exponential map parameterization + * @return a pair of [start, end] indices into the tangent space vector + */ + inline static std::pair translationInterval() { + return std::make_pair(3, 5); + } + + /** + * Return the start and end indices (inclusive) of the rotation component of the + * exponential map parameterization + * @return a pair of [start, end] indices into the tangent space vector + */ + static std::pair rotationInterval() { + return std::make_pair(0, 2); + } + + /// Output stream operator + GTSAM_EXPORT + friend std::ostream &operator<<(std::ostream &os, const Pose3& p); + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(R_); + ar & BOOST_SERIALIZATION_NVP(t_); + } + /// @} + +}; +// Pose3 class + +/** + * wedge for Pose3: + * @param xi 6-dim twist (omega,v) where + * omega = 3D angular velocity + * v = 3D velocity + * @return xihat, 4*4 element of Lie algebra that can be exponentiated + */ template<> inline Matrix wedge(const Vector& xi) { return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5)); @@ -352,21 +320,14 @@ inline Matrix wedge(const Vector& xi) { typedef std::pair Point3Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); -// Define GTSAM traits -namespace traits { +// For MATLAB wrapper +typedef std::vector Pose3Vector; template<> -struct GTSAM_EXPORT is_group : public boost::true_type{ -}; +struct traits : public internal::LieGroup {}; template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ -}; +struct traits : public internal::LieGroup {}; -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ -}; - -} } // namespace gtsam diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h new file mode 100644 index 000000000..cd093ca61 --- /dev/null +++ b/gtsam/geometry/Quaternion.h @@ -0,0 +1,165 @@ +/* ---------------------------------------------------------------------------- + + * 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 Quaternion.h + * @brief Lie Group wrapper for Eigen Quaternions + * @author Frank Dellaert + **/ + +#pragma once + +#include +#include +#include // Logmap/Expmap derivatives + +#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> + +namespace gtsam { + +// Define traits +template +struct traits { + typedef QUATERNION_TYPE ManifoldType; + typedef QUATERNION_TYPE Q; + + typedef lie_group_tag structure_category; + typedef multiplicative_group_tag group_flavor; + + /// @name Group traits + /// @{ + static Q Identity() { + return Q::Identity(); + } + + /// @} + /// @name Basic manifold traits + /// @{ + enum { + dimension = 3 + }; + typedef OptionalJacobian<3, 3> ChartJacobian; + typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector; + + /// @} + /// @name Lie group traits + /// @{ + static Q Compose(const Q &g, const Q & h, + ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) { + if (Hg) *Hg = h.toRotationMatrix().transpose(); + if (Hh) *Hh = I_3x3; + return g * h; + } + + static Q Between(const Q &g, const Q & h, + ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) { + Q d = g.inverse() * h; + if (Hg) *Hg = -d.toRotationMatrix().transpose(); + if (Hh) *Hh = I_3x3; + return d; + } + + static Q Inverse(const Q &g, + ChartJacobian H = boost::none) { + if (H) *H = -g.toRotationMatrix(); + return g.inverse(); + } + + /// Exponential map, simply be converting omega to axis/angle representation + static Q Expmap(const Eigen::Ref& omega, + ChartJacobian H = boost::none) { + if(H) *H = SO3::ExpmapDerivative(omega); + if (omega.isZero()) return Q::Identity(); + else { + _Scalar angle = omega.norm(); + return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle)); + } + } + + /// We use our own Logmap, as there is a slight bug in Eigen + static TangentVector Logmap(const Q& q, ChartJacobian H = boost::none) { + using std::acos; + using std::sqrt; + + // define these compile time constants to avoid std::abs: + static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10, + NearlyNegativeOne = -1.0 + 1e-10; + + Vector3 omega; + + const double qw = q.w(); + // See Quaternion-Logmap.nb in doc for Taylor expansions + if (qw > NearlyOne) { + // Taylor expansion of (angle / s) at 1 + // (2 + 2 * (1-qw) / 3) * q.vec(); + omega = ( 8. / 3. - 2. / 3. * qw) * q.vec(); + } else if (qw < NearlyNegativeOne) { + // Taylor expansion of (angle / s) at -1 + // (-2 - 2 * (1 + qw) / 3) * q.vec(); + omega = (-8. / 3. - 2. / 3. * qw) * q.vec(); + } else { + // Normal, away from zero case + double angle = 2 * acos(qw), s = sqrt(1 - qw * qw); + // Important: convert to [-pi,pi] to keep error continuous + if (angle > M_PI) + angle -= twoPi; + else if (angle < -M_PI) + angle += twoPi; + omega = (angle / s) * q.vec(); + } + + if(H) *H = SO3::LogmapDerivative(omega); + return omega; + } + + /// @} + /// @name Manifold traits + /// @{ + + static TangentVector Local(const Q& g, const Q& h, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + Q b = Between(g, h, H1, H2); + Matrix3 D_v_b; + TangentVector v = Logmap(b, (H1 || H2) ? &D_v_b : 0); + if (H1) *H1 = D_v_b * (*H1); + if (H2) *H2 = D_v_b * (*H2); + return v; + } + + static Q Retract(const Q& g, const TangentVector& v, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + Matrix3 D_h_v; + Q b = Expmap(v,H2 ? &D_h_v : 0); + Q h = Compose(g, b, H1, H2); + if (H2) *H2 = (*H2) * D_h_v; + return h; + } + + /// @} + /// @name Testable + /// @{ + static void Print(const Q& q, const std::string& str = "") { + if (str.size() == 0) + std::cout << "Eigen::Quaternion: "; + else + std::cout << str << " "; + std::cout << q.vec().transpose() << std::endl; + } + static bool Equals(const Q& q1, const Q& q2, double tol = 1e-8) { + return Between(q1, q2).vec().array().abs().maxCoeff() < tol; + } + /// @} +}; + +typedef Eigen::Quaternion Quaternion; + +} // \namespace gtsam + diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 0133c9440..41d56b6e3 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -17,15 +17,11 @@ */ #include -#include using namespace std; namespace gtsam { -/** Explicit instantiation of base class to export members */ -INSTANTIATE_LIE(Rot2); - /* ************************************************************************* */ Rot2 Rot2::fromCosSin(double c, double s) { if (fabs(c * c + s * s - 1.0) > 1e-9) { @@ -64,21 +60,43 @@ Rot2& Rot2::normalize() { } /* ************************************************************************* */ -Matrix Rot2::matrix() const { - return (Matrix(2, 2) << c_, -s_, s_, c_).finished(); +Rot2 Rot2::Expmap(const Vector1& v, OptionalJacobian<1, 1> H) { + if (H) + *H = I_1x1; + if (zero(v)) + return (Rot2()); + else + return Rot2::fromAngle(v(0)); } /* ************************************************************************* */ -Matrix Rot2::transpose() const { - return (Matrix(2, 2) << c_, s_, -s_, c_).finished(); +Vector1 Rot2::Logmap(const Rot2& r, OptionalJacobian<1, 1> H) { + if (H) + *H = I_1x1; + Vector1 v; + v << r.theta(); + return v; +} +/* ************************************************************************* */ +Matrix2 Rot2::matrix() const { + Matrix2 rvalue_; + rvalue_ << c_, -s_, s_, c_; + return rvalue_; +} + +/* ************************************************************************* */ +Matrix2 Rot2::transpose() const { + Matrix2 rvalue_; + rvalue_ << c_, s_, -s_, c_; + return rvalue_; } /* ************************************************************************* */ // see doc/math.lyx, SO(2) section -Point2 Rot2::rotate(const Point2& p, boost::optional H1, - boost::optional H2) const { +Point2 Rot2::rotate(const Point2& p, OptionalJacobian<2, 1> H1, + OptionalJacobian<2, 2> H2) const { const Point2 q = Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y()); - if (H1) *H1 = (Matrix(2, 1) << -q.y(), q.x()).finished(); + if (H1) *H1 << -q.y(), q.x(); if (H2) *H2 = matrix(); return q; } @@ -86,21 +104,23 @@ Point2 Rot2::rotate(const Point2& p, boost::optional H1, /* ************************************************************************* */ // see doc/math.lyx, SO(2) section Point2 Rot2::unrotate(const Point2& p, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<2, 1> H1, OptionalJacobian<2, 2> H2) const { const Point2 q = Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y()); - if (H1) *H1 = (Matrix(2, 1) << q.y(), -q.x()).finished(); // R_{pi/2}q + if (H1) *H1 << q.y(), -q.x(); if (H2) *H2 = transpose(); return q; } /* ************************************************************************* */ -Rot2 Rot2::relativeBearing(const Point2& d, boost::optional H) { +Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) { double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); if(fabs(n) > 1e-5) { - if (H) *H = (Matrix(1, 2) << -y / d2, x / d2).finished(); + if (H) { + *H << -y / d2, x / d2; + } return Rot2::fromCosSin(x / n, y / n); } else { - if (H) *H = (Matrix(1, 2) << 0.0, 0.0).finished(); + if (H) *H << 0.0, 0.0; return Rot2(); } } diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index af9148fd3..198cd5862 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -18,10 +18,9 @@ #pragma once -#include - -#include #include +#include +#include namespace gtsam { @@ -31,25 +30,16 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ - class GTSAM_EXPORT Rot2 { - - public: - /** get the dimension by the type */ - static const size_t dimension = 1; - - private: + class GTSAM_EXPORT Rot2 : public LieGroup { /** we store cos(theta) and sin(theta) */ double c_, s_; - /** normalize to make sure cos and sin form unit vector */ Rot2& normalize(); /** private constructor from cos/sin */ - inline Rot2(double c, double s) : - c_(c), s_(s) { - } + inline Rot2(double c, double s) : c_(c), s_(s) {} public: @@ -57,14 +47,10 @@ namespace gtsam { /// @{ /** default constructor, zero rotation */ - Rot2() : - c_(1.0), s_(0.0) { - } + Rot2() : c_(1.0), s_(0.0) {} /// Constructor from angle in radians == exponential map at identity - Rot2(double theta) : - c_(cos(theta)), s_(sin(theta)) { - } + Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {} /// Named constructor from angle in radians static Rot2 fromAngle(double theta) { @@ -87,7 +73,7 @@ namespace gtsam { * @param H optional reference for Jacobian * @return 2D rotation \f$ \in SO(2) \f$ */ - static Rot2 relativeBearing(const Point2& d, boost::optional H = + static Rot2 relativeBearing(const Point2& d, OptionalJacobian<1,2> H = boost::none); /** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */ @@ -111,68 +97,48 @@ namespace gtsam { inline static Rot2 identity() { return Rot2(); } /** The inverse rotation - negative angle */ - Rot2 inverse() const { - return Rot2(c_, -s_); - } - - /** Compose - make a new rotation by adding angles */ - inline Rot2 compose(const Rot2& R, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const { - if (H1) *H1 = eye(1); - if (H2) *H2 = eye(1); - return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); - } + Rot2 inverse() const { return Rot2(c_, -s_);} /** Compose - make a new rotation by adding angles */ Rot2 operator*(const Rot2& R) const { return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); } - /** Between using the default implementation */ - inline Rot2 between(const Rot2& R, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const { - if (H1) *H1 = -eye(1); - if (H2) *H2 = eye(1); - return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_); - } - - /// @} - /// @name Manifold - /// @{ - - /// dimension of the variable - used to autodetect sizes - inline static size_t Dim() { - return dimension; - } - - /// Dimensionality of the tangent space, DOF = 1 - inline size_t dim() const { - return dimension; - } - - /// Updates a with tangent space delta - inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); } - - /// Returns inverse retraction - inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); } - /// @} /// @name Lie Group /// @{ - ///Exponential map at identity - create a rotation from canonical coordinates - static Rot2 Expmap(const Vector& v) { - if (zero(v)) - return (Rot2()); - else - return Rot2::fromAngle(v(0)); + /// Exponential map at identity - create a rotation from canonical coordinates + static Rot2 Expmap(const Vector1& v, ChartJacobian H = boost::none); + + /// Log map at identity - return the canonical coordinates of this rotation + static Vector1 Logmap(const Rot2& r, ChartJacobian H = boost::none); + + /** Calculate Adjoint map */ + Matrix1 AdjointMap() const { return I_1x1; } + + /// Left-trivialized derivative of the exponential map + static Matrix ExpmapDerivative(const Vector& /*v*/) { + return ones(1); } - ///Log map at identity - return the canonical coordinates of this rotation - static inline Vector Logmap(const Rot2& r) { - return (Vector(1) << r.theta()).finished(); + /// Left-trivialized derivative inverse of the exponential map + static Matrix LogmapDerivative(const Vector& /*v*/) { + return ones(1); } + // Chart at origin simply uses exponential map and its inverse + struct ChartAtOrigin { + static Rot2 Retract(const Vector1& v, ChartJacobian H = boost::none) { + return Expmap(v, H); + } + static Vector1 Local(const Rot2& r, ChartJacobian H = boost::none) { + return Logmap(r, H); + } + }; + + using LieGroup::inverse; // version with derivative + /// @} /// @name Group Action on Point2 /// @{ @@ -180,8 +146,8 @@ namespace gtsam { /** * rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$ */ - Point2 rotate(const Point2& p, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none, + OptionalJacobian<2, 2> H2 = boost::none) const; /** syntactic sugar for rotate */ inline Point2 operator*(const Point2& p) const { @@ -191,8 +157,8 @@ namespace gtsam { /** * rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ */ - Point2 unrotate(const Point2& p, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none, + OptionalJacobian<2, 2> H2 = boost::none) const; /// @} /// @name Standard Interface @@ -225,36 +191,26 @@ namespace gtsam { } /** return 2*2 rotation matrix */ - Matrix matrix() const; + Matrix2 matrix() const; /** return 2*2 transpose (inverse) rotation matrix */ - Matrix transpose() const; + Matrix2 transpose() const; private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(c_); ar & BOOST_SERIALIZATION_NVP(s_); } }; - // Define GTSAM traits - namespace traits { + template<> + struct traits : public internal::LieGroup {}; template<> - struct GTSAM_EXPORT is_group : public boost::true_type{ - }; + struct traits : public internal::LieGroup {}; - template<> - struct GTSAM_EXPORT is_manifold : public boost::true_type{ - }; - - template<> - struct GTSAM_EXPORT dimension : public boost::integral_constant{ - }; - - } } // gtsam diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 846e0e070..fa09ddc21 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -19,6 +19,7 @@ */ #include +#include #include #include #include @@ -27,8 +28,6 @@ using namespace std; namespace gtsam { -static const Matrix3 I3 = Matrix3::Identity(); - /* ************************************************************************* */ void Rot3::print(const std::string& s) const { gtsam::print((Matrix)matrix(), s); @@ -54,7 +53,7 @@ Rot3 Rot3::Random(boost::mt19937 & rng) { } /* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Vector& w) { +Rot3 Rot3::rodriguez(const Vector3& w) { double t = w.norm(); if (t < 1e-10) return Rot3(); return rodriguez(w/t, t); @@ -72,23 +71,21 @@ Point3 Rot3::operator*(const Point3& p) const { /* ************************************************************************* */ Unit3 Rot3::rotate(const Unit3& p, - boost::optional HR, boost::optional Hp) const { - Unit3 q = Unit3(rotate(p.point3(Hp))); - if (Hp) - (*Hp) = q.basis().transpose() * matrix() * (*Hp); - if (HR) - (*HR) = -q.basis().transpose() * matrix() * p.skew(); + OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const { + Matrix32 Dp; + Unit3 q = Unit3(rotate(p.point3(Hp ? &Dp : 0))); + if (Hp) *Hp = q.basis().transpose() * matrix() * Dp; + if (HR) *HR = -q.basis().transpose() * matrix() * p.skew(); return q; } /* ************************************************************************* */ Unit3 Rot3::unrotate(const Unit3& p, - boost::optional HR, boost::optional Hp) const { - Unit3 q = Unit3(unrotate(p.point3(Hp))); - if (Hp) - (*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp); - if (HR) - (*HR) = q.basis().transpose() * q.skew(); + OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const { + Matrix32 Dp; + Unit3 q = Unit3(unrotate(p.point3(Dp))); + if (Hp) *Hp = q.basis().transpose() * matrix().transpose () * Dp; + if (HR) *HR = q.basis().transpose() * q.skew(); return q; } @@ -99,8 +96,8 @@ Unit3 Rot3::operator*(const Unit3& p) const { /* ************************************************************************* */ // see doc/math.lyx, SO(3) section -Point3 Rot3::unrotate(const Point3& p, boost::optional H1, - boost::optional H2) const { +Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1, + OptionalJacobian<3,3> H2) const { const Matrix3& Rt = transpose(); Point3 q(Rt * p.vector()); // q = Rt*p const double wx = q.x(), wy = q.y(), wz = q.z(); @@ -111,48 +108,6 @@ Point3 Rot3::unrotate(const Point3& p, boost::optional H1, return q; } -/* ************************************************************************* */ -// see doc/math.lyx, SO(3) section -Point3 Rot3::unrotate(const Point3& p, - boost::optional H1, boost::optional H2) const { - const Matrix3& Rt = transpose(); - Point3 q(Rt * p.vector()); // q = Rt*p - const double wx = q.x(), wy = q.y(), wz = q.z(); - if (H1) { - H1->resize(3,3); - *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; - } - if (H2) - *H2 = Rt; - return q; -} - -/* ************************************************************************* */ -/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) -Matrix3 Rot3::dexpL(const Vector3& v) { - if(zero(v)) return eye(3); - Matrix x = skewSymmetric(v); - Matrix x2 = x*x; - double theta = v.norm(), vi = theta/2.0; - double s1 = sin(vi)/vi; - double s2 = (theta - sin(theta))/(theta*theta*theta); - Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2; - return res; -} - -/* ************************************************************************* */ -/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) -Matrix3 Rot3::dexpInvL(const Vector3& v) { - if(zero(v)) return eye(3); - Matrix x = skewSymmetric(v); - Matrix x2 = x*x; - double theta = v.norm(), vi = theta/2.0; - double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); - Matrix res = eye(3) + 0.5*x - s2*x2; - return res; -} - - /* ************************************************************************* */ Point3 Rot3::column(int index) const{ if(index == 3) @@ -167,7 +122,7 @@ Point3 Rot3::column(int index) const{ /* ************************************************************************* */ Vector3 Rot3::xyz() const { - Matrix I;Vector3 q; + Matrix3 I;Vector3 q; boost::tie(I,q)=RQ(matrix()); return q; } @@ -195,36 +150,13 @@ Vector Rot3::quaternion() const { } /* ************************************************************************* */ -Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jr; - if (normx < 10e-8){ - Jr = Matrix3::Identity(); - } - else{ - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X + - ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian - } - return Jr; +Matrix3 Rot3::ExpmapDerivative(const Vector3& x) { + return SO3::ExpmapDerivative(x); } /* ************************************************************************* */ -Matrix3 Rot3::rightJacobianExpMapSO3inverse(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jrinv; - - if (normx < 10e-8){ - Jrinv = Matrix3::Identity(); - } - else{ - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jrinv = Matrix3::Identity() + - 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; - } - return Jrinv; +Matrix3 Rot3::LogmapDerivative(const Vector3& x) { + return SO3::LogmapDerivative(x); } /* ************************************************************************* */ @@ -255,18 +187,12 @@ ostream &operator<<(ostream &os, const Rot3& R) { return os; } -/* ************************************************************************* */ -Point3 Rot3::unrotate(const Point3& p) const { - // Eigen expression - return Point3(transpose()*p.vector()); // q = Rt*p -} - /* ************************************************************************* */ Rot3 Rot3::slerp(double t, const Rot3& other) const { // amazingly simple in GTSAM :-) assert(t>=0 && t<=1); - Vector3 omega = localCoordinates(other, Rot3::EXPMAP); - return retract(t * omega, Rot3::EXPMAP); + Vector3 omega = Logmap(between(other)); + return compose(Expmap(t * omega)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index ef2d19750..881c58579 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -16,11 +16,15 @@ * @author Christian Potthast * @author Frank Dellaert * @author Richard Roberts + * @author Luca Carlone */ // \callgraph #pragma once +#include +#include +#include #include // Get GTSAM_USE_QUATERNIONS macro // You can override the default coordinate mode using this flag @@ -39,18 +43,8 @@ #endif #endif -#include -#include -#include -#include - namespace gtsam { - /// Typedef to an Eigen Quaternion, we disable alignment because - /// geometry objects are stored in boost pool allocators, in Values - /// containers, and and these pool allocators do not support alignment. - typedef Eigen::Quaternion Quaternion; - /** * @brief A 3D rotation represented as a rotation matrix if the preprocessor * symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion if it @@ -58,7 +52,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ - class GTSAM_EXPORT Rot3 { + class GTSAM_EXPORT Rot3 : public LieGroup { private: @@ -160,7 +154,7 @@ namespace gtsam { * @param theta rotation angle * @return incremental rotation matrix */ - static Rot3 rodriguez(const Vector& w, double theta); + static Rot3 rodriguez(const Vector3& w, double theta); /** * Rodriguez' formula to compute an incremental rotation matrix @@ -183,7 +177,7 @@ namespace gtsam { * @param v a vector of incremental roll,pitch,yaw * @return incremental rotation matrix */ - static Rot3 rodriguez(const Vector& v); + static Rot3 rodriguez(const Vector3& v); /** * Rodriguez' formula to compute an incremental rotation matrix @@ -193,7 +187,7 @@ namespace gtsam { * @return incremental rotation matrix */ static Rot3 rodriguez(double wx, double wy, double wz) - { return rodriguez((Vector(3) << wx, wy, wz).finished());} + { return rodriguez(Vector3(wx, wy, wz));} /// @} /// @name Testable @@ -214,23 +208,14 @@ namespace gtsam { return Rot3(); } - /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity - Rot3 inverse(boost::optional H1=boost::none) const; - - /// Compose two rotations i.e., R= (*this) * R2 - Rot3 compose(const Rot3& R2) const; - - /// Compose two rotations i.e., R= (*this) * R2 - Rot3 compose(const Rot3& R2, boost::optional H1, - boost::optional H2) const; - - /// Compose two rotations i.e., R= (*this) * R2 - Rot3 compose(const Rot3& R2, boost::optional H1, - boost::optional H2) const; - - /** compose two rotations */ + /// Syntatic sugar for composing two rotations Rot3 operator*(const Rot3& R2) const; + /// inverse of a rotation, TODO should be different for M/Q + Rot3 inverse() const { + return Rot3(Matrix3(transpose())); + } + /** * Conjugation: given a rotation acting in frame B, compute rotation c1Rc2 acting in a frame C * @param cRb rotation from B frame to C frame @@ -241,23 +226,10 @@ namespace gtsam { return cRb * (*this) * cRb.inverse(); } - /** - * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' - */ - Rot3 between(const Rot3& R2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; - /// @} /// @name Manifold /// @{ - /// dimension of the variable - used to autodetect sizes - static size_t Dim() { return 3; } - - /// return dimensionality of tangent space, DOF = 3 - size_t dim() const { return 3; } - /** * The method retract() is used to map from the tangent space back to the manifold. * Its inverse, is localCoordinates(). For Lie groups, an obvious retraction is the @@ -271,21 +243,29 @@ namespace gtsam { EXPMAP, ///< Use the Lie group exponential map to retract #ifndef GTSAM_USE_QUATERNIONS CAYLEY, ///< Retract and localCoordinates using the Cayley transform. - SLOW_CAYLEY ///< Slow matrix implementation of Cayley transform (for tests only). #endif }; #ifndef GTSAM_USE_QUATERNIONS + + // Cayley chart around origin + struct CayleyChart { + static Rot3 Retract(const Vector3& v, OptionalJacobian<3, 3> H = boost::none); + static Vector3 Local(const Rot3& r, OptionalJacobian<3, 3> H = boost::none); + }; + /// Retraction from R^3 to Rot3 manifold using the Cayley transform - Rot3 retractCayley(const Vector& omega) const; + Rot3 retractCayley(const Vector& omega) const { + return compose(CayleyChart::Retract(omega)); + } + + /// Inverse of retractCayley + Vector3 localCayley(const Rot3& other) const { + return CayleyChart::Local(between(other)); + } + #endif - /// Retraction from R^3 \f$ [R_x,R_y,R_z] \f$ to Rot3 manifold neighborhood around current rotation - Rot3 retract(const Vector& omega, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const; - - /// Returns local retract coordinates \f$ [R_x,R_y,R_z] \f$ in neighborhood around current rotation - Vector3 localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const; - /// @} /// @name Lie Group /// @{ @@ -294,32 +274,33 @@ namespace gtsam { * Exponential map at identity - create a rotation from canonical coordinates * \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula */ - static Rot3 Expmap(const Vector& v) { - if(zero(v)) return Rot3(); - else return rodriguez(v); + static Rot3 Expmap(const Vector& v, OptionalJacobian<3,3> H = boost::none) { + if(H) *H = Rot3::ExpmapDerivative(v); + if (zero(v)) return Rot3(); else return rodriguez(v); } /** - * Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z] \f$ of this rotation + * Log map at identity - returns the canonical coordinates + * \f$ [R_x,R_y,R_z] \f$ of this rotation */ - static Vector3 Logmap(const Rot3& R); + static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = boost::none); - /// Left-trivialized derivative of the exponential map - static Matrix3 dexpL(const Vector3& v); + /// Derivative of Expmap + static Matrix3 ExpmapDerivative(const Vector3& x); - /// Left-trivialized derivative inverse of the exponential map - static Matrix3 dexpInvL(const Vector3& v); + /// Derivative of Logmap + static Matrix3 LogmapDerivative(const Vector3& x); - /** - * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in - * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - */ - static Matrix3 rightJacobianExpMapSO3(const Vector3& x); + /** Calculate Adjoint map */ + Matrix3 AdjointMap() const { return matrix(); } - /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in - * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - */ - static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x); + // Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE + struct ChartAtOrigin { + static Rot3 Retract(const Vector3& v, ChartJacobian H = boost::none); + static Vector3 Local(const Rot3& r, ChartJacobian H = boost::none); + }; + + using LieGroup::inverse; // version with derivative /// @} /// @name Group Action on Point3 @@ -328,34 +309,27 @@ namespace gtsam { /** * rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$ */ - Point3 rotate(const Point3& p, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, + OptionalJacobian<3,3> H2 = boost::none) const; /// rotate point from rotated coordinate frame to world = R*p Point3 operator*(const Point3& p) const; /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ - Point3 unrotate(const Point3& p) const; - - /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ - Point3 unrotate(const Point3& p, boost::optional H1, - boost::optional H2) const; - - /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ - Point3 unrotate(const Point3& p, boost::optional H1, - boost::optional H2) const; + Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, + OptionalJacobian<3,3> H2=boost::none) const; /// @} /// @name Group Action on Unit3 /// @{ /// rotate 3D direction from rotated coordinate frame to world frame - Unit3 rotate(const Unit3& p, boost::optional HR = boost::none, - boost::optional Hp = boost::none) const; + Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none, + OptionalJacobian<2,2> Hp = boost::none) const; /// unrotate 3D direction from world frame to rotated coordinate frame - Unit3 unrotate(const Unit3& p, boost::optional HR = boost::none, - boost::optional Hp = boost::none) const; + Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none, + OptionalJacobian<2,2> Hp = boost::none) const; /// rotate 3D direction from rotated coordinate frame to world frame Unit3 operator*(const Unit3& p) const; @@ -454,7 +428,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { #ifndef GTSAM_USE_QUATERNIONS ar & boost::serialization::make_nvp("rot11", rot_(0,0)); @@ -488,20 +462,10 @@ namespace gtsam { */ GTSAM_EXPORT std::pair RQ(const Matrix3& A); - // Define GTSAM traits - namespace traits { - template<> - struct GTSAM_EXPORT is_group : public boost::true_type{ - }; - + struct traits : public internal::LieGroup {}; + template<> - struct GTSAM_EXPORT is_manifold : public boost::true_type{ - }; - - template<> - struct GTSAM_EXPORT dimension : public boost::integral_constant{ - }; - - } + struct traits : public internal::LieGroup {}; } + diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index d0c7e95f3..b4c79de3b 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -23,6 +23,7 @@ #ifndef GTSAM_USE_QUATERNIONS #include +#include #include #include @@ -30,10 +31,8 @@ using namespace std; namespace gtsam { -static const Matrix3 I3 = Matrix3::Identity(); - /* ************************************************************************* */ -Rot3::Rot3() : rot_(Matrix3::Identity()) {} +Rot3::Rot3() : rot_(I_3x3) {} /* ************************************************************************* */ Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { @@ -119,47 +118,8 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) { } /* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Vector& w, double theta) { - // get components of axis \omega - double wx = w(0), wy=w(1), wz=w(2); - double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; -#ifndef NDEBUG - double l_n = wwTxx + wwTyy + wwTzz; - if (std::abs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); -#endif - - double c = cos(theta), s = sin(theta), c_1 = 1 - c; - - double swx = wx * s, swy = wy * s, swz = wz * s; - double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz; - double C11 = c_1*wwTyy, C12 = c_1*wy*wz; - double C22 = c_1*wwTzz; - - return Rot3( - c + C00, -swz + C01, swy + C02, - swz + C01, c + C11, -swx + C12, - -swy + C02, swx + C12, c + C22); -} - -/* ************************************************************************* */ -Rot3 Rot3::compose (const Rot3& R2) const { - return *this * R2; -} - -/* ************************************************************************* */ -Rot3 Rot3::compose (const Rot3& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; - return *this * R2; -} - -/* ************************************************************************* */ -Rot3 Rot3::compose (const Rot3& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; - return *this * R2; +Rot3 Rot3::rodriguez(const Vector3& w, double theta) { + return SO3::Rodrigues(w,theta); } /* ************************************************************************* */ @@ -173,24 +133,9 @@ Matrix3 Rot3::transpose() const { return rot_.transpose(); } -/* ************************************************************************* */ -Rot3 Rot3::inverse(boost::optional H1) const { - if (H1) *H1 = -rot_; - return Rot3(Matrix3(transpose())); -} - -/* ************************************************************************* */ -Rot3 Rot3::between (const Rot3& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = -(R2.transpose()*rot_); - if (H2) *H2 = I3; - Matrix3 R12 = transpose()*R2.rot_; - return Rot3(R12); -} - /* ************************************************************************* */ Point3 Rot3::rotate(const Point3& p, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1 || H2) { if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H2) *H2 = rot_; @@ -200,99 +145,54 @@ Point3 Rot3::rotate(const Point3& p, /* ************************************************************************* */ // Log map at identity - return the canonical coordinates of this rotation -Vector3 Rot3::Logmap(const Rot3& R) { - - static const double PI = boost::math::constants::pi(); - - const Matrix3& rot = R.rot_; - // Get trace(R) - double tr = rot.trace(); - - // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. - // we do something special - if (std::abs(tr+1.0) < 1e-10) { - if(std::abs(rot(2,2)+1.0) > 1e-10) - return (PI / sqrt(2.0+2.0*rot(2,2) )) * - Vector3(rot(0,2), rot(1,2), 1.0+rot(2,2)); - else if(std::abs(rot(1,1)+1.0) > 1e-10) - return (PI / sqrt(2.0+2.0*rot(1,1))) * - Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1)); - else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit - return (PI / sqrt(2.0+2.0*rot(0,0))) * - Vector3(1.0+rot(0,0), rot(1,0), rot(2,0)); - } else { - double magnitude; - double tr_3 = tr-3.0; // always negative - if (tr_3<-1e-7) { - double theta = acos((tr-1.0)/2.0); - magnitude = theta/(2.0*sin(theta)); - } else { - // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) - // use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2) - magnitude = 0.5 - tr_3*tr_3/12.0; - } - return magnitude*Vector3( - rot(2,1)-rot(1,2), - rot(0,2)-rot(2,0), - rot(1,0)-rot(0,1)); - } +Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) { + return SO3::Logmap(R.rot_,H); } /* ************************************************************************* */ -Rot3 Rot3::retractCayley(const Vector& omega) const { +Rot3 Rot3::CayleyChart::Retract(const Vector3& omega, OptionalJacobian<3,3> H) { + if (H) throw std::runtime_error("Rot3::CayleyChart::Retract Derivative"); const double x = omega(0), y = omega(1), z = omega(2); const double x2 = x * x, y2 = y * y, z2 = z * z; const double xy = x * y, xz = x * z, yz = y * z; const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f; - return (*this) - * Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f, + return Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f, (xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f, (xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f); } /* ************************************************************************* */ -Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { - if(mode == Rot3::EXPMAP) { - return (*this)*Expmap(omega); - } else if(mode == Rot3::CAYLEY) { - return retractCayley(omega); - } else if(mode == Rot3::SLOW_CAYLEY) { - Matrix Omega = skewSymmetric(omega); - return (*this)*CayleyFixed<3>(-Omega/2); - } else { - assert(false); - exit(1); - } +Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) { + if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative"); + // Create a fixed-size matrix + Matrix3 A = R.matrix(); + // Mathematica closed form optimization (procrastination?) gone wild: + const double a = A(0, 0), b = A(0, 1), c = A(0, 2); + const double d = A(1, 0), e = A(1, 1), f = A(1, 2); + const double g = A(2, 0), h = A(2, 1), i = A(2, 2); + const double di = d * i, ce = c * e, cd = c * d, fg = f * g; + const double M = 1 + e - f * h + i + e * i; + const double K = -4.0 / (cd * h + M + a * M - g * (c + ce) - b * (d + di - fg)); + const double x = a * f - cd + f; + const double y = b * f - ce - c; + const double z = fg - di - d; + return K * Vector3(x, y, z); } /* ************************************************************************* */ -Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { - if(mode == Rot3::EXPMAP) { - return Logmap(between(T)); - } else if(mode == Rot3::CAYLEY) { - // Create a fixed-size matrix - Matrix3 A = rot_.transpose() * T.matrix(); - // Mathematica closed form optimization (procrastination?) gone wild: - const double a=A(0,0),b=A(0,1),c=A(0,2); - const double d=A(1,0),e=A(1,1),f=A(1,2); - const double g=A(2,0),h=A(2,1),i=A(2,2); - const double di = d*i, ce = c*e, cd = c*d, fg=f*g; - const double M = 1 + e - f*h + i + e*i; - const double K = - 4.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg)); - const double x = a * f - cd + f; - const double y = b * f - ce - c; - const double z = fg - di - d; - return K * Vector3(x, y, z); - } else if(mode == Rot3::SLOW_CAYLEY) { - // Create a fixed-size matrix - Matrix3 A(between(T).matrix()); - // using templated version of Cayley - Matrix3 Omega = CayleyFixed<3>(A); - return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0)); - } else { - assert(false); - exit(1); - } +Rot3 Rot3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) { + static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE; + if (mode == Rot3::EXPMAP) return Expmap(omega, H); + if (mode == Rot3::CAYLEY) return CayleyChart::Retract(omega, H); + else throw std::runtime_error("Rot3::Retract: unknown mode"); +} + +/* ************************************************************************* */ +Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) { + static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE; + if (mode == Rot3::EXPMAP) return Logmap(R, H); + if (mode == Rot3::CAYLEY) return CayleyChart::Local(R, H); + else throw std::runtime_error("Rot3::Local: unknown mode"); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 19de92ca2..52fb4f262 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -84,41 +84,14 @@ namespace gtsam { } /* ************************************************************************* */ - Rot3 Rot3::rodriguez(const Vector& w, double theta) { - return Quaternion(Eigen::AngleAxisd(theta, w)); } - - /* ************************************************************************* */ - Rot3 Rot3::compose(const Rot3& R2) const { - return Rot3(quaternion_ * R2.quaternion_); + Rot3 Rot3::rodriguez(const Vector3& w, double theta) { + return Quaternion(Eigen::AngleAxis(theta, w)); } - - /* ************************************************************************* */ - Rot3 Rot3::compose(const Rot3& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; - return Rot3(quaternion_ * R2.quaternion_); - } - - /* ************************************************************************* */ - Rot3 Rot3::compose(const Rot3& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; - return Rot3(quaternion_ * R2.quaternion_); - } - /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { return Rot3(quaternion_ * R2.quaternion_); } - /* ************************************************************************* */ - Rot3 Rot3::inverse(boost::optional H1) const { - if (H1) *H1 = -matrix(); - return Rot3(quaternion_.inverse()); - } - /* ************************************************************************* */ // TODO: Could we do this? It works in Rot3M but not here, probably because // here we create an intermediate value by calling matrix() @@ -127,17 +100,9 @@ namespace gtsam { return matrix().transpose(); } - /* ************************************************************************* */ - Rot3 Rot3::between(const Rot3& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = -(R2.transpose()*matrix()); - if (H2) *H2 = I3; - return between_default(*this, R2); - } - /* ************************************************************************* */ Point3 Rot3::rotate(const Point3& p, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { Matrix R = matrix(); if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H2) *H2 = R; @@ -146,41 +111,22 @@ namespace gtsam { } /* ************************************************************************* */ - Vector3 Rot3::Logmap(const Rot3& R) { - using std::acos; - using std::sqrt; - static const double twoPi = 2.0 * M_PI, - // define these compile time constants to avoid std::abs: - NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10; - - const Quaternion& q = R.quaternion_; - const double qw = q.w(); - if (qw > NearlyOne) { - // Taylor expansion of (angle / s) at 1 - return (2 - 2 * (qw - 1) / 3) * q.vec(); - } else if (qw < NearlyNegativeOne) { - // Angle is zero, return zero vector - return Vector3::Zero(); - } else { - // Normal, away from zero case - double angle = 2 * acos(qw), s = sqrt(1 - qw * qw); - // Important: convert to [-pi,pi] to keep error continuous - if (angle > M_PI) - angle -= twoPi; - else if (angle < -M_PI) - angle += twoPi; - return (angle / s) * q.vec(); - } + Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) { + return traits::Logmap(R.quaternion_, H); } /* ************************************************************************* */ - Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { - return compose(Expmap(omega)); + Rot3 Rot3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) { + static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE; + if (mode == Rot3::EXPMAP) return Expmap(omega, H); + else throw std::runtime_error("Rot3::Retract: unknown mode"); } /* ************************************************************************* */ - Vector3 Rot3::localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode) const { - return Logmap(between(t2)); + Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) { + static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE; + if (mode == Rot3::EXPMAP) return Logmap(R, H); + else throw std::runtime_error("Rot3::Local: unknown mode"); } /* ************************************************************************* */ diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp new file mode 100644 index 000000000..7e755d680 --- /dev/null +++ b/gtsam/geometry/SO3.cpp @@ -0,0 +1,172 @@ +/* ---------------------------------------------------------------------------- + + * 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 SO3.cpp + * @brief 3*3 matrix representation o SO(3) + * @author Frank Dellaert + * @date December 2014 + */ + +#include +#include +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +SO3 SO3::Rodrigues(const Vector3& axis, double theta) { + using std::cos; + using std::sin; + + // get components of axis \omega + double wx = axis(0), wy = axis(1), wz = axis(2); + + double c = cos(theta), s = sin(theta), c_1 = 1 - c; + double wwTxx = wx * wx, wwTyy = wy * wy, wwTzz = wz * wz; + double swx = wx * s, swy = wy * s, swz = wz * s; + + double C00 = c_1 * wwTxx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz; + double C11 = c_1 * wwTyy, C12 = c_1 * wy * wz; + double C22 = c_1 * wwTzz; + + Matrix3 R; + R << c + C00, -swz + C01, swy + C02, // + swz + C01, c + C11, -swx + C12, // + -swy + C02, swx + C12, c + C22; + + return R; +} + +/// simply convert omega to axis/angle representation +SO3 SO3::Expmap(const Vector3& omega, + ChartJacobian H) { + + if (H) + *H = ExpmapDerivative(omega); + + if (omega.isZero()) + return Identity(); + else { + double angle = omega.norm(); + return Rodrigues(omega / angle, angle); + } +} + +/* ************************************************************************* */ +Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { + using std::sqrt; + using std::sin; + + // note switch to base 1 + const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2); + const double& R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2); + const double& R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2); + + // Get trace(R) + double tr = R.trace(); + + Vector3 omega; + + // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. + // we do something special + if (std::abs(tr + 1.0) < 1e-10) { + if (std::abs(R33 + 1.0) > 1e-10) + omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33); + else if (std::abs(R22 + 1.0) > 1e-10) + omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32); + else + // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit + omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); + } else { + double magnitude; + double tr_3 = tr - 3.0; // always negative + if (tr_3 < -1e-7) { + double theta = acos((tr - 1.0) / 2.0); + magnitude = theta / (2.0 * sin(theta)); + } else { + // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) + // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2) + magnitude = 0.5 - tr_3 * tr_3 / 12.0; + } + omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12); + } + + if(H) *H = LogmapDerivative(omega); + return omega; +} + +/* ************************************************************************* */ +Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { + using std::cos; + using std::sin; + + if(zero(omega)) return I_3x3; + double theta = omega.norm(); // rotation angle +#ifdef DUY_VERSION + /// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) + Matrix3 X = skewSymmetric(omega); + Matrix3 X2 = X*X; + double vi = theta/2.0; + double s1 = sin(vi)/vi; + double s2 = (theta - sin(theta))/(theta*theta*theta); + return I_3x3 - 0.5*s1*s1*X + s2*X2; +#else // Luca's version + /** + * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in + * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + * expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega) + * where Jr = ExpmapDerivative(thetahat); + * This maps a perturbation in the tangent space (omega) to + * a perturbation on the manifold (expmap(Jr * omega)) + */ + // element of Lie algebra so(3): X = omega^, normalized by normx + const Matrix3 Y = skewSymmetric(omega) / theta; + return I_3x3 - ((1 - cos(theta)) / (theta)) * Y + + (1 - sin(theta) / theta) * Y * Y; // right Jacobian +#endif +} + +/* ************************************************************************* */ +Matrix3 SO3::LogmapDerivative(const Vector3& omega) { + using std::cos; + using std::sin; + + if(zero(omega)) return I_3x3; + double theta = omega.norm(); +#ifdef DUY_VERSION + /// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) + Matrix3 X = skewSymmetric(omega); + Matrix3 X2 = X*X; + double vi = theta/2.0; + double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); + return I_3x3 + 0.5*X - s2*X2; +#else // Luca's version + /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in + * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + * logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega + * where Jrinv = LogmapDerivative(omega); + * This maps a perturbation on the manifold (expmap(omega)) + * to a perturbation in the tangent space (Jrinv * omega) + */ + const Matrix3 X = skewSymmetric(omega); // element of Lie algebra so(3): X = omega^ + return I_3x3 + 0.5 * X + + (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X + * X; +#endif +} + +/* ************************************************************************* */ + +} // end namespace gtsam + diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h new file mode 100644 index 000000000..a08168ed8 --- /dev/null +++ b/gtsam/geometry/SO3.h @@ -0,0 +1,139 @@ +/* ---------------------------------------------------------------------------- + + * 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 SO3.h + * @brief 3*3 matrix representation of SO(3) + * @author Frank Dellaert + * @date December 2014 + */ + +#pragma once + +#include +#include + +#include + +namespace gtsam { + +/** + * True SO(3), i.e., 3*3 matrix subgroup + * We guarantee (all but first) constructors only generate from sub-manifold. + * However, round-off errors in repeated composition could move off it... + */ +class GTSAM_EXPORT SO3: public Matrix3, public LieGroup { + +protected: + +public: + enum { + dimension = 3 + }; + + /// @name Constructors + /// @{ + + /// Constructor from AngleAxisd + SO3() : + Matrix3(I_3x3) { + } + + /// Constructor from Eigen Matrix + template + SO3(const MatrixBase& R) : + Matrix3(R.eval()) { + } + + /// Constructor from AngleAxisd + SO3(const Eigen::AngleAxisd& angleAxis) : + Matrix3(angleAxis) { + } + + /// Static, named constructor TODO think about relation with above + static SO3 Rodrigues(const Vector3& axis, double theta); + + /// @} + /// @name Testable + /// @{ + + void print(const std::string& s) const { + std::cout << s << *this << std::endl; + } + + bool equals(const SO3 & R, double tol) const { + return equal_with_abs_tol(*this, R, tol); + } + + /// @} + /// @name Group + /// @{ + + /// identity rotation for group operation + static SO3 identity() { + return I_3x3; + } + + /// inverse of a rotation = transpose + SO3 inverse() const { + return this->Matrix3::inverse(); + } + + /// @} + /// @name Lie Group + /// @{ + + /** + * Exponential map at identity - create a rotation from canonical coordinates + * \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula + */ + static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none); + + /** + * Log map at identity - returns the canonical coordinates + * \f$ [R_x,R_y,R_z] \f$ of this rotation + */ + static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none); + + /// Derivative of Expmap + static Matrix3 ExpmapDerivative(const Vector3& omega); + + /// Derivative of Logmap + static Matrix3 LogmapDerivative(const Vector3& omega); + + Matrix3 AdjointMap() const { + return *this; + } + + // Chart at origin + struct ChartAtOrigin { + static SO3 Retract(const Vector3& omega, ChartJacobian H = boost::none) { + return Expmap(omega, H); + } + static Vector3 Local(const SO3& R, ChartJacobian H = boost::none) { + return Logmap(R, H); + } + }; + + using LieGroup::inverse; + + /// @} +}; + +template<> +struct traits : public internal::LieGroup { +}; + +template<> +struct traits : public internal::LieGroup { +}; +} // end namespace gtsam + diff --git a/gtsam/geometry/SimpleCamera.cpp b/gtsam/geometry/SimpleCamera.cpp index cab871874..d92c5bdd7 100644 --- a/gtsam/geometry/SimpleCamera.cpp +++ b/gtsam/geometry/SimpleCamera.cpp @@ -21,27 +21,27 @@ namespace gtsam { - SimpleCamera simpleCamera(const Matrix& P) { + SimpleCamera simpleCamera(const Matrix34& P) { // P = [A|a] = s K cRw [I|-T], with s the unknown scale - Matrix A = P.topLeftCorner(3, 3); - Vector a = P.col(3); + Matrix3 A = P.topLeftCorner(3, 3); + Vector3 a = P.col(3); // do RQ decomposition to get s*K and cRw angles - Matrix sK; - Vector xyz; + Matrix3 sK; + Vector3 xyz; boost::tie(sK, xyz) = RQ(A); // Recover scale factor s and K double s = sK(2, 2); - Matrix K = sK / s; + Matrix3 K = sK / s; // Recover cRw itself, and its inverse Rot3 cRw = Rot3::RzRyRx(xyz); Rot3 wRc = cRw.inverse(); // Now, recover T from a = - s K cRw T = - A T - Vector T = -(A.inverse() * a); + Vector3 T = -(A.inverse() * a); return SimpleCamera(Pose3(wRc, T), Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2))); } diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index 557654d2d..a119096d4 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -24,8 +24,115 @@ namespace gtsam { /// A simple camera class with a Cal3_S2 calibration - typedef PinholeCamera SimpleCamera; +typedef gtsam::PinholeCamera PinholeCameraCal3_S2; + +/** + * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x + * Use PinholeCameraCal3_S2 instead + */ +class SimpleCamera : public PinholeCameraCal3_S2 { + + typedef PinholeCamera Base; + typedef boost::shared_ptr shared_ptr; + +public: + + /// @name Standard Constructors + /// @{ + + /** default constructor */ + SimpleCamera() : + Base() { + } + + /** constructor with pose */ + explicit SimpleCamera(const Pose3& pose) : + Base(pose) { + } + + /** constructor with pose and calibration */ + SimpleCamera(const Pose3& pose, const Cal3_S2& K) : + Base(pose, K) { + } + + /// @} + /// @name Named Constructors + /// @{ + + /** + * Create a level camera at the given 2D pose and height + * @param K the calibration + * @param pose2 specifies the location and viewing direction + * (theta 0 = looking in direction of positive X axis) + * @param height camera height + */ + static SimpleCamera Level(const Cal3_S2 &K, const Pose2& pose2, + double height) { + return SimpleCamera(Base::LevelPose(pose2, height), K); + } + + /// PinholeCamera::level with default calibration + static SimpleCamera Level(const Pose2& pose2, double height) { + return SimpleCamera::Level(Cal3_S2(), pose2, height); + } + + /** + * Create a camera at the given eye position looking at a target point in the scene + * with the specified up direction vector. + * @param eye specifies the camera position + * @param target the point to look at + * @param upVector specifies the camera up direction vector, + * doesn't need to be on the image plane nor orthogonal to the viewing axis + * @param K optional calibration parameter + */ + static SimpleCamera Lookat(const Point3& eye, const Point3& target, + const Point3& upVector, const Cal3_S2& K = Cal3_S2()) { + return SimpleCamera(Base::LookatPose(eye, target, upVector), K); + } + + /// @} + /// @name Advanced Constructors + /// @{ + + /// Init from vector, can be 6D (default calibration) or dim + explicit SimpleCamera(const Vector &v) : + Base(v) { + } + + /// Init from Vector and calibration + SimpleCamera(const Vector &v, const Vector &K) : + Base(v, K) { + } + + /// Copy this object as its actual derived type. + SimpleCamera::shared_ptr clone() const { return boost::make_shared(*this); } + + + /// @} + /// @name Manifold + /// @{ + + /// move a cameras according to d + SimpleCamera retract(const Vector& d) const { + if ((size_t) d.size() == 6) + return SimpleCamera(this->pose().retract(d), calibration()); + else + return SimpleCamera(this->pose().retract(d.head(6)), + calibration().retract(d.tail(calibration().dim()))); + } + + /// @} + +}; + +template<> +struct traits : public internal::Manifold { +}; + +template<> +struct traits : public internal::Manifold { +}; /// Recover camera from 3*4 camera matrix - GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix& P); + GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P); } diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index f48c188aa..9e5b88b31 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -29,15 +29,15 @@ namespace gtsam { } /* ************************************************************************* */ - StereoPoint2 StereoCamera::project(const Point3& point, - boost::optional H1, boost::optional H2) const { + StereoPoint2 StereoCamera::project(const Point3& point) const { + return project2(point); + } + + /* ************************************************************************* */ + StereoPoint2 StereoCamera::project2(const Point3& point, + OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2) const { -#ifdef STEREOCAMERA_CHAIN_RULE - const Point3 q = leftCamPose_.transform_to(point, H1, H2); -#else - // omit derivatives const Point3 q = leftCamPose_.transform_to(point); -#endif if ( q.z() <= 0 ) throw StereoCheiralityException(); @@ -55,30 +55,20 @@ namespace gtsam { // check if derivatives need to be computed if (H1 || H2) { -#ifdef STEREOCAMERA_CHAIN_RULE - // just implement chain rule - Matrix D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian - if (H1) *H1 = D_project_point*(*H1); - if (H2) *H2 = D_project_point*(*H2); -#else // optimized version, see StereoCamera.nb if (H1) { const double v1 = v/fy, v2 = fx*v1, dx=d*x; - *H1 = (Matrix(3, 6) << - uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL, + *H1 << uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL, uR*v1, -fx-dx*uR, v2, -dfx, 0.0, d*uR, - fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v - ).finished(); + fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v; } if (H2) { - const Matrix R(leftCamPose_.rotation().matrix()); - *H2 = d * (Matrix(3, 3) << - fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL, - fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR, - fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v - ).finished(); + const Matrix3 R(leftCamPose_.rotation().matrix()); + *H2 << fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL, + fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR, + fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v; + *H2 << d * (*H2); } -#endif } // finally translate @@ -86,15 +76,23 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const { - double d = 1.0 / P.z(), d2 = d*d; - const Cal3_S2Stereo& K = *K_; - double f_x = K.fx(), f_y = K.fy(), b = K.baseline(); - return (Matrix(3, 3) << - f_x*d, 0.0, -d2*f_x* P.x(), - f_x*d, 0.0, -d2*f_x*(P.x() - b), - 0.0, f_y*d, -d2*f_y* P.y() - ).finished(); + StereoPoint2 StereoCamera::project(const Point3& point, + OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2, + OptionalJacobian<3,0> H3) const { + if (H3) + throw std::runtime_error( + "StereoCamera::project does not support third derivative - BTW use project2"); + return project2(point,H1,H2); + } + + /* ************************************************************************* */ + Point3 StereoCamera::backproject(const StereoPoint2& z) const { + Vector measured = z.vector(); + double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]); + double X = Z * (measured[0] - K_->px()) / K_->fx(); + double Y = Z * (measured[2] - K_->py()) / K_->fy(); + Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z)); + return world_point; } } diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 9c326a8d2..35cf437e9 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -17,9 +17,6 @@ #pragma once -#include - -#include #include #include #include @@ -28,30 +25,47 @@ namespace gtsam { class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error { public: - StereoCheiralityException() : std::runtime_error("Stereo Cheirality Exception") {} + StereoCheiralityException() : + std::runtime_error("Stereo Cheirality Exception") { + } }; - /** * A stereo camera class, parameterize by left camera pose and stereo calibration * @addtogroup geometry */ class GTSAM_EXPORT StereoCamera { +public: + + /** + * Some classes template on either PinholeCamera or StereoCamera, + * and this typedef informs those classes what "project" returns. + */ + typedef StereoPoint2 Measurement; + private: Pose3 leftCamPose_; Cal3_S2Stereo::shared_ptr K_; public: + enum { + dimension = 6 + }; + /// @name Standard Constructors /// @{ - StereoCamera() { + /// Default constructor allocates a calibration! + StereoCamera() : + K_(new Cal3_S2Stereo()) { } + /// Construct from pose and shared calibration StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K); + /// Return shared pointer to calibration const Cal3_S2Stereo::shared_ptr calibration() const { return K_; } @@ -60,26 +74,28 @@ public: /// @name Testable /// @{ + /// print void print(const std::string& s = "") const { leftCamPose_.print(s + ".camera."); K_->print(s + ".calibration."); } + /// equals bool equals(const StereoCamera &camera, double tol = 1e-9) const { - return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals( - *camera.K_, tol); + return leftCamPose_.equals(camera.leftCamPose_, tol) + && K_->equals(*camera.K_, tol); } /// @} /// @name Manifold /// @{ - /** Dimensionality of the tangent space */ + /// Dimensionality of the tangent space inline size_t dim() const { return 6; } - /** Dimensionality of the tangent space */ + /// Dimensionality of the tangent space static inline size_t Dim() { return 6; } @@ -90,85 +106,69 @@ public: } /// Local coordinates of manifold neighborhood around current value - inline Vector localCoordinates(const StereoCamera& t2) const { - return Vector(leftCamPose_.localCoordinates(t2.leftCamPose_)); - } - - Pose3 between(const StereoCamera &camera, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - return leftCamPose_.between(camera.pose(), H1, H2); + inline Vector6 localCoordinates(const StereoCamera& t2) const { + return leftCamPose_.localCoordinates(t2.leftCamPose_); } /// @} /// @name Standard Interface /// @{ + /// pose const Pose3& pose() const { return leftCamPose_; } + /// baseline double baseline() const { return K_->baseline(); } - /* - * project 3D point and compute optional derivatives - */ - StereoPoint2 project(const Point3& point, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + /// Project 3D point to StereoPoint2 (uL,uR,v) + StereoPoint2 project(const Point3& point) const; - /* - * to accomodate tsam's assumption that K is estimated, too + /** Project 3D point and compute optional derivatives + * @param H1 derivative with respect to pose + * @param H2 derivative with respect to point */ - StereoPoint2 project(const Point3& point, - boost::optional H1, - boost::optional H1_k, - boost::optional H2) const { - return project(point, H1, H2); - } + StereoPoint2 project2(const Point3& point, OptionalJacobian<3, 6> H1 = + boost::none, OptionalJacobian<3, 3> H2 = boost::none) const; - Point3 backproject(const StereoPoint2& z) const { - Vector measured = z.vector(); - double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]); - double X = Z *(measured[0]- K_->px()) / K_->fx(); - double Y = Z *(measured[2]- K_->py()) / K_->fy(); - Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z)); - return world_point; - } + /// back-project a measurement + Point3 backproject(const StereoPoint2& z) const; + + /// @} + /// @name Deprecated + /// @{ + + /** Project 3D point and compute optional derivatives + * @deprecated, use project2 - this class has fixed calibration + * @param H1 derivative with respect to pose + * @param H2 derivative with respect to point + * @param H3 IGNORED (for calibration) + */ + StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1, + OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 = + boost::none) const; /// @} private: - /** utility functions */ - Matrix Dproject_to_stereo_camera1(const Point3& P) const; friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(leftCamPose_); ar & BOOST_SERIALIZATION_NVP(K_); } }; -// Define GTSAM traits -namespace traits { - template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ +struct traits : public internal::Manifold { }; template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ +struct traits : public internal::Manifold { }; - -template<> -struct GTSAM_EXPORT zero { - static StereoCamera value() { return StereoCamera();} -}; - -} - } diff --git a/gtsam/geometry/StereoPoint2.cpp b/gtsam/geometry/StereoPoint2.cpp index f599a2dea..cd6f09507 100644 --- a/gtsam/geometry/StereoPoint2.cpp +++ b/gtsam/geometry/StereoPoint2.cpp @@ -19,10 +19,18 @@ #include using namespace std; -using namespace gtsam; + +namespace gtsam { /* ************************************************************************* */ void StereoPoint2::print(const string& s) const { cout << s << "(" << uL_ << ", " << uR_ << ", " << v_ << ")" << endl; } + /* ************************************************************************* */ +ostream &operator<<(ostream &os, const StereoPoint2& p) { + os << '(' << p.uL() << ", " << p.uR() << ", " << p.v() << ')'; + return os; +} + +} // namespace gtsam diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index d62a3f067..1b9559e67 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -18,173 +18,145 @@ #pragma once -#include #include +#include +#include namespace gtsam { - /** - * A 2D stereo point, v will be same for rectified images - * @addtogroup geometry - * \nosubgrouping - */ - class GTSAM_EXPORT StereoPoint2 { - public: - static const size_t dimension = 3; - private: - double uL_, uR_, v_; +/** + * A 2D stereo point, v will be same for rectified images + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT StereoPoint2 { +private: - public: + double uL_, uR_, v_; - /// @name Standard Constructors - /// @{ +public: + enum { dimension = 3 }; + /// @name Standard Constructors + /// @{ - /** default constructor */ - StereoPoint2() : + /** default constructor */ + StereoPoint2() : uL_(0), uR_(0), v_(0) { - } - - /** constructor */ - StereoPoint2(double uL, double uR, double v) : - uL_(uL), uR_(uR), v_(v) { - } - - /// @} - /// @name Testable - /// @{ - - /** print */ - void print(const std::string& s="") const; - - /** equals */ - bool equals(const StereoPoint2& q, double tol=1e-9) const { - return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol && fabs(v_ - - q.v_) < tol); - } - - /// @} - /// @name Group - /// @{ - - /// identity - inline static StereoPoint2 identity() { return StereoPoint2(); } - - /// inverse - inline StereoPoint2 inverse() const { - return StereoPoint2()- (*this); - } - - /// "Compose", just adds the coordinates of two points. - inline StereoPoint2 compose(const StereoPoint2& p1) const { - return *this + p1; - } - - /// add two stereo points - StereoPoint2 operator+(const StereoPoint2& b) const { - return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_); - } - - /// subtract two stereo points - StereoPoint2 operator-(const StereoPoint2& b) const { - return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); - } - - /// @} - /// @name Manifold - /// @{ - - /// dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } - - /// return dimensionality of tangent space, DOF = 3 - inline size_t dim() const { return dimension; } - - /// Updates a with tangent space delta - inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); } - - /// Returns inverse retraction - inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); } - - /// @} - /// @name Lie Group - /// @{ - - /** Exponential map around identity - just create a Point2 from a vector */ - static inline StereoPoint2 Expmap(const Vector& d) { - return StereoPoint2(d(0), d(1), d(2)); - } - - /** Log map around identity - just return the Point2 as a vector */ - static inline Vector Logmap(const StereoPoint2& p) { - return p.vector(); - } - - /** The difference between another point and this point */ - inline StereoPoint2 between(const StereoPoint2& p2) const { - return gtsam::between_default(*this, p2); - } - - /// @} - /// @name Standard Interface - /// @{ - - /// get uL - inline double uL() const {return uL_;} - - /// get uR - inline double uR() const {return uR_;} - - /// get v - inline double v() const {return v_;} - - /** convert to vector */ - Vector3 vector() const { - return Vector3(uL_, uR_, v_); - } - - /** convenient function to get a Point2 from the left image */ - inline Point2 point2(){ - return Point2(uL_, v_); - } - - /** convenient function to get a Point2 from the right image */ - inline Point2 right(){ - return Point2(uR_, v_); - } - - private: - - /// @} - /// @name Advanced Interface - /// @{ - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(uL_); - ar & BOOST_SERIALIZATION_NVP(uR_); - ar & BOOST_SERIALIZATION_NVP(v_); - } - - /// @} - - }; - - // Define GTSAM traits - namespace traits { - - template<> - struct is_group : public boost::true_type { - }; - - template<> - struct is_manifold : public boost::true_type { - }; - - template<> - struct dimension : public boost::integral_constant { - }; - } + + /** constructor */ + StereoPoint2(double uL, double uR, double v) : + uL_(uL), uR_(uR), v_(v) { + } + + /// construct from 3D vector + StereoPoint2(const Vector3& v) : + uL_(v(0)), uR_(v(1)), v_(v(2)) {} + + /// @} + /// @name Testable + /// @{ + + /** print */ + void print(const std::string& s = "") const; + + /** equals */ + bool equals(const StereoPoint2& q, double tol = 1e-9) const { + return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol + && fabs(v_ - q.v_) < tol); + } + + /// @} + /// @name Group + /// @{ + + /// identity + inline static StereoPoint2 identity() { + return StereoPoint2(); + } + + /// inverse + StereoPoint2 operator-() const { + return StereoPoint2(-uL_, -uR_, -v_); + } + + /// add + inline StereoPoint2 operator +(const StereoPoint2& b) const { + return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_); + } + + /// subtract + inline StereoPoint2 operator -(const StereoPoint2& b) const { + return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); + } + + /// @} + /// @name Standard Interface + /// @{ + + /// equality + inline bool operator ==(const StereoPoint2& q) const {return uL_== q.uL_ && uR_==q.uR_ && v_ == q.v_;} + + /// get uL + inline double uL() const {return uL_;} + + /// get uR + inline double uR() const {return uR_;} + + /// get v + inline double v() const {return v_;} + + /** convert to vector */ + Vector3 vector() const { + return Vector3(uL_, uR_, v_); + } + + /** convenient function to get a Point2 from the left image */ + Point2 point2() const { + return Point2(uL_, v_); + } + + /** convenient function to get a Point2 from the right image */ + Point2 right() const { + return Point2(uR_, v_); + } + + /// @name Deprecated + /// @{ + inline StereoPoint2 inverse() const { return StereoPoint2()- (*this);} + inline StereoPoint2 compose(const StereoPoint2& p1) const { return *this + p1;} + inline StereoPoint2 between(const StereoPoint2& p2) const { return p2 - *this; } + inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); } + inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); } + static inline Vector Logmap(const StereoPoint2& p) { return p.vector(); } + static inline StereoPoint2 Expmap(const Vector& d) { return StereoPoint2(d(0), d(1), d(2)); } + /// @} + + /// Streaming + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p); + +private: + + /// @} + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(uL_); + ar & BOOST_SERIALIZATION_NVP(uR_); + ar & BOOST_SERIALIZATION_NVP(v_); + } + + /// @} + +}; + +template<> +struct traits : public internal::VectorSpace {}; + +template<> +struct traits : public internal::VectorSpace {}; } diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 5e13129cc..cc3865b8e 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -31,6 +31,10 @@ # pragma clang diagnostic pop #endif +#ifdef GTSAM_USE_TBB +#include +#endif + #include #include @@ -39,14 +43,13 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Unit3 Unit3::FromPoint3(const Point3& point, boost::optional H) { +Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { Unit3 direction(point); if (H) { // 3*3 Derivative of representation with respect to point is 3*3: - Matrix D_p_point; + Matrix3 D_p_point; point.normalize(D_p_point); // TODO, this calculates norm a second time :-( // Calculate the 2*3 Jacobian - H->resize(2, 3); *H << direction.basis().transpose() * D_p_point; } return direction; @@ -66,8 +69,15 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { return result; } +#ifdef GTSAM_USE_TBB +tbb::mutex unit3BasisMutex; +#endif + /* ************************************************************************* */ -const Unit3::Matrix32& Unit3::basis() const { +const Matrix32& Unit3::basis() const { +#ifdef GTSAM_USE_TBB + tbb::mutex::scoped_lock lock(unit3BasisMutex); +#endif // Return cached version if exists if (B_) @@ -92,7 +102,7 @@ const Unit3::Matrix32& Unit3::basis() const { b2 = b2 / b2.norm(); // Create the basis matrix - B_.reset(Unit3::Matrix32()); + B_.reset(Matrix32()); (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); return *B_; } @@ -104,38 +114,39 @@ void Unit3::print(const std::string& s) const { } /* ************************************************************************* */ -Matrix Unit3::skew() const { +Matrix3 Unit3::skew() const { return skewSymmetric(p_.x(), p_.y(), p_.z()); } /* ************************************************************************* */ -Vector Unit3::error(const Unit3& q, boost::optional H) const { +Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const { // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1 - Matrix Bt = basis().transpose(); - Vector xi = Bt * q.p_.vector(); + Matrix23 Bt = basis().transpose(); + Vector2 xi = Bt * q.p_.vector(); if (H) *H = Bt * q.basis(); return xi; } /* ************************************************************************* */ -double Unit3::distance(const Unit3& q, boost::optional H) const { - Vector xi = error(q, H); +double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const { + Matrix2 H_; + Vector2 xi = error(q, H_); double theta = xi.norm(); if (H) - *H = (xi.transpose() / theta) * (*H); + *H = (xi.transpose() / theta) * H_; return theta; } /* ************************************************************************* */ -Unit3 Unit3::retract(const Vector& v) const { +Unit3 Unit3::retract(const Vector2& v) const { // Get the vector form of the point and the basis matrix - Vector p = Point3::Logmap(p_); - Matrix B = basis(); + Vector3 p = p_.vector(); + Matrix32 B = basis(); // Compute the 3D xi_hat vector - Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1); + Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1); double xi_hat_norm = xi_hat.norm(); @@ -147,28 +158,27 @@ Unit3 Unit3::retract(const Vector& v) const { return Unit3(-point3()); } - Vector exp_p_xi_hat = cos(xi_hat_norm) * p + Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm); return Unit3(exp_p_xi_hat); } /* ************************************************************************* */ -Vector Unit3::localCoordinates(const Unit3& y) const { +Vector2 Unit3::localCoordinates(const Unit3& y) const { - Vector p = Point3::Logmap(p_); - Vector q = Point3::Logmap(y.p_); + Vector3 p = p_.vector(), q = y.p_.vector(); double dot = p.dot(q); // Check for special cases if (std::abs(dot - 1.0) < 1e-16) return Vector2(0, 0); else if (std::abs(dot + 1.0) < 1e-16) - return (Vector(2) << M_PI, 0).finished(); + return Vector2(M_PI, 0); else { // no special case double theta = acos(dot); - Vector result_hat = (theta / sin(theta)) * (q - p * dot); + Vector3 result_hat = (theta / sin(theta)) * (q - p * dot); return basis().transpose() * result_hat; } } diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index d04e57d4b..12bfac5ce 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -20,6 +20,7 @@ #pragma once +#include #include #include #include @@ -28,17 +29,19 @@ namespace gtsam { /// Represents a 3D point on a unit sphere. -class GTSAM_EXPORT Unit3{ +class GTSAM_EXPORT Unit3 { private: - typedef Eigen::Matrix Matrix32; - Point3 p_; ///< The location of the point on the unit sphere mutable boost::optional B_; ///< Cached basis public: + enum { + dimension = 2 + }; + /// @name Constructors /// @{ @@ -52,6 +55,11 @@ public: p_(p / p.norm()) { } + /// Construct from a vector3 + explicit Unit3(const Vector3& p) : + p_(p / p.norm()) { + } + /// Construct from x,y,z Unit3(double x, double y, double z) : p_(x, y, z) { @@ -59,8 +67,8 @@ public: } /// Named constructor from Point3 with optional Jacobian - static Unit3 FromPoint3(const Point3& point, boost::optional H = - boost::none); + static Unit3 FromPoint3(const Point3& point, // + OptionalJacobian<2, 3> H = boost::none); /// Random direction, using boost::uniform_on_sphere static Unit3 Random(boost::mt19937 & rng); @@ -90,27 +98,32 @@ public: const Matrix32& basis() const; /// Return skew-symmetric associated with 3D point on unit sphere - Matrix skew() const; + Matrix3 skew() const; /// Return unit-norm Point3 - const Point3& point3(boost::optional H = boost::none) const { + const Point3& point3(OptionalJacobian<3, 2> H = boost::none) const { if (H) *H = basis(); return p_; } + /// Return unit-norm Vector + Vector3 unitVector(boost::optional H = boost::none) const { + if (H) + *H = basis(); + return (p_.vector()); + } + /// Return scaled direction as Point3 friend Point3 operator*(double s, const Unit3& d) { return s * d.p_; } /// Signed, vector-valued error between two directions - Vector error(const Unit3& q, - boost::optional H = boost::none) const; + Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const; /// Distance between two directions - double distance(const Unit3& q, - boost::optional H = boost::none) const; + double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const; /// @} @@ -133,10 +146,10 @@ public: }; /// The retract function - Unit3 retract(const Vector& v) const; + Unit3 retract(const Vector2& v) const; /// The local coordinates function - Vector localCoordinates(const Unit3& s) const; + Vector2 localCoordinates(const Unit3& s) const; /// @} @@ -144,38 +157,30 @@ private: /// @name Advanced Interface /// @{ - /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(p_); - ar & BOOST_SERIALIZATION_NVP(B_); - } + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(p_); + // homebrew serialize Eigen Matrix + ar & boost::serialization::make_nvp("B11", (*B_)(0, 0)); + ar & boost::serialization::make_nvp("B12", (*B_)(0, 1)); + ar & boost::serialization::make_nvp("B21", (*B_)(1, 0)); + ar & boost::serialization::make_nvp("B22", (*B_)(1, 1)); + ar & boost::serialization::make_nvp("B31", (*B_)(2, 0)); + ar & boost::serialization::make_nvp("B32", (*B_)(2, 1)); + } /// @} }; // Define GTSAM traits -namespace traits { - -template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ +template<> struct traits : public internal::Manifold { }; -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ +template<> struct traits : public internal::Manifold { }; -template<> -struct GTSAM_EXPORT zero { - static Unit3 value() { - return Unit3(); - } -}; - -} - } // namespace gtsam diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index 86c0b7e33..aa06a2e29 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -60,7 +60,7 @@ TEST( Cal3_S2, calibrate_homogeneous) { Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { return k.uncalibrate(pt); } TEST( Cal3_S2, Duncalibrate1) { - Matrix computed; K.uncalibrate(p, computed, boost::none); + Matrix25 computed; K.uncalibrate(p, computed, boost::none); Matrix numerical = numericalDerivative21(uncalibrate_, K, p); CHECK(assert_equal(numerical,computed,1e-8)); } diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 5aeee03d4..c02a11928 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -15,18 +15,21 @@ * @brief test CalibratedCamera class */ -#include - -#include +#include +#include #include #include -#include + +#include + +#include using namespace std; using namespace gtsam; GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera) +// Camera situated at 0.5 meters high, looking down static const Pose3 pose1((Matrix)(Matrix(3,3) << 1., 0., 0., 0.,-1., 0., @@ -87,32 +90,45 @@ TEST( CalibratedCamera, project) /* ************************************************************************* */ TEST( CalibratedCamera, Dproject_to_camera1) { Point3 pp(155,233,131); - Matrix test1; - CalibratedCamera::project_to_camera(pp, test1); - Matrix test2 = numericalDerivative11( + Matrix actual; + CalibratedCamera::project_to_camera(pp, actual); + Matrix expected_numerical = numericalDerivative11( boost::bind(CalibratedCamera::project_to_camera, _1, boost::none), pp); - CHECK(assert_equal(test1, test2)); + CHECK(assert_equal(expected_numerical, actual)); } /* ************************************************************************* */ -static Point2 project2(const Pose3& pose, const Point3& point) { - return CalibratedCamera(pose).project(point); +static Point2 project2(const CalibratedCamera& camera, const Point3& point) { + return camera.project(point); } TEST( CalibratedCamera, Dproject_point_pose) { Matrix Dpose, Dpoint; Point2 result = camera.project(point1, Dpose, Dpoint); - Matrix numerical_pose = numericalDerivative21(project2, pose1, point1); - Matrix numerical_point = numericalDerivative22(project2, pose1, point1); + Matrix numerical_pose = numericalDerivative21(project2, camera, point1); + Matrix numerical_point = numericalDerivative22(project2, camera, point1); CHECK(assert_equal(Point3(-0.08, 0.08, 0.5), camera.pose().transform_to(point1))); CHECK(assert_equal(Point2(-.16, .16), result)); CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); } +/* ************************************************************************* */ +// Add a test with more arbitrary rotation +TEST( CalibratedCamera, Dproject_point_pose2) +{ + static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const CalibratedCamera camera(pose1); + Matrix Dpose, Dpoint; + camera.project(point1, Dpose, Dpoint); + Matrix numerical_pose = numericalDerivative21(project2, camera, point1); + Matrix numerical_point = numericalDerivative22(project2, camera, point1); + CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp new file mode 100644 index 000000000..42cf0f299 --- /dev/null +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -0,0 +1,114 @@ +/* ---------------------------------------------------------------------------- + + * 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 testCameraSet.cpp + * @brief Unit tests for testCameraSet Class + * @author Frank Dellaert + * @date Feb 19, 2015 + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +// Cal3Bundler test +#include +#include +TEST(CameraSet, Pinhole) { + typedef PinholeCamera Camera; + typedef vector ZZ; + CameraSet set; + Camera camera; + set.push_back(camera); + set.push_back(camera); + Point3 p(0, 0, 1); + CHECK(assert_equal(set, set)); + CameraSet set2 = set; + set2.push_back(camera); + CHECK(!set.equals(set2)); + + // Check measurements + Point2 expected; + ZZ z = set.project(p); + CHECK(assert_equal(expected, z[0])); + CHECK(assert_equal(expected, z[1])); + + // Calculate expected derivatives using Pinhole + Matrix46 actualF; + Matrix43 actualE; + Matrix43 actualH; + { + Matrix26 F1; + Matrix23 E1; + Matrix23 H1; + camera.project(p, F1, E1, H1); + actualE << E1, E1; + actualF << F1, F1; + actualH << H1, H1; + } + + // Check computed derivatives + Matrix F, E, H; + set.project(p, F, E, H); + CHECK(assert_equal(actualF, F)); + CHECK(assert_equal(actualE, E)); + CHECK(assert_equal(actualH, H)); +} + +/* ************************************************************************* */ +#include +TEST(CameraSet, Stereo) { + typedef vector ZZ; + CameraSet set; + StereoCamera camera; + set.push_back(camera); + set.push_back(camera); + Point3 p(0, 0, 1); + EXPECT_LONGS_EQUAL(6, traits::dimension); + + // Check measurements + StereoPoint2 expected(0, -1, 0); + ZZ z = set.project(p); + CHECK(assert_equal(expected, z[0])); + CHECK(assert_equal(expected, z[1])); + + // Calculate expected derivatives using Pinhole + Matrix66 actualF; + Matrix63 actualE; + { + Matrix36 F1; + Matrix33 E1; + camera.project(p, F1, E1); + actualE << E1, E1; + actualF << F1, F1; + } + + // Check computed derivatives + Matrix F, E; + set.project(p, F, E); + CHECK(assert_equal(actualF, F)); + CHECK(assert_equal(actualE, E)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp new file mode 100644 index 000000000..7becfc75f --- /dev/null +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -0,0 +1,141 @@ +/* ---------------------------------------------------------------------------- + + * 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 testCyclic.cpp + * @brief Unit tests for cyclic group + * @author Frank Dellaert + **/ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +typedef Cyclic<3> Z3; // Let's use the cyclic group of order 3 +typedef Cyclic<2> Z2; + +//****************************************************************************** +TEST(Cyclic, Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + EXPECT_LONGS_EQUAL(0, traits::Identity()); +} + +//****************************************************************************** +TEST(Cyclic, Constructor) { + Z3 g(0); +} + +//****************************************************************************** +TEST(Cyclic, Compose) { + EXPECT_LONGS_EQUAL(0, traits::Compose(Z3(0),Z3(0))); + EXPECT_LONGS_EQUAL(1, traits::Compose(Z3(0),Z3(1))); + EXPECT_LONGS_EQUAL(2, traits::Compose(Z3(0),Z3(2))); + + EXPECT_LONGS_EQUAL(2, traits::Compose(Z3(2),Z3(0))); + EXPECT_LONGS_EQUAL(0, traits::Compose(Z3(2),Z3(1))); + EXPECT_LONGS_EQUAL(1, traits::Compose(Z3(2),Z3(2))); +} + +//****************************************************************************** +TEST(Cyclic, Between) { + EXPECT_LONGS_EQUAL(0, traits::Between(Z3(0),Z3(0))); + EXPECT_LONGS_EQUAL(1, traits::Between(Z3(0),Z3(1))); + EXPECT_LONGS_EQUAL(2, traits::Between(Z3(0),Z3(2))); + + EXPECT_LONGS_EQUAL(1, traits::Between(Z3(2),Z3(0))); + EXPECT_LONGS_EQUAL(2, traits::Between(Z3(2),Z3(1))); + EXPECT_LONGS_EQUAL(0, traits::Between(Z3(2),Z3(2))); +} + +//****************************************************************************** +TEST(Cyclic, Inverse) { + EXPECT_LONGS_EQUAL(0, traits::Inverse(Z3(0))); + EXPECT_LONGS_EQUAL(2, traits::Inverse(Z3(1))); + EXPECT_LONGS_EQUAL(1, traits::Inverse(Z3(2))); +} + +//****************************************************************************** +TEST(Cyclic, Negation) { + EXPECT_LONGS_EQUAL(0, -Z3(0)); + EXPECT_LONGS_EQUAL(2, -Z3(1)); + EXPECT_LONGS_EQUAL(1, -Z3(2)); +} + +//****************************************************************************** +TEST(Cyclic, Negation2) { + EXPECT_LONGS_EQUAL(0, -Z2(0)); + EXPECT_LONGS_EQUAL(1, -Z2(1)); +} + +//****************************************************************************** +TEST(Cyclic , Invariants) { + Z3 g(2), h(1); + EXPECT(check_group_invariants(g,h)); +} + +//****************************************************************************** +// The Direct sum of Z2 and Z2 is *not* Cyclic<4>, but the +// smallest non-cyclic group called the Klein four-group: +typedef DirectSum K4; + +namespace gtsam { + +/// Define K4 to be a model of the Additive Group concept, and provide Testable +template<> +struct traits : internal::AdditiveGroupTraits { + static void Print(const K4& m, const string& s = "") { + cout << s << "(" << m.first << "," << m.second << ")" << endl; + } + static bool Equals(const K4& m1, const K4& m2, double tol = 1e-8) { + return m1 == m2; + } +}; + +} // namespace gtsam + +TEST(Cyclic , DirectSum) { + // The Direct sum of Z2 and Z2 is *not* Cyclic<4>, but the + // smallest non-cyclic group called the Klein four-group: + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsTestable)); + + // Refer to http://en.wikipedia.org/wiki/Klein_four-group + K4 e(0,0), a(0, 1), b(1, 0), c(1, 1); + EXPECT(assert_equal(a, - a)); + EXPECT(assert_equal(b, - b)); + EXPECT(assert_equal(c, - c)); + EXPECT(assert_equal(a, a + e)); + EXPECT(assert_equal(b, b + e)); + EXPECT(assert_equal(c, c + e)); + EXPECT(assert_equal(e, a + a)); + EXPECT(assert_equal(e, b + b)); + EXPECT(assert_equal(e, c + c)); + EXPECT(assert_equal(c, a + b)); + EXPECT(assert_equal(b, a + c)); + EXPECT(assert_equal(a, b + c)); + EXPECT(assert_equal(c, a - b)); + EXPECT(assert_equal(a, b - c)); + EXPECT(assert_equal(b, c - a)); + EXPECT(check_group_invariants(a, b)); + EXPECT(check_group_invariants(b, c)); + EXPECT(check_group_invariants(c, a)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index d2990a747..fe27b2911 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -30,6 +30,14 @@ TEST (EssentialMatrix, equality) { EXPECT(assert_equal(expected, actual)); } +//************************************************************************* +TEST (EssentialMatrix, FromPose3) { + EssentialMatrix expected(c1Rc2, Unit3(c1Tc2)); + Pose3 pose(c1Rc2, c1Tc2); + EssentialMatrix actual = EssentialMatrix::FromPose3(pose); + EXPECT(assert_equal(expected, actual)); +} + //******************************************************************************* TEST(EssentialMatrix, localCoordinates0) { EssentialMatrix E; @@ -47,11 +55,11 @@ TEST (EssentialMatrix, localCoordinates) { Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose)); EXPECT(assert_equal(zero(5), actual, 1e-8)); - Vector d = zero(6); - d(4) += 1e-5; + Vector6 d; + d << 0.1, 0.2, 0.3, 0, 0, 0; Vector actual2 = hx.localCoordinates( EssentialMatrix::FromPose3(pose.retract(d))); - EXPECT(assert_equal(zero(5), actual2, 1e-8)); + EXPECT(assert_equal(d.head(5), actual2, 1e-8)); } //************************************************************************* @@ -75,6 +83,15 @@ TEST (EssentialMatrix, retract2) { EXPECT(assert_equal(expected, actual)); } +//************************************************************************* +TEST (EssentialMatrix, RoundTrip) { + Vector5 d; + d << 0.1, 0.2, 0.3, 0.4, 0.5; + EssentialMatrix e, hx = e.retract(d); + Vector actual = e.localCoordinates(hx); + EXPECT(assert_equal(d, actual, 1e-8)); +} + //************************************************************************* Point3 transform_to_(const EssentialMatrix& E, const Point3& point) { return E.transform_to(point); diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp new file mode 100644 index 000000000..b2b4ecc43 --- /dev/null +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -0,0 +1,119 @@ +/* ---------------------------------------------------------------------------- + + * 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 testOrientedPlane3.cpp + * @date Dec 19, 2012 + * @author Alex Trevor + * @brief Tests the OrientedPlane3 class + */ + +#include +#include +#include +#include + +using namespace boost::assign; +using namespace gtsam; +using namespace std; +using boost::none; + +GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) +GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) + +//******************************************************************************* +TEST (OrientedPlane3, transform) { + // Test transforming a plane to a pose + gtsam::Pose3 pose(gtsam::Rot3::ypr(-M_PI / 4.0, 0.0, 0.0), + gtsam::Point3(2.0, 3.0, 4.0)); + OrientedPlane3 plane(-1, 0, 0, 5); + OrientedPlane3 expected_meas(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3); + OrientedPlane3 transformed_plane = OrientedPlane3::Transform(plane, pose, + none, none); + EXPECT(assert_equal(expected_meas, transformed_plane, 1e-9)); + + // Test the jacobians of transform + Matrix actualH1, expectedH1, actualH2, expectedH2; + { + expectedH1 = numericalDerivative11( + boost::bind(&OrientedPlane3::Transform, plane, _1, none, none), pose); + + OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, actualH1, + none); + EXPECT(assert_equal(expectedH1, actualH1, 1e-9)); + } + { + expectedH2 = numericalDerivative11( + boost::bind(&OrientedPlane3::Transform, _1, pose, none, none), plane); + + OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, none, + actualH2); + EXPECT(assert_equal(expectedH2, actualH2, 1e-9)); + } + +} + +//******************************************************************************* +// Returns a random vector -- copied from testUnit3.cpp +inline static Vector randomVector(const Vector& minLimits, + const Vector& maxLimits) { + + // Get the number of dimensions and create the return vector + size_t numDims = dim(minLimits); + Vector vector = zero(numDims); + + // Create the random vector + for (size_t i = 0; i < numDims; i++) { + double range = maxLimits(i) - minLimits(i); + vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i); + } + return vector; +} + +//******************************************************************************* +TEST(OrientedPlane3, localCoordinates_retract) { + + size_t numIterations = 10000; + gtsam::Vector minPlaneLimit(4), maxPlaneLimit(4); + minPlaneLimit << -1.0, -1.0, -1.0, 0.01; + maxPlaneLimit << 1.0, 1.0, 1.0, 10.0; + + Vector minXiLimit(3), maxXiLimit(3); + minXiLimit << -M_PI, -M_PI, -10.0; + maxXiLimit << M_PI, M_PI, 10.0; + for (size_t i = 0; i < numIterations; i++) { + + sleep(0); + + // Create a Plane + OrientedPlane3 p1(randomVector(minPlaneLimit, maxPlaneLimit)); + Vector v12 = randomVector(minXiLimit, maxXiLimit); + + // Magnitude of the rotation can be at most pi + if (v12.segment<3>(0).norm() > M_PI) + v12.segment<3>(0) = v12.segment<3>(0) / M_PI; + OrientedPlane3 p2 = p1.retract(v12); + + // Check if the local coordinates and retract return the same results. + Vector actual_v12 = p1.localCoordinates(p2); + EXPECT(assert_equal(v12, actual_v12, 1e-3)); + OrientedPlane3 actual_p2 = p1.retract(actual_v12); + EXPECT(assert_equal(p2, actual_p2, 1e-3)); + } +} + +/* ************************************************************************* */ +int main() { + srand(time(NULL)); + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index ddeae2b7d..9fa9e3468 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -15,29 +15,34 @@ * @brief test PinholeCamera class */ -#include -#include - -#include -#include -#include #include #include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include using namespace std; using namespace gtsam; +typedef PinholeCamera Camera; + static const Cal3_S2 K(625, 625, 0, 0, 0); -static const Pose3 pose1((Matrix)(Matrix(3,3) << - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ).finished(), - Point3(0,0,0.5)); +static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); +static const Camera camera(pose, K); -typedef PinholeCamera Camera; -static const Camera camera(pose1, K); +static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5)); +static const Camera camera1(pose1, K); static const Point3 point1(-0.08,-0.08, 0.0); static const Point3 point2(-0.08, 0.08, 0.0); @@ -52,8 +57,21 @@ static const Point3 point4_inf( 0.16,-0.16, -1.0); /* ************************************************************************* */ TEST( PinholeCamera, constructor) { - CHECK(assert_equal( camera.calibration(), K)); - CHECK(assert_equal( camera.pose(), pose1)); + EXPECT(assert_equal( K, camera.calibration())); + EXPECT(assert_equal( pose, camera.pose())); +} + +//****************************************************************************** +TEST(PinholeCamera, Pose) { + + Matrix actualH; + EXPECT(assert_equal(pose, camera.getPose(actualH))); + + // Check derivative + boost::function f = // + boost::bind(&Camera::getPose,_1,boost::none); + Matrix numericalH = numericalDerivative11(f,camera); + EXPECT(assert_equal(numericalH, actualH, 1e-9)); } /* ************************************************************************* */ @@ -67,7 +85,7 @@ TEST( PinholeCamera, level2) Point3 x(1,0,0),y(0,0,-1),z(0,1,0); Rot3 wRc(x,y,z); Pose3 expected(wRc,Point3(0.4,0.3,0.1)); - CHECK(assert_equal( camera.pose(), expected)); + EXPECT(assert_equal( camera.pose(), expected)); } /* ************************************************************************* */ @@ -80,72 +98,72 @@ TEST( PinholeCamera, lookat) // expected Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); Pose3 expected(Rot3(xc,yc,zc),C); - CHECK(assert_equal( camera.pose(), expected)); + EXPECT(assert_equal( camera.pose(), expected)); Point3 C2(30.0,0.0,10.0); Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0)); Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; - CHECK(assert_equal(I, eye(3))); + EXPECT(assert_equal(I, eye(3))); } /* ************************************************************************* */ TEST( PinholeCamera, project) { - CHECK(assert_equal( camera.project(point1), Point2(-100, 100) )); - CHECK(assert_equal( camera.project(point2), Point2(-100, -100) )); - CHECK(assert_equal( camera.project(point3), Point2( 100, -100) )); - CHECK(assert_equal( camera.project(point4), Point2( 100, 100) )); + EXPECT(assert_equal( camera.project(point1), Point2(-100, 100) )); + EXPECT(assert_equal( camera.project(point2), Point2(-100, -100) )); + EXPECT(assert_equal( camera.project(point3), Point2( 100, -100) )); + EXPECT(assert_equal( camera.project(point4), Point2( 100, 100) )); } /* ************************************************************************* */ TEST( PinholeCamera, backproject) { - CHECK(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1)); - CHECK(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2)); - CHECK(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3)); - CHECK(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4)); + EXPECT(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1)); + EXPECT(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2)); + EXPECT(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3)); + EXPECT(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4)); } /* ************************************************************************* */ TEST( PinholeCamera, backprojectInfinity) { - CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf)); - CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf)); - CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf)); - CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf)); } /* ************************************************************************* */ TEST( PinholeCamera, backproject2) { Point3 origin; - Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down + Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); Point3 actual = camera.backproject(Point2(), 1.); Point3 expected(0., 1., 0.); pair x = camera.projectSafe(expected); - CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(Point2(), x.first)); - CHECK(x.second); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(Point2(), x.first)); + EXPECT(x.second); } /* ************************************************************************* */ TEST( PinholeCamera, backprojectInfinity2) { Point3 origin; - Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down + Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); Point3 actual = camera.backprojectPointAtInfinity(Point2()); Point3 expected(0., 1., 0.); Point2 x = camera.projectPointAtInfinity(expected); - CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(Point2(), x)); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(Point2(), x)); } /* ************************************************************************* */ @@ -159,8 +177,8 @@ TEST( PinholeCamera, backprojectInfinity3) Point3 expected(0., 0., 1.); Point2 x = camera.projectPointAtInfinity(expected); - CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(Point2(), x)); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(Point2(), x)); } /* ************************************************************************* */ @@ -173,13 +191,13 @@ TEST( PinholeCamera, Dproject) { Matrix Dpose, Dpoint, Dcal; Point2 result = camera.project(point1, Dpose, Dpoint, Dcal); - Matrix numerical_pose = numericalDerivative31(project3, pose1, point1, K); - Matrix numerical_point = numericalDerivative32(project3, pose1, point1, K); - Matrix numerical_cal = numericalDerivative33(project3, pose1, point1, K); - CHECK(assert_equal(Point2(-100, 100), result)); - CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); - CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); - CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); + Matrix numerical_pose = numericalDerivative31(project3, pose, point1, K); + Matrix Hexpected2 = numericalDerivative32(project3, pose, point1, K); + Matrix numerical_cal = numericalDerivative33(project3, pose, point1, K); + EXPECT(assert_equal(Point2(-100, 100), result)); + EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); + EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7)); + EXPECT(assert_equal(numerical_cal, Dcal, 1e-7)); } /* ************************************************************************* */ @@ -190,21 +208,21 @@ static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const C TEST( PinholeCamera, Dproject_Infinity) { Matrix Dpose, Dpoint, Dcal; - Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera + Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1 // test Projection Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal); Point2 expected(-5.0, 5.0); - CHECK(assert_equal(actual, expected, 1e-7)); + EXPECT(assert_equal(actual, expected, 1e-7)); // test Jacobians - Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point3D, K); - Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point3D, K); - Matrix numerical_point2x2 = numerical_point.block(0,0,2,2); // only the direction to the point matters - Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point3D, K); - CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); - CHECK(assert_equal(numerical_point2x2, Dpoint, 1e-7)); - CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); + Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose, point3D, K); + Matrix Hexpected2 = numericalDerivative32(projectInfinity3, pose, point3D, K); + Matrix numerical_point2x2 = Hexpected2.block(0,0,2,2); // only the direction to the point matters + Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose, point3D, K); + EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); + EXPECT(assert_equal(numerical_point2x2, Dpoint, 1e-7)); + EXPECT(assert_equal(numerical_cal, Dcal, 1e-7)); } /* ************************************************************************* */ @@ -217,11 +235,122 @@ TEST( PinholeCamera, Dproject2) { Matrix Dcamera, Dpoint; Point2 result = camera.project2(point1, Dcamera, Dpoint); - Matrix numerical_camera = numericalDerivative21(project4, camera, point1); - Matrix numerical_point = numericalDerivative22(project4, camera, point1); - CHECK(assert_equal(result, Point2(-100, 100) )); - CHECK(assert_equal(numerical_camera, Dcamera, 1e-7)); - CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); + Matrix Hexpected1 = numericalDerivative21(project4, camera, point1); + Matrix Hexpected2 = numericalDerivative22(project4, camera, point1); + EXPECT(assert_equal(result, Point2(-100, 100) )); + EXPECT(assert_equal(Hexpected1, Dcamera, 1e-7)); + EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7)); +} + +/* ************************************************************************* */ +// Add a test with more arbitrary rotation +TEST( PinholeCamera, Dproject3) +{ + static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const Camera camera(pose1); + Matrix Dpose, Dpoint; + camera.project2(point1, Dpose, Dpoint); + Matrix numerical_pose = numericalDerivative21(project4, camera, point1); + Matrix numerical_point = numericalDerivative22(project4, camera, point1); + CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); +} + +/* ************************************************************************* */ +static double range0(const Camera& camera, const Point3& point) { + return camera.range(point); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, range0) { + Matrix D1; Matrix D2; + double result = camera.range(point1, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range0, camera, point1); + Matrix Hexpected2 = numericalDerivative22(range0, camera, point1); + EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result, + 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); +} + +/* ************************************************************************* */ +static double range1(const Camera& camera, const Pose3& pose) { + return camera.range(pose); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, range1) { + Matrix D1; Matrix D2; + double result = camera.range(pose1, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1); + Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); +} + +/* ************************************************************************* */ +typedef PinholeCamera Camera2; +static const Cal3Bundler K2(625, 1e-3, 1e-3); +static const Camera2 camera2(pose1, K2); +static double range2(const Camera& camera, const Camera2& camera2) { + return camera.range(camera2); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, range2) { + Matrix D1; Matrix D2; + double result = camera.range(camera2, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2); + Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); +} + +/* ************************************************************************* */ +static const CalibratedCamera camera3(pose1); +static double range3(const Camera& camera, const CalibratedCamera& camera3) { + return camera.range(camera3); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, range3) { + Matrix D1; Matrix D2; + double result = camera.range(camera3, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3); + Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); +} + +/* ************************************************************************* */ +typedef GeneralSFMFactor sfmFactor; +using symbol_shorthand::P; + +/* ************************************************************************* */ +TEST( PinholeCamera, BAL) { + string filename = findExampleDataFile("dubrovnik-3-7-pre"); + SfM_data db; + bool success = readBAL(filename, db); + if (!success) throw runtime_error("Could not access file!"); + + SharedNoiseModel unit2 = noiseModel::Unit::Create(2); + NonlinearFactorGraph graph; + + for (size_t j = 0; j < db.number_tracks(); j++) { + BOOST_FOREACH(const SfM_Measurement& m, db.tracks[j].measurements) + graph.push_back(sfmFactor(m.second, unit2, m.first, P(j))); + } + + Values initial = initialCamerasAndPointsEstimate(db); + + LevenbergMarquardtOptimizer lm(graph, initial); + + Values actual = lm.optimize(); + double actualError = graph.error(actual); + EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-7); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp new file mode 100644 index 000000000..411273c1f --- /dev/null +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -0,0 +1,259 @@ +/* ---------------------------------------------------------------------------- + + * 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 testPinholePose.cpp + * @author Frank Dellaert + * @brief test PinholePose class + * @date Feb 20, 2015 + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +typedef PinholePose Camera; + +static const Cal3_S2::shared_ptr K = boost::make_shared(625, 625, 0, 0, 0); + +static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); +static const Camera camera(pose, K); + +static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5)); +static const Camera camera1(pose1, K); + +static const Point3 point1(-0.08,-0.08, 0.0); +static const Point3 point2(-0.08, 0.08, 0.0); +static const Point3 point3( 0.08, 0.08, 0.0); +static const Point3 point4( 0.08,-0.08, 0.0); + +static const Point3 point1_inf(-0.16,-0.16, -1.0); +static const Point3 point2_inf(-0.16, 0.16, -1.0); +static const Point3 point3_inf( 0.16, 0.16, -1.0); +static const Point3 point4_inf( 0.16,-0.16, -1.0); + +/* ************************************************************************* */ +TEST( PinholePose, constructor) +{ + EXPECT(assert_equal( pose, camera.pose())); +} + +//****************************************************************************** +TEST(PinholeCamera, Pose) { + + Matrix actualH; + EXPECT(assert_equal(pose, camera.getPose(actualH))); + + // Check derivative + boost::function f = // + boost::bind(&Camera::getPose,_1,boost::none); + Matrix numericalH = numericalDerivative11(f,camera); + EXPECT(assert_equal(numericalH, actualH, 1e-9)); +} + +/* ************************************************************************* */ +TEST( PinholePose, lookat) +{ + // Create a level camera, looking in Y-direction + Point3 C(10.0,0.0,0.0); + Camera camera = Camera::Lookat(C, Point3(), Point3(0.0,0.0,1.0)); + + // expected + Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); + Pose3 expected(Rot3(xc,yc,zc),C); + EXPECT(assert_equal( camera.pose(), expected)); + + Point3 C2(30.0,0.0,10.0); + Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0)); + + Matrix R = camera2.pose().rotation().matrix(); + Matrix I = trans(R)*R; + EXPECT(assert_equal(I, eye(3))); +} + +/* ************************************************************************* */ +TEST( PinholePose, project) +{ + EXPECT(assert_equal( camera.project2(point1), Point2(-100, 100) )); + EXPECT(assert_equal( camera.project2(point2), Point2(-100, -100) )); + EXPECT(assert_equal( camera.project2(point3), Point2( 100, -100) )); + EXPECT(assert_equal( camera.project2(point4), Point2( 100, 100) )); +} + +/* ************************************************************************* */ +TEST( PinholePose, backproject) +{ + EXPECT(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1)); + EXPECT(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2)); + EXPECT(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3)); + EXPECT(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4)); +} + +/* ************************************************************************* */ +TEST( PinholePose, backprojectInfinity) +{ + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf)); +} + +/* ************************************************************************* */ +TEST( PinholePose, backproject2) +{ + Point3 origin; + Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down + Camera camera(Pose3(rot, origin), K); + + Point3 actual = camera.backproject(Point2(), 1.); + Point3 expected(0., 1., 0.); + pair x = camera.projectSafe(expected); + + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(Point2(), x.first)); + EXPECT(x.second); +} + +/* ************************************************************************* */ +static Point2 project3(const Pose3& pose, const Point3& point, + const Cal3_S2::shared_ptr& cal) { + return Camera(pose, cal).project2(point); +} + +/* ************************************************************************* */ +TEST( PinholePose, Dproject) +{ + Matrix Dpose, Dpoint; + Point2 result = camera.project2(point1, Dpose, Dpoint); + Matrix numerical_pose = numericalDerivative31(project3, pose, point1, K); + Matrix Hexpected2 = numericalDerivative32(project3, pose, point1, K); + EXPECT(assert_equal(Point2(-100, 100), result)); + EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); + EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7)); +} + +/* ************************************************************************* */ +static Point2 project4(const Camera& camera, const Point3& point) { + return camera.project2(point); +} + +/* ************************************************************************* */ +TEST( PinholePose, Dproject2) +{ + Matrix Dcamera, Dpoint; + Point2 result = camera.project2(point1, Dcamera, Dpoint); + Matrix Hexpected1 = numericalDerivative21(project4, camera, point1); + Matrix Hexpected2 = numericalDerivative22(project4, camera, point1); + EXPECT(assert_equal(result, Point2(-100, 100) )); + EXPECT(assert_equal(Hexpected1, Dcamera, 1e-7)); + EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7)); +} + +/* ************************************************************************* */ +// Add a test with more arbitrary rotation +TEST( CalibratedCamera, Dproject3) +{ + static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const Camera camera(pose1); + Matrix Dpose, Dpoint; + camera.project2(point1, Dpose, Dpoint); + Matrix numerical_pose = numericalDerivative21(project4, camera, point1); + Matrix numerical_point = numericalDerivative22(project4, camera, point1); + CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); +} + +/* ************************************************************************* */ +static double range0(const Camera& camera, const Point3& point) { + return camera.range(point); +} + +/* ************************************************************************* */ +TEST( PinholePose, range0) { + Matrix D1; Matrix D2; + double result = camera.range(point1, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range0, camera, point1); + Matrix Hexpected2 = numericalDerivative22(range0, camera, point1); + EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result, + 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); +} + +/* ************************************************************************* */ +static double range1(const Camera& camera, const Pose3& pose) { + return camera.range(pose); +} + +/* ************************************************************************* */ +TEST( PinholePose, range1) { + Matrix D1; Matrix D2; + double result = camera.range(pose1, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1); + Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); +} + +/* ************************************************************************* */ +typedef PinholePose Camera2; +static const boost::shared_ptr K2 = + boost::make_shared(625, 1e-3, 1e-3); +static const Camera2 camera2(pose1, K2); +static double range2(const Camera& camera, const Camera2& camera2) { + return camera.range(camera2); +} + +/* ************************************************************************* */ +TEST( PinholePose, range2) { + Matrix D1; Matrix D2; + double result = camera.range(camera2, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2); + Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); +} + +/* ************************************************************************* */ +static const CalibratedCamera camera3(pose1); +static double range3(const Camera& camera, const CalibratedCamera& camera3) { + return camera.range(camera3); +} + +/* ************************************************************************* */ +TEST( PinholePose, range3) { + Matrix D1; Matrix D2; + double result = camera.range(camera3, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3); + Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + + diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index ab40928ab..6f8d45b3e 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -27,27 +27,61 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Point2) GTSAM_CONCEPT_LIE_INST(Point2) +//****************************************************************************** +TEST(Double , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); +} + +//****************************************************************************** +TEST(Double , Invariants) { + double p1(2), p2(5); + EXPECT(check_group_invariants(p1, p2)); + EXPECT(check_manifold_invariants(p1, p2)); +} + +//****************************************************************************** +TEST(Point2 , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); +} + +//****************************************************************************** +TEST(Point2 , Invariants) { + Point2 p1(1, 2), p2(4, 5); + EXPECT(check_group_invariants(p1, p2)); + EXPECT(check_manifold_invariants(p1, p2)); +} + /* ************************************************************************* */ TEST(Point2, constructor) { Point2 p1(1, 2), p2 = p1; EXPECT(assert_equal(p1, p2)); } +/* ************************************************************************* */ +TEST(Point2, equality) { + Point2 p1(1, 2), p2(1,3); + EXPECT(!(p1 == p2)); +} + /* ************************************************************************* */ TEST(Point2, Lie) { Point2 p1(1, 2), p2(4, 5); Matrix H1, H2; - EXPECT(assert_equal(Point2(5,7), p1.compose(p2, H1, H2))); + EXPECT(assert_equal(Point2(5,7), traits::Compose(p1, p2, H1, H2))); EXPECT(assert_equal(eye(2), H1)); EXPECT(assert_equal(eye(2), H2)); - EXPECT(assert_equal(Point2(3,3), p1.between(p2, H1, H2))); + EXPECT(assert_equal(Point2(3,3), traits::Between(p1, p2, H1, H2))); EXPECT(assert_equal(-eye(2), H1)); EXPECT(assert_equal(eye(2), H2)); - EXPECT(assert_equal(Point2(5,7), p1.retract(Vector2(4., 5.)))); - EXPECT(assert_equal(Vector2(3.,3.), p1.localCoordinates(p2))); + EXPECT(assert_equal(Point2(5,7), traits::Retract(p1, Vector2(4., 5.)))); + EXPECT(assert_equal(Vector2(3.,3.), traits::Local(p1,p2))); } /* ************************************************************************* */ @@ -55,7 +89,7 @@ TEST( Point2, expmap) { Vector d(2); d(0) = 1; d(1) = -1; - Point2 a(4, 5), b = a.retract(d), c(5, 4); + Point2 a(4, 5), b = traits::Retract(a,d), c(5, 4); EXPECT(assert_equal(b,c)); } @@ -108,6 +142,10 @@ TEST( Point2, norm ) { EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual, 1e-9); expectedH = numericalDerivative11(norm_proxy, x2); EXPECT(assert_equal(expectedH,actualH)); + + // analytical + expectedH = (Matrix(1, 2) << x2.x()/actual, x2.y()/actual).finished(); + EXPECT(assert_equal(expectedH,actualH)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 18273b182..6811ed122 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -26,22 +26,36 @@ GTSAM_CONCEPT_LIE_INST(Point3) static Point3 P(0.2, 0.7, -2); +//****************************************************************************** +TEST(Point3 , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); +} + +//****************************************************************************** +TEST(Point3 , Invariants) { + Point3 p1(1, 2, 3), p2(4, 5, 6); + EXPECT(check_group_invariants(p1, p2)); + EXPECT(check_manifold_invariants(p1, p2)); +} + /* ************************************************************************* */ TEST(Point3, Lie) { Point3 p1(1, 2, 3); Point3 p2(4, 5, 6); Matrix H1, H2; - EXPECT(assert_equal(Point3(5, 7, 9), p1.compose(p2, H1, H2))); + EXPECT(assert_equal(Point3(5, 7, 9), traits::Compose(p1, p2, H1, H2))); EXPECT(assert_equal(eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(3, 3, 3), p1.between(p2, H1, H2))); + EXPECT(assert_equal(Point3(3, 3, 3), traits::Between(p1, p2, H1, H2))); EXPECT(assert_equal(-eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(5, 7, 9), p1.retract(Vector3(4., 5., 6.)))); - EXPECT(assert_equal((Vector)Vector3(3.,3.,3.), p1.localCoordinates(p2))); + EXPECT(assert_equal(Point3(5, 7, 9), traits::Retract(p1, Vector3(4,5,6)))); + EXPECT(assert_equal(Vector3(3, 3, 3), traits::Local(p1,p2))); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index b4de6eb7c..5b835200a 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include @@ -32,11 +32,16 @@ using namespace boost::assign; using namespace gtsam; using namespace std; -// #define SLOW_BUT_CORRECT_EXPMAP - GTSAM_CONCEPT_TESTABLE_INST(Pose2) GTSAM_CONCEPT_LIE_INST(Pose2) +//****************************************************************************** +TEST(Pose2 , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + /* ************************************************************************* */ TEST(Pose2, constructors) { Point2 p; @@ -44,7 +49,7 @@ TEST(Pose2, constructors) { Pose2 origin; assert_equal(pose,origin); Pose2 t(M_PI/2.0+0.018, Point2(1.015, 2.01)); - EXPECT(assert_equal(t,Pose2(t.matrix()))); + EXPECT(assert_equal(t,Pose2((Matrix)t.matrix()))); } /* ************************************************************************* */ @@ -138,24 +143,22 @@ TEST(Pose2, expmap0d) { EXPECT(assert_equal(expected, actual, 1e-5)); } -#ifdef SLOW_BUT_CORRECT_EXPMAP /* ************************************************************************* */ // test case for screw motion in the plane -namespace screw { +namespace screwPose2 { double w=0.3; - Vector xi = (Vector(3) << 0.0, w, w); + Vector xi = (Vector(3) << 0.0, w, w).finished(); Rot2 expectedR = Rot2::fromAngle(w); Point2 expectedT(-0.0446635, 0.29552); Pose2 expected(expectedR, expectedT); } -TEST(Pose3, expmap_c) +TEST(Pose2, expmap_c) { - EXPECT(assert_equal(screw::expected, expm(screw::xi),1e-6)); - EXPECT(assert_equal(screw::expected, Pose2::Expmap(screw::xi),1e-6)); - EXPECT(assert_equal(screw::xi, Pose2::Logmap(screw::expected),1e-6)); + EXPECT(assert_equal(screwPose2::expected, expm(screwPose2::xi),1e-6)); + EXPECT(assert_equal(screwPose2::expected, Pose2::Expmap(screwPose2::xi),1e-6)); + EXPECT(assert_equal(screwPose2::xi, Pose2::Logmap(screwPose2::expected),1e-6)); } -#endif /* ************************************************************************* */ TEST(Pose2, expmap_c_full) @@ -175,9 +178,9 @@ TEST(Pose2, logmap) { Pose2 pose0(M_PI/2.0, Point2(1, 2)); Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01)); #ifdef SLOW_BUT_CORRECT_EXPMAP - Vector expected = Vector3(0.00986473, -0.0150896, 0.018); + Vector3 expected(0.00986473, -0.0150896, 0.018); #else - Vector expected = Vector3(0.01, -0.015, 0.018); + Vector3 expected(0.01, -0.015, 0.018); #endif Vector actual = pose0.localCoordinates(pose); EXPECT(assert_equal(expected, actual, 1e-5)); @@ -192,6 +195,48 @@ TEST(Pose2, logmap_full) { EXPECT(assert_equal(expected, actual, 1e-5)); } +/* ************************************************************************* */ +TEST( Pose2, ExpmapDerivative1) { + Matrix3 actualH; + Vector3 w(0.1, 0.27, -0.3); + Pose2::Expmap(w,actualH); + Matrix3 expectedH = numericalDerivative21 >(&Pose2::Expmap, w, boost::none, 1e-2); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); +} + +/* ************************************************************************* */ +TEST( Pose2, ExpmapDerivative2) { + Matrix3 actualH; + Vector3 w0(0.1, 0.27, 0.0); // alpha = 0 + Pose2::Expmap(w0,actualH); + Matrix3 expectedH = numericalDerivative21 >(&Pose2::Expmap, w0, boost::none, 1e-2); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); +} + +/* ************************************************************************* */ +TEST( Pose2, LogmapDerivative1) { + Matrix3 actualH; + Vector3 w(0.1, 0.27, -0.3); + Pose2 p = Pose2::Expmap(w); + EXPECT(assert_equal(w, Pose2::Logmap(p,actualH), 1e-5)); + Matrix3 expectedH = numericalDerivative21 >(&Pose2::Logmap, p, boost::none, 1e-2); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); +} + +/* ************************************************************************* */ +TEST( Pose2, LogmapDerivative2) { + Matrix3 actualH; + Vector3 w0(0.1, 0.27, 0.0); // alpha = 0 + Pose2 p = Pose2::Expmap(w0); + EXPECT(assert_equal(w0, Pose2::Logmap(p,actualH), 1e-5)); + Matrix3 expectedH = numericalDerivative21 >(&Pose2::Logmap, p, boost::none, 1e-2); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); +} + /* ************************************************************************* */ static Point2 transform_to_proxy(const Pose2& pose, const Point2& point) { return pose.transform_to(point); @@ -482,13 +527,6 @@ TEST( Pose2, round_trip ) EXPECT(assert_equal(odo, p1.between(p2))); } -/* ************************************************************************* */ -TEST(Pose2, members) -{ - Pose2 pose; - EXPECT(pose.dim() == 3); -} - namespace { /* ************************************************************************* */ // some shared test values @@ -519,7 +557,7 @@ TEST( Pose2, bearing ) expectedH1 = numericalDerivative21(bearing_proxy, x2, l3); EXPECT(assert_equal(expectedH1,actualH1)); expectedH2 = numericalDerivative22(bearing_proxy, x2, l3); - EXPECT(assert_equal(expectedH1,actualH1)); + EXPECT(assert_equal(expectedH2,actualH2)); // establish bearing is indeed 45 degrees even if rotated Rot2 actual34 = x3.bearing(l4, actualH1, actualH2); @@ -529,7 +567,7 @@ TEST( Pose2, bearing ) expectedH1 = numericalDerivative21(bearing_proxy, x3, l4); expectedH2 = numericalDerivative22(bearing_proxy, x3, l4); EXPECT(assert_equal(expectedH1,actualH1)); - EXPECT(assert_equal(expectedH1,actualH1)); + EXPECT(assert_equal(expectedH2,actualH2)); } /* ************************************************************************* */ @@ -729,6 +767,47 @@ TEST(Pose2, align_4) { EXPECT(assert_equal(expected, *actual)); } +//****************************************************************************** +Pose2 T1(M_PI / 4.0, Point2(sqrt(0.5), sqrt(0.5))); +Pose2 T2(M_PI / 2.0, Point2(0.0, 2.0)); + +//****************************************************************************** +TEST(Pose2 , Invariants) { + Pose2 id; + + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T1)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T1)); + + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,T1)); + EXPECT(check_manifold_invariants(T2,id)); + EXPECT(check_manifold_invariants(T2,T1)); + +} + +//****************************************************************************** +TEST(Pose2 , LieGroupDerivatives) { + Pose2 id; + + CHECK_LIE_GROUP_DERIVATIVES(id,id); + CHECK_LIE_GROUP_DERIVATIVES(id,T2); + CHECK_LIE_GROUP_DERIVATIVES(T2,id); + CHECK_LIE_GROUP_DERIVATIVES(T2,T1); + +} + +//****************************************************************************** +TEST(Pose2 , ChartDerivatives) { + Pose2 id; + + CHECK_CHART_DERIVATIVES(id,id); + CHECK_CHART_DERIVATIVES(id,T2); + CHECK_CHART_DERIVATIVES(T2,id); + CHECK_CHART_DERIVATIVES(T2,T1); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 52f721f41..98c80e356 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -16,9 +16,8 @@ #include #include +#include #include -#include -#include #include // for operator += using namespace boost::assign; @@ -57,25 +56,24 @@ TEST( Pose3, constructors) } /* ************************************************************************* */ +#ifndef GTSAM_POSE3_EXPMAP TEST( Pose3, retract_first_order) { Pose3 id; Vector v = zero(6); v(0) = 0.3; - EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v, Pose3::FIRST_ORDER),1e-2)); + EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v),1e-2)); v(3)=0.2;v(4)=0.7;v(5)=-2; - EXPECT(assert_equal(Pose3(R, P),id.retract(v, Pose3::FIRST_ORDER),1e-2)); + EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-2)); } - +#endif /* ************************************************************************* */ TEST( Pose3, retract_expmap) { - Pose3 id; - Vector v = zero(6); - v(0) = 0.3; - EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v, Pose3::EXPMAP),1e-2)); - v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; - EXPECT(assert_equal(Pose3(R, P),id.retract(v, Pose3::EXPMAP),1e-2)); + Vector v = zero(6); v(0) = 0.3; + Pose3 pose = Pose3::Expmap(v); + EXPECT(assert_equal(Pose3(R, Point3()), pose, 1e-2)); + EXPECT(assert_equal(v,Pose3::Logmap(pose),1e-2)); } /* ************************************************************************* */ @@ -111,7 +109,7 @@ TEST(Pose3, expmap_b) /* ************************************************************************* */ // test case for screw motion in the plane -namespace screw { +namespace screwPose3 { double a=0.3, c=cos(a), s=sin(a), w=0.3; Vector xi = (Vector(6) << 0.0, 0.0, w, w, 0.0, 1.0).finished(); Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1); @@ -123,24 +121,24 @@ namespace screw { // Checks correct exponential map (Expmap) with brute force matrix exponential TEST(Pose3, expmap_c_full) { - EXPECT(assert_equal(screw::expected, expm(screw::xi),1e-6)); - EXPECT(assert_equal(screw::expected, Pose3::Expmap(screw::xi),1e-6)); + EXPECT(assert_equal(screwPose3::expected, expm(screwPose3::xi),1e-6)); + EXPECT(assert_equal(screwPose3::expected, Pose3::Expmap(screwPose3::xi),1e-6)); } /* ************************************************************************* */ // assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi)) TEST(Pose3, Adjoint_full) { - Pose3 expected = T * Pose3::Expmap(screw::xi) * T.inverse(); - Vector xiprime = T.Adjoint(screw::xi); + Pose3 expected = T * Pose3::Expmap(screwPose3::xi) * T.inverse(); + Vector xiprime = T.Adjoint(screwPose3::xi); EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6)); - Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * T2.inverse(); - Vector xiprime2 = T2.Adjoint(screw::xi); + Pose3 expected2 = T2 * Pose3::Expmap(screwPose3::xi) * T2.inverse(); + Vector xiprime2 = T2.Adjoint(screwPose3::xi); EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6)); - Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * T3.inverse(); - Vector xiprime3 = T3.Adjoint(screw::xi); + Pose3 expected3 = T3 * Pose3::Expmap(screwPose3::xi) * T3.inverse(); + Vector xiprime3 = T3.Adjoint(screwPose3::xi); EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6)); } @@ -151,11 +149,11 @@ Pose3 Agrawal06iros(const Vector& xi) { Vector v = xi.tail(3); double t = norm_2(w); if (t < 1e-5) - return Pose3(Rot3(), Point3::Expmap(v)); + return Pose3(Rot3(), Point3(v)); else { Matrix W = skewSymmetric(w/t); Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W); - return Pose3(Rot3::Expmap (w), Point3::Expmap(A * v)); + return Pose3(Rot3::Expmap (w), Point3(A * v)); } } @@ -188,6 +186,17 @@ TEST(Pose3, expmaps_galore_full) EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-6)); } +/* ************************************************************************* */ +// Check translation and its pushforward +TEST(Pose3, translation) { + Matrix actualH; + EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.translation(actualH), 1e-8)); + + Matrix numericalH = numericalDerivative11( + boost::bind(&Pose3::translation, _1, boost::none), T); + EXPECT(assert_equal(numericalH, actualH, 1e-6)); +} + /* ************************************************************************* */ TEST(Pose3, Adjoint_compose_full) { @@ -463,43 +472,51 @@ TEST( Pose3, transformPose_to) } /* ************************************************************************* */ -TEST(Pose3, localCoordinates_first_order) +TEST(Pose3, Retract_LocalCoordinates) +{ + Vector6 d; + d << 1,2,3,4,5,6; d/=10; + R = Rot3::Retract(d.head<3>()); + Pose3 t = Pose3::Retract(d); + EXPECT(assert_equal(d, Pose3::LocalCoordinates(t))); +} +/* ************************************************************************* */ +TEST(Pose3, retract_localCoordinates) +{ + Vector6 d12; + d12 << 1,2,3,4,5,6; d12/=10; + Pose3 t1 = T, t2 = t1.retract(d12); + EXPECT(assert_equal(d12, t1.localCoordinates(t2))); +} +/* ************************************************************************* */ +TEST(Pose3, expmap_logmap) { Vector d12 = repeat(6,0.1); - Pose3 t1 = T, t2 = t1.retract(d12, Pose3::FIRST_ORDER); - EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::FIRST_ORDER))); + Pose3 t1 = T, t2 = t1.expmap(d12); + EXPECT(assert_equal(d12, t1.logmap(t2))); } /* ************************************************************************* */ -TEST(Pose3, localCoordinates_expmap) -{ - Vector d12 = repeat(6,0.1); - Pose3 t1 = T, t2 = t1.retract(d12, Pose3::EXPMAP); - EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::EXPMAP))); -} - -/* ************************************************************************* */ -TEST(Pose3, manifold_first_order) +TEST(Pose3, retract_localCoordinates2) { Pose3 t1 = T; Pose3 t2 = T3; Pose3 origin; - Vector d12 = t1.localCoordinates(t2, Pose3::FIRST_ORDER); - EXPECT(assert_equal(t2, t1.retract(d12, Pose3::FIRST_ORDER))); - Vector d21 = t2.localCoordinates(t1, Pose3::FIRST_ORDER); - EXPECT(assert_equal(t1, t2.retract(d21, Pose3::FIRST_ORDER))); + Vector d12 = t1.localCoordinates(t2); + EXPECT(assert_equal(t2, t1.retract(d12))); + Vector d21 = t2.localCoordinates(t1); + EXPECT(assert_equal(t1, t2.retract(d21))); } - /* ************************************************************************* */ TEST(Pose3, manifold_expmap) { Pose3 t1 = T; Pose3 t2 = T3; Pose3 origin; - Vector d12 = t1.localCoordinates(t2, Pose3::EXPMAP); - EXPECT(assert_equal(t2, t1.retract(d12, Pose3::EXPMAP))); - Vector d21 = t2.localCoordinates(t1, Pose3::EXPMAP); - EXPECT(assert_equal(t1, t2.retract(d21, Pose3::EXPMAP))); + Vector d12 = t1.logmap(t2); + EXPECT(assert_equal(t2, t1.expmap(d12))); + Vector d21 = t2.logmap(t1); + EXPECT(assert_equal(t1, t2.expmap(d21))); // Check that log(t1,t2)=-log(t2,t1) EXPECT(assert_equal(d12,-d21)); @@ -628,9 +645,9 @@ TEST( Pose3, unicycle ) /* ************************************************************************* */ TEST( Pose3, adjointMap) { - Matrix res = Pose3::adjointMap(screw::xi); - Matrix wh = skewSymmetric(screw::xi(0), screw::xi(1), screw::xi(2)); - Matrix vh = skewSymmetric(screw::xi(3), screw::xi(4), screw::xi(5)); + Matrix res = Pose3::adjointMap(screwPose3::xi); + Matrix wh = skewSymmetric(screwPose3::xi(0), screwPose3::xi(1), screwPose3::xi(2)); + Matrix vh = skewSymmetric(screwPose3::xi(3), screwPose3::xi(4), screwPose3::xi(5)); Matrix Z3 = zeros(3,3); Matrix6 expected; expected << wh, Z3, vh, wh; @@ -672,43 +689,46 @@ TEST(Pose3, align_2) { } /* ************************************************************************* */ -/// exp(xi) exp(y) = exp(xi + x) -/// Hence, y = log (exp(-xi)*exp(xi+x)) -Vector xi = (Vector(6) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0).finished(); - -Vector testDerivExpmapInv(const Vector6& dxi) { - return Pose3::Logmap(Pose3::Expmap(-xi) * Pose3::Expmap(xi + dxi)); -} - -TEST( Pose3, dExpInv_TLN) { - Matrix res = Pose3::dExpInv_exp(xi); - - Matrix numericalDerivExpmapInv = numericalDerivative11( - testDerivExpmapInv, Vector6::Zero(), 1e-5); - - EXPECT(assert_equal(numericalDerivExpmapInv,res,3e-1)); +TEST( Pose3, ExpmapDerivative1) { + Matrix6 actualH; + Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0; + Pose3::Expmap(w,actualH); + Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, boost::none); + EXPECT(assert_equal(expectedH, actualH)); } /* ************************************************************************* */ -Vector testDerivAdjoint(const Vector6& xi, const Vector6& v) { +TEST( Pose3, LogmapDerivative1) { + Matrix6 actualH; + Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0; + Pose3 p = Pose3::Expmap(w); + EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5)); + Matrix expectedH = numericalDerivative21 >(&Pose3::Logmap, p, boost::none); + EXPECT(assert_equal(expectedH, actualH)); +} + +/* ************************************************************************* */ +Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) { return Pose3::adjointMap(xi) * v; } TEST( Pose3, adjoint) { - Vector expected = testDerivAdjoint(screw::xi, screw::xi); + Vector expected = testDerivAdjoint(screwPose3::xi, screwPose3::xi); Matrix actualH; - Vector actual = Pose3::adjoint(screw::xi, screw::xi, actualH); + Vector actual = Pose3::adjoint(screwPose3::xi, screwPose3::xi, actualH); - Matrix numericalH = numericalDerivative21( - testDerivAdjoint, screw::xi, screw::xi, 1e-5); + Matrix numericalH = numericalDerivative21( + testDerivAdjoint, screwPose3::xi, screwPose3::xi, 1e-5); EXPECT(assert_equal(expected,actual,1e-5)); EXPECT(assert_equal(numericalH,actualH,1e-5)); } /* ************************************************************************* */ -Vector testDerivAdjointTranspose(const Vector6& xi, const Vector6& v) { +Vector6 testDerivAdjointTranspose(const Vector6& xi, const Vector6& v) { return Pose3::adjointMap(xi).transpose() * v; } @@ -720,7 +740,7 @@ TEST( Pose3, adjointTranspose) { Matrix actualH; Vector actual = Pose3::adjointTranspose(xi, v, actualH); - Matrix numericalH = numericalDerivative21( + Matrix numericalH = numericalDerivative21( testDerivAdjointTranspose, xi, v, 1e-5); EXPECT(assert_equal(expected,actual,1e-15)); @@ -736,6 +756,45 @@ TEST( Pose3, stream) EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n\n[0, 0, 0]';\n"); } +//****************************************************************************** +TEST(Pose3 , Invariants) { + Pose3 id; + + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T3)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T3)); + + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,T3)); + EXPECT(check_manifold_invariants(T2,id)); + EXPECT(check_manifold_invariants(T2,T3)); +} + +//****************************************************************************** +TEST(Pose3 , LieGroupDerivatives) { + Pose3 id; + + CHECK_LIE_GROUP_DERIVATIVES(id,id); + CHECK_LIE_GROUP_DERIVATIVES(id,T2); + CHECK_LIE_GROUP_DERIVATIVES(T2,id); + CHECK_LIE_GROUP_DERIVATIVES(T2,T3); +} + +//****************************************************************************** +TEST(Pose3 , ChartDerivatives) { + Pose3 id; + if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) { + CHECK_CHART_DERIVATIVES(id,id); +// CHECK_CHART_DERIVATIVES(id,T2); +// CHECK_CHART_DERIVATIVES(T2,id); +// CHECK_CHART_DERIVATIVES(T2,T3); + } +} + /* ************************************************************************* */ -int main(){ TestResult tr; return TestRegistry::runAllTests(tr);} +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp new file mode 100644 index 000000000..0421e1e44 --- /dev/null +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -0,0 +1,143 @@ +/* ---------------------------------------------------------------------------- + + * 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 testQuaternion.cpp + * @brief Unit tests for Quaternion, as a GTSAM-adapted Lie Group + * @author Frank Dellaert + **/ + +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +typedef Quaternion Q; // Typedef +typedef traits::ChartJacobian QuaternionJacobian; + +//****************************************************************************** +TEST(Quaternion , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +//****************************************************************************** +TEST(Quaternion , Constructor) { + Q q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); +} + +//****************************************************************************** +TEST(Quaternion , Logmap) { + Q q1(5e-06, 0, 0, 1), q2(-5e-06, 0, 0, -1); + Vector3 v1 = traits::Logmap(q1); + Vector3 v2 = traits::Logmap(q2); + EXPECT(assert_equal(v1, v2)); +} + +//****************************************************************************** +TEST(Quaternion , Local) { + Vector3 z_axis(0, 0, 1); + Q q1(Eigen::AngleAxisd(0, z_axis)); + Q q2(Eigen::AngleAxisd(0.1, z_axis)); + QuaternionJacobian H1, H2; + Vector3 expected(0, 0, 0.1); + Vector3 actual = traits::Local(q1, q2, H1, H2); + EXPECT(assert_equal((Vector )expected, actual)); +} + +//****************************************************************************** +TEST(Quaternion , Retract) { + Vector3 z_axis(0, 0, 1); + Q q(Eigen::AngleAxisd(0, z_axis)); + Q expected(Eigen::AngleAxisd(0.1, z_axis)); + Vector3 v(0, 0, 0.1); + QuaternionJacobian Hq, Hv; + Q actual = traits::Retract(q, v, Hq, Hv); + EXPECT(actual.isApprox(expected)); +} + +//****************************************************************************** +TEST(Quaternion , Compose) { + Vector3 z_axis(0, 0, 1); + Q q1(Eigen::AngleAxisd(0.2, z_axis)); + Q q2(Eigen::AngleAxisd(0.1, z_axis)); + + Q expected = q1 * q2; + Q actual = traits::Compose(q1, q2); + EXPECT(traits::Equals(expected, actual)); +} + +//****************************************************************************** +Vector3 z_axis(0, 0, 1); +Q id(Eigen::AngleAxisd(0, z_axis)); +Q R1(Eigen::AngleAxisd(1, z_axis)); +Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); + +//****************************************************************************** +TEST(Quaternion , Between) { + Q q1(Eigen::AngleAxisd(0.2, z_axis)); + Q q2(Eigen::AngleAxisd(0.1, z_axis)); + + Q expected = q1.inverse() * q2; + Q actual = traits::Between(q1, q2); + EXPECT(traits::Equals(expected, actual)); +} + +//****************************************************************************** +TEST(Quaternion , Inverse) { + Q q1(Eigen::AngleAxisd(0.1, z_axis)); + Q expected(Eigen::AngleAxisd(-0.1, z_axis)); + + Q actual = traits::Inverse(q1); + EXPECT(traits::Equals(expected, actual)); +} + +//****************************************************************************** +TEST(Quaternion , Invariants) { + EXPECT(check_group_invariants(id, id)); + EXPECT(check_group_invariants(id, R1)); + EXPECT(check_group_invariants(R2, id)); + EXPECT(check_group_invariants(R2, R1)); + + EXPECT(check_manifold_invariants(id, id)); + EXPECT(check_manifold_invariants(id, R1)); + EXPECT(check_manifold_invariants(R2, id)); + EXPECT(check_manifold_invariants(R2, R1)); +} + +//****************************************************************************** +TEST(Quaternion , LieGroupDerivatives) { + CHECK_LIE_GROUP_DERIVATIVES(id, id); + CHECK_LIE_GROUP_DERIVATIVES(id, R2); + CHECK_LIE_GROUP_DERIVATIVES(R2, id); + CHECK_LIE_GROUP_DERIVATIVES(R2, R1); +} + +//****************************************************************************** +TEST(Quaternion , ChartDerivatives) { + CHECK_CHART_DERIVATIVES(id, id); + CHECK_CHART_DERIVATIVES(id, R2); + CHECK_CHART_DERIVATIVES(R2, id); + CHECK_CHART_DERIVATIVES(R2, R1); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 8a1f942d2..6ead22860 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -15,10 +15,10 @@ * @author Frank Dellaert */ -#include -#include -#include #include +#include +#include +#include using namespace gtsam; @@ -49,7 +49,9 @@ TEST( Rot2, unit) /* ************************************************************************* */ TEST( Rot2, transpose) { - CHECK(assert_equal(R.inverse().matrix(),R.transpose())); + Matrix expected = R.inverse().matrix(); + Matrix actual = R.transpose(); + CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */ @@ -153,6 +155,47 @@ TEST( Rot2, relativeBearing ) CHECK(assert_equal(expectedH,actualH)); } +//****************************************************************************** +Rot2 T1(0.1); +Rot2 T2(0.2); + +//****************************************************************************** +TEST(Rot2 , Invariants) { + Rot2 id; + + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T1)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T1)); + + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,T1)); + EXPECT(check_manifold_invariants(T2,id)); + EXPECT(check_manifold_invariants(T2,T1)); + +} + +//****************************************************************************** +TEST(Rot2 , LieGroupDerivatives) { + Rot2 id; + + CHECK_LIE_GROUP_DERIVATIVES(id,id); + CHECK_LIE_GROUP_DERIVATIVES(id,T2); + CHECK_LIE_GROUP_DERIVATIVES(T2,id); + CHECK_LIE_GROUP_DERIVATIVES(T2,T1); + +} + +//****************************************************************************** +TEST(Rot2 , ChartDerivatives) { + Rot2 id; + + CHECK_CHART_DERIVATIVES(id,id); + CHECK_CHART_DERIVATIVES(id,T2); + CHECK_CHART_DERIVATIVES(T2,id); + CHECK_CHART_DERIVATIVES(T2,T1); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 358524488..349f87029 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -18,7 +18,7 @@ #include #include - +#include #include #include #include @@ -36,12 +36,25 @@ GTSAM_CONCEPT_LIE_INST(Rot3) static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); static Point3 P(0.2, 0.7, -2.0); static double error = 1e-9, epsilon = 0.001; -static const Matrix I3 = eye(3); + +//****************************************************************************** +TEST(Rot3 , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +/* ************************************************************************* */ +TEST( Rot3, chart) +{ + Matrix R = (Matrix(3, 3) << 0, 1, 0, 1, 0, 0, 0, 0, -1).finished(); + Rot3 rot3(R); +} /* ************************************************************************* */ TEST( Rot3, constructor) { - Rot3 expected(I3); + Rot3 expected((Matrix)I_3x3); Point3 r1(1,0,0), r2(0,1,0), r3(0,0,1); Rot3 actual(r1, r2, r3); CHECK(assert_equal(actual,expected)); @@ -86,7 +99,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) { double t = norm_2(w); Matrix J = skewSymmetric(w / t); if (t < 1e-5) return Rot3(); - Matrix R = I3 + sin(t) * J + (1.0 - cos(t)) * (J * J); + Matrix R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J); return R; } @@ -140,10 +153,10 @@ TEST( Rot3, retract) Vector v = zero(3); CHECK(assert_equal(R, R.retract(v))); - // test Canonical coordinates - Canonical chart; - Vector v2 = chart.local(R); - CHECK(assert_equal(R, chart.retract(v2))); +// // test Canonical coordinates +// Canonical chart; +// Vector v2 = chart.local(R); +// CHECK(assert_equal(R, chart.retract(v2))); } /* ************************************************************************* */ @@ -187,7 +200,7 @@ TEST(Rot3, log) // Windows and Linux have flipped sign in quaternion mode #if !defined(__APPLE__) && defined (GTSAM_USE_QUATERNIONS) - w = (Vector(3) << x*PI, y*PI, z*PI); + w = (Vector(3) << x*PI, y*PI, z*PI).finished(); R = Rot3::rodriguez(w); EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R),1e-12)); #else @@ -206,33 +219,94 @@ TEST(Rot3, log) CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI) } -Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){ - return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) ); +/* ************************************************************************* */ +TEST(Rot3, retract_localCoordinates) +{ + Vector3 d12 = repeat(3,0.1); + Rot3 R2 = R.retract(d12); + EXPECT(assert_equal(d12, R.localCoordinates(R2))); +} +/* ************************************************************************* */ +TEST(Rot3, expmap_logmap) +{ + Vector3 d12 = repeat(3,0.1); + Rot3 R2 = R.expmap(d12); + EXPECT(assert_equal(d12, R.logmap(R2))); } /* ************************************************************************* */ -TEST( Rot3, rightJacobianExpMapSO3 ) +TEST(Rot3, retract_localCoordinates2) { - // Linearization point - Vector3 thetahat; thetahat << 0.1, 0, 0; + Rot3 t1 = R, t2 = R*R, origin; + Vector d12 = t1.localCoordinates(t2); + EXPECT(assert_equal(t2, t1.retract(d12))); + Vector d21 = t2.localCoordinates(t1); + EXPECT(assert_equal(t1, t2.retract(d21))); +} +/* ************************************************************************* */ +Vector w = Vector3(0.1, 0.27, -0.2); - Matrix expectedJacobian = numericalDerivative11( - boost::bind(&Rot3::Expmap, _1), thetahat); - Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat); - CHECK(assert_equal(expectedJacobian, actualJacobian)); +// Left trivialization Derivative of exp(w) wrpt w: +// How does exp(w) change when w changes? +// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 +// => y = log (exp(-w) * exp(w+dw)) +Vector3 testDexpL(const Vector3& dw) { + return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw)); +} + +TEST( Rot3, ExpmapDerivative) { + Matrix actualDexpL = Rot3::ExpmapDerivative(w); + Matrix expectedDexpL = numericalDerivative11(testDexpL, + Vector3::Zero(), 1e-2); + EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7)); + + Matrix actualDexpInvL = Rot3::LogmapDerivative(w); + EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7)); } /* ************************************************************************* */ -TEST( Rot3, rightJacobianExpMapSO3inverse ) +Vector3 thetahat(0.1, 0, 0.1); +TEST( Rot3, ExpmapDerivative2) { - // Linearization point - Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias - Vector3 deltatheta; deltatheta << 0, 0, 0; + Matrix Jexpected = numericalDerivative11( + boost::bind(&Rot3::Expmap, _1, boost::none), thetahat); - Matrix expectedJacobian = numericalDerivative11( - boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta); - Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat); - EXPECT(assert_equal(expectedJacobian, actualJacobian)); + Matrix Jactual = Rot3::ExpmapDerivative(thetahat); + CHECK(assert_equal(Jexpected, Jactual)); + + Matrix Jactual2 = Rot3::ExpmapDerivative(thetahat); + CHECK(assert_equal(Jexpected, Jactual2)); +} + +/* ************************************************************************* */ +TEST( Rot3, jacobianExpmap ) +{ + Matrix Jexpected = numericalDerivative11(boost::bind( + &Rot3::Expmap, _1, boost::none), thetahat); + Matrix3 Jactual; + const Rot3 R = Rot3::Expmap(thetahat, Jactual); + EXPECT(assert_equal(Jexpected, Jactual)); +} + +/* ************************************************************************* */ +TEST( Rot3, LogmapDerivative ) +{ + Rot3 R = Rot3::Expmap(thetahat); // some rotation + Matrix Jexpected = numericalDerivative11(boost::bind( + &Rot3::Logmap, _1, boost::none), R); + Matrix3 Jactual = Rot3::LogmapDerivative(thetahat); + EXPECT(assert_equal(Jexpected, Jactual)); +} + +/* ************************************************************************* */ +TEST( Rot3, jacobianLogmap ) +{ + Rot3 R = Rot3::Expmap(thetahat); // some rotation + Matrix Jexpected = numericalDerivative11(boost::bind( + &Rot3::Logmap, _1, boost::none), R); + Matrix3 Jactual; + Rot3::Logmap(R, Jactual); + EXPECT(assert_equal(Jexpected, Jactual)); } /* ************************************************************************* */ @@ -243,12 +317,10 @@ TEST(Rot3, manifold_expmap) Rot3 origin; // log behaves correctly - Vector d12 = gR1.localCoordinates(gR2, Rot3::EXPMAP); - CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::EXPMAP))); - Vector d21 = gR2.localCoordinates(gR1, Rot3::EXPMAP); - CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::EXPMAP))); + Vector d12 = Rot3::Logmap(gR1.between(gR2)); + Vector d21 = Rot3::Logmap(gR2.between(gR1)); - // Check that it is expmap + // Check expmap CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12))); CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21))); @@ -350,7 +422,7 @@ TEST( Rot3, inverse ) Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Rot3 I; - Matrix actualH; + Matrix3 actualH; Rot3 actual = R.inverse(actualH); CHECK(assert_equal(I,R*actual)); CHECK(assert_equal(I,actual*R)); @@ -392,27 +464,6 @@ TEST( Rot3, between ) CHECK(assert_equal(numericalH2,actualH2)); } -/* ************************************************************************* */ -Vector w = Vector3(0.1, 0.27, -0.2); - -// Left trivialization Derivative of exp(w) wrpt w: -// How does exp(w) change when w changes? -// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 -// => y = log (exp(-w) * exp(w+dw)) -Vector3 testDexpL(const Vector3& dw) { - return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw)); -} - -TEST( Rot3, dexpL) { - Matrix actualDexpL = Rot3::dexpL(w); - Matrix expectedDexpL = numericalDerivative11(testDexpL, - Vector3::Zero(), 1e-2); - EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7)); - - Matrix actualDexpInvL = Rot3::dexpInvL(w); - EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7)); -} - /* ************************************************************************* */ TEST( Rot3, xyz ) { @@ -473,7 +524,7 @@ TEST( Rot3, RQ) Vector actual; boost::tie(actualK, actual) = RQ(R.matrix()); Vector expected = Vector3(0.14715, 0.385821, 0.231671); - CHECK(assert_equal(I3,actualK)); + CHECK(assert_equal(I_3x3,actualK)); CHECK(assert_equal(expected,actual,1e-6)); // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] @@ -507,7 +558,7 @@ TEST( Rot3, expmapStability ) { w(2), 0.0, -w(0), -w(1), w(0), 0.0 ).finished(); Matrix W2 = W*W; - Matrix Rmat = I3 + (1.0-theta2/6.0 + theta2*theta2/120.0 + Matrix Rmat = I_3x3 + (1.0-theta2/6.0 + theta2*theta2/120.0 - theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ; Rot3 expectedR( Rmat ); CHECK(assert_equal(expectedR, actualR, 1e-10)); @@ -566,10 +617,16 @@ TEST(Rot3, quaternion) { } /* ************************************************************************* */ +Matrix Cayley(const Matrix& A) { + Matrix::Index n = A.cols(); + const Matrix I = eye(n); + return (I-A)*inverse(I+A); +} + TEST( Rot3, Cayley ) { Matrix A = skewSymmetric(1,2,-3); Matrix Q = Cayley(A); - EXPECT(assert_equal(I3, trans(Q)*Q)); + EXPECT(assert_equal((Matrix)I_3x3, trans(Q)*Q)); EXPECT(assert_equal(A, Cayley(Q))); } @@ -594,6 +651,50 @@ TEST( Rot3, slerp) EXPECT(assert_equal(R1, R1.slerp(0.5,R1))); } +//****************************************************************************** +Rot3 T1(Rot3::rodriguez(Vector3(0, 0, 1), 1)); +Rot3 T2(Rot3::rodriguez(Vector3(0, 1, 0), 2)); + +//****************************************************************************** +TEST(Rot3 , Invariants) { + Rot3 id; + + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T1)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T1)); + EXPECT(check_group_invariants(T1,T2)); + + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,T1)); + EXPECT(check_manifold_invariants(T2,id)); + EXPECT(check_manifold_invariants(T2,T1)); + EXPECT(check_manifold_invariants(T1,T2)); +} + +//****************************************************************************** +TEST(Rot3 , LieGroupDerivatives) { + Rot3 id; + + CHECK_LIE_GROUP_DERIVATIVES(id,id); + CHECK_LIE_GROUP_DERIVATIVES(id,T2); + CHECK_LIE_GROUP_DERIVATIVES(T2,id); + CHECK_LIE_GROUP_DERIVATIVES(T1,T2); + CHECK_LIE_GROUP_DERIVATIVES(T2,T1); +} + +//****************************************************************************** +TEST(Rot3 , ChartDerivatives) { + Rot3 id; + if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) { + CHECK_CHART_DERIVATIVES(id,id); + CHECK_CHART_DERIVATIVES(id,T2); + CHECK_CHART_DERIVATIVES(T2,id); + CHECK_CHART_DERIVATIVES(T1,T2); + CHECK_CHART_DERIVATIVES(T2,T1); + } +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index 1313a3be5..12fb94bbc 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -10,21 +10,14 @@ * -------------------------------------------------------------------------- */ /** - * @file testRot3.cpp - * @brief Unit tests for Rot3 class + * @file testRot3M.cpp + * @brief Unit tests for Rot3 class, matrix version * @author Alireza Fathi * @author Frank Dellaert */ -#include #include - #include -#include -#include - -#include - #include #ifndef GTSAM_USE_QUATERNIONS @@ -37,7 +30,6 @@ GTSAM_CONCEPT_LIE_INST(Rot3) static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); static Point3 P(0.2, 0.7, -2.0); -static const Matrix I3 = eye(3); /* ************************************************************************* */ TEST(Rot3, manifold_cayley) @@ -47,38 +39,10 @@ TEST(Rot3, manifold_cayley) Rot3 origin; // log behaves correctly - Vector d12 = gR1.localCoordinates(gR2, Rot3::CAYLEY); - CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::CAYLEY))); - Vector d21 = gR2.localCoordinates(gR1, Rot3::CAYLEY); - CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::CAYLEY))); - - // Check that log(t1,t2)=-log(t2,t1) - CHECK(assert_equal(d12,-d21)); - - // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = Vector3(0.1, 0.2, 0.3); - // exp(-d)=inverse(exp(d)) - CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); - // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) - Rot3 R2 = Rot3::Expmap (2 * d); - Rot3 R3 = Rot3::Expmap (3 * d); - Rot3 R5 = Rot3::Expmap (5 * d); - CHECK(assert_equal(R5,R2*R3)); - CHECK(assert_equal(R5,R3*R2)); -} - -/* ************************************************************************* */ -TEST(Rot3, manifold_slow_cayley) -{ - Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); - Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); - Rot3 origin; - - // log behaves correctly - Vector d12 = gR1.localCoordinates(gR2, Rot3::SLOW_CAYLEY); - CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::SLOW_CAYLEY))); - Vector d21 = gR2.localCoordinates(gR1, Rot3::SLOW_CAYLEY); - CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::SLOW_CAYLEY))); + Vector d12 = gR1.localCayley(gR2); + CHECK(assert_equal(gR2, gR1.retractCayley(d12))); + Vector d21 = gR2.localCayley(gR1); + CHECK(assert_equal(gR1, gR2.retractCayley(d21))); // Check that log(t1,t2)=-log(t2,t1) CHECK(assert_equal(d12,-d21)); diff --git a/gtsam/geometry/tests/testRot3Q.cpp b/gtsam/geometry/tests/testRot3Q.cpp index 65ca9e067..9ae8eef13 100644 --- a/gtsam/geometry/tests/testRot3Q.cpp +++ b/gtsam/geometry/tests/testRot3Q.cpp @@ -15,20 +15,82 @@ * @author Alireza Fathi */ -#include #include +#include #include #include -#include - -#include #include +using namespace std; +using namespace gtsam; + #ifdef GTSAM_USE_QUATERNIONS -// No quaternion only tests +//****************************************************************************** +TEST(Rot3Q , Compare) { + using boost::none; + + // We set up expected values with quaternions + typedef Quaternion Q; + typedef traits TQ; + typedef TQ::ChartJacobian OJ; + + // We check Rot3 expressions + typedef Rot3 R; + typedef traits TR; + + // Define test values + Q q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); + Q q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); + R R1(q1), R2(q2); + + // Check Compose + Q q3 = TQ::Compose(q1, q2, none, none); + R R3 = TR::Compose(R1, R2, none, none); + EXPECT(assert_equal(R(q3), R3)); + + // Check Retract + Vector3 v(1e-5, 0, 0); + Q q4 = TQ::Retract(q3, v); + R R4 = TR::Retract(R3, v); + EXPECT(assert_equal(R(q4), R4)); + + // Check Between + Q q5 = TQ::Between(q3, q4); + R R5 = R3.between(R4); + EXPECT(assert_equal(R(q5), R5)); + + // Check toQuaternion + EXPECT(assert_equal(q5, R5.toQuaternion())); + + // Check Logmap + Vector3 vQ = TQ::Logmap(q5); + Vector3 vR = R::Logmap(R5); + EXPECT(assert_equal(vQ, vR)); + + // Check Local + vQ = TQ::Local(q3, q4); + vR = TR::Local(R3, R4); + EXPECT(assert_equal(vQ, vR)); + + // Check Retract/Local of Compose + Vector3 vQ1 = TQ::Local(q3, TQ::Compose(q1, TQ::Retract(q2, v))); + Vector3 vR1 = TR::Local(R3, TR::Compose(R1, TR::Retract(R2, v))); + EXPECT(assert_equal(vQ1, vR1)); + Vector3 vQ2 = TQ::Local(q3, TQ::Compose(q1, TQ::Retract(q2, -v))); + Vector3 vR2 = TR::Local(R3, TR::Compose(R1, TR::Retract(R2, -v))); + EXPECT(assert_equal(vQ2, vR2)); + EXPECT(assert_equal((vQ1 - vQ2) / 0.2, (vR1 - vR2) / 0.2)); + + // Check Compose Derivatives + Matrix HQ, HR; + HQ = numericalDerivative42(TQ::Compose, q1, q2, none, none); + HR = numericalDerivative42(TR::Compose, R1, R2, none, none); + EXPECT(assert_equal(HQ, HR)); + +} #endif diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp new file mode 100644 index 000000000..bc32e0df0 --- /dev/null +++ b/gtsam/geometry/tests/testSO3.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 + + * -------------------------------------------------------------------------- */ + +/** + * @file testQuaternion.cpp + * @brief Unit tests for SO3, as a GTSAM-adapted Lie Group + * @author Frank Dellaert + **/ + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +//****************************************************************************** +TEST(SO3 , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +//****************************************************************************** +TEST(SO3 , Constructor) { + SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); +} + +//****************************************************************************** +SO3 id; +Vector3 z_axis(0, 0, 1); +SO3 R1(Eigen::AngleAxisd(0.1, z_axis)); +SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); + +//****************************************************************************** +TEST(SO3 , Local) { + Vector3 expected(0, 0, 0.1); + Vector3 actual = traits::Local(R1, R2); + EXPECT(assert_equal((Vector)expected,actual)); +} + +//****************************************************************************** +TEST(SO3 , Retract) { + Vector3 v(0, 0, 0.1); + SO3 actual = traits::Retract(R1, v); + EXPECT(actual.isApprox(R2)); +} + +//****************************************************************************** +TEST(SO3 , Invariants) { + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,R1)); + EXPECT(check_group_invariants(R2,id)); + EXPECT(check_group_invariants(R2,R1)); + + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,R1)); + EXPECT(check_manifold_invariants(R2,id)); + EXPECT(check_manifold_invariants(R2,R1)); +} + +//****************************************************************************** +TEST(SO3 , LieGroupDerivatives) { + CHECK_LIE_GROUP_DERIVATIVES(id,id); + CHECK_LIE_GROUP_DERIVATIVES(id,R2); + CHECK_LIE_GROUP_DERIVATIVES(R2,id); + CHECK_LIE_GROUP_DERIVATIVES(R2,R1); +} + +//****************************************************************************** +TEST(SO3 , ChartDerivatives) { + CHECK_CHART_DERIVATIVES(id,id); + CHECK_CHART_DERIVATIVES(id,R2); + CHECK_CHART_DERIVATIVES(R2,id); + CHECK_CHART_DERIVATIVES(R2,R1); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + diff --git a/gtsam/geometry/tests/testSerializationGeometry.cpp b/gtsam/geometry/tests/testSerializationGeometry.cpp index a7e792cb8..dfef0a9c5 100644 --- a/gtsam/geometry/tests/testSerializationGeometry.cpp +++ b/gtsam/geometry/tests/testSerializationGeometry.cpp @@ -16,9 +16,9 @@ * @date Feb 7, 2012 */ -#include -#include #include +#include +#include #include #include #include @@ -41,6 +41,9 @@ static Point3 pt3(1.0, 2.0, 3.0); static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); static Pose3 pose3(rt3, pt3); +static Unit3 unit3(1.0, 2.1, 3.4); +static EssentialMatrix ematrix(rt3, unit3); + static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); static Cal3Bundler cal3(1.0, 2.0, 3.0); @@ -54,11 +57,14 @@ static StereoCamera cam2(pose3, cal4ptr); static StereoPoint2 spt(1.0, 2.0, 3.0); /* ************************************************************************* */ -TEST (Serialization, text_geometry) { +TEST_DISABLED (Serialization, text_geometry) { EXPECT(equalsObj(Point2(1.0, 2.0))); EXPECT(equalsObj(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsObj(Rot2::fromDegrees(30.0))); + EXPECT(equalsObj(Unit3(1.0, 2.1, 3.4))); + EXPECT(equalsObj(EssentialMatrix(rt3, unit3))); + EXPECT(equalsObj(pt3)); EXPECT(equalsObj(rt3)); EXPECT(equalsObj(Pose3(rt3, pt3))); @@ -76,11 +82,14 @@ TEST (Serialization, text_geometry) { } /* ************************************************************************* */ -TEST (Serialization, xml_geometry) { +TEST_DISABLED (Serialization, xml_geometry) { EXPECT(equalsXML(Point2(1.0, 2.0))); EXPECT(equalsXML(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsXML(Rot2::fromDegrees(30.0))); + EXPECT(equalsXML(Unit3(1.0, 2.1, 3.4))); + EXPECT(equalsXML(EssentialMatrix(rt3, unit3))); + EXPECT(equalsXML(pt3)); EXPECT(equalsXML(rt3)); EXPECT(equalsXML(Pose3(rt3, pt3))); @@ -97,11 +106,14 @@ TEST (Serialization, xml_geometry) { } /* ************************************************************************* */ -TEST (Serialization, binary_geometry) { +TEST_DISABLED (Serialization, binary_geometry) { EXPECT(equalsBinary(Point2(1.0, 2.0))); EXPECT(equalsBinary(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsBinary(Rot2::fromDegrees(30.0))); + EXPECT(equalsBinary(Unit3(1.0, 2.1, 3.4))); + EXPECT(equalsBinary(EssentialMatrix(rt3, unit3))); + EXPECT(equalsBinary(pt3)); EXPECT(equalsBinary(rt3)); EXPECT(equalsBinary(Pose3(rt3, pt3))); diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 002cbe51b..71979481c 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -15,13 +15,13 @@ * @brief test SimpleCamera class */ -#include -#include - -#include +#include +#include #include #include -#include +#include +#include +#include using namespace std; using namespace gtsam; diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index eebe2d60a..45f26c244 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -74,7 +74,7 @@ TEST( StereoCamera, project) /* ************************************************************************* */ -static Pose3 camera1((Matrix)(Matrix(3,3) << +static Pose3 camPose((Matrix)(Matrix(3,3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. @@ -82,33 +82,37 @@ static Pose3 camera1((Matrix)(Matrix(3,3) << Point3(0,0,6.25)); static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); -static StereoCamera stereoCam(Pose3(), K); +static StereoCamera stereoCam(camPose, K); // point X Y Z in meters -static Point3 p(0, 0, 5); +static Point3 landmark(0, 0, 5); /* ************************************************************************* */ -static StereoPoint2 project_(const StereoCamera& cam, const Point3& point) { return cam.project(point); } -TEST( StereoCamera, Dproject_stereo_pose) -{ - Matrix expected = numericalDerivative21(project_,stereoCam, p); - Matrix actual; stereoCam.project(p, actual, boost::none); - CHECK(assert_equal(expected,actual,1e-7)); +static StereoPoint2 project3(const Pose3& pose, const Point3& point, const Cal3_S2Stereo& K) { + return StereoCamera(pose, boost::make_shared(K)).project(point); } /* ************************************************************************* */ -TEST( StereoCamera, Dproject_stereo_point) +TEST( StereoCamera, Dproject) { - Matrix expected = numericalDerivative22(project_,stereoCam, p); - Matrix actual; stereoCam.project(p, boost::none, actual); - CHECK(assert_equal(expected,actual,1e-8)); + Matrix expected1 = numericalDerivative31(project3, camPose, landmark, *K); + Matrix actual1; stereoCam.project2(landmark, actual1, boost::none); + CHECK(assert_equal(expected1,actual1,1e-7)); + + Matrix expected2 = numericalDerivative32(project3,camPose, landmark, *K); + Matrix actual2; stereoCam.project2(landmark, boost::none, actual2); + CHECK(assert_equal(expected2,actual2,1e-7)); } +/* ************************************************************************* */ TEST( StereoCamera, backproject) { + Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); + StereoCamera stereoCam2(Pose3(), K2); + Point3 expected(1.2, 2.3, 4.5); - StereoPoint2 stereo_point = stereoCam.project(expected); - Point3 actual = stereoCam.backproject(stereo_point); + StereoPoint2 stereo_point = stereoCam2.project(expected); + Point3 actual = stereoCam2.backproject(stereo_point); CHECK(assert_equal(expected,actual,1e-8)); } diff --git a/gtsam/geometry/tests/testStereoPoint2.cpp b/gtsam/geometry/tests/testStereoPoint2.cpp index 8e504ba0e..ddcc9238a 100644 --- a/gtsam/geometry/tests/testStereoPoint2.cpp +++ b/gtsam/geometry/tests/testStereoPoint2.cpp @@ -26,9 +26,16 @@ using namespace std; using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2) -GTSAM_CONCEPT_LIE_INST(StereoPoint2) +//GTSAM_CONCEPT_LIE_INST(StereoPoint2) +//****************************************************************************** +TEST(StereoPoint2 , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); +} + /* ************************************************************************* */ TEST(StereoPoint2, constructor) { StereoPoint2 p1(1, 2, 3), p2 = p1; @@ -49,7 +56,7 @@ TEST(StereoPoint2, Lie) { } /* ************************************************************************* */ -TEST( StereoPoint2, expmap) { +TEST( StereoPoint2, retract) { Vector d(3); d(0) = 1; d(1) = -1; diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 0bd553a40..f986cca1d 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -262,25 +262,6 @@ TEST( triangulation, twoIdenticalPoses) { } */ -//****************************************************************************** -TEST( triangulation, TriangulationFactor ) { - // Create the factor with a measurement that is 3 pixels off in x - Key pointKey(1); - SharedNoiseModel model; - typedef TriangulationFactor<> Factor; - Factor factor(camera1, z1, model, pointKey); - - // Use the factor to calculate the Jacobians - Matrix HActual; - factor.evaluateError(landmark, HActual); - - Matrix HExpected = numericalDerivative11( - boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); - - // Verify the Jacobians are correct - CHECK(assert_equal(HExpected, HActual, 1e-3)); -} - //****************************************************************************** int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 0102477b3..1d0c28ded 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -108,20 +108,20 @@ TEST(Unit3, unrotate) { TEST(Unit3, error) { Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), // r = p.retract(Vector2(0.8, 0)); - EXPECT(assert_equal(Vector2(0, 0), p.error(p), 1e-8)); - EXPECT(assert_equal(Vector2(0.479426, 0), p.error(q), 1e-5)); - EXPECT(assert_equal(Vector2(0.717356, 0), p.error(r), 1e-5)); + EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.error(p), 1e-8)); + EXPECT(assert_equal((Vector)(Vector2(0.479426, 0)), p.error(q), 1e-5)); + EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5)); Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&Unit3::error, &p, _1, boost::none), q); p.error(q, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); } { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&Unit3::error, &p, _1, boost::none), r); p.error(r, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 9e1575801..c0f69217c 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -23,14 +23,8 @@ namespace gtsam { -/** - * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 - * @param projection_matrices Projection matrices (K*P^-1) - * @param measurements 2D measurements - * @param rank_tol SVD rank tolerance - * @return Triangulated Point3 - */ -Point3 triangulateDLT(const std::vector& projection_matrices, +Vector4 triangulateHomogeneousDLT( + const std::vector& projection_matrices, const std::vector& measurements, double rank_tol) { // number of cameras @@ -41,7 +35,7 @@ Point3 triangulateDLT(const std::vector& projection_matrices, for (size_t i = 0; i < m; i++) { size_t row = i * 2; - const Matrix& projection = projection_matrices.at(i); + const Matrix34& projection = projection_matrices.at(i); const Point2& p = measurements.at(i); // build system of equations @@ -57,7 +51,16 @@ Point3 triangulateDLT(const std::vector& projection_matrices, if (rank < 3) throw(TriangulationUnderconstrainedException()); - // Create 3D point from eigenvector + return v; +} + +Point3 triangulateDLT(const std::vector& projection_matrices, + const std::vector& measurements, double rank_tol) { + + Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, + rank_tol); + + // Create 3D point from homogeneous coordinates return Point3(sub((v / v(3)), 0, 3)); } @@ -89,6 +92,5 @@ Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, return result.at(landmarkKey); } - } // \namespace gtsam diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index fabcc4c02..f4f40ccba 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -18,13 +18,10 @@ #pragma once - -#include +#include +#include #include #include -#include - -#include namespace gtsam { @@ -45,6 +42,17 @@ public: } }; +/** + * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 + * @param projection_matrices Projection matrices (K*P^-1) + * @param measurements 2D measurements + * @param rank_tol SVD rank tolerance + * @return Triangulated point, in homogeneous coordinates + */ +GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( + const std::vector& projection_matrices, + const std::vector& measurements, double rank_tol = 1e-9); + /** * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 * @param projection_matrices Projection matrices (K*P^-1) @@ -53,8 +61,8 @@ public: * @return Triangulated Point3 */ GTSAM_EXPORT Point3 triangulateDLT( - const std::vector& projection_matrices, - const std::vector& measurements, double rank_tol); + const std::vector& projection_matrices, + const std::vector& measurements, double rank_tol = 1e-9); /// /** @@ -94,9 +102,9 @@ std::pair triangulationGraph( * @param initialEstimate * @return graph and initial values */ -template +template std::pair triangulationGraph( - const std::vector >& cameras, + const std::vector& cameras, const std::vector& measurements, Key landmarkKey, const Point3& initialEstimate) { Values values; @@ -105,8 +113,8 @@ std::pair triangulationGraph( static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); for (size_t i = 0; i < measurements.size(); i++) { - const PinholeCamera& camera_i = cameras[i]; - graph.push_back(TriangulationFactor // + const CAMERA& camera_i = cameras[i]; + graph.push_back(TriangulationFactor // (camera_i, measurements[i], unit2, landmarkKey)); } return std::make_pair(graph, values); @@ -152,9 +160,9 @@ Point3 triangulateNonlinear(const std::vector& poses, * @param initialEstimate * @return refined Point3 */ -template +template Point3 triangulateNonlinear( - const std::vector >& cameras, + const std::vector& cameras, const std::vector& measurements, const Point3& initialEstimate) { // Create a factor graph and initial values @@ -166,6 +174,25 @@ Point3 triangulateNonlinear( return optimize(graph, values, Symbol('p', 0)); } +/** + * Create a 3*4 camera projection matrix from calibration and pose. + * Functor for partial application on calibration + * @param pose The camera pose + * @param cal The calibration + * @return Returns a Matrix34 + */ +template +struct CameraProjectionMatrix { + CameraProjectionMatrix(const CALIBRATION& calibration) : + K_(calibration.K()) { + } + Matrix34 operator()(const Pose3& pose) const { + return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0); + } +private: + const Matrix3 K_; +}; + /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. The function checks that the @@ -189,11 +216,10 @@ Point3 triangulatePoint3(const std::vector& poses, throw(TriangulationUnderconstrainedException()); // construct projection matrices from poses & calibration - std::vector projection_matrices; - BOOST_FOREACH(const Pose3& pose, poses) { - projection_matrices.push_back( - sharedCal->K() * sub(pose.inverse().matrix(), 0, 3, 0, 4)); - } + std::vector projection_matrices; + CameraProjectionMatrix createP(*sharedCal); // partially apply + BOOST_FOREACH(const Pose3& pose, poses) + projection_matrices.push_back(createP(pose)); // Triangulate linearly Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); @@ -207,7 +233,7 @@ Point3 triangulatePoint3(const std::vector& poses, BOOST_FOREACH(const Pose3& pose, poses) { const Point3& p_local = pose.transform_to(point); if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + throw(TriangulationCheiralityException()); } #endif @@ -226,26 +252,25 @@ Point3 triangulatePoint3(const std::vector& poses, * @param optimize Flag to turn on nonlinear refinement of triangulation * @return Returns a Point3 */ -template +template Point3 triangulatePoint3( - const std::vector >& cameras, + const std::vector& cameras, const std::vector& measurements, double rank_tol = 1e-9, bool optimize = false) { size_t m = cameras.size(); - assert(measurements.size()==m); + assert(measurements.size() == m); if (m < 2) throw(TriangulationUnderconstrainedException()); // construct projection matrices from poses & calibration - typedef PinholeCamera Camera; - std::vector projection_matrices; + typedef PinholeCamera Camera; + std::vector projection_matrices; BOOST_FOREACH(const Camera& camera, cameras) projection_matrices.push_back( - camera.calibration().K() - * sub(camera.pose().inverse().matrix(), 0, 3, 0, 4)); - + CameraProjectionMatrix(camera.calibration())( + camera.pose())); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); // The n refine using non-linear optimization @@ -257,7 +282,7 @@ Point3 triangulatePoint3( BOOST_FOREACH(const Camera& camera, cameras) { const Point3& p_local = camera.pose().transform_to(point); if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + throw(TriangulationCheiralityException()); } #endif diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 830ddd3ec..ff50c9781 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -253,7 +253,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(nodes_); ar & BOOST_SERIALIZATION_NVP(roots_); } diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index beb222178..eae886785 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -160,7 +160,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(conditional_); ar & BOOST_SERIALIZATION_NVP(parent_); ar & BOOST_SERIALIZATION_NVP(children); diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index bdfec9cd5..39c738a19 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -141,7 +141,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(nrFrontals_); } diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index b64ebe259..5e261e200 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -28,7 +28,8 @@ namespace gtsam { template boost::shared_ptr::BayesNetType> EliminateableFactorGraph::eliminateSequential( - OptionalOrdering ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const + OptionalOrdering ordering, const Eliminate& function, + OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const { if(ordering && variableIndex) { gttic(eliminateSequential); @@ -47,13 +48,16 @@ namespace gtsam { // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check // for no variable index first so that it's always computed if we need to call COLAMD because // no Ordering is provided. - return eliminateSequential(ordering, function, VariableIndex(asDerived())); + return eliminateSequential(ordering, function, VariableIndex(asDerived()), orderingType); } else /*if(!ordering)*/ { // If no Ordering provided, compute one and call this function again. We are guaranteed to // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. - return eliminateSequential(Ordering::COLAMD(*variableIndex), function); + if (orderingType == Ordering::METIS) + return eliminateSequential(Ordering::metis(asDerived()), function, variableIndex, orderingType); + else + return eliminateSequential(Ordering::colamd(*variableIndex), function, variableIndex, orderingType); } } @@ -61,7 +65,8 @@ namespace gtsam { template boost::shared_ptr::BayesTreeType> EliminateableFactorGraph::eliminateMultifrontal( - OptionalOrdering ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const + OptionalOrdering ordering, const Eliminate& function, + OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const { if(ordering && variableIndex) { gttic(eliminateMultifrontal); @@ -81,13 +86,16 @@ namespace gtsam { // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check // for no variable index first so that it's always computed if we need to call COLAMD because // no Ordering is provided. - return eliminateMultifrontal(ordering, function, VariableIndex(asDerived())); + return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()), orderingType); } else /*if(!ordering)*/ { // If no Ordering provided, compute one and call this function again. We are guaranteed to // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. - return eliminateMultifrontal(Ordering::COLAMD(*variableIndex), function); + if (orderingType == Ordering::METIS) + return eliminateMultifrontal(Ordering::metis(asDerived()), function, variableIndex, orderingType); + else + return eliminateMultifrontal(Ordering::colamd(*variableIndex), function, variableIndex, orderingType); } } @@ -117,7 +125,7 @@ namespace gtsam { if(variableIndex) { gttic(eliminatePartialSequential); // Compute full ordering - Ordering fullOrdering = Ordering::COLAMDConstrainedFirst(*variableIndex, variables); + Ordering fullOrdering = Ordering::colamdConstrainedFirst(*variableIndex, variables); // Split off the part of the ordering for the variables being eliminated Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size()); @@ -155,7 +163,7 @@ namespace gtsam { if(variableIndex) { gttic(eliminatePartialMultifrontal); // Compute full ordering - Ordering fullOrdering = Ordering::COLAMDConstrainedFirst(*variableIndex, variables); + Ordering fullOrdering = Ordering::colamdConstrainedFirst(*variableIndex, variables); // Split off the part of the ordering for the variables being eliminated Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size()); @@ -208,7 +216,7 @@ namespace gtsam { boost::get(&variables) : boost::get&>(&variables); Ordering totalOrdering = - Ordering::COLAMDConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + Ordering::colamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); // Split up ordering const size_t nVars = variablesOrOrdering->size(); @@ -267,7 +275,7 @@ namespace gtsam { boost::get(&variables) : boost::get&>(&variables); Ordering totalOrdering = - Ordering::COLAMDConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + Ordering::colamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); // Split up ordering const size_t nVars = variablesOrOrdering->size(); @@ -293,7 +301,7 @@ namespace gtsam { if(variableIndex) { // Compute a total ordering for all variables - Ordering totalOrdering = Ordering::COLAMDConstrainedLast(*variableIndex, variables); + Ordering totalOrdering = Ordering::colamdConstrainedLast(*variableIndex, variables); // Split out the part for the marginalized variables Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size()); diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 717f49167..5fb5fbdb1 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -94,6 +94,9 @@ namespace gtsam { /// Typedef for an optional variable index as an argument to elimination functions typedef boost::optional OptionalVariableIndex; + /// Typedef for an optional ordering type + typedef boost::optional OptionalOrderingType; + /** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not * provided, the ordering provided by COLAMD will be used. * @@ -101,6 +104,10 @@ namespace gtsam { * \code * boost::shared_ptr result = graph.eliminateSequential(EliminateCholesky); * \endcode + * + * Example - METIS ordering for elimination + * \code + * boost::shared_ptr result = graph.eliminateSequential(OrderingType::METIS); * * Example - Full QR elimination in specified order: * \code @@ -117,7 +124,8 @@ namespace gtsam { boost::shared_ptr eliminateSequential( OptionalOrdering ordering = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; + OptionalVariableIndex variableIndex = boost::none, + OptionalOrderingType orderingType = boost::none) const; /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not * provided, the ordering provided by COLAMD will be used. @@ -142,7 +150,8 @@ namespace gtsam { boost::shared_ptr eliminateMultifrontal( OptionalOrdering ordering = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; + OptionalVariableIndex variableIndex = boost::none, + OptionalOrderingType orderingType = boost::none) const; /** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net * and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index d66f6b8ac..3e41d7692 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -160,7 +160,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(keys_); } diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 043c4caf6..4d5428e5c 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -250,7 +250,6 @@ namespace gtsam { void print(const std::string& s = "FactorGraph", const KeyFormatter& formatter = DefaultKeyFormatter) const; - protected: /** Check equality */ bool equals(const This& fg, double tol = 1e-9) const; /// @} @@ -343,7 +342,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(factors_); } diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index 645499b2c..7cb3d9817 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -46,7 +46,7 @@ namespace gtsam { const VariableIndex varIndex(factors); const FastSet newFactorKeys = newFactors.keys(); const Ordering constrainedOrdering = - Ordering::COLAMDConstrainedLast(varIndex, std::vector(newFactorKeys.begin(), newFactorKeys.end())); + Ordering::colamdConstrainedLast(varIndex, std::vector(newFactorKeys.begin(), newFactorKeys.end())); Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex); this->roots_.insert(this->roots_.end(), bayesTree.roots().begin(), bayesTree.roots().end()); this->nodes_.insert(bayesTree.nodes().begin(), bayesTree.nodes().end()); diff --git a/gtsam/inference/LabeledSymbol.cpp b/gtsam/inference/LabeledSymbol.cpp index 7e573c13f..b9e93ceb1 100644 --- a/gtsam/inference/LabeledSymbol.cpp +++ b/gtsam/inference/LabeledSymbol.cpp @@ -17,12 +17,8 @@ #include -#include #include -#include -#include -#include -#include +#include #include @@ -111,23 +107,21 @@ bool LabeledSymbol::operator!=(gtsam::Key comp) const { } /* ************************************************************************* */ +static LabeledSymbol make(gtsam::Key key) { return LabeledSymbol(key);} + boost::function LabeledSymbol::TypeTest(unsigned char c) { - namespace bl = boost::lambda; - return bl::bind(&LabeledSymbol::chr, bl::bind(bl::constructor(), bl::_1)) == c; + return boost::bind(&LabeledSymbol::chr, boost::bind(make, _1)) == c; } -/* ************************************************************************* */ boost::function LabeledSymbol::LabelTest(unsigned char label) { - namespace bl = boost::lambda; - return bl::bind(&LabeledSymbol::label, bl::bind(bl::constructor(), bl::_1)) == label; + return boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label; } -/* ************************************************************************* */ boost::function LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) { - namespace bl = boost::lambda; - return bl::bind(&LabeledSymbol::chr, bl::bind(bl::constructor(), bl::_1)) == c && - bl::bind(&LabeledSymbol::label, bl::bind(bl::constructor(), bl::_1)) == label; + return boost::bind(&LabeledSymbol::chr, boost::bind(make, _1)) == c && + boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label; } +/* ************************************************************************* */ } // \namespace gtsam diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h index 4b125988c..452c98434 100644 --- a/gtsam/inference/LabeledSymbol.h +++ b/gtsam/inference/LabeledSymbol.h @@ -109,7 +109,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(c_); ar & BOOST_SERIALIZATION_NVP(label_); ar & BOOST_SERIALIZATION_NVP(j_); @@ -130,5 +130,8 @@ inline unsigned char mrsymbolLabel(Key key) { return LabeledSymbol(key).label(); /** Return the index portion of a symbol key. */ inline size_t mrsymbolIndex(Key key) { return LabeledSymbol(key).index(); } +/// traits +template<> struct traits : public Testable {}; + } // \namespace gtsam diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h new file mode 100644 index 000000000..7299d7c8a --- /dev/null +++ b/gtsam/inference/MetisIndex-inl.h @@ -0,0 +1,85 @@ +/* ---------------------------------------------------------------------------- + + * 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 MetisIndex-inl.h + * @author Andrew Melim + * @date Oct. 10, 2014 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +template +void MetisIndex::augment(const FactorGraph& factors) { + std::map > iAdjMap; // Stores a set of keys that are adjacent to key x, with adjMap.first + std::map >::iterator iAdjMapIt; + std::set keySet; + + /* ********** Convert to CSR format ********** */ + // Assuming that vertex numbering starts from 0 (C style), + // then the adjacency list of vertex i is stored in array adjncy + // starting at index xadj[i] and ending at(but not including) + // index xadj[i + 1](i.e., adjncy[xadj[i]] through + // and including adjncy[xadj[i + 1] - 1]). + int32_t keyCounter = 0; + + // First: Record a copy of each key inside the factorgraph and create a + // key to integer mapping. This is referenced during the adjaceny step + for (size_t i = 0; i < factors.size(); i++) { + if (factors[i]) { + BOOST_FOREACH(const Key& key, *factors[i]) { + keySet.insert(keySet.end(), key); // Keep a track of all unique keys + if (intKeyBMap_.left.find(key) == intKeyBMap_.left.end()) { + intKeyBMap_.insert(bm_type::value_type(key, keyCounter)); + keyCounter++; + } + } + } + } + + // Create an adjacency mapping that stores the set of all adjacent keys for every key + for (size_t i = 0; i < factors.size(); i++) { + if (factors[i]) { + BOOST_FOREACH(const Key& k1, *factors[i]) + BOOST_FOREACH(const Key& k2, *factors[i]) + if (k1 != k2) { + // Store both in Key and int32_t format + int i = intKeyBMap_.left.at(k1); + int j = intKeyBMap_.left.at(k2); + iAdjMap[i].insert(iAdjMap[i].end(), j); + } + } + } + + // Number of keys referenced in this factor graph + nKeys_ = keySet.size(); + + xadj_.push_back(0); // Always set the first index to zero + for (iAdjMapIt = iAdjMap.begin(); iAdjMapIt != iAdjMap.end(); ++iAdjMapIt) { + std::vector temp; + // Copy from the FastSet into a temporary vector + std::copy(iAdjMapIt->second.begin(), iAdjMapIt->second.end(), + std::back_inserter(temp)); + // Insert each index's set in order by appending them to the end of adj_ + adj_.insert(adj_.end(), temp.begin(), temp.end()); + //adj_.push_back(temp); + xadj_.push_back((int32_t) adj_.size()); + } +} + +} // \ gtsam diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h new file mode 100644 index 000000000..22b03ee5d --- /dev/null +++ b/gtsam/inference/MetisIndex.h @@ -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 + * -------------------------------------------------------------------------- */ + +/** + * @file MetisIndex.h + * @author Andrew Melim + * @date Oct. 10, 2014 + */ + +#pragma once + + +#include +#include +#include +#include +#include + +// Boost bimap generates many ugly warnings in CLANG +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wredeclared-class-member" +#endif +#include +#ifdef __clang__ +# pragma clang diagnostic pop +#endif + +#include + +namespace gtsam { +/** + * The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in + * METIS algorithms. Specifically, two vectors store the adjacency structure of the graph. It is built + * from a factor graph prior to elimination, and stores the list of factors + * that involve each variable. + * \nosubgrouping + */ +class GTSAM_EXPORT MetisIndex { +public: + typedef boost::shared_ptr shared_ptr; + typedef boost::bimap bm_type; + +private: + FastVector xadj_; // Index of node's adjacency list in adj + FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector + FastVector iadj_; // Integer keys for passing into metis. One to one mapping with adj_; + boost::bimap intKeyBMap_; // Stores Key <-> integer value relationship + size_t nKeys_; + +public: + /// @name Standard Constructors + /// @{ + + /** Default constructor, creates empty MetisIndex */ + MetisIndex() : + nKeys_(0) { + } + + template + MetisIndex(const FG& factorGraph) : + nKeys_(0) { + augment(factorGraph); + } + + ~MetisIndex() { + } + /// @} + /// @name Advanced Interface + /// @{ + + /** + * Augment the variable index with new factors. This can be used when + * solving problems incrementally. + */ + template + void augment(const FactorGraph& factors); + + std::vector xadj() const { + return xadj_; + } + std::vector adj() const { + return adj_; + } + size_t nValues() const { + return nKeys_; + } + Key intToKey(int32_t value) const { + assert(value >= 0); + return intKeyBMap_.right.find(value)->second; + } + + /// @} +}; + +} // \ namesace gtsam + +#include diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 4f4b14bb5..5ae25d543 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -12,6 +12,7 @@ /** * @file Ordering.cpp * @author Richard Roberts + * @author Andrew Melim * @date Sep 2, 2010 */ @@ -22,6 +23,7 @@ #include #include +#include using namespace std; @@ -37,15 +39,15 @@ namespace gtsam { } /* ************************************************************************* */ - Ordering Ordering::COLAMD(const VariableIndex& variableIndex) + Ordering Ordering::colamd(const VariableIndex& variableIndex) { // Call constrained version with all groups set to zero vector dummy_groups(variableIndex.size(), 0); - return Ordering::COLAMDConstrained(variableIndex, dummy_groups); + return Ordering::colamdConstrained(variableIndex, dummy_groups); } /* ************************************************************************* */ - Ordering Ordering::COLAMDConstrained( + Ordering Ordering::colamdConstrained( const VariableIndex& variableIndex, std::vector& cmember) { gttic(Ordering_COLAMDConstrained); @@ -112,7 +114,7 @@ namespace gtsam { } /* ************************************************************************* */ - Ordering Ordering::COLAMDConstrainedLast( + Ordering Ordering::colamdConstrainedLast( const VariableIndex& variableIndex, const std::vector& constrainLast, bool forceOrder) { gttic(Ordering_COLAMDConstrainedLast); @@ -135,11 +137,11 @@ namespace gtsam { ++ group; } - return Ordering::COLAMDConstrained(variableIndex, cmember); + return Ordering::colamdConstrained(variableIndex, cmember); } /* ************************************************************************* */ - Ordering Ordering::COLAMDConstrainedFirst( + Ordering Ordering::colamdConstrainedFirst( const VariableIndex& variableIndex, const std::vector& constrainFirst, bool forceOrder) { gttic(Ordering_COLAMDConstrainedFirst); @@ -169,11 +171,11 @@ namespace gtsam { if(c == none) c = group; - return Ordering::COLAMDConstrained(variableIndex, cmember); + return Ordering::colamdConstrained(variableIndex, cmember); } /* ************************************************************************* */ - Ordering Ordering::COLAMDConstrained(const VariableIndex& variableIndex, + Ordering Ordering::colamdConstrained(const VariableIndex& variableIndex, const FastMap& groups) { gttic(Ordering_COLAMDConstrained); @@ -193,7 +195,43 @@ namespace gtsam { cmember[keyIndices.at(p.first)] = p.second; } - return Ordering::COLAMDConstrained(variableIndex, cmember); + return Ordering::colamdConstrained(variableIndex, cmember); + } + + + /* ************************************************************************* */ + Ordering Ordering::metis(const MetisIndex& met) + { + gttic(Ordering_METIS); + + vector xadj = met.xadj(); + vector adj = met.adj(); + vector perm, iperm; + + idx_t size = met.nValues(); + for (idx_t i = 0; i < size; i++) + { + perm.push_back(0); + iperm.push_back(0); + } + + int outputError; + + outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0], &iperm[0]); + Ordering result; + + if (outputError != METIS_OK) + { + std::cout << "METIS failed during Nested Dissection ordering!\n"; + return result; + } + + result.resize(size); + for (size_t j = 0; j < (size_t)size; ++j){ + // We have to add the minKey value back to obtain the original key in the Values + result[j] = met.intToKey(perm[j]); + } + return result; } /* ************************************************************************* */ diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 7b1a2bb2e..274d5681c 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -12,6 +12,7 @@ /** * @file Ordering.h * @author Richard Roberts + * @author Andrew Melim * @date Sep 2, 2010 */ @@ -24,14 +25,22 @@ #include #include #include +#include #include namespace gtsam { + class Ordering : public std::vector { protected: typedef std::vector Base; public: + + /// Type of ordering to use + enum OrderingType { + COLAMD, METIS, CUSTOM + }; + typedef Ordering This; ///< Typedef to this class typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class @@ -62,11 +71,11 @@ namespace gtsam { /// performance). This internally builds a VariableIndex so if you already have a VariableIndex, /// it is faster to use COLAMD(const VariableIndex&) template - static Ordering COLAMD(const FactorGraph& graph) { - return COLAMD(VariableIndex(graph)); } + static Ordering colamd(const FactorGraph& graph) { + return colamd(VariableIndex(graph)); } /// Compute a fill-reducing ordering using COLAMD from a VariableIndex. - static GTSAM_EXPORT Ordering COLAMD(const VariableIndex& variableIndex); + static GTSAM_EXPORT Ordering colamd(const VariableIndex& variableIndex); /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details /// for note on performance). This internally builds a VariableIndex so if you already have a @@ -77,9 +86,9 @@ namespace gtsam { /// constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. template - static Ordering COLAMDConstrainedLast(const FactorGraph& graph, + static Ordering colamdConstrainedLast(const FactorGraph& graph, const std::vector& constrainLast, bool forceOrder = false) { - return COLAMDConstrainedLast(VariableIndex(graph), constrainLast, forceOrder); } + return colamdConstrainedLast(VariableIndex(graph), constrainLast, forceOrder); } /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This /// function constrains the variables in \c constrainLast to the end of the ordering, and orders @@ -87,7 +96,7 @@ namespace gtsam { /// variables in \c constrainLast will be ordered in the same order specified in the vector /// \c constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. - static GTSAM_EXPORT Ordering COLAMDConstrainedLast(const VariableIndex& variableIndex, + static GTSAM_EXPORT Ordering colamdConstrainedLast(const VariableIndex& variableIndex, const std::vector& constrainLast, bool forceOrder = false); /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details @@ -99,9 +108,9 @@ namespace gtsam { /// constrainLast. If \c forceOrder is false, the variables in \c constrainFirst will be /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. template - static Ordering COLAMDConstrainedFirst(const FactorGraph& graph, + static Ordering colamdConstrainedFirst(const FactorGraph& graph, const std::vector& constrainFirst, bool forceOrder = false) { - return COLAMDConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder); } + return colamdConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder); } /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This /// function constrains the variables in \c constrainFirst to the front of the ordering, and @@ -110,7 +119,7 @@ namespace gtsam { /// vector \c constrainFirst. If \c forceOrder is false, the variables in \c /// constrainFirst will be ordered after all the others, but will be rearranged by CCOLAMD to /// reduce fill-in as well. - static GTSAM_EXPORT Ordering COLAMDConstrainedFirst(const VariableIndex& variableIndex, + static GTSAM_EXPORT Ordering colamdConstrainedFirst(const VariableIndex& variableIndex, const std::vector& constrainFirst, bool forceOrder = false); /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details @@ -123,9 +132,9 @@ namespace gtsam { /// function simply fills the \c cmember argument to CCOLAMD with the supplied indices, see the /// CCOLAMD documentation for more information. template - static Ordering COLAMDConstrained(const FactorGraph& graph, + static Ordering colamdConstrained(const FactorGraph& graph, const FastMap& groups) { - return COLAMDConstrained(VariableIndex(graph), groups); } + return colamdConstrained(VariableIndex(graph), groups); } /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. In this /// function, a group for each variable should be specified in \c groups, and each group of @@ -134,7 +143,7 @@ namespace gtsam { /// appear in \c groups in arbitrary order. Any variables not present in \c groups will be /// assigned to group 0. This function simply fills the \c cmember argument to CCOLAMD with the /// supplied indices, see the CCOLAMD documentation for more information. - static GTSAM_EXPORT Ordering COLAMDConstrained(const VariableIndex& variableIndex, + static GTSAM_EXPORT Ordering colamdConstrained(const VariableIndex& variableIndex, const FastMap& groups); /// Return a natural Ordering. Typically used by iterative solvers @@ -146,6 +155,19 @@ namespace gtsam { return Ordering(keys); } + /// METIS Formatting function + template + static GTSAM_EXPORT void CSRFormat(std::vector& xadj, std::vector& adj, const FactorGraph& graph); + + /// Compute an ordering determined by METIS from a VariableIndex + static GTSAM_EXPORT Ordering metis(const MetisIndex& met); + + template + static Ordering metis(const FactorGraph& graph) + { + return metis(MetisIndex(graph)); + } + /// @} /// @name Testable @{ @@ -158,15 +180,20 @@ namespace gtsam { private: /// Internal COLAMD function - static GTSAM_EXPORT Ordering COLAMDConstrained( + static GTSAM_EXPORT Ordering colamdConstrained( const VariableIndex& variableIndex, std::vector& cmember); + /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; + + /// traits + template<> struct traits : public Testable {}; + } diff --git a/gtsam/inference/Symbol.cpp b/gtsam/inference/Symbol.cpp index 37a6d0897..f8b37d429 100644 --- a/gtsam/inference/Symbol.cpp +++ b/gtsam/inference/Symbol.cpp @@ -18,19 +18,8 @@ #include -#include #include -#include -#include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif -#include -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif +#include #include #include @@ -71,10 +60,10 @@ Symbol::operator std::string() const { return str(boost::format("%c%d") % c_ % j_); } +static Symbol make(gtsam::Key key) { return Symbol(key);} + boost::function Symbol::ChrTest(unsigned char c) { - namespace bl = boost::lambda; - return bl::bind(&Symbol::chr, bl::bind(bl::constructor(), bl::_1)) - == c; + return bind(&Symbol::chr, bind(make, _1)) == c; } } // namespace gtsam diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 6963905e0..68d927baf 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -18,9 +18,9 @@ #pragma once -#include - #include +#include +#include namespace gtsam { @@ -117,7 +117,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(c_); ar & BOOST_SERIALIZATION_NVP(j_); } @@ -161,5 +161,8 @@ inline Key Y(size_t j) { return Symbol('y', j); } inline Key Z(size_t j) { return Symbol('z', j); } } -} // namespace gtsam +/// traits +template<> struct traits : public Testable {}; + +} // \ namespace gtsam diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index 3985221d3..bcaec6ee4 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -17,16 +17,18 @@ #pragma once +#include +#include +#include +#include +#include +#include + +#include + #include #include #include -#include - -#include -#include -#include -#include -#include namespace gtsam { @@ -175,6 +177,11 @@ protected: /// @} }; -} +/// traits +template<> +struct traits : public Testable { +}; + +} //\ namespace gtsam #include diff --git a/gtsam/inference/VariableSlots.h b/gtsam/inference/VariableSlots.h index 9a16ca788..fad789436 100644 --- a/gtsam/inference/VariableSlots.h +++ b/gtsam/inference/VariableSlots.h @@ -22,12 +22,12 @@ #include #include #include - -#include +#include #include #include +#include #include namespace gtsam { @@ -82,6 +82,9 @@ public: /// @} }; +/// traits +template<> struct traits : public Testable {}; + /* ************************************************************************* */ template VariableSlots::VariableSlots(const FG& factorGraph) diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp index 5b57096cb..1033c0cc9 100644 --- a/gtsam/inference/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -14,14 +14,14 @@ * @author Alex Cunningham */ -#include // for operator += -using namespace boost::assign; - -#include +#include #include #include -#include +#include + +#include // for operator += +using namespace boost::assign; using namespace std; using namespace gtsam; @@ -65,6 +65,13 @@ TEST(Key, KeySymbolEncoding) { EXPECT(assert_equal(symbol, Symbol(key))); } +/* ************************************************************************* */ +TEST(Key, ChrTest) { + Key key = Symbol('c',3); + EXPECT(Symbol::ChrTest('c')(key)); + EXPECT(!Symbol::ChrTest('d')(key)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/inference/tests/testLabeledSymbol.cpp b/gtsam/inference/tests/testLabeledSymbol.cpp index 07727c8dc..18216453d 100644 --- a/gtsam/inference/tests/testLabeledSymbol.cpp +++ b/gtsam/inference/tests/testLabeledSymbol.cpp @@ -14,20 +14,19 @@ * @author Alex Cunningham */ -#include // for operator += -using namespace boost::assign; - -#include +#include #include #include -#include - +#include +#include // for operator += + +using namespace boost::assign; using namespace std; using namespace gtsam; /* ************************************************************************* */ -TEST( testLabeledSymbol, KeyLabeledSymbolConversion ) { +TEST(LabeledSymbol, KeyLabeledSymbolConversion ) { LabeledSymbol expected('x', 'A', 4); Key key(expected); LabeledSymbol actual(key); @@ -36,7 +35,7 @@ TEST( testLabeledSymbol, KeyLabeledSymbolConversion ) { } /* ************************************************************************* */ -TEST( testLabeledSymbol, KeyLabeledSymbolEncoding ) { +TEST(LabeledSymbol, KeyLabeledSymbolEncoding ) { // Test encoding of LabeledSymbol <-> size_t <-> string // Encoding scheme: @@ -69,6 +68,17 @@ TEST( testLabeledSymbol, KeyLabeledSymbolEncoding ) { } } +/* ************************************************************************* */ +TEST(LabeledSymbol, ChrTest) { + Key key = LabeledSymbol('c','A',3); + EXPECT(LabeledSymbol::TypeTest('c')(key)); + EXPECT(!LabeledSymbol::TypeTest('d')(key)); + EXPECT(LabeledSymbol::LabelTest('A')(key)); + EXPECT(!LabeledSymbol::LabelTest('D')(key)); + EXPECT(LabeledSymbol::TypeLabelTest('c','A')(key)); + EXPECT(!LabeledSymbol::TypeLabelTest('c','D')(key)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 3bf6f7ca0..013f569e0 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -12,15 +12,17 @@ /** * @file testOrdering * @author Alex Cunningham + * @author Andrew Melim */ #include #include #include +#include #include #include -#include +#include using namespace std; using namespace gtsam; @@ -38,17 +40,17 @@ TEST(Ordering, constrained_ordering) { sfg.push_factor(4,5); // unconstrained version - Ordering actUnconstrained = Ordering::COLAMD(sfg); + Ordering actUnconstrained = Ordering::colamd(sfg); Ordering expUnconstrained = Ordering(list_of(0)(1)(2)(3)(4)(5)); EXPECT(assert_equal(expUnconstrained, actUnconstrained)); // constrained version - push one set to the end - Ordering actConstrained = Ordering::COLAMDConstrainedLast(sfg, list_of(2)(4)); + Ordering actConstrained = Ordering::colamdConstrainedLast(sfg, list_of(2)(4)); Ordering expConstrained = Ordering(list_of(0)(1)(5)(3)(4)(2)); EXPECT(assert_equal(expConstrained, actConstrained)); // constrained version - push one set to the start - Ordering actConstrained2 = Ordering::COLAMDConstrainedFirst(sfg, list_of(2)(4)); + Ordering actConstrained2 = Ordering::colamdConstrainedFirst(sfg, list_of(2)(4)); Ordering expConstrained2 = Ordering(list_of(2)(4)(0)(1)(3)(5)); EXPECT(assert_equal(expConstrained2, actConstrained2)); } @@ -72,11 +74,163 @@ TEST(Ordering, grouped_constrained_ordering) { constraints[4] = 1; constraints[5] = 2; - Ordering actConstrained = Ordering::COLAMDConstrained(sfg, constraints); + Ordering actConstrained = Ordering::colamdConstrained(sfg, constraints); Ordering expConstrained = list_of(0)(1)(3)(2)(4)(5); EXPECT(assert_equal(expConstrained, actConstrained)); } +/* ************************************************************************* */ +TEST(Ordering, csr_format) { + // Example in METIS manual + SymbolicFactorGraph sfg; + sfg.push_factor(0, 1); + sfg.push_factor(1, 2); + sfg.push_factor(2, 3); + sfg.push_factor(3, 4); + sfg.push_factor(5, 6); + sfg.push_factor(6, 7); + sfg.push_factor(7, 8); + sfg.push_factor(8, 9); + sfg.push_factor(10, 11); + sfg.push_factor(11, 12); + sfg.push_factor(12, 13); + sfg.push_factor(13, 14); + + sfg.push_factor(0, 5); + sfg.push_factor(5, 10); + sfg.push_factor(1, 6); + sfg.push_factor(6, 11); + sfg.push_factor(2, 7); + sfg.push_factor(7, 12); + sfg.push_factor(3, 8); + sfg.push_factor(8, 13); + sfg.push_factor(4, 9); + sfg.push_factor(9, 14); + + MetisIndex mi(sfg); + + vector xadjExpected, adjExpected; + xadjExpected += 0, 2, 5, 8, 11, 13, 16, 20, 24, 28, 31, 33, 36, 39, 42, 44; + adjExpected += 1, 5, 0, 2, 6, 1, 3, 7, 2, 4, 8, 3, 9, 0, 6, 10, 1, 5, 7, 11, + 2, 6, 8, 12, 3, 7, 9, 13, 4, 8, 14, 5, 11, 6, 10, 12, 7, 11, + 13, 8, 12, 14, 9, 13 ; + + EXPECT(xadjExpected == mi.xadj()); + EXPECT(adjExpected.size() == mi.adj().size()); + EXPECT(adjExpected == mi.adj()); +} + +/* ************************************************************************* */ +TEST(Ordering, csr_format_2) { + SymbolicFactorGraph sfg; + + sfg.push_factor(0); + sfg.push_factor(0, 1); + sfg.push_factor(1, 2); + sfg.push_factor(2, 3); + sfg.push_factor(3, 4); + sfg.push_factor(4, 1); + + MetisIndex mi(sfg); + + vector xadjExpected, adjExpected; + xadjExpected += 0, 1, 4, 6, 8, 10; + adjExpected += 1, 0, 2, 4, 1, 3, 2, 4, 1, 3; + + EXPECT(xadjExpected == mi.xadj()); + EXPECT(adjExpected.size() == mi.adj().size()); + EXPECT(adjExpected == mi.adj()); + +} + +/* ************************************************************************* */ +TEST(Ordering, csr_format_3) { + SymbolicFactorGraph sfg; + + sfg.push_factor(100); + sfg.push_factor(100, 101); + sfg.push_factor(101, 102); + sfg.push_factor(102, 103); + sfg.push_factor(103, 104); + sfg.push_factor(104, 101); + + MetisIndex mi(sfg); + + vector xadjExpected, adjExpected; + xadjExpected += 0, 1, 4, 6, 8, 10; + adjExpected += 1, 0, 2, 4, 1, 3, 2, 4, 1, 3; + //size_t minKey = mi.minKey(); + + vector adjAcutal = mi.adj(); + + // Normalize, subtract the smallest key + //std::transform(adjAcutal.begin(), adjAcutal.end(), adjAcutal.begin(), + // std::bind2nd(std::minus(), minKey)); + + EXPECT(xadjExpected == mi.xadj()); + EXPECT(adjExpected.size() == mi.adj().size()); + EXPECT(adjExpected == adjAcutal); + +} + +/* ************************************************************************* */ +TEST(Ordering, csr_format_4) { + SymbolicFactorGraph sfg; + + sfg.push_factor(Symbol('x', 1)); + sfg.push_factor(Symbol('x', 1), Symbol('x', 2)); + sfg.push_factor(Symbol('x', 2), Symbol('x', 3)); + sfg.push_factor(Symbol('x', 3), Symbol('x', 4)); + sfg.push_factor(Symbol('x', 4), Symbol('x', 5)); + sfg.push_factor(Symbol('x', 5), Symbol('x', 6)); + + MetisIndex mi(sfg); + + vector xadjExpected, adjExpected; + xadjExpected += 0, 1, 3, 5, 7, 9, 10; + adjExpected += 1, 0, 2, 1, 3, 2, 4, 3, 5, 4; + + vector adjAcutal = mi.adj(); + vector xadjActual = mi.xadj(); + + EXPECT(xadjExpected == mi.xadj()); + EXPECT(adjExpected.size() == mi.adj().size()); + EXPECT(adjExpected == adjAcutal); + + Ordering metOrder = Ordering::metis(sfg); + + // Test different symbol types + sfg.push_factor(Symbol('l', 1)); + sfg.push_factor(Symbol('x', 1), Symbol('l', 1)); + sfg.push_factor(Symbol('x', 2), Symbol('l', 1)); + sfg.push_factor(Symbol('x', 3), Symbol('l', 1)); + sfg.push_factor(Symbol('x', 4), Symbol('l', 1)); + + Ordering metOrder2 = Ordering::metis(sfg); + +} + +/* ************************************************************************* */ +TEST(Ordering, metis) { + + SymbolicFactorGraph sfg; + + sfg.push_factor(0); + sfg.push_factor(0, 1); + sfg.push_factor(1, 2); + + MetisIndex mi(sfg); + + vector xadjExpected, adjExpected; + xadjExpected += 0, 1, 3, 4; + adjExpected += 1, 0, 2, 1; + + EXPECT(xadjExpected == mi.xadj()); + EXPECT(adjExpected.size() == mi.adj().size()); + EXPECT(adjExpected == mi.adj()); + + Ordering metis = Ordering::metis(sfg); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/linear/ConjugateGradientSolver.cpp b/gtsam/linear/ConjugateGradientSolver.cpp index 0505f6c01..a1b9e2d83 100644 --- a/gtsam/linear/ConjugateGradientSolver.cpp +++ b/gtsam/linear/ConjugateGradientSolver.cpp @@ -1,8 +1,20 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + /* - * ConjugateGradientSolver.cpp - * - * Created on: Jun 4, 2014 - * Author: ydjian + * @file ConjugateGradientSolver.cpp + * @brief Implementation of Conjugate Gradient solver for a linear system + * @author Yong-Dian Jian + * @author Sungtae An + * @date Nov 6, 2014 */ #include @@ -35,7 +47,8 @@ std::string ConjugateGradientParameters::blasTranslator(const BLASKernel value) } /*****************************************************************************/ -ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTranslator(const std::string &src) { +ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTranslator( + const std::string &src) { std::string s = src; boost::algorithm::to_upper(s); if (s == "GTSAM") return ConjugateGradientParameters::GTSAM; @@ -43,6 +56,7 @@ ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTransla return ConjugateGradientParameters::GTSAM; } +/*****************************************************************************/ } diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index 6e8509309..2596a7403 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -9,10 +9,17 @@ * -------------------------------------------------------------------------- */ +/** + * @file ConjugateGradientSolver.h + * @brief Implementation of Conjugate Gradient solver for a linear system + * @author Yong-Dian Jian + * @author Sungtae An + * @date Nov 6, 2014 + **/ + #pragma once #include -#include namespace gtsam { @@ -79,22 +86,26 @@ public: static BLASKernel blasTranslator(const std::string &s) ; }; -/*********************************************************************************************/ /* - * A template of linear preconditioned conjugate gradient method. - * System class should support residual(v, g), multiply(v,Av), leftPrecondition(v, S^{-t}v, - * rightPrecondition(v, S^{-1}v), scal(alpha,v), dot(v,v), axpy(alpha,x,y) + * A template for the linear preconditioned conjugate gradient method. + * System class should support residual(v, g), multiply(v,Av), scal(alpha,v), dot(v,v), axpy(alpha,x,y) + * leftPrecondition(v, L^{-1}v, rightPrecondition(v, L^{-T}v) where preconditioner M = L*L^T * Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book. + * + ** REFERENCES: + * [1] Y. Saad, "Preconditioned Iterations," in Iterative Methods for Sparse Linear Systems, + * 2nd ed. SIAM, 2003, ch. 9, sec. 2, pp.276-281. */ -template -V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters ¶meters) { +template +V preconditionedConjugateGradient(const S &system, const V &initial, + const ConjugateGradientParameters ¶meters) { V estimate, residual, direction, q1, q2; estimate = residual = direction = q1 = q2 = initial; system.residual(estimate, q1); /* q1 = b-Ax */ - system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */ - system.rightPrecondition(residual, direction);/* d = S^{-1} r */ + system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */ + system.rightPrecondition(residual, direction);/* p = L^{-T} r */ double currentGamma = system.dot(residual, residual), prevGamma, alpha, beta; @@ -116,27 +127,29 @@ V preconditionedConjugateGradient(const S &system, const V &initial, const Conju if ( k % iReset == 0 ) { system.residual(estimate, q1); /* q1 = b-Ax */ - system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */ - system.rightPrecondition(residual, direction); /* d = S^{-1} r */ + system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */ + system.rightPrecondition(residual, direction); /* p = L^{-T} r */ currentGamma = system.dot(residual, residual); } - system.multiply(direction, q1); /* q1 = A d */ - alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (d' A d) */ - system.axpy(alpha, direction, estimate); /* estimate += alpha * direction */ - system.leftPrecondition(q1, q2); /* q2 = S^{-T} * q1 */ - system.axpy(-alpha, q2, residual); /* residual -= alpha * q2 */ + system.multiply(direction, q1); /* q1 = A p */ + alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (p' A p) */ + system.axpy(alpha, direction, estimate); /* estimate += alpha * p */ + system.leftPrecondition(q1, q2); /* q2 = L^{-1} * q1 */ + system.axpy(-alpha, q2, residual); /* r -= alpha * q2 */ prevGamma = currentGamma; - currentGamma = system.dot(residual, residual); /* gamma = |residual|^2 */ + currentGamma = system.dot(residual, residual); /* gamma = |r|^2 */ beta = currentGamma / prevGamma; - system.rightPrecondition(residual, q1); /* q1 = S^{-1} residual */ + system.rightPrecondition(residual, q1); /* q1 = L^{-T} r */ system.scal(beta, direction); - system.axpy(1.0, q1, direction); /* direction = q1 + beta * direction */ + system.axpy(1.0, q1, direction); /* p = q1 + beta * p */ if (parameters.verbosity() >= ConjugateGradientParameters::ERROR ) std::cout << "[PCG] k = " << k << ", alpha = " << alpha << ", beta = " << beta << ", ||r||^2 = " << currentGamma +// << "\nx =\n" << estimate +// << "\nr =\n" << residual << std::endl; } if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY ) diff --git a/gtsam/linear/Errors.h b/gtsam/linear/Errors.h index 13da360a5..44d68be83 100644 --- a/gtsam/linear/Errors.h +++ b/gtsam/linear/Errors.h @@ -21,6 +21,7 @@ #include #include +#include #include @@ -70,4 +71,9 @@ namespace gtsam { /** print with optional string */ GTSAM_EXPORT void print(const Errors& a, const std::string& s = "Error"); -} // gtsam + /// traits + template<> + struct traits : public Testable { + }; + +} //\ namespace gtsam diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 69a70a5e4..401583bbf 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -166,9 +166,14 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; -} /// namespace gtsam + /// traits + template<> + struct traits : public Testable { + }; + +} //\ namespace gtsam diff --git a/gtsam/linear/GaussianBayesTree.h b/gtsam/linear/GaussianBayesTree.h index 5185154b4..1b2ad47e0 100644 --- a/gtsam/linear/GaussianBayesTree.h +++ b/gtsam/linear/GaussianBayesTree.h @@ -128,4 +128,9 @@ namespace gtsam { Matrix marginalCovariance(Key key) const; }; -} + /// traits + template<> + struct traits : public Testable { + }; + +} //\ namespace gtsam diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 9cce29d60..d9b75c69f 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -135,13 +135,17 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseFactor); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional); } }; // GaussianConditional -} // gtsam +/// traits +template<> +struct traits : public Testable {}; + +} // \ namespace gtsam #include diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 8c8e2cb2b..6a7d91bc9 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -20,8 +20,9 @@ #pragma once -#include #include +#include +#include namespace gtsam { @@ -99,7 +100,7 @@ namespace gtsam { /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const = 0; - /// Return the diagonal of the Hessian for this factor (raw memory version) + /// Raw memory access version of hessianDiagonal virtual void hessianDiagonal(double* d) const = 0; /// Return the block diagonal of the Hessian for this factor @@ -121,26 +122,28 @@ namespace gtsam { /// y += alpha * A'*A*x virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const = 0; - /// y += alpha * A'*A*x - virtual void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) const = 0; - - /// y += alpha * A'*A*x - virtual void multiplyHessianAdd(double alpha, const double* x, double* y) const = 0; - /// A'*b for Jacobian, eta for Hessian virtual VectorValues gradientAtZero() const = 0; - /// A'*b for Jacobian, eta for Hessian (raw memory version) + /// Raw memory access version of gradientAtZero virtual void gradientAtZero(double* d) const = 0; + /// Gradient wrt a key at any values + virtual Vector gradient(Key key, const VectorValues& x) const = 0; + private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; // GaussianFactor -} // namespace gtsam +/// traits +template<> +struct traits : public Testable { +}; + +} // \ namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 54e721cd7..6953d2969 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -254,6 +254,7 @@ namespace gtsam { map GaussianFactorGraph::hessianBlockDiagonal() const { map blocks; BOOST_FOREACH(const sharedFactor& factor, *this) { + if (!factor) continue; map BD = factor->hessianBlockDiagonal(); map::const_iterator it = BD.begin(); for(;it!=BD.end();it++) { @@ -303,6 +304,7 @@ namespace gtsam { // Zero-out the gradient VectorValues g; BOOST_FOREACH(const sharedFactor& factor, *this) { + if (!factor) continue; VectorValues gi = factor->gradientAtZero(); g.addInPlace_(gi); } @@ -355,15 +357,6 @@ namespace gtsam { f->multiplyHessianAdd(alpha, x, y); } - /* ************************************************************************* */ - void GaussianFactorGraph::multiplyHessianAdd(double alpha, - const double* x, double* y) const { - vector FactorKeys = getkeydim(); - BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this) - f->multiplyHessianAdd(alpha, x, y, FactorKeys); - - } - /* ************************************************************************* */ void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, Errors& e) const { multiplyInPlace(x, e.begin()); @@ -393,15 +386,15 @@ namespace gtsam { /* ************************************************************************* */ // x += alpha*A'*e -void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, - VectorValues& x) const { - // For each factor add the gradient contribution - Errors::const_iterator ei = e.begin(); - BOOST_FOREACH(const sharedFactor& Ai_G, *this) { - JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - Ai->transposeMultiplyAdd(alpha, *(ei++), x); + void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, + VectorValues& x) const { + // For each factor add the gradient contribution + Errors::const_iterator ei = e.begin(); + BOOST_FOREACH(const sharedFactor& Ai_G, *this) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + Ai->transposeMultiplyAdd(alpha, *(ei++), x); + } } -} ///* ************************************************************************* */ //void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 910b25d1e..832114ff6 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -310,10 +310,6 @@ namespace gtsam { void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; - ///** y += alpha*A'A*x */ - void multiplyHessianAdd(double alpha, const double* x, - double* y) const; - ///** In-place version e <- A*x that overwrites e. */ void multiplyInPlace(const VectorValues& x, Errors& e) const; @@ -326,7 +322,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } @@ -344,4 +340,9 @@ namespace gtsam { //GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r); //GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r); -} // namespace gtsam +/// traits +template<> +struct traits : public Testable { +}; + +} // \ namespace gtsam diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index f282682b3..f2bebcab9 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -359,20 +359,10 @@ VectorValues HessianFactor::hessianDiagonal() const { } /* ************************************************************************* */ -// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n +// Raw memory access version should be called in Regular Factors only currently void HessianFactor::hessianDiagonal(double* d) const { - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - - // Loop over all variables in the factor - for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { - Key j = keys_[pos]; - // Get the diagonal block, and insert its diagonal - const Matrix& B = info_(pos, pos).selfadjointView(); - DMap(d + 9 * j) += B.diagonal(); - } + throw std::runtime_error( + "HessianFactor::hessianDiagonal raw memory access is allowed for Regular Factors only"); } /* ************************************************************************* */ @@ -443,7 +433,8 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte } /* ************************************************************************* */ -void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatter) { +void HessianFactor::updateATA(const JacobianFactor& update, + const Scatter& scatter) { // This function updates 'combined' with the information in 'update'. // 'scatter' maps variables in the update factor to slots in the combined @@ -451,52 +442,47 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt gttic(updateATA); - if(update.rows() > 0) - { - // First build an array of slots - gttic(slots); - FastVector slots(update.size()); - DenseIndex slot = 0; - BOOST_FOREACH(Key j, update) { - slots[slot] = scatter.at(j).slot; - ++ slot; - } - gttoc(slots); - + if (update.rows() > 0) { gttic(whiten); // Whiten the factor if it has a noise model boost::optional _whitenedFactor; - const JacobianFactor* whitenedFactor; - if(update.get_model()) - { - if(update.get_model()->isConstrained()) - throw invalid_argument("Cannot update HessianFactor from JacobianFactor with constrained noise model"); - + const JacobianFactor* whitenedFactor = &update; + if (update.get_model()) { + if (update.get_model()->isConstrained()) + throw invalid_argument( + "Cannot update HessianFactor from JacobianFactor with constrained noise model"); _whitenedFactor = update.whiten(); whitenedFactor = &(*_whitenedFactor); } - else - { - whitenedFactor = &update; - } gttoc(whiten); - const VerticalBlockMatrix& updateBlocks = whitenedFactor->matrixObject(); + // A is the whitened Jacobian matrix A, and we are going to perform I += A'*A below + const VerticalBlockMatrix& A = whitenedFactor->matrixObject(); + + // N is number of variables in HessianFactor, n in JacobianFactor + DenseIndex N = this->info_.nBlocks() - 1, n = A.nBlocks() - 1; + + // First build an array of slots + gttic(slots); + FastVector slots(n + 1); + DenseIndex slot = 0; + BOOST_FOREACH(Key j, update) + slots[slot++] = scatter.at(j).slot; + slots[n] = N; + gttoc(slots); // Apply updates to the upper triangle gttic(update); - DenseIndex nrInfoBlocks = this->info_.nBlocks(), nrUpdateBlocks = updateBlocks.nBlocks(); - for(DenseIndex j2 = 0; j2 < nrUpdateBlocks; ++j2) { // Horizontal block of Hessian - DenseIndex slot2 = (j2 == (DenseIndex)update.size()) ? nrInfoBlocks-1 : slots[j2]; - assert(slot2 >= 0 && slot2 <= nrInfoBlocks); - for(DenseIndex j1 = 0; j1 <= j2; ++j1) { // Vertical block of Hessian - DenseIndex slot1 = (j1 == (DenseIndex)update.size()) ? nrInfoBlocks-1 : slots[j1]; - assert(slot1 >= 0 && slot1 < nrInfoBlocks); - if(slot1 != slot2) - info_(slot1, slot2).knownOffDiagonal() += updateBlocks(j1).transpose() * updateBlocks(j2); - else - info_(slot1, slot2).selfadjointView().rankUpdate(updateBlocks(j1).transpose()); + // Loop over blocks of A, including RHS with j==n + for (DenseIndex j = 0; j <= n; ++j) { + DenseIndex J = slots[j]; // get block in Hessian + // Fill off-diagonal blocks with Ai'*Aj + for (DenseIndex i = 0; i < j; ++i) { + DenseIndex I = slots[i]; // get block in Hessian + info_(I, J).knownOffDiagonal() += A(i).transpose() * A(j); } + // Fill diagonal block with Aj'*Aj + info_(J, J).selfadjointView().rankUpdate(A(j).transpose()); } gttoc(update); } @@ -548,48 +534,6 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, } } -/* ************************************************************************* */ -void HessianFactor::multiplyHessianAdd(double alpha, const double* x, - double* yvalues, vector offsets) const { - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - - // Create a vector of temporary y values, corresponding to rows i - vector y; - y.reserve(size()); - for (const_iterator it = begin(); it != end(); it++) - y.push_back(zero(getDim(it))); - - // Accessing the VectorValues one by one is expensive - // So we will loop over columns to access x only once per column - // And fill the above temporary y values, to be added into yvalues after - for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { - DenseIndex i = 0; - for (; i < j; ++i) - y[i] += info_(i, j).knownOffDiagonal() - * ConstDMap(x + offsets[keys_[j]], - offsets[keys_[j] + 1] - offsets[keys_[j]]); - // blocks on the diagonal are only half - y[i] += info_(j, j).selfadjointView() - * ConstDMap(x + offsets[keys_[j]], - offsets[keys_[j] + 1] - offsets[keys_[j]]); - // for below diagonal, we take transpose block from upper triangular part - for (i = j + 1; i < (DenseIndex) size(); ++i) - y[i] += info_(i, j).knownOffDiagonal() - * ConstDMap(x + offsets[keys_[j]], - offsets[keys_[j] + 1] - offsets[keys_[j]]); - } - - // copy to yvalues - for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) - DMap(yvalues + offsets[keys_[i]], offsets[keys_[i] + 1] - offsets[keys_[i]]) += - alpha * y[i]; -} - - /* ************************************************************************* */ VectorValues HessianFactor::gradientAtZero() const { VectorValues g; @@ -600,20 +544,34 @@ VectorValues HessianFactor::gradientAtZero() const { } /* ************************************************************************* */ -// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n +// Raw memory access version should be called in Regular Factors only currently void HessianFactor::gradientAtZero(double* d) const { + throw std::runtime_error( + "HessianFactor::gradientAtZero raw memory access is allowed for Regular Factors only"); +} - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - - // Loop over all variables in the factor - for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { - Key j = keys_[pos]; - // Get the diagonal block, and insert its diagonal - DVector dj = -info_(pos,size()).knownOffDiagonal(); - DMap(d + 9 * j) += dj; +/* ************************************************************************* */ +Vector HessianFactor::gradient(Key key, const VectorValues& x) const { + Factor::const_iterator i = find(key); + // Sum over G_ij*xj for all xj connecting to xi + Vector b = zero(x.at(key).size()); + for (Factor::const_iterator j = begin(); j != end(); ++j) { + // Obtain Gij from the Hessian factor + // Hessian factor only stores an upper triangular matrix, so be careful when i>j + Matrix Gij; + if (i > j) { + Matrix Gji = info(j, i); + Gij = Gji.transpose(); } + else { + Gij = info(i, j); + } + // Accumulate Gij*xj to gradf + b += Gij * x.at(*j); + } + // Subtract the linear term gi + b += -linearTerm(i); + return b; } /* ************************************************************************* */ diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 62d84925c..dbec5bb59 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -340,7 +340,7 @@ namespace gtsam { /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const; - /* ************************************************************************* */ + /// Raw memory access version of hessianDiagonal virtual void hessianDiagonal(double* d) const; /// Return the block diagonal of the Hessian for this factor @@ -380,15 +380,18 @@ namespace gtsam { /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; - void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) const; - - void multiplyHessianAdd(double alpha, const double* x, double* y) const {}; - /// eta for Hessian VectorValues gradientAtZero() const; + /// Raw memory access version of gradientAtZero virtual void gradientAtZero(double* d) const; + /** + * Compute the gradient at a key: + * \grad f(x_i) = \sum_j G_ij*x_j - g_i + */ + Vector gradient(Key key, const VectorValues& x) const; + /** * Densely partially eliminate with Cholesky factorization. JacobianFactors are * left-multiplied with their transpose to form the Hessian using the conversion constructor @@ -431,12 +434,17 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor); ar & BOOST_SERIALIZATION_NVP(info_); } }; -} +/// traits +template<> +struct traits : public Testable {}; + +} // \ namespace gtsam + #include diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index ab3ccde13..79ade1c8a 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -1,6 +1,17 @@ +/* ---------------------------------------------------------------------------- + + * 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 IterativeSolver.cpp - * @brief + * @brief Some support classes for iterative solvers * @date Sep 3, 2012 * @author Yong-Dian Jian */ @@ -9,18 +20,22 @@ #include #include #include +#include #include -#include using namespace std; namespace gtsam { /*****************************************************************************/ -string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); } +string IterativeOptimizationParameters::getVerbosity() const { + return verbosityTranslator(verbosity_); +} /*****************************************************************************/ -void IterativeOptimizationParameters::setVerbosity(const string &src) { verbosity_ = verbosityTranslator(src); } +void IterativeOptimizationParameters::setVerbosity(const string &src) { + verbosity_ = verbosityTranslator(src); +} /*****************************************************************************/ void IterativeOptimizationParameters::print() const { @@ -29,78 +44,82 @@ void IterativeOptimizationParameters::print() const { /*****************************************************************************/ void IterativeOptimizationParameters::print(ostream &os) const { - os << "IterativeOptimizationParameters:" << endl - << "verbosity: " << verbosityTranslator(verbosity_) << endl; + os << "IterativeOptimizationParameters:" << endl << "verbosity: " + << verbosityTranslator(verbosity_) << endl; } /*****************************************************************************/ - ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) { +ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) { p.print(os); return os; } /*****************************************************************************/ -IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const string &src) { - string s = src; boost::algorithm::to_upper(s); - if (s == "SILENT") return IterativeOptimizationParameters::SILENT; - else if (s == "COMPLEXITY") return IterativeOptimizationParameters::COMPLEXITY; - else if (s == "ERROR") return IterativeOptimizationParameters::ERROR; +IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator( + const string &src) { + string s = src; + boost::algorithm::to_upper(s); + if (s == "SILENT") + return IterativeOptimizationParameters::SILENT; + else if (s == "COMPLEXITY") + return IterativeOptimizationParameters::COMPLEXITY; + else if (s == "ERROR") + return IterativeOptimizationParameters::ERROR; /* default is default */ - else return IterativeOptimizationParameters::SILENT; + else + return IterativeOptimizationParameters::SILENT; } /*****************************************************************************/ - string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity) { - if (verbosity == SILENT) return "SILENT"; - else if (verbosity == COMPLEXITY) return "COMPLEXITY"; - else if (verbosity == ERROR) return "ERROR"; - else return "UNKNOWN"; +string IterativeOptimizationParameters::verbosityTranslator( + IterativeOptimizationParameters::Verbosity verbosity) { + if (verbosity == SILENT) + return "SILENT"; + else if (verbosity == COMPLEXITY) + return "COMPLEXITY"; + else if (verbosity == ERROR) + return "ERROR"; + else + return "UNKNOWN"; } /*****************************************************************************/ -VectorValues IterativeSolver::optimize( - const GaussianFactorGraph &gfg, +VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, boost::optional keyInfo, - boost::optional&> lambda) -{ - return optimize( - gfg, - keyInfo ? *keyInfo : KeyInfo(gfg), - lambda ? *lambda : std::map()); + boost::optional&> lambda) { + return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg), + lambda ? *lambda : std::map()); } /*****************************************************************************/ -VectorValues IterativeSolver::optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda) -{ +VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda) { return optimize(gfg, keyInfo, lambda, keyInfo.x0()); } /****************************************************************************/ -KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) - : ordering_(ordering) { +KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) : + ordering_(ordering) { initialize(fg); } /****************************************************************************/ -KeyInfo::KeyInfo(const GaussianFactorGraph &fg) - : ordering_(Ordering::Natural(fg)) { +KeyInfo::KeyInfo(const GaussianFactorGraph &fg) : + ordering_(Ordering::Natural(fg)) { initialize(fg); } /****************************************************************************/ -void KeyInfo::initialize(const GaussianFactorGraph &fg){ +void KeyInfo::initialize(const GaussianFactorGraph &fg) { const map colspec = fg.getKeyDimMap(); const size_t n = ordering_.size(); size_t start = 0; - for ( size_t i = 0 ; i < n ; ++i ) { + for (size_t i = 0; i < n; ++i) { const size_t key = ordering_[i]; const size_t dim = colspec.find(key)->second; insert(make_pair(key, KeyInfoEntry(i, dim, start))); - start += dim ; + start += dim; } numCols_ = start; } @@ -108,7 +127,7 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg){ /****************************************************************************/ vector KeyInfo::colSpec() const { std::vector result(size(), 0); - BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) { + BOOST_FOREACH ( const KeyInfo::value_type &item, *this ) { result[item.second.index()] = item.second.dim(); } return result; @@ -117,7 +136,7 @@ vector KeyInfo::colSpec() const { /****************************************************************************/ VectorValues KeyInfo::x0() const { VectorValues result; - BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) { + BOOST_FOREACH ( const KeyInfo::value_type &item, *this ) { result.insert(item.first, Vector::Zero(item.second.dim())); } return result; @@ -128,7 +147,5 @@ Vector KeyInfo::x0vector() const { return Vector::Zero(numCols_); } - } - diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 3c397c9e9..442a5fc6b 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -9,131 +9,178 @@ * -------------------------------------------------------------------------- */ +/** + * @file IterativeSolver.h + * @brief Some support classes for iterative solvers + * @date 2010 + * @author Yong-Dian Jian + */ + #pragma once -#include -#include #include +#include + #include -#include -#include +#include +#include + #include -#include #include -#include +#include namespace gtsam { - // Forward declarations - class KeyInfo; - class KeyInfoEntry; - class GaussianFactorGraph; - class Values; - class VectorValues; +// Forward declarations +class KeyInfo; +class KeyInfoEntry; +class GaussianFactorGraph; +class Values; +class VectorValues; - /************************************************************************************/ - /** - * parameters for iterative linear solvers - */ - class GTSAM_EXPORT IterativeOptimizationParameters { +/** + * parameters for iterative linear solvers + */ +class GTSAM_EXPORT IterativeOptimizationParameters { - public: +public: - typedef boost::shared_ptr shared_ptr; - enum Verbosity { SILENT = 0, COMPLEXITY, ERROR } verbosity_; + typedef boost::shared_ptr shared_ptr; + enum Verbosity { + SILENT = 0, COMPLEXITY, ERROR + } verbosity_; - public: +public: - IterativeOptimizationParameters(Verbosity v = SILENT) - : verbosity_(v) {} + IterativeOptimizationParameters(Verbosity v = SILENT) : + verbosity_(v) { + } - virtual ~IterativeOptimizationParameters() {} + virtual ~IterativeOptimizationParameters() { + } - /* utility */ - inline Verbosity verbosity() const { return verbosity_; } - std::string getVerbosity() const; - void setVerbosity(const std::string &s) ; + /* utility */ + inline Verbosity verbosity() const { + return verbosity_; + } + std::string getVerbosity() const; + void setVerbosity(const std::string &s); - /* matlab interface */ - void print() const ; + /* matlab interface */ + void print() const; - /* virtual print function */ - virtual void print(std::ostream &os) const ; + /* virtual print function */ + virtual void print(std::ostream &os) const; - /* for serialization */ - friend std::ostream& operator<<(std::ostream &os, const IterativeOptimizationParameters &p); + /* for serialization */ + friend std::ostream& operator<<(std::ostream &os, + const IterativeOptimizationParameters &p); - static Verbosity verbosityTranslator(const std::string &s); - static std::string verbosityTranslator(Verbosity v); - }; + static Verbosity verbosityTranslator(const std::string &s); + static std::string verbosityTranslator(Verbosity v); +}; - /************************************************************************************/ - class GTSAM_EXPORT IterativeSolver { - public: - typedef boost::shared_ptr shared_ptr; - IterativeSolver() {} - virtual ~IterativeSolver() {} +/** + * Base class for Iterative Solvers like SubgraphSolver + */ +class GTSAM_EXPORT IterativeSolver { +public: + typedef boost::shared_ptr shared_ptr; + IterativeSolver() { + } + virtual ~IterativeSolver() { + } - /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ - VectorValues optimize ( - const GaussianFactorGraph &gfg, + /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ + VectorValues optimize(const GaussianFactorGraph &gfg, boost::optional = boost::none, - boost::optional&> lambda = boost::none - ); + boost::optional&> lambda = boost::none); - /* interface to the nonlinear optimizer, without initial estimate */ - VectorValues optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda - ); + /* interface to the nonlinear optimizer, without initial estimate */ + VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, + const std::map &lambda); - /* interface to the nonlinear optimizer that the subclasses have to implement */ - virtual VectorValues optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda, - const VectorValues &initial - ) = 0; + /* interface to the nonlinear optimizer that the subclasses have to implement */ + virtual VectorValues optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial) = 0; - }; +}; - /************************************************************************************/ - /* Handy data structure for iterative solvers - * key to (index, dimension, colstart) */ - class GTSAM_EXPORT KeyInfoEntry : public boost::tuple { - public: - typedef boost::tuple Base; - KeyInfoEntry(){} - KeyInfoEntry(size_t idx, size_t d, Key start) : Base(idx, d, start) {} - const size_t index() const { return this->get<0>(); } - const size_t dim() const { return this->get<1>(); } - const size_t colstart() const { return this->get<2>(); } - }; +/** + * Handy data structure for iterative solvers + * key to (index, dimension, colstart) + */ +class GTSAM_EXPORT KeyInfoEntry: public boost::tuple { - /************************************************************************************/ - class GTSAM_EXPORT KeyInfo : public std::map { - public: - typedef std::map Base; - KeyInfo() : numCols_(0) {} - KeyInfo(const GaussianFactorGraph &fg); - KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering); +public: - std::vector colSpec() const ; - VectorValues x0() const; - Vector x0vector() const; + typedef boost::tuple Base; - inline size_t numCols() const { return numCols_; } - inline const Ordering & ordering() const { return ordering_; } + KeyInfoEntry() { + } + KeyInfoEntry(size_t idx, size_t d, Key start) : + Base(idx, d, start) { + } + size_t index() const { + return this->get<0>(); + } + size_t dim() const { + return this->get<1>(); + } + size_t colstart() const { + return this->get<2>(); + } +}; - protected: +/** + * Handy data structure for iterative solvers + */ +class GTSAM_EXPORT KeyInfo: public std::map { - void initialize(const GaussianFactorGraph &fg); +public: - Ordering ordering_; - size_t numCols_; + typedef std::map Base; - }; +protected: + Ordering ordering_; + size_t numCols_; -} + void initialize(const GaussianFactorGraph &fg); + +public: + + /// Default Constructor + KeyInfo() : + numCols_(0) { + } + + /// Construct from Gaussian factor graph, use "Natural" ordering + KeyInfo(const GaussianFactorGraph &fg); + + /// Construct from Gaussian factor graph and a given ordering + KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering); + + /// Return the total number of columns (scalar variables = sum of dimensions) + inline size_t numCols() const { + return numCols_; + } + + /// Return the ordering + inline const Ordering & ordering() const { + return ordering_; + } + + /// Return a vector of dimensions ordered by ordering() + std::vector colSpec() const; + + /// Return VectorValues with zeros, of correct dimension + VectorValues x0() const; + + /// Return zero Vector of correct dimension + Vector x0vector() const; + +}; + +} // \ namespace gtsam diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index a63bbf473..11025fc0f 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -365,32 +365,50 @@ void JacobianFactor::print(const string& s, /* ************************************************************************* */ // Check if two linear factors are equal bool JacobianFactor::equals(const GaussianFactor& f_, double tol) const { - if (!dynamic_cast(&f_)) + static const bool verbose = false; + if (!dynamic_cast(&f_)) { + if (verbose) + cout << "JacobianFactor::equals: Incorrect type" << endl; return false; - else { + } else { const JacobianFactor& f(static_cast(f_)); // Check keys - if (keys() != f.keys()) + if (keys() != f.keys()) { + if (verbose) + cout << "JacobianFactor::equals: keys do not match" << endl; return false; + } // Check noise model - if ((model_ && !f.model_) || (!model_ && f.model_)) + if ((model_ && !f.model_) || (!model_ && f.model_)) { + if (verbose) + cout << "JacobianFactor::equals: noise model mismatch" << endl; return false; - if (model_ && f.model_ && !model_->equals(*f.model_, tol)) + } + if (model_ && f.model_ && !model_->equals(*f.model_, tol)) { + if (verbose) + cout << "JacobianFactor::equals: noise modesl are not equal" << endl; return false; + } // Check matrix sizes - if (!(Ab_.rows() == f.Ab_.rows() && Ab_.cols() == f.Ab_.cols())) + if (!(Ab_.rows() == f.Ab_.rows() && Ab_.cols() == f.Ab_.cols())) { + if (verbose) + cout << "JacobianFactor::equals: augmented size mismatch" << endl; return false; + } // Check matrix contents constABlock Ab1(Ab_.range(0, Ab_.nBlocks())); constABlock Ab2(f.Ab_.range(0, f.Ab_.nBlocks())); for (size_t row = 0; row < (size_t) Ab1.rows(); ++row) if (!equal_with_abs_tol(Ab1.row(row), Ab2.row(row), tol) - && !equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol)) + && !equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol)) { + if (verbose) + cout << "JacobianFactor::equals: matrix mismatch at row " << row << endl; return false; + } return true; } @@ -461,22 +479,9 @@ VectorValues JacobianFactor::hessianDiagonal() const { } /* ************************************************************************* */ -// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n +// Raw memory access version should be called in Regular Factors only currently void JacobianFactor::hessianDiagonal(double* d) const { - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - - // Loop over all variables in the factor - for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { - // Get the diagonal block, and insert its diagonal - DVector dj; - for (size_t k = 0; k < 9; ++k) - dj(k) = Ab_(j).col(k).squaredNorm(); - - DMap(d + 9 * j) += dj; - } + throw std::runtime_error("JacobianFactor::hessianDiagonal raw memory access is allowed for Regular Factors only"); } /* ************************************************************************* */ @@ -528,40 +533,6 @@ void JacobianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, transposeMultiplyAdd(alpha, Ax, y); } -void JacobianFactor::multiplyHessianAdd(double alpha, const double* x, - double* y, std::vector keys) const { - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - - if (empty()) - return; - Vector Ax = zero(Ab_.rows()); - - // Just iterate over all A matrices and multiply in correct config part - for (size_t pos = 0; pos < size(); ++pos) - Ax += Ab_(pos) - * ConstDMap(x + keys[keys_[pos]], - keys[keys_[pos] + 1] - keys[keys_[pos]]); - - // Deal with noise properly, need to Double* whiten as we are dividing by variance - if (model_) { - model_->whitenInPlace(Ax); - model_->whitenInPlace(Ax); - } - - // multiply with alpha - Ax *= alpha; - - // Again iterate over all A matrices and insert Ai^e into y - for (size_t pos = 0; pos < size(); ++pos) - DMap(y + keys[keys_[pos]], keys[keys_[pos] + 1] - keys[keys_[pos]]) += Ab_( - pos).transpose() * Ax; - -} - /* ************************************************************************* */ VectorValues JacobianFactor::gradientAtZero() const { VectorValues g; @@ -574,8 +545,16 @@ VectorValues JacobianFactor::gradientAtZero() const { } /* ************************************************************************* */ +// Raw memory access version should be called in Regular Factors only currently void JacobianFactor::gradientAtZero(double* d) const { - //throw std::runtime_error("gradientAtZero not implemented for Jacobian factor"); + throw std::runtime_error("JacobianFactor::gradientAtZero raw memory access is allowed for Regular Factors only"); +} + +/* ************************************************************************* */ +Vector JacobianFactor::gradient(Key key, const VectorValues& x) const { + // TODO: optimize it for JacobianFactor without converting to a HessianFactor + HessianFactor hessian(*this); + return hessian.gradient(key, x); } /* ************************************************************************* */ diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 92f43b6be..194ba5c67 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -230,7 +230,9 @@ namespace gtsam { virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; } /** is noise model constrained ? */ - bool isConstrained() const { return model_->isConstrained(); } + bool isConstrained() const { + return model_ && model_->isConstrained(); + } /** Return the dimension of the variable pointed to by the given key iterator * todo: Remove this in favor of keeping track of dimensions with variables? @@ -281,16 +283,15 @@ namespace gtsam { /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; - void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) const; - - void multiplyHessianAdd(double alpha, const double* x, double* y) const {}; - /// A'*b for Jacobian VectorValues gradientAtZero() const; - /* ************************************************************************* */ + /// A'*b for Jacobian (raw memory version) virtual void gradientAtZero(double* d) const; + /// Compute the gradient wrt a key at any values + Vector gradient(Key key, const VectorValues& x) const; + /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ JacobianFactor whiten() const; @@ -348,14 +349,19 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(Ab_); ar & BOOST_SERIALIZATION_NVP(model_); } }; // JacobianFactor -} // gtsam +/// traits +template<> +struct traits : public Testable { +}; + +} // \ namespace gtsam #include diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 3b9c5d79c..a8b177b43 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -16,20 +16,20 @@ * @author Frank Dellaert */ -#include -#include -#include -#include +#include +#include #include #include #include #include -#include -#include +#include +#include +#include +#include +#include -static double inf = std::numeric_limits::infinity(); using namespace std; namespace gtsam { @@ -71,6 +71,11 @@ boost::optional checkIfDiagonal(const Matrix M) { } } +/* ************************************************************************* */ +Vector Base::sigmas() const { + throw("Base::sigmas: sigmas() not implemented for this noise model"); +} + /* ************************************************************************* */ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { size_t m = R.rows(), n = R.cols(); @@ -80,24 +85,25 @@ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { if (smart) diagonal = checkIfDiagonal(R); if (diagonal) - return Diagonal::Sigmas(reciprocal(*diagonal), true); + return Diagonal::Sigmas(diagonal->array().inverse(), true); else return shared_ptr(new Gaussian(R.rows(), R)); } /* ************************************************************************* */ -Gaussian::shared_ptr Gaussian::Information(const Matrix& M, bool smart) { - size_t m = M.rows(), n = M.cols(); +Gaussian::shared_ptr Gaussian::Information(const Matrix& information, bool smart) { + size_t m = information.rows(), n = information.cols(); if (m != n) throw invalid_argument("Gaussian::Information: R not square"); boost::optional diagonal = boost::none; if (smart) - diagonal = checkIfDiagonal(M); + diagonal = checkIfDiagonal(information); if (diagonal) return Diagonal::Precisions(*diagonal, true); else { - Matrix R = RtR(M); - return shared_ptr(new Gaussian(R.rows(), R)); + Eigen::LLT llt(information); + Matrix R = llt.matrixU(); + return shared_ptr(new Gaussian(n, R)); } } @@ -112,13 +118,15 @@ Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, variances = checkIfDiagonal(covariance); if (variances) return Diagonal::Variances(*variances, true); - else - return shared_ptr(new Gaussian(n, inverse_square_root(covariance))); + else { + // TODO: can we do this more efficiently and still get an upper triangular nmatrix?? + return Information(covariance.inverse(), false); + } } /* ************************************************************************* */ void Gaussian::print(const string& name) const { - gtsam::print(thisR(), "Gaussian"); + gtsam::print(thisR(), name + "Gaussian"); } /* ************************************************************************* */ @@ -130,6 +138,12 @@ bool Gaussian::equals(const Base& expected, double tol) const { return equal_with_abs_tol(R(), p->R(), sqrt(tol)); } +/* ************************************************************************* */ +Vector Gaussian::sigmas() const { + // TODO(frank): can this be done faster? + return Vector((thisR().transpose() * thisR()).inverse().diagonal()).cwiseSqrt(); +} + /* ************************************************************************* */ Vector Gaussian::whiten(const Vector& v) const { return thisR() * v; @@ -222,9 +236,11 @@ Diagonal::Diagonal() : } /* ************************************************************************* */ -Diagonal::Diagonal(const Vector& sigmas) : - Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(sigmas)), precisions_( - emul(invsigmas_, invsigmas_)) { +Diagonal::Diagonal(const Vector& sigmas) + : Gaussian(sigmas.size()), + sigmas_(sigmas), + invsigmas_(sigmas.array().inverse()), + precisions_(invsigmas_.array().square()) { } /* ************************************************************************* */ @@ -263,12 +279,12 @@ void Diagonal::print(const string& name) const { /* ************************************************************************* */ Vector Diagonal::whiten(const Vector& v) const { - return emul(v, invsigmas()); + return v.cwiseProduct(invsigmas_); } /* ************************************************************************* */ Vector Diagonal::unwhiten(const Vector& v) const { - return emul(v, sigmas_); + return v.cwiseProduct(sigmas_); } /* ************************************************************************* */ @@ -290,42 +306,42 @@ void Diagonal::WhitenInPlace(Eigen::Block H) const { // Constrained /* ************************************************************************* */ +namespace internal { +// switch precisions and invsigmas to finite value +// TODO: why?? And, why not just ask s==0.0 below ? +static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) { + for (Vector::Index i = 0; i < sigmas.size(); ++i) + if (!std::isfinite(1. / sigmas[i])) { + precisions[i] = 0.0; + invsigmas[i] = 0.0; + } +} +} + /* ************************************************************************* */ Constrained::Constrained(const Vector& sigmas) : Diagonal(sigmas), mu_(repeat(sigmas.size(), 1000.0)) { - for (int i=0; i H) const { - // selective scaling - // Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas) - // Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0, - // indicating a hard constraint, we leave H's row i in place. - const Vector& _invsigmas = invsigmas(); - for(DenseIndex row = 0; row < _invsigmas.size(); ++row) - if(isfinite(_invsigmas(row))) - H.row(row) *= _invsigmas(row); + for (DenseIndex i=0; i<(DenseIndex)dim_; ++i) + if (!constrained(i)) // if constrained, leave row of H as is + H.row(i) *= invsigmas_(i); } /* ************************************************************************* */ Constrained::shared_ptr Constrained::unit() const { Vector sigmas = ones(dim()); for (size_t i=0; isigmas_(i) == 0.0) + if (constrained(i)) sigmas(i) = 0.0; return MixedSigmas(mu_, sigmas); } @@ -419,8 +421,8 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const { list Rd; Vector pseudo(m); // allocate storage for pseudo-inverse - Vector invsigmas = reciprocal(sigmas_); - Vector weights = emul(invsigmas,invsigmas); // calculate weights once + Vector invsigmas = sigmas_.array().inverse(); + Vector weights = invsigmas.array().square(); // calculate weights once // We loop over all columns, because the columns that can be eliminated // are not necessarily contiguous. For each one, estimate the corresponding @@ -472,7 +474,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const { const size_t& j = t.get<0>(); const Vector& rd = t.get<1>(); precisions(i) = t.get<2>(); - if (precisions(i)==inf) mixed = true; + if (constrained(i)) mixed = true; for (size_t j2=0; j2 &A, Vector &error) const { BOOST_FOREACH(Matrix& Aj, A) { vector_scale_inplace(W,Aj); } - error = emul(W, error); + error = W.cwiseProduct(error); } } @@ -608,7 +599,7 @@ void Base::reweight(Matrix &A, Vector &error) const { else { const Vector W = sqrtWeight(error); vector_scale_inplace(W,A); - error = emul(W, error); + error = W.cwiseProduct(error); } } @@ -624,7 +615,7 @@ void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const { const Vector W = sqrtWeight(error); vector_scale_inplace(W,A1); vector_scale_inplace(W,A2); - error = emul(W, error); + error = W.cwiseProduct(error); } } @@ -642,7 +633,7 @@ void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const { vector_scale_inplace(W,A1); vector_scale_inplace(W,A2); vector_scale_inplace(W,A3); - error = emul(W, error); + error = W.cwiseProduct(error); } } @@ -656,7 +647,7 @@ void Null::print(const std::string &s="") const Null::shared_ptr Null::Create() { return shared_ptr(new Null()); } -Fair::Fair(const double c, const ReweightScheme reweight) +Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { if ( c_ <= 0 ) { cout << "mEstimator Fair takes only positive double in constructor. forced to 1.0" << endl; @@ -668,26 +659,26 @@ Fair::Fair(const double c, const ReweightScheme reweight) // Fair /* ************************************************************************* */ -double Fair::weight(const double &error) const +double Fair::weight(double error) const { return 1.0 / (1.0 + fabs(error)/c_); } void Fair::print(const std::string &s="") const { cout << s << "fair (" << c_ << ")" << endl; } -bool Fair::equals(const Base &expected, const double tol) const { +bool Fair::equals(const Base &expected, double tol) const { const Fair* p = dynamic_cast (&expected); if (p == NULL) return false; return fabs(c_ - p->c_ ) < tol; } -Fair::shared_ptr Fair::Create(const double c, const ReweightScheme reweight) +Fair::shared_ptr Fair::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Fair(c, reweight)); } /* ************************************************************************* */ // Huber /* ************************************************************************* */ -Huber::Huber(const double k, const ReweightScheme reweight) +Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { if ( k_ <= 0 ) { cout << "mEstimator Huber takes only positive double in constructor. forced to 1.0" << endl; @@ -695,7 +686,7 @@ Huber::Huber(const double k, const ReweightScheme reweight) } } -double Huber::weight(const double &error) const { +double Huber::weight(double error) const { return (error < k_) ? (1.0) : (k_ / fabs(error)); } @@ -703,13 +694,13 @@ void Huber::print(const std::string &s="") const { cout << s << "huber (" << k_ << ")" << endl; } -bool Huber::equals(const Base &expected, const double tol) const { +bool Huber::equals(const Base &expected, double tol) const { const Huber* p = dynamic_cast(&expected); if (p == NULL) return false; return fabs(k_ - p->k_) < tol; } -Huber::shared_ptr Huber::Create(const double c, const ReweightScheme reweight) { +Huber::shared_ptr Huber::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Huber(c, reweight)); } @@ -717,7 +708,7 @@ Huber::shared_ptr Huber::Create(const double c, const ReweightScheme reweight) { // Cauchy /* ************************************************************************* */ -Cauchy::Cauchy(const double k, const ReweightScheme reweight) +Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { if ( k_ <= 0 ) { cout << "mEstimator Cauchy takes only positive double in constructor. forced to 1.0" << endl; @@ -725,7 +716,7 @@ Cauchy::Cauchy(const double k, const ReweightScheme reweight) } } -double Cauchy::weight(const double &error) const { +double Cauchy::weight(double error) const { return k_*k_ / (k_*k_ + error*error); } @@ -733,24 +724,24 @@ void Cauchy::print(const std::string &s="") const { cout << s << "cauchy (" << k_ << ")" << endl; } -bool Cauchy::equals(const Base &expected, const double tol) const { +bool Cauchy::equals(const Base &expected, double tol) const { const Cauchy* p = dynamic_cast(&expected); if (p == NULL) return false; return fabs(k_ - p->k_) < tol; } -Cauchy::shared_ptr Cauchy::Create(const double c, const ReweightScheme reweight) { +Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Cauchy(c, reweight)); } /* ************************************************************************* */ // Tukey /* ************************************************************************* */ -Tukey::Tukey(const double c, const ReweightScheme reweight) +Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { } -double Tukey::weight(const double &error) const { +double Tukey::weight(double error) const { if (fabs(error) <= c_) { double xc2 = (error/c_)*(error/c_); double one_xc22 = (1.0-xc2)*(1.0-xc2); @@ -763,24 +754,24 @@ void Tukey::print(const std::string &s="") const { std::cout << s << ": Tukey (" << c_ << ")" << std::endl; } -bool Tukey::equals(const Base &expected, const double tol) const { +bool Tukey::equals(const Base &expected, double tol) const { const Tukey* p = dynamic_cast(&expected); if (p == NULL) return false; return fabs(c_ - p->c_) < tol; } -Tukey::shared_ptr Tukey::Create(const double c, const ReweightScheme reweight) { +Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Tukey(c, reweight)); } /* ************************************************************************* */ // Welsh /* ************************************************************************* */ -Welsh::Welsh(const double c, const ReweightScheme reweight) +Welsh::Welsh(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { } -double Welsh::weight(const double &error) const { +double Welsh::weight(double error) const { double xc2 = (error/c_)*(error/c_); return std::exp(-xc2); } @@ -789,13 +780,13 @@ void Welsh::print(const std::string &s="") const { std::cout << s << ": Welsh (" << c_ << ")" << std::endl; } -bool Welsh::equals(const Base &expected, const double tol) const { +bool Welsh::equals(const Base &expected, double tol) const { const Welsh* p = dynamic_cast(&expected); if (p == NULL) return false; return fabs(c_ - p->c_) < tol; } -Welsh::shared_ptr Welsh::Create(const double c, const ReweightScheme reweight) { +Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Welsh(c, reweight)); } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index caad6b01a..923e7c7e9 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -18,13 +18,14 @@ #pragma once +#include +#include + #include #include #include #include #include -#include -#include namespace gtsam { @@ -57,13 +58,13 @@ namespace gtsam { public: - /** primary constructor @param dim is the dimension of the model */ + /// primary constructor @param dim is the dimension of the model Base(size_t dim = 1):dim_(dim) {} virtual ~Base() {} - /** true if a constrained noise mode, saves slow/clumsy dynamic casting */ - virtual bool is_constrained() const { - return false; + /// true if a constrained noise mode, saves slow/clumsy dynamic casting + virtual bool isConstrained() const { + return false; // default false } /// Dimensionality @@ -73,14 +74,13 @@ namespace gtsam { virtual bool equals(const Base& expected, double tol=1e-9) const = 0; - /** - * Whiten an error vector. - */ + /// Calculate standard deviations + virtual Vector sigmas() const; + + /// Whiten an error vector. virtual Vector whiten(const Vector& v) const = 0; - /** - * Unwhiten an error vector. - */ + /// Unwhiten an error vector. virtual Vector unwhiten(const Vector& v) const = 0; virtual double distance(const Vector& v) const = 0; @@ -114,7 +114,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(dim_); } }; @@ -187,6 +187,7 @@ namespace gtsam { virtual void print(const std::string& name) const; virtual bool equals(const Base& expected, double tol=1e-9) const; + virtual Vector sigmas() const; virtual Vector whiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const; @@ -218,9 +219,9 @@ namespace gtsam { /** * Whiten a system, in place as well */ - virtual void WhitenSystem(std::vector& A, Vector& b) const ; - virtual void WhitenSystem(Matrix& A, Vector& b) const ; - virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const ; + virtual void WhitenSystem(std::vector& A, Vector& b) const; + virtual void WhitenSystem(Matrix& A, Vector& b) const; + virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const; virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const; /** @@ -232,22 +233,20 @@ namespace gtsam { */ virtual boost::shared_ptr QR(Matrix& Ab) const; - /** - * Return R itself, but note that Whiten(H) is cheaper than R*H - */ + /// Return R itself, but note that Whiten(H) is cheaper than R*H virtual Matrix R() const { return thisR();} - /** - * Simple check for constrained-ness - * FIXME Find a better way of handling this - */ - virtual bool isConstrained() const {return false;} + /// Compute information matrix + virtual Matrix information() const { return thisR().transpose() * thisR(); } + + /// Compute covariance matrix + virtual Matrix covariance() const { return information().inverse(); } private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(sqrt_information_); } @@ -307,6 +306,7 @@ namespace gtsam { } virtual void print(const std::string& name) const; + virtual Vector sigmas() const { return sigmas_; } virtual Vector whiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const; virtual Matrix Whiten(const Matrix& H) const; @@ -316,7 +316,6 @@ namespace gtsam { /** * Return standard deviations (sqrt of diagonal) */ - inline const Vector& sigmas() const { return sigmas_; } inline double sigma(size_t i) const { return sigmas_(i); } /** @@ -342,7 +341,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Gaussian); ar & BOOST_SERIALIZATION_NVP(sigmas_); ar & BOOST_SERIALIZATION_NVP(invsigmas_); @@ -390,11 +389,14 @@ namespace gtsam { virtual ~Constrained() {} - /** true if a constrained noise mode, saves slow/clumsy dynamic casting */ - virtual bool is_constrained() const { + /// true if a constrained noise mode, saves slow/clumsy dynamic casting + virtual bool isConstrained() const { return true; } + /// Return true if a particular dimension is free or constrained + bool constrained(size_t i) const; + /// Access mu as a vector const Vector& mu() const { return mu_; } @@ -402,24 +404,22 @@ namespace gtsam { * A diagonal noise model created by specifying a Vector of * standard devations, some of which might be zero */ - static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas, - bool smart = true); + static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas); /** * A diagonal noise model created by specifying a Vector of * standard devations, some of which might be zero */ - static shared_ptr MixedSigmas(const Vector& sigmas, bool smart = true) { - return MixedSigmas(repeat(sigmas.size(), 1000.0), sigmas, smart); + static shared_ptr MixedSigmas(const Vector& sigmas) { + return MixedSigmas(repeat(sigmas.size(), 1000.0), sigmas); } /** * A diagonal noise model created by specifying a Vector of * standard devations, some of which might be zero */ - static shared_ptr MixedSigmas(double m, const Vector& sigmas, - bool smart = true) { - return MixedSigmas(repeat(sigmas.size(), m), sigmas, smart); + static shared_ptr MixedSigmas(double m, const Vector& sigmas) { + return MixedSigmas(repeat(sigmas.size(), m), sigmas); } /** @@ -482,12 +482,6 @@ namespace gtsam { */ virtual Diagonal::shared_ptr QR(Matrix& Ab) const; - /** - * Check constrained is always true - * FIXME Find a better way of handling this - */ - virtual bool isConstrained() const { return true; } - /** * Returns a Unit version of a constrained noisemodel in which * constrained sigmas remain constrained and the rest are unit scaled @@ -498,7 +492,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal); ar & BOOST_SERIALIZATION_NVP(mu_); } @@ -565,7 +559,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal); ar & BOOST_SERIALIZATION_NVP(sigma_); ar & BOOST_SERIALIZATION_NVP(invsigma_); @@ -601,18 +595,18 @@ namespace gtsam { virtual Vector whiten(const Vector& v) const { return v; } virtual Vector unwhiten(const Vector& v) const { return v; } virtual Matrix Whiten(const Matrix& H) const { return H; } - virtual void WhitenInPlace(Matrix& H) const {} - virtual void WhitenInPlace(Eigen::Block H) const {} - virtual void whitenInPlace(Vector& v) const {} - virtual void unwhitenInPlace(Vector& v) const {} - virtual void whitenInPlace(Eigen::Block& v) const {} - virtual void unwhitenInPlace(Eigen::Block& v) const {} + virtual void WhitenInPlace(Matrix& /*H*/) const {} + virtual void WhitenInPlace(Eigen::Block /*H*/) const {} + virtual void whitenInPlace(Vector& /*v*/) const {} + virtual void unwhitenInPlace(Vector& /*v*/) const {} + virtual void whitenInPlace(Eigen::Block& /*v*/) const {} + virtual void unwhitenInPlace(Eigen::Block& /*v*/) const {} private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Isotropic); } }; @@ -638,20 +632,23 @@ namespace gtsam { virtual ~Base() {} /// robust error function to implement - virtual double weight(const double &error) const = 0; + virtual double weight(double error) const = 0; virtual void print(const std::string &s) const = 0; - virtual bool equals(const Base& expected, const double tol=1e-8) const = 0; + virtual bool equals(const Base& expected, double tol=1e-8) const = 0; - inline double sqrtWeight(const double &error) const - { return std::sqrt(weight(error)); } + double sqrtWeight(double error) const { + return std::sqrt(weight(error)); + } /** produce a weight vector according to an error vector and the implemented * robust function */ Vector weight(const Vector &error) const; /** square root version of the weight function */ - Vector sqrtWeight(const Vector &error) const; + Vector sqrtWeight(const Vector &error) const { + return weight(error).cwiseSqrt(); + } /** reweight block matrices and a vector according to their weight implementation */ void reweight(Vector &error) const; @@ -664,7 +661,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(reweight_); } }; @@ -676,16 +673,16 @@ namespace gtsam { Null(const ReweightScheme reweight = Block) : Base(reweight) {} virtual ~Null() {} - virtual double weight(const double &error) const { return 1.0; } + virtual double weight(double /*error*/) const { return 1.0; } virtual void print(const std::string &s) const; - virtual bool equals(const Base& expected, const double tol=1e-8) const { return true; } + virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; } static shared_ptr Create() ; private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; @@ -695,12 +692,12 @@ namespace gtsam { public: typedef boost::shared_ptr shared_ptr; - Fair(const double c = 1.3998, const ReweightScheme reweight = Block); + Fair(double c = 1.3998, const ReweightScheme reweight = Block); virtual ~Fair() {} - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double c, const ReweightScheme reweight = Block) ; + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double c, const ReweightScheme reweight = Block) ; protected: double c_; @@ -709,7 +706,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(c_); } @@ -721,11 +718,11 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; virtual ~Huber() {} - Huber(const double k = 1.345, const ReweightScheme reweight = Block); - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; + Huber(double k = 1.345, const ReweightScheme reweight = Block); + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; protected: double k_; @@ -734,7 +731,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(k_); } @@ -750,11 +747,11 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; virtual ~Cauchy() {} - Cauchy(const double k = 0.1, const ReweightScheme reweight = Block); - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; + Cauchy(double k = 0.1, const ReweightScheme reweight = Block); + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; protected: double k_; @@ -763,7 +760,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(k_); } @@ -774,12 +771,12 @@ namespace gtsam { public: typedef boost::shared_ptr shared_ptr; - Tukey(const double c = 4.6851, const ReweightScheme reweight = Block); + Tukey(double c = 4.6851, const ReweightScheme reweight = Block); virtual ~Tukey() {} - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; protected: double c_; @@ -788,7 +785,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(c_); } @@ -799,12 +796,12 @@ namespace gtsam { public: typedef boost::shared_ptr shared_ptr; - Welsh(const double c = 2.9846, const ReweightScheme reweight = Block); + Welsh(double c = 2.9846, const ReweightScheme reweight = Block); virtual ~Welsh() {} - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; protected: double c_; @@ -813,7 +810,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(c_); } @@ -857,7 +854,7 @@ namespace gtsam { // TODO: functions below are dummy but necessary for the noiseModel::Base inline virtual Vector whiten(const Vector& v) const { Vector r = v; this->WhitenSystem(r); return r; } - inline virtual Vector unwhiten(const Vector& v) const + inline virtual Vector unwhiten(const Vector& /*v*/) const { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); } inline virtual double distance(const Vector& v) const { return this->whiten(v).squaredNorm(); } @@ -876,7 +873,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & boost::serialization::make_nvp("robust_", const_cast(robust_)); ar & boost::serialization::make_nvp("noise_", const_cast(noise_)); @@ -895,7 +892,15 @@ namespace gtsam { typedef noiseModel::Gaussian::shared_ptr SharedGaussian; typedef noiseModel::Diagonal::shared_ptr SharedDiagonal; typedef noiseModel::Constrained::shared_ptr SharedConstrained; + typedef noiseModel::Isotropic::shared_ptr SharedIsotropic; -} // namespace gtsam + /// traits + template<> struct traits : public Testable {}; + template<> struct traits : public Testable {}; + template<> struct traits : public Testable {}; + template<> struct traits : public Testable {}; + template<> struct traits : public Testable {}; + +} //\ namespace gtsam diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 27eb57b44..caf7d0095 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -1,22 +1,31 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + /* - * PCGSolver.cpp - * - * Created on: Feb 14, 2012 - * Author: ydjian + * @file PCGSolver.cpp + * @brief Preconditioned Conjugate Gradient Solver for linear systems + * @date Feb 14, 2012 + * @author Yong-Dian Jian + * @author Sungtae An */ -#include -//#include -//#include -//#include -//#include #include +#include #include -//#include -//#include -//#include -//#include +#include + +#include #include + +#include #include #include @@ -27,40 +36,38 @@ namespace gtsam { /*****************************************************************************/ void PCGSolverParameters::print(ostream &os) const { Base::print(os); - os << "PCGSolverParameters:" << endl; + os << "PCGSolverParameters:" << endl; preconditioner_->print(os); } /*****************************************************************************/ PCGSolver::PCGSolver(const PCGSolverParameters &p) { + parameters_ = p; preconditioner_ = createPreconditioner(p.preconditioner_); } /*****************************************************************************/ -VectorValues PCGSolver::optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda, - const VectorValues &initial) -{ +VectorValues PCGSolver::optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial) { /* build preconditioner */ preconditioner_->build(gfg, keyInfo, lambda); /* apply pcg */ - const Vector sol = preconditionedConjugateGradient( - GaussianFactorGraphSystem(gfg, *preconditioner_, keyInfo, lambda), - initial.vector(keyInfo.ordering()), parameters_); + GaussianFactorGraphSystem system(gfg, *preconditioner_, keyInfo, lambda); + Vector x0 = initial.vector(keyInfo.ordering()); + const Vector sol = preconditionedConjugateGradient(system, x0, parameters_); return buildVectorValues(sol, keyInfo); } /*****************************************************************************/ GaussianFactorGraphSystem::GaussianFactorGraphSystem( - const GaussianFactorGraph &gfg, - const Preconditioner &preconditioner, - const KeyInfo &keyInfo, - const std::map &lambda) - : gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_(lambda) {} + const GaussianFactorGraph &gfg, const Preconditioner &preconditioner, + const KeyInfo &keyInfo, const std::map &lambda) : + gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_( + lambda) { +} /*****************************************************************************/ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const { @@ -72,114 +79,65 @@ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const { /* substract A*x */ Vector Ax = Vector::Zero(r.rows(), 1); multiply(x, Ax); - r -= Ax ; + r -= Ax; } /*****************************************************************************/ -void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& Ax) const { - /* implement Ax, assume x and Ax are pre-allocated */ +void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const { + /* implement A^T*(A*x), assume x and AtAx are pre-allocated */ - /* reset y */ - Ax.setZero(); + // Build a VectorValues for Vector x + VectorValues vvX = buildVectorValues(x, keyInfo_); - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) { - if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { - /* accumulate At A x*/ - for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { - const Matrix Ai = jf->getA(it); - /* this map lookup should be replaced */ - const KeyInfoEntry &entry = keyInfo_.find(*it)->second; - Ax.segment(entry.colstart(), entry.dim()) - += Ai.transpose() * (Ai * x.segment(entry.colstart(), entry.dim())); - } - } - else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { - /* accumulate H x */ + // VectorValues form of A'Ax for multiplyHessianAdd + VectorValues vvAtAx; - /* use buffer to avoid excessive table lookups */ - const size_t sz = hf->size(); - vector y; - y.reserve(sz); - for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) { - /* initialize y to zeros */ - y.push_back(zero(hf->getDim(it))); - } + // vvAtAx += 1.0 * A'Ax for each factor + gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx); - for (HessianFactor::const_iterator j = hf->begin(); j != hf->end(); j++ ) { - /* retrieve the key mapping */ - const KeyInfoEntry &entry = keyInfo_.find(*j)->second; - // xj is the input vector - const Vector xj = x.segment(entry.colstart(), entry.dim()); - size_t idx = 0; - for (HessianFactor::const_iterator i = hf->begin(); i != hf->end(); i++, idx++ ) { - if ( i == j ) y[idx] += hf->info(j, j).selfadjointView() * xj; - else y[idx] += hf->info(i, j).knownOffDiagonal() * xj; - } - } - - /* accumulate to r */ - for(DenseIndex i = 0; i < (DenseIndex) sz; ++i) { - /* retrieve the key mapping */ - const KeyInfoEntry &entry = keyInfo_.find(hf->keys()[i])->second; - Ax.segment(entry.colstart(), entry.dim()) += y[i]; - } - } - else { - throw invalid_argument("GaussianFactorGraphSystem::multiply gfg contains a factor that is neither a JacobianFactor nor a HessianFactor."); - } - } + // Make the result as Vector form + AtAx = vvAtAx.vector(keyInfo_.ordering()); } /*****************************************************************************/ void GaussianFactorGraphSystem::getb(Vector &b) const { /* compute rhs, assume b pre-allocated */ - /* reset */ - b.setZero(); + // Get whitened r.h.s (A^T * b) from each factor in the form of VectorValues + VectorValues vvb = gfg_.gradientAtZero(); - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) { - if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { - const Vector rhs = jf->getb(); - /* accumulate At rhs */ - for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { - /* this map lookup should be replaced */ - const KeyInfoEntry &entry = keyInfo_.find(*it)->second; - b.segment(entry.colstart(), entry.dim()) += jf->getA(it).transpose() * rhs ; - } - } - else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { - /* accumulate g */ - for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) { - const KeyInfoEntry &entry = keyInfo_.find(*it)->second; - b.segment(entry.colstart(), entry.dim()) += hf->linearTerm(it); - } - } - else { - throw invalid_argument("GaussianFactorGraphSystem::getb gfg contains a factor that is neither a JacobianFactor nor a HessianFactor."); - } - } + // Make the result as Vector form + b = -vvb.vector(keyInfo_.ordering()); } /**********************************************************************************/ -void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const -{ preconditioner_.transposeSolve(x, y); } +void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, + Vector &y) const { + // For a preconditioner M = L*L^T + // Calculate y = L^{-1} x + preconditioner_.solve(x, y); +} /**********************************************************************************/ -void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const -{ preconditioner_.solve(x, y); } +void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, + Vector &y) const { + // For a preconditioner M = L*L^T + // Calculate y = L^{-T} x + preconditioner_.transposeSolve(x, y); +} /**********************************************************************************/ -VectorValues buildVectorValues(const Vector &v, - const Ordering &ordering, - const map & dimensions) { +VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, + const map & dimensions) { VectorValues result; DenseIndex offset = 0; - for ( size_t i = 0 ; i < ordering.size() ; ++i ) { + for (size_t i = 0; i < ordering.size(); ++i) { const Key &key = ordering[i]; map::const_iterator it = dimensions.find(key); - if ( it == dimensions.end() ) { - throw invalid_argument("buildVectorValues: inconsistent ordering and dimensions"); + if (it == dimensions.end()) { + throw invalid_argument( + "buildVectorValues: inconsistent ordering and dimensions"); } const size_t dim = it->second; result.insert(key, v.segment(offset, dim)); @@ -193,7 +151,8 @@ VectorValues buildVectorValues(const Vector &v, VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) { VectorValues result; BOOST_FOREACH ( const KeyInfo::value_type &item, keyInfo ) { - result.insert(item.first, v.segment(item.second.colstart(), item.second.dim())); + result.insert(item.first, + v.segment(item.second.colstart(), item.second.dim())); } return result; } diff --git a/gtsam/linear/PCGSolver.h b/gtsam/linear/PCGSolver.h index 0201000ad..f5b278ae5 100644 --- a/gtsam/linear/PCGSolver.h +++ b/gtsam/linear/PCGSolver.h @@ -1,20 +1,25 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + /* - * PCGSolver.h - * - * Created on: Jan 31, 2012 - * Author: Yong-Dian Jian + * @file PCGSolver.h + * @brief Preconditioned Conjugate Gradient Solver for linear systems + * @date Jan 31, 2012 + * @author Yong-Dian Jian + * @author Sungtae An */ #pragma once -#include #include -#include -#include - -#include -#include -#include #include namespace gtsam { @@ -22,15 +27,19 @@ namespace gtsam { class GaussianFactorGraph; class KeyInfo; class Preconditioner; +class VectorValues; struct PreconditionerParameters; -/*****************************************************************************/ +/** + * Parameters for PCG + */ struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters { public: typedef ConjugateGradientParameters Base; typedef boost::shared_ptr shared_ptr; - PCGSolverParameters() {} + PCGSolverParameters() { + } virtual void print(std::ostream &os) const; @@ -42,8 +51,9 @@ public: boost::shared_ptr preconditioner_; }; -/*****************************************************************************/ -/* A virtual base class for the preconditioned conjugate gradient solver */ +/** + * A virtual base class for the preconditioned conjugate gradient solver + */ class GTSAM_EXPORT PCGSolver: public IterativeSolver { public: typedef IterativeSolver Base; @@ -57,7 +67,8 @@ protected: public: /* Interface to initialize a solver without a problem */ PCGSolver(const PCGSolverParameters &p); - virtual ~PCGSolver() {} + virtual ~PCGSolver() { + } using IterativeSolver::optimize; @@ -67,7 +78,9 @@ public: }; -/*****************************************************************************/ +/** + * System class needed for calling preconditionedConjugateGradient + */ class GTSAM_EXPORT GaussianFactorGraphSystem { public: @@ -97,13 +110,17 @@ public: void getb(Vector &b) const; }; -/* utility functions */ -/**********************************************************************************/ +/// @name utility functions +/// @{ + +/// Create VectorValues from a Vector VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, const std::map & dimensions); -/**********************************************************************************/ +/// Create VectorValues from a Vector and a KeyInfo class VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo); +/// @} + } diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index 9af362fba..538d886b4 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -2,7 +2,8 @@ * Preconditioner.cpp * * Created on: Jun 2, 2014 - * Author: ydjian + * Author: Yong-Dian Jian + * Author: Sungtae An */ #include @@ -94,7 +95,7 @@ void BlockJacobiPreconditioner::solve(const Vector& y, Vector &x) const { const Eigen::Map R(ptr, d, d); Eigen::Map b(dst, d, 1); - R.triangularView().solveInPlace(b); + R.triangularView().solveInPlace(b); dst += d; ptr += sz; @@ -125,7 +126,9 @@ void BlockJacobiPreconditioner::transposeSolve(const Vector& y, Vector& x) const void BlockJacobiPreconditioner::build( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda) { + // n is the number of keys const size_t n = keyInfo.size(); + // dims_ is a vector that contains the dimension of keys dims_ = keyInfo.colSpec(); /* prepare the buffer of block diagonals */ @@ -141,11 +144,9 @@ void BlockJacobiPreconditioner::build( } /* getting the block diagonals over the factors */ - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { - std::map hessianMap = gf->hessianBlockDiagonal(); - BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values) - blocks.push_back(hessian); - } + std::map hessianMap =gfg.hessianBlockDiagonal(); + BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values) + blocks.push_back(hessian); /* if necessary, allocating the memory for cacheing the factorization results */ if ( nnz > bufferSize_ ) { @@ -159,11 +160,12 @@ void BlockJacobiPreconditioner::build( double *ptr = buffer_; for ( size_t i = 0 ; i < n ; ++i ) { /* use eigen to decompose Di */ - const Matrix R = blocks[i].llt().matrixL().transpose(); + /* It is same as L = chol(M,'lower') in MATLAB where M is full preconditioner */ + const Matrix L = blocks[i].llt().matrixL(); /* store the data in the buffer */ size_t sz = dims_[i]*dims_[i] ; - std::copy(R.data(), R.data() + sz, ptr); + std::copy(L.data(), L.data() + sz, ptr); /* advance the pointer */ ptr += sz; diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index 10ceb78a9..623b29863 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -2,7 +2,8 @@ * Preconditioner.h * * Created on: Jun 2, 2014 - * Author: ydjian + * Author: Yong-Dian Jian + * Author: Sungtae An */ #pragma once @@ -57,7 +58,8 @@ struct GTSAM_EXPORT PreconditionerParameters { }; /* PCG aims to solve the problem: A x = b by reparametrizing it as - * S^t A S y = S^t b or M A x = M b, where A \approx S S, or A \approx M + * L^{-1} A L^{-T} y = L^{-1} b or M^{-1} A x = M^{-1} b, + * where A \approx L L^{T}, or A \approx M * The goal of this class is to provide a general interface to all preconditioners */ class GTSAM_EXPORT Preconditioner { public: @@ -70,15 +72,15 @@ public: /* Computation Interfaces */ - /* implement x = S^{-1} y */ + /* implement x = L^{-1} y */ virtual void solve(const Vector& y, Vector &x) const = 0; // virtual void solve(const VectorValues& y, VectorValues &x) const = 0; - /* implement x = S^{-T} y */ + /* implement x = L^{-T} y */ virtual void transposeSolve(const Vector& y, Vector& x) const = 0; // virtual void transposeSolve(const VectorValues& y, VectorValues &x) const = 0; -// /* implement x = S^{-1} S^{-T} y */ +// /* implement x = L^{-1} L^{-T} y */ // virtual void fullSolve(const Vector& y, Vector &x) const = 0; // virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0; diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index 86bee2188..350963bcf 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -44,7 +44,7 @@ namespace gtsam { private: friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(index_); ar & BOOST_SERIALIZATION_NVP(weight_); } @@ -70,7 +70,7 @@ namespace gtsam { Subgraph(const std::vector &indices) ; inline const Edges& edges() const { return edges_; } - inline const size_t size() const { return edges_.size(); } + inline size_t size() const { return edges_.size(); } EdgeIndices edgeIndices() const; iterator begin() { return edges_.begin(); } @@ -85,7 +85,7 @@ namespace gtsam { private: friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(edges_); } }; diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 6f10d04ad..3c7256c29 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -9,73 +9,81 @@ * -------------------------------------------------------------------------- */ -#include -#include +/** + * @file SubgraphSolver.cpp + * @brief Subgraph Solver from IROS 2010 + * @date 2010 + * @author Frank Dellaert + * @author Yong Dian Jian + */ + +#include #include #include #include #include -#include -#include -#include #include -#include -#include -#include using namespace std; namespace gtsam { /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters, const Ordering& ordering) - : parameters_(parameters), ordering_(ordering) -{ +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, + const Parameters ¶meters, const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { initialize(gfg); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters ¶meters, const Ordering& ordering) - : parameters_(parameters), ordering_(ordering) -{ +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, + const Parameters ¶meters, const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { initialize(*jfg); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters, const Ordering& ordering) - : parameters_(parameters), ordering_(ordering) { +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, + const GaussianFactorGraph &Ab2, const Parameters ¶meters, + const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { - GaussianBayesNet::shared_ptr Rc1 = Ab1.eliminateSequential(ordering_, EliminateQR); + GaussianBayesNet::shared_ptr Rc1 = Ab1.eliminateSequential(ordering_, + EliminateQR); initialize(Rc1, boost::make_shared(Ab2)); } /**************************************************************************************************/ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, - const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering) - : parameters_(parameters), ordering_(ordering) { + const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, + const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { - GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, EliminateQR); + GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, + EliminateQR); initialize(Rc1, Ab2); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, - const Parameters ¶meters, const Ordering& ordering) : parameters_(parameters), ordering_(ordering) -{ +SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, + const GaussianFactorGraph &Ab2, const Parameters ¶meters, + const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { initialize(Rc1, boost::make_shared(Ab2)); } /**************************************************************************************************/ SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, - const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering) : -parameters_(parameters), ordering_(ordering) -{ + const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, + const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { initialize(Rc1, Ab2); } /**************************************************************************************************/ VectorValues SubgraphSolver::optimize() { - VectorValues ybar = conjugateGradients(*pc_, pc_->zero(), parameters_); + VectorValues ybar = conjugateGradients(*pc_, pc_->zero(), parameters_); return pc_->x(ybar); } @@ -86,29 +94,33 @@ VectorValues SubgraphSolver::optimize(const VectorValues &initial) { } /**************************************************************************************************/ -void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) -{ - GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared(), - Ab2 = boost::make_shared(); +void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) { + GaussianFactorGraph::shared_ptr Ab1 = + boost::make_shared(), Ab2 = boost::make_shared< + GaussianFactorGraph>(); - boost::tie(Ab1, Ab2) = splitGraph(jfg) ; + boost::tie(Ab1, Ab2) = splitGraph(jfg); if (parameters_.verbosity()) - cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() << " factors" << endl; + cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() + << " factors" << endl; - GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, EliminateQR); - VectorValues::shared_ptr xbar = boost::make_shared(Rc1->optimize()); + GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, + EliminateQR); + VectorValues::shared_ptr xbar = boost::make_shared( + Rc1->optimize()); pc_ = boost::make_shared(Ab2, Rc1, xbar); } /**************************************************************************************************/ -void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2) -{ - VectorValues::shared_ptr xbar = boost::make_shared(Rc1->optimize()); +void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, + const GaussianFactorGraph::shared_ptr &Ab2) { + VectorValues::shared_ptr xbar = boost::make_shared( + Rc1->optimize()); pc_ = boost::make_shared(Ab2, Rc1, xbar); } /**************************************************************************************************/ -boost::tuple +boost::tuple // SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { const VariableIndex index(jfg); @@ -116,39 +128,43 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { DSFVector D(n); GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); - GaussianFactorGraph::shared_ptr Ac( new GaussianFactorGraph()); + GaussianFactorGraph::shared_ptr Ac(new GaussianFactorGraph()); size_t t = 0; BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) { - if ( gf->keys().size() > 2 ) { - throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed "); + if (gf->keys().size() > 2) { + throw runtime_error( + "SubgraphSolver::splitGraph the graph is not simple, sanity check failed "); } - bool augment = false ; + bool augment = false; /* check whether this factor should be augmented to the "tree" graph */ - if ( gf->keys().size() == 1 ) augment = true; + if (gf->keys().size() == 1) + augment = true; else { - const Key u = gf->keys()[0], v = gf->keys()[1], - u_root = D.findSet(u), v_root = D.findSet(v); - if ( u_root != v_root ) { - t++; augment = true ; + const Key u = gf->keys()[0], v = gf->keys()[1], u_root = D.findSet(u), + v_root = D.findSet(v); + if (u_root != v_root) { + t++; + augment = true; D.makeUnionInPlace(u_root, v_root); } } - if ( augment ) At->push_back(gf); - else Ac->push_back(gf); + if (augment) + At->push_back(gf); + else + Ac->push_back(gf); } return boost::tie(At, Ac); } /****************************************************************************/ -VectorValues SubgraphSolver::optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda, - const VectorValues &initial -) { return VectorValues(); } +VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial) { + return VectorValues(); +} } // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index ac8a9da87..318c029f3 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -9,27 +9,37 @@ * -------------------------------------------------------------------------- */ +/** + * @file SubgraphSolver.h + * @brief Subgraph Solver from IROS 2010 + * @date 2010 + * @author Frank Dellaert + * @author Yong Dian Jian + */ + #pragma once - #include -#include -#include -#include namespace gtsam { - // Forward declarations - class GaussianFactorGraph; - class GaussianBayesNet; - class SubgraphPreconditioner; +// Forward declarations +class GaussianFactorGraph; +class GaussianBayesNet; +class SubgraphPreconditioner; -class GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters { +class GTSAM_EXPORT SubgraphSolverParameters: public ConjugateGradientParameters { public: typedef ConjugateGradientParameters Base; - SubgraphSolverParameters() : Base() {} - void print() const { Base::print(); } - virtual void print(std::ostream &os) const { Base::print(os); } + SubgraphSolverParameters() : + Base() { + } + void print() const { + Base::print(); + } + virtual void print(std::ostream &os) const { + Base::print(os); + } }; /** @@ -53,8 +63,7 @@ public: * * \nosubgrouping */ - -class GTSAM_EXPORT SubgraphSolver : public IterativeSolver { +class GTSAM_EXPORT SubgraphSolver: public IterativeSolver { public: typedef SubgraphSolverParameters Parameters; @@ -62,41 +71,64 @@ public: protected: Parameters parameters_; Ordering ordering_; - boost::shared_ptr pc_; ///< preconditioner object + boost::shared_ptr pc_; ///< preconditioner object public: - /* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */ - SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters, const Ordering& ordering); - SubgraphSolver(const boost::shared_ptr &A, const Parameters ¶meters, const Ordering& ordering); - /* The user specify the subgraph part and the constraint part, may throw exception if A1 is underdetermined */ - SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters, const Ordering& ordering); - SubgraphSolver(const boost::shared_ptr &Ab1, const boost::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering); + /// Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG + SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters, + const Ordering& ordering); + + /// Shared pointer version + SubgraphSolver(const boost::shared_ptr &A, + const Parameters ¶meters, const Ordering& ordering); + + /** + * The user specify the subgraph part and the constraint part + * may throw exception if A1 is underdetermined + */ + SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, + const Parameters ¶meters, const Ordering& ordering); + + /// Shared pointer version + SubgraphSolver(const boost::shared_ptr &Ab1, + const boost::shared_ptr &Ab2, + const Parameters ¶meters, const Ordering& ordering); /* The same as above, but the A1 is solved before */ - SubgraphSolver(const boost::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters ¶meters, const Ordering& ordering); - SubgraphSolver(const boost::shared_ptr &Rc1, const boost::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering); + SubgraphSolver(const boost::shared_ptr &Rc1, + const GaussianFactorGraph &Ab2, const Parameters ¶meters, + const Ordering& ordering); - virtual ~SubgraphSolver() {} + /// Shared pointer version + SubgraphSolver(const boost::shared_ptr &Rc1, + const boost::shared_ptr &Ab2, + const Parameters ¶meters, const Ordering& ordering); - VectorValues optimize () ; - VectorValues optimize (const VectorValues &initial) ; + /// Destructor + virtual ~SubgraphSolver() { + } - /* interface to the nonlinear optimizer that the subclasses have to implement */ - virtual VectorValues optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda, - const VectorValues &initial - ) ; + /// Optimize from zero + VectorValues optimize(); + + /// Optimize from given initial values + VectorValues optimize(const VectorValues &initial); + + /** Interface that IterativeSolver subclasses have to implement */ + virtual VectorValues optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial); protected: void initialize(const GaussianFactorGraph &jfg); - void initialize(const boost::shared_ptr &Rc1, const boost::shared_ptr &Ab2); + void initialize(const boost::shared_ptr &Rc1, + const boost::shared_ptr &Ab2); - boost::tuple, boost::shared_ptr > - splitGraph(const GaussianFactorGraph &gfg) ; + boost::tuple, + boost::shared_ptr > + splitGraph(const GaussianFactorGraph &gfg); }; } // namespace gtsam diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 290249b68..968fc1adb 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -376,9 +376,14 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(values_); } }; // VectorValues definition + /// traits + template<> + struct traits : public Testable { + }; + } // \namespace gtsam diff --git a/gtsam/linear/linearExceptions.cpp b/gtsam/linear/linearExceptions.cpp index 3f62ed6d8..88758ae7a 100644 --- a/gtsam/linear/linearExceptions.cpp +++ b/gtsam/linear/linearExceptions.cpp @@ -17,7 +17,7 @@ */ #include - +#include #include namespace gtsam { @@ -25,16 +25,19 @@ namespace gtsam { /* ************************************************************************* */ const char* IndeterminantLinearSystemException::what() const throw() { - if(!description_) + if(!description_) { description_ = String( "\nIndeterminant linear system detected while working near variable\n" - + boost::lexical_cast(j_) + ".\n" + + boost::lexical_cast(j_) + + + " (Symbol: " + boost::lexical_cast( + gtsam::DefaultKeyFormatter(gtsam::Symbol(j_))) + ").\n" "\n\ Thrown when a linear system is ill-posed. The most common cause for this\n\ error is having underconstrained variables. Mathematically, the system is\n\ underdetermined. See the GTSAM Doxygen documentation at\n\ http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for\n\ more information."); + } return description_->c_str(); } diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index fb3884542..26f975296 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -148,7 +148,6 @@ TEST( GaussianBayesNet, DeterminantTest ) } /* ************************************************************************* */ -typedef Eigen::Matrix Vector10; namespace { double computeError(const GaussianBayesNet& gbn, const Vector10& values) { diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 78fe5187a..05f8c3d2a 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -175,7 +175,6 @@ TEST(GaussianBayesTree, complicatedMarginal) { } /* ************************************************************************* */ -typedef Eigen::Matrix Vector10; namespace { double computeError(const GaussianBayesTree& gbt, const Vector10& values) { pair Rd = GaussianFactorGraph(gbt).jacobian(); diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index d87d842de..7404caa14 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -315,30 +315,6 @@ TEST( GaussianFactorGraph, multiplyHessianAdd2 ) EXPECT(assert_equal(2*expected, actual)); } -/* ************************************************************************* */ -TEST( GaussianFactorGraph, multiplyHessianAdd3 ) -{ - GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); - - // brute force - Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian(); - Vector X(6); X<<1,2,3,4,5,6; - Vector Y(6); Y<<-450, -450, 300, 400, 2950, 3450; - EXPECT(assert_equal(Y,AtA*X)); - - double* x = &X[0]; - - Vector fast_y = gtsam::zero(6); - gfg.multiplyHessianAdd(1.0, x, fast_y.data()); - EXPECT(assert_equal(Y, fast_y)); - - // now, do it with non-zero y - gfg.multiplyHessianAdd(1.0, x, fast_y.data()); - EXPECT(assert_equal(2*Y, fast_y)); - -} - - /* ************************************************************************* */ TEST( GaussianFactorGraph, matricesMixed ) { @@ -351,7 +327,6 @@ TEST( GaussianFactorGraph, matricesMixed ) EXPECT(assert_equal(A.transpose()*b, eta)); } - /* ************************************************************************* */ TEST( GaussianFactorGraph, gradientAtZero ) { diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index cba65580e..3a2cd0fd4 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -449,6 +449,32 @@ TEST(HessianFactor, gradientAtZero) EXPECT(assert_equal(expectedG, actualG)); } +/* ************************************************************************* */ +TEST(HessianFactor, gradient) +{ + Matrix G11 = (Matrix(1, 1) << 1).finished(); + Matrix G12 = (Matrix(1, 2) << 0, 0).finished(); + Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1).finished(); + Vector g1 = (Vector(1) << -7).finished(); + Vector g2 = (Vector(2) << -8, -9).finished(); + double f = 194; + + HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f); + + // test gradient + Vector x0 = (Vector(1) << 3.0).finished(); + Vector x1 = (Vector(2) << -3.5, 7.1).finished(); + VectorValues x = pair_list_of(0, x0) (1, x1); + + Vector expectedGrad0 = (Vector(1) << 10.0).finished(); + Vector expectedGrad1 = (Vector(2) << 4.5, 16.1).finished(); + Vector grad0 = factor.gradient(0, x); + Vector grad1 = factor.gradient(1, x); + + EXPECT(assert_equal(expectedGrad0, grad0)); + EXPECT(assert_equal(expectedGrad1, grad1)); +} + /* ************************************************************************* */ TEST(HessianFactor, hessianDiagonal) { diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 5c6b2729d..b2afb1709 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -33,15 +33,16 @@ using namespace gtsam; using namespace noiseModel; using namespace boost::assign; -static double sigma = 2, s_1=1.0/sigma, var = sigma*sigma, prc = 1.0/var; -static Matrix R = (Matrix(3, 3) << +static const double kSigma = 2, s_1=1.0/kSigma, kVariance = kSigma*kSigma, prc = 1.0/kVariance; +static const Matrix R = (Matrix(3, 3) << s_1, 0.0, 0.0, 0.0, s_1, 0.0, 0.0, 0.0, s_1).finished(); -static Matrix Sigma = (Matrix(3, 3) << - var, 0.0, 0.0, - 0.0, var, 0.0, - 0.0, 0.0, var).finished(); +static const Matrix kCovariance = (Matrix(3, 3) << + kVariance, 0.0, 0.0, + 0.0, kVariance, 0.0, + 0.0, 0.0, kVariance).finished(); +static const Vector3 kSigmas(kSigma, kSigma, kSigma); //static double inf = numeric_limits::infinity(); @@ -53,15 +54,19 @@ TEST(NoiseModel, constructors) // Construct noise models vector m; - m.push_back(Gaussian::SqrtInformation(R)); - m.push_back(Gaussian::Covariance(Sigma)); - //m.push_back(Gaussian::Information(Q)); - m.push_back(Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma).finished())); - m.push_back(Diagonal::Variances((Vector(3) << var, var, var).finished())); - m.push_back(Diagonal::Precisions((Vector(3) << prc, prc, prc).finished())); - m.push_back(Isotropic::Sigma(3, sigma)); - m.push_back(Isotropic::Variance(3, var)); - m.push_back(Isotropic::Precision(3, prc)); + m.push_back(Gaussian::SqrtInformation(R,false)); + m.push_back(Gaussian::Covariance(kCovariance,false)); + m.push_back(Gaussian::Information(kCovariance.inverse(),false)); + m.push_back(Diagonal::Sigmas(kSigmas,false)); + m.push_back(Diagonal::Variances((Vector(3) << kVariance, kVariance, kVariance).finished(),false)); + m.push_back(Diagonal::Precisions((Vector(3) << prc, prc, prc).finished(),false)); + m.push_back(Isotropic::Sigma(3, kSigma,false)); + m.push_back(Isotropic::Variance(3, kVariance,false)); + m.push_back(Isotropic::Precision(3, prc,false)); + + // test kSigmas + BOOST_FOREACH(Gaussian::shared_ptr mi, m) + EXPECT(assert_equal(kSigmas,mi->sigmas())); // test whiten BOOST_FOREACH(Gaussian::shared_ptr mi, m) @@ -117,9 +122,9 @@ TEST(NoiseModel, equals) { Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R), g2 = Gaussian::SqrtInformation(eye(3,3)); - Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma).finished()), + Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vector(3) << kSigma, kSigma, kSigma).finished()), d2 = Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3)); - Isotropic::shared_ptr i1 = Isotropic::Sigma(3, sigma), + Isotropic::shared_ptr i1 = Isotropic::Sigma(3, kSigma), i2 = Isotropic::Sigma(3, 0.7); EXPECT(assert_equal(*g1,*g1)); @@ -155,7 +160,7 @@ TEST(NoiseModel, ConstrainedConstructors ) Constrained::shared_ptr actual; size_t d = 3; double m = 100.0; - Vector sigmas = (Vector(3) << sigma, 0.0, 0.0).finished(); + Vector sigmas = (Vector(3) << kSigma, 0.0, 0.0).finished(); Vector mu = Vector3(200.0, 300.0, 400.0); actual = Constrained::All(d); // TODO: why should this be a thousand ??? Dummy variable? @@ -182,7 +187,7 @@ TEST(NoiseModel, ConstrainedMixed ) { Vector feasible = Vector3(1.0, 0.0, 1.0), infeasible = Vector3(1.0, 1.0, 1.0); - Diagonal::shared_ptr d = Constrained::MixedSigmas((Vector(3) << sigma, 0.0, sigma).finished()); + Diagonal::shared_ptr d = Constrained::MixedSigmas((Vector(3) << kSigma, 0.0, kSigma).finished()); // NOTE: we catch constrained variables elsewhere, so whitening does nothing EXPECT(assert_equal(Vector3(0.5, 1.0, 0.5),d->whiten(infeasible))); EXPECT(assert_equal(Vector3(0.5, 0.0, 0.5),d->whiten(feasible))); @@ -357,6 +362,44 @@ TEST(NoiseModel, robustNoise) DOUBLES_EQUAL(sqrt(k/100.0)*1000.0, A(1,1), 1e-8); } +/* ************************************************************************* */ +#define TEST_GAUSSIAN(gaussian)\ + EQUALITY(info, gaussian->information());\ + EQUALITY(cov, gaussian->covariance());\ + EXPECT(assert_equal(white, gaussian->whiten(e)));\ + EXPECT(assert_equal(e, gaussian->unwhiten(white)));\ + EXPECT_DOUBLES_EQUAL(251, gaussian->distance(e), 1e-9);\ + Matrix A = R.inverse(); Vector b = e;\ + gaussian->WhitenSystem(A, b);\ + EXPECT(assert_equal(I, A));\ + EXPECT(assert_equal(white, b)); + +TEST(NoiseModel, NonDiagonalGaussian) +{ + Matrix3 R; + R << 6, 5, 4, 0, 3, 2, 0, 0, 1; + const Matrix3 info = R.transpose() * R; + const Matrix3 cov = info.inverse(); + const Vector3 e(1, 1, 1), white = R * e; + Matrix I = Matrix3::Identity(); + + + { + SharedGaussian gaussian = Gaussian::SqrtInformation(R); + TEST_GAUSSIAN(gaussian); + } + + { + SharedGaussian gaussian = Gaussian::Information(info); + TEST_GAUSSIAN(gaussian); + } + + { + SharedGaussian gaussian = Gaussian::Covariance(cov); + TEST_GAUSSIAN(gaussian); + } +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 137f102b3..2f03d72a4 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -18,9 +18,9 @@ **/ #include +#include -/* External or standard includes */ -#include +using namespace std; namespace gtsam { @@ -29,47 +29,35 @@ namespace gtsam { //------------------------------------------------------------------------------ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements( const Vector3& bias, const Matrix3& measuredOmegaCovariance) : - biasHat_(bias), deltaTij_(0.0) { - measurementCovariance_ << measuredOmegaCovariance; - delRdelBiasOmega_.setZero(); + PreintegratedRotation(measuredOmegaCovariance), biasHat_(bias) +{ preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() : - biasHat_(Vector3()), deltaTij_(0.0) { - measurementCovariance_.setZero(); - delRdelBiasOmega_.setZero(); - delRdelBiasOmega_.setZero(); + PreintegratedRotation(I_3x3), biasHat_(Vector3()) +{ preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ -void AHRSFactor::PreintegratedMeasurements::print(const std::string& s) const { - std::cout << s << std::endl; - std::cout << "biasHat [" << biasHat_.transpose() << "]" << std::endl; - deltaRij_.print(" deltaRij "); - std::cout << " measurementCovariance [" << measurementCovariance_ << " ]" - << std::endl; - std::cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << std::endl; +void AHRSFactor::PreintegratedMeasurements::print(const string& s) const { + PreintegratedRotation::print(s); + cout << "biasHat [" << biasHat_.transpose() << "]" << endl; + cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool AHRSFactor::PreintegratedMeasurements::equals( const PreintegratedMeasurements& other, double tol) const { - return equal_with_abs_tol(biasHat_, other.biasHat_, tol) - && equal_with_abs_tol(measurementCovariance_, - other.measurementCovariance_, tol) - && deltaRij_.equals(other.deltaRij_, tol) - && std::fabs(deltaTij_ - other.deltaTij_) < tol - && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); + return PreintegratedRotation::equals(other, tol) + && equal_with_abs_tol(biasHat_, other.biasHat_, tol); } //------------------------------------------------------------------------------ void AHRSFactor::PreintegratedMeasurements::resetIntegration() { - deltaRij_ = Rot3(); - deltaTij_ = 0.0; - delRdelBiasOmega_.setZero(); + PreintegratedRotation::resetIntegration(); preintMeasCov_.setZero(); } @@ -78,7 +66,6 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( const Vector3& measuredOmega, double deltaT, boost::optional body_P_sensor) { - // NOTE: order is important here because each update uses old values. // First we compensate the measurements for the bias Vector3 correctedOmega = measuredOmega - biasHat_; @@ -93,64 +80,27 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( // rotation vector describing rotation increment computed from the // current rotation rate measurement const Vector3 theta_incr = correctedOmega * deltaT; + Matrix3 D_Rincr_integratedOmega; + const Rot3 incrR = Rot3::Expmap(theta_incr, D_Rincr_integratedOmega); // expensive !! - // rotation increment computed from the current rotation rate measurement - const Rot3 incrR = Rot3::Expmap(theta_incr); - const Matrix3 incrRt = incrR.transpose(); + // Update Jacobian + update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT); - // Right Jacobian computed at theta_incr - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); - - // Update Jacobians - // --------------------------------------------------------------------------- - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - Jr_theta_incr * deltaT; - - // Update preintegrated measurements covariance - // --------------------------------------------------------------------------- - const Vector3 theta_i = Rot3::Logmap(deltaRij_); // Parameterization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i); - - Rot3 Rot_j = deltaRij_ * incrR; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // Parameterization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); - - // 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 - Matrix3 H_angles_angles = Jrinv_theta_j * incrRt * Jr_theta_i; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11 - // (boost::bind(&DeltaAngles, correctedOmega, deltaT, _1), thetaij); - - // overall Jacobian wrpt preintegrated measurements (df/dx) - const Matrix3& F = H_angles_angles; + // Update rotation and deltaTij. + Matrix3 Fr; // Jacobian of the update + updateIntegratedRotationAndDeltaT(incrR, deltaT, Fr); // first order uncertainty propagation // the deltaT allows to pass from continuous time noise to discrete time noise - preintMeasCov_ = F * preintMeasCov_ * F.transpose() - + measurementCovariance_ * deltaT; - - // Update preintegrated measurements - // --------------------------------------------------------------------------- - deltaRij_ = deltaRij_ * incrR; - deltaTij_ += deltaT; + preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() + + gyroscopeCovariance() * deltaT; } //------------------------------------------------------------------------------ Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias, boost::optional H) const { const Vector3 biasOmegaIncr = bias - biasHat_; - Vector3 delRdelBiasOmega_biasOmegaIncr = delRdelBiasOmega_ * biasOmegaIncr; - const Rot3 deltaRij_biascorrected = deltaRij_.retract( - delRdelBiasOmega_biasOmegaIncr, Rot3::EXPMAP); - const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - if (H) { - const Matrix3 Jrinv_theta_bc = // - Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = // - Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_biasOmegaIncr); - (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; - } - return theta_biascorrected; + return biascorrectedThetaRij(biasOmegaIncr, H); } //------------------------------------------------------------------------------ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles( @@ -172,7 +122,7 @@ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles( // AHRSFactor methods //------------------------------------------------------------------------------ AHRSFactor::AHRSFactor() : - preintegratedMeasurements_(Vector3(), Matrix3::Zero()) { + _PIM_(Vector3(), Z_3x3) { } AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, @@ -180,7 +130,7 @@ AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, const Vector3& omegaCoriolis, boost::optional body_P_sensor) : Base( noiseModel::Gaussian::Covariance( - preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), preintegratedMeasurements_( + preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), _PIM_( preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_( body_P_sensor) { } @@ -192,13 +142,12 @@ gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const { } //------------------------------------------------------------------------------ -void AHRSFactor::print(const std::string& s, +void AHRSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - std::cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," + cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","; - preintegratedMeasurements_.print(" preintegrated measurements:"); - std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" - << std::endl; + _PIM_.print(" preintegrated measurements:"); + cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; noiseModel_->print(" noise model: "); if (body_P_sensor_) body_P_sensor_->print(" sensor pose in body frame: "); @@ -207,8 +156,7 @@ void AHRSFactor::print(const std::string& s, //------------------------------------------------------------------------------ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { const This *e = dynamic_cast(&other); - return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) + return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ @@ -216,50 +164,49 @@ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { } //------------------------------------------------------------------------------ -Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j, +Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, const Vector3& bias, boost::optional H1, boost::optional H2, boost::optional H3) const { // Do bias correction, if (H3) will contain 3*3 derivative used below - const Vector3 theta_biascorrected = // - preintegratedMeasurements_.predict(bias, H3); + const Vector3 biascorrectedOmega = _PIM_.predict(bias, H3); // Coriolis term - const Vector3 coriolis = // - preintegratedMeasurements_.integrateCoriolis(rot_i, omegaCoriolis_); - const Vector3 theta_corrected = theta_biascorrected - coriolis; + const Vector3 coriolis = _PIM_.integrateCoriolis(Ri, omegaCoriolis_); + const Matrix3 coriolisHat = skewSymmetric(coriolis); + const Vector3 correctedOmega = biascorrectedOmega - coriolis; // Prediction - const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected); + const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); // Get error between actual and prediction - const Rot3 actualRij = rot_i.between(rot_j); - const Rot3 fRhat = deltaRij_corrected.between(actualRij); - Vector3 fR = Rot3::Logmap(fRhat); + const Rot3 actualRij = Ri.between(Rj); + const Rot3 fRrot = correctedDeltaRij.between(actualRij); + Vector3 fR = Rot3::Logmap(fRrot); // Terms common to derivatives - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected); - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(fR); + const Matrix3 D_cDeltaRij_cOmega = Rot3::ExpmapDerivative(correctedOmega); + const Matrix3 D_fR_fRrot = Rot3::LogmapDerivative(fR); if (H1) { // dfR/dRi H1->resize(3, 3); - Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(coriolis); + Matrix3 D_coriolis = -D_cDeltaRij_cOmega * coriolisHat; (*H1) - << Jrinv_fRhat * (-actualRij.transpose() - fRhat.transpose() * Jtheta); + << D_fR_fRrot * (-actualRij.transpose() - fRrot.transpose() * D_coriolis); } if (H2) { // dfR/dPosej H2->resize(3, 3); - (*H2) << Jrinv_fRhat * Matrix3::Identity(); + (*H2) << D_fR_fRrot * Matrix3::Identity(); } if (H3) { // dfR/dBias, note H3 contains derivative of predict - const Matrix3 JbiasOmega = Jr_theta_bcc * (*H3); + const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * (*H3); H3->resize(3, 3); - (*H3) << Jrinv_fRhat * (-fRhat.transpose() * JbiasOmega); + (*H3) << D_fR_fRrot * (-fRrot.transpose() * JbiasOmega); } Vector error(3); @@ -272,16 +219,16 @@ Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, const PreintegratedMeasurements preintegratedMeasurements, const Vector3& omegaCoriolis, boost::optional body_P_sensor) { - const Vector3 theta_biascorrected = preintegratedMeasurements.predict(bias); + const Vector3 biascorrectedOmega = preintegratedMeasurements.predict(bias); // Coriolis term const Vector3 coriolis = // preintegratedMeasurements.integrateCoriolis(rot_i, omegaCoriolis); - const Vector3 theta_corrected = theta_biascorrected - coriolis; - const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected); + const Vector3 correctedOmega = biascorrectedOmega - coriolis; + const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); - return rot_i.compose(deltaRij_corrected); + return rot_i.compose(correctedDeltaRij); } } //namespace gtsam diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index e62a24cca..fbf7d51a1 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -20,12 +20,13 @@ #pragma once /* GTSAM includes */ +#include #include #include namespace gtsam { -class AHRSFactor: public NoiseModelFactor3 { +class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3 { public: /** @@ -35,17 +36,12 @@ public: * Can be built incrementally so as to avoid costly integration at time of * factor construction. */ - class PreintegratedMeasurements { + class GTSAM_EXPORT PreintegratedMeasurements : public PreintegratedRotation { friend class AHRSFactor; protected: Vector3 biasHat_; ///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer - Matrix3 measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector [measuredOmega] in R^(3X3) - - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) public: @@ -61,31 +57,19 @@ public: PreintegratedMeasurements(const Vector3& bias, const Matrix3& measuredOmegaCovariance); + Vector3 biasHat() const { + return biasHat_; + } + const Matrix3& preintMeasCov() const { + return preintMeasCov_; + } + /// print void print(const std::string& s = "Preintegrated Measurements: ") const; /// equals bool equals(const PreintegratedMeasurements&, double tol = 1e-9) const; - const Matrix3& measurementCovariance() const { - return measurementCovariance_; - } - Matrix3 deltaRij() const { - return deltaRij_.matrix(); - } - double deltaTij() const { - return deltaTij_; - } - Vector3 biasHat() const { - return biasHat_; - } - const Matrix3& delRdelBiasOmega() const { - return delRdelBiasOmega_; - } - const Matrix3& preintMeasCov() const { - return preintMeasCov_; - } - /// TODO: Document void resetIntegration(); @@ -103,12 +87,6 @@ public: Vector3 predict(const Vector3& bias, boost::optional H = boost::none) const; - /// Integrate coriolis correction in body frame rot_i - Vector3 integrateCoriolis(const Rot3& rot_i, - const Vector3& omegaCoriolis) const { - return rot_i.transpose() * omegaCoriolis * deltaTij_; - } - // This function is only used for test purposes // (compare numerical derivatives wrt analytic ones) static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt, @@ -119,12 +97,9 @@ public: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation); ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); - ar & BOOST_SERIALIZATION_NVP(deltaRij_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); } }; @@ -132,7 +107,7 @@ private: typedef AHRSFactor This; typedef NoiseModelFactor3 Base; - PreintegratedMeasurements preintegratedMeasurements_; + PreintegratedMeasurements _PIM_; Vector3 gravity_; Vector3 omegaCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame @@ -178,7 +153,7 @@ public: /// Access the preintegrated measurements. const PreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; + return _PIM_; } const Vector3& omegaCoriolis() const { @@ -204,11 +179,11 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor3", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); + ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } diff --git a/gtsam/navigation/AttitudeFactor.cpp b/gtsam/navigation/AttitudeFactor.cpp index 7eba995d3..089747dc6 100644 --- a/gtsam/navigation/AttitudeFactor.cpp +++ b/gtsam/navigation/AttitudeFactor.cpp @@ -24,13 +24,14 @@ namespace gtsam { //*************************************************************************** Vector AttitudeFactor::attitudeError(const Rot3& nRb, - boost::optional H) const { + OptionalJacobian<2, 3> H) const { if (H) { - Matrix D_nRef_R, D_e_nRef; + Matrix23 D_nRef_R; + Matrix22 D_e_nRef; Unit3 nRef = nRb.rotate(bRef_, D_nRef_R); Vector e = nZ_.error(nRef, D_e_nRef); - H->resize(2, 3); - H->block < 2, 3 > (0, 0) = D_e_nRef * D_nRef_R; + + (*H) = D_e_nRef * D_nRef_R; return e; } else { Unit3 nRef = nRb * bRef_; diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index def36cc67..b61a861d6 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -54,7 +54,7 @@ public: /** vector of errors */ Vector attitudeError(const Rot3& p, - boost::optional H = boost::none) const; + OptionalJacobian<2,3> H = boost::none) const; }; /** @@ -122,7 +122,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); @@ -131,6 +131,9 @@ private: } }; +/// traits +template<> struct traits : public Testable {}; + /** * Version of AttitudeFactor for Pose3 * @addtogroup Navigation @@ -203,7 +206,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); @@ -212,5 +215,8 @@ private: } }; +/// traits +template<> struct traits : public Testable {}; + } /// namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp new file mode 100644 index 000000000..3547719ac --- /dev/null +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -0,0 +1,276 @@ +/* ---------------------------------------------------------------------------- + + * 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 CombinedImuFactor.cpp + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#include + +/* External or standard includes */ +#include + +namespace gtsam { + +using namespace std; + +//------------------------------------------------------------------------------ +// Inner class CombinedPreintegratedMeasurements +//------------------------------------------------------------------------------ +CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasurements( + const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, + const Matrix3& biasOmegaCovariance, const Matrix& biasAccOmegaInit, + const bool use2ndOrderIntegration) : + PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance, + integrationErrorCovariance, use2ndOrderIntegration), biasAccCovariance_( + biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance), biasAccOmegaInit_( + biasAccOmegaInit) { + preintMeasCov_.setZero(); +} + +//------------------------------------------------------------------------------ +void CombinedImuFactor::CombinedPreintegratedMeasurements::print( + const string& s) const { + PreintegrationBase::print(s); + cout << " biasAccCovariance [ " << biasAccCovariance_ << " ]" << endl; + cout << " biasOmegaCovariance [ " << biasOmegaCovariance_ << " ]" << endl; + cout << " biasAccOmegaInit [ " << biasAccOmegaInit_ << " ]" << endl; + cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl; +} + +//------------------------------------------------------------------------------ +bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals( + const CombinedPreintegratedMeasurements& expected, double tol) const { + return equal_with_abs_tol(biasAccCovariance_, expected.biasAccCovariance_, + tol) + && equal_with_abs_tol(biasOmegaCovariance_, expected.biasOmegaCovariance_, + tol) + && equal_with_abs_tol(biasAccOmegaInit_, expected.biasAccOmegaInit_, tol) + && equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) + && PreintegrationBase::equals(expected, tol); +} + +//------------------------------------------------------------------------------ +void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration() { + PreintegrationBase::resetIntegration(); + preintMeasCov_.setZero(); +} + +//------------------------------------------------------------------------------ +void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( + const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, + boost::optional body_P_sensor, + boost::optional F_test, boost::optional G_test) { + + // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. + // (i.e., we have to update jacobians and covariances before updating preintegrated measurements). + + Vector3 correctedAcc, correctedOmega; + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, + correctedAcc, correctedOmega, body_P_sensor); + + const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement + Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr + const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement + + // Update Jacobians + /* ----------------------------------------------------------------------------------------------------------------------- */ + updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, + deltaT); + + // 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 + /* ----------------------------------------------------------------------------------------------------------------------- */ + const Matrix3 R_i = deltaRij(); // store this + // Update preintegrated measurements. TODO Frank moved from end of this function !!! + Matrix9 F_9x9; + updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9); + + // Single Jacobians to propagate covariance + Matrix3 H_vel_biasacc = -R_i * deltaT; + Matrix3 H_angles_biasomega = -D_Rincr_integratedOmega * deltaT; + + // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix F(15, 15); + // for documentation: + // F << I_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, + // Z_3x3, I_3x3, H_vel_angles, H_vel_biasacc, Z_3x3, + // Z_3x3, Z_3x3, H_angles_angles, Z_3x3, H_angles_biasomega, + // Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, + // Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; + F.setZero(); + F.block<9, 9>(0, 0) = F_9x9; + F.block<6, 6>(9, 9) = I_6x6; + F.block<3, 3>(3, 9) = H_vel_biasacc; + F.block<3, 3>(6, 12) = H_angles_biasomega; + + // first order uncertainty propagation + // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() + Matrix G_measCov_Gt = Matrix::Zero(15, 15); + +// BLOCK DIAGONAL TERMS + G_measCov_Gt.block<3, 3>(0, 0) = deltaT * integrationCovariance(); + G_measCov_Gt.block<3, 3>(3, 3) = (1 / deltaT) * (H_vel_biasacc) + * (accelerometerCovariance() + biasAccOmegaInit_.block<3, 3>(0, 0)) + * (H_vel_biasacc.transpose()); + G_measCov_Gt.block<3, 3>(6, 6) = (1 / deltaT) * (H_angles_biasomega) + * (gyroscopeCovariance() + biasAccOmegaInit_.block<3, 3>(3, 3)) + * (H_angles_biasomega.transpose()); + G_measCov_Gt.block<3, 3>(9, 9) = (1 / deltaT) * biasAccCovariance_; + G_measCov_Gt.block<3, 3>(12, 12) = (1 / deltaT) * biasOmegaCovariance_; + // OFF BLOCK DIAGONAL TERMS + Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3, 3>(3, 0) + * H_angles_biasomega.transpose(); + G_measCov_Gt.block<3, 3>(3, 6) = block23; + G_measCov_Gt.block<3, 3>(6, 3) = block23.transpose(); + preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; + + // F_test and G_test are used for testing purposes and are not needed by the factor + if (F_test) { + F_test->resize(15, 15); + (*F_test) << F; + } + if (G_test) { + G_test->resize(15, 21); + // This is for testing & documentation + ///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) + (*G_test) << // + I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, // + Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, // + Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, // + Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, // + Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3; + } +} + +//------------------------------------------------------------------------------ +// CombinedImuFactor methods +//------------------------------------------------------------------------------ +CombinedImuFactor::CombinedImuFactor() : + ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, + Z_3x3, Z_6x6) { +} + +//------------------------------------------------------------------------------ +CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, + Key vel_j, Key bias_i, Key bias_j, + const CombinedPreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : + Base( + noiseModel::Gaussian::Covariance( + preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, + vel_j, bias_i, bias_j), ImuFactorBase(gravity, omegaCoriolis, + body_P_sensor, use2ndOrderCoriolis), _PIM_(preintegratedMeasurements) { +} + +//------------------------------------------------------------------------------ +gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); +} + +//------------------------------------------------------------------------------ +void CombinedImuFactor::print(const string& s, + const KeyFormatter& keyFormatter) const { + cout << s << "CombinedImuFactor(" << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << "," + << keyFormatter(this->key6()) << ")\n"; + ImuFactorBase::print(""); + _PIM_.print(" preintegrated measurements:"); + this->noiseModel_->print(" noise model: "); +} + +//------------------------------------------------------------------------------ +bool CombinedImuFactor::equals(const NonlinearFactor& expected, + double tol) const { + const This *e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) + && ImuFactorBase::equals(*e, tol); +} + +//------------------------------------------------------------------------------ +Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, + const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, + boost::optional H1, boost::optional H2, + boost::optional H3, boost::optional H4, + boost::optional H5, boost::optional H6) const { + + // error wrt bias evolution model (random walk) + Matrix6 Hbias_i, Hbias_j; + Vector6 fbias = traits::Between(bias_j, bias_i, + H6 ? &Hbias_j : 0, H5 ? &Hbias_i : 0).vector(); + + Matrix96 D_r_pose_i, D_r_pose_j, D_r_bias_i; + Matrix93 D_r_vel_i, D_r_vel_j; + + // error wrt preintegrated measurements + Vector9 r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, + bias_i, gravity_, omegaCoriolis_, use2ndOrderCoriolis_, + H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0, + H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0); + + // if we need the jacobians + if (H1) { + H1->resize(15, 6); + H1->block<9, 6>(0, 0) = D_r_pose_i; + // adding: [dBiasAcc/dPi ; dBiasOmega/dPi] + H1->block<6, 6>(9, 0) = Z_6x6; + } + if (H2) { + H2->resize(15, 3); + H2->block<9, 3>(0, 0) = D_r_vel_i; + // adding: [dBiasAcc/dVi ; dBiasOmega/dVi] + H2->block<6, 3>(9, 0) = Matrix::Zero(6, 3); + } + if (H3) { + H3->resize(15, 6); + H3->block<9, 6>(0, 0) = D_r_pose_j; + // adding: [dBiasAcc/dPj ; dBiasOmega/dPj] + H3->block<6, 6>(9, 0) = Z_6x6; + } + if (H4) { + H4->resize(15, 3); + H4->block<9, 3>(0, 0) = D_r_vel_j; + // adding: [dBiasAcc/dVi ; dBiasOmega/dVi] + H4->block<6, 3>(9, 0) = Matrix::Zero(6, 3); + } + if (H5) { + H5->resize(15, 6); + H5->block<9, 6>(0, 0) = D_r_bias_i; + // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i] + H5->block<6, 6>(9, 0) = Hbias_i; + } + if (H6) { + H6->resize(15, 6); + H6->block<9, 6>(0, 0) = Matrix::Zero(9, 6); + // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j] + H6->block<6, 6>(9, 0) = Hbias_j; + } + + // overall error + Vector r(15); + r << r_pvR, fbias; // vector of size 15 + return r; +} + +} /// namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 069723eca..a65a4d3f7 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -11,714 +11,252 @@ /** * @file CombinedImuFactor.h - * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert **/ #pragma once /* GTSAM includes */ -#include -#include #include -#include +#include +#include #include -/* External or standard includes */ -#include - - namespace gtsam { - /** - * Struct to hold all state variables of CombinedImuFactor - * returned by predict function - */ - struct PoseVelocityBias { - Pose3 pose; - Vector3 velocity; - imuBias::ConstantBias bias; +/** + * + * @addtogroup SLAM + * + * If you are using the factor, please cite: + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating + * conditionally independent sets in factor graphs: a unifying perspective based + * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014. + * + ** REFERENCES: + * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", + * Volume 2, 2008. + * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for + * High-Dynamic Motion in Built Environments Without Initial Conditions", + * TRO, 28(1):61-76, 2012. + * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: + * Computation of the Jacobian Matrices", Tech. Report, 2013. + */ - PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, - const imuBias::ConstantBias _bias) : - pose(_pose), velocity(_velocity), bias(_bias) { +/** + * CombinedImuFactor is a 6-ways factor involving previous state (pose and + * velocity of the vehicle, as well as bias at previous time step), and current + * state (pose, velocity, bias at current time step). Following the pre- + * integration scheme proposed in [2], the CombinedImuFactor includes many IMU + * measurements, which are "summarized" using the CombinedPreintegratedMeasurements + * class. There are 3 main differences wrpt the ImuFactor class: + * 1) The factor is 6-ways, meaning that it also involves both biases (previous + * and current time step).Therefore, the factor internally imposes the biases + * to be slowly varying; in particular, the matrices "biasAccCovariance" and + * "biasOmegaCovariance" described the random walk that models bias evolution. + * 2) The preintegration covariance takes into account the noise in the bias + * estimate used for integration. + * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves + * the correlation between the bias uncertainty and the preintegrated + * measurements uncertainty. + */ +class CombinedImuFactor: public NoiseModelFactor6, public ImuFactorBase { +public: + + /** + * CombinedPreintegratedMeasurements integrates the IMU measurements + * (rotation rates and accelerations) and the corresponding covariance matrix. + * The measurements are then used to build the CombinedImuFactor. Integration + * is done incrementally (ideally, one integrates the measurement as soon as + * it is received from the IMU) so as to avoid costly integration at time of + * factor construction. + */ + class CombinedPreintegratedMeasurements: public PreintegrationBase { + + friend class CombinedImuFactor; + + protected: + + Matrix3 biasAccCovariance_; ///< continuous-time "Covariance" describing accelerometer bias random walk + Matrix3 biasOmegaCovariance_; ///< continuous-time "Covariance" describing gyroscope bias random walk + Matrix6 biasAccOmegaInit_; ///< covariance of bias used for pre-integration + + Eigen::Matrix preintMeasCov_; ///< Covariance matrix of the preintegrated measurements + ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] + ///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation + ///< between the preintegrated measurements and the biases + + public: + + /** + * Default constructor, initializes the class with no measurements + * @param bias Current estimate of acceleration and rotation rate biases + * @param measuredAccCovariance Covariance matrix of measuredAcc + * @param measuredOmegaCovariance Covariance matrix of measured Angular Rate + * @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position) + * @param biasAccCovariance Covariance matrix of biasAcc (random walk describing BIAS evolution) + * @param biasOmegaCovariance Covariance matrix of biasOmega (random walk describing BIAS evolution) + * @param biasAccOmegaInit Covariance of biasAcc & biasOmega when preintegrating measurements + * @param use2ndOrderIntegration Controls the order of integration + * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) + */ + CombinedPreintegratedMeasurements(const imuBias::ConstantBias& bias, + const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, + const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, + const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration = + false); + + /// print + void print(const std::string& s = "Preintegrated Measurements:") const; + + /// equals + bool equals(const CombinedPreintegratedMeasurements& expected, double tol = + 1e-9) const; + + /// Re-initialize CombinedPreintegratedMeasurements + void resetIntegration(); + + /** + * Add a single IMU measurement to the preintegration. + * @param measuredAcc Measured acceleration (in body frame, as given by the sensor) + * @param measuredOmega Measured angular velocity (as given by the sensor) + * @param deltaT Time interval between two consecutive IMU measurements + * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) + */ + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, double deltaT, + boost::optional body_P_sensor = boost::none, + boost::optional F_test = boost::none, + boost::optional G_test = boost::none); + + /// methods to access class variables + Matrix preintMeasCov() const { + return preintMeasCov_; + } + + private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); } }; - /** - * - * @addtogroup SLAM - * - * If you are using the factor, please cite: - * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally - * independent sets in factor graphs: a unifying perspective based on smart factors, - * Int. Conf. on Robotics and Automation (ICRA), 2014. - * - * REFERENCES: - * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built - * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. - * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. - */ +private: - class CombinedImuFactor: public NoiseModelFactor6 { + typedef CombinedImuFactor This; + typedef NoiseModelFactor6 Base; - public: + CombinedPreintegratedMeasurements _PIM_; - /** Struct to store results of preintegrating IMU measurements. Can be build - * incrementally so as to avoid costly integration at time of factor construction. */ +public: - /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) - * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/ - class CombinedPreintegratedMeasurements { - friend class CombinedImuFactor; - protected: - imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration - Matrix measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector - ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) - - Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) - Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j - - Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias - Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias - Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias - Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) - bool use2ndOrderIntegration_; ///< Controls the order of integration -// public: - ///< In the combined factor is also includes the biases and keeps the correlation between the preintegrated measurements and the biases - ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] - /** Default constructor, initialize with no IMU measurements */ - public: - CombinedPreintegratedMeasurements( - const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases - const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) - const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) - const Matrix& biasAccOmegaInit, ///< Covariance of biasAcc & biasOmega when preintegrating measurements - const bool use2ndOrderIntegration = false ///< Controls the order of integration - ///< (this allows to consider the uncertainty of the BIAS choice when integrating the measurements) - ) : biasHat_(bias), measurementCovariance_(21,21), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0), - delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()), - delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()), - delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(Matrix::Zero(15,15)), - use2ndOrderIntegration_(use2ndOrderIntegration) - { - // COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21) - measurementCovariance_ << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(0,0,3,3), biasAccOmegaInit.block(0,3,3,3), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(3,0,3,3), biasAccOmegaInit.block(3,3,3,3); - - } - - CombinedPreintegratedMeasurements() : - biasHat_(imuBias::ConstantBias()), measurementCovariance_(21,21), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0), - delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()), - delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()), - delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(Matrix::Zero(15,15)), - use2ndOrderIntegration_(false) ///< Controls the order of integration - { - } - - /** print */ - void print(const std::string& s = "Preintegrated Measurements:") const { - std::cout << s << std::endl; - biasHat_.print(" biasHat"); - std::cout << " deltaTij " << deltaTij_ << std::endl; - std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; - std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; - deltaRij_.print(" deltaRij "); - std::cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << std::endl; - std::cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl; - } - - /** equals */ - bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const { - return biasHat_.equals(expected.biasHat_, tol) - && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) - && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) - && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) - && deltaRij_.equals(expected.deltaRij_, tol) - && std::fabs(deltaTij_ - expected.deltaTij_) < tol - && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) - && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) - && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) - && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); - } - - void resetIntegration(){ - deltaPij_ = Vector3::Zero(); - deltaVij_ = Vector3::Zero(); - deltaRij_ = Rot3(); - deltaTij_ = 0.0; - delPdelBiasAcc_ = Matrix3::Zero(); - delPdelBiasOmega_ = Matrix3::Zero(); - delVdelBiasAcc_ = Matrix3::Zero(); - delVdelBiasOmega_ = Matrix3::Zero(); - delRdelBiasOmega_ = Matrix3::Zero(); - PreintMeasCov_ = Matrix::Zero(15,15); - } - - /** Add a single IMU measurement to the preintegration. */ - void integrateMeasurement( - const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) - const Vector3& measuredOmega, ///< Measured angular velocity (in body frame) - double deltaT, ///< Time step - boost::optional body_P_sensor = boost::none ///< Sensor frame - ) { - // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. - // First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty - Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); - - // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame - if(body_P_sensor){ - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } - - const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr - - // Update Jacobians - /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; - }else{ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() - * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_; - } - - delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; - delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; - delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT; - - // 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 - /* ----------------------------------------------------------------------------------------------------------------------- */ - Matrix3 Z_3x3 = Matrix3::Zero(); - Matrix3 I_3x3 = Matrix3::Identity(); - const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); - - Rot3 Rot_j = deltaRij_ * Rincr; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); - - // Single Jacobians to propagate covariance - Matrix3 H_pos_pos = I_3x3; - Matrix3 H_pos_vel = I_3x3 * deltaT; - Matrix3 H_pos_angles = Z_3x3; - - Matrix3 H_vel_pos = Z_3x3; - Matrix3 H_vel_vel = I_3x3; - Matrix3 H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); - Matrix3 H_vel_biasacc = - deltaRij_.matrix() * deltaT; - - Matrix3 H_angles_pos = Z_3x3; - Matrix3 H_angles_vel = Z_3x3; - Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; - Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); - - // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix F(15,15); - F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3, - H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3, - H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega, - Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, - Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; - - - // first order uncertainty propagation - // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() - - Matrix G_measCov_Gt = Matrix::Zero(15,15); - // BLOCK DIAGONAL TERMS - G_measCov_Gt.block(0,0,3,3) = deltaT * measurementCovariance_.block(0,0,3,3); - - G_measCov_Gt.block(3,3,3,3) = (1/deltaT) * (H_vel_biasacc) * - (measurementCovariance_.block(3,3,3,3) + measurementCovariance_.block(15,15,3,3) ) * - (H_vel_biasacc.transpose()); - - G_measCov_Gt.block(6,6,3,3) = (1/deltaT) * (H_angles_biasomega) * - (measurementCovariance_.block(6,6,3,3) + measurementCovariance_.block(18,18,3,3) ) * - (H_angles_biasomega.transpose()); - - G_measCov_Gt.block(9,9,3,3) = deltaT * measurementCovariance_.block(9,9,3,3); - - G_measCov_Gt.block(12,12,3,3) = deltaT * measurementCovariance_.block(12,12,3,3); - - // NEW OFF BLOCK DIAGONAL TERMS - Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block(18,15,3,3) * H_angles_biasomega.transpose(); - G_measCov_Gt.block(3,6,3,3) = block23; - G_measCov_Gt.block(6,3,3,3) = block23.transpose(); - - PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + G_measCov_Gt; - - // Update preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - deltaPij_ += deltaVij_ * deltaT; - }else{ - deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT; - } - deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; - deltaRij_ = deltaRij_ * Rincr; - deltaTij_ += deltaT; - } - - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0){ - - // Note: all delta terms refer to an IMU\sensor system at t0 - - Vector body_t_a_body = msr_acc_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } - - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles){ - - // Note: all delta terms refer to an IMU\sensor system at t0 - - // Calculate the corrected measurements using the Bias object - Vector body_t_omega_body= msr_gyro_t; - - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - Matrix measurementCovariance() const {return measurementCovariance_;} - Matrix deltaRij() const {return deltaRij_.matrix();} - double deltaTij() const{return deltaTij_;} - Vector deltaPij() const {return deltaPij_;} - Vector deltaVij() const {return deltaVij_;} - Vector biasHat() const { return biasHat_.vector();} - Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;} - Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;} - Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;} - Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;} - Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;} - Matrix PreintMeasCov() const { return PreintMeasCov_;} - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); - ar & BOOST_SERIALIZATION_NVP(deltaPij_); - ar & BOOST_SERIALIZATION_NVP(deltaVij_); - ar & BOOST_SERIALIZATION_NVP(deltaRij_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); - } - }; - - private: - - typedef CombinedImuFactor This; - typedef NoiseModelFactor6 Base; - - CombinedPreintegratedMeasurements preintegratedMeasurements_; - Vector3 gravity_; - Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame - - bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect - - public: - - /** Shorthand for a smart pointer to a factor */ + /** Shorthand for a smart pointer to a factor */ #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 - typedef typename boost::shared_ptr shared_ptr; + typedef typename boost::shared_ptr shared_ptr; #else - typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; #endif - /** Default constructor - only use for serialization */ - CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {} + /** Default constructor - only use for serialization */ + CombinedImuFactor(); - /** Constructor */ - CombinedImuFactor( - Key pose_i, ///< previous pose key - Key vel_i, ///< previous velocity key - Key pose_j, ///< current pose key - Key vel_j, ///< current velocity key - Key bias_i, ///< previous bias key - Key bias_j, ///< current bias key - const CombinedPreintegratedMeasurements& preintegratedMeasurements, ///< Preintegrated IMU measurements - const Vector3& gravity, ///< gravity vector - const Vector3& omegaCoriolis, ///< rotation rate of inertial frame - boost::optional body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame - const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect - ) : - Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), - preintegratedMeasurements_(preintegratedMeasurements), - gravity_(gravity), - omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor), - use2ndOrderCoriolis_(use2ndOrderCoriolis){ - } + /** + * Constructor + * @param pose_i Previous pose key + * @param vel_i Previous velocity key + * @param pose_j Current pose key + * @param vel_j Current velocity key + * @param bias_i Previous bias key + * @param bias_j Current bias key + * @param CombinedPreintegratedMeasurements CombinedPreintegratedMeasurements IMU measurements + * @param gravity Gravity vector expressed in the global frame + * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame + * @param body_P_sensor Optional pose of the sensor frame in the body frame + * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect + */ + CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, + Key bias_j, + const CombinedPreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false); - virtual ~CombinedImuFactor() {} + virtual ~CombinedImuFactor() { + } - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const; - /** implement functions needed for Testable */ + /** implement functions needed for Testable */ - /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "CombinedImuFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << "," - << keyFormatter(this->key6()) << ")\n"; - preintegratedMeasurements_.print(" preintegrated measurements:"); - std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; - std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; - this->noiseModel_->print(" noise model: "); - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - } + /// print + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) - && equal_with_abs_tol(gravity_, e->gravity_, tol) - && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); - } + /// equals + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; - /** Access the preintegrated measurements. */ - const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; } + /** Access the preintegrated measurements. */ - const Vector3& gravity() const { return gravity_; } + const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { + return _PIM_; + } - const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + /** implement functions needed to derive from Factor */ - /** implement functions needed to derive from Factor */ + /// vector of errors + Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none, boost::optional H3 = boost::none, + boost::optional H4 = boost::none, boost::optional H5 = + boost::none, boost::optional H6 = boost::none) const; - /** vector of errors */ - Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none, - boost::optional H6 = boost::none) const - { + /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ + static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, + Vector3& vel_j, const imuBias::ConstantBias& bias_i, + const CombinedPreintegratedMeasurements& PIM, const Vector3& gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, + boost::optional deltaPij_biascorrected_out = boost::none, + boost::optional deltaVij_biascorrected_out = boost::none) { + PoseVelocityBias PVB( + PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, + use2ndOrderCoriolis, deltaPij_biascorrected_out, + deltaVij_biascorrected_out)); + pose_j = PVB.pose; + vel_j = PVB.velocity; + } - const double& deltaTij = preintegratedMeasurements_.deltaTij_; - const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); +private: - // we give some shorter name to rotations and translations - const Rot3 Rot_i = pose_i.rotation(); - const Rot3 Rot_j = pose_j.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - const Vector3 pos_j = pose_j.translation().vector(); + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("NoiseModelFactor6", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(_PIM_); + ar & BOOST_SERIALIZATION_NVP(gravity_); + ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + } +}; +// class CombinedImuFactor - // We compute factor's Jacobians, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term - - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - - const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); - - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); - - /* - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij - + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), - // dfP/dPi - - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij - + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), - // dfV/dPi - skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Matrix3::Zero(), - //dBiasAcc/dPi - Matrix3::Zero(), Matrix3::Zero(), - //dBiasOmega/dPi - Matrix3::Zero(), Matrix3::Zero(); - */ - if(H1) { - H1->resize(15,6); - - Matrix3 dfPdPi; - Matrix3 dfVdPi; - if(use2ndOrderCoriolis_){ - dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; - dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; - } - else{ - dfPdPi = - Rot_i.matrix(); - dfVdPi = Matrix3::Zero(); - } - - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), - // dfP/dPi - dfPdPi, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), - // dfV/dPi - dfVdPi, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Matrix3::Zero(), - //dBiasAcc/dPi - Matrix3::Zero(), Matrix3::Zero(), - //dBiasOmega/dPi - Matrix3::Zero(), Matrix3::Zero(); - } - - if(H2) { - H2->resize(15,3); - (*H2) << - // dfP/dVi - - Matrix3::Identity() * deltaTij - + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - - Matrix3::Identity() - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Matrix3::Zero(), - //dBiasAcc/dVi - Matrix3::Zero(), - //dBiasOmega/dVi - Matrix3::Zero(); - } - - if(H3) { - - H3->resize(15,6); - (*H3) << - // dfP/dPosej - Matrix3::Zero(), Rot_j.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(), - //dBiasAcc/dPosej - Matrix3::Zero(), Matrix3::Zero(), - //dBiasOmega/dPosej - Matrix3::Zero(), Matrix3::Zero(); - } - - if(H4) { - H4->resize(15,3); - (*H4) << - // dfP/dVj - Matrix3::Zero(), - // dfV/dVj - Matrix3::Identity(), - // dfR/dVj - Matrix3::Zero(), - //dBiasAcc/dVj - Matrix3::Zero(), - //dBiasOmega/dVj - Matrix3::Zero(); - } - - if(H5) { - const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); - const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; - - H5->resize(15,6); - (*H5) << - // dfP/dBias_i - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, - // dfV/dBias_i - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, - // dfR/dBias_i - Matrix::Zero(3,3), - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega), - //dBiasAcc/dBias_i - -Matrix3::Identity(), Matrix3::Zero(), - //dBiasOmega/dBias_i - Matrix3::Zero(), -Matrix3::Identity(); - } - - if(H6) { - - H6->resize(15,6); - (*H6) << - // dfP/dBias_j - Matrix3::Zero(), Matrix3::Zero(), - // dfV/dBias_j - Matrix3::Zero(), Matrix3::Zero(), - // dfR/dBias_j - Matrix3::Zero(), Matrix3::Zero(), - //dBiasAcc/dBias_j - Matrix3::Identity(), Matrix3::Zero(), - //dBiasOmega/dBias_j - Matrix3::Zero(), Matrix3::Identity(); - } - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 fp = - pos_j - pos_i - - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) - - vel_i * deltaTij - + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - - 0.5 * gravity_ * deltaTij*deltaTij; - - const Vector3 fv = - vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) - + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term - - gravity_ * deltaTij; - - const Vector3 fR = Rot3::Logmap(fRhat); - - const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer(); - - const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope(); - - Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15 - - return r; - } - - - /** predicted states from IMU */ - static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias_i, - const CombinedPreintegratedMeasurements& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none, - const bool use2ndOrderCoriolis = false) - { - - const double& deltaTij = preintegratedMeasurements.deltaTij_; - const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); - - const Rot3 Rot_i = pose_i.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ - + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; - - Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ - + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); - - if(use2ndOrderCoriolis){ - pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position - vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity - } - - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); - - Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); - - return PoseVelocityBias(pose_j, vel_j, bias_i); - } - - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor6", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); - ar & BOOST_SERIALIZATION_NVP(gravity_); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); - } - }; // \class CombinedImuFactor - - typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements; +typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements; } /// namespace gtsam diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index fd769b188..bfd3ebb52 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -24,15 +24,15 @@ namespace gtsam { //*************************************************************************** void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << s << "GPSFactor on " << keyFormatter(this->key()) << "\n"; + cout << s << "GPSFactor on " << keyFormatter(key()) << "\n"; nT_.print(" prior mean: "); - this->noiseModel_->print(" noise model: "); + noiseModel_->print(" noise model: "); } //*************************************************************************** bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && this->nT_.equals(e->nT_, tol); + return e != NULL && Base::equals(*e, tol) && nT_.equals(e->nT_, tol); } //*************************************************************************** @@ -43,8 +43,7 @@ Vector GPSFactor::evaluateError(const Pose3& p, H->block < 3, 3 > (0, 0) << zeros(3, 3); H->block < 3, 3 > (0, 3) << p.rotation().matrix(); } - // manifold equivalent of h(x)-z -> log(z,h(x)) - return nT_.localCoordinates(p.translation()); + return (p.translation() -nT_).vector(); } //*************************************************************************** diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index 6902f81f1..cd5fa71d2 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -103,7 +103,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 466d93203..571fb7249 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -17,11 +17,10 @@ #pragma once -#include #include #include -#include -#include +#include +#include /* * NOTES: @@ -39,53 +38,63 @@ namespace gtsam { /// All bias models live in the imuBias namespace namespace imuBias { - class ConstantBias { - private: - Vector3 biasAcc_; - Vector3 biasGyro_; +class ConstantBias { +private: + Vector3 biasAcc_; + Vector3 biasGyro_; - public: - /// dimension of the variable - used to autodetect sizes - static const size_t dimension = 6; +public: + /// dimension of the variable - used to autodetect sizes + static const size_t dimension = 6; - ConstantBias(): - biasAcc_(0.0, 0.0, 0.0), biasGyro_(0.0, 0.0, 0.0) { - } + ConstantBias() : + biasAcc_(0.0, 0.0, 0.0), biasGyro_(0.0, 0.0, 0.0) { + } - ConstantBias(const Vector3& biasAcc, const Vector3& biasGyro): + ConstantBias(const Vector3& biasAcc, const Vector3& biasGyro) : biasAcc_(biasAcc), biasGyro_(biasGyro) { + } + + ConstantBias(const Vector6& v) : + biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) { + } + + /** return the accelerometer and gyro biases in a single vector */ + Vector6 vector() const { + Vector6 v; + v << biasAcc_, biasGyro_; + return v; + } + + /** get accelerometer bias */ + const Vector3& accelerometer() const { + return biasAcc_; + } + + /** get gyroscope bias */ + const Vector3& gyroscope() const { + return biasGyro_; + } + + /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */ + Vector3 correctAccelerometer(const Vector3& measurement, + boost::optional H = boost::none) const { + if (H) { + H->resize(3, 6); + (*H) << -Matrix3::Identity(), Matrix3::Zero(); } + return measurement - biasAcc_; + } - /** return the accelerometer and gyro biases in a single vector */ - Vector6 vector() const { - Vector6 v; - v << biasAcc_, biasGyro_; - return v; - } - - /** get accelerometer bias */ - const Vector3& accelerometer() const { return biasAcc_; } - - /** get gyroscope bias */ - const Vector3& gyroscope() const { return biasGyro_; } - - /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */ - Vector3 correctAccelerometer(const Vector3& measurement, boost::optional H=boost::none) const { - if (H) { - H->resize(3, 6); - (*H) << -Matrix3::Identity(), Matrix3::Zero(); - } - return measurement - biasAcc_; - } - - /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */ - Vector3 correctGyroscope(const Vector3& measurement, boost::optional H=boost::none) const { - if (H) { - H->resize(3, 6); - (*H) << Matrix3::Zero(), -Matrix3::Identity(); - } - return measurement - biasGyro_; + /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */ + Vector3 correctGyroscope(const Vector3& measurement, + boost::optional H = boost::none) const { + if (H) { + H->resize(3, 6); + (*H) << Matrix3::Zero(), -Matrix3::Identity(); } + return measurement - biasGyro_; + } // // H1: Jacobian w.r.t. IMUBias // // H2: Jacobian w.r.t. pose @@ -117,123 +126,97 @@ namespace imuBias { //// 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 + ".biasAcc [" << biasAcc_.transpose() << "]" << std::endl; - std::cout << s + ".biasGyro [" << biasGyro_.transpose() << "]" << std::endl; - } +/// 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; + } - /** equality up to tolerance */ - inline bool equals(const ConstantBias& expected, double tol=1e-5) const { - return equal_with_abs_tol(biasAcc_, expected.biasAcc_, tol) - && equal_with_abs_tol(biasGyro_, expected.biasGyro_, tol); - } + /** equality up to tolerance */ + inline bool equals(const ConstantBias& expected, double tol = 1e-5) const { + return equal_with_abs_tol(biasAcc_, expected.biasAcc_, tol) + && equal_with_abs_tol(biasGyro_, expected.biasGyro_, tol); + } - /// @} - /// @name Manifold - /// @{ + /// @} + /// @name Group + /// @{ - /// dimension of the variable - used to autodetect sizes - inline static size_t Dim() { return dimension; } + /** identity for group operation */ + static ConstantBias identity() { + return ConstantBias(); + } - /// return dimensionality of tangent space - inline size_t dim() const { return dimension; } + /** inverse */ + inline ConstantBias operator-() const { + return ConstantBias(-biasAcc_, -biasGyro_); + } - /** Update the bias with a tangent space update */ - inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); } + /** addition */ + ConstantBias operator+(const ConstantBias& b) const { + return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_); + } - /** @return the local coordinates of another object */ - inline Vector localCoordinates(const ConstantBias& b) const { return b.vector() - vector(); } + /** subtraction */ + ConstantBias operator-(const ConstantBias& b) const { + return ConstantBias(biasAcc_ - b.biasAcc_, biasGyro_ - b.biasGyro_); + } - /// @} - /// @name Group - /// @{ + /// @} - /** identity for group operation */ - static ConstantBias identity() { return ConstantBias(); } + /// @name Deprecated + /// @{ + ConstantBias inverse() { + return -(*this); + } + ConstantBias compose(const ConstantBias& q) { + return (*this) + q; + } + ConstantBias between(const ConstantBias& q) { + return q - (*this); + } + Vector6 localCoordinates(const ConstantBias& q) { + return between(q).vector(); + } + ConstantBias retract(const Vector6& v) { + return compose(ConstantBias(v)); + } + static Vector6 Logmap(const ConstantBias& p) { + return p.vector(); + } + static ConstantBias Expmap(const Vector6& v) { + return ConstantBias(v); + } + /// @} - /** invert the object and yield a new one */ - inline ConstantBias inverse(boost::optional H=boost::none) const { - if(H) *H = -gtsam::Matrix::Identity(6,6); +private: - return ConstantBias(-biasAcc_, -biasGyro_); - } + /// @name Advanced Interface + /// @{ - ConstantBias compose(const ConstantBias& b, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = gtsam::Matrix::Identity(6,6); - if(H2) *H2 = gtsam::Matrix::Identity(6,6); + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("imuBias::ConstantBias", *this); + ar & BOOST_SERIALIZATION_NVP(biasAcc_); + ar & BOOST_SERIALIZATION_NVP(biasGyro_); + } - return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_); - } + /// @} - /** between operation */ - ConstantBias between(const ConstantBias& b, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -gtsam::Matrix::Identity(6,6); - if(H2) *H2 = gtsam::Matrix::Identity(6,6); - - return ConstantBias(b.biasAcc_ - biasAcc_, b.biasGyro_ - biasGyro_); - } - - /// @} - /// @name Lie Group - /// @{ - - /** Expmap around identity */ - static inline ConstantBias Expmap(const Vector& v) { return ConstantBias(v.head(3), v.tail(3)); } - - /** Logmap around identity - just returns with default cast back */ - static inline Vector Logmap(const ConstantBias& b) { return b.vector(); } - - /// @} - - private: - - /// @name Advanced Interface - /// @{ - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) - { - ar & boost::serialization::make_nvp("imuBias::ConstantBias",*this); - ar & BOOST_SERIALIZATION_NVP(biasAcc_); - ar & BOOST_SERIALIZATION_NVP(biasGyro_); - } - - /// @} - - }; // ConstantBias class - - -} // namespace ImuBias - -// Define GTSAM traits -namespace traits { +}; // ConstantBias class +} // namespace imuBias template<> -struct is_group : public boost::true_type { +struct traits : public internal::VectorSpace< + imuBias::ConstantBias> { }; -template<> -struct is_manifold : public boost::true_type { -}; - -template<> -struct dimension : public boost::integral_constant { -}; - -} - } // namespace gtsam - diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp new file mode 100644 index 000000000..01de5a8f3 --- /dev/null +++ b/gtsam/navigation/ImuFactor.cpp @@ -0,0 +1,179 @@ +/* ---------------------------------------------------------------------------- + + * 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 ImuFactor.cpp + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#include + +/* External or standard includes */ +#include + +namespace gtsam { + +using namespace std; + +//------------------------------------------------------------------------------ +// Inner class PreintegratedMeasurements +//------------------------------------------------------------------------------ +ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements( + const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, + const bool use2ndOrderIntegration) : + PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance, + integrationErrorCovariance, use2ndOrderIntegration) { + preintMeasCov_.setZero(); +} + +//------------------------------------------------------------------------------ +void ImuFactor::PreintegratedMeasurements::print(const string& s) const { + PreintegrationBase::print(s); +} + +//------------------------------------------------------------------------------ +bool ImuFactor::PreintegratedMeasurements::equals( + const PreintegratedMeasurements& expected, double tol) const { + return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) + && PreintegrationBase::equals(expected, tol); +} + +//------------------------------------------------------------------------------ +void ImuFactor::PreintegratedMeasurements::resetIntegration() { + PreintegrationBase::resetIntegration(); + preintMeasCov_.setZero(); +} + +//------------------------------------------------------------------------------ +void ImuFactor::PreintegratedMeasurements::integrateMeasurement( + const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, + boost::optional body_P_sensor, OptionalJacobian<9, 9> F_test, + OptionalJacobian<9, 9> G_test) { + + Vector3 correctedAcc, correctedOmega; + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, + correctedAcc, correctedOmega, body_P_sensor); + + const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement + Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr + const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement + + // Update Jacobians + updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); + + // Update preintegrated measurements (also get Jacobian) + const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test + Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx) + updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F); + + // first order covariance propagation: + // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework + /* ----------------------------------------------------------------------------------------------------------------------- */ + // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); + // NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise + // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) + // NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done block-wise, + // as G and measurementCovariance are block-diagonal matrices + preintMeasCov_ = F * preintMeasCov_ * F.transpose(); + preintMeasCov_.block<3, 3>(0, 0) += integrationCovariance() * deltaT; + preintMeasCov_.block<3, 3>(3, 3) += R_i * accelerometerCovariance() + * R_i.transpose() * deltaT; + preintMeasCov_.block<3, 3>(6, 6) += D_Rincr_integratedOmega + * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT; + + Matrix3 F_pos_noiseacc; + if (use2ndOrderIntegration()) { + F_pos_noiseacc = 0.5 * R_i * deltaT * deltaT; + preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc + * accelerometerCovariance() * F_pos_noiseacc.transpose(); + Matrix3 temp = F_pos_noiseacc * accelerometerCovariance() * R_i.transpose(); // already includes 1/deltaT + preintMeasCov_.block<3, 3>(0, 3) += temp; + preintMeasCov_.block<3, 3>(3, 0) += temp.transpose(); + } + + // F_test and G_test are given as output for testing purposes and are not needed by the factor + if (F_test) { // This in only for testing + (*F_test) << F; + } + if (G_test) { // This in only for testing & documentation, while the actual computation is done block-wise + if (!use2ndOrderIntegration()) + F_pos_noiseacc = Z_3x3; + + // intNoise accNoise omegaNoise + (*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos + Z_3x3, R_i * deltaT, Z_3x3, // vel + Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle + } +} + +//------------------------------------------------------------------------------ +// ImuFactor methods +//------------------------------------------------------------------------------ +ImuFactor::ImuFactor() : + ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) { +} + +//------------------------------------------------------------------------------ +ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, + const PreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : + Base( + noiseModel::Gaussian::Covariance( + preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, + vel_j, bias), ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, + use2ndOrderCoriolis), _PIM_(preintegratedMeasurements) { +} + +//------------------------------------------------------------------------------ +gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); +} + +//------------------------------------------------------------------------------ +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"; + ImuFactorBase::print(""); + _PIM_.print(" preintegrated measurements:"); + // Print standard deviations on covariance only + cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose() << endl; +} + +//------------------------------------------------------------------------------ +bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const { + const This *e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) + && ImuFactorBase::equals(*e, tol); +} + +//------------------------------------------------------------------------------ +Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, boost::optional H1, + boost::optional H2, boost::optional H3, + boost::optional H4, boost::optional H5) const { + + return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, + gravity_, omegaCoriolis_, use2ndOrderCoriolis_, H1, H2, H3, H4, H5); +} + +} /// namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index d1b8a9d80..50e6c835f 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -11,614 +11,232 @@ /** * @file ImuFactor.h - * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert **/ #pragma once /* GTSAM includes */ #include -#include -#include -#include +#include +#include #include -/* External or standard includes */ -#include - - namespace gtsam { + /** - * Struct to hold return variables by the Predict Function + * + * @addtogroup SLAM + * + * If you are using the factor, please cite: + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally + * independent sets in factor graphs: a unifying perspective based on smart factors, + * Int. Conf. on Robotics and Automation (ICRA), 2014. + * + ** REFERENCES: + * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", + * Volume 2, 2008. + * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for + * High-Dynamic Motion in Built Environments Without Initial Conditions", + * TRO, 28(1):61-76, 2012. + * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: + * Computation of the Jacobian Matrices", Tech. Report, 2013. */ -struct PoseVelocity { - Pose3 pose; - Vector3 velocity; - PoseVelocity(const Pose3& _pose, const Vector3& _velocity) : - pose(_pose), velocity(_velocity) { - } -}; + +/** + * ImuFactor is a 5-ways factor involving previous state (pose and velocity of + * the vehicle at previous time step), current state (pose and velocity at + * current time step), and the bias estimate. Following the preintegration + * scheme proposed in [2], the ImuFactor includes many IMU measurements, which + * are "summarized" using the PreintegratedMeasurements class. + * Note that this factor does not model "temporal consistency" of the biases + * (which are usually slowly varying quantities), which is up to the caller. + * See also CombinedImuFactor for a class that does this for you. + */ +class ImuFactor: public NoiseModelFactor5, public ImuFactorBase { +public: /** - * - * @addtogroup SLAM - * - * If you are using the factor, please cite: - * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally - * independent sets in factor graphs: a unifying perspective based on smart factors, - * Int. Conf. on Robotics and Automation (ICRA), 2014. - * - ** REFERENCES: - * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built - * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. - * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. + * PreintegratedMeasurements accumulates (integrates) the IMU measurements + * (rotation rates and accelerations) and the corresponding covariance matrix. + * The measurements are then used to build the Preintegrated IMU factor. + * Integration is done incrementally (ideally, one integrates the measurement + * as soon as it is received from the IMU) so as to avoid costly integration + * at time of factor construction. */ + class PreintegratedMeasurements: public PreintegrationBase { - class ImuFactor: public NoiseModelFactor5 { - public: + friend class ImuFactor; - /** Struct to store results of preintegrating IMU measurements. Can be build - * incrementally so as to avoid costly integration at time of factor construction. */ + protected: - /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) - * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/ - class PreintegratedMeasurements { - friend class ImuFactor; - protected: - imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration - Matrix measurementCovariance_; ///< (continuous-time uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9) - - Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) - Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j - - Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias - Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias - Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias - Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) - bool use2ndOrderIntegration_; ///< Controls the order of integration - public: - /** Default constructor, initialize with no IMU measurements */ - PreintegratedMeasurements( - const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases - const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measured Angular Rate - const Matrix3& integrationErrorCovariance, ///< Covariance matrix of integration errors - const bool use2ndOrderIntegration = false ///< Controls the order of integration - ) : biasHat_(bias), measurementCovariance_(9,9), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0), - delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()), - delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()), - delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(use2ndOrderIntegration) - { - measurementCovariance_ << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance; - PreintMeasCov_ = Matrix::Zero(9,9); - } - - PreintegratedMeasurements() : - biasHat_(imuBias::ConstantBias()), measurementCovariance_(9,9), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0), - delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()), - delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()), - delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(false) - { - measurementCovariance_ = Matrix::Zero(9,9); - PreintMeasCov_ = Matrix::Zero(9,9); - } - - /** print */ - void print(const std::string& s = "Preintegrated Measurements:") const { - std::cout << s << std::endl; - biasHat_.print(" biasHat"); - std::cout << " deltaTij " << deltaTij_ << std::endl; - std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; - std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; - deltaRij_.print(" deltaRij "); - std::cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << std::endl; - std::cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl; - } - - /** equals */ - bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const { - return biasHat_.equals(expected.biasHat_, tol) - && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) - && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) - && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) - && deltaRij_.equals(expected.deltaRij_, tol) - && std::fabs(deltaTij_ - expected.deltaTij_) < tol - && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) - && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) - && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) - && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); - } - Matrix measurementCovariance() const {return measurementCovariance_;} - Matrix deltaRij() const {return deltaRij_.matrix();} - double deltaTij() const{return deltaTij_;} - Vector deltaPij() const {return deltaPij_;} - Vector deltaVij() const {return deltaVij_;} - Vector biasHat() const { return biasHat_.vector();} - Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;} - Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;} - Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;} - Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;} - Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;} - Matrix preintMeasCov() const { return PreintMeasCov_;} - - - void resetIntegration(){ - deltaPij_ = Vector3::Zero(); - deltaVij_ = Vector3::Zero(); - deltaRij_ = Rot3(); - deltaTij_ = 0.0; - delPdelBiasAcc_ = Matrix3::Zero(); - delPdelBiasOmega_ = Matrix3::Zero(); - delVdelBiasAcc_ = Matrix3::Zero(); - delVdelBiasOmega_ = Matrix3::Zero(); - delRdelBiasOmega_ = Matrix3::Zero(); - PreintMeasCov_ = Matrix::Zero(9,9); - } - - /** Add a single IMU measurement to the preintegration. */ - void integrateMeasurement( - const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) - const Vector3& measuredOmega, ///< Measured angular velocity (in body frame) - double deltaT, ///< Time step - boost::optional body_P_sensor = boost::none ///< Sensor frame - ) { - - // NOTE: order is important here because each update uses old values. - // First we compensate the measurements for the bias - Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); - - // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame - if(body_P_sensor){ - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } - - const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr - - // Update Jacobians - /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; - }else{ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() - * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_; - } - delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; - delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; - delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT; - - // Update preintegrated measurements covariance - /* ----------------------------------------------------------------------------------------------------------------------- */ - Matrix3 Z_3x3 = Matrix3::Zero(); - Matrix3 I_3x3 = Matrix3::Identity(); - const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); - - Rot3 Rot_j = deltaRij_ * Rincr; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); - - // 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 - Matrix H_pos_pos = I_3x3; - Matrix H_pos_vel = I_3x3 * deltaT; - Matrix H_pos_angles = Z_3x3; - - Matrix H_vel_pos = Z_3x3; - Matrix H_vel_vel = I_3x3; - Matrix H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); - - Matrix H_angles_pos = Z_3x3; - Matrix H_angles_vel = Z_3x3; - Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); - - // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix F(9,9); - F << H_pos_pos, H_pos_vel, H_pos_angles, - H_vel_pos, H_vel_vel, H_vel_angles, - H_angles_pos, H_angles_vel, H_angles_angles; - - // first order uncertainty propagation: - // the deltaT allows to pass from continuous time noise to discrete time noise - // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) - // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT - PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ; - - // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT - // - // Matrix G(9,9); - // G << I_3x3 * deltaT, Z_3x3, Z_3x3, - // Z_3x3, deltaRij.matrix() * deltaT, Z_3x3, - // Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT; - // - // PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); - - // Update preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - deltaPij_ += deltaVij_ * deltaT; - }else{ - deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT; - } - deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; - deltaRij_ = deltaRij_ * Rincr; - deltaTij_ += deltaT; - } - - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0){ - - // Note: all delta terms refer to an IMU\sensor system at t0 - - Vector body_t_a_body = msr_acc_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } - - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles){ - - // Note: all delta terms refer to an IMU\sensor system at t0 - - // Calculate the corrected measurements using the Bias object - Vector body_t_omega_body= msr_gyro_t; - - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); - ar & BOOST_SERIALIZATION_NVP(deltaPij_); - ar & BOOST_SERIALIZATION_NVP(deltaVij_); - ar & BOOST_SERIALIZATION_NVP(deltaRij_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); - } - }; - - private: - - typedef ImuFactor This; - typedef NoiseModelFactor5 Base; - - PreintegratedMeasurements preintegratedMeasurements_; - Vector3 gravity_; - Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame - - bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + Eigen::Matrix preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] + ///< (first-order propagation from *measurementCovariance*). public: - /** Shorthand for a smart pointer to a factor */ -#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 - typedef typename boost::shared_ptr shared_ptr; -#else - typedef boost::shared_ptr shared_ptr; -#endif + /** + * Default constructor, initializes the class with no measurements + * @param bias Current estimate of acceleration and rotation rate biases + * @param measuredAccCovariance Covariance matrix of measuredAcc + * @param measuredOmegaCovariance Covariance matrix of measured Angular Rate + * @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position) + * @param use2ndOrderIntegration Controls the order of integration + * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) + */ + PreintegratedMeasurements(const imuBias::ConstantBias& bias, + const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, + const bool use2ndOrderIntegration = false); - /** Default constructor - only use for serialization */ - ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()), use2ndOrderCoriolis_(false){} + /// print + void print(const std::string& s = "Preintegrated Measurements:") const; - /** Constructor */ - ImuFactor( - Key pose_i, ///< previous pose key - Key vel_i, ///< previous velocity key - Key pose_j, ///< current pose key - Key vel_j, ///< current velocity key - Key bias, ///< previous bias key - const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated IMU measurements - const Vector3& gravity, ///< gravity vector - const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame - boost::optional body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame - const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect - ) : - Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias), - preintegratedMeasurements_(preintegratedMeasurements), - gravity_(gravity), - omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor), - use2ndOrderCoriolis_(use2ndOrderCoriolis){ + /// equals + bool equals(const PreintegratedMeasurements& expected, + double tol = 1e-9) const; + + /// Re-initialize PreintegratedMeasurements + void resetIntegration(); + + /** + * Add a single IMU measurement to the preintegration. + * @param measuredAcc Measured acceleration (in body frame, as given by the sensor) + * @param measuredOmega Measured angular velocity (as given by the sensor) + * @param deltaT Time interval between this and the last IMU measurement + * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) + * @param Fout, Gout Jacobians used internally (only needed for testing) + */ + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, double deltaT, + boost::optional body_P_sensor = boost::none, + OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = + boost::none); + + /// methods to access class variables + Matrix preintMeasCov() const { + return preintMeasCov_; } - virtual ~ImuFactor() {} - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - /** implement functions needed for Testable */ - - /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "ImuFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << ")\n"; - preintegratedMeasurements_.print(" preintegrated measurements:"); - std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; - std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; - this->noiseModel_->print(" noise model: "); - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - } - - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) - && equal_with_abs_tol(gravity_, e->gravity_, tol) - && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); - } - - /** Access the preintegrated measurements. */ - const PreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; } - - const Vector3& gravity() const { return gravity_; } - - const Vector3& omegaCoriolis() const { return omegaCoriolis_; } - - /** implement functions needed to derive from Factor */ - - /** vector of errors */ - Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none) const - { - - const double& deltaTij = preintegratedMeasurements_.deltaTij_; - const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); - - // we give some shorter name to rotations and translations - const Rot3 Rot_i = pose_i.rotation(); - const Rot3 Rot_j = pose_j.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - const Vector3 pos_j = pose_j.translation().vector(); - - // We compute factor's Jacobians - /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term - - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - - const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); - - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); - - if(H1) { - H1->resize(9,6); - - Matrix3 dfPdPi; - Matrix3 dfVdPi; - if(use2ndOrderCoriolis_){ - dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; - dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; - } - else{ - dfPdPi = - Rot_i.matrix(); - dfVdPi = Matrix3::Zero(); - } - - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), - // dfP/dPi - dfPdPi, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), - // dfV/dPi - dfVdPi, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Matrix3::Zero(); - } - - if(H2) { - H2->resize(9,3); - (*H2) << - // dfP/dVi - - Matrix3::Identity() * deltaTij - + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - - Matrix3::Identity() - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Matrix3::Zero(); - } - - if(H3) { - - H3->resize(9,6); - (*H3) << - // dfP/dPosej - Matrix3::Zero(), Rot_j.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(); - } - - if(H4) { - H4->resize(9,3); - (*H4) << - // dfP/dVj - Matrix3::Zero(), - // dfV/dVj - Matrix3::Identity(), - // dfR/dVj - Matrix3::Zero(); - } - - if(H5) { - - const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); - const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; - - H5->resize(9,6); - (*H5) << - // dfP/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, - // dfV/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, - // dfR/dBias - Matrix::Zero(3,3), - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); - } - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 fp = - pos_j - pos_i - - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) - - vel_i * deltaTij - + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - - 0.5 * gravity_ * deltaTij*deltaTij; - - const Vector3 fv = - vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) - + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term - - gravity_ * deltaTij; - - const Vector3 fR = Rot3::Logmap(fRhat); - - Vector r(9); r << fp, fv, fR; - return r; - } - - - /** predicted states from IMU */ - static PoseVelocity Predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none, - const bool use2ndOrderCoriolis = false) - { - - const double& deltaTij = preintegratedMeasurements.deltaTij_; - const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); - - const Rot3 Rot_i = pose_i.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ - + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; - - Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ - + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); - - if(use2ndOrderCoriolis){ - pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position - vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity - } - - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); - - Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); - return PoseVelocity(pose_j, vel_j); - } - - private: - /** Serialization function */ + /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor5", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); - ar & BOOST_SERIALIZATION_NVP(gravity_); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); } - }; // \class ImuFactor + }; - typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements; +private: + + typedef ImuFactor This; + typedef NoiseModelFactor5 Base; + + PreintegratedMeasurements _PIM_; + +public: + + /** Shorthand for a smart pointer to a factor */ +#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 + typedef typename boost::shared_ptr shared_ptr; +#else + typedef boost::shared_ptr shared_ptr; +#endif + + /** Default constructor - only use for serialization */ + ImuFactor(); + + /** + * Constructor + * @param pose_i Previous pose key + * @param vel_i Previous velocity key + * @param pose_j Current pose key + * @param vel_j Current velocity key + * @param bias Previous bias key + * @param preintegratedMeasurements Preintegrated IMU measurements + * @param gravity Gravity vector expressed in the global frame + * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame + * @param body_P_sensor Optional pose of the sensor frame in the body frame + * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect + */ + ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, + const PreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false); + + virtual ~ImuFactor() { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const; + + /** implement functions needed for Testable */ + + /// print + 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. */ + + const PreintegratedMeasurements& preintegratedMeasurements() const { + return _PIM_; + } + + /** implement functions needed to derive from Factor */ + + /// vector of errors + Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias, boost::optional H1 = + boost::none, boost::optional H2 = boost::none, + boost::optional H3 = boost::none, boost::optional H4 = + boost::none, boost::optional H5 = boost::none) const; + + /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ + static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, + Vector3& vel_j, const imuBias::ConstantBias& bias_i, + const PreintegratedMeasurements& PIM, const Vector3& gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, + boost::optional deltaPij_biascorrected_out = boost::none, + boost::optional deltaVij_biascorrected_out = boost::none) { + PoseVelocityBias PVB( + PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, + use2ndOrderCoriolis, deltaPij_biascorrected_out, + deltaVij_biascorrected_out)); + pose_j = PVB.pose; + vel_j = PVB.velocity; + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("NoiseModelFactor5", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(_PIM_); + ar & BOOST_SERIALIZATION_NVP(gravity_); + ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + } +}; +// class ImuFactor + +typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements; } /// namespace gtsam diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h new file mode 100644 index 000000000..1e4e51220 --- /dev/null +++ b/gtsam/navigation/ImuFactorBase.h @@ -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 + + * -------------------------------------------------------------------------- */ + +/** + * @file PreintegrationBase.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#pragma once + +/* GTSAM includes */ +#include +#include + +namespace gtsam { + +class ImuFactorBase { + +protected: + + Vector3 gravity_; + Vector3 omegaCoriolis_; + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + +public: + + /** Default constructor - only use for serialization */ + ImuFactorBase() : + gravity_(Vector3(0.0, 0.0, 9.81)), omegaCoriolis_(Vector3(0.0, 0.0, 0.0)), body_P_sensor_( + boost::none), use2ndOrderCoriolis_(false) { + } + + /** + * Default constructor, stores basic quantities required by the Imu factors + * @param gravity Gravity vector expressed in the global frame + * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame + * @param body_P_sensor Optional pose of the sensor frame in the body frame + * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect + */ + ImuFactorBase(const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false) : + gravity_(gravity), omegaCoriolis_(omegaCoriolis), body_P_sensor_( + body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) { + } + + /// Methods to access class variables + const Vector3& gravity() const { + return gravity_; + } + const Vector3& omegaCoriolis() const { + return omegaCoriolis_; + } + + /// Needed for testable + //------------------------------------------------------------------------------ + void print(const std::string& /*s*/) const { + std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; + std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" + << std::endl; + std::cout << " use2ndOrderCoriolis: [ " << use2ndOrderCoriolis_ << " ]" + << std::endl; + if (this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + } + + /// Needed for testable + //------------------------------------------------------------------------------ + bool equals(const ImuFactorBase& expected, double tol) const { + return equal_with_abs_tol(gravity_, expected.gravity_, tol) + && equal_with_abs_tol(omegaCoriolis_, expected.omegaCoriolis_, tol) + && (use2ndOrderCoriolis_ == expected.use2ndOrderCoriolis_) + && ((!body_P_sensor_ && !expected.body_P_sensor_) + || (body_P_sensor_ && expected.body_P_sensor_ + && body_P_sensor_->equals(*expected.body_P_sensor_))); + } + +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h new file mode 100644 index 000000000..2379f58af --- /dev/null +++ b/gtsam/navigation/PreintegratedRotation.h @@ -0,0 +1,154 @@ +/* ---------------------------------------------------------------------------- + + * 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 PreintegratedRotation.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#pragma once + +#include + +namespace gtsam { + +/** + * PreintegratedRotation is the base class for all PreintegratedMeasurements + * classes (in AHRSFactor, ImuFactor, and CombinedImuFactor). + * It includes the definitions of the preintegrated rotation. + */ +class PreintegratedRotation { + + Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) + double deltaTij_; ///< Time interval from i to j + + /// Jacobian of preintegrated rotation w.r.t. angular rate bias + Matrix3 delRdelBiasOmega_; + Matrix3 gyroscopeCovariance_; ///< continuous-time "Covariance" of gyroscope measurements + +public: + + /** + * Default constructor, initializes the variables in the base class + */ + PreintegratedRotation(const Matrix3& measuredOmegaCovariance) : + deltaRij_(Rot3()), deltaTij_(0.0), delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_( + measuredOmegaCovariance) { + } + + /// methods to access class variables + Matrix3 deltaRij() const { + return deltaRij_.matrix(); + } // expensive + Vector3 thetaRij(OptionalJacobian<3, 3> H = boost::none) const { + return Rot3::Logmap(deltaRij_, H); + } // super-expensive + const double& deltaTij() const { + return deltaTij_; + } + const Matrix3& delRdelBiasOmega() const { + return delRdelBiasOmega_; + } + const Matrix3& gyroscopeCovariance() const { + return gyroscopeCovariance_; + } + + /// Needed for testable + void print(const std::string& s) const { + std::cout << s << std::endl; + std::cout << " deltaTij [" << deltaTij_ << "]" << std::endl; + std::cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << std::endl; + } + + /// Needed for testable + bool equals(const PreintegratedRotation& expected, double tol) const { + return deltaRij_.equals(expected.deltaRij_, tol) + && fabs(deltaTij_ - expected.deltaTij_) < tol + && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, + tol) + && equal_with_abs_tol(gyroscopeCovariance_, + expected.gyroscopeCovariance_, tol); + } + + /// Re-initialize PreintegratedMeasurements + void resetIntegration() { + deltaRij_ = Rot3(); + deltaTij_ = 0.0; + delRdelBiasOmega_ = Z_3x3; + } + + /// Update preintegrated measurements + void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT, + OptionalJacobian<3, 3> H = boost::none) { + deltaRij_ = deltaRij_.compose(incrR, H, boost::none); + deltaTij_ += deltaT; + } + + /** + * Update Jacobians to be used during preintegration + * TODO: explain arguments + */ + void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega, + const Rot3& incrR, double deltaT) { + const Matrix3 incrRt = incrR.transpose(); + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + - D_Rincr_integratedOmega * deltaT; + } + + /// Return a bias corrected version of the integrated rotation - expensive + Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr) const { + return deltaRij_ * Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr); + } + + /// Get so<3> version of bias corrected rotation, with optional Jacobian + // Implements: log( deltaRij_ * expmap(delRdelBiasOmega_ * biasOmegaIncr) ) + Vector3 biascorrectedThetaRij(const Vector3& biasOmegaIncr, + OptionalJacobian<3, 3> H = boost::none) const { + // First, we correct deltaRij using the biasOmegaIncr, rotated + const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); + + if (H) { + Matrix3 Jrinv_theta_bc; + // This was done via an expmap, now we go *back* to so<3> + const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, + Jrinv_theta_bc); + const Matrix3 Jr_JbiasOmegaIncr = // + Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr); + (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; + return biascorrectedOmega; + } + //else + return Rot3::Logmap(deltaRij_biascorrected); + } + + /// Integrate coriolis correction in body frame rot_i + Vector3 integrateCoriolis(const Rot3& rot_i, + const Vector3& omegaCoriolis) const { + return rot_i.transpose() * omegaCoriolis * deltaTij(); + } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(deltaRij_); + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); + } +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp new file mode 100644 index 000000000..496569599 --- /dev/null +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -0,0 +1,357 @@ +/* ---------------------------------------------------------------------------- + + * 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 PreintegrationBase.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#include "PreintegrationBase.h" + +namespace gtsam { + +PreintegrationBase::PreintegrationBase(const imuBias::ConstantBias& bias, + const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3&integrationErrorCovariance, + const bool use2ndOrderIntegration) + : PreintegratedRotation(measuredOmegaCovariance), + biasHat_(bias), + use2ndOrderIntegration_(use2ndOrderIntegration), + deltaPij_(Vector3::Zero()), + deltaVij_(Vector3::Zero()), + delPdelBiasAcc_(Z_3x3), + delPdelBiasOmega_(Z_3x3), + delVdelBiasAcc_(Z_3x3), + delVdelBiasOmega_(Z_3x3), + accelerometerCovariance_(measuredAccCovariance), + integrationCovariance_(integrationErrorCovariance) { +} + +/// Needed for testable +void PreintegrationBase::print(const std::string& s) const { + PreintegratedRotation::print(s); + std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; + std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; + biasHat_.print(" biasHat"); +} + +/// Needed for testable +bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) const { + return PreintegratedRotation::equals(other, tol) && biasHat_.equals(other.biasHat_, tol) + && equal_with_abs_tol(deltaPij_, other.deltaPij_, tol) + && equal_with_abs_tol(deltaVij_, other.deltaVij_, tol) + && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) + && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) + && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) + && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol) + && equal_with_abs_tol(accelerometerCovariance_, other.accelerometerCovariance_, tol) + && equal_with_abs_tol(integrationCovariance_, other.integrationCovariance_, tol); +} + +/// Re-initialize PreintegratedMeasurements +void PreintegrationBase::resetIntegration() { + PreintegratedRotation::resetIntegration(); + deltaPij_ = Vector3::Zero(); + deltaVij_ = Vector3::Zero(); + delPdelBiasAcc_ = Z_3x3; + delPdelBiasOmega_ = Z_3x3; + delVdelBiasAcc_ = Z_3x3; + delVdelBiasOmega_ = Z_3x3; +} + +/// Update preintegrated measurements +void PreintegrationBase::updatePreintegratedMeasurements(const Vector3& correctedAcc, + const Rot3& incrR, const double deltaT, + OptionalJacobian<9, 9> F) { + + const Matrix3 dRij = deltaRij(); // expensive + const Vector3 temp = dRij * correctedAcc * deltaT; + if (!use2ndOrderIntegration_) { + deltaPij_ += deltaVij_ * deltaT; + } else { + deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; + } + deltaVij_ += temp; + + Matrix3 R_i, F_angles_angles; + if (F) + R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij + updateIntegratedRotationAndDeltaT(incrR, deltaT, F ? &F_angles_angles : 0); + + if (F) { + const Matrix3 F_vel_angles = -R_i * skewSymmetric(correctedAcc) * deltaT; + Matrix3 F_pos_angles; + if (use2ndOrderIntegration_) + F_pos_angles = 0.5 * F_vel_angles * deltaT; + else + F_pos_angles = Z_3x3; + + // pos vel angle + *F << // + I_3x3, I_3x3 * deltaT, F_pos_angles, // pos + Z_3x3, I_3x3, F_vel_angles, // vel + Z_3x3, Z_3x3, F_angles_angles; // angle + } +} + +/// Update Jacobians to be used during preintegration +void PreintegrationBase::updatePreintegratedJacobians(const Vector3& correctedAcc, + const Matrix3& D_Rincr_integratedOmega, + const Rot3& incrR, double deltaT) { + const Matrix3 dRij = deltaRij(); // expensive + const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega(); + if (!use2ndOrderIntegration_) { + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; + delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; + } else { + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT; + delPdelBiasOmega_ += deltaT * (delVdelBiasOmega_ + temp * 0.5); + } + delVdelBiasAcc_ += -dRij * deltaT; + delVdelBiasOmega_ += temp; + update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT); +} + +void PreintegrationBase::correctMeasurementsByBiasAndSensorPose( + const Vector3& measuredAcc, const Vector3& measuredOmega, Vector3& correctedAcc, + Vector3& correctedOmega, boost::optional body_P_sensor) { + correctedAcc = biasHat_.correctAccelerometer(measuredAcc); + correctedOmega = biasHat_.correctGyroscope(measuredOmega); + + // Then compensate for sensor-body displacement: we express the quantities + // (originally in the IMU frame) into the body frame + if (body_P_sensor) { + Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); + correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame + Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); + correctedAcc = body_R_sensor * correctedAcc + - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); + // linear acceleration vector in the body frame + } +} + +/// Predict state at time j +//------------------------------------------------------------------------------ +PoseVelocityBias PreintegrationBase::predict( + const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, + const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis, + boost::optional deltaPij_biascorrected_out, + boost::optional deltaVij_biascorrected_out) const { + + const imuBias::ConstantBias biasIncr = bias_i - biasHat(); + const Vector3& biasAccIncr = biasIncr.accelerometer(); + const Vector3& biasOmegaIncr = biasIncr.gyroscope(); + + const Rot3& Rot_i = pose_i.rotation(); + const Matrix3 Rot_i_matrix = Rot_i.matrix(); + const Vector3 pos_i = pose_i.translation().vector(); + + // Predict state at time j + /* ---------------------------------------------------------------------------------------------------- */ + const Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + + delPdelBiasOmega() * biasOmegaIncr; + if (deltaPij_biascorrected_out) // if desired, store this + *deltaPij_biascorrected_out = deltaPij_biascorrected; + + const double dt = deltaTij(), dt2 = dt * dt; + Vector3 pos_j = pos_i + Rot_i_matrix * deltaPij_biascorrected + vel_i * dt + - omegaCoriolis.cross(vel_i) * dt2 // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * dt2; + + const Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr + + delVdelBiasOmega() * biasOmegaIncr; + if (deltaVij_biascorrected_out) // if desired, store this + *deltaVij_biascorrected_out = deltaVij_biascorrected; + + Vector3 vel_j = Vector3( + vel_i + Rot_i_matrix * deltaVij_biascorrected - 2 * omegaCoriolis.cross(vel_i) * dt // Coriolis term + + gravity * dt); + + if (use2ndOrderCoriolis) { + pos_j += -0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt2; // 2nd order coriolis term for position + vel_j += -omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt; // 2nd order term for velocity + } + + const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); + // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr) + + const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected); + const Vector3 correctedOmega = biascorrectedOmega + - Rot_i_matrix.transpose() * omegaCoriolis * dt; // Coriolis term + const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); + const Rot3 Rot_j = Rot_i.compose(correctedDeltaRij); + + const Pose3 pose_j = Pose3(Rot_j, Point3(pos_j)); + return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant +} + +/// Compute errors w.r.t. preintegrated measurements and Jacobians wrpt pose_i, vel_i, bias_i, pose_j, bias_j +//------------------------------------------------------------------------------ +Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, + const Vector3& gravity, + const Vector3& omegaCoriolis, + const bool use2ndOrderCoriolis, + OptionalJacobian<9, 6> H1, + OptionalJacobian<9, 3> H2, + OptionalJacobian<9, 6> H3, + OptionalJacobian<9, 3> H4, + OptionalJacobian<9, 6> H5) const { + + // We need the mismatch w.r.t. the biases used for preintegration + const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); + + // we give some shorter name to rotations and translations + const Rot3& Ri = pose_i.rotation(); + const Matrix3 Ri_transpose_matrix = Ri.transpose(); + + const Rot3& Rj = pose_j.rotation(); + const Vector3 pos_j = pose_j.translation().vector(); + + // Evaluate residual error, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + Vector3 deltaPij_biascorrected, deltaVij_biascorrected; + PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, + use2ndOrderCoriolis, deltaPij_biascorrected, + deltaVij_biascorrected); + + // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance + const Vector3 fp = Ri_transpose_matrix * (pos_j - predictedState_j.pose.translation().vector()); + + // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance + const Vector3 fv = Ri_transpose_matrix * (vel_j - predictedState_j.velocity); + + // fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j) + + /* ---------------------------------------------------------------------------------------------------- */ + // Get Get so<3> version of bias corrected rotation + // If H5 is asked for, we will need the Jacobian, which we store in H5 + // H5 will then be corrected below to take into account the Coriolis effect + Matrix3 D_cThetaRij_biasOmegaIncr; + const Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, + H5 ? &D_cThetaRij_biasOmegaIncr : 0); + + // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration + const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); + const Vector3 correctedOmega = biascorrectedOmega - coriolis; + + // Calculate Jacobians, matrices below needed only for some Jacobians... + Vector3 fR; + Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot, Ritranspose_omegaCoriolisHat; + + if (H1 || H2) + Ritranspose_omegaCoriolisHat = Ri_transpose_matrix * skewSymmetric(omegaCoriolis); + + // This is done to save computation: we ask for the jacobians only when they are needed + const double dt = deltaTij(), dt2 = dt*dt; + Rot3 fRrot; + const Rot3 RiBetweenRj = Rot3(Ri_transpose_matrix) * Rj; + if (H1 || H2 || H3 || H4 || H5) { + const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega, D_cDeltaRij_cOmega); + // Residual rotation error + fRrot = correctedDeltaRij.between(RiBetweenRj); + fR = Rot3::Logmap(fRrot, D_fR_fRrot); + D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis); + } else { + const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); + // Residual rotation error + fRrot = correctedDeltaRij.between(RiBetweenRj); + fR = Rot3::Logmap(fRrot); + } + if (H1) { + Matrix3 dfPdPi = -I_3x3, dfVdPi = Z_3x3; + if (use2ndOrderCoriolis) { + // this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() + const Matrix3 temp = Ritranspose_omegaCoriolisHat + * (-Ritranspose_omegaCoriolisHat.transpose()); + dfPdPi += 0.5 * temp * dt2; + dfVdPi += temp * dt; + } + (*H1) << + // dfP/dRi + skewSymmetric(fp + deltaPij_biascorrected), + // dfP/dPi + dfPdPi, + // dfV/dRi + skewSymmetric(fv + deltaVij_biascorrected), + // dfV/dPi + dfVdPi, + // dfR/dRi + D_fR_fRrot * (-Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis), + // dfR/dPi + Z_3x3; + } + if (H2) { + (*H2) << + // dfP/dVi + -Ri_transpose_matrix * dt + Ritranspose_omegaCoriolisHat * dt2, // Coriolis term - we got rid of the 2 wrt ins paper + // dfV/dVi + -Ri_transpose_matrix + 2 * Ritranspose_omegaCoriolisHat * dt, // Coriolis term + // dfR/dVi + Z_3x3; + } + if (H3) { + (*H3) << + // dfP/dPosej + Z_3x3, Ri_transpose_matrix * Rj.matrix(), + // dfV/dPosej + Matrix::Zero(3, 6), + // dfR/dPosej + D_fR_fRrot, Z_3x3; + } + if (H4) { + (*H4) << + // dfP/dVj + Z_3x3, + // dfV/dVj + Ri_transpose_matrix, + // dfR/dVj + Z_3x3; + } + if (H5) { + // H5 by this point already contains 3*3 biascorrectedThetaRij derivative + const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr; + (*H5) << + // dfP/dBias + -delPdelBiasAcc(), -delPdelBiasOmega(), + // dfV/dBias + -delVdelBiasAcc(), -delVdelBiasOmega(), + // dfR/dBias + Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega); + } + Vector9 r; + r << fp, fv, fR; + return r; +} + +ImuBase::ImuBase() + : gravity_(Vector3(0.0, 0.0, 9.81)), + omegaCoriolis_(Vector3(0.0, 0.0, 0.0)), + body_P_sensor_(boost::none), + use2ndOrderCoriolis_(false) { +} + +ImuBase::ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor, const bool use2ndOrderCoriolis) + : gravity_(gravity), + omegaCoriolis_(omegaCoriolis), + body_P_sensor_(body_P_sensor), + use2ndOrderCoriolis_(use2ndOrderCoriolis) { +} + +} /// namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h new file mode 100644 index 000000000..f6b24e752 --- /dev/null +++ b/gtsam/navigation/PreintegrationBase.h @@ -0,0 +1,203 @@ +/* ---------------------------------------------------------------------------- + + * 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 PreintegrationBase.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#pragma once + +#include +#include +#include +#include +#include + +namespace gtsam { + +/** + * Struct to hold all state variables of returned by Predict function + */ +struct PoseVelocityBias { + Pose3 pose; + Vector3 velocity; + imuBias::ConstantBias bias; + + PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, const imuBias::ConstantBias _bias) + : pose(_pose), + velocity(_velocity), + bias(_bias) { + } +}; + +/** + * PreintegrationBase is the base class for PreintegratedMeasurements + * (in ImuFactor) and CombinedPreintegratedMeasurements (in CombinedImuFactor). + * It includes the definitions of the preintegrated variables and the methods + * to access, print, and compare them. + */ +class PreintegrationBase : public PreintegratedRotation { + + imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration + bool use2ndOrderIntegration_; ///< Controls the order of integration + + Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) + Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) + + Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias + Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias + Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias + Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + + const Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements + const Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty + /// (to compensate errors in Euler integration) + + public: + + /** + * Default constructor, initializes the variables in the base class + * @param bias Current estimate of acceleration and rotation rate biases + * @param use2ndOrderIntegration Controls the order of integration + * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) + */ + PreintegrationBase(const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration); + + /// methods to access class variables + bool use2ndOrderIntegration() const { + return use2ndOrderIntegration_; + } + const Vector3& deltaPij() const { + return deltaPij_; + } + const Vector3& deltaVij() const { + return deltaVij_; + } + const imuBias::ConstantBias& biasHat() const { + return biasHat_; + } + Vector6 biasHatVector() const { + return biasHat_.vector(); + } // expensive + 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 Matrix3& accelerometerCovariance() const { + return accelerometerCovariance_; + } + const Matrix3& integrationCovariance() const { + return integrationCovariance_; + } + + /// print + void print(const std::string& s) const; + + /// check equality + bool equals(const PreintegrationBase& other, double tol) const; + + /// Re-initialize PreintegratedMeasurements + void resetIntegration(); + + /// Update preintegrated measurements + void updatePreintegratedMeasurements(const Vector3& correctedAcc, const Rot3& incrR, + const double deltaT, OptionalJacobian<9, 9> F); + + /// Update Jacobians to be used during preintegration + void updatePreintegratedJacobians(const Vector3& correctedAcc, + const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, + double deltaT); + + void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, + const Vector3& measuredOmega, Vector3& correctedAcc, + Vector3& correctedOmega, + boost::optional body_P_sensor); + + /// Predict state at time j + PoseVelocityBias predict( + const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, + const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, + boost::optional deltaPij_biascorrected_out = boost::none, + boost::optional deltaVij_biascorrected_out = boost::none) 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, const Pose3& pose_j, + const Vector3& vel_j, const imuBias::ConstantBias& bias_i, + const Vector3& gravity, const Vector3& omegaCoriolis, + const bool use2ndOrderCoriolis, OptionalJacobian<9, 6> H1 = + boost::none, + OptionalJacobian<9, 3> H2 = boost::none, + OptionalJacobian<9, 6> H3 = boost::none, + OptionalJacobian<9, 3> H4 = boost::none, + OptionalJacobian<9, 6> H5 = boost::none) const; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation); + ar & BOOST_SERIALIZATION_NVP(biasHat_); + ar & BOOST_SERIALIZATION_NVP(deltaPij_); + ar & BOOST_SERIALIZATION_NVP(deltaVij_); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); + } +}; + +class ImuBase { + + protected: + + Vector3 gravity_; + Vector3 omegaCoriolis_; + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + + public: + + /// Default constructor, with decent gravity and zero coriolis + ImuBase(); + + /// Fully fledge constructor + ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false); + + const Vector3& gravity() const { + return gravity_; + } + const Vector3& omegaCoriolis() const { + return omegaCoriolis_; + } + +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/tests/CMakeLists.txt b/gtsam/navigation/tests/CMakeLists.txt index b03b8167c..0beca7769 100644 --- a/gtsam/navigation/tests/CMakeLists.txt +++ b/gtsam/navigation/tests/CMakeLists.txt @@ -14,15 +14,10 @@ if(GTSAM_INSTALL_GEOGRAPHICLIB) endif() else() - if(GEOGRAPHICLIB_FOUND) + if(GeographicLib_LIBRARIES) # If we're not installing, but it's already installed, use the installed one include_directories(${GeographicLib_INCLUDE_DIRS}) - list(APPEND test_link_libraries ) - #if(MSVC) - # list(APPEND test_link_libraries GeographicLib_STATIC) - #else() - list(APPEND test_link_libraries ${GeographicLib_LIBRARIES}) - #endif() + list(APPEND test_link_libraries ${GeographicLib_LIBRARIES}) else() # We don't have GeographicLib set(tests_excluded testGeographicLib.cpp testGPSFactor.cpp testMagFactor.cpp) diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index e6ecf42a3..928c0f74f 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -116,7 +116,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) { /* ************************************************************************* */ TEST(AHRSFactor, Error) { // Linearization point - Vector3 bias; // Bias + Vector3 bias(0.,0.,0.); // Bias Rot3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0)); Rot3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0)); @@ -132,21 +132,21 @@ TEST(AHRSFactor, Error) { pre_int_data.integrateMeasurement(measuredOmega, deltaT); // Create factor - AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis, false); + AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis, boost::none); - Vector errorActual = factor.evaluateError(x1, x2, bias); + Vector3 errorActual = factor.evaluateError(x1, x2, bias); // Expected error - Vector errorExpected(3); + Vector3 errorExpected(3); errorExpected << 0, 0, 0; - EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + EXPECT(assert_equal(Vector(errorExpected), Vector(errorActual), 1e-6)); // Expected Jacobians - Matrix H1e = numericalDerivative11( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, x2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, bias), x2); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, x2, _1), bias); // Check rotation Jacobians @@ -241,7 +241,7 @@ TEST( AHRSFactor, PartialDerivativeExpmap ) { Matrix expectedDelRdelBiasOmega = numericalDerivative11( boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), biasOmega); - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3( + const Matrix3 Jr = Rot3::ExpmapDerivative( (measuredOmega - biasOmega) * deltaT); Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign @@ -293,7 +293,7 @@ TEST( AHRSFactor, fistOrderExponential ) { Vector3 deltabiasOmega; deltabiasOmega << alpha, alpha, alpha; - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3( + const Matrix3 Jr = Rot3::ExpmapDerivative( (measuredOmega - biasOmega) * deltaT); Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index fda82eca9..9fb0f939b 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -10,19 +10,22 @@ * -------------------------------------------------------------------------- */ /** - * @file testImuFactor.cpp - * @brief Unit test for ImuFactor - * @author Luca Carlone, Stephen Williams, Richard Roberts + * @file testCombinedImuFactor.cpp + * @brief Unit test for Lupton-style combined IMU factor + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts */ -#include -#include #include #include #include #include +#include +#include #include #include + #include #include @@ -36,75 +39,99 @@ using symbol_shorthand::X; using symbol_shorthand::V; using symbol_shorthand::B; -/* ************************************************************************* */ namespace { +/* ************************************************************************* */ +// Auxiliary functions to test Jacobians F and G used for +// covariance propagation during preintegration +/* ************************************************************************* */ +Vector updatePreintegratedMeasurementsTest(const Vector3 deltaPij_old, + const Vector3& deltaVij_old, const Rot3& deltaRij_old, + const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc, + const Vector3& correctedOmega, const double deltaT, + const bool use2ndOrderIntegration) { -ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) - ) -{ - ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(), - Matrix3::Identity(), Matrix3::Identity()); + Matrix3 dRij = deltaRij_old.matrix(); + Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT; + Vector3 deltaPij_new; + if (!use2ndOrderIntegration) { + deltaPij_new = deltaPij_old + deltaVij_old * deltaT; + } else { + deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; + } + Vector3 deltaVij_new = deltaVij_old + temp; + Rot3 deltaRij_new = deltaRij_old + * Rot3::Expmap((correctedOmega - bias_old.gyroscope()) * deltaT); + Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); // not important any more + imuBias::ConstantBias bias_new(bias_old); + Vector result(15); + result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector(); + return result; +} + +Rot3 updatePreintegratedMeasurementsRot(const Vector3 deltaPij_old, + const Vector3& deltaVij_old, const Rot3& deltaRij_old, + const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc, + const Vector3& correctedOmega, const double deltaT, + const bool use2ndOrderIntegration) { + + Vector result = updatePreintegratedMeasurementsTest(deltaPij_old, + deltaVij_old, deltaRij_old, bias_old, correctedAcc, correctedOmega, + deltaT, use2ndOrderIntegration); + + return Rot3::Expmap(result.segment<3>(6)); +} + +// Auxiliary functions to test preintegrated Jacobians +// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ +/* ************************************************************************* */ +CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements( + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, + Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), + Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6, 6), false); list::const_iterator itAcc = measuredAccs.begin(); list::const_iterator itOmega = measuredOmegas.begin(); list::const_iterator itDeltaT = deltaTs.begin(); - for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { + for (; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); } - return result; } Vector3 evaluatePreintegratedMeasurementsPosition( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ - return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaPij(); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaPij(); } Vector3 evaluatePreintegratedMeasurementsVelocity( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ - return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaVij(); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaVij(); } Rot3 evaluatePreintegratedMeasurementsRotation( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ - return Rot3(evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaRij()); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return Rot3( + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaRij()); } } /* ************************************************************************* */ -TEST( CombinedImuFactor, PreintegratedMeasurements ) -{ - //cout << "++++++++++++++++++++++++++++++ PreintegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl; +TEST( CombinedImuFactor, PreintegratedMeasurements ) { // Linearization point - imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases + imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases // Measurements Vector3 measuredAcc(0.1, 0.0, 0.0); - Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); + Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); double deltaT = 0.5; double tol = 1e-6; @@ -114,92 +141,86 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias, - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)); - -// const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases -// const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc -// const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc -// const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc -// const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) -// const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) -// const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), Matrix::Zero(6, 6)); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT(assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()), tol)); -// EXPECT(assert_equal(Vector(expected1.deltaVij), Vector(actual1.deltaVij), tol)); -// EXPECT(assert_equal(expected1.deltaRij, actual1.deltaRij, tol)); -// DOUBLES_EQUAL(expected1.deltaTij, actual1.deltaTij, tol); + EXPECT( + assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()), + tol)); + EXPECT( + assert_equal(Vector(expected1.deltaVij()), Vector(actual1.deltaVij()), + tol)); + EXPECT( + assert_equal(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()), + tol)); + DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol); } - /* ************************************************************************* */ -TEST( CombinedImuFactor, ErrorWithBiases ) -{ - //cout << "++++++++++++++++++++++++++++++ ErrorWithBiases +++++++++++++++++++++++++++++++++++++++ " << endl; - +TEST( CombinedImuFactor, ErrorWithBiases ) { imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) - Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); + Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0)); Vector3 v1(0.5, 0.0, 0.0); - Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)), + Point3(5.5, 1.0, -50.0)); Vector3 v2(0.5, 0.0, 0.0); // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0.1, 0.1; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; double tol = 1e-6; - // const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases - // const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc - // const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc - // const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc - // const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) - // const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) - // const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements + Matrix I6x6(6, 6); + I6x6 = Matrix::Identity(6, 6); - Matrix I6x6(6,6); - I6x6 = Matrix::Identity(6,6); - - - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + ImuFactor::PreintegratedMeasurements pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), - Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 ); + CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), + Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6); - Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, + deltaT); + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis); - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + noiseModel::Gaussian::shared_ptr Combinedmodel = + noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), + Combined_pre_int_data, gravity, omegaCoriolis); - noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov()); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); + Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); + Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, + bias2); - Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); + EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); - Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2); + // Expected Jacobians + Matrix H1e, H2e, H3e, H4e, H5e; + (void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e); - - EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); - - // Expected Jacobians - Matrix H1e, H2e, H3e, H4e, H5e; - (void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e); - - - // Actual Jacobians + // Actual Jacobians Matrix H1a, H2a, H3a, H4a, H5a, H6a; - (void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a); + (void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, + H3a, H4a, H5a, H6a); EXPECT(assert_equal(H1e, H1a.topRows(9))); EXPECT(assert_equal(H2e, H2a.topRows(9))); @@ -209,102 +230,128 @@ TEST( CombinedImuFactor, ErrorWithBiases ) } /* ************************************************************************* */ -TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) -{ - //cout << "++++++++++++++++++++++++++++++ FirstOrderPreIntegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl; +TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) { // Linearization point imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases - Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); + Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1)); // Measurements list measuredAccs, measuredOmegas; list deltaTs; measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); - for(int i=1;i<100;i++) - { + for (int i = 1; i < 100; i++) { measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + measuredOmegas.push_back( + Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); deltaTs.push_back(0.01); } // Actual preintegrated values - ImuFactor::PreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); + CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs); // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); - Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); + Matrix expectedDelPdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, + measuredOmegas, deltaTs), bias); + Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - Matrix expectedDelVdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); - Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); + Matrix expectedDelVdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, + measuredOmegas, deltaTs), bias); + Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); - Matrix expectedDelRdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); - Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); + Matrix expectedDelRdelBias = + numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, + measuredAccs, measuredOmegas, deltaTs), bias); + Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); // Compare Jacobians EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc())); - EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); + EXPECT( + assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc())); - EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); - EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); - EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations + EXPECT( + assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); + EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); + EXPECT( + assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), + 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } -TEST(CombinedImuFactor, PredictPositionAndVelocity){ - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) +/* ************************************************************************* */ +TEST(CombinedImuFactor, PredictPositionAndVelocity) { + imuBias::ConstantBias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; measuredOmega << 0, 0, 0;//M_PI/10.0+0.3; - Vector3 measuredAcc; measuredAcc << 0,1,-9.81; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0, 0.1, 0; //M_PI/10.0+0.3; + Vector3 measuredAcc; + measuredAcc << 0, 1.1, -9.81; double deltaT = 0.001; - Matrix I6x6(6,6); - I6x6 = Matrix::Identity(6,6); + Matrix I6x6(6, 6); + I6x6 = Matrix::Identity(6, 6); CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( - bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true ); + bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), + Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true); - for (int i = 0; i<1000; ++i) Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - - // Create factor - noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov()); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); - - // Predict - Pose3 x1; - Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocityBias = Combinedfactor.Predict(x1, v1, bias, Combined_pre_int_data, gravity, omegaCoriolis); - Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); - Vector3 expectedVelocity; expectedVelocity<<0,1,0; - EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity))); + for (int i = 0; i < 1000; ++i) + Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, + deltaT); + // Create factor + noiseModel::Gaussian::shared_ptr Combinedmodel = + noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), + Combined_pre_int_data, gravity, omegaCoriolis); + // Predict + Pose3 x1; + Vector3 v1(0, 0.0, 0.0); + PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, + bias, gravity, omegaCoriolis); + Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); + Vector3 expectedVelocity; + expectedVelocity << 0, 1, 0; + EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); + EXPECT( + assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity))); } +/* ************************************************************************* */ TEST(CombinedImuFactor, PredictRotation) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) - Matrix I6x6(6,6); + Matrix I6x6(6, 6); CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( - bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true ); - Vector3 measuredAcc; measuredAcc << 0, 0, -9.81; - Vector3 gravity; gravity<<0,0,9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0,0,0; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0; + bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), + Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true); + Vector3 measuredAcc; + measuredAcc << 0, 0, -9.81; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0; double deltaT = 0.001; double tol = 1e-4; for (int i = 0; i < 1000; ++i) @@ -314,16 +361,172 @@ TEST(CombinedImuFactor, PredictRotation) { Combined_pre_int_data, gravity, omegaCoriolis); // Predict - Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0)); - Vector3 v(0,0,0); - PoseVelocityBias poseVelocityBias = Combinedfactor.Predict(x,v,bias, Combined_pre_int_data, gravity, omegaCoriolis); - Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0)); - EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol)); + Pose3 x(Rot3().ypr(0, 0, 0), Point3(0, 0, 0)), x2; + Vector3 v(0, 0, 0), v2; + CombinedImuFactor::Predict(x, v, x2, v2, bias, + Combinedfactor.preintegratedMeasurements(), gravity, omegaCoriolis); + Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); + EXPECT(assert_equal(expectedPose, x2, tol)); } -#include +/* ************************************************************************* */ +TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) { + // Linearization point + imuBias::ConstantBias bias_old = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases + Pose3 body_P_sensor = Pose3(); + // Measurements + list measuredAccs, measuredOmegas; + list deltaTs; + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + for (int i = 1; i < 100; i++) { + measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); + measuredOmegas.push_back( + Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); + deltaTs.push_back(0.01); + } + // Actual preintegrated values + CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = + evaluatePreintegratedMeasurements(bias_old, measuredAccs, measuredOmegas, + deltaTs); + + // so far we only created a nontrivial linearization point for the preintegrated measurements + // Now we add a new measurement and ask for Jacobians + const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); + const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0); + const double newDeltaT = 0.01; + const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement + const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement + const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement + + Matrix oldPreintCovariance = preintegrated.preintMeasCov(); + + Matrix Factual, Gactual; + preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, + newDeltaT, body_P_sensor, Factual, Gactual); + + bool use2ndOrderIntegration = false; + + ////////////////////////////////////////////////////////////////////////////////////////////// + // COMPUTE NUMERICAL DERIVATIVES FOR F + ////////////////////////////////////////////////////////////////////////////////////////////// + // Compute expected F wrt positions (15,3) + Matrix df_dpos = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsTest, _1, deltaVij_old, + deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), deltaPij_old); + // rotation part has to be done properly, on manifold + df_dpos.block<3, 3>(6, 0) = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, _1, deltaVij_old, + deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), deltaPij_old); + + // Compute expected F wrt velocities (15,3) + Matrix df_dvel = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, _1, + deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), deltaVij_old); + // rotation part has to be done properly, on manifold + df_dvel.block<3, 3>(6, 0) = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, _1, + deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), deltaVij_old); + + // Compute expected F wrt angles (15,3) + Matrix df_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, + deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega, + newDeltaT, use2ndOrderIntegration), deltaRij_old); + // rotation part has to be done properly, on manifold + df_dangle.block<3, 3>(6, 0) = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, + deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega, + newDeltaT, use2ndOrderIntegration), deltaRij_old); + + // Compute expected F wrt biases (15,6) + Matrix df_dbias = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, + deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, + newDeltaT, use2ndOrderIntegration), bias_old); + // rotation part has to be done properly, on manifold + df_dbias.block<3, 6>(6, 0) = + numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, + deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, + newDeltaT, use2ndOrderIntegration), bias_old); + + Matrix Fexpected(15, 15); + Fexpected << df_dpos, df_dvel, df_dangle, df_dbias; + EXPECT(assert_equal(Fexpected, Factual)); + + ////////////////////////////////////////////////////////////////////////////////////////////// + // COMPUTE NUMERICAL DERIVATIVES FOR G + ////////////////////////////////////////////////////////////////////////////////////////////// + // Compute expected G wrt integration noise + Matrix df_dintNoise(15, 3); + df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3; + + // Compute expected G wrt acc noise (15,3) + Matrix df_daccNoise = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, + deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), newMeasuredAcc); + // rotation part has to be done properly, on manifold + df_daccNoise.block<3, 3>(6, 0) = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, + deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), newMeasuredAcc); + + // Compute expected G wrt gyro noise (15,3) + Matrix df_domegaNoise = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, + deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT, + use2ndOrderIntegration), newMeasuredOmega); + // rotation part has to be done properly, on manifold + df_domegaNoise.block<3, 3>(6, 0) = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, + deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT, + use2ndOrderIntegration), newMeasuredOmega); + + // Compute expected G wrt bias random walk noise (15,6) + Matrix df_rwBias(15, 6); // random walk on the bias does not appear in the first 9 entries + df_rwBias.setZero(); + df_rwBias.block<6, 6>(9, 0) = eye(6); + + // Compute expected G wrt gyro noise (15,6) + Matrix df_dinitBias = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, + deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, + newDeltaT, use2ndOrderIntegration), bias_old); + // rotation part has to be done properly, on manifold + df_dinitBias.block<3, 6>(6, 0) = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, + deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, + newDeltaT, use2ndOrderIntegration), bias_old); + df_dinitBias.block<6, 6>(9, 0) = Matrix::Zero(6, 6); // only has to influence first 9 rows + + Matrix Gexpected(15, 21); + Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias; + + EXPECT(assert_equal(Gexpected, Gactual)); + + // Check covariance propagation + Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance + * Factual.transpose() + (1 / newDeltaT) * Gactual * Gactual.transpose(); + + Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); + EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); +} /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp index bd321d51d..4d5df3f05 100644 --- a/gtsam/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -21,23 +21,111 @@ using namespace std; using namespace gtsam; +Vector biasAcc1(Vector3(0.2, -0.1, 0)); +Vector biasGyro1(Vector3(0.1, -0.3, -0.2)); +imuBias::ConstantBias bias1(biasAcc1, biasGyro1); + +Vector biasAcc2(Vector3(0.1, 0.2, 0.04)); +Vector biasGyro2(Vector3(-0.002, 0.005, 0.03)); +imuBias::ConstantBias bias2(biasAcc2, biasGyro2); /* ************************************************************************* */ -TEST( ImuBias, Constructor) -{ - Vector bias_acc(Vector3(0.1,0.2,0.4)); - Vector bias_gyro(Vector3(-0.2, 0.5, 0.03)); - +TEST( ImuBias, Constructor) { // Default Constructor - gtsam::imuBias::ConstantBias bias1; + imuBias::ConstantBias bias1; // Acc + Gyro Constructor - gtsam::imuBias::ConstantBias bias2(bias_acc, bias_gyro); + imuBias::ConstantBias bias2(biasAcc2, biasGyro2); // Copy Constructor - gtsam::imuBias::ConstantBias bias3(bias2); + imuBias::ConstantBias bias3(bias2); } /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +TEST( ImuBias, inverse) { + imuBias::ConstantBias biasActual = bias1.inverse(); + imuBias::ConstantBias biasExpected = imuBias::ConstantBias(-biasAcc1, + -biasGyro1); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, compose) { + imuBias::ConstantBias biasActual = bias2.compose(bias1); + imuBias::ConstantBias biasExpected = imuBias::ConstantBias( + biasAcc1 + biasAcc2, biasGyro1 + biasGyro2); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, between) { + // p.between(q) == q - p + imuBias::ConstantBias biasActual = bias2.between(bias1); + imuBias::ConstantBias biasExpected = imuBias::ConstantBias( + biasAcc1 - biasAcc2, biasGyro1 - biasGyro2); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, localCoordinates) { + Vector deltaActual = Vector(bias2.localCoordinates(bias1)); + Vector deltaExpected = (imuBias::ConstantBias(biasAcc1 - biasAcc2, + biasGyro1 - biasGyro2)).vector(); + EXPECT(assert_equal(deltaExpected, deltaActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, retract) { + Vector6 delta; + delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; + imuBias::ConstantBias biasActual = bias2.retract(delta); + imuBias::ConstantBias biasExpected = imuBias::ConstantBias( + biasAcc2 + delta.block<3, 1>(0, 0), biasGyro2 + delta.block<3, 1>(3, 0)); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, Logmap) { + Vector deltaActual = bias2.Logmap(bias1); + Vector deltaExpected = bias1.vector(); + EXPECT(assert_equal(deltaExpected, deltaActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, Expmap) { + Vector6 delta; + delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; + imuBias::ConstantBias biasActual = bias2.Expmap(delta); + imuBias::ConstantBias biasExpected = imuBias::ConstantBias(delta); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, operatorSub) { + imuBias::ConstantBias biasActual = -bias1; + imuBias::ConstantBias biasExpected(-biasAcc1, -biasGyro1); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, operatorAdd) { + imuBias::ConstantBias biasActual = bias2 + bias1; + imuBias::ConstantBias biasExpected(biasAcc2 + biasAcc1, + biasGyro2 + biasGyro1); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, operatorSubB) { + imuBias::ConstantBias biasActual = bias2 - bias1; + imuBias::ConstantBias biasExpected(biasAcc2 - biasAcc1, + biasGyro2 - biasGyro1); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index eac021f4e..c27921fc9 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -37,407 +38,809 @@ using symbol_shorthand::B; /* ************************************************************************* */ namespace { -Vector callEvaluateError(const ImuFactor& factor, - const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias) -{ +// Auxiliary functions to test evaluate error in ImuFactor +/* ************************************************************************* */ +Vector callEvaluateError(const ImuFactor& factor, const Pose3& pose_i, + const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias) { return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias); } -Rot3 evaluateRotationError(const ImuFactor& factor, - const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias) -{ - return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; +Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, + const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias) { + return Rot3::Expmap( + factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3)); } +// Auxiliary functions to test Jacobians F and G used for +// covariance propagation during preintegration +/* ************************************************************************* */ +Vector updatePreintegratedPosVel(const Vector3 deltaPij_old, + const Vector3& deltaVij_old, const Rot3& deltaRij_old, + const Vector3& correctedAcc, const Vector3& correctedOmega, + const double deltaT, const bool use2ndOrderIntegration_) { + + Matrix3 dRij = deltaRij_old.matrix(); + Vector3 temp = dRij * correctedAcc * deltaT; + Vector3 deltaPij_new; + if (!use2ndOrderIntegration_) { + deltaPij_new = deltaPij_old + deltaVij_old * deltaT; + } else { + deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; + } + Vector3 deltaVij_new = deltaVij_old + temp; + + Vector result(6); + result << deltaPij_new, deltaVij_new; + return result; +} + +Rot3 updatePreintegratedRot(const Rot3& deltaRij_old, + const Vector3& correctedOmega, const double deltaT) { + Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT); + return deltaRij_new; +} + +// Define covariance matrices +/* ************************************************************************* */ +double accNoiseVar = 0.01; +double omegaNoiseVar = 0.03; +double intNoiseVar = 0.0001; +const Matrix3 kMeasuredAccCovariance = accNoiseVar * Matrix3::Identity(); +const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * Matrix3::Identity(); +const Matrix3 kIntegrationErrorCovariance = intNoiseVar * Matrix3::Identity(); + +// Auxiliary functions to test preintegrated Jacobians +// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ +/* ************************************************************************* */ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) - ) -{ - ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(), - Matrix3::Identity(), Matrix3::Identity()); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs, + const bool use2ndOrderIntegration = false) { + ImuFactor::PreintegratedMeasurements result(bias, + kMeasuredAccCovariance, + kMeasuredOmegaCovariance, + kIntegrationErrorCovariance, + use2ndOrderIntegration); list::const_iterator itAcc = measuredAccs.begin(); list::const_iterator itOmega = measuredOmegas.begin(); list::const_iterator itDeltaT = deltaTs.begin(); - for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { + for (; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); } - return result; } Vector3 evaluatePreintegratedMeasurementsPosition( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ - return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaPij(); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaPij(); } Vector3 evaluatePreintegratedMeasurementsVelocity( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ - return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaVij(); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaVij(); } Rot3 evaluatePreintegratedMeasurementsRotation( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ - return Rot3(evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij()); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return Rot3( + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaRij()); } -Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT) -{ +Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, + const double deltaT) { return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); } - -Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) -{ - return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) ); +Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) { + return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta))); } } /* ************************************************************************* */ -TEST( ImuFactor, PreintegratedMeasurements ) -{ +TEST( ImuFactor, PreintegratedMeasurements ) { // Linearization point - imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases + imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases // Measurements Vector3 measuredAcc(0.1, 0.0, 0.0); - Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); + Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); double deltaT = 0.5; // Expected preintegrated values - Vector3 expectedDeltaP1; expectedDeltaP1 << 0.5*0.1*0.5*0.5, 0, 0; + 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); + Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0); double expectedDeltaT1(0.5); bool use2ndOrderIntegration = true; // Actual preintegrated values - ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); + ImuFactor::PreintegratedMeasurements actual1(bias, + kMeasuredAccCovariance, + kMeasuredOmegaCovariance, + kIntegrationErrorCovariance, + use2ndOrderIntegration); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6)); - EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6)); + EXPECT( + assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6)); + EXPECT( + assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6)); EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6)); DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6); // Integrate again - Vector3 expectedDeltaP2; expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5*0.1*0.5*0.5, 0, 0; - Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) + expectedDeltaR1.matrix() * measuredAcc * 0.5; - Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI/100.0, 0.0, 0.0); + Vector3 expectedDeltaP2; + expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5 * 0.1 * 0.5 * 0.5, 0, 0; + Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) + + expectedDeltaR1.matrix() * measuredAcc * 0.5; + Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0); double expectedDeltaT2(1); // Actual preintegrated values ImuFactor::PreintegratedMeasurements actual2 = actual1; actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6)); - EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6)); + EXPECT( + assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6)); + EXPECT( + assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6)); EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6)); DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6); } /* ************************************************************************* */ -TEST( ImuFactor, Error ) -{ +TEST( ImuFactor, ErrorAndJacobians ) { // Linearization point imuBias::ConstantBias bias; // Bias - Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); + Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0), + Point3(5.0, 1.0, -50.0)); Vector3 v1(Vector3(0.5, 0.0, 0.0)); - Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); + Pose3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0), + Point3(5.5, 1.0, -50.0)); Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; measuredOmega << M_PI/100, 0, 0; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; + measuredOmega << M_PI / 100, 0, 0; Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector(); double deltaT = 1.0; bool use2ndOrderIntegration = true; - ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); + ImuFactor::PreintegratedMeasurements pre_int_data(bias, + kMeasuredAccCovariance, + kMeasuredOmegaCovariance, + kIntegrationErrorCovariance, + use2ndOrderIntegration); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis); Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); // Expected error - Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; + Vector errorExpected(9); + errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); - // Expected Jacobians - Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); - - // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); - Matrix RH3e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); - // Actual Jacobians Matrix H1a, H2a, H3a, H4a, H5a; (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); + // Expected Jacobians + /////////////////// H1 /////////////////////////// + Matrix H1e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); + // Jacobians are around zero, so the rotation part is the same as: + Matrix H1Rot3 = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); + EXPECT(assert_equal(H1Rot3, H1e.bottomRows(3))); + EXPECT(assert_equal(H1e, H1a)); - // positions and velocities - Matrix H1etop6 = H1e.topRows(6); - Matrix H1atop6 = H1a.topRows(6); - EXPECT(assert_equal(H1etop6, H1atop6)); - // rotations - EXPECT(assert_equal(RH1e, H1a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations - + /////////////////// H2 /////////////////////////// + Matrix H2e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); EXPECT(assert_equal(H2e, H2a)); - // positions and velocities - Matrix H3etop6 = H3e.topRows(6); - Matrix H3atop6 = H3a.topRows(6); - EXPECT(assert_equal(H3etop6, H3atop6)); - // rotations - EXPECT(assert_equal(RH3e, H3a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations + /////////////////// H3 /////////////////////////// + Matrix H3e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); + // Jacobians are around zero, so the rotation part is the same as: + Matrix H3Rot3 = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); + EXPECT(assert_equal(H3Rot3, H3e.bottomRows(3))); + EXPECT(assert_equal(H3e, H3a)); + /////////////////// H4 /////////////////////////// + Matrix H4e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); EXPECT(assert_equal(H4e, H4a)); -// EXPECT(assert_equal(H5e, H5a)); + + /////////////////// H5 /////////////////////////// + Matrix H5e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); + EXPECT(assert_equal(H5e, H5a)); + + //////////////////////////////////////////////////////////////////////////// + // Evaluate error with wrong values + Vector3 v2_wrong = v2 + Vector3(0.1, 0.1, 0.1); + errorActual = factor.evaluateError(x1, v1, x2, v2_wrong, bias); + errorExpected << 0, 0, 0, 0.0724744871, 0.040715657, 0.151952901, 0, 0, 0; + EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + + Values values; + values.insert(X(1), x1); + values.insert(V(1), v1); + values.insert(X(2), x2); + values.insert(V(2), v2_wrong); + values.insert(B(1), bias); + errorExpected << 0, 0, 0, 0.0724744871, 0.040715657, 0.151952901, 0, 0, 0; + EXPECT(assert_equal(factor.unwhitenedError(values), errorActual, 1e-6)); + + // Make sure the whitening is done correctly + Matrix cov = pre_int_data.preintMeasCov(); + Matrix R = RtR(cov.inverse()); + Vector whitened = R * errorActual; + EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-6)); + + /////////////////////////////////////////////////////////////////////////////// + // Make sure linearization is correct + // Create expected value by numerical differentiation + JacobianFactor expected = linearizeNumerically(factor, values, 1e-8); + + // Create actual value by linearize + GaussianFactor::shared_ptr linearized = factor.linearize(values); + JacobianFactor* actual = dynamic_cast(linearized.get()); + + // Check cast result and then equality + EXPECT(assert_equal(expected, *actual, 1e-4)); } /* ************************************************************************* */ -TEST( ImuFactor, ErrorWithBiases ) -{ - // Linearization point -// Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot) -// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// Vector3 v1(Vector3(0.5, 0.0, 0.0)); -// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// Vector3 v2(Vector3(0.5, 0.0, 0.0)); - - - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) - Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); +TEST( ImuFactor, ErrorAndJacobianWithBiases ) { + imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) + Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 10.0), + Point3(5.0, 1.0, -50.0)); Vector3 v1(Vector3(0.5, 0.0, 0.0)); - Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), + Point3(5.5, 1.0, -50.0)); Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0.1, 0.1; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + ImuFactor::PreintegratedMeasurements pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); -// ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3)); -// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis); - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + SETDEBUG("ImuFactor evaluateError", false); + Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); + SETDEBUG("ImuFactor evaluateError", false); - SETDEBUG("ImuFactor evaluateError", false); - Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); - SETDEBUG("ImuFactor evaluateError", false); + // Expected error (should not be zero in this test, as we want to evaluate Jacobians + // at a nontrivial linearization point) + // Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; + // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); - // Expected error - Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; -// EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + // Expected Jacobians + Matrix H1e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); + Matrix H2e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); + Matrix H3e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); + Matrix H4e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); + Matrix H5e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); - // Expected Jacobians - Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); + // Check rotation Jacobians + Matrix RH1e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); + Matrix RH3e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); + Matrix RH5e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); - // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); - Matrix RH3e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); - Matrix RH5e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); + // Actual Jacobians + Matrix H1a, H2a, H3a, H4a, H5a; + (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); - // Actual Jacobians - Matrix H1a, H2a, H3a, H4a, H5a; - (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); - - EXPECT(assert_equal(H1e, H1a)); - EXPECT(assert_equal(H2e, H2a)); - EXPECT(assert_equal(H3e, H3a)); - EXPECT(assert_equal(H4e, H4a)); - EXPECT(assert_equal(H5e, H5a)); + EXPECT(assert_equal(H1e, H1a)); + EXPECT(assert_equal(H2e, H2a)); + EXPECT(assert_equal(H3e, H3a)); + EXPECT(assert_equal(H4e, H4a)); + EXPECT(assert_equal(H5e, H5a)); } /* ************************************************************************* */ -TEST( ImuFactor, PartialDerivativeExpmap ) -{ - // Linearization point - Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias +TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) { + imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) + Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 10.0), + Point3(5.0, 1.0, -50.0)); + Vector3 v1(Vector3(0.5, 0.0, 0.0)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), + Point3(5.5, 1.0, -50.0)); + Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0.1, 0.1; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + + Vector3(0.2, 0.0, 0.0); + double deltaT = 1.0; + + ImuFactor::PreintegratedMeasurements pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // Create factor + Pose3 bodyPsensor = Pose3(); + bool use2ndOrderCoriolis = true; + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis, bodyPsensor, use2ndOrderCoriolis); + + SETDEBUG("ImuFactor evaluateError", false); + Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); + SETDEBUG("ImuFactor evaluateError", false); + + // Expected error (should not be zero in this test, as we want to evaluate Jacobians + // at a nontrivial linearization point) + // Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; + // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + + // Expected Jacobians + Matrix H1e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); + Matrix H2e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); + Matrix H3e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); + Matrix H4e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); + Matrix H5e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); + + // Check rotation Jacobians + Matrix RH1e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); + Matrix RH3e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); + Matrix RH5e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); + + // Actual Jacobians + Matrix H1a, H2a, H3a, H4a, H5a; + (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); + + EXPECT(assert_equal(H1e, H1a)); + EXPECT(assert_equal(H2e, H2a)); + EXPECT(assert_equal(H3e, H3a)); + EXPECT(assert_equal(H4e, H4a)); + EXPECT(assert_equal(H5e, H5a)); +} + +/* ************************************************************************* */ +TEST( ImuFactor, PartialDerivative_wrt_Bias ) { + // Linearization point + Vector3 biasOmega; + biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias + + // Measurements + Vector3 measuredOmega; + measuredOmega << 0.1, 0, 0; double deltaT = 0.5; - // Compute numerical derivatives - Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( - &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega)); + Matrix expectedDelRdelBiasOmega = numericalDerivative11( + boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), + Vector3(biasOmega)); - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::ExpmapDerivative( + (measuredOmega - biasOmega) * deltaT); - Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign + Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign // Compare Jacobians - EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations - + EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } /* ************************************************************************* */ -TEST( ImuFactor, PartialDerivativeLogmap ) -{ +TEST( ImuFactor, PartialDerivativeLogmap ) { // Linearization point - Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias + Vector3 thetahat; + thetahat << 0.1, 0.1, 0; ///< Current estimate of rotation rate bias // Measurements - Vector3 deltatheta; deltatheta << 0, 0, 0; - + Vector3 deltatheta; + deltatheta << 0, 0, 0; // Compute numerical derivatives - Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( - &evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); + Matrix expectedDelFdeltheta = numericalDerivative11( + boost::bind(&evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); - const Vector3 x = thetahat; // parametrization of so(3) - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - double normx = norm_2(x); - const Matrix3 actualDelFdeltheta = Matrix3::Identity() + - 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; - -// std::cout << "actualDelFdeltheta" << actualDelFdeltheta << std::endl; -// std::cout << "expectedDelFdeltheta" << expectedDelFdeltheta << std::endl; + Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat); // Compare Jacobians EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); - } /* ************************************************************************* */ -TEST( ImuFactor, fistOrderExponential ) -{ +TEST( ImuFactor, fistOrderExponential ) { // Linearization point - Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias + Vector3 biasOmega; + biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias - // Measurements - Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; - double deltaT = 1.0; + // Measurements + Vector3 measuredOmega; + measuredOmega << 0.1, 0, 0; + double deltaT = 1.0; - // change w.r.t. linearization point - double alpha = 0.0; - Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; + // change w.r.t. linearization point + double alpha = 0.0; + Vector3 deltabiasOmega; + deltabiasOmega << alpha, alpha, alpha; + const Matrix3 Jr = Rot3::ExpmapDerivative( + (measuredOmega - biasOmega) * deltaT); - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); + Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign - Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign + const Matrix expectedRot = Rot3::Expmap( + (measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); - const Matrix expectedRot = Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); + const Matrix3 hatRot = + Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); + const Matrix3 actualRot = hatRot + * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); + //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); - const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); - const Matrix3 actualRot = - hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); - //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); - - // Compare Jacobians - EXPECT(assert_equal(expectedRot, actualRot)); + // This is a first order expansion so the equality is only an approximation + EXPECT(assert_equal(expectedRot, actualRot)); } /* ************************************************************************* */ -TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) -{ +TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) { // Linearization point imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases - Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); + Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1)); // Measurements list measuredAccs, measuredOmegas; list deltaTs; measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); - for(int i=1;i<100;i++) - { + for (int i = 1; i < 100; i++) { measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + measuredOmegas.push_back( + Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); deltaTs.push_back(0.01); } // Actual preintegrated values ImuFactor::PreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs); // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); - Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); + Matrix expectedDelPdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, + measuredOmegas, deltaTs), bias); + Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - Matrix expectedDelVdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); - Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); + Matrix expectedDelVdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, + measuredOmegas, deltaTs), bias); + Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); - Matrix expectedDelRdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); - Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); + Matrix expectedDelRdelBias = + numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, + measuredAccs, measuredOmegas, deltaTs), bias); + Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); // Compare Jacobians EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc())); - EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); + EXPECT( + assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc())); - EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); - EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); - EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations + EXPECT( + assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); + EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); + EXPECT( + assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), + 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations +} + +/* ************************************************************************* */ +TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) { + // Linearization point + imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases + Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); + + // Measurements + list measuredAccs, measuredOmegas; + list deltaTs; + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + for (int i = 1; i < 100; i++) { + measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); + measuredOmegas.push_back( + Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); + deltaTs.push_back(0.01); + } + bool use2ndOrderIntegration = false; + // Actual preintegrated values + ImuFactor::PreintegratedMeasurements preintegrated = + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs, use2ndOrderIntegration); + + // so far we only created a nontrivial linearization point for the preintegrated measurements + // Now we add a new measurement and ask for Jacobians + const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); + const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0); + const double newDeltaT = 0.01; + const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement + const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement + const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement + + Matrix oldPreintCovariance = preintegrated.preintMeasCov(); + + Matrix Factual, Gactual; + preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, + newDeltaT, body_P_sensor, Factual, Gactual); + + ////////////////////////////////////////////////////////////////////////////////////////////// + // COMPUTE NUMERICAL DERIVATIVES FOR F + ////////////////////////////////////////////////////////////////////////////////////////////// + // Compute expected f_pos_vel wrt positions + Matrix dfpv_dpos = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), + deltaPij_old); + + // Compute expected f_pos_vel wrt velocities + Matrix dfpv_dvel = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), + deltaVij_old); + + // Compute expected f_pos_vel wrt angles + Matrix dfpv_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), + deltaRij_old); + + Matrix FexpectedTop6(6, 9); + FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; + + // Compute expected f_rot wrt angles + Matrix dfr_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT), + deltaRij_old); + + Matrix FexpectedBottom3(3, 9); + FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle; + Matrix Fexpected(9, 9); + Fexpected << FexpectedTop6, FexpectedBottom3; + + EXPECT(assert_equal(Fexpected, Factual)); + + ////////////////////////////////////////////////////////////////////////////////////////////// + // COMPUTE NUMERICAL DERIVATIVES FOR G + ////////////////////////////////////////////////////////////////////////////////////////////// + // Compute jacobian wrt integration noise + Matrix dgpv_dintNoise(6, 3); + dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3; + + // Compute jacobian wrt acc noise + Matrix dgpv_daccNoise = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, + deltaRij_old, _1, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), newMeasuredAcc); + + // Compute expected F wrt gyro noise + Matrix dgpv_domegaNoise = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, + deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), + newMeasuredOmega); + Matrix GexpectedTop6(6, 9); + GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; + + // Compute expected f_rot wrt gyro noise + Matrix dgr_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT), + newMeasuredOmega); + + Matrix GexpectedBottom3(3, 9); + GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle; + Matrix Gexpected(9, 9); + Gexpected << GexpectedTop6, GexpectedBottom3; + + EXPECT(assert_equal(Gexpected, Gactual)); + + // Check covariance propagation + Matrix9 measurementCovariance; + measurementCovariance << intNoiseVar * I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar + * I_3x3, Z_3x3, Z_3x3, Z_3x3, omegaNoiseVar * I_3x3; + + Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance + * Factual.transpose() + + (1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose(); + + Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); + EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); +} + +/* ************************************************************************* */ +TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) { + // Linearization point + imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases + Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); + + // Measurements + list measuredAccs, measuredOmegas; + list deltaTs; + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + for (int i = 1; i < 100; i++) { + measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); + measuredOmegas.push_back( + Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); + deltaTs.push_back(0.01); + } + bool use2ndOrderIntegration = true; + // Actual preintegrated values + ImuFactor::PreintegratedMeasurements preintegrated = + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs, use2ndOrderIntegration); + + // so far we only created a nontrivial linearization point for the preintegrated measurements + // Now we add a new measurement and ask for Jacobians + const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); + const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0); + const double newDeltaT = 0.01; + const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement + const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement + const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement + + Matrix oldPreintCovariance = preintegrated.preintMeasCov(); + + Matrix Factual, Gactual; + preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, + newDeltaT, body_P_sensor, Factual, Gactual); + + ////////////////////////////////////////////////////////////////////////////////////////////// + // COMPUTE NUMERICAL DERIVATIVES FOR F + ////////////////////////////////////////////////////////////////////////////////////////////// + // Compute expected f_pos_vel wrt positions + Matrix dfpv_dpos = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), + deltaPij_old); + + // Compute expected f_pos_vel wrt velocities + Matrix dfpv_dvel = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), + deltaVij_old); + + // Compute expected f_pos_vel wrt angles + Matrix dfpv_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), + deltaRij_old); + + Matrix FexpectedTop6(6, 9); + FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; + + // Compute expected f_rot wrt angles + Matrix dfr_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT), + deltaRij_old); + + Matrix FexpectedBottom3(3, 9); + FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle; + Matrix Fexpected(9, 9); + Fexpected << FexpectedTop6, FexpectedBottom3; + + EXPECT(assert_equal(Fexpected, Factual)); + + ////////////////////////////////////////////////////////////////////////////////////////////// + // COMPUTE NUMERICAL DERIVATIVES FOR G + ////////////////////////////////////////////////////////////////////////////////////////////// + // Compute jacobian wrt integration noise + Matrix dgpv_dintNoise(6, 3); + dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3; + + // Compute jacobian wrt acc noise + Matrix dgpv_daccNoise = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, + deltaRij_old, _1, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), newMeasuredAcc); + + // Compute expected F wrt gyro noise + Matrix dgpv_domegaNoise = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, + deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), + newMeasuredOmega); + Matrix GexpectedTop6(6, 9); + GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; + + // Compute expected f_rot wrt gyro noise + Matrix dgr_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT), + newMeasuredOmega); + + Matrix GexpectedBottom3(3, 9); + GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle; + Matrix Gexpected(9, 9); + Gexpected << GexpectedTop6, GexpectedBottom3; + + EXPECT(assert_equal(Gexpected, Gactual)); + + // Check covariance propagation + Matrix9 measurementCovariance; + measurementCovariance << intNoiseVar * I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar + * I_3x3, Z_3x3, Z_3x3, Z_3x3, omegaNoiseVar * I_3x3; + + Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance + * Factual.transpose() + + (1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose(); + + Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); + EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); } //#include @@ -495,135 +898,157 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) // tictoc_print_(); //} - /* ************************************************************************* */ -TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) -{ +TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) - Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); + Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0)); Vector3 v1(Vector3(0.5, 0.0, 0.0)); - Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)), + Point3(5.5, 1.0, -50.0)); Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0.1, 0.1; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; - const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.10,0.10)), Point3(1,0,0)); - -// ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), -// Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmega); - - - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), - Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); - + const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), + Point3(1, 0, 0)); + ImuFactor::PreintegratedMeasurements pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis); - // Expected Jacobians - Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); + // Expected Jacobians + Matrix H1e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); + Matrix H2e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); + Matrix H3e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); + Matrix H4e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); + Matrix H5e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); - // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); - Matrix RH3e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); - Matrix RH5e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); + // Check rotation Jacobians + Matrix RH1e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); + Matrix RH3e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); + Matrix RH5e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); - // Actual Jacobians - Matrix H1a, H2a, H3a, H4a, H5a; - (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); + // Actual Jacobians + Matrix H1a, H2a, H3a, H4a, H5a; + (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); - EXPECT(assert_equal(H1e, H1a)); - EXPECT(assert_equal(H2e, H2a)); - EXPECT(assert_equal(H3e, H3a)); - EXPECT(assert_equal(H4e, H4a)); - EXPECT(assert_equal(H5e, H5a)); + EXPECT(assert_equal(H1e, H1a)); + EXPECT(assert_equal(H2e, H2a)); + EXPECT(assert_equal(H3e, H3a)); + EXPECT(assert_equal(H4e, H4a)); + EXPECT(assert_equal(H5e, H5a)); } -TEST(ImuFactor, PredictPositionAndVelocity){ +/* ************************************************************************* */ +TEST(ImuFactor, PredictPositionAndVelocity) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; measuredOmega << 0, 0, 0;//M_PI/10.0+0.3; - Vector3 measuredAcc; measuredAcc << 0,1,-9.81; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0, 0, 0; //M_PI/10.0+0.3; + Vector3 measuredAcc; + measuredAcc << 0, 1, -9.81; double deltaT = 0.001; - Matrix I6x6(6,6); - I6x6 = Matrix::Identity(6,6); + Matrix I6x6(6, 6); + I6x6 = Matrix::Identity(6, 6); - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), - Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true); + ImuFactor::PreintegratedMeasurements pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance, true); - for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); - - // Predict - Pose3 x1; - Vector3 v1(0, 0.0, 0.0); - PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis); - Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); - Vector3 expectedVelocity; expectedVelocity<<0,1,0; - EXPECT(assert_equal(expectedPose, poseVelocity.pose)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); + for (int i = 0; i < 1000; ++i) + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis); + // Predict + Pose3 x1; + Vector3 v1(0, 0.0, 0.0); + PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, + omegaCoriolis); + Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); + Vector3 expectedVelocity; + expectedVelocity << 0, 1, 0; + EXPECT(assert_equal(expectedPose, poseVelocity.pose)); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); } +/* ************************************************************************* */ TEST(ImuFactor, PredictRotation) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10;//M_PI/10.0+0.3; - Vector3 measuredAcc; measuredAcc << 0,0,-9.81; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10; //M_PI/10.0+0.3; + Vector3 measuredAcc; + measuredAcc << 0, 0, -9.81; double deltaT = 0.001; - Matrix I6x6(6,6); - I6x6 = Matrix::Identity(6,6); + Matrix I6x6(6, 6); + I6x6 = Matrix::Identity(6, 6); - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), - Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true); + ImuFactor::PreintegratedMeasurements pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance, true); - for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + for (int i = 0; i < 1000; ++i) + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis); - // Predict - Pose3 x1; - Vector3 v1(0, 0.0, 0.0); - PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis); - Pose3 expectedPose(Rot3().ypr(M_PI/10, 0, 0), Point3(0, 0, 0)); - Vector3 expectedVelocity; expectedVelocity<<0,0,0; - EXPECT(assert_equal(expectedPose, poseVelocity.pose)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); + // Predict + Pose3 x1, x2; + Vector3 v1 = Vector3(0, 0.0, 0.0); + Vector3 v2; + ImuFactor::Predict(x1, v1, x2, v2, bias, factor.preintegratedMeasurements(), + gravity, omegaCoriolis); + Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); + Vector3 expectedVelocity; + expectedVelocity << 0, 0, 0; + EXPECT(assert_equal(expectedPose, x2)); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(v2))); } /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 80f67c58d..950c6ce63 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -72,7 +72,7 @@ TEST( MagFactor, Factors ) { // MagFactor MagFactor f(1, measured, s, dir, bias, model); EXPECT( assert_equal(zero(3),f.evaluateError(theta,H1),1e-5)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT( assert_equal((Matrix)numericalDerivative11 // (boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); // MagFactor1 @@ -94,7 +94,7 @@ TEST( MagFactor, Factors ) { // MagFactor2 MagFactor3 f3(1, 2, 3, measured, nRb, model); EXPECT(assert_equal(zero(3),f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); - EXPECT(assert_equal(numericalDerivative11 // + EXPECT(assert_equal((Matrix)numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// H1, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h new file mode 100644 index 000000000..7295f3160 --- /dev/null +++ b/gtsam/nonlinear/AdaptAutoDiff.h @@ -0,0 +1,129 @@ +/* ---------------------------------------------------------------------------- + + * 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 AdaptAutoDiff.h + * @date October 22, 2014 + * @author Frank Dellaert + * @brief Adaptor for Ceres style auto-differentiated functions + */ + +#pragma once + +#include +#include +#include + +#include +#include + +namespace gtsam { + +namespace detail { + +// By default, we assume an Identity element +template +struct Origin { T operator()() { return traits::Identity();} }; + +// but dimple manifolds don't have one, so we just use the default constructor +template +struct Origin { T operator()() { return T();} }; + +} // \ detail + +/** + * Canonical is a template that creates canonical coordinates for a given type. + * A simple manifold type (i.e., not a Lie Group) has no concept of identity, + * hence in that case we use the value given by the default constructor T() as + * the origin of a "canonical coordinate" parameterization. + */ +template +struct Canonical { + + GTSAM_CONCEPT_MANIFOLD_TYPE(T) + + typedef traits Traits; + enum { dimension = Traits::dimension }; + typedef typename Traits::TangentVector TangentVector; + typedef typename Traits::structure_category Category; + typedef detail::Origin Origin; + + static TangentVector Local(const T& other) { + return Traits::Local(Origin()(), other); + } + + static T Retract(const TangentVector& v) { + return Traits::Retract(Origin()(), v); + } +}; + +/// Adapt ceres-style autodiff +template +class AdaptAutoDiff { + + static const int N = traits::dimension; + static const int M1 = traits::dimension; + static const int M2 = traits::dimension; + + typedef Eigen::Matrix RowMajor1; + typedef Eigen::Matrix RowMajor2; + + typedef Canonical CanonicalT; + typedef Canonical Canonical1; + typedef Canonical Canonical2; + + typedef typename CanonicalT::TangentVector VectorT; + typedef typename Canonical1::TangentVector Vector1; + typedef typename Canonical2::TangentVector Vector2; + + F f; + +public: + + T operator()(const A1& a1, const A2& a2, OptionalJacobian H1 = boost::none, + OptionalJacobian H2 = boost::none) { + + using ceres::internal::AutoDiff; + + // Make arguments + Vector1 v1 = Canonical1::Local(a1); + Vector2 v2 = Canonical2::Local(a2); + + bool success; + VectorT result; + + if (H1 || H2) { + + // Get derivatives with AutoDiff + double *parameters[] = { v1.data(), v2.data() }; + double rowMajor1[N * M1], rowMajor2[N * M2]; // om the stack + double *jacobians[] = { rowMajor1, rowMajor2 }; + success = AutoDiff::Differentiate(f, parameters, 2, + result.data(), jacobians); + + // Convert from row-major to columnn-major + // TODO: if this is a bottleneck (probably not!) fix Autodiff to be Column-Major + *H1 = Eigen::Map(rowMajor1); + *H2 = Eigen::Map(rowMajor2); + + } else { + // Apply the mapping, to get result + success = f(v1.data(), v2.data(), result.data()); + } + if (!success) + throw std::runtime_error( + "AdaptAutoDiff: function call resulted in failure"); + return CanonicalT::Retract(result); + } + +}; + +} diff --git a/gtsam/nonlinear/CMakeLists.txt b/gtsam/nonlinear/CMakeLists.txt index 5bb2bd7d2..ad4824817 100644 --- a/gtsam/nonlinear/CMakeLists.txt +++ b/gtsam/nonlinear/CMakeLists.txt @@ -2,5 +2,8 @@ file(GLOB nonlinear_headers "*.h") install(FILES ${nonlinear_headers} DESTINATION include/gtsam/nonlinear) +file(GLOB nonlinear_headers_internal "internal/*.h") +install(FILES ${nonlinear_headers_internal} DESTINATION include/gtsam/nonlinear/internal) + # Build tests add_subdirectory(tests) diff --git a/gtsam/nonlinear/DoglegOptimizer.cpp b/gtsam/nonlinear/DoglegOptimizer.cpp index 082cc66c8..e148bd65d 100644 --- a/gtsam/nonlinear/DoglegOptimizer.cpp +++ b/gtsam/nonlinear/DoglegOptimizer.cpp @@ -96,7 +96,7 @@ void DoglegOptimizer::iterate(void) { /* ************************************************************************* */ DoglegParams DoglegOptimizer::ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph) const { if(!params.ordering) - params.ordering = Ordering::COLAMD(graph); + params.ordering = Ordering::colamd(graph); return params; } diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h new file mode 100644 index 000000000..6c6c155c7 --- /dev/null +++ b/gtsam/nonlinear/Expression-inl.h @@ -0,0 +1,262 @@ +/* ---------------------------------------------------------------------------- + + * 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 Expression-inl.h + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief Internals for Expression.h, not for general consumption + */ + +#pragma once + +#include + +#include +#include +#include + +namespace gtsam { + +template +void Expression::print(const std::string& s) const { + std::cout << s << *root_ << std::endl; +} + +template +Expression::Expression(const T& value) : + root_(new internal::ConstantExpression(value)) { +} + +template +Expression::Expression(const Key& key) : + root_(new internal::LeafExpression(key)) { +} + +template +Expression::Expression(const Symbol& symbol) : + root_(new internal::LeafExpression(symbol)) { +} + +template +Expression::Expression(unsigned char c, size_t j) : + root_(new internal::LeafExpression(Symbol(c, j))) { +} + +/// Construct a unary function expression +template +template +Expression::Expression(typename UnaryFunction::type function, + const Expression& expression) : + root_(new internal::UnaryExpression(function, expression)) { +} + +/// Construct a binary function expression +template +template +Expression::Expression(typename BinaryFunction::type function, + const Expression& expression1, const Expression& expression2) : + root_( + new internal::BinaryExpression(function, expression1, + expression2)) { +} + +/// Construct a ternary function expression +template +template +Expression::Expression(typename TernaryFunction::type function, + const Expression& expression1, const Expression& expression2, + const Expression& expression3) : + root_( + new internal::TernaryExpression(function, expression1, + expression2, expression3)) { +} + +/// Construct a nullary method expression +template +template +Expression::Expression(const Expression& expression, + T (A::*method)(typename MakeOptionalJacobian::type) const) : + root_( + new internal::UnaryExpression(boost::bind(method, _1, _2), + expression)) { +} + +/// Construct a unary method expression +template +template +Expression::Expression(const Expression& expression1, + T (A1::*method)(const A2&, typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) const, + const Expression& expression2) : + root_( + new internal::BinaryExpression( + boost::bind(method, _1, _2, _3, _4), expression1, expression2)) { +} + +/// Construct a binary method expression +template +template +Expression::Expression(const Expression& expression1, + T (A1::*method)(const A2&, const A3&, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) const, + const Expression& expression2, const Expression& expression3) : + root_( + new internal::TernaryExpression( + boost::bind(method, _1, _2, _3, _4, _5, _6), expression1, + expression2, expression3)) { +} + +template +std::set Expression::keys() const { + return root_->keys(); +} + +template +void Expression::dims(std::map& map) const { + root_->dims(map); +} + +template +T Expression::value(const Values& values, + boost::optional&> H) const { + + if (H) { + // Call private version that returns derivatives in H + KeysAndDims pair = keysAndDims(); + return valueAndDerivatives(values, pair.first, pair.second, *H); + } else + // no derivatives needed, just return value + return root_->value(values); +} + +template +const boost::shared_ptr >& Expression::root() const { + return root_; +} + +template +size_t Expression::traceSize() const { + return root_->traceSize(); +} + +// Private methods: + +template +T Expression::valueAndDerivatives(const Values& values, + const FastVector& keys, const FastVector& dims, + std::vector& H) const { + + // H should be pre-allocated + assert(H.size()==keys.size()); + + // Pre-allocate and zero VerticalBlockMatrix + static const int Dim = traits::dimension; + VerticalBlockMatrix Ab(dims, Dim); + Ab.matrix().setZero(); + internal::JacobianMap jacobianMap(keys, Ab); + + // Call unsafe version + T result = valueAndJacobianMap(values, jacobianMap); + + // Copy blocks into the vector of jacobians passed in + for (DenseIndex i = 0; i < static_cast(keys.size()); i++) + H[i] = Ab(i); + + return result; +} + +template +T Expression::traceExecution(const Values& values, + internal::ExecutionTrace& trace, void* traceStorage) const { + return root_->traceExecution(values, trace, + static_cast(traceStorage)); +} + +template +T Expression::valueAndJacobianMap(const Values& values, + internal::JacobianMap& jacobians) const { + // The following piece of code is absolutely crucial for performance. + // We allocate a block of memory on the stack, which can be done at runtime + // with modern C++ compilers. The traceExecution then fills this memory + // with an execution trace, made up entirely of "Record" structs, see + // the FunctionalNode class in expression-inl.h + size_t size = traceSize(); + + // Windows does not support variable length arrays, so memory must be dynamically + // allocated on Visual Studio. For more information see the issue below + // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio +#ifdef _MSC_VER + internal::ExecutionTraceStorage* traceStorage = new internal::ExecutionTraceStorage[size]; +#else + internal::ExecutionTraceStorage traceStorage[size]; +#endif + + internal::ExecutionTrace trace; + T value(this->traceExecution(values, trace, traceStorage)); + trace.startReverseAD1(jacobians); + +#ifdef _MSC_VER + delete[] traceStorage; +#endif + + return value; +} + +template +typename Expression::KeysAndDims Expression::keysAndDims() const { + std::map map; + dims(map); + size_t n = map.size(); + KeysAndDims pair = std::make_pair(FastVector(n), FastVector(n)); + boost::copy(map | boost::adaptors::map_keys, pair.first.begin()); + boost::copy(map | boost::adaptors::map_values, pair.second.begin()); + return pair; +} + +namespace internal { +// http://stackoverflow.com/questions/16260445/boost-bind-to-operator +template +struct apply_compose { + typedef T result_type; + static const int Dim = traits::dimension; + T operator()(const T& x, const T& y, OptionalJacobian H1 = + boost::none, OptionalJacobian H2 = boost::none) const { + return x.compose(y, H1, H2); + } +}; +} + +// Global methods: + +/// Construct a product expression, assumes T::compose(T) -> T +template +Expression operator*(const Expression& expression1, + const Expression& expression2) { + return Expression( + boost::bind(internal::apply_compose(), _1, _2, _3, _4), expression1, + expression2); +} + +/// Construct an array of leaves +template +std::vector > createUnknowns(size_t n, char c, size_t start) { + std::vector > unknowns; + unknowns.reserve(n); + for (size_t i = start; i < start + n; i++) + unknowns.push_back(Expression(c, i)); + return unknowns; +} + +} // namespace gtsam diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h new file mode 100644 index 000000000..d24a568f5 --- /dev/null +++ b/gtsam/nonlinear/Expression.h @@ -0,0 +1,218 @@ +/* ---------------------------------------------------------------------------- + + * 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 Expression.h + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief Expressions for Block Automatic Differentiation + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include + +// Forward declare tests +class ExpressionFactorShallowTest; + +namespace gtsam { + +// Forward declares +class Values; +template class ExpressionFactor; + +namespace internal { +template class ExecutionTrace; +template class ExpressionNode; +} + +/** + * Expression class that supports automatic differentiation + */ +template +class Expression { + +public: + + /// Define type so we can apply it as a meta-function + typedef Expression type; + +private: + + // Paul's trick shared pointer, polymorphic root of entire expression tree + boost::shared_ptr > root_; + +public: + + // Expressions wrap trees of functions that can evaluate their own derivatives. + // The meta-functions below are useful to specify the type of those functions. + // Example, a function taking a camera and a 3D point and yielding a 2D point: + // Expression::BinaryFunction::type + template + struct UnaryFunction { + typedef boost::function< + T(const A1&, typename MakeOptionalJacobian::type)> type; + }; + + template + struct BinaryFunction { + typedef boost::function< + T(const A1&, const A2&, typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type)> type; + }; + + template + struct TernaryFunction { + typedef boost::function< + T(const A1&, const A2&, const A3&, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type)> type; + }; + + /// Print + void print(const std::string& s) const; + + /// Construct a constant expression + Expression(const T& value); + + /// Construct a leaf expression, with Key + Expression(const Key& key); + + /// Construct a leaf expression, with Symbol + Expression(const Symbol& symbol); + + /// Construct a leaf expression, creating Symbol + Expression(unsigned char c, size_t j); + + /// Construct a unary function expression + template + Expression(typename UnaryFunction::type function, + const Expression& expression); + + /// Construct a binary function expression + template + Expression(typename BinaryFunction::type function, + const Expression& expression1, const Expression& expression2); + + /// Construct a ternary function expression + template + Expression(typename TernaryFunction::type function, + const Expression& expression1, const Expression& expression2, + const Expression& expression3); + + /// Construct a nullary method expression + template + Expression(const Expression& expression, + T (A::*method)(typename MakeOptionalJacobian::type) const); + + /// Construct a unary method expression + template + Expression(const Expression& expression1, + T (A1::*method)(const A2&, typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) const, + const Expression& expression2); + + /// Construct a binary method expression + template + Expression(const Expression& expression1, + T (A1::*method)(const A2&, const A3&, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) const, + const Expression& expression2, const Expression& expression3); + + /// Destructor + virtual ~Expression() { + } + + /// Return keys that play in this expression + std::set keys() const; + + /// Return dimensions for each argument, as a map + void dims(std::map& map) const; + + /** + * @brief Return value and optional derivatives, reverse AD version + * Notes: this is not terribly efficient, and H should have correct size. + * The order of the Jacobians is same as keys in either keys() or dims() + */ + T value(const Values& values, boost::optional&> H = + boost::none) const; + + /** + * @return a "deep" copy of this Expression + * "deep" is in quotes because the ExpressionNode hierarchy is *not* cloned. + * The intent is for derived classes to be copied using only a Base pointer. + */ + virtual boost::shared_ptr clone() const { + return boost::make_shared(*this); + } + + /// Return root + const boost::shared_ptr >& root() const; + + /// Return size needed for memory buffer in traceExecution + size_t traceSize() const; + +private: + + /// Keys and dimensions in same order + typedef std::pair, FastVector > KeysAndDims; + KeysAndDims keysAndDims() const; + + /// private version that takes keys and dimensions, returns derivatives + T valueAndDerivatives(const Values& values, const FastVector& keys, + const FastVector& dims, std::vector& H) const; + + /// trace execution, very unsafe + T traceExecution(const Values& values, internal::ExecutionTrace& trace, + void* traceStorage) const; + + /// brief Return value and derivatives, reverse AD version + T valueAndJacobianMap(const Values& values, + internal::JacobianMap& jacobians) const; + + // be very selective on who can access these private methods: + friend class ExpressionFactor ; + friend class internal::ExpressionNode; + + // and add tests + friend class ::ExpressionFactorShallowTest; +}; + +/** + * Construct a product expression, assumes T::compose(T) -> T + * Example: + * Expression a(0), b(1), c = a*b; + */ +template +Expression operator*(const Expression& e1, const Expression& e2); + +/** + * Construct an array of unknown expressions with successive symbol keys + * Example: + * createUnknowns(3,'x') creates unknown expressions for x0,x1,x2 + */ +template +std::vector > createUnknowns(size_t n, char c, size_t start = 0); + +} // namespace gtsam + +#include + diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h similarity index 55% rename from gtsam_unstable/nonlinear/ExpressionFactor.h rename to gtsam/nonlinear/ExpressionFactor.h index 37a89af6b..b32b84df3 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -17,11 +17,11 @@ * @brief Expressions for Block Automatic Differentiation */ -#include +#pragma once + +#include #include #include -#include -#include #include namespace gtsam { @@ -32,14 +32,19 @@ namespace gtsam { template class ExpressionFactor: public NoiseModelFactor { + protected: + + typedef ExpressionFactor This; + T measurement_; ///< the measurement to be compared with the expression Expression expression_; ///< the expression that is AD enabled - std::vector dimensions_; ///< dimensions of the Jacobian matrices - size_t augmentedCols_; ///< total number of columns + 1 (for RHS) + FastVector dims_; ///< dimensions of the Jacobian matrices - static const int Dim = traits::dimension::value; + static const int Dim = traits::dimension; -public: + public: + + typedef boost::shared_ptr > shared_ptr; /// Constructor ExpressionFactor(const SharedNoiseModel& noiseModel, // @@ -52,26 +57,9 @@ public: "ExpressionFactor was created with a NoiseModel of incorrect dimension."); noiseModel_ = noiseModel; - // Get dimensions of Jacobian matrices + // Get keys and dimensions for Jacobian matrices // An Expression is assumed unmutable, so we do this now - std::map map; - expression_.dims(map); - size_t n = map.size(); - - keys_.resize(n); - boost::copy(map | boost::adaptors::map_keys, keys_.begin()); - - dimensions_.resize(n); - boost::copy(map | boost::adaptors::map_values, dimensions_.begin()); - - // Add sizes to know how much memory to allocate on stack in linearize - augmentedCols_ = std::accumulate(dimensions_.begin(), dimensions_.end(), 1); - -#ifdef DEBUG_ExpressionFactor - BOOST_FOREACH(size_t d, dimensions_) - std::cout << d << " "; - std::cout << " -> " << Dim << "x" << augmentedCols_ << std::endl; -#endif + boost::tie(keys_, dims_) = expression_.keysAndDims(); } /** @@ -82,62 +70,53 @@ public: virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if (H) { - // H should be pre-allocated - assert(H->size()==size()); - - VerticalBlockMatrix Ab(dimensions_, Dim); - - // Wrap keys and VerticalBlockMatrix into structure passed to expression_ - JacobianMap map(keys_, Ab); - Ab.matrix().setZero(); - - // Evaluate error to get Jacobians and RHS vector b - T value = expression_.value(x, map); // <<< Reverse AD happens here ! - - // Copy blocks into the vector of jacobians passed in - for (DenseIndex i = 0; i < size(); i++) - H->at(i) = Ab(i); - - return measurement_.localCoordinates(value); + const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H); + return traits::Local(measurement_, value); } else { - const T& value = expression_.value(x); - return measurement_.localCoordinates(value); + const T value = expression_.value(x); + return traits::Local(measurement_, value); } } virtual boost::shared_ptr linearize(const Values& x) const { - // Only linearize if the factor is active if (!active(x)) return boost::shared_ptr(); // Create a writeable JacobianFactor in advance // In case noise model is constrained, we need to provide a noise model - bool constrained = noiseModel_->is_constrained(); + bool constrained = noiseModel_->isConstrained(); boost::shared_ptr factor( - constrained ? new JacobianFactor(keys_, dimensions_, Dim, + constrained ? new JacobianFactor(keys_, dims_, Dim, boost::static_pointer_cast(noiseModel_)->unit()) : - new JacobianFactor(keys_, dimensions_, Dim)); + new JacobianFactor(keys_, dims_, Dim)); // Wrap keys and VerticalBlockMatrix into structure passed to expression_ VerticalBlockMatrix& Ab = factor->matrixObject(); - JacobianMap map(keys_, Ab); + internal::JacobianMap jacobianMap(keys_, Ab); // Zero out Jacobian so we can simply add to it Ab.matrix().setZero(); - // Evaluate error to get Jacobians and RHS vector b - T value = expression_.value(x, map); // <<< Reverse AD happens here ! - Ab(size()).col(0) = -measurement_.localCoordinates(value); + // Get value and Jacobians, writing directly into JacobianFactor + T value = expression_.valueAndJacobianMap(x, jacobianMap); // <<< Reverse AD happens here ! + + // Evaluate error and set RHS vector b + Ab(size()).col(0) = -traits::Local(measurement_, value); // Whiten the corresponding system, Ab already contains RHS - Vector dummy(Dim); - noiseModel_->WhitenSystem(Ab.matrix(),dummy); + Vector b = Ab(size()).col(0); // need b to be valid for Robust noise models + noiseModel_->WhitenSystem(Ab.matrix(), b); return factor; } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } }; // ExpressionFactor -} +}// \ namespace gtsam diff --git a/gtsam/nonlinear/ExpressionFactorGraph.h b/gtsam/nonlinear/ExpressionFactorGraph.h new file mode 100644 index 000000000..122bd429f --- /dev/null +++ b/gtsam/nonlinear/ExpressionFactorGraph.h @@ -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 + + * -------------------------------------------------------------------------- */ + +/** + * @file ExpressionFactorGraph.h + * @brief Factor graph that supports adding ExpressionFactors directly + * @author Frank Dellaert + * @date December 2014 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * Factor graph that supports adding ExpressionFactors directly + */ +class ExpressionFactorGraph: public NonlinearFactorGraph { + +public: + + /// @name Adding Factors + /// @{ + + /** + * Directly add ExpressionFactor that implements |h(x)-z|^2_R + * @param h expression that implements measurement function + * @param z measurement + * @param R model + */ + template + void addExpressionFactor(const Expression& h, const T& z, + const SharedNoiseModel& R) { + push_back(boost::make_shared >(R, z, h)); + } + + /// @} +}; + +} diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index 5765eca01..9251aac07 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -42,7 +42,8 @@ namespace gtsam { // Extract the current estimate of x1,P1 VectorValues result = marginal->solve(VectorValues()); - T x = linearizationPoints.at(lastKey).retract(result[lastKey]); + const T& current = linearizationPoints.at(lastKey); + T x = traits::Retract(current, result[lastKey]); // Create a Jacobian Factor from the root node of the produced Bayes Net. // This will act as a prior for the next iteration. @@ -70,9 +71,10 @@ namespace gtsam { // Create a Jacobian Prior Factor directly P_initial. // Since x0 is set to the provided mean, the b vector in the prior will be zero // TODO Frank asks: is there a reason why noiseModel is not simply P_initial ? + int n = traits::GetDimension(x_initial); priorFactor_ = JacobianFactor::shared_ptr( - new JacobianFactor(key_initial, P_initial->R(), Vector::Zero(x_initial.dim()), - noiseModel::Unit::Create(P_initial->dim()))); + new JacobianFactor(key_initial, P_initial->R(), Vector::Zero(n), + noiseModel::Unit::Create(n))); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index 7bbd14980..4adad676e 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -42,6 +42,11 @@ namespace gtsam { template class ExtendedKalmanFilter { + + // Check that VALUE type is a testable Manifold + BOOST_CONCEPT_ASSERT((IsTestable)); + BOOST_CONCEPT_ASSERT((IsManifold)); + public: typedef boost::shared_ptr > shared_ptr; diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.cpp b/gtsam/nonlinear/GaussNewtonOptimizer.cpp index 823f3a6ac..7a115e1c4 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.cpp +++ b/gtsam/nonlinear/GaussNewtonOptimizer.cpp @@ -49,7 +49,7 @@ GaussNewtonParams GaussNewtonOptimizer::ensureHasOrdering( GaussNewtonParams params, const NonlinearFactorGraph& graph) const { if(!params.ordering) - params.ordering = Ordering::COLAMD(graph); + params.ordering = Ordering::colamd(graph); return params; } diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 36ebd7033..f7c6d0345 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -29,7 +29,7 @@ namespace gtsam { template VALUE ISAM2::calculateEstimate(Key key) const { const Vector& delta = getDelta()[key]; - return theta_.at(key).retract(delta); + return traits::Retract(theta_.at(key), delta); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 6d6785b14..00ffdccef 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -98,7 +98,7 @@ DoglegOptimizerImpl::TrustRegionAdaptationMode ISAM2DoglegParams::adaptationMode } /* ************************************************************************* */ -ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::string& str) const { +ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::string& str) { std::string s = str; boost::algorithm::to_upper(s); if (s == "QR") return ISAM2Params::QR; if (s == "CHOLESKY") return ISAM2Params::CHOLESKY; @@ -108,7 +108,7 @@ ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::strin } /* ************************************************************************* */ -std::string ISAM2Params::factorizationTranslator(const ISAM2Params::Factorization& value) const { +std::string ISAM2Params::factorizationTranslator(const ISAM2Params::Factorization& value) { std::string s; switch (value) { case ISAM2Params::QR: s = "QR"; break; @@ -341,7 +341,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe Ordering order; if(constrainKeys) { - order = Ordering::COLAMDConstrained(variableIndex_, *constrainKeys); + order = Ordering::colamdConstrained(variableIndex_, *constrainKeys); } else { @@ -351,11 +351,11 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe FastMap constraintGroups; BOOST_FOREACH(Key var, observedKeys) constraintGroups[var] = 1; - order = Ordering::COLAMDConstrained(variableIndex_, constraintGroups); + order = Ordering::colamdConstrained(variableIndex_, constraintGroups); } else { - order = Ordering::COLAMD(variableIndex_); + order = Ordering::colamd(variableIndex_); } } gttoc(ordering); @@ -481,7 +481,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe // Generate ordering gttic(Ordering); - Ordering ordering = Ordering::COLAMDConstrained(affectedFactorsVarIndex, constraintGroups); + Ordering ordering = Ordering::colamdConstrained(affectedFactorsVarIndex, constraintGroups); gttoc(Ordering); ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree( diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index c2e728a2b..5f5554e91 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -186,6 +186,7 @@ struct GTSAM_EXPORT ISAM2Params { enableDetailedResults(false), enablePartialRelinearizationCheck(false), findUnusedFactorSlots(false) {} + /// print iSAM2 parameters void print(const std::string& str = "") const { std::cout << str << "\n"; if(optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) @@ -214,7 +215,9 @@ struct GTSAM_EXPORT ISAM2Params { std::cout.flush(); } - /** Getters and Setters for all properties */ + /// @name Getters and Setters for all properties + /// @{ + OptimizationParams getOptimizationParams() const { return this->optimizationParams; } RelinearizationThreshold getRelinearizeThreshold() const { return relinearizeThreshold; } int getRelinearizeSkip() const { return relinearizeSkip; } @@ -237,14 +240,21 @@ struct GTSAM_EXPORT ISAM2Params { void setEnableDetailedResults(bool enableDetailedResults) { this->enableDetailedResults = enableDetailedResults; } void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck) { this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck; } - Factorization factorizationTranslator(const std::string& str) const; - std::string factorizationTranslator(const Factorization& value) const; - GaussianFactorGraph::Eliminate getEliminationFunction() const { return factorization == CHOLESKY ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky : (GaussianFactorGraph::Eliminate)EliminateQR; } + + /// @} + + /// @name Some utilities + /// @{ + + static Factorization factorizationTranslator(const std::string& str); + static std::string factorizationTranslator(const Factorization& value); + + /// @} }; @@ -404,7 +414,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(cachedFactor_); ar & BOOST_SERIALIZATION_NVP(gradientContribution_); @@ -544,8 +554,15 @@ public: boost::optional&> marginalFactorsIndices = boost::none, boost::optional&> deletedFactorsIndices = boost::none); - /** Access the current linearization point */ - const Values& getLinearizationPoint() const { return theta_; } + /// Access the current linearization point + const Values& getLinearizationPoint() const { + return theta_; + } + + /// Check whether variable with given key exists in linearization point + bool valueExists(Key key) const { + return theta_.exists(key); + } /** Compute an estimate from the incomplete linear delta computed during the last update. * This delta is incomplete because it was not updated below wildfire_threshold. If only @@ -633,6 +650,9 @@ protected: }; // ISAM2 +/// traits +template<> struct traits : public Testable {}; + /// Optimize the BayesTree, starting from the root. /// @param replaced Needs to contain /// all variables that are contained in the top of the Bayes tree that has been diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 002f8b237..5fb51a243 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -25,9 +25,10 @@ #include #include +#include +#include #include #include -#include using namespace std; @@ -42,6 +43,8 @@ LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTrans boost::algorithm::to_upper(s); if (s == "SILENT") return LevenbergMarquardtParams::SILENT; + if (s == "SUMMARY") + return LevenbergMarquardtParams::SUMMARY; if (s == "LAMBDA") return LevenbergMarquardtParams::LAMBDA; if (s == "TRYLAMBDA") @@ -65,6 +68,9 @@ std::string LevenbergMarquardtParams::verbosityLMTranslator( case LevenbergMarquardtParams::SILENT: s = "SILENT"; break; + case LevenbergMarquardtParams::SUMMARY: + s = "SUMMARY"; + break; case LevenbergMarquardtParams::TERMINATION: s = "TERMINATION"; break; @@ -173,7 +179,7 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); damped += boost::make_shared(key_vector.first, A, b, model); - } catch (std::exception e) { + } catch (const std::exception& e) { // Don't attempt any damping if no key found in diagonal continue; } @@ -219,9 +225,15 @@ void LevenbergMarquardtOptimizer::iterate() { cout << "linearizing = " << endl; GaussianFactorGraph::shared_ptr linear = linearize(); - if(state_.totalNumberInnerIterations==0) // write initial error + if(state_.totalNumberInnerIterations==0) { // write initial error writeLogFile(state_.error); + if (lmVerbosity == LevenbergMarquardtParams::SUMMARY) { + cout << "Initial error: " << state_.error << ", values: " << state_.values.size() + << std::endl; + } + } + // Keep increasing lambda until we make make progress while (true) { @@ -236,7 +248,7 @@ void LevenbergMarquardtOptimizer::iterate() { double modelFidelity = 0.0; bool step_is_successful = false; bool stopSearchingLambda = false; - double newError; + double newError = numeric_limits::infinity(); Values newValues; VectorValues delta; @@ -244,10 +256,12 @@ void LevenbergMarquardtOptimizer::iterate() { try { delta = solve(dampedSystem, state_.values, params_); systemSolvedSuccessfully = true; - } catch (IndeterminantLinearSystemException) { + } catch (const IndeterminantLinearSystemException& e) { systemSolvedSuccessfully = false; } + double linearizedCostChange = 0, + newlinearizedError = 0; if (systemSolvedSuccessfully) { params_.reuse_diagonal_ = true; @@ -257,9 +271,9 @@ void LevenbergMarquardtOptimizer::iterate() { delta.print("delta"); // cost change in the linearized system (old - new) - double newlinearizedError = linear->error(delta); + newlinearizedError = linear->error(delta); - double linearizedCostChange = state_.error - newlinearizedError; + linearizedCostChange = state_.error - newlinearizedError; if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "newlinearizedError = " << newlinearizedError << " linearizedCostChange = " << linearizedCostChange << endl; @@ -304,6 +318,12 @@ void LevenbergMarquardtOptimizer::iterate() { } } + if (lmVerbosity == LevenbergMarquardtParams::SUMMARY) { + cout << "[" << state_.iterations << "]: " << "new error = " << newlinearizedError + << ", delta = " << linearizedCostChange << ", lambda = " << state_.lambda + << ", success = " << systemSolvedSuccessfully << std::endl; + } + ++state_.totalNumberInnerIterations; if (step_is_successful) { // we have successfully decreased the cost and we have good modelFidelity @@ -320,7 +340,8 @@ void LevenbergMarquardtOptimizer::iterate() { // check if lambda is too big if (state_.lambda >= params_.lambdaUpperBound) { - if (nloVerbosity >= NonlinearOptimizerParams::TERMINATION) + if (nloVerbosity >= NonlinearOptimizerParams::TERMINATION || + lmVerbosity == LevenbergMarquardtParams::SUMMARY) cout << "Warning: Levenberg-Marquardt giving up because " "cannot decrease error with maximum lambda" << endl; break; @@ -340,8 +361,12 @@ void LevenbergMarquardtOptimizer::iterate() { /* ************************************************************************* */ LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering( LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const { - if (!params.ordering) - params.ordering = Ordering::COLAMD(graph); + if (!params.ordering){ + if (params.orderingType == Ordering::METIS) + params.ordering = Ordering::metis(graph); + else + params.ordering = Ordering::colamd(graph); + } return params; } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 20f9dec3c..009b480f2 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -38,7 +38,7 @@ class GTSAM_EXPORT LevenbergMarquardtParams: public NonlinearOptimizerParams { public: /** See LevenbergMarquardtParams::lmVerbosity */ enum VerbosityLM { - SILENT = 0, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA + SILENT = 0, SUMMARY, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA }; static VerbosityLM verbosityLMTranslator(const std::string &s); diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index 340f3f6bc..3ae8d4c98 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -146,7 +146,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(factor_); @@ -155,5 +155,7 @@ private: }; // \class LinearContainerFactor +template<> struct traits : public Testable {}; + } // \namespace gtsam diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp index 107ec7d3f..6d7196ff5 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp @@ -1,7 +1,18 @@ +/* ---------------------------------------------------------------------------- + + * 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 GradientDescentOptimizer.cpp - * @brief - * @author ydjian + * @file NonlinearConjugateGradientOptimizer.cpp + * @brief Simple non-linear optimizer that solves using *non-preconditioned* CG + * @author Yong-Dian Jian * @date Jun 11, 2012 */ @@ -16,36 +27,49 @@ using namespace std; namespace gtsam { -/* Return the gradient vector of a nonlinear factor given a linearization point and a variable ordering - * Can be moved to NonlinearFactorGraph.h if desired */ -VectorValues gradientInPlace(const NonlinearFactorGraph &nfg, const Values &values) { +/** + * @brief Return the gradient vector of a nonlinear factor graph + * @param nfg the graph + * @param values a linearization point + * Can be moved to NonlinearFactorGraph.h if desired + */ +static VectorValues gradientInPlace(const NonlinearFactorGraph &nfg, + const Values &values) { // Linearize graph GaussianFactorGraph::shared_ptr linear = nfg.linearize(values); return linear->gradientAtZero(); } -double NonlinearConjugateGradientOptimizer::System::error(const State &state) const { +double NonlinearConjugateGradientOptimizer::System::error( + const State &state) const { return graph_.error(state); } -NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradientOptimizer::System::gradient(const State &state) const { +NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradientOptimizer::System::gradient( + const State &state) const { return gradientInPlace(graph_, state); } -NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance(const State ¤t, const double alpha, const Gradient &g) const { + +NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance( + const State ¤t, const double alpha, const Gradient &g) const { Gradient step = g; step *= alpha; return current.retract(step); } void NonlinearConjugateGradientOptimizer::iterate() { - int dummy ; - boost::tie(state_.values, dummy) = nonlinearConjugateGradient(System(graph_), state_.values, params_, true /* single iterations */); + int dummy; + boost::tie(state_.values, dummy) = nonlinearConjugateGradient( + System(graph_), state_.values, params_, true /* single iterations */); ++state_.iterations; state_.error = graph_.error(state_.values); } const Values& NonlinearConjugateGradientOptimizer::optimize() { - boost::tie(state_.values, state_.iterations) = nonlinearConjugateGradient(System(graph_), state_.values, params_, false /* up to convergent */); + // Optimize until convergence + System system(graph_); + boost::tie(state_.values, state_.iterations) = // + nonlinearConjugateGradient(system, state_.values, params_, false); state_.error = graph_.error(state_.values); return state_.values; } diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index 1ad8fa2ab..04d4734a4 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -1,8 +1,19 @@ +/* ---------------------------------------------------------------------------- + + * 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 GradientDescentOptimizer.cpp - * @brief + * @file NonlinearConjugateGradientOptimizer.h + * @brief Simple non-linear optimizer that solves using *non-preconditioned* CG * @author Yong-Dian Jian - * @date Jun 11, 2012 + * @date June 11, 2012 */ #pragma once @@ -13,15 +24,18 @@ namespace gtsam { -/** An implementation of the nonlinear cg method using the template below */ -class GTSAM_EXPORT NonlinearConjugateGradientState : public NonlinearOptimizerState { +/** An implementation of the nonlinear CG method using the template below */ +class GTSAM_EXPORT NonlinearConjugateGradientState: public NonlinearOptimizerState { public: typedef NonlinearOptimizerState Base; - NonlinearConjugateGradientState(const NonlinearFactorGraph& graph, const Values& values) - : Base(graph, values) {} + NonlinearConjugateGradientState(const NonlinearFactorGraph& graph, + const Values& values) : + Base(graph, values) { + } }; -class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimizer { +class GTSAM_EXPORT NonlinearConjugateGradientOptimizer: public NonlinearOptimizer { + /* a class for the nonlinearConjugateGradient template */ class System { public: @@ -33,37 +47,49 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimiz const NonlinearFactorGraph &graph_; public: - System(const NonlinearFactorGraph &graph): graph_(graph) {} - double error(const State &state) const ; - Gradient gradient(const State &state) const ; - State advance(const State ¤t, const double alpha, const Gradient &g) const ; + System(const NonlinearFactorGraph &graph) : + graph_(graph) { + } + double error(const State &state) const; + Gradient gradient(const State &state) const; + State advance(const State ¤t, const double alpha, + const Gradient &g) const; }; public: + typedef NonlinearOptimizer Base; - typedef NonlinearConjugateGradientState States; typedef NonlinearOptimizerParams Parameters; typedef boost::shared_ptr shared_ptr; protected: - States state_; + NonlinearConjugateGradientState state_; Parameters params_; public: - NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, - const Parameters& params = Parameters()) - : Base(graph), state_(graph, initialValues), params_(params) {} + /// Constructor + NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph, + const Values& initialValues, const Parameters& params = Parameters()) : + Base(graph), state_(graph, initialValues), params_(params) { + } + + /// Destructor + virtual ~NonlinearConjugateGradientOptimizer() { + } - virtual ~NonlinearConjugateGradientOptimizer() {} virtual void iterate(); - virtual const Values& optimize (); - virtual const NonlinearOptimizerState& _state() const { return state_; } - virtual const NonlinearOptimizerParams& _params() const { return params_; } + virtual const Values& optimize(); + virtual const NonlinearOptimizerState& _state() const { + return state_; + } + virtual const NonlinearOptimizerParams& _params() const { + return params_; + } }; /** Implement the golden-section line search algorithm */ -template +template double lineSearch(const S &system, const V currentValues, const W &gradient) { /* normalize it such that it becomes a unit vector */ @@ -71,37 +97,40 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { // perform the golden section search algorithm to decide the the optimal step size // detail refer to http://en.wikipedia.org/wiki/Golden_section_search - const double phi = 0.5*(1.0+std::sqrt(5.0)), resphi = 2.0 - phi, tau = 1e-5; - double minStep = -1.0/g, maxStep = 0, - newStep = minStep + (maxStep-minStep) / (phi+1.0) ; + const double phi = 0.5 * (1.0 + std::sqrt(5.0)), resphi = 2.0 - phi, tau = + 1e-5; + double minStep = -1.0 / g, maxStep = 0, newStep = minStep + + (maxStep - minStep) / (phi + 1.0); V newValues = system.advance(currentValues, newStep, gradient); double newError = system.error(newValues); while (true) { - const bool flag = (maxStep - newStep > newStep - minStep) ? true : false ; - const double testStep = flag ? - newStep + resphi * (maxStep - newStep) : newStep - resphi * (newStep - minStep); + const bool flag = (maxStep - newStep > newStep - minStep) ? true : false; + const double testStep = + flag ? newStep + resphi * (maxStep - newStep) : + newStep - resphi * (newStep - minStep); - if ( (maxStep- minStep) < tau * (std::fabs(testStep) + std::fabs(newStep)) ) { - return 0.5*(minStep+maxStep); + if ((maxStep - minStep) + < tau * (std::fabs(testStep) + std::fabs(newStep))) { + return 0.5 * (minStep + maxStep); } const V testValues = system.advance(currentValues, testStep, gradient); const double testError = system.error(testValues); // update the working range - if ( testError >= newError ) { - if ( flag ) maxStep = testStep; - else minStep = testStep; - } - else { - if ( flag ) { + if (testError >= newError) { + if (flag) + maxStep = testStep; + else + minStep = testStep; + } else { + if (flag) { minStep = newStep; newStep = testStep; newError = testError; - } - else { + } else { maxStep = newStep; newStep = testStep; newError = testError; @@ -112,7 +141,7 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { } /** - * Implement the nonlinear conjugate gradient method using the Polak-Ribieve formula suggested in + * Implement the nonlinear conjugate gradient method using the Polak-Ribiere formula suggested in * http://en.wikipedia.org/wiki/Nonlinear_conjugate_gradient_method. * * The S (system) class requires three member functions: error(state), gradient(state) and @@ -120,8 +149,10 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { * * The last parameter is a switch between gradient-descent and conjugate gradient */ -template -boost::tuple nonlinearConjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams ¶ms, const bool singleIteration, const bool gradientDescent = false) { +template +boost::tuple nonlinearConjugateGradient(const S &system, + const V &initial, const NonlinearOptimizerParams ¶ms, + const bool singleIteration, const bool gradientDescent = false) { // GTSAM_CONCEPT_MANIFOLD_TYPE(V); @@ -129,57 +160,67 @@ boost::tuple nonlinearConjugateGradient(const S &system, const V &initia // check if we're already close enough double currentError = system.error(initial); - if(currentError <= params.errorTol) { - if (params.verbosity >= NonlinearOptimizerParams::ERROR){ - std::cout << "Exiting, as error = " << currentError << " < " << params.errorTol << std::endl; + if (currentError <= params.errorTol) { + if (params.verbosity >= NonlinearOptimizerParams::ERROR) { + std::cout << "Exiting, as error = " << currentError << " < " + << params.errorTol << std::endl; } return boost::tie(initial, iteration); } V currentValues = initial; - typename S::Gradient currentGradient = system.gradient(currentValues), prevGradient, - direction = currentGradient; + typename S::Gradient currentGradient = system.gradient(currentValues), + prevGradient, direction = currentGradient; /* do one step of gradient descent */ - V prevValues = currentValues; double prevError = currentError; + V prevValues = currentValues; + double prevError = currentError; double alpha = lineSearch(system, currentValues, direction); currentValues = system.advance(prevValues, alpha, direction); currentError = system.error(currentValues); // Maybe show output - if (params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "Initial error: " << currentError << std::endl; + if (params.verbosity >= NonlinearOptimizerParams::ERROR) + std::cout << "Initial error: " << currentError << std::endl; // Iterative loop do { - if ( gradientDescent == true) { + if (gradientDescent == true) { direction = system.gradient(currentValues); - } - else { + } else { prevGradient = currentGradient; currentGradient = system.gradient(currentValues); - const double beta = std::max(0.0, currentGradient.dot(currentGradient-prevGradient)/currentGradient.dot(currentGradient)); - direction = currentGradient + (beta*direction); + // Polak-Ribiere: beta = g'*(g_n-g_n-1)/g_n-1'*g_n-1 + const double beta = std::max(0.0, + currentGradient.dot(currentGradient - prevGradient) + / prevGradient.dot(prevGradient)); + direction = currentGradient + (beta * direction); } alpha = lineSearch(system, currentValues, direction); - prevValues = currentValues; prevError = currentError; + prevValues = currentValues; + prevError = currentError; currentValues = system.advance(prevValues, alpha, direction); currentError = system.error(currentValues); // Maybe show output - if(params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "currentError: " << currentError << std::endl; - } while( ++iteration < params.maxIterations && - !singleIteration && - !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol, prevError, currentError, params.verbosity)); + if (params.verbosity >= NonlinearOptimizerParams::ERROR) + std::cout << "iteration: " << iteration << ", currentError: " << currentError << std::endl; + } while (++iteration < params.maxIterations && !singleIteration + && !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, + params.errorTol, prevError, currentError, params.verbosity)); // Printing if verbose - if (params.verbosity >= NonlinearOptimizerParams::ERROR && iteration >= params.maxIterations) - std::cout << "nonlinearConjugateGradient: Terminating because reached maximum iterations" << std::endl; + if (params.verbosity >= NonlinearOptimizerParams::ERROR + && iteration >= params.maxIterations) + std::cout + << "nonlinearConjugateGradient: Terminating because reached maximum iterations" + << std::endl; return boost::tie(currentValues, iteration); } -} +} // \ namespace gtsam diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 5d3319d97..86fb34fe0 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -17,289 +17,333 @@ #pragma once -#include -#include - #include #include #include +#include + +#include +#include +#include + namespace gtsam { +/** + * An equality factor that forces either one variable to a constant, + * or a set of variables to be equal to each other. + * + * Depending on flag, throws an error at linearization if the constraints are not met. + * + * Switchable implementation: + * - ALLLOW_ERROR : if we allow that there can be nonzero error, does not throw, and uses gain + * - ONLY_EXACT : throws error at linearization if not at exact feasible point, and infinite error + * + * \nosubgrouping + */ +template +class NonlinearEquality: public NoiseModelFactor1 { + +public: + typedef VALUE T; + +private: + + // feasible value + T feasible_; + + // error handling flag + bool allow_error_; + + // error gain in allow error case + double error_gain_; + + // typedef to this class + typedef NonlinearEquality This; + + // typedef to base class + typedef NoiseModelFactor1 Base; + +public: + /** - * Template default compare function that assumes a testable T + * Function that compares two values */ - template - bool compare(const T& a, const T& b) { - GTSAM_CONCEPT_TESTABLE_TYPE(T); - return a.equals(b); + typedef boost::function CompareFunction; + CompareFunction compare_; +// bool (*compare_)(const T& a, const T& b); + + /** default constructor - only for serialization */ + NonlinearEquality() { + } + + virtual ~NonlinearEquality() { + } + + /// @name Standard Constructors + /// @{ + + /** + * Constructor - forces exact evaluation + */ + NonlinearEquality(Key j, const T& feasible, + const CompareFunction &_compare = boost::bind(traits::Equals,_1,_2,1e-9)) : + Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), + j), feasible_(feasible), allow_error_(false), error_gain_(0.0), // + compare_(_compare) { } /** - * An equality factor that forces either one variable to a constant, - * or a set of variables to be equal to each other. - * - * Depending on flag, throws an error at linearization if the constraints are not met. - * - * Switchable implementation: - * - ALLLOW_ERROR : if we allow that there can be nonzero error, does not throw, and uses gain - * - ONLY_EXACT : throws error at linearization if not at exact feasible point, and infinite error - * - * \nosubgrouping + * Constructor - allows inexact evaluation */ - template - class NonlinearEquality: public NoiseModelFactor1 { - - public: - typedef VALUE T; - - private: - - // feasible value - T feasible_; - - // error handling flag - bool allow_error_; - - // error gain in allow error case - double error_gain_; - - // typedef to this class - typedef NonlinearEquality This; - - // typedef to base class - typedef NoiseModelFactor1 Base; - - public: - - /** - * Function that compares two values - */ - bool (*compare_)(const T& a, const T& b); - - - /** default constructor - only for serialization */ - NonlinearEquality() {} - - virtual ~NonlinearEquality() {} - - /// @name Standard Constructors - /// @{ - - /** - * Constructor - forces exact evaluation - */ - NonlinearEquality(Key j, const T& feasible, bool (*_compare)(const T&, const T&) = compare) : - Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible), - allow_error_(false), error_gain_(0.0), + NonlinearEquality(Key j, const T& feasible, double error_gain, + const CompareFunction &_compare = boost::bind(traits::Equals,_1,_2,1e-9)) : + Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), + j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), // compare_(_compare) { + } + + /// @} + /// @name Testable + /// @{ + + virtual void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n"; + traits::Print(feasible_, "Feasible Point:\n"); + std::cout << "Variable Dimension: " << traits::GetDimension(feasible_) << std::endl; + } + + /** Check if two factors are equal */ + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { + const This* e = dynamic_cast(&f); + return e && Base::equals(f) && traits::Equals(feasible_,e->feasible_, tol) + && std::abs(error_gain_ - e->error_gain_) < tol; + } + + /// @} + /// @name Standard Interface + /// @{ + + /** actual error function calculation */ + virtual double error(const Values& c) const { + const T& xj = c.at(this->key()); + Vector e = this->unwhitenedError(c); + if (allow_error_ || !compare_(xj, feasible_)) { + return error_gain_ * dot(e, e); + } else { + return 0.0; } + } - /** - * Constructor - allows inexact evaluation - */ - NonlinearEquality(Key j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare) : - Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible), - allow_error_(true), error_gain_(error_gain), - compare_(_compare) { + /** error function */ + Vector evaluateError(const T& xj, + boost::optional H = boost::none) const { + const size_t nj = traits::GetDimension(feasible_); + if (allow_error_) { + if (H) + *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare + return traits::Local(xj,feasible_); + } else if (compare_(feasible_, xj)) { + if (H) + *H = eye(nj); + return zero(nj); // set error to zero if equal + } else { + if (H) + throw std::invalid_argument( + "Linearization point not feasible for " + + DefaultKeyFormatter(this->key()) + "!"); + return repeat(nj, std::numeric_limits::infinity()); // set error to infinity if not equal } + } - /// @} - /// @name Testable - /// @{ + // Linearize is over-written, because base linearization tries to whiten + virtual GaussianFactor::shared_ptr linearize(const Values& x) const { + const T& xj = x.at(this->key()); + Matrix A; + Vector b = evaluateError(xj, A); + SharedDiagonal model = noiseModel::Constrained::All(b.size()); + return GaussianFactor::shared_ptr( + new JacobianFactor(this->key(), A, b, model)); + } - virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n"; - gtsam::print(feasible_,"Feasible Point:\n"); - std::cout << "Variable Dimension: " << feasible_.dim() << std::endl; - } + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } - /** Check if two factors are equal */ - virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { - const This* e = dynamic_cast(&f); - return e && Base::equals(f) && feasible_.equals(e->feasible_, tol) && - fabs(error_gain_ - e->error_gain_) < tol; - } + /// @} - /// @} - /// @name Standard Interface - /// @{ +private: - /** actual error function calculation */ - virtual double error(const Values& c) const { - const T& xj = c.at(this->key()); - Vector e = this->unwhitenedError(c); - if (allow_error_ || !compare_(xj, feasible_)) { - return error_gain_ * dot(e,e); - } else { - return 0.0; - } - } + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar + & boost::serialization::make_nvp("NoiseModelFactor1", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(feasible_); + ar & BOOST_SERIALIZATION_NVP(allow_error_); + ar & BOOST_SERIALIZATION_NVP(error_gain_); + } - /** error function */ - Vector evaluateError(const T& xj, boost::optional H = boost::none) const { - size_t nj = feasible_.dim(); - if (allow_error_) { - if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare - return xj.localCoordinates(feasible_); - } else if (compare_(feasible_,xj)) { - if (H) *H = eye(nj); - return zero(nj); // set error to zero if equal - } else { - if (H) throw std::invalid_argument( - "Linearization point not feasible for " + DefaultKeyFormatter(this->key()) + "!"); - return repeat(nj, std::numeric_limits::infinity()); // set error to infinity if not equal - } - } +}; +// \class NonlinearEquality - // Linearize is over-written, because base linearization tries to whiten - virtual GaussianFactor::shared_ptr linearize(const Values& x) const { - const T& xj = x.at(this->key()); - Matrix A; - Vector b = evaluateError(xj, A); - SharedDiagonal model = noiseModel::Constrained::All(b.size()); - return GaussianFactor::shared_ptr(new JacobianFactor(this->key(), A, b, model)); - } +template +struct traits > : Testable > { +}; - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } +/* ************************************************************************* */ +/** + * Simple unary equality constraint - fixes a value for a variable + */ +template +class NonlinearEquality1: public NoiseModelFactor1 { - /// @} +public: + typedef VALUE X; - private: +protected: + typedef NoiseModelFactor1 Base; + typedef NonlinearEquality1 This; - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor1", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(feasible_); - ar & BOOST_SERIALIZATION_NVP(allow_error_); - ar & BOOST_SERIALIZATION_NVP(error_gain_); - } + /** default constructor to allow for serialization */ + NonlinearEquality1() { + } - }; // \class NonlinearEquality + X value_; /// fixed value for variable + + GTSAM_CONCEPT_MANIFOLD_TYPE(X) + + GTSAM_CONCEPT_TESTABLE_TYPE(X) + +public: + + typedef boost::shared_ptr > shared_ptr; - /* ************************************************************************* */ /** - * Simple unary equality constraint - fixes a value for a variable + * Constructor + * @param value feasible value that values(key) shouild be equal to + * @param key the key for the unknown variable to be constrained + * @param mu a parameter which really turns this into a strong prior + * */ - template - class NonlinearEquality1 : public NoiseModelFactor1 { + NonlinearEquality1(const X& value, Key key, double mu = 1000.0) : + Base( noiseModel::Constrained::All(traits::GetDimension(value), + std::abs(mu)), key), value_(value) { + } - public: - typedef VALUE X; + virtual ~NonlinearEquality1() { + } - protected: - typedef NoiseModelFactor1 Base; - typedef NonlinearEquality1 This; + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } - /** default constructor to allow for serialization */ - NonlinearEquality1() {} + /** g(x) with optional derivative */ + Vector evaluateError(const X& x1, + boost::optional H = boost::none) const { + if (H) + (*H) = eye(traits::GetDimension(x1)); + // manifold equivalent of h(x)-z -> log(z,h(x)) + return traits::Local(value_,x1); + } - X value_; /// fixed value for variable + /** Print */ + virtual void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key()) + << ")," << "\n"; + this->noiseModel_->print(); + value_.print("Value"); + } - GTSAM_CONCEPT_MANIFOLD_TYPE(X); - GTSAM_CONCEPT_TESTABLE_TYPE(X); +private: - public: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar + & boost::serialization::make_nvp("NoiseModelFactor1", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(value_); + } +}; +// \NonlinearEquality1 - typedef boost::shared_ptr > shared_ptr; +template +struct traits > : Testable > { +}; - ///TODO: comment - NonlinearEquality1(const X& value, Key key1, double mu = 1000.0) - : Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {} +/* ************************************************************************* */ +/** + * Simple binary equality constraint - this constraint forces two factors to + * be the same. + */ +template +class NonlinearEquality2: public NoiseModelFactor2 { +public: + typedef VALUE X; - virtual ~NonlinearEquality1() {} +protected: + typedef NoiseModelFactor2 Base; + typedef NonlinearEquality2 This; - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + GTSAM_CONCEPT_MANIFOLD_TYPE(X) - /** g(x) with optional derivative */ - Vector evaluateError(const X& x1, boost::optional H = boost::none) const { - if (H) (*H) = eye(x1.dim()); - // manifold equivalent of h(x)-z -> log(z,h(x)) - return value_.localCoordinates(x1); - } + /** default constructor to allow for serialization */ + NonlinearEquality2() { + } - /** Print */ - virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << ": NonlinearEquality1(" - << keyFormatter(this->key()) << "),"<< "\n"; - this->noiseModel_->print(); - value_.print("Value"); - } +public: - private: + typedef boost::shared_ptr > shared_ptr; - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor1", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(value_); - } - }; // \NonlinearEquality1 + ///TODO: comment + NonlinearEquality2(Key key1, Key key2, double mu = 1000.0) : + Base(noiseModel::Constrained::All(traits::dimension, std::abs(mu)), key1, key2) { + } + virtual ~NonlinearEquality2() { + } - /* ************************************************************************* */ - /** - * Simple binary equality constraint - this constraint forces two factors to - * be the same. - */ - template - class NonlinearEquality2 : public NoiseModelFactor2 { - public: - typedef VALUE X; + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } - protected: - typedef NoiseModelFactor2 Base; - typedef NonlinearEquality2 This; + /** g(x) with optional derivative2 */ + Vector evaluateError(const X& x1, const X& x2, boost::optional H1 = + boost::none, boost::optional H2 = boost::none) const { + static const size_t p = traits::dimension; + if (H1) *H1 = -eye(p); + if (H2) *H2 = eye(p); + return traits::Local(x1,x2); + } - GTSAM_CONCEPT_MANIFOLD_TYPE(X); +private: - /** default constructor to allow for serialization */ - NonlinearEquality2() {} + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar + & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + } +}; +// \NonlinearEquality2 - public: +template +struct traits > : Testable > { +}; - typedef boost::shared_ptr > shared_ptr; - ///TODO: comment - NonlinearEquality2(Key key1, Key key2, double mu = 1000.0) - : Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {} - virtual ~NonlinearEquality2() {} - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - /** g(x) with optional derivative2 */ - Vector evaluateError(const X& x1, const X& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { - const size_t p = X::Dim(); - if (H1) *H1 = -eye(p); - if (H2) *H2 = eye(p); - return x1.localCoordinates(x2); - } - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor2", - boost::serialization::base_object(*this)); - } - }; // \NonlinearEquality2 - -} // namespace gtsam +}// namespace gtsam diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index e9b97d644..7c8337fc8 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -132,10 +132,11 @@ boost::shared_ptr NoiseModelFactor::linearize( } // TODO pass unwhitened + noise model to Gaussian factor - if (noiseModel_ && noiseModel_->is_constrained()) + using noiseModel::Constrained; + if (noiseModel_ && noiseModel_->isConstrained()) return GaussianFactor::shared_ptr( new JacobianFactor(terms, b, - boost::static_pointer_cast(noiseModel_)->unit())); + boost::static_pointer_cast(noiseModel_)->unit())); else return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); } diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 9fd4f8383..5b46742e7 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -20,14 +20,14 @@ #pragma once -#include -#include - #include #include #include #include +#include +#include +#include /** * Macro to add a standard clone function to a derived factor @@ -112,7 +112,7 @@ public: * when the constraint is *NOT* fulfilled. * @return true if the constraint is active */ - virtual bool active(const Values& c) const { return true; } + virtual bool active(const Values& /*c*/) const { return true; } /** linearize to a GaussianFactor */ virtual boost::shared_ptr @@ -144,6 +144,10 @@ public: }; // \class NonlinearFactor +/// traits +template<> struct traits : public Testable { +}; + /* ************************************************************************* */ /** * A nonlinear sum-of-squares factor with a zero-mean noise model @@ -243,7 +247,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(noiseModel_); @@ -321,7 +325,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); } @@ -397,7 +401,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); } @@ -474,7 +478,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); } @@ -555,7 +559,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); } @@ -640,7 +644,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); } @@ -729,7 +733,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); } diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 3bbb63b88..f23418d2a 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -47,11 +47,12 @@ double NonlinearFactorGraph::probPrime(const Values& c) const { /* ************************************************************************* */ void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& keyFormatter) const { - cout << str << "size: " << size() << endl; + cout << str << "size: " << size() << endl << endl; for (size_t i = 0; i < factors_.size(); i++) { stringstream ss; - ss << "factor " << i << ": "; + ss << "Factor " << i << ": "; if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter); + cout << endl; } } @@ -62,12 +63,12 @@ bool NonlinearFactorGraph::equals(const NonlinearFactorGraph& other, double tol) /* ************************************************************************* */ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, - const GraphvizFormatting& graphvizFormatting, + const GraphvizFormatting& formatting, const KeyFormatter& keyFormatter) const { stm << "graph {\n"; - stm << " size=\"" << graphvizFormatting.figureWidthInches << "," << - graphvizFormatting.figureHeightInches << "\";\n\n"; + stm << " size=\"" << formatting.figureWidthInches << "," << + formatting.figureHeightInches << "\";\n\n"; FastSet keys = this->keys(); @@ -75,72 +76,38 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, struct { boost::optional operator()( const Value& value, const GraphvizFormatting& graphvizFormatting) { - if(const Pose2* p = dynamic_cast(&value)) { - double x, y; - switch (graphvizFormatting.paperHorizontalAxis) { - case GraphvizFormatting::X: x = p->x(); break; - case GraphvizFormatting::Y: x = p->y(); break; - case GraphvizFormatting::Z: x = 0.0; break; - case GraphvizFormatting::NEGX: x = -p->x(); break; - case GraphvizFormatting::NEGY: x = -p->y(); break; - case GraphvizFormatting::NEGZ: x = 0.0; break; - default: throw std::runtime_error("Invalid enum value"); - } - switch (graphvizFormatting.paperVerticalAxis) { - case GraphvizFormatting::X: y = p->x(); break; - case GraphvizFormatting::Y: y = p->y(); break; - case GraphvizFormatting::Z: y = 0.0; break; - case GraphvizFormatting::NEGX: y = -p->x(); break; - case GraphvizFormatting::NEGY: y = -p->y(); break; - case GraphvizFormatting::NEGZ: y = 0.0; break; - default: throw std::runtime_error("Invalid enum value"); - } - return Point2(x,y); - } else if(const Pose3* p = dynamic_cast(&value)) { - double x, y; - switch (graphvizFormatting.paperHorizontalAxis) { - case GraphvizFormatting::X: x = p->x(); break; - case GraphvizFormatting::Y: x = p->y(); break; - case GraphvizFormatting::Z: x = p->z(); break; - case GraphvizFormatting::NEGX: x = -p->x(); break; - case GraphvizFormatting::NEGY: x = -p->y(); break; - case GraphvizFormatting::NEGZ: x = -p->z(); break; - default: throw std::runtime_error("Invalid enum value"); - } - switch (graphvizFormatting.paperVerticalAxis) { - case GraphvizFormatting::X: y = p->x(); break; - case GraphvizFormatting::Y: y = p->y(); break; - case GraphvizFormatting::Z: y = p->z(); break; - case GraphvizFormatting::NEGX: y = -p->x(); break; - case GraphvizFormatting::NEGY: y = -p->y(); break; - case GraphvizFormatting::NEGZ: y = -p->z(); break; - default: throw std::runtime_error("Invalid enum value"); - } - return Point2(x,y); - } else if(const Point3* p = dynamic_cast(&value)) { - double x, y; - switch (graphvizFormatting.paperHorizontalAxis) { - case GraphvizFormatting::X: x = p->x(); break; - case GraphvizFormatting::Y: x = p->y(); break; - case GraphvizFormatting::Z: x = p->z(); break; - case GraphvizFormatting::NEGX: x = -p->x(); break; - case GraphvizFormatting::NEGY: x = -p->y(); break; - case GraphvizFormatting::NEGZ: x = -p->z(); break; - default: throw std::runtime_error("Invalid enum value"); - } - switch (graphvizFormatting.paperVerticalAxis) { - case GraphvizFormatting::X: y = p->x(); break; - case GraphvizFormatting::Y: y = p->y(); break; - case GraphvizFormatting::Z: y = p->z(); break; - case GraphvizFormatting::NEGX: y = -p->x(); break; - case GraphvizFormatting::NEGY: y = -p->y(); break; - case GraphvizFormatting::NEGZ: y = -p->z(); break; - default: throw std::runtime_error("Invalid enum value"); - } - return Point2(x,y); + Vector3 t; + if (const GenericValue* p = dynamic_cast*>(&value)) { + t << p->value().x(), p->value().y(), 0; + } else if (const GenericValue* p = dynamic_cast*>(&value)) { + t << p->value().x(), p->value().y(), 0; + } else if (const GenericValue* p = dynamic_cast*>(&value)) { + t = p->value().translation().vector(); + } else if (const GenericValue* p = dynamic_cast*>(&value)) { + t = p->value().vector(); } else { return boost::none; } + double x, y; + switch (graphvizFormatting.paperHorizontalAxis) { + case GraphvizFormatting::X: x = t.x(); break; + case GraphvizFormatting::Y: x = t.y(); break; + case GraphvizFormatting::Z: x = t.z(); break; + case GraphvizFormatting::NEGX: x = -t.x(); break; + case GraphvizFormatting::NEGY: x = -t.y(); break; + case GraphvizFormatting::NEGZ: x = -t.z(); break; + default: throw std::runtime_error("Invalid enum value"); + } + switch (graphvizFormatting.paperVerticalAxis) { + case GraphvizFormatting::X: y = t.x(); break; + case GraphvizFormatting::Y: y = t.y(); break; + case GraphvizFormatting::Z: y = t.z(); break; + case GraphvizFormatting::NEGX: y = -t.x(); break; + case GraphvizFormatting::NEGY: y = -t.y(); break; + case GraphvizFormatting::NEGZ: y = -t.z(); break; + default: throw std::runtime_error("Invalid enum value"); + } + return Point2(x,y); }} getXY; // Find bounds @@ -148,7 +115,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, double minY = numeric_limits::infinity(), maxY = -numeric_limits::infinity(); BOOST_FOREACH(Key key, keys) { if(values.exists(key)) { - boost::optional xy = getXY(values.at(key), graphvizFormatting); + boost::optional xy = getXY(values.at(key), formatting); if(xy) { if(xy->x() < minX) minX = xy->x(); @@ -163,33 +130,22 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, } // Create nodes for each variable in the graph - bool firstTimePoses = true; - Key lastKey; - BOOST_FOREACH(Key key, keys) { + BOOST_FOREACH(Key key, keys){ // Label the node with the label from the KeyFormatter stm << " var" << key << "[label=\"" << keyFormatter(key) << "\""; if(values.exists(key)) { - boost::optional xy = getXY(values.at(key), graphvizFormatting); + boost::optional xy = getXY(values.at(key), formatting); if(xy) - stm << ", pos=\"" << graphvizFormatting.scale*(xy->x() - minX) << "," << graphvizFormatting.scale*(xy->y() - minY) << "!\""; + stm << ", pos=\"" << formatting.scale*(xy->x() - minX) << "," << formatting.scale*(xy->y() - minY) << "!\""; } stm << "];\n"; - - if (firstTimePoses) { - lastKey = key; - firstTimePoses = false; - } else { - stm << " var" << key << "--" << "var" << lastKey << ";\n"; - lastKey = key; - } } stm << "\n"; - - if(graphvizFormatting.mergeSimilarFactors) { + if (formatting.mergeSimilarFactors) { // Remove duplicate factors FastSet > structure; - BOOST_FOREACH(const sharedFactor& factor, *this) { + BOOST_FOREACH(const sharedFactor& factor, *this){ if(factor) { vector factorKeys = factor->keys(); std::sort(factorKeys.begin(), factorKeys.end()); @@ -199,57 +155,65 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, // Create factors and variable connections size_t i = 0; - BOOST_FOREACH(const vector& factorKeys, structure) { + BOOST_FOREACH(const vector& factorKeys, structure){ // Make each factor a dot stm << " factor" << i << "[label=\"\", shape=point"; { - map::const_iterator pos = graphvizFormatting.factorPositions.find(i); - if(pos != graphvizFormatting.factorPositions.end()) - stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\""; + map::const_iterator pos = formatting.factorPositions.find(i); + if(pos != formatting.factorPositions.end()) + stm << ", pos=\"" << formatting.scale*(pos->second.x() - minX) << "," + << formatting.scale*(pos->second.y() - minY) << "!\""; } stm << "];\n"; // Make factor-variable connections BOOST_FOREACH(Key key, factorKeys) { - stm << " var" << key << "--" << "factor" << i << ";\n"; } + stm << " var" << key << "--" << "factor" << i << ";\n"; + } ++ i; } } else { // Create factors and variable connections for(size_t i = 0; i < this->size(); ++i) { - if(graphvizFormatting.plotFactorPoints){ - // Make each factor a dot - stm << " factor" << i << "[label=\"\", shape=point"; - { - map::const_iterator pos = graphvizFormatting.factorPositions.find(i); - if(pos != graphvizFormatting.factorPositions.end()) - stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\""; - } - stm << "];\n"; + const NonlinearFactor::shared_ptr& factor = this->at(i); + if(formatting.plotFactorPoints) { + const FastVector& keys = factor->keys(); + if (formatting.binaryEdges && keys.size()==2) { + stm << " var" << keys[0] << "--" << "var" << keys[1] << ";\n"; + } else { + // Make each factor a dot + stm << " factor" << i << "[label=\"\", shape=point"; + { + map::const_iterator pos = formatting.factorPositions.find(i); + if(pos != formatting.factorPositions.end()) + stm << ", pos=\"" << formatting.scale*(pos->second.x() - minX) << "," + << formatting.scale*(pos->second.y() - minY) << "!\""; + } + stm << "];\n"; - // Make factor-variable connections - if(graphvizFormatting.connectKeysToFactor && this->at(i)) { - BOOST_FOREACH(Key key, *this->at(i)) { - stm << " var" << key << "--" << "factor" << i << ";\n"; + // Make factor-variable connections + if(formatting.connectKeysToFactor && factor) { + BOOST_FOREACH(Key key, *factor) { + stm << " var" << key << "--" << "factor" << i << ";\n"; + } + } + } } - } - - } else { - if(this->at(i)) { - Key k; - bool firstTime = true; - BOOST_FOREACH(Key key, *this->at(i)) { - if(firstTime){ - k = key; - firstTime = false; - continue; + if(factor) { + Key k; + bool firstTime = true; + BOOST_FOREACH(Key key, *this->at(i)) { + if(firstTime) { + k = key; + firstTime = false; + continue; + } + stm << " var" << key << "--" << "var" << k << ";\n"; + k = key; + } } - stm << " var" << key << "--" << "var" << k << ";\n"; - k = key; - } - } } } } @@ -282,13 +246,13 @@ FastSet NonlinearFactorGraph::keys() const { /* ************************************************************************* */ Ordering NonlinearFactorGraph::orderingCOLAMD() const { - return Ordering::COLAMD(*this); + return Ordering::colamd(*this); } /* ************************************************************************* */ Ordering NonlinearFactorGraph::orderingCOLAMDConstrained(const FastMap& constraints) const { - return Ordering::COLAMDConstrained(*this, constraints); + return Ordering::colamdConstrained(*this, constraints); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 35e532262..169d12455 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -32,6 +32,10 @@ namespace gtsam { class Ordering; class GaussianFactorGraph; class SymbolicFactorGraph; + template + class Expression; + template + class ExpressionFactor; /** * Formatting options when saving in GraphViz format using @@ -47,6 +51,7 @@ namespace gtsam { bool mergeSimilarFactors; ///< Merge multiple factors that have the same connectivity bool plotFactorPoints; ///< Plots each factor as a dot between the variables bool connectKeysToFactor; ///< Draw a line from each key within a factor to the dot of the factor + bool binaryEdges; ///< just use non-dotted edges for binary factors std::map factorPositions; ///< (optional for each factor) Manually specify factor "dot" positions. /// Default constructor sets up robot coordinates. Paper horizontal is robot Y, /// paper vertical is robot X. Default figure size of 5x5 in. @@ -54,7 +59,7 @@ namespace gtsam { paperHorizontalAxis(Y), paperVerticalAxis(X), figureWidthInches(5), figureHeightInches(5), scale(1), mergeSimilarFactors(false), plotFactorPoints(true), - connectKeysToFactor(true) {} + connectKeysToFactor(true), binaryEdges(true) {} }; @@ -150,16 +155,33 @@ namespace gtsam { */ NonlinearFactorGraph rekey(const std::map& rekey_mapping) const; + /** + * Directly add ExpressionFactor that implements |h(x)-z|^2_R + * @param h expression that implements measurement function + * @param z measurement + * @param R model + */ + template + void addExpressionFactor(const SharedNoiseModel& R, const T& z, + const Expression& h) { + push_back(boost::make_shared >(R, z, h)); + } + private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactorGraph", boost::serialization::base_object(*this)); } }; -} // namespace +/// traits +template<> +struct traits : public Testable { +}; + +} //\ namespace gtsam diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 80407a372..00746d9b7 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -109,7 +109,8 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg, delta = gfg.optimize(*params.ordering, params.getEliminationFunction()); } else if (params.isSequential()) { // Sequential QR or Cholesky (decided by params.getEliminationFunction()) - delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction())->optimize(); + delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(), + boost::none, params.orderingType)->optimize(); } else if (params.isIterative()) { // Conjugate Gradient -> needs params.iterativeParams diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index ef0f2aa9b..5a163ffb9 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -5,6 +5,7 @@ * @author Yong-Dian Jian * @author Richard Roberts * @author Frank Dellaert + * @author Andrew Melim */ #include @@ -107,10 +108,17 @@ void NonlinearOptimizerParams::print(const std::string& str) const { break; } - if (ordering) - std::cout << " ordering: custom\n"; - else - std::cout << " ordering: COLAMD\n"; + switch (orderingType){ + case Ordering::COLAMD: + std::cout << " ordering: COLAMD\n"; + break; + case Ordering::METIS: + std::cout << " ordering: METIS\n"; + break; + default: + std::cout << " ordering: custom\n"; + break; + } std::cout.flush(); } @@ -155,6 +163,32 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve throw std::invalid_argument( "Unknown linear solver type in SuccessiveLinearizationOptimizer"); } + /* ************************************************************************* */ +std::string NonlinearOptimizerParams::orderingTypeTranslator(Ordering::OrderingType type) const{ + switch (type) { + case Ordering::METIS: + return "METIS"; + case Ordering::COLAMD: + return "COLAMD"; + default: + if (ordering) + return "CUSTOM"; + else + throw std::invalid_argument( + "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); + } +} + +/* ************************************************************************* */ +Ordering::OrderingType NonlinearOptimizerParams::orderingTypeTranslator(const std::string& type) const{ + if (type == "METIS") + return Ordering::METIS; + if (type == "COLAMD") + return Ordering::COLAMD; + throw std::invalid_argument( + "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); +} + } // namespace diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index dafc1f065..10de6994f 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -15,6 +15,7 @@ * @author Yong-Dian Jian * @author Richard Roberts * @author Frank Dellaert + * @author Andrew Melim * @date Apr 1, 2012 */ @@ -42,11 +43,12 @@ public: double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5) double errorTol; ///< The maximum total error to stop iterating (default 0.0) Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT) + Ordering::OrderingType orderingType; ///< The method of ordering use during variable elimination (default COLAMD) NonlinearOptimizerParams() : maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), errorTol( - 0.0), verbosity(SILENT), linearSolverType(MULTIFRONTAL_CHOLESKY) { - } + 0.0), verbosity(SILENT), orderingType(Ordering::COLAMD), + linearSolverType(MULTIFRONTAL_CHOLESKY) {} virtual ~NonlinearOptimizerParams() { } @@ -152,12 +154,27 @@ public: void setOrdering(const Ordering& ordering) { this->ordering = ordering; + this->orderingType = Ordering::CUSTOM; + } + + std::string getOrderingType() const { + return orderingTypeTranslator(orderingType); + } + + // Note that if you want to use a custom ordering, you must set the ordering directly, this will switch to custom type + void setOrderingType(const std::string& ordering){ + orderingType = orderingTypeTranslator(ordering); } private: std::string linearSolverTranslator(LinearSolverType linearSolverType) const; - LinearSolverType linearSolverTranslator( - const std::string& linearSolverType) const; + + LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const; + + std::string orderingTypeTranslator(Ordering::OrderingType type) const; + + Ordering::OrderingType orderingTypeTranslator(const std::string& type) const; + }; // For backward compatibility: diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 4595a70ed..fe2c3f3ca 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -289,40 +289,16 @@ namespace gtsam { } /* ************************************************************************* */ - // insert a plain value using the default chart + // insert a templated value template void Values::insert(Key j, const ValueType& val) { - insert(j, static_cast(ChartValue >(val))); + insert(j, static_cast(GenericValue(val))); } - // insert with custom chart type - template - void Values::insert(Key j, const ValueType& val) { - insert(j, static_cast(ChartValue(val))); - } - - // overloaded insert with chart initializer - template - void Values::insert(Key j, const ValueType& val, ChartInit chart) { - insert(j, static_cast(ChartValue(val, chart))); - } - - // update with default chart + // update with templated value template void Values::update(Key j, const ValueType& val) { - update(j, static_cast(ChartValue >(val))); - } - - // update with custom chart - template - void Values::update(Key j, const ValueType& val) { - update(j, static_cast(ChartValue(val))); - } - - // update with chart initializer, /todo: perhaps there is a way to init chart from old value... - template - void Values::update(Key j, const ValueType& val, ChartInit chart) { - update(j, static_cast(ChartValue(val, chart))); + update(j, static_cast(GenericValue(val))); } } diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index b92e54143..372bced8e 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -112,6 +112,24 @@ namespace gtsam { return result; } + /* ************************************************************************* */ + Vector Values::atFixed(Key j, size_t n) { + switch (n) { + case 1: return at(j); + case 2: return at(j); + case 3: return at(j); + case 4: return at(j); + case 5: return at(j); + case 6: return at(j); + case 7: return at(j); + case 8: return at(j); + case 9: return at(j); + default: + throw runtime_error( + "Values::at fixed size can only handle n in 1..9"); + } + } + /* ************************************************************************* */ const Value& Values::at(Key j) const { // Find the item @@ -130,6 +148,24 @@ namespace gtsam { throw ValuesKeyAlreadyExists(j); } + /* ************************************************************************* */ + void Values::insertFixed(Key j, const Vector& v, size_t n) { + switch (n) { + case 1: insert(j,v); break; + case 2: insert(j,v); break; + case 3: insert(j,v); break; + case 4: insert(j,v); break; + case 5: insert(j,v); break; + case 6: insert(j,v); break; + case 7: insert(j,v); break; + case 8: insert(j,v); break; + case 9: insert(j,v); break; + default: + throw runtime_error( + "Values::insert fixed size can only handle n in 1..9"); + } + } + /* ************************************************************************* */ void Values::insert(const Values& values) { for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index e4a27849d..4eb89b084 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -24,12 +24,11 @@ #pragma once -#include -#include -#include +#include +#include +#include #include #include -#include #ifdef __GNUC__ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" @@ -39,15 +38,10 @@ #pragma GCC diagnostic pop #endif #include -#include #include #include -#include -#include -#include - namespace gtsam { // Forward declarations / utilities @@ -180,6 +174,13 @@ namespace gtsam { template const ValueType& at(Key j) const; + /// Special version for small fixed size vectors, for matlab/python + /// throws truntime error if n<1 || n>9 + Vector atFixed(Key j, size_t n); + + /// version for double + double atDouble(size_t key) const { return at(key);} + /** Retrieve a variable by key \c j. This version returns a reference * to the base Value class, and needs to be casted before use. * @param j Retrieve the value associated with this key @@ -253,19 +254,16 @@ namespace gtsam { /** Templated version to add a variable with the given j, * throws KeyAlreadyExists if j is already present - * if no chart is specified, the DefaultChart is used */ template void insert(Key j, const ValueType& val); - /// overloaded insert version that also specifies a chart - template - void insert(Key j, const ValueType& val); - - /// overloaded insert version that also specifies a chart initializer - template - void insert(Key j, const ValueType& val, ChartInit chart); + /// Special version for small fixed size vectors, for matlab/python + /// throws truntime error if n<1 || n>9 + void insertFixed(Key j, const Vector& v, size_t n); + /// version for double + void insertDouble(Key j, double c) { insert(j,c); } /** insert that mimics the STL map insert - if the value already exists, the map is not modified * and an iterator to the existing value is returned, along with 'false'. If the value did not @@ -283,14 +281,6 @@ namespace gtsam { template void update(Key j, const T& val); - /// overloaded insert version that also specifies a chart - template - void update(Key j, const T& val); - - /// overloaded insert version that also specifies a chart initializer - template - void update(Key j, const T& val, ChartInit chart); - /** update the current available values without adding new ones */ void update(const Values& values); @@ -408,7 +398,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(values_); } @@ -509,6 +499,12 @@ namespace gtsam { } }; -} + /// traits + template<> + struct traits : public Testable { + }; + +} //\ namespace gtsam + #include diff --git a/gtsam/nonlinear/expressionTesting.h b/gtsam/nonlinear/expressionTesting.h new file mode 100644 index 000000000..47f61b8b1 --- /dev/null +++ b/gtsam/nonlinear/expressionTesting.h @@ -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 + + * -------------------------------------------------------------------------- */ + +/** + * @file expressionTesting.h + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief Test harness methods for expressions. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace gtsam { + +namespace internal { +// CPPUnitLite-style test for linearization of a factor +void testFactorJacobians(TestResult& result_, const std::string& name_, + const NoiseModelFactor& factor, const gtsam::Values& values, double delta, + double tolerance) { + + // Create expected value by numerical differentiation + JacobianFactor expected = linearizeNumerically(factor, values, delta); + + // Create actual value by linearize + boost::shared_ptr actual = // + boost::dynamic_pointer_cast(factor.linearize(values)); + + // Check cast result and then equality + CHECK(actual); + EXPECT(assert_equal(expected, *actual, tolerance)); +} +} + +/// \brief Check the Jacobians produced by a factor against finite differences. +/// \param factor The factor to test. +/// \param values Values filled in for testing the Jacobians. +/// \param numerical_derivative_step The step to use when computing the numerical derivative Jacobians +/// \param tolerance The numerical tolerance to use when comparing Jacobians. +#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \ + { gtsam::internal::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance); } + +namespace internal { +// CPPUnitLite-style test for linearization of an ExpressionFactor +template +void testExpressionJacobians(TestResult& result_, const std::string& name_, + const gtsam::Expression& expression, const gtsam::Values& values, + double nd_step, double tolerance) { + // Create factor + size_t size = traits::dimension; + ExpressionFactor f(noiseModel::Unit::Create(size), + expression.value(values), expression); + testFactorJacobians(result_, name_, f, values, nd_step, tolerance); +} +} // namespace internal +} // namespace gtsam + +/// \brief Check the Jacobians produced by an expression against finite differences. +/// \param expression The expression to test. +/// \param values Values filled in for testing the Jacobians. +/// \param numerical_derivative_step The step to use when computing the finite difference Jacobians +/// \param tolerance The numerical tolerance to use when comparing Jacobians. +#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance) \ + { gtsam::internal::testExpressionJacobians(result_, name_, expression, values, numerical_derivative_step, tolerance); } diff --git a/gtsam/nonlinear/expressions.h b/gtsam/nonlinear/expressions.h new file mode 100644 index 000000000..2f46d6d74 --- /dev/null +++ b/gtsam/nonlinear/expressions.h @@ -0,0 +1,31 @@ +/** + * @file expressions.h + * @brief Common expressions, both linear and non-linear + * @date Nov 23, 2014 + * @author Frank Dellaert + */ + +#pragma once + +#include +#include + +namespace gtsam { + +// Generics +template +Expression between(const Expression& t1, const Expression& t2) { + return Expression(traits::Between, t1, t2); +} + +// Generics +template +Expression compose(const Expression& t1, const Expression& t2) { + return Expression(traits::Compose, t1, t2); +} + +typedef Expression double_; +typedef Expression Vector3_; + +} // \namespace gtsam + diff --git a/gtsam/nonlinear/factorTesting.h b/gtsam/nonlinear/factorTesting.h new file mode 100644 index 000000000..442b29382 --- /dev/null +++ b/gtsam/nonlinear/factorTesting.h @@ -0,0 +1,68 @@ +/* ---------------------------------------------------------------------------- + + * 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 factorTesting.h + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief Evaluate derivatives of a nonlinear factor numerically + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * Linearize a nonlinear factor using numerical differentiation + * The benefit of this method is that it does not need to know what types are + * involved to evaluate the factor. If all the machinery of gtsam is working + * correctly, we should get the correct numerical derivatives out the other side. + */ +JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, + const Values& values, double delta = 1e-5) { + + // We will fill a vector of key/Jacobians pairs (a map would sort) + std::vector > jacobians; + + // Get size + Eigen::VectorXd e = factor.whitenedError(values); + const size_t rows = e.size(); + + // Loop over all variables + VectorValues dX = values.zeroVectors(); + BOOST_FOREACH(Key key, factor) { + // Compute central differences using the values struct. + const size_t cols = dX.dim(key); + Matrix J = Matrix::Zero(rows, cols); + for (size_t col = 0; col < cols; ++col) { + Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); + dx[col] = delta; + dX[key] = dx; + Values eval_values = values.retract(dX); + Eigen::VectorXd left = factor.whitenedError(eval_values); + dx[col] = -delta; + dX[key] = dx; + eval_values = values.retract(dX); + Eigen::VectorXd right = factor.whitenedError(eval_values); + J.col(col) = (left - right) * (1.0 / (2.0 * delta)); + } + jacobians.push_back(std::make_pair(key,J)); + } + + // Next step...return JacobianFactor + return JacobianFactor(jacobians, -e); +} + +} // namespace gtsam diff --git a/gtsam/nonlinear/internal/CallRecord.h b/gtsam/nonlinear/internal/CallRecord.h new file mode 100644 index 000000000..90aa8a8be --- /dev/null +++ b/gtsam/nonlinear/internal/CallRecord.h @@ -0,0 +1,183 @@ +/* ---------------------------------------------------------------------------- + + * 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 CallRecord.h + * @date November 21, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @author Hannes Sommer + * @brief Internals for Expression.h, not for general consumption + */ + +#pragma once + +#include + +namespace gtsam { +namespace internal { + +/** + * ConvertToDynamicIf converts to a dense matrix with dynamic rows iff + * ConvertToDynamicRows (colums stay as they are) otherwise + * it just passes dense Eigen matrices through. + */ +template +struct ConvertToVirtualFunctionSupportedMatrixType { + template + static Eigen::Matrix convert( + const Eigen::MatrixBase & x) { + return x; + } +}; + +template<> +struct ConvertToVirtualFunctionSupportedMatrixType { + template + static const Eigen::Matrix convert( + const Eigen::MatrixBase & x) { + return x; + } + // special treatment of matrices that don't need conversion + template + static const Eigen::Matrix & convert( + const Eigen::Matrix & x) { + return x; + } +}; + +/** + * The CallRecord is an abstract base class for the any class that stores + * the Jacobians of applying a function with respect to each of its arguments, + * as well as an execution trace for each of its arguments. + */ +template +struct CallRecord { + + // Print entire record, recursively + inline void print(const std::string& indent) const { + _print(indent); + } + + // Main entry point for the reverse AD process of a functional expression. + // Called *once* by the main AD entry point, ExecutionTrace::startReverseAD1 + // This function then calls ExecutionTrace::reverseAD for every argument + // which will in turn call the reverseAD method below. + // This non-virtual function _startReverseAD3, implemented in derived + inline void startReverseAD2(JacobianMap& jacobians) const { + _startReverseAD3(jacobians); + } + + // Dispatch the reverseAD2 calls issued by ExecutionTrace::reverseAD1 + // Here we convert to dynamic if the + template + inline void reverseAD2(const Eigen::MatrixBase & dFdT, + JacobianMap& jacobians) const { + _reverseAD3( + ConvertToVirtualFunctionSupportedMatrixType< + (Derived::RowsAtCompileTime > 5)>::convert(dFdT), jacobians); + } + + // This overload supports matrices with both rows and columns dynamically sized. + // The template version above would be slower by introducing an extra conversion + // to statically sized columns. + inline void reverseAD2(const Matrix & dFdT, JacobianMap& jacobians) const { + _reverseAD3(dFdT, jacobians); + } + + virtual ~CallRecord() { + } + +private: + + virtual void _print(const std::string& indent) const = 0; + virtual void _startReverseAD3(JacobianMap& jacobians) const = 0; + + virtual void _reverseAD3(const Matrix & dFdT, + JacobianMap& jacobians) const = 0; + + virtual void _reverseAD3( + const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; +}; + +/** + * CallRecordMaxVirtualStaticRows tells which separate virtual reverseAD with specific + * static rows (1..CallRecordMaxVirtualStaticRows) methods are part of the CallRecord + * interface. It is used to keep the testCallRecord unit test in sync. + */ +const int CallRecordMaxVirtualStaticRows = 5; + +/** + * The CallRecordImplementor implements the CallRecord interface for a Derived class by + * delegating to its corresponding (templated) non-virtual methods. + */ +template +struct CallRecordImplementor: public CallRecord { +private: + + const Derived & derived() const { + return static_cast(*this); + } + + virtual void _print(const std::string& indent) const { + derived().print(indent); + } + + virtual void _startReverseAD3(JacobianMap& jacobians) const { + derived().startReverseAD4(jacobians); + } + + virtual void _reverseAD3(const Matrix & dFdT, JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + + virtual void _reverseAD3( + const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } +}; + +} // namespace internal +} // gtsam diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h new file mode 100644 index 000000000..37ce4dfd5 --- /dev/null +++ b/gtsam/nonlinear/internal/ExecutionTrace.h @@ -0,0 +1,166 @@ +/* ---------------------------------------------------------------------------- + + * 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 ExecutionTrace.h + * @date May 11, 2015 + * @author Frank Dellaert + * @brief Execution trace for expressions + */ + +#pragma once + +#include +#include +#include + +#include + +#include + +namespace gtsam { +namespace internal { + +template struct CallRecord; + +/// Storage type for the execution trace. +/// It enforces the proper alignment in a portable way. +/// Provide a traceSize() sized array of this type to traceExecution as traceStorage. +static const unsigned TraceAlignment = 16; +typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage; + +template +struct UseBlockIf { + static void addToJacobian(const Eigen::MatrixBase& dTdA, + JacobianMap& jacobians, Key key) { + // block makes HUGE difference + jacobians(key).block( + 0, 0) += dTdA; + } +}; + +/// Handle Leaf Case for Dynamic Matrix type (slower) +template +struct UseBlockIf { + static void addToJacobian(const Eigen::MatrixBase& dTdA, + JacobianMap& jacobians, Key key) { + jacobians(key) += dTdA; + } +}; + +/// Handle Leaf Case: reverse AD ends here, by writing a matrix into Jacobians +template +void handleLeafCase(const Eigen::MatrixBase& dTdA, + JacobianMap& jacobians, Key key) { + UseBlockIf< + Derived::RowsAtCompileTime != Eigen::Dynamic + && Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian( + dTdA, jacobians, key); +} + +/** + * The ExecutionTrace class records a tree-structured expression's execution. + * + * The class looks a bit complicated but it is so for performance. + * It is a tagged union that obviates the need to create + * a ExecutionTrace subclass for Constants and Leaf Expressions. Instead + * the key for the leaf is stored in the space normally used to store a + * CallRecord*. Nothing is stored for a Constant. + * + * A full execution trace of a Binary(Unary(Binary(Leaf,Constant)),Leaf) would be: + * Trace(Function) -> + * BinaryRecord with two traces in it + * trace1(Function) -> + * UnaryRecord with one trace in it + * trace1(Function) -> + * BinaryRecord with two traces in it + * trace1(Leaf) + * trace2(Constant) + * trace2(Leaf) + * Hence, there are three Record structs, written to memory by traceExecution + */ +template +class ExecutionTrace { + static const int Dim = traits::dimension; + enum { + Constant, Leaf, Function + } kind; + union { + Key key; + CallRecord* ptr; + } content; +public: + /// Pointer always starts out as a Constant + ExecutionTrace() : + kind(Constant) { + } + /// Change pointer to a Leaf Record + void setLeaf(Key key) { + kind = Leaf; + content.key = key; + } + /// Take ownership of pointer to a Function Record + void setFunction(CallRecord* record) { + kind = Function; + content.ptr = record; + } + /// Print + void print(const std::string& indent = "") const { + if (kind == Constant) + std::cout << indent << "Constant" << std::endl; + else if (kind == Leaf) + std::cout << indent << "Leaf, key = " << content.key << std::endl; + else if (kind == Function) { + std::cout << indent << "Function" << std::endl; + content.ptr->print(indent + " "); + } + } + /// Return record pointer, quite unsafe, used only for testing + template + boost::optional record() { + if (kind != Function) + return boost::none; + else { + Record* p = dynamic_cast(content.ptr); + return p ? boost::optional(p) : boost::none; + } + } + /** + * *** This is the main entry point for reverse AD, called from Expression *** + * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function) + */ + typedef Eigen::Matrix JacobianTT; + void startReverseAD1(JacobianMap& jacobians) const { + if (kind == Leaf) { + // This branch will only be called on trivial Leaf expressions, i.e. Priors + static const JacobianTT I = JacobianTT::Identity(); + handleLeafCase(I, jacobians, content.key); + } else if (kind == Function) + // This is the more typical entry point, starting the AD pipeline + // Inside startReverseAD2 the correctly dimensioned pipeline is chosen. + content.ptr->startReverseAD2(jacobians); + } + // Either add to Jacobians (Leaf) or propagate (Function) + template + void reverseAD1(const Eigen::MatrixBase & dTdA, + JacobianMap& jacobians) const { + if (kind == Leaf) + handleLeafCase(dTdA, jacobians, content.key); + else if (kind == Function) + content.ptr->reverseAD2(dTdA, jacobians); + } + + /// Define type so we can apply it as a meta-function + typedef ExecutionTrace type; +}; + +} // namespace internal +} // namespace gtsam diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h new file mode 100644 index 000000000..e7aa34db0 --- /dev/null +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -0,0 +1,516 @@ +/* ---------------------------------------------------------------------------- + + * 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 ExpressionNode.h + * @date May 10, 2015 + * @author Frank Dellaert + * @author Paul Furgale + * @brief ExpressionNode class + */ + +#pragma once + +#include +#include +#include + +#include // operator typeid +#include +#include + +class ExpressionFactorBinaryTest; +// Forward declare for testing + +namespace gtsam { +namespace internal { + +template +T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) { + // right now only word sized types are supported. + // Easy to extend if needed, + // by somehow inferring the unsigned integer of same size + BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t)); + size_t & uiValue = reinterpret_cast(value); + size_t misAlignment = uiValue % requiredAlignment; + if (misAlignment) { + uiValue += requiredAlignment - misAlignment; + } + return value; +} +template +T upAligned(T value, unsigned requiredAlignment = TraceAlignment) { + return upAlign(value, requiredAlignment); +} + +//----------------------------------------------------------------------------- + +/** + * Expression node. The superclass for objects that do the heavy lifting + * An Expression has a pointer to an ExpressionNode underneath + * allowing Expressions to have polymorphic behaviour even though they + * are passed by value. This is the same way boost::function works. + * http://loki-lib.sourceforge.net/html/a00652.html + */ +template +class ExpressionNode { + +protected: + + size_t traceSize_; + + /// Constructor, traceSize is size of the execution trace of expression rooted here + ExpressionNode(size_t traceSize = 0) : + traceSize_(traceSize) { + } + +public: + + /// Destructor + virtual ~ExpressionNode() { + } + + /// Streaming + GTSAM_EXPORT + friend std::ostream &operator<<(std::ostream &os, + const ExpressionNode& node) { + os << "Expression of type " << typeid(T).name(); + if (node.traceSize_ > 0) + os << ", trace size = " << node.traceSize_; + os << "\n"; + return os; + } + + /// Return keys that play in this expression as a set + virtual std::set keys() const { + std::set keys; + return keys; + } + + /// Return dimensions for each argument, as a map + virtual void dims(std::map& map) const { + } + + // Return size needed for memory buffer in traceExecution + size_t traceSize() const { + return traceSize_; + } + + /// Return value + virtual T value(const Values& values) const = 0; + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* traceStorage) const = 0; +}; + +//----------------------------------------------------------------------------- +/// Constant Expression +template +class ConstantExpression: public ExpressionNode { + + /// The constant value + T constant_; + + /// Constructor with a value, yielding a constant + ConstantExpression(const T& value) : + constant_(value) { + } + + friend class Expression ; + +public: + + /// Destructor + virtual ~ConstantExpression() { + } + + /// Return value + virtual T value(const Values& values) const { + return constant_; + } + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* traceStorage) const { + return constant_; + } +}; + +//----------------------------------------------------------------------------- +/// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value +template +class LeafExpression: public ExpressionNode { + typedef T value_type; + + /// The key into values + Key key_; + + /// Constructor with a single key + LeafExpression(Key key) : + key_(key) { + } + + friend class Expression ; + +public: + + /// Destructor + virtual ~LeafExpression() { + } + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys; + keys.insert(key_); + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + map[key_] = traits::dimension; + } + + /// Return value + virtual T value(const Values& values) const { + return values.at(key_); + } + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* traceStorage) const { + trace.setLeaf(key_); + return values.at(key_); + } + +}; + +//----------------------------------------------------------------------------- +/// meta-function to generate fixed-size JacobianTA type +template +struct Jacobian { + typedef Eigen::Matrix::dimension, traits::dimension> type; +}; + +// Eigen format for printing Jacobians +static const Eigen::IOFormat kMatlabFormat(0, 1, " ", "; ", "", "", "[", "]"); + +//----------------------------------------------------------------------------- +/// Unary Function Expression +template +class UnaryExpression: public ExpressionNode { + + typedef typename Expression::template UnaryFunction::type Function; + boost::shared_ptr > expression1_; + Function function_; + + /// Constructor with a unary function f, and input argument e1 + UnaryExpression(Function f, const Expression& e1) : + expression1_(e1.root()), function_(f) { + ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize(); + } + + friend class Expression ; + +public: + + /// Destructor + virtual ~UnaryExpression() { + } + + /// Return value + virtual T value(const Values& values) const { + return function_(expression1_->value(values), boost::none); + } + + /// Return keys that play in this expression + virtual std::set keys() const { + return expression1_->keys(); + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + expression1_->dims(map); + } + + // Inner Record Class + struct Record: public CallRecordImplementor::dimension> { + + A1 value1; + ExecutionTrace trace1; + typename Jacobian::type dTdA1; + + /// Print to std::cout + void print(const std::string& indent) const { + std::cout << indent << "UnaryExpression::Record {" << std::endl; + std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl; + trace1.print(indent); + std::cout << indent << "}" << std::endl; + } + + /// Start the reverse AD process + void startReverseAD4(JacobianMap& jacobians) const { + // This is the crucial point where the size of the AD pipeline is selected. + // One pipeline is started for each argument, but the number of rows in each + // pipeline is the same, namely the dimension of the output argument T. + // For example, if the entire expression is rooted by a binary function + // yielding a 2D result, then the matrix dTdA will have 2 rows. + // ExecutionTrace::reverseAD1 just passes this on to CallRecord::reverseAD2 + // which calls the correctly sized CallRecord::reverseAD3, which in turn + // calls reverseAD4 below. + trace1.reverseAD1(dTdA1, jacobians); + } + + /// Given df/dT, multiply in dT/dA and continue reverse AD process + template + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + trace1.reverseAD1(dFdT * dTdA1, jacobians); + } + }; + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* ptr) const { + assert(reinterpret_cast(ptr) % TraceAlignment == 0); + + // Create the record at the start of the traceStorage and advance the pointer + Record* record = new (ptr) Record(); + ptr += upAligned(sizeof(Record)); + + // Record the traces for all arguments + // After this, the traceStorage pointer is set to after what was written + // Write an Expression execution trace in record->trace + // Iff Constant or Leaf, this will not write to traceStorage, only to trace. + // Iff the expression is functional, write all Records in traceStorage buffer + // Return value of type T is recorded in record->value + record->value1 = expression1_->traceExecution(values, record->trace1, ptr); + + // ptr is never modified by traceExecution, but if traceExecution has + // written in the buffer, the next caller expects we advance the pointer + ptr += expression1_->traceSize(); + trace.setFunction(record); + + return function_(record->value1, record->dTdA1); + } +}; + +//----------------------------------------------------------------------------- +/// Binary Expression +template +class BinaryExpression: public ExpressionNode { + + typedef typename Expression::template BinaryFunction::type Function; + boost::shared_ptr > expression1_; + boost::shared_ptr > expression2_; + Function function_; + + /// Constructor with a binary function f, and two input arguments + BinaryExpression(Function f, const Expression& e1, + const Expression& e2) : + expression1_(e1.root()), expression2_(e2.root()), function_(f) { + ExpressionNode::traceSize_ = // + upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize(); + } + + friend class Expression ; + friend class ::ExpressionFactorBinaryTest; + +public: + + /// Destructor + virtual ~BinaryExpression() { + } + + /// Return value + virtual T value(const Values& values) const { + using boost::none; + return function_(expression1_->value(values), expression2_->value(values), + none, none); + } + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys = expression1_->keys(); + std::set myKeys = expression2_->keys(); + keys.insert(myKeys.begin(), myKeys.end()); + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + expression1_->dims(map); + expression2_->dims(map); + } + + // Inner Record Class + struct Record: public CallRecordImplementor::dimension> { + + A1 value1; + ExecutionTrace trace1; + typename Jacobian::type dTdA1; + + A2 value2; + ExecutionTrace trace2; + typename Jacobian::type dTdA2; + + /// Print to std::cout + void print(const std::string& indent) const { + std::cout << indent << "BinaryExpression::Record {" << std::endl; + std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl; + trace1.print(indent); + std::cout << indent << dTdA2.format(kMatlabFormat) << std::endl; + trace2.print(indent); + std::cout << indent << "}" << std::endl; + } + + /// Start the reverse AD process, see comments in Base + void startReverseAD4(JacobianMap& jacobians) const { + trace1.reverseAD1(dTdA1, jacobians); + trace2.reverseAD1(dTdA2, jacobians); + } + + /// Given df/dT, multiply in dT/dA and continue reverse AD process + template + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + trace1.reverseAD1(dFdT * dTdA1, jacobians); + trace2.reverseAD1(dFdT * dTdA2, jacobians); + } + }; + + /// Construct an execution trace for reverse AD, see UnaryExpression for explanation + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* ptr) const { + assert(reinterpret_cast(ptr) % TraceAlignment == 0); + Record* record = new (ptr) Record(); + ptr += upAligned(sizeof(Record)); + record->value1 = expression1_->traceExecution(values, record->trace1, ptr); + record->value2 = expression2_->traceExecution(values, record->trace2, ptr); + ptr += expression1_->traceSize() + expression2_->traceSize(); + trace.setFunction(record); + return function_(record->value1, record->value2, record->dTdA1, + record->dTdA2); + } +}; + +//----------------------------------------------------------------------------- +/// Ternary Expression +template +class TernaryExpression: public ExpressionNode { + + typedef typename Expression::template TernaryFunction::type Function; + boost::shared_ptr > expression1_; + boost::shared_ptr > expression2_; + boost::shared_ptr > expression3_; + Function function_; + + /// Constructor with a ternary function f, and two input arguments + TernaryExpression(Function f, const Expression& e1, + const Expression& e2, const Expression& e3) : + expression1_(e1.root()), expression2_(e2.root()), expression3_(e3.root()), // + function_(f) { + ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + // + e1.traceSize() + e2.traceSize() + e3.traceSize(); + } + + friend class Expression ; + +public: + + /// Destructor + virtual ~TernaryExpression() { + } + + /// Return value + virtual T value(const Values& values) const { + using boost::none; + return function_(expression1_->value(values), expression2_->value(values), + expression3_->value(values), none, none, none); + } + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys = expression1_->keys(); + std::set myKeys = expression2_->keys(); + keys.insert(myKeys.begin(), myKeys.end()); + myKeys = expression3_->keys(); + keys.insert(myKeys.begin(), myKeys.end()); + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + expression1_->dims(map); + expression2_->dims(map); + expression3_->dims(map); + } + + // Inner Record Class + struct Record: public CallRecordImplementor::dimension> { + + A1 value1; + ExecutionTrace trace1; + typename Jacobian::type dTdA1; + + A2 value2; + ExecutionTrace trace2; + typename Jacobian::type dTdA2; + + A3 value3; + ExecutionTrace trace3; + typename Jacobian::type dTdA3; + + /// Print to std::cout + void print(const std::string& indent) const { + std::cout << indent << "TernaryExpression::Record {" << std::endl; + std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl; + trace1.print(indent); + std::cout << indent << dTdA2.format(kMatlabFormat) << std::endl; + trace2.print(indent); + std::cout << indent << dTdA3.format(kMatlabFormat) << std::endl; + trace3.print(indent); + std::cout << indent << "}" << std::endl; + } + + /// Start the reverse AD process, see comments in Base + void startReverseAD4(JacobianMap& jacobians) const { + trace1.reverseAD1(dTdA1, jacobians); + trace2.reverseAD1(dTdA2, jacobians); + trace3.reverseAD1(dTdA3, jacobians); + } + + /// Given df/dT, multiply in dT/dA and continue reverse AD process + template + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + trace1.reverseAD1(dFdT * dTdA1, jacobians); + trace2.reverseAD1(dFdT * dTdA2, jacobians); + trace3.reverseAD1(dFdT * dTdA3, jacobians); + } + }; + + /// Construct an execution trace for reverse AD, see UnaryExpression for explanation + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* ptr) const { + assert(reinterpret_cast(ptr) % TraceAlignment == 0); + Record* record = new (ptr) Record(); + ptr += upAligned(sizeof(Record)); + record->value1 = expression1_->traceExecution(values, record->trace1, ptr); + record->value2 = expression2_->traceExecution(values, record->trace2, ptr); + record->value3 = expression3_->traceExecution(values, record->trace3, ptr); + ptr += expression1_->traceSize() + expression2_->traceSize() + + expression3_->traceSize(); + trace.setFunction(record); + return function_(record->value1, record->value2, record->value3, + record->dTdA1, record->dTdA2, record->dTdA3); + } +}; + +} // namespace internal +} // namespace gtsam diff --git a/gtsam/nonlinear/internal/JacobianMap.h b/gtsam/nonlinear/internal/JacobianMap.h new file mode 100644 index 000000000..f4d2e9565 --- /dev/null +++ b/gtsam/nonlinear/internal/JacobianMap.h @@ -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 + + * -------------------------------------------------------------------------- */ + +/** + * @file JacobianMap.h + * @date May 11, 2015 + * @author Frank Dellaert + * @brief JacobianMap for returning derivatives from expressions + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { +namespace internal { + +// A JacobianMap is the primary mechanism by which derivatives are returned. +// Expressions are designed to write their derivatives into an already allocated +// Jacobian of the correct size, of type VerticalBlockMatrix. +// The JacobianMap provides a mapping from keys to the underlying blocks. +class JacobianMap { +private: + typedef FastVector Keys; + const FastVector& keys_; + VerticalBlockMatrix& Ab_; + +public: + /// Construct a JacobianMap for writing into a VerticalBlockMatrix Ab + JacobianMap(const Keys& keys, VerticalBlockMatrix& Ab) : + keys_(keys), Ab_(Ab) { + } + + /// Access blocks of via key + VerticalBlockMatrix::Block operator()(Key key) { + Keys::const_iterator it = std::find(keys_.begin(), keys_.end(), key); + DenseIndex block = it - keys_.begin(); + return Ab_(block); + } +}; + +} // namespace internal +} // namespace gtsam + diff --git a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp similarity index 71% rename from gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp rename to gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index edb26c4f4..3f477098a 100644 --- a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -17,30 +17,94 @@ * @brief unit tests for Block Automatic Differentiation */ -#include -#include +#include +#include +#include #include #include #include #include #include #include -#include -#include - -#undef CHECK #include #include using boost::assign::list_of; using boost::assign::map_list_of; +namespace gtsam { + +// Special version of Cal3Bundler so that default constructor = 0,0,0 +struct Cal: public Cal3Bundler { + Cal(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0, double v0 = 0) : + Cal3Bundler(f, k1, k2, u0, v0) { + } + Cal retract(const Vector& d) const { + return Cal(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0()); + } + Vector3 localCoordinates(const Cal& T2) const { + return T2.vector() - vector(); + } +}; + +template<> +struct traits : public internal::Manifold {}; + +// With that, camera below behaves like Snavely's 9-dim vector +typedef PinholeCamera Camera; + +} + using namespace std; using namespace gtsam; -// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector -typedef PinholeCamera Camera; +/* ************************************************************************* */ +// charts +TEST(AdaptAutoDiff, Canonical) { + + Canonical chart1; + EXPECT(chart1.Local(Point2(1, 0))==Vector2(1, 0)); + EXPECT(chart1.Retract(Vector2(1, 0))==Point2(1, 0)); + + Vector v2(2); + v2 << 1, 0; + Canonical chart2; + EXPECT(assert_equal(v2, chart2.Local(Vector2(1, 0)))); + EXPECT(chart2.Retract(v2)==Vector2(1, 0)); + + Canonical chart3; + Eigen::Matrix v1; + v1 << 1; + EXPECT(chart3.Local(1)==v1); + EXPECT_DOUBLES_EQUAL(chart3.Retract(v1), 1, 1e-9); + + Canonical chart4; + Point3 point(1, 2, 3); + Vector v3(3); + v3 << 1, 2, 3; + EXPECT(assert_equal(v3, chart4.Local(point))); + EXPECT(assert_equal(chart4.Retract(v3), point)); + + Canonical chart5; + Pose3 pose(Rot3::identity(), point); + Vector v6(6); + v6 << 0, 0, 0, 1, 2, 3; + EXPECT(assert_equal(v6, chart5.Local(pose))); + EXPECT(assert_equal(chart5.Retract(v6), pose)); + + Canonical chart6; + Cal cal0; + Vector z3 = Vector3::Zero(); + EXPECT(assert_equal(z3, chart6.Local(cal0))); + EXPECT(assert_equal(chart6.Retract(z3), cal0)); + + Canonical chart7; + Camera camera(Pose3(), cal0); + Vector z9 = Vector9::Zero(); + EXPECT(assert_equal(z9, chart7.Local(camera))); + EXPECT(assert_equal(chart7.Retract(z9), camera)); +} /* ************************************************************************* */ // Some Ceres Snippets copied for testing @@ -89,7 +153,7 @@ struct Projective { /* ************************************************************************* */ // Test Ceres AutoDiff -TEST(Expression, AutoDiff) { +TEST(AdaptAutoDiff, AutoDiff) { using ceres::internal::AutoDiff; // Instantiate function @@ -133,7 +197,7 @@ Vector2 adapted(const Vector9& P, const Vector3& X) { throw std::runtime_error("Snavely fail"); } -TEST(Expression, AutoDiff2) { +TEST(AdaptAutoDiff, AutoDiff2) { using ceres::internal::AutoDiff; // Instantiate function @@ -166,10 +230,10 @@ TEST(Expression, AutoDiff2) { /* ************************************************************************* */ // Test AutoDiff wrapper Snavely -TEST(Expression, AutoDiff3) { +TEST(AdaptAutoDiff, AutoDiff3) { // Make arguments - Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal3Bundler(1, 0, 0)); + Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal(1, 0, 0)); Point3 X(10, 0, -5); // negative Z-axis convention of Snavely! typedef AdaptAutoDiff Adaptor; @@ -190,19 +254,21 @@ TEST(Expression, AutoDiff3) { Point2 actual2 = snavely(P, X, H1, H2); EXPECT(assert_equal(expected,actual2,1e-9)); EXPECT(assert_equal(E1,H1,1e-8)); + EXPECT(assert_equal(E2,H2,1e-8)); } /* ************************************************************************* */ // Test AutoDiff wrapper in an expression -TEST(Expression, Snavely) { +TEST(AdaptAutoDiff, Snavely) { Expression P(1); Expression X(2); typedef AdaptAutoDiff Adaptor; Expression expression(Adaptor(), P, X); #ifdef GTSAM_USE_QUATERNIONS - EXPECT_LONGS_EQUAL(400,expression.traceSize()); // Todo, should be zero + EXPECT_LONGS_EQUAL(384,expression.traceSize()); // Todo, should be zero #else - EXPECT_LONGS_EQUAL(432,expression.traceSize()); // Todo, should be zero + EXPECT_LONGS_EQUAL(sizeof(internal::BinaryExpression::Record), + expression.traceSize()); // Todo, should be zero #endif set expected = list_of(1)(2); EXPECT(expected == expression.keys()); diff --git a/gtsam_unstable/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp similarity index 69% rename from gtsam_unstable/nonlinear/tests/testCallRecord.cpp rename to gtsam/nonlinear/tests/testCallRecord.cpp index a4561b349..208f0b284 100644 --- a/gtsam_unstable/nonlinear/tests/testCallRecord.cpp +++ b/gtsam/nonlinear/tests/testCallRecord.cpp @@ -18,13 +18,12 @@ * @brief unit tests for CallRecord class */ -#include - -#include - +#include #include #include +#include + using namespace std; using namespace gtsam; @@ -33,7 +32,7 @@ static const int Cols = 3; int dynamicIfAboveMax(int i){ - if(i > MaxVirtualStaticRows){ + if(i > internal::CallRecordMaxVirtualStaticRows){ return Eigen::Dynamic; } else return i; @@ -43,7 +42,6 @@ struct CallConfig { int compTimeCols; int runTimeRows; int runTimeCols; - CallConfig() {} CallConfig(int rows, int cols): compTimeRows(dynamicIfAboveMax(rows)), compTimeCols(cols), @@ -71,29 +69,35 @@ struct CallConfig { } }; +/// traits +namespace gtsam { +template<> struct traits : public Testable {}; +} + struct Record: public internal::CallRecordImplementor { + Record() : cc(0, 0) {} virtual ~Record() { } void print(const std::string& indent) const { } - void startReverseAD(JacobianMap& jacobians) const { + void startReverseAD4(internal::JacobianMap& jacobians) const { } mutable CallConfig cc; private: template - void reverseAD(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + void reverseAD4(const SomeMatrix & dFdT, internal::JacobianMap& jacobians) const { cc.compTimeRows = SomeMatrix::RowsAtCompileTime; cc.compTimeCols = SomeMatrix::ColsAtCompileTime; cc.runTimeRows = dFdT.rows(); cc.runTimeCols = dFdT.cols(); } - template - friend struct internal::ReverseADImplementor; + template + friend struct internal::CallRecordImplementor; }; -JacobianMap & NJM= *static_cast(NULL); +internal::JacobianMap & NJM= *static_cast(NULL); /* ************************************************************************* */ typedef Eigen::Matrix DynRowMat; @@ -102,56 +106,56 @@ TEST(CallRecord, virtualReverseAdDispatching) { Record record; { const int Rows = 1; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } { const int Rows = 2; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } { const int Rows = 3; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } { - const int Rows = MaxVirtualStaticRows; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + const int Rows = 4; + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } { - const int Rows = MaxVirtualStaticRows + 1; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + const int Rows = 5; + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } { - const int Rows = MaxVirtualStaticRows + 2; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + const int Rows = 6; + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } } diff --git a/gtsam/nonlinear/tests/testExecutionTrace.cpp b/gtsam/nonlinear/tests/testExecutionTrace.cpp new file mode 100644 index 000000000..731f69816 --- /dev/null +++ b/gtsam/nonlinear/tests/testExecutionTrace.cpp @@ -0,0 +1,39 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------1------------------------------------------- */ + +/** + * @file testExecutionTrace.cpp + * @date May 11, 2015 + * @author Frank Dellaert + * @brief unit tests for Expression internals + */ + +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +// Constant +TEST(ExecutionTrace, construct) { + internal::ExecutionTrace trace; +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp similarity index 64% rename from gtsam_unstable/nonlinear/tests/testExpression.cpp rename to gtsam/nonlinear/tests/testExpression.cpp index d660d28b5..3e86bcb8c 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -17,11 +17,10 @@ * @brief unit tests for Block Automatic Differentiation */ +#include #include #include -#include #include -#include #include @@ -34,8 +33,8 @@ using namespace gtsam; /* ************************************************************************* */ template -Point2 uncalibrate(const CAL& K, const Point2& p, - boost::optional Dcal, boost::optional Dp) { +Point2 uncalibrate(const CAL& K, const Point2& p, OptionalJacobian<2, 5> Dcal, + OptionalJacobian<2, 2> Dp) { return K.uncalibrate(p, Dcal, Dp); } @@ -43,7 +42,7 @@ static const Rot3 someR = Rot3::RzRyRx(1, 2, 3); /* ************************************************************************* */ // Constant -TEST(Expression, constant) { +TEST(Expression, Constant) { Expression R(someR); Values values; Rot3 actual = R.value(values); @@ -69,31 +68,86 @@ TEST(Expression, Leaves) { Point3 somePoint(1, 2, 3); values.insert(Symbol('p', 10), somePoint); std::vector > points = createUnknowns(10, 'p', 1); - EXPECT(assert_equal(somePoint,points.back().value(values))); + EXPECT(assert_equal(somePoint, points.back().value(values))); } /* ************************************************************************* */ +// Unary(Leaf) +namespace unary { +Point2 f1(const Point3& p, OptionalJacobian<2, 3> H) { + return Point2(); +} +double f2(const Point3& p, OptionalJacobian<1, 3> H) { + return 0.0; +} +Vector f3(const Point3& p, OptionalJacobian H) { + return p.vector(); +} +Expression p(1); +set expected = list_of(1); +} +TEST(Expression, Unary1) { + using namespace unary; + Expression e(f1, p); + EXPECT(expected == e.keys()); +} +TEST(Expression, Unary2) { + using namespace unary; + Expression e(f2, p); + EXPECT(expected == e.keys()); +} +/* ************************************************************************* */ +// Unary(Leaf), dynamic +TEST(Expression, Unary3) { + using namespace unary; +// Expression e(f3, p); +} +/* ************************************************************************* */ +//Nullary Method +TEST(Expression, NullaryMethod) { -//TEST(Expression, NullaryMethod) { -// Expression p(67); -// Expression norm(p, &Point3::norm); -// Values values; -// values.insert(67,Point3(3,4,5)); -// Augmented a = norm.augmented(values); -// EXPECT(a.value() == sqrt(50)); -// JacobianMap expected; -// expected[67] = (Matrix(1,3) << 3/sqrt(50),4/sqrt(50),5/sqrt(50)); -// EXPECT(assert_equal(expected.at(67),a.jacobians().at(67))); -//} + // Create expression + Expression p(67); + Expression norm(p, &Point3::norm); + + // Create Values + Values values; + values.insert(67, Point3(3, 4, 5)); + + // Check dims as map + std::map map; + norm.dims(map); + LONGS_EQUAL(1, map.size()); + + // Get value and Jacobians + std::vector H(1); + double actual = norm.value(values, H); + + // Check all + EXPECT(actual == sqrt(50)); + Matrix expected(1, 3); + expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0); + EXPECT(assert_equal(expected, H[0])); +} /* ************************************************************************* */ // Binary(Leaf,Leaf) namespace binary { // Create leaves +double doubleF(const Pose3& pose, // + const Point3& point, OptionalJacobian<1, 6> H1, OptionalJacobian<1, 3> H2) { + return 0.0; +} Expression x(1); Expression p(2); Expression p_cam(x, &Pose3::transform_to, p); } /* ************************************************************************* */ +// Check that creating an expression to double compiles +TEST(Expression, BinaryToDouble) { + using namespace binary; + Expression p_cam(doubleF, x, p); +} +/* ************************************************************************* */ // keys TEST(Expression, BinaryKeys) { set expected = list_of(1)(2); @@ -102,14 +156,14 @@ TEST(Expression, BinaryKeys) { /* ************************************************************************* */ // dimensions TEST(Expression, BinaryDimensions) { - map actual, expected = map_list_of(1, 6)(2, 3); + map actual, expected = map_list_of(1, 6)(2, 3); binary::p_cam.dims(actual); - EXPECT(actual==expected); + EXPECT(actual == expected); } /* ************************************************************************* */ // dimensions TEST(Expression, BinaryTraceSize) { - typedef BinaryExpression Binary; + typedef internal::BinaryExpression Binary; size_t expectedTraceSize = sizeof(Binary::Record); EXPECT_LONGS_EQUAL(expectedTraceSize, binary::p_cam.traceSize()); } @@ -133,20 +187,28 @@ TEST(Expression, TreeKeys) { /* ************************************************************************* */ // dimensions TEST(Expression, TreeDimensions) { - map actual, expected = map_list_of(1, 6)(2, 3)(3, - 5); + map actual, expected = map_list_of(1, 6)(2, 3)(3, 5); tree::uv_hat.dims(actual); - EXPECT(actual==expected); + EXPECT(actual == expected); } /* ************************************************************************* */ // TraceSize TEST(Expression, TreeTraceSize) { - typedef UnaryExpression Unary; - typedef BinaryExpression Binary1; - typedef BinaryExpression Binary2; - size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary1::Record) - + sizeof(Binary2::Record); - EXPECT_LONGS_EQUAL(expectedTraceSize, tree::uv_hat.traceSize()); + typedef internal::BinaryExpression Binary1; + EXPECT_LONGS_EQUAL(internal::upAligned(sizeof(Binary1::Record)), + tree::p_cam.traceSize()); + + typedef internal::UnaryExpression Unary; + EXPECT_LONGS_EQUAL( + internal::upAligned(sizeof(Unary::Record)) + tree::p_cam.traceSize(), + tree::projection.traceSize()); + + EXPECT_LONGS_EQUAL(0, tree::K.traceSize()); + + typedef internal::BinaryExpression Binary2; + EXPECT_LONGS_EQUAL( + internal::upAligned(sizeof(Binary2::Record)) + tree::K.traceSize() + + tree::projection.traceSize(), tree::uv_hat.traceSize()); } /* ************************************************************************* */ @@ -190,8 +252,8 @@ TEST(Expression, compose3) { /* ************************************************************************* */ // Test with ternary function Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, - boost::optional H1, boost::optional H2, - boost::optional H3) { + OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, + OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) *H1 = eye(3); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam/nonlinear/tests/testExpressionFactor.cpp similarity index 75% rename from gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp rename to gtsam/nonlinear/tests/testExpressionFactor.cpp index e609af953..2fb544edf 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam/nonlinear/tests/testExpressionFactor.cpp @@ -17,13 +17,13 @@ * @brief unit tests for Block Automatic Differentiation */ -#include -#include +#include #include #include #include -#include -#include +#include +#include +#include #include #include @@ -77,10 +77,13 @@ TEST(ExpressionFactor, Model) { // Concise version ExpressionFactor f(model, Point2(0, 0), p); + + // Check values and derivatives EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f.dim()); boost::shared_ptr gf2 = f.linearize(values); EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); // another way } /* ************************************************************************* */ @@ -125,16 +128,49 @@ TEST(ExpressionFactor, Unary) { EXPECT( assert_equal(expected, *jf, 1e-9)); } +/* ************************************************************************* */ +// Unary(Leaf)) and Unary(Unary(Leaf))) +// wide version (not handled in fixed-size pipeline) +typedef Eigen::Matrix Matrix93; +Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) { + Vector9 v; + v << p.vector(), p.vector(), p.vector(); + if (H) *H << eye(3), eye(3), eye(3); + return v; +} +typedef Eigen::Matrix Matrix9; +Vector9 id9(const Vector9& v, OptionalJacobian<9,9> H) { + if (H) *H = Matrix9::Identity(); + return v; +} +TEST(ExpressionFactor, Wide) { + // Create some values + Values values; + values.insert(2, Point3(0, 0, 1)); + Point3_ point(2); + Vector9 measured; + measured.setZero(); + Expression expression(wide,point); + SharedNoiseModel model = noiseModel::Unit::Create(9); + + ExpressionFactor f1(model, measured, expression); + EXPECT_CORRECT_FACTOR_JACOBIANS(f1, values, 1e-5, 1e-9); + + Expression expression2(id9,expression); + ExpressionFactor f2(model, measured, expression2); + EXPECT_CORRECT_FACTOR_JACOBIANS(f2, values, 1e-5, 1e-9); +} + /* ************************************************************************* */ static Point2 myUncal(const Cal3_S2& K, const Point2& p, - boost::optional Dcal, boost::optional Dp) { + OptionalJacobian<2,5> Dcal, OptionalJacobian<2,2> Dp) { return K.uncalibrate(p, Dcal, Dp); } // Binary(Leaf,Leaf) TEST(ExpressionFactor, Binary) { - typedef BinaryExpression Binary; + typedef internal::BinaryExpression Binary; Cal3_S2_ K_(1); Point2_ p_(2); @@ -149,11 +185,15 @@ TEST(ExpressionFactor, Binary) { EXPECT_LONGS_EQUAL(8, sizeof(double)); EXPECT_LONGS_EQUAL(16, sizeof(Point2)); EXPECT_LONGS_EQUAL(40, sizeof(Cal3_S2)); - EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); - EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); - EXPECT_LONGS_EQUAL(2*5*8, sizeof(Jacobian::type)); - EXPECT_LONGS_EQUAL(2*2*8, sizeof(Jacobian::type)); - size_t expectedRecordSize = 16 + 16 + 40 + 2 * 16 + 80 + 32; + EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace)); + EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace)); + EXPECT_LONGS_EQUAL(2*5*8, sizeof(internal::Jacobian::type)); + EXPECT_LONGS_EQUAL(2*2*8, sizeof(internal::Jacobian::type)); + size_t expectedRecordSize = sizeof(Cal3_S2) + + sizeof(internal::ExecutionTrace) + + +sizeof(internal::Jacobian::type) + sizeof(Point2) + + sizeof(internal::ExecutionTrace) + + sizeof(internal::Jacobian::type); EXPECT_LONGS_EQUAL(expectedRecordSize + 8, sizeof(Binary::Record)); // Check size @@ -162,9 +202,9 @@ TEST(ExpressionFactor, Binary) { EXPECT_LONGS_EQUAL(expectedRecordSize + 8, size); // Use Variable Length Array, allocated on stack by gcc // Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla - char raw[size]; - ExecutionTrace trace; - Point2 value = binary.traceExecution(values, trace, raw); + internal::ExecutionTraceStorage traceStorage[size]; + internal::ExecutionTrace trace; + Point2 value = binary.traceExecution(values, trace, traceStorage); EXPECT(assert_equal(Point2(),value, 1e-9)); // trace.print(); @@ -177,9 +217,8 @@ TEST(ExpressionFactor, Binary) { // Check matrices boost::optional r = trace.record(); CHECK(r); - EXPECT( - assert_equal(expected25, (Matrix) (*r)-> jacobian(), 1e-9)); - EXPECT( assert_equal(expected22, (Matrix) (*r)->jacobian(), 1e-9)); + EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9)); + EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9)); } /* ************************************************************************* */ // Unary(Binary(Leaf,Leaf)) @@ -203,24 +242,35 @@ TEST(ExpressionFactor, Shallow) { // Construct expression, concise evrsion Point2_ expression = project(transform_to(x_, p_)); + // Get and check keys and dims + FastVector keys; + FastVector dims; + boost::tie(keys, dims) = expression.keysAndDims(); + LONGS_EQUAL(2,keys.size()); + LONGS_EQUAL(2,dims.size()); + LONGS_EQUAL(1,keys[0]); + LONGS_EQUAL(2,keys[1]); + LONGS_EQUAL(6,dims[0]); + LONGS_EQUAL(3,dims[1]); + // traceExecution of shallow tree - typedef UnaryExpression Unary; - typedef BinaryExpression Binary; + typedef internal::UnaryExpression Unary; + typedef internal::BinaryExpression Binary; size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary::Record); - EXPECT_LONGS_EQUAL(112, sizeof(Unary::Record)); + EXPECT_LONGS_EQUAL(96, sizeof(Unary::Record)); #ifdef GTSAM_USE_QUATERNIONS EXPECT_LONGS_EQUAL(352, sizeof(Binary::Record)); - LONGS_EQUAL(112+352, expectedTraceSize); + LONGS_EQUAL(96+352, expectedTraceSize); #else - EXPECT_LONGS_EQUAL(400, sizeof(Binary::Record)); - LONGS_EQUAL(112+400, expectedTraceSize); + EXPECT_LONGS_EQUAL(384, sizeof(Binary::Record)); + LONGS_EQUAL(96+384, expectedTraceSize); #endif size_t size = expression.traceSize(); CHECK(size); EXPECT_LONGS_EQUAL(expectedTraceSize, size); - char raw[size]; - ExecutionTrace trace; - Point2 value = expression.traceExecution(values, trace, raw); + internal::ExecutionTraceStorage traceStorage[size]; + internal::ExecutionTrace trace; + Point2 value = expression.traceExecution(values, trace, traceStorage); EXPECT(assert_equal(Point2(),value, 1e-9)); // trace.print(); @@ -231,7 +281,7 @@ TEST(ExpressionFactor, Shallow) { // Check matrices boost::optional r = trace.record(); CHECK(r); - EXPECT(assert_equal(expected23, (Matrix)(*r)->jacobian(), 1e-9)); + EXPECT(assert_equal(expected23, (Matrix)(*r)->dTdA1, 1e-9)); // Linearization ExpressionFactor f2(model, measured, expression); @@ -281,7 +331,7 @@ TEST(ExpressionFactor, tree) { boost::shared_ptr gf2 = f2.linearize(values); EXPECT( assert_equal(*expected, *gf2, 1e-9)); - TernaryExpression::Function fff = project6; + Expression::TernaryFunction::type fff = project6; // Try ternary version ExpressionFactor f3(model, measured, project3(x, p, K)); @@ -382,8 +432,7 @@ TEST(ExpressionFactor, compose3) { /* ************************************************************************* */ // Test compose with three arguments Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, - boost::optional H1, boost::optional H2, - boost::optional H3) { + OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) *H1 = eye(3); @@ -425,6 +474,34 @@ TEST(ExpressionFactor, composeTernary) { EXPECT( assert_equal(expected, *jf,1e-9)); } +TEST(ExpressionFactor, tree_finite_differences) { + + // Create some values + Values values; + values.insert(1, Pose3()); + values.insert(2, Point3(0, 0, 1)); + values.insert(3, Cal3_S2()); + + // Create leaves + Pose3_ x(1); + Point3_ p(2); + Cal3_S2_ K(3); + + // Create expression tree + Point3_ p_cam(x, &Pose3::transform_to, p); + Point2_ xy_hat(PinholeCamera::project_to_camera, p_cam); + Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); + + const double fd_step = 1e-5; + const double tolerance = 1e-5; + EXPECT_CORRECT_EXPRESSION_JACOBIANS(uv_hat, values, fd_step, tolerance); +} + +TEST(ExpressionFactor, push_back) { + NonlinearFactorGraph graph; + graph.addExpressionFactor(model, Point2(0, 0), leaf::p); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index a8f287d1e..6dc3e3d2f 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -32,37 +33,26 @@ using namespace gtsam::serializationTestHelpers; /* ************************************************************************* */ // Export all classes derived from Value -BOOST_CLASS_EXPORT(gtsam::Cal3_S2); -BOOST_CLASS_EXPORT(gtsam::Cal3Bundler); -BOOST_CLASS_EXPORT(gtsam::Point3); -BOOST_CLASS_EXPORT(gtsam::Pose3); -BOOST_CLASS_EXPORT(gtsam::Rot3); -BOOST_CLASS_EXPORT(gtsam::PinholeCamera); -BOOST_CLASS_EXPORT(gtsam::PinholeCamera); -BOOST_CLASS_EXPORT(gtsam::PinholeCamera); +GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); +GTSAM_VALUE_EXPORT(gtsam::Cal3Bundler); +GTSAM_VALUE_EXPORT(gtsam::Point3); +GTSAM_VALUE_EXPORT(gtsam::Pose3); +GTSAM_VALUE_EXPORT(gtsam::Rot3); +GTSAM_VALUE_EXPORT(gtsam::PinholeCamera); +GTSAM_VALUE_EXPORT(gtsam::PinholeCamera); +GTSAM_VALUE_EXPORT(gtsam::PinholeCamera); namespace detail { template struct pack { typedef T type; }; } -#define CHART_VALUE_EXPORT(UNIQUE_NAME, TYPE) \ - typedef gtsam::ChartValue > UNIQUE_NAME; \ - BOOST_CLASS_EXPORT( UNIQUE_NAME ); - /* ************************************************************************* */ typedef PinholeCamera PinholeCal3S2; typedef PinholeCamera PinholeCal3DS2; typedef PinholeCamera PinholeCal3Bundler; -CHART_VALUE_EXPORT(gtsamPoint3Chart, gtsam::Point3); -CHART_VALUE_EXPORT(Cal3S2Chart, PinholeCal3S2); -CHART_VALUE_EXPORT(Cal3DS2Chart, PinholeCal3DS2); -CHART_VALUE_EXPORT(Cal3BundlerChart, PinholeCal3Bundler); - - - /* ************************************************************************* */ static Point3 pt3(1.0, 2.0, 3.0); static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); @@ -73,27 +63,21 @@ static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); static Cal3Bundler cal3(1.0, 2.0, 3.0); TEST (Serialization, TemplatedValues) { - std::cout << __LINE__ << std::endl; EXPECT(equalsObj(pt3)); - std::cout << __LINE__ << std::endl; - ChartValue chv1(pt3); - std::cout << __LINE__ << std::endl; + GenericValue chv1(pt3); EXPECT(equalsObj(chv1)); - std::cout << __LINE__ << std::endl; PinholeCal3S2 pc(pose3,cal1); EXPECT(equalsObj(pc)); - std::cout << __LINE__ << std::endl; + Values values; values.insert(1,pt3); - std::cout << __LINE__ << std::endl; + EXPECT(equalsObj(values)); - std::cout << __LINE__ << std::endl; values.insert(Symbol('a',0), PinholeCal3S2(pose3, cal1)); values.insert(Symbol('s',5), PinholeCal3DS2(pose3, cal2)); values.insert(Symbol('d',47), PinholeCal3Bundler(pose3, cal3)); values.insert(Symbol('a',5), PinholeCal3S2(pose3, cal1)); EXPECT(equalsObj(values)); - std::cout << __LINE__ << std::endl; EXPECT(equalsXML(values)); EXPECT(equalsBinary(values)); } diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 09fe0f253..faa285cd5 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -12,6 +12,8 @@ /** * @file testValues.cpp * @author Richard Roberts + * @author Frank Dellaert + * @author Mike Bosse */ #include @@ -53,22 +55,27 @@ int TestValueData::DestructorCount = 0; class TestValue { TestValueData data_; public: + enum {dimension = 0}; void print(const std::string& str = "") const {} bool equals(const TestValue& other, double tol = 1e-9) const { return true; } size_t dim() const { return 0; } - TestValue retract(const Vector&) const { return TestValue(); } - Vector localCoordinates(const TestValue&) const { return Vector(); } + TestValue retract(const Vector&, + OptionalJacobian H1=boost::none, + OptionalJacobian H2=boost::none) const { + return TestValue(); + } + Vector localCoordinates(const TestValue&, + OptionalJacobian H1=boost::none, + OptionalJacobian H2=boost::none) const { + return Vector(); + } }; namespace gtsam { -namespace traits { -template <> -struct is_manifold : public boost::true_type {}; -template <> -struct dimension : public boost::integral_constant {}; -} +template <> struct traits : public internal::Manifold {}; } + /* ************************************************************************* */ TEST( Values, equals1 ) { @@ -169,8 +176,8 @@ TEST(Values, basic_functions) const Values& values_c = values; values.insert(2, Vector3()); values.insert(4, Vector3()); - values.insert(6, Vector3()); - values.insert(8, Vector3()); + values.insert(6, Matrix23()); + values.insert(8, Matrix23()); // find EXPECT_LONGS_EQUAL(4, values.find(4)->key); @@ -468,6 +475,15 @@ TEST(Values, Destructors) { LONGS_EQUAL(4+2, (long)TestValueData::DestructorCount); } +/* ************************************************************************* */ +TEST(Values, FixedSize) { + Values values; + Vector v(3); v << 5.0, 6.0, 7.0; + values.insertFixed(key1, v, 3); + Vector3 expected(5.0, 6.0, 7.0); + CHECK(assert_equal((Vector)expected, values.at(key1))); + CHECK_EXCEPTION(values.insertFixed(key1, v, 12),runtime_error); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h index ae794db6a..b3fd76c67 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -109,7 +109,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("AntiFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(factor_); diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index d5740a6ab..c78b5a3bf 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -91,7 +91,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index ef02b5cb1..1b51224d2 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -113,7 +113,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measuredBearing_); diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 1b630374c..bfd7d4e34 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -31,6 +31,10 @@ namespace gtsam { template class BetweenFactor: public NoiseModelFactor2 { + // Check that VALUE type is a testable Lie group + BOOST_CONCEPT_ASSERT((IsTestable)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); + public: typedef VALUE T; @@ -42,10 +46,6 @@ namespace gtsam { VALUE measured_; /** The measurement */ - /** concept check by type */ - GTSAM_CONCEPT_LIE_TYPE(T) - GTSAM_CONCEPT_TESTABLE_TYPE(T) - public: // shorthand for a smart pointer to a factor @@ -74,25 +74,32 @@ namespace gtsam { std::cout << s << "BetweenFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; - measured_.print(" measured: "); + traits::Print(measured_, " measured: "); this->noiseModel_->print(" noise model: "); } /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); + return e != NULL && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); } /** implement functions needed to derive from Factor */ /** vector of errors */ - Vector evaluateError(const T& p1, const T& p2, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - T hx = p1.between(p2, H1, H2); // h(x) + Vector evaluateError(const T& p1, const T& p2, boost::optional H1 = + boost::none, boost::optional H2 = boost::none) const { + T hx = traits::Between(p1, p2, H1, H2); // h(x) // manifold equivalent of h(x)-z -> log(z,h(x)) - return measured_.localCoordinates(hx); +#ifdef SLOW_BUT_CORRECT_BETWEENFACTOR + typename traits::ChartJacobian::Fixed Hlocal; + Vector rval = traits::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0); + if (H1) *H1 = Hlocal * (*H1); + if (H1) *H2 = Hlocal * (*H2); + return rval; +#else + return traits::Local(measured_, hx); +#endif } /** return the measured */ @@ -110,13 +117,17 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); } }; // \class BetweenFactor + /// traits + template + struct traits > : public Testable > {}; + /** * Binary between constraint - forces between to a given value * This constraint requires the underlying type to a Lie type @@ -129,17 +140,23 @@ namespace gtsam { /** Syntactic sugar for constrained version */ BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) : - BetweenFactor(key1, key2, measured, noiseModel::Constrained::All(VALUE::Dim(), fabs(mu))) {} + BetweenFactor(key1, key2, measured, + noiseModel::Constrained::All(traits::GetDimension(measured), fabs(mu))) + {} private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("BetweenFactor", boost::serialization::base_object >(*this)); } }; // \class BetweenConstraint + /// traits + template + struct traits > : public Testable > {}; + } /// namespace gtsam diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index 9cb1e3017..44a5ee5f2 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -84,7 +84,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(threshold_); @@ -157,7 +157,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(threshold_); diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h index 0e4fb48cf..92a78279b 100644 --- a/gtsam/slam/EssentialMatrixConstraint.h +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -97,7 +97,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 9142f9d3d..31c8270a4 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -35,16 +35,22 @@ namespace gtsam { * @addtogroup SLAM */ template - class GeneralSFMFactor: public NoiseModelFactor2 { + class GeneralSFMFactor: public NoiseModelFactor2 { + + GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA) + GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK) + + static const int DimC = FixedDimension::value; + static const int DimL = FixedDimension::value; + protected: + Point2 measured_; ///< the 2D measurement public: - typedef CAMERA Cam; ///< typedef for camera type - typedef GeneralSFMFactor This; ///< typedef for this object + typedef GeneralSFMFactor This; ///< typedef for this object typedef NoiseModelFactor2 Base; ///< typedef for the base class - typedef Point2 Measurement; ///< typedef for the measurement // shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -89,7 +95,7 @@ namespace gtsam { } /** h(x)-z */ - Vector evaluateError(const Cam& camera, const Point3& point, + Vector evaluateError(const CAMERA& camera, const LANDMARK& point, boost::optional H1=boost::none, boost::optional H2=boost::none) const { try { @@ -97,8 +103,8 @@ namespace gtsam { return reprojError.vector(); } catch( CheiralityException& e) { - if (H1) *H1 = zeros(2, camera.dim()); - if (H2) *H2 = zeros(2, point.dim()); + if (H1) *H1 = zeros(2, DimC); + if (H2) *H2 = zeros(2, DimL); std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; return zero(2); @@ -114,28 +120,37 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); } }; + template + struct traits > : Testable< + GeneralSFMFactor > { + }; + /** * Non-linear factor for a constraint derived from a 2D measurement. * Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera.. */ template class GeneralSFMFactor2: public NoiseModelFactor3 { + + GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) + static const int DimK = FixedDimension::value; + protected: - Point2 measured_; ///< the 2D measurement + + Point2 measured_; ///< the 2D measurement public: typedef GeneralSFMFactor2 This; typedef PinholeCamera Camera; ///< typedef for camera type typedef NoiseModelFactor3 Base; ///< typedef for the base class - typedef Point2 Measurement; ///< typedef for the measurement // shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -189,9 +204,9 @@ namespace gtsam { return reprojError.vector(); } catch( CheiralityException& e) { - if (H1) *H1 = zeros(2, pose3.dim()); - if (H2) *H2 = zeros(2, point.dim()); - if (H3) *H3 = zeros(2, calib.dim()); + if (H1) *H1 = zeros(2, 6); + if (H2) *H2 = zeros(2, 3); + if (H3) *H3 = zeros(2, DimK); std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; } @@ -207,13 +222,16 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor3", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); } }; - + template + struct traits > : Testable< + GeneralSFMFactor2 > { + }; } //namespace diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index f4dfb9422..ed6213058 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * 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 JacobianFactorQ.h * @date Oct 27, 2013 @@ -6,16 +17,18 @@ #pragma once -#include "JacobianSchurFactor.h" +#include "RegularJacobianFactor.h" namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ -template -class JacobianFactorQ: public JacobianSchurFactor { +template +class JacobianFactorQ: public RegularJacobianFactor { - typedef JacobianSchurFactor Base; + typedef RegularJacobianFactor Base; + typedef Eigen::Matrix MatrixZD; + typedef std::pair KeyMatrixZD; public: @@ -24,37 +37,45 @@ public: } /// Empty constructor with keys - JacobianFactorQ(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { - Matrix zeroMatrix = Matrix::Zero(0,D); + JacobianFactorQ(const FastVector& keys, // + const SharedDiagonal& model = SharedDiagonal()) : + Base() { + Matrix zeroMatrix = Matrix::Zero(0, D); Vector zeroVector = Vector::Zero(0); typedef std::pair KeyMatrix; std::vector QF; QF.reserve(keys.size()); BOOST_FOREACH(const Key& key, keys) - QF.push_back(KeyMatrix(key, zeroMatrix)); + QF.push_back(KeyMatrix(key, zeroMatrix)); JacobianFactor::fillTerms(QF, zeroVector, model); } /// Constructor - JacobianFactorQ(const std::vector& Fblocks, - const Matrix& E, const Matrix3& P, const Vector& b, - const SharedDiagonal& model = SharedDiagonal()) : - JacobianSchurFactor() { - size_t j = 0, m2 = E.rows(), m = m2 / 2; + JacobianFactorQ(const std::vector& Fblocks, const Matrix& E, + const Matrix3& P, const Vector& b, const SharedDiagonal& model = + SharedDiagonal()) : + Base() { + size_t j = 0, m2 = E.rows(), m = m2 / ZDim; // Calculate projector Q Matrix Q = eye(m2) - E * P * E.transpose(); // Calculate pre-computed Jacobian matrices // TODO: can we do better ? typedef std::pair KeyMatrix; - std::vector < KeyMatrix > QF; + std::vector QF; QF.reserve(m); - // Below, we compute each 2m*D block A_j = Q_j * F_j = (2m*2) * (2*D) - BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) - QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); + // Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D) + BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) + QF.push_back( + KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second)); // Which is then passed to the normal JacobianFactor constructor JacobianFactor::fillTerms(QF, Q * b, model); } }; +// end class JacobianFactorQ + +// traits +template struct traits > : public Testable< + JacobianFactorQ > { +}; } diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h index a928106a8..4c1b0ff14 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -6,41 +6,48 @@ */ #pragma once -#include +#include +#include +#include namespace gtsam { + +class GaussianBayesNet; + /** * JacobianFactor for Schur complement that uses Q noise model */ -template -class JacobianFactorQR: public JacobianSchurFactor { +template +class JacobianFactorQR: public RegularJacobianFactor { - typedef JacobianSchurFactor Base; + typedef RegularJacobianFactor Base; + typedef Eigen::Matrix MatrixZD; + typedef std::pair KeyMatrixZD; public: /** * Constructor */ - JacobianFactorQR(const std::vector& Fblocks, - const Matrix& E, const Matrix3& P, const Vector& b, + JacobianFactorQR(const std::vector& Fblocks, const Matrix& E, + const Matrix3& P, const Vector& b, // const SharedDiagonal& model = SharedDiagonal()) : - JacobianSchurFactor() { + Base() { // Create a number of Jacobian factors in a factor graph GaussianFactorGraph gfg; Symbol pointKey('p', 0); size_t i = 0; - BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) { - gfg.add(pointKey, E.block<2, 3>(2 * i, 0), it.first, it.second, - b.segment<2>(2 * i), model); + BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) { + gfg.add(pointKey, E.block(ZDim * i, 0), it.first, it.second, + b.segment(ZDim * i), model); i += 1; } //gfg.print("gfg"); // eliminate the point - GaussianBayesNet::shared_ptr bn; + boost::shared_ptr bn; GaussianFactorGraph::shared_ptr fg; - std::vector < Key > variables; + std::vector variables; variables.push_back(pointKey); boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR); //fg->print("fg"); @@ -48,6 +55,6 @@ public: JacobianFactor::operator=(JacobianFactor(*fg)); } }; -// class +// end class JacobianFactorQR -}// gtsam +}// end namespace gtsam diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index e28185038..b4389d681 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -5,50 +5,57 @@ */ #pragma once -#include "gtsam/slam/JacobianSchurFactor.h" +#include "gtsam/slam/RegularJacobianFactor.h" namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ -template -class JacobianFactorSVD: public JacobianSchurFactor { +template +class JacobianFactorSVD: public RegularJacobianFactor { + + typedef RegularJacobianFactor Base; + typedef Eigen::Matrix MatrixZD; // e.g 2 x 6 with Z=Point2 + typedef std::pair KeyMatrixZD; + typedef std::pair KeyMatrix; public: - typedef Eigen::Matrix Matrix2D; - typedef std::pair KeyMatrix2D; - typedef std::pair KeyMatrix; - /// Default constructor - JacobianFactorSVD() {} + JacobianFactorSVD() { + } /// Empty constructor with keys - JacobianFactorSVD(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { - Matrix zeroMatrix = Matrix::Zero(0,D); + JacobianFactorSVD(const FastVector& keys, // + const SharedDiagonal& model = SharedDiagonal()) : + Base() { + Matrix zeroMatrix = Matrix::Zero(0, D); Vector zeroVector = Vector::Zero(0); std::vector QF; QF.reserve(keys.size()); BOOST_FOREACH(const Key& key, keys) - QF.push_back(KeyMatrix(key, zeroMatrix)); + QF.push_back(KeyMatrix(key, zeroMatrix)); JacobianFactor::fillTerms(QF, zeroVector, model); } /// Constructor - JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { - size_t numKeys = Enull.rows() / 2; - size_t j = 0, m2 = 2*numKeys-3; + JacobianFactorSVD(const std::vector& Fblocks, + const Matrix& Enull, const Vector& b, // + const SharedDiagonal& model = SharedDiagonal()) : + Base() { + size_t numKeys = Enull.rows() / ZDim; + size_t j = 0, m2 = ZDim * numKeys - 3; // PLAIN NULL SPACE TRICK // Matrix Q = Enull * Enull.transpose(); - // BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) + // BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); // JacobianFactor factor(QF, Q * b); std::vector QF; QF.reserve(numKeys); - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) - QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, 2 * j++, m2, 2) * it.second)); + BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) + QF.push_back( + KeyMatrix(it.first, + (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second)); JacobianFactor::fillTerms(QF, Enull.transpose() * b, model); } }; diff --git a/gtsam/slam/JacobianSchurFactor.h b/gtsam/slam/JacobianSchurFactor.h deleted file mode 100644 index 2beecc264..000000000 --- a/gtsam/slam/JacobianSchurFactor.h +++ /dev/null @@ -1,93 +0,0 @@ -/* - * @file JacobianSchurFactor.h - * @brief Jacobianfactor that combines and eliminates points - * @date Oct 27, 2013 - * @uthor Frank Dellaert - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace gtsam { -/** - * JacobianFactor for Schur complement that uses Q noise model - */ -template -class JacobianSchurFactor: public JacobianFactor { - -public: - - typedef Eigen::Matrix Matrix2D; - typedef std::pair KeyMatrix2D; - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - - /** - * @brief double* Matrix-vector multiply, i.e. y = A*x - * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way - */ - Vector operator*(const double* x) const { - Vector Ax = zero(Ab_.rows()); - if (empty()) return Ax; - - // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhiten(Ax) : Ax; - } - - /** - * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e - * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way - */ - void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const - { - Vector E = alpha * (model_ ? model_->whiten(e) : e); - // Just iterate over all A matrices and insert Ai^e into y - for(size_t pos=0; postransposeMultiplyAdd(alpha,Ax,y); - if (empty()) return; - Vector Ax = zero(Ab_.rows()); - - // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhitenInPlace(Ax); model_->whitenInPlace(Ax); } - - // multiply with alpha - Ax *= alpha; - - // Again iterate over all A matrices and insert Ai^e into y - for(size_t pos=0; posnoiseModel_->print(" noise model: "); +} + +//*************************************************************************** +void OrientedPlane3DirectionPrior::print(const string& s, + const KeyFormatter& keyFormatter) const { + cout << "Prior Factor on " << landmarkKey_ << "\n"; + measured_p_.print("Measured Plane"); + this->noiseModel_->print(" noise model: "); +} + +//*************************************************************************** +bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected, + double tol) const { + const This* e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) + && this->measured_p_.equals(e->measured_p_, tol); +} + +//*************************************************************************** + +Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane, + boost::optional H) const { + + if (H) { + Matrix H_p; + Unit3 n_hat_p = measured_p_.normal(); + Unit3 n_hat_q = plane.normal(); + Vector e = n_hat_p.error(n_hat_q, H_p); + H->resize(2, 3); + H->block<2, 2>(0, 0) << H_p; + H->block<2, 1>(0, 2) << Matrix::Zero(2, 1); + return e; + } else { + Unit3 n_hat_p = measured_p_.normal(); + Unit3 n_hat_q = plane.normal(); + Vector e = n_hat_p.error(n_hat_q); + return e; + } + +} +} + diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h new file mode 100644 index 000000000..ab77ec612 --- /dev/null +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -0,0 +1,91 @@ +/* + * @file OrientedPlane3Factor.cpp + * @brief OrientedPlane3 Factor class + * @author Alex Trevor + * @date December 22, 2013 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * Factor to measure a planar landmark from a given pose + */ +class OrientedPlane3Factor: public NoiseModelFactor2 { + +protected: + Key poseKey_; + Key landmarkKey_; + Vector measured_coeffs_; + OrientedPlane3 measured_p_; + + typedef NoiseModelFactor2 Base; + +public: + + /// Constructor + OrientedPlane3Factor() { + } + + /// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol + OrientedPlane3Factor(const Vector&z, const SharedGaussian& noiseModel, + const Key& pose, const Key& landmark) : + Base(noiseModel, pose, landmark), poseKey_(pose), landmarkKey_(landmark), measured_coeffs_( + z) { + measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3)); + } + + /// print + virtual void print(const std::string& s = "OrientedPlane3Factor", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// evaluateError + virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1, + H2); + Vector err(3); + err << predicted_plane.error(measured_p_); + return (err); + } + ; +}; + +// TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior +class OrientedPlane3DirectionPrior: public NoiseModelFactor1 { +protected: + OrientedPlane3 measured_p_; /// measured plane parameters + Key landmarkKey_; + typedef NoiseModelFactor1 Base; +public: + + typedef OrientedPlane3DirectionPrior This; + /// Constructor + OrientedPlane3DirectionPrior() { + } + + /// Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol + OrientedPlane3DirectionPrior(Key key, const Vector&z, + const SharedGaussian& noiseModel) : + Base(noiseModel, key), landmarkKey_(key) { + measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3)); + } + + /// print + virtual void print(const std::string& s = "OrientedPlane3DirectionPrior", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// equals + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + + virtual Vector evaluateError(const OrientedPlane3& plane, + boost::optional H = boost::none) const; +}; + +} // gtsam + diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index 7d31e2e2c..94c19a9d0 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -29,6 +29,10 @@ public: GTSAM_CONCEPT_GROUP_TYPE(Pose) GTSAM_CONCEPT_LIE_TYPE(Rotation) + // Get dimensions of pose and rotation type at compile time + static const int xDim = FixedDimension::value; + static const int rDim = FixedDimension::value; + protected: Rotation measured_; @@ -70,7 +74,6 @@ public: /** h(x)-z */ Vector evaluateError(const Pose& pose, boost::optional H = boost::none) const { const Rotation& newR = pose.rotation(); - const size_t rDim = newR.dim(), xDim = pose.dim(); if (H) { *H = gtsam::zeros(rDim, xDim); std::pair rotInterval = POSE::rotationInterval(); @@ -85,7 +88,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index 2d9b3fdd4..e9914955c 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -62,7 +62,8 @@ public: Vector evaluateError(const Pose& pose, boost::optional H = boost::none) const { const Translation& newTrans = pose.translation(); const Rotation& R = pose.rotation(); - const size_t tDim = newTrans.dim(), xDim = pose.dim(); + const int tDim = traits::GetDimension(newTrans); + const int xDim = traits::GetDimension(pose); if (H) { *H = gtsam::zeros(tDim, xDim); std::pair transInterval = POSE::translationInterval(); @@ -89,7 +90,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 888dcb76b..a738d4cf0 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -57,6 +57,11 @@ namespace gtsam { Base(model, key), prior_(prior) { } + /** Convenience constructor that takes a full covariance argument */ + PriorFactor(Key key, const VALUE& prior, const Matrix& covariance) : + Base(noiseModel::Gaussian::Covariance(covariance), key), prior_(prior) { + } + /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { return boost::static_pointer_cast( @@ -67,23 +72,24 @@ namespace gtsam { /** print */ virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; - prior_.print(" prior mean: "); + traits::Print(prior_, " prior mean: "); this->noiseModel_->print(" noise model: "); } /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This* e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol); + return e != NULL && Base::equals(*e, tol) && traits::Equals(prior_, e->prior_, tol); } /** implement functions needed to derive from Factor */ /** vector of errors */ Vector evaluateError(const T& p, boost::optional H = boost::none) const { - if (H) (*H) = eye(p.dim()); + if (H) (*H) = eye(traits::GetDimension(p)); // manifold equivalent of h(x)-z -> log(z,h(x)) - return prior_.localCoordinates(p); + // TODO(ASL) Add Jacobians. + return traits::Local(prior_,p); } const VALUE & prior() const { return prior_; } @@ -93,7 +99,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index db37e6b8d..1056527d1 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -178,7 +178,7 @@ namespace gtsam { /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); @@ -187,4 +187,11 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); } }; + + /// traits + template + struct traits > : + public Testable > { + }; + } // \ namespace gtsam diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index af1c1a1bd..d45dd5ce0 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -16,99 +16,182 @@ #pragma once -#include -#include #include +#include +#include namespace gtsam { - /** - * Binary factor for a range measurement - * @addtogroup SLAM - */ - template - class RangeFactor: public NoiseModelFactor2 { - private: +/** + * Binary factor for a range measurement + * @addtogroup SLAM + */ +template +class RangeFactor: public NoiseModelFactor2 { +private: - double measured_; /** measurement */ - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + double measured_; /** measurement */ - typedef RangeFactor This; - typedef NoiseModelFactor2 Base; + typedef RangeFactor This; + typedef NoiseModelFactor2 Base; - typedef POSE Pose; - typedef POINT Point; + // Concept requirements for this factor + GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(T1, T2) - // Concept requirements for this factor - GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(POSE, POINT) +public: - public: + RangeFactor() { + } /* Default constructor */ - RangeFactor() {} /* Default constructor */ + RangeFactor(Key key1, Key key2, double measured, + const SharedNoiseModel& model) : + Base(model, key1, key2), measured_(measured) { + } - RangeFactor(Key poseKey, Key pointKey, double measured, - const SharedNoiseModel& model, boost::optional body_P_sensor = boost::none) : - Base(model, poseKey, pointKey), measured_(measured), body_P_sensor_(body_P_sensor) { + virtual ~RangeFactor() { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /** h(x)-z */ + Vector evaluateError(const T1& t1, const T2& t2, boost::optional H1 = + boost::none, boost::optional H2 = boost::none) const { + double hx; + hx = t1.range(t2, H1, H2); + return (Vector(1) << hx - measured_).finished(); + } + + /** return the measured */ + double measured() const { + return measured_; + } + + /** equals specialized to this factor */ + virtual bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const { + const This *e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) + && fabs(this->measured_ - e->measured_) < tol; + } + + /** print contents */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "RangeFactor, range = " << measured_ << std::endl; + Base::print("", keyFormatter); + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar + & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + } +}; // \ RangeFactor + +/// traits +template +struct traits > : public Testable > {}; + +/** + * Binary factor for a range measurement, with a transform applied + * @addtogroup SLAM + */ +template +class RangeFactorWithTransform: public NoiseModelFactor2 { +private: + + double measured_; /** measurement */ + POSE body_P_sensor_; ///< The pose of the sensor in the body frame + + typedef RangeFactorWithTransform This; + typedef NoiseModelFactor2 Base; + + // Concept requirements for this factor + GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(POSE, T2) + +public: + + RangeFactorWithTransform() { + } /* Default constructor */ + + RangeFactorWithTransform(Key key1, Key key2, double measured, + const SharedNoiseModel& model, const POSE& body_P_sensor) : + Base(model, key1, key2), measured_(measured), body_P_sensor_( + body_P_sensor) { + } + + virtual ~RangeFactorWithTransform() { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /** h(x)-z */ + Vector evaluateError(const POSE& t1, const T2& t2, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + double hx; + if (H1) { + Matrix H0; + hx = t1.compose(body_P_sensor_, H0).range(t2, H1, H2); + *H1 = *H1 * H0; + } else { + hx = t1.compose(body_P_sensor_).range(t2, H1, H2); } + return (Vector(1) << hx - measured_).finished(); + } - virtual ~RangeFactor() {} + /** return the measured */ + double measured() const { + return measured_; + } - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + /** equals specialized to this factor */ + virtual bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const { + const This *e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) + && fabs(this->measured_ - e->measured_) < tol + && body_P_sensor_.equals(e->body_P_sensor_); + } - /** h(x)-z */ - Vector evaluateError(const POSE& pose, const POINT& point, - boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - double hx; - if(body_P_sensor_) { - if(H1) { - Matrix H0; - hx = pose.compose(*body_P_sensor_, H0).range(point, H1, H2); - *H1 = *H1 * H0; - } else { - hx = pose.compose(*body_P_sensor_).range(point, H1, H2); - } - } else { - hx = pose.range(point, H1, H2); - } - return (Vector(1) << hx - measured_).finished(); - } + /** print contents */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "RangeFactor, range = " << measured_ << std::endl; + this->body_P_sensor_.print(" sensor pose in body frame: "); + Base::print("", keyFormatter); + } - /** return the measured */ - double measured() const { - return measured_; - } +private: - /** equals specialized to this factor */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL - && Base::equals(*e, tol) - && fabs(this->measured_ - e->measured_) < tol - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); - } + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar + & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + } +}; // \ RangeFactorWithTransform - /** print contents */ - void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "RangeFactor, range = " << measured_ << std::endl; - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - Base::print("", keyFormatter); - } +/// traits +template +struct traits > : public Testable > {}; - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor2", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); - } - }; // RangeFactor - -} // namespace gtsam +} // \ namespace gtsam diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 13dec7de3..90ceeafc8 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -84,7 +84,7 @@ public: * each degree of freedom. */ ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma = 1e-2) - : Base(noiseModel::Isotropic::Sigma(POINT().dim(), sigma), + : Base(noiseModel::Isotropic::Sigma(POINT::dimension, sigma), globalKey, transKey, localKey) {} virtual ~ReferenceFrameFactor(){} @@ -99,11 +99,9 @@ public: boost::optional Dtrans = boost::none, boost::optional Dlocal = boost::none) const { Point newlocal = transform_point(trans, global, Dtrans, Dforeign); - if (Dlocal) { - Point dummy; - *Dlocal = -1* gtsam::eye(dummy.dim()); - } - return local.localCoordinates(newlocal); + if (Dlocal) + *Dlocal = -1* gtsam::eye(Point::dimension); + return traits::Local(local,newlocal); } virtual void print(const std::string& s="", @@ -124,10 +122,14 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor3", boost::serialization::base_object(*this)); } }; +/// traits +template +struct traits > : public Testable > {}; + } // \namespace gtsam diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h index 0e20bb709..5765f67fd 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/slam/RegularHessianFactor.h @@ -10,10 +10,10 @@ * -------------------------------------------------------------------------- */ /** - * @file RegularHessianFactor.h - * @brief HessianFactor class with constant sized blcoks - * @author Richard Roberts - * @date Dec 8, 2010 + * @file RegularHessianFactor.h + * @brief HessianFactor class with constant sized blocks + * @author Sungtae An + * @date March 2014 */ #pragma once @@ -29,8 +29,10 @@ class RegularHessianFactor: public HessianFactor { private: - typedef Eigen::Matrix MatrixDD; // camera hessian block - typedef Eigen::Matrix VectorD; + // Use Eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; public: @@ -43,6 +45,15 @@ public: HessianFactor(js, Gs, gs, f) { } + /** Construct a binary factor. Gxx are the upper-triangle blocks of the + * quadratic term (the Hessian matrix), gx the pieces of the linear vector + * term, and f the constant term. + */ + RegularHessianFactor(Key j1, Key j2, const Matrix& G11, const Matrix& G12, + const Vector& g1, const Matrix& G22, const Vector& g2, double f) : + HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) { + } + /** Constructor with an arbitrary number of keys and with the augmented information matrix * specified as a block matrix. */ template @@ -51,16 +62,16 @@ public: HessianFactor(keys, augmentedInformation) { } + // Scratch space for multiplyHessianAdd + mutable std::vector y; + /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const VectorValues& x, + virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const { HessianFactor::multiplyHessianAdd(alpha, x, y); } - // Scratch space for multiplyHessianAdd - typedef Eigen::Matrix DVector; - mutable std::vector y; - + /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const { // Create a vector of temporary y values, corresponding to rows i @@ -68,9 +79,6 @@ public: BOOST_FOREACH(DVector & yi, y) yi.setZero(); - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - // Accessing the VectorValues one by one is expensive // So we will loop over columns to access x only once per column // And fill the above temporary y values, to be added into yvalues after @@ -95,6 +103,74 @@ public: } } + /// Raw memory version, with offsets TODO document reasoning + void multiplyHessianAdd(double alpha, const double* x, double* yvalues, + std::vector offsets) const { + + // Create a vector of temporary y values, corresponding to rows i + std::vector y; + y.reserve(size()); + for (const_iterator it = begin(); it != end(); it++) + y.push_back(zero(getDim(it))); + + // Accessing the VectorValues one by one is expensive + // So we will loop over columns to access x only once per column + // And fill the above temporary y values, to be added into yvalues after + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { + DenseIndex i = 0; + for (; i < j; ++i) + y[i] += info_(i, j).knownOffDiagonal() + * ConstDMap(x + offsets[keys_[j]], + offsets[keys_[j] + 1] - offsets[keys_[j]]); + // blocks on the diagonal are only half + y[i] += info_(j, j).selfadjointView() + * ConstDMap(x + offsets[keys_[j]], + offsets[keys_[j] + 1] - offsets[keys_[j]]); + // for below diagonal, we take transpose block from upper triangular part + for (i = j + 1; i < (DenseIndex) size(); ++i) + y[i] += info_(i, j).knownOffDiagonal() + * ConstDMap(x + offsets[keys_[j]], + offsets[keys_[j] + 1] - offsets[keys_[j]]); + } + + // copy to yvalues + for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) + DMap(yvalues + offsets[keys_[i]], + offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y[i]; + } + + /** Return the diagonal of the Hessian for this factor (raw memory version) */ + virtual void hessianDiagonal(double* d) const { + + // Loop over all variables in the factor + for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { + Key j = keys_[pos]; + // Get the diagonal block, and insert its diagonal + const Matrix& B = info_(pos, pos).selfadjointView(); + DMap(d + D * j) += B.diagonal(); + } + } + + /// Add gradient at zero to d TODO: is it really the goal to add ?? + virtual void gradientAtZero(double* d) const { + + // Loop over all variables in the factor + for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { + Key j = keys_[pos]; + // Get the diagonal block, and insert its diagonal + DVector dj = -info_(pos, size()).knownOffDiagonal(); + DMap(d + D * j) += dj; + } + } + + /* ************************************************************************* */ + +}; +// end class RegularHessianFactor + +// traits +template struct traits > : public Testable< + RegularHessianFactor > { }; } diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index ecf8adfec..b1490d465 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -7,12 +7,10 @@ #pragma once -#include #include #include #include -#include -#include +#include namespace gtsam { @@ -89,18 +87,27 @@ public: const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << " RegularImplicitSchurFactor " << std::endl; Factor::print(s); - std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl; - std::cout << " E_ \n" << E_ << std::endl; - std::cout << " b_ \n" << b_.transpose() << std::endl; + for (size_t pos = 0; pos < size(); ++pos) { + std::cout << "Fblock:\n" << Fblocks_[pos].second << std::endl; + } + std::cout << "PointCovariance:\n" << PointCovariance_ << std::endl; + std::cout << "E:\n" << E_ << std::endl; + std::cout << "b:\n" << b_.transpose() << std::endl; } /// equals bool equals(const GaussianFactor& lf, double tol) const { - if (!dynamic_cast(&lf)) - return false; - else { + const This* f = dynamic_cast(&lf); + if (!f) return false; + for (size_t pos = 0; pos < size(); ++pos) { + if (keys_[pos] != f->keys_[pos]) return false; + if (Fblocks_[pos].first != f->Fblocks_[pos].first) return false; + if (!equal_with_abs_tol(Fblocks_[pos].second,f->Fblocks_[pos].second,tol)) return false; } + return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol) + && equal_with_abs_tol(E_, f->E_, tol) + && equal_with_abs_tol(b_, f->b_, tol); } /// Degrees of freedom of camera @@ -159,7 +166,7 @@ public: * @brief add the contribution of this factor to the diagonal of the hessian * d(output) = d(input) + deltaHessianFactor */ - void hessianDiagonal(double* d) const { + virtual void hessianDiagonal(double* d) const { // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); // Use eigen magic to access raw memory typedef Eigen::Matrix DVector; @@ -434,7 +441,7 @@ public: /** * Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS */ - void gradientAtZero(double* d) const { + virtual void gradientAtZero(double* d) const { // Use eigen magic to access raw memory typedef Eigen::Matrix DVector; @@ -453,8 +460,19 @@ public: } } + /// Gradient wrt a key at any values + Vector gradient(Key key, const VectorValues& x) const { + throw std::runtime_error("gradient for RegularImplicitSchurFactor is not implemented yet"); + } + + +}; +// end class RegularImplicitSchurFactor + +// traits +template struct traits > : public Testable< + RegularImplicitSchurFactor > { }; -// RegularImplicitSchurFactor } diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h new file mode 100644 index 000000000..38e1407f0 --- /dev/null +++ b/gtsam/slam/RegularJacobianFactor.h @@ -0,0 +1,232 @@ +/* ---------------------------------------------------------------------------- + + * 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 RegularJacobianFactor.h + * @brief JacobianFactor class with fixed sized blcoks + * @author Sungtae An + * @date Nov 11, 2014 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * JacobianFactor with constant sized blocks + * Provides raw memory access versions of linear operator. + * Is base class for JacobianQFactor, JacobianFactorQR, and JacobianFactorSVD + */ +template +class RegularJacobianFactor: public JacobianFactor { + +private: + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + +public: + + /// Default constructor + RegularJacobianFactor() {} + + /** Construct an n-ary factor + * @tparam TERMS A container whose value type is std::pair, specifying the + * collection of keys and matrices making up the factor. + * TODO Verify terms are regular + */ + template + RegularJacobianFactor(const TERMS& terms, const Vector& b, + const SharedDiagonal& model = SharedDiagonal()) : + JacobianFactor(terms, b, model) { + } + + /** Constructor with arbitrary number keys, and where the augmented matrix is given all together + * instead of in block terms. Note that only the active view of the provided augmented matrix + * is used, and that the matrix data is copied into a newly-allocated matrix in the constructed + * factor. + * TODO Verify complies to regular + */ + template + RegularJacobianFactor(const KEYS& keys, + const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = + SharedDiagonal()) : + JacobianFactor(keys, augmentedMatrix, sigmas) { + } + + /** y += alpha * A'*A*x */ + virtual void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const { + JacobianFactor::multiplyHessianAdd(alpha, x, y); + } + + /** + * @brief double* Hessian-vector multiply, i.e. y += A'*(A*x) + * RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way + */ + void multiplyHessianAdd(double alpha, const double* x, double* y) const { + if (empty()) + return; + Vector Ax = zero(Ab_.rows()); + + // Just iterate over all A matrices and multiply in correct config part + for (size_t pos = 0; pos < size(); ++pos) + Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]); + + // Deal with noise properly, need to Double* whiten as we are dividing by variance + if (model_) { + model_->whitenInPlace(Ax); + model_->whitenInPlace(Ax); + } + + // multiply with alpha + Ax *= alpha; + + // Again iterate over all A matrices and insert Ai^e into y + for (size_t pos = 0; pos < size(); ++pos) + DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax; + } + + /// Expose base class hessianDiagonal + virtual VectorValues hessianDiagonal() const { + return JacobianFactor::hessianDiagonal(); + } + + /// Raw memory access version of hessianDiagonal + void hessianDiagonal(double* d) const { + // Loop over all variables in the factor + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { + // Get the diagonal block, and insert its diagonal + DVector dj; + for (size_t k = 0; k < D; ++k) { + if (model_) { + Vector column_k = Ab_(j).col(k); + column_k = model_->whiten(column_k); + dj(k) = dot(column_k, column_k); + } else { + dj(k) = Ab_(j).col(k).squaredNorm(); + } + } + DMap(d + D * j) += dj; + } + } + + /// Expose base class gradientAtZero + virtual VectorValues gradientAtZero() const { + return JacobianFactor::gradientAtZero(); + } + + /// Raw memory access version of gradientAtZero + void gradientAtZero(double* d) const { + + // Get vector b not weighted + Vector b = getb(); + + // Whitening b + if (model_) { + b = model_->whiten(b); + b = model_->whiten(b); + } + + // Just iterate over all A matrices + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { + DVector dj; + // gradient -= A'*b/sigma^2 + // Computing with each column + for (size_t k = 0; k < D; ++k) { + Vector column_k = Ab_(j).col(k); + dj(k) = -1.0 * dot(b, column_k); + } + DMap(d + D * j) += dj; + } + } + + /** + * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e + * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way + */ + void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const { + Vector E = alpha * (model_ ? model_->whiten(e) : e); + // Just iterate over all A matrices and insert Ai^e into y + for (size_t pos = 0; pos < size(); ++pos) + DMap(x + D * keys_[pos]) += Ab_(pos).transpose() * E; + } + + /** + * @brief double* Matrix-vector multiply, i.e. y = A*x + * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way + */ + Vector operator*(const double* x) const { + Vector Ax = zero(Ab_.rows()); + if (empty()) + return Ax; + + // Just iterate over all A matrices and multiply in correct config part + for (size_t pos = 0; pos < size(); ++pos) + Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]); + + return model_ ? model_->whiten(Ax) : Ax; + } + + /** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x + * Note: this is not assuming a fixed dimension for the variables, + * but requires the vector accumulatedDims to tell the dimension of + * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, + * then accumulatedDims is [0 3 9 11 13] + * NOTE: size of accumulatedDims is size of keys + 1!! + * TODO Frank asks: why is this here if not regular ???? + */ + void multiplyHessianAdd(double alpha, const double* x, double* y, + const std::vector& accumulatedDims) const { + + /// Use Eigen magic to access raw memory + typedef Eigen::Map VectorMap; + typedef Eigen::Map ConstVectorMap; + + if (empty()) + return; + Vector Ax = zero(Ab_.rows()); + + /// Just iterate over all A matrices and multiply in correct config part (looping over keys) + /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' + /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate) + for (size_t pos = 0; pos < size(); ++pos) { + size_t offset = accumulatedDims[keys_[pos]]; + size_t dim = accumulatedDims[keys_[pos] + 1] - offset; + Ax += Ab_(pos) * ConstVectorMap(x + offset, dim); + } + /// Deal with noise properly, need to Double* whiten as we are dividing by variance + if (model_) { + model_->whitenInPlace(Ax); + model_->whitenInPlace(Ax); + } + + /// multiply with alpha + Ax *= alpha; + + /// Again iterate over all A matrices and insert Ai^T into y + for (size_t pos = 0; pos < size(); ++pos) { + size_t offset = accumulatedDims[keys_[pos]]; + size_t dim = accumulatedDims[keys_[pos] + 1] - offset; + VectorMap(y + offset, dim) += Ab_(pos).transpose() * Ax; + } + } + +}; +// end class RegularJacobianFactor + +}// end namespace gtsam diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 8ae26e7cd..cdbe71718 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -19,63 +19,77 @@ #pragma once -#include "JacobianFactorQ.h" -#include "JacobianFactorSVD.h" -#include "RegularImplicitSchurFactor.h" -#include "RegularHessianFactor.h" +#include +#include +#include +#include #include -#include -#include -#include -#include +#include #include #include #include namespace gtsam { -/// Base class with no internal point, completely functional -template + +/** + * @brief Base class for smart factors + * This base class has no internal point, but it has a measurement, noise model + * and an optional sensor pose. + * This class mainly computes the derivatives and returns them as a variety of factors. + * The methods take a Cameras argument, which should behave like PinholeCamera, and + * the value of a point, which is kept in the base class. + */ +template class SmartFactorBase: public NonlinearFactor { + protected: - // Keep a copy of measurement and calibration for I/O - std::vector measured_; ///< 2D measurement for each of the m views - std::vector noise_; ///< noise model used - ///< (important that the order is the same as the keys that we use to create the factor) + typedef typename CAMERA::Measurement Z; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) + /** + * 2D measurement and noise model for each of the m views + * We keep a copy of measurements for I/O and computing the error. + * The order is kept the same as the keys that we use to create the factor. + */ + std::vector measured_; + + std::vector noise_; ///< noise model used + + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) + + static const int ZDim = traits::dimension; ///< Measurement dimension /// Definitions for blocks of F - typedef Eigen::Matrix Matrix2D; // F - typedef Eigen::Matrix MatrixD2; // F' + typedef Eigen::Matrix Matrix2D; // F + typedef Eigen::Matrix MatrixD2; // F' typedef std::pair KeyMatrix2D; // Fblocks typedef Eigen::Matrix MatrixDD; // camera hessian block - typedef Eigen::Matrix Matrix23; + typedef Eigen::Matrix Matrix23; typedef Eigen::Matrix VectorD; - typedef Eigen::Matrix Matrix2; + typedef Eigen::Matrix Matrix2; /// shorthand for base class type typedef NonlinearFactor Base; /// shorthand for this class - typedef SmartFactorBase This; + typedef SmartFactorBase This; public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - /// shorthand for a pinhole camera - typedef PinholeCamera Camera; - typedef std::vector Cameras; + typedef CameraSet Cameras; /** * Constructor * @param body_P_sensor is the transform from sensor to body frame (default identity) */ - SmartFactorBase(boost::optional body_P_sensor = boost::none) : + SmartFactorBase(boost::optional body_P_sensor = boost::none) : body_P_sensor_(body_P_sensor) { } @@ -84,12 +98,12 @@ public: } /** - * add a new measurement and pose key + * Add a new measurement and pose key * @param measured_i is the 2m dimensional projection of a single landmark * @param poseKey is the index corresponding to the camera observing the landmark * @param noise_i is the measurement noise */ - void add(const Point2& measured_i, const Key& poseKey_i, + void add(const Z& measured_i, const Key& poseKey_i, const SharedNoiseModel& noise_i) { this->measured_.push_back(measured_i); this->keys_.push_back(poseKey_i); @@ -97,10 +111,9 @@ public: } /** - * variant of the previous add: adds a bunch of measurements, together with the camera keys and noises + * Add a bunch of measurements, together with the camera keys and noises */ - // **************************************************************************************************** - void add(std::vector& measurements, std::vector& poseKeys, + void add(std::vector& measurements, std::vector& poseKeys, std::vector& noises) { for (size_t i = 0; i < measurements.size(); i++) { this->measured_.push_back(measurements.at(i)); @@ -110,10 +123,9 @@ public: } /** - * variant of the previous add: adds a bunch of measurements and uses the same noise model for all of them + * Add a bunch of measurements and uses the same noise model for all of them */ - // **************************************************************************************************** - void add(std::vector& measurements, std::vector& poseKeys, + void add(std::vector& measurements, std::vector& poseKeys, const SharedNoiseModel& noise) { for (size_t i = 0; i < measurements.size(); i++) { this->measured_.push_back(measurements.at(i)); @@ -126,8 +138,8 @@ public: * Adds an entire SfM_track (collection of cameras observing a single point). * The noise is assumed to be the same for all measurements */ - // **************************************************************************************************** - void add(const SfM_Track& trackToAdd, const SharedNoiseModel& noise) { + template + void add(const SFM_TRACK& trackToAdd, const SharedNoiseModel& noise) { for (size_t k = 0; k < trackToAdd.number_measurements(); k++) { this->measured_.push_back(trackToAdd.measurements[k].second); this->keys_.push_back(trackToAdd.measurements[k].first); @@ -135,12 +147,17 @@ public: } } + /// get the dimension (number of rows!) of the factor + virtual size_t dim() const { + return ZDim * this->measured_.size(); + } + /** return the measurements */ - const std::vector& measured() const { + const std::vector& measured() const { return measured_; } - /** return the noise model */ + /** return the noise models */ const std::vector& noise() const { return noise_; } @@ -178,30 +195,38 @@ public: && body_P_sensor_->equals(*e->body_P_sensor_))); } - // **************************************************************************************************** /// Calculate vector of re-projection errors, before applying noise model Vector reprojectionError(const Cameras& cameras, const Point3& point) const { - Vector b = zero(2 * cameras.size()); + // Project into CameraSet + std::vector predicted; + try { + predicted = cameras.project(point); + } catch (CheiralityException&) { + std::cout << "reprojectionError: Cheirality exception " << std::endl; + exit(EXIT_FAILURE); // TODO: throw exception + } - size_t i = 0; - BOOST_FOREACH(const Camera& camera, cameras) { - const Point2& zi = this->measured_.at(i); - try { - Point2 e(camera.project(point) - zi); - b[2 * i] = e.x(); - b[2 * i + 1] = e.y(); - } catch (CheiralityException& e) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); - } - i += 1; + // Calculate vector of errors + size_t nrCameras = cameras.size(); + Vector b(ZDim * nrCameras); + for (size_t i = 0, row = 0; i < nrCameras; i++, row += ZDim) { + Z e = predicted[i] - measured_.at(i); + b.segment(row) = e.vector(); } return b; } - // **************************************************************************************************** + /// Calculate vector of re-projection errors, noise model applied + Vector whitenedError(const Cameras& cameras, const Point3& point) const { + Vector b = reprojectionError(cameras, point); + size_t nrCameras = cameras.size(); + for (size_t i = 0, row = 0; i < nrCameras; i++, row += ZDim) + b.segment(row) = noise_.at(i)->whiten(b.segment(row)); + return b; + } + /** * Calculate the error of the factor. * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. @@ -211,178 +236,233 @@ public: */ double totalReprojectionError(const Cameras& cameras, const Point3& point) const { - + Vector b = reprojectionError(cameras, point); double overallError = 0; - - size_t i = 0; - BOOST_FOREACH(const Camera& camera, cameras) { - const Point2& zi = this->measured_.at(i); - try { - Point2 reprojectionError(camera.project(point) - zi); - overallError += 0.5 - * this->noise_.at(i)->distance(reprojectionError.vector()); - } catch (CheiralityException&) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); - } - i += 1; - } - return overallError; + size_t nrCameras = cameras.size(); + for (size_t i = 0; i < nrCameras; i++) + overallError += noise_.at(i)->distance(b.segment(i * ZDim)); + return 0.5 * overallError; } - // **************************************************************************************************** - /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras, - const Point3& point) const { + /** + * Compute whitenedError, returning only the derivative E, i.e., + * the stacked ZDim*3 derivatives of project with respect to the point. + * Assumes non-degenerate ! TODO explain this + */ + Vector whitenedError(const Cameras& cameras, const Point3& point, + Matrix& E) const { - int numKeys = this->keys_.size(); - E = zeros(2 * numKeys, 3); - Vector b = zero(2 * numKeys); - - Matrix Ei(2, 3); - for (size_t i = 0; i < this->measured_.size(); i++) { - try { - cameras[i].project(point, boost::none, Ei); - } catch (CheiralityException& e) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); - } - this->noise_.at(i)->WhitenSystem(Ei, b); - E.block<2, 3>(2 * i, 0) = Ei; + // Project into CameraSet, calculating derivatives + std::vector predicted; + try { + using boost::none; + predicted = cameras.project(point, none, E, none); + } catch (CheiralityException&) { + std::cout << "whitenedError(E): Cheirality exception " << std::endl; + exit(EXIT_FAILURE); // TODO: throw exception } - // Matrix PointCov; - PointCov.noalias() = (E.transpose() * E).inverse(); + // if needed, whiten + size_t m = keys_.size(); + Vector b(ZDim * m); + for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { + + // Calculate error + const Z& zi = measured_.at(i); + Vector bi = (zi - predicted[i]).vector(); + + // if needed, whiten + SharedNoiseModel model = noise_.at(i); + if (model) { + // TODO: re-factor noiseModel to take any block/fixed vector/matrix + Vector dummy; + Matrix Ei = E.block(row, 0); + model->WhitenSystem(Ei, dummy); + E.block(row, 0) = Ei; + } + b.segment(row) = bi; + } + return b; } - // **************************************************************************************************** - /// Compute F, E only (called below in both vanilla and SVD versions) - /// Given a Point3, assumes dimensionality is 3 - double computeJacobians(std::vector& Fblocks, Matrix& E, - Vector& b, const Cameras& cameras, const Point3& point) const { + /** + * Compute F, E, and optionally H, where F and E are the stacked derivatives + * with respect to the cameras, point, and calibration, respectively. + * The value of cameras/point are passed as parameters. + * Returns error vector b + * TODO: the treatment of body_P_sensor_ is weird: the transformation + * is applied in the caller, but the derivatives are computed here. + */ + Vector whitenedError(const Cameras& cameras, const Point3& point, Matrix& F, + Matrix& E, boost::optional G = boost::none) const { - size_t numKeys = this->keys_.size(); - E = zeros(2 * numKeys, 3); - b = zero(2 * numKeys); - double f = 0; + // Project into CameraSet, calculating derivatives + std::vector predicted; + try { + predicted = cameras.project(point, F, E, G); + } catch (CheiralityException&) { + std::cout << "whitenedError(E,F): Cheirality exception " << std::endl; + exit(EXIT_FAILURE); // TODO: throw exception + } - Matrix Fi(2, 6), Ei(2, 3), Hcali(2, D - 6), Hcam(2, D); - for (size_t i = 0; i < this->measured_.size(); i++) { + // Calculate error and whiten derivatives if needed + size_t m = keys_.size(); + Vector b(ZDim * m); + for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { - Vector bi; - try { - bi = -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector(); - if(body_P_sensor_){ - Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse()); - Matrix J(6, 6); - Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); - Fi = Fi * J; + // Calculate error + const Z& zi = measured_.at(i); + Vector bi = (zi - predicted[i]).vector(); + + // if we have a sensor offset, correct camera derivatives + if (body_P_sensor_) { + // TODO: no simpler way ?? + const Pose3& pose_i = cameras[i].pose(); + Pose3 w_Pose_body = pose_i.compose(body_P_sensor_->inverse()); + Matrix66 J; + Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); + F.block(row, 0) *= J; + } + + // if needed, whiten + SharedNoiseModel model = noise_.at(i); + if (model) { + // TODO, refactor noiseModel so we can take blocks + Matrix Fi = F.block(row, 0); + Matrix Ei = E.block(row, 0); + if (!G) + model->WhitenSystem(Fi, Ei, bi); + else { + Matrix Gi = G->block(row, 0); + model->WhitenSystem(Fi, Ei, Gi, bi); + G->block(row, 0) = Gi; } - } catch (CheiralityException&) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); + F.block(row, 0) = Fi; + E.block(row, 0) = Ei; } - this->noise_.at(i)->WhitenSystem(Fi, Ei, Hcali, bi); - - f += bi.squaredNorm(); - if (D == 6) { // optimize only camera pose - Fblocks.push_back(KeyMatrix2D(this->keys_[i], Fi)); - } else { - Hcam.block<2, 6>(0, 0) = Fi; // 2 x 6 block for the cameras - Hcam.block<2, D - 6>(0, 6) = Hcali; // 2 x nrCal block for the cameras - Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam)); - } - E.block<2, 3>(2 * i, 0) = Ei; - subInsert(b, bi, 2 * i); + b.segment(row) = bi; } - return f; + return b; } - // **************************************************************************************************** - /// Version that computes PointCov, with optional lambda parameter - double computeJacobians(std::vector& Fblocks, Matrix& E, - Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point, - double lambda = 0.0, bool diagonalDamping = false) const { + /// Computes Point Covariance P from E + static Matrix3 PointCov(Matrix& E) { + return (E.transpose() * E).inverse(); + } - double f = computeJacobians(Fblocks, E, b, cameras, point); + /// Computes Point Covariance P, with lambda parameter + static Matrix3 PointCov(Matrix& E, double lambda, + bool diagonalDamping = false) { - // Point covariance inv(E'*E) Matrix3 EtE = E.transpose() * E; if (diagonalDamping) { // diagonal of the hessian EtE(0, 0) += lambda * EtE(0, 0); EtE(1, 1) += lambda * EtE(1, 1); EtE(2, 2) += lambda * EtE(2, 2); - }else{ + } else { EtE(0, 0) += lambda; EtE(1, 1) += lambda; EtE(2, 2) += lambda; } - PointCov.noalias() = (EtE).inverse(); - - return f; + return (EtE).inverse(); } - // **************************************************************************************************** - // TODO, there should not be a Matrix version, really - double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, - const Cameras& cameras, const Point3& point, - const double lambda = 0.0) const { + /// Assumes non-degenerate ! + void computeEP(Matrix& E, Matrix& P, const Cameras& cameras, + const Point3& point) const { + whitenedError(cameras, point, E); + P = PointCov(E); + } - size_t numKeys = this->keys_.size(); - std::vector Fblocks; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, - lambda); - F = zeros(2 * numKeys, D * numKeys); + /** + * Compute F, E, and b (called below in both vanilla and SVD versions), where + * F is a vector of derivatives wrpt the cameras, and E the stacked derivatives + * with respect to the point. The value of cameras/point are passed as parameters. + */ + double computeJacobians(std::vector& Fblocks, Matrix& E, + Vector& b, const Cameras& cameras, const Point3& point) const { - for (size_t i = 0; i < this->keys_.size(); ++i) { - F.block<2, D>(2 * i, D * i) = Fblocks.at(i).second; // 2 x 6 block for the cameras + // Project into Camera set and calculate derivatives + // TODO: if D==6 we optimize only camera pose. That is fairly hacky! + Matrix F, G; + using boost::none; + boost::optional optionalG(G); + b = whitenedError(cameras, point, F, E, D == 6 ? none : optionalG); + + // Now calculate f and divide up the F derivatives into Fblocks + double f = 0.0; + size_t m = keys_.size(); + for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { + + // Accumulate normalized error + f += b.segment(row).squaredNorm(); + + // Get piece of F and possibly G + Matrix2D Fi; + if (D == 6) + Fi << F.block(row, 0); + else + Fi << F.block(row, 0), G.block(row, 0); + + // Push it onto Fblocks + Fblocks.push_back(KeyMatrix2D(keys_[i], Fi)); } return f; } - // **************************************************************************************************** + /// Create BIG block-diagonal matrix F from Fblocks + static void FillDiagonalF(const std::vector& Fblocks, Matrix& F) { + size_t m = Fblocks.size(); + F.resize(ZDim * m, D * m); + F.setZero(); + for (size_t i = 0; i < m; ++i) + F.block(This::ZDim * i, D * i) = Fblocks.at(i).second; + } + + /** + * Compute F, E, and b, where F and E are the stacked derivatives + * with respect to the point. The value of cameras/point are passed as parameters. + */ + double computeJacobians(Matrix& F, Matrix& E, Vector& b, + const Cameras& cameras, const Point3& point) const { + std::vector Fblocks; + double f = computeJacobians(Fblocks, E, b, cameras, point); + FillDiagonalF(Fblocks, F); + return f; + } + /// SVD version double computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, - Vector& b, const Cameras& cameras, const Point3& point, double lambda = - 0.0, bool diagonalDamping = false) const { + Vector& b, const Cameras& cameras, const Point3& point) const { Matrix E; - Matrix3 PointCov; // useless - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); // diagonalDamping should have no effect (only on PointCov) + double f = computeJacobians(Fblocks, E, b, cameras, point); // Do SVD on A Eigen::JacobiSVD svd(E, Eigen::ComputeFullU); Vector s = svd.singularValues(); - // Enull = zeros(2 * numKeys, 2 * numKeys - 3); - size_t numKeys = this->keys_.size(); - Enull = svd.matrixU().block(0, 3, 2 * numKeys, 2 * numKeys - 3); // last 2m-3 columns + size_t m = this->keys_.size(); + // Enull = zeros(ZDim * m, ZDim * m - 3); + Enull = svd.matrixU().block(0, 3, ZDim * m, ZDim * m - 3); // last ZDim*m-3 columns return f; } - // **************************************************************************************************** /// Matrix version of SVD // TODO, there should not be a Matrix version, really double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, const Cameras& cameras, const Point3& point) const { - - int numKeys = this->keys_.size(); std::vector Fblocks; double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point); - F.resize(2 * numKeys, D * numKeys); - F.setZero(); - - for (size_t i = 0; i < this->keys_.size(); ++i) - F.block<2, D>(2 * i, D * i) = Fblocks.at(i).second; // 2 x 6 block for the cameras - + FillDiagonalF(Fblocks, F); return f; } - // **************************************************************************************************** - /// linearize returns a Hessianfactor that is an approximation of error(p) + /** + * Linearize returns a Hessianfactor that is an approximation of error(p) + */ boost::shared_ptr > createHessianFactor( const Cameras& cameras, const Point3& point, const double lambda = 0.0, bool diagonalDamping = false) const { @@ -391,10 +471,9 @@ public: std::vector Fblocks; Matrix E; - Matrix3 PointCov; Vector b; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); + double f = computeJacobians(Fblocks, E, b, cameras, point); + Matrix3 P = PointCov(E, lambda, diagonalDamping); //#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix #ifdef HESSIAN_BLOCKS @@ -402,14 +481,13 @@ public: std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); std::vector < Vector > gs(numKeys); - sparseSchurComplement(Fblocks, E, PointCov, b, Gs, gs); - // schurComplement(Fblocks, E, PointCov, b, Gs, gs); + sparseSchurComplement(Fblocks, E, P, b, Gs, gs); + // schurComplement(Fblocks, E, P, b, Gs, gs); //std::vector < Matrix > Gs2(Gs.begin(), Gs.end()); //std::vector < Vector > gs2(gs.begin(), gs.end()); - return boost::make_shared < RegularHessianFactor - > (this->keys_, Gs, gs, f); + return boost::make_shared < RegularHessianFactor > (this->keys_, Gs, gs, f); #else // we create directly a SymmetricBlockMatrix size_t n1 = D * numKeys + 1; std::vector dims(numKeys + 1); // this also includes the b term @@ -417,17 +495,19 @@ public: dims.back() = 1; SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1) - sparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... + sparseSchurComplement(Fblocks, E, P, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... augmentedHessian(numKeys, numKeys)(0, 0) = f; return boost::make_shared >(this->keys_, augmentedHessian); #endif } - // **************************************************************************************************** - // slow version - works on full (sparse) matrices + /** + * Do Schur complement, given Jacobian as F,E,P. + * Slow version - works on full matrices + */ void schurComplement(const std::vector& Fblocks, const Matrix& E, - const Matrix& PointCov, const Vector& b, + const Matrix3& P, const Vector& b, /*output ->*/std::vector& Gs, std::vector& gs) const { // Schur complement trick // Gs = F' * F - F' * E * inv(E'*E) * E' * F @@ -437,16 +517,14 @@ public: int numKeys = this->keys_.size(); /// Compute Full F ???? - Matrix F = zeros(2 * numKeys, D * numKeys); - for (size_t i = 0; i < this->keys_.size(); ++i) - F.block<2, D>(2 * i, D * i) = Fblocks.at(i).second; // 2 x 6 block for the cameras + Matrix F; + FillDiagonalF(Fblocks, F); Matrix H(D * numKeys, D * numKeys); Vector gs_vector; - H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); - gs_vector.noalias() = F.transpose() - * (b - (E * (PointCov * (E.transpose() * b)))); + H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F)))); + gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b)))); // Populate Gs and gs int GsCount2 = 0; @@ -462,9 +540,12 @@ public: } } - // **************************************************************************************************** + /** + * Do Schur complement, given Jacobian as F,E,P, return SymmetricBlockMatrix + * Fast version - works on with sparsity + */ void sparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, + const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b, /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { // Schur complement trick // Gs = F' * F - F' * E * P * E' * F @@ -477,16 +558,17 @@ public: for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera const Matrix2D& Fi1 = Fblocks.at(i1).second; - const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P; + const Matrix23 Ei1_P = E.block(ZDim * i1, 0) * P; // D = (Dx2) * (2) // (augmentedHessian.matrix()).block (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b - augmentedHessian(i1, numKeys) = Fi1.transpose() * b.segment<2>(2 * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) + augmentedHessian(i1, numKeys) = Fi1.transpose() + * b.segment(ZDim * i1) // F' * b + - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) augmentedHessian(i1, i1) = Fi1.transpose() - * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); + * (Fi1 - Ei1_P * E.block(ZDim * i1, 0).transpose() * Fi1); // upper triangular part of the hessian for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera @@ -494,14 +576,17 @@ public: // (DxD) = (Dx2) * ( (2x2) * (2xD) ) augmentedHessian(i1, i2) = -Fi1.transpose() - * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); + * (Ei1_P * E.block(ZDim * i2, 0).transpose() * Fi2); } } // end of for over cameras } - // **************************************************************************************************** + /** + * Do Schur complement, given Jacobian as F,E,P, return Gs/gs + * Fast version - works on with sparsity + */ void sparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, + const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b, /*output ->*/std::vector& Gs, std::vector& gs) const { // Schur complement trick // Gs = F' * F - F' * E * P * E' * F @@ -521,16 +606,16 @@ public: // X X X X 14 const Matrix2D& Fi1 = Fblocks.at(i1).second; - const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P; + const Matrix23 Ei1_P = E.block(ZDim * i1, 0) * P; { // for i1 = i2 // D = (Dx2) * (2) - gs.at(i1) = Fi1.transpose() * b.segment<2>(2 * i1) // F' * b - -Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) + gs.at(i1) = Fi1.transpose() * b.segment(ZDim * i1) // F' * b + - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) Gs.at(GsIndex) = Fi1.transpose() - * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); + * (Fi1 - Ei1_P * E.block(ZDim * i1, 0).transpose() * Fi1); GsIndex++; } // upper triangular part of the hessian @@ -539,33 +624,18 @@ public: // (DxD) = (Dx2) * ( (2x2) * (2xD) ) Gs.at(GsIndex) = -Fi1.transpose() - * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); + * (Ei1_P * E.block(ZDim * i2, 0).transpose() * Fi2); GsIndex++; } } // end of for over cameras } - // **************************************************************************************************** - void updateAugmentedHessian(const Cameras& cameras, const Point3& point, - const double lambda, bool diagonalDamping, - SymmetricBlockMatrix& augmentedHessian, - const FastVector allKeys) const { - - // int numKeys = this->keys_.size(); - - std::vector Fblocks; - Matrix E; - Matrix3 PointCov; - Vector b; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); - - updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... - } - - // **************************************************************************************************** + /** + * Applies Schur complement (exploiting block structure) to get a smart factor on cameras, + * and adds the contribution of the smart factor to a pre-allocated augmented Hessian. + */ void updateSparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, + const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b, const double f, const FastVector allKeys, /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { // Schur complement trick @@ -575,9 +645,9 @@ public: MatrixDD matrixBlock; typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix - FastMap KeySlotMap; - for (size_t slot=0; slot < allKeys.size(); slot++) - KeySlotMap.insert(std::make_pair(allKeys[slot],slot)); + FastMap KeySlotMap; + for (size_t slot = 0; slot < allKeys.size(); slot++) + KeySlotMap.insert(std::make_pair(allKeys[slot], slot)); // a single point is observed in numKeys cameras size_t numKeys = this->keys_.size(); // cameras observing current point @@ -587,9 +657,9 @@ public: for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera in the current factor const Matrix2D& Fi1 = Fblocks.at(i1).second; - const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P; + const Matrix23 Ei1_P = E.block(ZDim * i1, 0) * P; - // D = (Dx2) * (2) + // D = (DxZDim) * (ZDim) // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7) // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4) // Key cameraKey_i1 = this->keys_[i1]; @@ -598,16 +668,20 @@ public: // information vector - store previous vector // vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal(); // add contribution of current factor - augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal() - + Fi1.transpose() * b.segment<2>(2 * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) + augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, + aug_numKeys).knownOffDiagonal() + + Fi1.transpose() * b.segment(ZDim * i1) // F' * b + - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) // main block diagonal - store previous block matrixBlock = augmentedHessian(aug_i1, aug_i1); // add contribution of current factor - augmentedHessian(aug_i1, aug_i1) = matrixBlock + - ( Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1) ); + augmentedHessian(aug_i1, aug_i1) = + matrixBlock + + (Fi1.transpose() + * (Fi1 + - Ei1_P * E.block(ZDim * i1, 0).transpose() * Fi1)); // upper triangular part of the hessian for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera @@ -616,52 +690,80 @@ public: //Key cameraKey_i2 = this->keys_[i2]; DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]]; - // (DxD) = (Dx2) * ( (2x2) * (2xD) ) + // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) ) // off diagonal block - store previous block // matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal(); // add contribution of current factor - augmentedHessian(aug_i1, aug_i2) = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() - - Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); + augmentedHessian(aug_i1, aug_i2) = + augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() + - Fi1.transpose() + * (Ei1_P * E.block(ZDim * i2, 0).transpose() * Fi2); } } // end of for over cameras augmentedHessian(aug_numKeys, aug_numKeys)(0, 0) += f; } - // **************************************************************************************************** + /** + * Add the contribution of the smart factor to a pre-allocated Hessian, + * using sparse linear algebra. More efficient than the creation of the + * Hessian without preallocation of the SymmetricBlockMatrix + */ + void updateAugmentedHessian(const Cameras& cameras, const Point3& point, + const double lambda, bool diagonalDamping, + SymmetricBlockMatrix& augmentedHessian, + const FastVector allKeys) const { + + // int numKeys = this->keys_.size(); + + std::vector Fblocks; + Matrix E; + Vector b; + double f = computeJacobians(Fblocks, E, b, cameras, point); + Matrix3 P = PointCov(E, lambda, diagonalDamping); + updateSparseSchurComplement(Fblocks, E, P, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... + } + + /** + * Return Jacobians as RegularImplicitSchurFactor with raw access + */ boost::shared_ptr > createRegularImplicitSchurFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { typename boost::shared_ptr > f( new RegularImplicitSchurFactor()); - computeJacobians(f->Fblocks(), f->E(), f->PointCovariance(), f->b(), - cameras, point, lambda, diagonalDamping); + computeJacobians(f->Fblocks(), f->E(), f->b(), cameras, point); + f->PointCovariance() = PointCov(f->E(), lambda, diagonalDamping); f->initKeys(); return f; } - // **************************************************************************************************** - boost::shared_ptr > createJacobianQFactor( + /** + * Return Jacobians as JacobianFactorQ + */ + boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { std::vector Fblocks; Matrix E; - Matrix3 PointCov; Vector b; - computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); - return boost::make_shared >(Fblocks, E, PointCov, b); + computeJacobians(Fblocks, E, b, cameras, point); + Matrix3 P = PointCov(E, lambda, diagonalDamping); + return boost::make_shared >(Fblocks, E, P, b); } - // **************************************************************************************************** + /** + * Return Jacobians as JacobianFactor + * TODO lambda is currently ignored + */ boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0) const { size_t numKeys = this->keys_.size(); - std::vector < KeyMatrix2D > Fblocks; + std::vector Fblocks; Vector b; - Matrix Enull(2*numKeys, 2*numKeys-3); - computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); - return boost::make_shared< JacobianFactorSVD<6> >(Fblocks, Enull, b); + Matrix Enull(ZDim * numKeys, ZDim * numKeys - 3); + computeJacobiansSVD(Fblocks, Enull, b, cameras, point); + return boost::make_shared >(Fblocks, Enull, b); } private: @@ -669,11 +771,15 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } -}; +} +; + +template +const int SmartFactorBase::ZDim; } // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index bfd73d9fb..a28482583 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -19,7 +19,7 @@ #pragma once -#include "SmartFactorBase.h" +#include #include #include @@ -58,11 +58,11 @@ enum LinearizationMode { }; /** - * SmartProjectionFactor: triangulates point - * TODO: why LANDMARK parameter? + * SmartProjectionFactor: triangulates point and keeps an estimate of it around. */ -template -class SmartProjectionFactor: public SmartFactorBase { +template +class SmartProjectionFactor: public SmartFactorBase, + D> { protected: // Some triangulation parameters @@ -92,7 +92,7 @@ protected: typedef boost::shared_ptr SmartFactorStatePtr; /// shorthand for base class type - typedef SmartFactorBase Base; + typedef SmartFactorBase, D> Base; double landmarkDistanceThreshold_; // if the landmark is triangulated at a // distance larger than that the factor is considered degenerate @@ -102,7 +102,7 @@ protected: // and the factor is disregarded if the error is large /// shorthand for this class - typedef SmartProjectionFactor This; + typedef SmartProjectionFactor This; public: @@ -111,7 +111,7 @@ public: /// shorthand for a pinhole camera typedef PinholeCamera Camera; - typedef std::vector Cameras; + typedef CameraSet Cameras; /** * Constructor @@ -124,16 +124,16 @@ public: */ SmartProjectionFactor(const double rankTol, const double linThreshold, const bool manageDegeneracy, const bool enableEPI, - boost::optional body_P_sensor = boost::none, + boost::optional body_P_sensor = boost::none, double landmarkDistanceThreshold = 1e10, - double dynamicOutlierRejectionThreshold = -1, - SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : + double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state = + SmartFactorStatePtr(new SmartProjectionFactorState())) : Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_( 1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_( - false), verboseCheirality_(false), state_(state), - landmarkDistanceThreshold_(landmarkDistanceThreshold), - dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) { + false), verboseCheirality_(false), state_(state), landmarkDistanceThreshold_( + landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_( + dynamicOutlierRejectionThreshold) { } /** Virtual destructor */ @@ -243,18 +243,18 @@ public: // We triangulate the 3D position of the landmark try { // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; - point_ = triangulatePoint3(cameras, this->measured_, + point_ = triangulatePoint3(cameras, this->measured_, rankTolerance_, enableEPI_); degenerate_ = false; cheiralityException_ = false; // Check landmark distance and reprojection errors to avoid outliers double totalReprojError = 0.0; - size_t i=0; + size_t i = 0; BOOST_FOREACH(const Camera& camera, cameras) { Point3 cameraTranslation = camera.pose().translation(); // we discard smart factors corresponding to points that are far away - if(cameraTranslation.distance(point_) > landmarkDistanceThreshold_){ + if (cameraTranslation.distance(point_) > landmarkDistanceThreshold_) { degenerate_ = true; break; } @@ -268,8 +268,8 @@ public: i += 1; } // we discard smart factors that have large reprojection error - if(dynamicOutlierRejectionThreshold_ > 0 && - totalReprojError/m > dynamicOutlierRejectionThreshold_) + if (dynamicOutlierRejectionThreshold_ > 0 + && totalReprojError / m > dynamicOutlierRejectionThreshold_) degenerate_ = true; } catch (TriangulationUnderconstrainedException&) { @@ -298,7 +298,8 @@ public: || (!this->manageDegeneracy_ && (this->cheiralityException_ || this->degenerate_))) { if (isDebug) { - std::cout << "createRegularImplicitSchurFactor: degenerate configuration" + std::cout + << "createRegularImplicitSchurFactor: degenerate configuration" << std::endl; } return false; @@ -319,9 +320,9 @@ public: bool isDebug = false; size_t numKeys = this->keys_.size(); // Create structures for Hessian Factors - std::vector < Key > js; - std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); - std::vector < Vector > gs(numKeys); + std::vector js; + std::vector Gs(numKeys * (numKeys + 1) / 2); + std::vector gs(numKeys); if (this->measured_.size() != cameras.size()) { std::cout @@ -336,7 +337,7 @@ public: || (!this->manageDegeneracy_ && (this->cheiralityException_ || this->degenerate_))) { // std::cout << "In linearize: exception" << std::endl; - BOOST_FOREACH(gtsam::Matrix& m, Gs) + BOOST_FOREACH(Matrix& m, Gs) m = zeros(D, D); BOOST_FOREACH(Vector& v, gs) v = zero(D); @@ -371,9 +372,8 @@ public: // ================================================================== Matrix F, E; - Matrix3 PointCov; Vector b; - double f = computeJacobians(F, E, PointCov, b, cameras, lambda); + double f = computeJacobians(F, E, b, cameras); // Schur complement trick // Frank says: should be possible to do this more efficiently? @@ -381,20 +381,20 @@ public: Matrix H(D * numKeys, D * numKeys); Vector gs_vector; - H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); - gs_vector.noalias() = F.transpose() - * (b - (E * (PointCov * (E.transpose() * b)))); + Matrix3 P = Base::PointCov(E, lambda); + H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F)))); + gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b)))); if (isDebug) std::cout << "gs_vector size " << gs_vector.size() << std::endl; // Populate Gs and gs int GsCount2 = 0; - for (DenseIndex i1 = 0; i1 < (DenseIndex)numKeys; i1++) { // for each camera + for (DenseIndex i1 = 0; i1 < (DenseIndex) numKeys; i1++) { // for each camera DenseIndex i1D = i1 * D; - gs.at(i1) = gs_vector.segment < D > (i1D); - for (DenseIndex i2 = 0; i2 < (DenseIndex)numKeys; i2++) { + gs.at(i1) = gs_vector.segment(i1D); + for (DenseIndex i2 = 0; i2 < (DenseIndex) numKeys; i2++) { if (i2 >= i1) { - Gs.at(GsCount2) = H.block < D, D > (i1D, i2 * D); + Gs.at(GsCount2) = H.block(i1D, i2 * D); GsCount2++; } } @@ -418,16 +418,16 @@ public: } /// create factor - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) return Base::createJacobianQFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorQ >(this->keys_); + return boost::make_shared >(this->keys_); } /// Create a factor, takes values - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Values& values, double lambda) const { Cameras myCameras; // TODO triangulate twice ?? @@ -435,16 +435,16 @@ public: if (nonDegenerate) return createJacobianQFactor(myCameras, lambda); else - return boost::make_shared< JacobianFactorQ >(this->keys_); + return boost::make_shared >(this->keys_); } /// different (faster) way to compute Jacobian factor - boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, - double lambda) const { + boost::shared_ptr createJacobianSVDFactor( + const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) return Base::createJacobianSVDFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorSVD >(this->keys_); + return boost::make_shared >(this->keys_); } /// Returns true if nonDegenerate @@ -477,27 +477,27 @@ public: return true; } + /// Assumes non-degenerate ! + void computeEP(Matrix& E, Matrix& P, const Cameras& cameras) const { + return Base::computeEP(E, P, cameras, point_); + } + /// Takes values - bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const { + bool computeEP(Matrix& E, Matrix& P, const Values& values) const { Cameras myCameras; bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); if (nonDegenerate) - computeEP(E, PointCov, myCameras); + computeEP(E, P, myCameras); return nonDegenerate; } - /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const { - return Base::computeEP(E, PointCov, cameras, point_); - } - /// Version that takes values, and creates the point bool computeJacobians(std::vector& Fblocks, - Matrix& E, Matrix& PointCov, Vector& b, const Values& values) const { + Matrix& E, Vector& b, const Values& values) const { Cameras myCameras; bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); if (nonDegenerate) - computeJacobians(Fblocks, E, PointCov, b, myCameras, 0.0); + computeJacobians(Fblocks, E, b, myCameras); return nonDegenerate; } @@ -536,7 +536,7 @@ public: this->noise_.at(i)->WhitenSystem(Fi, Ei, bi); f += bi.squaredNorm(); Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi)); - E.block < 2, 2 > (2 * i, 0) = Ei; + E.block<2, 2>(2 * i, 0) = Ei; subInsert(b, bi, 2 * i); } return f; @@ -546,19 +546,6 @@ public: } // end else } - /// Version that computes PointCov, with optional lambda parameter - double computeJacobians(std::vector& Fblocks, - Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras, - const double lambda = 0.0) const { - - double f = computeJacobians(Fblocks, E, b, cameras); - - // Point covariance inv(E'*E) - PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse(); - - return f; - } - /// takes values bool computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, Vector& b, const Values& values) const { @@ -583,9 +570,9 @@ public: } /// Returns Matrix, TODO: maybe should not exist -> not sparse ! - double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, - const Cameras& cameras, const double lambda) const { - return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda); + double computeJacobians(Matrix& F, Matrix& E, Vector& b, + const Cameras& cameras) const { + return Base::computeJacobians(F, E, b, cameras, point_); } /// Calculate vector of re-projection errors, before applying noise model @@ -700,7 +687,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(throwCheirality_); ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index f871ab3aa..127bf284f 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -19,7 +19,7 @@ #pragma once -#include "SmartProjectionFactor.h" +#include namespace gtsam { /** @@ -37,8 +37,8 @@ namespace gtsam { * The calibration is known here. The factor only constraints poses (variable dimension is 6) * @addtogroup SLAM */ -template -class SmartProjectionPoseFactor: public SmartProjectionFactor { +template +class SmartProjectionPoseFactor: public SmartProjectionFactor { protected: LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q) @@ -48,10 +48,10 @@ protected: public: /// shorthand for base class type - typedef SmartProjectionFactor Base; + typedef SmartProjectionFactor Base; /// shorthand for this class - typedef SmartProjectionPoseFactor This; + typedef SmartProjectionPoseFactor This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -67,7 +67,7 @@ public: */ SmartProjectionPoseFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, - const bool enableEPI = false, boost::optional body_P_sensor = boost::none, + const bool enableEPI = false, boost::optional body_P_sensor = boost::none, LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1) : Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor, @@ -141,11 +141,6 @@ public: return e && Base::equals(p, tol); } - /// get the dimension of the factor - virtual size_t dim() const { - return 6 * this->keys_.size(); - } - /** * Collect all cameras involved in this factor * @param values Values structure which must contain camera poses corresponding @@ -208,11 +203,17 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(K_all_); } }; // end of class declaration +/// traits +template +struct traits > : public Testable< + SmartProjectionPoseFactor > { +}; + } // \ namespace gtsam diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index f54ee47ba..578422eaf 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -169,7 +169,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); @@ -180,4 +180,8 @@ private: } }; +/// traits +template +struct traits > : public Testable > {}; + } // \ namespace gtsam diff --git a/gtsam/geometry/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h similarity index 97% rename from gtsam/geometry/TriangulationFactor.h rename to gtsam/slam/TriangulationFactor.h index fc8d546d3..d0371d1f8 100644 --- a/gtsam/geometry/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -10,14 +10,13 @@ * -------------------------------------------------------------------------- */ /** - * testTriangulationFactor.h + * triangulationFactor.h * @date March 2, 2014 * @author Frank Dellaert */ #include #include -#include #include #include @@ -185,7 +184,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(camera_); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index a6fb2d830..851adacf5 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -63,8 +64,8 @@ string findExampleDataFile(const string& name) { throw invalid_argument( "gtsam::findExampleDataFile could not find a matching file in\n" - SOURCE_TREE_DATASET_DIR " or\n" - INSTALLED_DATASET_DIR " named\n" + + GTSAM_SOURCE_TREE_DATASET_DIR " or\n" + GTSAM_INSTALLED_DATASET_DIR " named\n" + name + ", " + name + ".graph, or " + name + ".txt"); } @@ -669,6 +670,7 @@ bool readBundler(const string& filename, SfM_data &data) { float u, v; is >> cam_idx >> point_idx >> u >> v; track.measurements.push_back(make_pair(cam_idx, Point2(u, -v))); + track.siftIndices.push_back(make_pair(cam_idx, point_idx)); } data.tracks.push_back(track); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 3a0f8edb7..21c23f0e0 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -137,11 +137,15 @@ GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); /// A measurement with its camera index typedef std::pair SfM_Measurement; +/// SfM_Track +typedef std::pair SIFT_Index; + /// Define the structure for the 3D points struct SfM_Track { Point3 p; ///< 3D position of the point float r, g, b; ///< RGB color of the 3D point std::vector measurements; ///< The 2D image projections (id,(u,v)) + std::vector siftIndices; size_t number_measurements() const { return measurements.size(); } diff --git a/gtsam_unstable/slam/expressions.h b/gtsam/slam/expressions.h similarity index 51% rename from gtsam_unstable/slam/expressions.h rename to gtsam/slam/expressions.h index d9cbd5716..b819993ef 100644 --- a/gtsam_unstable/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -7,27 +7,21 @@ #pragma once -#include +#include +#include #include +#include #include -#include namespace gtsam { -// Generics - -template -Expression between(const Expression& t1, const Expression& t2) { - return Expression(t1, &T::between, t2); -} - // 2D Geometry typedef Expression Point2_; typedef Expression Rot2_; typedef Expression Pose2_; -Point2_ transform_to(const Pose2_& x, const Point2_& p) { +inline Point2_ transform_to(const Pose2_& x, const Point2_& p) { return Point2_(x, &Pose2::transform_to, p); } @@ -37,25 +31,36 @@ typedef Expression Point3_; typedef Expression Rot3_; typedef Expression Pose3_; -Point3_ transform_to(const Pose3_& x, const Point3_& p) { +inline Point3_ transform_to(const Pose3_& x, const Point3_& p) { return Point3_(x, &Pose3::transform_to, p); } // Projection typedef Expression Cal3_S2_; +typedef Expression Cal3Bundler_; -Point2_ project(const Point3_& p_cam) { +inline Point2_ project(const Point3_& p_cam) { return Point2_(PinholeCamera::project_to_camera, p_cam); } -Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K, - boost::optional Dpose, boost::optional Dpoint, - boost::optional Dcal) { +template +Point2 project4(const CAMERA& camera, const Point3& p, + OptionalJacobian<2, CAMERA::dimension> Dcam, OptionalJacobian<2, 3> Dpoint) { + return camera.project2(p, Dcam, Dpoint); +} + +template +Point2_ project2(const Expression& camera_, const Point3_& p_) { + return Point2_(project4, camera_, p_); +} + +inline Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K, + OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, OptionalJacobian<2, 5> Dcal) { return PinholeCamera(x, K).project(p, Dpose, Dpoint, Dcal); } -Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) { +inline Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) { return Point2_(project6, x, p, K); } diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index 5d73bcab8..1dc897599 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -41,7 +41,7 @@ TEST( AntiFactor, NegativeHessian) Pose3 pose1(Rot3(), Point3(0, 0, 0)); Pose3 pose2(Rot3(), Point3(2, 1, 3)); Pose3 z(Rot3(), Point3(1, 1, 1)); - SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim())); + SharedNoiseModel sigma(noiseModel::Unit::Create(6)); // Create a configuration corresponding to the ground truth Values values; @@ -92,7 +92,7 @@ TEST( AntiFactor, EquivalentBayesNet) Pose3 pose1(Rot3(), Point3(0, 0, 0)); Pose3 pose2(Rot3(), Point3(2, 1, 3)); Pose3 z(Rot3(), Point3(1, 1, 1)); - SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim())); + SharedNoiseModel sigma(noiseModel::Unit::Create(6)); NonlinearFactorGraph graph; graph.push_back(PriorFactor(1, pose1, sigma)); diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index d0dbe7908..adb759dc0 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -31,19 +31,43 @@ TEST(BetweenFactor, Rot3) { Vector expected = Rot3::Logmap(measured.inverse() * R1.between(R2)); EXPECT(assert_equal(expected,actual/*, 1e-100*/)); // Uncomment to make unit test fail - Matrix numericalH1 = numericalDerivative21( + Matrix numericalH1 = numericalDerivative21( boost::function(boost::bind( &BetweenFactor::evaluateError, factor, _1, _2, boost::none, boost::none)), R1, R2, 1e-5); EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); - Matrix numericalH2 = numericalDerivative22( + Matrix numericalH2 = numericalDerivative22( boost::function(boost::bind( &BetweenFactor::evaluateError, factor, _1, _2, boost::none, boost::none)), R1, R2, 1e-5); EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); } +/* ************************************************************************* */ +/* +// Constructor scalar +TEST(BetweenFactor, ConstructorScalar) { + SharedNoiseModel model; + double measured_value = 0.0; + BetweenFactor factor(1, 2, measured_value, model); +} + +// Constructor vector3 +TEST(BetweenFactor, ConstructorVector3) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); + Vector3 measured_value(1, 2, 3); + BetweenFactor factor(1, 2, measured_value, model); +} + +// Constructor dynamic sized vector +TEST(BetweenFactor, ConstructorDynamicSizeVector) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0); + Vector measured_value(5); measured_value << 1, 2, 3, 4, 5; + BetweenFactor factor(1, 2, measured_value, model); +} +*/ + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index d6a5ffa8c..0406c3d27 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -15,16 +15,16 @@ * @author Richard Roberts, Luca Carlone */ -#include - -#include - -#include -#include -#include #include #include +#include +#include +#include + +#include + +#include using namespace gtsam::symbol_shorthand; using namespace std; diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index 4c0edf5c9..31b276642 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -50,10 +50,10 @@ TEST( EssentialMatrixConstraint, test ) { CHECK(assert_equal(expected, actual, 1e-8)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11( + Matrix expectedH1 = numericalDerivative11( boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); - Matrix expectedH2 = numericalDerivative11( + Matrix expectedH2 = numericalDerivative11( boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index e8acb0db0..e0e26ecff 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -96,7 +96,8 @@ TEST (EssentialMatrixFactor, factor) { // Use numerical derivatives to calculate the expected Jacobian Matrix Hexpected; - Hexpected = numericalDerivative11( + typedef Eigen::Matrix Vector1; + Hexpected = numericalDerivative11( boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1, boost::none), trueE); @@ -173,8 +174,8 @@ TEST (EssentialMatrixFactor2, factor) { boost::function f = boost::bind( &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, trueE, d); - Hexpected2 = numericalDerivative22(f, trueE, d); + Hexpected1 = numericalDerivative21(f, trueE, d); + Hexpected2 = numericalDerivative22(f, trueE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); @@ -247,8 +248,8 @@ TEST (EssentialMatrixFactor3, factor) { boost::function f = boost::bind( &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, bodyE, d); - Hexpected2 = numericalDerivative22(f, bodyE, d); + Hexpected1 = numericalDerivative21(f, bodyE, d); + Hexpected2 = numericalDerivative22(f, bodyE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); @@ -389,8 +390,8 @@ TEST (EssentialMatrixFactor2, extraTest) { boost::function f = boost::bind( &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, trueE, d); - Hexpected2 = numericalDerivative22(f, trueE, d); + Hexpected1 = numericalDerivative21(f, trueE, d); + Hexpected2 = numericalDerivative22(f, trueE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); @@ -458,8 +459,8 @@ TEST (EssentialMatrixFactor3, extraTest) { boost::function f = boost::bind( &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, bodyE, d); - Hexpected2 = numericalDerivative22(f, bodyE, d); + Hexpected1 = numericalDerivative21(f, bodyE, d); + Hexpected2 = numericalDerivative22(f, bodyE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 8adbaa62d..240b19a46 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -86,7 +86,7 @@ static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); TEST( GeneralSFMFactor, equals ) { // Create two identical factors and make sure they're equal - Vector z = Vector2(323.,240.); + Point2 z(323.,240.); const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 9cc634b37..df56f5260 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -86,7 +86,7 @@ static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); TEST( GeneralSFMFactor_Cal3Bundler, equals ) { // Create two identical factors and make sure they're equal - Vector z = Vector2(323.,240.); + Point2 z(323.,240.); const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp index 2d1793417..7db71d8ce 100644 --- a/gtsam/slam/tests/testLago.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp new file mode 100644 index 000000000..4a7b4c3fe --- /dev/null +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -0,0 +1,183 @@ +/* ---------------------------------------------------------------------------- + + * 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 testOrientedPlane3.cpp + * @date Dec 19, 2012 + * @author Alex Trevor + * @brief Tests the OrientedPlane3Factor class + */ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +using namespace boost::assign; +using namespace gtsam; +using namespace std; + +GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) +GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) + +// ************************************************************************* +TEST (OrientedPlane3Factor, lm_translation_error) { + // Tests one pose, two measurements of the landmark that differ in range only. + // Normal along -x, 3m away + Symbol lm_sym('p', 0); + OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0); + + ISAM2 isam2; + Values new_values; + NonlinearFactorGraph new_graph; + + // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose + Symbol init_sym('x', 0); + Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); + Vector sigmas(6); + sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001; + PriorFactor pose_prior(init_sym, init_pose, + noiseModel::Diagonal::Sigmas(sigmas)); + new_values.insert(init_sym, init_pose); + new_graph.add(pose_prior); + + // Add two landmark measurements, differing in range + new_values.insert(lm_sym, test_lm0); + Vector sigmas3(3); + sigmas3 << 0.1, 0.1, 0.1; + Vector test_meas0_mean(4); + test_meas0_mean << -1.0, 0.0, 0.0, 3.0; + OrientedPlane3Factor test_meas0(test_meas0_mean, + noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym); + new_graph.add(test_meas0); + Vector test_meas1_mean(4); + test_meas1_mean << -1.0, 0.0, 0.0, 1.0; + OrientedPlane3Factor test_meas1(test_meas1_mean, + noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym); + new_graph.add(test_meas1); + + // Optimize + ISAM2Result result = isam2.update(new_graph, new_values); + Values result_values = isam2.calculateEstimate(); + OrientedPlane3 optimized_plane_landmark = result_values.at( + lm_sym); + + // Given two noisy measurements of equal weight, expect result between the two + OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0); + EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); +} + +// ************************************************************************* +TEST (OrientedPlane3Factor, lm_rotation_error) { + // Tests one pose, two measurements of the landmark that differ in angle only. + // Normal along -x, 3m away + Symbol lm_sym('p', 0); + OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0); + + ISAM2 isam2; + Values new_values; + NonlinearFactorGraph new_graph; + + // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose + Symbol init_sym('x', 0); + Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); + PriorFactor pose_prior(init_sym, init_pose, + noiseModel::Diagonal::Sigmas( + (Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished())); + new_values.insert(init_sym, init_pose); + new_graph.add(pose_prior); + +// // Add two landmark measurements, differing in angle + new_values.insert(lm_sym, test_lm0); + Vector test_meas0_mean(4); + test_meas0_mean << -1.0, 0.0, 0.0, 3.0; + OrientedPlane3Factor test_meas0(test_meas0_mean, + noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym, lm_sym); + new_graph.add(test_meas0); + Vector test_meas1_mean(4); + test_meas1_mean << 0.0, -1.0, 0.0, 3.0; + OrientedPlane3Factor test_meas1(test_meas1_mean, + noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym, lm_sym); + new_graph.add(test_meas1); + + // Optimize + ISAM2Result result = isam2.update(new_graph, new_values); + Values result_values = isam2.calculateEstimate(); + OrientedPlane3 optimized_plane_landmark = result_values.at( + lm_sym); + + // Given two noisy measurements of equal weight, expect result between the two + OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, + 0.0, 3.0); + EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); +} + +// ************************************************************************* +TEST( OrientedPlane3DirectionPrior, Constructor ) { + + // Example: pitch and roll of aircraft in an ENU Cartesian frame. + // If pitch and roll are zero for an aerospace frame, + // that means Z is pointing down, i.e., direction of Z = (0,0,-1) + + Vector planeOrientation = (Vector(4) << 0.0, 0.0, -1.0, 10.0).finished(); // all vertical planes directly facing the origin + + // Factor + Key key(1); + SharedGaussian model = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 10.0)); + OrientedPlane3DirectionPrior factor(key, planeOrientation, model); + + // Create a linearization point at the zero-error point + Vector theta1 = Vector4(0.0, 0.02, -1.2, 10.0); + Vector theta2 = Vector4(0.0, 0.1, -0.8, 10.0); + Vector theta3 = Vector4(0.0, 0.2, -0.9, 10.0); + + OrientedPlane3 T1(theta1); + OrientedPlane3 T2(theta2); + OrientedPlane3 T3(theta3); + + // Calculate numerical derivatives + Matrix expectedH1 = numericalDerivative11( + boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, + boost::none), T1); + + Matrix expectedH2 = numericalDerivative11( + boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, + boost::none), T2); + + Matrix expectedH3 = numericalDerivative11( + boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, + boost::none), T3); + + // Use the factor to calculate the derivative + Matrix actualH1, actualH2, actualH3; + factor.evaluateError(T1, actualH1); + factor.evaluateError(T2, actualH2); + factor.evaluateError(T3, actualH3); + + // Verify we get the expected error + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); + EXPECT(assert_equal(expectedH3, actualH3, 1e-8)); +} + +/* ************************************************************************* */ +int main() { + srand(time(NULL)); + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testPoseRotationPrior.cpp b/gtsam/slam/tests/testPoseRotationPrior.cpp index 9165ff17a..db04a74eb 100644 --- a/gtsam/slam/tests/testPoseRotationPrior.cpp +++ b/gtsam/slam/tests/testPoseRotationPrior.cpp @@ -53,7 +53,7 @@ TEST( testPoseRotationFactor, level3_zero_error ) { Pose3RotationPrior factor(poseKey, rot3A, model3); Matrix actH1; EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -67,7 +67,7 @@ TEST( testPoseRotationFactor, level3_error ) { #else EXPECT(assert_equal(Vector3(-0.1, -0.2, -0.3), factor.evaluateError(pose1, actH1),1e-2)); #endif - Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); // the derivative is more complex, but is close to the identity for Rot3 around the origin // If not using true expmap will be close, but not exact around the origin // EXPECT(assert_equal(expH1, actH1, tol)); @@ -79,7 +79,7 @@ TEST( testPoseRotationFactor, level2_zero_error ) { Pose2RotationPrior factor(poseKey, rot2A, model1); Matrix actH1; EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -89,7 +89,7 @@ TEST( testPoseRotationFactor, level2_error ) { Pose2RotationPrior factor(poseKey, rot2B, model1); Matrix actH1; EXPECT(assert_equal((Vector(1) << -M_PI_2).finished(), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -99,7 +99,7 @@ TEST( testPoseRotationFactor, level2_error_wrap ) { Pose2RotationPrior factor(poseKey, rot2D, model1); Matrix actH1; EXPECT(assert_equal((Vector(1) << -0.02).finished(), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testPoseTranslationPrior.cpp b/gtsam/slam/tests/testPoseTranslationPrior.cpp index f7bd412fe..2f39701f7 100644 --- a/gtsam/slam/tests/testPoseTranslationPrior.cpp +++ b/gtsam/slam/tests/testPoseTranslationPrior.cpp @@ -49,7 +49,7 @@ TEST( testPoseTranslationFactor, level3_zero_error ) { Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -59,7 +59,7 @@ TEST( testPoseTranslationFactor, level3_error ) { Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; EXPECT(assert_equal(Vector3(-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -69,7 +69,7 @@ TEST( testPoseTranslationFactor, pitched3_zero_error ) { Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -79,7 +79,7 @@ TEST( testPoseTranslationFactor, pitched3_error ) { Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; EXPECT(assert_equal(Vector3(-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -89,7 +89,7 @@ TEST( testPoseTranslationFactor, smallrot3_zero_error ) { Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -99,7 +99,7 @@ TEST( testPoseTranslationFactor, smallrot3_error ) { Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; EXPECT(assert_equal(Vector3(-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -109,7 +109,7 @@ TEST( testPoseTranslationFactor, level2_zero_error ) { Pose2TranslationPrior factor(poseKey, point2A, model2); Matrix actH1; EXPECT(assert_equal(zero(2), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -119,7 +119,7 @@ TEST( testPoseTranslationFactor, level2_error ) { Pose2TranslationPrior factor(poseKey, point2B, model2); Matrix actH1; EXPECT(assert_equal(Vector2(-3.0,-4.0), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testPriorFactor.cpp b/gtsam/slam/tests/testPriorFactor.cpp index b3e54bedb..b26d633f5 100644 --- a/gtsam/slam/tests/testPriorFactor.cpp +++ b/gtsam/slam/tests/testPriorFactor.cpp @@ -5,8 +5,8 @@ * @date Nov 4, 2014 */ +#include #include -#include #include using namespace std; @@ -14,10 +14,23 @@ using namespace gtsam; /* ************************************************************************* */ -// Constructor -TEST(PriorFactor, Constructor) { +// Constructor scalar +TEST(PriorFactor, ConstructorScalar) { SharedNoiseModel model; - PriorFactor factor(1, LieScalar(1.0), model); + PriorFactor factor(1, 1.0, model); +} + +// Constructor vector3 +TEST(PriorFactor, ConstructorVector3) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); + PriorFactor factor(1, Vector3(1,2,3), model); +} + +// Constructor dynamic sized vector +TEST(PriorFactor, ConstructorDynamicSizeVector) { + Vector v(5); v << 1, 2, 3, 4, 5; + SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0); + PriorFactor factor(1, v, model); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testRangeFactor.cpp b/gtsam/slam/tests/testRangeFactor.cpp index 6bffa3ce9..a8455d685 100644 --- a/gtsam/slam/tests/testRangeFactor.cpp +++ b/gtsam/slam/tests/testRangeFactor.cpp @@ -34,14 +34,30 @@ static SharedNoiseModel model(noiseModel::Unit::Create(1)); typedef RangeFactor RangeFactor2D; typedef RangeFactor RangeFactor3D; +typedef RangeFactorWithTransform RangeFactorWithTransform2D; +typedef RangeFactorWithTransform RangeFactorWithTransform3D; /* ************************************************************************* */ -Vector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) { +Vector factorError2D(const Pose2& pose, const Point2& point, + const RangeFactor2D& factor) { return factor.evaluateError(pose, point); } /* ************************************************************************* */ -Vector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) { +Vector factorError3D(const Pose3& pose, const Point3& point, + const RangeFactor3D& factor) { + return factor.evaluateError(pose, point); +} + +/* ************************************************************************* */ +Vector factorErrorWithTransform2D(const Pose2& pose, const Point2& point, + const RangeFactorWithTransform2D& factor) { + return factor.evaluateError(pose, point); +} + +/* ************************************************************************* */ +Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point, + const RangeFactorWithTransform3D& factor) { return factor.evaluateError(pose, point); } @@ -61,10 +77,13 @@ TEST( RangeFactor, ConstructorWithTransform) { Key pointKey(2); double measurement(10.0); Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2); - Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); - RangeFactor2D factor2D(poseKey, pointKey, measurement, model, body_P_sensor_2D); - RangeFactor3D factor3D(poseKey, pointKey, measurement, model, body_P_sensor_3D); + RangeFactorWithTransform2D factor2D(poseKey, pointKey, measurement, model, + body_P_sensor_2D); + RangeFactorWithTransform3D factor3D(poseKey, pointKey, measurement, model, + body_P_sensor_3D); } /* ************************************************************************* */ @@ -90,14 +109,19 @@ TEST( RangeFactor, EqualsWithTransform ) { Key pointKey(2); double measurement(10.0); Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2); - Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); - RangeFactor2D factor2D_1(poseKey, pointKey, measurement, model, body_P_sensor_2D); - RangeFactor2D factor2D_2(poseKey, pointKey, measurement, model, body_P_sensor_2D); + RangeFactorWithTransform2D factor2D_1(poseKey, pointKey, measurement, model, + body_P_sensor_2D); + RangeFactorWithTransform2D factor2D_2(poseKey, pointKey, measurement, model, + body_P_sensor_2D); CHECK(assert_equal(factor2D_1, factor2D_2)); - RangeFactor3D factor3D_1(poseKey, pointKey, measurement, model, body_P_sensor_3D); - RangeFactor3D factor3D_2(poseKey, pointKey, measurement, model, body_P_sensor_3D); + RangeFactorWithTransform3D factor3D_1(poseKey, pointKey, measurement, model, + body_P_sensor_3D); + RangeFactorWithTransform3D factor3D_2(poseKey, pointKey, measurement, model, + body_P_sensor_3D); CHECK(assert_equal(factor3D_1, factor3D_2)); } @@ -130,7 +154,8 @@ TEST( RangeFactor, Error2DWithTransform ) { Key pointKey(2); double measurement(10.0); Pose2 body_P_sensor(0.25, -0.10, -M_PI_2); - RangeFactor2D factor(poseKey, pointKey, measurement, model, body_P_sensor); + RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model, + body_P_sensor); // Set the linearization point Rot2 R(0.57); @@ -176,8 +201,10 @@ TEST( RangeFactor, Error3DWithTransform ) { Key poseKey(1); Key pointKey(2); double measurement(10.0); - Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); - RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); + RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model, + body_P_sensor); // Set the linearization point Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75); @@ -213,8 +240,10 @@ TEST( RangeFactor, Jacobian2D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); + H1Expected = numericalDerivative11( + boost::bind(&factorError2D, _1, point, factor), pose); + H2Expected = numericalDerivative11( + boost::bind(&factorError2D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -228,7 +257,8 @@ TEST( RangeFactor, Jacobian2DWithTransform ) { Key pointKey(2); double measurement(10.0); Pose2 body_P_sensor(0.25, -0.10, -M_PI_2); - RangeFactor2D factor(poseKey, pointKey, measurement, model, body_P_sensor); + RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model, + body_P_sensor); // Set the linearization point Rot2 R(0.57); @@ -242,8 +272,10 @@ TEST( RangeFactor, Jacobian2DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); + H1Expected = numericalDerivative11( + boost::bind(&factorErrorWithTransform2D, _1, point, factor), pose); + H2Expected = numericalDerivative11( + boost::bind(&factorErrorWithTransform2D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -268,8 +300,10 @@ TEST( RangeFactor, Jacobian3D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); + H1Expected = numericalDerivative11( + boost::bind(&factorError3D, _1, point, factor), pose); + H2Expected = numericalDerivative11( + boost::bind(&factorError3D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -282,8 +316,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { Key poseKey(1); Key pointKey(2); double measurement(10.0); - Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); - RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); + RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model, + body_P_sensor); // Set the linearization point Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75); @@ -297,8 +333,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); + H1Expected = numericalDerivative11( + boost::bind(&factorErrorWithTransform3D, _1, point, factor), pose); + H2Expected = numericalDerivative11( + boost::bind(&factorErrorWithTransform3D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -306,6 +344,9 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testRegularHessianFactor.cpp b/gtsam/slam/tests/testRegularHessianFactor.cpp new file mode 100644 index 000000000..e2b8ac3cf --- /dev/null +++ b/gtsam/slam/tests/testRegularHessianFactor.cpp @@ -0,0 +1,106 @@ +/* ---------------------------------------------------------------------------- + + * 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 testRegularHessianFactor.cpp + * @author Frank Dellaert + * @date March 4, 2014 + */ + +#include +#include + +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +/* ************************************************************************* */ +TEST(RegularHessianFactor, ConstructorNWay) +{ + Matrix G11 = (Matrix(2,2) << 111, 112, 113, 114).finished(); + Matrix G12 = (Matrix(2,2) << 121, 122, 123, 124).finished(); + Matrix G13 = (Matrix(2,2) << 131, 132, 133, 134).finished(); + + Matrix G22 = (Matrix(2,2) << 221, 222, 222, 224).finished(); + Matrix G23 = (Matrix(2,2) << 231, 232, 233, 234).finished(); + + Matrix G33 = (Matrix(2,2) << 331, 332, 332, 334).finished(); + + Vector g1 = (Vector(2) << -7, -9).finished(); + Vector g2 = (Vector(2) << -9, 1).finished(); + Vector g3 = (Vector(2) << 2, 3).finished(); + + double f = 10; + + std::vector js; + js.push_back(0); js.push_back(1); js.push_back(3); + std::vector Gs; + Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33); + std::vector gs; + gs.push_back(g1); gs.push_back(g2); gs.push_back(g3); + RegularHessianFactor<2> factor(js, Gs, gs, f); + + // multiplyHessianAdd: + { + // brute force + Matrix AtA = factor.information(); + HessianFactor::const_iterator i1 = factor.begin(); + HessianFactor::const_iterator i2 = i1 + 1; + Vector X(6); X << 1,2,3,4,5,6; + Vector Y(6); Y << 2633, 2674, 4465, 4501, 5669, 5696; + EXPECT(assert_equal(Y,AtA*X)); + + VectorValues x = map_list_of + (0, (Vector(2) << 1,2).finished()) + (1, (Vector(2) << 3,4).finished()) + (3, (Vector(2) << 5,6).finished()); + + VectorValues expected; + expected.insert(0, Y.segment<2>(0)); + expected.insert(1, Y.segment<2>(2)); + expected.insert(3, Y.segment<2>(4)); + + // VectorValues version + double alpha = 1.0; + VectorValues actualVV; + actualVV.insert(0, zero(2)); + actualVV.insert(1, zero(2)); + actualVV.insert(3, zero(2)); + factor.multiplyHessianAdd(alpha, x, actualVV); + EXPECT(assert_equal(expected, actualVV)); + + // RAW ACCESS + Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696; + Vector fast_y = gtsam::zero(8); + double xvalues[8] = {1,2,3,4,0,0,5,6}; + factor.multiplyHessianAdd(alpha, xvalues, fast_y.data()); + EXPECT(assert_equal(expected_y, fast_y)); + + // now, do it with non-zero y + factor.multiplyHessianAdd(alpha, xvalues, fast_y.data()); + EXPECT(assert_equal(2*expected_y, fast_y)); + + // check some expressions + EXPECT(assert_equal(G12,factor.info(i1,i2).knownOffDiagonal())); + EXPECT(assert_equal(G22,factor.info(i2,i2).selfadjointView())); + EXPECT(assert_equal((Matrix)G12.transpose(),factor.info(i2,i1).knownOffDiagonal())); + } +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index e96c8101c..8571a345d 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -1,16 +1,24 @@ +/* ---------------------------------------------------------------------------- + + * 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 testImplicitSchurFactor.cpp + * @file testRegularImplicitSchurFactor.cpp * @brief unit test implicit jacobian factors * @author Frank Dellaert * @date Oct 20, 2013 */ -//#include -#include -//#include #include -//#include "gtsam_unstable/slam/JacobianFactorQR.h" -#include "gtsam/slam/JacobianFactorQR.h" +#include +#include #include #include @@ -114,7 +122,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { // Create JacobianFactor with same error const SharedDiagonal model; - JacobianFactorQ<6> jf(Fblocks, E, P, b, model); + JacobianFactorQ<6, 2> jf(Fblocks, E, P, b, model); { // error double expectedError = jf.error(xvalues); @@ -164,7 +172,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { } // Create JacobianFactorQR - JacobianFactorQR<6> jfq(Fblocks, E, P, b, model); + JacobianFactorQR<6, 2> jfq(Fblocks, E, P, b, model); { const SharedDiagonal model; VectorValues yActual = zero; diff --git a/gtsam/slam/tests/testRegularJacobianFactor.cpp b/gtsam/slam/tests/testRegularJacobianFactor.cpp new file mode 100644 index 000000000..5803516a1 --- /dev/null +++ b/gtsam/slam/tests/testRegularJacobianFactor.cpp @@ -0,0 +1,235 @@ +/* ---------------------------------------------------------------------------- + + * 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 testRegularJacobianFactor.cpp + * @brief unit test regular jacobian factors + * @author Sungtae An + * @date Nov 12, 2014 + */ + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +static const size_t fixedDim = 3; +static const size_t nrKeys = 3; + +// Keys are assumed to be from 0 to n +namespace { + namespace simple { + // Terms we'll use + const vector > terms = list_of > + (make_pair(0, Matrix3::Identity())) + (make_pair(1, 2*Matrix3::Identity())) + (make_pair(2, 3*Matrix3::Identity())); + + // RHS and sigmas + const Vector b = (Vector(3) << 1., 2., 3.).finished(); + const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5,0.5,0.5).finished()); + } + + namespace simple2 { + // Terms + const vector > terms2 = list_of > + (make_pair(0, 2*Matrix3::Identity())) + (make_pair(1, 4*Matrix3::Identity())) + (make_pair(2, 6*Matrix3::Identity())); + + // RHS + const Vector b2 = (Vector(3) << 2., 4., 6.).finished(); + } +} + +/* ************************************************************************* */ +// Convert from double* to VectorValues +VectorValues double2vv(const double* x, + const size_t nrKeys, const size_t dim) { + // create map with dimensions + std::map dims; + for (size_t i = 0; i < nrKeys; i++) + dims.insert(make_pair(i, dim)); + + size_t n = nrKeys*dim; + Vector xVec(n); + for (size_t i = 0; i < n; i++){ + xVec(i) = x[i]; + } + return VectorValues(xVec, dims); +} + +/* ************************************************************************* */ +void vv2double(const VectorValues& vv, double* y, + const size_t nrKeys, const size_t dim) { + // create map with dimensions + std::map dims; + for (size_t i = 0; i < nrKeys; i++) + dims.insert(make_pair(i, dim)); + + Vector yvector = vv.vector(dims); + size_t n = nrKeys*dim; + for (size_t j = 0; j < n; j++) + y[j] = yvector[j]; +} + +/* ************************************************************************* */ +TEST(RegularJacobianFactor, constructorNway) +{ + using namespace simple; + JacobianFactor factor(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); + RegularJacobianFactor regularFactor(terms, b, noise); + + LONGS_EQUAL((long)terms[2].first, (long)regularFactor.keys().back()); + EXPECT(assert_equal(terms[2].second, regularFactor.getA(regularFactor.end() - 1))); + EXPECT(assert_equal(b, factor.getb())); + EXPECT(assert_equal(b, regularFactor.getb())); + EXPECT(noise == factor.get_model()); + EXPECT(noise == regularFactor.get_model()); +} + +/* ************************************************************************* */ +TEST(RegularJacobianFactor, hessianDiagonal) +{ + using namespace simple; + JacobianFactor factor(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); + RegularJacobianFactor regularFactor(terms, b, noise); + + // we compute hessian diagonal from the standard Jacobian + VectorValues expectedHessianDiagonal = factor.hessianDiagonal(); + + // we compare against the Raw memory access implementation of hessianDiagonal + double actualValue[9]={0}; + regularFactor.hessianDiagonal(actualValue); + VectorValues actualHessianDiagonalRaw = double2vv(actualValue,nrKeys,fixedDim); + EXPECT(assert_equal(expectedHessianDiagonal, actualHessianDiagonalRaw)); +} + +/* ************************************************************************* */ +TEST(RegularJacobian, gradientAtZero) +{ + using namespace simple; + JacobianFactor factor(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); + RegularJacobianFactor regularFactor(terms, b, noise); + + // we compute gradient at zero from the standard Jacobian + VectorValues expectedGradientAtZero = factor.gradientAtZero(); + + //EXPECT(assert_equal(expectedGradientAtZero, regularFactor.gradientAtZero())); + + // we compare against the Raw memory access implementation of gradientAtZero + double actualValue[9]={0}; + regularFactor.gradientAtZero(actualValue); + VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim); + EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw)); +} + +/* ************************************************************************* */ +TEST(RegularJacobian, gradientAtZero_multiFactors) +{ + using namespace simple; + JacobianFactor factor(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); + RegularJacobianFactor regularFactor(terms, b, noise); + + // we compute gradient at zero from the standard Jacobian + VectorValues expectedGradientAtZero = factor.gradientAtZero(); + + // we compare against the Raw memory access implementation of gradientAtZero + double actualValue[9]={0}; + regularFactor.gradientAtZero(actualValue); + VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim); + EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw)); + + // One more factor + using namespace simple2; + JacobianFactor factor2(terms2[0].first, terms2[0].second, + terms2[1].first, terms2[1].second, terms2[2].first, terms2[2].second, b2, noise); + RegularJacobianFactor regularFactor2(terms2, b2, noise); + + // we accumulate computed gradient at zero from the standard Jacobian + VectorValues expectedGradientAtZero2 = expectedGradientAtZero.add(factor2.gradientAtZero()); + + // we compare against the Raw memory access implementation of gradientAtZero + regularFactor2.gradientAtZero(actualValue); + VectorValues actualGradientAtZeroRaw2 = double2vv(actualValue,nrKeys,fixedDim); + EXPECT(assert_equal(expectedGradientAtZero2, actualGradientAtZeroRaw2)); + +} + +/* ************************************************************************* */ +TEST(RegularJacobian, multiplyHessianAdd) +{ + using namespace simple; + JacobianFactor factor(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); + RegularJacobianFactor regularFactor(terms, b, noise); + + // arbitrary vector X + VectorValues X; + X.insert(0, (Vector(3) << 10.,20.,30.).finished()); + X.insert(1, (Vector(3) << 10.,20.,30.).finished()); + X.insert(2, (Vector(3) << 10.,20.,30.).finished()); + + // arbitrary vector Y + VectorValues Y; + Y.insert(0, (Vector(3) << 10.,10.,10.).finished()); + Y.insert(1, (Vector(3) << 20.,20.,20.).finished()); + Y.insert(2, (Vector(3) << 30.,30.,30.).finished()); + + // multiplyHessianAdd Y += alpha*A'A*X + double alpha = 2.0; + VectorValues expectedMHA = Y; + factor.multiplyHessianAdd(alpha, X, expectedMHA); + + // create data for raw memory access + double XRaw[9]; + vv2double(X, XRaw, nrKeys, fixedDim); + + // test 1st version: multiplyHessianAdd(double alpha, const double* x, double* y) + double actualMHARaw[9]; + vv2double(Y, actualMHARaw, nrKeys, fixedDim); + regularFactor.multiplyHessianAdd(alpha, XRaw, actualMHARaw); + VectorValues actualMHARawVV = double2vv(actualMHARaw,nrKeys,fixedDim); + EXPECT(assert_equal(expectedMHA,actualMHARawVV)); + + // test 2nd version: multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) + double actualMHARaw2[9]; + vv2double(Y, actualMHARaw2, nrKeys, fixedDim); + vector dims; + size_t accumulatedDim = 0; + for (size_t i = 0; i < nrKeys+1; i++){ + dims.push_back(accumulatedDim); + accumulatedDim += fixedDim; + } + regularFactor.multiplyHessianAdd(alpha, XRaw, actualMHARaw2, dims); + VectorValues actualMHARawVV2 = double2vv(actualMHARaw2,nrKeys,fixedDim); + EXPECT(assert_equal(expectedMHA,actualMHARawVV2)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index 2a2e3b40f..9c99628e6 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -69,13 +69,13 @@ TEST (RotateFactor, test) { Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc); f.evaluateError(iRc, actual); EXPECT(assert_equal(expected, actual, 1e-9)); } { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R); f.evaluateError(R, actual); EXPECT(assert_equal(expected, actual, 1e-9)); diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp new file mode 100644 index 000000000..b5ef18842 --- /dev/null +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -0,0 +1,71 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSmartFactorBase.cpp + * @brief Unit tests for testSmartFactorBase Class + * @author Frank Dellaert + * @date Feb 2015 + */ + +#include "../SmartFactorBase.h" +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +#include +#include +class PinholeFactor: public SmartFactorBase, 9> { + virtual double error(const Values& values) const { + return 0.0; + } + virtual boost::shared_ptr linearize( + const Values& values) const { + return boost::shared_ptr(new JacobianFactor()); + } +}; + +TEST(SmartFactorBase, Pinhole) { + PinholeFactor f; + f.add(Point2(), 1, SharedNoiseModel()); + f.add(Point2(), 2, SharedNoiseModel()); + EXPECT_LONGS_EQUAL(2 * 2, f.dim()); +} + +/* ************************************************************************* */ +#include +class StereoFactor: public SmartFactorBase { + virtual double error(const Values& values) const { + return 0.0; + } + virtual boost::shared_ptr linearize( + const Values& values) const { + return boost::shared_ptr(new JacobianFactor()); + } +}; + +TEST(SmartFactorBase, Stereo) { + StereoFactor f; + f.add(StereoPoint2(), 1, SharedNoiseModel()); + f.add(StereoPoint2(), 2, SharedNoiseModel()); + EXPECT_LONGS_EQUAL(2 * 3, f.dim()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 4e4fde3e4..07c49008d 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file TestSmartProjectionPoseFactor.cpp + * @file testSmartProjectionPoseFactor.cpp * @brief Unit tests for ProjectionFactor Class * @author Chris Beall * @author Luca Carlone @@ -20,11 +20,12 @@ #include "../SmartProjectionPoseFactor.h" -#include #include #include -#include +#include +#include #include +#include #include using namespace std; @@ -35,37 +36,38 @@ static bool isDebugTest = false; // make a realistic calibration matrix static double fov = 60; // degrees -static size_t w=640,h=480; +static size_t w = 640, h = 480; -static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); +static Cal3_S2::shared_ptr K(new Cal3_S2(fov, w, h)); static Cal3_S2::shared_ptr K2(new Cal3_S2(1500, 1200, 0, 640, 480)); -static boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); +static boost::shared_ptr Kbundler( + new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); static double rankTol = 1.0; static double linThreshold = -1.0; static bool manageDegeneracy = true; // Create a noise model for the pixel error -static SharedNoiseModel model(noiseModel::Unit::Create(2)); +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(2, 0.1)); // Convenience for named keys using symbol_shorthand::X; using symbol_shorthand::L; // tests data -static Symbol x1('X', 1); -static Symbol x2('X', 2); -static Symbol x3('X', 3); +static Symbol x1('X', 1); +static Symbol x2('X', 2); +static Symbol x3('X', 3); static Key poseKey1(x1); static Point2 measurement1(323.0, 240.0); -static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); -typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionPoseFactor SmartFactorBundler; +typedef SmartProjectionPoseFactor SmartFactor; +typedef SmartProjectionPoseFactor SmartFactorBundler; -void projectToMultipleCameras( - SimpleCamera cam1, SimpleCamera cam2, SimpleCamera cam3, Point3 landmark, - vector& measurements_cam){ +void projectToMultipleCameras(SimpleCamera cam1, SimpleCamera cam2, + SimpleCamera cam3, Point3 landmark, vector& measurements_cam) { Point2 cam1_uv1 = cam1.project(landmark); Point2 cam2_uv1 = cam2.project(landmark); @@ -101,7 +103,8 @@ TEST( SmartProjectionPoseFactor, Constructor4) { TEST( SmartProjectionPoseFactor, ConstructorWithTransform) { bool manageDegeneracy = true; bool enableEPI = false; - SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor1); + SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, + body_P_sensor1); factor1.add(measurement1, poseKey1, model, K); } @@ -117,21 +120,22 @@ TEST( SmartProjectionPoseFactor, Equals ) { } /* *************************************************************************/ -TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ){ +TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ) { // cout << " ************************ SmartProjectionPoseFactor: noisy ****************************" << endl; // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); SimpleCamera level_camera(level_pose, *K2); // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera level_camera_right(level_pose_right, *K2); // landmark ~5 meters infront of camera Point3 landmark(5, 0.5, 1.2); - // 1. Project two landmarks into two cameras and triangulate + // Project two landmarks into two cameras Point2 level_uv = level_camera.project(landmark); Point2 level_uv_right = level_camera_right.project(landmark); @@ -139,93 +143,114 @@ TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ){ values.insert(x1, level_pose); values.insert(x2, level_pose_right); - SmartFactor factor1; - factor1.add(level_uv, x1, model, K); - factor1.add(level_uv_right, x2, model, K); + SmartFactor factor; + factor.add(level_uv, x1, model, K); + factor.add(level_uv_right, x2, model, K); - double actualError = factor1.error(values); + double actualError = factor.error(values); double expectedError = 0.0; EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); - SmartFactor::Cameras cameras = factor1.cameras(values); - double actualError2 = factor1.totalReprojectionError(cameras); + SmartFactor::Cameras cameras = factor.cameras(values); + double actualError2 = factor.totalReprojectionError(cameras); EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); // test vector of errors - //Vector actual = factor1.unwhitenedError(values); + //Vector actual = factor.unwhitenedError(values); //EXPECT(assert_equal(zero(4),actual,1e-8)); + + // Check derivatives + + // Calculate expected derivative for point (easiest to check) + boost::function f = // + boost::bind(&SmartFactor::whitenedError, factor, cameras, _1); + boost::optional point = factor.point(); + CHECK(point); + Matrix expectedE = numericalDerivative11(f, *point); + + // Calculate using computeEP + Matrix actualE, PointCov; + factor.computeEP(actualE, PointCov, cameras); + EXPECT(assert_equal(expectedE, actualE, 1e-7)); + + // Calculate using whitenedError + Matrix F, actualE2; + Vector actualErrors = factor.whitenedError(cameras, *point, F, actualE2); + EXPECT(assert_equal(expectedE, actualE2, 1e-7)); + EXPECT(assert_equal(f(*point), actualErrors, 1e-7)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, noisy ){ +TEST( SmartProjectionPoseFactor, noisy ) { // cout << " ************************ SmartProjectionPoseFactor: noisy ****************************" << endl; // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); SimpleCamera level_camera(level_pose, *K2); // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera level_camera_right(level_pose_right, *K2); // landmark ~5 meters infront of camera Point3 landmark(5, 0.5, 1.2); // 1. Project two landmarks into two cameras and triangulate - Point2 pixelError(0.2,0.2); + Point2 pixelError(0.2, 0.2); Point2 level_uv = level_camera.project(landmark) + pixelError; Point2 level_uv_right = level_camera_right.project(landmark); Values values; values.insert(x1, level_pose); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); values.insert(x2, level_pose_right.compose(noise_pose)); - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(level_uv, x1, model, K); - factor1->add(level_uv_right, x2, model, K); + SmartFactor::shared_ptr factor(new SmartFactor()); + factor->add(level_uv, x1, model, K); + factor->add(level_uv_right, x2, model, K); - double actualError1= factor1->error(values); + double actualError1 = factor->error(values); SmartFactor::shared_ptr factor2(new SmartFactor()); vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); - std::vector< SharedNoiseModel > noises; + vector noises; noises.push_back(model); noises.push_back(model); - std::vector< boost::shared_ptr > Ks; ///< shared pointer to calibration object (one for each camera) + vector > Ks; ///< shared pointer to calibration object (one for each camera) Ks.push_back(K); Ks.push_back(K); - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); factor2->add(measurements, views, noises, Ks); - double actualError2= factor2->error(values); + double actualError2 = factor2->error(values); DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } - /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ +TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K2); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K2); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K2); // three landmarks ~5 meters infront of camera @@ -240,7 +265,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); @@ -263,18 +288,26 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; gttic_(SmartProjectionPoseFactor); @@ -287,25 +320,27 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ // VectorValues delta = GFG->optimize(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3),1e-8)); - if(isDebugTest) tictoc_print_(); + EXPECT(assert_equal(pose3, result.at(x3), 1e-8)); + if (isDebugTest) + tictoc_print_(); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ +TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses - Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0)); - Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); // body poses + Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1, 0, 0)); + Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam1(cameraPose1, *K); // with camera poses SimpleCamera cam2(cameraPose2, *K); SimpleCamera cam3(cameraPose3, *K); // create arbitrary body_Pose_sensor (transforms from sensor to body) - Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // + Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(1, 1, 1)); // Pose3(); // // These are the poses we want to estimate, from camera measurements Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse()); @@ -325,7 +360,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // Create smart factors - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); @@ -335,13 +370,19 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ bool manageDegeneracy = false; bool enableEPI = false; - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI, + sensor_to_body)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI, + sensor_to_body)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI, + sensor_to_body)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -363,12 +404,13 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ double expectedError = 0.0; DOUBLES_EQUAL(expectedError, actualError, 1e-7) - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); Values values; values.insert(x1, bodyPose1); values.insert(x2, bodyPose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, bodyPose3*noise_pose); + values.insert(x3, bodyPose3 * noise_pose); LevenbergMarquardtParams params; Values result; @@ -376,30 +418,175 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ result = optimizer.optimize(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(bodyPose3,result.at(x3))); + EXPECT(assert_equal(bodyPose3, result.at(x3))); + + // Check derivatives + + // Calculate expected derivative for point (easiest to check) + SmartFactor::Cameras cameras = smartFactor1->cameras(values); + boost::function f = // + boost::bind(&SmartFactor::whitenedError, *smartFactor1, cameras, _1); + boost::optional point = smartFactor1->point(); + CHECK(point); + Matrix expectedE = numericalDerivative11(f, *point); + + // Calculate using computeEP + Matrix actualE, PointCov; + smartFactor1->computeEP(actualE, PointCov, cameras); + EXPECT(assert_equal(expectedE, actualE, 1e-7)); + + // Calculate using whitenedError + Matrix F, actualE2; + Vector actualErrors = smartFactor1->whitenedError(cameras, *point, F, + actualE2); + EXPECT(assert_equal(expectedE, actualE2, 1e-7)); + + // TODO the derivatives agree with f, but returned errors are -f :-( + // TODO We should merge the two whitenedErrors functions and make derivatives optional + EXPECT(assert_equal(-f(*point), actualErrors, 1e-7)); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, Factors ) { + + // Default cameras for simple derivatives + Rot3 R; + static Cal3_S2::shared_ptr K(new Cal3_S2(100, 100, 0, 0, 0)); + SimpleCamera cam1(Pose3(R, Point3(0, 0, 0)), *K), cam2( + Pose3(R, Point3(1, 0, 0)), *K); + + // one landmarks 1m in front of camera + Point3 landmark1(0, 0, 10); + + vector measurements_cam1; + + // Project 2 landmarks into 2 cameras + measurements_cam1.push_back(cam1.project(landmark1)); + measurements_cam1.push_back(cam2.project(landmark1)); + + // Create smart factors + std::vector views; + views.push_back(x1); + views.push_back(x2); + + SmartFactor::shared_ptr smartFactor1 = boost::make_shared(); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::Cameras cameras; + cameras.push_back(cam1); + cameras.push_back(cam2); + + // Make sure triangulation works + LONGS_EQUAL(2, smartFactor1->triangulateSafe(cameras)); + CHECK(!smartFactor1->isDegenerate()); + CHECK(!smartFactor1->isPointBehindCamera()); + boost::optional p = smartFactor1->point(); + CHECK(p); + EXPECT(assert_equal(landmark1, *p)); + + // After eliminating the point, A1 and A2 contain 2-rank information on cameras: + Matrix16 A1, A2; + A1 << -1000, 0, 0, 0, 100, 0; + A2 << 1000, 0, 100, 0, -100, 0; + { + // createHessianFactor + Matrix66 G11 = 0.5 * A1.transpose() * A1; + Matrix66 G12 = 0.5 * A1.transpose() * A2; + Matrix66 G22 = 0.5 * A2.transpose() * A2; + + Vector6 g1; + g1.setZero(); + Vector6 g2; + g2.setZero(); + + double f = 0; + + RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); + + boost::shared_ptr > actual = + smartFactor1->createHessianFactor(cameras, 0.0); + CHECK(assert_equal(expected.information(), actual->information(), 1e-8)); + CHECK(assert_equal(expected, *actual, 1e-8)); + } + + { + Matrix26 F1; + F1.setZero(); + F1(0, 1) = -100; + F1(0, 3) = -10; + F1(1, 0) = 100; + F1(1, 4) = -10; + Matrix26 F2; + F2.setZero(); + F2(0, 1) = -101; + F2(0, 3) = -10; + F2(0, 5) = -1; + F2(1, 0) = 100; + F2(1, 2) = 10; + F2(1, 4) = -10; + Matrix43 E; + E.setZero(); + E(0, 0) = 100; + E(1, 1) = 100; + E(2, 0) = 100; + E(2, 2) = 10; + E(3, 1) = 100; + const vector > Fblocks = list_of > // + (make_pair(x1, 10 * F1))(make_pair(x2, 10 * F2)); + Matrix3 P = (E.transpose() * E).inverse(); + Vector4 b; + b.setZero(); + + // createRegularImplicitSchurFactor + RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); + + boost::shared_ptr > actual = + smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); + CHECK(actual); + CHECK(assert_equal(expected, *actual)); + + // createJacobianQFactor + JacobianFactorQ<6, 2> expectedQ(Fblocks, E, P, b); + + boost::shared_ptr > actualQ = + smartFactor1->createJacobianQFactor(cameras, 0.0); + CHECK(actual); + CHECK(assert_equal(expectedQ, *actualQ)); + } + + { + // createJacobianSVDFactor + Vector1 b; + b.setZero(); + double s = sin(M_PI_4); + JacobianFactor expected(x1, s * A1, x2, s * A2, b); + + boost::shared_ptr actual = + smartFactor1->createJacobianSVDFactor(cameras, 0.0); + CHECK(actual); + CHECK(assert_equal(expected, *actual)); + } +} /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ +TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -432,18 +619,27 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, + -0.0313952598), Point3(0.1, -0.1, 1.9)), + values.at(x3))); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; gttic_(SmartProjectionPoseFactor); @@ -453,27 +649,27 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3), 1e-7)); - if(isDebugTest) tictoc_print_(); + EXPECT(assert_equal(pose3, result.at(x3), 1e-7)); + if (isDebugTest) + tictoc_print_(); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, jacobianSVD ){ +TEST( SmartProjectionPoseFactor, jacobianSVD ) { - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -488,13 +684,16 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ){ projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -506,38 +705,39 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); - values.insert(x3, pose3*noise_pose); + values.insert(x3, pose3 * noise_pose); LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3), 1e-8)); + EXPECT(assert_equal(pose3, result.at(x3), 1e-8)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, landmarkDistance ){ +TEST( SmartProjectionPoseFactor, landmarkDistance ) { double excludeLandmarksFutherThanDist = 2; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -552,13 +752,19 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ){ projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -570,40 +776,41 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); - values.insert(x3, pose3*noise_pose); + values.insert(x3, pose3 * noise_pose); // All factors are disabled and pose should remain where it is LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(values.at(x3),result.at(x3))); + EXPECT(assert_equal(values.at(x3), result.at(x3))); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ +TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { double excludeLandmarksFutherThanDist = 1e10; double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -612,29 +819,34 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ Point3 landmark3(3, 0, 3.0); Point3 landmark4(5, -0.5, 1); - vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4; + vector measurements_cam1, measurements_cam2, measurements_cam3, + measurements_cam4; // 1. Project three landmarks into three cameras and triangulate projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); - measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10,10); // add outlier + measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, - JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor3->add(measurements_cam3, views, model, K); - SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor4( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor4->add(measurements_cam4, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -647,7 +859,8 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); @@ -658,25 +871,25 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3, result.at(x3))); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, jacobianQ ){ +TEST( SmartProjectionPoseFactor, jacobianQ ) { - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -691,13 +904,16 @@ TEST( SmartProjectionPoseFactor, jacobianQ ){ projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -709,39 +925,40 @@ TEST( SmartProjectionPoseFactor, jacobianQ ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); - values.insert(x3, pose3*noise_pose); + values.insert(x3, pose3 * noise_pose); LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3), 1e-8)); + EXPECT(assert_equal(pose3, result.at(x3), 1e-8)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){ +TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K2); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K2); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K2); // three landmarks ~5 meters infront of camera @@ -753,60 +970,74 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){ NonlinearFactorGraph graph; // 1. Project three landmarks into three cameras and triangulate - graph.push_back(ProjectionFactor(cam1.project(landmark1), model, x1, L(1), K2)); - graph.push_back(ProjectionFactor(cam2.project(landmark1), model, x2, L(1), K2)); - graph.push_back(ProjectionFactor(cam3.project(landmark1), model, x3, L(1), K2)); + graph.push_back( + ProjectionFactor(cam1.project(landmark1), model, x1, L(1), K2)); + graph.push_back( + ProjectionFactor(cam2.project(landmark1), model, x2, L(1), K2)); + graph.push_back( + ProjectionFactor(cam3.project(landmark1), model, x3, L(1), K2)); - graph.push_back(ProjectionFactor(cam1.project(landmark2), model, x1, L(2), K2)); - graph.push_back(ProjectionFactor(cam2.project(landmark2), model, x2, L(2), K2)); - graph.push_back(ProjectionFactor(cam3.project(landmark2), model, x3, L(2), K2)); + graph.push_back( + ProjectionFactor(cam1.project(landmark2), model, x1, L(2), K2)); + graph.push_back( + ProjectionFactor(cam2.project(landmark2), model, x2, L(2), K2)); + graph.push_back( + ProjectionFactor(cam3.project(landmark2), model, x3, L(2), K2)); - graph.push_back(ProjectionFactor(cam1.project(landmark3), model, x1, L(3), K2)); - graph.push_back(ProjectionFactor(cam2.project(landmark3), model, x2, L(3), K2)); - graph.push_back(ProjectionFactor(cam3.project(landmark3), model, x3, L(3), K2)); + graph.push_back( + ProjectionFactor(cam1.project(landmark3), model, x1, L(3), K2)); + graph.push_back( + ProjectionFactor(cam2.project(landmark3), model, x2, L(3), K2)); + graph.push_back( + ProjectionFactor(cam3.project(landmark3), model, x3, L(3), K2)); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); Values values; values.insert(x1, pose1); values.insert(x2, pose2); - values.insert(x3, pose3* noise_pose); + values.insert(x3, pose3 * noise_pose); values.insert(L(1), landmark1); values.insert(L(2), landmark2); values.insert(L(3), landmark3); - if(isDebugTest) values.at(x3).print("Pose3 before optimization: "); + if (isDebugTest) + values.at(x3).print("Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; LevenbergMarquardtOptimizer optimizer(graph, values, params); Values result = optimizer.optimize(); - if(isDebugTest) result.at(x3).print("Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3), 1e-7)); + if (isDebugTest) + result.at(x3).print("Pose3 after optimization: "); + EXPECT(assert_equal(pose3, result.at(x3), 1e-7)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, CheckHessian){ +TEST( SmartProjectionPoseFactor, CheckHessian) { - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -837,57 +1068,71 @@ TEST( SmartProjectionPoseFactor, CheckHessian){ graph.push_back(smartFactor2); graph.push_back(smartFactor3); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); - boost::shared_ptr hessianFactor1 = smartFactor1->linearize(values); - boost::shared_ptr hessianFactor2 = smartFactor2->linearize(values); - boost::shared_ptr hessianFactor3 = smartFactor3->linearize(values); + boost::shared_ptr hessianFactor1 = smartFactor1->linearize( + values); + boost::shared_ptr hessianFactor2 = smartFactor2->linearize( + values); + boost::shared_ptr hessianFactor3 = smartFactor3->linearize( + values); - Matrix CumulativeInformation = hessianFactor1->information() + hessianFactor2->information() + hessianFactor3->information(); + Matrix CumulativeInformation = hessianFactor1->information() + + hessianFactor2->information() + hessianFactor3->information(); - boost::shared_ptr GaussianGraph = graph.linearize(values); + boost::shared_ptr GaussianGraph = graph.linearize( + values); Matrix GraphInformation = GaussianGraph->hessian().first; // Check Hessian EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8)); - Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + - hessianFactor2->augmentedInformation() + hessianFactor3->augmentedInformation(); + Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + + hessianFactor2->augmentedInformation() + + hessianFactor3->augmentedInformation(); // Check Information vector // cout << AugInformationMatrix.size() << endl; - Vector InfoVector = AugInformationMatrix.block(0,18,18,1); // 18x18 Hessian + information vector + Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector // Check Hessian EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){ +TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) { // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K2); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); SimpleCamera cam2(pose2, *K2); // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); SimpleCamera cam3(pose3, *K2); // three landmarks ~5 meters infront of camera @@ -901,34 +1146,49 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); double rankTol = 50; - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(rankTol, linThreshold, manageDegeneracy)); smartFactor1->add(measurements_cam1, views, model, K2); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(rankTol, linThreshold, manageDegeneracy)); smartFactor2->add(measurements_cam2, views, model, K2); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = gtsam::Point3(0,0,1); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, + 0.10); + Point3 positionPrior = Point3(0, 0, 1); NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.push_back( + PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); + graph.push_back( + PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); - values.insert(x2, pose2*noise_pose); + values.insert(x2, pose2 * noise_pose); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0.154256096, -0.632754061, 0.75883289, -0.753276814, + -0.572308662, -0.324093872, 0.639358349, -0.521617766, + -0.564921063), + Point3(0.145118171, -0.252907438, 0.819956033)), + values.at(x3))); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; gttic_(SmartProjectionPoseFactor); @@ -938,31 +1198,40 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; - // EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); + cout + << "TEST COMMENTED: rotation only version of smart factors has been deprecated " + << endl; + EXPECT( + assert_equal( + Pose3( + Rot3(0.154256096, -0.632754061, 0.75883289, -0.753276814, + -0.572308662, -0.324093872, 0.639358349, -0.521617766, + -0.564921063), + Point3(0.145118171, -0.252907438, 0.819956033)), + result.at(x3))); + if (isDebugTest) + tictoc_print_(); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ +TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) { // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -979,39 +1248,55 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ double rankTol = 10; - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(rankTol, linThreshold, manageDegeneracy)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(rankTol, linThreshold, manageDegeneracy)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(rankTol, linThreshold, manageDegeneracy)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = gtsam::Point3(0,0,1); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, + 0.10); + Point3 positionPrior = Point3(0, 0, 1); NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.push_back( + PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); + graph.push_back( + PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; gttic_(SmartProjectionPoseFactor); @@ -1021,26 +1306,35 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; - // EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); + cout + << "TEST COMMENTED: rotation only version of smart factors has been deprecated " + << endl; + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + result.at(x3))); + if (isDebugTest) + tictoc_print_(); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Hessian ){ +TEST( SmartProjectionPoseFactor, Hessian ) { // cout << " ************************ SmartProjectionPoseFactor: Hessian **********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K2); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K2); // three landmarks ~5 meters infront of camera @@ -1054,15 +1348,18 @@ TEST( SmartProjectionPoseFactor, Hessian ){ measurements_cam1.push_back(cam2_uv1); SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1,views, model, K2); + smartFactor1->add(measurements_cam1, views, model, K2); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); Values values; values.insert(x1, pose1); values.insert(x2, pose2); - boost::shared_ptr hessianFactor = smartFactor1->linearize(values); - if(isDebugTest) hessianFactor->print("Hessian factor \n"); + boost::shared_ptr hessianFactor = smartFactor1->linearize( + values); + if (isDebugTest) + hessianFactor->print("Hessian factor \n"); // compute triangulation from linearization point // compute reprojection errors (sum squared) @@ -1070,26 +1367,25 @@ TEST( SmartProjectionPoseFactor, Hessian ){ // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] } - /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, HessianWithRotation ){ +TEST( SmartProjectionPoseFactor, HessianWithRotation ) { // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian **********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); Point3 landmark1(5, 0.5, 1.2); @@ -1106,54 +1402,62 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ){ values.insert(x2, pose2); values.insert(x3, pose3); - boost::shared_ptr hessianFactor = smartFactorInstance->linearize(values); + boost::shared_ptr hessianFactor = + smartFactorInstance->linearize(values); // hessianFactor->print("Hessian factor \n"); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(pose1)); rotValues.insert(x2, poseDrift.compose(pose2)); rotValues.insert(x3, poseDrift.compose(pose3)); - boost::shared_ptr hessianFactorRot = smartFactorInstance->linearize(rotValues); + boost::shared_ptr hessianFactorRot = + smartFactorInstance->linearize(rotValues); // hessianFactorRot->print("Hessian factor \n"); // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRot->information(), 1e-7)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Point3(10, -4, 5)); Values tranValues; tranValues.insert(x1, poseDrift2.compose(pose1)); tranValues.insert(x2, poseDrift2.compose(pose2)); tranValues.insert(x3, poseDrift2.compose(pose3)); - boost::shared_ptr hessianFactorRotTran = smartFactorInstance->linearize(tranValues); + boost::shared_ptr hessianFactorRotTran = + smartFactorInstance->linearize(tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRotTran->information(), 1e-7)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ){ +TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K2); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0, 0, 0)); SimpleCamera cam2(pose2, *K2); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,0,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, 0, 0)); SimpleCamera cam3(pose3, *K2); Point3 landmark1(5, 0.5, 1.2); @@ -1165,62 +1469,72 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ){ SmartFactor::shared_ptr smartFactor(new SmartFactor()); smartFactor->add(measurements_cam1, views, model, K2); - Values values; values.insert(x1, pose1); values.insert(x2, pose2); values.insert(x3, pose3); - boost::shared_ptr hessianFactor = smartFactor->linearize(values); - if(isDebugTest) hessianFactor->print("Hessian factor \n"); + boost::shared_ptr hessianFactor = smartFactor->linearize( + values); + if (isDebugTest) + hessianFactor->print("Hessian factor \n"); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(pose1)); rotValues.insert(x2, poseDrift.compose(pose2)); rotValues.insert(x3, poseDrift.compose(pose3)); - boost::shared_ptr hessianFactorRot = smartFactor->linearize(rotValues); - if(isDebugTest) hessianFactorRot->print("Hessian factor \n"); + boost::shared_ptr hessianFactorRot = smartFactor->linearize( + rotValues); + if (isDebugTest) + hessianFactorRot->print("Hessian factor \n"); // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRot->information(), 1e-8)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Point3(10, -4, 5)); Values tranValues; tranValues.insert(x1, poseDrift2.compose(pose1)); tranValues.insert(x2, poseDrift2.compose(pose2)); tranValues.insert(x3, poseDrift2.compose(pose3)); - boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); + boost::shared_ptr hessianFactorRotTran = + smartFactor->linearize(tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRotTran->information(), 1e-8)); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { - SmartProjectionPoseFactor factor1(rankTol, linThreshold); - boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); - factor1.add(measurement1, poseKey1, model, Kbundler); + SmartFactorBundler factor(rankTol, linThreshold); + boost::shared_ptr Kbundler( + new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); + factor.add(measurement1, poseKey1, model, Kbundler); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Cal3Bundler ){ +TEST( SmartProjectionPoseFactor, Cal3Bundler ) { // cout << " ************************ SmartProjectionPoseFactor: Cal3Bundler **********************" << endl; // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); PinholeCamera cam1(pose1, *Kbundler); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); PinholeCamera cam2(pose2, *Kbundler); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); PinholeCamera cam3(pose3, *Kbundler); // three landmarks ~5 meters infront of camera @@ -1252,7 +1566,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ){ measurements_cam3.push_back(cam2_uv3); measurements_cam3.push_back(cam3_uv3); - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); @@ -1275,18 +1589,25 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); - + values.insert(x3, pose3 * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; gttic_(SmartProjectionPoseFactor); @@ -1295,30 +1616,29 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ){ gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); - // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3), 1e-6)); - if(isDebugTest) tictoc_print_(); - } + EXPECT(assert_equal(pose3, result.at(x3), 1e-6)); + if (isDebugTest) + tictoc_print_(); +} /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ){ +TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); PinholeCamera cam1(pose1, *Kbundler); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); PinholeCamera cam2(pose2, *Kbundler); // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); PinholeCamera cam3(pose3, *Kbundler); // three landmarks ~5 meters infront of camera @@ -1352,39 +1672,55 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ){ double rankTol = 10; - SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); + SmartFactorBundler::shared_ptr smartFactor1( + new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); smartFactor1->add(measurements_cam1, views, model, Kbundler); - SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); + SmartFactorBundler::shared_ptr smartFactor2( + new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); smartFactor2->add(measurements_cam2, views, model, Kbundler); - SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); + SmartFactorBundler::shared_ptr smartFactor3( + new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); smartFactor3->add(measurements_cam3, views, model, Kbundler); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = gtsam::Point3(0,0,1); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, + 0.10); + Point3 positionPrior = Point3(0, 0, 1); NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.push_back( + PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); + graph.push_back( + PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; gttic_(SmartProjectionPoseFactor); @@ -1394,14 +1730,26 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ){ tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); + cout + << "TEST COMMENTED: rotation only version of smart factors has been deprecated " + << endl; // EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); + if (isDebugTest) + tictoc_print_(); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ - diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp new file mode 100644 index 000000000..6b79171df --- /dev/null +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -0,0 +1,70 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * testTriangulationFactor.cpp + * + * Created on: July 30th, 2013 + * Author: cbeall3 + */ + +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +// Some common constants +static const boost::shared_ptr sharedCal = // + boost::make_shared(1500, 1200, 0, 640, 480); + +// Looking along X-axis, 1 meter above ground plane (x-y) +static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2); +static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); +PinholeCamera camera1(pose1, *sharedCal); + +// landmark ~5 meters infront of camera +static const Point3 landmark(5, 0.5, 1.2); + +// 1. Project two landmarks into two cameras and triangulate +Point2 z1 = camera1.project(landmark); + +//****************************************************************************** +TEST( triangulation, TriangulationFactor ) { + // Create the factor with a measurement that is 3 pixels off in x + Key pointKey(1); + SharedNoiseModel model; + typedef TriangulationFactor<> Factor; + Factor factor(camera1, z1, model, pointKey); + + // Use the factor to calculate the Jacobians + Matrix HActual; + factor.evaluateError(landmark, HActual); + + Matrix HExpected = numericalDerivative11( + boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); + + // Verify the Jacobians are correct + CHECK(assert_equal(HExpected, HActual, 1e-3)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index 2e59a98a4..4db265036 100644 --- a/gtsam/symbolic/SymbolicBayesNet.h +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -76,9 +76,14 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; -} // namespace gtsam + /// traits + template<> + struct traits : public Testable { + }; + +} //\ namespace gtsam diff --git a/gtsam/symbolic/SymbolicBayesTree.h b/gtsam/symbolic/SymbolicBayesTree.h index 526120a30..c4f09c71c 100644 --- a/gtsam/symbolic/SymbolicBayesTree.h +++ b/gtsam/symbolic/SymbolicBayesTree.h @@ -65,9 +65,14 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; -} +/// traits +template<> struct traits : public Testable {}; +template<> struct traits : public Testable {}; + +} //\ namespace gtsam + diff --git a/gtsam/symbolic/SymbolicConditional.h b/gtsam/symbolic/SymbolicConditional.h index 01f7366d6..0530af556 100644 --- a/gtsam/symbolic/SymbolicConditional.h +++ b/gtsam/symbolic/SymbolicConditional.h @@ -17,9 +17,10 @@ #pragma once -#include -#include #include +#include +#include +#include namespace gtsam { @@ -115,10 +116,15 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseFactor); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional); } }; -} +/// traits +template<> +struct traits : public Testable { +}; + +} //\ namespace gtsam diff --git a/gtsam/symbolic/SymbolicEliminationTree.h b/gtsam/symbolic/SymbolicEliminationTree.h index 5a44d451d..e20fb67bd 100644 --- a/gtsam/symbolic/SymbolicEliminationTree.h +++ b/gtsam/symbolic/SymbolicEliminationTree.h @@ -59,4 +59,9 @@ namespace gtsam { }; -} +/// traits +template<> +struct traits : public Testable { +}; + +} //\ namespace gtsam diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h index 0054f9fbc..aca1ba043 100644 --- a/gtsam/symbolic/SymbolicFactor.h +++ b/gtsam/symbolic/SymbolicFactor.h @@ -17,13 +17,15 @@ #pragma once -#include +#include +#include +#include + #include #include #include -#include -#include +#include namespace gtsam { @@ -142,7 +144,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; // IndexFactor @@ -158,4 +160,10 @@ namespace gtsam { GTSAM_EXPORT std::pair, boost::shared_ptr > EliminateSymbolic(const SymbolicFactorGraph& factors, const Ordering& keys); -} + /// traits + template<> + struct traits : public Testable { + }; + +} //\ namespace gtsam + diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index 37aae2400..0a5427ba3 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -111,9 +111,14 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; -} // namespace gtsam +/// traits +template<> +struct traits : public Testable { +}; + +} //\ namespace gtsam diff --git a/gtsam_extra.cmake.in b/gtsam_extra.cmake.in index 89a97d51b..781b08d57 100644 --- a/gtsam_extra.cmake.in +++ b/gtsam_extra.cmake.in @@ -13,6 +13,10 @@ if("@GTSAM_USE_TBB@") list(APPEND GTSAM_INCLUDE_DIR "@TBB_INCLUDE_DIRS@") endif() +# Append Eigen include path, set in top-level CMakeLists.txt to either +# system-eigen, or GTSAM eigen path +list(APPEND GTSAM_INCLUDE_DIR "@GTSAM_EIGEN_INCLUDE_PREFIX@") + if("@GTSAM_USE_EIGEN_MKL@") list(APPEND GTSAM_INCLUDE_DIR "@MKL_INCLUDE_DIR@") endif() diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index cd42b8d4f..6abfe4336 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -3,6 +3,7 @@ set (gtsam_unstable_subdirs base geometry + linear discrete dynamics nonlinear @@ -47,6 +48,7 @@ endforeach(subdir) set(gtsam_unstable_srcs ${base_srcs} ${geometry_srcs} + ${linear_srcs} ${discrete_srcs} ${dynamics_srcs} ${nonlinear_srcs} diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 1c35fc9b8..66538e802 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -75,7 +75,7 @@ public: // access const Vector3& gyro() const { return gyro_; } const Vector3& accel() const { return accel_; } - Vector6 z() const { return (Vector6() << accel_, gyro_); } + Vector6 z() const { return (Vector(6) << accel_, gyro_).finished(); } /** * Error evaluation with optional derivatives - calculates @@ -100,7 +100,7 @@ public: boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { assert(false); - return zero(x1.dim()); + return Vector6::Zero(); } private: diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index 73fc58fa6..5ed079acb 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -90,7 +90,7 @@ public: boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { assert(false); // no corresponding factor here - return zero(x1.dim()); + return Vector6::Zero(); } private: diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 06aa4ac24..942e1ab55 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -3,18 +3,15 @@ * @author Alex Cunningham */ -#include -#include -#include -#include - #include +#include +#include namespace gtsam { using namespace std; -static const Vector g = delta(3, 2, 9.81); +static const Vector kGravity = delta(3, 2, 9.81); /* ************************************************************************* */ double bound(double a, double min, double max) { @@ -24,28 +21,30 @@ double bound(double a, double min, double max) { } /* ************************************************************************* */ -PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, - double vx, double vy, double vz) -: Rt_(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)), v_(vx, vy, vz) {} +PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y, + double z, double vx, double vy, double vz) : + Base(Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)), + Velocity3(vx, vy, vz)) { +} /* ************************************************************************* */ -PoseRTV::PoseRTV(const Vector& rtv) -: Rt_(Rot3::RzRyRx(rtv.head(3)), Point3(rtv.segment(3, 3))), v_(rtv.tail(3)) -{ +PoseRTV::PoseRTV(const Vector& rtv) : + Base(Pose3(Rot3::RzRyRx(rtv.head(3)), Point3(rtv.segment(3, 3))), + Velocity3(rtv.tail(3))) { } /* ************************************************************************* */ Vector PoseRTV::vector() const { Vector rtv(9); - rtv.head(3) = Rt_.rotation().xyz(); - rtv.segment(3,3) = Rt_.translation().vector(); - rtv.tail(3) = v_.vector(); + rtv.head(3) = rotation().xyz(); + rtv.segment(3,3) = translation().vector(); + rtv.tail(3) = velocity().vector(); return rtv; } /* ************************************************************************* */ bool PoseRTV::equals(const PoseRTV& other, double tol) const { - return Rt_.equals(other.Rt_, tol) && v_.equals(other.v_, tol); + return pose().equals(other.pose(), tol) && velocity().equals(other.velocity(), tol); } /* ************************************************************************* */ @@ -53,66 +52,7 @@ void PoseRTV::print(const string& s) const { cout << s << ":" << endl; gtsam::print((Vector)R().xyz(), " R:rpy"); t().print(" T"); - v_.print(" V"); -} - -/* ************************************************************************* */ -PoseRTV PoseRTV::Expmap(const Vector9& v) { - Pose3 newPose = Pose3::Expmap(v.head<6>()); - Velocity3 newVel = Velocity3::Expmap(v.tail<3>()); - return PoseRTV(newPose, newVel); -} - -/* ************************************************************************* */ -Vector9 PoseRTV::Logmap(const PoseRTV& p) { - Vector6 Lx = Pose3::Logmap(p.Rt_); - Vector3 Lv = Velocity3::Logmap(p.v_); - return (Vector9() << Lx, Lv).finished(); -} - -/* ************************************************************************* */ -PoseRTV PoseRTV::retract(const Vector& v) const { - assert(v.size() == 9); - // First order approximation - Pose3 newPose = Rt_.retract(sub(v, 0, 6)); - Velocity3 newVel = v_ + Rt_.rotation() * Point3(sub(v, 6, 9)); - return PoseRTV(newPose, newVel); -} - -/* ************************************************************************* */ -Vector PoseRTV::localCoordinates(const PoseRTV& p1) const { - const Pose3& x0 = pose(), &x1 = p1.pose(); - // First order approximation - Vector6 poseLogmap = x0.localCoordinates(x1); - Vector3 lv = rotation().unrotate(p1.velocity() - v_).vector(); - return (Vector(9) << poseLogmap, lv).finished(); -} - -/* ************************************************************************* */ -PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); } -PoseRTV PoseRTV::inverse(boost::optional H1) const { - if (H1) *H1 = numericalDerivative11(inverse_, *this, 1e-5); - return PoseRTV(Rt_.inverse(), v_.inverse()); -} - -/* ************************************************************************* */ -PoseRTV compose_(const PoseRTV& p1, const PoseRTV& p2) { return p1.compose(p2); } -PoseRTV PoseRTV::compose(const PoseRTV& p, - boost::optional H1, - boost::optional H2) const { - if (H1) *H1 = numericalDerivative21(compose_, *this, p, 1e-5); - if (H2) *H2 = numericalDerivative22(compose_, *this, p, 1e-5); - return PoseRTV(Rt_.compose(p.Rt_), v_.compose(p.v_)); -} - -/* ************************************************************************* */ -PoseRTV between_(const PoseRTV& p1, const PoseRTV& p2) { return p1.between(p2); } -PoseRTV PoseRTV::between(const PoseRTV& p, - boost::optional H1, - boost::optional H2) const { - if (H1) *H1 = numericalDerivative21(between_, *this, p, 1e-5); - if (H2) *H2 = numericalDerivative22(between_, *this, p, 1e-5); - return inverse().compose(p); + velocity().print(" V"); } /* ************************************************************************* */ @@ -180,7 +120,7 @@ PoseRTV PoseRTV::generalDynamics( Rot3 r2 = rotation().retract(gyro * dt); // Integrate Velocity Equations - Velocity3 v2 = v_.compose(Velocity3(dt * (r2.matrix() * accel + g))); + Velocity3 v2 = velocity() + Velocity3(dt * (r2.matrix() * accel + kGravity)); // Integrate Position Equations Point3 t2 = translationIntegration(r2, v2, dt); @@ -197,15 +137,15 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { Vector6 imu; // acceleration - Vector accel = v1.localCoordinates(v2) / dt; - imu.head<3>() = r2.transpose() * (accel - g); + Vector3 accel = (v2-v1).vector() / dt; + imu.head<3>() = r2.transpose() * (accel - kGravity); // rotation rates // just using euler angles based on matlab code // FIXME: this is silly - we shouldn't use differences in Euler angles Matrix Enb = RRTMnb(r1); - Vector euler1 = r1.xyz(), euler2 = r2.xyz(); - Vector dR = euler2 - euler1; + Vector3 euler1 = r1.xyz(), euler2 = r2.xyz(); + Vector3 dR = euler2 - euler1; // normalize yaw in difference (as per Mitch's code) dR(2) = Rot2::fromAngle(dR(2)).theta(); @@ -225,55 +165,40 @@ Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, doub } /* ************************************************************************* */ -double range_(const PoseRTV& A, const PoseRTV& B) { return A.range(B); } double PoseRTV::range(const PoseRTV& other, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = numericalDerivative21(range_, *this, other, 1e-5); - if (H2) *H2 = numericalDerivative22(range_, *this, other, 1e-5); - return t().distance(other.t()); + OptionalJacobian<1,9> H1, OptionalJacobian<1,9> H2) const { + Matrix36 D_t1_pose, D_t2_other; + const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0); + const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0); + Matrix13 D_d_t1, D_d_t2; + double d = t1.distance(t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0); + if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0; + if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0; + return d; } /* ************************************************************************* */ -PoseRTV transformed_from_(const PoseRTV& global, const Pose3& transform) { - return global.transformed_from(transform); -} - -PoseRTV PoseRTV::transformed_from(const Pose3& trans, - boost::optional Dglobal, - boost::optional Dtrans) const { - // Note that we rotate the velocity - Matrix DVr, DTt; - Velocity3 newvel = trans.rotation().rotate(v_, DVr, DTt); - if (!Dglobal && !Dtrans) - return PoseRTV(trans.compose(pose()), newvel); +PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal, + OptionalJacobian<9, 6> Dtrans) const { // Pose3 transform is just compose - Matrix DTc, DGc; - Pose3 newpose = trans.compose(pose(), DTc, DGc); + Matrix6 D_newpose_trans, D_newpose_pose; + Pose3 newpose = trans.compose(pose(), D_newpose_trans, D_newpose_pose); + + // Note that we rotate the velocity + Matrix3 D_newvel_R, D_newvel_v; + Velocity3 newvel = trans.rotation().rotate(velocity(), D_newvel_R, D_newvel_v); if (Dglobal) { - *Dglobal = zeros(9,9); - insertSub(*Dglobal, DGc, 0, 0); - - // Rotate velocity - insertSub(*Dglobal, eye(3,3), 6, 6); // FIXME: should this actually be an identity matrix? + Dglobal->setZero(); + Dglobal->topLeftCorner<6,6>() = D_newpose_pose; + Dglobal->bottomRightCorner<3,3>() = D_newvel_v; } if (Dtrans) { - *Dtrans = numericalDerivative22(transformed_from_, *this, trans, 1e-8); - // - // *Dtrans = zeros(9,6); - // // directly affecting the pose - // insertSub(*Dtrans, DTc, 0, 0); // correct in tests - // - // // rotating the velocity - // Matrix vRhat = skewSymmetric(-v_.x(), -v_.y(), -v_.z()); - // trans.rotation().print("Transform rotation"); - // gtsam::print(vRhat, "vRhat"); - // gtsam::print(DVr, "DVr"); - // // FIXME: find analytic derivative - //// insertSub(*Dtrans, vRhat, 6, 0); // works if PoseRTV.rotation() = I - //// insertSub(*Dtrans, trans.rotation().matrix() * vRhat, 6, 0); // FAIL: both tests fail + Dtrans->setZero(); + Dtrans->topLeftCorner<6,6>() = D_newpose_trans; + Dtrans->bottomLeftCorner<3,3>() = D_newvel_R; } return PoseRTV(newpose, newvel); } diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index 43d018752..78bd1fe6f 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -7,8 +7,8 @@ #pragma once #include -#include #include +#include namespace gtsam { @@ -19,25 +19,30 @@ typedef Point3 Velocity3; * Robot state for use with IMU measurements * - contains translation, translational velocity and rotation */ -class GTSAM_UNSTABLE_EXPORT PoseRTV { +class GTSAM_UNSTABLE_EXPORT PoseRTV : public ProductLieGroup { protected: - Pose3 Rt_; - Velocity3 v_; + typedef ProductLieGroup Base; + typedef OptionalJacobian<9, 9> ChartJacobian; public: + // constructors - with partial versions PoseRTV() {} - PoseRTV(const Point3& pt, const Rot3& rot, const Velocity3& vel) - : Rt_(rot, pt), v_(vel) {} - PoseRTV(const Rot3& rot, const Point3& pt, const Velocity3& vel) - : Rt_(rot, pt), v_(vel) {} - explicit PoseRTV(const Point3& pt) - : Rt_(Rot3::identity(), pt) {} + PoseRTV(const Point3& t, const Rot3& rot, const Velocity3& vel) + : Base(Pose3(rot, t), vel) {} + PoseRTV(const Rot3& rot, const Point3& t, const Velocity3& vel) + : Base(Pose3(rot, t), vel) {} + explicit PoseRTV(const Point3& t) + : Base(Pose3(Rot3(), t),Velocity3()) {} PoseRTV(const Pose3& pose, const Velocity3& vel) - : Rt_(pose), v_(vel) {} + : Base(pose, vel) {} explicit PoseRTV(const Pose3& pose) - : Rt_(pose) {} + : Base(pose,Velocity3()) {} + + // Construct from Base + PoseRTV(const Base& base) + : Base(base) {} /** build from components - useful for data files */ PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, @@ -47,71 +52,46 @@ public: explicit PoseRTV(const Vector& v); // access - const Point3& t() const { return Rt_.translation(); } - const Rot3& R() const { return Rt_.rotation(); } - const Velocity3& v() const { return v_; } - const Pose3& pose() const { return Rt_; } + const Pose3& pose() const { return first; } + const Velocity3& v() const { return second; } + const Point3& t() const { return pose().translation(); } + const Rot3& R() const { return pose().rotation(); } // longer function names - const Point3& translation() const { return Rt_.translation(); } - const Rot3& rotation() const { return Rt_.rotation(); } - const Velocity3& velocity() const { return v_; } + const Point3& translation() const { return pose().translation(); } + const Rot3& rotation() const { return pose().rotation(); } + const Velocity3& velocity() const { return second; } // Access to vector for ease of use with Matlab // and avoidance of Point3 Vector vector() const; - Vector translationVec() const { return Rt_.translation().vector(); } - Vector velocityVec() const { return v_.vector(); } + Vector translationVec() const { return pose().translation().vector(); } + Vector velocityVec() const { return velocity().vector(); } // testable bool equals(const PoseRTV& other, double tol=1e-6) const; void print(const std::string& s="") const; - // Manifold - static size_t Dim() { return 9; } - size_t dim() const { return Dim(); } + /// @name Manifold + /// @{ + using Base::dimension; + using Base::dim; + using Base::Dim; + using Base::retract; + using Base::localCoordinates; + /// @} - /** - * retract/unretract assume independence of components - * Tangent space parameterization: - * - v(0-2): Rot3 (roll, pitch, yaw) - * - v(3-5): Point3 - * - v(6-8): Translational velocity - */ - PoseRTV retract(const Vector& v) const; - Vector localCoordinates(const PoseRTV& p) const; + /// @name measurement functions + /// @{ - // Lie - /** - * expmap/logmap are poor approximations that assume independence of components - * Currently implemented using the poor retract/unretract approximations - */ - static PoseRTV Expmap(const Vector9& v); - static Vector9 Logmap(const PoseRTV& p); - - static PoseRTV identity() { return PoseRTV(); } - - /** Derivatives calculated numerically */ - PoseRTV inverse(boost::optional H1=boost::none) const; - - /** Derivatives calculated numerically */ - PoseRTV compose(const PoseRTV& p, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; - - - /** Derivatives calculated numerically */ - PoseRTV between(const PoseRTV& p, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; - - // measurement functions - /** Derivatives calculated numerically */ + /** range between translations */ double range(const PoseRTV& other, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<1,9> H1=boost::none, + OptionalJacobian<1,9> H2=boost::none) const; + /// @} - // IMU-specific + /// @name IMU-specific + /// @{ /// Dynamics integrator for ground robots /// Always move from time 1 to time 2 @@ -156,10 +136,12 @@ public: * Note: the transform jacobian convention is flipped */ PoseRTV transformed_from(const Pose3& trans, - boost::optional Dglobal=boost::none, - boost::optional Dtrans=boost::none) const; + ChartJacobian Dglobal = boost::none, + OptionalJacobian<9, 6> Dtrans = boost::none) const; - // Utility functions + /// @} + /// @name Utility functions + /// @{ /// RRTMbn - Function computes the rotation rate transformation matrix from /// body axis rates to euler angle (global) rates @@ -172,34 +154,20 @@ public: static Matrix RRTMnb(const Vector& euler); static Matrix RRTMnb(const Rot3& att); + /// @} private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(Rt_); - ar & BOOST_SERIALIZATION_NVP(v_); + void serialize(Archive & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(first); + ar & BOOST_SERIALIZATION_NVP(second); } }; -// Define GTSAM traits -namespace traits { template<> -struct is_manifold : public boost::true_type { -}; +struct traits : public internal::LieGroup {}; -template<> -struct dimension : public boost::integral_constant { -}; - -template<> -struct zero { - static PoseRTV value() { - return PoseRTV(); - } -}; - -} } // \namespace gtsam diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index b1c254a9e..a3dd6327b 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -7,9 +7,10 @@ #pragma once -#include -#include #include +#include +#include +#include namespace gtsam { @@ -29,7 +30,7 @@ class Reconstruction : public NoiseModelFactor3 { typedef NoiseModelFactor3 Base; public: Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) : - Base(noiseModel::Constrained::All(Pose3::Dim(), fabs(mu)), gKey1, gKey, + Base(noiseModel::Constrained::All(6, std::abs(mu)), gKey1, gKey, xiKey), h_(h) { } virtual ~Reconstruction() {} @@ -45,27 +46,26 @@ public: boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - Matrix D_gkxi_gk, D_gkxi_exphxi; - Pose3 gkxi = gk.compose(Pose3::Expmap(h_*xik), D_gkxi_gk, D_gkxi_exphxi); + Matrix6 D_exphxi_xi; + Pose3 exphxi = Pose3::Expmap(h_ * xik, H3 ? &D_exphxi_xi : 0); - Matrix D_hx_gk1, D_hx_gkxi; - Pose3 hx = gkxi.between(gk1, D_hx_gkxi, D_hx_gk1); + Matrix6 D_gkxi_gk, D_gkxi_exphxi; + Pose3 gkxi = gk.compose(exphxi, D_gkxi_gk, H3 ? &D_gkxi_exphxi : 0); - if (H1) { - *H1 = D_hx_gk1; - } - if (H2) { - Matrix D_hx_gk = D_hx_gkxi * D_gkxi_gk; - *H2 = D_hx_gk; + Matrix6 D_hx_gk1, D_hx_gkxi; + Pose3 hx = gkxi.between(gk1, (H2 || H3) ? &D_hx_gkxi : 0, H1 ? &D_hx_gk1 : 0); + + Matrix6 D_log_hx; + Vector error = Pose3::Logmap(hx, D_log_hx); + + if (H1) *H1 = D_log_hx * D_hx_gk1; + if (H2 || H3) { + Matrix6 D_log_gkxi = D_log_hx * D_hx_gkxi; + if (H2) *H2 = D_log_gkxi * D_gkxi_gk; + if (H3) *H3 = D_log_gkxi * D_gkxi_exphxi * D_exphxi_xi * h_; } - if (H3) { - Matrix D_exphxi_xi = Pose3::dExpInv_exp(h_*xik)*h_; - Matrix D_hx_xi = D_hx_gkxi * D_gkxi_exphxi * D_exphxi_xi; - *H3 = D_hx_xi; - } - - return Pose3::Logmap(hx); + return error; } }; @@ -92,7 +92,7 @@ public: DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey, double h, const Matrix& Inertia, const Vector& Fu, double m, double mu = 1000.0) : - Base(noiseModel::Constrained::All(Pose3::Dim(), fabs(mu)), xiKey1, xiKey_1, gKey), + Base(noiseModel::Constrained::All(6, std::abs(mu)), xiKey1, xiKey_1, gKey), h_(h), Inertia_(Inertia), Fu_(Fu), m_(m) { } virtual ~DiscreteEulerPoincareHelicopter() {} diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index a92ae0426..9ecc7619e 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -52,7 +52,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor3", boost::serialization::base_object(*this)); } diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index 56850a0fb..d97f185e7 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -56,9 +56,9 @@ TEST( testIMUSystem, optimize_chain ) { // create a simple chain of poses to generate IMU measurements const double dt = 1.0; PoseRTV pose1, - pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), (Vector)Vector3(2.0, 2.0, 0.0)), - pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), (Vector)Vector3(0.0, 0.0, 0.0)), - pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), (Vector)Vector3(2.0, 2.0, 0.0)); + pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Point3(2.0, 2.0, 0.0)), + pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Point3(0.0, 0.0, 0.0)), + pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Point3(2.0, 2.0, 0.0)); // create measurements SharedDiagonal model = noiseModel::Unit::Create(6); @@ -102,9 +102,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) { // create a simple chain of poses to generate IMU measurements const double dt = 1.0; PoseRTV pose1, - pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)Vector3(1.0, 0.0, 0.0)), - pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)Vector3(1.0, 0.0, 0.0)), - pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)Vector3(1.0, 0.0, 0.0)); + pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0)), + pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0)), + pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0)); // create measurements SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0); diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index 0261257be..2ea582292 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -76,11 +76,12 @@ TEST( testPoseRTV, equals ) { /* ************************************************************************* */ TEST( testPoseRTV, Lie ) { // origin and zero deltas - EXPECT(assert_equal(PoseRTV(), PoseRTV().retract(zero(9)), tol)); - EXPECT(assert_equal(zero(9), PoseRTV().localCoordinates(PoseRTV()), tol)); + PoseRTV identity; + EXPECT(assert_equal(identity, (PoseRTV)identity.retract(zero(9)), tol)); + EXPECT(assert_equal(zero(9), identity.localCoordinates(identity), tol)); PoseRTV state1(pt, rot, vel); - EXPECT(assert_equal(state1, state1.retract(zero(9)), tol)); + EXPECT(assert_equal(state1, (PoseRTV)state1.retract(zero(9)), tol)); EXPECT(assert_equal(zero(9), state1.localCoordinates(state1), tol)); Vector delta = (Vector(9) << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3).finished(); @@ -88,9 +89,9 @@ TEST( testPoseRTV, Lie ) { Point3 pt2 = pt + rot * Point3(0.2, 0.3, 0.4); Velocity3 vel2 = vel + rot * Velocity3(-0.1,-0.2,-0.3); PoseRTV state2(pt2, rot2, vel2); - EXPECT(assert_equal(state2, state1.retract(delta), 1e-1)); + EXPECT(assert_equal(state2, (PoseRTV)state1.retract(delta), 1e-1)); EXPECT(assert_equal(delta, state1.localCoordinates(state2), 1e-1)); - EXPECT(assert_equal(-delta, state2.localCoordinates(state1), 1e-1)); // loose tolerance due to retract approximation + EXPECT(assert_equal(delta, -state2.localCoordinates(state1), 1e-1)); } /* ************************************************************************* */ @@ -119,6 +120,43 @@ TEST( testPoseRTV, dynamics_identities ) { } +/* ************************************************************************* */ +PoseRTV compose_proxy(const PoseRTV& A, const PoseRTV& B) { return A.compose(B); } +TEST( testPoseRTV, compose ) { + PoseRTV state1(pt, rot, vel), state2 = state1; + + Matrix actH1, actH2; + state1.compose(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2); + Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, tol)); + EXPECT(assert_equal(numericH2, actH2, tol)); +} + +/* ************************************************************************* */ +PoseRTV between_proxy(const PoseRTV& A, const PoseRTV& B) { return A.between(B); } +TEST( testPoseRTV, between ) { + PoseRTV state1(pt, rot, vel), state2 = state1; + + Matrix actH1, actH2; + state1.between(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2); + Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, tol)); + EXPECT(assert_equal(numericH2, actH2, tol)); +} + +/* ************************************************************************* */ +PoseRTV inverse_proxy(const PoseRTV& A) { return A.inverse(); } +TEST( testPoseRTV, inverse ) { + PoseRTV state1(pt, rot, vel); + + Matrix actH1; + state1.inverse(actH1); + Matrix numericH1 = numericalDerivative11(inverse_proxy, state1); + EXPECT(assert_equal(numericH1, actH1, tol)); +} + /* ************************************************************************* */ double range_proxy(const PoseRTV& A, const PoseRTV& B) { return A.range(B); } TEST( testPoseRTV, range ) { diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index bdc71fea2..a0d969c4d 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -21,7 +21,7 @@ Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0)); //Vector6 v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0).finished()); Vector6 V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0).finished()); Vector6 V1_g1 = g1.inverse().Adjoint(V1_w); -Pose3 g2(g1.retract(h*V1_g1, Pose3::EXPMAP)); +Pose3 g2(g1.expmap(h*V1_g1)); //Vector6 v2 = Pose3::Logmap(g1.between(g2)); double mass = 100.0; @@ -45,22 +45,6 @@ Vector computeFu(const Vector& gamma, const Vector& control) { return F*control; } -/* ************************************************************************* */ -Vector testExpmapDeriv(const Vector6& v) { - return Pose3::Logmap(Pose3::Expmap(-h*V1_g1)*Pose3::Expmap(h*V1_g1+v)); -} - -TEST(Reconstruction, ExpmapInvDeriv) { - Matrix numericalExpmap = numericalDerivative11( - boost::function( - boost::bind(testExpmapDeriv, _1) - ), - Vector6(Vector::Zero(6)), 1e-5 - ); - Matrix dExpInv = Pose3::dExpInv_exp(h*V1_g1); - EXPECT(assert_equal(numericalExpmap, dExpInv, 1e-2)); -} - /* ************************************************************************* */ TEST( Reconstruction, evaluateError) { // hard constraints don't need a noise model @@ -68,29 +52,23 @@ TEST( Reconstruction, evaluateError) { // verify error function Matrix H1, H2, H3; - EXPECT(assert_equal(zero(6), constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol)); - + EXPECT( + assert_equal(zero(6), constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol)); Matrix numericalH1 = numericalDerivative31( boost::function( - boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) - ), - g2, g1, V1_g1, 1e-5 - ); + boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, + boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5); Matrix numericalH2 = numericalDerivative32( boost::function( - boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) - ), - g2, g1, V1_g1, 1e-5 - ); + boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, + boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5); Matrix numericalH3 = numericalDerivative33( boost::function( - boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) - ), - g2, g1, V1_g1, 1e-5 - ); + boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, + boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5); EXPECT(assert_equal(numericalH1,H1,1e-5)); EXPECT(assert_equal(numericalH2,H2,1e-5)); diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp index 7e4802393..56d38714a 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp @@ -15,9 +15,9 @@ const Key x1 = 1, x2 = 2; const double dt = 1.0; PoseRTV origin, - pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), (Vector)Vector3(1.0, 0.0, 0.0)), + pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Point3(1.0, 0.0, 0.0)), pose1a(Point3(0.5, 0.0, 0.0)), - pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), (Vector)Vector3(1.0, 0.0, 0.0)); + pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Point3(1.0, 0.0, 0.0)); /* ************************************************************************* */ TEST( testVelocityConstraint, trapezoidal ) { diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index 01c2ab3e1..dc921a7da 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -46,7 +46,7 @@ using namespace gtsam; int main(int argc, char** argv){ - typedef SmartProjectionPoseFactor SmartFactor; + typedef SmartProjectionPoseFactor SmartFactor; Values initial_estimate; NonlinearFactorGraph graph; diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp new file mode 100644 index 000000000..ac2c31077 --- /dev/null +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -0,0 +1,154 @@ +/* ---------------------------------------------------------------------------- + +* 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 SmartProjectionFactorExample.cpp +* @brief A stereo visual odometry example +* @date May 30, 2014 +* @author Stephen Camp +* @author Chris Beall +*/ + + +/** + * A smart projection factor example based on stereo data, throwing away the + * measurement from the right camera + * -robot starts at origin + * -moves forward, taking periodic stereo measurements + * -makes monocular observations of many landmarks + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main(int argc, char** argv){ + + typedef SmartStereoProjectionPoseFactor SmartFactor; + + bool output_poses = true; + string poseOutput("../../../examples/data/optimized_poses.txt"); + string init_poseOutput("../../../examples/data/initial_poses.txt"); + Values initial_estimate; + NonlinearFactorGraph graph; + const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1); + ofstream pose3Out, init_pose3Out; + + bool add_initial_noise = true; + + string calibration_loc = findExampleDataFile("VO_calibration.txt"); + string pose_loc = findExampleDataFile("VO_camera_poses_large.txt"); + string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt"); + + //read camera calibration info from file + // focal lengths fx, fy, skew s, principal point u0, v0, baseline b + cout << "Reading calibration info" << endl; + ifstream calibration_file(calibration_loc.c_str()); + + double fx, fy, s, u0, v0, b; + calibration_file >> fx >> fy >> s >> u0 >> v0 >> b; + const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, s, u0, v0,b)); + + cout << "Reading camera poses" << endl; + ifstream pose_file(pose_loc.c_str()); + + int pose_id; + MatrixRowMajor m(4,4); + //read camera pose parameters and use to make initial estimates of camera poses + while (pose_file >> pose_id) { + for (int i = 0; i < 16; i++) { + pose_file >> m.data()[i]; + } + if(add_initial_noise){ + m(1,3) += (pose_id % 10)/10.0; + } + initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); + } + + Values initial_pose_values = initial_estimate.filter(); + if (output_poses) { + init_pose3Out.open(init_poseOutput.c_str(), ios::out); + for (size_t i = 1; i <= initial_pose_values.size(); i++) { + init_pose3Out + << i << " " + << initial_pose_values.at(Symbol('x', i)).matrix().format( + Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl; + } + } + + // camera and landmark keys + size_t x, l; + + // pixel coordinates uL, uR, v (same for left/right images due to rectification) + // landmark coordinates X, Y, Z in camera frame, resulting from triangulation + double uL, uR, v, X, Y, Z; + ifstream factor_file(factor_loc.c_str()); + cout << "Reading stereo factors" << endl; + + //read stereo measurements and construct smart factors + + SmartFactor::shared_ptr factor(new SmartFactor()); + size_t current_l = 3; // hardcoded landmark ID from first measurement + + while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { + + if(current_l != l) { + graph.push_back(factor); + factor = SmartFactor::shared_ptr(new SmartFactor()); + current_l = l; + } + factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), model, K); + } + + Pose3 first_pose = initial_estimate.at(Symbol('x',1)); + //constrain the first pose such that it cannot change from its original value during optimization + // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky + // QR is much slower than Cholesky, but numerically more stable + graph.push_back(NonlinearEquality(Symbol('x',1),first_pose)); + + LevenbergMarquardtParams params; + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + params.verbosity = NonlinearOptimizerParams::ERROR; + + cout << "Optimizing" << endl; + //create Levenberg-Marquardt optimizer to optimize the factor graph + LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params); + Values result = optimizer.optimize(); + + cout << "Final result sample:" << endl; + Values pose_values = result.filter(); + pose_values.print("Final camera poses:\n"); + + if(output_poses){ + pose3Out.open(poseOutput.c_str(),ios::out); + for(size_t i = 1; i<=pose_values.size(); i++){ + pose3Out << i << " " << pose_values.at(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, + " ", " ")) << endl; + } + cout << "Writing output" << endl; + } + + return 0; +} diff --git a/gtsam_unstable/geometry/BearingS2.h b/gtsam_unstable/geometry/BearingS2.h index 4c84bbe56..70a22b9a5 100644 --- a/gtsam_unstable/geometry/BearingS2.h +++ b/gtsam_unstable/geometry/BearingS2.h @@ -92,11 +92,14 @@ private: // Serialization function friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(azimuth_); ar & BOOST_SERIALIZATION_NVP(elevation_); } }; +/// traits +template<> struct traits : public Testable {}; + } // \namespace gtsam diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h new file mode 100644 index 000000000..9d35331bb --- /dev/null +++ b/gtsam_unstable/geometry/Event.h @@ -0,0 +1,109 @@ +/* ---------------------------------------------------------------------------- + + * 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 Event + * @brief Space-time event + * @author Frank Dellaert + * @author Jay Chakravarty + * @date December 2014 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/// A space-time event +class Event { + + double time_; ///< Time event was generated + Point3 location_; ///< Location at time event was generated + +public: + enum { dimension = 4 }; + + /// Speed of sound + static const double Speed; + static const Matrix14 JacobianZ; + + /// Default Constructor + Event() : + time_(0) { + } + + /// Constructor from time and location + Event(double t, const Point3& p) : + time_(t), location_(p) { + } + + /// Constructor with doubles + Event(double t, double x, double y, double z) : + time_(t), location_(x, y, z) { + } + + double time() const { return time_;} + Point3 location() const { return location_;} + + // TODO we really have to think of a better way to do linear arguments + double height(OptionalJacobian<1,4> H = boost::none) const { + if (H) *H = JacobianZ; + return location_.z(); + } + + /** print with optional string */ + void print(const std::string& s = "") const { + std::cout << s << "time = " << time_; + location_.print("; location"); + } + + /** equals with an tolerance */ + bool equals(const Event& other, double tol = 1e-9) const { + return std::abs(time_ - other.time_) < tol + && location_.equals(other.location_, tol); + } + + /// Updates a with tangent space delta + inline Event retract(const Vector4& v) const { + return Event(time_ + v[0], location_.retract(v.tail(3))); + } + + /// Returns inverse retraction + inline Vector4 localCoordinates(const Event& q) const { + return Vector4::Zero(); // TODO + } + + /// Time of arrival to given microphone + double toa(const Point3& microphone, // + OptionalJacobian<1, 4> H1 = boost::none, // + OptionalJacobian<1, 3> H2 = boost::none) const { + Matrix13 D1, D2; + double distance = location_.distance(microphone, D1, D2); + if (H1) + // derivative of toa with respect to event + *H1 << 1.0, D1 / Speed; + if (H2) + // derivative of toa with respect to microphone location + *H2 << D2 / Speed; + return time_ + distance / Speed; + } +}; + +const double Event::Speed = 330; +const Matrix14 Event::JacobianZ = (Matrix14() << 0,0,0,1).finished(); + +// Define GTSAM traits +template<> +struct GTSAM_EXPORT traits : internal::Manifold {}; + +} //\ namespace gtsam diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 9347b9ffb..4ce0eaedb 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -175,7 +175,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(pose_); ar & BOOST_SERIALIZATION_NVP(k_); } diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h index 22aab5d44..9d01e20a5 100644 --- a/gtsam_unstable/geometry/Pose3Upright.h +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -133,25 +133,15 @@ private: // Serialization function friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(T_); ar & BOOST_SERIALIZATION_NVP(z_); } }; // \class Pose3Upright -// Define GTSAM traits -namespace traits { - template<> -struct is_manifold : public boost::true_type { -}; - -template<> -struct dimension : public boost::integral_constant { -}; - -} +struct traits : public internal::Manifold {}; } // \namespace gtsam diff --git a/gtsam_unstable/geometry/SimPolygon2D.h b/gtsam_unstable/geometry/SimPolygon2D.h index 02892c519..835bb4083 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.h +++ b/gtsam_unstable/geometry/SimPolygon2D.h @@ -128,7 +128,6 @@ public: }; typedef std::vector SimPolygon2DVector; -typedef std::vector Point2Vector; } //\namespace gtsam diff --git a/gtsam_unstable/geometry/SimWall2D.cpp b/gtsam_unstable/geometry/SimWall2D.cpp index eb8ddace6..9087cac2a 100644 --- a/gtsam_unstable/geometry/SimWall2D.cpp +++ b/gtsam_unstable/geometry/SimWall2D.cpp @@ -156,7 +156,7 @@ std::pair moveWithBounce(const Pose2& cur_pose, double step_size, // Simple check to make sure norm is on side closest robot if (cur_pose.t().distance(intersection + norm) > cur_pose.t().distance(intersection - norm)) - norm = norm.inverse(); + norm = - norm; // using the reflection const double inside_bias = 0.05; diff --git a/gtsam_unstable/geometry/SimWall2D.h b/gtsam_unstable/geometry/SimWall2D.h index cf2142af8..38bba2ee3 100644 --- a/gtsam_unstable/geometry/SimWall2D.h +++ b/gtsam_unstable/geometry/SimWall2D.h @@ -68,6 +68,9 @@ namespace gtsam { typedef std::vector SimWall2DVector; + /// traits + template<> struct traits : public Testable {}; + /** * Calculates the next pose in a trajectory constrained by walls, with noise on * angular drift and reflection noise diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp new file mode 100644 index 000000000..8d75d767c --- /dev/null +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -0,0 +1,130 @@ +/* ---------------------------------------------------------------------------- + + * 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 Similarity3.cpp + * @brief Implementation of Similarity3 transform + * @author Paul Drews + */ + +#include + +#include +#include +#include + +#include + +namespace gtsam { + +Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) : + R_(R), t_(t), s_(s) { +} + +Similarity3::Similarity3() : + R_(), t_(), s_(1){ +} + +Similarity3::Similarity3(double s) : + s_ (s) { +} + +Similarity3::Similarity3(const Rotation& R, const Translation& t, double s) : + R_ (R), t_ (t), s_ (s) { +} + +Similarity3::operator Pose3() const { + return Pose3(R_, s_*t_); +} + +Similarity3 Similarity3::identity() { + return Similarity3(); } + +//Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { +// return Vector7(); +//} +// +//Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { +// return Similarity3(); +//} + +bool Similarity3::operator==(const Similarity3& other) const { + return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_); +} + +bool Similarity3::equals(const Similarity3& sim, double tol) const { + return R_.equals(sim.R_, tol) && t_.equals(sim.t_, tol) + && s_ < (sim.s_+tol) && s_ > (sim.s_-tol); +} + +Similarity3::Translation Similarity3::transform_from(const Translation& p) const { + return R_ * (s_ * p) + t_; +} + +Matrix7 Similarity3::AdjointMap() const{ + const Matrix3 R = R_.matrix(); + const Vector3 t = t_.vector(); + Matrix3 A = s_ * skewSymmetric(t) * R; + Matrix7 adj; + adj << s_*R, A, -s_*t, Z_3x3, R, Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), 1; + return adj; +} + +inline Similarity3::Translation Similarity3::operator*(const Translation& p) const { + return transform_from(p); +} + +Similarity3 Similarity3::inverse() const { + Rotation Rt = R_.inverse(); + Translation sRt = R_.inverse() * (-s_ * t_); + return Similarity3(Rt, sRt, 1.0/s_); +} + +Similarity3 Similarity3::operator*(const Similarity3& T) const { + return Similarity3(R_ * T.R_, ((1.0/T.s_)*t_) + R_ * T.t_, s_*T.s_); +} + +void Similarity3::print(const std::string& s) const { + std::cout << std::endl; + std::cout << s; + rotation().print("R:\n"); + translation().print("t: "); + std::cout << "s: " << scale() << std::endl; +} + +std::ostream &operator<<(std::ostream &os, const Similarity3& p) { + os << "[" << p.rotation().xyz().transpose() << " " << p.translation().vector().transpose() << " " << + p.scale() << "]\';"; + return os; +} + +Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v, ChartJacobian H) { + // Will retracting or localCoordinating R work if R is not a unit rotation? + // Also, how do we actually get s out? Seems like we need to store it somewhere. + Rotation r; //Create a zero rotation to do our retraction. + return Similarity3( // + r.retract(v.head<3>()), // retract rotation using v[0,1,2] + Translation(v.segment<3>(3)), // Retract the translation + 1.0 + v[6]); //finally, update scale using v[6] +} + +Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other, ChartJacobian H) { + Rotation r; //Create a zero rotation to do the retraction + Vector7 v; + v.head<3>() = r.localCoordinates(other.R_); + v.segment<3>(3) = other.t_.vector(); + //v.segment<3>(3) = translation().localCoordinates(other.translation()); + v[6] = other.s_ - 1.0; + return v; +} +} + + diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h new file mode 100644 index 000000000..eec2124ee --- /dev/null +++ b/gtsam_unstable/geometry/Similarity3.h @@ -0,0 +1,152 @@ +/* ---------------------------------------------------------------------------- + + * 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 Similarity3.h + * @brief Implementation of Similarity3 transform + * @author Paul Drews + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +// Forward declarations +class Pose3; + +/** + * 3D similarity transform + */ +class Similarity3: public LieGroup { + + /** Pose Concept requirements */ + typedef Rot3 Rotation; + typedef Point3 Translation; + +private: + Rotation R_; + Translation t_; + double s_; + +public: + + /// @name Constructors + /// @{ + + Similarity3(); + + /// Construct pure scaling + Similarity3(double s); + + /// Construct from GTSAM types + Similarity3(const Rotation& R, const Translation& t, double s); + + /// Construct from Eigen types + Similarity3(const Matrix3& R, const Vector3& t, double s); + + /// @} + /// @name Testable + /// @{ + + /// Compare with tolerance + bool equals(const Similarity3& sim, double tol) const; + + /// Compare with standard tolerance + bool operator==(const Similarity3& other) const; + + /// Print with optional string + void print(const std::string& s) const; + + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Similarity3& p); + + /// @} + /// @name Group + /// @{ + + /// Return an identity transform + static Similarity3 identity(); + + /// Return the inverse + Similarity3 inverse() const; + + Translation transform_from(const Translation& p) const; + + /** syntactic sugar for transform_from */ + inline Translation operator*(const Translation& p) const; + + Similarity3 operator*(const Similarity3& T) const; + + /// @} + /// @name Standard interface + /// @{ + + /// Return a GTSAM rotation + const Rotation& rotation() const { + return R_; + }; + + /// Return a GTSAM translation + const Translation& translation() const { + return t_; + }; + + /// Return the scale + double scale() const { + return s_; + }; + + /// Convert to a rigid body pose + operator Pose3() const; + + /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes + inline static size_t Dim() { + return 7; + }; + + /// Dimensionality of tangent space = 7 DOF + inline size_t dim() const { + return 7; + }; + + /// @} + /// @name Chart + /// @{ + + /// Update Similarity transform via 7-dim vector in tangent space + struct ChartAtOrigin { + static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none); + + /// 7-dimensional vector v in tangent space that makes other = this->retract(v) + static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none); + }; + + /// Project from one tangent space to another + Matrix7 AdjointMap() const; + + /// @} + /// @name Stubs + /// @{ + + /// Not currently implemented, required because this is a lie group + static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = boost::none); + static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = boost::none); + + using LieGroup::inverse; // version with derivative +}; + +template<> +struct traits : public internal::LieGroup {}; +} diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp new file mode 100644 index 000000000..433ca7e7f --- /dev/null +++ b/gtsam_unstable/geometry/tests/testEvent.cpp @@ -0,0 +1,100 @@ +/* ---------------------------------------------------------------------------- + + * 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 testEvent.cpp + * @brief Unit tests for space time "Event" + * @author Frank Dellaert + * @author Jay Chakravarty + * @date December 2014 + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// Create a noise model for the TOA error +static const double ms = 1e-3; +static const double cm = 1e-2; +typedef Eigen::Matrix Vector1; +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1,0.5*ms)); + +static const double timeOfEvent = 25; +static const Event exampleEvent(timeOfEvent, 1, 0, 0); +static const Point3 microphoneAt0; + +//***************************************************************************** +TEST( Event, Constructor ) { + const double t = 0; + Event actual(t, 201.5 * cm, 201.5 * cm, (212 - 45) * cm); +} + +//***************************************************************************** +TEST( Event, Toa1 ) { + Event event(0, 1, 0, 0); + double expected = 1 / Event::Speed; + EXPECT_DOUBLES_EQUAL(expected, event.toa(microphoneAt0), 1e-9); +} + +//***************************************************************************** +TEST( Event, Toa2 ) { + double expectedTOA = timeOfEvent + 1 / Event::Speed; + EXPECT_DOUBLES_EQUAL(expectedTOA, exampleEvent.toa(microphoneAt0), 1e-9); +} + +//************************************************************************* +TEST (Event, Derivatives) { + Matrix14 actualH1; + Matrix13 actualH2; + exampleEvent.toa(microphoneAt0, actualH1, actualH2); + Matrix expectedH1 = numericalDerivative11( + boost::bind(&Event::toa, _1, microphoneAt0, boost::none, boost::none), + exampleEvent); + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + Matrix expectedH2 = numericalDerivative11( + boost::bind(&Event::toa, exampleEvent, _1, boost::none, boost::none), + microphoneAt0); + EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); +} + +//***************************************************************************** +TEST( Event, Expression ) { + Key key = 12; + Expression event_(key); + Expression knownMicrophone_(microphoneAt0); // constant expression + Expression expression(&Event::toa, event_, knownMicrophone_); + + Values values; + values.insert(key, exampleEvent); + double expectedTOA = timeOfEvent + 1 / Event::Speed; + EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9); +} + +//***************************************************************************** +TEST(Event, Retract) { + Event event, expected(1, 2, 3, 4); + Vector4 v; + v << 1, 2, 3, 4; + EXPECT(assert_equal(expected, event.retract(v))); +} + +//***************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//***************************************************************************** + diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp new file mode 100644 index 000000000..b2b5d5702 --- /dev/null +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -0,0 +1,256 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSimilarity3.cpp + * @brief Unit tests for Similarity3 class + * @author Paul Drews + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace gtsam; +using namespace std; +using symbol_shorthand::X; + +GTSAM_CONCEPT_TESTABLE_INST(Similarity3) + +static Point3 P(0.2,0.7,-2); +static Rot3 R = Rot3::rodriguez(0.3,0,0); +static Similarity3 T(R,Point3(3.5,-8.2,4.2),1); +static Similarity3 T2(Rot3::rodriguez(0.3,0.2,0.1),Point3(3.5,-8.2,4.2),1); +static Similarity3 T3(Rot3::rodriguez(-90, 0, 0), Point3(1, 2, 3), 1); + +//****************************************************************************** +TEST(Similarity3, Constructors) { + Similarity3 test; +} + +//****************************************************************************** +TEST(Similarity3, Getters) { + Similarity3 test; + EXPECT(assert_equal(Rot3(), test.rotation())); + EXPECT(assert_equal(Point3(), test.translation())); + EXPECT_DOUBLES_EQUAL(1.0, test.scale(), 1e-9); +} + +//****************************************************************************** +TEST(Similarity3, Getters2) { + Similarity3 test(Rot3::ypr(1, 2, 3), Point3(4, 5, 6), 7); + EXPECT(assert_equal(Rot3::ypr(1, 2, 3), test.rotation())); + EXPECT(assert_equal(Point3(4, 5, 6), test.translation())); + EXPECT_DOUBLES_EQUAL(7.0, test.scale(), 1e-9); +} + +TEST(Similarity3, AdjointMap) { + Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7); + Matrix7 result; + result << -1.5739, -2.4512, -6.3651, -50.7671, -11.2503, 16.8859, -28.0000, + 6.3167, -2.9884, -0.4111, 0.8502, 8.6373, -49.7260, -35.0000, + -2.5734, -5.8362, 2.8839, 33.1363, 0.3024, 30.1811, -42.0000, + 0, 0, 0, -0.2248, -0.3502, -0.9093, 0, + 0, 0, 0, 0.9024, -0.4269, -0.0587, 0, + 0, 0, 0, -0.3676, -0.8337, 0.4120, 0, + 0, 0, 0, 0, 0, 0, 1.0000; + EXPECT(assert_equal(result, test.AdjointMap(), 1e-3)); +} + +TEST(Similarity3, inverse) { + Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7); + Matrix3 Re; + Re << -0.2248, 0.9024, -0.3676, + -0.3502, -0.4269, -0.8337, + -0.9093, -0.0587, 0.4120; + Vector3 te(-9.8472, 59.7640, 10.2125); + Similarity3 expected(Re, te, 1.0/7.0); + EXPECT(assert_equal(expected, test.inverse(), 1e-3)); +} + +TEST(Similarity3, multiplication) { + Similarity3 test1(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7); + Similarity3 test2(Rot3::ypr(1,2,3).inverse(), Point3(8,9,10), 11); + Matrix3 re; + re << 0.0688, 0.9863, -0.1496, + -0.5665, -0.0848, -0.8197, + -0.8211, 0.1412, 0.5530; + Vector3 te(-13.6797, 3.2441, -5.7794); + Similarity3 expected(re, te, 77); + EXPECT(assert_equal(expected, test1*test2, 1e-2)); +} + +//****************************************************************************** +TEST(Similarity3, Manifold) { + EXPECT_LONGS_EQUAL(7, Similarity3::Dim()); + Vector z = Vector7::Zero(); + Similarity3 sim; + EXPECT(sim.retract(z) == sim); + + Vector7 v = Vector7::Zero(); + v(6) = 2; + Similarity3 sim2; + EXPECT(sim2.retract(z) == sim2); + + EXPECT(assert_equal(z, sim2.localCoordinates(sim))); + + Similarity3 sim3 = Similarity3(Rot3(), Point3(1, 2, 3), 1); + Vector v3(7); + v3 << 0, 0, 0, 1, 2, 3, 0; + EXPECT(assert_equal(v3, sim2.localCoordinates(sim3))); + +// Similarity3 other = Similarity3(Rot3::ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1); + Similarity3 other = Similarity3(Rot3::ypr(0.1, 0.2, 0.3),Point3(4,5,6),1); + + Vector vlocal = sim.localCoordinates(other); + + EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2)); + + Similarity3 other2 = Similarity3(Rot3::ypr(0.3, 0, 0),Point3(4,5,6),1); + Rot3 R = Rot3::rodriguez(0.3,0,0); + + Vector vlocal2 = sim.localCoordinates(other2); + + EXPECT(assert_equal(sim.retract(vlocal2), other2, 1e-2)); + + // TODO add unit tests for retract and localCoordinates +} + +/* ************************************************************************* */ +TEST( Similarity3, retract_first_order) +{ + Similarity3 id; + Vector v = zero(7); + v(0) = 0.3; + EXPECT(assert_equal(Similarity3(R, Point3(), 1), id.retract(v),1e-2)); + v(3)=0.2;v(4)=0.7;v(5)=-2; + EXPECT(assert_equal(Similarity3(R, P, 1),id.retract(v),1e-2)); +} + +/* ************************************************************************* */ +TEST(Similarity3, localCoordinates_first_order) +{ + Vector d12 = repeat(7,0.1); + d12(6) = 1.0; + Similarity3 t1 = T, t2 = t1.retract(d12); + EXPECT(assert_equal(d12, t1.localCoordinates(t2))); +} + +/* ************************************************************************* */ +TEST(Similarity3, manifold_first_order) +{ + Similarity3 t1 = T; + Similarity3 t2 = T3; + Similarity3 origin; + Vector d12 = t1.localCoordinates(t2); + EXPECT(assert_equal(t2, t1.retract(d12))); + Vector d21 = t2.localCoordinates(t1); + EXPECT(assert_equal(t1, t2.retract(d21))); +} + +TEST(Similarity3, Optimization) { + Similarity3 prior = Similarity3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4); + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); + Symbol key('x',1); + PriorFactor factor(key, prior, model); + + NonlinearFactorGraph graph; + graph.push_back(factor); + + Values initial; + initial.insert(key, Similarity3()); + + Values result; + LevenbergMarquardtParams params; + params.setVerbosityLM("TRYCONFIG"); + result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + EXPECT(assert_equal(prior, result.at(key), 1e-4)); +} + +TEST(Similarity3, Optimization2) { + Similarity3 prior = Similarity3(); + Similarity3 m1 = Similarity3(Rot3::ypr(M_PI/4.0, 0, 0), Point3(2.0, 0, 0), 1.0); + Similarity3 m2 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(sqrt(8)*0.9, 0, 0), 1.0); + Similarity3 m3 = Similarity3(Rot3::ypr(3*M_PI/4.0, 0, 0), Point3(sqrt(32)*0.8, 0, 0), 1.0); + Similarity3 m4 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(6*0.7, 0, 0), 1.0); + Similarity3 loop = Similarity3(1.42); + + //prior.print("Goal Transform"); + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 0.01); + SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas( + (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished()); + SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas( + (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished()); + PriorFactor factor(X(1), prior, model); + BetweenFactor b1(X(1), X(2), m1, betweenNoise); + BetweenFactor b2(X(2), X(3), m2, betweenNoise); + BetweenFactor b3(X(3), X(4), m3, betweenNoise); + BetweenFactor b4(X(4), X(5), m4, betweenNoise); + BetweenFactor lc(X(5), X(1), loop, betweenNoise2); + + + + NonlinearFactorGraph graph; + graph.push_back(factor); + graph.push_back(b1); + graph.push_back(b2); + graph.push_back(b3); + graph.push_back(b4); + graph.push_back(lc); + + //graph.print("Full Graph\n"); + + Values initial; + initial.insert(X(1), Similarity3()); + initial.insert(X(2), Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1)); + initial.insert(X(3), Similarity3(Rot3::ypr(2.0*M_PI/2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2)); + initial.insert(X(4), Similarity3(Rot3::ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.3)); + initial.insert(X(5), Similarity3(Rot3::ypr(4.0*M_PI/2.0, 0, 0), Point3(0, 0, 0), 1.0)); + + //initial.print("Initial Estimate\n"); + + Values result; + result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + //result.print("Optimized Estimate\n"); + Pose3 p1, p2, p3, p4, p5; + p1 = Pose3(result.at(X(1))); + p2 = Pose3(result.at(X(2))); + p3 = Pose3(result.at(X(3))); + p4 = Pose3(result.at(X(4))); + p5 = Pose3(result.at(X(5))); + + //p1.print("Pose1"); + //p2.print("Pose2"); + //p3.print("Pose3"); + //p4.print("Pose4"); + //p5.print("Pose5"); + + Similarity3 expected(0.7); + EXPECT(assert_equal(expected, result.at(X(5)), 0.4)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 95e635516..6b57bfcd0 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -8,6 +8,7 @@ class gtsam::Vector6; class gtsam::LieScalar; class gtsam::LieVector; class gtsam::Point2; +class gtsam::Point2Vector; class gtsam::Rot2; class gtsam::Pose2; class gtsam::Point3; @@ -159,32 +160,6 @@ class BearingS2 { void serializable() const; // enabling serialization functionality }; -// std::vector -class Point2Vector -{ - // Constructors - Point2Vector(); - Point2Vector(const gtsam::Point2Vector& v); - - //Capacity - size_t size() const; - size_t max_size() const; - void resize(size_t sz); - size_t capacity() const; - bool empty() const; - void reserve(size_t n); - - //Element access - gtsam::Point2 at(size_t n) const; - gtsam::Point2 front() const; - gtsam::Point2 back() const; - - //Modifiers - void assign(size_t n, const gtsam::Point2& u); - void push_back(const gtsam::Point2& x); - void pop_back(); -}; - #include class SimWall2D { SimWall2D(); @@ -477,7 +452,7 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor { DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model); }; -#include +#include #include virtual class VelocityConstraint3 : gtsam::NonlinearFactor { diff --git a/gtsam_unstable/linear/CMakeLists.txt b/gtsam_unstable/linear/CMakeLists.txt new file mode 100644 index 000000000..99a4b814e --- /dev/null +++ b/gtsam_unstable/linear/CMakeLists.txt @@ -0,0 +1,6 @@ +# Install headers +file(GLOB linear_headers "*.h") +install(FILES ${linear_headers} DESTINATION include/gtsam_unstable/linear) + +# Add all tests +add_subdirectory(tests) diff --git a/gtsam_unstable/linear/LinearEquality.h b/gtsam_unstable/linear/LinearEquality.h new file mode 100644 index 000000000..bc1b2bc12 --- /dev/null +++ b/gtsam_unstable/linear/LinearEquality.h @@ -0,0 +1,123 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * LinearEquality.h + * @brief: LinearEquality derived from Base with constrained noise model + * @date: Nov 27, 2014 + * @author: thduynguyen + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * This class defines Linear constraints by inherit Base + * with the special Constrained noise model + */ +class LinearEquality: public JacobianFactor { +public: + typedef LinearEquality This; ///< Typedef to this class + typedef JacobianFactor Base; ///< Typedef to base class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + +private: + Key dualKey_; + +public: + /** default constructor for I/O */ + LinearEquality() : + Base() { + } + + /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ + explicit LinearEquality(const HessianFactor& hf) { + throw std::runtime_error("Cannot convert HessianFactor to LinearEquality"); + } + + /** Construct unary factor */ + LinearEquality(Key i1, const Matrix& A1, const Vector& b, Key dualKey) : + Base(i1, A1, b, noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) { + } + + /** Construct binary factor */ + LinearEquality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, + const Vector& b, Key dualKey) : + Base(i1, A1, i2, A2, b, noiseModel::Constrained::All(b.rows())), dualKey_( + dualKey) { + } + + /** Construct ternary factor */ + LinearEquality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, const Vector& b, Key dualKey) : + Base(i1, A1, i2, A2, i3, A3, b, noiseModel::Constrained::All(b.rows())), dualKey_( + dualKey) { + } + + /** Construct an n-ary factor + * @tparam TERMS A container whose value type is std::pair, specifying the + * collection of keys and matrices making up the factor. */ + template + LinearEquality(const TERMS& terms, const Vector& b, Key dualKey) : + Base(terms, b, noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) { + } + + /** Virtual destructor */ + virtual ~LinearEquality() { + } + + /** equals */ + virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const { + return Base::equals(lf, tol); + } + + /** print */ + virtual void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const { + Base::print(s, formatter); + } + + /** Clone this LinearEquality */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + boost::make_shared(*this)); + } + + /// dual key + Key dualKey() const { return dualKey_; } + + /// for active set method: equality constraints are always active + bool active() const { return true; } + + /** Special error_vector for constraints (A*x-b) */ + Vector error_vector(const VectorValues& c) const { + return unweighted_error(c); + } + + /** Special error for constraints. + * I think it should be zero, as this function is meant for objective cost. + * But the name "error" can be misleading. + * TODO: confirm with Frank!! */ + virtual double error(const VectorValues& c) const { + return 0.0; + } + +}; // \ LinearEquality + + +/// traits +template<> struct traits : public Testable {}; + +} // \ namespace gtsam + diff --git a/gtsam_unstable/linear/LinearEqualityFactorGraph.h b/gtsam_unstable/linear/LinearEqualityFactorGraph.h new file mode 100644 index 000000000..9c067ae3d --- /dev/null +++ b/gtsam_unstable/linear/LinearEqualityFactorGraph.h @@ -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 + + * -------------------------------------------------------------------------- */ + +/* + * LinearEqualityFactorGraph.h + * @brief: Factor graph of all LinearEquality factors + * @date: Dec 8, 2014 + * @author: thduynguyen + */ + +#pragma once + +#include +#include + +namespace gtsam { + +class LinearEqualityFactorGraph : public FactorGraph { +public: + typedef boost::shared_ptr shared_ptr; +}; + +/// traits +template<> struct traits : public Testable< + LinearEqualityFactorGraph> { +}; + +} // \ namespace gtsam + diff --git a/gtsam_unstable/linear/LinearInequality.h b/gtsam_unstable/linear/LinearInequality.h new file mode 100644 index 000000000..7c62c3d54 --- /dev/null +++ b/gtsam_unstable/linear/LinearInequality.h @@ -0,0 +1,145 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * LinearInequality.h + * @brief: LinearInequality derived from Base with constrained noise model + * @date: Nov 27, 2014 + * @author: thduynguyen + */ + +#pragma once + +#include + +namespace gtsam { + +typedef Eigen::RowVectorXd RowVector; + +/** + * This class defines Linear constraints by inherit Base + * with the special Constrained noise model + */ +class LinearInequality: public JacobianFactor { +public: + typedef LinearInequality This; ///< Typedef to this class + typedef JacobianFactor Base; ///< Typedef to base class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + +private: + Key dualKey_; + bool active_; + +public: + /** default constructor for I/O */ + LinearInequality() : + Base(), active_(true) { + } + + /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ + explicit LinearInequality(const HessianFactor& hf) { + throw std::runtime_error( + "Cannot convert HessianFactor to LinearInequality"); + } + + /** Construct unary factor */ + LinearInequality(Key i1, const RowVector& A1, double b, Key dualKey) : + Base(i1, A1, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_( + dualKey), active_(true) { + } + + /** Construct binary factor */ + LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, double b, + Key dualKey) : + Base(i1, A1, i2, A2, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_( + dualKey), active_(true) { + } + + /** Construct ternary factor */ + LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3, + const RowVector& A3, double b, Key dualKey) : + Base(i1, A1, i2, A2, i3, A3, (Vector(1) << b).finished(), + noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) { + } + + /** Construct an n-ary factor + * @tparam TERMS A container whose value type is std::pair, specifying the + * collection of keys and matrices making up the factor. */ + template + LinearInequality(const TERMS& terms, double b, Key dualKey) : + Base(terms, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_( + dualKey), active_(true) { + } + + /** Virtual destructor */ + virtual ~LinearInequality() { + } + + /** equals */ + virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const { + return Base::equals(lf, tol); + } + + /** print */ + virtual void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const { + if (active()) + Base::print(s + " Active", formatter); + else + Base::print(s + " Inactive", formatter); + } + + /** Clone this LinearInequality */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + boost::make_shared(*this)); + } + + /// dual key + Key dualKey() const { return dualKey_; } + + /// return true if this constraint is active + bool active() const { return active_; } + + /// Make this inequality constraint active + void activate() { active_ = true; } + + /// Make this inequality constraint inactive + void inactivate() { active_ = false; } + + /** Special error_vector for constraints (A*x-b) */ + Vector error_vector(const VectorValues& c) const { + return unweighted_error(c); + } + + /** Special error for single-valued inequality constraints. */ + virtual double error(const VectorValues& c) const { + return error_vector(c)[0]; + } + + /** dot product of row s with the corresponding vector in p */ + double dotProductRow(const VectorValues& p) const { + double aTp = 0.0; + for (const_iterator xj = begin(); xj != end(); ++xj) { + Vector pj = p.at(*xj); + Vector aj = getA(xj).transpose(); + aTp += aj.dot(pj); + } + return aTp; + } + +}; // \ LinearInequality + +/// traits +template<> struct traits : public Testable {}; + +} // \ namespace gtsam + diff --git a/gtsam_unstable/linear/LinearInequalityFactorGraph.h b/gtsam_unstable/linear/LinearInequalityFactorGraph.h new file mode 100644 index 000000000..eca271941 --- /dev/null +++ b/gtsam_unstable/linear/LinearInequalityFactorGraph.h @@ -0,0 +1,52 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * LinearInequalityFactorGraph.h + * @brief: Factor graph of all LinearInequality factors + * @date: Dec 8, 2014 + * @author: thduynguyen + */ + +#pragma once + +#include +#include + +namespace gtsam { + +class LinearInequalityFactorGraph: public FactorGraph { +private: + typedef FactorGraph Base; + +public: + typedef boost::shared_ptr shared_ptr; + + /** print */ + void print(const std::string& str, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + Base::print(str, keyFormatter); + } + + /** equals */ + bool equals(const LinearInequalityFactorGraph& other, + double tol = 1e-9) const { + return Base::equals(other, tol); + } +}; + +/// traits +template<> struct traits : public Testable< + LinearInequalityFactorGraph> { +}; + +} // \ namespace gtsam + diff --git a/gtsam_unstable/linear/QP.h b/gtsam_unstable/linear/QP.h new file mode 100644 index 000000000..111ab506f --- /dev/null +++ b/gtsam_unstable/linear/QP.h @@ -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 + + * -------------------------------------------------------------------------- */ + +/* + * QP.h + * @brief: Factor graphs of a Quadratic Programming problem + * @date: Dec 8, 2014 + * @author: thduynguyen + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * struct contains factor graphs of a Quadratic Programming problem + */ +struct QP { + GaussianFactorGraph cost; //!< Quadratic cost factors + LinearEqualityFactorGraph equalities; //!< linear equality constraints + LinearInequalityFactorGraph inequalities; //!< linear inequality constraints + + /** default constructor */ + QP() : + cost(), equalities(), inequalities() { + } + + /** constructor */ + QP(const GaussianFactorGraph& _cost, + const LinearEqualityFactorGraph& _linearEqualities, + const LinearInequalityFactorGraph& _linearInequalities) : + cost(_cost), equalities(_linearEqualities), inequalities( + _linearInequalities) { + } + + /** print */ + void print(const std::string& s = "") { + std::cout << s << std::endl; + cost.print("Quadratic cost factors: "); + equalities.print("Linear equality factors: "); + inequalities.print("Linear inequality factors: "); + } +}; + +} // namespace gtsam + diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp new file mode 100644 index 000000000..f0eb7d7fb --- /dev/null +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -0,0 +1,252 @@ +/* + * QPSolver.cpp + * @brief: + * @date: Apr 15, 2014 + * @author: thduynguyen + */ + +#include +#include +#include + +#include + +using namespace std; + +namespace gtsam { + +//****************************************************************************** +QPSolver::QPSolver(const QP& qp) : qp_(qp) { + baseGraph_ = qp_.cost; + baseGraph_.push_back(qp_.equalities.begin(), qp_.equalities.end()); + costVariableIndex_ = VariableIndex(qp_.cost); + equalityVariableIndex_ = VariableIndex(qp_.equalities); + inequalityVariableIndex_ = VariableIndex(qp_.inequalities); + constrainedKeys_ = qp_.equalities.keys(); + constrainedKeys_.merge(qp_.inequalities.keys()); +} + +//****************************************************************************** +VectorValues QPSolver::solveWithCurrentWorkingSet( + const LinearInequalityFactorGraph& workingSet) const { + GaussianFactorGraph workingGraph = baseGraph_; + BOOST_FOREACH(const LinearInequality::shared_ptr& factor, workingSet) { + if (factor->active()) + workingGraph.push_back(factor); + } + return workingGraph.optimize(); +} + +//****************************************************************************** +JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key, + const LinearInequalityFactorGraph& workingSet, const VectorValues& delta) const { + + // Transpose the A matrix of constrained factors to have the jacobian of the dual key + std::vector > Aterms = collectDualJacobians + < LinearEquality > (key, qp_.equalities, equalityVariableIndex_); + std::vector > AtermsInequalities = collectDualJacobians + < LinearInequality > (key, workingSet, inequalityVariableIndex_); + Aterms.insert(Aterms.end(), AtermsInequalities.begin(), + AtermsInequalities.end()); + + // Collect the gradients of unconstrained cost factors to the b vector + if (Aterms.size() > 0) { + Vector b = zero(delta.at(key).size()); + if (costVariableIndex_.find(key) != costVariableIndex_.end()) { + BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) { + GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx); + b += factor->gradient(key, delta); + } + } + return boost::make_shared(Aterms, b, noiseModel::Constrained::All(b.rows())); + } + else { + return boost::make_shared(); + } +} + +//****************************************************************************** +GaussianFactorGraph::shared_ptr QPSolver::buildDualGraph( + const LinearInequalityFactorGraph& workingSet, const VectorValues& delta) const { + GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); + BOOST_FOREACH(Key key, constrainedKeys_) { + // Each constrained key becomes a factor in the dual graph + JacobianFactor::shared_ptr dualFactor = createDualFactor(key, workingSet, delta); + if (!dualFactor->empty()) + dualGraph->push_back(dualFactor); + } + return dualGraph; +} + +//****************************************************************************** +int QPSolver::identifyLeavingConstraint( + const LinearInequalityFactorGraph& workingSet, + const VectorValues& lambdas) const { + int worstFactorIx = -1; + // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either + // inactive or a good inequality constraint, so we don't care! + double maxLambda = 0.0; + for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { + const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); + if (factor->active()) { + double lambda = lambdas.at(factor->dualKey())[0]; + if (lambda > maxLambda) { + worstFactorIx = factorIx; + maxLambda = lambda; + } + } + } + return worstFactorIx; +} + +//****************************************************************************** +/* We have to make sure the new solution with alpha satisfies all INACTIVE inequality constraints + * If some inactive inequality constraints complain about the full step (alpha = 1), + * we have to adjust alpha to stay within the inequality constraints' feasible regions. + * + * For each inactive inequality j: + * - We already have: aj'*xk - bj <= 0, since xk satisfies all inequality constraints + * - We want: aj'*(xk + alpha*p) - bj <= 0 + * - If aj'*p <= 0, we have: aj'*(xk + alpha*p) <= aj'*xk <= bj, for all alpha>0 + * it's good! + * - We only care when aj'*p > 0. In this case, we need to choose alpha so that + * aj'*xk + alpha*aj'*p - bj <= 0 --> alpha <= (bj - aj'*xk) / (aj'*p) + * We want to step as far as possible, so we should choose alpha = (bj - aj'*xk) / (aj'*p) + * + * We want the minimum of all those alphas among all inactive inequality. + */ +boost::tuple QPSolver::computeStepSize( + const LinearInequalityFactorGraph& workingSet, const VectorValues& xk, + const VectorValues& p) const { + static bool debug = false; + + double minAlpha = 1.0; + int closestFactorIx = -1; + for(size_t factorIx = 0; factorIxgetb()[0]; + // only check inactive factors + if (!factor->active()) { + // Compute a'*p + double aTp = factor->dotProductRow(p); + + // Check if a'*p >0. Don't care if it's not. + if (aTp <= 0) + continue; + + // Compute a'*xk + double aTx = factor->dotProductRow(xk); + + // alpha = (b - a'*xk) / (a'*p) + double alpha = (b - aTx) / aTp; + if (debug) + cout << "alpha: " << alpha << endl; + + // We want the minimum of all those max alphas + if (alpha < minAlpha) { + closestFactorIx = factorIx; + minAlpha = alpha; + } + } + + } + + return boost::make_tuple(minAlpha, closestFactorIx); +} + +//****************************************************************************** +QPState QPSolver::iterate(const QPState& state) const { + static bool debug = false; + + // Solve with the current working set + VectorValues newValues = solveWithCurrentWorkingSet(state.workingSet); + if (debug) + newValues.print("New solution:"); + + // If we CAN'T move further + if (newValues.equals(state.values, 1e-5)) { + // Compute lambda from the dual graph + if (debug) + cout << "Building dual graph..." << endl; + GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, newValues); + if (debug) + dualGraph->print("Dual graph: "); + VectorValues duals = dualGraph->optimize(); + if (debug) + duals.print("Duals :"); + + int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); + if (debug) + cout << "leavingFactor: " << leavingFactor << endl; + + // If all inequality constraints are satisfied: We have the solution!! + if (leavingFactor < 0) { + return QPState(newValues, duals, state.workingSet, true); + } + else { + // Inactivate the leaving constraint + LinearInequalityFactorGraph newWorkingSet = state.workingSet; + newWorkingSet.at(leavingFactor)->inactivate(); + return QPState(newValues, duals, newWorkingSet, false); + } + } + else { + // If we CAN make some progress + // Adapt stepsize if some inactive constraints complain about this move + double alpha; + int factorIx; + VectorValues p = newValues - state.values; + boost::tie(alpha, factorIx) = // + computeStepSize(state.workingSet, state.values, p); + if (debug) + cout << "alpha, factorIx: " << alpha << " " << factorIx << " " + << endl; + + // also add to the working set the one that complains the most + LinearInequalityFactorGraph newWorkingSet = state.workingSet; + if (factorIx >= 0) + newWorkingSet.at(factorIx)->activate(); + + // step! + newValues = state.values + alpha * p; + + return QPState(newValues, state.duals, newWorkingSet, false); + } +} + +//****************************************************************************** +LinearInequalityFactorGraph QPSolver::identifyActiveConstraints( + const LinearInequalityFactorGraph& inequalities, + const VectorValues& initialValues) const { + LinearInequalityFactorGraph workingSet; + BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities){ + LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); + double error = workingFactor->error(initialValues); + if (fabs(error)>1e-7){ + workingFactor->inactivate(); + } else { + workingFactor->activate(); + } + workingSet.push_back(workingFactor); + } + return workingSet; +} + +//****************************************************************************** +pair QPSolver::optimize( + const VectorValues& initialValues) const { + + // Initialize workingSet from the feasible initialValues + LinearInequalityFactorGraph workingSet = + identifyActiveConstraints(qp_.inequalities, initialValues); + QPState state(initialValues, VectorValues(), workingSet, false); + + /// main loop of the solver + while (!state.converged) { + state = iterate(state); + } + + return make_pair(state.values, state.duals); +} + +} /* namespace gtsam */ diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h new file mode 100644 index 000000000..f7a575f8c --- /dev/null +++ b/gtsam_unstable/linear/QPSolver.h @@ -0,0 +1,188 @@ +/* + * QPSolver.h + * @brief: A quadratic programming solver implements the active set method + * @date: Apr 15, 2014 + * @author: thduynguyen + */ + +#pragma once + +#include +#include + +#include +#include + +namespace gtsam { + +/// This struct holds the state of QPSolver at each iteration +struct QPState { + VectorValues values; + VectorValues duals; + LinearInequalityFactorGraph workingSet; + bool converged; + + /// default constructor + QPState() : + values(), duals(), workingSet(), converged(false) { + } + + /// constructor with initial values + QPState(const VectorValues& initialValues, const VectorValues& initialDuals, + const LinearInequalityFactorGraph& initialWorkingSet, bool _converged) : + values(initialValues), duals(initialDuals), workingSet(initialWorkingSet), converged( + _converged) { + } +}; + +/** + * This class implements the active set method to solve quadratic programming problems + * encoded in a GaussianFactorGraph with special mixed constrained noise models, in which + * a negative sigma denotes an inequality <=0 constraint, + * a zero sigma denotes an equality =0 constraint, + * and a positive sigma denotes a normal Gaussian noise model. + */ +class QPSolver { + + const QP& qp_; //!< factor graphs of the QP problem, can't be modified! + GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities. The working set of inequalities will be added to this base graph in the process. + VariableIndex costVariableIndex_, equalityVariableIndex_, + inequalityVariableIndex_; + FastSet constrainedKeys_; //!< all constrained keys, will become factors in the dual graph + +public: + /// Constructor + QPSolver(const QP& qp); + + /// Find solution with the current working set + VectorValues solveWithCurrentWorkingSet( + const LinearInequalityFactorGraph& workingSet) const; + + /// @name Build the dual graph + /// @{ + + /// Collect the Jacobian terms for a dual factor + template + std::vector > collectDualJacobians(Key key, + const FactorGraph& graph, + const VariableIndex& variableIndex) const { + std::vector > Aterms; + if (variableIndex.find(key) != variableIndex.end()) { + BOOST_FOREACH(size_t factorIx, variableIndex[key]){ + typename FACTOR::shared_ptr factor = graph.at(factorIx); + if (!factor->active()) continue; + Matrix Ai = factor->getA(factor->find(key)).transpose(); + Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); + } + } + return Aterms; + } + + /// Create a dual factor + JacobianFactor::shared_ptr createDualFactor(Key key, + const LinearInequalityFactorGraph& workingSet, + const VectorValues& delta) const; + + /** + * Build the dual graph to solve for the Lagrange multipliers. + * + * The Lagrangian function is: + * L(X,lambdas) = f(X) - \sum_k lambda_k * c_k(X), + * where the unconstrained part is + * f(X) = 0.5*X'*G*X - X'*g + 0.5*f0 + * and the linear equality constraints are + * c1(X), c2(X), ..., cm(X) + * + * Take the derivative of L wrt X at the solution and set it to 0, we have + * \grad f(X) = \sum_k lambda_k * \grad c_k(X) (*) + * + * For each set of rows of (*) corresponding to a variable xi involving in some constraints + * we have: + * \grad f(xi) = \frac{\partial f}{\partial xi}' = \sum_j G_ij*xj - gi + * \grad c_k(xi) = \frac{\partial c_k}{\partial xi}' + * + * Note: If xi does not involve in any constraint, we have the trivial condition + * \grad f(Xi) = 0, which should be satisfied as a usual condition for unconstrained variables. + * + * So each variable xi involving in some constraints becomes a linear factor A*lambdas - b = 0 + * on the constraints' lambda multipliers, as follows: + * - The jacobian term A_k for each lambda_k is \grad c_k(xi) + * - The constant term b is \grad f(xi), which can be computed from all unconstrained + * Hessian factors connecting to xi: \grad f(xi) = \sum_j G_ij*xj - gi + */ + GaussianFactorGraph::shared_ptr buildDualGraph( + const LinearInequalityFactorGraph& workingSet, + const VectorValues& delta) const; + + /// @} + + /** + * The goal of this function is to find currently active inequality constraints + * that violate the condition to be active. The one that violates the condition + * the most will be removed from the active set. See Nocedal06book, pg 469-471 + * + * Find the BAD active inequality that pulls x strongest to the wrong direction + * of its constraint (i.e. it is pulling towards >0, while its feasible region is <=0) + * + * For active inequality constraints (those that are enforced as equality constraints + * in the current working set), we want lambda < 0. + * This is because: + * - From the Lagrangian L = f - lambda*c, we know that the constraint force + * is (lambda * \grad c) = \grad f. Intuitively, to keep the solution x stay + * on the constraint surface, the constraint force has to balance out with + * other unconstrained forces that are pulling x towards the unconstrained + * minimum point. The other unconstrained forces are pulling x toward (-\grad f), + * hence the constraint force has to be exactly \grad f, so that the total + * force is 0. + * - We also know that at the constraint surface c(x)=0, \grad c points towards + (>= 0), + * while we are solving for - (<=0) constraint. + * - We want the constraint force (lambda * \grad c) to pull x towards the - (<=0) direction + * i.e., the opposite direction of \grad c where the inequality constraint <=0 is satisfied. + * That means we want lambda < 0. + * - This is because when the constrained force pulls x towards the infeasible region (+), + * the unconstrained force is pulling x towards the opposite direction into + * the feasible region (again because the total force has to be 0 to make x stay still) + * So we can drop this constraint to have a lower error but feasible solution. + * + * In short, active inequality constraints with lambda > 0 are BAD, because they + * violate the condition to be active. + * + * And we want to remove the worst one with the largest lambda from the active set. + * + */ + int identifyLeavingConstraint(const LinearInequalityFactorGraph& workingSet, + const VectorValues& lambdas) const; + + /** + * Compute step size alpha for the new solution x' = xk + alpha*p, where alpha \in [0,1] + * + * @return a tuple of (alpha, factorIndex, sigmaIndex) where (factorIndex, sigmaIndex) + * is the constraint that has minimum alpha, or (-1,-1) if alpha = 1. + * This constraint will be added to the working set and become active + * in the next iteration + */ + boost::tuple computeStepSize( + const LinearInequalityFactorGraph& workingSet, const VectorValues& xk, + const VectorValues& p) const; + + /** Iterate 1 step, return a new state with a new workingSet and values */ + QPState iterate(const QPState& state) const; + + /** + * Identify active constraints based on initial values. + */ + LinearInequalityFactorGraph identifyActiveConstraints( + const LinearInequalityFactorGraph& inequalities, + const VectorValues& initialValues) const; + + /** Optimize with a provided initial values + * For this version, it is the responsibility of the caller to provide + * a feasible initial value. + * @return a pair of solutions + */ + std::pair optimize( + const VectorValues& initialValues) const; + +}; + +} /* namespace gtsam */ diff --git a/gtsam_unstable/linear/tests/CMakeLists.txt b/gtsam_unstable/linear/tests/CMakeLists.txt new file mode 100644 index 000000000..43df23daa --- /dev/null +++ b/gtsam_unstable/linear/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(linear_unstable "test*.cpp" "" "gtsam_unstable") diff --git a/gtsam_unstable/linear/tests/testLinearEquality.cpp b/gtsam_unstable/linear/tests/testLinearEquality.cpp new file mode 100644 index 000000000..bf41743a2 --- /dev/null +++ b/gtsam_unstable/linear/tests/testLinearEquality.cpp @@ -0,0 +1,237 @@ +/* ---------------------------------------------------------------------------- + + * 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 testLinearEquality.cpp + * @brief Unit tests for LinearEquality + * @author thduynguyen + **/ + +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +GTSAM_CONCEPT_TESTABLE_INST(LinearEquality) + +namespace { + namespace simple { + // Terms we'll use + const vector > terms = list_of > + (make_pair(5, Matrix3::Identity())) + (make_pair(10, 2*Matrix3::Identity())) + (make_pair(15, 3*Matrix3::Identity())); + + // RHS and sigmas + const Vector b = (Vector(3) << 1., 2., 3.).finished(); + const SharedDiagonal noise = noiseModel::Constrained::All(3); + } +} + +/* ************************************************************************* */ +TEST(LinearEquality, constructors_and_accessors) +{ + using namespace simple; + + // Test for using different numbers of terms + { + // One term constructor + LinearEquality expected( + boost::make_iterator_range(terms.begin(), terms.begin() + 1), b, 0); + LinearEquality actual(terms[0].first, terms[0].second, b, 0); + EXPECT(assert_equal(expected, actual)); + LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back()); + EXPECT(assert_equal(terms[0].second, actual.getA(actual.end() - 1))); + EXPECT(assert_equal(b, expected.getb())); + EXPECT(assert_equal(b, actual.getb())); + EXPECT(assert_equal(*noise, *actual.get_model())); + } + { + // Two term constructor + LinearEquality expected( + boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, 0); + LinearEquality actual(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, b, 0); + EXPECT(assert_equal(expected, actual)); + LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back()); + EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1))); + EXPECT(assert_equal(b, expected.getb())); + EXPECT(assert_equal(b, actual.getb())); + EXPECT(assert_equal(*noise, *actual.get_model())); + } + { + // Three term constructor + LinearEquality expected( + boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, 0); + LinearEquality actual(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, 0); + EXPECT(assert_equal(expected, actual)); + LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back()); + EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1))); + EXPECT(assert_equal(b, expected.getb())); + EXPECT(assert_equal(b, actual.getb())); + EXPECT(assert_equal(*noise, *actual.get_model())); + } +} + +/* ************************************************************************* */ +TEST(LinearEquality, Hessian_conversion) { + HessianFactor hessian(0, (Matrix(4,4) << + 1.57, 2.695, -1.1, -2.35, + 2.695, 11.3125, -0.65, -10.225, + -1.1, -0.65, 1, 0.5, + -2.35, -10.225, 0.5, 9.25).finished(), + (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(), + 73.1725); + + try { + LinearEquality actual(hessian); + EXPECT(false); + } + catch (const std::runtime_error& exception) { + EXPECT(true); + } +} + +/* ************************************************************************* */ +TEST(LinearEquality, error) +{ + LinearEquality factor(simple::terms, simple::b, 0); + + VectorValues values; + values.insert(5, Vector::Constant(3, 1.0)); + values.insert(10, Vector::Constant(3, 0.5)); + values.insert(15, Vector::Constant(3, 1.0/3.0)); + + Vector expected_unwhitened(3); expected_unwhitened << 2.0, 1.0, 0.0; + Vector actual_unwhitened = factor.unweighted_error(values); + EXPECT(assert_equal(expected_unwhitened, actual_unwhitened)); + + // whitened is meaningless in constraints + Vector expected_whitened(3); expected_whitened = expected_unwhitened; + Vector actual_whitened = factor.error_vector(values); + EXPECT(assert_equal(expected_whitened, actual_whitened)); + + double expected_error = 0.0; + double actual_error = factor.error(values); + DOUBLES_EQUAL(expected_error, actual_error, 1e-10); +} + +/* ************************************************************************* */ +TEST(LinearEquality, matrices_NULL) +{ + // Make sure everything works with NULL noise model + LinearEquality factor(simple::terms, simple::b, 0); + + Matrix AExpected(3, 9); + AExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second; + Vector rhsExpected = simple::b; + Matrix augmentedJacobianExpected(3, 10); + augmentedJacobianExpected << AExpected, rhsExpected; + + // Whitened Jacobian + EXPECT(assert_equal(AExpected, factor.jacobian().first)); + EXPECT(assert_equal(rhsExpected, factor.jacobian().second)); + EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobian())); + + // Unwhitened Jacobian + EXPECT(assert_equal(AExpected, factor.jacobianUnweighted().first)); + EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second)); + EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted())); +} + +/* ************************************************************************* */ +TEST(LinearEquality, matrices) +{ + // And now witgh a non-unit noise model + LinearEquality factor(simple::terms, simple::b, 0); + + Matrix jacobianExpected(3, 9); + jacobianExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second; + Vector rhsExpected = simple::b; + Matrix augmentedJacobianExpected(3, 10); + augmentedJacobianExpected << jacobianExpected, rhsExpected; + + Matrix augmentedHessianExpected = + augmentedJacobianExpected.transpose() * simple::noise->R().transpose() + * simple::noise->R() * augmentedJacobianExpected; + + // Whitened Jacobian + EXPECT(assert_equal(jacobianExpected, factor.jacobian().first)); + EXPECT(assert_equal(rhsExpected, factor.jacobian().second)); + EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobian())); + + // Unwhitened Jacobian + EXPECT(assert_equal(jacobianExpected, factor.jacobianUnweighted().first)); + EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second)); + EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted())); +} + +/* ************************************************************************* */ +TEST(LinearEquality, operators ) +{ + Matrix I = eye(2); + Vector b = (Vector(2) << 0.2,-0.1).finished(); + LinearEquality lf(1, -I, 2, I, b, 0); + + VectorValues c; + c.insert(1, (Vector(2) << 10.,20.).finished()); + c.insert(2, (Vector(2) << 30.,60.).finished()); + + // test A*x + Vector expectedE = (Vector(2) << 20.,40.).finished(); + Vector actualE = lf * c; + EXPECT(assert_equal(expectedE, actualE)); + + // test A^e + VectorValues expectedX; + expectedX.insert(1, (Vector(2) << -20.,-40.).finished()); + expectedX.insert(2, (Vector(2) << 20., 40.).finished()); + VectorValues actualX = VectorValues::Zero(expectedX); + lf.transposeMultiplyAdd(1.0, actualE, actualX); + EXPECT(assert_equal(expectedX, actualX)); + + // test gradient at zero + Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian(); + VectorValues expectedG; + expectedG.insert(1, (Vector(2) << 0.2, -0.1).finished()); + expectedG.insert(2, (Vector(2) << -0.2, 0.1).finished()); + VectorValues actualG = lf.gradientAtZero(); + EXPECT(assert_equal(expectedG, actualG)); +} + +/* ************************************************************************* */ +TEST(LinearEquality, default_error ) +{ + LinearEquality f; + double actual = f.error(VectorValues()); + DOUBLES_EQUAL(0.0, actual, 1e-15); +} + +//* ************************************************************************* */ +TEST(LinearEquality, empty ) +{ + // create an empty factor + LinearEquality f; + EXPECT(f.empty()); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp new file mode 100644 index 000000000..8fca61ca4 --- /dev/null +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -0,0 +1,311 @@ +/* ---------------------------------------------------------------------------- + + * 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 testQPSolver.cpp + * @brief Test simple QP solver for a linear inequality constraint + * @date Apr 10, 2014 + * @author Duy-Nguyen Ta + */ + +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::symbol_shorthand; + +const Matrix One = ones(1,1); + +/* ************************************************************************* */ +// Create test graph according to Forst10book_pg171Ex5 +QP createTestCase() { + QP qp; + + // Objective functions x1^2 - x1*x2 + x2^2 - 3*x1 + 5 + // Note the Hessian encodes: + // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f + // Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10 + qp.cost.push_back( + HessianFactor(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), 3.0 * ones(1), + 2.0 * ones(1, 1), zero(1), 10.0)); + + // Inequality constraints + qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), X(2), ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 + qp.inequalities.push_back(LinearInequality(X(1), -ones(1,1), 0, 1)); // -x1 <= 0 + qp.inequalities.push_back(LinearInequality(X(2), -ones(1,1), 0, 2)); // -x2 <= 0 + qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), 1.5, 3)); // x1 <= 3/2 + + return qp; +} + +TEST(QPSolver, TestCase) { + VectorValues values; + double x1 = 5, x2 = 7; + values.insert(X(1), x1 * ones(1, 1)); + values.insert(X(2), x2 * ones(1, 1)); + QP qp = createTestCase(); + DOUBLES_EQUAL(29, x1 * x1 - x1 * x2 + x2 * x2 - 3 * x1 + 5, 1e-9); + DOUBLES_EQUAL(29, qp.cost[0]->error(values), 1e-9); +} + +TEST(QPSolver, constraintsAux) { + QP qp = createTestCase(); + + QPSolver solver(qp); + + VectorValues lambdas; + lambdas.insert(0, (Vector(1) << -0.5).finished()); + lambdas.insert(1, (Vector(1) << 0.0).finished()); + lambdas.insert(2, (Vector(1) << 0.3).finished()); + lambdas.insert(3, (Vector(1) << 0.1).finished()); + int factorIx = solver.identifyLeavingConstraint(qp.inequalities, lambdas); + LONGS_EQUAL(2, factorIx); + + VectorValues lambdas2; + lambdas2.insert(0, (Vector(1) << -0.5).finished()); + lambdas2.insert(1, (Vector(1) << 0.0).finished()); + lambdas2.insert(2, (Vector(1) << -0.3).finished()); + lambdas2.insert(3, (Vector(1) << -0.1).finished()); + int factorIx2 = solver.identifyLeavingConstraint(qp.inequalities, lambdas2); + LONGS_EQUAL(-1, factorIx2); +} + +/* ************************************************************************* */ +// Create a simple test graph with one equality constraint +QP createEqualityConstrainedTest() { + QP qp; + + // Objective functions x1^2 + x2^2 + // Note the Hessian encodes: + // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f + // Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0 + qp.cost.push_back( + HessianFactor(X(1), X(2), 2.0 * ones(1, 1), zeros(1, 1), zero(1), + 2.0 * ones(1, 1), zero(1), 0.0)); + + // Equality constraints + // x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector + Matrix A1 = (Matrix(1, 1) << 1).finished(); + Matrix A2 = (Matrix(1, 1) << 1).finished(); + Vector b = -(Vector(1) << 1).finished(); + qp.equalities.push_back(LinearEquality(X(1), A1, X(2), A2, b, 0)); + + return qp; +} + +TEST(QPSolver, dual) { + QP qp = createEqualityConstrainedTest(); + + // Initials values + VectorValues initialValues; + initialValues.insert(X(1), ones(1)); + initialValues.insert(X(2), ones(1)); + + QPSolver solver(qp); + + GaussianFactorGraph::shared_ptr dualGraph = solver.buildDualGraph( + qp.inequalities, initialValues); + VectorValues dual = dualGraph->optimize(); + VectorValues expectedDual; + expectedDual.insert(0, (Vector(1) << 2.0).finished()); + CHECK(assert_equal(expectedDual, dual, 1e-10)); +} + +/* ************************************************************************* */ +TEST(QPSolver, indentifyActiveConstraints) { + QP qp = createTestCase(); + QPSolver solver(qp); + + VectorValues currentSolution; + currentSolution.insert(X(1), zero(1)); + currentSolution.insert(X(2), zero(1)); + + LinearInequalityFactorGraph workingSet = + solver.identifyActiveConstraints(qp.inequalities, currentSolution); + + CHECK(!workingSet.at(0)->active()); // inactive + CHECK(workingSet.at(1)->active()); // active + CHECK(workingSet.at(2)->active()); // active + CHECK(!workingSet.at(3)->active()); // inactive + + VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet); + VectorValues expectedSolution; + expectedSolution.insert(X(1), (Vector(1) << 0.0).finished()); + expectedSolution.insert(X(2), (Vector(1) << 0.0).finished()); + CHECK(assert_equal(expectedSolution, solution, 1e-100)); + +} + +/* ************************************************************************* */ +TEST(QPSolver, iterate) { + QP qp = createTestCase(); + QPSolver solver(qp); + + VectorValues currentSolution; + currentSolution.insert(X(1), zero(1)); + currentSolution.insert(X(2), zero(1)); + + std::vector expectedSolutions(4), expectedDuals(4); + expectedSolutions[0].insert(X(1), (Vector(1) << 0.0).finished()); + expectedSolutions[0].insert(X(2), (Vector(1) << 0.0).finished()); + expectedDuals[0].insert(1, (Vector(1) << 3).finished()); + expectedDuals[0].insert(2, (Vector(1) << 0).finished()); + + expectedSolutions[1].insert(X(1), (Vector(1) << 1.5).finished()); + expectedSolutions[1].insert(X(2), (Vector(1) << 0.0).finished()); + expectedDuals[1].insert(3, (Vector(1) << 1.5).finished()); + + expectedSolutions[2].insert(X(1), (Vector(1) << 1.5).finished()); + expectedSolutions[2].insert(X(2), (Vector(1) << 0.75).finished()); + + expectedSolutions[3].insert(X(1), (Vector(1) << 1.5).finished()); + expectedSolutions[3].insert(X(2), (Vector(1) << 0.5).finished()); + + LinearInequalityFactorGraph workingSet = + solver.identifyActiveConstraints(qp.inequalities, currentSolution); + + QPState state(currentSolution, VectorValues(), workingSet, false); + + int it = 0; + while (!state.converged) { + state = solver.iterate(state); + // These checks will fail because the expected solutions obtained from + // Forst10book do not follow exactly what we implemented from Nocedal06book. + // Specifically, we do not re-identify active constraints and + // do not recompute dual variables after every step!!! +// CHECK(assert_equal(expectedSolutions[it], state.values, 1e-10)); +// CHECK(assert_equal(expectedDuals[it], state.duals, 1e-10)); + it++; + } + + CHECK(assert_equal(expectedSolutions[3], state.values, 1e-10)); +} + +/* ************************************************************************* */ + +TEST(QPSolver, optimizeForst10book_pg171Ex5) { + QP qp = createTestCase(); + QPSolver solver(qp); + VectorValues initialValues; + initialValues.insert(X(1), zero(1)); + initialValues.insert(X(2), zero(1)); + VectorValues solution; + boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); + VectorValues expectedSolution; + expectedSolution.insert(X(1), (Vector(1) << 1.5).finished()); + expectedSolution.insert(X(2), (Vector(1) << 0.5).finished()); + CHECK(assert_equal(expectedSolution, solution, 1e-100)); +} + +/* ************************************************************************* */ +// Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html +QP createTestMatlabQPEx() { + QP qp; + + // Objective functions 0.5*x1^2 + x2^2 - x1*x2 - 2*x1 -6*x2 + // Note the Hessian encodes: + // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f + // Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0 + qp.cost.push_back( + HessianFactor(X(1), X(2), 1.0 * ones(1, 1), -ones(1, 1), 2.0 * ones(1), + 2.0 * ones(1, 1), 6 * ones(1), 1000.0)); + + // Inequality constraints + qp.inequalities.push_back(LinearInequality(X(1), One, X(2), One, 2, 0)); // x1 + x2 <= 2 + qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2*One, 2, 1)); //-x1 + 2*x2 <=2 + qp.inequalities.push_back(LinearInequality(X(1), 2*One, X(2), One, 3, 2)); // 2*x1 + x2 <=3 + qp.inequalities.push_back(LinearInequality(X(1), -One, 0, 3)); // -x1 <= 0 + qp.inequalities.push_back(LinearInequality(X(2), -One, 0, 4)); // -x2 <= 0 + + return qp; +} + +TEST(QPSolver, optimizeMatlabEx) { + QP qp = createTestMatlabQPEx(); + QPSolver solver(qp); + VectorValues initialValues; + initialValues.insert(X(1), zero(1)); + initialValues.insert(X(2), zero(1)); + VectorValues solution; + boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); + VectorValues expectedSolution; + expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished()); + expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished()); + CHECK(assert_equal(expectedSolution, solution, 1e-7)); +} + +/* ************************************************************************* */ +// Create test graph as in Nocedal06book, Ex 16.4, pg. 475 +QP createTestNocedal06bookEx16_4() { + QP qp; + + qp.cost.push_back(JacobianFactor(X(1), ones(1, 1), ones(1))); + qp.cost.push_back(JacobianFactor(X(2), ones(1, 1), 2.5 * ones(1))); + + // Inequality constraints + qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, 2, 0)); + qp.inequalities.push_back(LinearInequality(X(1), One, X(2), 2 * One, 6, 1)); + qp.inequalities.push_back(LinearInequality(X(1), One, X(2), -2 * One, 2, 2)); + qp.inequalities.push_back(LinearInequality(X(1), -One, 0.0, 3)); + qp.inequalities.push_back(LinearInequality(X(2), -One, 0.0, 4)); + + return qp; +} + +TEST(QPSolver, optimizeNocedal06bookEx16_4) { + QP qp = createTestNocedal06bookEx16_4(); + QPSolver solver(qp); + VectorValues initialValues; + initialValues.insert(X(1), (Vector(1) << 2.0).finished()); + initialValues.insert(X(2), zero(1)); + + VectorValues solution; + boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); + VectorValues expectedSolution; + expectedSolution.insert(X(1), (Vector(1) << 1.4).finished()); + expectedSolution.insert(X(2), (Vector(1) << 1.7).finished()); + CHECK(assert_equal(expectedSolution, solution, 1e-7)); +} + +/* ************************************************************************* */ + +TEST(QPSolver, failedSubproblem) { + QP qp; + qp.cost.push_back(JacobianFactor(X(1), eye(2), zero(2))); + qp.cost.push_back(HessianFactor(X(1), zeros(2, 2), zero(2), 100.0)); + qp.inequalities.push_back( + LinearInequality(X(1), (Matrix(1,2) << -1.0, 0.0).finished(), -1.0, 0)); + + VectorValues expected; + expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished()); + + VectorValues initialValues; + initialValues.insert(X(1), (Vector(2) << 10.0, 100.0).finished()); + + QPSolver solver(qp); + VectorValues solution; + boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); +// graph.print("Graph: "); +// solution.print("Solution: "); + CHECK(assert_equal(expected, solution, 1e-7)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam_unstable/nonlinear/AdaptAutoDiff.h b/gtsam_unstable/nonlinear/AdaptAutoDiff.h deleted file mode 100644 index 96978d9cf..000000000 --- a/gtsam_unstable/nonlinear/AdaptAutoDiff.h +++ /dev/null @@ -1,94 +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 AdaptAutoDiff.h - * @date October 22, 2014 - * @author Frank Dellaert - * @brief Adaptor for Ceres style auto-differentiated functions - */ - -#pragma once - -#include -#include - -namespace gtsam { - -/// Adapt ceres-style autodiff -template -class AdaptAutoDiff { - - static const int N = traits::dimension::value; - static const int M1 = traits::dimension::value; - static const int M2 = traits::dimension::value; - - typedef Eigen::Matrix RowMajor1; - typedef Eigen::Matrix RowMajor2; - - typedef Canonical CanonicalT; - typedef Canonical Canonical1; - typedef Canonical Canonical2; - - typedef typename CanonicalT::vector VectorT; - typedef typename Canonical1::vector Vector1; - typedef typename Canonical2::vector Vector2; - - // Instantiate function and charts - CanonicalT chartT; - Canonical1 chart1; - Canonical2 chart2; - F f; - -public: - - typedef Eigen::Matrix JacobianTA1; - typedef Eigen::Matrix JacobianTA2; - - T operator()(const A1& a1, const A2& a2, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) { - - using ceres::internal::AutoDiff; - - // Make arguments - Vector1 v1 = chart1.local(a1); - Vector2 v2 = chart2.local(a2); - - bool success; - VectorT result; - - if (H1 || H2) { - - // Get derivatives with AutoDiff - double *parameters[] = { v1.data(), v2.data() }; - double rowMajor1[N * M1], rowMajor2[N * M2]; // om the stack - double *jacobians[] = { rowMajor1, rowMajor2 }; - success = AutoDiff::Differentiate(f, parameters, 2, - result.data(), jacobians); - - // Convert from row-major to columnn-major - // TODO: if this is a bottleneck (probably not!) fix Autodiff to be Column-Major - *H1 = Eigen::Map(rowMajor1); - *H2 = Eigen::Map(rowMajor2); - - } else { - // Apply the mapping, to get result - success = f(v1.data(), v2.data(), result.data()); - } - if (!success) - throw std::runtime_error( - "AdaptAutoDiff: function call resulted in failure"); - return chartT.retract(result); - } - -}; - -} diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index ed4b66616..563da4a9f 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -28,25 +28,34 @@ namespace gtsam { /* ************************************************************************* */ -void BatchFixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const { +void BatchFixedLagSmoother::print(const std::string& s, + const KeyFormatter& keyFormatter) const { FixedLagSmoother::print(s, keyFormatter); // TODO: What else to print? } /* ************************************************************************* */ -bool BatchFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { - const BatchFixedLagSmoother* e = dynamic_cast (&rhs); - return e != NULL - && FixedLagSmoother::equals(*e, tol) - && factors_.equals(e->factors_, tol) - && theta_.equals(e->theta_, tol); +bool BatchFixedLagSmoother::equals(const FixedLagSmoother& rhs, + double tol) const { + const BatchFixedLagSmoother* e = + dynamic_cast(&rhs); + return e != NULL && FixedLagSmoother::equals(*e, tol) + && factors_.equals(e->factors_, tol) && theta_.equals(e->theta_, tol); } /* ************************************************************************* */ -FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyTimestampMap& timestamps) { +Matrix BatchFixedLagSmoother::marginalCovariance(Key key) const { + throw std::runtime_error( + "BatchFixedLagSmoother::marginalCovariance not implemented"); +} + +/* ************************************************************************* */ +FixedLagSmoother::Result BatchFixedLagSmoother::update( + const NonlinearFactorGraph& newFactors, const Values& newTheta, + const KeyTimestampMap& timestamps) { const bool debug = ISDEBUG("BatchFixedLagSmoother update"); - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::update() START" << std::endl; } @@ -70,11 +79,13 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap // Get current timestamp double current_timestamp = getCurrentTimestamp(); - if(debug) std::cout << "Current Timestamp: " << current_timestamp << std::endl; + if (debug) + std::cout << "Current Timestamp: " << current_timestamp << std::endl; // Find the set of variables to be marginalized out - std::set marginalizableKeys = findKeysBefore(current_timestamp - smootherLag_); - if(debug) { + std::set marginalizableKeys = findKeysBefore( + current_timestamp - smootherLag_); + if (debug) { std::cout << "Marginalizable Keys: "; BOOST_FOREACH(Key key, marginalizableKeys) { std::cout << DefaultKeyFormatter(key) << " "; @@ -90,19 +101,19 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap // Optimize gttic(optimize); Result result; - if(factors_.size() > 0) { + if (factors_.size() > 0) { result = optimize(); } gttoc(optimize); // Marginalize out old variables. gttic(marginalize); - if(marginalizableKeys.size() > 0) { + if (marginalizableKeys.size() > 0) { marginalize(marginalizableKeys); } gttoc(marginalize); - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::update() FINISH" << std::endl; } @@ -110,11 +121,12 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap } /* ************************************************************************* */ -void BatchFixedLagSmoother::insertFactors(const NonlinearFactorGraph& newFactors) { +void BatchFixedLagSmoother::insertFactors( + const NonlinearFactorGraph& newFactors) { BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) { Key index; // Insert the factor into an existing hole in the factor graph, if possible - if(availableSlots_.size() > 0) { + if (availableSlots_.size() > 0) { index = availableSlots_.front(); availableSlots_.pop(); factors_.replace(index, factor); @@ -130,9 +142,10 @@ void BatchFixedLagSmoother::insertFactors(const NonlinearFactorGraph& newFactors } /* ************************************************************************* */ -void BatchFixedLagSmoother::removeFactors(const std::set& deleteFactors) { +void BatchFixedLagSmoother::removeFactors( + const std::set& deleteFactors) { BOOST_FOREACH(size_t slot, deleteFactors) { - if(factors_.at(slot)) { + if (factors_.at(slot)) { // Remove references to this factor from the FactorIndex BOOST_FOREACH(Key key, *(factors_.at(slot))) { factorIndex_[key].erase(slot); @@ -143,7 +156,8 @@ void BatchFixedLagSmoother::removeFactors(const std::set& deleteFactors) availableSlots_.push(slot); } else { // TODO: Throw an error?? - std::cout << "Attempting to remove a factor from slot " << slot << ", but it is already NULL." << std::endl; + std::cout << "Attempting to remove a factor from slot " << slot + << ", but it is already NULL." << std::endl; } } } @@ -159,7 +173,7 @@ void BatchFixedLagSmoother::eraseKeys(const std::set& keys) { factorIndex_.erase(key); // Erase the key from the set of linearized keys - if(linearKeys_.exists(key)) { + if (linearKeys_.exists(key)) { linearKeys_.erase(key); } } @@ -178,11 +192,11 @@ void BatchFixedLagSmoother::reorder(const std::set& marginalizeKeys) { const bool debug = ISDEBUG("BatchFixedLagSmoother reorder"); - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::reorder() START" << std::endl; } - if(debug) { + if (debug) { std::cout << "Marginalizable Keys: "; BOOST_FOREACH(Key key, marginalizeKeys) { std::cout << DefaultKeyFormatter(key) << " "; @@ -191,13 +205,14 @@ void BatchFixedLagSmoother::reorder(const std::set& marginalizeKeys) { } // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 - ordering_ = Ordering::COLAMDConstrainedFirst(factors_, std::vector(marginalizeKeys.begin(), marginalizeKeys.end())); + ordering_ = Ordering::colamdConstrainedFirst(factors_, + std::vector(marginalizeKeys.begin(), marginalizeKeys.end())); - if(debug) { + if (debug) { ordering_.print("New Ordering: "); } - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::reorder() FINISH" << std::endl; } } @@ -207,7 +222,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { const bool debug = ISDEBUG("BatchFixedLagSmoother optimize"); - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::optimize() START" << std::endl; } @@ -231,14 +246,19 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { result.error = factors_.error(evalpoint); // check if we're already close enough - if(result.error <= errorTol) { - if(debug) { std::cout << "BatchFixedLagSmoother::optimize Exiting, as error = " << result.error << " < " << errorTol << std::endl; } + if (result.error <= errorTol) { + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize Exiting, as error = " + << result.error << " < " << errorTol << std::endl; + } return result; } - if(debug) { - std::cout << "BatchFixedLagSmoother::optimize linearValues: " << linearKeys_.size() << std::endl; - std::cout << "BatchFixedLagSmoother::optimize Initial error: " << result.error << std::endl; + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize linearValues: " + << linearKeys_.size() << std::endl; + std::cout << "BatchFixedLagSmoother::optimize Initial error: " + << result.error << std::endl; } // Use a custom optimization loop so the linearization points can be controlled @@ -254,9 +274,12 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_); // Keep increasing lambda until we make make progress - while(true) { + while (true) { - if(debug) { std::cout << "BatchFixedLagSmoother::optimize trying lambda = " << lambda << std::endl; } + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize trying lambda = " + << lambda << std::endl; + } // Add prior factors at the current solution gttic(damp); @@ -267,10 +290,11 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { double sigma = 1.0 / std::sqrt(lambda); BOOST_FOREACH(const VectorValues::KeyValuePair& key_value, delta_) { size_t dim = key_value.second.size(); - Matrix A = Matrix::Identity(dim,dim); + Matrix A = Matrix::Identity(dim, dim); Vector b = key_value.second; SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); - GaussianFactor::shared_ptr prior(new JacobianFactor(key_value.first, A, b, model)); + GaussianFactor::shared_ptr prior( + new JacobianFactor(key_value.first, A, b, model)); dampedFactorGraph.push_back(prior); } } @@ -279,7 +303,8 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { gttic(solve); // Solve Damped Gaussian Factor Graph - newDelta = dampedFactorGraph.optimize(ordering_, parameters_.getEliminationFunction()); + newDelta = dampedFactorGraph.optimize(ordering_, + parameters_.getEliminationFunction()); // update the evalpoint with the new delta evalpoint = theta_.retract(newDelta); gttoc(solve); @@ -289,12 +314,14 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { double error = factors_.error(evalpoint); gttoc(compute_error); - if(debug) { - std::cout << "BatchFixedLagSmoother::optimize linear delta norm = " << newDelta.norm() << std::endl; - std::cout << "BatchFixedLagSmoother::optimize next error = " << error << std::endl; + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize linear delta norm = " + << newDelta.norm() << std::endl; + std::cout << "BatchFixedLagSmoother::optimize next error = " << error + << std::endl; } - if(error < result.error) { + if (error < result.error) { // Keep this change // Update the error value result.error = error; @@ -303,7 +330,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { // Reset the deltas to zeros delta_.setZero(); // Put the linearization points and deltas back for specific variables - if(enforceConsistency_ && (linearKeys_.size() > 0)) { + if (enforceConsistency_ && (linearKeys_.size() > 0)) { theta_.update(linearKeys_); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linearKeys_) { delta_.at(key_value.key) = newDelta.at(key_value.key); @@ -311,16 +338,18 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { } // Decrease lambda for next time lambda /= lambdaFactor; - if(lambda < lambdaLowerBound) { + if (lambda < lambdaLowerBound) { lambda = lambdaLowerBound; } // End this lambda search iteration break; } else { // Reject this change - if(lambda >= lambdaUpperBound) { + if (lambda >= lambdaUpperBound) { // The maximum lambda has been used. Print a warning and end the search. - std::cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << std::endl; + std::cout + << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" + << std::endl; break; } else { // Increase lambda and continue searching @@ -331,15 +360,22 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { } gttoc(optimizer_iteration); - if(debug) { std::cout << "BatchFixedLagSmoother::optimize using lambda = " << lambda << std::endl; } + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize using lambda = " << lambda + << std::endl; + } result.iterations++; - } while(result.iterations < maxIterations && - !checkConvergence(relativeErrorTol, absoluteErrorTol, errorTol, previousError, result.error, NonlinearOptimizerParams::SILENT)); + } while (result.iterations < maxIterations + && !checkConvergence(relativeErrorTol, absoluteErrorTol, errorTol, + previousError, result.error, NonlinearOptimizerParams::SILENT)); - if(debug) { std::cout << "BatchFixedLagSmoother::optimize newError: " << result.error << std::endl; } + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize newError: " << result.error + << std::endl; + } - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::optimize() FINISH" << std::endl; } @@ -356,9 +392,10 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { const bool debug = ISDEBUG("BatchFixedLagSmoother marginalize"); - if(debug) std::cout << "BatchFixedLagSmoother::marginalize Begin" << std::endl; + if (debug) + std::cout << "BatchFixedLagSmoother::marginalize Begin" << std::endl; - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::marginalize Marginalize Keys: "; BOOST_FOREACH(Key key, marginalizeKeys) { std::cout << DefaultKeyFormatter(key) << " "; @@ -374,7 +411,7 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { removedFactorSlots.insert(slots.begin(), slots.end()); } - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::marginalize Removed Factor Slots: "; BOOST_FOREACH(size_t slot, removedFactorSlots) { std::cout << slot << " "; @@ -385,20 +422,24 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { // Add the removed factors to a factor graph NonlinearFactorGraph removedFactors; BOOST_FOREACH(size_t slot, removedFactorSlots) { - if(factors_.at(slot)) { + if (factors_.at(slot)) { removedFactors.push_back(factors_.at(slot)); } } - if(debug) { - PrintSymbolicGraph(removedFactors, "BatchFixedLagSmoother::marginalize Removed Factors: "); + if (debug) { + PrintSymbolicGraph(removedFactors, + "BatchFixedLagSmoother::marginalize Removed Factors: "); } // Calculate marginal factors on the remaining keys - NonlinearFactorGraph marginalFactors = calculateMarginalFactors(removedFactors, theta_, marginalizeKeys, parameters_.getEliminationFunction()); + NonlinearFactorGraph marginalFactors = calculateMarginalFactors( + removedFactors, theta_, marginalizeKeys, + parameters_.getEliminationFunction()); - if(debug) { - PrintSymbolicGraph(removedFactors, "BatchFixedLagSmoother::marginalize Marginal Factors: "); + if (debug) { + PrintSymbolicGraph(removedFactors, + "BatchFixedLagSmoother::marginalize Marginal Factors: "); } // Remove marginalized factors from the factor graph @@ -412,7 +453,8 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintKeySet(const std::set& keys, const std::string& label) { +void BatchFixedLagSmoother::PrintKeySet(const std::set& keys, + const std::string& label) { std::cout << label; BOOST_FOREACH(gtsam::Key key, keys) { std::cout << " " << gtsam::DefaultKeyFormatter(key); @@ -421,7 +463,8 @@ void BatchFixedLagSmoother::PrintKeySet(const std::set& keys, const std::st } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet& keys, const std::string& label) { +void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet& keys, + const std::string& label) { std::cout << label; BOOST_FOREACH(gtsam::Key key, keys) { std::cout << " " << gtsam::DefaultKeyFormatter(key); @@ -430,9 +473,10 @@ void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet& keys, const s } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor) { +void BatchFixedLagSmoother::PrintSymbolicFactor( + const NonlinearFactor::shared_ptr& factor) { std::cout << "f("; - if(factor) { + if (factor) { BOOST_FOREACH(Key key, factor->keys()) { std::cout << " " << gtsam::DefaultKeyFormatter(key); } @@ -443,7 +487,8 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(const NonlinearFactor::shared_pt } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor) { +void BatchFixedLagSmoother::PrintSymbolicFactor( + const GaussianFactor::shared_ptr& factor) { std::cout << "f("; BOOST_FOREACH(Key key, factor->keys()) { std::cout << " " << gtsam::DefaultKeyFormatter(key); @@ -452,7 +497,8 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label) { +void BatchFixedLagSmoother::PrintSymbolicGraph( + const NonlinearFactorGraph& graph, const std::string& label) { std::cout << label << std::endl; BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, graph) { PrintSymbolicFactor(factor); @@ -460,59 +506,79 @@ void BatchFixedLagSmoother::PrintSymbolicGraph(const NonlinearFactorGraph& graph } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label) { +void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, + const std::string& label) { std::cout << label << std::endl; BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { PrintSymbolicFactor(factor); } } - - /* ************************************************************************* */ -NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, - const std::set& marginalizeKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) { +NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors( + const NonlinearFactorGraph& graph, const Values& theta, + const std::set& marginalizeKeys, + const GaussianFactorGraph::Eliminate& eliminateFunction) { const bool debug = ISDEBUG("BatchFixedLagSmoother calculateMarginalFactors"); - if(debug) std::cout << "BatchFixedLagSmoother::calculateMarginalFactors START" << std::endl; + if (debug) + std::cout << "BatchFixedLagSmoother::calculateMarginalFactors START" + << std::endl; - if(debug) PrintKeySet(marginalizeKeys, "BatchFixedLagSmoother::calculateMarginalFactors Marginalize Keys: "); + if (debug) + PrintKeySet(marginalizeKeys, + "BatchFixedLagSmoother::calculateMarginalFactors Marginalize Keys: "); // Get the set of all keys involved in the factor graph FastSet allKeys(graph.keys()); - if(debug) PrintKeySet(allKeys, "BatchFixedLagSmoother::calculateMarginalFactors All Keys: "); + if (debug) + PrintKeySet(allKeys, + "BatchFixedLagSmoother::calculateMarginalFactors All Keys: "); // Calculate the set of RemainingKeys = AllKeys \Intersect marginalizeKeys FastSet remainingKeys; - std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(), marginalizeKeys.end(), std::inserter(remainingKeys, remainingKeys.end())); - if(debug) PrintKeySet(remainingKeys, "BatchFixedLagSmoother::calculateMarginalFactors Remaining Keys: "); + std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(), + marginalizeKeys.end(), std::inserter(remainingKeys, remainingKeys.end())); + if (debug) + PrintKeySet(remainingKeys, + "BatchFixedLagSmoother::calculateMarginalFactors Remaining Keys: "); - if(marginalizeKeys.size() == 0) { + if (marginalizeKeys.size() == 0) { // There are no keys to marginalize. Simply return the input factors - if(debug) std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" << std::endl; + if (debug) + std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" + << std::endl; return graph; } else { // Create the linear factor graph GaussianFactorGraph linearFactorGraph = *graph.linearize(theta); // .first is the eliminated Bayes tree, while .second is the remaining factor graph - GaussianFactorGraph marginalLinearFactors = *linearFactorGraph.eliminatePartialMultifrontal(std::vector(marginalizeKeys.begin(), marginalizeKeys.end())).second; + GaussianFactorGraph marginalLinearFactors = + *linearFactorGraph.eliminatePartialMultifrontal( + std::vector(marginalizeKeys.begin(), marginalizeKeys.end())).second; // Wrap in nonlinear container factors NonlinearFactorGraph marginalFactors; marginalFactors.reserve(marginalLinearFactors.size()); BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, marginalLinearFactors) { - marginalFactors += boost::make_shared(gaussianFactor, theta); - if(debug) { - std::cout << "BatchFixedLagSmoother::calculateMarginalFactors Marginal Factor: "; + marginalFactors += boost::make_shared( + gaussianFactor, theta); + if (debug) { + std::cout + << "BatchFixedLagSmoother::calculateMarginalFactors Marginal Factor: "; PrintSymbolicFactor(marginalFactors.back()); } } - if(debug) PrintSymbolicGraph(marginalFactors, "BatchFixedLagSmoother::calculateMarginalFactors All Marginal Factors: "); + if (debug) + PrintSymbolicGraph(marginalFactors, + "BatchFixedLagSmoother::calculateMarginalFactors All Marginal Factors: "); - if(debug) std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" << std::endl; + if (debug) + std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" + << std::endl; return marginalFactors; } diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index 9b2903a14..89da3d7e5 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -67,7 +67,7 @@ public: template VALUE calculateEstimate(Key key) const { const Vector delta = delta_.at(key); - return theta_.at(key).retract(delta); + return traits::Retract(theta_.at(key), delta); } /** read the current set of optimizer parameters */ @@ -100,6 +100,12 @@ public: return delta_; } + /// Calculate marginal covariance on given variable + Matrix marginalCovariance(Key key) const; + + static NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, + const std::set& marginalizeKeys, const GaussianFactorGraph::Eliminate& eliminateFunction); + protected: /** A typedef defining an Key-Factor mapping **/ @@ -134,8 +140,6 @@ protected: /** A cross-reference structure to allow efficient factor lookups by key **/ FactorIndex factorIndex_; - - /** Augment the list of factors with a set of new factors */ void insertFactors(const NonlinearFactorGraph& newFactors); @@ -154,9 +158,6 @@ protected: /** Marginalize out selected variables */ void marginalize(const std::set& marginalizableKeys); - static NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, - const std::set& marginalizeKeys, const GaussianFactorGraph::Eliminate& eliminateFunction); - private: /** Private methods for printing debug information */ static void PrintKeySet(const std::set& keys, const std::string& label); diff --git a/gtsam_unstable/nonlinear/CallRecord.h b/gtsam_unstable/nonlinear/CallRecord.h deleted file mode 100644 index 3806f1803..000000000 --- a/gtsam_unstable/nonlinear/CallRecord.h +++ /dev/null @@ -1,190 +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 CallRecord.h - * @date November 21, 2014 - * @author Frank Dellaert - * @author Paul Furgale - * @author Hannes Sommer - * @brief Internals for Expression.h, not for general consumption - */ - -#pragma once - -#include -#include -#include - -#include - -namespace gtsam { - -class JacobianMap; -// forward declaration - -//----------------------------------------------------------------------------- -/** - * MaxVirtualStaticRows defines how many separate virtual reverseAD with specific - * static rows (1..MaxVirtualStaticRows) methods will be part of the CallRecord interface. - */ -const int MaxVirtualStaticRows = 4; - -namespace internal { - -/** - * ConvertToDynamicIf converts to a dense matrix with dynamic rows iff - * ConvertToDynamicRows (colums stay as they are) otherwise - * it just passes dense Eigen matrices through. - */ -template -struct ConvertToDynamicRowsIf { - template - static Eigen::Matrix convert( - const Eigen::MatrixBase & x) { - return x; - } -}; - -template<> -struct ConvertToDynamicRowsIf { - template - static const Eigen::Matrix & convert( - const Eigen::Matrix & x) { - return x; - } -}; - -/** - * Recursive definition of an interface having several purely - * virtual _reverseAD(const Eigen::Matrix &, JacobianMap&) - * with Rows in 1..MaxSupportedStaticRows - */ -template -struct ReverseADInterface: ReverseADInterface { - using ReverseADInterface::_reverseAD; - virtual void _reverseAD( - const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const = 0; -}; - -template -struct ReverseADInterface<0, Cols> { - virtual void _reverseAD( - const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const = 0; - virtual void _reverseAD(const Matrix & dFdT, - JacobianMap& jacobians) const = 0; -}; - -/** - * ReverseADImplementor is a utility class used by CallRecordImplementor to - * implementing the recursive ReverseADInterface interface. - */ -template -struct ReverseADImplementor: ReverseADImplementor { -private: - using ReverseADImplementor::_reverseAD; - virtual void _reverseAD( - const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const { - static_cast(this)->reverseAD(dFdT, jacobians); - } - friend struct internal::ReverseADImplementor; -}; - -template -struct ReverseADImplementor : virtual internal::ReverseADInterface< - MaxVirtualStaticRows, Cols> { -private: - using internal::ReverseADInterface::_reverseAD; - const Derived & derived() const { - return static_cast(*this); - } - virtual void _reverseAD( - const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const { - derived().reverseAD(dFdT, jacobians); - } - virtual void _reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const { - derived().reverseAD(dFdT, jacobians); - } - friend struct internal::ReverseADImplementor; -}; - -} // namespace internal - -/** - * The CallRecord class stores the Jacobians of applying a function - * with respect to each of its arguments. It also stores an execution trace - * (defined below) for each of its arguments. - * - * It is implemented in the function-style ExpressionNode's nested Record class below. - */ -template -struct CallRecord: virtual private internal::ReverseADInterface< - MaxVirtualStaticRows, Cols> { - - inline void print(const std::string& indent) const { - _print(indent); - } - - inline void startReverseAD(JacobianMap& jacobians) const { - _startReverseAD(jacobians); - } - - template - inline void reverseAD(const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const { - _reverseAD( - internal::ConvertToDynamicRowsIf<(Rows > MaxVirtualStaticRows)>::convert( - dFdT), jacobians); - } - - inline void reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const { - _reverseAD(dFdT, jacobians); - } - - virtual ~CallRecord() { - } - -private: - virtual void _print(const std::string& indent) const = 0; - virtual void _startReverseAD(JacobianMap& jacobians) const = 0; - using internal::ReverseADInterface::_reverseAD; -}; - -namespace internal { -/** - * The CallRecordImplementor implements the CallRecord interface for a Derived class by - * delegating to its corresponding (templated) non-virtual methods. - */ -template -struct CallRecordImplementor: public CallRecord, - private ReverseADImplementor { -private: - const Derived & derived() const { - return static_cast(*this); - } - virtual void _print(const std::string& indent) const { - derived().print(indent); - } - virtual void _startReverseAD(JacobianMap& jacobians) const { - derived().startReverseAD(jacobians); - } - template friend struct ReverseADImplementor; -}; - -} // namespace internal - -} // gtsam diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 3bb9c7e44..fcaae9626 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -362,9 +362,9 @@ void ConcurrentBatchFilter::reorder(const boost::optional >& keysT // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 if(keysToMove && keysToMove->size() > 0) { - ordering_ = Ordering::COLAMDConstrainedFirst(factors_, std::vector(keysToMove->begin(), keysToMove->end())); + ordering_ = Ordering::colamdConstrainedFirst(factors_, std::vector(keysToMove->begin(), keysToMove->end())); }else{ - ordering_ = Ordering::COLAMD(factors_); + ordering_ = Ordering::colamd(factors_); } } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index fce28b214..9a458ee5a 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -253,4 +253,9 @@ void ConcurrentBatchFilter::PrintKeys(const Container& keys, const std::string& /// Typedef for Matlab wrapping typedef ConcurrentBatchFilter::Result ConcurrentBatchFilterResult; -}/// namespace gtsam +/// traits +template<> +struct traits : public Testable { +}; + +} // \ namespace gtsam diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 8606538bf..0f6515056 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -231,7 +231,7 @@ void ConcurrentBatchSmoother::reorder() { variableIndex_ = VariableIndex(factors_); FastList separatorKeys = separatorValues_.keys(); - ordering_ = Ordering::COLAMDConstrainedLast(variableIndex_, std::vector(separatorKeys.begin(), separatorKeys.end())); + ordering_ = Ordering::colamdConstrainedLast(variableIndex_, std::vector(separatorKeys.begin(), separatorKeys.end())); } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h index d85109605..f3808686c 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h @@ -204,4 +204,9 @@ private: /// Typedef for Matlab wrapping typedef ConcurrentBatchSmoother::Result ConcurrentBatchSmootherResult; -}/// namespace gtsam +/// traits +template<> +struct traits : public Testable { +}; + +} //\ namespace gtsam diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h index 4f2e1b0aa..11012674e 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h @@ -198,4 +198,9 @@ private: /// Typedef for Matlab wrapping typedef ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilterResult; -}/// namespace gtsam +/// traits +template<> +struct traits : public Testable { +}; + +} //\ namespace gtsam diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h index 5fe6effb2..78bab5079 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h @@ -168,4 +168,9 @@ private: /// Typedef for Matlab wrapping typedef ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmootherResult; -}/// namespace gtsam +/// traits +template<> +struct traits : public Testable { +}; + +} // \ namespace gtsam diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h deleted file mode 100644 index a98ab349f..000000000 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ /dev/null @@ -1,761 +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 Expression-inl.h - * @date September 18, 2014 - * @author Frank Dellaert - * @author Paul Furgale - * @brief Internals for Expression.h, not for general consumption - */ - -#pragma once - -#include -#include -#include -#include - -#include -#include -#include - -// template meta-programming headers -#include -namespace MPL = boost::mpl::placeholders; - -#include - -class ExpressionFactorBinaryTest; -// Forward declare for testing - -namespace gtsam { - -template -class Expression; - -/** - * Expressions are designed to write their derivatives into an already allocated - * Jacobian of the correct size, of type VerticalBlockMatrix. - * The JacobianMap provides a mapping from keys to the underlying blocks. - */ -class JacobianMap { - const FastVector& keys_; - VerticalBlockMatrix& Ab_; -public: - JacobianMap(const FastVector& keys, VerticalBlockMatrix& Ab) : - keys_(keys), Ab_(Ab) { - } - /// Access via key - VerticalBlockMatrix::Block operator()(Key key) { - FastVector::const_iterator it = std::find(keys_.begin(), keys_.end(), - key); - DenseIndex block = it - keys_.begin(); - return Ab_(block); - } -}; - -//----------------------------------------------------------------------------- -/// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians -template -void handleLeafCase(const Eigen::Matrix& dTdA, - JacobianMap& jacobians, Key key) { - jacobians(key).block(0, 0) += dTdA; // block makes HUGE difference -} -/// Handle Leaf Case for Dynamic ROWS Matrix type (slower) -template -inline void handleLeafCase( - const Eigen::Matrix& dTdA, - JacobianMap& jacobians, Key key) { - jacobians(key) += dTdA; -} - -//----------------------------------------------------------------------------- -/** - * The ExecutionTrace class records a tree-structured expression's execution. - * - * The class looks a bit complicated but it is so for performance. - * It is a tagged union that obviates the need to create - * a ExecutionTrace subclass for Constants and Leaf Expressions. Instead - * the key for the leaf is stored in the space normally used to store a - * CallRecord*. Nothing is stored for a Constant. - * - * A full execution trace of a Binary(Unary(Binary(Leaf,Constant)),Leaf) would be: - * Trace(Function) -> - * BinaryRecord with two traces in it - * trace1(Function) -> - * UnaryRecord with one trace in it - * trace1(Function) -> - * BinaryRecord with two traces in it - * trace1(Leaf) - * trace2(Constant) - * trace2(Leaf) - * Hence, there are three Record structs, written to memory by traceExecution - */ -template -class ExecutionTrace { - static const int Dim = traits::dimension::value; - enum { - Constant, Leaf, Function - } kind; - union { - Key key; - CallRecord* ptr; - } content; -public: - /// Pointer always starts out as a Constant - ExecutionTrace() : - kind(Constant) { - } - /// Change pointer to a Leaf Record - void setLeaf(Key key) { - kind = Leaf; - content.key = key; - } - /// Take ownership of pointer to a Function Record - void setFunction(CallRecord* record) { - kind = Function; - content.ptr = record; - } - /// Print - void print(const std::string& indent = "") const { - if (kind == Constant) - std::cout << indent << "Constant" << std::endl; - else if (kind == Leaf) - std::cout << indent << "Leaf, key = " << content.key << std::endl; - else if (kind == Function) { - std::cout << indent << "Function" << std::endl; - content.ptr->print(indent + " "); - } - } - /// Return record pointer, quite unsafe, used only for testing - template - boost::optional record() { - if (kind != Function) - return boost::none; - else { - Record* p = dynamic_cast(content.ptr); - return p ? boost::optional(p) : boost::none; - } - } - /** - * *** This is the main entry point for reverseAD, called from Expression *** - * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function) - */ - typedef Eigen::Matrix JacobianTT; - void startReverseAD(JacobianMap& jacobians) const { - if (kind == Leaf) { - // This branch will only be called on trivial Leaf expressions, i.e. Priors - static const JacobianTT I = JacobianTT::Identity(); - handleLeafCase(I, jacobians, content.key); - } else if (kind == Function) - // This is the more typical entry point, starting the AD pipeline - // Inside the startReverseAD that the correctly dimensioned pipeline is chosen. - content.ptr->startReverseAD(jacobians); - } - // Either add to Jacobians (Leaf) or propagate (Function) - template - void reverseAD(const Eigen::MatrixBase & dTdA, - JacobianMap& jacobians) const { - if (kind == Leaf) - handleLeafCase(dTdA.eval(), jacobians, content.key); - else if (kind == Function) - content.ptr->reverseAD(dTdA.eval(), jacobians); - } - - /// Define type so we can apply it as a meta-function - typedef ExecutionTrace type; -}; - -//----------------------------------------------------------------------------- -/** - * Expression node. The superclass for objects that do the heavy lifting - * An Expression has a pointer to an ExpressionNode underneath - * allowing Expressions to have polymorphic behaviour even though they - * are passed by value. This is the same way boost::function works. - * http://loki-lib.sourceforge.net/html/a00652.html - */ -template -class ExpressionNode { - -protected: - - size_t traceSize_; - - /// Constructor, traceSize is size of the execution trace of expression rooted here - ExpressionNode(size_t traceSize = 0) : - traceSize_(traceSize) { - } - -public: - - /// Destructor - virtual ~ExpressionNode() { - } - - /// Return keys that play in this expression as a set - virtual std::set keys() const { - std::set keys; - return keys; - } - - /// Return dimensions for each argument, as a map - virtual void dims(std::map& map) const { - } - - // Return size needed for memory buffer in traceExecution - size_t traceSize() const { - return traceSize_; - } - - /// Return value - virtual T value(const Values& values) const = 0; - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const = 0; -}; - -//----------------------------------------------------------------------------- -/// Constant Expression -template -class ConstantExpression: public ExpressionNode { - - /// The constant value - T constant_; - - /// Constructor with a value, yielding a constant - ConstantExpression(const T& value) : - constant_(value) { - } - - friend class Expression ; - -public: - - /// Return value - virtual T value(const Values& values) const { - return constant_; - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { - return constant_; - } -}; - -//----------------------------------------------------------------------------- -/// Leaf Expression -template > -class LeafExpression: public ExpressionNode { - typedef ChartValue value_type; // perhaps this can be something else like a std::pair ?? - - /// The key into values - Key key_; - - /// Constructor with a single key - LeafExpression(Key key) : - key_(key) { - } - // todo: do we need a virtual destructor here too? - - friend class Expression ; - -public: - - /// Return keys that play in this expression - virtual std::set keys() const { - std::set keys; - keys.insert(key_); - return keys; - } - - /// Return dimensions for each argument - virtual void dims(std::map& map) const { - // get dimension from the chart; only works for fixed dimension charts - map[key_] = traits::dimension::value; - } - - /// Return value - virtual const value_type& value(const Values& values) const { - return dynamic_cast(values.at(key_)); - } - - /// Construct an execution trace for reverse AD - virtual const value_type& traceExecution(const Values& values, - ExecutionTrace& trace, char* raw) const { - trace.setLeaf(key_); - return dynamic_cast(values.at(key_)); - } - -}; - -//----------------------------------------------------------------------------- -/// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value -template -class LeafExpression > : public ExpressionNode { - typedef T value_type; - - /// The key into values - Key key_; - - /// Constructor with a single key - LeafExpression(Key key) : - key_(key) { - } - // todo: do we need a virtual destructor here too? - - friend class Expression ; - -public: - - /// Return keys that play in this expression - virtual std::set keys() const { - std::set keys; - keys.insert(key_); - return keys; - } - - /// Return dimensions for each argument - virtual void dims(std::map& map) const { - map[key_] = traits::dimension::value; - } - - /// Return value - virtual T value(const Values& values) const { - return values.at(key_); - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { - trace.setLeaf(key_); - return values.at(key_); - } - -}; - -//----------------------------------------------------------------------------- -// Below we use the "Class Composition" technique described in the book -// C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost -// and Beyond. Abrahams, David; Gurtovoy, Aleksey. Pearson Education. -// to recursively generate a class, that will be the base for function nodes. -// -// The class generated, for three arguments A1, A2, and A3 will be -// -// struct Base1 : Argument, FunctionalBase { -// ... storage related to A1 ... -// ... methods that work on A1 ... -// }; -// -// struct Base2 : Argument, Base1 { -// ... storage related to A2 ... -// ... methods that work on A2 and (recursively) on A1 ... -// }; -// -// struct Base3 : Argument, Base2 { -// ... storage related to A3 ... -// ... methods that work on A3 and (recursively) on A2 and A1 ... -// }; -// -// struct FunctionalNode : Base3 { -// Provides convenience access to storage in hierarchy by using -// static_cast &>(*this) -// } -// -// All this magic happens when we generate the Base3 base class of FunctionalNode -// by invoking boost::mpl::fold over the meta-function GenerateFunctionalNode -// -// Similarly, the inner Record struct will be -// -// struct Record1 : JacobianTrace, CallRecord::value> { -// ... storage related to A1 ... -// ... methods that work on A1 ... -// }; -// -// struct Record2 : JacobianTrace, Record1 { -// ... storage related to A2 ... -// ... methods that work on A2 and (recursively) on A1 ... -// }; -// -// struct Record3 : JacobianTrace, Record2 { -// ... storage related to A3 ... -// ... methods that work on A3 and (recursively) on A2 and A1 ... -// }; -// -// struct Record : Record3 { -// Provides convenience access to storage in hierarchy by using -// static_cast &>(*this) -// } -// - -//----------------------------------------------------------------------------- - -/// meta-function to generate fixed-size JacobianTA type -template -struct Jacobian { - typedef Eigen::Matrix::value, - traits::dimension::value> type; -}; - -/// meta-function to generate JacobianTA optional reference -template -struct OptionalJacobian { - typedef Eigen::Matrix::value, - traits::dimension::value> Jacobian; - typedef boost::optional type; -}; - -/** - * Base case for recursive FunctionalNode class - */ -template -struct FunctionalBase: ExpressionNode { - static size_t const N = 0; // number of arguments - - struct Record { - void print(const std::string& indent) const { - } - void startReverseAD(JacobianMap& jacobians) const { - } - template - void reverseAD(const SomeMatrix & dFdT, JacobianMap& jacobians) const { - } - }; - /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, char*& raw) const { - // base case: does not do anything - } -}; - -/** - * Building block for recursive FunctionalNode class - * The integer argument N is to guarantee a unique type signature, - * so we are guaranteed to be able to extract their values by static cast. - */ -template -struct Argument { - /// Expression that will generate value/derivatives for argument - boost::shared_ptr > expression; -}; - -/** - * Building block for Recursive Record Class - * Records the evaluation of a single argument in a functional expression - */ -template -struct JacobianTrace { - A value; - ExecutionTrace trace; - typename Jacobian::type dTdA; -}; - -/** - * Recursive Definition of Functional ExpressionNode - */ -template -struct GenerateFunctionalNode: Argument, Base { - - static size_t const N = Base::N + 1; ///< Number of arguments in hierarchy - typedef Argument This; ///< The storage we have direct access to - - /// Return keys that play in this expression - virtual std::set keys() const { - std::set keys = Base::keys(); - std::set myKeys = This::expression->keys(); - keys.insert(myKeys.begin(), myKeys.end()); - return keys; - } - - /// Return dimensions for each argument - virtual void dims(std::map& map) const { - Base::dims(map); - This::expression->dims(map); - } - - /// Recursive Record Class for Functional Expressions - struct Record: JacobianTrace, Base::Record { - - typedef T return_type; - typedef JacobianTrace This; - - /// Print to std::cout - void print(const std::string& indent) const { - Base::Record::print(indent); - static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); - std::cout << This::dTdA.format(matlab) << std::endl; - This::trace.print(indent); - } - - /// Start the reverse AD process - void startReverseAD(JacobianMap& jacobians) const { - Base::Record::startReverseAD(jacobians); - This::trace.reverseAD(This::dTdA, jacobians); - } - - /// Given df/dT, multiply in dT/dA and continue reverse AD process - template - void reverseAD(const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const { - Base::Record::reverseAD(dFdT, jacobians); - This::trace.reverseAD(dFdT * This::dTdA, jacobians); - } - }; - - /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, char*& raw) const { - Base::trace(values, record, raw); // recurse - // Write an Expression execution trace in record->trace - // Iff Constant or Leaf, this will not write to raw, only to trace. - // Iff the expression is functional, write all Records in raw buffer - // Return value of type T is recorded in record->value - record->Record::This::value = This::expression->traceExecution(values, - record->Record::This::trace, raw); - // raw is never modified by traceExecution, but if traceExecution has - // written in the buffer, the next caller expects we advance the pointer - raw += This::expression->traceSize(); - } -}; - -/** - * Recursive GenerateFunctionalNode class Generator - */ -template -struct FunctionalNode { - - /// The following typedef generates the recursively defined Base class - typedef typename boost::mpl::fold, - GenerateFunctionalNode >::type Base; - - /** - * The type generated by this meta-function derives from Base - * and adds access functions as well as the crucial [trace] function - */ - struct type: public Base { - - // Argument types and derived, note these are base 0 ! - // These are currently not used - useful for Phoenix in future -#ifdef EXPRESSIONS_PHOENIX - typedef TYPES Arguments; - typedef typename boost::mpl::transform >::type Jacobians; - typedef typename boost::mpl::transform >::type Optionals; -#endif - - /// Reset expression shared pointer - template - void reset(const boost::shared_ptr >& ptr) { - static_cast &>(*this).expression = ptr; - } - - /// Access Expression shared pointer - template - boost::shared_ptr > expression() const { - return static_cast const &>(*this).expression; - } - - /// Provide convenience access to Record storage and implement - /// the virtual function based interface of CallRecord using the CallRecordImplementor - struct Record: public internal::CallRecordImplementor::value>, public Base::Record { - using Base::Record::print; - using Base::Record::startReverseAD; - using Base::Record::reverseAD; - - virtual ~Record() { - } - - /// Access Value - template - const A& value() const { - return static_cast const &>(*this).value; - } - - /// Access Jacobian - template - typename Jacobian::type& jacobian() { - return static_cast&>(*this).dTdA; - } - }; - - /// Construct an execution trace for reverse AD - Record* trace(const Values& values, char* raw) const { - - // Create the record and advance the pointer - Record* record = new (raw) Record(); - raw = (char*) (record + 1); - - // Record the traces for all arguments - // After this, the raw pointer is set to after what was written - Base::trace(values, record, raw); - - // Return the record for this function evaluation - return record; - } - }; -}; -//----------------------------------------------------------------------------- - -/// Unary Function Expression -template -class UnaryExpression: public FunctionalNode >::type { - -public: - - typedef boost::function::type)> Function; - typedef typename FunctionalNode >::type Base; - typedef typename Base::Record Record; - -private: - - Function function_; - - /// Constructor with a unary function f, and input argument e - UnaryExpression(Function f, const Expression& e1) : - function_(f) { - this->template reset(e1.root()); - ExpressionNode::traceSize_ = sizeof(Record) + e1.traceSize(); - } - - friend class Expression ; - -public: - - /// Return value - virtual T value(const Values& values) const { - return function_(this->template expression()->value(values), boost::none); - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { - - Record* record = Base::trace(values, raw); - trace.setFunction(record); - - return function_(record->template value(), - record->template jacobian()); - } -}; - -//----------------------------------------------------------------------------- -/// Binary Expression - -template -class BinaryExpression: public FunctionalNode >::type { - -public: - - typedef boost::function< - T(const A1&, const A2&, typename OptionalJacobian::type, - typename OptionalJacobian::type)> Function; - typedef typename FunctionalNode >::type Base; - typedef typename Base::Record Record; - -private: - - Function function_; - - /// Constructor with a ternary function f, and three input arguments - BinaryExpression(Function f, const Expression& e1, - const Expression& e2) : - function_(f) { - this->template reset(e1.root()); - this->template reset(e2.root()); - ExpressionNode::traceSize_ = // - sizeof(Record) + e1.traceSize() + e2.traceSize(); - } - - friend class Expression ; - friend class ::ExpressionFactorBinaryTest; - -public: - - /// Return value - virtual T value(const Values& values) const { - using boost::none; - return function_(this->template expression()->value(values), - this->template expression()->value(values), - none, none); - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { - - Record* record = Base::trace(values, raw); - trace.setFunction(record); - - return function_(record->template value(), - record->template value(), record->template jacobian(), - record->template jacobian()); - } -}; - -//----------------------------------------------------------------------------- -/// Ternary Expression - -template -class TernaryExpression: public FunctionalNode >::type { - -public: - - typedef boost::function< - T(const A1&, const A2&, const A3&, typename OptionalJacobian::type, - typename OptionalJacobian::type, - typename OptionalJacobian::type)> Function; - typedef typename FunctionalNode >::type Base; - typedef typename Base::Record Record; - -private: - - Function function_; - - /// Constructor with a ternary function f, and three input arguments - TernaryExpression(Function f, const Expression& e1, - const Expression& e2, const Expression& e3) : - function_(f) { - this->template reset(e1.root()); - this->template reset(e2.root()); - this->template reset(e3.root()); - ExpressionNode::traceSize_ = // - sizeof(Record) + e1.traceSize() + e2.traceSize() + e3.traceSize(); - } - - friend class Expression ; - -public: - - /// Return value - virtual T value(const Values& values) const { - using boost::none; - return function_(this->template expression()->value(values), - this->template expression()->value(values), - this->template expression()->value(values), - none, none, none); - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { - - Record* record = Base::trace(values, raw); - trace.setFunction(record); - - return function_( - record->template value(), record->template value(), - record->template value(), record->template jacobian(), - record->template jacobian(), record->template jacobian()); - } - -}; -//----------------------------------------------------------------------------- -} diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h deleted file mode 100644 index e8bd8bbe7..000000000 --- a/gtsam_unstable/nonlinear/Expression.h +++ /dev/null @@ -1,184 +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 Expression.h - * @date September 18, 2014 - * @author Frank Dellaert - * @author Paul Furgale - * @brief Expressions for Block Automatic Differentiation - */ - -#pragma once - -#include "Expression-inl.h" -#include -#include - -namespace gtsam { - -/** - * Expression class that supports automatic differentiation - */ -template -class Expression { - -private: - - // Paul's trick shared pointer, polymorphic root of entire expression tree - boost::shared_ptr > root_; - -public: - - // Construct a constant expression - Expression(const T& value) : - root_(new ConstantExpression(value)) { - } - - // Construct a leaf expression, with Key - Expression(const Key& key) : - root_(new LeafExpression(key)) { - } - - // Construct a leaf expression, with Symbol - Expression(const Symbol& symbol) : - root_(new LeafExpression(symbol)) { - } - - // Construct a leaf expression, creating Symbol - Expression(unsigned char c, size_t j) : - root_(new LeafExpression(Symbol(c, j))) { - } - - /// Construct a nullary method expression - template - Expression(const Expression& expression, - T (A::*method)(typename OptionalJacobian::type) const) : - root_(new UnaryExpression(boost::bind(method, _1, _2), expression)) { - } - - /// Construct a unary function expression - template - Expression(typename UnaryExpression::Function function, - const Expression& expression) : - root_(new UnaryExpression(function, expression)) { - } - - /// Construct a unary method expression - template - Expression(const Expression& expression1, - T (A1::*method)(const A2&, typename OptionalJacobian::type, - typename OptionalJacobian::type) const, - const Expression& expression2) : - root_( - new BinaryExpression(boost::bind(method, _1, _2, _3, _4), - expression1, expression2)) { - } - - /// Construct a binary function expression - template - Expression(typename BinaryExpression::Function function, - const Expression& expression1, const Expression& expression2) : - root_( - new BinaryExpression(function, expression1, expression2)) { - } - - /// Construct a ternary function expression - template - Expression(typename TernaryExpression::Function function, - const Expression& expression1, const Expression& expression2, - const Expression& expression3) : - root_( - new TernaryExpression(function, expression1, - expression2, expression3)) { - } - - /// Return keys that play in this expression - std::set keys() const { - return root_->keys(); - } - - /// Return dimensions for each argument, as a map - void dims(std::map& map) const { - root_->dims(map); - } - - // Return size needed for memory buffer in traceExecution - size_t traceSize() const { - return root_->traceSize(); - } - - /// trace execution, very unsafe, for testing purposes only //TODO this is not only used for testing, but in value() below! - T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { - return root_->traceExecution(values, trace, raw); - } - - /// Return value and derivatives, reverse AD version - T value(const Values& values, JacobianMap& jacobians) const { - // The following piece of code is absolutely crucial for performance. - // We allocate a block of memory on the stack, which can be done at runtime - // with modern C++ compilers. The traceExecution then fills this memory - // with an execution trace, made up entirely of "Record" structs, see - // the FunctionalNode class in expression-inl.h - size_t size = traceSize(); - char raw[size]; - ExecutionTrace trace; - T value(traceExecution(values, trace, raw)); - trace.startReverseAD(jacobians); - return value; - } - - /// Return value - T value(const Values& values) const { - return root_->value(values); - } - - const boost::shared_ptr >& root() const { - return root_; - } - - /// Define type so we can apply it as a meta-function - typedef Expression type; -}; - -// http://stackoverflow.com/questions/16260445/boost-bind-to-operator -template -struct apply_compose { - typedef T result_type; - static const int Dim = traits::dimension::value; - typedef Eigen::Matrix Jacobian; - T operator()(const T& x, const T& y, boost::optional H1, - boost::optional H2) const { - return x.compose(y, H1, H2); - } -}; - -/// Construct a product expression, assumes T::compose(T) -> T -template -Expression operator*(const Expression& expression1, - const Expression& expression2) { - return Expression(boost::bind(apply_compose(), _1, _2, _3, _4), - expression1, expression2); -} - -/// Construct an array of leaves -template -std::vector > createUnknowns(size_t n, char c, size_t start = 0) { - std::vector > unknowns; - unknowns.reserve(n); - for (size_t i = start; i < start + n; i++) - unknowns.push_back(Expression(c, i)); - return unknowns; -} - -} - diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp index 66d367148..369db51c3 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp @@ -37,7 +37,7 @@ bool FixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { void FixedLagSmoother::updateKeyTimestampMap(const KeyTimestampMap& timestamps) { // Loop through each key and add/update it in the map BOOST_FOREACH(const KeyTimestampMap::value_type& key_timestamp, timestamps) { - // Check to see if this key already exists inthe database + // Check to see if this key already exists in the database KeyTimestampMap::iterator keyIter = keyTimestampMap_.find(key_timestamp.first); // If the key already exists diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index b5556994c..23cd42a0a 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -25,10 +25,13 @@ namespace gtsam { /* ************************************************************************* */ -void recursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set& additionalKeys) { +void recursiveMarkAffectedKeys(const Key& key, + const ISAM2Clique::shared_ptr& clique, std::set& additionalKeys) { // Check if the separator keys of the current clique contain the specified key - if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), key) != clique->conditional()->endParents()) { + if (std::find(clique->conditional()->beginParents(), + clique->conditional()->endParents(), key) + != clique->conditional()->endParents()) { // Mark the frontal keys of the current clique BOOST_FOREACH(Key i, clique->conditional()->frontals()) { @@ -44,32 +47,36 @@ void recursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& cl } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const { +void IncrementalFixedLagSmoother::print(const std::string& s, + const KeyFormatter& keyFormatter) const { FixedLagSmoother::print(s, keyFormatter); // TODO: What else to print? } /* ************************************************************************* */ -bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { - const IncrementalFixedLagSmoother* e = dynamic_cast (&rhs); - return e != NULL - && FixedLagSmoother::equals(*e, tol) +bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs, + double tol) const { + const IncrementalFixedLagSmoother* e = + dynamic_cast(&rhs); + return e != NULL && FixedLagSmoother::equals(*e, tol) && isam_.equals(e->isam_, tol); } /* ************************************************************************* */ -FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyTimestampMap& timestamps) { +FixedLagSmoother::Result IncrementalFixedLagSmoother::update( + const NonlinearFactorGraph& newFactors, const Values& newTheta, + const KeyTimestampMap& timestamps) { const bool debug = ISDEBUG("IncrementalFixedLagSmoother update"); - if(debug) { + if (debug) { std::cout << "IncrementalFixedLagSmoother::update() Start" << std::endl; PrintSymbolicTree(isam_, "Bayes Tree Before Update:"); std::cout << "END" << std::endl; } FastVector removedFactors; - boost::optional > constrainedKeys = boost::none; + boost::optional > constrainedKeys = boost::none; // Update the Timestamps associated with the factor keys updateKeyTimestampMap(timestamps); @@ -77,12 +84,14 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact // Get current timestamp double current_timestamp = getCurrentTimestamp(); - if(debug) std::cout << "Current Timestamp: " << current_timestamp << std::endl; + if (debug) + std::cout << "Current Timestamp: " << current_timestamp << std::endl; // Find the set of variables to be marginalized out - std::set marginalizableKeys = findKeysBefore(current_timestamp - smootherLag_); + std::set marginalizableKeys = findKeysBefore( + current_timestamp - smootherLag_); - if(debug) { + if (debug) { std::cout << "Marginalizable Keys: "; BOOST_FOREACH(Key key, marginalizableKeys) { std::cout << DefaultKeyFormatter(key) << " "; @@ -93,11 +102,13 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact // Force iSAM2 to put the marginalizable variables at the beginning createOrderingConstraints(marginalizableKeys, constrainedKeys); - if(debug) { + if (debug) { std::cout << "Constrained Keys: "; - if(constrainedKeys) { - for(FastMap::const_iterator iter = constrainedKeys->begin(); iter != constrainedKeys->end(); ++iter) { - std::cout << DefaultKeyFormatter(iter->first) << "(" << iter->second << ") "; + if (constrainedKeys) { + for (FastMap::const_iterator iter = constrainedKeys->begin(); + iter != constrainedKeys->end(); ++iter) { + std::cout << DefaultKeyFormatter(iter->first) << "(" << iter->second + << ") "; } } std::cout << std::endl; @@ -114,23 +125,26 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end()); // Update iSAM2 - ISAM2Result isamResult = isam_.update(newFactors, newTheta, FastVector(), constrainedKeys, boost::none, additionalMarkedKeys); + ISAM2Result isamResult = isam_.update(newFactors, newTheta, + FastVector(), constrainedKeys, boost::none, additionalMarkedKeys); - if(debug) { - PrintSymbolicTree(isam_, "Bayes Tree After Update, Before Marginalization:"); + if (debug) { + PrintSymbolicTree(isam_, + "Bayes Tree After Update, Before Marginalization:"); std::cout << "END" << std::endl; } // Marginalize out any needed variables - if(marginalizableKeys.size() > 0) { - FastList leafKeys(marginalizableKeys.begin(), marginalizableKeys.end()); + if (marginalizableKeys.size() > 0) { + FastList leafKeys(marginalizableKeys.begin(), + marginalizableKeys.end()); isam_.marginalizeLeaves(leafKeys); } // Remove marginalized keys from the KeyTimestampMap eraseKeyTimestampMap(marginalizableKeys); - if(debug) { + if (debug) { PrintSymbolicTree(isam_, "Final Bayes Tree:"); std::cout << "END" << std::endl; } @@ -142,7 +156,8 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact result.nonlinearVariables = 0; result.error = 0; - if(debug) std::cout << "IncrementalFixedLagSmoother::update() Finish" << std::endl; + if (debug) + std::cout << "IncrementalFixedLagSmoother::update() Finish" << std::endl; return result; } @@ -151,30 +166,33 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact void IncrementalFixedLagSmoother::eraseKeysBefore(double timestamp) { TimestampKeyMap::iterator end = timestampKeyMap_.lower_bound(timestamp); TimestampKeyMap::iterator iter = timestampKeyMap_.begin(); - while(iter != end) { + while (iter != end) { keyTimestampMap_.erase(iter->second); timestampKeyMap_.erase(iter++); } } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::createOrderingConstraints(const std::set& marginalizableKeys, boost::optional >& constrainedKeys) const { - if(marginalizableKeys.size() > 0) { - constrainedKeys = FastMap(); +void IncrementalFixedLagSmoother::createOrderingConstraints( + const std::set& marginalizableKeys, + boost::optional >& constrainedKeys) const { + if (marginalizableKeys.size() > 0) { + constrainedKeys = FastMap(); // Generate ordering constraints so that the marginalizable variables will be eliminated first // Set all variables to Group1 BOOST_FOREACH(const TimestampKeyMap::value_type& timestamp_key, timestampKeyMap_) { constrainedKeys->operator[](timestamp_key.second) = 1; } // Set marginalizable variables to Group0 - BOOST_FOREACH(Key key, marginalizableKeys){ + BOOST_FOREACH(Key key, marginalizableKeys) { constrainedKeys->operator[](key) = 0; } } } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintKeySet(const std::set& keys, const std::string& label) { +void IncrementalFixedLagSmoother::PrintKeySet(const std::set& keys, + const std::string& label) { std::cout << label; BOOST_FOREACH(gtsam::Key key, keys) { std::cout << " " << gtsam::DefaultKeyFormatter(key); @@ -183,7 +201,8 @@ void IncrementalFixedLagSmoother::PrintKeySet(const std::set& keys, const s } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor) { +void IncrementalFixedLagSmoother::PrintSymbolicFactor( + const GaussianFactor::shared_ptr& factor) { std::cout << "f("; BOOST_FOREACH(gtsam::Key key, factor->keys()) { std::cout << " " << gtsam::DefaultKeyFormatter(key); @@ -192,7 +211,8 @@ void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shar } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label) { +void IncrementalFixedLagSmoother::PrintSymbolicGraph( + const GaussianFactorGraph& graph, const std::string& label) { std::cout << label << std::endl; BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { PrintSymbolicFactor(factor); @@ -200,27 +220,28 @@ void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label) { +void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam, + const std::string& label) { std::cout << label << std::endl; - if(!isam.roots().empty()) - { - BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots()){ - PrintSymbolicTreeHelper(root); + if (!isam.roots().empty()) { + BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots()) { + PrintSymbolicTreeHelper(root); } - } - else + } else std::cout << "{Empty Tree}" << std::endl; } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent) { +void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper( + const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent) { // Print the current clique std::cout << indent << "P( "; BOOST_FOREACH(gtsam::Key key, clique->conditional()->frontals()) { std::cout << gtsam::DefaultKeyFormatter(key) << " "; } - if(clique->conditional()->nrParents() > 0) std::cout << "| "; + if (clique->conditional()->nrParents() > 0) + std::cout << "| "; BOOST_FOREACH(gtsam::Key key, clique->conditional()->parents()) { std::cout << gtsam::DefaultKeyFormatter(key) << " "; } @@ -228,7 +249,7 @@ void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(const gtsam::ISAM2Cliq // Recursively print all of the children BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) { - PrintSymbolicTreeHelper(child, indent+" "); + PrintSymbolicTreeHelper(child, indent + " "); } } diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index 9f015ef19..31ae20c50 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -23,7 +23,6 @@ #include #include - namespace gtsam { /** @@ -31,7 +30,7 @@ namespace gtsam { * such that the active states are placed in/near the root. This base class implements a function * to calculate the ordering, and an update function to incorporate new factors into the HMF. */ -class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother : public FixedLagSmoother { +class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother: public FixedLagSmoother { public: @@ -39,20 +38,30 @@ public: typedef boost::shared_ptr shared_ptr; /** default constructor */ - IncrementalFixedLagSmoother(double smootherLag = 0.0, const ISAM2Params& parameters = ISAM2Params()) - : FixedLagSmoother(smootherLag), isam_(parameters) {} + IncrementalFixedLagSmoother(double smootherLag = 0.0, + const ISAM2Params& parameters = ISAM2Params()) : + FixedLagSmoother(smootherLag), isam_(parameters) { + } /** destructor */ - virtual ~IncrementalFixedLagSmoother() {} + virtual ~IncrementalFixedLagSmoother() { + } /** Print the factor for debugging and testing (implementing Testable) */ - virtual void print(const std::string& s = "IncrementalFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print(const std::string& s = "IncrementalFixedLagSmoother:\n", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** Check if two IncrementalFixedLagSmoother Objects are equal */ virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const; - /** Add new factors, updating the solution and relinearizing as needed. */ - Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), + /** + * Add new factors, updating the solution and re-linearizing as needed. + * @param newFactors new factors on old and/or new variables + * @param newTheta new values for new variables only + * @param timestamps an (optional) map from keys to real time stamps + */ + Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), + const Values& newTheta = Values(), // const KeyTimestampMap& timestamps = KeyTimestampMap()); /** Compute an estimate from the incomplete linear delta computed during the last update. @@ -94,6 +103,11 @@ public: return isam_.getDelta(); } + /// Calculate marginal covariance on given variable + Matrix marginalCovariance(Key key) const { + return isam_.marginalCovariance(key); + } + protected: /** An iSAM2 object used to perform inference. The smoother lag is controlled * by what factors are removed each iteration */ @@ -103,16 +117,23 @@ protected: void eraseKeysBefore(double timestamp); /** Fill in an iSAM2 ConstrainedKeys structure such that the provided keys are eliminated before all others */ - void createOrderingConstraints(const std::set& marginalizableKeys, boost::optional >& constrainedKeys) const; + void createOrderingConstraints(const std::set& marginalizableKeys, + boost::optional >& constrainedKeys) const; private: /** Private methods for printing debug information */ - static void PrintKeySet(const std::set& keys, const std::string& label = "Keys:"); + static void PrintKeySet(const std::set& keys, const std::string& label = + "Keys:"); static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor); - static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label = "Factor Graph:"); - static void PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label = "Bayes Tree:"); - static void PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent = ""); + static void PrintSymbolicGraph(const GaussianFactorGraph& graph, + const std::string& label = "Factor Graph:"); + static void PrintSymbolicTree(const gtsam::ISAM2& isam, + const std::string& label = "Bayes Tree:"); + static void PrintSymbolicTreeHelper( + const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent = + ""); -}; // IncrementalFixedLagSmoother +}; +// IncrementalFixedLagSmoother -} /// namespace gtsam +}/// namespace gtsam diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h index 7be219187..c6710bd70 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -62,7 +62,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("LinearizedGaussianFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(lin_points_); @@ -149,15 +149,17 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("LinearizedJacobianFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(Ab_); } }; - - +/// traits +template<> +struct traits : public Testable { +}; /** * A factor that takes a linear, Hessian factor and inserts it into @@ -262,13 +264,16 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("LinearizedHessianFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(info_); } }; - +/// traits +template<> +struct traits : public Testable { +}; } // \namespace aspn diff --git a/gtsam_unstable/nonlinear/tests/CMakeLists.txt b/gtsam_unstable/nonlinear/tests/CMakeLists.txt index 5b1fd07d4..ad14fcd9b 100644 --- a/gtsam_unstable/nonlinear/tests/CMakeLists.txt +++ b/gtsam_unstable/nonlinear/tests/CMakeLists.txt @@ -1 +1 @@ -gtsamAddTestsGlob(nonlinear_unstable "test*.cpp" "" "gtsam_unstable") +gtsamAddTestsGlob(nonlinear_unstable "test*.cpp" "testCustomChartExpression.cpp" "gtsam_unstable") diff --git a/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp new file mode 100644 index 000000000..440400fb4 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp @@ -0,0 +1,113 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------1------------------------------------------- */ + +/** + * @file testCustomChartExpression.cpp + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief unit tests for Block Automatic Differentiation + */ + +#include +#include + +using namespace gtsam; + +// A simple prototype custom chart that does two things: +// 1. it reduces the dimension of the variable being estimated +// 2. it scales the input to retract. +// +// The Jacobian of this chart is: +// [ 2 0 ] +// [ 0 3 ] +// [ 0 0 ] +struct ProjectionChart { + typedef Eigen::Vector3d type; + typedef Eigen::Vector2d vector; + static vector local(const type& origin, const type& other) { + return (other - origin).head<2>(); + } + static type retract(const type& origin, const vector& d) { + return origin + Eigen::Vector3d(2.0 * d[0], 3.0 * d[1], 0.0); + } + static int getDimension(const type& origin) { + return 2; + } +}; + +namespace gtsam { +namespace traits { +template<> struct is_chart : public boost::true_type {}; +template<> struct dimension : public boost::integral_constant {}; +} // namespace traits +} // namespace gtsam + +TEST(ExpressionCustomChart, projection) { + // Create expression + Expression point(1); + + Eigen::Vector3d pval(1.0, 2.0, 3.0); + Values standard; + standard.insert(1, pval); + + Values custom; + custom.insert(1, pval, ProjectionChart()); + + Eigen::Vector3d pstandard = point.value(standard); + Eigen::Vector3d pcustom = point.value(custom); + + // The values should be the same. + EXPECT(assert_equal(Matrix(pstandard), Matrix(pcustom), 1e-10)); + + + EXPECT_LONGS_EQUAL(3, standard.at(1).dim()); + VectorValues vstandard = standard.zeroVectors(); + EXPECT_LONGS_EQUAL(3, vstandard.at(1).size()); + + + EXPECT_LONGS_EQUAL(2, custom.at(1).dim()); + VectorValues vcustom = custom.zeroVectors(); + EXPECT_LONGS_EQUAL(2, vcustom.at(1).size()); + + ExpressionFactor f(noiseModel::Unit::Create(pval.size()), pval, point); + + boost::shared_ptr gfstandard = f.linearize(standard); + boost::shared_ptr jfstandard = // + boost::dynamic_pointer_cast(gfstandard); + + typedef std::pair Jacobian; + Jacobian Jstandard = jfstandard->jacobianUnweighted(); + EXPECT(assert_equal(Eigen::Matrix3d::Identity(), Jstandard.first, 1e-10)); + + boost::shared_ptr gfcustom = f.linearize(custom); + boost::shared_ptr jfcustom = // + boost::dynamic_pointer_cast(gfcustom); + + Eigen::MatrixXd expectedJacobian = Eigen::MatrixXd::Zero(3,2); + expectedJacobian(0,0) = 2.0; + expectedJacobian(1,1) = 3.0; + Jacobian Jcustom = jfcustom->jacobianUnweighted(); + EXPECT(assert_equal(expectedJacobian, Jcustom.first, 1e-10)); + + // Amazingly, the finite differences get the expected Jacobian right. + const double fd_step = 1e-5; + const double tolerance = 1e-5; + EXPECT_CORRECT_EXPRESSION_JACOBIANS(point, custom, fd_step, tolerance); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp deleted file mode 100644 index d10e31002..000000000 --- a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp +++ /dev/null @@ -1,247 +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 testExpressionMeta.cpp - * @date October 14, 2014 - * @author Frank Dellaert - * @brief Test meta-programming constructs for Expressions - */ - -#include -#include -#include -#include - -#include -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -namespace mpl = boost::mpl; - -#include -#include -template struct Incomplete; - -// Check generation of FunctionalNode -typedef mpl::vector MyTypes; -typedef FunctionalNode::type Generated; -//Incomplete incomplete; - -// Try generating vectors of ExecutionTrace -typedef mpl::vector, ExecutionTrace > ExpectedTraces; - -typedef mpl::transform >::type MyTraces; -BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces >)); - -template -struct MakeTrace { - typedef ExecutionTrace type; -}; -typedef mpl::transform >::type MyTraces1; -BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces1 >)); - -// Try generating vectors of Expression types -typedef mpl::vector, Expression > ExpectedExpressions; -typedef mpl::transform >::type Expressions; -BOOST_MPL_ASSERT((mpl::equal< ExpectedExpressions, Expressions >)); - -// Try generating vectors of Jacobian types -typedef mpl::vector ExpectedJacobians; -typedef mpl::transform >::type Jacobians; -BOOST_MPL_ASSERT((mpl::equal< ExpectedJacobians, Jacobians >)); - -// Try accessing a Jacobian -typedef mpl::at_c::type Jacobian23; // base zero ! -BOOST_MPL_ASSERT((boost::is_same< Matrix23, Jacobian23>)); - -/* ************************************************************************* */ -// Boost Fusion includes allow us to create/access values from MPL vectors -#include -#include - -// Create a value and access it -TEST(ExpressionFactor, JacobiansValue) { - using boost::fusion::at_c; - Matrix23 expected; - Jacobians jacobians; - - at_c<1>(jacobians) << 1, 2, 3, 4, 5, 6; - - Matrix23 actual = at_c<1>(jacobians); - CHECK(actual.cols() == expected.cols()); - CHECK(actual.rows() == expected.rows()); -} - -/* ************************************************************************* */ -// Test out polymorphic transform -#include -#include -#include - -struct triple { - template struct result; // says we will provide result - - template - struct result { - typedef double type; // result for int argument - }; - - template - struct result { - typedef double type; // result for int argument - }; - - template - struct result { - typedef double type; // result for double argument - }; - - template - struct result { - typedef double type; // result for double argument - }; - - // actual function - template - typename result::type operator()(const T& x) const { - return (double) x; - } -}; - -// Test out polymorphic transform -TEST(ExpressionFactor, Triple) { - typedef boost::fusion::vector IntDouble; - IntDouble H = boost::fusion::make_vector(1, 2.0); - - // Only works if I use Double2 - typedef boost::fusion::result_of::transform::type Result; - typedef boost::fusion::vector Double2; - Double2 D = boost::fusion::transform(H, triple()); - - using boost::fusion::at_c; - DOUBLES_EQUAL(1.0, at_c<0>(D), 1e-9); - DOUBLES_EQUAL(2.0, at_c<1>(D), 1e-9); -} - -/* ************************************************************************* */ -#include -#include -#include -#include - -// Test out invoke -TEST(ExpressionFactor, Invoke) { - EXPECT_LONGS_EQUAL(2, invoke(plus(),boost::fusion::make_vector(1,1))); - - // Creating a Pose3 (is there another way?) - boost::fusion::vector pair; - Pose3 pose = boost::fusion::invoke(boost::value_factory(), pair); -} - -/* ************************************************************************* */ -// debug const issue (how to make read/write arguments for invoke) -struct test { - typedef void result_type; - void operator()(int& a, int& b) const { - a = 6; - b = 7; - } -}; - -TEST(ExpressionFactor, ConstIssue) { - int a, b; - boost::fusion::invoke(test(), - boost::fusion::make_vector(boost::ref(a), boost::ref(b))); - LONGS_EQUAL(6, a); - LONGS_EQUAL(7, b); -} - -/* ************************************************************************* */ -// Test out invoke on a given GTSAM function -// then construct prototype for it's derivatives -TEST(ExpressionFactor, InvokeDerivatives) { - // This is the method in Pose3: - // Point3 transform_to(const Point3& p) const; - // Point3 transform_to(const Point3& p, - // boost::optional Dpose, boost::optional Dpoint) const; - - // Let's assign it it to a boost function object - // cast is needed because Pose3::transform_to is overloaded - typedef boost::function F; - F f = static_cast(&Pose3::transform_to); - - // Create arguments -Pose3 pose; - Point3 point; - typedef boost::fusion::vector Arguments; - Arguments args = boost::fusion::make_vector(pose, point); - - // Create fused function (takes fusion vector) and call it - boost::fusion::fused g(f); - Point3 actual = g(args); - CHECK(assert_equal(point,actual)); - - // We can *immediately* do this using invoke - Point3 actual2 = boost::fusion::invoke(f, args); - CHECK(assert_equal(point,actual2)); - - // Now, let's create the optional Jacobian arguments - typedef Point3 T; - typedef boost::mpl::vector TYPES; - typedef boost::mpl::transform >::type Optionals; - - // Unfortunately this is moot: we need a pointer to a function with the - // optional derivatives; I don't see a way of calling a function that we - // did not get access to by the caller passing us a pointer. - // Let's test below whether we can have a proxy object -} - -struct proxy { - typedef Point3 result_type; - Point3 operator()(const Pose3& pose, const Point3& point) const { - return pose.transform_to(point); - } - Point3 operator()(const Pose3& pose, const Point3& point, - boost::optional Dpose, - boost::optional Dpoint) const { - return pose.transform_to(point, Dpose, Dpoint); - } -}; - -TEST(ExpressionFactor, InvokeDerivatives2) { - // without derivatives - Pose3 pose; - Point3 point; - Point3 actual = boost::fusion::invoke(proxy(), - boost::fusion::make_vector(pose, point)); - CHECK(assert_equal(point,actual)); - - // with derivatives, does not work, const issue again - Matrix36 Dpose; - Matrix3 Dpoint; - Point3 actual2 = boost::fusion::invoke(proxy(), - boost::fusion::make_vector(pose, point, boost::ref(Dpose), - boost::ref(Dpoint))); - CHECK(assert_equal(point,actual2)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ - diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index 832d37d14..0b86a60e9 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -17,19 +17,20 @@ */ -#include #include -#include -#include -#include -#include -#include -#include #include #include +#include #include #include +#include +#include +#include +#include #include +#include + +#include using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 2a756b5af..2a27730f4 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -12,17 +12,14 @@ using namespace std; namespace gtsam { -Matrix cov(const Matrix& m) { +/* ************************************************************************* */ +Matrix3 AHRS::Cov(const Vector3s& m) { const double num_observations = m.cols(); - const Vector mean = m.rowwise().sum() / num_observations; - Matrix D = m.colwise() - mean; - Matrix DDt = D * trans(D); - return DDt / (num_observations - 1); + const Vector3 mean = m.rowwise().sum() / num_observations; + Vector3s D = m.colwise() - mean; + return D * trans(D) / (num_observations - 1); } -Matrix I3 = eye(3); -Matrix Z3 = zeros(3, 3); - /* ************************************************************************* */ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, bool flat) : @@ -34,11 +31,11 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, size_t T = stationaryU.cols(); // estimate standard deviation on gyroscope readings - Pg_ = cov(stationaryU); - Vector sigmas_v_g = esqrt(Pg_.diagonal() * T); + Pg_ = Cov(stationaryU); + Vector3 sigmas_v_g = esqrt(Pg_.diagonal() * T); // estimate standard deviation on accelerometer readings - Pa_ = cov(stationaryF); + Pa_ = Cov(stationaryF); // Quantities needed for integration @@ -46,15 +43,13 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, double tau_g = 730; // correlation time for gyroscope double tau_a = 730; // correlation time for accelerometer - F_g_ = -I3 / tau_g; - F_a_ = -I3 / tau_a; + F_g_ = -I_3x3 / tau_g; + F_a_ = -I_3x3 / tau_a; Vector3 var_omega_w = 0 * ones(3); // TODO Vector3 var_omega_g = (0.0034 * 0.0034) * ones(3); Vector3 var_omega_a = (0.034 * 0.034) * ones(3); - Vector3 sigmas_v_g_sq = emul(sigmas_v_g, sigmas_v_g); - Vector vars(12); - vars << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a; - var_w_ = diag(vars); + Vector3 sigmas_v_g_sq = sigmas_v_g.cwiseProduct(sigmas_v_g); + var_w_ << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a; // Quantities needed for aiding sigmas_v_a_ = esqrt(T * Pa_.diagonal()); @@ -95,15 +90,15 @@ std::pair AHRS::initialize(double g_e) Matrix P_plus_k2 = Matrix(9, 9); P_plus_k2.block<3,3>(0, 0) = P11; - P_plus_k2.block<3,3>(0, 3) = Z3; + P_plus_k2.block<3,3>(0, 3) = Z_3x3; P_plus_k2.block<3,3>(0, 6) = P12; - P_plus_k2.block<3,3>(3, 0) = Z3; + P_plus_k2.block<3,3>(3, 0) = Z_3x3; P_plus_k2.block<3,3>(3, 3) = Pg_; - P_plus_k2.block<3,3>(3, 6) = Z3; + P_plus_k2.block<3,3>(3, 6) = Z_3x3; P_plus_k2.block<3,3>(6, 0) = trans(P12); - P_plus_k2.block<3,3>(6, 3) = Z3; + P_plus_k2.block<3,3>(6, 3) = Z_3x3; P_plus_k2.block<3,3>(6, 6) = Pa; Vector dx = zero(9); @@ -123,26 +118,26 @@ std::pair AHRS::integrate( // FIXME: //if nargout>1 Matrix bRn = mech.bRn().matrix(); - Matrix I3 = eye(3); - Matrix Z3 = zeros(3, 3); - Matrix F_k = zeros(9, 9); + Matrix9 F_k; F_k.setZero(); F_k.block<3,3>(0, 3) = -bRn; F_k.block<3,3>(3, 3) = F_g_; F_k.block<3,3>(6, 6) = F_a_; - Matrix G_k = zeros(9, 12); + typedef Eigen::Matrix Matrix9_12; + Matrix9_12 G_k; G_k.setZero(); G_k.block<3,3>(0, 0) = -bRn; G_k.block<3,3>(0, 6) = -bRn; - G_k.block<3,3>(3, 3) = I3; - G_k.block<3,3>(6, 9) = I3; + G_k.block<3,3>(3, 3) = I_3x3; + G_k.block<3,3>(6, 9) = I_3x3; - Matrix Q_k = G_k * var_w_ * trans(G_k); - Matrix Psi_k = eye(9) + dt * F_k; // +dt*dt*F_k*F_k/2; // transition matrix + Matrix9 Q_k = G_k * var_w_.asDiagonal() * G_k.transpose(); + Matrix9 Psi_k = I_9x9 + dt * F_k; // +dt*dt*F_k*F_k/2; // transition matrix // This implements update in section 10.6 - Matrix B = zeros(9, 9); - Vector u2 = zero(9); + Matrix9 B; B.setZero(); + Vector9 u2; u2.setZero(); + // TODO predictQ should be templated to also take fixed size matrices. KalmanFilter::State newState = KF_.predictQ(state, Psi_k,B,u2,dt*Q_k); return make_pair(newMech, newState); } @@ -175,7 +170,7 @@ std::pair AHRS::aid( if (Farrell) { // calculate residual gravity measurement z = n_g_ - trans(bRn) * measured_b_g; - H = collect(3, &n_g_cross_, &Z3, &bRn); + H = collect(3, &n_g_cross_, &Z_3x3, &bRn); R = trans(bRn) * diag(emul(sigmas_v_a_, sigmas_v_a_)) * bRn; } else { // my measurement prediction (in body frame): @@ -189,7 +184,7 @@ std::pair AHRS::aid( z = bRn * n_g_ - measured_b_g; // Now the Jacobian H Matrix b_g = bRn * n_g_cross_; - H = collect(3, &b_g, &Z3, &I3); + H = collect(3, &b_g, &Z_3x3, &I_3x3); // And the measurement noise, TODO: should be created once where sigmas_v_a is given R = diag(emul(sigmas_v_a_, sigmas_v_a_)); } @@ -219,7 +214,7 @@ std::pair AHRS::aidGeneral( Vector z = f - increment * f_previous; //Vector z = increment * f_previous - f; Matrix b_g = skewSymmetric(increment* f_previous); - Matrix H = collect(3, &b_g, &I3, &Z3); + Matrix H = collect(3, &b_g, &I_3x3, &Z_3x3); // Matrix R = diag(emul(sigmas_v_a_, sigmas_v_a_)); // Matrix R = diag(Vector3(1.0, 0.2, 1.0)); // good for L_twice Matrix R = diag(Vector3(0.01, 0.0001, 0.01)); @@ -240,16 +235,16 @@ std::pair AHRS::aidGeneral( /* ************************************************************************* */ void AHRS::print(const std::string& s) const { mech0_.print(s + ".mech0_"); - gtsam::print(F_g_, s + ".F_g"); - gtsam::print(F_a_, s + ".F_a"); - gtsam::print(var_w_, s + ".var_w"); + gtsam::print((Matrix)F_g_, s + ".F_g"); + gtsam::print((Matrix)F_a_, s + ".F_a"); + gtsam::print((Vector)var_w_, s + ".var_w"); - gtsam::print(sigmas_v_a_, s + ".sigmas_v_a"); - gtsam::print(n_g_, s + ".n_g"); - gtsam::print(n_g_cross_, s + ".n_g_cross"); + gtsam::print((Vector)sigmas_v_a_, s + ".sigmas_v_a"); + gtsam::print((Vector)n_g_, s + ".n_g"); + gtsam::print((Matrix)n_g_cross_, s + ".n_g_cross"); - gtsam::print(Pg_, s + ".P_g"); - gtsam::print(Pa_, s + ".P_a"); + gtsam::print((Matrix)Pg_, s + ".P_g"); + gtsam::print((Matrix)Pa_, s + ".P_a"); } diff --git a/gtsam_unstable/slam/AHRS.h b/gtsam_unstable/slam/AHRS.h index 81d74a9f5..e15e6e0f7 100644 --- a/gtsam_unstable/slam/AHRS.h +++ b/gtsam_unstable/slam/AHRS.h @@ -14,8 +14,6 @@ namespace gtsam { -GTSAM_UNSTABLE_EXPORT Matrix cov(const Matrix& m); - class GTSAM_UNSTABLE_EXPORT AHRS { private: @@ -25,18 +23,24 @@ private: KalmanFilter KF_; ///< initial Kalman filter // Quantities needed for integration - Matrix F_g_; ///< gyro bias dynamics - Matrix F_a_; ///< acc bias dynamics - Matrix var_w_; ///< dynamic noise variances + Matrix3 F_g_; ///< gyro bias dynamics + Matrix3 F_a_; ///< acc bias dynamics + + typedef Eigen::Matrix Variances; + Variances var_w_; ///< dynamic noise variances // Quantities needed for aiding - Vector sigmas_v_a_; ///< measurement sigma - Vector n_g_; ///< gravity in nav frame - Matrix n_g_cross_; ///< and its skew-symmetric matrix + Vector3 sigmas_v_a_; ///< measurement sigma + Vector3 n_g_; ///< gravity in nav frame + Matrix3 n_g_cross_; ///< and its skew-symmetric matrix - Matrix Pg_, Pa_; + Matrix3 Pg_, Pa_; public: + + typedef Eigen::Matrix Vector3s; + static Matrix3 Cov(const Vector3s& m); + /** * AHRS constructor * @param stationaryU initial interval of gyro measurements, 3xn matrix diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index dcf43c569..56b1269f0 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -417,7 +417,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h index c3f3f1d10..6f39ce1b6 100644 --- a/gtsam_unstable/slam/BiasedGPSFactor.h +++ b/gtsam_unstable/slam/BiasedGPSFactor.h @@ -95,7 +95,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 614521e76..79a37c2ff 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -652,7 +652,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor2", boost::serialization::base_object(*this)); } diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 8cf22946a..8216942cd 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -574,7 +574,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor2", boost::serialization::base_object(*this)); } diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index 810673b64..7f9564ee3 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -16,13 +16,13 @@ **/ #pragma once -#include - -#include -#include #include #include #include +#include +#include + +#include namespace gtsam { @@ -91,8 +91,8 @@ public: boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - Vector v1( VALUE::Logmap(p1) ); - Vector v2( VALUE::Logmap(p2) ); + Vector v1( traits::Logmap(p1) ); + Vector v2( traits::Logmap(p2) ); Vector alpha(tau_.size()); Vector alpha_v1(tau_.size()); @@ -114,7 +114,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(dt_); ar & BOOST_SERIALIZATION_NVP(tau_); @@ -132,4 +132,9 @@ private: }; // \class GaussMarkov1stOrderFactor +/// traits +template struct traits > : + public Testable > { +}; + } /// namespace gtsam diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 805c0a71a..d8fceeb68 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -386,12 +386,17 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor2", boost::serialization::base_object(*this)); } -}; // \class GaussMarkov1stOrderFactor +}; // \class InertialNavFactor_GlobalVelocity +/// traits +template +struct traits > : + public Testable > { +}; } /// namespace aspn diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index efa7f5f7d..ac481f979 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -114,7 +114,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 2998fdc9b..2fd964a35 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -132,7 +132,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 154afbdff..879e1e1dd 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -140,7 +140,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index 2e2d1e310..feff0b62c 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -131,7 +131,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); @@ -253,7 +253,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index c5b1fc2a4..4218a5561 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -25,29 +25,29 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, const Matrix& F, const double g_e, bool flat) { // estimate gyro bias by averaging gyroscope readings (10.62) - Vector x_g = U.rowwise().mean(); - Vector meanF = F.rowwise().mean(); + Vector3 x_g = U.rowwise().mean(); + Vector3 meanF = F.rowwise().mean(); // estimate gravity - Vector b_g; + Vector3 b_g; if(g_e == 0) { if (flat) // acceleration measured is along the z-axis. - b_g = (Vector(3) << 0.0, 0.0, norm_2(meanF)).finished(); + b_g = (Vector3(3) << 0.0, 0.0, norm_2(meanF)).finished(); else // acceleration measured is the opposite of gravity (10.13) b_g = -meanF; } else { if (flat) // gravity is downward along the z-axis since we are flat on the ground - b_g = (Vector(3) << 0.0,0.0,g_e).finished(); + b_g = (Vector3(3) << 0.0,0.0,g_e).finished(); else // normalize b_g and attribute remainder to biases b_g = - g_e * meanF/meanF.norm(); } // estimate accelerometer bias - Vector x_a = meanF + b_g; + Vector3 x_a = meanF + b_g; // initialize roll, pitch (eqns. 10.14, 10.15) double g1=b_g(0); @@ -66,42 +66,42 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, } /* ************************************************************************* */ -Mechanization_bRn2 Mechanization_bRn2::correct(const Vector& dx) const { - Vector rho = sub(dx, 0, 3); +Mechanization_bRn2 Mechanization_bRn2::correct(const Vector3& dx) const { + Vector3 rho = sub(dx, 0, 3); Rot3 delta_nRn = Rot3::rodriguez(rho); Rot3 bRn = bRn_ * delta_nRn; - Vector x_g = x_g_ + sub(dx, 3, 6); - Vector x_a = x_a_ + sub(dx, 6, 9); + Vector3 x_g = x_g_ + sub(dx, 3, 6); + Vector3 x_a = x_a_ + sub(dx, 6, 9); return Mechanization_bRn2(bRn, x_g, x_a); } /* ************************************************************************* */ -Mechanization_bRn2 Mechanization_bRn2::integrate(const Vector& u, +Mechanization_bRn2 Mechanization_bRn2::integrate(const Vector3& u, const double dt) const { // integrate rotation nRb based on gyro measurement u and bias x_g #ifndef MODEL_NAV_FRAME_ROTATION // get body to inertial (measured in b) from gyro, subtract bias - Vector b_omega_ib = u - x_g_; + Vector3 b_omega_ib = u - x_g_; // nav to inertial due to Earth's rotation and our movement on Earth surface // TODO: figure out how to do this if we need it - Vector b_omega_in = zero(3); + Vector3 b_omega_in; b_omega_in.setZero(); // calculate angular velocity on bRn measured in body frame - Vector b_omega_bn = b_omega_in - b_omega_ib; + Vector3 b_omega_bn = b_omega_in - b_omega_ib; #else // Assume a non-rotating nav frame (probably an approximation) // calculate angular velocity on bRn measured in body frame - Vector b_omega_bn = x_g_ - u; + Vector3 b_omega_bn = x_g_ - u; #endif // convert to navigation frame Rot3 nRb = bRn_.inverse(); - Vector n_omega_bn = (nRb*b_omega_bn).vector(); + Vector3 n_omega_bn = (nRb*b_omega_bn).vector(); // integrate bRn using exponential map, assuming constant over dt Rot3 delta_nRn = Rot3::rodriguez(n_omega_bn*dt); diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index dda267a59..fa33ce5b9 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -18,8 +18,8 @@ class GTSAM_UNSTABLE_EXPORT Mechanization_bRn2 { private: Rot3 bRn_; ///< rotation from nav to body - Vector x_g_; ///< gyroscope bias - Vector x_a_; ///< accelerometer bias + Vector3 x_g_; ///< gyroscope bias + Vector3 x_a_; ///< accelerometer bias public: @@ -30,7 +30,7 @@ public: * @param r3 Z-axis of rotated frame */ Mechanization_bRn2(const Rot3& initial_bRn = Rot3(), - const Vector& initial_x_g = zero(3), const Vector& initial_x_a = zero(3)) : + const Vector3& initial_x_g = zero(3), const Vector3& initial_x_a = zero(3)) : bRn_(initial_bRn), x_g_(initial_x_g), x_a_(initial_x_a) { } @@ -40,14 +40,14 @@ public: } /// gravity in the body frame - Vector b_g(double g_e) const { - Vector n_g = (Vector(3) << 0, 0, g_e).finished(); + Vector3 b_g(double g_e) const { + Vector3 n_g(0, 0, g_e); return (bRn_ * n_g).vector(); } const Rot3& bRn() const {return bRn_; } - const Vector& x_g() const { return x_g_; } - const Vector& x_a() const { return x_a_; } + const Vector3& x_g() const { return x_g_; } + const Vector3& x_a() const { return x_a_; } /** * Initialize the first Mechanization state @@ -68,7 +68,7 @@ public: * @param obj The current state * @param dx The error used to correct and return a new state */ - Mechanization_bRn2 correct(const Vector& dx) const; + Mechanization_bRn2 correct(const Vector3& dx) const; /** * Integrate to get new state @@ -76,14 +76,14 @@ public: * @param u gyro measurement to integrate * @param dt time elapsed since previous state in seconds */ - Mechanization_bRn2 integrate(const Vector& u, const double dt) const; + Mechanization_bRn2 integrate(const Vector3& u, const double dt) const; /// print void print(const std::string& s = "") const { bRn_.print(s + ".R"); - gtsam::print(x_g_, s + ".x_g"); - gtsam::print(x_a_, s + ".x_a"); + std::cout << s + ".x_g" << x_g_ << std::endl; + std::cout << s + ".x_a" << x_a_ << std::endl; } }; diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index 088bfd2ea..da60c2c31 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -216,7 +216,7 @@ namespace gtsam { /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index 8d6fcc33b..e3080466f 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -70,7 +70,7 @@ namespace gtsam { /** Single Element Constructor: acts on a single parameter specified by idx */ PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) : - Base(model, key), prior_((Vector(1) << prior).finished()), mask_(1, idx), H_(zeros(1, T::Dim())) { + Base(model, key), prior_((Vector(1) << prior).finished()), mask_(1, idx), H_(zeros(1, T::dimension)) { assert(model->dim() == 1); this->fillH(); } @@ -78,7 +78,7 @@ namespace gtsam { /** Indices Constructor: specify the mask with a set of indices */ PartialPriorFactor(Key key, const std::vector& mask, const Vector& prior, const SharedNoiseModel& model) : - Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::Dim())) { + Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::dimension)) { assert((size_t)prior_.size() == mask.size()); assert(model->dim() == (size_t) prior.size()); this->fillH(); @@ -136,7 +136,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index 87b9f4f5c..d2a9110d9 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -120,7 +120,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index 24f083907..2de060302 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -88,7 +88,7 @@ namespace gtsam { // manifold equivalent of h(x)-z -> log(z,h(x)) return prior_.localCoordinates(p.compose(*body_P_sensor_, H)); } else { - if(H) (*H) = eye(p.dim()); + if(H) (*H) = I_6x6; // manifold equivalent of h(x)-z -> log(z,h(x)) return prior_.localCoordinates(p); } @@ -101,7 +101,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(prior_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index b69f852b4..5b1540c83 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -170,7 +170,7 @@ namespace gtsam { /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); @@ -178,4 +178,11 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); } }; + + /// traits + template + struct traits > : + public Testable > { + }; + } // \ namespace gtsam diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index d3bfbbb7d..069274065 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -161,11 +161,18 @@ namespace gtsam { /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(throwCheirality_); ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); } }; + + /// traits + template + struct traits > : + public Testable > { + }; + } // \ namespace gtsam diff --git a/gtsam_unstable/slam/RelativeElevationFactor.h b/gtsam_unstable/slam/RelativeElevationFactor.h index d331053b6..407652583 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.h +++ b/gtsam_unstable/slam/RelativeElevationFactor.h @@ -65,7 +65,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h new file mode 100644 index 000000000..1358e1349 --- /dev/null +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -0,0 +1,752 @@ +/* ---------------------------------------------------------------------------- + + * 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 SmartStereoProjectionFactor.h + * @brief Base class to create smart factors on poses or cameras + * @author Luca Carlone + * @author Zsolt Kira + * @author Frank Dellaert + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace gtsam { + +/** + * Structure for storing some state memory, used to speed up optimization + * @addtogroup SLAM + */ +class SmartStereoProjectionFactorState { + +protected: + +public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + SmartStereoProjectionFactorState() { + } + // Hessian representation (after Schur complement) + bool calculatedHessian; + Matrix H; + Vector gs_vector; + std::vector Gs; + std::vector gs; + double f; +}; + +enum LinearizationMode { + HESSIAN, JACOBIAN_SVD, JACOBIAN_Q +}; + +/** + * SmartStereoProjectionFactor: triangulates point + */ +template +class SmartStereoProjectionFactor: public SmartFactorBase { +protected: + + // Some triangulation parameters + const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_ + const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate + mutable std::vector cameraPosesTriangulation_; ///< current triangulation poses + + const bool manageDegeneracy_; ///< if set to true will use the rotation-only version for degenerate cases + + const bool enableEPI_; ///< if set to true, will refine triangulation using LM + + const double linearizationThreshold_; ///< threshold to decide whether to re-linearize + mutable std::vector cameraPosesLinearization_; ///< current linearization poses + + mutable Point3 point_; ///< Current estimate of the 3D point + + mutable bool degenerate_; + mutable bool cheiralityException_; + + // verbosity handling for Cheirality Exceptions + const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) + const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) + + boost::shared_ptr state_; + + /// shorthand for smart projection factor state variable + typedef boost::shared_ptr SmartFactorStatePtr; + + /// shorthand for base class type + typedef SmartFactorBase Base; + + double landmarkDistanceThreshold_; // if the landmark is triangulated at a + // distance larger than that the factor is considered degenerate + + double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the + // average reprojection error is smaller than this threshold after triangulation, + // and the factor is disregarded if the error is large + + /// shorthand for this class + typedef SmartStereoProjectionFactor This; + + enum {ZDim = 3}; ///< Dimension trait of measurement type + +public: + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// shorthand for a StereoCamera // TODO: Get rid of this? + typedef StereoCamera Camera; + + /// Vector of cameras + typedef CameraSet Cameras; + + /** + * Constructor + * @param rankTol tolerance used to check if point triangulation is degenerate + * @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization) + * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, + * otherwise the factor is simply neglected + * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations + * @param body_P_sensor is the transform from body to sensor frame (default identity) + */ + SmartStereoProjectionFactor(const double rankTol, const double linThreshold, + const bool manageDegeneracy, const bool enableEPI, + boost::optional body_P_sensor = boost::none, + double landmarkDistanceThreshold = 1e10, + double dynamicOutlierRejectionThreshold = -1, + SmartFactorStatePtr state = SmartFactorStatePtr(new SmartStereoProjectionFactorState())) : + Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_( + 1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( + linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_( + false), verboseCheirality_(false), state_(state), + landmarkDistanceThreshold_(landmarkDistanceThreshold), + dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) { + } + + /** Virtual destructor */ + virtual ~SmartStereoProjectionFactor() { + } + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "SmartStereoProjectionFactor, z = \n"; + std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl; + std::cout << "degenerate_ = " << degenerate_ << std::endl; + std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl; + std::cout << "linearizationThreshold_ = " << linearizationThreshold_ << std::endl; + Base::print("", keyFormatter); + } + + /// Check if the new linearization point_ is the same as the one used for previous triangulation + bool decideIfTriangulate(const Cameras& cameras) const { + // several calls to linearize will be done from the same linearization point_, hence it is not needed to re-triangulate + // Note that this is not yet "selecting linearization", that will come later, and we only check if the + // current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point_ + + size_t m = cameras.size(); + + bool retriangulate = false; + + // if we do not have a previous linearization point_ or the new linearization point_ includes more poses + if (cameraPosesTriangulation_.empty() + || cameras.size() != cameraPosesTriangulation_.size()) + retriangulate = true; + + if (!retriangulate) { + for (size_t i = 0; i < cameras.size(); i++) { + if (!cameras[i].pose().equals(cameraPosesTriangulation_[i], + retriangulationThreshold_)) { + retriangulate = true; // at least two poses are different, hence we retriangulate + break; + } + } + } + + if (retriangulate) { // we store the current poses used for triangulation + cameraPosesTriangulation_.clear(); + cameraPosesTriangulation_.reserve(m); + for (size_t i = 0; i < m; i++) + // cameraPosesTriangulation_[i] = cameras[i].pose(); + cameraPosesTriangulation_.push_back(cameras[i].pose()); + } + + return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation + } + + /// This function checks if the new linearization point_ is 'close' to the previous one used for linearization + bool decideIfLinearize(const Cameras& cameras) const { + // "selective linearization" + // The function evaluates how close are the old and the new poses, transformed in the ref frame of the first pose + // (we only care about the "rigidity" of the poses, not about their absolute pose) + + if (this->linearizationThreshold_ < 0) //by convention if linearizationThreshold is negative we always relinearize + return true; + + // if we do not have a previous linearization point_ or the new linearization point_ includes more poses + if (cameraPosesLinearization_.empty() + || (cameras.size() != cameraPosesLinearization_.size())) + return true; + + Pose3 firstCameraPose, firstCameraPoseOld; + for (size_t i = 0; i < cameras.size(); i++) { + + if (i == 0) { // we store the initial pose, this is useful for selective re-linearization + firstCameraPose = cameras[i].pose(); + firstCameraPoseOld = cameraPosesLinearization_[i]; + continue; + } + + // we compare the poses in the frame of the first pose + Pose3 localCameraPose = firstCameraPose.between(cameras[i].pose()); + Pose3 localCameraPoseOld = firstCameraPoseOld.between( + cameraPosesLinearization_[i]); + if (!localCameraPose.equals(localCameraPoseOld, + this->linearizationThreshold_)) + return true; // at least two "relative" poses are different, hence we re-linearize + } + return false; // if we arrive to this point_ all poses are the same and we don't need re-linearize + } + + /// triangulateSafe + size_t triangulateSafe(const Values& values) const { + return triangulateSafe(this->cameras(values)); + } + + /// triangulateSafe + size_t triangulateSafe(const Cameras& cameras) const { + + size_t m = cameras.size(); + if (m < 2) { // if we have a single pose the corresponding factor is uninformative + degenerate_ = true; + return m; + } + bool retriangulate = decideIfTriangulate(cameras); + + if (retriangulate) { + // We triangulate the 3D position of the landmark + try { + // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; + + //TODO: Chris will replace this with something else for stereo +// point_ = triangulatePoint3(cameras, this->measured_, +// rankTolerance_, enableEPI_); + + // // // Temporary hack to use monocular triangulation + std::vector mono_measurements; + BOOST_FOREACH(const StereoPoint2& sp, this->measured_) { + mono_measurements.push_back(sp.point2()); + } + + std::vector > mono_cameras; + BOOST_FOREACH(const Camera& camera, cameras) { + const Pose3& pose = camera.pose(); + const Cal3_S2& K = camera.calibration()->calibration(); + mono_cameras.push_back(PinholeCamera(pose, K)); + } + point_ = triangulatePoint3 >(mono_cameras, mono_measurements, + rankTolerance_, enableEPI_); + + // // // End temporary hack + + // FIXME: temporary: triangulation using only first camera +// const StereoPoint2& z0 = this->measured_.at(0); +// point_ = cameras[0].backproject(z0); + + degenerate_ = false; + cheiralityException_ = false; + + // Check landmark distance and reprojection errors to avoid outliers + double totalReprojError = 0.0; + size_t i=0; + BOOST_FOREACH(const Camera& camera, cameras) { + Point3 cameraTranslation = camera.pose().translation(); + // we discard smart factors corresponding to points that are far away + if(cameraTranslation.distance(point_) > landmarkDistanceThreshold_){ + degenerate_ = true; + break; + } + const StereoPoint2& zi = this->measured_.at(i); + try { + StereoPoint2 reprojectionError(camera.project(point_) - zi); + totalReprojError += reprojectionError.vector().norm(); + } catch (CheiralityException) { + cheiralityException_ = true; + } + i += 1; + } + //std::cout << "totalReprojError error: " << totalReprojError << std::endl; + // we discard smart factors that have large reprojection error + if(dynamicOutlierRejectionThreshold_ > 0 && + totalReprojError/m > dynamicOutlierRejectionThreshold_) + degenerate_ = true; + + } catch (TriangulationUnderconstrainedException&) { + // if TriangulationUnderconstrainedException can be + // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before + // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) + // in the second case we want to use a rotation-only smart factor + degenerate_ = true; + cheiralityException_ = false; + } catch (TriangulationCheiralityException&) { + // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers + // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint + cheiralityException_ = true; + } + } + return m; + } + + /// triangulate + bool triangulateForLinearize(const Cameras& cameras) const { + + bool isDebug = false; + size_t nrCameras = this->triangulateSafe(cameras); + + if (nrCameras < 2 + || (!this->manageDegeneracy_ + && (this->cheiralityException_ || this->degenerate_))) { + if (isDebug) { + std::cout << "createImplicitSchurFactor: degenerate configuration" + << std::endl; + } + return false; + } else { + + // instead, if we want to manage the exception.. + if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors + this->degenerate_ = true; + } + return true; + } + } + + /// linearize returns a Hessianfactor that is an approximation of error(p) + boost::shared_ptr > createHessianFactor( + const Cameras& cameras, const double lambda = 0.0) const { + + bool isDebug = false; + size_t numKeys = this->keys_.size(); + // Create structures for Hessian Factors + std::vector < Key > js; + std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); + std::vector < Vector > gs(numKeys); + + if (this->measured_.size() != cameras.size()) { + std::cout + << "SmartProjectionHessianFactor: this->measured_.size() inconsistent with input" + << std::endl; + exit(1); + } + + this->triangulateSafe(cameras); + if (isDebug) std::cout << "point_ = " << point_ << std::endl; + + if (numKeys < 2 + || (!this->manageDegeneracy_ + && (this->cheiralityException_ || this->degenerate_))) { + if (isDebug) std::cout << "In linearize: exception" << std::endl; + BOOST_FOREACH(Matrix& m, Gs) + m = zeros(D, D); + BOOST_FOREACH(Vector& v, gs) + v = zero(D); + return boost::make_shared >(this->keys_, Gs, gs, + 0.0); + } + + // instead, if we want to manage the exception.. + if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors + this->degenerate_ = true; + if (isDebug) std::cout << "degenerate_ = true" << std::endl; + } + + bool doLinearize = this->decideIfLinearize(cameras); + + if (isDebug) std::cout << "doLinearize = " << doLinearize << std::endl; + + if (this->linearizationThreshold_ >= 0 && doLinearize) // if we apply selective relinearization and we need to relinearize + for (size_t i = 0; i < cameras.size(); i++) + this->cameraPosesLinearization_[i] = cameras[i].pose(); + + if (!doLinearize) { // return the previous Hessian factor + std::cout << "=============================" << std::endl; + std::cout << "doLinearize " << doLinearize << std::endl; + std::cout << "this->linearizationThreshold_ " + << this->linearizationThreshold_ << std::endl; + std::cout << "this->degenerate_ " << this->degenerate_ << std::endl; + std::cout + << "something wrong in SmartProjectionHessianFactor: selective relinearization should be disabled" + << std::endl; + exit(1); + return boost::make_shared >(this->keys_, + this->state_->Gs, this->state_->gs, this->state_->f); + } + + // ================================================================== + Matrix F, E; + Vector b; + double f = computeJacobians(F, E, b, cameras); + + // Schur complement trick + // Frank says: should be possible to do this more efficiently? + // And we care, as in grouped factors this is called repeatedly + Matrix H(D * numKeys, D * numKeys); + Vector gs_vector; + + Matrix3 P = Base::PointCov(E,lambda); + H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F)))); + gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b)))); + + if (isDebug) std::cout << "gs_vector size " << gs_vector.size() << std::endl; + if (isDebug) std::cout << "H:\n" << H << std::endl; + + // Populate Gs and gs + int GsCount2 = 0; + for (DenseIndex i1 = 0; i1 < (DenseIndex)numKeys; i1++) { // for each camera + DenseIndex i1D = i1 * D; + gs.at(i1) = gs_vector.segment < D > (i1D); + for (DenseIndex i2 = 0; i2 < (DenseIndex)numKeys; i2++) { + if (i2 >= i1) { + Gs.at(GsCount2) = H.block < D, D > (i1D, i2 * D); + GsCount2++; + } + } + } + // ================================================================== + if (this->linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables + this->state_->Gs = Gs; + this->state_->gs = gs; + this->state_->f = f; + } + return boost::make_shared >(this->keys_, Gs, gs, f); + } + +// // create factor +// boost::shared_ptr > createImplicitSchurFactor( +// const Cameras& cameras, double lambda) const { +// if (triangulateForLinearize(cameras)) +// return Base::createImplicitSchurFactor(cameras, point_, lambda); +// else +// return boost::shared_ptr >(); +// } +// +// /// create factor +// boost::shared_ptr > createJacobianQFactor( +// const Cameras& cameras, double lambda) const { +// if (triangulateForLinearize(cameras)) +// return Base::createJacobianQFactor(cameras, point_, lambda); +// else +// return boost::make_shared< JacobianFactorQ >(this->keys_); +// } +// +// /// Create a factor, takes values +// boost::shared_ptr > createJacobianQFactor( +// const Values& values, double lambda) const { +// Cameras myCameras; +// // TODO triangulate twice ?? +// bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); +// if (nonDegenerate) +// return createJacobianQFactor(myCameras, lambda); +// else +// return boost::make_shared< JacobianFactorQ >(this->keys_); +// } +// + /// different (faster) way to compute Jacobian factor + boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, + double lambda) const { + if (triangulateForLinearize(cameras)) + return Base::createJacobianSVDFactor(cameras, point_, lambda); + else + return boost::make_shared< JacobianFactorSVD >(this->keys_); + } + + /// Returns true if nonDegenerate + bool computeCamerasAndTriangulate(const Values& values, + Cameras& myCameras) const { + Values valuesFactor; + + // Select only the cameras + BOOST_FOREACH(const Key key, this->keys_) + valuesFactor.insert(key, values.at(key)); + + myCameras = this->cameras(valuesFactor); + size_t nrCameras = this->triangulateSafe(myCameras); + + if (nrCameras < 2 + || (!this->manageDegeneracy_ + && (this->cheiralityException_ || this->degenerate_))) + return false; + + // instead, if we want to manage the exception.. + if (this->cheiralityException_ || this->degenerate_) // if we want to manage the exceptions with rotation-only factors + this->degenerate_ = true; + + if (this->degenerate_) { + std::cout << "SmartStereoProjectionFactor: this is not ready" << std::endl; + std::cout << "this->cheiralityException_ " << this->cheiralityException_ + << std::endl; + std::cout << "this->degenerate_ " << this->degenerate_ << std::endl; + } + return true; + } + + /// Assumes non-degenerate ! + void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const { + Base::computeEP(E, PointCov, cameras, point_); + } + + /// Takes values + bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const { + Cameras myCameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + if (nonDegenerate) + computeEP(E, PointCov, myCameras); + return nonDegenerate; + } + + /// Version that takes values, and creates the point + bool computeJacobians(std::vector& Fblocks, + Matrix& E, Vector& b, const Values& values) const { + Cameras myCameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + if (nonDegenerate) + computeJacobians(Fblocks, E, b, myCameras, 0.0); + return nonDegenerate; + } + + /// Compute F, E only (called below in both vanilla and SVD versions) + /// Assumes the point has been computed + /// Note E can be 2m*3 or 2m*2, in case point is degenerate + double computeJacobians(std::vector& Fblocks, + Matrix& E, Vector& b, const Cameras& cameras) const { + if (this->degenerate_) { + throw("FIXME: computeJacobians degenerate case commented out!"); +// std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl; +// std::cout << "point " << point_ << std::endl; +// std::cout +// << "SmartStereoProjectionFactor: Management of degeneracy is disabled - not ready to be used" +// << std::endl; +// if (D > 6) { +// std::cout +// << "Management of degeneracy is not yet ready when one also optimizes for the calibration " +// << std::endl; +// } +// +// int numKeys = this->keys_.size(); +// E = zeros(2 * numKeys, 2); +// b = zero(2 * numKeys); +// double f = 0; +// for (size_t i = 0; i < this->measured_.size(); i++) { +// if (i == 0) { // first pose +// this->point_ = cameras[i].backprojectPointAtInfinity( +// this->measured_.at(i)); +// // 3D parametrization of point at infinity: [px py 1] +// } +// Matrix Fi, Ei; +// Vector bi = -(cameras[i].projectPointAtInfinity(this->point_, Fi, Ei) +// - this->measured_.at(i)).vector(); +// +// this->noise_.at(i)->WhitenSystem(Fi, Ei, bi); +// f += bi.squaredNorm(); +// Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi)); +// E.block < 2, 2 > (2 * i, 0) = Ei; +// subInsert(b, bi, 2 * i); +// } +// return f; + } else { + // nondegenerate: just return Base version + return Base::computeJacobians(Fblocks, E, b, cameras, point_); + } // end else + } + +// /// Version that computes PointCov, with optional lambda parameter +// double computeJacobians(std::vector& Fblocks, +// Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras, +// const double lambda = 0.0) const { +// +// double f = computeJacobians(Fblocks, E, b, cameras); +// +// // Point covariance inv(E'*E) +// PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse(); +// +// return f; +// } +// +// /// takes values +// bool computeJacobiansSVD(std::vector& Fblocks, +// Matrix& Enull, Vector& b, const Values& values) const { +// typename Base::Cameras myCameras; +// double good = computeCamerasAndTriangulate(values, myCameras); +// if (good) +// computeJacobiansSVD(Fblocks, Enull, b, myCameras); +// return true; +// } +// +// /// SVD version +// double computeJacobiansSVD(std::vector& Fblocks, +// Matrix& Enull, Vector& b, const Cameras& cameras) const { +// return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_); +// } +// +// /// Returns Matrix, TODO: maybe should not exist -> not sparse ! +// // TODO should there be a lambda? +// double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, +// const Cameras& cameras) const { +// return Base::computeJacobiansSVD(F, Enull, b, cameras, point_); +// } + + /// Returns Matrix, TODO: maybe should not exist -> not sparse ! + double computeJacobians(Matrix& F, Matrix& E, Vector& b, + const Cameras& cameras) const { + return Base::computeJacobians(F, E, b, cameras, point_); + } + + /// Calculate vector of re-projection errors, before applying noise model + /// Assumes triangulation was done and degeneracy handled + Vector reprojectionError(const Cameras& cameras) const { + return Base::reprojectionError(cameras, point_); + } + + /// Calculate vector of re-projection errors, before applying noise model + Vector reprojectionError(const Values& values) const { + Cameras myCameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + if (nonDegenerate) + return reprojectionError(myCameras); + else + return zero(myCameras.size() * 3); + } + + /** + * Calculate the error of the factor. + * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. + * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model + * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. + */ + double totalReprojectionError(const Cameras& cameras, + boost::optional externalPoint = boost::none) const { + + size_t nrCameras; + if (externalPoint) { + nrCameras = this->keys_.size(); + point_ = *externalPoint; + degenerate_ = false; + cheiralityException_ = false; + } else { + nrCameras = this->triangulateSafe(cameras); + } + + if (nrCameras < 2 + || (!this->manageDegeneracy_ + && (this->cheiralityException_ || this->degenerate_))) { + // if we don't want to manage the exceptions we discard the factor + // std::cout << "In error evaluation: exception" << std::endl; + return 0.0; + } + + if (this->cheiralityException_) { // if we want to manage the exceptions with rotation-only factors + std::cout + << "SmartProjectionHessianFactor: cheirality exception (this should not happen if CheiralityException is disabled)!" + << std::endl; + this->degenerate_ = true; + } + + if (this->degenerate_) { + return 0.0; // TODO: this maybe should be zero? +// std::cout +// << "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!" +// << std::endl; +// size_t i = 0; +// double overallError = 0; +// BOOST_FOREACH(const Camera& camera, cameras) { +// const StereoPoint2& zi = this->measured_.at(i); +// if (i == 0) // first pose +// this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity +// StereoPoint2 reprojectionError( +// camera.projectPointAtInfinity(this->point_) - zi); +// overallError += 0.5 +// * this->noise_.at(i)->distance(reprojectionError.vector()); +// i += 1; +// } +// return overallError; + } else { + // Just use version in base class + return Base::totalReprojectionError(cameras, point_); + } + } + + /// Cameras are computed in derived class + virtual Cameras cameras(const Values& values) const = 0; + + /** return the landmark */ + boost::optional point() const { + return point_; + } + + /** COMPUTE the landmark */ + boost::optional point(const Values& values) const { + triangulateSafe(values); + return point_; + } + + /** return the degenerate state */ + inline bool isDegenerate() const { + return (cheiralityException_ || degenerate_); + } + + /** return the cheirality status flag */ + inline bool isPointBehindCamera() const { + return cheiralityException_; + } + /** return chirality verbosity */ + inline bool verboseCheirality() const { + return verboseCheirality_; + } + + /** return flag for throwing cheirality exceptions */ + inline bool throwCheirality() const { + return throwCheirality_; + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(throwCheirality_); + ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); + } +}; + +/// traits +template +struct traits > : + public Testable > { +}; + +} // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h new file mode 100644 index 000000000..3db1c883e --- /dev/null +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -0,0 +1,219 @@ +/* ---------------------------------------------------------------------------- + + * 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 SmartStereoProjectionPoseFactor.h + * @brief Produces an Hessian factors on POSES from monocular measurements of a single landmark + * @author Luca Carlone + * @author Chris Beall + * @author Zsolt Kira + */ + +#pragma once + +#include + +namespace gtsam { +/** + * + * @addtogroup SLAM + * + * If you are using the factor, please cite: + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally + * independent sets in factor graphs: a unifying perspective based on smart factors, + * Int. Conf. on Robotics and Automation (ICRA), 2014. + * + */ + +/** + * The calibration is known here. The factor only constraints poses (variable dimension is 6) + * @addtogroup SLAM + */ +template +class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { +protected: + + LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q) + + std::vector > K_all_; ///< shared pointer to calibration object (one for each camera) + +public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// shorthand for base class type + typedef SmartStereoProjectionFactor Base; + + /// shorthand for this class + typedef SmartStereoProjectionPoseFactor This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** + * Constructor + * @param rankTol tolerance used to check if point triangulation is degenerate + * @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization) + * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, + * otherwise the factor is simply neglected + * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations + * @param body_P_sensor is the transform from body to sensor frame (default identity) + */ + SmartStereoProjectionPoseFactor(const double rankTol = 1, + const double linThreshold = -1, const bool manageDegeneracy = false, + const bool enableEPI = false, boost::optional body_P_sensor = boost::none, + LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, + double dynamicOutlierRejectionThreshold = -1) : + Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor, + landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {} + + /** Virtual destructor */ + virtual ~SmartStereoProjectionPoseFactor() {} + + /** + * add a new measurement and pose key + * @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement) + * @param poseKey is key corresponding to the camera observing the same landmark + * @param noise_i is the measurement noise + * @param K_i is the (known) camera calibration + */ + void add(const StereoPoint2 measured_i, const Key poseKey_i, + const SharedNoiseModel noise_i, + const boost::shared_ptr K_i) { + Base::add(measured_i, poseKey_i, noise_i); + K_all_.push_back(K_i); + } + + /** + * Variant of the previous one in which we include a set of measurements + * @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) + * @param poseKeys vector of keys corresponding to the camera observing the same landmark + * @param noises vector of measurement noises + * @param Ks vector of calibration objects + */ + void add(std::vector measurements, std::vector poseKeys, + std::vector noises, + std::vector > Ks) { + Base::add(measurements, poseKeys, noises); + for (size_t i = 0; i < measurements.size(); i++) { + K_all_.push_back(Ks.at(i)); + } + } + + /** + * Variant of the previous one in which we include a set of measurements with the same noise and calibration + * @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) + * @param poseKeys vector of keys corresponding to the camera observing the same landmark + * @param noise measurement noise (same for all measurements) + * @param K the (known) camera calibration (same for all measurements) + */ + void add(std::vector measurements, std::vector poseKeys, + const SharedNoiseModel noise, const boost::shared_ptr K) { + for (size_t i = 0; i < measurements.size(); i++) { + Base::add(measurements.at(i), poseKeys.at(i), noise); + K_all_.push_back(K); + } + } + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "SmartStereoProjectionPoseFactor, z = \n "; + BOOST_FOREACH(const boost::shared_ptr& K, K_all_) + K->print("calibration = "); + Base::print("", keyFormatter); + } + + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + + return e && Base::equals(p, tol); + } + + /** + * Collect all cameras involved in this factor + * @param values Values structure which must contain camera poses corresponding + * to keys involved in this factor + * @return vector of Values + */ + typename Base::Cameras cameras(const Values& values) const { + typename Base::Cameras cameras; + size_t i=0; + BOOST_FOREACH(const Key& k, this->keys_) { + Pose3 pose = values.at(k); + typename Base::Camera camera(pose, K_all_[i++]); + cameras.push_back(camera); + } + return cameras; + } + + /** + * Linearize to Gaussian Factor + * @param values Values structure which must contain camera poses for this factor + * @return + */ + virtual boost::shared_ptr linearize( + const Values& values) const { + // depending on flag set on construction we may linearize to different linear factors + switch(linearizeTo_){ + case JACOBIAN_SVD : + return this->createJacobianSVDFactor(cameras(values), 0.0); + break; + case JACOBIAN_Q : + throw("JacobianQ not working yet!"); +// return this->createJacobianQFactor(cameras(values), 0.0); + break; + default: + return this->createHessianFactor(cameras(values)); + break; + } + } + + /** + * error calculates the error of the factor. + */ + virtual double error(const Values& values) const { + if (this->active(values)) { + return this->totalReprojectionError(cameras(values)); + } else { // else of active flag + return 0.0; + } + } + + /** return the calibration object */ + inline const std::vector > calibration() const { + return K_all_; + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(K_all_); + } + +}; // end of class declaration + +/// traits +template +struct traits > : public Testable< + SmartStereoProjectionPoseFactor > { +}; + +} // \ namespace gtsam diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h new file mode 100644 index 000000000..b500b36e3 --- /dev/null +++ b/gtsam_unstable/slam/TOAFactor.h @@ -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 + + * -------------------------------------------------------------------------- */ + +/** + * @file TOAFactor.h + * @brief "Time of Arrival" factor + * @author Frank Dellaert + * @author Jay Chakravarty + * @date December 2014 + */ + +#include +#include + +namespace gtsam { + +/// A "Time of Arrival" factor - so little code seems hardly worth it :-) +class TOAFactor: public ExpressionFactor { + + typedef Expression double_; + +public: + + /** + * Constructor + * @param some expression yielding an event + * @param microphone_ expression yielding a microphone location + * @param toaMeasurement time of arrival at microphone + * @param model noise model + */ + TOAFactor(const Expression& eventExpression, + const Expression& microphone_, double toaMeasurement, + const SharedNoiseModel& model) : + ExpressionFactor(model, toaMeasurement, + double_(&Event::toa, eventExpression, microphone_)) { + } + +}; + +} //\ namespace gtsam + diff --git a/gtsam_unstable/slam/TSAMFactors.h b/gtsam_unstable/slam/TSAMFactors.h index d4c5f8172..aae4e413d 100644 --- a/gtsam_unstable/slam/TSAMFactors.h +++ b/gtsam_unstable/slam/TSAMFactors.h @@ -49,7 +49,7 @@ public: boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { Point2 d = pose.transform_to(point, H1, H2); - Point2 e = measured_.between(d); + Point2 e = d - measured_; return e.vector(); } }; @@ -99,12 +99,12 @@ public: *H3 = D_e_point_g * D_point_g_base2; if (H4) *H4 = D_e_point_g * D_point_g_point; - return measured_.localCoordinates(d); + return (d - measured_).vector(); } else { Pose2 pose_g = base1.compose(pose); Point2 point_g = base2.transform_from(point); Point2 d = pose_g.transform_to(point_g); - return measured_.localCoordinates(d); + return (d - measured_).vector(); } } }; diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index 249e4fc20..156ac242e 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -16,13 +16,13 @@ **/ #pragma once -#include - -#include -#include +#include #include #include -#include +#include +#include + +#include namespace gtsam { @@ -217,11 +217,17 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); //ar & BOOST_SERIALIZATION_NVP(measured_); } }; // \class TransformBtwRobotsUnaryFactor + /// traits + template + struct traits > : + public Testable > { + }; + } /// namespace gtsam diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index e3de43628..bf10ae531 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -16,15 +16,15 @@ **/ #pragma once -#include - -#include -#include +#include #include #include #include #include -#include +#include +#include + +#include namespace gtsam { @@ -415,11 +415,17 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); //ar & BOOST_SERIALIZATION_NVP(measured_); } }; // \class TransformBtwRobotsUnaryFactorEM + /// traits + template + struct traits > : + public Testable > { + }; + } /// namespace gtsam diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index 2f54528b8..a598fb689 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -5,6 +5,8 @@ * @author Alex Cunningham */ +#include +#include #include #include @@ -22,9 +24,6 @@ #include #include #include -#include -#include -//#include #include #include #include @@ -117,21 +116,21 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); /* Create GUIDs for geometry */ /* ************************************************************************* */ -BOOST_CLASS_EXPORT(gtsam::LieVector); -BOOST_CLASS_EXPORT(gtsam::LieMatrix); -BOOST_CLASS_EXPORT(gtsam::Point2); -BOOST_CLASS_EXPORT(gtsam::StereoPoint2); -BOOST_CLASS_EXPORT(gtsam::Point3); -BOOST_CLASS_EXPORT(gtsam::Rot2); -BOOST_CLASS_EXPORT(gtsam::Rot3); -BOOST_CLASS_EXPORT(gtsam::Pose2); -BOOST_CLASS_EXPORT(gtsam::Pose3); -BOOST_CLASS_EXPORT(gtsam::Cal3_S2); -BOOST_CLASS_EXPORT(gtsam::Cal3DS2); -BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo); -BOOST_CLASS_EXPORT(gtsam::CalibratedCamera); -BOOST_CLASS_EXPORT(gtsam::SimpleCamera); -BOOST_CLASS_EXPORT(gtsam::StereoCamera); +GTSAM_VALUE_EXPORT(gtsam::LieVector); +GTSAM_VALUE_EXPORT(gtsam::LieMatrix); +GTSAM_VALUE_EXPORT(gtsam::Point2); +GTSAM_VALUE_EXPORT(gtsam::StereoPoint2); +GTSAM_VALUE_EXPORT(gtsam::Point3); +GTSAM_VALUE_EXPORT(gtsam::Rot2); +GTSAM_VALUE_EXPORT(gtsam::Rot3); +GTSAM_VALUE_EXPORT(gtsam::Pose2); +GTSAM_VALUE_EXPORT(gtsam::Pose3); +GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); +GTSAM_VALUE_EXPORT(gtsam::Cal3DS2); +GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo); +GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera); +GTSAM_VALUE_EXPORT(gtsam::SimpleCamera); +GTSAM_VALUE_EXPORT(gtsam::StereoCamera); /* Create GUIDs for factors */ /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testAHRS.cpp b/gtsam_unstable/slam/tests/testAHRS.cpp index 44f516ae9..d0980f452 100644 --- a/gtsam_unstable/slam/tests/testAHRS.cpp +++ b/gtsam_unstable/slam/tests/testAHRS.cpp @@ -30,7 +30,7 @@ TEST (AHRS, cov) { 9.0, 4.0, 7.0, 6.0, 3.0, 2.0).finished(); - Matrix actual = cov(trans(A)); + Matrix actual = AHRS::Cov(trans(A)); Matrix expected = (Matrix(3, 3) << 10.9167, 2.3333, 5.0000, 2.3333, 4.6667, -2.6667, @@ -42,7 +42,7 @@ TEST (AHRS, cov) { /* ************************************************************************* */ TEST (AHRS, covU) { - Matrix actual = cov(10000*stationaryU); + Matrix actual = AHRS::Cov(10000*stationaryU); Matrix expected = (Matrix(3, 3) << 33.3333333, -1.66666667, 53.3333333, -1.66666667, 0.333333333, -5.16666667, @@ -54,7 +54,7 @@ TEST (AHRS, covU) { /* ************************************************************************* */ TEST (AHRS, covF) { - Matrix actual = cov(100*stationaryF); + Matrix actual = AHRS::Cov(100*stationaryF); Matrix expected = (Matrix(3, 3) << 63.3808333, -0.432166667, -48.1706667, -0.432166667, 8.31053333, -16.6792667, diff --git a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp index ff7307840..a86306a8d 100644 --- a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp +++ b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp @@ -20,9 +20,8 @@ #include #include #include -#include - #include +#include using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index 6cfcd93e6..efe1df640 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -218,7 +218,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector3 Vel1(Vector3(0.5, -0.5, 0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); - Point3 t2 = t1.compose(Point3(Vel1 * measurement_dt)); + Point3 t2 = t1 + Point3(Vel1 * measurement_dt); Pose3 Pose2(R2, t2); Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g); Vector3 Vel2 = Vel1 + dv; @@ -568,7 +568,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector3 Vel1(Vector3(0.5, -0.5, 0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); - Point3 t2 = t1.compose(Point3(Vel1 * measurement_dt)); + Point3 t2 = t1+ Point3(Vel1 * measurement_dt); Pose3 Pose2(R2, t2); Vector dv = measurement_dt diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index 20e96e6bf..99a4f90a4 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -73,9 +73,8 @@ TEST( InvDepthFactorVariant1, optimize) { // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); - Vector6 actual = result.at(landmarkKey); - +// Vector6 actual = result.at(landmarkKey); // values.at(poseKey1).print("Pose1 Before:\n"); // result.at(poseKey1).print("Pose1 After:\n"); // pose1.print("Pose1 Truth:\n"); @@ -116,8 +115,11 @@ TEST( InvDepthFactorVariant1, optimize) { // However, since this is an over-parameterization, there can be // many 6D landmark values that equate to the same 3D world position // Instead, directly test the recovered 3D landmark position - //EXPECT(assert_equal(expected, actual, 1e-9)); EXPECT(assert_equal(landmark, world_landmarkAfter, 1e-9)); + + // Frank asks: why commented out? + //Vector6 actual = result.at(landmarkKey); + //EXPECT(assert_equal(expected, actual, 1e-9)); } diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp index 238bece62..b468a2b6e 100644 --- a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp +++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp @@ -28,6 +28,13 @@ using namespace gtsam; typedef PoseBetweenFactor TestPoseBetweenFactor; +/// traits +namespace gtsam { +template<> +struct traits : public Testable { +}; +} + /* ************************************************************************* */ TEST( PoseBetweenFactor, Constructor) { Key poseKey1(1); diff --git a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp index 9aa87fb83..ddb5cf7a2 100644 --- a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp @@ -28,6 +28,13 @@ using namespace gtsam; typedef PosePriorFactor TestPosePriorFactor; +/// traits +namespace gtsam { +template<> +struct traits : public Testable { +}; +} + /* ************************************************************************* */ TEST( PosePriorFactor, Constructor) { Key poseKey(1); diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp index 0e5f6421f..573352e87 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp @@ -48,6 +48,13 @@ using symbol_shorthand::T; typedef ProjectionFactorPPP TestProjectionFactor; +/// traits +namespace gtsam { +template<> +struct traits : public Testable { +}; +} + /* ************************************************************************* */ TEST( ProjectionFactorPPP, nonStandard ) { ProjectionFactorPPP f; diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp new file mode 100644 index 000000000..eac63006d --- /dev/null +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -0,0 +1,1109 @@ +/* ---------------------------------------------------------------------------- + + * 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 TestSmartStereoProjectionPoseFactor.cpp + * @brief Unit tests for ProjectionFactor Class + * @author Chris Beall + * @author Luca Carlone + * @author Zsolt Kira + * @date Sept 2013 + */ + +#include + +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace boost::assign; +using namespace gtsam; + +static bool isDebugTest = false; + +// make a realistic calibration matrix +static double fov = 60; // degrees +static size_t w = 640, h = 480; +static double b = 1; + +static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b)); +static Cal3_S2Stereo::shared_ptr K2( + new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b)); +static boost::shared_ptr Kbundler( + new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); + +static double rankTol = 1.0; +static double linThreshold = -1.0; +// static bool manageDegeneracy = true; +// Create a noise model for the pixel error +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1)); + +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; + +// tests data +static Symbol x1('X', 1); +static Symbol x2('X', 2); +static Symbol x3('X', 3); + +static Key poseKey1(x1); +static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value? +static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); + +typedef SmartStereoProjectionPoseFactor SmartFactor; +typedef SmartStereoProjectionPoseFactor SmartFactorBundler; + +vector stereo_projectToMultipleCameras(const StereoCamera& cam1, + const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { + + vector measurements_cam; + + StereoPoint2 cam1_uv1 = cam1.project(landmark); + StereoPoint2 cam2_uv1 = cam2.project(landmark); + StereoPoint2 cam3_uv1 = cam3.project(landmark); + measurements_cam.push_back(cam1_uv1); + measurements_cam.push_back(cam2_uv1); + measurements_cam.push_back(cam3_uv1); + + return measurements_cam; +} + +/* ************************************************************************* */ +TEST( SmartStereoProjectionPoseFactor, Constructor) { + fprintf(stderr, "Test 1 Complete"); + SmartFactor::shared_ptr factor1(new SmartFactor()); +} + +/* ************************************************************************* */ +TEST( SmartStereoProjectionPoseFactor, Constructor2) { + SmartFactor factor1(rankTol, linThreshold); +} + +/* ************************************************************************* */ +TEST( SmartStereoProjectionPoseFactor, Constructor3) { + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(measurement1, poseKey1, model, K); +} + +/* ************************************************************************* */ +TEST( SmartStereoProjectionPoseFactor, Constructor4) { + SmartFactor factor1(rankTol, linThreshold); + factor1.add(measurement1, poseKey1, model, K); +} + +/* ************************************************************************* */ +TEST( SmartStereoProjectionPoseFactor, ConstructorWithTransform) { + bool manageDegeneracy = true; + bool enableEPI = false; + SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, + body_P_sensor1); + factor1.add(measurement1, poseKey1, model, K); +} + +/* ************************************************************************* */ +TEST( SmartStereoProjectionPoseFactor, Equals ) { + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(measurement1, poseKey1, model, K); + + SmartFactor::shared_ptr factor2(new SmartFactor()); + factor2->add(measurement1, poseKey1, model, K); + + CHECK(assert_equal(*factor1, *factor2)); +} + +/* *************************************************************************/ +TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { + // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl; + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); + StereoCamera level_camera(level_pose, K2); + + // create second camera 1 meter to the right of first camera + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); + StereoCamera level_camera_right(level_pose_right, K2); + + // landmark ~5 meters infront of camera + Point3 landmark(5, 0.5, 1.2); + + // 1. Project two landmarks into two cameras and triangulate + StereoPoint2 level_uv = level_camera.project(landmark); + StereoPoint2 level_uv_right = level_camera_right.project(landmark); + + Values values; + values.insert(x1, level_pose); + values.insert(x2, level_pose_right); + + SmartFactor factor1; + factor1.add(level_uv, x1, model, K2); + factor1.add(level_uv_right, x2, model, K2); + + double actualError = factor1.error(values); + double expectedError = 0.0; + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); + + SmartFactor::Cameras cameras = factor1.cameras(values); + double actualError2 = factor1.totalReprojectionError(cameras); + EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); + + // test vector of errors + //Vector actual = factor1.unwhitenedError(values); + //EXPECT(assert_equal(zero(4),actual,1e-8)); +} + +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, noisy ) { + // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl; + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); + StereoCamera level_camera(level_pose, K2); + + // create second camera 1 meter to the right of first camera + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); + StereoCamera level_camera_right(level_pose_right, K2); + + // landmark ~5 meters infront of camera + Point3 landmark(5, 0.5, 1.2); + + // 1. Project two landmarks into two cameras and triangulate + StereoPoint2 pixelError(0.2, 0.2, 0); + StereoPoint2 level_uv = level_camera.project(landmark) + pixelError; + StereoPoint2 level_uv_right = level_camera_right.project(landmark); + + Values values; + values.insert(x1, level_pose); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + values.insert(x2, level_pose_right.compose(noise_pose)); + + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(level_uv, x1, model, K); + factor1->add(level_uv_right, x2, model, K); + + double actualError1 = factor1->error(values); + + SmartFactor::shared_ptr factor2(new SmartFactor()); + vector measurements; + measurements.push_back(level_uv); + measurements.push_back(level_uv_right); + + vector noises; + noises.push_back(model); + noises.push_back(model); + + vector > Ks; ///< shared pointer to calibration object (one for each camera) + Ks.push_back(K); + Ks.push_back(K); + + vector views; + views.push_back(x1); + views.push_back(x2); + + factor2->add(measurements, views, noises, Ks); + + double actualError2 = factor2->error(values); + + DOUBLES_EQUAL(actualError1, actualError2, 1e-7); +} + +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { + cout + << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************" + << endl; + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + StereoCamera cam1(pose1, K2); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); + StereoCamera cam2(pose2, K2); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); + StereoCamera cam3(pose3, K2); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + // 1. Project three landmarks into three cameras and triangulate + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + + vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + smartFactor1->add(measurements_cam1, views, model, K2); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor()); + smartFactor2->add(measurements_cam2, views, model, K2); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor()); + smartFactor3->add(measurements_cam3, views, model, K2); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams params; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + gttic_ (SmartStereoProjectionPoseFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + gttoc_(SmartStereoProjectionPoseFactor); + tictoc_finishedIteration_(); + +// GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); +// VectorValues delta = GFG->optimize(); + + // result.print("results of 3 camera, 3 landmark optimization \n"); + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(pose3, result.at(x3))); + if (isDebugTest) + tictoc_print_(); +} + +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { + + vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + StereoCamera cam1(pose1, K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); + StereoCamera cam2(pose2, K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); + StereoCamera cam3(pose3, K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + // 1. Project three landmarks into three cameras and triangulate + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + smartFactor3->add(measurements_cam3, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3 * noise_pose); + + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3, result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { + + double excludeLandmarksFutherThanDist = 2; + + vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + StereoCamera cam1(pose1, K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); + StereoCamera cam2(pose2, K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); + StereoCamera cam3(pose3, K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + // 1. Project three landmarks into three cameras and triangulate + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); + smartFactor3->add(measurements_cam3, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3 * noise_pose); + + // All factors are disabled and pose should remain where it is + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(values.at(x3), result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { + + double excludeLandmarksFutherThanDist = 1e10; + double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + + vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + StereoCamera cam1(pose1, K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); + StereoCamera cam2(pose2, K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); + StereoCamera cam3(pose3, K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + Point3 landmark4(5, -0.5, 1); + + // 1. Project four landmarks into three cameras and triangulate + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + vector measurements_cam4 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark4); + + measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor3->add(measurements_cam3, views, model, K); + + SmartFactor::shared_ptr smartFactor4( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor4->add(measurements_cam4, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(smartFactor4); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3); + + // All factors are disabled and pose should remain where it is + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3, result.at(x3))); +} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, jacobianQ ){ +// +// vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); +// StereoCamera cam1(pose1, K); +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// StereoCamera cam2(pose2, K); +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// StereoCamera cam3(pose3, K); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// Point3 landmark3(3, 0, 3.0); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // 1. Project three landmarks into three cameras and triangulate +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); +// +// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); +// smartFactor1->add(measurements_cam1, views, model, K); +// +// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); +// smartFactor2->add(measurements_cam2, views, model, K); +// +// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); +// smartFactor3->add(measurements_cam3, views, model, K); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.push_back(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PriorFactor(x2, pose2, noisePrior)); +// +// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// values.insert(x3, pose3*noise_pose); +// +// LevenbergMarquardtParams params; +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// result = optimizer.optimize(); +// EXPECT(assert_equal(pose3,result.at(x3))); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){ +// // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; +// +// vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); +// StereoCamera cam1(pose1, K2); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// StereoCamera cam2(pose2, K2); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// StereoCamera cam3(pose3, K2); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// Point3 landmark3(3, 0, 3.0); +// +// typedef GenericStereoFactor ProjectionFactor; +// NonlinearFactorGraph graph; +// +// // 1. Project three landmarks into three cameras and triangulate +// graph.push_back(ProjectionFactor(cam1.project(landmark1), model, x1, L(1), K2)); +// graph.push_back(ProjectionFactor(cam2.project(landmark1), model, x2, L(1), K2)); +// graph.push_back(ProjectionFactor(cam3.project(landmark1), model, x3, L(1), K2)); +// +// graph.push_back(ProjectionFactor(cam1.project(landmark2), model, x1, L(2), K2)); +// graph.push_back(ProjectionFactor(cam2.project(landmark2), model, x2, L(2), K2)); +// graph.push_back(ProjectionFactor(cam3.project(landmark2), model, x3, L(2), K2)); +// +// graph.push_back(ProjectionFactor(cam1.project(landmark3), model, x1, L(3), K2)); +// graph.push_back(ProjectionFactor(cam2.project(landmark3), model, x2, L(3), K2)); +// graph.push_back(ProjectionFactor(cam3.project(landmark3), model, x3, L(3), K2)); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// graph.push_back(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PriorFactor(x2, pose2, noisePrior)); +// +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// values.insert(x3, pose3* noise_pose); +// values.insert(L(1), landmark1); +// values.insert(L(2), landmark2); +// values.insert(L(3), landmark3); +// if(isDebugTest) values.at(x3).print("Pose3 before optimization: "); +// +// LevenbergMarquardtParams params; +// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; +// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// Values result = optimizer.optimize(); +// +// if(isDebugTest) result.at(x3).print("Pose3 after optimization: "); +// EXPECT(assert_equal(pose3,result.at(x3))); +//} +// +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, CheckHessian) { + + vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + StereoCamera cam1(pose1, K); + + // create second camera + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + StereoCamera cam2(pose2, K); + + // create third camera + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + StereoCamera cam3(pose3, K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + // Project three landmarks into three cameras and triangulate + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + + // Create smartfactors + double rankTol = 10; + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol)); + smartFactor3->add(measurements_cam3, views, model, K); + + // Create graph to optimize + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); + + // TODO: next line throws Cheirality exception on Mac + boost::shared_ptr hessianFactor1 = smartFactor1->linearize( + values); + boost::shared_ptr hessianFactor2 = smartFactor2->linearize( + values); + boost::shared_ptr hessianFactor3 = smartFactor3->linearize( + values); + + Matrix CumulativeInformation = hessianFactor1->information() + + hessianFactor2->information() + hessianFactor3->information(); + + boost::shared_ptr GaussianGraph = graph.linearize( + values); + Matrix GraphInformation = GaussianGraph->hessian().first; + + // Check Hessian + EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8)); + + Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + + hessianFactor2->augmentedInformation() + + hessianFactor3->augmentedInformation(); + + // Check Information vector + // cout << AugInformationMatrix.size() << endl; + Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector + + // Check Hessian + EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); +} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){ +// // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; +// +// vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); +// StereoCamera cam1(pose1, K2); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); +// StereoCamera cam2(pose2, K2); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); +// StereoCamera cam3(pose3, K2); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // 1. Project three landmarks into three cameras and triangulate +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// +// double rankTol = 50; +// SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// smartFactor1->add(measurements_cam1, views, model, K2); +// +// SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// smartFactor2->add(measurements_cam2, views, model, K2); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); +// Point3 positionPrior = Point3(0,0,1); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); +// graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); +// +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.1,0.1,0.1)); // smaller noise +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2*noise_pose); +// // initialize third pose with some noise, we expect it to move back to original pose3 +// values.insert(x3, pose3*noise_pose*noise_pose); +// if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); +// +// LevenbergMarquardtParams params; +// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; +// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; +// +// Values result; +// gttic_(SmartStereoProjectionPoseFactor); +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// result = optimizer.optimize(); +// gttoc_(SmartStereoProjectionPoseFactor); +// tictoc_finishedIteration_(); +// +// // result.print("results of 3 camera, 3 landmark optimization \n"); +// if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); +// cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl; +// // EXPECT(assert_equal(pose3,result.at(x3))); +// if(isDebugTest) tictoc_print_(); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ +// // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; +// +// vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); +// StereoCamera cam1(pose1, K); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); +// StereoCamera cam2(pose2, K); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); +// StereoCamera cam3(pose3, K); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// Point3 landmark3(3, 0, 3.0); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // 1. Project three landmarks into three cameras and triangulate +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); +// +// double rankTol = 10; +// +// SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// smartFactor1->add(measurements_cam1, views, model, K); +// +// SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// smartFactor2->add(measurements_cam2, views, model, K); +// +// SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// smartFactor3->add(measurements_cam3, views, model, K); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); +// Point3 positionPrior = Point3(0,0,1); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.push_back(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); +// graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); +// +// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// // initialize third pose with some noise, we expect it to move back to original pose3 +// values.insert(x3, pose3*noise_pose); +// if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); +// +// LevenbergMarquardtParams params; +// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; +// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; +// +// Values result; +// gttic_(SmartStereoProjectionPoseFactor); +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// result = optimizer.optimize(); +// gttoc_(SmartStereoProjectionPoseFactor); +// tictoc_finishedIteration_(); +// +// // result.print("results of 3 camera, 3 landmark optimization \n"); +// if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); +// cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl; +// // EXPECT(assert_equal(pose3,result.at(x3))); +// if(isDebugTest) tictoc_print_(); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, Hessian ){ +// // cout << " ************************ SmartStereoProjectionPoseFactor: Hessian **********************" << endl; +// +// vector views; +// views.push_back(x1); +// views.push_back(x2); +// +// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); +// StereoCamera cam1(pose1, K2); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// StereoCamera cam2(pose2, K2); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// +// // 1. Project three landmarks into three cameras and triangulate +// StereoPoint2 cam1_uv1 = cam1.project(landmark1); +// StereoPoint2 cam2_uv1 = cam2.project(landmark1); +// vector measurements_cam1; +// measurements_cam1.push_back(cam1_uv1); +// measurements_cam1.push_back(cam2_uv1); +// +// SmartFactor::shared_ptr smartFactor1(new SmartFactor()); +// smartFactor1->add(measurements_cam1,views, model, K2); +// +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// +// boost::shared_ptr hessianFactor = smartFactor1->linearize(values); +// if(isDebugTest) hessianFactor->print("Hessian factor \n"); +// +// // compute triangulation from linearization point +// // compute reprojection errors (sum squared) +// // compare with hessianFactor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) +// // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] +//} +// + +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { + // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian **********************" << endl; + + vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + StereoCamera cam1(pose1, K); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); + StereoCamera cam2(pose2, K); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); + StereoCamera cam3(pose3, K); + + Point3 landmark1(5, 0.5, 1.2); + + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + + SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); + smartFactorInstance->add(measurements_cam1, views, model, K); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3); + + boost::shared_ptr hessianFactor = + smartFactorInstance->linearize(values); + // hessianFactor->print("Hessian factor \n"); + + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); + + Values rotValues; + rotValues.insert(x1, poseDrift.compose(pose1)); + rotValues.insert(x2, poseDrift.compose(pose2)); + rotValues.insert(x3, poseDrift.compose(pose3)); + + boost::shared_ptr hessianFactorRot = + smartFactorInstance->linearize(rotValues); + // hessianFactorRot->print("Hessian factor \n"); + + // Hessian is invariant to rotations in the nondegenerate case + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRot->information(), 1e-7)); + + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Point3(10, -4, 5)); + + Values tranValues; + tranValues.insert(x1, poseDrift2.compose(pose1)); + tranValues.insert(x2, poseDrift2.compose(pose2)); + tranValues.insert(x3, poseDrift2.compose(pose3)); + + boost::shared_ptr hessianFactorRotTran = + smartFactorInstance->linearize(tranValues); + + // Hessian is invariant to rotations and translations in the nondegenerate case + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRotTran->information(), 1e-6)); +} + +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { + // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; + + vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + StereoCamera cam1(pose1, K2); + + // Second and third cameras in same place, which is a degenerate configuration + Pose3 pose2 = pose1; + Pose3 pose3 = pose1; + StereoCamera cam2(pose2, K2); + StereoCamera cam3(pose3, K2); + + Point3 landmark1(5, 0.5, 1.2); + + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + + SmartFactor::shared_ptr smartFactor(new SmartFactor()); + smartFactor->add(measurements_cam1, views, model, K2); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3); + + boost::shared_ptr hessianFactor = smartFactor->linearize( + values); + if (isDebugTest) + hessianFactor->print("Hessian factor \n"); + + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); + + Values rotValues; + rotValues.insert(x1, poseDrift.compose(pose1)); + rotValues.insert(x2, poseDrift.compose(pose2)); + rotValues.insert(x3, poseDrift.compose(pose3)); + + boost::shared_ptr hessianFactorRot = smartFactor->linearize( + rotValues); + if (isDebugTest) + hessianFactorRot->print("Hessian factor \n"); + + // Hessian is invariant to rotations in the nondegenerate case + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRot->information(), 1e-8)); + + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Point3(10, -4, 5)); + + Values tranValues; + tranValues.insert(x1, poseDrift2.compose(pose1)); + tranValues.insert(x2, poseDrift2.compose(pose2)); + tranValues.insert(x3, poseDrift2.compose(pose3)); + + boost::shared_ptr hessianFactorRotTran = + smartFactor->linearize(tranValues); + + // Hessian is invariant to rotations and translations in the nondegenerate case + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRotTran->information(), 1e-8)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp new file mode 100644 index 000000000..879f7095e --- /dev/null +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -0,0 +1,133 @@ +/* ---------------------------------------------------------------------------- + + * 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 testTOAFactor.cpp + * @brief Unit tests for "Time of Arrival" factor + * @author Frank Dellaert + * @author Jay Chakravarty + * @date December 2014 + */ + +#include +#include +#include +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// typedefs +typedef Eigen::Matrix Vector1; +typedef Expression Double_; +typedef Expression Point3_; +typedef Expression Event_; + +// units +static const double ms = 1e-3; +static const double cm = 1e-2; + +// Create a noise model for the TOA error +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms)); + +static const double timeOfEvent = 25; +static const Event exampleEvent(timeOfEvent, 1, 0, 0); +static const Point3 microphoneAt0; + +//***************************************************************************** +TEST( TOAFactor, NewWay ) { + Key key = 12; + Event_ eventExpression(key); + Point3_ microphoneConstant(microphoneAt0); // constant expression + double measurement = 7; + Double_ expression(&Event::toa, eventExpression, microphoneConstant); + ExpressionFactor factor(model, measurement, expression); +} + +//***************************************************************************** +TEST( TOAFactor, WholeEnchilada ) { + + static const bool verbose = false; + + // Create microphones + const double height = 0.5; + vector microphones; + microphones.push_back(Point3(0, 0, height)); + microphones.push_back(Point3(403 * cm, 0, height)); + microphones.push_back(Point3(403 * cm, 403 * cm, height)); + microphones.push_back(Point3(0, 403 * cm, 2 * height)); + EXPECT_LONGS_EQUAL(4, microphones.size()); +// microphones.push_back(Point3(200 * cm, 200 * cm, height)); + + // Create a ground truth point + const double timeOfEvent = 0; + Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm); + + // Simulate simulatedTOA + size_t K = microphones.size(); + vector simulatedTOA(K); + for (size_t i = 0; i < K; i++) { + simulatedTOA[i] = groundTruthEvent.toa(microphones[i]); + if (verbose) { + cout << "mic" << i << " = " << microphones[i] << endl; + cout << "z" << i << " = " << simulatedTOA[i] / ms << endl; + } + } + + // Now, estimate using non-linear optimization + ExpressionFactorGraph graph; + Key key = 12; + Event_ eventExpression(key); + for (size_t i = 0; i < K; i++) { + Point3_ microphone_i(microphones[i]); // constant expression + Double_ predictTOA(&Event::toa, eventExpression, microphone_i); + graph.addExpressionFactor(predictTOA, simulatedTOA[i], model); + } + + /// Print the graph + if (verbose) + GTSAM_PRINT(graph); + + // Create initial estimate + Values initialEstimate; + //Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm); + Vector4 delta; + delta << 0.1, 0.1, -0.1, 0.1; + Event estimatedEvent = groundTruthEvent.retract(delta); + initialEstimate.insert(key, estimatedEvent); + + // Print + if (verbose) + initialEstimate.print("Initial Estimate:\n"); + + // Optimize using Levenberg-Marquardt optimization. + LevenbergMarquardtParams params; + params.setAbsoluteErrorTol(1e-10); + if (verbose) + params.setVerbosity("ERROR"); + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params); + Values result = optimizer.optimize(); + if (verbose) + result.print("Final Result:\n"); + + EXPECT(assert_equal(groundTruthEvent, result.at(key), 1e-6)); +} +//***************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//***************************************************************************** + diff --git a/gtsam_unstable/slam/tests/testTSAMFactors.cpp b/gtsam_unstable/slam/tests/testTSAMFactors.cpp index a8a3ae5e9..ebb52d276 100644 --- a/gtsam_unstable/slam/tests/testTSAMFactors.cpp +++ b/gtsam_unstable/slam/tests/testTSAMFactors.cpp @@ -44,10 +44,10 @@ TEST( DeltaFactor, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11( + H1Expected = numericalDerivative11( boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none, boost::none), pose); - H2Expected = numericalDerivative11( + H2Expected = numericalDerivative11( boost::bind(&DeltaFactor::evaluateError, &factor, pose, _1, boost::none, boost::none), point); @@ -78,16 +78,16 @@ TEST( DeltaFactorBase, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected, H3Expected, H4Expected; - H1Expected = numericalDerivative11( + H1Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2, point, boost::none, boost::none, boost::none, boost::none), base1); - H2Expected = numericalDerivative11( + H2Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, _1, base2, point, boost::none, boost::none, boost::none, boost::none), pose); - H3Expected = numericalDerivative11( + H3Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, _1, point, boost::none, boost::none, boost::none, boost::none), base2); - H4Expected = numericalDerivative11( + H4Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2, _1, boost::none, boost::none, boost::none, boost::none), point); @@ -119,16 +119,16 @@ TEST( OdometryFactorBase, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected, H3Expected, H4Expected; - H1Expected = numericalDerivative11( + H1Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2, pose2, boost::none, boost::none, boost::none, boost::none), base1); - H2Expected = numericalDerivative11( + H2Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, _1, base2, pose2, boost::none, boost::none, boost::none, boost::none), pose1); - H3Expected = numericalDerivative11( + H3Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, _1, pose2, boost::none, boost::none, boost::none, boost::none), base2); - H4Expected = numericalDerivative11( + H4Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, base2, _1, boost::none, boost::none, boost::none, boost::none), pose2); diff --git a/matlab.h b/matlab.h index c4891a615..349cdeea4 100644 --- a/matlab.h +++ b/matlab.h @@ -143,7 +143,7 @@ void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { sigma); Sampler sampler(model, seed); BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); + values.update(key_value.key, key_value.value + Point2(sampler.sample())); } } @@ -164,7 +164,7 @@ void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { sigma); Sampler sampler(model, seed); BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); + values.update(key_value.key, key_value.value + Point3(sampler.sample())); } } diff --git a/matlab/+gtsam/covarianceEllipse.m b/matlab/+gtsam/covarianceEllipse.m index cdc239bb2..829487dc6 100644 --- a/matlab/+gtsam/covarianceEllipse.m +++ b/matlab/+gtsam/covarianceEllipse.m @@ -8,6 +8,8 @@ function h = covarianceEllipse(x,P,color, k) % it is assumed x and y are the first two components of state x % k is scaling for std deviations, defaults to 1 std +hold on + [e,s] = eig(P(1:2,1:2)); s1 = s(1,1); s2 = s(2,2); @@ -32,4 +34,4 @@ h = line(ex,ey,'color',color); y = c(2) + points(2,:); end -end \ No newline at end of file +end diff --git a/matlab/+gtsam/covarianceEllipse3D.m b/matlab/+gtsam/covarianceEllipse3D.m index 9682a9fc1..4364e0fe4 100644 --- a/matlab/+gtsam/covarianceEllipse3D.m +++ b/matlab/+gtsam/covarianceEllipse3D.m @@ -1,4 +1,4 @@ -function covarianceEllipse3D(c,P) +function sc = covarianceEllipse3D(c,P,scale) % covarianceEllipse3D plots a Gaussian as an uncertainty ellipse % Based on Maybeck Vol 1, page 366 % k=2.296 corresponds to 1 std, 68.26% of all probability @@ -6,10 +6,16 @@ function covarianceEllipse3D(c,P) % % Modified from http://www.mathworks.com/matlabcentral/newsreader/view_thread/42966 +hold on + [e,s] = svd(P); k = 11.82; radii = k*sqrt(diag(s)); +if exist('scale', 'var') + radii = radii * scale; +end + % generate data for "unrotated" ellipsoid [xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3),8); diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m new file mode 100644 index 000000000..0abd9cc3c --- /dev/null +++ b/matlab/+gtsam/cylinderSampleProjection.m @@ -0,0 +1,89 @@ +function [visiblePoints] = cylinderSampleProjection(K, pose, imageSize, cylinders) + +% Input: +% Output: +% visiblePoints: data{k} 3D Point in overal point clouds with index k +% Z{k} 2D measurements in overal point clouds with index k +% index {i}{j} +% i: the cylinder index; +% j: the point index on the cylinder; +% +% @Description: Project sampled points on cylinder to camera frame +% @Authors: Zhaoyang Lv + +import gtsam.* + +camera = SimpleCamera(pose, K); + +%% memory allocation +cylinderNum = length(cylinders); + +%% check visiblity of points on each cylinder +pointCloudIndex = 0; +visiblePointIdx = 1; +for i = 1:cylinderNum + + pointNum = length(cylinders{i}.Points); + + % to check point visibility + for j = 1:pointNum + + pointCloudIndex = pointCloudIndex + 1; + + % Cheirality Exception + sampledPoint3 = cylinders{i}.Points{j}; + sampledPoint3local = pose.transform_to(sampledPoint3); + if sampledPoint3local.z <= 0 + continue; + end + Z2d = camera.project(sampledPoint3); + + % ignore points not visible in the scene + if Z2d.x < 0 || Z2d.x >= imageSize.x || Z2d.y < 0 || Z2d.y >= imageSize.y + continue; + end + + % ignore points occluded + % use a simple math hack to check occlusion: + % 1. All points in front of cylinders' surfaces are visible + % 2. For points behind the cylinders' surfaces, the cylinder + visible = true; + for k = 1:cylinderNum + + rayCameraToPoint = pose.translation().between(sampledPoint3).vector(); + rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector(); + rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector(); + + % Condition 1: all points in front of the cylinders' + % surfaces are visible + if dot(rayCylinderToPoint, rayCameraToCylinder) < 0 + continue; + else + projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder); + if projectedRay > 0 + %rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint; + if rayCylinderToPoint(1) > cylinders{k}.radius && ... + rayCylinderToPoint(2) > cylinders{k}.radius + continue; + else + visible = false; + break; + end + end + end + + end + + if visible + visiblePoints.data{visiblePointIdx} = sampledPoint3; + visiblePoints.Z{visiblePointIdx} = Z2d; + visiblePoints.cylinderIdx{visiblePointIdx} = i; + visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex; + visiblePointIdx = visiblePointIdx + 1; + end + + end + +end + +end diff --git a/matlab/+gtsam/cylinderSampleProjectionStereo.m b/matlab/+gtsam/cylinderSampleProjectionStereo.m new file mode 100644 index 000000000..6512231e8 --- /dev/null +++ b/matlab/+gtsam/cylinderSampleProjectionStereo.m @@ -0,0 +1,93 @@ +function [visiblePoints] = cylinderSampleProjectionStereo(K, pose, imageSize, cylinders) + +import gtsam.* + +%% memory allocation +cylinderNum = length(cylinders); + +visiblePoints.data = cell(1); +visiblePoints.Z = cell(1); +visiblePoints.cylinderIdx = cell(1); +visiblePoints.overallIdx = cell(1); + +%% check visiblity of points on each cylinder +pointCloudIndex = 0; +visiblePointIdx = 1; +for i = 1:cylinderNum + + pointNum = length(cylinders{i}.Points); + + % to check point visibility + for j = 1:pointNum + + pointCloudIndex = pointCloudIndex + 1; + + % For Cheirality Exception + sampledPoint3 = cylinders{i}.Points{j}; + sampledPoint3local = pose.transform_to(sampledPoint3); + if sampledPoint3local.z < 0 + continue; + end + + % measurements + Z.du = K.fx() * K.baseline() / sampledPoint3local.z; + Z.uL = K.fx() * sampledPoint3local.x / sampledPoint3local.z + K.px(); + Z.uR = Z.uL + Z.du; + Z.v = K.fy() / sampledPoint3local.z + K.py(); + + % ignore points not visible in the scene + if Z.uL < 0 || Z.uL >= imageSize.x || ... + Z.uR < 0 || Z.uR >= imageSize.x || ... + Z.v < 0 || Z.v >= imageSize.y + continue; + end + + % too small disparity may call indeterminant system exception + if Z.du < 0.6 + continue; + end + + % ignore points occluded + % use a simple math hack to check occlusion: + % 1. All points in front of cylinders' surfaces are visible + % 2. For points behind the cylinders' surfaces, the cylinder + visible = true; + for k = 1:cylinderNum + + rayCameraToPoint = pose.translation().between(sampledPoint3).vector(); + rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector(); + rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector(); + + % Condition 1: all points in front of the cylinders' + % surfaces are visible + if dot(rayCylinderToPoint, rayCameraToCylinder) < 0 + continue; + else + projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder); + if projectedRay > 0 + %rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint; + if rayCylinderToPoint(1) > cylinders{k}.radius && ... + rayCylinderToPoint(2) > cylinders{k}.radius + continue; + else + visible = false; + break; + end + end + end + + end + + if visible + visiblePoints.data{visiblePointIdx} = sampledPoint3; + visiblePoints.Z{visiblePointIdx} = Z; + visiblePoints.cylinderIdx{visiblePointIdx} = i; + visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex; + visiblePointIdx = visiblePointIdx + 1; + end + + end + +end + +end diff --git a/matlab/+gtsam/cylinderSampling.m b/matlab/+gtsam/cylinderSampling.m new file mode 100644 index 000000000..dcaa9c721 --- /dev/null +++ b/matlab/+gtsam/cylinderSampling.m @@ -0,0 +1,26 @@ +function [cylinder] = cylinderSampling(baseCentroid, radius, height, density) +% +% @author: Zhaoyang Lv + import gtsam.* + % calculate the cylinder area + area = 2 * pi * radius * height; + + pointsNum = round(area * density); + + points3 = cell(pointsNum, 1); + + % sample the points + for i = 1:pointsNum + theta = 2 * pi * rand; + x = radius * cos(theta) + baseCentroid.x; + y = radius * sin(theta) + baseCentroid.y; + z = height * rand; + points3{i,1} = Point3([x,y,z]'); + end + + cylinder.area = area; + cylinder.radius = radius; + cylinder.height = height; + cylinder.Points = points3; + cylinder.centroid = Point3(baseCentroid.x, baseCentroid.y, height/2); +end \ No newline at end of file diff --git a/matlab/+gtsam/plot3DTrajectory.m b/matlab/+gtsam/plot3DTrajectory.m index 05483e589..2900aed99 100644 --- a/matlab/+gtsam/plot3DTrajectory.m +++ b/matlab/+gtsam/plot3DTrajectory.m @@ -33,12 +33,12 @@ for i = 0:keys.size-1 end gtsam.plotPose3(lastPose, P, scale); catch err - warning(['no Pose3 at ' lastKey]); + % warning(['no Pose3 at ' lastKey]); end - lastIndex = i; end + lastIndex = i; catch - warning(['no Pose3 at ' key]); + % warning(['no Pose3 at ' key]); end % Draw final pose @@ -53,7 +53,7 @@ for i = 0:keys.size-1 end gtsam.plotPose3(lastPose, P, scale); catch - warning(['no Pose3 at ' lastIndex]); + % warning(['no Pose3 at ' lastIndex]); end end diff --git a/matlab/+gtsam/plotCamera.m b/matlab/+gtsam/plotCamera.m index ba352b757..d0d1f45b7 100644 --- a/matlab/+gtsam/plotCamera.m +++ b/matlab/+gtsam/plotCamera.m @@ -1,18 +1,20 @@ function plotCamera(pose, axisLength) + hold on + C = pose.translation().vector(); R = pose.rotation().matrix(); xAxis = C+R(:,1)*axisLength; L = [C xAxis]'; - line(L(:,1),L(:,2),L(:,3),'Color','r'); + h_x = line(L(:,1),L(:,2),L(:,3),'Color','r'); yAxis = C+R(:,2)*axisLength; L = [C yAxis]'; - line(L(:,1),L(:,2),L(:,3),'Color','g'); + h_y = line(L(:,1),L(:,2),L(:,3),'Color','g'); zAxis = C+R(:,3)*axisLength; L = [C zAxis]'; - line(L(:,1),L(:,2),L(:,3),'Color','b'); + h_z = line(L(:,1),L(:,2),L(:,3),'Color','b'); axis equal -end \ No newline at end of file +end diff --git a/matlab/+gtsam/plotCylinderSamples.m b/matlab/+gtsam/plotCylinderSamples.m new file mode 100644 index 000000000..ec1795dea --- /dev/null +++ b/matlab/+gtsam/plotCylinderSamples.m @@ -0,0 +1,35 @@ +function plotCylinderSamples(cylinders, options, figID) +% plot the cylinders on the given field +% @author: Zhaoyang Lv + + figure(figID); + + holdstate = ishold; + hold on + + num = size(cylinders, 1); + + sampleDensity = 120; + + for i = 1:num + [X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height); + + X = X + cylinders{i}.centroid.x; + Y = Y + cylinders{i}.centroid.y; + Z = Z * cylinders{i}.height; + + cylinderHandle = surf(X,Y,Z); + set(cylinderHandle, 'FaceAlpha', 0.5); + hold on + end + + axis equal + axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); + + grid on + + if ~holdstate + hold off + end + +end diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m new file mode 100644 index 000000000..5d4a287c4 --- /dev/null +++ b/matlab/+gtsam/plotFlyingResults.m @@ -0,0 +1,177 @@ +function plotFlyingResults(pts3d, poses, posesCov, cylinders, options) +% plot the visible points on the cylinders and trajectories +% +% author: Zhaoyang Lv + +import gtsam.* + +figID = 1; +figure(figID); +set(gcf, 'Position', [80,1,1800,1000]); + + +%% plot all the cylinders and sampled points + +axis equal +axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]); +xlabel('X (m)'); +ylabel('Y (m)'); +zlabel('Height (m)'); + +h = cameratoolbar('Show'); + +if options.camera.IS_MONO + h_title = title('Quadrotor Flight Simulation with Monocular Camera'); +else + h_title = title('Quadrotor Flight Simulation with Stereo Camera'); +end + +text(100,1750,0, sprintf('Flying Speed: %0.1f\n', options.speed)) + +view([30, 30]); + +hlight = camlight('headlight'); +lighting gouraud + +if(options.writeVideo) + videoObj = VideoWriter('Camera_Flying_Example.avi'); + videoObj.Quality = 100; + videoObj.FrameRate = options.camera.fps; + open(videoObj); +end + + +sampleDensity = 120; +cylinderNum = length(cylinders); +h_cylinder = cell(cylinderNum); +for i = 1:cylinderNum + + hold on + + [X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height); + + X = X + cylinders{i}.centroid.x; + Y = Y + cylinders{i}.centroid.y; + Z = Z * cylinders{i}.height; + + h_cylinder{i} = surf(X,Y,Z); + set(h_cylinder{i}, 'FaceColor', [0 0 1], 'FaceAlpha', 0.2); + h_cylinder{i}.AmbientStrength = 0.8; + +end + +%% plot trajectories and points +posesSize = length(poses); +pointSize = length(pts3d); +for i = 1:posesSize + if i > 1 + hold on + plot3([poses{i}.x; poses{i-1}.x], [poses{i}.y; poses{i-1}.y], [poses{i}.z; poses{i-1}.z], '-b'); + end + + if exist('h_pose_cov', 'var') + delete(h_pose_cov); + end + + %plotCamera(poses{i}, 3); + + gRp = poses{i}.rotation().matrix(); % rotation from pose to global + C = poses{i}.translation().vector(); + axisLength = 3; + + xAxis = C+gRp(:,1)*axisLength; + L = [C xAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','r'); + + yAxis = C+gRp(:,2)*axisLength; + L = [C yAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','g'); + + zAxis = C+gRp(:,3)*axisLength; + L = [C zAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','b'); + + pPp = posesCov{i}(4:6,4:6); % covariance matrix in pose coordinate frame + gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame + h_pose_cov = gtsam.covarianceEllipse3D(C, gPp, options.plot.covarianceScale); + + if exist('h_point', 'var') + for j = 1:pointSize + delete(h_point{j}); + end + end + if exist('h_point_cov', 'var') + for j = 1:pointSize + delete(h_point_cov{j}); + end + end + + h_point = cell(pointSize, 1); + h_point_cov = cell(pointSize, 1); + for j = 1:pointSize + if ~isempty(pts3d{j}.cov{i}) + hold on + h_point{j} = plot3(pts3d{j}.data.x, pts3d{j}.data.y, pts3d{j}.data.z); + h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data.x; pts3d{j}.data.y; pts3d{j}.data.z], ... + pts3d{j}.cov{i}, options.plot.covarianceScale); + end + end + + axis equal + axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]); + + drawnow + + if options.writeVideo + currFrame = getframe(gcf); + writeVideo(videoObj, currFrame); + end +end + + +if exist('h_pose_cov', 'var') + delete(h_pose_cov); +end + +% wait for two seconds +pause(2); + +%% change views angle +for i = 30 : i : 90 + view([30, i]); + + if options.writeVideo + currFrame = getframe(gcf); + writeVideo(videoObj, currFrame); + end + + drawnow +end + +% changing perspective + + +%% camera flying through video +camzoom(0.8); +for i = 1 : posesSize + + hold on + + campos([poses{i}.x, poses{i}.y, poses{i}.z]); + camtarget([options.fieldSize.x/2, options.fieldSize.y/2, 0]); + camlight(hlight, 'headlight'); + + if options.writeVideo + currFrame = getframe(gcf); + writeVideo(videoObj, currFrame); + end + + drawnow +end + +%%close video +if(options.writeVideo) + close(videoObj); +end + +end \ No newline at end of file diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m new file mode 100644 index 000000000..fc48f587e --- /dev/null +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -0,0 +1,113 @@ +function pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders) +% Assess how accurately we can reconstruct points from a particular monocular camera setup. +% After creation of the factor graph for each track, linearize it around ground truth. +% There is no optimization +% @author: Zhaoyang Lv + +import gtsam.* + +%% create graph +graph = NonlinearFactorGraph; + +%% create the noise factors +poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; +posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); +measurementNoiseSigma = 1.0; +measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma); + +cameraPosesNum = length(cameraPoses); + +%% add measurements and initial camera & points values +pointsNum = 0; +cylinderNum = length(cylinders); +points3d = cell(0); +for i = 1:cylinderNum + cylinderPointsNum = length(cylinders{i}.Points); + pointsNum = pointsNum + cylinderPointsNum; + for j = 1:cylinderPointsNum + points3d{end+1}.data = cylinders{i}.Points{j}; + points3d{end}.Z = cell(0); + points3d{end}.camConstraintIdx = cell(0); + points3d{end}.added = cell(0); + points3d{end}.visiblity = false; + points3d{end}.cov = cell(cameraPosesNum); + end +end + +graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise)); + +%% initialize graph and values +initialEstimate = Values; +for i = 1:pointsNum + point_j = points3d{i}.data.retract(0.1*randn(3,1)); + initialEstimate.insert(symbol('p', i), point_j); +end + +pts3d = cell(cameraPosesNum, 1); +cameraPosesCov = cell(cameraPosesNum, 1); +marginals = Values; +for i = 1:cameraPosesNum + cameraPose = cameraPoses{i}; + pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders); + + measurementNum = length(pts3d{i}.Z); + for j = 1:measurementNum + index = pts3d{i}.overallIdx{j}; + points3d{index}.Z{end+1} = pts3d{i}.Z{j}; + points3d{index}.camConstraintIdx{end+1} = i; + points3d{index}.added{end+1} = false; + + if length(points3d{index}.Z) < 2 + continue; + else + for k = 1:length(points3d{index}.Z) + if ~points3d{index}.added{k} + graph.add(GenericProjectionFactorCal3_S2(points3d{index}.Z{k}, ... + measurementNoise, symbol('x', points3d{index}.camConstraintIdx{k}), ... + symbol('p', index), K) ); + points3d{index}.added{k} = true; + end + end + end + + points3d{index}.visiblity = true; + end + + pose_i = cameraPoses{i}.retract(0.1*randn(6,1)); + initialEstimate.insert(symbol('x', i), pose_i); + + marginals = Marginals(graph, initialEstimate); + + for j = 1:pointsNum + if points3d{j}.visiblity + points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p',j)); + end + end + + cameraPosesCov{i} = marginals.marginalCovariance(symbol('x',i)); + +end + +%% Print the graph +graph.print(sprintf('\nFactor graph:\n')); + +%% Plot the result +plotFlyingResults(points3d, cameraPoses, cameraPosesCov, cylinders, options); + +%% get all the points track information +for i = 1:pointsNum + if ~points3d{i}.visiblity + continue; + end + + pts2dTracksMono.pt3d{end+1} = points3d{i}.data; + pts2dTracksMono.Z{end+1} = points3d{i}.Z; + + if length(points3d{i}.Z) == 1 + %pts2dTracksMono.cov{i} singular matrix + else + pts2dTracksMono.cov{end+1} = marginals.marginalCovariance(symbol('p', i)); + end +end + +end diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m new file mode 100644 index 000000000..60c9f1295 --- /dev/null +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -0,0 +1,101 @@ +function [pts2dTracksStereo, initialEstimate] = points2DTrackStereo(K, cameraPoses, options, cylinders) +% Assess how accurately we can reconstruct points from a particular monocular camera setup. +% After creation of the factor graph for each track, linearize it around ground truth. +% There is no optimization +% +% @author: Zhaoyang Lv + +import gtsam.* + +%% create graph +graph = NonlinearFactorGraph; + +%% create the noise factors +poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; +posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); +stereoNoise = noiseModel.Isotropic.Sigma(3, 0.05); + +cameraPosesNum = length(cameraPoses); + +%% add measurements and initial camera & points values +pointsNum = 0; +cylinderNum = length(cylinders); +points3d = cell(0); +for i = 1:cylinderNum + cylinderPointsNum = length(cylinders{i}.Points); + pointsNum = pointsNum + length(cylinders{i}.Points); + for j = 1:cylinderPointsNum + points3d{end+1}.data = cylinders{i}.Points{j}; + points3d{end}.Z = cell(0); + points3d{end}.cameraConstraint = cell(0); + points3d{end}.visiblity = false; + points3d{end}.cov = cell(cameraPosesNum); + end +end + +graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise)); + +%% initialize graph and values +initialEstimate = Values; +for i = 1:pointsNum + point_j = points3d{i}.data.retract(0.05*randn(3,1)); + initialEstimate.insert(symbol('p', i), point_j); +end + +pts3d = cell(cameraPosesNum, 1); +cameraPosesCov = cell(cameraPosesNum, 1); +for i = 1:cameraPosesNum + pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, options.camera.resolution, cylinders); + + if isempty(pts3d{i}.Z) + continue; + end + + measurementNum = length(pts3d{i}.Z); + for j = 1:measurementNum + index = pts3d{i}.overallIdx{j}; + points3d{index}.Z{end+1} = pts3d{i}.Z{j}; + points3d{index}.cameraConstraint{end+1} = i; + points3d{index}.visiblity = true; + + graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ... + stereoNoise, symbol('x', i), symbol('p', index), K)); + end + + pose_i = cameraPoses{i}.retract(poseNoiseSigmas); + initialEstimate.insert(symbol('x', i), pose_i); + + %% linearize the graph + marginals = Marginals(graph, initialEstimate); + + for j = 1:pointsNum + if points3d{j}.visiblity + points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p', j)); + end + end + + cameraPosesCov{i} = marginals.marginalCovariance(symbol('x', i)); +end + +%% Plot the result +plotFlyingResults(points3d, cameraPoses, cameraPosesCov, cylinders, options); + +%% get all the 2d points track information +pts2dTracksStereo.pt3d = cell(0); +pts2dTracksStereo.Z = cell(0); +pts2dTracksStereo.cov = cell(0); +for i = 1:pointsNum + if ~points3d{i}.visiblity + continue; + end + + pts2dTracksStereo.pt3d{end+1} = points3d{i}.data; + pts2dTracksStereo.Z{end+1} = points3d{i}.Z; + pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p', i)); +end + +% +% %% plot the result with covariance ellipses +% plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, cameraPoses, cameraPosesCov, cylinders, options); + +end diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index c7b7d6441..9abd4e31d 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -25,7 +25,7 @@ set(matlab_examples_data ${matlab_examples_data_graph} ${matlab_examples_data_ma if(GTSAM_BUILD_TYPE_POSTFIXES) foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) string(TOUPPER "${build_type}" build_type_upper) - if("${build_type_upper}" STREQUAL "RELEASE") + if(${build_type_upper} STREQUAL "RELEASE") set(build_type_tag "") # Don't create release mode tag on installed directory else() set(build_type_tag "${build_type}") diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m new file mode 100644 index 000000000..36b7635e2 --- /dev/null +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -0,0 +1,179 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 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 A camera flying example through a field of cylinder landmarks +% @author Zhaoyang Lv +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + +clear all; +clc; +clf; + +import gtsam.* + +% test or run +options.enableTests = false; + +%% cylinder options +% the number of cylinders in the field +options.cylinder.cylinderNum = 15; % pls be smaller than 20 +% cylinder size +options.cylinder.radius = 3; % pls be smaller than 5 +options.cylinder.height = 10; +% point density on cylinder +options.cylinder.pointDensity = 0.1; + +%% camera options +% parameters set according to the stereo camera: +% http://www.matrix-vision.com/USB2.0-single-board-camera-mvbluefox-mlc.html + +% set up monocular camera or stereo camera +options.camera.IS_MONO = false; +% the field of view of camera +options.camera.fov = 120; +% fps for image +options.camera.fps = 25; +% camera pixel resolution +options.camera.resolution = Point2(752, 480); +% camera horizon +options.camera.horizon = 60; +% camera baseline +options.camera.baseline = 0.05; +% camera focal length +options.camera.f = round(options.camera.resolution.x * options.camera.horizon / ... + options.camera.fov); +% camera focal baseline +options.camera.fB = options.camera.f * options.camera.baseline; +% camera disparity +options.camera.disparity = options.camera.fB / options.camera.horizon; +% Monocular Camera Calibration +options.camera.monoK = Cal3_S2(options.camera.f, options.camera.f, 0, ... + options.camera.resolution.x/2, options.camera.resolution.y/2); +% Stereo Camera Calibration +options.camera.stereoK = Cal3_S2Stereo(options.camera.f, options.camera.f, 0, ... + options.camera.resolution.x/2, options.camera.resolution.y/2, options.camera.disparity); + +% write video output +options.writeVideo = true; +% the testing field size (unit: meter) +options.fieldSize = Point2([100, 100]'); +% camera flying speed (unit: meter / second) +options.speed = 20; +% camera flying height +options.height = 30; + +%% ploting options +% display covariance scaling factor, default to be 1. +% The covariance visualization default models 99% of all probablity +options.plot.covarianceScale = 1; +% plot the trajectory covariance +options.plot.DISP_TRAJ_COV = true; +% plot points covariance +options.plot.POINTS_COV = true; + +%% This is for tests +if options.enableTests + % test1: visibility test in monocular camera + cylinders{1}.centroid = Point3(30, 50, 5); + cylinders{2}.centroid = Point3(50, 50, 5); + cylinders{3}.centroid = Point3(70, 50, 5); + + for i = 1:3 + cylinders{i}.radius = 5; + cylinders{i}.height = 10; + + cylinders{i}.Points{1} = cylinders{i}.centroid.compose(Point3(-cylinders{i}.radius, 0, 0)); + cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0)); + end + + camera = SimpleCamera.Lookat(Point3(10, 50, 10), ... + Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... + Point3([0,0,1]'), options.monoK); + + pose = camera.pose; + prjMonoResult = cylinderSampleProjection(options.camera.monoK, pose, ... + options.camera.resolution, cylinders); + + % test2: visibility test in stereo camera + prjStereoResult = cylinderSampleProjectionStereo(options.camera.stereoK, ... + pose, options.camera.resolution, cylinders); +end + +%% generate a set of cylinders and point samples on cylinders +cylinderNum = options.cylinder.cylinderNum; +cylinders = cell(cylinderNum, 1); +baseCentroid = cell(cylinderNum, 1); +theta = 0; +i = 1; +while i <= cylinderNum + theta = theta + 2*pi/10; + x = 40 * rand * cos(theta) + options.fieldSize.x/2; + y = 20 * rand * sin(theta) + options.fieldSize.y/2; + baseCentroid{i} = Point2([x, y]'); + + % prevent two cylinders interact with each other + regenerate = false; + for j = 1:i-1 + if i > 1 && baseCentroid{i}.dist(baseCentroid{j}) < options.cylinder.radius * 2 + regenerate = true; + break; + end + end + if regenerate + continue; + end + + cylinders{i,1} = cylinderSampling(baseCentroid{i}, options.cylinder.radius, ... + options.cylinder.height, options.cylinder.pointDensity); + i = i+1; +end + +%% generate ground truth camera trajectories: a line +KMono = Cal3_S2(525,525,0,320,240); +cameraPoses = cell(0); +theta = 0; +t = Point3(5, 5, options.height); +i = 0; +while 1 + i = i+1; + distance = options.speed / options.camera.fps; + angle = 0.1*pi*(rand-0.5); + inc_t = Point3(distance * cos(angle), ... + distance * sin(angle), 0); + t = t.compose(inc_t); + + if t.x > options.fieldSize.x - 10 || t.y > options.fieldSize.y - 10; + break; + end + + %t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ... + % 15, 10]'); + camera = SimpleCamera.Lookat(t, ... + Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... + Point3([0,0,1]'), options.camera.monoK); + cameraPoses{end+1} = camera.pose; +end + +%% set up camera and get measurements +if options.camera.IS_MONO + % use Monocular Camera + pts2dTracksMono = points2DTrackMonocular(options.camera.monoK, cameraPoses, ... + options.camera.resolution, cylinders); +else + % use Stereo Camera + pts2dTracksStereo = points2DTrackStereo(options.camera.stereoK, ... + cameraPoses, options, cylinders); +end + + + + + + + diff --git a/matlab/gtsam_examples/IMUKittiExampleGPS.m b/matlab/gtsam_examples/IMUKittiExampleGPS.m index e205d918c..32f61e47f 100644 --- a/matlab/gtsam_examples/IMUKittiExampleGPS.m +++ b/matlab/gtsam_examples/IMUKittiExampleGPS.m @@ -33,7 +33,7 @@ GPSskip = 10; % Skip this many GPS measurements each time %% Get initial conditions for the estimated trajectory currentPoseGlobal = Pose3(Rot3, GPS_data(firstGPSPose).Position); % initial pose is the reference frame (navigation frame) -currentVelocityGlobal = LieVector([0;0;0]); % the vehicle is stationary at the beginning +currentVelocityGlobal = [0;0;0]; % the vehicle is stationary at the beginning currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); sigma_init_x = noiseModel.Isotropic.Precisions([ 0.0; 0.0; 0.0; 1; 1; 1 ]); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.0); @@ -72,7 +72,7 @@ for measurementIndex = firstGPSPose:length(GPS_data) newValues.insert(currentVelKey, currentVelocityGlobal); newValues.insert(currentBiasKey, currentBias); newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseGlobal, sigma_init_x)); - newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); + newFactors.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); newFactors.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b)); else t_previous = GPS_data(measurementIndex-1, 1).Time; diff --git a/matlab/gtsam_examples/MonocularVOExample.m b/matlab/gtsam_examples/MonocularVOExample.m index 9fd2df463..11c4253de 100644 --- a/matlab/gtsam_examples/MonocularVOExample.m +++ b/matlab/gtsam_examples/MonocularVOExample.m @@ -40,7 +40,7 @@ end %% Create initial estimate initial = Values; -trueE = EssentialMatrix(aRb,Sphere2(aTb)); +trueE = EssentialMatrix(aRb,Unit3(aTb)); initialE = trueE.retract([0.1, -0.1, 0.1, 0.1, -0.1]'); initial.insert(1, initialE); @@ -52,5 +52,5 @@ result = optimizer.optimize(); %% Print result (as essentialMatrix) and error error = graph.error(result) -E = result.at(1) +E = result.atEssentialMatrix(1) diff --git a/matlab/gtsam_tests/.gitignore b/matlab/gtsam_tests/.gitignore new file mode 100644 index 000000000..6d725d9bc --- /dev/null +++ b/matlab/gtsam_tests/.gitignore @@ -0,0 +1 @@ +*.m~ diff --git a/matlab/gtsam_tests/testCal3Unified.m b/matlab/gtsam_tests/testCal3Unified.m new file mode 100644 index 000000000..498c65343 --- /dev/null +++ b/matlab/gtsam_tests/testCal3Unified.m @@ -0,0 +1,7 @@ +% test Cal3Unified +import gtsam.*; + +K = Cal3Unified; +EXPECT('fx',K.fx()==1); +EXPECT('fy',K.fy()==1); + diff --git a/matlab/gtsam_tests/testPriorFactor.m b/matlab/gtsam_tests/testPriorFactor.m new file mode 100644 index 000000000..4d567a6ce --- /dev/null +++ b/matlab/gtsam_tests/testPriorFactor.m @@ -0,0 +1,18 @@ +% test wrapping of Values +import gtsam.*; + +values = Values; + +key = 5; +priorPose3 = Pose3; +model = noiseModel.Unit.Create(6); +factor = PriorFactorPose3(key, priorPose3, model); +values.insert(key, priorPose3); +EXPECT('error', factor.error(values) == 0); + +key = 3; +priorVector = [0,0,0]'; +model = noiseModel.Unit.Create(3); +factor = PriorFactorVector(key, priorVector, model); +values.insert(key, priorVector); +EXPECT('error', factor.error(values) == 0); diff --git a/matlab/gtsam_tests/testTriangulation.m b/matlab/gtsam_tests/testTriangulation.m new file mode 100644 index 000000000..d46493328 --- /dev/null +++ b/matlab/gtsam_tests/testTriangulation.m @@ -0,0 +1,70 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 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 Test triangulation +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +import gtsam.* + +%% Some common constants +sharedCal = Cal3_S2(1500, 1200, 0, 640, 480); + +%% Looking along X-axis, 1 meter above ground plane (x-y) +upright = Rot3.Ypr(-pi / 2, 0., -pi / 2); +pose1 = Pose3(upright, Point3(0, 0, 1)); +camera1 = SimpleCamera(pose1, sharedCal); + +%% create second camera 1 meter to the right of first camera +pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))); +camera2 = SimpleCamera(pose2, sharedCal); + +%% landmark ~5 meters infront of camera +landmark =Point3 (5, 0.5, 1.2); + +%% 1. Project two landmarks into two cameras and triangulate +z1 = camera1.project(landmark); +z2 = camera2.project(landmark); + +%% twoPoses +poses = Pose3Vector; +measurements = Point2Vector; + +poses.push_back(pose1); +poses.push_back(pose2); +measurements.push_back(z1); +measurements.push_back(z2); + +optimize = true; +rank_tol = 1e-9; + +triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize); +CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9)); + +%% 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) +measurements = Point2Vector; +measurements.push_back(z1.retract([0.1;0.5])); +measurements.push_back(z2.retract([-0.2;0.3])); + +triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize); +CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-2)); + +%% two Poses with Bundler Calibration +bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480); +camera1 = PinholeCameraCal3Bundler(pose1, bundlerCal); +camera2 = PinholeCameraCal3Bundler(pose2, bundlerCal); + +z1 = camera1.project(landmark); +z2 = camera2.project(landmark); + +measurements = Point2Vector; +measurements.push_back(z1); +measurements.push_back(z2); + +triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize); +CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9)); diff --git a/matlab/gtsam_tests/testValues.m b/matlab/gtsam_tests/testValues.m new file mode 100644 index 000000000..fe2cd30fe --- /dev/null +++ b/matlab/gtsam_tests/testValues.m @@ -0,0 +1,40 @@ +% test wrapping of Values +import gtsam.*; + +values = Values; +E = EssentialMatrix(Rot3,Unit3); +tol = 1e-9; + +values.insert(0, Point2); +values.insert(1, Point3); +values.insert(2, Rot2); +values.insert(3, Pose2); +values.insert(4, Rot3); +values.insert(5, Pose3); +values.insert(6, Cal3_S2); +values.insert(7, Cal3DS2); +values.insert(8, Cal3Bundler); +values.insert(9, E); +values.insert(10, imuBias.ConstantBias); + +% special cases for Vector and Matrix: +values.insert(11, [1;2;3]); +values.insert(12, [1 2;3 4]); + +EXPECT('at',values.atPoint2(0).equals(Point2,tol)); +EXPECT('at',values.atPoint3(1).equals(Point3,tol)); +EXPECT('at',values.atRot2(2).equals(Rot2,tol)); +EXPECT('at',values.atPose2(3).equals(Pose2,tol)); +EXPECT('at',values.atRot3(4).equals(Rot3,tol)); +EXPECT('at',values.atPose3(5).equals(Pose3,tol)); +EXPECT('at',values.atCal3_S2(6).equals(Cal3_S2,tol)); +EXPECT('at',values.atCal3DS2(7).equals(Cal3DS2,tol)); +EXPECT('at',values.atCal3Bundler(8).equals(Cal3Bundler,tol)); +EXPECT('at',values.atEssentialMatrix(9).equals(E,tol)); +EXPECT('at',values.atConstantBias(10).equals(imuBias.ConstantBias,tol)); + +% special cases for Vector and Matrix: +actualVector = values.atVector(11); +EQUALITY('at',[1;2;3],actualVector,tol); +actualMatrix = values.atMatrix(12); +EQUALITY('at',[1 2;3 4],actualMatrix,tol); diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index e3705948d..e43fbcf76 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -1,11 +1,25 @@ % Test runner script - runs each test +%% geometry +display 'Starting: testCal3Unified' +testCal3Unified + +%% linear +display 'Starting: testKalmanFilter' +testKalmanFilter + display 'Starting: testJacobianFactor' testJacobianFactor -display 'Starting: testKalmanFilter' -testKalmanFilter +%% nonlinear +display 'Starting: testValues' +testValues +%% SLAM +display 'Starting: testPriorFactor' +testPriorFactor + +%% examples display 'Starting: testLocalizationExample' testLocalizationExample @@ -30,11 +44,14 @@ testStereoVOExample display 'Starting: testVisualISAMExample' testVisualISAMExample +%% MATLAB specific display 'Starting: testUtilities' testUtilities -display 'Starting: testSerialization' -testSerialization +if(exist('testSerialization.m','file')) + display 'Starting: testSerialization' + testSerialization +end % end of tests display 'Tests complete!' diff --git a/matlab/unstable_examples/.gitignore b/matlab/unstable_examples/.gitignore index 6d725d9bc..c47168f67 100644 --- a/matlab/unstable_examples/.gitignore +++ b/matlab/unstable_examples/.gitignore @@ -1 +1,2 @@ *.m~ +*.avi diff --git a/matlab/unstable_examples/FlightCameraTransformIMU.m b/matlab/unstable_examples/FlightCameraTransformIMU.m index 1b05d2877..b0b37ee33 100644 --- a/matlab/unstable_examples/FlightCameraTransformIMU.m +++ b/matlab/unstable_examples/FlightCameraTransformIMU.m @@ -57,7 +57,7 @@ isamParams.setFactorization('QR'); isam = ISAM2(isamParams); %% Get initial conditions for the estimated trajectory -currentVelocityGlobal = LieVector([10;0;0]); % (This is slightly wrong!) +currentVelocityGlobal = [10;0;0]; % (This is slightly wrong!) Zhaoyang: Fixed currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0); @@ -101,7 +101,7 @@ axis equal; for i=1:size(trajectory)-1 xKey = symbol('x',i); - pose = trajectory.at(xKey); % GT pose + pose = trajectory.atPose3(xKey); % GT pose pose_t = pose.translation(); % GT pose-translation if exist('h_cursor','var') @@ -165,13 +165,13 @@ for i=1:size(trajectory)-1 if i > 1 xKey_prev = symbol('x',i-1); - pose_prev = trajectory.at(xKey_prev); + pose_prev = trajectory.atPose3(xKey_prev); step = pose_prev.between(pose); % insert estimate for current pose with some normal noise on % translation - initial.insert(xKey,result.at(xKey_prev).compose(step.retract([0; 0; 0; normrnd(0,0.2,3,1)]))); + initial.insert(xKey,result.atPose3(xKey_prev).compose(step.retract([0; 0; 0; normrnd(0,0.2,3,1)]))); % visual measurements if measurements.size > 0 && use_camera @@ -181,12 +181,12 @@ for i=1:size(trajectory)-1 zKey = measurementKeys.at(zz); lKey = symbol('l',symbolIndex(zKey)); - fg.add(TransformCalProjectionFactorCal3_S2(measurements.at(zKey), ... + fg.add(TransformCalProjectionFactorCal3_S2(measurements.atPoint2(zKey), ... z_cov, xKey, transformKey, lKey, calibrationKey, false, true)); % only add landmark to values if doesn't exist yet if ~result.exists(lKey) - noisy_landmark = landmarks.at(lKey).compose(Point3(normrnd(0,landmark_noise,3,1))); + noisy_landmark = landmarks.atPoint3(lKey).compose(Point3(normrnd(0,landmark_noise,3,1))); initial.insert(lKey, noisy_landmark); % and add a prior since its position is known diff --git a/matlab/unstable_examples/IMUKittiExampleAdvanced.m b/matlab/unstable_examples/IMUKittiExampleAdvanced.m index 1db60a5ad..cb13eacee 100644 --- a/matlab/unstable_examples/IMUKittiExampleAdvanced.m +++ b/matlab/unstable_examples/IMUKittiExampleAdvanced.m @@ -7,7 +7,7 @@ disp('Example of application of ISAM2 for visual-inertial navigation on the KITT %% Read metadata and compute relative sensor pose transforms % IMU metadata disp('-- Reading sensor metadata') -IMU_metadata = importdata('KittiEquivBiasedImu_metadata.txt'); +IMU_metadata = importdata(findExampleDataFile('KittiEquivBiasedImu_metadata.txt')); IMU_metadata = cell2struct(num2cell(IMU_metadata.data), IMU_metadata.colheaders, 2); IMUinBody = Pose3.Expmap([IMU_metadata.BodyPtx; IMU_metadata.BodyPty; IMU_metadata.BodyPtz; IMU_metadata.BodyPrx; IMU_metadata.BodyPry; IMU_metadata.BodyPrz; ]); @@ -16,14 +16,14 @@ if ~IMUinBody.equals(Pose3, 1e-5) end % VO metadata -VO_metadata = importdata('KittiRelativePose_metadata.txt'); +VO_metadata = importdata(findExampleDataFile('KittiRelativePose_metadata.txt')); VO_metadata = cell2struct(num2cell(VO_metadata.data), VO_metadata.colheaders, 2); VOinBody = Pose3.Expmap([VO_metadata.BodyPtx; VO_metadata.BodyPty; VO_metadata.BodyPtz; VO_metadata.BodyPrx; VO_metadata.BodyPry; VO_metadata.BodyPrz; ]); VOinIMU = IMUinBody.inverse().compose(VOinBody); % GPS metadata -GPS_metadata = importdata('KittiGps_metadata.txt'); +GPS_metadata = importdata(findExampleDataFile('KittiGps_metadata.txt')); GPS_metadata = cell2struct(num2cell(GPS_metadata.data), GPS_metadata.colheaders, 2); GPSinBody = Pose3.Expmap([GPS_metadata.BodyPtx; GPS_metadata.BodyPty; GPS_metadata.BodyPtz; GPS_metadata.BodyPrx; GPS_metadata.BodyPry; GPS_metadata.BodyPrz; ]); @@ -32,7 +32,7 @@ GPSinIMU = IMUinBody.inverse().compose(GPSinBody); %% Read data and change coordinate frame of GPS and VO measurements to IMU frame disp('-- Reading sensor data from file') % IMU data -IMU_data = importdata('KittiEquivBiasedImu.txt'); +IMU_data = importdata(findExampleDataFile('KittiEquivBiasedImu.txt')); IMU_data = cell2struct(num2cell(IMU_data.data), IMU_data.colheaders, 2); imum = cellfun(@(x) x', num2cell([ [IMU_data.accelX]' [IMU_data.accelY]' [IMU_data.accelZ]' [IMU_data.omegaX]' [IMU_data.omegaY]' [IMU_data.omegaZ]' ], 2), 'UniformOutput', false); [IMU_data.acc_omega] = deal(imum{:}); @@ -40,7 +40,7 @@ imum = cellfun(@(x) x', num2cell([ [IMU_data.accelX]' [IMU_data.accelY]' [IMU_da clear imum % VO data -VO_data = importdata('KittiRelativePose.txt'); +VO_data = importdata(findExampleDataFile('KittiRelativePose.txt')); VO_data = cell2struct(num2cell(VO_data.data), VO_data.colheaders, 2); % Merge relative pose fields and convert to Pose3 logposes = [ [VO_data.dtx]' [VO_data.dty]' [VO_data.dtz]' [VO_data.drx]' [VO_data.dry]' [VO_data.drz]' ]; @@ -71,7 +71,7 @@ clear logposes relposes %% Get initial conditions for the estimated trajectory % % % currentPoseGlobal = Pose3(Rot3, GPS_data(firstGPSPose).Position); % initial pose is the reference frame (navigation frame) currentPoseGlobal = Pose3; -currentVelocityGlobal = LieVector([0;0;0]); % the vehicle is stationary at the beginning +currentVelocityGlobal = [0;0;0]; % the vehicle is stationary at the beginning currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); sigma_init_x = noiseModel.Isotropic.Sigma(6, 0.01); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.0); @@ -120,7 +120,7 @@ for measurementIndex = 1:length(timestamps) newValues.insert(currentVelKey, currentVelocityGlobal); newValues.insert(currentBiasKey, currentBias); newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseGlobal, sigma_init_x)); - newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); + newFactors.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); newFactors.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b)); else t_previous = timestamps(measurementIndex-1, 1); diff --git a/matlab/unstable_examples/SmartRangeFactorExample.m b/matlab/unstable_examples/SmartRangeFactorExample.m index 93e5dce0c..7535447df 100644 --- a/matlab/unstable_examples/SmartRangeFactorExample.m +++ b/matlab/unstable_examples/SmartRangeFactorExample.m @@ -82,8 +82,8 @@ for ind_pose = 2:7 key_prev = poseKey(ind_pose-1); key_curr = poseKey(ind_pose); - prev_pose = smartValues.at(key_prev); - curr_pose = smartValues.at(key_curr); + prev_pose = smartValues.atPose2(key_prev); + curr_pose = smartValues.atPose2(key_curr); GTDeltaPose = prev_pose.between(curr_pose); noisePose = Pose2([t_noise*rand; t_noise*rand; r_noise*rand]); NoisyDeltaPose = GTDeltaPose.compose(noisePose); diff --git a/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m index 236327b05..c9e912ea4 100644 --- a/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m +++ b/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m @@ -96,17 +96,17 @@ for i=1:20 if i > 1 if i < 11 - initial.insert(i,result.at(i-1).compose(move_forward)); + initial.insert(i,result.atPose3(i-1).compose(move_forward)); fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance)); else - initial.insert(i,result.at(i-1).compose(move_circle)); + initial.insert(i,result.atPose3(i-1).compose(move_circle)); fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance)); end end % generate some camera measurements - cam_pose = initial.at(i).compose(actual_transform); + cam_pose = initial.atPose3(i).compose(actual_transform); % gtsam.plotPose3(cam_pose); cam = SimpleCamera(cam_pose,K); i @@ -137,10 +137,10 @@ for i=1:20 hold on; %% plot results - result_camera_transform = result.at(transformKey); + result_camera_transform = result.atPose3(transformKey); for j=1:i - gtsam.plotPose3(result.at(j),[],0.5); - gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5); + gtsam.plotPose3(result.atPose3(j),[],0.5); + gtsam.plotPose3(result.atPose3(j).compose(result_camera_transform),[],0.5); end xlabel('x (m)'); @@ -153,12 +153,12 @@ for i=1:20 % axis equal for l=101:100+nrPoints - plotPoint3(result.at(l),'g'); + plotPoint3(result.atPoint3(l),'g'); end - ty = result.at(transformKey).translation().y(); - fx = result.at(calibrationKey).fx(); - fy = result.at(calibrationKey).fy(); + ty = result.atPose3(transformKey).translation().y(); + fx = result.atCal3_S2(calibrationKey).fx(); + fy = result.atCal3_S2(calibrationKey).fy(); text(1,5,5,sprintf('Y-Transform(0.0): %0.2f',ty)); text(1,5,3,sprintf('fx(900): %.0f',fx)); text(1,5,1,sprintf('fy(900): %.0f',fy)); @@ -180,10 +180,10 @@ end fprintf('Cheirality Exception count: %d\n', cheirality_exception_count); disp('Transform after optimization'); -result.at(transformKey) +result.atPose3(transformKey) disp('Calibration after optimization'); -result.at(calibrationKey) +result.atCal3_S2(calibrationKey) diff --git a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m index 834472a7d..fd4a17469 100644 --- a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m +++ b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m @@ -89,7 +89,7 @@ isam = ISAM2(isamParams); currentIMUPoseGlobal = Pose3(); %% Get initial conditions for the estimated trajectory -currentVelocityGlobal = LieVector([1;0;0]); % the vehicle is stationary at the beginning +currentVelocityGlobal = [1;0;0]; % the vehicle is stationary at the beginning currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0); @@ -127,7 +127,7 @@ for i=1:steps fg.add(PriorFactorCal3_S2(calibrationKey,K_corrupt,K_cov)); % velocity and bias evolution - fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); + fg.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b)); result = initial; diff --git a/matlab/unstable_examples/TransformProjectionFactorExample.m b/matlab/unstable_examples/TransformProjectionFactorExample.m index 410b7df0f..669073e56 100644 --- a/matlab/unstable_examples/TransformProjectionFactorExample.m +++ b/matlab/unstable_examples/TransformProjectionFactorExample.m @@ -61,17 +61,17 @@ cheirality_exception_count = 0; for i=1:20 if i > 1 if i < 11 - initial.insert(i,initial.at(i-1).compose(move_forward)); + initial.insert(i,initial.atPose3(i-1).compose(move_forward)); fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance)); else - initial.insert(i,initial.at(i-1).compose(move_circle)); + initial.insert(i,initial.atPose3(i-1).compose(move_circle)); fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance)); end end % generate some camera measurements - cam_pose = initial.at(i).compose(actual_transform); + cam_pose = initial.atPose3(i).compose(actual_transform); gtsam.plotPose3(cam_pose); cam = SimpleCamera(cam_pose,K); i @@ -93,14 +93,14 @@ fprintf('Cheirality Exception count: %d\n', cheirality_exception_count); %% camera plotting for i=1:20 - gtsam.plotPose3(initial.at(i).compose(camera_transform)); + gtsam.plotPose3(initial.atPose3(i).compose(camera_transform)); end xlabel('x (m)'); ylabel('y (m)'); disp('Transform before optimization'); -initial.at(1000) +initial.atPose3(1000) params = LevenbergMarquardtParams; params.setAbsoluteErrorTol(1e-15); @@ -112,7 +112,7 @@ optimizer = LevenbergMarquardtOptimizer(fg, initial, params); result = optimizer.optimizeSafely(); disp('Transform after optimization'); -result.at(1000) +result.atPose3(1000) % results is empty here. optimizer doesn't generate result? axis([0 25 0 25 0 10]); axis equal diff --git a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m index 169736f4b..8edcfade7 100644 --- a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m +++ b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m @@ -86,17 +86,17 @@ for i=1:20 if i > 1 if i < 11 - initial.insert(i,result.at(i-1).compose(move_forward)); + initial.insert(i,result.atPose3(i-1).compose(move_forward)); fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance)); else - initial.insert(i,result.at(i-1).compose(move_circle)); + initial.insert(i,result.atPose3(i-1).compose(move_circle)); fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance)); end end % generate some camera measurements - cam_pose = initial.at(i).compose(actual_transform); + cam_pose = initial.atPose3(i).compose(actual_transform); % gtsam.plotPose3(cam_pose); cam = SimpleCamera(cam_pose,K); i @@ -127,10 +127,10 @@ for i=1:20 hold on; %% plot results - result_camera_transform = result.at(1000); + result_camera_transform = result.atPose3(1000); for j=1:i - gtsam.plotPose3(result.at(j)); - gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5); + gtsam.plotPose3(result.atPose3(j)); + gtsam.plotPose3(result.atPose3(j).compose(result_camera_transform),[],0.5); end xlabel('x (m)'); @@ -143,10 +143,10 @@ for i=1:20 % axis equal for l=101:100+nrPoints - plotPoint3(result.at(l),'g'); + plotPoint3(result.atPoint3(l),'g'); end - ty = result.at(1000).translation().y(); + ty = result.atPose3(1000).translation().y(); text(5,5,5,sprintf('Y-Transform: %0.2g',ty)); if(write_video) @@ -168,7 +168,7 @@ fprintf('Cheirality Exception count: %d\n', cheirality_exception_count); disp('Transform after optimization'); -result.at(1000) +result.atPose3(1000) diff --git a/matlab/unstable_examples/plot_projected_landmarks.m b/matlab/unstable_examples/plot_projected_landmarks.m index cfe08ec54..6b8101844 100644 --- a/matlab/unstable_examples/plot_projected_landmarks.m +++ b/matlab/unstable_examples/plot_projected_landmarks.m @@ -23,7 +23,7 @@ z = zeros(1,nrMeasurements); for i = 0:measurement_keys.size-1 key = measurement_keys.at(i); key_index = gtsam.symbolIndex(key); - p = landmarks.at(gtsam.symbol('l',key_index)); + p = landmarks.atPoint3(gtsam.symbol('l',key_index)); x(i+1) = p.x; y(i+1) = p.y; diff --git a/matlab/unstable_examples/project_landmarks.m b/matlab/unstable_examples/project_landmarks.m index b11c43bc5..629c6d994 100644 --- a/matlab/unstable_examples/project_landmarks.m +++ b/matlab/unstable_examples/project_landmarks.m @@ -8,7 +8,7 @@ function [ measurements ] = project_landmarks( pose, landmarks, K ) measurements = Values; for i=1:size(landmarks)-1 - z = camera.project(landmarks.at(symbol('l',i))); + z = camera.project(landmarks.atPoint3(symbol('l',i))); % check bounding box if z.x < 0 || z.x > 1280 diff --git a/matlab/unstable_examples/testTSAMFactors.m b/matlab/unstable_examples/testTSAMFactors.m index abdfc5922..341725723 100644 --- a/matlab/unstable_examples/testTSAMFactors.m +++ b/matlab/unstable_examples/testTSAMFactors.m @@ -48,12 +48,12 @@ optimizer = LevenbergMarquardtOptimizer(graph, initial, params); result = optimizer.optimize(); % Check result -CHECK('error',result.at(100).equals(b1,1e-5)) -CHECK('error',result.at(10).equals(origin,1e-5)) -CHECK('error',result.at(1).equals(Point2(0,1),1e-5)) -CHECK('error',result.at(2).equals(Point2(0,1),1e-5)) -CHECK('error',result.at(20).equals(origin,1e-5)) -CHECK('error',result.at(200).equals(b2,1e-5)) +CHECK('error',result.atPose2(100).equals(b1,1e-5)) +CHECK('error',result.atPose2(10).equals(origin,1e-5)) +CHECK('error',result.atPoint2(1).equals(Point2(0,1),1e-5)) +CHECK('error',result.atPoint2(2).equals(Point2(0,1),1e-5)) +CHECK('error',result.atPose2(20).equals(origin,1e-5)) +CHECK('error',result.atPose2(200).equals(b2,1e-5)) % Check error CHECK('error',abs(graph.error(result))<1e-9) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 008dbb78d..068b39eca 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -2,7 +2,7 @@ # note the source dir on each set (tests_exclude "") -if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") # might not be best test - Richard & Jason & Frank +if (${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") # might not be best test - Richard & Jason & Frank # clang linker segfaults on large testSerializationSLAM list (APPEND tests_exclude "testSerializationSLAM.cpp") endif() diff --git a/tests/simulated2D.h b/tests/simulated2D.h index b67ef0aef..a1080a6de 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -18,9 +18,9 @@ // \callgraph #pragma once -#include #include #include +#include // \namespace @@ -158,7 +158,7 @@ namespace simulated2D { /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); } @@ -204,7 +204,7 @@ namespace simulated2D { /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); } @@ -251,7 +251,7 @@ namespace simulated2D { /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); } @@ -270,3 +270,16 @@ namespace simulated2D { }; } // namespace simulated2D + +/// traits +namespace gtsam { +template +struct traits > : Testable< + simulated2D::GenericMeasurement > { +}; + +template<> +struct traits : public Testable { +}; +} + diff --git a/tests/simulated2DConstraints.h b/tests/simulated2DConstraints.h index 6ddf9ebdb..f593ab23a 100644 --- a/tests/simulated2DConstraints.h +++ b/tests/simulated2DConstraints.h @@ -90,11 +90,11 @@ namespace simulated2D { virtual double value(const Point& x, boost::optional H = boost::none) const { if (H) { - Matrix D = zeros(1, x.dim()); + Matrix D = zeros(1, traits::GetDimension(x)); D(0, IDX) = 1.0; *H = D; } - return Point::Logmap(x)(IDX); + return traits::Logmap(x)(IDX); } }; diff --git a/tests/smallExample.h b/tests/smallExample.h index b7cb7aefe..9866d22aa 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -349,7 +349,7 @@ inline boost::shared_ptr sharedReallyNonlinearFactor using symbol_shorthand::X; using symbol_shorthand::L; boost::shared_ptr fg(new NonlinearFactorGraph); - Vector z = Vector2(1.0, 0.0); + Point2 z(1.0, 0.0); double sigma = 0.1; boost::shared_ptr factor( new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 8748e7464..a7bcf7153 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -92,10 +92,7 @@ TEST( ExtendedKalmanFilter, linear ) { // Create Motion Model Factor class NonlinearMotionModel : public NoiseModelFactor2 { -public: - typedef Point2 T; -private: typedef NoiseModelFactor2 Base; typedef NonlinearMotionModel This; @@ -126,19 +123,19 @@ public: virtual ~NonlinearMotionModel() {} // Calculate the next state prediction using the nonlinear function f() - T f(const T& x_t0) const { + Point2 f(const Point2& x_t0) const { // Calculate f(x) double x = x_t0.x() * x_t0.x(); double y = x_t0.x() * x_t0.y(); - T x_t1(x,y); + Point2 x_t1(x,y); // In this example, the noiseModel remains constant return x_t1; } // Calculate the Jacobian of the nonlinear function f() - Matrix F(const T& x_t0) const { + Matrix F(const Point2& x_t0) const { // Calculate Jacobian of f() Matrix F = Matrix(2,2); F(0,0) = 2*x_t0.x(); @@ -150,7 +147,7 @@ public: } // Calculate the inverse square root of the motion model covariance, Q - Matrix QInvSqrt(const T& x_t0) const { + Matrix QInvSqrt(const Point2& x_t0) const { // This motion model has a constant covariance return Q_invsqrt_; } @@ -184,7 +181,7 @@ public: /** Vector of errors, whitened ! */ Vector whitenedError(const Values& c) const { - return QInvSqrt(c.at(key1()))*unwhitenedError(c); + return QInvSqrt(c.at(key1()))*unwhitenedError(c); } /** @@ -213,35 +210,29 @@ public: } /** vector of errors */ - Vector evaluateError(const T& p1, const T& p2, + Vector evaluateError(const Point2& p1, const Point2& p2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { // error = p2 - f(p1) // H1 = d error / d p1 = -d f/ d p1 = -F // H2 = d error / d p2 = I - T prediction = f(p1); + Point2 prediction = f(p1); - if(H1){ + if(H1) *H1 = -F(p1); - } - if(H2){ + if(H2) *H2 = eye(dim()); - } // Return the error between the prediction and the supplied value of p2 - return prediction.localCoordinates(p2); + return (p2 - prediction).vector(); } }; // Create Measurement Model Factor class NonlinearMeasurementModel : public NoiseModelFactor1 { -public: - typedef Point2 T; - -private: typedef NoiseModelFactor1 Base; typedef NonlinearMeasurementModel This; @@ -268,7 +259,7 @@ public: // Calculate the predicted measurement using the nonlinear function h() // Byproduct: updates Jacobian H, and noiseModel (R) - Vector h(const T& x_t1) const { + Vector h(const Point2& x_t1) const { // Calculate prediction Vector z_hat(1); @@ -277,7 +268,7 @@ public: return z_hat; } - Matrix H(const T& x_t1) const { + Matrix H(const Point2& x_t1) const { // Update Jacobian Matrix H(1,2); H(0,0) = 4*x_t1.x() - x_t1.y() - 2.5; @@ -287,7 +278,7 @@ public: } // Calculate the inverse square root of the motion model covariance, Q - Matrix RInvSqrt(const T& x_t0) const { + Matrix RInvSqrt(const Point2& x_t0) const { // This motion model has a constant covariance return R_invsqrt_; } @@ -320,7 +311,7 @@ public: /** Vector of errors, whitened ! */ Vector whitenedError(const Values& c) const { - return RInvSqrt(c.at(key()))*unwhitenedError(c); + return RInvSqrt(c.at(key()))*unwhitenedError(c); } /** @@ -345,7 +336,7 @@ public: } /** vector of errors */ - Vector evaluateError(const T& p, boost::optional H1 = boost::none) const { + Vector evaluateError(const Point2& p, boost::optional H1 = boost::none) const { // error = z - h(p) // H = d error / d p = -d h/ d p = -H Vector z_hat = h(p); diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index b43f0d3ef..4e14d49b3 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -23,10 +23,9 @@ #include #include #include -#include - #include #include +#include using namespace boost::assign; #include namespace br { using namespace boost::adaptors; using namespace boost::range; } diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index 81bcac344..e45f234aa 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -1,7 +1,14 @@ +/** + * @file NonlinearConjugateGradientOptimizer.cpp + * @brief Test simple CG optimizer + * @author Yong-Dian Jian + * @date June 11, 2012 + */ + /** * @file testGradientDescentOptimizer.cpp - * @brief - * @author ydjian + * @brief Small test of NonlinearConjugateGradientOptimizer + * @author Yong-Dian Jian * @date Jun 11, 2012 */ @@ -18,89 +25,74 @@ #include #include - using namespace std; using namespace gtsam; - +// Generate a small PoseSLAM problem boost::tuple generateProblem() { // 1. Create graph container and add factors to it - NonlinearFactorGraph graph ; + NonlinearFactorGraph graph; // 2a. Add Gaussian prior Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas( + Vector3(0.3, 0.3, 0.1)); graph += PriorFactor(1, priorMean, priorNoise); // 2b. Add odometry factors - SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph += BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); + SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas( + Vector3(0.2, 0.2, 0.1)); + graph += BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise); graph += BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph += BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph += BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); // 2c. Add pose constraint - SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph += BetweenFactor(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); + SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas( + Vector3(0.2, 0.2, 0.1)); + graph += BetweenFactor(5, 2, Pose2(2.0, 0.0, M_PI_2), + constraintUncertainty); // 3. Create the data structure to hold the initialEstimate estimate to the solution Values initialEstimate; - Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insert(1, x1); - Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insert(2, x2); - Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insert(3, x3); - Pose2 x4(4.0, 2.0, M_PI ); initialEstimate.insert(4, x4); - Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insert(5, x5); + Pose2 x1(0.5, 0.0, 0.2); + initialEstimate.insert(1, x1); + Pose2 x2(2.3, 0.1, -0.2); + initialEstimate.insert(2, x2); + Pose2 x3(4.1, 0.1, M_PI_2); + initialEstimate.insert(3, x3); + Pose2 x4(4.0, 2.0, M_PI); + initialEstimate.insert(4, x4); + Pose2 x5(2.1, 2.1, -M_PI_2); + initialEstimate.insert(5, x5); return boost::tie(graph, initialEstimate); } /* ************************************************************************* */ -TEST(optimize, GradientDescentOptimizer) { +TEST(NonlinearConjugateGradientOptimizer, Optimize) { NonlinearFactorGraph graph; Values initialEstimate; boost::tie(graph, initialEstimate) = generateProblem(); - // cout << "initial error = " << graph.error(initialEstimate) << endl ; +// cout << "initial error = " << graph.error(initialEstimate) << endl; - // Single Step Optimization using Levenberg-Marquardt NonlinearOptimizerParams param; - param.maxIterations = 500; /* requires a larger number of iterations to converge */ - param.verbosity = NonlinearOptimizerParams::SILENT; - - NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param); - Values result = optimizer.optimize(); -// cout << "gd1 solver final error = " << graph.error(result) << endl; - - /* the optimality of the solution is not comparable to the */ - DOUBLES_EQUAL(0.0, graph.error(result), 1e-2); - - CHECK(1); -} - -/* ************************************************************************* */ -TEST(optimize, ConjugateGradientOptimizer) { - - NonlinearFactorGraph graph; - Values initialEstimate; - - boost::tie(graph, initialEstimate) = generateProblem(); -// cout << "initial error = " << graph.error(initialEstimate) << endl ; - - // Single Step Optimization using Levenberg-Marquardt - NonlinearOptimizerParams param; - param.maxIterations = 500; /* requires a larger number of iterations to converge */ + param.maxIterations = 500; /* requires a larger number of iterations to converge */ param.verbosity = NonlinearOptimizerParams::SILENT; NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param); Values result = optimizer.optimize(); // cout << "cg final error = " << graph.error(result) << endl; - /* the optimality of the solution is not comparable to the */ - DOUBLES_EQUAL(0.0, graph.error(result), 1e-2); + EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-4); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/tests/testLie.cpp b/tests/testLie.cpp new file mode 100644 index 000000000..7be629053 --- /dev/null +++ b/tests/testLie.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 + + * -------------------------------1------------------------------------------- */ + +/** + * @file testLie.cpp + * @date May, 2015 + * @author Frank Dellaert + * @brief unit tests for Lie group type machinery + */ + +#include + +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +static const double tol = 1e-9; + +//****************************************************************************** +typedef ProductLieGroup Product; + +// Define any direct product group to be a model of the multiplicative Group concept +namespace gtsam { +template<> struct traits : internal::LieGroupTraits { + static void Print(const Product& m, const string& s = "") { + cout << s << "(" << m.first << "," << m.second.translation() << "/" + << m.second.theta() << ")" << endl; + } + static bool Equals(const Product& m1, const Product& m2, double tol = 1e-8) { + return m1.first.equals(m2.first, tol) && m1.second.equals(m2.second, tol); + } +}; +} + +//****************************************************************************** +TEST(Lie, ProductLieGroup) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); + Product pair1; + Vector5 d; + d << 1, 2, 0.1, 0.2, 0.3; + Product expected(Point2(1, 2), Pose2::Expmap(Vector3(0.1, 0.2, 0.3))); + Product pair2 = pair1.retract(d); + EXPECT(assert_equal(expected, pair2, 1e-9)); + EXPECT(assert_equal(d, pair1.localCoordinates(pair2), 1e-9)); +} + +/* ************************************************************************* */ +Product compose_proxy(const Product& A, const Product& B) { + return A.compose(B); +} +TEST( testProduct, compose ) { + Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1; + + Matrix actH1, actH2; + state1.compose(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2); + Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, tol)); + EXPECT(assert_equal(numericH2, actH2, tol)); +} + +/* ************************************************************************* */ +Product between_proxy(const Product& A, const Product& B) { + return A.between(B); +} +TEST( testProduct, between ) { + Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1; + + Matrix actH1, actH2; + state1.between(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2); + Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, tol)); + EXPECT(assert_equal(numericH2, actH2, tol)); +} + +/* ************************************************************************* */ +Product inverse_proxy(const Product& A) { + return A.inverse(); +} +TEST( testProduct, inverse ) { + Product state1(Point2(1, 2), Pose2(3, 4, 5)); + + Matrix actH1; + state1.inverse(actH1); + Matrix numericH1 = numericalDerivative11(inverse_proxy, state1); + EXPECT(assert_equal(numericH1, actH1, tol)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index 32f04225f..89b296824 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -10,21 +10,21 @@ * -------------------------------1------------------------------------------- */ /** - * @file testExpression.cpp + * @file testManifold.cpp * @date September 18, 2014 * @author Frank Dellaert * @author Paul Furgale - * @brief unit tests for Block Automatic Differentiation + * @brief unit tests for Manifold type machinery */ -#include +#include #include -#include +#include #include #include -#include +#include +#include #include -#include #undef CHECK #include @@ -39,173 +39,146 @@ using namespace gtsam; // The DefaultChart of Camera below is laid out like Snavely's 9-dim vector typedef PinholeCamera Camera; -/* ************************************************************************* */ -// is_manifold -TEST(Manifold, _is_manifold) { - using namespace traits; - EXPECT(!is_manifold::value); - EXPECT(is_manifold::value); - EXPECT(is_manifold::value); - EXPECT(is_manifold::value); - EXPECT(is_manifold::value); - EXPECT(is_manifold::value); +//****************************************************************************** +TEST(Manifold, SomeManifoldsGTSAM) { + //BOOST_CONCEPT_ASSERT((IsManifold)); // integer is not a manifold + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsManifold)); } -/* ************************************************************************* */ +//****************************************************************************** +TEST(Manifold, SomeLieGroupsGTSAM) { + BOOST_CONCEPT_ASSERT((IsLieGroup)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +//****************************************************************************** +TEST(Manifold, SomeVectorSpacesGTSAM) { + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); +} + +//****************************************************************************** // dimension TEST(Manifold, _dimension) { - using namespace traits; - EXPECT_LONGS_EQUAL(2, dimension::value); - EXPECT_LONGS_EQUAL(8, dimension::value); - EXPECT_LONGS_EQUAL(1, dimension::value); - EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); - EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); + //using namespace traits; + EXPECT_LONGS_EQUAL(2, traits::dimension); + EXPECT_LONGS_EQUAL(8, traits::dimension); + EXPECT_LONGS_EQUAL(1, traits::dimension); } -/* ************************************************************************* */ +//****************************************************************************** +TEST(Manifold, Identity) { + EXPECT_DOUBLES_EQUAL(0.0, traits::Identity(), 0.0); + EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::Identity()))); + EXPECT(assert_equal(Pose3(), traits::Identity())); + EXPECT(assert_equal(Point2(), traits::Identity())); +} + +//****************************************************************************** // charts TEST(Manifold, DefaultChart) { - DefaultChart chart1; - EXPECT(chart1.local(Point2(0, 0), Point2(1, 0)) == Vector2(1, 0)); - EXPECT(chart1.retract(Point2(0, 0), Vector2(1, 0)) == Point2(1, 0)); + //DefaultChart chart1; + EXPECT(traits::Local(Point2(0, 0), Point2(1, 0)) == Vector2(1, 0)); + EXPECT(traits::Retract(Point2(0, 0), Vector2(1, 0)) == Point2(1, 0)); Vector v2(2); v2 << 1, 0; - DefaultChart chart2; - EXPECT(assert_equal(v2, chart2.local(Vector2(0, 0), Vector2(1, 0)))); - EXPECT(chart2.retract(Vector2(0, 0), v2) == Vector2(1, 0)); + //DefaultChart chart2; + EXPECT(assert_equal(v2, traits::Local(Vector2(0, 0), Vector2(1, 0)))); + EXPECT(traits::Retract(Vector2(0, 0), v2) == Vector2(1, 0)); { typedef Matrix2 ManifoldPoint; ManifoldPoint m; - DefaultChart chart; + //DefaultChart chart; m << 1, 3, 2, 4; // m as per default is in column-major storage mode. So this yields a linear representation of (1, 2, 3, 4)! - EXPECT(assert_equal(Vector(Vector4(1, 2, 3, 4)), Vector(chart.local(ManifoldPoint::Zero(), m)))); - EXPECT(chart.retract(m, Vector4(1, 2, 3, 4)) == 2 * m); + EXPECT(assert_equal(Vector(Vector4(1, 2, 3, 4)), Vector(traits::Local(ManifoldPoint::Zero(), m)))); + EXPECT(traits::Retract(m, Vector4(1, 2, 3, 4)) == 2 * m); } { typedef Eigen::Matrix ManifoldPoint; ManifoldPoint m; - DefaultChart chart; + //DefaultChart chart; m << 1, 2; - EXPECT(assert_equal(Vector(Vector2(1, 2)), Vector(chart.local(ManifoldPoint::Zero(), m)))); - EXPECT(chart.retract(m, Vector2(1, 2)) == 2 * m); + EXPECT(assert_equal(Vector(Vector2(1, 2)), Vector(traits::Local(ManifoldPoint::Zero(), m)))); + EXPECT(traits::Retract(m, Vector2(1, 2)) == 2 * m); } { typedef Eigen::Matrix ManifoldPoint; ManifoldPoint m; - DefaultChart chart; + //DefaultChart chart; m << 1; - EXPECT(assert_equal(Vector(ManifoldPoint::Ones()), Vector(chart.local(ManifoldPoint::Zero(), m)))); - EXPECT(chart.retract(m, ManifoldPoint::Ones()) == 2 * m); + EXPECT(assert_equal(Vector(ManifoldPoint::Ones()), Vector(traits::Local(ManifoldPoint::Zero(), m)))); + EXPECT(traits::Retract(m, ManifoldPoint::Ones()) == 2 * m); } - DefaultChart chart3; + //DefaultChart chart3; Vector v1(1); v1 << 1; - EXPECT(assert_equal(v1, chart3.local(0, 1))); - EXPECT_DOUBLES_EQUAL(chart3.retract(0, v1), 1, 1e-9); + EXPECT(assert_equal(v1, traits::Local(0, 1))); + EXPECT_DOUBLES_EQUAL(traits::Retract(0, v1), 1, 1e-9); // Dynamic does not work yet ! Vector z = zero(2), v(2); v << 1, 0; - DefaultChart chart4; - EXPECT(assert_equal(chart4.local(z, v), v)); - EXPECT(assert_equal(chart4.retract(z, v), v)); + //DefaultChart chart4; +// EXPECT(assert_equal(traits::Local(z, v), v)); +// EXPECT(assert_equal(traits::Retract(z, v), v)); Vector v3(3); v3 << 1, 1, 1; Rot3 I = Rot3::identity(); Rot3 R = I.retract(v3); - DefaultChart chart5; - EXPECT(assert_equal(v3, chart5.local(I, R))); - EXPECT(assert_equal(chart5.retract(I, v3), R)); + //DefaultChart chart5; + EXPECT(assert_equal(v3, traits::Local(I, R))); + EXPECT(assert_equal(traits::Retract(I, v3), R)); // Check zero vector - DefaultChart chart6; - EXPECT(assert_equal(zero(3), chart6.local(R, R))); + //DefaultChart chart6; + EXPECT(assert_equal(zero(3), traits::Local(R, R))); } -/* ************************************************************************* */ -// zero -TEST(Manifold, _zero) { - EXPECT(assert_equal(Pose3(), traits::zero::value())); - Cal3Bundler cal(0, 0, 0); - EXPECT(assert_equal(cal, traits::zero::value())); - EXPECT(assert_equal(Camera(Pose3(), cal), traits::zero::value())); - EXPECT(assert_equal(Point2(), traits::zero::value())); - EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::zero::value()))); - EXPECT_DOUBLES_EQUAL(0.0, traits::zero::value(), 0.0); +//****************************************************************************** +typedef ProductManifold MyPoint2Pair; + +// Define any direct product group to be a model of the multiplicative Group concept +namespace gtsam { +template<> struct traits : internal::ManifoldTraits { + static void Print(const MyPoint2Pair& m, const string& s = "") { + cout << s << "(" << m.first << "," << m.second << ")" << endl; + } + static bool Equals(const MyPoint2Pair& m1, const MyPoint2Pair& m2, double tol = 1e-8) { + return m1 == m2; + } +}; } -/* ************************************************************************* */ -// identity -TEST(Manifold, _identity) { - EXPECT(assert_equal(Pose3(), traits::identity::value())); - EXPECT(assert_equal(Cal3Bundler(), traits::identity::value())); - EXPECT(assert_equal(Camera(), traits::identity::value())); - EXPECT(assert_equal(Point2(), traits::identity::value())); - EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::identity::value()))); - EXPECT_DOUBLES_EQUAL(0.0, traits::identity::value(), 0.0); +TEST(Manifold, ProductManifold) { + BOOST_CONCEPT_ASSERT((IsManifold)); + MyPoint2Pair pair1; + Vector4 d; + d << 1,2,3,4; + MyPoint2Pair expected(Point2(1,2),Point2(3,4)); + MyPoint2Pair pair2 = pair1.retract(d); + EXPECT(assert_equal(expected,pair2,1e-9)); + EXPECT(assert_equal(d, pair1.localCoordinates(pair2),1e-9)); } -/* ************************************************************************* */ -// charts -TEST(Manifold, Canonical) { - - Canonical chart1; - EXPECT(chart1.local(Point2(1, 0))==Vector2(1, 0)); - EXPECT(chart1.retract(Vector2(1, 0))==Point2(1, 0)); - - Vector v2(2); - v2 << 1, 0; - Canonical chart2; - EXPECT(assert_equal(v2, chart2.local(Vector2(1, 0)))); - EXPECT(chart2.retract(v2)==Vector2(1, 0)); - - Canonical chart3; - Eigen::Matrix v1; - v1 << 1; - EXPECT(chart3.local(1)==v1); - EXPECT_DOUBLES_EQUAL(chart3.retract(v1), 1, 1e-9); - - Canonical chart4; - Point3 point(1, 2, 3); - Vector v3(3); - v3 << 1, 2, 3; - EXPECT(assert_equal(v3, chart4.local(point))); - EXPECT(assert_equal(chart4.retract(v3), point)); - - Canonical chart5; - Pose3 pose(Rot3::identity(), point); - Vector v6(6); - v6 << 0, 0, 0, 1, 2, 3; - EXPECT(assert_equal(v6, chart5.local(pose))); - EXPECT(assert_equal(chart5.retract(v6), pose)); - - Canonical chart6; - Cal3Bundler cal0(0, 0, 0); - Camera camera(Pose3(), cal0); - Vector z9 = Vector9::Zero(); - EXPECT(assert_equal(z9, chart6.local(camera))); - EXPECT(assert_equal(chart6.retract(z9), camera)); - - Cal3Bundler cal; // Note !! Cal3Bundler() != zero::value() - Camera camera2(pose, cal); - Vector v9(9); - v9 << 0, 0, 0, 1, 2, 3, 1, 0, 0; - EXPECT(assert_equal(v9, chart6.local(camera2))); - EXPECT(assert_equal(chart6.retract(v9), camera2)); -} - -/* ************************************************************************* */ +//****************************************************************************** int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ +//****************************************************************************** diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 6366d9fa5..a3c99ece7 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -42,9 +42,9 @@ typedef PriorFactor PosePrior; typedef NonlinearEquality PoseNLE; typedef boost::shared_ptr shared_poseNLE; -static Symbol key('x',1); +static Symbol key('x', 1); -/* ************************************************************************* */ +//****************************************************************************** TEST ( NonlinearEquality, linearization ) { Pose2 value = Pose2(2.1, 1.0, 2.0); Values linearize; @@ -60,10 +60,10 @@ TEST ( NonlinearEquality, linearization ) { EXPECT(assert_equal((const GaussianFactor&)expLF, *actualLF)); } -/* ********************************************************************** */ +//****************************************************************************** TEST ( NonlinearEquality, linearization_pose ) { - Symbol key('x',1); + Symbol key('x', 1); Pose2 value; Values config; config.insert(key, value); @@ -75,7 +75,7 @@ TEST ( NonlinearEquality, linearization_pose ) { EXPECT(true); } -/* ********************************************************************** */ +//****************************************************************************** TEST ( NonlinearEquality, linearization_fail ) { Pose2 value = Pose2(2.1, 1.0, 2.0); Pose2 wrong = Pose2(2.1, 3.0, 4.0); @@ -89,12 +89,11 @@ TEST ( NonlinearEquality, linearization_fail ) { CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument); } -/* ********************************************************************** */ +//****************************************************************************** TEST ( NonlinearEquality, linearization_fail_pose ) { - Symbol key('x',1); - Pose2 value(2.0, 1.0, 2.0), - wrong(2.0, 3.0, 4.0); + Symbol key('x', 1); + Pose2 value(2.0, 1.0, 2.0), wrong(2.0, 3.0, 4.0); Values bad_linearize; bad_linearize.insert(key, wrong); @@ -105,12 +104,11 @@ TEST ( NonlinearEquality, linearization_fail_pose ) { CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument); } -/* ********************************************************************** */ +//****************************************************************************** TEST ( NonlinearEquality, linearization_fail_pose_origin ) { - Symbol key('x',1); - Pose2 value, - wrong(2.0, 3.0, 4.0); + Symbol key('x', 1); + Pose2 value, wrong(2.0, 3.0, 4.0); Values bad_linearize; bad_linearize.insert(key, wrong); @@ -121,7 +119,7 @@ TEST ( NonlinearEquality, linearization_fail_pose_origin ) { CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument); } -/* ************************************************************************* */ +//****************************************************************************** TEST ( NonlinearEquality, error ) { Pose2 value = Pose2(2.1, 1.0, 2.0); Pose2 wrong = Pose2(2.1, 3.0, 4.0); @@ -137,10 +135,11 @@ TEST ( NonlinearEquality, error ) { EXPECT(assert_equal(actual, zero(3))); actual = nle->unwhitenedError(bad_linearize); - EXPECT(assert_equal(actual, repeat(3, std::numeric_limits::infinity()))); + EXPECT( + assert_equal(actual, repeat(3, std::numeric_limits::infinity()))); } -/* ************************************************************************* */ +//****************************************************************************** TEST ( NonlinearEquality, equals ) { Pose2 value1 = Pose2(2.1, 1.0, 2.0); Pose2 value2 = Pose2(2.1, 3.0, 4.0); @@ -151,14 +150,17 @@ TEST ( NonlinearEquality, equals ) { shared_poseNLE nle3(new PoseNLE(key, value2)); // verify - EXPECT(nle1->equals(*nle2)); // basic equality = true - EXPECT(nle2->equals(*nle1)); // test symmetry of equals() - EXPECT(!nle1->equals(*nle3)); // test config + EXPECT(nle1->equals(*nle2)); + // basic equality = true + EXPECT(nle2->equals(*nle1)); + // test symmetry of equals() + EXPECT(!nle1->equals(*nle3)); + // test config } -/* ************************************************************************* */ +//****************************************************************************** TEST ( NonlinearEquality, allow_error_pose ) { - Symbol key1('x',1); + Symbol key1('x', 1); Pose2 feasible1(1.0, 2.0, 3.0); double error_gain = 500.0; PoseNLE nle(key1, feasible1, error_gain); @@ -177,16 +179,17 @@ TEST ( NonlinearEquality, allow_error_pose ) { // check linearization GaussianFactor::shared_ptr actLinFactor = nle.linearize(config); - Matrix A1 = eye(3,3); + Matrix A1 = eye(3, 3); Vector b = expVec; SharedDiagonal model = noiseModel::Constrained::All(3); - GaussianFactor::shared_ptr expLinFactor(new JacobianFactor(key1, A1, b, model)); + GaussianFactor::shared_ptr expLinFactor( + new JacobianFactor(key1, A1, b, model)); EXPECT(assert_equal(*expLinFactor, *actLinFactor, 1e-5)); } -/* ************************************************************************* */ +//****************************************************************************** TEST ( NonlinearEquality, allow_error_optimize ) { - Symbol key1('x',1); + Symbol key1('x', 1); Pose2 feasible1(1.0, 2.0, 3.0); double error_gain = 500.0; PoseNLE nle(key1, feasible1, error_gain); @@ -211,11 +214,11 @@ TEST ( NonlinearEquality, allow_error_optimize ) { EXPECT(assert_equal(expected, result)); } -/* ************************************************************************* */ +//****************************************************************************** TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { // create a hard constraint - Symbol key1('x',1); + Symbol key1('x', 1); Pose2 feasible1(1.0, 2.0, 3.0); // initialize away from the ideal @@ -245,14 +248,14 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { EXPECT(assert_equal(expected, actual)); } -/* ************************************************************************* */ +//****************************************************************************** static SharedDiagonal hard_model = noiseModel::Constrained::All(2); static SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0); -/* ************************************************************************* */ +//****************************************************************************** TEST( testNonlinearEqualityConstraint, unary_basics ) { Point2 pt(1.0, 2.0); - Symbol key1('x',1); + Symbol key1('x', 1); double mu = 1000.0; eq2D::UnaryEqualityConstraint constraint(pt, key, mu); @@ -267,38 +270,42 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) { Point2 ptBad1(2.0, 2.0); config2.insert(key, ptBad1); EXPECT(constraint.active(config2)); - EXPECT(assert_equal(Vector2(1.0, 0.0), constraint.evaluateError(ptBad1), tol)); - EXPECT(assert_equal(Vector2(1.0, 0.0), constraint.unwhitenedError(config2), tol)); + EXPECT( + assert_equal(Vector2(1.0, 0.0), constraint.evaluateError(ptBad1), tol)); + EXPECT( + assert_equal(Vector2(1.0, 0.0), constraint.unwhitenedError(config2), tol)); EXPECT_DOUBLES_EQUAL(500.0, constraint.error(config2), tol); } -/* ************************************************************************* */ +//****************************************************************************** TEST( testNonlinearEqualityConstraint, unary_linearization ) { Point2 pt(1.0, 2.0); - Symbol key1('x',1); + Symbol key1('x', 1); double mu = 1000.0; eq2D::UnaryEqualityConstraint constraint(pt, key, mu); Values config1; config1.insert(key, pt); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); - GaussianFactor::shared_ptr expected1(new JacobianFactor(key, eye(2,2), zero(2), hard_model)); + GaussianFactor::shared_ptr expected1( + new JacobianFactor(key, eye(2, 2), zero(2), hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); Values config2; Point2 ptBad(2.0, 2.0); config2.insert(key, ptBad); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); - GaussianFactor::shared_ptr expected2(new JacobianFactor(key, eye(2,2), Vector2(-1.0,0.0), hard_model)); + GaussianFactor::shared_ptr expected2( + new JacobianFactor(key, eye(2, 2), Vector2(-1.0, 0.0), hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } -/* ************************************************************************* */ +//****************************************************************************** TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) { // create a single-node graph with a soft and hard constraint to // ensure that the hard constraint overrides the soft constraint Point2 truth_pt(1.0, 2.0); - Symbol key('x',1); + Symbol key('x', 1); double mu = 10.0; eq2D::UnaryEqualityConstraint::shared_ptr constraint( new eq2D::UnaryEqualityConstraint(truth_pt, key, mu)); @@ -326,10 +333,10 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) { EXPECT(assert_equal(expected, actual, tol)); } -/* ************************************************************************* */ +//****************************************************************************** TEST( testNonlinearEqualityConstraint, odo_basics ) { Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); - Symbol key1('x',1), key2('x',2); + Symbol key1('x', 1), key2('x', 2); double mu = 1000.0; eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); @@ -347,15 +354,17 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) { config2.insert(key1, x1bad); config2.insert(key2, x2bad); EXPECT(constraint.active(config2)); - EXPECT(assert_equal(Vector2(-1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol)); - EXPECT(assert_equal(Vector2(-1.0, -1.0), constraint.unwhitenedError(config2), tol)); + EXPECT( + assert_equal(Vector2(-1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol)); + EXPECT( + assert_equal(Vector2(-1.0, -1.0), constraint.unwhitenedError(config2), tol)); EXPECT_DOUBLES_EQUAL(1000.0, constraint.error(config2), tol); } -/* ************************************************************************* */ +//****************************************************************************** TEST( testNonlinearEqualityConstraint, odo_linearization ) { Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); - Symbol key1('x',1), key2('x',2); + Symbol key1('x', 1), key2('x', 2); double mu = 1000.0; eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); @@ -364,8 +373,8 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { config1.insert(key2, x2); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); GaussianFactor::shared_ptr expected1( - new JacobianFactor(key1, -eye(2,2), key2, - eye(2,2), zero(2), hard_model)); + new JacobianFactor(key1, -eye(2, 2), key2, eye(2, 2), zero(2), + hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); Values config2; @@ -375,18 +384,18 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { config2.insert(key2, x2bad); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); GaussianFactor::shared_ptr expected2( - new JacobianFactor(key1, -eye(2,2), key2, - eye(2,2), Vector2(1.0, 1.0), hard_model)); + new JacobianFactor(key1, -eye(2, 2), key2, eye(2, 2), Vector2(1.0, 1.0), + hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } -/* ************************************************************************* */ +//****************************************************************************** TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { // create a two-node graph, connected by an odometry constraint, with // a hard prior on one variable, and a conflicting soft prior // on the other variable - the constraints should override the soft constraint Point2 truth_pt1(1.0, 2.0), truth_pt2(3.0, 2.0); - Symbol key1('x',1), key2('x',2); + Symbol key1('x', 1), key2('x', 2); // hard prior on x1 eq2D::UnaryEqualityConstraint::shared_ptr constraint1( @@ -399,8 +408,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { // odometry constraint eq2D::OdoEqualityConstraint::shared_ptr constraint2( - new eq2D::OdoEqualityConstraint( - truth_pt1.between(truth_pt2), key1, key2)); + new eq2D::OdoEqualityConstraint(truth_pt2-truth_pt1, key1, key2)); NonlinearFactorGraph graph; graph.push_back(constraint1); @@ -418,7 +426,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { CHECK(assert_equal(expected, actual, tol)); } -/* ********************************************************************* */ +//****************************************************************************** TEST (testNonlinearEqualityConstraint, two_pose ) { /* * Determining a ground truth linear system @@ -428,19 +436,18 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { NonlinearFactorGraph graph; - Symbol x1('x',1), x2('x',2); - Symbol l1('l',1), l2('l',2); - Point2 pt_x1(1.0, 1.0), - pt_x2(5.0, 6.0); + Symbol x1('x', 1), x2('x', 2); + Symbol l1('l', 1), l2('l', 2); + Point2 pt_x1(1.0, 1.0), pt_x2(5.0, 6.0); graph += eq2D::UnaryEqualityConstraint(pt_x1, x1); graph += eq2D::UnaryEqualityConstraint(pt_x2, x2); Point2 z1(0.0, 5.0); SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(2, 0.1)); - graph += simulated2D::Measurement(z1, sigma, x1,l1); + graph += simulated2D::Measurement(z1, sigma, x1, l1); Point2 z2(-4.0, 0.0); - graph += simulated2D::Measurement(z2, sigma, x2,l2); + graph += simulated2D::Measurement(z2, sigma, x2, l2); graph += eq2D::PointEqualityConstraint(l1, l2); @@ -450,7 +457,8 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { initialEstimate.insert(l1, Point2(1.0, 6.0)); // ground truth initialEstimate.insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame - Values actual = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize(); + Values actual = + LevenbergMarquardtOptimizer(graph, initialEstimate).optimize(); Values expected; expected.insert(x1, pt_x1); @@ -460,14 +468,14 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { CHECK(assert_equal(expected, actual, 1e-5)); } -/* ********************************************************************* */ +//****************************************************************************** TEST (testNonlinearEqualityConstraint, map_warp ) { // get a graph NonlinearFactorGraph graph; // keys - Symbol x1('x',1), x2('x',2); - Symbol l1('l',1), l2('l',2); + Symbol x1('x', 1), x2('x', 2); + Symbol l1('l', 1), l2('l', 2); // constant constraint on x1 Point2 pose1(1.0, 1.0); @@ -488,13 +496,14 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { // create an initial estimate Values initialEstimate; - initialEstimate.insert(x1, Point2( 1.0, 1.0)); - initialEstimate.insert(l1, Point2( 1.0, 6.0)); + initialEstimate.insert(x1, Point2(1.0, 1.0)); + initialEstimate.insert(l1, Point2(1.0, 6.0)); initialEstimate.insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame - initialEstimate.insert(x2, Point2( 0.0, 0.0)); // other pose starts at origin + initialEstimate.insert(x2, Point2(0.0, 0.0)); // other pose starts at origin // optimize - Values actual = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize(); + Values actual = + LevenbergMarquardtOptimizer(graph, initialEstimate).optimize(); Values expected; expected.insert(x1, Point2(1.0, 1.0)); @@ -506,8 +515,8 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { // make a realistic calibration matrix static double fov = 60; // degrees -static int w=640,h=480; -static Cal3_S2 K(fov,w,h); +static int w = 640, h = 480; +static Cal3_S2 K(fov, w, h); static boost::shared_ptr shK(new Cal3_S2(K)); // typedefs for visual SLAM example @@ -516,14 +525,12 @@ typedef NonlinearFactorGraph VGraph; // factors for visual slam typedef NonlinearEquality2 Point3Equality; -/* ********************************************************************* */ +//****************************************************************************** TEST (testNonlinearEqualityConstraint, stereo_constrained ) { // create initial estimates - Rot3 faceDownY((Matrix)(Matrix(3,3) << - 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, - 0.0, -1.0, 0.0).finished()); + Rot3 faceDownY( + (Matrix) (Matrix(3, 3) << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0).finished()); Pose3 pose1(faceDownY, Point3()); // origin, left camera SimpleCamera camera1(pose1, K); Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left @@ -531,8 +538,8 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away // keys - Symbol x1('x',1), x2('x',2); - Symbol l1('l',1), l2('l',2); + Symbol x1('x', 1), x2('x', 2); + Symbol l1('l', 1), l2('l', 2); // create graph VGraph graph; @@ -543,8 +550,10 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { // create factors SharedDiagonal vmodel = noiseModel::Unit::Create(2); - graph += GenericProjectionFactor(camera1.project(landmark), vmodel, x1, l1, shK); - graph += GenericProjectionFactor(camera2.project(landmark), vmodel, x2, l2, shK); + graph += GenericProjectionFactor( + camera1.project(landmark), vmodel, x1, l1, shK); + graph += GenericProjectionFactor( + camera2.project(landmark), vmodel, x2, l2, shK); // add equality constraint graph += Point3Equality(l1, l2); @@ -573,6 +582,9 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { CHECK(assert_equal(truthValues, actual, 1e-5)); } -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 9c2eddcc3..f398a33fe 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -79,14 +79,14 @@ TEST( NonlinearFactorGraph, GET_ORDERING) { Ordering expected; expected += L(1), X(2), X(1); // For starting with l1,x1,x2 NonlinearFactorGraph nlfg = createNonlinearFactorGraph(); - Ordering actual = Ordering::COLAMD(nlfg); + Ordering actual = Ordering::colamd(nlfg); EXPECT(assert_equal(expected,actual)); // Constrained ordering - put x2 at the end Ordering expectedConstrained; expectedConstrained += L(1), X(1), X(2); FastMap constraints; constraints[X(2)] = 1; - Ordering actualConstrained = Ordering::COLAMDConstrained(nlfg, constraints); + Ordering actualConstrained = Ordering::colamdConstrained(nlfg, constraints); EXPECT(assert_equal(expectedConstrained, actualConstrained)); } diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index da3462b57..66b022c82 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -17,21 +17,11 @@ */ #include -#include -#include -#include -#include -#include -#include -#include #include #include #include -#include #include -#include #include -#include #include #include @@ -91,6 +81,49 @@ TEST( PCGSolver, llt ) { } +/* ************************************************************************* */ +// Test GaussianFactorGraphSystem::multiply and getb +TEST( GaussianFactorGraphSystem, multiply_getb) +{ + // Create a Gaussian Factor Graph + GaussianFactorGraph simpleGFG; + SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.3)); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2); + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2); + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); + simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); + + // Create a dummy-preconditioner and a GaussianFactorGraphSystem + DummyPreconditioner dummyPreconditioner; + KeyInfo keyInfo(simpleGFG); + std::map lambda; + dummyPreconditioner.build(simpleGFG, keyInfo, lambda); + GaussianFactorGraphSystem gfgs(simpleGFG, dummyPreconditioner, keyInfo, lambda); + + // Prepare container for each variable + Vector initial, residual, preconditionedResidual, p, actualAp; + initial = (Vector(6) << 0., 0., 0., 0., 0., 0.).finished(); + + // Calculate values using GaussianFactorGraphSystem same as inside of PCGSolver + gfgs.residual(initial, residual); /* r = b-Ax */ + gfgs.leftPrecondition(residual, preconditionedResidual); /* pr = L^{-1} (b-Ax) */ + gfgs.rightPrecondition(preconditionedResidual, p); /* p = L^{-T} pr */ + gfgs.multiply(p, actualAp); /* A p */ + + // Expected value of Ap for the first iteration of this example problem + Vector expectedAp = (Vector(6) << 100400, -249074.074, -2080, 148148.148, -146480, 37962.963).finished(); + EXPECT(assert_equal(expectedAp, actualAp, 1e-3)); + + // Expected value of getb + Vector expectedb = (Vector(6) << 100.0, -194.444, -20.0, 138.889, -120.0, -55.556).finished(); + Vector actualb; + gfgs.getb(actualb); + EXPECT(assert_equal(expectedb, actualb, 1e-3)); +} + /* ************************************************************************* */ // Test Dummy Preconditioner TEST( PCGSolver, dummy ) diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index ad2c0767a..f00916475 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -18,96 +18,110 @@ #include -#include -#include #include -#include +#include +#include +#include #include +#include using namespace std; using namespace gtsam; /* ************************************************************************* */ -static GaussianFactorGraph createSimpleGaussianFactorGraph() { - GaussianFactorGraph fg; - SharedDiagonal unit2 = noiseModel::Unit::Create(2); - // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2); - // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg += JacobianFactor(2, -10*eye(2), 0, 10*eye(2), Vector2(2.0, -1.0), unit2); - // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), Vector2(0.0, 1.0), unit2); - // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), Vector2(-1.0, 1.5), unit2); - return fg; +TEST( PCGsolver, verySimpleLinearSystem) { + + // Ax = [4 1][u] = [1] x0 = [2] + // [1 3][v] [2] [1] + // + // exact solution x = [1/11, 7/11]'; + // + + // Create a Gaussian Factor Graph + GaussianFactorGraph simpleGFG; + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 4, 1, 1, 3).finished(), (Vector(2) << 1,2 ).finished(), noiseModel::Unit::Create(2)); + + // Exact solution already known + VectorValues exactSolution; + exactSolution.insert(0, (Vector(2) << 1./11., 7./11.).finished()); + //exactSolution.print("Exact"); + + // Solve the system using direct method + VectorValues deltaDirect = simpleGFG.optimize(); + EXPECT(assert_equal(exactSolution, deltaDirect, 1e-7)); + //deltaDirect.print("Direct"); + + // Solve the system using Preconditioned Conjugate Gradient solver + // Common PCG parameters + gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared(); + pcg->setMaxIterations(500); + pcg->setEpsilon_abs(0.0); + pcg->setEpsilon_rel(0.0); + //pcg->setVerbosity("ERROR"); + + // With Dummy preconditioner + pcg->preconditioner_ = boost::make_shared(); + VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG); + EXPECT(assert_equal(exactSolution, deltaPCGDummy, 1e-7)); + //deltaPCGDummy.print("PCG Dummy"); + + // With Block-Jacobi preconditioner + pcg->preconditioner_ = boost::make_shared(); + // It takes more than 1000 iterations for this test + pcg->setMaxIterations(1500); + VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG); + + EXPECT(assert_equal(exactSolution, deltaPCGJacobi, 1e-5)); + //deltaPCGJacobi.print("PCG Jacobi"); } /* ************************************************************************* */ -// Copy of BlockJacobiPreconditioner::build -std::vector buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo) -{ - const size_t n = keyInfo.size(); - std::vector dims_ = keyInfo.colSpec(); +TEST(PCGSolver, simpleLinearSystem) { + // Create a Gaussian Factor Graph + GaussianFactorGraph simpleGFG; + //SharedDiagonal unit2 = noiseModel::Unit::Create(2); + SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.3)); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2); + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2); + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); + simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); + //simpleGFG.print("system"); - /* prepare the buffer of block diagonals */ - std::vector blocks; blocks.reserve(n); + // Expected solution + VectorValues expectedSolution; + expectedSolution.insert(0, (Vector(2) << 0.100498, -0.196756).finished()); + expectedSolution.insert(2, (Vector(2) << -0.0990413, -0.0980577).finished()); + expectedSolution.insert(1, (Vector(2) << -0.0973252, 0.100582).finished()); + //expectedSolution.print("Expected"); - /* allocate memory for the factorization of block diagonals */ - size_t nnz = 0; - for ( size_t i = 0 ; i < n ; ++i ) { - const size_t dim = dims_[i]; - blocks.push_back(Matrix::Zero(dim, dim)); - // nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2 ; - nnz += dim*dim; - } + // Solve the system using direct method + VectorValues deltaDirect = simpleGFG.optimize(); + EXPECT(assert_equal(expectedSolution, deltaDirect, 1e-5)); + //deltaDirect.print("Direct"); - /* compute the block diagonal by scanning over the factors */ - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { - if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { - for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { - const KeyInfoEntry &entry = keyInfo.find(*it)->second; - const Matrix &Ai = jf->getA(it); - blocks[entry.index()] += (Ai.transpose() * Ai); - } - } - else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { - for ( HessianFactor::const_iterator it = hf->begin() ; it != hf->end() ; ++it ) { - const KeyInfoEntry &entry = keyInfo.find(*it)->second; - const Matrix &Hii = hf->info(it, it).selfadjointView(); - blocks[entry.index()] += Hii; - } - } - else { - throw invalid_argument("BlockJacobiPreconditioner::build gfg contains a factor that is neither a JacobianFactor nor a HessianFactor."); - } - } + // Solve the system using Preconditioned Conjugate Gradient solver + // Common PCG parameters + gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared(); + pcg->setMaxIterations(500); + pcg->setEpsilon_abs(0.0); + pcg->setEpsilon_rel(0.0); + //pcg->setVerbosity("ERROR"); - return blocks; -} + // With Dummy preconditioner + pcg->preconditioner_ = boost::make_shared(); + VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG); + EXPECT(assert_equal(expectedSolution, deltaPCGDummy, 1e-5)); + //deltaPCGDummy.print("PCG Dummy"); -/* ************************************************************************* */ -TEST( Preconditioner, buildBlocks ) { - // Create simple Gaussian factor graph and initial values - GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); - Values initial; - initial.insert(0,Point2(4, 5)); - initial.insert(1,Point2(0, 1)); - initial.insert(2,Point2(-5, 7)); + // With Block-Jacobi preconditioner + pcg->preconditioner_ = boost::make_shared(); + VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG); + EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5)); + //deltaPCGJacobi.print("PCG Jacobi"); - // Expected Hessian block diagonal matrices - std::map expectedHessian =gfg.hessianBlockDiagonal(); - - // Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build - std::vector actualHessian = buildBlocks(gfg, KeyInfo(gfg)); - - // Compare the number of block diagonal matrices - EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size()); - - // Compare the values of matrices - std::map::const_iterator it1 = expectedHessian.begin(); - std::vector::const_iterator it2 = actualHessian.begin(); - for(; it1!=expectedHessian.end(); it1++, it2++) - EXPECT(assert_equal(it1->second, *it2)); } /* ************************************************************************* */ diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index ffde900e9..2514c80d9 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -148,21 +148,21 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); /* Create GUIDs for geometry */ /* ************************************************************************* */ -BOOST_CLASS_EXPORT(gtsam::LieVector); -BOOST_CLASS_EXPORT(gtsam::LieMatrix); -BOOST_CLASS_EXPORT(gtsam::Point2); -BOOST_CLASS_EXPORT(gtsam::StereoPoint2); -BOOST_CLASS_EXPORT(gtsam::Point3); -BOOST_CLASS_EXPORT(gtsam::Rot2); -BOOST_CLASS_EXPORT(gtsam::Rot3); -BOOST_CLASS_EXPORT(gtsam::Pose2); -BOOST_CLASS_EXPORT(gtsam::Pose3); -BOOST_CLASS_EXPORT(gtsam::Cal3_S2); -BOOST_CLASS_EXPORT(gtsam::Cal3DS2); -BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo); -BOOST_CLASS_EXPORT(gtsam::CalibratedCamera); -BOOST_CLASS_EXPORT(gtsam::SimpleCamera); -BOOST_CLASS_EXPORT(gtsam::StereoCamera); +GTSAM_VALUE_EXPORT(gtsam::LieVector); +GTSAM_VALUE_EXPORT(gtsam::LieMatrix); +GTSAM_VALUE_EXPORT(gtsam::Point2); +GTSAM_VALUE_EXPORT(gtsam::StereoPoint2); +GTSAM_VALUE_EXPORT(gtsam::Point3); +GTSAM_VALUE_EXPORT(gtsam::Rot2); +GTSAM_VALUE_EXPORT(gtsam::Rot3); +GTSAM_VALUE_EXPORT(gtsam::Pose2); +GTSAM_VALUE_EXPORT(gtsam::Pose3); +GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); +GTSAM_VALUE_EXPORT(gtsam::Cal3DS2); +GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo); +GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera); +GTSAM_VALUE_EXPORT(gtsam::SimpleCamera); +GTSAM_VALUE_EXPORT(gtsam::StereoCamera); /* Create GUIDs for factors */ /* ************************************************************************* */ diff --git a/gtsam_unstable/timing/timeAdaptAutoDiff.cpp b/timing/timeAdaptAutoDiff.cpp similarity index 92% rename from gtsam_unstable/timing/timeAdaptAutoDiff.cpp rename to timing/timeAdaptAutoDiff.cpp index 32bce9ba5..edfd420ef 100644 --- a/gtsam_unstable/timing/timeAdaptAutoDiff.cpp +++ b/timing/timeAdaptAutoDiff.cpp @@ -17,9 +17,9 @@ */ #include "timeLinearize.h" -#include -#include -#include +#include +#include +#include #include #include #include diff --git a/timing/timeCalibratedCamera.cpp b/timing/timeCalibratedCamera.cpp index 0a003a4c7..2d0576aea 100644 --- a/timing/timeCalibratedCamera.cpp +++ b/timing/timeCalibratedCamera.cpp @@ -15,10 +15,10 @@ * @author Frank Dellaert */ -#include -#include - +#include #include +#include +#include using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/timing/timeCameraExpression.cpp b/timing/timeCameraExpression.cpp similarity index 94% rename from gtsam_unstable/timing/timeCameraExpression.cpp rename to timing/timeCameraExpression.cpp index 92522c440..208c991fd 100644 --- a/gtsam_unstable/timing/timeCameraExpression.cpp +++ b/timing/timeCameraExpression.cpp @@ -16,8 +16,8 @@ * @date October 3, 2014 */ -#include -#include +#include +#include #include #include #include @@ -32,7 +32,7 @@ using namespace gtsam; boost::shared_ptr fixedK(new Cal3_S2()); Point2 myProject(const Pose3& pose, const Point3& point, - boost::optional H1, boost::optional H2) { + OptionalJacobian<2,6> H1, OptionalJacobian<2,3> H2) { PinholeCamera camera(pose, *fixedK); return camera.project(point, H1, H2, boost::none); } diff --git a/timing/timeIncremental.cpp b/timing/timeIncremental.cpp index 6f2909b7a..d82ef9442 100644 --- a/timing/timeIncremental.cpp +++ b/timing/timeIncremental.cpp @@ -14,15 +14,15 @@ * @author Richard Roberts */ -#include #include -#include #include #include #include +#include #include #include #include +#include #include #include @@ -39,20 +39,20 @@ typedef NoiseModelFactor1 NM1; typedef NoiseModelFactor2 NM2; typedef BearingRangeFactor BR; -BOOST_CLASS_EXPORT(Value); -BOOST_CLASS_EXPORT(Pose); -BOOST_CLASS_EXPORT(NonlinearFactor); -BOOST_CLASS_EXPORT(NoiseModelFactor); -BOOST_CLASS_EXPORT(NM1); -BOOST_CLASS_EXPORT(NM2); -BOOST_CLASS_EXPORT(BetweenFactor); -BOOST_CLASS_EXPORT(PriorFactor); -BOOST_CLASS_EXPORT(BR); -BOOST_CLASS_EXPORT(noiseModel::Base); -BOOST_CLASS_EXPORT(noiseModel::Isotropic); -BOOST_CLASS_EXPORT(noiseModel::Gaussian); -BOOST_CLASS_EXPORT(noiseModel::Diagonal); -BOOST_CLASS_EXPORT(noiseModel::Unit); +//GTSAM_VALUE_EXPORT(Value); +//GTSAM_VALUE_EXPORT(Pose); +//GTSAM_VALUE_EXPORT(NonlinearFactor); +//GTSAM_VALUE_EXPORT(NoiseModelFactor); +//GTSAM_VALUE_EXPORT(NM1); +//GTSAM_VALUE_EXPORT(NM2); +//GTSAM_VALUE_EXPORT(BetweenFactor); +//GTSAM_VALUE_EXPORT(PriorFactor); +//GTSAM_VALUE_EXPORT(BR); +//GTSAM_VALUE_EXPORT(noiseModel::Base); +//GTSAM_VALUE_EXPORT(noiseModel::Isotropic); +//GTSAM_VALUE_EXPORT(noiseModel::Gaussian); +//GTSAM_VALUE_EXPORT(noiseModel::Diagonal); +//GTSAM_VALUE_EXPORT(noiseModel::Unit); double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) { // Compute degrees of freedom (observations - variables) @@ -97,7 +97,7 @@ int main(int argc, char *argv[]) { // cout << "Initializing " << 0 << endl; newVariables.insert(0, Pose()); // Add prior - newFactors.add(PriorFactor(0, Pose(), noiseModel::Unit::Create(Pose::Dim()))); + newFactors.add(PriorFactor(0, Pose(), noiseModel::Unit::Create(3))); } while(nextMeasurement < measurements.size()) { diff --git a/timing/timeLago.cpp b/timing/timeLago.cpp index 931e2498f..3a4da89b5 100644 --- a/timing/timeLago.cpp +++ b/timing/timeLago.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include diff --git a/gtsam_unstable/timing/timeLinearize.h b/timing/timeLinearize.h similarity index 100% rename from gtsam_unstable/timing/timeLinearize.h rename to timing/timeLinearize.h diff --git a/gtsam_unstable/timing/timeOneCameraExpression.cpp b/timing/timeOneCameraExpression.cpp similarity index 92% rename from gtsam_unstable/timing/timeOneCameraExpression.cpp rename to timing/timeOneCameraExpression.cpp index 4cb655825..962e7ee21 100644 --- a/gtsam_unstable/timing/timeOneCameraExpression.cpp +++ b/timing/timeOneCameraExpression.cpp @@ -16,8 +16,8 @@ * @date October 3, 2014 */ -#include -#include +#include +#include #include "timeLinearize.h" using namespace std; diff --git a/timing/timePose2.cpp b/timing/timePose2.cpp index 06ab633c6..92aece7e5 100644 --- a/timing/timePose2.cpp +++ b/timing/timePose2.cpp @@ -32,7 +32,7 @@ using namespace gtsam; /* ************************************************************************* */ Pose2 Pose2betweenDefault(const Pose2& r1, const Pose2& r2) { - return between_default(r1, r2); + return r1.inverse() * r2; } /* ************************************************************************* */ diff --git a/timing/timeRot2.cpp b/timing/timeRot2.cpp index 6860914ef..26eed0207 100644 --- a/timing/timeRot2.cpp +++ b/timing/timeRot2.cpp @@ -15,10 +15,10 @@ * @author Richard Roberts */ -#include +#include #include -#include +#include using namespace std; using namespace gtsam; @@ -32,7 +32,7 @@ using namespace gtsam; /* ************************************************************************* */ Rot2 Rot2betweenDefault(const Rot2& r1, const Rot2& r2) { - return between_default(r1, r2); + return r1.inverse() * r2; } /* ************************************************************************* */ diff --git a/gtsam_unstable/timing/timeSFMExpressions.cpp b/timing/timeSFMExpressions.cpp similarity index 95% rename from gtsam_unstable/timing/timeSFMExpressions.cpp rename to timing/timeSFMExpressions.cpp index 678e4db60..cfb8e0dc6 100644 --- a/gtsam_unstable/timing/timeSFMExpressions.cpp +++ b/timing/timeSFMExpressions.cpp @@ -16,8 +16,8 @@ * @date October 3, 2014 */ -#include -#include +#include +#include #include #include diff --git a/timing/timeiSAM2Chain.cpp b/timing/timeiSAM2Chain.cpp index d15ccfaf4..c4196f414 100644 --- a/timing/timeiSAM2Chain.cpp +++ b/timing/timeiSAM2Chain.cpp @@ -39,7 +39,7 @@ typedef Pose2 Pose; typedef NoiseModelFactor1 NM1; typedef NoiseModelFactor2 NM2; -noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(Pose::Dim()); +noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(3); int main(int argc, char *argv[]) { @@ -61,10 +61,10 @@ int main(int argc, char *argv[]) { gttic_(Create_measurements); if(step == 0) { // Add prior - newFactors.add(PriorFactor(0, Pose(), noiseModel::Unit::Create(Pose::Dim()))); + newFactors.add(PriorFactor(0, Pose(), noiseModel::Unit::Create(3))); newVariables.insert(0, Pose()); } else { - Vector eta = Vector::Random(Pose::Dim()) * 0.1; + Vector eta = Vector::Random(3) * 0.1; Pose2 between = Pose().retract(eta); // Add between newFactors.add(BetweenFactor(step-1, step, between, model)); diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 4989afb0d..848998eb0 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -31,12 +32,13 @@ using namespace wrap; /* ************************************************************************* */ Argument Argument::expandTemplate(const TemplateSubstitution& ts) const { Argument instArg = *this; - instArg.type = ts(type); + instArg.type = ts.tryToSubstitite(type); return instArg; } /* ************************************************************************* */ -ArgumentList ArgumentList::expandTemplate(const TemplateSubstitution& ts) const { +ArgumentList ArgumentList::expandTemplate( + const TemplateSubstitution& ts) const { ArgumentList instArgList; BOOST_FOREACH(const Argument& arg, *this) { Argument instArg = arg.expandTemplate(ts); @@ -48,25 +50,25 @@ ArgumentList ArgumentList::expandTemplate(const TemplateSubstitution& ts) const /* ************************************************************************* */ string Argument::matlabClass(const string& delim) const { string result; - BOOST_FOREACH(const string& ns, type.namespaces) + BOOST_FOREACH(const string& ns, type.namespaces()) result += ns + delim; - if (type.name == "string" || type.name == "unsigned char" - || type.name == "char") + if (type.name() == "string" || type.name() == "unsigned char" + || type.name() == "char") return result + "char"; - if (type.name == "Vector" || type.name == "Matrix") + if (type.name() == "Vector" || type.name() == "Matrix") return result + "double"; - if (type.name == "int" || type.name == "size_t") + if (type.name() == "int" || type.name() == "size_t") return result + "numeric"; - if (type.name == "bool") + if (type.name() == "bool") return result + "logical"; - return result + type.name; + return result + type.name(); } /* ************************************************************************* */ bool Argument::isScalar() const { - return (type.name == "bool" || type.name == "char" - || type.name == "unsigned char" || type.name == "int" - || type.name == "size_t" || type.name == "double"); + return (type.name() == "bool" || type.name() == "char" + || type.name() == "unsigned char" || type.name() == "int" + || type.name() == "size_t" || type.name() == "double"); } /* ************************************************************************* */ @@ -76,11 +78,11 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { string cppType = type.qualifiedName("::"); string matlabUniqueType = type.qualifiedName(); - if (is_ptr) + if (is_ptr && type.category != Qualified::EIGEN) // A pointer: emit an "unwrap_shared_ptr" call which returns a pointer file.oss << "boost::shared_ptr<" << cppType << "> " << name << " = unwrap_shared_ptr< "; - else if (is_ref) + else if (is_ref && type.category != Qualified::EIGEN) // A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer file.oss << cppType << "& " << name << " = *unwrap_shared_ptr< "; else @@ -92,11 +94,18 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << cppType << " " << name << " = unwrap< "; file.oss << cppType << " >(" << matlabName; - if (is_ptr || is_ref) + if( (is_ptr || is_ref) && type.category != Qualified::EIGEN) file.oss << ", \"ptr_" << matlabUniqueType << "\""; file.oss << ");" << endl; } +/* ************************************************************************* */ +void Argument::proxy_check(FileWriter& proxyFile, const string& s) const { + proxyFile.oss << "isa(" << s << ",'" << matlabClass(".") << "')"; + if (type.name() == "Vector") + proxyFile.oss << " && size(" << s << ",2)==1"; +} + /* ************************************************************************* */ string ArgumentList::types() const { string str; @@ -104,7 +113,7 @@ string ArgumentList::types() const { BOOST_FOREACH(Argument arg, *this) { if (!first) str += ","; - str += arg.type.name; + str += arg.type.name(); first = false; } return str; @@ -116,14 +125,14 @@ string ArgumentList::signature() const { bool cap = false; BOOST_FOREACH(Argument arg, *this) { - BOOST_FOREACH(char ch, arg.type.name) + BOOST_FOREACH(char ch, arg.type.name()) if (isupper(ch)) { sig += ch; //If there is a capital letter, we don't want to read it below cap = true; } if (!cap) - sig += arg.type.name[0]; + sig += arg.type.name()[0]; //Reset to default cap = false; } @@ -170,25 +179,14 @@ void ArgumentList::emit_prototype(FileWriter& file, const string& name) const { BOOST_FOREACH(Argument arg, *this) { if (!first) file.oss << ", "; - file.oss << arg.type.name << " " << arg.name; + file.oss << arg.type.name() << " " << arg.name; first = false; } file.oss << ")"; } + /* ************************************************************************* */ -void ArgumentList::emit_call(FileWriter& proxyFile, - const ReturnValue& returnVal, const string& wrapperName, int id, - bool staticMethod) const { - returnVal.emit_matlab(proxyFile); - proxyFile.oss << wrapperName << "(" << id; - if (!staticMethod) - proxyFile.oss << ", this"; - proxyFile.oss << ", varargin{:});\n"; -} -/* ************************************************************************* */ -void ArgumentList::emit_conditional_call(FileWriter& proxyFile, - const ReturnValue& returnVal, const string& wrapperName, int id, - bool staticMethod) const { +void ArgumentList::proxy_check(FileWriter& proxyFile) const { // Check nr of arguments proxyFile.oss << "if length(varargin) == " << size(); if (size() > 0) @@ -198,15 +196,12 @@ void ArgumentList::emit_conditional_call(FileWriter& proxyFile, for (size_t i = 0; i < size(); i++) { if (!first) proxyFile.oss << " && "; - proxyFile.oss << "isa(varargin{" << i + 1 << "},'" - << (*this)[i].matlabClass(".") << "')"; + string s = "varargin{" + boost::lexical_cast(i + 1) + "}"; + (*this)[i].proxy_check(proxyFile, s); first = false; } proxyFile.oss << "\n"; - - // output call to C++ wrapper - proxyFile.oss << " "; - emit_call(proxyFile, returnVal, wrapperName, id, staticMethod); } + /* ************************************************************************* */ diff --git a/wrap/Argument.h b/wrap/Argument.h index 3d8d7288f..fd7e82061 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -28,13 +28,23 @@ namespace wrap { /// Argument class struct Argument { Qualified type; - bool is_const, is_ref, is_ptr; std::string name; + bool is_const, is_ref, is_ptr; Argument() : is_const(false), is_ref(false), is_ptr(false) { } + Argument(const Qualified& t, const std::string& n) : + type(t), name(n), is_const(false), is_ref(false), is_ptr(false) { + } + + bool operator==(const Argument& other) const { + return type == other.type && name == other.name + && is_const == other.is_const && is_ref == other.is_ref + && is_ptr == other.is_ptr; + } + Argument expandTemplate(const TemplateSubstitution& ts) const; /// return MATLAB class for use in isa(x,class) @@ -46,6 +56,12 @@ struct Argument { /// MATLAB code generation, MATLAB to C++ void matlab_unwrap(FileWriter& file, const std::string& matlabName) const; + /** + * emit checking argument to MATLAB proxy + * @param proxyFile output stream + */ + void proxy_check(FileWriter& proxyFile, const std::string& s) const; + friend std::ostream& operator<<(std::ostream& os, const Argument& arg) { os << (arg.is_const ? "const " : "") << arg.type << (arg.is_ptr ? "*" : "") << (arg.is_ref ? "&" : ""); @@ -88,26 +104,12 @@ struct ArgumentList: public std::vector { void emit_prototype(FileWriter& file, const std::string& name) const; /** - * emit emit MATLAB call to proxy + * emit checking arguments to MATLAB proxy * @param proxyFile output stream - * @param returnVal the return value - * @param wrapperName of method or function - * @param staticMethod flag to emit "this" in call */ - void emit_call(FileWriter& proxyFile, const ReturnValue& returnVal, - const std::string& wrapperName, int id, bool staticMethod = false) const; - - /** - * emit conditional MATLAB call to proxy (checking arguments first) - * @param proxyFile output stream - * @param returnVal the return value - * @param wrapperName of method or function - * @param staticMethod flag to emit "this" in call - */ - void emit_conditional_call(FileWriter& proxyFile, - const ReturnValue& returnVal, const std::string& wrapperName, int id, - bool staticMethod = false) const; + void proxy_check(FileWriter& proxyFile) const; + /// Output stream operator friend std::ostream& operator<<(std::ostream& os, const ArgumentList& argList) { os << "("; @@ -122,5 +124,88 @@ struct ArgumentList: public std::vector { }; -} // \namespace wrap +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct ArgumentGrammar: public classic::grammar { + + wrap::Argument& result_; ///< successful parse will be placed in here + TypeGrammar argument_type_g; ///< Type parser for Argument::type + + /// Construct type grammar and specify where result is placed + ArgumentGrammar(wrap::Argument& result) : + result_(result), argument_type_g(result.type) { + } + + /// Definition of type grammar + template + struct definition: BasicRules { + + typedef classic::rule Rule; + + Rule argument_p; + + definition(ArgumentGrammar const& self) { + using namespace classic; + + // NOTE: allows for pointers to all types + // Slightly more permissive than before on basis/eigen type qualification + // Also, currently parses Point2*&, can't make it work otherwise :-( + argument_p = !str_p("const")[assign_a(self.result_.is_const, T)] // + >> self.argument_type_g // + >> !ch_p('*')[assign_a(self.result_.is_ptr, T)] + >> !ch_p('&')[assign_a(self.result_.is_ref, T)] + >> BasicRules::name_p[assign_a(self.result_.name)]; + } + + Rule const& start() const { + return argument_p; + } + + }; +}; +// ArgumentGrammar + +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct ArgumentListGrammar: public classic::grammar { + + wrap::ArgumentList& result_; ///< successful parse will be placed in here + + /// Construct type grammar and specify where result is placed + ArgumentListGrammar(wrap::ArgumentList& result) : + result_(result) { + } + + /// Definition of type grammar + template + struct definition { + + const Argument arg0; ///< used to reset arg + Argument arg; ///< temporary argument for use during parsing + ArgumentGrammar argument_g; ///< single Argument parser + + classic::rule argument_p, argumentList_p; + + definition(ArgumentListGrammar const& self) : + argument_g(arg) { + using namespace classic; + + argument_p = argument_g // + [classic::push_back_a(self.result_, arg)] // + [assign_a(arg, arg0)]; + + argumentList_p = '(' >> !argument_p >> *(',' >> argument_p) >> ')'; + } + + classic::rule const& start() const { + return argumentList_p; + } + + }; +}; +// ArgumentListGrammar + +/* ************************************************************************* */ + +}// \namespace wrap diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 0e480f0fd..6e415065d 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -22,23 +22,68 @@ #include #include +#include +#include #include #include #include +#include // std::ostream_iterator //#include // on Linux GCC: fails with error regarding needing C++0x std flags //#include // same failure as above #include // works on Linux GCC using namespace std; using namespace wrap; +/* ************************************************************************* */ +void Class::assignParent(const Qualified& parent) { + parentClass.reset(parent); +} + +/* ************************************************************************* */ +boost::optional Class::qualifiedParent() const { + boost::optional result = boost::none; + if (parentClass) + result = parentClass->qualifiedName("::"); + return result; +} + +/* ************************************************************************* */ +static void handleException(const out_of_range& oor, + const Class::Methods& methods) { + cerr << "Class::method: key not found: " << oor.what() << ", methods are:\n"; + using boost::adaptors::map_keys; + ostream_iterator out_it(cerr, "\n"); + boost::copy(methods | map_keys, out_it); +} + +/* ************************************************************************* */ +Method& Class::mutableMethod(Str key) { + try { + return methods_.at(key); + } catch (const out_of_range& oor) { + handleException(oor, methods_); + throw runtime_error("Internal error in wrap"); + } +} + +/* ************************************************************************* */ +const Method& Class::method(Str key) const { + try { + return methods_.at(key); + } catch (const out_of_range& oor) { + handleException(oor, methods_); + throw runtime_error("Internal error in wrap"); + } +} + /* ************************************************************************* */ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, vector& functionNames) const { // Create namespace folders - createNamespaceStructure(namespaces, toolboxPath); + createNamespaceStructure(namespaces(), toolboxPath); // open destination classFile string classFile = matlabName(toolboxPath); @@ -48,21 +93,20 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, const string matlabQualName = qualifiedName("."); const string matlabUniqueName = qualifiedName(); const string cppName = qualifiedName("::"); - const string matlabBaseName = qualifiedParent.qualifiedName("."); - const string cppBaseName = qualifiedParent.qualifiedName("::"); // emit class proxy code // we want our class to inherit the handle class for memory purposes - const string parent = qualifiedParent.empty() ? "handle" : matlabBaseName; + const string parent = + parentClass ? parentClass->qualifiedName(".") : "handle"; comment_fragment(proxyFile); - proxyFile.oss << "classdef " << name << " < " << parent << endl; + proxyFile.oss << "classdef " << name() << " < " << parent << endl; proxyFile.oss << " properties\n"; proxyFile.oss << " ptr_" << matlabUniqueName << " = 0\n"; proxyFile.oss << " end\n"; proxyFile.oss << " methods\n"; // Constructor - proxyFile.oss << " function obj = " << name << "(varargin)\n"; + proxyFile.oss << " function obj = " << name() << "(varargin)\n"; // Special pointer constructors - one in MATLAB to create an object and // assign a pointer returned from a C++ function. In turn this MATLAB // constructor calls a special C++ function that just adds the object to @@ -75,11 +119,12 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, wrapperFile.oss << "\n"; // Regular constructors + boost::optional cppBaseName = qualifiedParent(); for (size_t i = 0; i < constructor.nrOverloads(); i++) { ArgumentList args = constructor.argumentList(i); const int id = (int) functionNames.size(); - constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(), - id, args); + constructor.proxy_fragment(proxyFile, wrapperName, (bool) parentClass, id, + args); const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile, cppName, matlabUniqueName, cppBaseName, id, args); wrapperFile.oss << "\n"; @@ -89,9 +134,9 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, proxyFile.oss << " error('Arguments do not match any overload of " << matlabQualName << " constructor');\n"; proxyFile.oss << " end\n"; - if (!qualifiedParent.empty()) - proxyFile.oss << " obj = obj@" << matlabBaseName << "(uint64(" - << ptr_constructor_key << "), base_ptr);\n"; + if (parentClass) + proxyFile.oss << " obj = obj@" << parentClass->qualifiedName(".") + << "(uint64(" << ptr_constructor_key << "), base_ptr);\n"; proxyFile.oss << " obj.ptr_" << matlabUniqueName << " = my_ptr;\n"; proxyFile.oss << " end\n\n"; @@ -111,7 +156,7 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, << " function disp(obj), obj.display; end\n %DISP Calls print on the object\n"; // Methods - BOOST_FOREACH(const Methods::value_type& name_m, methods) { + BOOST_FOREACH(const Methods::value_type& name_m, methods_) { const Method& m = name_m.second; m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames); @@ -151,7 +196,6 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, const string matlabUniqueName = qualifiedName(); const string cppName = qualifiedName("::"); - const string baseCppName = qualifiedParent.qualifiedName("::"); const int collectorInsertId = (int) functionNames.size(); const string collectorInsertFunctionName = matlabUniqueName @@ -188,7 +232,7 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, } else { proxyFile.oss << " my_ptr = varargin{2};\n"; } - if (qualifiedParent.empty()) // If this class has a base class, we'll get a base class pointer back + if (!parentClass) // If this class has a base class, we'll get a base class pointer back proxyFile.oss << " "; else proxyFile.oss << " base_ptr = "; @@ -211,9 +255,10 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, // Add to collector wrapperFile.oss << " collector_" << matlabUniqueName << ".insert(self);\n"; // If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy) - if (!qualifiedParent.empty()) { + boost::optional cppBaseName = qualifiedParent(); + if (cppBaseName) { wrapperFile.oss << "\n"; - wrapperFile.oss << " typedef boost::shared_ptr<" << baseCppName + wrapperFile.oss << " typedef boost::shared_ptr<" << *cppBaseName << "> SharedBase;\n"; wrapperFile.oss << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; @@ -244,10 +289,10 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, /* ************************************************************************* */ Class Class::expandTemplate(const TemplateSubstitution& ts) const { Class inst = *this; - inst.methods = expandMethodTemplate(methods, ts); + inst.methods_ = expandMethodTemplate(methods_, ts); inst.static_methods = expandMethodTemplate(static_methods, ts); inst.constructor = constructor.expandTemplate(ts); - inst.deconstructor.name = inst.name; + inst.deconstructor.name = inst.name(); return inst; } @@ -257,10 +302,10 @@ vector Class::expandTemplate(Str templateArg, vector result; BOOST_FOREACH(const Qualified& instName, instantiations) { Qualified expandedClass = (Qualified) (*this); - expandedClass.name += instName.name; + expandedClass.expand(instName.name()); const TemplateSubstitution ts(templateArg, instName, expandedClass); Class inst = expandTemplate(ts); - inst.name = expandedClass.name; + inst.name_ = expandedClass.name(); inst.templateArgs.clear(); inst.typedefName = qualifiedName("::") + "<" + instName.qualifiedName("::") + ">"; @@ -272,50 +317,50 @@ vector Class::expandTemplate(Str templateArg, /* ************************************************************************* */ void Class::addMethod(bool verbose, bool is_const, Str methodName, const ArgumentList& argumentList, const ReturnValue& returnValue, - Str templateArgName, const vector& templateArgValues) { + const Template& tmplate) { // Check if templated - if (!templateArgName.empty() && templateArgValues.size() > 0) { + if (tmplate.valid()) { // Create method to expand // For all values of the template argument, create a new method - BOOST_FOREACH(const Qualified& instName, templateArgValues) { - const TemplateSubstitution ts(templateArgName, instName, this->name); + BOOST_FOREACH(const Qualified& instName, tmplate.argValues()) { + const TemplateSubstitution ts(tmplate.argName(), instName, *this); // substitute template in arguments ArgumentList expandedArgs = argumentList.expandTemplate(ts); // do the same for return type ReturnValue expandedRetVal = returnValue.expandTemplate(ts); // Now stick in new overload stack with expandedMethodName key // but note we use the same, unexpanded methodName in overload - string expandedMethodName = methodName + instName.name; - methods[expandedMethodName].addOverload(methodName, expandedArgs, + string expandedMethodName = methodName + instName.name(); + methods_[expandedMethodName].addOverload(methodName, expandedArgs, expandedRetVal, is_const, instName, verbose); } } else // just add overload - methods[methodName].addOverload(methodName, argumentList, returnValue, - is_const, Qualified(), verbose); + methods_[methodName].addOverload(methodName, argumentList, returnValue, + is_const, boost::none, verbose); } /* ************************************************************************* */ void Class::erase_serialization() { - Methods::iterator it = methods.find("serializable"); - if (it != methods.end()) { + Methods::iterator it = methods_.find("serializable"); + if (it != methods_.end()) { #ifndef WRAP_DISABLE_SERIALIZE isSerializable = true; #else // cout << "Ignoring serializable() flag in class " << name << endl; #endif - methods.erase(it); + methods_.erase(it); } - it = methods.find("serialize"); - if (it != methods.end()) { + it = methods_.find("serialize"); + if (it != methods_.end()) { #ifndef WRAP_DISABLE_SERIALIZE isSerializable = true; hasSerialization = true; #else // cout << "Ignoring serialize() flag in class " << name << endl; #endif - methods.erase(it); + methods_.erase(it); } } @@ -327,31 +372,31 @@ void Class::verifyAll(vector& validTypes, bool& hasSerialiable) const { // verify all of the function arguments //TODO:verifyArguments(validTypes, constructor.args_list); verifyArguments(validTypes, static_methods); - verifyArguments(validTypes, methods); + verifyArguments(validTypes, methods_); // verify function return types verifyReturnTypes(validTypes, static_methods); - verifyReturnTypes(validTypes, methods); + verifyReturnTypes(validTypes, methods_); // verify parents - if (!qualifiedParent.empty() - && find(validTypes.begin(), validTypes.end(), - qualifiedParent.qualifiedName("::")) == validTypes.end()) - throw DependencyMissing(qualifiedParent.qualifiedName("::"), - qualifiedName("::")); + boost::optional parent = qualifiedParent(); + if (parent + && find(validTypes.begin(), validTypes.end(), *parent) + == validTypes.end()) + throw DependencyMissing(*parent, qualifiedName("::")); } /* ************************************************************************* */ void Class::appendInheritedMethods(const Class& cls, const vector& classes) { - if (!cls.qualifiedParent.empty()) { + if (cls.parentClass) { // Find parent BOOST_FOREACH(const Class& parent, classes) { // We found a parent class for our parent, TODO improve ! - if (parent.name == cls.qualifiedParent.name) { - methods.insert(parent.methods.begin(), parent.methods.end()); + if (parent.name() == cls.parentClass->name()) { + methods_.insert(parent.methods_.begin(), parent.methods_.end()); appendInheritedMethods(parent, classes); } } @@ -361,11 +406,11 @@ void Class::appendInheritedMethods(const Class& cls, /* ************************************************************************* */ string Class::getTypedef() const { string result; - BOOST_FOREACH(Str namesp, namespaces) { + BOOST_FOREACH(Str namesp, namespaces()) { result += ("namespace " + namesp + " { "); } - result += ("typedef " + typedefName + " " + name + ";"); - for (size_t i = 0; i < namespaces.size(); ++i) { + result += ("typedef " + typedefName + " " + name() + ";"); + for (size_t i = 0; i < namespaces().size(); ++i) { result += " }"; } return result; @@ -373,15 +418,15 @@ string Class::getTypedef() const { /* ************************************************************************* */ void Class::comment_fragment(FileWriter& proxyFile) const { - proxyFile.oss << "%class " << name << ", see Doxygen page for details\n"; + proxyFile.oss << "%class " << name() << ", see Doxygen page for details\n"; proxyFile.oss << "%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; constructor.comment_fragment(proxyFile); - if (!methods.empty()) + if (!methods_.empty()) proxyFile.oss << "%\n%-------Methods-------\n"; - BOOST_FOREACH(const Methods::value_type& name_m, methods) + BOOST_FOREACH(const Methods::value_type& name_m, methods_) name_m.second.comment_fragment(proxyFile); if (!static_methods.empty()) @@ -393,7 +438,7 @@ void Class::comment_fragment(FileWriter& proxyFile) const { proxyFile.oss << "%\n%-------Serialization Interface-------\n"; proxyFile.oss << "%string_serialize() : returns string\n"; proxyFile.oss << "%string_deserialize(string serialized) : returns " - << this->name << "\n"; + << name() << "\n"; } proxyFile.oss << "%\n"; @@ -586,12 +631,12 @@ string Class::getSerializationExport() const { /* ************************************************************************* */ void Class::python_wrapper(FileWriter& wrapperFile) const { - wrapperFile.oss << "class_<" << name << ">(\"" << name << "\")\n"; - constructor.python_wrapper(wrapperFile, name); + wrapperFile.oss << "class_<" << name() << ">(\"" << name() << "\")\n"; + constructor.python_wrapper(wrapperFile, name()); BOOST_FOREACH(const StaticMethod& m, static_methods | boost::adaptors::map_values) - m.python_wrapper(wrapperFile, name); - BOOST_FOREACH(const Method& m, methods | boost::adaptors::map_values) - m.python_wrapper(wrapperFile, name); + m.python_wrapper(wrapperFile, name()); + BOOST_FOREACH(const Method& m, methods_ | boost::adaptors::map_values) + m.python_wrapper(wrapperFile, name()); wrapperFile.oss << ";\n\n"; } diff --git a/wrap/Class.h b/wrap/Class.h index 34323b797..f4c687eca 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -19,14 +19,29 @@ #pragma once +#include "spirit.h" +#include "Template.h" #include "Constructor.h" #include "Deconstructor.h" #include "Method.h" #include "StaticMethod.h" #include "TypeAttributesTable.h" +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" +#endif +#include +#include +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif + +namespace bl = boost::lambda; + #include #include +#include #include #include @@ -36,13 +51,20 @@ namespace wrap { /// Class has name, constructors, methods class Class: public Qualified { +public: + typedef const std::string& Str; typedef std::map Methods; - Methods methods; ///< Class methods + typedef std::map StaticMethods; + +private: + + boost::optional parentClass; ///< The *single* parent + Methods methods_; ///< Class methods + Method& mutableMethod(Str key); public: - typedef const std::string& Str; - typedef std::map StaticMethods; + StaticMethods static_methods; ///< Static methods // Then the instance variables are set directly by the Module constructor std::vector templateArgs; ///< Template arguments @@ -50,26 +72,28 @@ public: bool isVirtual; ///< Whether the class is part of a virtual inheritance chain bool isSerializable; ///< Whether we can use boost.serialization to serialize the class - creates exports bool hasSerialization; ///< Whether we should create the serialization functions - Qualified qualifiedParent; ///< The *single* parent - StaticMethods static_methods; ///< Static methods Constructor constructor; ///< Class constructors Deconstructor deconstructor; ///< Deconstructor to deallocate C++ object bool verbose_; ///< verbose flag /// Constructor creates an empty class Class(bool verbose = true) : - isVirtual(false), isSerializable(false), hasSerialization(false), deconstructor( - verbose), verbose_(verbose) { + parentClass(boost::none), isVirtual(false), isSerializable(false), hasSerialization( + false), deconstructor(verbose), verbose_(verbose) { } + void assignParent(const Qualified& parent); + + boost::optional qualifiedParent() const; + size_t nrMethods() const { - return methods.size(); - } - Method& method(Str name) { - return methods.at(name); + return methods_.size(); } + + const Method& method(Str key) const; + bool exists(Str name) const { - return methods.find(name) != methods.end(); + return methods_.find(name) != methods_.end(); } // And finally MATLAB code is emitted, methods below called by Module::matlab_code @@ -85,7 +109,7 @@ public: /// Add potentially overloaded, potentially templated method void addMethod(bool verbose, bool is_const, Str methodName, const ArgumentList& argumentList, const ReturnValue& returnValue, - Str templateArgName, const std::vector& templateArgValues); + const Template& tmplate); /// Post-process classes for serialization markers void erase_serialization(); // non-const ! @@ -115,11 +139,11 @@ public: void python_wrapper(FileWriter& wrapperFile) const; friend std::ostream& operator<<(std::ostream& os, const Class& cls) { - os << "class " << cls.name << "{\n"; + os << "class " << cls.name() << "{\n"; os << cls.constructor << ";\n"; BOOST_FOREACH(const StaticMethod& m, cls.static_methods | boost::adaptors::map_values) os << m << ";\n"; - BOOST_FOREACH(const Method& m, cls.methods | boost::adaptors::map_values) + BOOST_FOREACH(const Method& m, cls.methods_ | boost::adaptors::map_values) os << m << ";\n"; os << "};" << std::endl; return os; @@ -134,5 +158,122 @@ private: void comment_fragment(FileWriter& proxyFile) const; }; -} // \namespace wrap +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct ClassGrammar: public classic::grammar { + + Class& cls_; ///< successful parse will be placed in here + Template& template_; ///< result needs to be visible outside + + /// Construct type grammar and specify where result is placed + ClassGrammar(Class& cls, Template& t) : + cls_(cls), template_(t) { + } + + /// Definition of type grammar + template + struct definition: BasicRules { + + using BasicRules::name_p; + using BasicRules::className_p; + using BasicRules::comments_p; + + // NOTE: allows for pointers to all types + ArgumentList args; + ArgumentListGrammar argumentList_g; + + Constructor constructor0, constructor; + + ReturnValue retVal0, retVal; + ReturnValueGrammar returnValue_g; + + Template methodTemplate; + TemplateGrammar methodTemplate_g, classTemplate_g; + + std::string methodName; + bool isConst, T, F; + + // Parent class + Qualified possibleParent; + TypeGrammar classParent_g; + + classic::rule constructor_p, methodName_p, method_p, + staticMethodName_p, static_method_p, templateList_p, classParent_p, + functions_p, class_p; + + definition(ClassGrammar const& self) : + argumentList_g(args), returnValue_g(retVal), // + methodTemplate_g(methodTemplate), classTemplate_g(self.template_), // + T(true), F(false), classParent_g(possibleParent) { + + using namespace classic; + bool verbose = false; // TODO + + // ConstructorGrammar + constructor_p = (className_p >> argumentList_g >> ';' >> !comments_p) // + [bl::bind(&Constructor::push_back, bl::var(constructor), + bl::var(args))] // + [clear_a(args)]; + + // MethodGrammar + methodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; + + // gtsam::Values retract(const gtsam::VectorValues& delta) const; + method_p = !methodTemplate_g + >> (returnValue_g >> methodName_p[assign_a(methodName)] + >> argumentList_g >> !str_p("const")[assign_a(isConst, T)] >> ';' + >> *comments_p) // + [bl::bind(&Class::addMethod, bl::var(self.cls_), verbose, + bl::var(isConst), bl::var(methodName), bl::var(args), + bl::var(retVal), bl::var(methodTemplate))] // + [assign_a(retVal, retVal0)][clear_a(args)] // + [clear_a(methodTemplate)][assign_a(isConst, F)]; + + // StaticMethodGrammar + staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; + + static_method_p = (str_p("static") >> returnValue_g + >> staticMethodName_p[assign_a(methodName)] >> argumentList_g >> ';' + >> *comments_p) // + [bl::bind(&StaticMethod::addOverload, + bl::var(self.cls_.static_methods)[bl::var(methodName)], + bl::var(methodName), bl::var(args), bl::var(retVal), boost::none, + verbose)] // + [assign_a(retVal, retVal0)][clear_a(args)]; + + // template + templateList_p = (str_p("template") >> '<' + >> name_p[push_back_a(self.cls_.templateArgs)] + >> *(',' >> name_p[push_back_a(self.cls_.templateArgs)]) >> '>'); + + // parse a full class + classParent_p = (':' >> classParent_g >> '{') // + [bl::bind(&Class::assignParent, bl::var(self.cls_), + bl::var(possibleParent))][clear_a(possibleParent)]; + + functions_p = constructor_p | method_p | static_method_p; + + // parse a full class + class_p = (!(classTemplate_g[push_back_a(self.cls_.templateArgs, + self.template_.argName())] | templateList_p) + >> !(str_p("virtual")[assign_a(self.cls_.isVirtual, T)]) + >> str_p("class") >> className_p[assign_a(self.cls_.name_)] + >> (classParent_p | '{') >> // + *(functions_p | comments_p) >> str_p("};")) // + [bl::bind(&Constructor::initializeOrCheck, bl::var(constructor), + bl::var(self.cls_.name_), boost::none, verbose)][assign_a( + self.cls_.constructor, constructor)] // + [assign_a(self.cls_.deconstructor.name, self.cls_.name_)] // + [assign_a(constructor, constructor0)]; + } + + classic::rule const& start() const { + return class_p; + } + + }; +}; +// ClassGrammar + +}// \namespace wrap diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index 782c0ca80..35cc0fa53 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -69,7 +69,7 @@ void Constructor::proxy_fragment(FileWriter& file, /* ************************************************************************* */ string Constructor::wrapper_fragment(FileWriter& file, Str cppClassName, - Str matlabUniqueName, Str cppBaseClassName, int id, + Str matlabUniqueName, boost::optional cppBaseClassName, int id, const ArgumentList& al) const { const string wrapFunctionName = matlabUniqueName + "_constructor_" @@ -100,9 +100,9 @@ string Constructor::wrapper_fragment(FileWriter& file, Str cppClassName, << endl; // If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy) - if (!cppBaseClassName.empty()) { + if (cppBaseClassName) { file.oss << "\n"; - file.oss << " typedef boost::shared_ptr<" << cppBaseClassName + file.oss << " typedef boost::shared_ptr<" << *cppBaseClassName << "> SharedBase;\n"; file.oss << " out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; diff --git a/wrap/Constructor.h b/wrap/Constructor.h index a026bf423..1e061d17c 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -19,7 +19,7 @@ #pragma once #include "OverloadedFunction.h" - +#include #include #include @@ -68,7 +68,7 @@ struct Constructor: public OverloadedFunction { /// cpp wrapper std::string wrapper_fragment(FileWriter& file, Str cppClassName, - Str matlabUniqueName, Str cppBaseClassName, int id, + Str matlabUniqueName, boost::optional cppBaseClassName, int id, const ArgumentList& al) const; /// constructor function diff --git a/wrap/ForwardDeclaration.h b/wrap/ForwardDeclaration.h index 5506dd778..5ec022ca4 100644 --- a/wrap/ForwardDeclaration.h +++ b/wrap/ForwardDeclaration.h @@ -27,6 +27,7 @@ namespace wrap { std::string name; bool isVirtual; ForwardDeclaration() : isVirtual(false) {} + ForwardDeclaration(const std::string& s) : name(s), isVirtual(false) {} }; -} \ No newline at end of file +} diff --git a/wrap/FullyOverloadedFunction.h b/wrap/FullyOverloadedFunction.h index ac22ec3a8..30e27764b 100644 --- a/wrap/FullyOverloadedFunction.h +++ b/wrap/FullyOverloadedFunction.h @@ -109,8 +109,8 @@ class FullyOverloadedFunction: public Function, public SignatureOverloads { public: bool addOverload(const std::string& name, const ArgumentList& args, - const ReturnValue& retVal, const Qualified& instName = Qualified(), - bool verbose = false) { + const ReturnValue& retVal, boost::optional instName = + boost::none, bool verbose = false) { bool first = initializeOrCheck(name, instName, verbose); SignatureOverloads::push_back(args, retVal); return first; diff --git a/wrap/Function.cpp b/wrap/Function.cpp index 6faa70fb9..7e4ded040 100644 --- a/wrap/Function.cpp +++ b/wrap/Function.cpp @@ -29,12 +29,11 @@ using namespace std; using namespace wrap; /* ************************************************************************* */ -bool Function::initializeOrCheck(const std::string& name, - const Qualified& instName, bool verbose) { +bool Function::initializeOrCheck(const string& name, + boost::optional instName, bool verbose) { if (name.empty()) - throw std::runtime_error( - "Function::initializeOrCheck called with empty name"); + throw runtime_error("Function::initializeOrCheck called with empty name"); // Check if this overload is give to the correct method if (name_.empty()) { @@ -43,15 +42,38 @@ bool Function::initializeOrCheck(const std::string& name, verbose_ = verbose; return true; } else { - if (name_ != name || templateArgValue_ != instName || verbose_ != verbose) - throw std::runtime_error( - "Function::initializeOrCheck called with different arguments: with name " - + name + " instead of expected " + name_ - + ", or with template argument " + instName.qualifiedName(":") - + " instead of expected " + templateArgValue_.qualifiedName(":")); + if (name_ != name || verbose_ != verbose + || ((bool) templateArgValue_ != (bool) instName) + || ((bool) templateArgValue_ && (bool) instName + && !(*templateArgValue_ == *instName))) + throw runtime_error( + "Function::initializeOrCheck called with different arguments"); return false; } } /* ************************************************************************* */ +void Function::emit_call(FileWriter& proxyFile, const ReturnValue& returnVal, + const string& wrapperName, int id) const { + returnVal.emit_matlab(proxyFile); + proxyFile.oss << wrapperName << "(" << id; + if (!isStatic()) + proxyFile.oss << ", this"; + proxyFile.oss << ", varargin{:});\n"; +} + +/* ************************************************************************* */ +void Function::emit_conditional_call(FileWriter& proxyFile, + const ReturnValue& returnVal, const ArgumentList& args, + const string& wrapperName, int id) const { + + // Check all arguments + args.proxy_check(proxyFile); + + // output call to C++ wrapper + proxyFile.oss << " "; + emit_call(proxyFile, returnVal, wrapperName, id); +} + +/* ************************************************************************* */ diff --git a/wrap/Function.h b/wrap/Function.h index 49a26bd8d..ad57a28c8 100644 --- a/wrap/Function.h +++ b/wrap/Function.h @@ -19,6 +19,7 @@ #pragma once #include "Argument.h" +#include namespace wrap { @@ -28,7 +29,7 @@ class Function { protected: std::string name_; ///< name of method - Qualified templateArgValue_; ///< value of template argument if applicable + boost::optional templateArgValue_; ///< value of template argument if applicable bool verbose_; public: @@ -37,19 +38,35 @@ public: * @brief first time, fill in instance variables, otherwise check if same * @return true if first time, false thereafter */ - bool initializeOrCheck(const std::string& name, const Qualified& instName = - Qualified(), bool verbose = false); + bool initializeOrCheck(const std::string& name, + boost::optional instName = boost::none, bool verbose = + false); std::string name() const { return name_; } - std::string matlabName() const { - if (templateArgValue_.empty()) - return name_; - else - return name_ + templateArgValue_.name; + /// Only Methods are non-static + virtual bool isStatic() const { + return true; } + + std::string matlabName() const { + if (templateArgValue_) + return name_ + templateArgValue_->name(); + else + return name_; + } + + /// Emit function call to MATLAB (no argument checking) + void emit_call(FileWriter& proxyFile, const ReturnValue& returnVal, + const std::string& wrapperName, int id) const; + + /// Emit checking arguments and function call to MATLAB + void emit_conditional_call(FileWriter& proxyFile, + const ReturnValue& returnVal, const ArgumentList& args, + const std::string& wrapperName, int id) const; + }; } // \namespace wrap diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index e843481a1..013ef6d28 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -18,9 +18,9 @@ using namespace std; /* ************************************************************************* */ void GlobalFunction::addOverload(const Qualified& overload, const ArgumentList& args, const ReturnValue& retVal, - const Qualified& instName, bool verbose) { - string name(overload.name); - FullyOverloadedFunction::addOverload(name, args, retVal, instName, verbose); + boost::optional instName, bool verbose) { + FullyOverloadedFunction::addOverload(overload.name(), args, retVal, instName, + verbose); overloads.push_back(overload); } @@ -37,7 +37,7 @@ void GlobalFunction::matlab_proxy(const string& toolboxPath, for (size_t i = 0; i < overloads.size(); ++i) { Qualified overload = overloads.at(i); // use concatenated namespaces as key - string str_ns = qualifiedName("", overload.namespaces); + string str_ns = qualifiedName("", overload.namespaces()); const ReturnValue& ret = returnValue(i); const ArgumentList& args = argumentList(i); grouped_functions[str_ns].addOverload(overload, args, ret); @@ -59,7 +59,7 @@ void GlobalFunction::generateSingleFunction(const string& toolboxPath, // create the folder for the namespace const Qualified& overload1 = overloads.front(); - createNamespaceStructure(overload1.namespaces, toolboxPath); + createNamespaceStructure(overload1.namespaces(), toolboxPath); // open destination mfunctionFileName string mfunctionFileName = overload1.matlabName(toolboxPath); @@ -80,7 +80,7 @@ void GlobalFunction::generateSingleFunction(const string& toolboxPath, // Output proxy matlab code mfunctionFile.oss << " " << (i == 0 ? "" : "else"); - args.emit_conditional_call(mfunctionFile, returnVal, wrapperName, id, true); // true omits "this" + emit_conditional_call(mfunctionFile, returnVal, args, wrapperName, id); // Output C++ wrapper code @@ -104,7 +104,7 @@ void GlobalFunction::generateSingleFunction(const string& toolboxPath, args.matlab_unwrap(file, 0); // We start at 0 because there is no self object // call method with default type and wrap result - if (returnVal.type1.name != "void") + if (returnVal.type1.name() != "void") returnVal.wrap_result(cppName + "(" + args.names() + ")", file, typeAttributes); else diff --git a/wrap/GlobalFunction.h b/wrap/GlobalFunction.h index 6a49fe5ce..b2a582654 100644 --- a/wrap/GlobalFunction.h +++ b/wrap/GlobalFunction.h @@ -11,6 +11,18 @@ #include "FullyOverloadedFunction.h" +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" +#endif +#include +#include +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif + +namespace bl = boost::lambda; + namespace wrap { struct GlobalFunction: public FullyOverloadedFunction { @@ -19,8 +31,8 @@ struct GlobalFunction: public FullyOverloadedFunction { // adds an overloaded version of this function, void addOverload(const Qualified& overload, const ArgumentList& args, - const ReturnValue& retVal, const Qualified& instName = Qualified(), - bool verbose = false); + const ReturnValue& retVal, boost::optional instName = + boost::none, bool verbose = false); void verifyArguments(const std::vector& validArgs) const { SignatureOverloads::verifyArguments(validArgs, name_); @@ -47,5 +59,67 @@ private: }; -} // \namespace wrap +typedef std::map GlobalFunctions; + +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct GlobalFunctionGrammar: public classic::grammar { + + GlobalFunctions& global_functions_; ///< successful parse will be placed in here + std::vector& namespaces_; + + /// Construct type grammar and specify where result is placed + GlobalFunctionGrammar(GlobalFunctions& global_functions, + std::vector& namespaces) : + global_functions_(global_functions), namespaces_(namespaces) { + } + + /// Definition of type grammar + template + struct definition: BasicRules { + +// using BasicRules::name_p; +// using BasicRules::className_p; + using BasicRules::comments_p; + + ArgumentList args; + ArgumentListGrammar argumentList_g; + + ReturnValue retVal0, retVal; + ReturnValueGrammar returnValue_g; + + Qualified globalFunction; + + classic::rule globalFunctionName_p, global_function_p; + + definition(GlobalFunctionGrammar const& self) : + argumentList_g(args), returnValue_g(retVal) { + + using namespace classic; + bool verbose = false; // TODO + + globalFunctionName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; + + // parse a global function + global_function_p = (returnValue_g + >> globalFunctionName_p[assign_a(globalFunction.name_)] + >> argumentList_g >> ';' >> *comments_p) // + [assign_a(globalFunction.namespaces_, self.namespaces_)][bl::bind( + &GlobalFunction::addOverload, + bl::var(self.global_functions_)[bl::var(globalFunction.name_)], + bl::var(globalFunction), bl::var(args), bl::var(retVal), + boost::none, verbose)] // + [assign_a(retVal, retVal0)][clear_a(globalFunction)][clear_a(args)]; + + } + + classic::rule const& start() const { + return global_function_p; + } + + }; +}; +// GlobalFunctionGrammar + +}// \namespace wrap diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 49d90378d..2ad6e8259 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -30,9 +30,9 @@ using namespace wrap; /* ************************************************************************* */ bool Method::addOverload(Str name, const ArgumentList& args, - const ReturnValue& retVal, bool is_const, const Qualified& instName, - bool verbose) { - bool first = StaticMethod::addOverload(name, args, retVal, instName, verbose); + const ReturnValue& retVal, bool is_const, + boost::optional instName, bool verbose) { + bool first = MethodBase::addOverload(name, args, retVal, instName, verbose); if (first) is_const_ = is_const; else if (is_const && !is_const_) @@ -52,14 +52,12 @@ void Method::proxy_header(FileWriter& proxyFile) const { /* ************************************************************************* */ string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, const ArgumentList& args, - const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes, - const Qualified& instName) const { + Str matlabUniqueName, const ArgumentList& args) const { // check arguments // extra argument obj -> nargin-1 is passed ! // example: checkArguments("equals",nargout,nargin-1,2); - wrapperFile.oss << " checkArguments(\"" << name_ << "\",nargout,nargin-1," - << args.size() << ");\n"; + wrapperFile.oss << " checkArguments(\"" << matlabName() + << "\",nargout,nargin-1," << args.size() << ");\n"; // get class pointer // example: shared_ptr = unwrap_shared_ptr< Test >(in[0], "Test"); @@ -72,8 +70,8 @@ string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName, // call method and wrap result // example: out[0]=wrap(obj->return_field(t)); string expanded = "obj->" + name_; - if (!instName.empty()) - expanded += ("<" + instName.qualifiedName("::") + ">"); + if (templateArgValue_) + expanded += ("<" + templateArgValue_->qualifiedName("::") + ">"); return expanded; } diff --git a/wrap/Method.h b/wrap/Method.h index db9e6bb9f..33ff7072e 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -18,12 +18,12 @@ #pragma once -#include "StaticMethod.h" +#include "MethodBase.h" namespace wrap { /// Method class -class Method: public StaticMethod { +class Method: public MethodBase { bool is_const_; @@ -32,8 +32,9 @@ public: typedef const std::string& Str; bool addOverload(Str name, const ArgumentList& args, - const ReturnValue& retVal, bool is_const, const Qualified& instName = - Qualified(), bool verbose = false); + const ReturnValue& retVal, bool is_const, + boost::optional instName = boost::none, bool verbose = + false); virtual bool isStatic() const { return false; @@ -55,9 +56,7 @@ private: void proxy_header(FileWriter& proxyFile) const; virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, const ArgumentList& args, - const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes, - const Qualified& instName) const; + Str matlabUniqueName, const ArgumentList& args) const; }; } // \namespace wrap diff --git a/wrap/MethodBase.cpp b/wrap/MethodBase.cpp new file mode 100644 index 000000000..9b5f7135f --- /dev/null +++ b/wrap/MethodBase.cpp @@ -0,0 +1,141 @@ +/* ---------------------------------------------------------------------------- + + * 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 MethodBase.ccp + * @author Frank Dellaert + * @author Andrew Melim + * @author Richard Roberts + **/ + +#include "Method.h" +#include "utilities.h" + +#include +#include +#include + +#include +#include + +using namespace std; +using namespace wrap; + +/* ************************************************************************* */ +void MethodBase::proxy_wrapper_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, Str cppClassName, Str matlabQualName, + Str matlabUniqueName, Str wrapperName, + const TypeAttributesTable& typeAttributes, + vector& functionNames) const { + + // emit header, e.g., function varargout = templatedMethod(this, varargin) + proxy_header(proxyFile); + + // Emit comments for documentation + string up_name = boost::to_upper_copy(matlabName()); + proxyFile.oss << " % " << up_name << " usage: "; + usage_fragment(proxyFile, matlabName()); + + // Emit URL to Doxygen page + proxyFile.oss << " % " + << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" + << endl; + + // Handle special case of single overload with all numeric arguments + if (nrOverloads() == 1 && argumentList(0).allScalar()) { + // Output proxy matlab code + // TODO: document why is it OK to not check arguments in this case + proxyFile.oss << " "; + const int id = (int) functionNames.size(); + emit_call(proxyFile, returnValue(0), wrapperName, id); + + // Output C++ wrapper code + const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName, + matlabUniqueName, 0, id, typeAttributes); + + // Add to function list + functionNames.push_back(wrapFunctionName); + } else { + // Check arguments for all overloads + for (size_t i = 0; i < nrOverloads(); ++i) { + + // Output proxy matlab code + proxyFile.oss << " " << (i == 0 ? "" : "else"); + const int id = (int) functionNames.size(); + emit_conditional_call(proxyFile, returnValue(i), argumentList(i), + wrapperName, id); + + // Output C++ wrapper code + const string wrapFunctionName = wrapper_fragment(wrapperFile, + cppClassName, matlabUniqueName, i, id, typeAttributes); + + // Add to function list + functionNames.push_back(wrapFunctionName); + } + proxyFile.oss << " else\n"; + proxyFile.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << "." << name_ << "');" << endl; + proxyFile.oss << " end\n"; + } + + proxyFile.oss << " end\n"; +} + +/* ************************************************************************* */ +string MethodBase::wrapper_fragment(FileWriter& wrapperFile, Str cppClassName, + Str matlabUniqueName, int overload, int id, + const TypeAttributesTable& typeAttributes) const { + + // generate code + + const string wrapFunctionName = matlabUniqueName + "_" + name_ + "_" + + boost::lexical_cast(id); + + const ArgumentList& args = argumentList(overload); + const ReturnValue& returnVal = returnValue(overload); + + // call + wrapperFile.oss << "void " << wrapFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + // start + wrapperFile.oss << "{\n"; + + returnVal.wrapTypeUnwrap(wrapperFile); + + wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName + << "> Shared;" << endl; + + // get call + // for static methods: cppClassName::staticMethod + // for instance methods: obj->instanceMethod + string expanded = wrapper_call(wrapperFile, cppClassName, matlabUniqueName, + args); + + expanded += ("(" + args.names() + ")"); + if (returnVal.type1.name() != "void") + returnVal.wrap_result(expanded, wrapperFile, typeAttributes); + else + wrapperFile.oss << " " + expanded + ";\n"; + + // finish + wrapperFile.oss << "}\n"; + + return wrapFunctionName; +} + +/* ************************************************************************* */ +void MethodBase::python_wrapper(FileWriter& wrapperFile, Str className) const { + wrapperFile.oss << " .def(\"" << name_ << "\", &" << className << "::" + << name_ << ");\n"; +} + +/* ************************************************************************* */ diff --git a/wrap/MethodBase.h b/wrap/MethodBase.h new file mode 100644 index 000000000..903b89569 --- /dev/null +++ b/wrap/MethodBase.h @@ -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 + + * -------------------------------------------------------------------------- */ + +/** + * @file MethodBase.h + * @brief describes and generates code for static methods + * @author Frank Dellaert + * @author Alex Cunningham + * @author Richard Roberts + **/ + +#pragma once + +#include "FullyOverloadedFunction.h" + +namespace wrap { + +/// MethodBase class +struct MethodBase: public FullyOverloadedFunction { + + typedef const std::string& Str; + + // emit a list of comments, one for each overload + void comment_fragment(FileWriter& proxyFile) const { + SignatureOverloads::comment_fragment(proxyFile, matlabName()); + } + + void verifyArguments(const std::vector& validArgs) const { + SignatureOverloads::verifyArguments(validArgs, name_); + } + + void verifyReturnTypes(const std::vector& validtypes) const { + SignatureOverloads::verifyReturnTypes(validtypes, name_); + } + + // MATLAB code generation + // classPath is class directory, e.g., ../matlab/@Point2 + void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, + Str cppClassName, Str matlabQualName, Str matlabUniqueName, + Str wrapperName, const TypeAttributesTable& typeAttributes, + std::vector& functionNames) const; + + // emit python wrapper + void python_wrapper(FileWriter& wrapperFile, Str className) const; + +protected: + + virtual void proxy_header(FileWriter& proxyFile) const = 0; + + std::string wrapper_fragment(FileWriter& wrapperFile, Str cppClassName, + Str matlabUniqueName, int overload, int id, + const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper + + virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName, + Str matlabUniqueName, const ArgumentList& args) const = 0; +}; + +} // \namespace wrap + diff --git a/wrap/Module.cpp b/wrap/Module.cpp index ac0e0a579..092c732f7 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -22,22 +22,6 @@ #include "TypeAttributesTable.h" #include "utilities.h" -//#define BOOST_SPIRIT_DEBUG -#include "spirit_actors.h" - -#include -#include -#include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif -#include -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif -#include #include #include #include @@ -51,8 +35,6 @@ using namespace BOOST_SPIRIT_CLASSIC_NS; namespace bl = boost::lambda; namespace fs = boost::filesystem; -typedef rule Rule; - /* ************************************************************************* */ // We parse an interface file into a Module object. // The grammar is defined using the boost/spirit combinatorial parser. @@ -102,7 +84,6 @@ Module::Module(const string& interfacePath, void Module::parseMarkup(const std::string& data) { // The parse imperatively :-( updates variables gradually during parse // The one with postfix 0 are used to reset the variables after parse. - vector namespaces; // current namespace tag //---------------------------------------------------------------------------- // Grammar with actions that build the Class object. Actions are @@ -113,211 +94,40 @@ void Module::parseMarkup(const std::string& data) { // http://www.boost.org/doc/libs/1_37_0/libs/spirit/classic/doc/directives.html // ---------------------------------------------------------------------------- - Rule comments_p = comment_p("/*", "*/") | comment_p("//", eol_p); - - Rule basisType_p = - (str_p("string") | "bool" | "size_t" | "int" | "double" | "char" | "unsigned char"); - - Rule keywords_p = - (str_p("const") | "static" | "namespace" | "void" | basisType_p); - - Rule eigenType_p = - (str_p("Vector") | "Matrix"); + // Define Rule and instantiate basic rules + typedef rule Rule; + BasicRules basic; - //Rule for STL Containers (class names are lowercase) - Rule stlType_p = (str_p("vector") | "list"); - - Rule className_p = (lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p - keywords_p) | stlType_p; - - Rule namespace_name_p = lexeme_d[lower_p >> *(alnum_p | '_')] - keywords_p; - - Argument arg0, arg; - Rule namespace_arg_p = namespace_name_p - [push_back_a(arg.type.namespaces)] >> str_p("::"); - - Rule argEigenType_p = - eigenType_p[assign_a(arg.type.name)]; - - Rule eigenRef_p = - !str_p("const") [assign_a(arg.is_const,true)] >> - eigenType_p [assign_a(arg.type.name)] >> - ch_p('&') [assign_a(arg.is_ref,true)]; - - Rule classArg_p = - !str_p("const") [assign_a(arg.is_const,true)] >> - *namespace_arg_p >> - className_p[assign_a(arg.type.name)] >> - !ch_p('&')[assign_a(arg.is_ref,true)]; - - Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')]; + vector namespaces; // current namespace tag - // TODO, do we really need cls here? Non-local + // parse a full class Class cls0(verbose),cls(verbose); - Rule classParent_p = - *(namespace_name_p[push_back_a(cls.qualifiedParent.namespaces)] >> str_p("::")) >> - className_p[assign_a(cls.qualifiedParent.name)]; - - // parse "gtsam::Pose2" and add to templateArgValues - Qualified templateArgValue; - vector templateArgValues; - Rule templateArgValue_p = - (*(namespace_name_p[push_back_a(templateArgValue.namespaces)] >> str_p("::")) >> - className_p[assign_a(templateArgValue.name)]) - [push_back_a(templateArgValues, templateArgValue)] - [clear_a(templateArgValue)]; - - // template - string templateArgName; - Rule templateArgValues_p = - (str_p("template") >> - '<' >> name_p[assign_a(templateArgName)] >> '=' >> - '{' >> !(templateArgValue_p >> *(',' >> templateArgValue_p)) >> '}' >> - '>'); - + Template classTemplate; + ClassGrammar class_g(cls,classTemplate); + Rule class_p = class_g // + [assign_a(cls.namespaces_, namespaces)] + [bl::bind(&handle_possible_template, bl::var(classes), bl::var(cls), + bl::var(classTemplate.argValues()))] + [clear_a(classTemplate)] // + [assign_a(cls,cls0)]; + // parse "gtsam::Pose2" and add to singleInstantiation.typeList - TemplateInstantiationTypedef singleInstantiation; - Rule templateSingleInstantiationArg_p = - (*(namespace_name_p[push_back_a(templateArgValue.namespaces)] >> str_p("::")) >> - className_p[assign_a(templateArgValue.name)]) - [push_back_a(singleInstantiation.typeList, templateArgValue)] - [clear_a(templateArgValue)]; + TemplateInstantiationTypedef singleInstantiation, singleInstantiation0; + TypeListGrammar<'<','>'> typelist_g(singleInstantiation.typeList); // typedef gtsam::RangeFactor RangeFactorPosePoint2; - TemplateInstantiationTypedef singleInstantiation0; + TypeGrammar instantiationClass_g(singleInstantiation.class_); Rule templateSingleInstantiation_p = - (str_p("typedef") >> - *(namespace_name_p[push_back_a(singleInstantiation.class_.namespaces)] >> str_p("::")) >> - className_p[assign_a(singleInstantiation.class_.name)] >> - '<' >> templateSingleInstantiationArg_p >> *(',' >> templateSingleInstantiationArg_p) >> - '>' >> - className_p[assign_a(singleInstantiation.name)] >> + (str_p("typedef") >> instantiationClass_g >> + typelist_g >> + basic.className_p[assign_a(singleInstantiation.name_)] >> ';') - [assign_a(singleInstantiation.namespaces, namespaces)] + [assign_a(singleInstantiation.namespaces_, namespaces)] [push_back_a(templateInstantiationTypedefs, singleInstantiation)] [assign_a(singleInstantiation, singleInstantiation0)]; - // template - Rule templateList_p = - (str_p("template") >> - '<' >> name_p[push_back_a(cls.templateArgs)] >> *(',' >> name_p[push_back_a(cls.templateArgs)]) >> - '>'); - - // NOTE: allows for pointers to all types - ArgumentList args; - Rule argument_p = - ((basisType_p[assign_a(arg.type.name)] | argEigenType_p | eigenRef_p | classArg_p) - >> !ch_p('*')[assign_a(arg.is_ptr,true)] - >> name_p[assign_a(arg.name)]) - [push_back_a(args, arg)] - [assign_a(arg,arg0)]; - - Rule argumentList_p = !argument_p >> * (',' >> argument_p); - - // parse class constructor - Constructor constructor0(verbose), constructor(verbose); - Rule constructor_p = - (className_p >> '(' >> argumentList_p >> ')' >> ';' >> !comments_p) - [bl::bind(&Constructor::push_back, bl::var(constructor), bl::var(args))] - [clear_a(args)]; - - vector namespaces_return; /// namespace for current return type - Rule namespace_ret_p = namespace_name_p[push_back_a(namespaces_return)] >> str_p("::"); - - // HACK: use const values instead of using enums themselves - somehow this doesn't result in values getting assigned to gibberish - static const ReturnType::return_category RETURN_EIGEN = ReturnType::EIGEN; - static const ReturnType::return_category RETURN_BASIS = ReturnType::BASIS; - static const ReturnType::return_category RETURN_CLASS = ReturnType::CLASS; - static const ReturnType::return_category RETURN_VOID = ReturnType::VOID; - - ReturnType retType0, retType; - Rule returnType_p = - (basisType_p[assign_a(retType.name)][assign_a(retType.category, RETURN_BASIS)]) | - ((*namespace_ret_p)[assign_a(retType.namespaces, namespaces_return)][clear_a(namespaces_return)] - >> (className_p[assign_a(retType.name)][assign_a(retType.category, RETURN_CLASS)]) >> - !ch_p('*')[assign_a(retType.isPtr,true)]) | - (eigenType_p[assign_a(retType.name)][assign_a(retType.category, RETURN_EIGEN)]); - - ReturnValue retVal0, retVal; - Rule returnType1_p = returnType_p[assign_a(retVal.type1,retType)][assign_a(retType,retType0)]; - Rule returnType2_p = returnType_p[assign_a(retVal.type2,retType)][assign_a(retType,retType0)]; - - Rule pair_p = - (str_p("pair") >> '<' >> returnType1_p >> ',' >> returnType2_p >> '>') - [assign_a(retVal.isPair,true)]; - - Rule void_p = str_p("void")[assign_a(retVal.type1.name)][assign_a(retVal.type1.category, RETURN_VOID)]; - - Rule returnValue_p = void_p | pair_p | returnType1_p; - - Rule methodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; - - // gtsam::Values retract(const gtsam::VectorValues& delta) const; - string methodName; - bool isConst, isConst0 = false; - Rule method_p = - !templateArgValues_p >> - (returnValue_p >> methodName_p[assign_a(methodName)] >> - '(' >> argumentList_p >> ')' >> - !str_p("const")[assign_a(isConst,true)] >> ';' >> *comments_p) - [bl::bind(&Class::addMethod, bl::var(cls), verbose, bl::var(isConst), - bl::var(methodName), bl::var(args), bl::var(retVal), - bl::var(templateArgName), bl::var(templateArgValues))] - [assign_a(retVal,retVal0)] - [clear_a(args)] - [clear_a(templateArgValues)] - [assign_a(isConst,isConst0)]; - - Rule staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; - - Rule static_method_p = - (str_p("static") >> returnValue_p >> staticMethodName_p[assign_a(methodName)] >> - '(' >> argumentList_p >> ')' >> ';' >> *comments_p) - [bl::bind(&StaticMethod::addOverload, - bl::var(cls.static_methods)[bl::var(methodName)], - bl::var(methodName), bl::var(args), bl::var(retVal), Qualified(),verbose)] - [assign_a(retVal,retVal0)] - [clear_a(args)]; - - Rule functions_p = constructor_p | method_p | static_method_p; - - // parse a full class - vector templateInstantiations; - Rule class_p = - eps_p[assign_a(cls,cls0)] - >> (!(templateArgValues_p - [push_back_a(cls.templateArgs, templateArgName)] - [assign_a(templateInstantiations,templateArgValues)] - [clear_a(templateArgValues)] - | templateList_p) - >> !(str_p("virtual")[assign_a(cls.isVirtual, true)]) - >> str_p("class") - >> className_p[assign_a(cls.name)] - >> ((':' >> classParent_p >> '{') | '{') - >> *(functions_p | comments_p) - >> str_p("};")) - [bl::bind(&Constructor::initializeOrCheck, bl::var(constructor), - bl::var(cls.name), Qualified(), verbose)] - [assign_a(cls.constructor, constructor)] - [assign_a(cls.namespaces, namespaces)] - [assign_a(cls.deconstructor.name,cls.name)] - [bl::bind(&handle_possible_template, bl::var(classes), bl::var(cls), - bl::var(templateInstantiations))] - [clear_a(templateInstantiations)] - [assign_a(constructor, constructor0)] - [assign_a(cls,cls0)]; - - // parse a global function - Qualified globalFunction; - Rule global_function_p = - (returnValue_p >> staticMethodName_p[assign_a(globalFunction.name)] >> - '(' >> argumentList_p >> ')' >> ';' >> *comments_p) - [assign_a(globalFunction.namespaces,namespaces)] - [bl::bind(&GlobalFunction::addOverload, - bl::var(global_functions)[bl::var(globalFunction.name)], - bl::var(globalFunction), bl::var(args), bl::var(retVal), Qualified(),verbose)] - [assign_a(retVal,retVal0)] - [clear_a(globalFunction)] - [clear_a(args)]; + // Create grammar for global functions + GlobalFunctionGrammar global_function_g(global_functions,namespaces); Rule include_p = str_p("#include") >> ch_p('<') >> (*(anychar_p - '>'))[push_back_a(includes)] >> ch_p('>'); @@ -328,9 +138,9 @@ void Module::parseMarkup(const std::string& data) { Rule namespace_def_p = (str_p("namespace") - >> namespace_name_p[push_back_a(namespaces)] + >> basic.namespace_p[push_back_a(namespaces)] >> ch_p('{') - >> *(include_p | class_p | templateSingleInstantiation_p | global_function_p | namespace_def_p | comments_p) + >> *(include_p | class_p | templateSingleInstantiation_p | global_function_g | namespace_def_p | basic.comments_p) >> ch_p('}')) [pop_a(namespaces)]; @@ -343,47 +153,24 @@ void Module::parseMarkup(const std::string& data) { Rule forward_declaration_p = !(str_p("virtual")[assign_a(fwDec.isVirtual, true)]) >> str_p("class") - >> (*(namespace_name_p >> str_p("::")) >> className_p)[assign_a(fwDec.name)] + >> (*(basic.namespace_p >> str_p("::")) >> basic.className_p)[assign_a(fwDec.name)] >> ch_p(';') [push_back_a(forward_declarations, fwDec)] + [assign_a(cls,cls0)] // also clear class to avoid partial parse [assign_a(fwDec, fwDec0)]; - Rule module_content_p = comments_p | include_p | class_p | templateSingleInstantiation_p | forward_declaration_p | global_function_p | namespace_def_p; + Rule module_content_p = basic.comments_p | include_p | class_p + | templateSingleInstantiation_p | forward_declaration_p + | global_function_g | namespace_def_p; Rule module_p = *module_content_p >> !end_p; - //---------------------------------------------------------------------------- - // for debugging, define BOOST_SPIRIT_DEBUG -# ifdef BOOST_SPIRIT_DEBUG - BOOST_SPIRIT_DEBUG_NODE(className_p); - BOOST_SPIRIT_DEBUG_NODE(classPtr_p); - BOOST_SPIRIT_DEBUG_NODE(classRef_p); - BOOST_SPIRIT_DEBUG_NODE(basisType_p); - BOOST_SPIRIT_DEBUG_NODE(name_p); - BOOST_SPIRIT_DEBUG_NODE(argument_p); - BOOST_SPIRIT_DEBUG_NODE(argumentList_p); - BOOST_SPIRIT_DEBUG_NODE(constructor_p); - BOOST_SPIRIT_DEBUG_NODE(returnType1_p); - BOOST_SPIRIT_DEBUG_NODE(returnType2_p); - BOOST_SPIRIT_DEBUG_NODE(pair_p); - BOOST_SPIRIT_DEBUG_NODE(void_p); - BOOST_SPIRIT_DEBUG_NODE(returnValue_p); - BOOST_SPIRIT_DEBUG_NODE(methodName_p); - BOOST_SPIRIT_DEBUG_NODE(method_p); - BOOST_SPIRIT_DEBUG_NODE(class_p); - BOOST_SPIRIT_DEBUG_NODE(namespace_def_p); - BOOST_SPIRIT_DEBUG_NODE(module_p); -# endif - //---------------------------------------------------------------------------- - // and parse contents parse_info info = parse(data.c_str(), module_p, space_p); if(!info.full) { printf("parsing stopped at \n%.20s\n",info.stop); - cout << "Stopped near:\n" - "class '" << cls.name << "'\n" - "method '" << methodName << "'\n" - "argument '" << arg.name << "'" << endl; + cout << "Stopped in:\n" + "class '" << cls.name_ << "'" << endl; throw ParseFailed((int)info.length); } @@ -416,6 +203,11 @@ void Module::parseMarkup(const std::string& data) { // Create type attributes table and check validity typeAttributes.addClasses(expandedClasses); typeAttributes.addForwardDeclarations(forward_declarations); + // add Eigen types as template arguments are also checked ? + vector eigen; + eigen.push_back(ForwardDeclaration("Vector")); + eigen.push_back(ForwardDeclaration("Matrix")); + typeAttributes.addForwardDeclarations(eigen); typeAttributes.checkValidity(expandedClasses); } @@ -546,7 +338,7 @@ vector Module::ExpandTypedefInstantiations(const vector& classes, vector Module::GenerateValidTypes(const vector& classes, const vector forwardDeclarations) { vector validTypes; BOOST_FOREACH(const ForwardDeclaration& fwDec, forwardDeclarations) { - validTypes.push_back(fwDec.name); + validTypes.push_back(fwDec.name); } validTypes.push_back("void"); validTypes.push_back("string"); diff --git a/wrap/Module.h b/wrap/Module.h index a4659dc3a..e0c1b3f31 100644 --- a/wrap/Module.h +++ b/wrap/Module.h @@ -18,15 +18,15 @@ #pragma once -#include -#include -#include - #include "Class.h" #include "GlobalFunction.h" #include "TemplateInstantiationTypedef.h" #include "ForwardDeclaration.h" +#include +#include +#include + namespace wrap { /** @@ -34,9 +34,6 @@ namespace wrap { */ struct Module { - typedef std::map GlobalFunctions; - typedef std::map Methods; - // Filled during parsing: std::string name; ///< module name bool verbose; ///< verbose flag diff --git a/wrap/OverloadedFunction.h b/wrap/OverloadedFunction.h index 47c418748..7718bc139 100644 --- a/wrap/OverloadedFunction.h +++ b/wrap/OverloadedFunction.h @@ -87,7 +87,8 @@ class OverloadedFunction: public Function, public ArgumentOverloads { public: bool addOverload(const std::string& name, const ArgumentList& args, - const Qualified& instName = Qualified(), bool verbose = false) { + boost::optional instName = boost::none, bool verbose = + false) { bool first = initializeOrCheck(name, instName, verbose); ArgumentOverloads::push_back(args); return first; diff --git a/wrap/Qualified.h b/wrap/Qualified.h index b23e9020d..bcc4c0829 100644 --- a/wrap/Qualified.h +++ b/wrap/Qualified.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include @@ -26,43 +27,122 @@ namespace wrap { /** * Class to encapuslate a qualified name, i.e., with (nested) namespaces */ -struct Qualified { +class Qualified { - std::vector namespaces; ///< Stack of namespaces - std::string name; ///< type name +//protected: +public: - Qualified(const std::string& name_ = "") : - name(name_) { + std::vector namespaces_; ///< Stack of namespaces + std::string name_; ///< type name + + friend struct TypeGrammar; + friend class TemplateSubstitution; + +public: + + /// the different categories + typedef enum { + CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4 + } Category; + Category category; + + Qualified() : + category(VOID) { + } + + Qualified(const std::string& n, Category c = CLASS) : + name_(n), category(c) { + } + + Qualified(const std::string& ns1, const std::string& ns2, + const std::string& n, Category c = CLASS) : + name_(n), category(c) { + namespaces_.push_back(ns1); + namespaces_.push_back(ns2); + } + + Qualified(const std::string& ns1, const std::string& n, Category c = CLASS) : + name_(n), category(c) { + namespaces_.push_back(ns1); + } + + Qualified(std::vector ns, const std::string& name) : + namespaces_(ns), name_(name), category(CLASS) { + } + + std::string name() const { + return name_; + } + + std::vector namespaces() const { + return namespaces_; + } + + // Qualified is 'abused' as template argument name as well + // this function checks whether *this matches with templateArg + bool match(const std::string& templateArg) const { + return (name_ == templateArg && namespaces_.empty()); //TODO && category == CLASS); + } + + void rename(const Qualified& q) { + namespaces_ = q.namespaces_; + name_ = q.name_; + category = q.category; + } + + void expand(const std::string& expansion) { + name_ += expansion; + } + + bool operator==(const Qualified& other) const { + return namespaces_ == other.namespaces_ && name_ == other.name_ + && category == other.category; } bool empty() const { - return namespaces.empty() && name.empty(); + return namespaces_.empty() && name_.empty(); } - void clear() { - namespaces.clear(); - name.clear(); + virtual void clear() { + namespaces_.clear(); + name_.clear(); + category = VOID; } - bool operator!=(const Qualified& other) const { - return other.name != name || other.namespaces != namespaces; +public: + + static Qualified MakeClass(std::vector namespaces, + const std::string& name) { + return Qualified(namespaces, name); + } + + static Qualified MakeEigen(const std::string& name) { + return Qualified(name, EIGEN); + } + + static Qualified MakeBasis(const std::string& name) { + return Qualified(name, BASIS); + } + + static Qualified MakeVoid() { + return Qualified("void", VOID); } /// Return a qualified string using given delimiter std::string qualifiedName(const std::string& delimiter = "") const { std::string result; - for (std::size_t i = 0; i < namespaces.size(); ++i) - result += (namespaces[i] + delimiter); - result += name; + for (std::size_t i = 0; i < namespaces_.size(); ++i) + result += (namespaces_[i] + delimiter); + result += name_; return result; } /// Return a matlab file name, i.e. "toolboxPath/+ns1/+ns2/name.m" std::string matlabName(const std::string& toolboxPath) const { std::string result = toolboxPath; - for (std::size_t i = 0; i < namespaces.size(); ++i) - result += ("/+" + namespaces[i]); - result += "/" + name + ".m"; + for (std::size_t i = 0; i < namespaces_.size(); ++i) + result += ("/+" + namespaces_[i]); + result += "/" + name_ + ".m"; return result; } @@ -73,5 +153,109 @@ struct Qualified { }; +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct TypeGrammar: classic::grammar { + + wrap::Qualified& result_; ///< successful parse will be placed in here + + /// Construct type grammar and specify where result is placed + TypeGrammar(wrap::Qualified& result) : + result_(result) { + } + + /// Definition of type grammar + template + struct definition: BasicRules { + + typedef classic::rule Rule; + + Rule void_p, basisType_p, eigenType_p, namespace_del_p, class_p, type_p; + + definition(TypeGrammar const& self) { + + using namespace wrap; + using namespace classic; + typedef BasicRules Basic; + + // HACK: use const values instead of using enums themselves - somehow this doesn't result in values getting assigned to gibberish + static const Qualified::Category EIGEN = Qualified::EIGEN; + static const Qualified::Category BASIS = Qualified::BASIS; + static const Qualified::Category CLASS = Qualified::CLASS; + static const Qualified::Category VOID = Qualified::VOID; + + void_p = str_p("void") // + [assign_a(self.result_.name_)] // + [assign_a(self.result_.category, VOID)]; + + basisType_p = Basic::basisType_p // + [assign_a(self.result_.name_)] // + [assign_a(self.result_.category, BASIS)]; + + eigenType_p = Basic::eigenType_p // + [assign_a(self.result_.name_)] // + [assign_a(self.result_.category, EIGEN)]; + + namespace_del_p = Basic::namespace_p // + [push_back_a(self.result_.namespaces_)] >> str_p("::"); + + class_p = *namespace_del_p >> Basic::className_p // + [assign_a(self.result_.name_)] // + [assign_a(self.result_.category, CLASS)]; + + type_p = void_p | basisType_p | class_p | eigenType_p; + } + + Rule const& start() const { + return type_p; + } + + }; +}; +// type_grammar + +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +template +struct TypeListGrammar: public classic::grammar > { + + typedef std::vector TypeList; + TypeList& result_; ///< successful parse will be placed in here + + /// Construct type grammar and specify where result is placed + TypeListGrammar(TypeList& result) : + result_(result) { + } + + /// Definition of type grammar + template + struct definition { + + wrap::Qualified type; ///< temporary for use during parsing + TypeGrammar type_g; ///< Individual Type grammars + + classic::rule type_p, typeList_p; + + definition(TypeListGrammar const& self) : + type_g(type) { + using namespace classic; + + type_p = type_g[push_back_a(self.result_, type)][clear_a(type)]; + + typeList_p = OPEN >> !type_p >> *(',' >> type_p) >> CLOSE; + } + + classic::rule const& start() const { + return typeList_p; + } + + }; +}; +// TypeListGrammar + +/* ************************************************************************* */ +// Needed for other parsers in Argument.h and ReturnType.h +static const bool T = true; + } // \namespace wrap diff --git a/wrap/ReturnType.cpp b/wrap/ReturnType.cpp index a317a5420..41fd51680 100644 --- a/wrap/ReturnType.cpp +++ b/wrap/ReturnType.cpp @@ -14,7 +14,7 @@ using namespace wrap; /* ************************************************************************* */ string ReturnType::str(bool add_ptr) const { - return maybe_shared_ptr(add_ptr && isPtr, qualifiedName("::"), name); + return maybe_shared_ptr(add_ptr && isPtr, qualifiedName("::"), name()); } /* ************************************************************************* */ @@ -24,37 +24,46 @@ void ReturnType::wrap_result(const string& out, const string& result, string cppType = qualifiedName("::"), matlabType = qualifiedName("."); if (category == CLASS) { + + // Handle Classes string objCopy, ptrType; - ptrType = "Shared" + name; - const bool isVirtual = typeAttributes.at(cppType).isVirtual; - if (isVirtual) { - if (isPtr) - objCopy = result; - else + const bool isVirtual = typeAttributes.attributes(cppType).isVirtual; + if (isPtr) + objCopy = result; // a shared pointer can always be passed as is + else { + // but if we want an actual new object, things get more complex + if (isVirtual) + // A virtual class needs to be cloned, so the whole hierarchy is returned objCopy = result + ".clone()"; - } else { - if (isPtr) - objCopy = result; else - objCopy = ptrType + "(new " + cppType + "(" + result + "))"; + // ...but a non-virtual class can just be copied + objCopy = "Shared" + name() + "(new " + cppType + "(" + result + "))"; } + // e.g. out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point3", false); wrapperFile.oss << out << " = wrap_shared_ptr(" << objCopy << ",\"" << matlabType << "\", " << (isVirtual ? "true" : "false") << ");\n"; + } else if (isPtr) { - wrapperFile.oss << " Shared" << name << "* ret = new Shared" << name << "(" - << result << ");" << endl; + + // Handle shared pointer case for BASIS/EIGEN/VOID + wrapperFile.oss << " {\n Shared" << name() << "* ret = new Shared" << name() + << "(" << result << ");" << endl; wrapperFile.oss << out << " = wrap_shared_ptr(ret,\"" << matlabType - << "\");\n"; + << "\");\n }\n"; + } else if (matlabType != "void") + + // Handle normal case case for BASIS/EIGEN wrapperFile.oss << out << " = wrap< " << str(false) << " >(" << result << ");\n"; + } /* ************************************************************************* */ void ReturnType::wrapTypeUnwrap(FileWriter& wrapperFile) const { if (category == CLASS) wrapperFile.oss << " typedef boost::shared_ptr<" << qualifiedName("::") - << "> Shared" << name << ";" << endl; + << "> Shared" << name() << ";" << endl; } /* ************************************************************************* */ diff --git a/wrap/ReturnType.h b/wrap/ReturnType.h index 1829fbc81..1c67a1d9a 100644 --- a/wrap/ReturnType.h +++ b/wrap/ReturnType.h @@ -9,6 +9,7 @@ #include "FileWriter.h" #include "TypeAttributesTable.h" #include "utilities.h" +#include #pragma once @@ -17,28 +18,25 @@ namespace wrap { /** * Encapsulates return value of a method or function */ -struct ReturnType: Qualified { - - /// the different supported return value categories - typedef enum { - CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4 - } return_category; +struct ReturnType: public Qualified { bool isPtr; - return_category category; + friend struct ReturnValueGrammar; + + /// Makes a void type ReturnType() : - isPtr(false), category(CLASS) { + isPtr(false) { } - ReturnType(const std::string& name) : - isPtr(false), category(CLASS) { - Qualified::name = name; + /// Constructor, no namespaces + ReturnType(const std::string& name, Category c = CLASS, bool ptr = false) : + Qualified(name, c), isPtr(ptr) { } - void rename(const Qualified& q) { - name = q.name; - namespaces = q.namespaces; + virtual void clear() { + Qualified::clear(); + isPtr = false; } /// Check if this type is in a set of valid types @@ -64,4 +62,36 @@ private: }; +//****************************************************************************** +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct ReturnTypeGrammar: public classic::grammar { + + wrap::ReturnType& result_; ///< successful parse will be placed in here + + TypeGrammar type_g; + + /// Construct ReturnType grammar and specify where result is placed + ReturnTypeGrammar(wrap::ReturnType& result) : + result_(result), type_g(result_) { + } + + /// Definition of type grammar + template + struct definition { + + classic::rule type_p; + + definition(ReturnTypeGrammar const& self) { + using namespace classic; + type_p = self.type_g >> !ch_p('*')[assign_a(self.result_.isPtr, T)]; + } + + classic::rule const& start() const { + return type_p; + } + + }; +}; +// ReturnTypeGrammar + } // \namespace wrap diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index 54e585cad..596acb2c3 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -17,9 +17,9 @@ using namespace wrap; /* ************************************************************************* */ ReturnValue ReturnValue::expandTemplate(const TemplateSubstitution& ts) const { ReturnValue instRetVal = *this; - instRetVal.type1 = ts(type1); + instRetVal.type1 = ts.tryToSubstitite(type1); if (isPair) - instRetVal.type2 = ts(type2); + instRetVal.type2 = ts.tryToSubstitite(type2); return instRetVal; } diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index abfcec2b0..629684a34 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -25,6 +25,8 @@ struct ReturnValue { bool isPair; ReturnType type1, type2; + friend struct ReturnValueGrammar; + /// Constructor ReturnValue() : isPair(false) { @@ -35,6 +37,22 @@ struct ReturnValue { isPair(false), type1(type) { } + /// Constructor + ReturnValue(const ReturnType& t1, const ReturnType& t2) : + isPair(true), type1(t1), type2(t2) { + } + + virtual void clear() { + type1.clear(); + type2.clear(); + isPair = false; + } + + bool operator==(const ReturnValue& other) const { + return isPair == other.isPair && type1 == other.type1 + && type2 == other.type2; + } + /// Substitute template argument ReturnValue expandTemplate(const TemplateSubstitution& ts) const; @@ -59,4 +77,39 @@ struct ReturnValue { }; -} // \namespace wrap +//****************************************************************************** +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct ReturnValueGrammar: public classic::grammar { + + wrap::ReturnValue& result_; ///< successful parse will be placed in here + ReturnTypeGrammar returnType1_g, returnType2_g; ///< Type parsers + + /// Construct type grammar and specify where result is placed + ReturnValueGrammar(wrap::ReturnValue& result) : + result_(result), returnType1_g(result.type1), returnType2_g(result.type2) { + } + + /// Definition of type grammar + template + struct definition { + + classic::rule pair_p, returnValue_p; + + definition(ReturnValueGrammar const& self) { + using namespace classic; + + pair_p = (str_p("pair") >> '<' >> self.returnType1_g >> ',' + >> self.returnType2_g >> '>')[assign_a(self.result_.isPair, T)]; + + returnValue_p = pair_p | self.returnType1_g; + } + + classic::rule const& start() const { + return returnValue_p; + } + + }; +}; +// ReturnValueGrammar + +}// \namespace wrap diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 5f91a15b4..e4e7c89ae 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -16,7 +16,7 @@ * @author Richard Roberts **/ -#include "Method.h" +#include "StaticMethod.h" #include "utilities.h" #include @@ -36,117 +36,9 @@ void StaticMethod::proxy_header(FileWriter& proxyFile) const { proxyFile.oss << " function varargout = " << upperName << "(varargin)\n"; } -/* ************************************************************************* */ -void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, - FileWriter& wrapperFile, Str cppClassName, Str matlabQualName, - Str matlabUniqueName, Str wrapperName, - const TypeAttributesTable& typeAttributes, - vector& functionNames) const { - - // emit header, e.g., function varargout = templatedMethod(this, varargin) - proxy_header(proxyFile); - - // Emit comments for documentation - string up_name = boost::to_upper_copy(matlabName()); - proxyFile.oss << " % " << up_name << " usage: "; - usage_fragment(proxyFile, matlabName()); - - // Emit URL to Doxygen page - proxyFile.oss << " % " - << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" - << endl; - - // Handle special case of single overload with all numeric arguments - if (nrOverloads() == 1 && argumentList(0).allScalar()) { - // Output proxy matlab code - // TODO: document why is it OK to not check arguments in this case - proxyFile.oss << " "; - const int id = (int) functionNames.size(); - argumentList(0).emit_call(proxyFile, returnValue(0), wrapperName, id, - isStatic()); - - // Output C++ wrapper code - const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName, - matlabUniqueName, 0, id, typeAttributes, templateArgValue_); - - // Add to function list - functionNames.push_back(wrapFunctionName); - } else { - // Check arguments for all overloads - for (size_t i = 0; i < nrOverloads(); ++i) { - - // Output proxy matlab code - proxyFile.oss << " " << (i == 0 ? "" : "else"); - const int id = (int) functionNames.size(); - argumentList(i).emit_conditional_call(proxyFile, returnValue(i), - wrapperName, id, isStatic()); - - // Output C++ wrapper code - const string wrapFunctionName = wrapper_fragment(wrapperFile, - cppClassName, matlabUniqueName, i, id, typeAttributes, - templateArgValue_); - - // Add to function list - functionNames.push_back(wrapFunctionName); - } - proxyFile.oss << " else\n"; - proxyFile.oss - << " error('Arguments do not match any overload of function " - << matlabQualName << "." << name_ << "');" << endl; - proxyFile.oss << " end\n"; - } - - proxyFile.oss << " end\n"; -} - -/* ************************************************************************* */ -string StaticMethod::wrapper_fragment(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, int overload, int id, - const TypeAttributesTable& typeAttributes, - const Qualified& instName) const { - - // generate code - - const string wrapFunctionName = matlabUniqueName + "_" + name_ + "_" - + boost::lexical_cast(id); - - const ArgumentList& args = argumentList(overload); - const ReturnValue& returnVal = returnValue(overload); - - // call - wrapperFile.oss << "void " << wrapFunctionName - << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; - // start - wrapperFile.oss << "{\n"; - - returnVal.wrapTypeUnwrap(wrapperFile); - - wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName - << "> Shared;" << endl; - - // get call - // for static methods: cppClassName::staticMethod - // for instance methods: obj->instanceMethod - string expanded = wrapper_call(wrapperFile, cppClassName, matlabUniqueName, - args, returnVal, typeAttributes, instName); - - expanded += ("(" + args.names() + ")"); - if (returnVal.type1.name != "void") - returnVal.wrap_result(expanded, wrapperFile, typeAttributes); - else - wrapperFile.oss << " " + expanded + ";\n"; - - // finish - wrapperFile.oss << "}\n"; - - return wrapFunctionName; -} - /* ************************************************************************* */ string StaticMethod::wrapper_call(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, const ArgumentList& args, - const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes, - const Qualified& instName) const { + Str matlabUniqueName, const ArgumentList& args) const { // check arguments // NOTE: for static functions, there is no object passed wrapperFile.oss << " checkArguments(\"" << matlabUniqueName << "." << name_ @@ -158,17 +50,10 @@ string StaticMethod::wrapper_call(FileWriter& wrapperFile, Str cppClassName, // call method and wrap result // example: out[0]=wrap(staticMethod(t)); string expanded = cppClassName + "::" + name_; - if (!instName.empty()) - expanded += ("<" + instName.qualifiedName("::") + ">"); + if (templateArgValue_) + expanded += ("<" + templateArgValue_->qualifiedName("::") + ">"); return expanded; } /* ************************************************************************* */ -void StaticMethod::python_wrapper(FileWriter& wrapperFile, - Str className) const { - wrapperFile.oss << " .def(\"" << name_ << "\", &" << className << "::" << name_ - << ");\n"; -} - -/* ************************************************************************* */ diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index de8e4a94e..a01eeff62 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -19,42 +19,15 @@ #pragma once -#include "FullyOverloadedFunction.h" +#include "MethodBase.h" namespace wrap { /// StaticMethod class -struct StaticMethod: public FullyOverloadedFunction { +struct StaticMethod: public MethodBase { typedef const std::string& Str; - virtual bool isStatic() const { - return true; - } - - // emit a list of comments, one for each overload - void comment_fragment(FileWriter& proxyFile) const { - SignatureOverloads::comment_fragment(proxyFile, matlabName()); - } - - void verifyArguments(const std::vector& validArgs) const { - SignatureOverloads::verifyArguments(validArgs, name_); - } - - void verifyReturnTypes(const std::vector& validtypes) const { - SignatureOverloads::verifyReturnTypes(validtypes, name_); - } - - // MATLAB code generation - // classPath is class directory, e.g., ../matlab/@Point2 - void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - Str cppClassName, Str matlabQualName, Str matlabUniqueName, - Str wrapperName, const TypeAttributesTable& typeAttributes, - std::vector& functionNames) const; - - // emit python wrapper - void python_wrapper(FileWriter& wrapperFile, Str className) const; - friend std::ostream& operator<<(std::ostream& os, const StaticMethod& m) { for (size_t i = 0; i < m.nrOverloads(); i++) os << "static " << m.returnVals_[i] << " " << m.name_ << m.argLists_[i]; @@ -65,15 +38,8 @@ protected: virtual void proxy_header(FileWriter& proxyFile) const; - std::string wrapper_fragment(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, int overload, int id, - const TypeAttributesTable& typeAttributes, const Qualified& instName = - Qualified()) const; ///< cpp wrapper - virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, const ArgumentList& args, - const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes, - const Qualified& instName) const; + Str matlabUniqueName, const ArgumentList& args) const; }; } // \namespace wrap diff --git a/wrap/Template.h b/wrap/Template.h new file mode 100644 index 000000000..5a64412ed --- /dev/null +++ b/wrap/Template.h @@ -0,0 +1,102 @@ +/* ---------------------------------------------------------------------------- + + * 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 Template.h + * @brief Template name + * @author Frank Dellaert + * @date Nov 11, 2014 + **/ + +#pragma once + +#include + +namespace wrap { + +/// The template specification that goes before a method or a class +class Template { + std::string argName_; + std::vector argValues_; + friend struct TemplateGrammar; +public: + /// The only way to get values into a Template is via our friendly Grammar + Template() { + } + void clear() { + argName_.clear(); + argValues_.clear(); + } + const std::string& argName() const { + return argName_; + } + const std::vector& argValues() const { + return argValues_; + } + size_t nrValues() const { + return argValues_.size(); + } + const Qualified& operator[](size_t i) const { + return argValues_[i]; + } + bool valid() const { + return !argName_.empty() && argValues_.size() > 0; + } + +}; + +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct TemplateGrammar: public classic::grammar { + + Template& result_; ///< successful parse will be placed in here + TypeListGrammar<'{', '}'> argValues_g; ///< TypeList parser + + /// Construct type grammar and specify where result is placed + TemplateGrammar(Template& result) : + result_(result), argValues_g(result.argValues_) { + } + + /// Definition of type grammar + template + struct definition: BasicRules { + + classic::rule templateArgValues_p; + + definition(TemplateGrammar const& self) { + using classic::str_p; + using classic::assign_a; + templateArgValues_p = (str_p("template") >> '<' + >> (BasicRules::name_p)[assign_a(self.result_.argName_)] + >> '=' >> self.argValues_g >> '>'); + } + + classic::rule const& start() const { + return templateArgValues_p; + } + + }; +}; +// TemplateGrammar + +/// Cool initializer for tests +static inline boost::optional