diff --git a/.cproject b/.cproject index ab1d0cdfc..10b16f91c 100644 --- a/.cproject +++ b/.cproject @@ -5,13 +5,13 @@ - - + + @@ -60,13 +60,13 @@ - - + + @@ -116,13 +116,13 @@ - - + + @@ -484,333 +484,6 @@ - - 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 - -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 @@ -859,167 +532,39 @@ 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 + testSimilarity3.run true true true - + + make + -j5 + testInvDepthCamera3.run + true + true + true + + + make + -j5 + testTriangulation.run + true + true + true + + make -j4 - testQuaternion.run + testEvent.run true true true - - make - -j4 - testOrientedPlane3.run - true - true - true - - + make -j2 all @@ -1027,7 +572,7 @@ true true - + make -j2 clean @@ -1035,23 +580,143 @@ true true - + + make + -k + check + true + false + true + + + make + + tests/testBayesTree.run + true + false + true + + + make + + testBinaryBayesNet.run + true + false + true + + make -j2 - clean all + testFactorGraph.run true true true - + make -j2 - install + testISAM.run true true true - + + make + -j2 + testJunctionTree.run + true + true + true + + + make + -j2 + testKey.run + true + true + true + + + make + -j2 + testOrdering.run + true + true + true + + + make + + testSymbolicBayesNet.run + true + false + true + + + make + + tests/testSymbolicFactor.run + true + false + true + + + make + + testSymbolicFactorGraph.run + true + false + true + + + make + -j2 + timeSymbolMaps.run + true + true + true + + + make + + tests/testBayesTree + true + false + true + + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + make -j2 clean @@ -1203,146 +868,154 @@ true true - + make - -j2 - all + -j5 + testGaussianFactorGraphUnordered.run true true true - + make - -j2 - check + -j5 + testGaussianBayesNetUnordered.run true true true - + make - -j2 + -j5 + testGaussianConditional.run + true + true + true + + + make + -j5 + testGaussianDensity.run + true + true + true + + + make + -j5 + testGaussianJunctionTree.run + true + true + true + + + make + -j5 + testHessianFactor.run + true + true + true + + + make + -j5 + testJacobianFactor.run + true + true + true + + + make + -j5 + testKalmanFilter.run + true + true + true + + + make + -j5 + testNoiseModel.run + true + true + true + + + make + -j5 + testSampler.run + true + true + true + + + make + -j5 + testSerializationLinear.run + true + true + true + + + make + -j5 + testVectorValues.run + true + true + true + + + make + -j5 + testGaussianBayesTree.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + + + make + -j5 + testImuFactor.run + true + true + true + + + make + -j5 + testAHRSFactor.run + true + true + true + + + make + -j8 + testAttitudeFactor.run + true + true + true + + + make + -j5 clean true true true - + make - -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 + -j5 + all true true true @@ -1437,103 +1110,477 @@ make + testErrors.run true false true - + make - -j5 - testAntiFactor.run + -j2 + check true true true - + make - -j5 - testPriorFactor.run + -j2 + tests/testGaussianJunctionTree.run true true true - + make - -j5 - testDataset.run + -j2 + tests/testGaussianFactor.run true true true - + make - -j5 - testEssentialMatrixFactor.run + -j2 + tests/testGaussianConditional.run true true true - + make - -j5 - testGeneralSFMFactor_Cal3Bundler.run + -j2 + tests/timeSLAMlike.run true true true - + make - -j5 - testGeneralSFMFactor.run + -j2 + check true true true - + make - -j5 - testProjectionFactor.run + -j2 + clean true true true - + make - -j5 - testRotateFactor.run + -j2 + testBTree.run true true true - + make - -j5 - testPoseRotationPrior.run + -j2 + testDSF.run true true true - + make - -j5 - testImplicitSchurFactor.run + -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 + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + + + make + -j5 + testEliminationTree.run + true + true + true + + + make + -j5 + testInference.run + true + true + true + + + make + -j5 + testKey.run + true + true + true + + + make + -j1 + testSymbolicBayesTree.run + true + false + true + + + make + -j1 + testSymbolicSequentialSolver.run + true + false + true + + make -j4 - testRangeFactor.run + testLabeledSymbol.run true true true - + make - -j4 - testOrientedPlane3Factor.run + -j2 + check + true + true + true + + + make + -j2 + tests/testLieConfig.run true true true @@ -1739,6 +1786,7 @@ cpack + -G DEB true false @@ -1746,6 +1794,7 @@ cpack + -G RPM true false @@ -1753,6 +1802,7 @@ cpack + -G TGZ true false @@ -1760,6 +1810,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -1941,7 +1992,15 @@ false true - + + make + -j4 + check.sam + true + true + true + + make -j2 check @@ -1949,55 +2008,38 @@ true true - + make - - tests/testGaussianISAM2 + -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/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 @@ -2005,26 +2047,458 @@ true true - + make -j5 - testParticleFactor.run + testCal3Bundler.run true true true - + make -j5 - testExpressionMeta.run + 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 - testCustomChartExpression.run + 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 + -j4 + testUnit3.run + true + true + true + + + make + -j4 + testBearingRange.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 + -j1 + testDiscreteBayesTree.run + true + false + true + + + make + -j5 + testDiscreteConditional.run + true + true + true + + + make + -j5 + testDiscreteFactor.run + true + true + true + + + make + -j5 + testDiscreteFactorGraph.run + true + true + true + + + make + -j5 + testDiscreteMarginals.run + true + true + true + + + make + -j5 + testIMUSystem.run + true + true + true + + + make + -j5 + testPoseRTV.run + true + true + true + + + make + -j5 + testVelocityConstraint.run + true + true + true + + + make + -j5 + testVelocityConstraint3.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + timeCalibratedCamera.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + testMethod.run + true + true + true + + + make + -j5 + testClass.run + true + true + true + + + make + -j4 + testType.run + true + true + true + + + make + -j4 + testArgument.run + true + true + true + + + make + -j4 + testReturnValue.run + true + true + true + + + make + -j4 + testTemplate.run + true + true + true + + + make + -j4 + testGlobalFunction.run true true true @@ -2077,7 +2551,343 @@ true true - + + make + -j2 + vSFMexample.run + true + true + true + + + make + -j2 + testVSLAMGraph + true + true + true + + + make + -j5 + testMatrix.run + true + true + true + + + make + -j5 + testVector.run + true + true + true + + + make + -j5 + testNumericalDerivative.run + true + true + true + + + make + -j5 + testVerticalBlockMatrix.run + true + true + true + + + make + -j4 + testOptionalJacobian.run + true + true + true + + + make + -j4 + testGroup.run + true + true + true + + + make + -j5 + 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 + -j4 + testSerializationSLAM.run + true + true + true + + + make + -j5 + testParticleFactor.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + make -j2 all @@ -2085,7 +2895,7 @@ true true - + make -j2 clean @@ -2093,129 +2903,15 @@ 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 + all true true true - - make - -j2 - testISAM.run - true - true - true - - - make - -j2 - testJunctionTree.run - true - true - true - - - make - -j2 - testKey.run - true - true - true - - - make - -j2 - testOrdering.run - true - true - true - - - make - testSymbolicBayesNet.run - true - false - true - - - make - tests/testSymbolicFactor.run - true - false - true - - - make - testSymbolicFactorGraph.run - true - false - true - - - make - -j2 - timeSymbolMaps.run - true - true - true - - - make - tests/testBayesTree - true - false - true - - - make - -j2 - check - true - true - true - - - make - -j2 - timeCalibratedCamera.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - + make -j2 clean @@ -2223,18 +2919,106 @@ true true - + make - -j2 - tests/testPose2.run + -j5 + testAntiFactor.run true true true - + make - -j2 - tests/testPose3.run + -j5 + testPriorFactor.run + true + true + true + + + make + -j5 + testDataset.run + true + true + true + + + make + -j5 + testEssentialMatrixFactor.run + true + true + true + + + make + -j5 + testGeneralSFMFactor_Cal3Bundler.run + true + true + true + + + make + -j5 + testGeneralSFMFactor.run + true + true + true + + + make + -j5 + testProjectionFactor.run + true + true + true + + + make + -j5 + testRotateFactor.run + true + true + true + + + make + -j5 + testPoseRotationPrior.run + true + true + true + + + make + -j5 + testImplicitSchurFactor.run + true + true + true + + + make + -j4 + testOrientedPlane3Factor.run + true + true + true + + + make + -j4 + testSmartProjectionPoseFactor.run + true + true + true + + + make + -j4 + testInitializePose3.run true true true @@ -2439,6 +3223,213 @@ 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 + -j4 + testSerializationNonlinear.run + true + true + true + + + make + -j4 + testImuFactor.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + tests/testGaussianISAM2 + true + false + 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 + -j4 + timeSchurFactors.run + true + true + true + + + make + -j4 + timeRot2.run + true + true + true + make -j2 @@ -2535,895 +3526,34 @@ true true - - make - -j1 - testDiscreteBayesTree.run - true - false - true - - - make - -j5 - testDiscreteConditional.run - true - true - true - - - make - -j5 - testDiscreteFactor.run - true - true - true - - - make - -j5 - testDiscreteFactorGraph.run - true - true - true - - - make - -j5 - testDiscreteMarginals.run - true - true - true - - - make - -j5 - 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 + testBearingFactor.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 + testRangeFactor.run true true true - + make -j4 - testAdaptAutoDiff.run + testBearingRangeFactor.run true true true - - make - -j4 - testCallRecord.run - true - true - true - - - make - -j4 - testExpressionFactor.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 - -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 - -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 + wrap true true true diff --git a/.gitignore b/.gitignore index d46bddd10..4d0aedb3c 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ /build* +.idea *.pyc *.DS_Store /examples/Data/dubrovnik-3-7-pre-rewritten.txt 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/CMakeLists.txt b/CMakeLists.txt index 38ee89760..0380b8a2f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -131,7 +131,7 @@ endif() if(NOT (${Boost_VERSION} LESS 105600)) message("Ignoring Boost restriction on optional lvalue assignment from rvalues") - add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES) + add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES -DBOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES) endif() ############################################################################### @@ -158,6 +158,12 @@ else() set(GTSAM_USE_TBB 0) # This will go into config.h endif() +############################################################################### +# Prohibit Timing build mode in combination with TBB +if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE STREQUAL "Timing")) + message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.") +endif() + ############################################################################### # Find Google perftools @@ -173,6 +179,11 @@ if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL) set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL include_directories(${MKL_INCLUDE_DIR}) list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES}) + + # --no-as-needed is required with gcc according to the MKL link advisor + if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed") + endif() else() set(GTSAM_USE_EIGEN_MKL 0) set(EIGEN_USE_MKL_ALL 0) @@ -192,36 +203,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}") + + # 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 paths e.g. - set(GTSAM_EIGEN_INCLUDE_PREFIX "gtsam/3rdparty/Eigen/") - + # 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 @@ -262,7 +277,8 @@ endif() # Include boost - use 'BEFORE' so that a specific boost specified to CMake # takes precedence over a system-installed one. -include_directories(BEFORE ${Boost_INCLUDE_DIR}) +# Use 'SYSTEM' to ignore compiler warnings in Boost headers +include_directories(BEFORE SYSTEM ${Boost_INCLUDE_DIR}) # Add includes for source directories 'BEFORE' boost and any system include # paths so that the compiler uses GTSAM headers in our source directory instead @@ -289,6 +305,13 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") endif() endif() +# As of XCode 7, clang also complains about this +if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") + if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 7.0) + add_definitions(-Wno-unused-local-typedefs) + endif() +endif() + if(GTSAM_ENABLE_CONSISTENCY_CHECKS) add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS) endif() @@ -371,6 +394,8 @@ set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>= # Print configuration variables message(STATUS "===============================================================") message(STATUS "================ Configuration Options ======================") +message(STATUS " CMAKE_CXX_COMPILER_ID type : ${CMAKE_CXX_COMPILER_ID}") +message(STATUS " CMAKE_CXX_COMPILER_VERSION : ${CMAKE_CXX_COMPILER_VERSION}") message(STATUS "Build flags ") print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ") print_config_flag(${GTSAM_BUILD_EXAMPLES_ALWAYS} "Build examples with 'make all' ") @@ -389,6 +414,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) diff --git a/README.md b/README.md index 679af5a2f..ccdc7f07e 100644 --- a/README.md +++ b/README.md @@ -31,6 +31,7 @@ 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`) +- A modern compiler, i.e., at least gcc 4.7.3 on Linux. Optional prerequisites - used automatically if findable by CMake: @@ -46,6 +47,6 @@ 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. - +Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) 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/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 1bead58d8..43ae36929 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -34,30 +34,31 @@ 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 -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) - set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE) + set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -std=c11 -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} -std=c++11 -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) + set(CMAKE_C_FLAGS_RELWITHDEBINFO "-std=c11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) + set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-std=c++11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) + set(CMAKE_C_FLAGS_RELEASE "-std=c11 -O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE) + set(CMAKE_CXX_FLAGS_RELEASE "-std=c++11 -O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE) set(CMAKE_C_FLAGS_TIMING "${CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE) set(CMAKE_CXX_FLAGS_TIMING "${CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE) set(CMAKE_EXE_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE) set(CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE) mark_as_advanced(CMAKE_C_FLAGS_TIMING CMAKE_CXX_FLAGS_TIMING CMAKE_EXE_LINKER_FLAGS_TIMING CMAKE_SHARED_LINKER_FLAGS_TIMING) - set(CMAKE_C_FLAGS_PROFILING "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) - set(CMAKE_CXX_FLAGS_PROFILING "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) + set(CMAKE_C_FLAGS_PROFILING "-std=c11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) + set(CMAKE_CXX_FLAGS_PROFILING "-std=c++11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) set(CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) set(CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE__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) endif() endif() -# Clang on Mac uses a template depth that is less than standard and is too small +# Clang 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() + # 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 4b3af9810..3e5cadd32 100644 --- a/cmake/GtsamTesting.cmake +++ b/cmake/GtsamTesting.cmake @@ -195,7 +195,9 @@ 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}) + if(NOT XCODE_VERSION) + add_dependencies(all.tests ${script_name}) + endif() # Add TOPSRCDIR set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") diff --git a/doc/.gitignore b/doc/.gitignore new file mode 100644 index 000000000..8a3139177 --- /dev/null +++ b/doc/.gitignore @@ -0,0 +1,3 @@ +/html/ +*.lyx~ +*.bib~ diff --git a/examples/Data/.gitignore b/examples/Data/.gitignore new file mode 100644 index 000000000..2211df63d --- /dev/null +++ b/examples/Data/.gitignore @@ -0,0 +1 @@ +*.txt diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index 84f9be3a1..a716f9cd8 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -42,7 +42,7 @@ // Also, we will initialize the robot at the origin using a Prior factor. #include #include -#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. diff --git a/examples/Pose2SLAMExampleExpressions.cpp b/examples/Pose2SLAMExampleExpressions.cpp index 92f94c3f3..1f6de6cb1 100644 --- a/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 -// 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; 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 04632e9e3..4d116c7ec 100644 --- a/examples/RangeISAMExample_plaza2.cpp +++ b/examples/RangeISAMExample_plaza2.cpp @@ -39,7 +39,7 @@ // have been provided with the library for solving robotics SLAM problems. #include #include -#include +#include #include // Standard headers, added last, so we know headers above work on their own diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 5be266d11..8da9847b8 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). @@ -103,7 +97,7 @@ int main(int argc, char* argv[]) { // Intentionally initialize the variables off from the ground truth Values initialEstimate; 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)))); + initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); for (size_t j = 0; j < points.size(); ++j) initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); initialEstimate.print("Initial Estimates:\n"); diff --git a/examples/SFMExampleExpressions.cpp b/examples/SFMExampleExpressions.cpp index e9c9e920d..df5488ec3 100644 --- a/examples/SFMExampleExpressions.cpp +++ b/examples/SFMExampleExpressions.cpp @@ -84,7 +84,7 @@ int main(int argc, char* argv[]) { // Create perturbed initial Values initial; - Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); + Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); for (size_t i = 0; i < poses.size(); ++i) initial.insert(Symbol('x', i), poses[i].compose(delta)); for (size_t j = 0; j < points.size(); ++j) diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index b999e6600..e7c0aa696 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -17,51 +17,25 @@ * @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; + +// create a typedef to the camera type +typedef PinholePose Camera; /* ************************************************************************* */ int main(int argc, char* argv[]) { @@ -84,12 +58,12 @@ int main(int argc, char* argv[]) { 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()); + SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K)); for (size_t i = 0; i < poses.size(); ++i) { // generate the 2D measurement - SimpleCamera camera(poses[i], *K); + Camera 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: @@ -97,7 +71,7 @@ int main(int argc, char* argv[]) { // 2. the corresponding camera's key // 3. camera noise model // 4. camera calibration - smartfactor->add(measurement, i, measurementNoise, K); + smartfactor->add(measurement, i); } // insert the smart factor in the graph @@ -106,28 +80,29 @@ int main(int argc, char* argv[]) { // 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( + noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); - graph.push_back(PriorFactor(0, poses[0], poseNoise)); + graph.push_back(PriorFactor(0, Camera(poses[0],K), noise)); // Because the structure-from-motion problem has a scale ambiguity, the problem is // still under-constrained. Here we add a prior on the second pose x1, so this will // fix the scale by indicating the distance between x0 and x1. // Because these two are fixed, the rest of the poses will be also be fixed. - graph.push_back(PriorFactor(1, poses[1], poseNoise)); // add directly to graph + graph.push_back(PriorFactor(1, Camera(poses[1],K), noise)); // add directly to graph graph.print("Factor Graph:\n"); // Create the initial estimate to the solution // Intentionally initialize the variables off from the ground truth Values initialEstimate; - Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); + Pose3 delta(Rot3::Rodrigues(-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)); + initialEstimate.insert(i, Camera(poses[i].compose(delta), K)); 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. @@ -136,20 +111,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..743934c7c --- /dev/null +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -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 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; + +// create a typedef to the camera type +typedef PinholePose Camera; + +/* ************************************************************************* */ +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(measurementNoise, K)); + + for (size_t i = 0; i < poses.size(); ++i) { + + // generate the 2D measurement + Camera 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); + } + + // 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 noise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); + graph.push_back(PriorFactor(0, Camera(poses[0],K), noise)); + + // Fix the scale ambiguity by adding a prior + graph.push_back(PriorFactor(1, Camera(poses[0],K), noise)); + + // Create the initial estimate to the solution + Values initialEstimate; + Pose3 delta(Rot3::Rodrigues(-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, Camera(poses[i].compose(delta),K)); + + // 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_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp new file mode 100644 index 000000000..4bbaac3ef --- /dev/null +++ b/examples/SFMExample_bal_COLAMD_METIS.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 SFMExample.cpp + * @brief This file is to compare the ordering performance for COLAMD vs METIS. + * Example problem is to solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file. + * @author Frank Dellaert, Zhaoyang Lv + */ + +// For an explanation of headers, see SFMExample.cpp +#include +#include +#include +#include +#include +#include +#include // for loading BAL datasets ! + +#include + +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::C; +using symbol_shorthand::P; + +// We will be using a projection factor that ties a SFM_Camera to a 3D point. +// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration +// and has a total of 9 free parameters +typedef GeneralSFMFactor MyFactor; + +/* ************************************************************************* */ +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 + NonlinearFactorGraph graph; + + // 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 + + // Add measurements to the factor graph + size_t j = 0; + BOOST_FOREACH(const SfM_Track& track, mydata.tracks) { + BOOST_FOREACH(const SfM_Measurement& m, track.measurements) { + size_t i = m.first; + Point2 uv = m.second; + graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P + } + j += 1; + } + + // Add a prior on pose x1. This indirectly specifies where the origin is. + // and a prior on the position of the first landmark to fix the scale + graph.push_back(PriorFactor(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); + graph.push_back(PriorFactor (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.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); + + /** --------------- COMPARISON -----------------------**/ + /** ----------------------------------------------------**/ + + LevenbergMarquardtParams params_using_COLAMD, params_using_METIS; + try { + params_using_METIS.setVerbosity("ERROR"); + gttic_(METIS_ORDERING); + params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph); + gttoc_(METIS_ORDERING); + + params_using_COLAMD.setVerbosity("ERROR"); + gttic_(COLAMD_ORDERING); + params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph); + gttoc_(COLAMD_ORDERING); + } catch (exception& e) { + cout << e.what(); + } + + // expect they have different ordering results + if(params_using_COLAMD.ordering == params_using_METIS.ordering) { + cout << "COLAMD and METIS produce the same ordering. " + << "Problem here!!!" << endl; + } + + /* Optimize the graph with METIS and COLAMD and time the results */ + + Values result_METIS, result_COLAMD; + try { + gttic_(OPTIMIZE_WITH_METIS); + LevenbergMarquardtOptimizer lm_METIS(graph, initial, params_using_METIS); + result_METIS = lm_METIS.optimize(); + gttoc_(OPTIMIZE_WITH_METIS); + + gttic_(OPTIMIZE_WITH_COLAMD); + LevenbergMarquardtOptimizer lm_COLAMD(graph, initial, params_using_COLAMD); + result_COLAMD = lm_COLAMD.optimize(); + gttoc_(OPTIMIZE_WITH_COLAMD); + } catch (exception& e) { + cout << e.what(); + } + + + { // printing the result + + cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl; + cout << "METIS final error: " << graph.error(result_METIS) << endl; + + cout << endl << endl; + + cout << "Time comparison by solving " << filename << " results:" << endl; + cout << boost::format("%1% point tracks and %2% cameras\n") \ + % mydata.number_tracks() % mydata.number_cameras() \ + << endl; + + tictoc_print_(); + } + + + return 0; +} +/* ************************************************************************* */ + diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp index 8ebf005ab..f5702e7fb 100644 --- a/examples/SelfCalibrationExample.cpp +++ b/examples/SelfCalibrationExample.cpp @@ -82,7 +82,7 @@ int main(int argc, char* argv[]) { Values initialEstimate; initialEstimate.insert(Symbol('K', 0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0)); 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)))); + initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); for (size_t j = 0; j < points.size(); ++j) initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 923b0b9de..2000b348b 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -31,28 +31,30 @@ * */ -#include -#include -#include -#include -#include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include +#include +#include +#include +#include +#include +#include // for GTSAM_USE_TBB -#include -#include -#include #include -#include +#include #include #include #include +#include + +#include +#include #ifdef GTSAM_USE_TBB #include @@ -72,23 +74,6 @@ typedef NoiseModelFactor1 NM1; typedef NoiseModelFactor2 NM2; typedef BearingRangeFactor BR; -//GTSAM_VALUE_EXPORT(Value); -//GTSAM_VALUE_EXPORT(Pose); -//GTSAM_VALUE_EXPORT(Rot2); -//GTSAM_VALUE_EXPORT(Point2); -//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) // In ocaml, +1 was added to the observations to account for the prior, but @@ -269,12 +254,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); @@ -303,7 +288,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; @@ -589,7 +576,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/TimeTBB.cpp b/examples/TimeTBB.cpp index a35980836..602a00593 100644 --- a/examples/TimeTBB.cpp +++ b/examples/TimeTBB.cpp @@ -17,6 +17,7 @@ #include #include + #include #include #include diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index a5b91ff38..75c0a2fa5 100644 --- a/examples/VisualISAM2Example.cpp +++ b/examples/VisualISAM2Example.cpp @@ -95,7 +95,7 @@ int main(int argc, char* argv[]) { // Add an initial guess for the current pose // Intentionally initialize the variables off from the ground truth - initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); // If this is the first iteration, add a prior on the first pose to set the coordinate frame // and a prior on the first landmark to set the scale diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index 8abe84eb6..9380410ea 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -95,7 +95,7 @@ int main(int argc, char* argv[]) { } // Intentionally initialize the variables off from the ground truth - Pose3 noise(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); + Pose3 noise(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); Pose3 initial_xi = poses[i].compose(noise); // Add an initial guess for the current pose diff --git a/gtsam.h b/gtsam.h index c5528309e..ca014ad5d 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1,4 +1,5 @@ /** + * GTSAM Wrap Module Definition * * These are the current classes available through the matlab toolbox interface, @@ -156,7 +157,7 @@ virtual class Value { size_t dim() const; }; -#include +#include class LieScalar { // Standard constructors LieScalar(); @@ -185,7 +186,7 @@ class LieScalar { static Vector Logmap(const gtsam::LieScalar& p); }; -#include +#include class LieVector { // Standard constructors LieVector(); @@ -217,7 +218,7 @@ class LieVector { void serialize() const; }; -#include +#include class LieMatrix { // Standard constructors LieMatrix(); @@ -288,6 +289,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(); @@ -304,8 +331,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; @@ -414,7 +439,7 @@ class Rot3 { static gtsam::Rot3 roll(double t); // positive roll is to right (increasing yaw in aircraft) static gtsam::Rot3 ypr(double y, double p, double r); static gtsam::Rot3 quaternion(double w, double x, double y, double z); - static gtsam::Rot3 rodriguez(Vector v); + static gtsam::Rot3 Rodrigues(Vector v); // Testable void print(string s) const; @@ -550,6 +575,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 @@ -778,7 +813,7 @@ class CalibratedCamera { // Action on Point3 gtsam::Point2 project(const gtsam::Point3& point) const; - static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); // Standard Interface gtsam::Pose3 pose() const; @@ -788,56 +823,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; @@ -854,7 +849,7 @@ class PinholeCamera { static size_t Dim(); // Transformations and measurement functions - static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); + static gtsam::Point2 Project(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; @@ -865,6 +860,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(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(); @@ -877,7 +916,7 @@ class StereoCamera { // Standard Interface gtsam::Pose3 pose() const; double baseline() const; - gtsam::Cal3_S2Stereo* calibration() const; + gtsam::Cal3_S2Stereo calibration() const; // Manifold gtsam::StereoCamera retract(const Vector& d) const; @@ -893,6 +932,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 //************************************************************************* @@ -1606,12 +1655,12 @@ char symbolChr(size_t key); size_t symbolIndex(size_t key); // Default keyformatter -void printKeyList (const gtsam::KeyList& keys); -void printKeyList (const gtsam::KeyList& keys, string s); -void printKeyVector(const gtsam::KeyVector& keys); -void printKeyVector(const gtsam::KeyVector& keys, string s); -void printKeySet (const gtsam::KeySet& keys); -void printKeySet (const gtsam::KeySet& keys, string s); +void PrintKeyList (const gtsam::KeyList& keys); +void PrintKeyList (const gtsam::KeyList& keys, string s); +void PrintKeyVector(const gtsam::KeyVector& keys); +void PrintKeyVector(const gtsam::KeyVector& keys, string s); +void PrintKeySet (const gtsam::KeySet& keys); +void PrintKeySet (const gtsam::KeySet& keys, string s); #include class LabeledSymbol { @@ -1728,7 +1777,7 @@ class Values { void swap(gtsam::Values& values); bool exists(size_t j) const; - gtsam::KeyList keys() const; + gtsam::KeyVector keys() const; gtsam::VectorValues zeroVectors() const; @@ -1845,8 +1894,6 @@ class KeySet { class KeyVector { KeyVector(); KeyVector(const gtsam::KeyVector& other); - KeyVector(const gtsam::KeySet& other); - KeyVector(const gtsam::KeyList& other); // Note: no print function @@ -2176,9 +2223,6 @@ class NonlinearISAM { //************************************************************************* // Nonlinear factor types //************************************************************************* -#include -#include -#include #include #include #include @@ -2219,7 +2263,7 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor { }; -#include +#include template virtual class RangeFactor : gtsam::NoiseModelFactor { RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); @@ -2229,20 +2273,16 @@ typedef gtsam::RangeFactor RangeFactorPosePoint2; typedef gtsam::RangeFactor RangeFactorPosePoint3; typedef gtsam::RangeFactor RangeFactorPose2; typedef gtsam::RangeFactor RangeFactorPose3; - -// 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; +typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; +typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; +typedef gtsam::RangeFactor RangeFactorCalibratedCamera; +typedef gtsam::RangeFactor RangeFactorSimpleCamera; -#include -template +#include +template virtual class BearingFactor : gtsam::NoiseModelFactor { - BearingFactor(size_t key1, size_t key2, const ROTATION& measured, const gtsam::noiseModel::Base* noiseModel); + BearingFactor(size_t key1, size_t key2, const BEARING& measured, const gtsam::noiseModel::Base* noiseModel); // enabling serialization functionality void serialize() const; @@ -2250,19 +2290,18 @@ virtual class BearingFactor : gtsam::NoiseModelFactor { typedef gtsam::BearingFactor BearingFactor2D; - -#include -template +#include +template virtual class BearingRangeFactor : gtsam::NoiseModelFactor { - BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel); - - pair measured() const; + BearingRangeFactor(size_t poseKey, size_t pointKey, + const BEARING& measuredBearing, const RANGE& measuredRange, + const gtsam::noiseModel::Base* noiseModel); // enabling serialization functionality void serialize() const; }; -typedef gtsam::BearingRangeFactor BearingRangeFactor2D; +typedef gtsam::BearingRangeFactor BearingRangeFactor2D; #include @@ -2309,24 +2348,39 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { void serialize() const; }; +#include +class SmartProjectionParams { + SmartProjectionParams(); + // TODO(frank): make these work: + // void setLinearizationMode(LinearizationMode linMode); + // void setDegeneracyMode(DegeneracyMode degMode); + void setRankTolerance(double rankTol); + void setEnableEPI(bool enableEPI); + void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); + void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); +}; + #include -template -virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { +template +virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor { - SmartProjectionPoseFactor(double rankTol, double linThreshold, - bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::Pose3& body_P_sensor); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::Pose3& body_P_sensor, + const gtsam::SmartProjectionParams& params); - 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); // enabling serialization functionality //void serialize() const; }; -typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; +typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; #include @@ -2429,18 +2483,18 @@ class ConstantBias { class PoseVelocityBias{ PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias); }; -class ImuFactorPreintegratedMeasurements { +class PreintegratedImuMeasurements { // 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); + PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration); + PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); + // PreintegratedImuMeasurements(const gtsam::PreintegratedImuMeasurements& rhs); // Testable void print(string s) const; - bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol); + bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); double deltaTij() const; - Matrix deltaRij() const; + gtsam::Rot3 deltaRij() const; Vector deltaPij() const; Vector deltaVij() const; Vector biasHatVector() const; @@ -2453,25 +2507,24 @@ class ImuFactorPreintegratedMeasurements { // 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 { ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, - const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis); + const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis); ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, - const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis, + const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis, const gtsam::Pose3& body_P_sensor); // Standard Interface - gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; + gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; }; #include -class CombinedImuFactorPreintegratedMeasurements { +class PreintegratedCombinedMeasurements { // Standard Constructor - CombinedImuFactorPreintegratedMeasurements( + PreintegratedCombinedMeasurements( const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance, Matrix measuredOmegaCovariance, @@ -2479,7 +2532,7 @@ class CombinedImuFactorPreintegratedMeasurements { Matrix biasAccCovariance, Matrix biasOmegaCovariance, Matrix biasAccOmegaInit); - CombinedImuFactorPreintegratedMeasurements( + PreintegratedCombinedMeasurements( const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance, Matrix measuredOmegaCovariance, @@ -2488,14 +2541,14 @@ class CombinedImuFactorPreintegratedMeasurements { Matrix biasOmegaCovariance, Matrix biasAccOmegaInit, bool use2ndOrderIntegration); - // CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs); + // PreintegratedCombinedMeasurements(const gtsam::PreintegratedCombinedMeasurements& rhs); // Testable void print(string s) const; - bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol); + bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, double tol); double deltaTij() const; - Matrix deltaRij() const; + gtsam::Rot3 deltaRij() const; Vector deltaPij() const; Vector deltaVij() const; Vector biasHatVector() const; @@ -2508,53 +2561,51 @@ class CombinedImuFactorPreintegratedMeasurements { // 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); + const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); // Standard Interface - gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; + gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; }; #include -class AHRSFactorPreintegratedMeasurements { +class PreintegratedAhrsMeasurements { // Standard Constructor - AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance); - AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance); - AHRSFactorPreintegratedMeasurements(const gtsam::AHRSFactorPreintegratedMeasurements& rhs); + PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); + PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); + PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); // Testable void print(string s) const; - bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol); + bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol); // get Data - Matrix deltaRij() const; + gtsam::Rot3 deltaRij() const; double deltaTij() const; Vector biasHat() const; // Standard Interface void integrateMeasurement(Vector measuredOmega, double deltaT); - void integrateMeasurement(Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); void resetIntegration() ; }; virtual class AHRSFactor : gtsam::NonlinearFactor { AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, - const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis); + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis); AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, - const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis, const gtsam::Pose3& body_P_sensor); // Standard Interface - gtsam::AHRSFactorPreintegratedMeasurements preintegratedMeasurements() const; + gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, Vector bias) const; gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, - const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis) const; }; diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 301548dcf..4adbfb250 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -58,10 +58,10 @@ add_subdirectory(ceres) 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}) diff --git a/gtsam/3rdparty/Eigen/.hg_archival.txt b/gtsam/3rdparty/Eigen/.hg_archival.txt index 9fceca658..8c88afc32 100644 --- a/gtsam/3rdparty/Eigen/.hg_archival.txt +++ b/gtsam/3rdparty/Eigen/.hg_archival.txt @@ -1,4 +1,4 @@ repo: 8a21fd850624c931e448cbcfb38168cb2717c790 -node: ffa86ffb557094721ca71dcea6aed2651b9fd610 +node: b30b87236a1b1552af32ac34075ee5696a9b5a33 branch: 3.2 -tag: 3.2.0 +tag: 3.2.7 diff --git a/gtsam/3rdparty/Eigen/.hgtags b/gtsam/3rdparty/Eigen/.hgtags index 1b9b1142e..c4ccd33fa 100644 --- a/gtsam/3rdparty/Eigen/.hgtags +++ b/gtsam/3rdparty/Eigen/.hgtags @@ -23,3 +23,10 @@ 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 +10219c95fe653d4962aa9db4946f6fbea96dd740 3.2.4 +bdd17ee3b1b3a166cd5ec36dcad4fc1f3faf774a 3.2.5 +c58038c56923e0fd86de3ded18e03df442e66dfb 3.2.6 diff --git a/gtsam/3rdparty/Eigen/CMakeLists.txt b/gtsam/3rdparty/Eigen/CMakeLists.txt index 76a11b9d2..ed3e67fe9 100644 --- a/gtsam/3rdparty/Eigen/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/CMakeLists.txt @@ -301,7 +301,7 @@ if(EIGEN_INCLUDE_INSTALL_DIR) ) else() set(INCLUDE_INSTALL_DIR - "${CMAKE_INSTALL_PREFIX}/include/eigen3" + "include/eigen3" CACHE INTERNAL "The directory where we install the header files (internal)" ) @@ -404,7 +404,7 @@ if(cmake_generator_tolower MATCHES "makefile") message(STATUS "make install | Install to ${CMAKE_INSTALL_PREFIX}. To change that:") message(STATUS " | cmake . -DCMAKE_INSTALL_PREFIX=yourpath") message(STATUS " | Eigen headers will then be installed to:") - message(STATUS " | ${INCLUDE_INSTALL_DIR}") + message(STATUS " | ${CMAKE_INSTALL_PREFIX}/${INCLUDE_INSTALL_DIR}") message(STATUS " | To install Eigen headers to a separate location, do:") message(STATUS " | cmake . -DEIGEN_INCLUDE_INSTALL_DIR=yourpath") message(STATUS "make doc | Generate the API documentation, requires Doxygen & LaTeX") diff --git a/gtsam/3rdparty/Eigen/Eigen/Core b/gtsam/3rdparty/Eigen/Eigen/Core index c87f99df3..509c529e1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/Core +++ b/gtsam/3rdparty/Eigen/Eigen/Core @@ -123,7 +123,7 @@ #undef bool #undef vector #undef pixel - #elif defined __ARM_NEON__ + #elif defined __ARM_NEON #define EIGEN_VECTORIZE #define EIGEN_VECTORIZE_NEON #include diff --git a/gtsam/3rdparty/Eigen/Eigen/SparseCore b/gtsam/3rdparty/Eigen/Eigen/SparseCore index 9b5be5e15..24bcf0156 100644 --- a/gtsam/3rdparty/Eigen/Eigen/SparseCore +++ b/gtsam/3rdparty/Eigen/Eigen/SparseCore @@ -14,7 +14,7 @@ /** * \defgroup SparseCore_Module SparseCore module * - * This module provides a sparse matrix representation, and basic associatd matrix manipulations + * This module provides a sparse matrix representation, and basic associated matrix manipulations * and operations. * * See the \ref TutorialSparse "Sparse tutorial" diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h index c52b7d1a6..abd30bd91 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h @@ -235,6 +235,11 @@ template class LDLT } protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } /** \internal * Used to compute and store the Cholesky decomposition A = L D L^* = U^* D U. @@ -434,6 +439,8 @@ template struct LDLT_Traits template LDLT& LDLT::compute(const MatrixType& a) { + check_template_parameters(); + eigen_assert(a.rows()==a.cols()); const Index size = a.rows(); @@ -442,6 +449,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); @@ -456,7 +464,7 @@ LDLT& LDLT::compute(const MatrixType& a) */ template template -LDLT& LDLT::rankUpdate(const MatrixBase& w, const typename NumTraits::Real& sigma) +LDLT& LDLT::rankUpdate(const MatrixBase& w, const typename LDLT::RealScalar& sigma) { const Index size = w.rows(); if (m_isInitialized) @@ -502,7 +510,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/Cholesky/LLT.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h index 2e6189f7d..7c11a2dc2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h @@ -174,6 +174,12 @@ template class LLT LLT rankUpdate(const VectorType& vec, const RealScalar& sigma = 1); protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } + /** \internal * Used to compute and store L * The strict upper part is not used and even not initialized. @@ -283,7 +289,7 @@ template struct llt_inplace return k; mat.coeffRef(k,k) = x = sqrt(x); if (k>0 && rs>0) A21.noalias() -= A20 * A10.adjoint(); - if (rs>0) A21 *= RealScalar(1)/x; + if (rs>0) A21 /= x; } return -1; } @@ -384,6 +390,8 @@ template struct LLT_Traits template LLT& LLT::compute(const MatrixType& a) { + check_template_parameters(); + eigen_assert(a.rows()==a.cols()); const Index size = a.rows(); m_matrix.resize(size, size); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT_MKL.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT_MKL.h index b9bcb5240..f7c365aee 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT_MKL.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT_MKL.h @@ -60,7 +60,7 @@ template<> struct mkl_llt \ lda = m.outerStride(); \ \ info = LAPACKE_##MKLPREFIX##potrf( matrix_order, uplo, size, (MKLTYPE*)a, lda ); \ - info = (info==0) ? -1 : 1; \ + info = (info==0) ? -1 : info>0 ? info-1 : size; \ return info; \ } \ }; \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h index c449960de..99dbe171c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h @@ -78,7 +78,7 @@ cholmod_sparse viewAsCholmod(SparseMatrix<_Scalar,_Options,_Index>& mat) { res.itype = CHOLMOD_INT; } - else if (internal::is_same<_Index,UF_long>::value) + else if (internal::is_same<_Index,SuiteSparse_long>::value) { res.itype = CHOLMOD_LONG; } @@ -395,7 +395,7 @@ class CholmodSimplicialLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimpl CholmodSimplicialLLT(const MatrixType& matrix) : Base() { init(); - compute(matrix); + Base::compute(matrix); } ~CholmodSimplicialLLT() {} @@ -442,7 +442,7 @@ class CholmodSimplicialLDLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimp CholmodSimplicialLDLT(const MatrixType& matrix) : Base() { init(); - compute(matrix); + Base::compute(matrix); } ~CholmodSimplicialLDLT() {} @@ -487,7 +487,7 @@ class CholmodSupernodalLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSuper CholmodSupernodalLLT(const MatrixType& matrix) : Base() { init(); - compute(matrix); + Base::compute(matrix); } ~CholmodSupernodalLLT() {} @@ -534,7 +534,7 @@ class CholmodDecomposition : public CholmodBase<_MatrixType, _UpLo, CholmodDecom CholmodDecomposition(const MatrixType& matrix) : Base() { init(); - compute(matrix); + Base::compute(matrix); } ~CholmodDecomposition() {} diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h index 0ab03eff0..0b9c38c82 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h @@ -124,6 +124,21 @@ class Array } #endif +#ifdef EIGEN_HAVE_RVALUE_REFERENCES + Array(Array&& other) + : Base(std::move(other)) + { + Base::_check_template_params(); + if (RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic) + Base::_set_noalias(other); + } + Array& operator=(Array&& other) + { + other.swap(*this); + return *this; + } +#endif + /** Constructs a vector or row-vector with given dimension. \only_for_vectors * * Note that this is only useful for dynamic-size vectors. For fixed-size vectors, diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h index 38852600d..33ff55371 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h @@ -46,9 +46,6 @@ template class ArrayBase typedef ArrayBase Eigen_BaseClassForSpecializationOfGlobalMathFuncImpl; - using internal::special_scalar_op_base::Scalar, - typename NumTraits::Scalar>::Real>::operator*; - typedef typename internal::traits::StorageKind StorageKind; typedef typename internal::traits::Index Index; typedef typename internal::traits::Scalar Scalar; @@ -56,6 +53,7 @@ template class ArrayBase typedef typename NumTraits::Real RealScalar; typedef DenseBase Base; + using Base::operator*; using Base::RowsAtCompileTime; using Base::ColsAtCompileTime; using Base::SizeAtCompileTime; 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/Assign.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign.h index 1dccc2f42..f48173172 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign.h @@ -439,19 +439,26 @@ struct assign_impl PacketTraits; + typedef typename Derived1::Scalar Scalar; + typedef packet_traits PacketTraits; enum { packetSize = PacketTraits::size, alignable = PacketTraits::AlignedOnScalar, - dstAlignment = alignable ? Aligned : int(assign_traits::DstIsAligned) , + dstIsAligned = assign_traits::DstIsAligned, + dstAlignment = alignable ? Aligned : int(dstIsAligned), srcAlignment = assign_traits::JointAlignment }; + const Scalar *dst_ptr = &dst.coeffRef(0,0); + if((!bool(dstIsAligned)) && (size_t(dst_ptr) % sizeof(Scalar))>0) + { + // the pointer is not aligend-on scalar, so alignment is not possible + return assign_impl::run(dst, src); + } const Index packetAlignedMask = packetSize - 1; const Index innerSize = dst.innerSize(); const Index outerSize = dst.outerSize(); const Index alignedStep = alignable ? (packetSize - dst.outerStride() % packetSize) & packetAlignedMask : 0; - Index alignedStart = ((!alignable) || assign_traits::DstIsAligned) ? 0 - : internal::first_aligned(&dst.coeffRef(0,0), innerSize); + Index alignedStart = ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned(dst_ptr, innerSize); for(Index outer = 0; outer < outerSize; ++outer) { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h index 87bedfa46..827894443 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h @@ -66,8 +66,9 @@ struct traits > : traits::MaxColsAtCompileTime), XprTypeIsRowMajor = (int(traits::Flags)&RowMajorBit) != 0, - IsRowMajor = (MaxRowsAtCompileTime==1&&MaxColsAtCompileTime!=1) ? 1 - : (MaxColsAtCompileTime==1&&MaxRowsAtCompileTime!=1) ? 0 + IsDense = is_same::value, + IsRowMajor = (IsDense&&MaxRowsAtCompileTime==1&&MaxColsAtCompileTime!=1) ? 1 + : (IsDense&&MaxColsAtCompileTime==1&&MaxRowsAtCompileTime!=1) ? 0 : XprTypeIsRowMajor, HasSameStorageOrderAsXprType = (IsRowMajor == XprTypeIsRowMajor), InnerSize = IsRowMajor ? int(ColsAtCompileTime) : int(RowsAtCompileTime), diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h.orig b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h.orig deleted file mode 100644 index a036d8c3b..000000000 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h.orig +++ /dev/null @@ -1,154 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008 Gael Guennebaud -// Copyright (C) 2006-2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_COMMAINITIALIZER_H -#define EIGEN_COMMAINITIALIZER_H - -namespace Eigen { - -/** \class CommaInitializer - * \ingroup Core_Module - * - * \brief Helper class used by the comma initializer operator - * - * This class is internally used to implement the comma initializer feature. It is - * the return type of MatrixBase::operator<<, and most of the time this is the only - * way it is used. - * - * \sa \ref MatrixBaseCommaInitRef "MatrixBase::operator<<", CommaInitializer::finished() - */ -template -struct CommaInitializer -{ - typedef typename XprType::Scalar Scalar; - typedef typename XprType::Index Index; - - inline CommaInitializer(XprType& xpr, const Scalar& s) - : m_xpr(xpr), m_row(0), m_col(1), m_currentBlockRows(1) - { - m_xpr.coeffRef(0,0) = s; - } - - template - inline CommaInitializer(XprType& xpr, const DenseBase& other) - : m_xpr(xpr), m_row(0), m_col(other.cols()), m_currentBlockRows(other.rows()) - { - m_xpr.block(0, 0, other.rows(), other.cols()) = other; - } - - /* Copy/Move constructor which transfers ownership. This is crucial in - * absence of return value optimization to avoid assertions during destruction. */ - // FIXME in C++11 mode this could be replaced by a proper RValue constructor - inline CommaInitializer(const CommaInitializer& o) - : m_xpr(o.m_xpr), m_row(o.m_row), m_col(o.m_col), m_currentBlockRows(o.m_currentBlockRows) { - // Mark original object as finished. In absence of R-value references we need to const_cast: - const_cast(o).m_row = m_xpr.rows(); - const_cast(o).m_col = m_xpr.cols(); - const_cast(o).m_currentBlockRows = 0; - } - - /* inserts a scalar value in the target matrix */ - CommaInitializer& operator,(const Scalar& s) - { - if (m_col==m_xpr.cols()) - { - m_row+=m_currentBlockRows; - m_col = 0; - m_currentBlockRows = 1; - eigen_assert(m_row - CommaInitializer& operator,(const DenseBase& other) - { - if(other.cols()==0 || other.rows()==0) - return *this; - if (m_col==m_xpr.cols()) - { - m_row+=m_currentBlockRows; - m_col = 0; - m_currentBlockRows = other.rows(); - eigen_assert(m_row+m_currentBlockRows<=m_xpr.rows() - && "Too many rows passed to comma initializer (operator<<)"); - } - eigen_assert(m_col - (m_row, m_col) = other; - else - m_xpr.block(m_row, m_col, other.rows(), other.cols()) = other; - m_col += other.cols(); - return *this; - } - - inline ~CommaInitializer() - { - eigen_assert((m_row+m_currentBlockRows) == m_xpr.rows() - && m_col == m_xpr.cols() - && "Too few coefficients passed to comma initializer (operator<<)"); - } - - /** \returns the built matrix once all its coefficients have been set. - * Calling finished is 100% optional. Its purpose is to write expressions - * like this: - * \code - * quaternion.fromRotationMatrix((Matrix3f() << axis0, axis1, axis2).finished()); - * \endcode - */ - inline XprType& finished() { return m_xpr; } - - XprType& m_xpr; // target expression - Index m_row; // current row id - Index m_col; // current col id - Index m_currentBlockRows; // current block height -}; - -/** \anchor MatrixBaseCommaInitRef - * Convenient operator to set the coefficients of a matrix. - * - * The coefficients must be provided in a row major order and exactly match - * the size of the matrix. Otherwise an assertion is raised. - * - * Example: \include MatrixBase_set.cpp - * Output: \verbinclude MatrixBase_set.out - * - * \note According the c++ standard, the argument expressions of this comma initializer are evaluated in arbitrary order. - * - * \sa CommaInitializer::finished(), class CommaInitializer - */ -template -inline CommaInitializer DenseBase::operator<< (const Scalar& s) -{ - return CommaInitializer(*static_cast(this), s); -} - -/** \sa operator<<(const Scalar&) */ -template -template -inline CommaInitializer -DenseBase::operator<<(const DenseBase& other) -{ - return CommaInitializer(*static_cast(this), other); -} - -} // end namespace Eigen - -#endif // EIGEN_COMMAINITIALIZER_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseBinaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseBinaryOp.h index 586f77aaf..519a866e6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseBinaryOp.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseBinaryOp.h @@ -81,7 +81,8 @@ struct traits > ) ), Flags = (Flags0 & ~RowMajorBit) | (LhsFlags & RowMajorBit), - CoeffReadCost = LhsCoeffReadCost + RhsCoeffReadCost + functor_traits::Cost + Cost0 = EIGEN_ADD_COST(LhsCoeffReadCost,RhsCoeffReadCost), + CoeffReadCost = EIGEN_ADD_COST(Cost0,functor_traits::Cost) }; }; } // end namespace internal diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryOp.h index f2de749f9..f7ee60e98 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryOp.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryOp.h @@ -47,7 +47,7 @@ struct traits > Flags = _XprTypeNested::Flags & ( HereditaryBits | LinearAccessBit | AlignedBit | (functor_traits::PacketAccess ? PacketAccessBit : 0)), - CoeffReadCost = _XprTypeNested::CoeffReadCost + functor_traits::Cost + CoeffReadCost = EIGEN_ADD_COST(_XprTypeNested::CoeffReadCost, functor_traits::Cost) }; }; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h index c5800f6c8..354c739f7 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h @@ -41,14 +41,13 @@ static inline void check_DenseIndex_is_signed() { template class DenseBase #ifndef EIGEN_PARSED_BY_DOXYGEN : public internal::special_scalar_op_base::Scalar, - typename NumTraits::Scalar>::Real> + typename NumTraits::Scalar>::Real, + DenseCoeffsBase > #else : public DenseCoeffsBase #endif // not EIGEN_PARSED_BY_DOXYGEN { public: - using internal::special_scalar_op_base::Scalar, - typename NumTraits::Scalar>::Real>::operator*; class InnerIterator; @@ -63,8 +62,9 @@ template class DenseBase typedef typename internal::traits::Scalar Scalar; typedef typename internal::packet_traits::type PacketScalar; typedef typename NumTraits::Real RealScalar; + typedef internal::special_scalar_op_base > Base; - typedef DenseCoeffsBase Base; + using Base::operator*; using Base::derived; using Base::const_cast_derived; using Base::rows; @@ -183,10 +183,6 @@ template class DenseBase /** \returns the number of nonzero coefficients which is in practice the number * of stored coefficients. */ inline Index nonZeros() const { return size(); } - /** \returns true if either the number of rows or the number of columns is equal to 1. - * In other words, this function returns - * \code rows()==1 || cols()==1 \endcode - * \sa rows(), cols(), IsVectorAtCompileTime. */ /** \returns the outer size. * @@ -266,11 +262,13 @@ template class DenseBase template Derived& operator=(const ReturnByValue& func); -#ifndef EIGEN_PARSED_BY_DOXYGEN - /** Copies \a other into *this without evaluating other. \returns a reference to *this. */ + /** \internal Copies \a other into *this without evaluating other. \returns a reference to *this. */ template Derived& lazyAssign(const DenseBase& other); -#endif // not EIGEN_PARSED_BY_DOXYGEN + + /** \internal Evaluates \a other into *this. \returns a reference to *this. */ + template + Derived& lazyAssign(const ReturnByValue& other); CommaInitializer operator<< (const Scalar& s); @@ -462,8 +460,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/DenseStorage.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h index a72738e55..568493cba 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h @@ -122,33 +122,41 @@ template class DenseSt { internal::plain_array m_data; public: - inline DenseStorage() {} - inline DenseStorage(internal::constructor_without_unaligned_array_assert) + DenseStorage() {} + DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(internal::constructor_without_unaligned_array_assert()) {} - inline DenseStorage(DenseIndex,DenseIndex,DenseIndex) {} - inline void swap(DenseStorage& other) { std::swap(m_data,other.m_data); } - static inline DenseIndex rows(void) {return _Rows;} - static inline DenseIndex cols(void) {return _Cols;} - inline void conservativeResize(DenseIndex,DenseIndex,DenseIndex) {} - inline void resize(DenseIndex,DenseIndex,DenseIndex) {} - inline const T *data() const { return m_data.array; } - inline T *data() { return m_data.array; } + DenseStorage(const DenseStorage& other) : m_data(other.m_data) {} + DenseStorage& operator=(const DenseStorage& other) + { + if (this != &other) m_data = other.m_data; + return *this; + } + DenseStorage(DenseIndex,DenseIndex,DenseIndex) {} + void swap(DenseStorage& other) { std::swap(m_data,other.m_data); } + static DenseIndex rows(void) {return _Rows;} + static DenseIndex cols(void) {return _Cols;} + void conservativeResize(DenseIndex,DenseIndex,DenseIndex) {} + void resize(DenseIndex,DenseIndex,DenseIndex) {} + const T *data() const { return m_data.array; } + T *data() { return m_data.array; } }; // null matrix template class DenseStorage { public: - inline DenseStorage() {} - inline DenseStorage(internal::constructor_without_unaligned_array_assert) {} - inline DenseStorage(DenseIndex,DenseIndex,DenseIndex) {} - inline void swap(DenseStorage& ) {} - static inline DenseIndex rows(void) {return _Rows;} - static inline DenseIndex cols(void) {return _Cols;} - inline void conservativeResize(DenseIndex,DenseIndex,DenseIndex) {} - inline void resize(DenseIndex,DenseIndex,DenseIndex) {} - inline const T *data() const { return 0; } - inline T *data() { return 0; } + DenseStorage() {} + DenseStorage(internal::constructor_without_unaligned_array_assert) {} + DenseStorage(const DenseStorage&) {} + DenseStorage& operator=(const DenseStorage&) { return *this; } + DenseStorage(DenseIndex,DenseIndex,DenseIndex) {} + void swap(DenseStorage& ) {} + static DenseIndex rows(void) {return _Rows;} + static DenseIndex cols(void) {return _Cols;} + void conservativeResize(DenseIndex,DenseIndex,DenseIndex) {} + void resize(DenseIndex,DenseIndex,DenseIndex) {} + const T *data() const { return 0; } + T *data() { return 0; } }; // more specializations for null matrices; these are necessary to resolve ambiguities @@ -168,18 +176,29 @@ template class DenseStorage class DenseStorage m_data; DenseIndex m_rows; public: - inline DenseStorage() : m_rows(0) {} - inline DenseStorage(internal::constructor_without_unaligned_array_assert) + DenseStorage() : m_rows(0) {} + DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(0) {} - inline DenseStorage(DenseIndex, DenseIndex nbRows, DenseIndex) : m_rows(nbRows) {} - inline void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); } - inline DenseIndex rows(void) const {return m_rows;} - inline DenseIndex cols(void) const {return _Cols;} - inline void conservativeResize(DenseIndex, DenseIndex nbRows, DenseIndex) { m_rows = nbRows; } - inline void resize(DenseIndex, DenseIndex nbRows, DenseIndex) { m_rows = nbRows; } - inline const T *data() const { return m_data.array; } - inline T *data() { return m_data.array; } + DenseStorage(const DenseStorage& other) : m_data(other.m_data), m_rows(other.m_rows) {} + DenseStorage& operator=(const DenseStorage& other) + { + if (this != &other) + { + m_data = other.m_data; + m_rows = other.m_rows; + } + return *this; + } + DenseStorage(DenseIndex, DenseIndex nbRows, DenseIndex) : m_rows(nbRows) {} + void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); } + DenseIndex rows(void) const {return m_rows;} + DenseIndex cols(void) const {return _Cols;} + void conservativeResize(DenseIndex, DenseIndex nbRows, DenseIndex) { m_rows = nbRows; } + void resize(DenseIndex, DenseIndex nbRows, DenseIndex) { m_rows = nbRows; } + const T *data() const { return m_data.array; } + T *data() { return m_data.array; } }; // dynamic-size matrix with fixed-size storage and fixed height @@ -207,17 +236,27 @@ template class DenseStorage m_data; DenseIndex m_cols; public: - inline DenseStorage() : m_cols(0) {} - inline DenseStorage(internal::constructor_without_unaligned_array_assert) + DenseStorage() : m_cols(0) {} + DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(internal::constructor_without_unaligned_array_assert()), m_cols(0) {} - inline DenseStorage(DenseIndex, DenseIndex, DenseIndex nbCols) : m_cols(nbCols) {} - inline void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); } - inline DenseIndex rows(void) const {return _Rows;} - inline DenseIndex cols(void) const {return m_cols;} - inline void conservativeResize(DenseIndex, DenseIndex, DenseIndex nbCols) { m_cols = nbCols; } - inline void resize(DenseIndex, DenseIndex, DenseIndex nbCols) { m_cols = nbCols; } - inline const T *data() const { return m_data.array; } - inline T *data() { return m_data.array; } + DenseStorage(const DenseStorage& other) : m_data(other.m_data), m_cols(other.m_cols) {} + DenseStorage& operator=(const DenseStorage& other) + { + if (this != &other) + { + m_data = other.m_data; + m_cols = other.m_cols; + } + return *this; + } + DenseStorage(DenseIndex, DenseIndex, DenseIndex nbCols) : m_cols(nbCols) {} + void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); } + DenseIndex rows(void) const {return _Rows;} + DenseIndex cols(void) const {return m_cols;} + void conservativeResize(DenseIndex, DenseIndex, DenseIndex nbCols) { m_cols = nbCols; } + void resize(DenseIndex, DenseIndex, DenseIndex nbCols) { m_cols = nbCols; } + const T *data() const { return m_data.array; } + T *data() { return m_data.array; } }; // purely dynamic matrix. @@ -227,18 +266,35 @@ template class DenseStorage(size)), m_rows(nbRows), m_cols(nbCols) { EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN } - inline ~DenseStorage() { internal::conditional_aligned_delete_auto(m_data, m_rows*m_cols); } - inline void swap(DenseStorage& other) +#ifdef EIGEN_HAVE_RVALUE_REFERENCES + DenseStorage(DenseStorage&& other) + : m_data(std::move(other.m_data)) + , m_rows(std::move(other.m_rows)) + , m_cols(std::move(other.m_cols)) + { + other.m_data = nullptr; + } + DenseStorage& operator=(DenseStorage&& other) + { + using std::swap; + swap(m_data, other.m_data); + swap(m_rows, other.m_rows); + swap(m_cols, other.m_cols); + return *this; + } +#endif + ~DenseStorage() { internal::conditional_aligned_delete_auto(m_data, m_rows*m_cols); } + void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); std::swap(m_cols,other.m_cols); } - inline DenseIndex rows(void) const {return m_rows;} - inline DenseIndex cols(void) const {return m_cols;} - inline void conservativeResize(DenseIndex size, DenseIndex nbRows, DenseIndex nbCols) + DenseIndex rows(void) const {return m_rows;} + DenseIndex cols(void) const {return m_cols;} + void conservativeResize(DenseIndex size, DenseIndex nbRows, DenseIndex nbCols) { m_data = internal::conditional_aligned_realloc_new_auto(m_data, size, m_rows*m_cols); m_rows = nbRows; @@ -258,8 +314,11 @@ template class DenseStorage class DenseStorage(size)), m_cols(nbCols) + DenseStorage() : m_data(0), m_cols(0) {} + DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(0), m_cols(0) {} + DenseStorage(DenseIndex size, DenseIndex, DenseIndex nbCols) : m_data(internal::conditional_aligned_new_auto(size)), m_cols(nbCols) { EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN } - inline ~DenseStorage() { internal::conditional_aligned_delete_auto(m_data, _Rows*m_cols); } - inline void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); } - static inline DenseIndex rows(void) {return _Rows;} - inline DenseIndex cols(void) const {return m_cols;} - inline void conservativeResize(DenseIndex size, DenseIndex, DenseIndex nbCols) +#ifdef EIGEN_HAVE_RVALUE_REFERENCES + DenseStorage(DenseStorage&& other) + : m_data(std::move(other.m_data)) + , m_cols(std::move(other.m_cols)) + { + other.m_data = nullptr; + } + DenseStorage& operator=(DenseStorage&& other) + { + using std::swap; + swap(m_data, other.m_data); + swap(m_cols, other.m_cols); + return *this; + } +#endif + ~DenseStorage() { internal::conditional_aligned_delete_auto(m_data, _Rows*m_cols); } + void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); } + static DenseIndex rows(void) {return _Rows;} + DenseIndex cols(void) const {return m_cols;} + void conservativeResize(DenseIndex size, DenseIndex, DenseIndex nbCols) { m_data = internal::conditional_aligned_realloc_new_auto(m_data, size, _Rows*m_cols); m_cols = nbCols; @@ -294,8 +368,11 @@ template class DenseStorage class DenseStorage(size)), m_rows(nbRows) + DenseStorage() : m_data(0), m_rows(0) {} + DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(0), m_rows(0) {} + DenseStorage(DenseIndex size, DenseIndex nbRows, DenseIndex) : m_data(internal::conditional_aligned_new_auto(size)), m_rows(nbRows) { EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN } - inline ~DenseStorage() { internal::conditional_aligned_delete_auto(m_data, _Cols*m_rows); } - inline void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); } - inline DenseIndex rows(void) const {return m_rows;} - static inline DenseIndex cols(void) {return _Cols;} - inline void conservativeResize(DenseIndex size, DenseIndex nbRows, DenseIndex) +#ifdef EIGEN_HAVE_RVALUE_REFERENCES + DenseStorage(DenseStorage&& other) + : m_data(std::move(other.m_data)) + , m_rows(std::move(other.m_rows)) + { + other.m_data = nullptr; + } + DenseStorage& operator=(DenseStorage&& other) + { + using std::swap; + swap(m_data, other.m_data); + swap(m_rows, other.m_rows); + return *this; + } +#endif + ~DenseStorage() { internal::conditional_aligned_delete_auto(m_data, _Cols*m_rows); } + void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); } + DenseIndex rows(void) const {return m_rows;} + static DenseIndex cols(void) {return _Cols;} + void conservativeResize(DenseIndex size, DenseIndex nbRows, DenseIndex) { m_data = internal::conditional_aligned_realloc_new_auto(m_data, size, m_rows*_Cols); m_rows = nbRows; @@ -330,8 +422,11 @@ template class DenseStorage::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/DiagonalProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalProduct.h index c03a0c2e1..cc6b536e1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalProduct.h @@ -34,8 +34,9 @@ struct traits > _Vectorizable = bool(int(MatrixType::Flags)&PacketAccessBit) && _SameTypes && (_ScalarAccessOnDiag || (bool(int(DiagonalType::DiagonalVectorType::Flags)&PacketAccessBit))), _LinearAccessMask = (RowsAtCompileTime==1 || ColsAtCompileTime==1) ? LinearAccessBit : 0, - Flags = ((HereditaryBits|_LinearAccessMask) & (unsigned int)(MatrixType::Flags)) | (_Vectorizable ? PacketAccessBit : 0) | AlignedBit,//(int(MatrixType::Flags)&int(DiagonalType::DiagonalVectorType::Flags)&AlignedBit), - CoeffReadCost = NumTraits::MulCost + MatrixType::CoeffReadCost + DiagonalType::DiagonalVectorType::CoeffReadCost + Flags = ((HereditaryBits|_LinearAccessMask|AlignedBit) & (unsigned int)(MatrixType::Flags)) | (_Vectorizable ? PacketAccessBit : 0),//(int(MatrixType::Flags)&int(DiagonalType::DiagonalVectorType::Flags)&AlignedBit), + Cost0 = EIGEN_ADD_COST(NumTraits::MulCost, MatrixType::CoeffReadCost), + CoeffReadCost = EIGEN_ADD_COST(Cost0,DiagonalType::DiagonalVectorType::CoeffReadCost) }; }; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h index b08b967ff..5f14c6587 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h @@ -259,6 +259,47 @@ template<> struct functor_traits { }; }; +/** \internal + * \brief Template functors for comparison of two scalars + * \todo Implement packet-comparisons + */ +template struct scalar_cmp_op; + +template +struct functor_traits > { + enum { + Cost = NumTraits::AddCost, + PacketAccess = false + }; +}; + +template +struct result_of(Scalar,Scalar)> { + typedef bool type; +}; + + +template struct scalar_cmp_op { + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_STRONG_INLINE bool operator()(const Scalar& a, const Scalar& b) const {return a==b;} +}; +template struct scalar_cmp_op { + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_STRONG_INLINE bool operator()(const Scalar& a, const Scalar& b) const {return a struct scalar_cmp_op { + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_STRONG_INLINE bool operator()(const Scalar& a, const Scalar& b) const {return a<=b;} +}; +template struct scalar_cmp_op { + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_STRONG_INLINE bool operator()(const Scalar& a, const Scalar& b) const {return !(a<=b || b<=a);} +}; +template struct scalar_cmp_op { + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_STRONG_INLINE bool operator()(const Scalar& a, const Scalar& b) const {return a!=b;} +}; + // unary functors: /** \internal diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h index 2a59d9464..0eae52990 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 @@ -257,7 +257,7 @@ template class GeneralProduct : public ProductBase, Lhs, Rhs> { - template struct IsRowMajor : internal::conditional<(int(T::Flags)&RowMajorBit), internal::true_type, internal::false_type>::type {}; + template struct is_row_major : internal::conditional<(int(T::Flags)&RowMajorBit), internal::true_type, internal::false_type>::type {}; public: EIGEN_PRODUCT_PUBLIC_INTERFACE(GeneralProduct) @@ -281,22 +281,22 @@ class GeneralProduct template inline void evalTo(Dest& dest) const { - internal::outer_product_selector_run(*this, dest, set(), IsRowMajor()); + internal::outer_product_selector_run(*this, dest, set(), is_row_major()); } template inline void addTo(Dest& dest) const { - internal::outer_product_selector_run(*this, dest, add(), IsRowMajor()); + internal::outer_product_selector_run(*this, dest, add(), is_row_major()); } template inline void subTo(Dest& dest) const { - internal::outer_product_selector_run(*this, dest, sub(), IsRowMajor()); + internal::outer_product_selector_run(*this, dest, sub(), is_row_major()); } template void scaleAndAddTo(Dest& dest, const Scalar& alpha) const { - internal::outer_product_selector_run(*this, dest, adds(alpha), IsRowMajor()); + internal::outer_product_selector_run(*this, dest, adds(alpha), is_row_major()); } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h index ab50c9b81..a9828f7f4 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h @@ -123,7 +123,7 @@ template class MapBase return internal::ploadt(m_data + index * innerStride()); } - inline MapBase(PointerType dataPtr) : m_data(dataPtr), m_rows(RowsAtCompileTime), m_cols(ColsAtCompileTime) + explicit inline MapBase(PointerType dataPtr) : m_data(dataPtr), m_rows(RowsAtCompileTime), m_cols(ColsAtCompileTime) { EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived) checkSanity(); @@ -157,7 +157,7 @@ template class MapBase internal::inner_stride_at_compile_time::ret==1), PACKET_ACCESS_REQUIRES_TO_HAVE_INNER_STRIDE_FIXED_TO_1); eigen_assert(EIGEN_IMPLIES(internal::traits::Flags&AlignedBit, (size_t(m_data) % 16) == 0) - && "data is not aligned"); + && "input pointer is not aligned on a 16 byte boundary"); } PointerType m_data; @@ -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/MathFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h index 2bfc5ebd9..adf2f9c51 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h @@ -294,7 +294,7 @@ struct hypot_impl RealScalar _x = abs(x); RealScalar _y = abs(y); RealScalar p = (max)(_x, _y); - if(p==RealScalar(0)) return 0; + if(p==RealScalar(0)) return RealScalar(0); RealScalar q = (min)(_x, _y); RealScalar qp = q/p; return p * sqrt(RealScalar(1) + qp*qp); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h index d7d0b5b9a..02be142d8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h @@ -211,6 +211,21 @@ class Matrix : Base(internal::constructor_without_unaligned_array_assert()) { Base::_check_template_params(); EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED } +#ifdef EIGEN_HAVE_RVALUE_REFERENCES + Matrix(Matrix&& other) + : Base(std::move(other)) + { + Base::_check_template_params(); + if (RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic) + Base::_set_noalias(other); + } + Matrix& operator=(Matrix&& other) + { + other.swap(*this); + return *this; + } +#endif + /** \brief Constructs a vector or row-vector with given dimension. \only_for_vectors * * Note that this is only useful for dynamic-size vectors. For fixed-size vectors, diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h index 344b38f2f..e83ef4dc0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h @@ -159,13 +159,11 @@ template class MatrixBase template Derived& operator=(const ReturnByValue& other); -#ifndef EIGEN_PARSED_BY_DOXYGEN template Derived& lazyAssign(const ProductBase& other); template Derived& lazyAssign(const MatrixPowerProduct& other); -#endif // not EIGEN_PARSED_BY_DOXYGEN template Derived& operator+=(const MatrixBase& other); @@ -215,7 +213,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 +221,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(); @@ -446,6 +440,15 @@ template class MatrixBase template void applyOnTheRight(Index p, Index q, const JacobiRotation& j); +///////// SparseCore module ///////// + + template + EIGEN_STRONG_INLINE const typename SparseMatrixBase::template CwiseProductDenseReturnType::Type + cwiseProduct(const SparseMatrixBase &other) const + { + return other.cwiseProduct(derived()); + } + ///////// MatrixFunctions module ///////// typedef typename internal::stem_function::type StemFunction; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h index 1297b8413..85ffae265 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h @@ -250,6 +250,35 @@ class PermutationBase : public EigenBase template friend inline PlainPermutationType operator*(const Transpose >& other, const PermutationBase& perm) { return PlainPermutationType(internal::PermPermProduct, other.eval(), perm); } + + /** \returns the determinant of the permutation matrix, which is either 1 or -1 depending on the parity of the permutation. + * + * This function is O(\c n) procedure allocating a buffer of \c n booleans. + */ + Index determinant() const + { + Index res = 1; + Index n = size(); + Matrix mask(n); + mask.fill(false); + Index r = 0; + while(r < n) + { + // search for the next seed + while(r=n) + break; + // we got one, let's follow it until we are back to the seed + Index k0 = r++; + mask.coeffRef(k0) = true; + for(Index k=indices().coeff(k0); k!=k0; k=indices().coeff(k)) + { + mask.coeffRef(k) = true; + res = -res; + } + } + return res; + } protected: @@ -555,7 +584,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/PlainObjectBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h index dd34b59e5..a4e4af4a7 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h @@ -437,6 +437,36 @@ class PlainObjectBase : public internal::dense_xpr_base::type } #endif +#ifdef EIGEN_HAVE_RVALUE_REFERENCES + PlainObjectBase(PlainObjectBase&& other) + : m_storage( std::move(other.m_storage) ) + { + } + + PlainObjectBase& operator=(PlainObjectBase&& other) + { + using std::swap; + swap(m_storage, other.m_storage); + return *this; + } +#endif + + /** Copy constructor */ + EIGEN_STRONG_INLINE PlainObjectBase(const PlainObjectBase& other) + : m_storage() + { + _check_template_params(); + lazyAssign(other); + } + + template + EIGEN_STRONG_INLINE PlainObjectBase(const DenseBase &other) + : m_storage() + { + _check_template_params(); + lazyAssign(other); + } + EIGEN_STRONG_INLINE PlainObjectBase(Index a_size, Index nbRows, Index nbCols) : m_storage(a_size, nbRows, nbCols) { @@ -573,6 +603,8 @@ class PlainObjectBase : public internal::dense_xpr_base::type : (rows() == other.rows() && cols() == other.cols()))) && "Size mismatch. Automatic resizing is disabled because EIGEN_NO_AUTOMATIC_RESIZING is defined"); EIGEN_ONLY_USED_FOR_DEBUG(other); + if(this->size()==0) + resizeLike(other); #else resizeLike(other); #endif 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/Redux.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h index 50548fa9a..9b8662a6f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h @@ -247,8 +247,9 @@ struct redux_impl } }; -template -struct redux_impl +// NOTE: for SliceVectorizedTraversal we simply bypass unrolling +template +struct redux_impl { typedef typename Derived::Scalar Scalar; typedef typename packet_traits::type PacketScalar; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h index cd6d949c4..7a3becaf8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h @@ -108,7 +108,8 @@ struct traits > OuterStrideMatch = Derived::IsVectorAtCompileTime || int(StrideType::OuterStrideAtCompileTime)==int(Dynamic) || int(StrideType::OuterStrideAtCompileTime)==int(Derived::OuterStrideAtCompileTime), AlignmentMatch = (_Options!=Aligned) || ((PlainObjectType::Flags&AlignedBit)==0) || ((traits::Flags&AlignedBit)==AlignedBit), - MatchAtCompileTime = HasDirectAccess && StorageOrderMatch && InnerStrideMatch && OuterStrideMatch && AlignmentMatch + ScalarTypeMatch = internal::is_same::value, + MatchAtCompileTime = HasDirectAccess && StorageOrderMatch && InnerStrideMatch && OuterStrideMatch && AlignmentMatch && ScalarTypeMatch }; typedef typename internal::conditional::type type; }; @@ -187,7 +188,11 @@ protected: template class Ref : public RefBase > { + private: typedef internal::traits Traits; + template + inline Ref(const PlainObjectBase& expr, + typename internal::enable_if::MatchAtCompileTime),Derived>::type* = 0); public: typedef RefBase Base; @@ -199,17 +204,20 @@ template class Ref inline Ref(PlainObjectBase& expr, typename internal::enable_if::MatchAtCompileTime),Derived>::type* = 0) { - 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) + typename internal::enable_if::MatchAtCompileTime),Derived>::type* = 0) #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()); } @@ -228,13 +236,23 @@ template class Ref< EIGEN_DENSE_PUBLIC_INTERFACE(Ref) template - inline Ref(const DenseBase& expr) + inline Ref(const DenseBase& expr, + typename internal::enable_if::ScalarTypeMatch),Derived>::type* = 0) { // std::cout << match_helper::HasDirectAccess << "," << match_helper::OuterStrideMatch << "," << match_helper::InnerStrideMatch << "\n"; // std::cout << int(StrideType::OuterStrideAtCompileTime) << " - " << int(Derived::OuterStrideAtCompileTime) << "\n"; // std::cout << int(StrideType::InnerStrideAtCompileTime) << " - " << int(Derived::InnerStrideAtCompileTime) << "\n"; construct(expr.derived(), typename Traits::template match::type()); } + + inline Ref(const Ref& other) : Base(other) { + // copy constructor shall not copy the m_object, to avoid unnecessary malloc and copy + } + + template + inline Ref(const RefBase& other) { + construct(other.derived(), typename Traits::template match::type()); + } protected: 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/ReturnByValue.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ReturnByValue.h index d66c24ba0..f635598dc 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ReturnByValue.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ReturnByValue.h @@ -72,6 +72,8 @@ template class ReturnByValue const Unusable& coeff(Index,Index) const { return *reinterpret_cast(this); } Unusable& coeffRef(Index) { return *reinterpret_cast(this); } Unusable& coeffRef(Index,Index) { return *reinterpret_cast(this); } + template Unusable& packet(Index) const; + template Unusable& packet(Index, Index) const; #endif }; @@ -83,6 +85,15 @@ Derived& DenseBase::operator=(const ReturnByValue& other) return derived(); } +template +template +Derived& DenseBase::lazyAssign(const ReturnByValue& other) +{ + other.evalTo(derived()); + return derived(); +} + + } // end namespace Eigen #endif // EIGEN_RETURNBYVALUE_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfCwiseBinaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfCwiseBinaryOp.h index 22f3047b4..0956475af 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfCwiseBinaryOp.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfCwiseBinaryOp.h @@ -180,15 +180,9 @@ inline Derived& DenseBase::operator*=(const Scalar& other) template inline Derived& DenseBase::operator/=(const Scalar& other) { - typedef typename internal::conditional::IsInteger, - internal::scalar_quotient_op, - internal::scalar_product_op >::type BinOp; typedef typename Derived::PlainObject PlainObject; - SelfCwiseBinaryOp tmp(derived()); - Scalar actual_other; - if(NumTraits::IsInteger) actual_other = other; - else actual_other = Scalar(1)/other; - tmp = PlainObject::Constant(rows(),cols(), actual_other); + SelfCwiseBinaryOp, Derived, typename PlainObject::ConstantReturnType> tmp(derived()); + tmp = PlainObject::Constant(rows(),cols(), other); return derived(); } 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..d49670e04 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]; } @@ -375,6 +384,7 @@ template<> EIGEN_STRONG_INLINE int predux_max(const Packet4i& a) a_lo = vget_low_s32(a); a_hi = vget_high_s32(a); max = vpmax_s32(a_lo, a_hi); + max = vpmax_s32(max, max); return vget_lane_s32(max, 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..2b07168ae 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 */ @@ -126,7 +126,7 @@ Packet4f pexp(const Packet4f& _x) _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p4, 1.6666665459E-1f); _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p5, 5.0000001201E-1f); - Packet4f tmp = _mm_setzero_ps(), fx; + Packet4f tmp, fx; Packet4i emm0; // clamp x @@ -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) @@ -195,7 +195,7 @@ Packet2d pexp(const Packet2d& _x) _EIGEN_DECLARE_CONST_Packet2d(cephes_exp_C2, 1.42860682030941723212e-6); static const __m128i p4i_1023_0 = _mm_setr_epi32(1023, 1023, 0, 0); - Packet2d tmp = _mm_setzero_pd(), fx; + Packet2d tmp, fx; Packet4i emm0; // clamp 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. @@ -279,7 +279,7 @@ Packet4f psin(const Packet4f& _x) _EIGEN_DECLARE_CONST_Packet4f(coscof_p2, 4.166664568298827E-002f); _EIGEN_DECLARE_CONST_Packet4f(cephes_FOPI, 1.27323954473516f); // 4 / M_PI - Packet4f xmm1, xmm2 = _mm_setzero_ps(), xmm3, sign_bit, y; + Packet4f xmm1, xmm2, xmm3, sign_bit, y; Packet4i emm0, emm2; sign_bit = x; @@ -378,7 +378,7 @@ Packet4f pcos(const Packet4f& _x) _EIGEN_DECLARE_CONST_Packet4f(coscof_p2, 4.166664568298827E-002f); _EIGEN_DECLARE_CONST_Packet4f(cephes_FOPI, 1.27323954473516f); // 4 / M_PI - Packet4f xmm1, xmm2 = _mm_setzero_ps(), xmm3, y; + Packet4f xmm1, xmm2, xmm3, y; Packet4i emm0, emm2; x = pabs(x); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h index c06a0df1c..2a9d65b94 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; @@ -242,12 +243,12 @@ struct product_coeff_impl static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, RetScalar &res) { product_coeff_impl::run(row, col, lhs, rhs, res); - res += lhs.coeff(row, UnrollingIndex) * rhs.coeff(UnrollingIndex, col); + res += lhs.coeff(row, UnrollingIndex-1) * rhs.coeff(UnrollingIndex-1, col); } }; template -struct product_coeff_impl +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) @@ -256,16 +257,23 @@ struct product_coeff_impl } }; +template +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) + { + res = RetScalar(0); + } +}; + template 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(); } }; @@ -295,6 +303,16 @@ struct product_coeff_vectorized_unroller<0, Lhs, Rhs, Packet> } }; +template +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) + { + res = 0; + } +}; + template struct product_coeff_impl { @@ -304,8 +322,7 @@ struct product_coeff_impl::run(row, col, lhs, rhs, pres); - product_coeff_impl::run(row, col, lhs, rhs, res); + product_coeff_vectorized_unroller::run(row, col, lhs, rhs, pres); res = predux(pres); } }; @@ -373,7 +390,7 @@ struct product_packet_impl static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Packet &res) { product_packet_impl::run(row, col, lhs, rhs, res); - res = pmadd(pset1(lhs.coeff(row, UnrollingIndex)), rhs.template packet(UnrollingIndex, col), res); + res = pmadd(pset1(lhs.coeff(row, UnrollingIndex-1)), rhs.template packet(UnrollingIndex-1, col), res); } }; @@ -384,12 +401,12 @@ struct product_packet_impl static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Packet &res) { product_packet_impl::run(row, col, lhs, rhs, res); - res = pmadd(lhs.template packet(row, UnrollingIndex), pset1(rhs.coeff(UnrollingIndex, col)), res); + res = pmadd(lhs.template packet(row, UnrollingIndex-1), pset1(rhs.coeff(UnrollingIndex-1, col)), res); } }; template -struct product_packet_impl +struct product_packet_impl { typedef typename Lhs::Index Index; static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Packet &res) @@ -399,7 +416,7 @@ struct product_packet_impl }; template -struct product_packet_impl +struct product_packet_impl { typedef typename Lhs::Index Index; static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Packet &res) @@ -408,16 +425,35 @@ struct product_packet_impl } }; +template +struct product_packet_impl +{ + typedef typename Lhs::Index Index; + static EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, const Rhs& /*rhs*/, Packet &res) + { + res = pset1(0); + } +}; + +template +struct product_packet_impl +{ + typedef typename Lhs::Index Index; + static EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, const Rhs& /*rhs*/, Packet &res) + { + res = pset1(0); + } +}; + template struct product_packet_impl { typedef typename Lhs::Index Index; static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Packet& res) { - eigen_assert(lhs.cols()>0 && "you are using a non initialized matrix"); - res = pmul(pset1(lhs.coeff(row, 0)),rhs.template packet(0, col)); - for(Index i = 1; i < lhs.cols(); ++i) - res = pmadd(pset1(lhs.coeff(row, i)), rhs.template packet(i, col), res); + res = pset1(0); + for(Index i = 0; i < lhs.cols(); ++i) + res = pmadd(pset1(lhs.coeff(row, i)), rhs.template packet(i, col), res); } }; @@ -427,10 +463,9 @@ struct product_packet_impl typedef typename Lhs::Index Index; static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Packet& res) { - eigen_assert(lhs.cols()>0 && "you are using a non initialized matrix"); - res = pmul(lhs.template packet(row, 0), pset1(rhs.coeff(0, col))); - for(Index i = 1; i < lhs.cols(); ++i) - res = pmadd(lhs.template packet(row, i), pset1(rhs.coeff(i, col)), res); + res = pset1(0); + for(Index i = 0; i < lhs.cols(); ++i) + res = pmadd(lhs.template packet(row, i), pset1(rhs.coeff(i, col)), res); } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/Parallelizer.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/Parallelizer.h index 5c3e9b7ac..6937ee332 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/Parallelizer.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/Parallelizer.h @@ -125,19 +125,22 @@ void parallelize_gemm(const Functor& func, Index rows, Index cols, bool transpos if(transpose) std::swap(rows,cols); - Index blockCols = (cols / threads) & ~Index(0x3); - Index blockRows = (rows / threads) & ~Index(0x7); - GemmParallelInfo* info = new GemmParallelInfo[threads]; - #pragma omp parallel for schedule(static,1) num_threads(threads) - for(Index i=0; i class Rotation2D; template class AngleAxis; template class Translation; +// Sparse module: +template class SparseMatrixBase; + #ifdef EIGEN2_SUPPORT template class eigen2_RotationBase; template class eigen2_Cross; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/MKL_support.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/MKL_support.h index 8acca9c8c..1ef3b61db 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/MKL_support.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/MKL_support.h @@ -76,6 +76,38 @@ #include #define EIGEN_MKL_VML_THRESHOLD 128 +/* MKL_DOMAIN_BLAS, etc are defined only in 10.3 update 7 */ +/* MKL_BLAS, etc are not defined in 11.2 */ +#ifdef MKL_DOMAIN_ALL +#define EIGEN_MKL_DOMAIN_ALL MKL_DOMAIN_ALL +#else +#define EIGEN_MKL_DOMAIN_ALL MKL_ALL +#endif + +#ifdef MKL_DOMAIN_BLAS +#define EIGEN_MKL_DOMAIN_BLAS MKL_DOMAIN_BLAS +#else +#define EIGEN_MKL_DOMAIN_BLAS MKL_BLAS +#endif + +#ifdef MKL_DOMAIN_FFT +#define EIGEN_MKL_DOMAIN_FFT MKL_DOMAIN_FFT +#else +#define EIGEN_MKL_DOMAIN_FFT MKL_FFT +#endif + +#ifdef MKL_DOMAIN_VML +#define EIGEN_MKL_DOMAIN_VML MKL_DOMAIN_VML +#else +#define EIGEN_MKL_DOMAIN_VML MKL_VML +#endif + +#ifdef MKL_DOMAIN_PARDISO +#define EIGEN_MKL_DOMAIN_PARDISO MKL_DOMAIN_PARDISO +#else +#define EIGEN_MKL_DOMAIN_PARDISO MKL_PARDISO +#endif + namespace Eigen { typedef std::complex dcomplex; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h index 3a010ec6a..53fb5fae4 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 7 #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,27 @@ #define EIGEN_DEFAULT_DENSE_INDEX_TYPE std::ptrdiff_t #endif +// A Clang feature extension to determine compiler features. +// We use it to determine 'cxx_rvalue_references' +#ifndef __has_feature +# define __has_feature(x) 0 +#endif + +// Do we support r-value references? +#if (__has_feature(cxx_rvalue_references) || \ + defined(__GXX_EXPERIMENTAL_CXX0X__) || \ + (defined(_MSC_VER) && _MSC_VER >= 1600)) + #define EIGEN_HAVE_RVALUE_REFERENCES +#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 +268,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 @@ -271,6 +292,7 @@ namespace Eigen { #error Please tell me what is the equivalent of __attribute__((aligned(n))) for your compiler #endif +#define EIGEN_ALIGN8 EIGEN_ALIGN_TO_BOUNDARY(8) #define EIGEN_ALIGN16 EIGEN_ALIGN_TO_BOUNDARY(16) #if EIGEN_ALIGN_STATICALLY @@ -306,7 +328,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) @@ -325,8 +347,11 @@ namespace Eigen { } #endif -#define EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) \ - EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) +/** \internal + * \brief Macro to manually inherit assignment operators. + * This is necessary, because the implicitly defined assignment operator gets deleted when a custom operator= is defined. + */ +#define EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) /** * Just a side note. Commenting within defines works only by documenting @@ -398,6 +423,8 @@ namespace Eigen { #define EIGEN_SIZE_MAX(a,b) (((int)a == Dynamic || (int)b == Dynamic) ? Dynamic \ : ((int)a >= (int)b) ? (int)a : (int)b) +#define EIGEN_ADD_COST(a,b) int(a)==Dynamic || int(b)==Dynamic ? Dynamic : int(a)+int(b) + #define EIGEN_LOGICAL_XOR(a,b) (((a) || (b)) && !((a) && (b))) #define EIGEN_IMPLIES(a,b) (!(a) || (b)) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h index 6c2461725..bc1ea69ed 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) { @@ -522,7 +523,7 @@ template struct smart_copy_helper { // you can overwrite Eigen's default behavior regarding alloca by defining EIGEN_ALLOCA // to the appropriate stack allocation function #ifndef EIGEN_ALLOCA - #if (defined __linux__) + #if (defined __linux__) || (defined __APPLE__) || (defined alloca) #define EIGEN_ALLOCA alloca #elif defined(_MSC_VER) #define EIGEN_ALLOCA _alloca @@ -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..d05f8e5f6 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); } @@ -366,17 +366,17 @@ struct dense_xpr_base /** \internal Helper base class to add a scalar multiple operator * overloads for complex types */ -template::value > -struct special_scalar_op_base : public DenseCoeffsBase +struct special_scalar_op_base : public BaseType { // dummy operator* so that the // "using special_scalar_op_base::operator*" compiles void operator*() const; }; -template -struct special_scalar_op_base : public DenseCoeffsBase +template +struct special_scalar_op_base : public BaseType { const CwiseUnaryOp, Derived> operator*(const OtherScalar& scalar) const 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/ComplexEigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexEigenSolver.h index af434bc9b..417c72944 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexEigenSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexEigenSolver.h @@ -234,6 +234,12 @@ template class ComplexEigenSolver } protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } + EigenvectorType m_eivec; EigenvalueType m_eivalues; ComplexSchur m_schur; @@ -251,6 +257,8 @@ template ComplexEigenSolver& ComplexEigenSolver::compute(const MatrixType& matrix, bool computeEigenvectors) { + check_template_parameters(); + // this code is inspired from Jampack eigen_assert(matrix.cols() == matrix.rows()); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h index 6e7150685..20c59a7a2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h @@ -298,6 +298,13 @@ template class EigenSolver void doComputeEigenvectors(); protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + EIGEN_STATIC_ASSERT(!NumTraits::IsComplex, NUMERIC_TYPE_MUST_BE_REAL); + } + MatrixType m_eivec; EigenvalueType m_eivalues; bool m_isInitialized; @@ -364,6 +371,8 @@ template EigenSolver& EigenSolver::compute(const MatrixType& matrix, bool computeEigenvectors) { + check_template_parameters(); + using std::sqrt; using std::abs; eigen_assert(matrix.cols() == matrix.rows()); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h index dc240e13e..956e80d9e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h @@ -263,6 +263,13 @@ template class GeneralizedEigenSolver } protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + EIGEN_STATIC_ASSERT(!NumTraits::IsComplex, NUMERIC_TYPE_MUST_BE_REAL); + } + MatrixType m_eivec; ComplexVectorType m_alphas; VectorType m_betas; @@ -290,6 +297,8 @@ template GeneralizedEigenSolver& GeneralizedEigenSolver::compute(const MatrixType& A, const MatrixType& B, bool computeEigenvectors) { + check_template_parameters(); + using std::sqrt; using std::abs; eigen_assert(A.cols() == A.rows() && B.cols() == A.rows() && B.cols() == B.rows()); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h index 5706eeebe..aa3833eba 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h @@ -240,10 +240,10 @@ namespace Eigen { m_S.coeffRef(i,j) = Scalar(0.0); m_S.rightCols(dim-j-1).applyOnTheLeft(i-1,i,G.adjoint()); m_T.rightCols(dim-i+1).applyOnTheLeft(i-1,i,G.adjoint()); + // update Q + if (m_computeQZ) + m_Q.applyOnTheRight(i-1,i,G); } - // update Q - if (m_computeQZ) - m_Q.applyOnTheRight(i-1,i,G); // kill T(i,i-1) if(m_T.coeff(i,i-1)!=Scalar(0)) { @@ -251,10 +251,10 @@ namespace Eigen { m_T.coeffRef(i,i-1) = Scalar(0.0); m_S.applyOnTheRight(i,i-1,G); m_T.topRows(i).applyOnTheRight(i,i-1,G); + // update Z + if (m_computeQZ) + m_Z.applyOnTheLeft(i,i-1,G.adjoint()); } - // update Z - if (m_computeQZ) - m_Z.applyOnTheLeft(i,i-1,G.adjoint()); } } } @@ -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/RealSchur.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur.h index 64d136341..16d387537 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur.h @@ -234,7 +234,7 @@ template class RealSchur typedef Matrix Vector3s; Scalar computeNormOfT(); - Index findSmallSubdiagEntry(Index iu, const Scalar& norm); + Index findSmallSubdiagEntry(Index iu); void splitOffTwoRows(Index iu, bool computeU, const Scalar& exshift); void computeShift(Index iu, Index iter, Scalar& exshift, Vector3s& shiftInfo); void initFrancisQRStep(Index il, Index iu, const Vector3s& shiftInfo, Index& im, Vector3s& firstHouseholderVector); @@ -286,7 +286,7 @@ RealSchur& RealSchur::computeFromHessenberg(const HessMa { while (iu >= 0) { - Index il = findSmallSubdiagEntry(iu, norm); + Index il = findSmallSubdiagEntry(iu); // Check for convergence if (il == iu) // One root found @@ -343,16 +343,14 @@ inline typename MatrixType::Scalar RealSchur::computeNormOfT() /** \internal Look for single small sub-diagonal element and returns its index */ template -inline typename MatrixType::Index RealSchur::findSmallSubdiagEntry(Index iu, const Scalar& norm) +inline typename MatrixType::Index RealSchur::findSmallSubdiagEntry(Index iu) { using std::abs; Index res = iu; while (res > 0) { Scalar s = abs(m_matT.coeff(res-1,res-1)) + abs(m_matT.coeff(res,res)); - if (s == 0.0) - s = norm; - if (abs(m_matT.coeff(res,res-1)) < NumTraits::epsilon() * s) + if (abs(m_matT.coeff(res,res-1)) <= NumTraits::epsilon() * s) break; res--; } @@ -457,9 +455,7 @@ inline void RealSchur::initFrancisQRStep(Index il, Index iu, const V const Scalar lhs = m_matT.coeff(im,im-1) * (abs(v.coeff(1)) + abs(v.coeff(2))); const Scalar rhs = v.coeff(0) * (abs(m_matT.coeff(im-1,im-1)) + abs(Tmm) + abs(m_matT.coeff(im+1,im+1))); if (abs(lhs) < NumTraits::epsilon() * rhs) - { break; - } } } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h index 3993046a8..1131c8af8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h @@ -80,6 +80,8 @@ template class SelfAdjointEigenSolver /** \brief Scalar type for matrices of type \p _MatrixType. */ typedef typename MatrixType::Scalar Scalar; typedef typename MatrixType::Index Index; + + typedef Matrix EigenvectorsType; /** \brief Real scalar type for \p _MatrixType. * @@ -225,7 +227,7 @@ template class SelfAdjointEigenSolver * * \sa eigenvalues() */ - const MatrixType& eigenvectors() const + const EigenvectorsType& eigenvectors() const { eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized."); eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues."); @@ -351,7 +353,12 @@ template class SelfAdjointEigenSolver #endif // EIGEN2_SUPPORT protected: - MatrixType m_eivec; + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } + + EigenvectorsType m_eivec; RealVectorType m_eivalues; typename TridiagonalizationType::SubDiagonalType m_subdiag; ComputationInfo m_info; @@ -376,7 +383,7 @@ template class SelfAdjointEigenSolver * "implicit symmetric QR step with Wilkinson shift" */ namespace internal { -template +template static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end, Scalar* matrixQ, Index n); } @@ -384,6 +391,8 @@ template SelfAdjointEigenSolver& SelfAdjointEigenSolver ::compute(const MatrixType& matrix, int options) { + check_template_parameters(); + using std::abs; eigen_assert(matrix.cols() == matrix.rows()); eigen_assert((options&~(EigVecMask|GenEigMask))==0 @@ -406,7 +415,7 @@ SelfAdjointEigenSolver& SelfAdjointEigenSolver // declare some aliases RealVectorType& diag = m_eivalues; - MatrixType& mat = m_eivec; + EigenvectorsType& mat = m_eivec; // map the matrix coefficients to [-1:1] to avoid over- and underflow. mat = matrix.template triangularView(); @@ -442,7 +451,7 @@ SelfAdjointEigenSolver& SelfAdjointEigenSolver while (start>0 && m_subdiag[start-1]!=0) start--; - internal::tridiagonal_qr_step(diag.data(), m_subdiag.data(), start, end, computeEigenvectors ? m_eivec.data() : (Scalar*)0, n); + internal::tridiagonal_qr_step(diag.data(), m_subdiag.data(), start, end, computeEigenvectors ? m_eivec.data() : (Scalar*)0, n); } if (iter <= m_maxIterations * n) @@ -490,7 +499,13 @@ template struct direct_selfadjoint_eigenvalues struct direct_selfadjoint_eigenvalues Scalar(0)) + Scalar a_over_3 = (c2*c2_over_3 - c1)*s_inv3; + if(a_over_3 Scalar(0)) + Scalar q = a_over_3*a_over_3*a_over_3 - half_b*half_b; + if(q 0, atan2 is in [0, pi] and theta is in [0, pi/3] Scalar cos_theta = cos(theta); Scalar sin_theta = sin(theta); - roots(0) = c2_over_3 + Scalar(2)*rho*cos_theta; - roots(1) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta); - roots(2) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta); - - // Sort in increasing order. - if (roots(0) >= roots(1)) - std::swap(roots(0),roots(1)); - if (roots(1) >= roots(2)) - { - std::swap(roots(1),roots(2)); - if (roots(0) >= roots(1)) - std::swap(roots(0),roots(1)); - } + // roots are already sorted, since cos is monotonically decreasing on [0, pi] + roots(0) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta); // == 2*rho*cos(theta+2pi/3) + roots(1) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta); // == 2*rho*cos(theta+ pi/3) + roots(2) = c2_over_3 + Scalar(2)*rho*cos_theta; } - + + static inline bool extract_kernel(MatrixType& mat, Ref res, Ref representative) + { + using std::abs; + Index i0; + // Find non-zero column i0 (by construction, there must exist a non zero coefficient on the diagonal): + mat.diagonal().cwiseAbs().maxCoeff(&i0); + // mat.col(i0) is a good candidate for an orthogonal vector to the current eigenvector, + // so let's save it: + representative = mat.col(i0); + Scalar n0, n1; + VectorType c0, c1; + n0 = (c0 = representative.cross(mat.col((i0+1)%3))).squaredNorm(); + n1 = (c1 = representative.cross(mat.col((i0+2)%3))).squaredNorm(); + if(n0>n1) res = c0/std::sqrt(n0); + else res = c1/std::sqrt(n1); + + return true; + } + static inline void run(SolverType& solver, const MatrixType& mat, int options) { - using std::sqrt; eigen_assert(mat.cols() == 3 && mat.cols() == mat.rows()); eigen_assert((options&~(EigVecMask|GenEigMask))==0 && (options&EigVecMask)!=EigVecMask && "invalid option parameter"); bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors; - MatrixType& eivecs = solver.m_eivec; + EigenvectorsType& eivecs = solver.m_eivec; VectorType& eivals = solver.m_eivalues; - // map the matrix coefficients to [-1:1] to avoid over- and underflow. - Scalar scale = mat.cwiseAbs().maxCoeff(); - MatrixType scaledMat = mat / scale; + // Shift the matrix to the mean eigenvalue and map the matrix coefficients to [-1:1] to avoid over- and underflow. + Scalar shift = mat.trace() / Scalar(3); + // TODO Avoid this copy. Currently it is necessary to suppress bogus values when determining maxCoeff and for computing the eigenvectors later + MatrixType scaledMat = mat.template selfadjointView(); + scaledMat.diagonal().array() -= shift; + Scalar scale = scaledMat.cwiseAbs().maxCoeff(); + if(scale > 0) scaledMat /= scale; // TODO for scale==0 we could save the remaining operations // compute the eigenvalues computeRoots(scaledMat,eivals); - // compute the eigen vectors + // compute the eigenvectors if(computeEigenvectors) { - Scalar safeNorm2 = Eigen::NumTraits::epsilon(); - safeNorm2 *= safeNorm2; if((eivals(2)-eivals(0))<=Eigen::NumTraits::epsilon()) { + // All three eigenvalues are numerically the same eivecs.setIdentity(); } else { - scaledMat = scaledMat.template selfadjointView(); MatrixType tmp; tmp = scaledMat; + // Compute the eigenvector of the most distinct eigenvalue Scalar d0 = eivals(2) - eivals(1); Scalar d1 = eivals(1) - eivals(0); - int k = d0 > d1 ? 2 : 0; - d0 = d0 > d1 ? d1 : d0; - - tmp.diagonal().array () -= eivals(k); - VectorType cross; - Scalar n; - n = (cross = tmp.row(0).cross(tmp.row(1))).squaredNorm(); - - if(n>safeNorm2) - eivecs.col(k) = cross / sqrt(n); - else + Index k(0), l(2); + if(d0 > d1) { - 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, - // => exit - // scale back to the original size. - eivals *= scale; - - solver.m_info = NumericalIssue; - solver.m_isInitialized = true; - solver.m_eigenvectorsOk = computeEigenvectors; - return; - } - } + std::swap(k,l); + d0 = d1; } - tmp = scaledMat; - tmp.diagonal().array() -= eivals(1); - - if(d0<=Eigen::NumTraits::epsilon()) - eivecs.col(1) = eivecs.col(k).unitOrthogonal(); - else + // Compute the eigenvector of index k { - n = (cross = eivecs.col(k).cross(tmp.row(0).normalized())).squaredNorm(); - if(n>safeNorm2) - eivecs.col(1) = cross / sqrt(n); - else - { - n = (cross = eivecs.col(k).cross(tmp.row(1))).squaredNorm(); - if(n>safeNorm2) - eivecs.col(1) = cross / sqrt(n); - else - { - n = (cross = eivecs.col(k).cross(tmp.row(2))).squaredNorm(); - if(n>safeNorm2) - eivecs.col(1) = cross / sqrt(n); - else - { - // we should never reach this point, - // if so the last two eigenvalues are likely to ve very closed to each other - eivecs.col(1) = eivecs.col(k).unitOrthogonal(); - } - } - } - - // make sure that eivecs[1] is orthogonal to eivecs[2] - Scalar d = eivecs.col(1).dot(eivecs.col(k)); - eivecs.col(1) = (eivecs.col(1) - d * eivecs.col(k)).normalized(); + tmp.diagonal().array () -= eivals(k); + // By construction, 'tmp' is of rank 2, and its kernel corresponds to the respective eigenvector. + extract_kernel(tmp, eivecs.col(k), eivecs.col(l)); } - eivecs.col(k==2 ? 0 : 2) = eivecs.col(k).cross(eivecs.col(1)).normalized(); + // Compute eigenvector of index l + if(d0<=2*Eigen::NumTraits::epsilon()*d1) + { + // If d0 is too small, then the two other eigenvalues are numerically the same, + // and thus we only have to ortho-normalize the near orthogonal vector we saved above. + eivecs.col(l) -= eivecs.col(k).dot(eivecs.col(l))*eivecs.col(l); + eivecs.col(l).normalize(); + } + else + { + tmp = scaledMat; + tmp.diagonal().array () -= eivals(l); + + VectorType dummy; + extract_kernel(tmp, eivecs.col(l), dummy); + } + + // Compute last eigenvector from the other two + eivecs.col(1) = eivecs.col(2).cross(eivecs.col(0)).normalized(); } } + // Rescale back to the original size. eivals *= scale; + eivals.array() += shift; solver.m_info = Success; solver.m_isInitialized = true; @@ -665,11 +655,12 @@ template struct direct_selfadjoint_eigenvalues struct direct_selfadjoint_eigenvalues struct direct_selfadjoint_eigenvaluesc2) + if((eivals(1)-eivals(0))<=abs(eivals(1))*Eigen::NumTraits::epsilon()) { - eivecs.col(1) << -scaledMat(1,0), scaledMat(0,0); - eivecs.col(1) /= sqrt(a2+b2); + eivecs.setIdentity(); } else { - eivecs.col(1) << -scaledMat(1,1), scaledMat(1,0); - eivecs.col(1) /= sqrt(c2+b2); - } + scaledMat.diagonal().array () -= eivals(1); + Scalar a2 = numext::abs2(scaledMat(0,0)); + Scalar c2 = numext::abs2(scaledMat(1,1)); + Scalar b2 = numext::abs2(scaledMat(1,0)); + if(a2>c2) + { + eivecs.col(1) << -scaledMat(1,0), scaledMat(0,0); + eivecs.col(1) /= sqrt(a2+b2); + } + else + { + eivecs.col(1) << -scaledMat(1,1), scaledMat(1,0); + eivecs.col(1) /= sqrt(c2+b2); + } - eivecs.col(0) << eivecs.col(1).unitOrthogonal(); + eivecs.col(0) << eivecs.col(1).unitOrthogonal(); + } } // Rescale back to the original size. @@ -736,7 +736,7 @@ SelfAdjointEigenSolver& SelfAdjointEigenSolver } namespace internal { -template +template static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end, Scalar* matrixQ, Index n) { using std::abs; @@ -788,8 +788,7 @@ static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index sta // apply the givens rotation to the unit matrix Q = Q * G if (matrixQ) { - // FIXME if StorageOrder == RowMajor this operation is not very efficient - Map > q(matrixQ,n,n); + Map > q(matrixQ,n,n); q.applyOnTheRight(k,k+1,rot); } } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h index 8e186d57a..7e1cd9eb7 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h @@ -19,10 +19,12 @@ namespace Eigen { * * \brief An axis aligned box * - * \param _Scalar the type of the scalar coefficients - * \param _AmbientDim the dimension of the ambient space, can be a compile time value or Dynamic. + * \tparam _Scalar the type of the scalar coefficients + * \tparam _AmbientDim the dimension of the ambient space, can be a compile time value or Dynamic. * * This class represents an axis aligned box as a pair of the minimal and maximal corners. + * \warning The result of most methods is undefined when applied to an empty box. You can check for empty boxes using isEmpty(). + * \sa alignedboxtypedefs */ template class AlignedBox @@ -40,18 +42,21 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) /** Define constants to name the corners of a 1D, 2D or 3D axis aligned bounding box */ enum CornerType { - /** 1D names */ + /** 1D names @{ */ Min=0, Max=1, + /** @} */ - /** Added names for 2D */ + /** Identifier for 2D corner @{ */ BottomLeft=0, BottomRight=1, TopLeft=2, TopRight=3, + /** @} */ - /** Added names for 3D */ + /** Identifier for 3D corner @{ */ BottomLeftFloor=0, BottomRightFloor=1, TopLeftFloor=2, TopRightFloor=3, BottomLeftCeil=4, BottomRightCeil=5, TopLeftCeil=6, TopRightCeil=7 + /** @} */ }; @@ -63,34 +68,33 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) inline explicit AlignedBox(Index _dim) : m_min(_dim), m_max(_dim) { setEmpty(); } - /** Constructs a box with extremities \a _min and \a _max. */ + /** Constructs a box with extremities \a _min and \a _max. + * \warning If either component of \a _min is larger than the same component of \a _max, the constructed box is empty. */ template inline AlignedBox(const OtherVectorType1& _min, const OtherVectorType2& _max) : m_min(_min), m_max(_max) {} /** Constructs a box containing a single point \a p. */ template - inline explicit AlignedBox(const MatrixBase& a_p) - { - typename internal::nested::type p(a_p.derived()); - m_min = p; - m_max = p; - } + inline explicit AlignedBox(const MatrixBase& p) : m_min(p), m_max(m_min) + { } ~AlignedBox() {} /** \returns the dimension in which the box holds */ inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_min.size() : Index(AmbientDimAtCompileTime); } - /** \deprecated use isEmpty */ + /** \deprecated use isEmpty() */ inline bool isNull() const { return isEmpty(); } - /** \deprecated use setEmpty */ + /** \deprecated use setEmpty() */ inline void setNull() { setEmpty(); } - /** \returns true if the box is empty. */ + /** \returns true if the box is empty. + * \sa setEmpty */ inline bool isEmpty() const { return (m_min.array() > m_max.array()).any(); } - /** Makes \c *this an empty box. */ + /** Makes \c *this an empty box. + * \sa isEmpty */ inline void setEmpty() { m_min.setConstant( ScalarTraits::highest() ); @@ -159,7 +163,7 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) * a uniform distribution */ inline VectorType sample() const { - VectorType r; + VectorType r(dim()); for(Index d=0; d - inline bool contains(const MatrixBase& a_p) const + inline bool contains(const MatrixBase& p) const { - typename internal::nested::type p(a_p.derived()); - return (m_min.array()<=p.array()).all() && (p.array()<=m_max.array()).all(); + typename internal::nested::type p_n(p.derived()); + return (m_min.array()<=p_n.array()).all() && (p_n.array()<=m_max.array()).all(); } /** \returns true if the box \a b is entirely inside the box \c *this. */ inline bool contains(const AlignedBox& b) const { return (m_min.array()<=(b.min)().array()).all() && ((b.max)().array()<=m_max.array()).all(); } - /** Extends \c *this such that it contains the point \a p and returns a reference to \c *this. */ + /** \returns true if the box \a b is intersecting the box \c *this. + * \sa intersection, clamp */ + inline bool intersects(const AlignedBox& b) const + { return (m_min.array()<=(b.max)().array()).all() && ((b.min)().array()<=m_max.array()).all(); } + + /** Extends \c *this such that it contains the point \a p and returns a reference to \c *this. + * \sa extend(const AlignedBox&) */ template - inline AlignedBox& extend(const MatrixBase& a_p) + inline AlignedBox& extend(const MatrixBase& p) { - typename internal::nested::type p(a_p.derived()); - m_min = m_min.cwiseMin(p); - m_max = m_max.cwiseMax(p); + typename internal::nested::type p_n(p.derived()); + m_min = m_min.cwiseMin(p_n); + m_max = m_max.cwiseMax(p_n); return *this; } - /** Extends \c *this such that it contains the box \a b and returns a reference to \c *this. */ + /** Extends \c *this such that it contains the box \a b and returns a reference to \c *this. + * \sa merged, extend(const MatrixBase&) */ inline AlignedBox& extend(const AlignedBox& b) { m_min = m_min.cwiseMin(b.m_min); @@ -203,7 +214,9 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) return *this; } - /** Clamps \c *this by the box \a b and returns a reference to \c *this. */ + /** Clamps \c *this by the box \a b and returns a reference to \c *this. + * \note If the boxes don't intersect, the resulting box is empty. + * \sa intersection(), intersects() */ inline AlignedBox& clamp(const AlignedBox& b) { m_min = m_min.cwiseMax(b.m_min); @@ -211,11 +224,15 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) return *this; } - /** Returns an AlignedBox that is the intersection of \a b and \c *this */ + /** Returns an AlignedBox that is the intersection of \a b and \c *this + * \note If the boxes don't intersect, the resulting box is empty. + * \sa intersects(), clamp, contains() */ inline AlignedBox intersection(const AlignedBox& b) const {return AlignedBox(m_min.cwiseMax(b.m_min), m_max.cwiseMin(b.m_max)); } - /** Returns an AlignedBox that is the union of \a b and \c *this */ + /** Returns an AlignedBox that is the union of \a b and \c *this. + * \note Merging with an empty box may result in a box bigger than \c *this. + * \sa extend(const AlignedBox&) */ inline AlignedBox merged(const AlignedBox& b) const { return AlignedBox(m_min.cwiseMin(b.m_min), m_max.cwiseMax(b.m_max)); } @@ -231,20 +248,20 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) /** \returns the squared distance between the point \a p and the box \c *this, * and zero if \a p is inside the box. - * \sa exteriorDistance() + * \sa exteriorDistance(const MatrixBase&), squaredExteriorDistance(const AlignedBox&) */ template - inline Scalar squaredExteriorDistance(const MatrixBase& a_p) const; + inline Scalar squaredExteriorDistance(const MatrixBase& p) const; /** \returns the squared distance between the boxes \a b and \c *this, * and zero if the boxes intersect. - * \sa exteriorDistance() + * \sa exteriorDistance(const AlignedBox&), squaredExteriorDistance(const MatrixBase&) */ inline Scalar squaredExteriorDistance(const AlignedBox& b) const; /** \returns the distance between the point \a p and the box \c *this, * and zero if \a p is inside the box. - * \sa squaredExteriorDistance() + * \sa squaredExteriorDistance(const MatrixBase&), exteriorDistance(const AlignedBox&) */ template inline NonInteger exteriorDistance(const MatrixBase& p) const @@ -252,7 +269,7 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) /** \returns the distance between the boxes \a b and \c *this, * and zero if the boxes intersect. - * \sa squaredExteriorDistance() + * \sa squaredExteriorDistance(const AlignedBox&), exteriorDistance(const MatrixBase&) */ inline NonInteger exteriorDistance(const AlignedBox& b) const { using std::sqrt; return sqrt(NonInteger(squaredExteriorDistance(b))); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AngleAxis.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AngleAxis.h index 553d38c74..bbf6a7ed8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AngleAxis.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AngleAxis.h @@ -131,7 +131,7 @@ public: m_angle = Scalar(other.angle()); } - static inline const AngleAxis Identity() { return AngleAxis(0, Vector3::UnitX()); } + static inline const AngleAxis Identity() { return AngleAxis(Scalar(0), Vector3::UnitX()); } /** \returns \c true if \c *this is approximately equal to \a other, within the precision * determined by \a prec. @@ -165,8 +165,8 @@ AngleAxis& AngleAxis::operator=(const QuaternionBase::dummy_precision()*NumTraits::dummy_precision()) { - m_angle = 0; - m_axis << 1, 0, 0; + m_angle = Scalar(0); + m_axis << Scalar(1), Scalar(0), Scalar(0); } else { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Homogeneous.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Homogeneous.h index 00e71d190..372e422b9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Homogeneous.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Homogeneous.h @@ -79,7 +79,7 @@ template class Homogeneous { if( (int(Direction)==Vertical && row==m_matrix.rows()) || (int(Direction)==Horizontal && col==m_matrix.cols())) - return 1; + return Scalar(1); return m_matrix.coeff(row, col); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Hyperplane.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Hyperplane.h index aeff43fef..00b7c4300 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Hyperplane.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Hyperplane.h @@ -100,7 +100,17 @@ public: { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 3) Hyperplane result(p0.size()); - result.normal() = (p2 - p0).cross(p1 - p0).normalized(); + VectorType v0(p2 - p0), v1(p1 - p0); + result.normal() = v0.cross(v1); + RealScalar norm = result.normal().norm(); + if(norm <= v0.norm() * v1.norm() * NumTraits::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/Quaternion.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h index f06653f1c..25ed17bb6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h @@ -102,11 +102,11 @@ public: /** \returns a quaternion representing an identity rotation * \sa MatrixBase::Identity() */ - static inline Quaternion Identity() { return Quaternion(1, 0, 0, 0); } + static inline Quaternion Identity() { return Quaternion(Scalar(1), Scalar(0), Scalar(0), Scalar(0)); } /** \sa QuaternionBase::Identity(), MatrixBase::setIdentity() */ - inline QuaternionBase& setIdentity() { coeffs() << 0, 0, 0, 1; return *this; } + inline QuaternionBase& setIdentity() { coeffs() << Scalar(0), Scalar(0), Scalar(0), Scalar(1); return *this; } /** \returns the squared norm of the quaternion's coefficients * \sa QuaternionBase::norm(), MatrixBase::squaredNorm() @@ -161,7 +161,7 @@ public: { return coeffs().isApprox(other.coeffs(), prec); } /** return the result vector of \a v through the rotation*/ - EIGEN_STRONG_INLINE Vector3 _transformVector(Vector3 v) const; + EIGEN_STRONG_INLINE Vector3 _transformVector(const Vector3& v) const; /** \returns \c *this with scalar type casted to \a NewScalarType * @@ -231,7 +231,7 @@ class Quaternion : public QuaternionBase > public: typedef _Scalar Scalar; - EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Quaternion) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Quaternion) using Base::operator*=; typedef typename internal::traits::Coefficients Coefficients; @@ -341,7 +341,7 @@ class Map, _Options > public: typedef _Scalar Scalar; typedef typename internal::traits::Coefficients Coefficients; - EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map) using Base::operator*=; /** Constructs a Mapped Quaternion object from the pointer \a coeffs @@ -378,7 +378,7 @@ class Map, _Options > public: typedef _Scalar Scalar; typedef typename internal::traits::Coefficients Coefficients; - EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map) using Base::operator*=; /** Constructs a Mapped Quaternion object from the pointer \a coeffs @@ -461,7 +461,7 @@ EIGEN_STRONG_INLINE Derived& QuaternionBase::operator*= (const Quaterni */ template EIGEN_STRONG_INLINE typename QuaternionBase::Vector3 -QuaternionBase::_transformVector(Vector3 v) const +QuaternionBase::_transformVector(const Vector3& v) const { // Note that this algorithm comes from the optimization by hand // of the conversion to a Matrix followed by a Matrix/Vector product. @@ -637,7 +637,7 @@ inline Quaternion::Scalar> QuaternionBasesquaredNorm(); - if (n2 > 0) + if (n2 > Scalar(0)) return Quaternion(conjugate().coeffs() / n2); else { @@ -667,12 +667,10 @@ template inline typename internal::traits::Scalar QuaternionBase::angularDistance(const QuaternionBase& other) const { - using std::acos; + using std::atan2; using std::abs; - Scalar d = abs(this->dot(other)); - if (d>=Scalar(1)) - return Scalar(0); - return Scalar(2) * acos(d); + Quaternion d = (*this) * other.conjugate(); + return Scalar(2) * atan2( d.vec().norm(), abs(d.w()) ); } @@ -712,7 +710,7 @@ QuaternionBase::slerp(const Scalar& t, const QuaternionBase(scale0 * coeffs() + scale1 * other.coeffs()); } 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/BasicPreconditioners.h b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h index 73ca9bfde..1f3c060d0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h @@ -65,10 +65,10 @@ class DiagonalPreconditioner { typename MatType::InnerIterator it(mat,j); while(it && it.index()!=j) ++it; - if(it && it.index()==j) + if(it && it.index()==j && it.value()!=Scalar(0)) m_invdiag(j) = Scalar(1)/it.value(); else - m_invdiag(j) = 0; + m_invdiag(j) = Scalar(1); } m_isInitialized = true; return *this; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h index 2b9fb7f88..551221907 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; @@ -152,20 +151,7 @@ struct traits > * \endcode * * By default the iterations start with x=0 as an initial guess of the solution. - * One can control the start using the solveWithGuess() method. Here is a step by - * step execution example starting with a random guess and printing the evolution - * of the estimated error: - * * \code - * x = VectorXd::Random(n); - * solver.setMaxIterations(1); - * int i = 0; - * do { - * x = solver.solveWithGuess(b,x); - * std::cout << i << " : " << solver.error() << std::endl; - * ++i; - * } while (solver.info()!=Success && i<100); - * \endcode - * Note that such a step by step excution is slightly slower. + * One can control the start using the solveWithGuess() method. * * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner */ @@ -200,7 +186,8 @@ public: * this class becomes invalid. Call compute() to update it with the new * matrix A, or modify a copy of A. */ - BiCGSTAB(const MatrixType& A) : Base(A) {} + template + explicit BiCGSTAB(const EigenBase& A) : Base(A.derived()) {} ~BiCGSTAB() {} diff --git a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h index a74a8155e..1a7e569c8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h @@ -112,9 +112,9 @@ struct traits > * This class allows to solve for A.x = b sparse linear problems using a conjugate gradient algorithm. * The sparse matrix A must be selfadjoint. The vectors x and b can be either dense or sparse. * - * \tparam _MatrixType the type of the sparse matrix A, can be a dense or a sparse matrix. - * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower - * or Upper. Default is Lower. + * \tparam _MatrixType the type of the matrix A, can be a dense or a sparse matrix. + * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower, + * Upper, or Lower|Upper in which the full matrix entries will be considered. Default is Lower. * \tparam _Preconditioner the type of the preconditioner. Default is DiagonalPreconditioner * * The maximal number of iterations and tolerance value can be controlled via the setMaxIterations() @@ -137,20 +137,7 @@ struct traits > * \endcode * * By default the iterations start with x=0 as an initial guess of the solution. - * One can control the start using the solveWithGuess() method. Here is a step by - * step execution example starting with a random guess and printing the evolution - * of the estimated error: - * * \code - * x = VectorXd::Random(n); - * cg.setMaxIterations(1); - * int i = 0; - * do { - * x = cg.solveWithGuess(b,x); - * std::cout << i << " : " << cg.error() << std::endl; - * ++i; - * } while (cg.info()!=Success && i<100); - * \endcode - * Note that such a step by step excution is slightly slower. + * One can control the start using the solveWithGuess() method. * * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner */ @@ -189,7 +176,8 @@ public: * this class becomes invalid. Call compute() to update it with the new * matrix A, or modify a copy of A. */ - ConjugateGradient(const MatrixType& A) : Base(A) {} + template + explicit ConjugateGradient(const EigenBase& A) : Base(A.derived()) {} ~ConjugateGradient() {} @@ -213,6 +201,10 @@ public: template void _solveWithGuess(const Rhs& b, Dest& x) const { + typedef typename internal::conditional + >::type MatrixWrapperType; m_iterations = Base::maxIterations(); m_error = Base::m_tolerance; @@ -222,8 +214,7 @@ public: m_error = Base::m_tolerance; typename Dest::ColXpr xj(x,j); - internal::conjugate_gradient(mp_matrix->template selfadjointView(), b.col(j), xj, - Base::m_preconditioner, m_iterations, m_error); + internal::conjugate_gradient(MatrixWrapperType(*mp_matrix), b.col(j), xj, Base::m_preconditioner, m_iterations, m_error); } m_isInitialized = true; @@ -234,7 +225,7 @@ public: template void _solve(const Rhs& b, Dest& x) const { - x.setOnes(); + x.setZero(); _solveWithGuess(b,x); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h index b55afc136..d3f37fea2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h @@ -150,7 +150,6 @@ class IncompleteLUT : internal::noncopyable { analyzePattern(amat); factorize(amat); - m_isInitialized = m_factorizationIsOk; return *this; } @@ -160,7 +159,7 @@ class IncompleteLUT : internal::noncopyable template void _solve(const Rhs& b, Dest& x) const { - x = m_Pinv * b; + x = m_Pinv * b; x = m_lu.template triangularView().solve(x); x = m_lu.template triangularView().solve(x); x = m_P * x; @@ -223,18 +222,29 @@ template void IncompleteLUT::analyzePattern(const _MatrixType& amat) { // Compute the Fill-reducing permutation + // Since ILUT does not perform any numerical pivoting, + // it is highly preferable to keep the diagonal through symmetric permutations. +#ifndef EIGEN_MPL2_ONLY + // To this end, let's symmetrize the pattern and perform AMD on it. SparseMatrix mat1 = amat; SparseMatrix mat2 = amat.transpose(); - // Symmetrize the pattern // FIXME for a matrix with nearly symmetric pattern, mat2+mat1 is the appropriate choice. // on the other hand for a really non-symmetric pattern, mat2*mat1 should be prefered... SparseMatrix AtA = mat2 + mat1; - AtA.prune(keep_diag()); - internal::minimum_degree_ordering(AtA, m_P); // Then compute the AMD ordering... - - m_Pinv = m_P.inverse(); // ... and the inverse permutation + AMDOrdering ordering; + ordering(AtA,m_P); + m_Pinv = m_P.inverse(); // cache the inverse permutation +#else + // If AMD is not available, (MPL2-only), then let's use the slower COLAMD routine. + SparseMatrix mat1 = amat; + COLAMDOrdering ordering; + ordering(mat1,m_Pinv); + m_P = m_Pinv.inverse(); +#endif m_analysisIsOk = true; + m_factorizationIsOk = false; + m_isInitialized = false; } template @@ -442,6 +452,7 @@ void IncompleteLUT::factorize(const _MatrixType& amat) m_lu.makeCompressed(); m_factorizationIsOk = true; + m_isInitialized = m_factorizationIsOk; m_info = Success; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h index 2036922d6..501ef2f8d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h @@ -49,10 +49,11 @@ public: * this class becomes invalid. Call compute() to update it with the new * matrix A, or modify a copy of A. */ - IterativeSolverBase(const MatrixType& A) + template + IterativeSolverBase(const EigenBase& A) { init(); - compute(A); + compute(A.derived()); } ~IterativeSolverBase() {} @@ -62,9 +63,11 @@ public: * Currently, this function mostly call analyzePattern on the preconditioner. In the future * we might, for instance, implement column reodering for faster matrix vector products. */ - Derived& analyzePattern(const MatrixType& A) + template + Derived& analyzePattern(const EigenBase& A) { - m_preconditioner.analyzePattern(A); + grabInput(A.derived()); + m_preconditioner.analyzePattern(*mp_matrix); m_isInitialized = true; m_analysisIsOk = true; m_info = Success; @@ -80,11 +83,12 @@ public: * this class becomes invalid. Call compute() to update it with the new * matrix A, or modify a copy of A. */ - Derived& factorize(const MatrixType& A) + template + Derived& factorize(const EigenBase& A) { + grabInput(A.derived()); eigen_assert(m_analysisIsOk && "You must first call analyzePattern()"); - mp_matrix = &A; - m_preconditioner.factorize(A); + m_preconditioner.factorize(*mp_matrix); m_factorizationIsOk = true; m_info = Success; return derived(); @@ -100,10 +104,11 @@ public: * this class becomes invalid. Call compute() to update it with the new * matrix A, or modify a copy of A. */ - Derived& compute(const MatrixType& A) + template + Derived& compute(const EigenBase& A) { - mp_matrix = &A; - m_preconditioner.compute(A); + grabInput(A.derived()); + m_preconditioner.compute(*mp_matrix); m_isInitialized = true; m_analysisIsOk = true; m_factorizationIsOk = true; @@ -212,6 +217,28 @@ public: } protected: + + template + void grabInput(const EigenBase& A) + { + // we const cast to prevent the creation of a MatrixType temporary by the compiler. + grabInput_impl(A.const_cast_derived()); + } + + template + void grabInput_impl(const EigenBase& A) + { + m_copyMatrix = A; + mp_matrix = &m_copyMatrix; + } + + void grabInput_impl(MatrixType& A) + { + if(MatrixType::RowsAtCompileTime==Dynamic && MatrixType::ColsAtCompileTime==Dynamic) + m_copyMatrix.resize(0,0); + mp_matrix = &A; + } + void init() { m_isInitialized = false; @@ -220,6 +247,7 @@ protected: m_maxIterations = -1; m_tolerance = NumTraits::epsilon(); } + MatrixType m_copyMatrix; const MatrixType* mp_matrix; Preconditioner m_preconditioner; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h b/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h index 79ab6a8c8..26bc71447 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h @@ -374,6 +374,12 @@ template class FullPivLU inline Index cols() const { return m_lu.cols(); } protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } + MatrixType m_lu; PermutationPType m_p; PermutationQType m_q; @@ -418,6 +424,8 @@ FullPivLU::FullPivLU(const MatrixType& matrix) template FullPivLU& FullPivLU::compute(const MatrixType& matrix) { + check_template_parameters(); + // the permutations are stored as int indices, so just to be sure: eigen_assert(matrix.rows()<=NumTraits::highest() && matrix.cols()<=NumTraits::highest()); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU.h b/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU.h index 740ee694c..7d1db948c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU.h @@ -171,6 +171,12 @@ template class PartialPivLU inline Index cols() const { return m_lu.cols(); } protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } + MatrixType m_lu; PermutationType m_p; TranspositionType m_rowsTranspositions; @@ -386,6 +392,8 @@ void partial_lu_inplace(MatrixType& lu, TranspositionType& row_transpositions, t template PartialPivLU& PartialPivLU::compute(const MatrixType& matrix) { + check_template_parameters(); + // the row permutation is stored as int indices, so just to be sure: eigen_assert(matrix.rows()::highest()); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Amd.h b/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Amd.h index 41b4fd7e3..70550b8a9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Amd.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Amd.h @@ -137,22 +137,27 @@ void minimum_degree_ordering(SparseMatrix& C, Permutation degree[i] = len[i]; // degree of node i } mark = internal::cs_wclear(0, 0, w, n); /* clear w */ - elen[n] = -2; /* n is a dead element */ - Cp[n] = -1; /* n is a root of assembly tree */ - w[n] = 0; /* n is a dead element */ /* --- Initialize degree lists ------------------------------------------ */ for(i = 0; i < n; i++) { + bool has_diag = false; + for(p = Cp[i]; p dense) /* node i is dense */ + else if(d > dense || !has_diag) /* node i is dense or has no structural diagonal element */ { nv[i] = 0; /* absorb i into element n */ elen[i] = -1; /* node i is dead */ @@ -168,6 +173,10 @@ void minimum_degree_ordering(SparseMatrix& C, Permutation } } + elen[n] = -2; /* n is a dead element */ + Cp[n] = -1; /* n is a root of assembly tree */ + w[n] = 0; /* n is a dead element */ + while (nel < n) /* while (selecting pivots) do */ { /* --- Select node of minimum approximate degree -------------------- */ 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/QR/ColPivHouseholderQR.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h index 773d1df8f..567eab7cd 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h @@ -384,6 +384,12 @@ template class ColPivHouseholderQR } protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } + MatrixType m_qr; HCoeffsType m_hCoeffs; PermutationType m_colsPermutation; @@ -422,6 +428,8 @@ typename MatrixType::RealScalar ColPivHouseholderQR::logAbsDetermina template ColPivHouseholderQR& ColPivHouseholderQR::compute(const MatrixType& matrix) { + check_template_parameters(); + using std::abs; Index rows = matrix.rows(); Index cols = matrix.cols(); @@ -463,20 +471,10 @@ ColPivHouseholderQR& ColPivHouseholderQR::compute(const // we store that back into our table: it can't hurt to correct our table. m_colSqNorms.coeffRef(biggest_col_index) = biggest_col_sq_norm; - // if the current biggest column is smaller than epsilon times the initial biggest column, - // terminate to avoid generating nan/inf values. - // Note that here, if we test instead for "biggest == 0", we get a failure every 1000 (or so) - // repetitions of the unit test, with the result of solve() filled with large values of the order - // of 1/(size*epsilon). - if(biggest_col_sq_norm < threshold_helper * RealScalar(rows-k)) - { + // Track the number of meaningful pivots but do not stop the decomposition to make + // sure that the initial matrix is properly reproduced. See bug 941. + if(m_nonzero_pivots==size && biggest_col_sq_norm < threshold_helper * RealScalar(rows-k)) m_nonzero_pivots = k; - m_hCoeffs.tail(size-k).setZero(); - m_qr.bottomRightCorner(rows-k,cols-k) - .template triangularView() - .setZero(); - break; - } // apply the transposition to the columns m_colsTranspositions.coeffRef(k) = biggest_col_index; @@ -505,7 +503,7 @@ ColPivHouseholderQR& ColPivHouseholderQR::compute(const } m_colsPermutation.setIdentity(PermIndexType(cols)); - for(PermIndexType k = 0; k < m_nonzero_pivots; ++k) + for(PermIndexType k = 0; k < size/*m_nonzero_pivots*/; ++k) m_colsPermutation.applyTranspositionOnTheRight(k, PermIndexType(m_colsTranspositions.coeff(k))); m_det_pq = (number_of_transpositions%2) ? -1 : 1; @@ -555,13 +553,15 @@ struct solve_retval, Rhs> } // end namespace internal -/** \returns the matrix Q as a sequence of householder transformations */ +/** \returns the matrix Q as a sequence of householder transformations. + * You can extract the meaningful part only by using: + * \code qr.householderQ().setLength(qr.nonzeroPivots()) \endcode*/ template typename ColPivHouseholderQR::HouseholderSequenceType ColPivHouseholderQR ::householderQ() const { eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized."); - return HouseholderSequenceType(m_qr, m_hCoeffs.conjugate()).setLength(m_nonzero_pivots); + return HouseholderSequenceType(m_qr, m_hCoeffs.conjugate()); } /** \return the column-pivoting Householder QR decomposition of \c *this. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h index e66196b1d..b1332be5e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h @@ -63,12 +63,12 @@ ColPivHouseholderQR class FullPivHouseholderQR RealScalar maxPivot() const { return m_maxpivot; } protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } + MatrixType m_qr; HCoeffsType m_hCoeffs; IntDiagSizeVectorType m_rows_transpositions; @@ -407,6 +413,8 @@ typename MatrixType::RealScalar FullPivHouseholderQR::logAbsDetermin template FullPivHouseholderQR& FullPivHouseholderQR::compute(const MatrixType& matrix) { + check_template_parameters(); + using std::abs; Index rows = matrix.rows(); Index cols = matrix.cols(); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR.h index 3a94a3c34..343a66499 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR.h @@ -189,6 +189,12 @@ template class HouseholderQR const HCoeffsType& hCoeffs() const { return m_hCoeffs; } protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } + MatrixType m_qr; HCoeffsType m_hCoeffs; RowVectorType m_temp; @@ -349,6 +355,8 @@ struct solve_retval, Rhs> template HouseholderQR& HouseholderQR::compute(const MatrixType& matrix) { + check_template_parameters(); + Index rows = matrix.rows(); Index cols = matrix.cols(); Index size = (std::min)(rows,cols); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h index a2cc2a9e2..36138101d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h @@ -47,7 +47,7 @@ namespace Eigen { * You can then apply it to a vector. * * R is the sparse triangular factor. Use matrixQR() to get it as SparseMatrix. - * NOTE : The Index type of R is always UF_long. You can get it with SPQR::Index + * NOTE : The Index type of R is always SuiteSparse_long. You can get it with SPQR::Index * * \tparam _MatrixType The type of the sparse matrix A, must be a column-major SparseMatrix<> * NOTE @@ -59,24 +59,18 @@ class SPQR public: typedef typename _MatrixType::Scalar Scalar; typedef typename _MatrixType::RealScalar RealScalar; - typedef UF_long Index ; + typedef SuiteSparse_long Index ; typedef SparseMatrix MatrixType; typedef PermutationMatrix PermutationType; public: SPQR() - : m_isInitialized(false), - m_ordering(SPQR_ORDERING_DEFAULT), - m_allow_tol(SPQR_DEFAULT_TOL), - m_tolerance (NumTraits::epsilon()) + : m_isInitialized(false), m_ordering(SPQR_ORDERING_DEFAULT), m_allow_tol(SPQR_DEFAULT_TOL), m_tolerance (NumTraits::epsilon()), m_useDefaultThreshold(true) { cholmod_l_start(&m_cc); } - SPQR(const _MatrixType& matrix) - : m_isInitialized(false), - m_ordering(SPQR_ORDERING_DEFAULT), - m_allow_tol(SPQR_DEFAULT_TOL), - m_tolerance (NumTraits::epsilon()) + SPQR(const _MatrixType& matrix) + : m_isInitialized(false), m_ordering(SPQR_ORDERING_DEFAULT), m_allow_tol(SPQR_DEFAULT_TOL), m_tolerance (NumTraits::epsilon()), m_useDefaultThreshold(true) { cholmod_l_start(&m_cc); compute(matrix); @@ -101,10 +95,26 @@ class SPQR if(m_isInitialized) SPQR_free(); MatrixType mat(matrix); + + /* Compute the default threshold as in MatLab, see: + * Tim Davis, "Algorithm 915, SuiteSparseQR: Multifrontal Multithreaded Rank-Revealing + * Sparse QR Factorization, ACM Trans. on Math. Soft. 38(1), 2011, Page 8:3 + */ + RealScalar pivotThreshold = m_tolerance; + if(m_useDefaultThreshold) + { + using std::max; + RealScalar max2Norm = 0.0; + for (int j = 0; j < mat.cols(); j++) max2Norm = (max)(max2Norm, mat.col(j).norm()); + if(max2Norm==RealScalar(0)) + max2Norm = RealScalar(1); + pivotThreshold = 20 * (mat.rows() + mat.cols()) * max2Norm * NumTraits::epsilon(); + } + cholmod_sparse A; A = viewAsCholmod(mat); Index col = matrix.cols(); - m_rank = SuiteSparseQR(m_ordering, m_tolerance, col, &A, + m_rank = SuiteSparseQR(m_ordering, pivotThreshold, col, &A, &m_cR, &m_E, &m_H, &m_HPinv, &m_HTau, &m_cc); if (!m_cR) @@ -120,7 +130,7 @@ class SPQR /** * Get the number of rows of the input matrix and the Q matrix */ - inline Index rows() const {return m_H->nrow; } + inline Index rows() const {return m_cR->nrow; } /** * Get the number of columns of the input matrix. @@ -145,16 +155,25 @@ class SPQR { eigen_assert(m_isInitialized && " The QR factorization should be computed first, call compute()"); eigen_assert(b.cols()==1 && "This method is for vectors only"); - + //Compute Q^T * b - typename Dest::PlainObject y; + typename Dest::PlainObject y, y2; y = matrixQ().transpose() * b; - // Solves with the triangular matrix R + + // Solves with the triangular matrix R Index rk = this->rank(); - y.topRows(rk) = this->matrixR().topLeftCorner(rk, rk).template triangularView().solve(y.topRows(rk)); - y.bottomRows(cols()-rk).setZero(); + y2 = y; + y.resize((std::max)(cols(),Index(y.rows())),y.cols()); + y.topRows(rk) = this->matrixR().topLeftCorner(rk, rk).template triangularView().solve(y2.topRows(rk)); + // Apply the column permutation - dest.topRows(cols()) = colsPermutation() * y.topRows(cols()); + // colsPermutation() performs a copy of the permutation, + // so let's apply it manually: + for(Index i = 0; i < rk; ++i) dest.row(m_E[i]) = y.row(i); + for(Index i = rk; i < cols(); ++i) dest.row(m_E[i]).setZero(); + +// y.bottomRows(y.rows()-rk).setZero(); +// dest = colsPermutation() * y.topRows(cols()); m_info = Success; } @@ -197,7 +216,11 @@ class SPQR /// Set the fill-reducing ordering method to be used void setSPQROrdering(int ord) { m_ordering = ord;} /// Set the tolerance tol to treat columns with 2-norm < =tol as zero - void setPivotThreshold(const RealScalar& tol) { m_tolerance = tol; } + void setPivotThreshold(const RealScalar& tol) + { + m_useDefaultThreshold = false; + m_tolerance = tol; + } /** \returns a pointer to the SPQR workspace */ cholmod_common *cholmodCommon() const { return &m_cc; } @@ -230,6 +253,7 @@ class SPQR mutable cholmod_dense *m_HTau; // The Householder coefficients mutable Index m_rank; // The rank of the matrix mutable cholmod_common m_cc; // Workspace and parameters + bool m_useDefaultThreshold; // Use default threshold template friend struct SPQR_QProduct; }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h index dff9e44eb..1b2977419 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h @@ -742,6 +742,11 @@ template class JacobiSVD private: void allocate(Index rows, Index cols, unsigned int computationOptions); + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } protected: MatrixUType m_matrixU; @@ -762,6 +767,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,14 +814,17 @@ 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 JacobiSVD& JacobiSVD::compute(const MatrixType& matrix, unsigned int computationOptions) { + check_template_parameters(); + using std::abs; allocate(matrix.rows(), matrix.cols(), computationOptions); @@ -826,21 +835,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 +875,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..0c90bafbe 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h @@ -57,6 +57,16 @@ public: inline BlockImpl(const XprType& 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 coeff(int row, int col) const + { + return m_matrix.coeff(row + IsRowMajor ? m_outerStart : 0, col +IsRowMajor ? 0 : m_outerStart); + } + + inline const Scalar coeff(int index) const + { + return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart); + } 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(); } @@ -68,6 +78,8 @@ public: const internal::variable_if_dynamic m_outerSize; EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) + private: + Index nonZeros() const; }; @@ -82,6 +94,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) @@ -223,6 +236,118 @@ public: else return Map >(m_matrix.innerNonZeroPtr()+m_outerStart, m_outerSize.value()).sum(); } + + inline Scalar& coeffRef(int row, int col) + { + return m_matrix.const_cast_derived().coeffRef(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart)); + } + + inline const Scalar coeff(int row, int col) const + { + return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart)); + } + + inline const Scalar coeff(int index) const + { + return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart); + } + + 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; + +}; + + +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(); + } + + inline const Scalar coeff(int row, int col) const + { + return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart)); + } + + inline const Scalar coeff(int index) const + { + return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart); + } const Scalar& lastCoeff() const { @@ -265,7 +390,8 @@ const typename SparseMatrixBase::ConstInnerVectorReturnType SparseMatri * is col-major (resp. row-major). */ template -Block SparseMatrixBase::innerVectors(Index outerStart, Index outerSize) +typename SparseMatrixBase::InnerVectorsReturnType +SparseMatrixBase::innerVectors(Index outerStart, Index outerSize) { return Block(derived(), IsRowMajor ? outerStart : 0, IsRowMajor ? 0 : outerStart, @@ -277,7 +403,8 @@ Block SparseMatrixBase::innerVectors(Inde * is col-major (resp. row-major). Read-only. */ template -const Block SparseMatrixBase::innerVectors(Index outerStart, Index outerSize) const +const typename SparseMatrixBase::ConstInnerVectorsReturnType +SparseMatrixBase::innerVectors(Index outerStart, Index outerSize) const { return Block(derived(), IsRowMajor ? outerStart : 0, IsRowMajor ? 0 : outerStart, @@ -304,8 +431,8 @@ public: : m_matrix(xpr), m_startRow( (BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) ? i : 0), m_startCol( (BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) ? i : 0), - m_blockRows(xpr.rows()), - m_blockCols(xpr.cols()) + m_blockRows(BlockRows==1 ? 1 : xpr.rows()), + m_blockCols(BlockCols==1 ? 1 : xpr.cols()) {} /** Dynamic-size constructor @@ -407,3 +534,4 @@ public: } // end namespace Eigen #endif // EIGEN_SPARSE_BLOCK_H + diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseBinaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseBinaryOp.h index 60ca7690c..4ca912833 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseBinaryOp.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseBinaryOp.h @@ -314,10 +314,10 @@ SparseMatrixBase::operator+=(const SparseMatrixBase& othe template template -EIGEN_STRONG_INLINE const EIGEN_SPARSE_CWISE_PRODUCT_RETURN_TYPE +EIGEN_STRONG_INLINE const typename SparseMatrixBase::template CwiseProductDenseReturnType::Type SparseMatrixBase::cwiseProduct(const MatrixBase &other) const { - return EIGEN_SPARSE_CWISE_PRODUCT_RETURN_TYPE(derived(), other.derived()); + return typename CwiseProductDenseReturnType::Type(derived(), other.derived()); } } // end namespace Eigen diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h index 78411db98..ccb6ae7b7 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h @@ -180,7 +180,7 @@ struct sparse_time_dense_product_impl -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/SparseMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h index ba5e3a9b6..2ff201551 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h @@ -691,7 +691,8 @@ class SparseMatrix m_data.swap(other.m_data); } - /** Sets *this to the identity matrix */ + /** Sets *this to the identity matrix. + * This function also turns the matrix into compressed mode, and drop any reserved memory. */ inline void setIdentity() { eigen_assert(rows() == cols() && "ONLY FOR SQUARED MATRICES"); @@ -699,6 +700,8 @@ class SparseMatrix Eigen::Map >(&this->m_data.index(0), rows()).setLinSpaced(0, rows()-1); Eigen::Map >(&this->m_data.value(0), rows()).setOnes(); Eigen::Map >(this->m_outerIndex, rows()+1).setLinSpaced(0, rows()); + std::free(m_innerNonZeros); + m_innerNonZeros = 0; } inline SparseMatrix& operator=(const SparseMatrix& other) { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h index bbcf7fb1c..9341d9ad2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h @@ -23,7 +23,14 @@ namespace Eigen { * This class can be extended with the help of the plugin mechanism described on the page * \ref TopicCustomizingEigen by defining the preprocessor symbol \c EIGEN_SPARSEMATRIXBASE_PLUGIN. */ -template class SparseMatrixBase : public EigenBase +template class SparseMatrixBase +#ifndef EIGEN_PARSED_BY_DOXYGEN + : public internal::special_scalar_op_base::Scalar, + typename NumTraits::Scalar>::Real, + EigenBase > +#else + : public EigenBase +#endif // not EIGEN_PARSED_BY_DOXYGEN { public: @@ -36,7 +43,6 @@ template class SparseMatrixBase : public EigenBase >::type PacketReturnType; typedef SparseMatrixBase StorageBaseType; - typedef EigenBase Base; template Derived& operator=(const EigenBase &other) @@ -132,6 +138,9 @@ template class SparseMatrixBase : public EigenBase inline Derived& derived() { return *static_cast(this); } inline Derived& const_cast_derived() const { return *static_cast(const_cast(this)); } + + typedef internal::special_scalar_op_base > Base; + using Base::operator*; #endif // not EIGEN_PARSED_BY_DOXYGEN #define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::SparseMatrixBase @@ -317,20 +326,18 @@ template class SparseMatrixBase : public EigenBase Derived& operator*=(const Scalar& other); Derived& operator/=(const Scalar& other); - #define EIGEN_SPARSE_CWISE_PRODUCT_RETURN_TYPE \ - CwiseBinaryOp< \ - internal::scalar_product_op< \ - typename internal::scalar_product_traits< \ - typename internal::traits::Scalar, \ - typename internal::traits::Scalar \ - >::ReturnType \ - >, \ - const Derived, \ - const OtherDerived \ - > + template struct CwiseProductDenseReturnType { + typedef CwiseBinaryOp::Scalar, + typename internal::traits::Scalar + >::ReturnType>, + const Derived, + const OtherDerived + > Type; + }; template - EIGEN_STRONG_INLINE const EIGEN_SPARSE_CWISE_PRODUCT_RETURN_TYPE + EIGEN_STRONG_INLINE const typename CwiseProductDenseReturnType::Type cwiseProduct(const MatrixBase &other) const; // sparse * sparse @@ -358,7 +365,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 @@ -403,8 +411,10 @@ template class SparseMatrixBase : public EigenBase const ConstInnerVectorReturnType innerVector(Index outer) const; // set of inner-vectors - Block innerVectors(Index outerStart, Index outerSize); - const Block innerVectors(Index outerStart, Index outerSize) const; + typedef Block InnerVectorsReturnType; + typedef Block ConstInnerVectorsReturnType; + InnerVectorsReturnType innerVectors(Index outerStart, Index outerSize); + const ConstInnerVectorsReturnType innerVectors(Index outerStart, Index outerSize) const; /** \internal use operator= */ template 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; j class SparseMatrixBase; template class SparseMatrix; template class DynamicSparseMatrix; template class SparseVector; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseVector.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseVector.h index 7e15c814b..49865d0e7 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseVector.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseVector.h @@ -158,6 +158,7 @@ class SparseVector Index inner = IsColVector ? row : col; Index outer = IsColVector ? col : row; + EIGEN_ONLY_USED_FOR_DEBUG(outer); eigen_assert(outer==0); return insert(inner); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/TriangularSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/TriangularSolver.h index cb8ad82b4..ccc12af79 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/TriangularSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/TriangularSolver.h @@ -69,7 +69,7 @@ struct sparse_solve_triangular_selector for(int i=lhs.rows()-1 ; i>=0 ; --i) { Scalar tmp = other.coeff(i,col); - Scalar l_ii = 0; + Scalar l_ii(0); typename Lhs::InnerIterator it(lhs, i); while(it && it.index()cols(); ++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()); + using std::abs; + det *= abs(it.value()); break; } } @@ -296,7 +296,8 @@ class SparseLU : public internal::SparseLUImplcols(); ++j) + { + for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it) + { + if(it.index() == j) + { + if(it.value()<0) + det = -det; + else if(it.value()==0) + return 0; + break; + } + } + } + return det * m_detPermR * m_detPermC; + } + + /** \returns The determinant of the matrix. + * + * \sa absDeterminant(), logAbsDeterminant() + */ + Scalar determinant() + { + eigen_assert(m_factorizationIsOk && "The matrix should be factorized first."); + // Initialize with the determinant of the row matrix + Scalar det = Scalar(1.); + // Note that the diagonal blocks of U are stored in supernodes, + // which are available in the L part :) + for (Index j = 0; j < this->cols(); ++j) + { + for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it) + { + if(it.index() == j) + { + det *= it.value(); + break; + } + } + } + return det * Scalar(m_detPermR * m_detPermC); + } protected: // Functions void initperfvalues() { - m_perfv.panel_size = 1; + m_perfv.panel_size = 16; m_perfv.relax = 1; m_perfv.maxsuper = 128; m_perfv.rowblk = 16; @@ -346,8 +390,8 @@ class SparseLU : public internal::SparseLUImpl m_perfv; RealScalar m_diagpivotthresh; // Specifies the threshold used for a diagonal entry to be an acceptable pivot - Index m_nnzL, m_nnzU; // Nonzeros in L and U factors - Index m_detPermR; // Determinant of the coefficient matrix + Index m_nnzL, m_nnzU; // Nonzeros in L and U factors + Index m_detPermR, m_detPermC; // Determinants of the permutation matrices private: // Disable copy constructor SparseLU (const SparseLU& ); @@ -623,7 +667,8 @@ void SparseLU::factorize(const MatrixType& matrix) } // Update the determinant of the row permutation matrix - if (pivrow != jj) m_detPermR *= -1; + // FIXME: the following test is not correct, we should probably take iperm_c into account and pivrow is not directly the row pivot. + if (pivrow != jj) m_detPermR = -m_detPermR; // Prune columns (0:jj-1) using column jj Base::pruneL(jj, m_perm_r.indices(), pivrow, nseg, segrep, repfnz_k, xprune, m_glu); @@ -638,10 +683,13 @@ void SparseLU::factorize(const MatrixType& matrix) jcol += panel_size; // Move to the next panel } // end for -- end elimination + m_detPermR = m_perm_r.determinant(); + m_detPermC = m_perm_c.determinant(); + // Count the number of nonzeros in factors Base::countnz(n, m_nnzL, m_nnzU, m_glu); // Apply permutation to the L subscripts - Base::fixupL(n, m_perm_r.indices(), m_glu); + Base::fixupL(n, m_perm_r.indices(), m_glu); // Create supernode matrix L m_Lstore.setInfos(m, n, m_glu.lusup, m_glu.xlusup, m_glu.lsub, m_glu.xlsub, m_glu.supno, m_glu.xsup); @@ -701,8 +749,8 @@ struct SparseLUMatrixUReturnType : internal::no_assignment_operator } else { - Map, 0, OuterStride<> > A( &(m_mapL.valuePtr()[luptr]), nsupc, nsupc, OuterStride<>(lda) ); - Map< Matrix, 0, OuterStride<> > U (&(X(fsupc,0)), nsupc, nrhs, OuterStride<>(n) ); + Map, 0, OuterStride<> > A( &(m_mapL.valuePtr()[luptr]), nsupc, nsupc, OuterStride<>(lda) ); + Map< Matrix, 0, OuterStride<> > U (&(X(fsupc,0)), nsupc, nrhs, OuterStride<>(n) ); U = A.template triangularView().solve(U); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLUImpl.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLUImpl.h index 14d70897d..99d651e40 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLUImpl.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLUImpl.h @@ -21,6 +21,8 @@ class SparseLUImpl { public: typedef Matrix ScalarVector; + typedef Matrix ScalarMatrix; + typedef Map > MappedMatrixBlock; typedef Matrix IndexVector; typedef typename ScalarVector::RealScalar RealScalar; typedef Ref > BlockScalarVector; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h index 1ffa7d54e..45f96d16a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h @@ -153,8 +153,8 @@ Index SparseLUImpl::memInit(Index m, Index n, Index annz, Index lw { Index& num_expansions = glu.num_expansions; //No memory expansions so far num_expansions = 0; - glu.nzumax = glu.nzlumax = (std::min)(fillratio * annz / n, m) * n; // estimated number of nonzeros in U - glu.nzlmax = (std::max)(Index(4), fillratio) * annz / 4; // estimated nnz in L factor + glu.nzumax = glu.nzlumax = (std::min)(fillratio * (annz+1) / n, m) * n; // estimated number of nonzeros in U + glu.nzlmax = (std::max)(Index(4), fillratio) * (annz+1) / 4; // estimated nnz in L factor // Return the estimated size to the user if necessary Index tempSpace; tempSpace = (2*panel_size + 4 + LUNoMarker) * m * sizeof(Index) + (panel_size + 1) * m * sizeof(Scalar); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h index ad6f2183f..54a569408 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++() { @@ -236,7 +236,7 @@ void MappedSuperNodalMatrix::solveInPlace( MatrixBase&X) con Index n = X.rows(); Index nrhs = X.cols(); const Scalar * Lval = valuePtr(); // Nonzero values - Matrix work(n, nrhs); // working vector + Matrix work(n, nrhs); // working vector work.setZero(); for (Index k = 0; k <= nsuper(); k ++) { @@ -267,12 +267,12 @@ void MappedSuperNodalMatrix::solveInPlace( MatrixBase&X) con Index lda = colIndexPtr()[fsupc+1] - luptr; // Triangular solve - Map, 0, OuterStride<> > A( &(Lval[luptr]), nsupc, nsupc, OuterStride<>(lda) ); - Map< Matrix, 0, OuterStride<> > U (&(X(fsupc,0)), nsupc, nrhs, OuterStride<>(n) ); + Map, 0, OuterStride<> > A( &(Lval[luptr]), nsupc, nsupc, OuterStride<>(lda) ); + Map< Matrix, 0, OuterStride<> > U (&(X(fsupc,0)), nsupc, nrhs, OuterStride<>(n) ); U = A.template triangularView().solve(U); // Matrix-vector product - new (&A) Map, 0, OuterStride<> > ( &(Lval[luptr+nsupc]), nrow, nsupc, OuterStride<>(lda) ); + new (&A) Map, 0, OuterStride<> > ( &(Lval[luptr+nsupc]), nrow, nsupc, OuterStride<>(lda) ); work.block(0, 0, nrow, nrhs) = A * U; //Begin Scatter diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_column_bmod.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_column_bmod.h index f24bd87d3..cacc7e987 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_column_bmod.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_column_bmod.h @@ -162,11 +162,11 @@ Index SparseLUImpl::column_bmod(const Index jcol, const Index nseg // points to the beginning of jcol in snode L\U(jsupno) ufirst = glu.xlusup(jcol) + d_fsupc; Index lda = glu.xlusup(jcol+1) - glu.xlusup(jcol); - Map, 0, OuterStride<> > A( &(glu.lusup.data()[luptr]), nsupc, nsupc, OuterStride<>(lda) ); + MappedMatrixBlock A( &(glu.lusup.data()[luptr]), nsupc, nsupc, OuterStride<>(lda) ); VectorBlock u(glu.lusup, ufirst, nsupc); u = A.template triangularView().solve(u); - new (&A) Map, 0, OuterStride<> > ( &(glu.lusup.data()[luptr+nsupc]), nrow, nsupc, OuterStride<>(lda) ); + new (&A) MappedMatrixBlock ( &(glu.lusup.data()[luptr+nsupc]), nrow, nsupc, OuterStride<>(lda) ); VectorBlock l(glu.lusup, ufirst+nsupc, nrow); l.noalias() -= A * u; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_kernel_bmod.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_kernel_bmod.h index 0d0283b13..6af026754 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_kernel_bmod.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_kernel_bmod.h @@ -56,7 +56,7 @@ EIGEN_DONT_INLINE void LU_kernel_bmod::run(const int segsi // Dense triangular solve -- start effective triangle luptr += lda * no_zeros + no_zeros; // Form Eigen matrix and vector - Map, 0, OuterStride<> > A( &(lusup.data()[luptr]), segsize, segsize, OuterStride<>(lda) ); + Map, 0, OuterStride<> > A( &(lusup.data()[luptr]), segsize, segsize, OuterStride<>(lda) ); Map > u(tempv.data(), segsize); u = A.template triangularView().solve(u); @@ -65,7 +65,7 @@ EIGEN_DONT_INLINE void LU_kernel_bmod::run(const int segsi luptr += segsize; const Index PacketSize = internal::packet_traits::size; Index ldl = internal::first_multiple(nrow, PacketSize); - Map, 0, OuterStride<> > B( &(lusup.data()[luptr]), nrow, segsize, OuterStride<>(lda) ); + Map, 0, OuterStride<> > B( &(lusup.data()[luptr]), nrow, segsize, OuterStride<>(lda) ); Index aligned_offset = internal::first_aligned(tempv.data()+segsize, PacketSize); Index aligned_with_B_offset = (PacketSize-internal::first_aligned(B.data(), PacketSize))%PacketSize; Map, 0, OuterStride<> > l(tempv.data()+segsize+aligned_offset+aligned_with_B_offset, nrow, OuterStride<>(ldl) ); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_panel_bmod.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_panel_bmod.h index da0e0fc3c..9d2ff2906 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_panel_bmod.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_panel_bmod.h @@ -102,7 +102,7 @@ void SparseLUImpl::panel_bmod(const Index m, const Index w, const if(nsupc >= 2) { Index ldu = internal::first_multiple(u_rows, PacketSize); - Map, Aligned, OuterStride<> > U(tempv.data(), u_rows, u_cols, OuterStride<>(ldu)); + Map > U(tempv.data(), u_rows, u_cols, OuterStride<>(ldu)); // gather U Index u_col = 0; @@ -136,17 +136,17 @@ void SparseLUImpl::panel_bmod(const Index m, const Index w, const Index lda = glu.xlusup(fsupc+1) - glu.xlusup(fsupc); no_zeros = (krep - u_rows + 1) - fsupc; luptr += lda * no_zeros + no_zeros; - Map, 0, OuterStride<> > A(glu.lusup.data()+luptr, u_rows, u_rows, OuterStride<>(lda) ); + MappedMatrixBlock A(glu.lusup.data()+luptr, u_rows, u_rows, OuterStride<>(lda) ); U = A.template triangularView().solve(U); // update luptr += u_rows; - Map, 0, OuterStride<> > B(glu.lusup.data()+luptr, nrow, u_rows, OuterStride<>(lda) ); + MappedMatrixBlock B(glu.lusup.data()+luptr, nrow, u_rows, OuterStride<>(lda) ); eigen_assert(tempv.size()>w*ldu + nrow*w + 1); Index ldl = internal::first_multiple(nrow, PacketSize); Index offset = (PacketSize-internal::first_aligned(B.data(), PacketSize)) % PacketSize; - Map, 0, OuterStride<> > L(tempv.data()+w*ldu+offset, nrow, u_cols, OuterStride<>(ldl)); + MappedMatrixBlock L(tempv.data()+w*ldu+offset, nrow, u_cols, OuterStride<>(ldl)); L.setZero(); internal::sparselu_gemm(L.rows(), L.cols(), B.cols(), B.data(), B.outerStride(), U.data(), U.outerStride(), L.data(), L.outerStride()); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_pivotL.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_pivotL.h index ddcd4ec98..2e49ef667 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_pivotL.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_pivotL.h @@ -71,13 +71,14 @@ Index SparseLUImpl::pivotL(const Index jcol, const RealScalar& dia // Determine the largest abs numerical value for partial pivoting Index diagind = iperm_c(jcol); // diagonal index - RealScalar pivmax = 0.0; + RealScalar pivmax(-1.0); Index pivptr = nsupc; Index diag = emptyIdxLU; RealScalar rtemp; Index isub, icol, itemp, k; for (isub = nsupc; isub < nsupr; ++isub) { - rtemp = std::abs(lu_col_ptr[isub]); + using std::abs; + rtemp = abs(lu_col_ptr[isub]); if (rtemp > pivmax) { pivmax = rtemp; pivptr = isub; @@ -86,8 +87,9 @@ Index SparseLUImpl::pivotL(const Index jcol, const RealScalar& dia } // Test for singularity - if ( pivmax == 0.0 ) { - pivrow = lsub_ptr[pivptr]; + if ( pivmax <= RealScalar(0.0) ) { + // if pivmax == -1, the column is structurally empty, otherwise it is only numerically zero + pivrow = pivmax < RealScalar(0.0) ? diagind : lsub_ptr[pivptr]; perm_r(pivrow) = jcol; return (jcol+1); } @@ -101,7 +103,8 @@ Index SparseLUImpl::pivotL(const Index jcol, const RealScalar& dia if (diag >= 0 ) { // Diagonal element exists - rtemp = std::abs(lu_col_ptr[diag]); + using std::abs; + rtemp = abs(lu_col_ptr[diag]); if (rtemp != 0.0 && rtemp >= thresh) pivptr = diag; } pivrow = lsub_ptr[pivptr]; 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/Eigen/src/plugins/ArrayCwiseBinaryOps.h b/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseBinaryOps.h index 5c8c476ee..1951286f3 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseBinaryOps.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseBinaryOps.h @@ -70,6 +70,43 @@ max return (max)(Derived::PlainObject::Constant(rows(), cols(), other)); } + +#define EIGEN_MAKE_CWISE_COMP_OP(OP, COMPARATOR) \ +template \ +EIGEN_STRONG_INLINE const CwiseBinaryOp, const Derived, const OtherDerived> \ +OP(const EIGEN_CURRENT_STORAGE_BASE_CLASS &other) const \ +{ \ + return CwiseBinaryOp, const Derived, const OtherDerived>(derived(), other.derived()); \ +}\ +typedef CwiseBinaryOp, const Derived, const CwiseNullaryOp, PlainObject> > Cmp ## COMPARATOR ## ReturnType; \ +typedef CwiseBinaryOp, const CwiseNullaryOp, PlainObject>, const Derived > RCmp ## COMPARATOR ## ReturnType; \ +EIGEN_STRONG_INLINE const Cmp ## COMPARATOR ## ReturnType \ +OP(const Scalar& s) const { \ + return this->OP(Derived::PlainObject::Constant(rows(), cols(), s)); \ +} \ +friend EIGEN_STRONG_INLINE const RCmp ## COMPARATOR ## ReturnType \ +OP(const Scalar& s, const Derived& d) { \ + return Derived::PlainObject::Constant(d.rows(), d.cols(), s).OP(d); \ +} + +#define EIGEN_MAKE_CWISE_COMP_R_OP(OP, R_OP, RCOMPARATOR) \ +template \ +EIGEN_STRONG_INLINE const CwiseBinaryOp, const OtherDerived, const Derived> \ +OP(const EIGEN_CURRENT_STORAGE_BASE_CLASS &other) const \ +{ \ + return CwiseBinaryOp, const OtherDerived, const Derived>(other.derived(), derived()); \ +} \ +\ +inline const RCmp ## RCOMPARATOR ## ReturnType \ +OP(const Scalar& s) const { \ + return Derived::PlainObject::Constant(rows(), cols(), s).R_OP(*this); \ +} \ +friend inline const Cmp ## RCOMPARATOR ## ReturnType \ +OP(const Scalar& s, const Derived& d) { \ + return d.R_OP(Derived::PlainObject::Constant(d.rows(), d.cols(), s)); \ +} + + /** \returns an expression of the coefficient-wise \< operator of *this and \a other * * Example: \include Cwise_less.cpp @@ -77,7 +114,7 @@ max * * \sa all(), any(), operator>(), operator<=() */ -EIGEN_MAKE_CWISE_BINARY_OP(operator<,std::less) +EIGEN_MAKE_CWISE_COMP_OP(operator<, LT) /** \returns an expression of the coefficient-wise \<= operator of *this and \a other * @@ -86,7 +123,7 @@ EIGEN_MAKE_CWISE_BINARY_OP(operator<,std::less) * * \sa all(), any(), operator>=(), operator<() */ -EIGEN_MAKE_CWISE_BINARY_OP(operator<=,std::less_equal) +EIGEN_MAKE_CWISE_COMP_OP(operator<=, LE) /** \returns an expression of the coefficient-wise \> operator of *this and \a other * @@ -95,7 +132,7 @@ EIGEN_MAKE_CWISE_BINARY_OP(operator<=,std::less_equal) * * \sa all(), any(), operator>=(), operator<() */ -EIGEN_MAKE_CWISE_BINARY_OP(operator>,std::greater) +EIGEN_MAKE_CWISE_COMP_R_OP(operator>, operator<, LT) /** \returns an expression of the coefficient-wise \>= operator of *this and \a other * @@ -104,7 +141,7 @@ EIGEN_MAKE_CWISE_BINARY_OP(operator>,std::greater) * * \sa all(), any(), operator>(), operator<=() */ -EIGEN_MAKE_CWISE_BINARY_OP(operator>=,std::greater_equal) +EIGEN_MAKE_CWISE_COMP_R_OP(operator>=, operator<=, LE) /** \returns an expression of the coefficient-wise == operator of *this and \a other * @@ -118,7 +155,7 @@ EIGEN_MAKE_CWISE_BINARY_OP(operator>=,std::greater_equal) * * \sa all(), any(), isApprox(), isMuchSmallerThan() */ -EIGEN_MAKE_CWISE_BINARY_OP(operator==,std::equal_to) +EIGEN_MAKE_CWISE_COMP_OP(operator==, EQ) /** \returns an expression of the coefficient-wise != operator of *this and \a other * @@ -132,7 +169,10 @@ EIGEN_MAKE_CWISE_BINARY_OP(operator==,std::equal_to) * * \sa all(), any(), isApprox(), isMuchSmallerThan() */ -EIGEN_MAKE_CWISE_BINARY_OP(operator!=,std::not_equal_to) +EIGEN_MAKE_CWISE_COMP_OP(operator!=, NEQ) + +#undef EIGEN_MAKE_CWISE_COMP_OP +#undef EIGEN_MAKE_CWISE_COMP_R_OP // scalar addition @@ -209,3 +249,5 @@ operator||(const EIGEN_CURRENT_STORAGE_BASE_CLASS &other) const THIS_METHOD_IS_ONLY_FOR_EXPRESSIONS_OF_BOOL); return CwiseBinaryOp(derived(),other.derived()); } + + diff --git a/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseUnaryOps.h b/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseUnaryOps.h index a59636790..1c3ed3fcd 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseUnaryOps.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseUnaryOps.h @@ -185,19 +185,3 @@ cube() const { return derived(); } - -#define EIGEN_MAKE_SCALAR_CWISE_UNARY_OP(METHOD_NAME,FUNCTOR) \ - inline const CwiseUnaryOp >, const Derived> \ - METHOD_NAME(const Scalar& s) const { \ - return CwiseUnaryOp >, const Derived> \ - (derived(), std::bind2nd(FUNCTOR(), s)); \ - } - -EIGEN_MAKE_SCALAR_CWISE_UNARY_OP(operator==, std::equal_to) -EIGEN_MAKE_SCALAR_CWISE_UNARY_OP(operator!=, std::not_equal_to) -EIGEN_MAKE_SCALAR_CWISE_UNARY_OP(operator<, std::less) -EIGEN_MAKE_SCALAR_CWISE_UNARY_OP(operator<=, std::less_equal) -EIGEN_MAKE_SCALAR_CWISE_UNARY_OP(operator>, std::greater) -EIGEN_MAKE_SCALAR_CWISE_UNARY_OP(operator>=, std::greater_equal) - - diff --git a/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h b/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h index 7f62149e0..c4a042b70 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h @@ -124,3 +124,20 @@ cwiseQuotient(const EIGEN_CURRENT_STORAGE_BASE_CLASS &other) const { return CwiseBinaryOp, const Derived, const OtherDerived>(derived(), other.derived()); } + +typedef CwiseBinaryOp, const Derived, const ConstantReturnType> CwiseScalarEqualReturnType; + +/** \returns an expression of the coefficient-wise == operator of \c *this and a scalar \a s + * + * \warning this performs an exact comparison, which is generally a bad idea with floating-point types. + * In order to check for equality between two vectors or matrices with floating-point coefficients, it is + * generally a far better idea to use a fuzzy comparison as provided by isApprox() and + * isMuchSmallerThan(). + * + * \sa cwiseEqual(const MatrixBase &) const + */ +inline const CwiseScalarEqualReturnType +cwiseEqual(const Scalar& s) const +{ + return CwiseScalarEqualReturnType(derived(), Derived::Constant(rows(), cols(), s), internal::scalar_cmp_op()); +} diff --git a/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseUnaryOps.h b/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseUnaryOps.h index 0cf0640ba..8de10935d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseUnaryOps.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseUnaryOps.h @@ -50,18 +50,3 @@ cwiseSqrt() const { return derived(); } inline const CwiseUnaryOp, const Derived> cwiseInverse() const { return derived(); } -/** \returns an expression of the coefficient-wise == operator of \c *this and a scalar \a s - * - * \warning this performs an exact comparison, which is generally a bad idea with floating-point types. - * In order to check for equality between two vectors or matrices with floating-point coefficients, it is - * generally a far better idea to use a fuzzy comparison as provided by isApprox() and - * isMuchSmallerThan(). - * - * \sa cwiseEqual(const MatrixBase &) const - */ -inline const CwiseUnaryOp >, const Derived> -cwiseEqual(const Scalar& s) const -{ - return CwiseUnaryOp >,const Derived> - (derived(), std::bind1st(std::equal_to(), s)); -} diff --git a/gtsam/3rdparty/Eigen/blas/xerbla.cpp b/gtsam/3rdparty/Eigen/blas/xerbla.cpp index 0d57710fe..dd39a5244 100644 --- a/gtsam/3rdparty/Eigen/blas/xerbla.cpp +++ b/gtsam/3rdparty/Eigen/blas/xerbla.cpp @@ -1,7 +1,7 @@ #include -#if (defined __GNUC__) +#if (defined __GNUC__) && (!defined __MINGW32__) && (!defined __CYGWIN__) #define EIGEN_WEAK_LINKING __attribute__ ((weak)) #else #define EIGEN_WEAK_LINKING diff --git a/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake b/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake index 11ecc9585..2b11d8360 100644 --- a/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake +++ b/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake @@ -26,29 +26,18 @@ include(CTest) set(EIGEN_TEST_BUILD_FLAGS " " CACHE STRING "Options passed to the build command of unit tests") -# overwrite default DartConfiguration.tcl -# The worarounds are different for each version of the MSVC IDE -if(MSVC_IDE) - if(CMAKE_MAKE_PROGRAM_SAVE MATCHES "devenv") # devenv - set(EIGEN_MAKECOMMAND_PLACEHOLDER "${CMAKE_MAKE_PROGRAM_SAVE} Eigen.sln /build \"Release\" /project buildtests ${EIGEN_TEST_BUILD_FLAGS} \n# ") - else() # msbuild - set(EIGEN_MAKECOMMAND_PLACEHOLDER "${CMAKE_MAKE_PROGRAM_SAVE} buildtests.vcxproj /p:Configuration=\${CTEST_CONFIGURATION_TYPE} ${EIGEN_TEST_BUILD_FLAGS}\n# ") - endif() -else() - # for make and nmake - set(EIGEN_MAKECOMMAND_PLACEHOLDER "${CMAKE_MAKE_PROGRAM_SAVE} buildtests ${EIGEN_TEST_BUILD_FLAGS}") +# Overwrite default DartConfiguration.tcl such that ctest can build our unit tests. +# Recall that our unit tests are not in the "all" target, so we have to explicitely ask ctest to build our custom 'buildtests' target. +# At this stage, we can also add custom flags to the build tool through the user defined EIGEN_TEST_BUILD_FLAGS variable. +file(READ "${CMAKE_CURRENT_BINARY_DIR}/DartConfiguration.tcl" EIGEN_DART_CONFIG_FILE) +# try to grab the default flags +string(REGEX MATCH "MakeCommand:.*-- (.*)\nDefaultCTestConfigurationType" EIGEN_DUMMY ${EIGEN_DART_CONFIG_FILE}) +if(NOT CMAKE_MATCH_1) +string(REGEX MATCH "MakeCommand:.*[^c]make (.*)\nDefaultCTestConfigurationType" EIGEN_DUMMY ${EIGEN_DART_CONFIG_FILE}) endif() - -# copy ctest properties, which currently -# o raise the warning levels -configure_file(${CMAKE_CURRENT_BINARY_DIR}/DartConfiguration.tcl ${CMAKE_BINARY_DIR}/DartConfiguration.tcl) - -# restore default CMAKE_MAKE_PROGRAM -set(CMAKE_MAKE_PROGRAM ${CMAKE_MAKE_PROGRAM_SAVE}) -# un-set temporary variables so that it is like they never existed. -# CMake 2.6.3 introduces the more logical unset() syntax for this. -set(CMAKE_MAKE_PROGRAM_SAVE) -set(EIGEN_MAKECOMMAND_PLACEHOLDER) +string(REGEX REPLACE "MakeCommand:.*DefaultCTestConfigurationType" "MakeCommand: ${CMAKE_COMMAND} --build . --target buildtests --config \"\${CTEST_CONFIGURATION_TYPE}\" -- ${CMAKE_MATCH_1} ${EIGEN_TEST_BUILD_FLAGS}\nDefaultCTestConfigurationType" + EIGEN_DART_CONFIG_FILE2 ${EIGEN_DART_CONFIG_FILE}) +file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/DartConfiguration.tcl" ${EIGEN_DART_CONFIG_FILE2}) configure_file(${CMAKE_CURRENT_SOURCE_DIR}/CTestCustom.cmake.in ${CMAKE_BINARY_DIR}/CTestCustom.cmake) diff --git a/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake b/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake index d9e22ab1a..cbe12d51b 100644 --- a/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake +++ b/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake @@ -322,22 +322,21 @@ macro(ei_get_compilerver VAR) endif() else() # on all other system we rely on ${CMAKE_CXX_COMPILER} - # supporting a "--version" flag + # supporting a "--version" or "/version" flag - # check whether the head command exists - find_program(HEAD_EXE head NO_CMAKE_ENVIRONMENT_PATH NO_CMAKE_PATH NO_CMAKE_SYSTEM_PATH) - if(HEAD_EXE) - execute_process(COMMAND ${CMAKE_CXX_COMPILER} --version - COMMAND head -n 1 - OUTPUT_VARIABLE eigen_cxx_compiler_version_string OUTPUT_STRIP_TRAILING_WHITESPACE) + if(WIN32 AND ${CMAKE_CXX_COMPILER_ID} EQUAL "Intel") + set(EIGEN_CXX_FLAG_VERSION "/version") else() - execute_process(COMMAND ${CMAKE_CXX_COMPILER} --version - OUTPUT_VARIABLE eigen_cxx_compiler_version_string OUTPUT_STRIP_TRAILING_WHITESPACE) - string(REGEX REPLACE "[\n\r].*" "" eigen_cxx_compiler_version_string ${eigen_cxx_compiler_version_string}) + set(EIGEN_CXX_FLAG_VERSION "--version") endif() + execute_process(COMMAND ${CMAKE_CXX_COMPILER} ${EIGEN_CXX_FLAG_VERSION} + OUTPUT_VARIABLE eigen_cxx_compiler_version_string OUTPUT_STRIP_TRAILING_WHITESPACE) + string(REGEX REPLACE "[\n\r].*" "" eigen_cxx_compiler_version_string ${eigen_cxx_compiler_version_string}) + ei_get_compilerver_from_cxx_version_string("${eigen_cxx_compiler_version_string}" CNAME CVER) set(${VAR} "${CNAME}-${CVER}") + endif() endmacro(ei_get_compilerver) @@ -452,20 +451,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..6a0ce790c 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(STATUS "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/FindSPQR.cmake b/gtsam/3rdparty/Eigen/cmake/FindSPQR.cmake index 794c212af..1e958c3c1 100644 --- a/gtsam/3rdparty/Eigen/cmake/FindSPQR.cmake +++ b/gtsam/3rdparty/Eigen/cmake/FindSPQR.cmake @@ -26,7 +26,12 @@ if(SPQR_LIBRARIES) find_library(SUITESPARSE_LIBRARY SuiteSparse PATHS $ENV{SPQRDIR} ${LIB_INSTALL_DIR}) if (SUITESPARSE_LIBRARY) set(SPQR_LIBRARIES ${SPQR_LIBRARIES} ${SUITESPARSE_LIBRARY}) - endif (SUITESPARSE_LIBRARY) + endif() + + find_library(CHOLMOD_LIBRARY cholmod PATHS $ENV{UMFPACK_LIBDIR} $ENV{UMFPACKDIR} ${LIB_INSTALL_DIR}) + if(CHOLMOD_LIBRARY) + set(SPQR_LIBRARIES ${SPQR_LIBRARIES} ${CHOLMOD_LIBRARY}) + endif() endif(SPQR_LIBRARIES) diff --git a/gtsam/3rdparty/Eigen/cmake/FindUmfpack.cmake b/gtsam/3rdparty/Eigen/cmake/FindUmfpack.cmake index 16b046cd6..53cf0b49b 100644 --- a/gtsam/3rdparty/Eigen/cmake/FindUmfpack.cmake +++ b/gtsam/3rdparty/Eigen/cmake/FindUmfpack.cmake @@ -20,24 +20,29 @@ find_library(UMFPACK_LIBRARIES umfpack PATHS $ENV{UMFPACKDIR} ${LIB_INSTALL_DIR} if(UMFPACK_LIBRARIES) - if (NOT UMFPACK_LIBDIR) + if(NOT UMFPACK_LIBDIR) get_filename_component(UMFPACK_LIBDIR ${UMFPACK_LIBRARIES} PATH) endif(NOT UMFPACK_LIBDIR) find_library(COLAMD_LIBRARY colamd PATHS ${UMFPACK_LIBDIR} $ENV{UMFPACKDIR} ${LIB_INSTALL_DIR}) - if (COLAMD_LIBRARY) + if(COLAMD_LIBRARY) set(UMFPACK_LIBRARIES ${UMFPACK_LIBRARIES} ${COLAMD_LIBRARY}) - endif (COLAMD_LIBRARY) + endif () find_library(AMD_LIBRARY amd PATHS ${UMFPACK_LIBDIR} $ENV{UMFPACKDIR} ${LIB_INSTALL_DIR}) - if (AMD_LIBRARY) + if(AMD_LIBRARY) set(UMFPACK_LIBRARIES ${UMFPACK_LIBRARIES} ${AMD_LIBRARY}) - endif (AMD_LIBRARY) + endif () find_library(SUITESPARSE_LIBRARY SuiteSparse PATHS ${UMFPACK_LIBDIR} $ENV{UMFPACKDIR} ${LIB_INSTALL_DIR}) - if (SUITESPARSE_LIBRARY) + if(SUITESPARSE_LIBRARY) set(UMFPACK_LIBRARIES ${UMFPACK_LIBRARIES} ${SUITESPARSE_LIBRARY}) - endif (SUITESPARSE_LIBRARY) + endif () + + find_library(CHOLMOD_LIBRARY cholmod PATHS $ENV{UMFPACK_LIBDIR} $ENV{UMFPACKDIR} ${LIB_INSTALL_DIR}) + if(CHOLMOD_LIBRARY) + set(UMFPACK_LIBRARIES ${UMFPACK_LIBRARIES} ${CHOLMOD_LIBRARY}) + endif() endif(UMFPACK_LIBRARIES) @@ -45,4 +50,4 @@ include(FindPackageHandleStandardArgs) find_package_handle_standard_args(UMFPACK DEFAULT_MSG UMFPACK_INCLUDES UMFPACK_LIBRARIES) -mark_as_advanced(UMFPACK_INCLUDES UMFPACK_LIBRARIES AMD_LIBRARY COLAMD_LIBRARY SUITESPARSE_LIBRARY) +mark_as_advanced(UMFPACK_INCLUDES UMFPACK_LIBRARIES AMD_LIBRARY COLAMD_LIBRARY CHOLMOD_LIBRARY SUITESPARSE_LIBRARY) 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/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/CMakeLists.txt index 8a493031c..2fc2a0dfc 100644 --- a/gtsam/3rdparty/Eigen/doc/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/doc/CMakeLists.txt @@ -91,7 +91,8 @@ add_custom_target(doc ALL COMMAND doxygen Doxyfile-unsupported COMMAND ${CMAKE_COMMAND} -E rename html eigen-doc COMMAND ${CMAKE_COMMAND} -E remove eigen-doc/eigen-doc.tgz - COMMAND ${CMAKE_COMMAND} -E tar cfz eigen-doc/eigen-doc.tgz eigen-doc + COMMAND ${CMAKE_COMMAND} -E tar cfz eigen-doc.tgz eigen-doc + COMMAND ${CMAKE_COMMAND} -E rename eigen-doc.tgz eigen-doc/eigen-doc.tgz COMMAND ${CMAKE_COMMAND} -E rename eigen-doc html WORKING_DIRECTORY ${Eigen_BINARY_DIR}/doc) diff --git a/gtsam/3rdparty/Eigen/doc/Doxyfile.in b/gtsam/3rdparty/Eigen/doc/Doxyfile.in index 1a2603b04..696dd2af1 100644 --- a/gtsam/3rdparty/Eigen/doc/Doxyfile.in +++ b/gtsam/3rdparty/Eigen/doc/Doxyfile.in @@ -222,7 +222,7 @@ ALIASES = "only_for_vectors=This is only for vectors (either row- "note_about_using_kernel_to_study_multiple_solutions=If you need a complete analysis of the space of solutions, take the one solution obtained by this method and add to it elements of the kernel, as determined by kernel()." \ "note_about_checking_solutions=This method just tries to find as good a solution as possible. If you want to check whether a solution exists or if it is accurate, just call this function to get a result and then compute the error of this result, or use MatrixBase::isApprox() directly, for instance like this: \code bool a_solution_exists = (A*result).isApprox(b, precision); \endcode This method avoids dividing by zero, so that the non-existence of a solution doesn't by itself mean that you'll get \c inf or \c nan values." \ "note_try_to_help_rvo=This function returns the result by value. In order to make that efficient, it is implemented as just a return statement using a special constructor, hopefully allowing the compiler to perform a RVO (return value optimization)." \ - "nonstableyet=\warning This is not considered to be part of the stable public API yet. Changes may happen in future releases. See \ref Experimental \"Experimental parts of Eigen\" + "nonstableyet=\warning This is not considered to be part of the stable public API yet. Changes may happen in future releases. See \ref Experimental \"Experimental parts of Eigen\"" ALIASES += "eigenAutoToc= " ALIASES += "eigenManualPage=\defgroup" @@ -315,7 +315,7 @@ IDL_PROPERTY_SUPPORT = YES # member in the group (if any) for the other members of the group. By default # all members of a group must be documented explicitly. -DISTRIBUTE_GROUP_DOC = NO +DISTRIBUTE_GROUP_DOC = YES # Set the SUBGROUPING tag to YES (the default) to allow class member groups of # the same type (for instance a group of public functions) to be put as a @@ -365,7 +365,7 @@ TYPEDEF_HIDES_STRUCT = NO # 2^(16+SYMBOL_CACHE_SIZE). The valid range is 0..9, the default is 0, # corresponding to a cache size of 2^16 = 65536 symbols. -SYMBOL_CACHE_SIZE = 0 +# SYMBOL_CACHE_SIZE = 0 # Similar to the SYMBOL_CACHE_SIZE the size of the symbol lookup cache can be # set using LOOKUP_CACHE_SIZE. This cache is used to resolve symbols given @@ -562,7 +562,7 @@ GENERATE_BUGLIST = NO # disable (NO) the deprecated list. This list is created by putting # \deprecated commands in the documentation. -GENERATE_DEPRECATEDLIST= NO +GENERATE_DEPRECATEDLIST= YES # The ENABLED_SECTIONS tag can be used to enable conditional # documentation sections, marked by \if sectionname ... \endif. @@ -1465,13 +1465,13 @@ XML_OUTPUT = xml # which can be used by a validating XML parser to check the # syntax of the XML files. -XML_SCHEMA = +# XML_SCHEMA = # The XML_DTD tag can be used to specify an XML DTD, # which can be used by a validating XML parser to check the # syntax of the XML files. -XML_DTD = +# XML_DTD = # If the XML_PROGRAMLISTING tag is set to YES Doxygen will # dump the program listings (including syntax highlighting @@ -1699,7 +1699,7 @@ DOT_NUM_THREADS = 0 # the DOTFONTPATH environment variable or by setting DOT_FONTPATH to the # directory containing the font. -DOT_FONTNAME = FreeSans +DOT_FONTNAME = # The DOT_FONTSIZE tag can be used to set the size of the font of dot graphs. # The default size is 10pt. diff --git a/gtsam/3rdparty/Eigen/doc/Manual.dox b/gtsam/3rdparty/Eigen/doc/Manual.dox index 3367982ca..55057d213 100644 --- a/gtsam/3rdparty/Eigen/doc/Manual.dox +++ b/gtsam/3rdparty/Eigen/doc/Manual.dox @@ -11,6 +11,7 @@ namespace Eigen { - \subpage TopicCustomizingEigen - \subpage TopicMultiThreading - \subpage TopicUsingIntelMKL + - \subpage TopicPitfalls - \subpage TopicTemplateKeyword - \subpage UserManual_UnderstandingEigen */ diff --git a/gtsam/3rdparty/Eigen/doc/Pitfalls.dox b/gtsam/3rdparty/Eigen/doc/Pitfalls.dox new file mode 100644 index 000000000..cf42effef --- /dev/null +++ b/gtsam/3rdparty/Eigen/doc/Pitfalls.dox @@ -0,0 +1,38 @@ +namespace Eigen { + +/** \page TopicPitfalls Common pitfalls + +\section TopicPitfalls_template_keyword Compilation error with template methods + +See this \link TopicTemplateKeyword page \endlink. + +\section TopicPitfalls_auto_keyword C++11 and the auto keyword + +In short: do not use the auto keywords with Eigen's expressions, unless you are 100% sure about what you are doing. In particular, do not use the auto keyword as a replacement for a Matrix<> type. Here is an example: + +\code +MatrixXd A, B; +auto C = A*B; +for(...) { ... w = C * v; ...} +\endcode + +In this example, the type of C is not a MatrixXd but an abstract expression representing a matrix product and storing references to A and B. Therefore, the product of A*B will be carried out multiple times, once per iteration of the for loop. Moreover, if the coefficients of A or B change during the iteration, then C will evaluate to different values. + +Here is another example leading to a segfault: +\code +auto C = ((A+B).eval()).transpose(); +// do something with C +\endcode +The problem is that eval() returns a temporary object (in this case a MatrixXd) which is then referenced by the Transpose<> expression. However, this temporary is deleted right after the first line, and there the C expression reference a dead object. The same issue might occur when sub expressions are automatically evaluated by Eigen as in the following example: +\code +VectorXd u, v; +auto C = u + (A*v).normalized(); +// do something with C +\endcode +where the normalized() method has to evaluate the expensive product A*v to avoid evaluating it twice. On the other hand, the following example is perfectly fine: +\code +auto C = (u + (A*v).normalized()).eval(); +\endcode +In this case, C will be a regular VectorXd object. +*/ +} 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/TopicMultithreading.dox b/gtsam/3rdparty/Eigen/doc/TopicMultithreading.dox index f7d082668..7db2b0e07 100644 --- a/gtsam/3rdparty/Eigen/doc/TopicMultithreading.dox +++ b/gtsam/3rdparty/Eigen/doc/TopicMultithreading.dox @@ -17,7 +17,7 @@ You can control the number of thread that will be used using either the OpenMP A Unless setNbThreads has been called, Eigen uses the number of threads specified by OpenMP. You can restore this bahavior by calling \code setNbThreads(0); \endcode You can query the number of threads that will be used with: \code -n = Eigen::nbThreads(n); +n = Eigen::nbThreads( ); \endcode You can disable Eigen's multi threading at compile time by defining the EIGEN_DONT_PARALLELIZE preprocessor token. diff --git a/gtsam/3rdparty/Eigen/doc/UsingIntelMKL.dox b/gtsam/3rdparty/Eigen/doc/UsingIntelMKL.dox index 4b624a156..84db992b6 100644 --- a/gtsam/3rdparty/Eigen/doc/UsingIntelMKL.dox +++ b/gtsam/3rdparty/Eigen/doc/UsingIntelMKL.dox @@ -40,7 +40,8 @@ Since Eigen version 3.1 and later, users can benefit from built-in Intel MKL opt Intel MKL provides highly optimized multi-threaded mathematical routines for x86-compatible architectures. Intel MKL is available on Linux, Mac and Windows for both Intel64 and IA32 architectures. -\warning Be aware that Intel® MKL is a proprietary software. It is the responsibility of the users to buy MKL licenses for their products. Moreover, the license of the user product has to allow linking to proprietary software that excludes any unmodified versions of the GPL. +\note +Intel® MKL is a proprietary software and it is the responsibility of users to buy or register for community (free) Intel MKL licenses for their products. Moreover, the license of the user product has to allow linking to proprietary software that excludes any unmodified versions of the GPL. Using Intel MKL through Eigen is easy: -# define the \c EIGEN_USE_MKL_ALL macro before including any Eigen's header 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..3ab75dbfe 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,17 @@ 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 + TARGET Tutorial_sparse_example + POST_BUILD + COMMAND ${CMAKE_COMMAND} -E make_directory ${CMAKE_CURRENT_BINARY_DIR}/../html/ + 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..cadc6a255 100644 --- a/gtsam/3rdparty/Eigen/failtest/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/failtest/CMakeLists.txt @@ -26,6 +26,23 @@ 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") + +ei_add_failtest("partialpivlu_int") +ei_add_failtest("fullpivlu_int") +ei_add_failtest("llt_int") +ei_add_failtest("ldlt_int") +ei_add_failtest("qr_int") +ei_add_failtest("colpivqr_int") +ei_add_failtest("fullpivqr_int") +ei_add_failtest("jacobisvd_int") +ei_add_failtest("eigensolver_int") +ei_add_failtest("eigensolver_cplx") + 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/failtest/colpivqr_int.cpp b/gtsam/3rdparty/Eigen/failtest/colpivqr_int.cpp new file mode 100644 index 000000000..db11910d4 --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/colpivqr_int.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/QR" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define SCALAR int +#else +#define SCALAR float +#endif + +using namespace Eigen; + +int main() +{ + ColPivHouseholderQR > qr(Matrix::Random(10,10)); +} diff --git a/gtsam/3rdparty/Eigen/failtest/eigensolver_cplx.cpp b/gtsam/3rdparty/Eigen/failtest/eigensolver_cplx.cpp new file mode 100644 index 000000000..c2e21e189 --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/eigensolver_cplx.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/Eigenvalues" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define SCALAR std::complex +#else +#define SCALAR float +#endif + +using namespace Eigen; + +int main() +{ + EigenSolver > eig(Matrix::Random(10,10)); +} diff --git a/gtsam/3rdparty/Eigen/failtest/eigensolver_int.cpp b/gtsam/3rdparty/Eigen/failtest/eigensolver_int.cpp new file mode 100644 index 000000000..eda8dc20b --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/eigensolver_int.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/Eigenvalues" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define SCALAR int +#else +#define SCALAR float +#endif + +using namespace Eigen; + +int main() +{ + EigenSolver > eig(Matrix::Random(10,10)); +} diff --git a/gtsam/3rdparty/Eigen/failtest/fullpivlu_int.cpp b/gtsam/3rdparty/Eigen/failtest/fullpivlu_int.cpp new file mode 100644 index 000000000..e9d2c6eb3 --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/fullpivlu_int.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/LU" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define SCALAR int +#else +#define SCALAR float +#endif + +using namespace Eigen; + +int main() +{ + FullPivLU > lu(Matrix::Random(10,10)); +} diff --git a/gtsam/3rdparty/Eigen/failtest/fullpivqr_int.cpp b/gtsam/3rdparty/Eigen/failtest/fullpivqr_int.cpp new file mode 100644 index 000000000..d182a7b6b --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/fullpivqr_int.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/QR" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define SCALAR int +#else +#define SCALAR float +#endif + +using namespace Eigen; + +int main() +{ + FullPivHouseholderQR > qr(Matrix::Random(10,10)); +} diff --git a/gtsam/3rdparty/Eigen/failtest/jacobisvd_int.cpp b/gtsam/3rdparty/Eigen/failtest/jacobisvd_int.cpp new file mode 100644 index 000000000..12790aef1 --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/jacobisvd_int.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/SVD" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define SCALAR int +#else +#define SCALAR float +#endif + +using namespace Eigen; + +int main() +{ + JacobiSVD > qr(Matrix::Random(10,10)); +} diff --git a/gtsam/3rdparty/Eigen/failtest/ldlt_int.cpp b/gtsam/3rdparty/Eigen/failtest/ldlt_int.cpp new file mode 100644 index 000000000..243e45746 --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/ldlt_int.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/Cholesky" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define SCALAR int +#else +#define SCALAR float +#endif + +using namespace Eigen; + +int main() +{ + LDLT > ldlt(Matrix::Random(10,10)); +} diff --git a/gtsam/3rdparty/Eigen/failtest/llt_int.cpp b/gtsam/3rdparty/Eigen/failtest/llt_int.cpp new file mode 100644 index 000000000..cb020650d --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/llt_int.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/Cholesky" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define SCALAR int +#else +#define SCALAR float +#endif + +using namespace Eigen; + +int main() +{ + LLT > llt(Matrix::Random(10,10)); +} diff --git a/gtsam/3rdparty/Eigen/failtest/partialpivlu_int.cpp b/gtsam/3rdparty/Eigen/failtest/partialpivlu_int.cpp new file mode 100644 index 000000000..98ef282ea --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/partialpivlu_int.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/LU" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define SCALAR int +#else +#define SCALAR float +#endif + +using namespace Eigen; + +int main() +{ + PartialPivLU > lu(Matrix::Random(10,10)); +} diff --git a/gtsam/3rdparty/Eigen/failtest/qr_int.cpp b/gtsam/3rdparty/Eigen/failtest/qr_int.cpp new file mode 100644 index 000000000..ce200e818 --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/qr_int.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/QR" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define SCALAR int +#else +#define SCALAR float +#endif + +using namespace Eigen; + +int main() +{ + HouseholderQR > qr(Matrix::Random(10,10)); +} diff --git a/gtsam/3rdparty/Eigen/failtest/ref_1.cpp b/gtsam/3rdparty/Eigen/failtest/ref_1.cpp new file mode 100644 index 000000000..8b798d53d --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/ref_1.cpp @@ -0,0 +1,18 @@ +#include "../Eigen/Core" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define CV_QUALIFIER const +#else +#define CV_QUALIFIER +#endif + +using namespace Eigen; + +void call_ref(Ref a) { } + +int main() +{ + VectorXf a(10); + CV_QUALIFIER VectorXf& ac(a); + call_ref(ac); +} diff --git a/gtsam/3rdparty/Eigen/failtest/ref_2.cpp b/gtsam/3rdparty/Eigen/failtest/ref_2.cpp new file mode 100644 index 000000000..0b779ccf5 --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/ref_2.cpp @@ -0,0 +1,15 @@ +#include "../Eigen/Core" + +using namespace Eigen; + +void call_ref(Ref a) { } + +int main() +{ + MatrixXf A(10,10); +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD + call_ref(A.row(3)); +#else + call_ref(A.col(3)); +#endif +} diff --git a/gtsam/3rdparty/Eigen/failtest/ref_3.cpp b/gtsam/3rdparty/Eigen/failtest/ref_3.cpp new file mode 100644 index 000000000..f46027d48 --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/ref_3.cpp @@ -0,0 +1,15 @@ +#include "../Eigen/Core" + +using namespace Eigen; + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +void call_ref(Ref a) { } +#else +void call_ref(const Ref &a) { } +#endif + +int main() +{ + VectorXf a(10); + call_ref(a+a); +} diff --git a/gtsam/3rdparty/Eigen/failtest/ref_4.cpp b/gtsam/3rdparty/Eigen/failtest/ref_4.cpp new file mode 100644 index 000000000..6c11fa4cb --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/ref_4.cpp @@ -0,0 +1,15 @@ +#include "../Eigen/Core" + +using namespace Eigen; + +void call_ref(Ref > a) {} + +int main() +{ + MatrixXf A(10,10); +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD + call_ref(A.transpose()); +#else + call_ref(A); +#endif +} diff --git a/gtsam/3rdparty/Eigen/failtest/ref_5.cpp b/gtsam/3rdparty/Eigen/failtest/ref_5.cpp new file mode 100644 index 000000000..846d52795 --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/ref_5.cpp @@ -0,0 +1,16 @@ +#include "../Eigen/Core" + +using namespace Eigen; + +void call_ref(Ref a) { } + +int main() +{ + VectorXf a(10); + DenseBase &ac(a); +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD + call_ref(ac); +#else + call_ref(ac.derived()); +#endif +} diff --git a/gtsam/3rdparty/Eigen/test/CMakeLists.txt b/gtsam/3rdparty/Eigen/test/CMakeLists.txt index ccb0fc798..3739268d2 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}) @@ -222,6 +222,8 @@ ei_add_test(sizeoverflow) ei_add_test(prec_inverse_4x4) ei_add_test(vectorwiseop) ei_add_test(special_numbers) +ei_add_test(rvalue_types) +ei_add_test(mpl2only) ei_add_test(simplicial_cholesky) ei_add_test(conjugate_gradient) @@ -279,6 +281,13 @@ 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() + + +option(EIGEN_TEST_BUILD_DOCUMENTATION "Test building the doxygen documentation" OFF) +IF(EIGEN_TEST_BUILD_DOCUMENTATION) + add_dependencies(buildtests doc) +ENDIF() diff --git a/gtsam/3rdparty/Eigen/test/array.cpp b/gtsam/3rdparty/Eigen/test/array.cpp index c607da631..68f6b3d7a 100644 --- a/gtsam/3rdparty/Eigen/test/array.cpp +++ b/gtsam/3rdparty/Eigen/test/array.cpp @@ -109,6 +109,8 @@ template void comparisons(const ArrayType& m) VERIFY(! (m1 < m3).all() ); VERIFY(! (m1 > m3).all() ); } + VERIFY(!(m1 > m2 && m1 < m2).any()); + VERIFY((m1 <= m2 || m1 >= m2).all()); // comparisons to scalar VERIFY( (m1 != (m1(r,c)+1) ).any() ); diff --git a/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp b/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp index 9a50f99ab..9667e1f14 100644 --- a/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp +++ b/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp @@ -102,6 +102,7 @@ template void comparisons(const MatrixType& m) VERIFY( (m1.array() > (m1(r,c)-1) ).any() ); VERIFY( (m1.array() < (m1(r,c)+1) ).any() ); VERIFY( (m1.array() == m1(r,c) ).any() ); + VERIFY( m1.cwiseEqual(m1(r,c)).any() ); // test Select VERIFY_IS_APPROX( (m1.array() 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/conjugate_gradient.cpp b/gtsam/3rdparty/Eigen/test/conjugate_gradient.cpp index 869051b31..019cc4d64 100644 --- a/gtsam/3rdparty/Eigen/test/conjugate_gradient.cpp +++ b/gtsam/3rdparty/Eigen/test/conjugate_gradient.cpp @@ -12,13 +12,15 @@ template void test_conjugate_gradient_T() { - ConjugateGradient, Lower> cg_colmajor_lower_diag; - ConjugateGradient, Upper> cg_colmajor_upper_diag; + ConjugateGradient, Lower > cg_colmajor_lower_diag; + ConjugateGradient, Upper > cg_colmajor_upper_diag; + ConjugateGradient, Lower|Upper> cg_colmajor_loup_diag; ConjugateGradient, Lower, IdentityPreconditioner> cg_colmajor_lower_I; ConjugateGradient, Upper, IdentityPreconditioner> cg_colmajor_upper_I; CALL_SUBTEST( check_sparse_spd_solving(cg_colmajor_lower_diag) ); CALL_SUBTEST( check_sparse_spd_solving(cg_colmajor_upper_diag) ); + CALL_SUBTEST( check_sparse_spd_solving(cg_colmajor_loup_diag) ); CALL_SUBTEST( check_sparse_spd_solving(cg_colmajor_lower_I) ); CALL_SUBTEST( check_sparse_spd_solving(cg_colmajor_upper_I) ); } 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_alignedbox.cpp b/gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp index 8e36adbe3..84663ad1f 100644 --- a/gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp @@ -172,6 +172,8 @@ void test_geo_alignedbox() CALL_SUBTEST_9( alignedbox(AlignedBox1i()) ); CALL_SUBTEST_10( alignedbox(AlignedBox2i()) ); CALL_SUBTEST_11( alignedbox(AlignedBox3i()) ); + + CALL_SUBTEST_14( alignedbox(AlignedBox(4)) ); } CALL_SUBTEST_12( specificTest1() ); CALL_SUBTEST_13( specificTest2() ); 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/lu.cpp b/gtsam/3rdparty/Eigen/test/lu.cpp index 25f86755a..374652694 100644 --- a/gtsam/3rdparty/Eigen/test/lu.cpp +++ b/gtsam/3rdparty/Eigen/test/lu.cpp @@ -100,7 +100,9 @@ template void lu_invertible() LU.h */ typedef typename NumTraits::Real RealScalar; - int size = internal::random(1,EIGEN_TEST_MAX_SIZE); + DenseIndex size = MatrixType::RowsAtCompileTime; + if( size==Dynamic) + size = internal::random(1,EIGEN_TEST_MAX_SIZE); MatrixType m1(size, size), m2(size, size), m3(size, size); FullPivLU lu; @@ -122,6 +124,10 @@ template void lu_invertible() m2 = lu.solve(m3); VERIFY_IS_APPROX(m3, m1*m2); VERIFY_IS_APPROX(m2, lu.inverse()*m3); + + // Regression test for Bug 302 + MatrixType m4 = MatrixType::Random(size,size); + VERIFY_IS_APPROX(lu.solve(m3*m4), lu.solve(m3)*m4); } template void lu_partial_piv() @@ -171,6 +177,7 @@ void test_lu() { for(int i = 0; i < g_repeat; i++) { CALL_SUBTEST_1( lu_non_invertible() ); + CALL_SUBTEST_1( lu_invertible() ); CALL_SUBTEST_1( lu_verify_assert() ); CALL_SUBTEST_2( (lu_non_invertible >()) ); 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/mapped_matrix.cpp b/gtsam/3rdparty/Eigen/test/mapped_matrix.cpp index de9dbbde3..58904fa37 100644 --- a/gtsam/3rdparty/Eigen/test/mapped_matrix.cpp +++ b/gtsam/3rdparty/Eigen/test/mapped_matrix.cpp @@ -114,6 +114,28 @@ template void check_const_correctness(const PlainObjec VERIFY( !(Map::Flags & LvalueBit) ); } +template +void map_not_aligned_on_scalar() +{ + typedef Matrix MatrixType; + typedef typename MatrixType::Index Index; + Index size = 11; + Scalar* array1 = internal::aligned_new((size+1)*(size+1)+1); + Scalar* array2 = reinterpret_cast(sizeof(Scalar)/2+std::size_t(array1)); + Map > map2(array2, size, size, OuterStride<>(size+1)); + MatrixType m2 = MatrixType::Random(size,size); + map2 = m2; + VERIFY_IS_EQUAL(m2, map2); + + typedef Matrix VectorType; + Map map3(array2, size); + MatrixType v3 = VectorType::Random(size); + map3 = v3; + VERIFY_IS_EQUAL(v3, map3); + + internal::aligned_delete(array1, (size+1)*(size+1)+1); +} + void test_mapped_matrix() { for(int i = 0; i < g_repeat; i++) { @@ -137,5 +159,7 @@ void test_mapped_matrix() CALL_SUBTEST_8( map_static_methods(RowVector3d()) ); CALL_SUBTEST_9( map_static_methods(VectorXcd(8)) ); CALL_SUBTEST_10( map_static_methods(VectorXf(12)) ); + + CALL_SUBTEST_11( map_not_aligned_on_scalar() ); } } 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/product_extra.cpp b/gtsam/3rdparty/Eigen/test/product_extra.cpp index 744a1ef7f..ea2486937 100644 --- a/gtsam/3rdparty/Eigen/test/product_extra.cpp +++ b/gtsam/3rdparty/Eigen/test/product_extra.cpp @@ -109,8 +109,67 @@ void mat_mat_scalar_scalar_product() double det = 6.0, wt = 0.5; VERIFY_IS_APPROX(dNdxy.transpose()*dNdxy*det*wt, det*wt*dNdxy.transpose()*dNdxy); } + +template +void zero_sized_objects(const MatrixType& m) +{ + typedef typename MatrixType::Scalar Scalar; + const int PacketSize = internal::packet_traits::size; + const int PacketSize1 = PacketSize>1 ? PacketSize-1 : 1; + DenseIndex rows = m.rows(); + DenseIndex cols = m.cols(); -void zero_sized_objects() + { + MatrixType res, a(rows,0), b(0,cols); + VERIFY_IS_APPROX( (res=a*b), MatrixType::Zero(rows,cols) ); + VERIFY_IS_APPROX( (res=a*a.transpose()), MatrixType::Zero(rows,rows) ); + VERIFY_IS_APPROX( (res=b.transpose()*b), MatrixType::Zero(cols,cols) ); + VERIFY_IS_APPROX( (res=b.transpose()*a.transpose()), MatrixType::Zero(cols,rows) ); + } + + { + MatrixType res, a(rows,cols), b(cols,0); + res = a*b; + VERIFY(res.rows()==rows && res.cols()==0); + b.resize(0,rows); + res = b*a; + VERIFY(res.rows()==0 && res.cols()==cols); + } + + { + Matrix a; + Matrix b; + Matrix res; + VERIFY_IS_APPROX( (res=a*b), MatrixType::Zero(PacketSize,1) ); + VERIFY_IS_APPROX( (res=a.lazyProduct(b)), MatrixType::Zero(PacketSize,1) ); + } + + { + Matrix a; + Matrix b; + Matrix res; + VERIFY_IS_APPROX( (res=a*b), MatrixType::Zero(PacketSize1,1) ); + VERIFY_IS_APPROX( (res=a.lazyProduct(b)), MatrixType::Zero(PacketSize1,1) ); + } + + { + Matrix a(PacketSize,0); + Matrix b(0,1); + Matrix res; + VERIFY_IS_APPROX( (res=a*b), MatrixType::Zero(PacketSize,1) ); + VERIFY_IS_APPROX( (res=a.lazyProduct(b)), MatrixType::Zero(PacketSize,1) ); + } + + { + Matrix a(PacketSize1,0); + Matrix b(0,1); + Matrix res; + VERIFY_IS_APPROX( (res=a*b), MatrixType::Zero(PacketSize1,1) ); + VERIFY_IS_APPROX( (res=a.lazyProduct(b)), MatrixType::Zero(PacketSize1,1) ); + } +} + +void bug_127() { // Bug 127 // @@ -171,7 +230,8 @@ void test_product_extra() CALL_SUBTEST_2( mat_mat_scalar_scalar_product() ); CALL_SUBTEST_3( product_extra(MatrixXcf(internal::random(1,EIGEN_TEST_MAX_SIZE/2), internal::random(1,EIGEN_TEST_MAX_SIZE/2))) ); CALL_SUBTEST_4( product_extra(MatrixXcd(internal::random(1,EIGEN_TEST_MAX_SIZE/2), internal::random(1,EIGEN_TEST_MAX_SIZE/2))) ); + CALL_SUBTEST_1( zero_sized_objects(MatrixXf(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); } - CALL_SUBTEST_5( zero_sized_objects() ); + CALL_SUBTEST_5( bug_127() ); CALL_SUBTEST_6( unaligned_objects() ); } diff --git a/gtsam/3rdparty/Eigen/test/product_mmtr.cpp b/gtsam/3rdparty/Eigen/test/product_mmtr.cpp index 7d6746800..aeba009f4 100644 --- a/gtsam/3rdparty/Eigen/test/product_mmtr.cpp +++ b/gtsam/3rdparty/Eigen/test/product_mmtr.cpp @@ -24,8 +24,8 @@ template void mmtr(int size) DenseIndex othersize = internal::random(1,200); - MatrixColMaj matc(size, size); - MatrixRowMaj matr(size, size); + MatrixColMaj matc = MatrixColMaj::Zero(size, size); + MatrixRowMaj matr = MatrixRowMaj::Zero(size, size); MatrixColMaj ref1(size, size), ref2(size, size); MatrixColMaj soc(size,othersize); soc.setRandom(); diff --git a/gtsam/3rdparty/Eigen/test/real_qz.cpp b/gtsam/3rdparty/Eigen/test/real_qz.cpp index 7d743a734..a1766c6d9 100644 --- a/gtsam/3rdparty/Eigen/test/real_qz.cpp +++ b/gtsam/3rdparty/Eigen/test/real_qz.cpp @@ -25,6 +25,22 @@ template void real_qz(const MatrixType& m) MatrixType A = MatrixType::Random(dim,dim), B = MatrixType::Random(dim,dim); + + // Regression test for bug 985: Randomly set rows or columns to zero + Index k=internal::random(0, dim-1); + switch(internal::random(0,10)) { + case 0: + A.row(k).setZero(); break; + case 1: + A.col(k).setZero(); break; + case 2: + B.row(k).setZero(); break; + case 3: + B.col(k).setZero(); break; + default: + break; + } + RealQZ qz(A,B); VERIFY_IS_EQUAL(qz.info(), Success); diff --git a/gtsam/3rdparty/Eigen/test/redux.cpp b/gtsam/3rdparty/Eigen/test/redux.cpp index 0d176e500..50b473838 100644 --- a/gtsam/3rdparty/Eigen/test/redux.cpp +++ b/gtsam/3rdparty/Eigen/test/redux.cpp @@ -53,6 +53,14 @@ template void matrixRedux(const MatrixType& m) VERIFY_IS_APPROX(m1_for_prod.block(r0,c0,r1,c1).prod(), m1_for_prod.block(r0,c0,r1,c1).eval().prod()); VERIFY_IS_APPROX(m1.block(r0,c0,r1,c1).real().minCoeff(), m1.block(r0,c0,r1,c1).real().eval().minCoeff()); VERIFY_IS_APPROX(m1.block(r0,c0,r1,c1).real().maxCoeff(), m1.block(r0,c0,r1,c1).real().eval().maxCoeff()); + + // regression for bug 1090 + const int R1 = MatrixType::RowsAtCompileTime>=2 ? MatrixType::RowsAtCompileTime/2 : 6; + const int C1 = MatrixType::ColsAtCompileTime>=2 ? MatrixType::ColsAtCompileTime/2 : 6; + if(R1<=rows-r0 && C1<=cols-c0) + { + VERIFY_IS_APPROX( (m1.template block(r0,c0).sum()), m1.block(r0,c0,R1,C1).sum() ); + } // test empty objects VERIFY_IS_APPROX(m1.block(r0,c0,0,0).sum(), Scalar(0)); diff --git a/gtsam/3rdparty/Eigen/test/ref.cpp b/gtsam/3rdparty/Eigen/test/ref.cpp index 19e81549c..44bbd3bf1 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 RowMatrixXd; +int test_ref_overload_fun1(Ref ) { return 1; } +int test_ref_overload_fun1(Ref ) { return 2; } +int test_ref_overload_fun1(Ref ) { return 3; } + +int test_ref_overload_fun2(Ref ) { return 4; } +int test_ref_overload_fun2(Ref ) { return 5; } + +// See also bug 969 +void test_ref_overloads() +{ + MatrixXd Ad, Bd; + RowMatrixXd rAd, rBd; + VERIFY( test_ref_overload_fun1(Ad)==1 ); + VERIFY( test_ref_overload_fun1(rAd)==2 ); + + MatrixXf Af, Bf; + VERIFY( test_ref_overload_fun2(Ad)==4 ); + VERIFY( test_ref_overload_fun2(Ad+Bd)==4 ); + VERIFY( test_ref_overload_fun2(Af+Bf)==5 ); +} + void test_ref() { for(int i = 0; i < g_repeat; i++) { @@ -249,4 +271,6 @@ void test_ref() CALL_SUBTEST_5( ref_matrix(MatrixXi(internal::random(1,10),internal::random(1,10))) ); CALL_SUBTEST_6( call_ref() ); } + + CALL_SUBTEST_7( test_ref_overloads() ); } diff --git a/gtsam/3rdparty/Eigen/test/sparse_basic.cpp b/gtsam/3rdparty/Eigen/test/sparse_basic.cpp index 498ecfe29..abe6a9d14 100644 --- a/gtsam/3rdparty/Eigen/test/sparse_basic.cpp +++ b/gtsam/3rdparty/Eigen/test/sparse_basic.cpp @@ -24,6 +24,7 @@ template void sparse_basic(const SparseMatrixType& re double density = (std::max)(8./(rows*cols), 0.01); typedef Matrix DenseMatrix; typedef Matrix DenseVector; + typedef Matrix RowDenseVector; Scalar eps = 1e-6; Scalar s1 = internal::random(); @@ -52,7 +53,7 @@ template void sparse_basic(const SparseMatrixType& re refMat.coeffRef(nonzeroCoords[0].x(), nonzeroCoords[0].y()) = Scalar(5); VERIFY_IS_APPROX(m, refMat); - /* + // test InnerIterators and Block expressions for (int t=0; t<10; ++t) { @@ -61,23 +62,54 @@ template void sparse_basic(const SparseMatrixType& re int w = internal::random(1,cols-j-1); int h = internal::random(1,rows-i-1); - // VERIFY_IS_APPROX(m.block(i,j,h,w), refMat.block(i,j,h,w)); + VERIFY_IS_APPROX(m.block(i,j,h,w), refMat.block(i,j,h,w)); for(int c=0; c void sparse_basic(const SparseMatrixType& re VERIFY_IS_APPROX(m.row(r) + m.row(r), (m + m).row(r)); VERIFY_IS_APPROX(m.row(r) + m.row(r), refMat.row(r) + refMat.row(r)); } - */ + // test assertion VERIFY_RAISES_ASSERT( m.coeffRef(-1,1) = 0 ); @@ -274,6 +306,8 @@ template void sparse_basic(const SparseMatrixType& re refM4.setRandom(); // sparse cwise* dense VERIFY_IS_APPROX(m3.cwiseProduct(refM4), refM3.cwiseProduct(refM4)); + // dense cwise* sparse + VERIFY_IS_APPROX(refM4.cwiseProduct(m3), refM4.cwiseProduct(refM3)); // VERIFY_IS_APPROX(m3.cwise()/refM4, refM3.cwise()/refM4); // test aliasing @@ -326,6 +360,15 @@ template void sparse_basic(const SparseMatrixType& re refMat2.col(i) = refMat2.col(i) * s1; VERIFY_IS_APPROX(m2,refMat2); } + + VERIFY_IS_APPROX(DenseVector(m2.col(j0)), refMat2.col(j0)); + VERIFY_IS_APPROX(m2.col(j0), refMat2.col(j0)); + + VERIFY_IS_APPROX(RowDenseVector(m2.row(j0)), refMat2.row(j0)); + VERIFY_IS_APPROX(m2.row(j0), refMat2.row(j0)); + + VERIFY_IS_APPROX(m2.block(j0,j1,n0,n0), refMat2.block(j0,j1,n0,n0)); + VERIFY_IS_APPROX((2*m2).block(j0,j1,n0,n0), (2*refMat2).block(j0,j1,n0,n0)); } // test prune @@ -488,6 +531,20 @@ template void sparse_basic(const SparseMatrixType& re SparseMatrixType m1(rows, rows); m1.setIdentity(); VERIFY_IS_APPROX(m1, refMat1); + for(int k=0; k(0,rows-1); + Index j = internal::random(0,rows-1); + Scalar v = internal::random(); + m1.coeffRef(i,j) = v; + refMat1.coeffRef(i,j) = v; + VERIFY_IS_APPROX(m1, refMat1); + if(internal::random(0,10)<2) + m1.makeCompressed(); + } + m1.setIdentity(); + refMat1.setIdentity(); + VERIFY_IS_APPROX(m1, refMat1); } } diff --git a/gtsam/3rdparty/Eigen/test/sparse_solver.h b/gtsam/3rdparty/Eigen/test/sparse_solver.h index d84aff070..e1619d62a 100644 --- a/gtsam/3rdparty/Eigen/test/sparse_solver.h +++ b/gtsam/3rdparty/Eigen/test/sparse_solver.h @@ -67,6 +67,22 @@ void check_sparse_solving(Solver& solver, const typename Solver::MatrixType& A, VERIFY(oldb.isApprox(db) && "sparse solver testing: the rhs should not be modified!"); VERIFY(x.isApprox(refX,test_precision())); } + + // if not too large, do some extra check: + if(A.rows()<2000) + { + + // test expression as input + { + solver.compute(0.5*(A+A)); + Rhs x = solver.solve(b); + VERIFY(x.isApprox(refX,test_precision())); + + Solver solver2(0.5*(A+A)); + Rhs x2 = solver2.solve(b); + VERIFY(x2.isApprox(refX,test_precision())); + } + } } template @@ -124,7 +140,23 @@ void check_sparse_determinant(Solver& solver, const typename Solver::MatrixType& Scalar refDet = dA.determinant(); VERIFY_IS_APPROX(refDet,solver.determinant()); } +template +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) @@ -145,7 +177,10 @@ int generate_sparse_spd_problem(Solver& , typename Solver::MatrixType& A, typena dA = dM * dM.adjoint(); halfA.resize(size,size); - halfA.template selfadjointView().rankUpdate(M); + if(Solver::UpLo==(Lower|Upper)) + halfA = A; + else + halfA.template selfadjointView().rankUpdate(M); return size; } @@ -258,7 +293,17 @@ int generate_sparse_square_problem(Solver&, typename Solver::MatrixType& A, Dens return size; } -template void check_sparse_square_solving(Solver& solver) + +struct prune_column { + int m_col; + prune_column(int col) : m_col(col) {} + template + bool operator()(int, int col, const Scalar&) const { + return col != m_col; + } +}; + +template void check_sparse_square_solving(Solver& solver, bool checkDeficient = false) { typedef typename Solver::MatrixType Mat; typedef typename Mat::Scalar Scalar; @@ -290,6 +335,13 @@ template void check_sparse_square_solving(Solver& solver) b = DenseVector::Zero(size); check_sparse_solving(solver, A, b, dA, b); } + // regression test for Bug 792 (structurally rank deficient matrices): + if(checkDeficient && size>1) { + int col = internal::random(0,size-1); + A.prune(prune_column(col)); + solver.compute(A); + VERIFY_IS_EQUAL(solver.info(), NumericalIssue); + } } // First, get the folder @@ -324,3 +376,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..b3d67aea8 100644 --- a/gtsam/3rdparty/Eigen/test/sparselu.cpp +++ b/gtsam/3rdparty/Eigen/test/sparselu.cpp @@ -41,9 +41,15 @@ template void test_sparselu_T() SparseLU, AMDOrdering > sparselu_amd; SparseLU, NaturalOrdering > sparselu_natural; - check_sparse_square_solving(sparselu_colamd); - check_sparse_square_solving(sparselu_amd); - check_sparse_square_solving(sparselu_natural); + check_sparse_square_solving(sparselu_colamd, true); + check_sparse_square_solving(sparselu_amd, true); + check_sparse_square_solving(sparselu_natural,true); + + check_sparse_square_abs_determinant(sparselu_colamd); + check_sparse_square_abs_determinant(sparselu_amd); + + check_sparse_square_determinant(sparselu_colamd); + check_sparse_square_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/test/vectorwiseop.cpp b/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp index 6cd1acdda..d32fd10cc 100644 --- a/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp +++ b/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp @@ -111,6 +111,18 @@ template void vectorwiseop_array(const ArrayType& m) m2.rowwise() /= m2.colwise().sum(); VERIFY_IS_APPROX(m2, m1.rowwise() / m1.colwise().sum()); } + + // all/any + Array mb(rows,cols); + mb = (m1.real()<=0.7).colwise().all(); + VERIFY( (mb.col(c) == (m1.real().col(c)<=0.7).all()).all() ); + mb = (m1.real()<=0.7).rowwise().all(); + VERIFY( (mb.row(r) == (m1.real().row(r)<=0.7).all()).all() ); + + mb = (m1.real()>=0.7).colwise().any(); + VERIFY( (mb.col(c) == (m1.real().col(c)>=0.7).any()).all() ); + mb = (m1.real()>=0.7).rowwise().any(); + VERIFY( (mb.row(r) == (m1.real().row(r)>=0.7).any()).all() ); } template void vectorwiseop_matrix(const MatrixType& m) diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CMakeLists.txt b/gtsam/3rdparty/Eigen/unsupported/Eigen/CMakeLists.txt index e06f1238b..e1fbf97e2 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CMakeLists.txt @@ -1,6 +1,6 @@ -set(Eigen_HEADERS AdolcForward BVH IterativeSolvers MatrixFunctions MoreVectorization AutoDiff AlignedVector3 Polynomials - FFT NonLinearOptimization SparseExtra IterativeSolvers - NumericalDiff Skyline MPRealSupport OpenGLSupport KroneckerProduct Splines LevenbergMarquardt +set(Eigen_HEADERS AdolcForward AlignedVector3 ArpackSupport AutoDiff BVH FFT IterativeSolvers KroneckerProduct LevenbergMarquardt + MatrixFunctions MoreVectorization MPRealSupport NonLinearOptimization NumericalDiff OpenGLSupport Polynomials + Skyline SparseExtra Splines ) install(FILES 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/AutoDiff/AutoDiffScalar.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h index 8d42e69b9..fde3ff61a 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h @@ -631,7 +631,7 @@ template struct NumTraits > { typedef AutoDiffScalar::Real,DerType::RowsAtCompileTime,DerType::ColsAtCompileTime> > Real; typedef AutoDiffScalar NonInteger; - typedef AutoDiffScalar& Nested; + typedef AutoDiffScalar Nested; enum{ RequireInitialization = 1 }; diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/CMakeLists.txt b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/CMakeLists.txt index f3180b52b..d8b9f4068 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/CMakeLists.txt @@ -1,7 +1,10 @@ ADD_SUBDIRECTORY(AutoDiff) ADD_SUBDIRECTORY(BVH) +ADD_SUBDIRECTORY(Eigenvalues) ADD_SUBDIRECTORY(FFT) ADD_SUBDIRECTORY(IterativeSolvers) +ADD_SUBDIRECTORY(KroneckerProduct) +ADD_SUBDIRECTORY(LevenbergMarquardt) ADD_SUBDIRECTORY(MatrixFunctions) ADD_SUBDIRECTORY(MoreVectorization) ADD_SUBDIRECTORY(NonLinearOptimization) @@ -9,5 +12,4 @@ ADD_SUBDIRECTORY(NumericalDiff) ADD_SUBDIRECTORY(Polynomials) ADD_SUBDIRECTORY(Skyline) ADD_SUBDIRECTORY(SparseExtra) -ADD_SUBDIRECTORY(KroneckerProduct) ADD_SUBDIRECTORY(Splines) diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/DGMRES.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/DGMRES.h index 9fcc8a8d9..68fc997f7 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/DGMRES.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/DGMRES.h @@ -133,8 +133,8 @@ class DGMRES : public IterativeSolverBase > * this class becomes invalid. Call compute() to update it with the new * matrix A, or modify a copy of A. */ - DGMRES(const MatrixType& A) : Base(A),m_restart(30),m_neig(0),m_r(0),m_maxNeig(5),m_isDeflAllocated(false),m_isDeflInitialized(false) - {} + template + explicit DGMRES(const EigenBase& A) : Base(A.derived()), m_restart(30),m_neig(0),m_r(0),m_maxNeig(5),m_isDeflAllocated(false),m_isDeflInitialized(false) {} ~DGMRES() {} diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h index c8c84069e..ea5deb26d 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h @@ -246,20 +246,7 @@ struct traits > * \endcode * * By default the iterations start with x=0 as an initial guess of the solution. - * One can control the start using the solveWithGuess() method. Here is a step by - * step execution example starting with a random guess and printing the evolution - * of the estimated error: - * * \code - * x = VectorXd::Random(n); - * solver.setMaxIterations(1); - * int i = 0; - * do { - * x = solver.solveWithGuess(b,x); - * std::cout << i << " : " << solver.error() << std::endl; - * ++i; - * } while (solver.info()!=Success && i<100); - * \endcode - * Note that such a step by step excution is slightly slower. + * One can control the start using the solveWithGuess() method. * * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner */ @@ -298,7 +285,8 @@ public: * this class becomes invalid. Call compute() to update it with the new * matrix A, or modify a copy of A. */ - GMRES(const MatrixType& A) : Base(A), m_restart(30) {} + template + explicit GMRES(const EigenBase& A) : Base(A.derived()), m_restart(30) {} ~GMRES() {} diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/MINRES.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/MINRES.h index 0e56342a8..670f274bb 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/MINRES.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/MINRES.h @@ -37,22 +37,31 @@ namespace Eigen { typedef typename Dest::Scalar Scalar; typedef Matrix VectorType; + // Check for zero rhs + const RealScalar rhsNorm2(rhs.squaredNorm()); + if(rhsNorm2 == 0) + { + x.setZero(); + iters = 0; + tol_error = 0; + return; + } + // initialize const int maxIters(iters); // initialize maxIters to iters const int N(mat.cols()); // the size of the matrix - const RealScalar rhsNorm2(rhs.squaredNorm()); const RealScalar threshold2(tol_error*tol_error*rhsNorm2); // convergence threshold (compared to residualNorm2) // Initialize preconditioned Lanczos -// VectorType v_old(N); // will be initialized inside loop + VectorType v_old(N); // will be initialized inside loop VectorType v( VectorType::Zero(N) ); //initialize v VectorType v_new(rhs-mat*x); //initialize v_new RealScalar residualNorm2(v_new.squaredNorm()); -// VectorType w(N); // will be initialized inside loop + VectorType w(N); // will be initialized inside loop VectorType w_new(precond.solve(v_new)); // initialize w_new // RealScalar beta; // will be initialized inside loop RealScalar beta_new2(v_new.dot(w_new)); - eigen_assert(beta_new2 >= 0 && "PRECONDITIONER IS NOT POSITIVE DEFINITE"); + eigen_assert(beta_new2 >= 0.0 && "PRECONDITIONER IS NOT POSITIVE DEFINITE"); RealScalar beta_new(sqrt(beta_new2)); const RealScalar beta_one(beta_new); v_new /= beta_new; @@ -62,14 +71,14 @@ namespace Eigen { RealScalar c_old(1.0); RealScalar s(0.0); // the sine of the Givens rotation RealScalar s_old(0.0); // the sine of the Givens rotation -// VectorType p_oold(N); // will be initialized in loop + VectorType p_oold(N); // will be initialized in loop VectorType p_old(VectorType::Zero(N)); // initialize p_old=0 VectorType p(p_old); // initialize p=0 RealScalar eta(1.0); iters = 0; // reset iters - while ( iters < maxIters ){ - + while ( iters < maxIters ) + { // Preconditioned Lanczos /* Note that there are 4 variants on the Lanczos algorithm. These are * described in Paige, C. C. (1972). Computational variants of @@ -81,17 +90,17 @@ namespace Eigen { * A. Greenbaum, Iterative Methods for Solving Linear Systems, SIAM (1987). */ const RealScalar beta(beta_new); -// v_old = v; // update: at first time step, this makes v_old = 0 so value of beta doesn't matter - const VectorType v_old(v); // NOT SURE IF CREATING v_old EVERY ITERATION IS EFFICIENT + v_old = v; // update: at first time step, this makes v_old = 0 so value of beta doesn't matter +// const VectorType v_old(v); // NOT SURE IF CREATING v_old EVERY ITERATION IS EFFICIENT v = v_new; // update -// w = w_new; // update - const VectorType w(w_new); // NOT SURE IF CREATING w EVERY ITERATION IS EFFICIENT + w = w_new; // update +// const VectorType w(w_new); // NOT SURE IF CREATING w EVERY ITERATION IS EFFICIENT v_new.noalias() = mat*w - beta*v_old; // compute v_new const RealScalar alpha = v_new.dot(w); v_new -= alpha*v; // overwrite v_new w_new = precond.solve(v_new); // overwrite w_new beta_new2 = v_new.dot(w_new); // compute beta_new - eigen_assert(beta_new2 >= 0 && "PRECONDITIONER IS NOT POSITIVE DEFINITE"); + eigen_assert(beta_new2 >= 0.0 && "PRECONDITIONER IS NOT POSITIVE DEFINITE"); beta_new = sqrt(beta_new2); // compute beta_new v_new /= beta_new; // overwrite v_new for next iteration w_new /= beta_new; // overwrite w_new for next iteration @@ -107,21 +116,28 @@ namespace Eigen { s=beta_new/r1; // new sine // Update solution -// p_oold = p_old; - const VectorType p_oold(p_old); // NOT SURE IF CREATING p_oold EVERY ITERATION IS EFFICIENT + p_oold = p_old; +// const VectorType p_oold(p_old); // NOT SURE IF CREATING p_oold EVERY ITERATION IS EFFICIENT p_old = p; p.noalias()=(w-r2*p_old-r3*p_oold) /r1; // IS NOALIAS REQUIRED? x += beta_one*c*eta*p; + + /* Update the squared residual. Note that this is the estimated residual. + The real residual |Ax-b|^2 may be slightly larger */ residualNorm2 *= s*s; - if ( residualNorm2 < threshold2){ + if ( residualNorm2 < threshold2) + { break; } eta=-s*eta; // update eta iters++; // increment iteration number (for output purposes) } - tol_error = std::sqrt(residualNorm2 / rhsNorm2); // return error. Note that this is the estimated error. The real error |Ax-b|/|b| may be slightly larger + + /* Compute error. Note that this is the estimated error. The real + error |Ax-b|/|b| may be slightly larger */ + tol_error = std::sqrt(residualNorm2 / rhsNorm2); } } @@ -174,20 +190,7 @@ namespace Eigen { * \endcode * * By default the iterations start with x=0 as an initial guess of the solution. - * One can control the start using the solveWithGuess() method. Here is a step by - * step execution example starting with a random guess and printing the evolution - * of the estimated error: - * * \code - * x = VectorXd::Random(n); - * mr.setMaxIterations(1); - * int i = 0; - * do { - * x = mr.solveWithGuess(b,x); - * std::cout << i << " : " << mr.error() << std::endl; - * ++i; - * } while (mr.info()!=Success && i<100); - * \endcode - * Note that such a step by step excution is slightly slower. + * One can control the start using the solveWithGuess() method. * * \sa class ConjugateGradient, BiCGSTAB, SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner */ @@ -225,7 +228,8 @@ namespace Eigen { * this class becomes invalid. Call compute() to update it with the new * matrix A, or modify a copy of A. */ - MINRES(const MatrixType& A) : Base(A) {} + template + explicit MINRES(const EigenBase& A) : Base(A.derived()) {} /** Destructor. */ ~MINRES(){} @@ -250,6 +254,11 @@ namespace Eigen { template void _solveWithGuess(const Rhs& b, Dest& x) const { + typedef typename internal::conditional + >::type MatrixWrapperType; + m_iterations = Base::maxIterations(); m_error = Base::m_tolerance; @@ -259,7 +268,7 @@ namespace Eigen { m_error = Base::m_tolerance; typename Dest::ColXpr xj(x,j); - internal::minres(mp_matrix->template selfadjointView(), b.col(j), xj, + internal::minres(MatrixWrapperType(*mp_matrix), b.col(j), xj, Base::m_preconditioner, m_iterations, m_error); } diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/CMakeLists.txt b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/CMakeLists.txt index 8513803ce..d9690854d 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/CMakeLists.txt @@ -2,5 +2,5 @@ FILE(GLOB Eigen_LevenbergMarquardt_SRCS "*.h") INSTALL(FILES ${Eigen_LevenbergMarquardt_SRCS} - DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/LevenbergMarquardt COMPONENT Devel + DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/LevenbergMarquardt COMPONENT Devel ) 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/minres.cpp b/gtsam/3rdparty/Eigen/unsupported/test/minres.cpp index fd12da548..509ebe09a 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/minres.cpp +++ b/gtsam/3rdparty/Eigen/unsupported/test/minres.cpp @@ -14,15 +14,32 @@ template void test_minres_T() { - MINRES, Lower, DiagonalPreconditioner > minres_colmajor_diag; - MINRES, Lower, IdentityPreconditioner > minres_colmajor_I; + MINRES, Lower|Upper, DiagonalPreconditioner > minres_colmajor_diag; + MINRES, Lower, IdentityPreconditioner > minres_colmajor_lower_I; + MINRES, Upper, IdentityPreconditioner > minres_colmajor_upper_I; // MINRES, Lower, IncompleteLUT > minres_colmajor_ilut; //minres, SSORPreconditioner > minres_colmajor_ssor; - CALL_SUBTEST( check_sparse_square_solving(minres_colmajor_diag) ); - CALL_SUBTEST( check_sparse_spd_solving(minres_colmajor_I) ); + +// CALL_SUBTEST( check_sparse_square_solving(minres_colmajor_diag) ); // CALL_SUBTEST( check_sparse_square_solving(minres_colmajor_ilut) ); //CALL_SUBTEST( check_sparse_square_solving(minres_colmajor_ssor) ); + + // Diagonal preconditioner + MINRES, Lower, DiagonalPreconditioner > minres_colmajor_lower_diag; + MINRES, Upper, DiagonalPreconditioner > minres_colmajor_upper_diag; + MINRES, Upper|Lower, DiagonalPreconditioner > minres_colmajor_uplo_diag; + + // call tests for SPD matrix + CALL_SUBTEST( check_sparse_spd_solving(minres_colmajor_lower_I) ); + CALL_SUBTEST( check_sparse_spd_solving(minres_colmajor_upper_I) ); + + CALL_SUBTEST( check_sparse_spd_solving(minres_colmajor_lower_diag) ); + CALL_SUBTEST( check_sparse_spd_solving(minres_colmajor_upper_diag) ); +// CALL_SUBTEST( check_sparse_spd_solving(minres_colmajor_uplo_diag) ); + + // TO DO: symmetric semi-definite matrix + // TO DO: symmetric indefinite matrix } void test_minres() diff --git a/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h b/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h index ef0a6a9f0..7d6f4e79f 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,22 @@ #include #include #include +#include #include // Options -#define MPREAL_HAVE_INT64_SUPPORT // Enable int64_t support if possible. Available only for MSVC 2010 & GCC. +// FIXME HAVE_INT64_SUPPORT leads to clashes with long int and int64_t on some systems. +//#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 +81,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 +122,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 +136,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 +174,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 +183,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 +198,11 @@ public: ~mpreal(); +#ifdef MPREAL_HAVE_MOVE_SUPPORT + mpreal& operator=(mpreal&& v); + mpreal(mpreal&& u); +#endif + // Operations // = // +, -, *, /, ++, --, <<, >> @@ -292,14 +325,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 +361,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 +490,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 +581,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 +611,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 +701,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 +742,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 +759,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 +854,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 +909,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 +961,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 +978,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 +986,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 +994,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 +1002,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 +1011,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 +1034,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 +1042,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 +1050,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 +1067,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 +1090,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 +1107,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 +1121,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 +1143,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 +1154,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 +1195,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 +1228,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 +1257,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 +1268,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 +1297,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 +1353,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 +1382,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 +1392,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 +1458,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 +1468,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 +1546,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 +1644,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 +1652,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 +1732,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 +1747,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 +1777,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 +1863,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 +1912,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 +1967,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 +1979,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 +2013,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 +2028,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 +2050,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 +2133,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 +2163,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 +2179,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 +2317,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 +2332,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 +2347,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 +2364,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 +2405,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 +2420,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 +2431,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 +2502,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 +2515,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 +2588,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 +2612,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 +2650,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 +2676,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 +2698,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 +2713,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 +2748,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 +2951,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 +2979,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 +2986,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 +2994,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 +3009,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 +3048,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/eigen.h b/gtsam/3rdparty/ceres/eigen.h index 18a602cf4..a25fde97f 100644 --- a/gtsam/3rdparty/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/3rdparty/ceres/fixed_array.h b/gtsam/3rdparty/ceres/fixed_array.h index db1591636..455fce383 100644 --- a/gtsam/3rdparty/ceres/fixed_array.h +++ b/gtsam/3rdparty/ceres/fixed_array.h @@ -33,7 +33,7 @@ #define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ #include -#include +#include #include #include diff --git a/gtsam/3rdparty/ceres/jet.h b/gtsam/3rdparty/ceres/jet.h index 12d4e8bc9..4a7683f50 100644 --- a/gtsam/3rdparty/ceres/jet.h +++ b/gtsam/3rdparty/ceres/jet.h @@ -162,7 +162,7 @@ #include #include -#include +#include #include namespace ceres { 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/CMakeLists.txt b/gtsam/CMakeLists.txt index 0ebd6c07d..cab5e8639 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -9,7 +9,10 @@ set (gtsam_subdirs discrete linear nonlinear + sam + sfm slam + smart navigation ) @@ -131,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 diff --git a/gtsam/base/CMakeLists.txt b/gtsam/base/CMakeLists.txt index 66d3ec721..99984e7b3 100644 --- a/gtsam/base/CMakeLists.txt +++ b/gtsam/base/CMakeLists.txt @@ -5,5 +5,8 @@ install(FILES ${base_headers} DESTINATION include/gtsam/base) file(GLOB base_headers_tree "treeTraversal/*.h") install(FILES ${base_headers_tree} DESTINATION include/gtsam/base/treeTraversal) +file(GLOB deprecated_headers "deprecated/*.h") +install(FILES ${deprecated_headers} DESTINATION include/gtsam/base/deprecated) + # Build tests add_subdirectory(tests) 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/FastDefaultAllocator.h b/gtsam/base/FastDefaultAllocator.h index 156a87f55..bf5cfc498 100644 --- a/gtsam/base/FastDefaultAllocator.h +++ b/gtsam/base/FastDefaultAllocator.h @@ -17,8 +17,7 @@ */ #pragma once - -#include +#include // Configuration from CMake #if !defined GTSAM_ALLOCATOR_BOOSTPOOL && !defined GTSAM_ALLOCATOR_TBB && !defined GTSAM_ALLOCATOR_STL # ifdef GTSAM_USE_TBB @@ -85,4 +84,4 @@ namespace gtsam }; } -} \ No newline at end of file +} 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..65d532191 100644 --- a/gtsam/base/FastMap.h +++ b/gtsam/base/FastMap.h @@ -19,9 +19,9 @@ #pragma once #include -#include #include #include +#include namespace gtsam { @@ -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..8c23ae9e5 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -18,26 +18,22 @@ #pragma once -#include - -#include -#include -#include -#include -#include -#include #include #include +#include +#include -BOOST_MPL_HAS_XXX_TRAIT_DEF(print) +#include +#include + +namespace boost { +namespace serialization { +class access; +} /* namespace serialization */ +} /* namespace boost */ namespace gtsam { -// This is used internally to allow this container to be Testable even when it -// contains non-testable elements. -template -struct FastSetTestableHelper; - /** * FastSet is a thin wrapper around std::set that uses the boost * fast_pool_allocator instead of the default STL allocator. This is just a @@ -45,12 +41,16 @@ struct FastSetTestableHelper; * we've seen that the fast_pool_allocator can lead to speedups of several %. * @addtogroup base */ -template -class FastSet: public std::set, typename internal::FastDefaultAllocator::type> { +template +class FastSet: public std::set, + typename internal::FastDefaultAllocator::type> { + + BOOST_CONCEPT_ASSERT ((IsTestable )); public: - typedef std::set, typename internal::FastDefaultAllocator::type> Base; + typedef std::set, + typename internal::FastDefaultAllocator::type> Base; /** Default constructor */ FastSet() { @@ -59,23 +59,23 @@ public: /** Constructor from a range, passes through to base class */ template explicit FastSet(INPUTITERATOR first, INPUTITERATOR last) : - Base(first, last) { + Base(first, last) { } /** Constructor from a iterable container, passes through to base class */ template explicit FastSet(const INPUTCONTAINER& container) : - Base(container.begin(), container.end()) { + Base(container.begin(), container.end()) { } /** Copy constructor from another FastSet */ FastSet(const FastSet& x) : - Base(x) { + Base(x) { } /** Copy constructor from the base set class */ FastSet(const Base& x) : - Base(x) { + Base(x) { } #ifdef GTSAM_ALLOCATOR_BOOSTPOOL @@ -85,7 +85,7 @@ public: // STL vector where if the size is zero, the pool allocator will allocate // huge amounts of memory. if(x.size() > 0) - Base::insert(x.begin(), x.end()); + Base::insert(x.begin(), x.end()); } #endif @@ -95,81 +95,40 @@ public: } /** Handy 'exists' function */ - bool exists(const VALUE& e) const { return this->find(e) != this->end(); } + bool exists(const VALUE& e) const { + return this->find(e) != this->end(); + } - /** Print to implement Testable */ - void print(const std::string& str = "") const { FastSetTestableHelper::print(*this, str); } + /** Print to implement Testable: pretty basic */ + void print(const std::string& str = "") const { + for (typename Base::const_iterator it = this->begin(); it != this->end(); ++it) + traits::Print(*it, str); + } /** Check for equality within tolerance to implement Testable */ - bool equals(const FastSet& other, double tol = 1e-9) const { return FastSetTestableHelper::equals(*this, other, tol); } + bool equals(const FastSet& other, double tol = 1e-9) const { + typename Base::const_iterator it1 = this->begin(), it2 = other.begin(); + while (it1 != this->end()) { + if (it2 == other.end() || !traits::Equals(*it2, *it2, tol)) + return false; + ++it1; + ++it2; + } + return true; + } /** insert another set: handy for MATLAB access */ void merge(const FastSet& other) { - Base::insert(other.begin(),other.end()); + Base::insert(other.begin(), other.end()); } 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); } }; -// This is the default Testable interface for *non*Testable elements, which -// uses stream operators. -template -struct FastSetTestableHelper { - - typedef FastSet Set; - - static void print(const Set& set, const std::string& str) { - std::cout << str << "\n"; - for (typename Set::const_iterator it = set.begin(); it != set.end(); ++it) - std::cout << " " << *it << "\n"; - std::cout.flush(); - } - - static bool equals(const Set& set1, const Set& set2, double tol) { - typename Set::const_iterator it1 = set1.begin(); - typename Set::const_iterator it2 = set2.begin(); - while (it1 != set1.end()) { - if (it2 == set2.end() || - fabs((double)(*it1) - (double)(*it2)) > tol) - return false; - ++it1; - ++it2; - } - return true; - } -}; - -// This is the Testable interface for Testable elements -template -struct FastSetTestableHelper >::type> { - - typedef FastSet Set; - - static void print(const Set& set, const std::string& str) { - std::cout << str << "\n"; - for (typename Set::const_iterator it = set.begin(); it != set.end(); ++it) - it->print(" "); - std::cout.flush(); - } - - static bool equals(const Set& set1, const Set& set2, double tol) { - typename Set::const_iterator it1 = set1.begin(); - typename Set::const_iterator it2 = set2.begin(); - while (it1 != set1.end()) { - if (it2 == set2.end() || - !it1->equals(*it2, tol)) - return false; - ++it1; - ++it2; - } - return true; - } -}; - } diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h index be3eaa981..7d1cb970c 100644 --- a/gtsam/base/FastVector.h +++ b/gtsam/base/FastVector.h @@ -18,12 +18,10 @@ #pragma once -#include +#include #include #include - -#include -#include +#include namespace gtsam { @@ -35,20 +33,27 @@ namespace gtsam { * @addtogroup base */ template -class FastVector: public std::vector::type> { +class FastVector: public std::vector::type> { public: - typedef std::vector::type> Base; + typedef std::vector::type> Base; /** Default constructor */ - FastVector() {} + FastVector() { + } /** Constructor from size */ - explicit FastVector(size_t size) : Base(size) {} + explicit FastVector(size_t size) : + Base(size) { + } /** Constructor from size and initial values */ - explicit FastVector(size_t size, const VALUE& initial) : Base(size, initial) {} + explicit FastVector(size_t size, const VALUE& initial) : + Base(size, initial) { + } /** Constructor from a range, passes through to base class */ template @@ -56,33 +61,22 @@ public: // This if statement works around a bug in boost pool allocator and/or // STL vector where if the size is zero, the pool allocator will allocate // huge amounts of memory. - if(first != last) + if (first != last) Base::assign(first, last); } - /** Copy constructor from a FastList */ - FastVector(const FastList& x) { - if(x.size() > 0) - Base::assign(x.begin(), x.end()); - } - - /** Copy constructor from a FastSet */ - FastVector(const FastSet& x) { - if(x.size() > 0) - Base::assign(x.begin(), x.end()); - } - /** Copy constructor from the base class */ - FastVector(const Base& x) : Base(x) {} + FastVector(const Base& x) : + Base(x) { + } /** Copy constructor from a standard STL container */ template - FastVector(const std::vector& x) - { + FastVector(const std::vector& x) { // This if statement works around a bug in boost pool allocator and/or // STL vector where if the size is zero, the pool allocator will allocate // huge amounts of memory. - if(x.size() > 0) + if (x.size() > 0) Base::assign(x.begin(), x.end()); } @@ -95,7 +89,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 c67f7dd61..17783c5b9 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -26,7 +26,7 @@ #include #include -#include +#include #include // operator typeid namespace gtsam { @@ -183,7 +183,7 @@ 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::make_nvp("GenericValue", boost::serialization::base_object(*this)); ar & boost::serialization::make_nvp("value", value_); @@ -194,6 +194,11 @@ public: }; +// traits +template +struct traits > + : public Testable > {}; + // define Value::cast here since now GenericValue has been declared template const ValueType& Value::cast() const { diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index aec4b5f1c..39541416e 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -13,16 +13,20 @@ * @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 namespace gtsam { @@ -83,21 +87,119 @@ check_group_invariants(const G& a, const G& b, double tol = 1e-9) { && traits::Equals(traits::Compose(a, traits::Between(a, b)), b, tol); } -/// Macro to add group traits, additive flavor -#define GTSAM_ADDITIVE_GROUP(GROUP) \ - typedef additive_group_tag group_flavor; \ - static GROUP Compose(const GROUP &g, const GROUP & h) { return g + h;} \ - static GROUP Between(const GROUP &g, const GROUP & h) { return h - g;} \ - static GROUP Inverse(const GROUP &g) { return -g;} +namespace internal { -/// Macro to add group traits, multiplicative flavor -#define GTSAM_MULTIPLICATIVE_GROUP(GROUP) \ - typedef additive_group_tag group_flavor; \ - static GROUP Compose(const GROUP &g, const GROUP & h) { return g * h;} \ - static GROUP Between(const GROUP &g, const GROUP & h) { return g.inverse() * h;} \ - static GROUP Inverse(const GROUP &g) { return g.inverse();} +/// 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();} +}; -} // \ namespace gtsam +/// 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()); + } +}; + +// 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 IsGroup diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index bf2056cc8..1576aca1d 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -75,54 +75,89 @@ struct LieGroup { return derived().inverse(); } + /// expmap as required by manifold concept + /// Applies exponential map to v and composes with *this Class expmap(const TangentVector& v) const { return compose(Class::Expmap(v)); } + /// logmap as required by manifold concept + /// Applies logarithmic map to group element that takes *this to g TangentVector logmap(const Class& g) const { return Class::Logmap(between(g)); } + /// expmap with optional derivatives + Class expmap(const TangentVector& v, // + ChartJacobian H1, ChartJacobian H2 = boost::none) const { + Jacobian D_g_v; + Class g = Class::Expmap(v,H2 ? &D_g_v : 0); + Class h = compose(g); // derivatives inlined below + if (H1) *H1 = g.inverse().AdjointMap(); + if (H2) *H2 = D_g_v; + return h; + } + + /// logmap with optional derivatives + TangentVector logmap(const Class& g, // + ChartJacobian H1, ChartJacobian H2 = boost::none) const { + Class h = between(g); // derivatives inlined below + Jacobian D_v_h; + TangentVector v = Class::Logmap(h, (H1 || H2) ? &D_v_h : 0); + if (H1) *H1 = - D_v_h * h.inverse().AdjointMap(); + if (H2) *H2 = D_v_h; + return v; + } + + /// Retract at origin: possible in Lie group because it has an identity static Class Retract(const TangentVector& v) { return Class::ChartAtOrigin::Retract(v); } + /// LocalCoordinates at origin: possible in Lie group because it has an identity static TangentVector LocalCoordinates(const Class& g) { return Class::ChartAtOrigin::Local(g); } + /// Retract at origin with optional derivative static Class Retract(const TangentVector& v, ChartJacobian H) { return Class::ChartAtOrigin::Retract(v,H); } + /// LocalCoordinates at origin with optional derivative static TangentVector LocalCoordinates(const Class& g, ChartJacobian H) { return Class::ChartAtOrigin::Local(g,H); } + /// retract as required by manifold concept: applies v at *this Class retract(const TangentVector& v) const { return compose(Class::ChartAtOrigin::Retract(v)); } + /// localCoordinates as required by manifold concept: finds tangent vector between *this and g TangentVector localCoordinates(const Class& g) const { return Class::ChartAtOrigin::Local(between(g)); } + /// retract with optional derivatives 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; + Class g = Class::ChartAtOrigin::Retract(v, H2 ? &D_g_v : 0); + Class h = compose(g); // derivatives inlined below + if (H1) *H1 = g.inverse().AdjointMap(); + if (H2) *H2 = D_g_v; return h; } + /// localCoordinates with optional derivatives TangentVector localCoordinates(const Class& g, // ChartJacobian H1, ChartJacobian H2 = boost::none) const { - Class h = between(g,H1,H2); + Class h = between(g); // derivatives inlined below 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); + if (H1) *H1 = - D_v_h * h.inverse().AdjointMap(); + if (H2) *H2 = D_v_h; return v; } @@ -136,8 +171,10 @@ 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 : Testable { +struct LieGroupTraits { typedef lie_group_tag structure_category; /// @name Group @@ -167,12 +204,10 @@ struct LieGroupTraits : Testable { 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); } @@ -195,11 +230,12 @@ struct LieGroupTraits : Testable { ChartJacobian H = boost::none) { return m.inverse(H); } - /// @} - }; +/// Both LieGroupTraits and Testable +template struct LieGroup: LieGroupTraits, Testable {}; + } // \ namepsace internal /** @@ -248,7 +284,7 @@ public: // log and exp map without Jacobians g = traits::Expmap(v); v = traits::Logmap(g); - // log and expnential map with Jacobians + // log and exponential map with Jacobians g = traits::Expmap(v, Hg); v = traits::Logmap(g, Hg); } diff --git a/gtsam/base/LieMatrix.cpp b/gtsam/base/LieMatrix.cpp deleted file mode 100644 index 69054fc1c..000000000 --- a/gtsam/base/LieMatrix.cpp +++ /dev/null @@ -1,27 +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 LieMatrix.cpp - * @brief A wrapper around Matrix providing Lie compatibility - * @author Richard Roberts and Alex Cunningham - */ - -#include - -namespace gtsam { - -/* ************************************************************************* */ -void LieMatrix::print(const std::string& name) const { - gtsam::print(matrix(), name); -} - -} diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 90b7207a2..77edd5fd5 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -11,147 +11,16 @@ /** * @file LieMatrix.h - * @brief A wrapper around Matrix providing Lie compatibility - * @author Richard Roberts and Alex Cunningham + * @brief External deprecation warning, see deprecated/LieMatrix.h for details + * @author Paul Drews */ #pragma once -#include - #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 -#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 +#include "gtsam/base/deprecated/LieMatrix.h" diff --git a/gtsam/base/LieScalar.cpp b/gtsam/base/LieScalar.cpp deleted file mode 100644 index 5f59c29bc..000000000 --- a/gtsam/base/LieScalar.cpp +++ /dev/null @@ -1,17 +0,0 @@ -/* - * LieScalar.cpp - * - * Created on: Apr 12, 2013 - * Author: thduynguyen - */ - - - - -#include - -namespace gtsam { - void LieScalar::print(const std::string& name) const { - std::cout << name << ": " << d_ << std::endl; - } -} diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 9f6c56b28..417570604 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -11,7 +11,7 @@ /** * @file LieScalar.h - * @brief A wrapper around scalar providing Lie compatibility + * @brief External deprecation warning, see deprecated/LieScalar.h for details * @author Kai Ni */ @@ -23,69 +23,4 @@ #warning "LieScalar.h is deprecated. Please use double/float instead." #endif -#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 +#include diff --git a/gtsam/base/LieVector.cpp b/gtsam/base/LieVector.cpp deleted file mode 100644 index 84afdabc8..000000000 --- a/gtsam/base/LieVector.cpp +++ /dev/null @@ -1,40 +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 LieVector.cpp - * @brief Implementations for LieVector functions - * @author Alex Cunningham - */ - -#include -#include - -using namespace std; - -namespace gtsam { - -/* ************************************************************************* */ -LieVector::LieVector(size_t m, const double* const data) -: Vector(m) -{ - for(size_t i = 0; i < m; i++) - (*this)(i) = data[i]; -} - -/* ************************************************************************* */ -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 c2003df3c..310abcf02 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -11,8 +11,8 @@ /** * @file LieVector.h - * @brief A wrapper around vector providing Lie compatibility - * @author Alex Cunningham + * @brief Deprecation warning for LieVector, see deprecated/LieVector.h for details. + * @author Paul Drews */ #pragma once @@ -23,100 +23,4 @@ #warning "LieVector.h is deprecated. Please use Eigen::Vector instead." #endif -#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 +#include diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 6998b3b18..b30edb3df 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -46,7 +46,7 @@ struct manifold_tag {}; * 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. * */ @@ -90,9 +90,9 @@ struct ManifoldImpl { /// A helper that implements the traits interface for GTSAM manifolds. /// To use this for your class type, define: -/// template<> struct traits : public Manifold { }; +/// template<> struct traits : public internal::ManifoldTraits { }; template -struct Manifold: Testable, ManifoldImpl { +struct ManifoldTraits: ManifoldImpl { // Check that Class has the necessary machinery BOOST_CONCEPT_ASSERT((HasManifoldPrereqs)); @@ -116,6 +116,9 @@ struct Manifold: Testable, ManifoldImpl { } }; +/// Both ManifoldTraits and Testable +template struct Manifold: ManifoldTraits, Testable {}; + } // \ namespace internal /// Check invariants for Manifold type @@ -135,7 +138,7 @@ class IsManifold { public: typedef typename traits::structure_category structure_category_tag; - static const size_t dim = traits::dimension; + static const int dim = traits::dimension; typedef typename traits::ManifoldType ManifoldType; typedef typename traits::TangentVector TangentVector; @@ -165,6 +168,54 @@ struct FixedDimension { "FixedDimension instantiated for dymanically-sized type."); }; +/// Helper class to construct the product manifold of two other manifolds, M1 and M2 +/// Assumes nothing except manifold structure for M1 and M2, and the existence +/// of default constructor for those types +template +class ProductManifold: public std::pair { + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsManifold)); + +protected: + enum { dimension1 = traits::dimension }; + enum { dimension2 = traits::dimension }; + +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 needs default constructors to be defined + ProductManifold():std::pair(M1(),M2()) {} + + // 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); + } + + /// 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; + } +}; + +// Define any direct product group to be a model of the multiplicative Group concept +template +struct traits > : internal::Manifold > { +}; + } // \ namespace gtsam ///** diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 7bcd32b9f..c6af89486 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -20,6 +20,8 @@ #include #include #include +#include +#include #include #include @@ -180,22 +182,25 @@ 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(); + static const Eigen::IOFormat matlab( + Eigen::StreamPrecision, // precision + 0, // flags + " ", // coeffSeparator + ";\n", // rowSeparator + " \t", // rowPrefix + "", // rowSuffix + "[\n", // matPrefix + "\n ]" // matSuffix + ); + cout << s << A.format(matlab) << endl; +} - // print out all elements - stream << s << "[\n"; - for( size_t i = 0 ; i < m ; i++) { - for( size_t j = 0 ; j < n ; j++) { - double aij = A(i,j); - if(aij != 0.0) - stream << setw(12) << setprecision(9) << aij << ",\t"; - else - stream << " 0.0,\t"; - } - stream << endl; - } - stream << "];" << endl; +/* ************************************************************************* */ +//1 or 2 argument call +void print(const Matrix& A, const string &s){ + print(A, s, cout); } /* ************************************************************************* */ @@ -575,15 +580,6 @@ Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask) { return M; } -/* ************************************************************************* */ -Matrix3 skewSymmetric(double wx, double wy, double wz) -{ - return (Matrix3() << - 0.0, -wz, +wy, - +wz, 0.0, -wx, - -wy, +wx, 0.0).finished(); -} - /* ************************************************************************* */ Matrix LLt(const Matrix& A) { @@ -697,6 +693,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 c3cbfa341..e2b40b02b 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -21,11 +21,16 @@ // \callgraph #pragma once - #include +#include // Configuration from CMake + +#include +#include +#include +#include #include #include -#include + /** * Matrix is a typedef in the gtsam namespace @@ -198,9 +203,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 @@ -367,21 +377,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 @@ -481,9 +477,15 @@ GTSAM_EXPORT Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask * @param wz * @return a 3*3 skew symmetric matrix */ -GTSAM_EXPORT Matrix3 skewSymmetric(double wx, double wy, double wz); -template -inline Matrix3 skewSymmetric(const Eigen::MatrixBase& w) { return skewSymmetric(w(0),w(1),w(2));} + +inline Matrix3 skewSymmetric(double wx, double wy, double wz) { + return (Matrix3() << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0).finished(); +} + +template +inline Matrix3 skewSymmetric(const Eigen::MatrixBase& w) { + return skewSymmetric(w(0), w(1), w(2)); +} /** Use Cholesky to calculate inverse square root of a matrix */ GTSAM_EXPORT Matrix inverse_square_root(const Matrix& A); @@ -541,7 +543,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); @@ -549,7 +551,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); diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 2ea9d672e..eb1c1bbcc 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -18,8 +18,8 @@ */ #pragma once - -#include +#include // Configuration from CMake +#include #ifndef OPTIONALJACOBIAN_NOBOOST #include @@ -40,7 +40,8 @@ class OptionalJacobian { public: - /// ::Jacobian size type + /// Jacobian size type + /// TODO(frank): how to enforce RowMajor? Or better, make it work with any storage order? typedef Eigen::Matrix Jacobian; private: @@ -53,6 +54,14 @@ private: new (&map_) Eigen::Map(data); } + // Private and very dangerous constructor straight from memory + OptionalJacobian(double* data) : map_(NULL) { + if (data) usurp(data); + } + + template + friend class OptionalJacobian; + public: /// Default constructor acts like boost::none @@ -83,7 +92,7 @@ public: #ifndef OPTIONALJACOBIAN_NOBOOST /// Constructor with boost::none just makes empty - OptionalJacobian(boost::none_t none) : + OptionalJacobian(boost::none_t /*none*/) : map_(NULL) { } @@ -98,6 +107,11 @@ public: #endif + /// Constructor that will usurp data of a block expression + /// TODO(frank): unfortunately using a Map makes usurping non-contiguous memory impossible + // template + // OptionalJacobian(Eigen::Block block) : map_(NULL) { ?? } + /// Return true is allocated, false if default constructor was used operator bool() const { return map_.data() != NULL; @@ -108,8 +122,36 @@ public: return map_; } - /// TODO: operator->() - Eigen::Map* operator->(){ return &map_; } + /// operator->() + Eigen::Map* operator->() { + return &map_; + } + + /// Access M*N sub-block if we are allocated, otherwise none + /// TODO(frank): this could work as is below if only constructor above worked + // template + // OptionalJacobian block(int startRow, int startCol) { + // if (*this) + // OptionalJacobian(map_.block(startRow, startCol)); + // else + // return OptionalJacobian(); + // } + + /// Access Rows*N sub-block if we are allocated, otherwise return an empty OptionalJacobian + /// The use case is functions with arguments that are dissected, e.g. Pose3 into Rot3, Point3 + /// TODO(frank): ideally, we'd like full block functionality, but see note above. + template + OptionalJacobian cols(int startCol) { + if (*this) + return OptionalJacobian(&map_(0,startCol)); + else + return OptionalJacobian(); + } + + /// Access M*Cols sub-block if we are allocated, otherwise return empty OptionalJacobian + /// The use case is functions that create their return value piecemeal by calling other functions + /// TODO(frank): Unfortunately we assume column-major storage order and hence this can't work + /// template OptionalJacobian rows(int startRow) { ?? } }; // The pure dynamic specialization of this is needed to support @@ -142,7 +184,7 @@ public: #ifndef OPTIONALJACOBIAN_NOBOOST /// Constructor with boost::none just makes empty - OptionalJacobian(boost::none_t none) : + OptionalJacobian(boost::none_t /*none*/) : pointer_(NULL) { } @@ -168,5 +210,30 @@ public: Jacobian* operator->(){ return pointer_; } }; +// forward declare +template struct traits; + +/** + * @brief: meta-function to generate Jacobian + * @param T return type + * @param A argument type + */ +template +struct MakeJacobian { + typedef Eigen::Matrix::dimension, traits::dimension> type; +}; + +/** + * @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..463b5f5d9 --- /dev/null +++ b/gtsam/base/ProductLieGroup.h @@ -0,0 +1,169 @@ +/* ---------------------------------------------------------------------------- + + * 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(traits::Inverse(this->first), traits::Inverse(this->second)); + } + 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; + + ProductLieGroup retract(const TangentVector& v, // + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { + if (H1||H2) throw std::runtime_error("ProductLieGroup::retract derivatives not implemented yet"); + G g = traits::Retract(this->first, v.template head()); + H h = traits::Retract(this->second, v.template tail()); + return ProductLieGroup(g,h); + } + TangentVector localCoordinates(const ProductLieGroup& g, // + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { + if (H1||H2) throw std::runtime_error("ProductLieGroup::localCoordinates derivatives not implemented yet"); + typename traits::TangentVector v1 = traits::Local(this->first, g.first); + typename traits::TangentVector v2 = traits::Local(this->second, g.second); + TangentVector v; + v << v1, v2; + return v; + } + /// @} + + /// @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; + } + ProductLieGroup expmap(const TangentVector& v) const { + return compose(ProductLieGroup::Expmap(v)); + } + TangentVector logmap(const ProductLieGroup& g) const { + return ProductLieGroup::Logmap(between(g)); + } + /// @} + +}; + +// Define any direct product group to be a model of the multiplicative Group concept +template +struct traits > : internal::LieGroupTraits > {}; + +} // namespace gtsam + diff --git a/gtsam/base/SymmetricBlockMatrix.cpp b/gtsam/base/SymmetricBlockMatrix.cpp index 546b6a7f1..7cca63092 100644 --- a/gtsam/base/SymmetricBlockMatrix.cpp +++ b/gtsam/base/SymmetricBlockMatrix.cpp @@ -20,6 +20,7 @@ #include #include #include +#include namespace gtsam { @@ -61,13 +62,13 @@ VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial( // Split conditional - // Create one big conditionals with many frontal variables. - gttic(Construct_conditional); - const size_t varDim = offset(nFrontals); - VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim); - Ab.full() = matrix_.topRows(varDim); - Ab.full().triangularView().setZero(); - gttoc(Construct_conditional); + // Create one big conditionals with many frontal variables. + gttic(Construct_conditional); + const size_t varDim = offset(nFrontals); + VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim); + Ab.full() = matrix_.topRows(varDim); + Ab.full().triangularView().setZero(); + gttoc(Construct_conditional); gttic(Remaining_factor); // Take lower-right block of Ab_ to get the remaining factor diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 5dcc79a68..1f81ca1f9 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -17,9 +17,21 @@ */ #pragma once -#include #include +#include #include +#include +#include +#include +#include +#include +#include + +namespace boost { +namespace serialization { +class access; +} /* namespace serialization */ +} /* namespace boost */ namespace gtsam { @@ -199,6 +211,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 +248,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 @@ -246,13 +259,8 @@ namespace gtsam { } }; - /* ************************************************************************* */ - class CholeskyFailed : public gtsam::ThreadsafeException - { - public: - CholeskyFailed() throw() {} - virtual ~CholeskyFailed() throw() {} - }; + /// Foward declare exception class + class CholeskyFailed; } 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 4790530ab..92ccb9156 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -123,8 +123,8 @@ namespace gtsam { double tol_; 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 (traits::Equals(*actual,*expected, tol_)); + if (!actual || !expected) return true; + return actual && expected && traits::Equals(*actual,*expected, tol_); } }; diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 04d3fc676..f976be0e7 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -17,13 +17,14 @@ #pragma once -#include -#include -#include +#include +#include + #include #include -#include -#include +#include +#include +#include namespace gtsam { diff --git a/gtsam/base/ThreadsafeException.h b/gtsam/base/ThreadsafeException.h new file mode 100644 index 000000000..ca13047a8 --- /dev/null +++ b/gtsam/base/ThreadsafeException.h @@ -0,0 +1,155 @@ +/* ---------------------------------------------------------------------------- + + * 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 ThreadSafeException.h + * @brief Base exception type that uses tbb_exception if GTSAM is compiled with TBB + * @author Richard Roberts + * @date Aug 21, 2010 + * @addtogroup base + */ + +#pragma once + +#include // for GTSAM_USE_TBB + +#include +#include +#include + +#ifdef GTSAM_USE_TBB +#include +#include +#include +#include +#endif + +namespace gtsam { + +/// Base exception type that uses tbb_exception if GTSAM is compiled with TBB. +template +class ThreadsafeException: +#ifdef GTSAM_USE_TBB + public tbb::tbb_exception +#else +public std::exception +#endif +{ +#ifdef GTSAM_USE_TBB +private: + typedef tbb::tbb_exception Base; +protected: + typedef std::basic_string, + tbb::tbb_allocator > String; +#else +private: + typedef std::exception Base; +protected: + typedef std::string String; +#endif + +protected: + bool dynamic_; ///< Whether this object was moved + mutable boost::optional description_; ///< Optional description + + /// Default constructor is protected - may only be created from derived classes + ThreadsafeException() : + dynamic_(false) { + } + + /// Copy constructor is protected - may only be created from derived classes + ThreadsafeException(const ThreadsafeException& other) : + Base(other), dynamic_(false) { + } + + /// Construct with description string + ThreadsafeException(const std::string& description) : + dynamic_(false), description_( + String(description.begin(), description.end())) { + } + + /// Default destructor doesn't have the throw() + virtual ~ThreadsafeException() throw () { + } + +public: + // Implement functions for tbb_exception +#ifdef GTSAM_USE_TBB + virtual tbb::tbb_exception* move() throw () { + void* cloneMemory = scalable_malloc(sizeof(DERIVED)); + if (!cloneMemory) { + std::cerr << "Failed allocating memory to copy thrown exception, exiting now." << std::endl; + exit(-1); + } + DERIVED* clone = ::new(cloneMemory) DERIVED(static_cast(*this)); + clone->dynamic_ = true; + return clone; + } + + virtual void destroy() throw () { + if (dynamic_) { + DERIVED* derivedPtr = static_cast(this); + derivedPtr->~DERIVED(); + scalable_free(derivedPtr); + } + } + + virtual void throw_self() { + throw *static_cast(this); + } + + virtual const char* name() const throw () { + return typeid(DERIVED).name(); + } +#endif + + virtual const char* what() const throw () { + return description_ ? description_->c_str() : ""; + } +}; + +/// Thread-safe runtime error exception +class RuntimeErrorThreadsafe: public ThreadsafeException { +public: + /// Construct with a string describing the exception + RuntimeErrorThreadsafe(const std::string& description) : + ThreadsafeException(description) { + } +}; + +/// Thread-safe out of range exception +class OutOfRangeThreadsafe: public ThreadsafeException { +public: + /// Construct with a string describing the exception + OutOfRangeThreadsafe(const std::string& description) : + ThreadsafeException(description) { + } +}; + +/// Thread-safe invalid argument exception +class InvalidArgumentThreadsafe: public ThreadsafeException< + InvalidArgumentThreadsafe> { +public: + /// Construct with a string describing the exception + InvalidArgumentThreadsafe(const std::string& description) : + ThreadsafeException(description) { + } +}; + +/// Indicate Cholesky factorization failure +class CholeskyFailed : public gtsam::ThreadsafeException +{ +public: + CholeskyFailed() throw() {} + virtual ~CholeskyFailed() throw() {} +}; + +} // namespace gtsam diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index a12f453f4..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; } @@ -165,7 +166,7 @@ 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 a9ef8fe10..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(); } /* ************************************************************************* */ diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 74cd248a1..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 { @@ -97,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 @@ -346,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); @@ -362,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 index 094e6f162..c356dcc07 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -20,17 +20,10 @@ template struct traits; namespace internal { -/// VectorSpace Implementation for Fixed sizes +/// VectorSpaceTraits Implementation for Fixed sizes 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::Matrix TangentVector; @@ -68,21 +61,21 @@ struct VectorSpaceImpl { return Class(v); } - static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1, - ChartJacobian H2) { + 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, - ChartJacobian H2) { + 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) { + static Class Inverse(const Class& v, ChartJacobian H = boost::none) { if (H) *H = - Jacobian::Identity(); return -v; } @@ -90,7 +83,7 @@ struct VectorSpaceImpl { /// @} }; -/// VectorSpace implementation for dynamic types. +/// VectorSpaceTraits implementation for dynamic types. template struct VectorSpaceImpl { @@ -166,11 +159,11 @@ struct VectorSpaceImpl { /// @} }; -/// A helper that implements the traits interface for GTSAM lie groups. +/// A helper that implements the traits interface for GTSAM vector space types. /// To use this for your gtsam type, define: -/// template<> struct traits : public VectorSpace { }; +/// template<> struct traits : public VectorSpaceTraits { }; template -struct VectorSpace: Testable, VectorSpaceImpl { +struct VectorSpaceTraits: VectorSpaceImpl { typedef vector_space_tag structure_category; @@ -185,9 +178,12 @@ struct VectorSpace: Testable, VectorSpaceImpl { 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 { }; @@ -401,7 +397,8 @@ struct DynamicTraits { return result; } - static Dynamic Expmap(const TangentVector& v, ChartJacobian H = boost::none) { + static Dynamic Expmap(const TangentVector& /*v*/, ChartJacobian H = boost::none) { + static_cast(H); throw std::runtime_error("Expmap not defined for dynamic types"); } 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/debug.cpp b/gtsam/base/debug.cpp index 6abc98642..1c4d08dcd 100644 --- a/gtsam/base/debug.cpp +++ b/gtsam/base/debug.cpp @@ -17,6 +17,8 @@ */ #include +#include // for GTSAM_USE_TBB + #ifdef GTSAM_USE_TBB #include #endif diff --git a/gtsam/base/deprecated/LieMatrix.h b/gtsam/base/deprecated/LieMatrix.h new file mode 100644 index 000000000..e26f45511 --- /dev/null +++ b/gtsam/base/deprecated/LieMatrix.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 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 { + gtsam::print(matrix(), name); + } + /** 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/deprecated/LieScalar.h b/gtsam/base/deprecated/LieScalar.h new file mode 100644 index 000000000..f9c424e95 --- /dev/null +++ b/gtsam/base/deprecated/LieScalar.h @@ -0,0 +1,87 @@ +/* ---------------------------------------------------------------------------- + + * 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/deprecated/LieVector.h b/gtsam/base/deprecated/LieVector.h new file mode 100644 index 000000000..4a85036e0 --- /dev/null +++ b/gtsam/base/deprecated/LieVector.h @@ -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 LieVector.h + * @brief A wrapper around vector providing Lie compatibility + * @author Alex Cunningham + */ + +#pragma once + +#include +#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) : Vector(m) { + for (size_t i = 0; i < m; i++) (*this)(i) = data[i]; + } + + /// @name Testable + /// @{ + GTSAM_EXPORT void print(const std::string& name="") const { + gtsam::print(vector(), name); + } + 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/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index b6593bd9f..f408427d8 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -49,15 +49,15 @@ bool equality(const T& input = T()) { return input==output; } -// This version requires equals +// This version requires Testable template bool equalsObj(const T& input = T()) { T output; roundtrip(input,output); - return input.equals(output); + return assert_equal(input, output); } -// De-referenced version for pointers +// De-referenced version for pointers, requires equals method template bool equalsDereferenced(const T& input) { T output; @@ -84,15 +84,15 @@ bool equalityXML(const T& input = T()) { return input==output; } -// This version requires equals +// This version requires Testable template bool equalsXML(const T& input = T()) { T output; roundtripXML(input,output); - return input.equals(output); + return assert_equal(input, output); } -// This version is for pointers +// This version is for pointers, requires equals method template bool equalsDereferencedXML(const T& input = T()) { T output; @@ -119,15 +119,15 @@ bool equalityBinary(const T& input = T()) { return input==output; } -// This version requires equals +// This version requires Testable template bool equalsBinary(const T& input = T()) { T output; roundtripBinary(input,output); - return input.equals(output); + return assert_equal(input, output); } -// This version is for pointers +// This version is for pointers, requires equals method template bool equalsDereferencedBinary(const T& input = T()) { T output; diff --git a/gtsam/base/tests/testFastContainers.cpp b/gtsam/base/tests/testFastContainers.cpp index 464624bd4..19870fdeb 100644 --- a/gtsam/base/tests/testFastContainers.cpp +++ b/gtsam/base/tests/testFastContainers.cpp @@ -7,13 +7,14 @@ * @author Alex Cunningham */ -#include +#include +#include +#include #include #include -#include -#include +#include using namespace boost::assign; using namespace gtsam; @@ -21,7 +22,7 @@ using namespace gtsam; /* ************************************************************************* */ TEST( testFastContainers, KeySet ) { - FastVector init_vector; + FastVector init_vector; init_vector += 2, 3, 4, 5; FastSet actSet(init_vector); 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 7cc649e66..5e18e1495 100644 --- a/gtsam/base/tests/testLieMatrix.cpp +++ b/gtsam/base/tests/testLieMatrix.cpp @@ -15,10 +15,9 @@ */ #include - +#include #include #include -#include using namespace gtsam; diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp index 060087c2a..bacb9dd1e 100644 --- a/gtsam/base/tests/testLieScalar.cpp +++ b/gtsam/base/tests/testLieScalar.cpp @@ -15,10 +15,9 @@ */ #include - +#include #include #include -#include using namespace gtsam; @@ -37,8 +36,8 @@ TEST(LieScalar , Concept) { //****************************************************************************** TEST(LieScalar , Invariants) { LieScalar lie1(2), lie2(3); - check_group_invariants(lie1, lie2); - check_manifold_invariants(lie1, lie2); + CHECK(check_group_invariants(lie1, lie2)); + CHECK(check_manifold_invariants(lie1, lie2)); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp index 81e03c63c..3c2885c18 100644 --- a/gtsam/base/tests/testLieVector.cpp +++ b/gtsam/base/tests/testLieVector.cpp @@ -15,10 +15,9 @@ */ #include - +#include #include #include -#include using namespace gtsam; diff --git a/gtsam/base/tests/testOptionalJacobian.cpp b/gtsam/base/tests/testOptionalJacobian.cpp index 331fb49eb..de1c07dcf 100644 --- a/gtsam/base/tests/testOptionalJacobian.cpp +++ b/gtsam/base/tests/testOptionalJacobian.cpp @@ -61,20 +61,15 @@ TEST( OptionalJacobian, Constructors ) { } //****************************************************************************** +Matrix kTestMatrix = (Matrix23() << 11,12,13,21,22,23).finished(); + void test(OptionalJacobian<2, 3> H = boost::none) { if (H) - *H = Matrix23::Zero(); -} - -void testPtr(Matrix23* H = NULL) { - if (H) - *H = Matrix23::Zero(); + *H = kTestMatrix; } TEST( OptionalJacobian, Fixed) { - Matrix expected = Matrix23::Zero(); - // Default argument does nothing test(); @@ -82,61 +77,83 @@ TEST( OptionalJacobian, Fixed) { Matrix23 fixed1; fixed1.setOnes(); test(fixed1); - EXPECT(assert_equal(expected,fixed1)); + EXPECT(assert_equal(kTestMatrix,fixed1)); // Fixed size, no copy, pointer style Matrix23 fixed2; fixed2.setOnes(); test(&fixed2); - EXPECT(assert_equal(expected,fixed2)); + EXPECT(assert_equal(kTestMatrix,fixed2)); - // Empty is no longer a sign we don't want a matrix, we want it resized + // Passing in an empty matrix means we want it resized Matrix dynamic0; test(dynamic0); - EXPECT(assert_equal(expected,dynamic0)); + EXPECT(assert_equal(kTestMatrix,dynamic0)); // Dynamic wrong size Matrix dynamic1(3, 5); dynamic1.setOnes(); test(dynamic1); - EXPECT(assert_equal(expected,dynamic1)); + EXPECT(assert_equal(kTestMatrix,dynamic1)); // Dynamic right size Matrix dynamic2(2, 5); dynamic2.setOnes(); test(dynamic2); - EXPECT(assert_equal(expected, dynamic2)); + EXPECT(assert_equal(kTestMatrix, dynamic2)); } //****************************************************************************** void test2(OptionalJacobian<-1,-1> H = boost::none) { if (H) - *H = Matrix23::Zero(); // resizes + *H = kTestMatrix; // 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 + // Passing in an empty matrix means we want it resized Matrix dynamic0; test2(dynamic0); - EXPECT(assert_equal(expected,dynamic0)); + EXPECT(assert_equal(kTestMatrix,dynamic0)); // Dynamic wrong size Matrix dynamic1(3, 5); dynamic1.setOnes(); test2(dynamic1); - EXPECT(assert_equal(expected,dynamic1)); + EXPECT(assert_equal(kTestMatrix,dynamic1)); // Dynamic right size Matrix dynamic2(2, 5); dynamic2.setOnes(); test2(dynamic2); - EXPECT(assert_equal(expected, dynamic2)); + EXPECT(assert_equal(kTestMatrix, dynamic2)); +} + +//****************************************************************************** +void test3(double add, OptionalJacobian<2,1> H = boost::none) { + if (H) *H << add + 10, add + 20; +} + +// This function calls the above function three times, one for each column +void test4(OptionalJacobian<2, 3> H = boost::none) { + if (H) { + test3(1, H.cols<1>(0)); + test3(2, H.cols<1>(1)); + test3(3, H.cols<1>(2)); + } +} + +TEST(OptionalJacobian, Block) { + // Default argument does nothing + test4(); + + Matrix23 fixed; + fixed.setOnes(); + test4(fixed); + EXPECT(assert_equal(kTestMatrix, fixed)); } //****************************************************************************** diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp index fceb2f4b4..1db28bcc8 100644 --- a/gtsam/base/tests/testSerializationBase.cpp +++ b/gtsam/base/tests/testSerializationBase.cpp @@ -16,6 +16,8 @@ * @date Feb 7, 2012 */ +#include + #include #include #include @@ -60,10 +62,10 @@ TEST (Serialization, FastMap) { /* ************************************************************************* */ TEST (Serialization, FastSet) { - FastSet set; - set.insert(1.0); - set.insert(2.0); - set.insert(3.0); + KeySet set; + set.insert(1); + set.insert(2); + set.insert(3); EXPECT(equality(set)); EXPECT(equalityXML(set)); diff --git a/gtsam/base/tests/testTestableAssertions.cpp b/gtsam/base/tests/testTestableAssertions.cpp index d8c62b121..305aa7ca9 100644 --- a/gtsam/base/tests/testTestableAssertions.cpp +++ b/gtsam/base/tests/testTestableAssertions.cpp @@ -15,8 +15,7 @@ */ #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/timing.cpp b/gtsam/base/timing.cpp index 36f1c2f5f..b76746ba0 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -16,24 +16,28 @@ * @date Oct 5, 2010 */ -#include -#include -#include -#include -#include -#include -#include -#include - #include #include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + namespace gtsam { namespace internal { -GTSAM_EXPORT boost::shared_ptr timingRoot( +GTSAM_EXPORT boost::shared_ptr gTimingRoot( new TimingOutline("Total", getTicTocID("Total"))); -GTSAM_EXPORT boost::weak_ptr timingCurrent(timingRoot); +GTSAM_EXPORT boost::weak_ptr gCurrentTimer(gTimingRoot); /* ************************************************************************* */ // Implementation of TimingOutline @@ -50,8 +54,8 @@ void TimingOutline::add(size_t usecs, size_t usecsWall) { } /* ************************************************************************* */ -TimingOutline::TimingOutline(const std::string& label, size_t myId) : - myId_(myId), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_( +TimingOutline::TimingOutline(const std::string& label, size_t id) : + id_(id), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_( 0), lastChildOrder_(0), label_(label) { #ifdef GTSAM_USING_NEW_BOOST_TIMERS timer_.stop(); @@ -153,7 +157,7 @@ const boost::shared_ptr& TimingOutline::child(size_t child, } /* ************************************************************************* */ -void TimingOutline::ticInternal() { +void TimingOutline::tic() { #ifdef GTSAM_USING_NEW_BOOST_TIMERS assert(timer_.is_stopped()); timer_.start(); @@ -169,7 +173,7 @@ void TimingOutline::ticInternal() { } /* ************************************************************************* */ -void TimingOutline::tocInternal() { +void TimingOutline::toc() { #ifdef GTSAM_USING_NEW_BOOST_TIMERS assert(!timer_.is_stopped()); @@ -212,7 +216,6 @@ void TimingOutline::finishedIteration() { } /* ************************************************************************* */ -// Generate or retrieve a unique global ID number that will be used to look up tic_/toc statements size_t getTicTocID(const char *descriptionC) { const std::string description(descriptionC); // Global (static) map from strings to ID numbers and current next ID number @@ -232,37 +235,33 @@ size_t getTicTocID(const char *descriptionC) { } /* ************************************************************************* */ -void ticInternal(size_t id, const char *labelC) { +void tic(size_t id, const char *labelC) { const std::string label(labelC); - if (ISDEBUG("timing-verbose")) - std::cout << "gttic_(" << id << ", " << label << ")" << std::endl; boost::shared_ptr node = // - timingCurrent.lock()->child(id, label, timingCurrent); - timingCurrent = node; - node->ticInternal(); + gCurrentTimer.lock()->child(id, label, gCurrentTimer); + gCurrentTimer = node; + node->tic(); } /* ************************************************************************* */ -void tocInternal(size_t id, const char *label) { - if (ISDEBUG("timing-verbose")) - std::cout << "gttoc(" << id << ", " << label << ")" << std::endl; - boost::shared_ptr current(timingCurrent.lock()); - if (id != current->myId_) { - timingRoot->print(); +void toc(size_t id, const char *label) { + boost::shared_ptr current(gCurrentTimer.lock()); + if (id != current->id_) { + gTimingRoot->print(); throw std::invalid_argument( (boost::format( "gtsam timing: Mismatched tic/toc: gttoc(\"%s\") called when last tic was \"%s\".") % label % current->label_).str()); } if (!current->parent_.lock()) { - timingRoot->print(); + gTimingRoot->print(); throw std::invalid_argument( (boost::format( "gtsam timing: Mismatched tic/toc: extra gttoc(\"%s\"), already at the root") % label).str()); } - current->tocInternal(); - timingCurrent = current->parent_; + current->toc(); + gCurrentTimer = current->parent_; } } // namespace internal diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index 7a43ef884..b89e15637 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -17,12 +17,16 @@ */ #pragma once -#include -#include -#include -#include -#include #include +#include +#include // for GTSAM_USE_TBB + +#include +#include +#include + +#include +#include // This file contains the GTSAM timing instrumentation library, a low-overhead method for // learning at a medium-fine level how much time various components of an algorithm take @@ -113,6 +117,7 @@ # include #else # include +# include #endif #ifdef GTSAM_USE_TBB @@ -125,16 +130,21 @@ namespace gtsam { namespace internal { + // Generate/retrieve a unique global ID number that will be used to look up tic/toc statements GTSAM_EXPORT size_t getTicTocID(const char *description); - GTSAM_EXPORT void ticInternal(size_t id, const char *label); - GTSAM_EXPORT void tocInternal(size_t id, const char *label); + + // Create new TimingOutline child for gCurrentTimer, make it gCurrentTimer, and call tic method + GTSAM_EXPORT void tic(size_t id, const char *label); + + // Call toc on gCurrentTimer and then set gCurrentTimer to the parent of gCurrentTimer + GTSAM_EXPORT void toc(size_t id, const char *label); /** * Timing Entry, arranged in a tree */ class GTSAM_EXPORT TimingOutline { protected: - size_t myId_; + size_t id_; size_t t_; size_t tWall_; double t2_ ; ///< cache the \sum t_i^2 @@ -176,29 +186,38 @@ namespace gtsam { void print2(const std::string& outline = "", const double parentTotal = -1.0) const; const boost::shared_ptr& child(size_t child, const std::string& label, const boost::weak_ptr& thisPtr); - void ticInternal(); - void tocInternal(); + void tic(); + void toc(); void finishedIteration(); - GTSAM_EXPORT friend void tocInternal(size_t id, const char *label); + GTSAM_EXPORT friend void toc(size_t id, const char *label); }; // \TimingOutline /** - * No documentation + * Small class that calls internal::tic at construction, and internol::toc when destroyed */ class AutoTicToc { - private: + private: size_t id_; - const char *label_; + const char* label_; bool isSet_; - public: - AutoTicToc(size_t id, const char* label) : id_(id), label_(label), isSet_(true) { ticInternal(id_, label_); } - void stop() { tocInternal(id_, label_); isSet_ = false; } - ~AutoTicToc() { if(isSet_) stop(); } + + public: + AutoTicToc(size_t id, const char* label) + : id_(id), label_(label), isSet_(true) { + tic(id_, label_); + } + void stop() { + toc(id_, label_); + isSet_ = false; + } + ~AutoTicToc() { + if (isSet_) stop(); + } }; - GTSAM_EXTERN_EXPORT boost::shared_ptr timingRoot; - GTSAM_EXTERN_EXPORT boost::weak_ptr timingCurrent; + GTSAM_EXTERN_EXPORT boost::shared_ptr gTimingRoot; + GTSAM_EXTERN_EXPORT boost::weak_ptr gCurrentTimer; } // Tic and toc functions that are always active (whether or not ENABLE_TIMING is defined) @@ -210,7 +229,7 @@ namespace gtsam { // tic #define gttic_(label) \ static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \ - ::gtsam::internal::AutoTicToc label##_obj = ::gtsam::internal::AutoTicToc(label##_id_tic, #label) + ::gtsam::internal::AutoTicToc label##_obj(label##_id_tic, #label) // toc #define gttoc_(label) \ @@ -228,26 +247,26 @@ namespace gtsam { // indicate iteration is finished inline void tictoc_finishedIteration_() { - ::gtsam::internal::timingRoot->finishedIteration(); } + ::gtsam::internal::gTimingRoot->finishedIteration(); } // print inline void tictoc_print_() { - ::gtsam::internal::timingRoot->print(); } + ::gtsam::internal::gTimingRoot->print(); } // print mean and standard deviation inline void tictoc_print2_() { - ::gtsam::internal::timingRoot->print2(); } + ::gtsam::internal::gTimingRoot->print2(); } // get a node by label and assign it to variable #define tictoc_getNode(variable, label) \ static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID(#label); \ const boost::shared_ptr variable = \ - ::gtsam::internal::timingCurrent.lock()->child(label##_id_getnode, #label, ::gtsam::internal::timingCurrent); + ::gtsam::internal::gCurrentTimer.lock()->child(label##_id_getnode, #label, ::gtsam::internal::gCurrentTimer); // reset inline void tictoc_reset_() { - ::gtsam::internal::timingRoot.reset(new ::gtsam::internal::TimingOutline("Total", ::gtsam::internal::getTicTocID("Total"))); - ::gtsam::internal::timingCurrent = ::gtsam::internal::timingRoot; } + ::gtsam::internal::gTimingRoot.reset(new ::gtsam::internal::TimingOutline("Total", ::gtsam::internal::getTicTocID("Total"))); + ::gtsam::internal::gCurrentTimer = ::gtsam::internal::gTimingRoot; } #ifdef ENABLE_TIMING #define gttic(label) gttic_(label) diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 3edd7d076..12a29e45b 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -1,19 +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) + * 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 + * See LICENSE for the license information -* -------------------------------------------------------------------------- */ + * -------------------------------------------------------------------------- */ /** -* @file treeTraversal-inst.h -* @author Richard Roberts -* @date April 9, 2013 -*/ + * @file treeTraversal-inst.h + * @author Richard Roberts + * @date April 9, 2013 + */ #pragma once #include @@ -22,6 +22,7 @@ #include #include #include +#include // for GTSAM_USE_TBB #include #include @@ -33,192 +34,197 @@ namespace gtsam { - /** Internal functions used for traversing trees */ - namespace treeTraversal { +/** Internal functions used for traversing trees */ +namespace treeTraversal { - /* ************************************************************************* */ - namespace { - // Internal node used in DFS preorder stack - template - struct TraversalNode { - bool expanded; - const boost::shared_ptr& treeNode; - DATA& parentData; - typename FastList::iterator dataPointer; - TraversalNode(const boost::shared_ptr& _treeNode, DATA& _parentData) : - expanded(false), treeNode(_treeNode), parentData(_parentData) {} - }; - - // Do nothing - default argument for post-visitor for tree traversal - struct no_op { - template - void operator()(const boost::shared_ptr& node, const DATA& data) {} - }; - - } - - /** Traverse a forest depth-first with pre-order and post-order visits. - * @param forest The forest of trees to traverse. The method \c forest.roots() should exist - * and return a collection of (shared) pointers to \c FOREST::Node. - * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before - * visiting its children, and will be passed, by reference, the \c DATA object returned - * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object - * to pass to the children. The returned \c DATA object will be copy-constructed only - * upon returning to store internally, thus may be modified by visiting the children. - * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. - * @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting - * its children, and will be passed, by reference, the \c DATA object returned by the - * call to \c visitorPre (the \c DATA object may be modified by visiting the children). - * @param rootData The data to pass by reference to \c visitorPre when it is called on each - * root node. */ - template - void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost) - { - // Typedefs - typedef typename FOREST::Node Node; - typedef boost::shared_ptr sharedNode; - - // Depth first traversal stack - typedef TraversalNode TraversalNode; - typedef FastList Stack; - Stack stack; - FastList dataList; // List to store node data as it is returned from the pre-order visitor - - // Add roots to stack (insert such that they are visited and processed in order - { - typename Stack::iterator insertLocation = stack.begin(); - BOOST_FOREACH(const sharedNode& root, forest.roots()) - stack.insert(insertLocation, TraversalNode(root, rootData)); - } - - // Traverse - while(!stack.empty()) - { - // Get next node - TraversalNode& node = stack.front(); - - if(node.expanded) { - // If already expanded, then the data stored in the node is no longer needed, so visit - // then delete it. - (void) visitorPost(node.treeNode, *node.dataPointer); - dataList.erase(node.dataPointer); - stack.pop_front(); - } else { - // If not already visited, visit the node and add its children (use reverse iterators so - // children are processed in the order they appear) - node.dataPointer = dataList.insert(dataList.end(), visitorPre(node.treeNode, node.parentData)); - typename Stack::iterator insertLocation = stack.begin(); - BOOST_FOREACH(const sharedNode& child, node.treeNode->children) - stack.insert(insertLocation, TraversalNode(child, *node.dataPointer)); - node.expanded = true; - } - } - assert(dataList.empty()); - } - - /** Traverse a forest depth-first, with a pre-order visit but no post-order visit. - * @param forest The forest of trees to traverse. The method \c forest.roots() should exist - * and return a collection of (shared) pointers to \c FOREST::Node. - * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before - * visiting its children, and will be passed, by reference, the \c DATA object returned - * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object - * to pass to the children. The returned \c DATA object will be copy-constructed only - * upon returning to store internally, thus may be modified by visiting the children. - * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. - * @param rootData The data to pass by reference to \c visitorPre when it is called on each - * root node. */ - template - void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) - { - no_op visitorPost; - DepthFirstForest(forest, rootData, visitorPre, visitorPost); - } - - /** Traverse a forest depth-first with pre-order and post-order visits. - * @param forest The forest of trees to traverse. The method \c forest.roots() should exist - * and return a collection of (shared) pointers to \c FOREST::Node. - * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before - * visiting its children, and will be passed, by reference, the \c DATA object returned - * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object - * to pass to the children. The returned \c DATA object will be copy-constructed only - * upon returning to store internally, thus may be modified by visiting the children. - * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. - * @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting - * its children, and will be passed, by reference, the \c DATA object returned by the - * call to \c visitorPre (the \c DATA object may be modified by visiting the children). - * @param rootData The data to pass by reference to \c visitorPre when it is called on each - * root node. */ - template - void DepthFirstForestParallel(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, - int problemSizeThreshold = 10) - { -#ifdef GTSAM_USE_TBB - // Typedefs - typedef typename FOREST::Node Node; - typedef boost::shared_ptr sharedNode; - - tbb::task::spawn_root_and_wait(internal::CreateRootTask( - forest.roots(), rootData, visitorPre, visitorPost, problemSizeThreshold)); -#else - DepthFirstForest(forest, rootData, visitorPre, visitorPost); -#endif - } - - - /* ************************************************************************* */ - /** Traversal function for CloneForest */ - namespace { - template - boost::shared_ptr - CloneForestVisitorPre(const boost::shared_ptr& node, const boost::shared_ptr& parentPointer) - { - // Clone the current node and add it to its cloned parent - boost::shared_ptr clone = boost::make_shared(*node); - clone->children.clear(); - parentPointer->children.push_back(clone); - return clone; - } - } - - /** Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child - * pointers for a clone of the original tree. - * @param forest The forest of trees to clone. The method \c forest.roots() should exist and - * return a collection of shared pointers to \c FOREST::Node. - * @return The new collection of roots. */ - template - FastVector > CloneForest(const FOREST& forest) - { - typedef typename FOREST::Node Node; - boost::shared_ptr rootContainer = boost::make_shared(); - DepthFirstForest(forest, rootContainer, CloneForestVisitorPre); - return FastVector >(rootContainer->children.begin(), rootContainer->children.end()); - } - - - /* ************************************************************************* */ - /** Traversal function for PrintForest */ - namespace { - struct PrintForestVisitorPre - { - const KeyFormatter& formatter; - PrintForestVisitorPre(const KeyFormatter& formatter) : formatter(formatter) {} - template std::string operator()(const boost::shared_ptr& node, const std::string& parentString) - { - // Print the current node - node->print(parentString + "-", formatter); - // Increment the indentation - return parentString + "| "; - } - }; - } - - /** Print a tree, prefixing each line with \c str, and formatting keys using \c keyFormatter. - * To print each node, this function calls the \c print function of the tree nodes. */ - template - void PrintForest(const FOREST& forest, std::string str, const KeyFormatter& keyFormatter) { - PrintForestVisitorPre visitor(keyFormatter); - DepthFirstForest(forest, str, visitor); - } +/* ************************************************************************* */ +namespace { +// Internal node used in DFS preorder stack +template +struct TraversalNode { + bool expanded; + const boost::shared_ptr& treeNode; + DATA& parentData; + typename FastList::iterator dataPointer; + TraversalNode(const boost::shared_ptr& _treeNode, DATA& _parentData) : + expanded(false), treeNode(_treeNode), parentData(_parentData) { } +}; + +// Do nothing - default argument for post-visitor for tree traversal +struct no_op { + template + void operator()(const boost::shared_ptr& node, const DATA& data) { + } +}; + +} + +/** Traverse a forest depth-first with pre-order and post-order visits. + * @param forest The forest of trees to traverse. The method \c forest.roots() should exist + * and return a collection of (shared) pointers to \c FOREST::Node. + * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before + * visiting its children, and will be passed, by reference, the \c DATA object returned + * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object + * to pass to the children. The returned \c DATA object will be copy-constructed only + * upon returning to store internally, thus may be modified by visiting the children. + * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. + * @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting + * its children, and will be passed, by reference, the \c DATA object returned by the + * call to \c visitorPre (the \c DATA object may be modified by visiting the children). + * @param rootData The data to pass by reference to \c visitorPre when it is called on each + * root node. */ +template +void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, + VISITOR_POST& visitorPost) { + // Typedefs + typedef typename FOREST::Node Node; + typedef boost::shared_ptr sharedNode; + + // Depth first traversal stack + typedef TraversalNode TraversalNode; + typedef FastList Stack; + Stack stack; + FastList dataList; // List to store node data as it is returned from the pre-order visitor + + // Add roots to stack (insert such that they are visited and processed in order + { + typename Stack::iterator insertLocation = stack.begin(); + BOOST_FOREACH(const sharedNode& root, forest.roots()) + stack.insert(insertLocation, TraversalNode(root, rootData)); + } + + // Traverse + while (!stack.empty()) { + // Get next node + TraversalNode& node = stack.front(); + + if (node.expanded) { + // If already expanded, then the data stored in the node is no longer needed, so visit + // then delete it. + (void) visitorPost(node.treeNode, *node.dataPointer); + dataList.erase(node.dataPointer); + stack.pop_front(); + } else { + // If not already visited, visit the node and add its children (use reverse iterators so + // children are processed in the order they appear) + node.dataPointer = dataList.insert(dataList.end(), + visitorPre(node.treeNode, node.parentData)); + typename Stack::iterator insertLocation = stack.begin(); + BOOST_FOREACH(const sharedNode& child, node.treeNode->children) + stack.insert(insertLocation, TraversalNode(child, *node.dataPointer)); + node.expanded = true; + } + } + assert(dataList.empty()); +} + +/** Traverse a forest depth-first, with a pre-order visit but no post-order visit. + * @param forest The forest of trees to traverse. The method \c forest.roots() should exist + * and return a collection of (shared) pointers to \c FOREST::Node. + * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before + * visiting its children, and will be passed, by reference, the \c DATA object returned + * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object + * to pass to the children. The returned \c DATA object will be copy-constructed only + * upon returning to store internally, thus may be modified by visiting the children. + * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. + * @param rootData The data to pass by reference to \c visitorPre when it is called on each + * root node. */ +template +void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) { + no_op visitorPost; + DepthFirstForest(forest, rootData, visitorPre, visitorPost); +} + +/** Traverse a forest depth-first with pre-order and post-order visits. + * @param forest The forest of trees to traverse. The method \c forest.roots() should exist + * and return a collection of (shared) pointers to \c FOREST::Node. + * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before + * visiting its children, and will be passed, by reference, the \c DATA object returned + * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object + * to pass to the children. The returned \c DATA object will be copy-constructed only + * upon returning to store internally, thus may be modified by visiting the children. + * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. + * @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting + * its children, and will be passed, by reference, the \c DATA object returned by the + * call to \c visitorPre (the \c DATA object may be modified by visiting the children). + * @param rootData The data to pass by reference to \c visitorPre when it is called on each + * root node. */ +template +void DepthFirstForestParallel(FOREST& forest, DATA& rootData, + VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, + int problemSizeThreshold = 10) { +#ifdef GTSAM_USE_TBB + // Typedefs + typedef typename FOREST::Node Node; + typedef boost::shared_ptr sharedNode; + + tbb::task::spawn_root_and_wait( + internal::CreateRootTask(forest.roots(), rootData, visitorPre, + visitorPost, problemSizeThreshold)); +#else + DepthFirstForest(forest, rootData, visitorPre, visitorPost); +#endif +} + +/* ************************************************************************* */ +/** Traversal function for CloneForest */ +namespace { +template +boost::shared_ptr CloneForestVisitorPre( + const boost::shared_ptr& node, + const boost::shared_ptr& parentPointer) { + // Clone the current node and add it to its cloned parent + boost::shared_ptr clone = boost::make_shared(*node); + clone->children.clear(); + parentPointer->children.push_back(clone); + return clone; +} +} + +/** Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child + * pointers for a clone of the original tree. + * @param forest The forest of trees to clone. The method \c forest.roots() should exist and + * return a collection of shared pointers to \c FOREST::Node. + * @return The new collection of roots. */ +template +FastVector > CloneForest( + const FOREST& forest) { + typedef typename FOREST::Node Node; + boost::shared_ptr rootContainer = boost::make_shared(); + DepthFirstForest(forest, rootContainer, CloneForestVisitorPre); + return FastVector >(rootContainer->children.begin(), + rootContainer->children.end()); +} + +/* ************************************************************************* */ +/** Traversal function for PrintForest */ +namespace { +struct PrintForestVisitorPre { + const KeyFormatter& formatter; + PrintForestVisitorPre(const KeyFormatter& formatter) : + formatter(formatter) { + } + template std::string operator()( + const boost::shared_ptr& node, const std::string& parentString) { + // Print the current node + node->print(parentString + "-", formatter); + // Increment the indentation + return parentString + "| "; + } +}; +} + +/** Print a tree, prefixing each line with \c str, and formatting keys using \c keyFormatter. + * To print each node, this function calls the \c print function of the tree nodes. */ +template +void PrintForest(const FOREST& forest, std::string str, + const KeyFormatter& keyFormatter) { + PrintForestVisitorPre visitor(keyFormatter); + DepthFirstForest(forest, str, visitor); +} +} } diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index 7986f9534..c1df47a01 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -24,6 +24,7 @@ #ifdef GTSAM_USE_TBB # include +# include # undef max // TBB seems to include windows.h and we don't want these macros # undef min # undef ERROR diff --git a/gtsam/base/types.cpp b/gtsam/base/types.cpp deleted file mode 100644 index 3f86dc0c1..000000000 --- a/gtsam/base/types.cpp +++ /dev/null @@ -1,36 +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 types.h - * @brief Typedefs for easier changing of types - * @author Richard Roberts - * @date Aug 21, 2010 - * @addtogroup base - */ - -#include - -#include -#include - -namespace gtsam { - - /* ************************************************************************* */ - std::string _defaultKeyFormatter(Key key) { - const Symbol asSymbol(key); - if(asSymbol.chr() > 0) - return (std::string)asSymbol; - else - return boost::lexical_cast(key); - } - -} \ No newline at end of file diff --git a/gtsam/base/types.h b/gtsam/base/types.h index aca04a14b..84c94e73d 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -20,14 +20,11 @@ #pragma once #include -#include +#include +#include +#include // for GTSAM_USE_TBB #include -#include -#include -#include -#include -#include #ifdef GTSAM_USE_TBB #include @@ -58,22 +55,9 @@ namespace gtsam { /// Integer nonlinear key type typedef size_t Key; - /// Typedef for a function to format a key, i.e. to convert it to a string - typedef boost::function KeyFormatter; - - // Helper function for DefaultKeyFormatter - GTSAM_EXPORT std::string _defaultKeyFormatter(Key key); - - /// The default KeyFormatter, which is used if no KeyFormatter is passed to - /// a nonlinear 'print' function. Automatically detects plain integer keys - /// and Symbol keys. - static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter; - - /// The index type for Eigen objects typedef ptrdiff_t DenseIndex; - /* ************************************************************************* */ /** * Helper class that uses templates to select between two types based on @@ -148,104 +132,6 @@ namespace gtsam { return ListOfOneContainer(element); } - /* ************************************************************************* */ - /// Base exception type that uses tbb_exception if GTSAM is compiled with TBB. - template - class ThreadsafeException : -#ifdef GTSAM_USE_TBB - public tbb::tbb_exception -#else - public std::exception -#endif - { -#ifdef GTSAM_USE_TBB - private: - typedef tbb::tbb_exception Base; - protected: - typedef std::basic_string, tbb::tbb_allocator > String; -#else - private: - typedef std::exception Base; - protected: - typedef std::string String; -#endif - - protected: - bool dynamic_; ///< Whether this object was moved - mutable boost::optional description_; ///< Optional description - - /// Default constructor is protected - may only be created from derived classes - ThreadsafeException() : dynamic_(false) {} - - /// Copy constructor is protected - may only be created from derived classes - ThreadsafeException(const ThreadsafeException& other) : Base(other), dynamic_(false) {} - - /// Construct with description string - ThreadsafeException(const std::string& description) : dynamic_(false), description_(String(description.begin(), description.end())) {} - - /// Default destructor doesn't have the throw() - virtual ~ThreadsafeException() throw () {} - - public: - // Implement functions for tbb_exception -#ifdef GTSAM_USE_TBB - virtual tbb::tbb_exception* move() throw () { - void* cloneMemory = scalable_malloc(sizeof(DERIVED)); - if (!cloneMemory) { - std::cerr << "Failed allocating memory to copy thrown exception, exiting now." << std::endl; - exit(-1); - } - DERIVED* clone = ::new(cloneMemory) DERIVED(static_cast(*this)); - clone->dynamic_ = true; - return clone; - } - - virtual void destroy() throw() { - if (dynamic_) { - DERIVED* derivedPtr = static_cast(this); - derivedPtr->~DERIVED(); - scalable_free(derivedPtr); - } - } - - virtual void throw_self() { - throw *static_cast(this); } - - virtual const char* name() const throw() { - return typeid(DERIVED).name(); } -#endif - - virtual const char* what() const throw() { - return description_ ? description_->c_str() : ""; } - }; - - /* ************************************************************************* */ - /// Threadsafe runtime error exception - class RuntimeErrorThreadsafe : public ThreadsafeException - { - public: - /// Construct with a string describing the exception - RuntimeErrorThreadsafe(const std::string& description) : ThreadsafeException(description) {} - }; - - /* ************************************************************************* */ - /// Threadsafe runtime error exception - class OutOfRangeThreadsafe : public ThreadsafeException - { - public: - /// Construct with a string describing the exception - OutOfRangeThreadsafe(const std::string& description) : ThreadsafeException(description) {} - }; - - /* ************************************************************************* */ - /// Threadsafe invalid argument exception - class InvalidArgumentThreadsafe : public ThreadsafeException - { - public: - /// Construct with a string describing the exception - InvalidArgumentThreadsafe(const std::string& description) : ThreadsafeException(description) {} - }; - /* ************************************************************************* */ #ifdef __clang__ # pragma clang diagnostic push 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/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index de5ec8042..dcc336f89 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -90,7 +90,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); } }; diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 77be1d277..c2128c776 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -39,8 +39,8 @@ namespace gtsam { } /* ************************************************************************* */ - FastSet DiscreteFactorGraph::keys() const { - FastSet keys; + KeySet DiscreteFactorGraph::keys() const { + KeySet keys; BOOST_FOREACH(const sharedFactor& factor, *this) if (factor) keys.insert(factor->begin(), factor->end()); return keys; diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index c5b11adf1..cdfd7d522 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -120,7 +120,7 @@ public: } /** Return the set of variables involved in the factors (set union) */ - FastSet keys() const; + KeySet keys() const; /** return product of all factors as a single factor */ DecisionTreeFactor product() const; diff --git a/gtsam/discrete/Potentials.h b/gtsam/discrete/Potentials.h index 978781d63..1078b4c61 100644 --- a/gtsam/discrete/Potentials.h +++ b/gtsam/discrete/Potentials.h @@ -19,6 +19,7 @@ #include #include +#include #include #include diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h new file mode 100644 index 000000000..27fe2f197 --- /dev/null +++ b/gtsam/geometry/BearingRange.h @@ -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 BearingRange.h + * @date July, 2015 + * @author Frank Dellaert + * @brief Bearing-Range product + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +// Forward declaration of Bearing functor which should be of A1*A2 -> return_type +// For example Bearing(pose,point), defined in Pose3.h will return Unit3 +// At time of writing only Pose2 and Pose3 specialize this functor. +template +struct Bearing; + +// Forward declaration of Range functor which should be of A1*A2 -> return_type +// For example Range(T1,T2), defined in Pose2.h will return double +// At time of writing Pose2, Pose3, and several Camera variants specialize this for several types +template +struct Range; + +/** + * Bearing-Range product for a particular A1,A2 combination will use the functors above to create + * a similar functor of type A1*A2 -> pair + * For example BearingRange(pose,point) will return pair + * and BearingRange(pose,point) will return pair + */ +template +struct BearingRange + : public ProductManifold::result_type, + typename Range::result_type> { + typedef typename Bearing::result_type B; + typedef typename Range::result_type R; + typedef ProductManifold Base; + + BearingRange() {} + BearingRange(const ProductManifold& br) : Base(br) {} + BearingRange(const B& b, const R& r) : Base(b, r) {} + + /// Prediction function that stacks measurements + static BearingRange Measure( + const A1& a1, const A2& a2, + OptionalJacobian::dimension> H1 = boost::none, + OptionalJacobian::dimension> H2 = + boost::none) { + typename MakeJacobian::type HB1; + typename MakeJacobian::type HB2; + typename MakeJacobian::type HR1; + typename MakeJacobian::type HR2; + + B b = Bearing()(a1, a2, H1 ? &HB1 : 0, H2 ? &HB2 : 0); + R r = Range()(a1, a2, H1 ? &HR1 : 0, H2 ? &HR2 : 0); + + if (H1) *H1 << HB1, HR1; + if (H2) *H2 << HB2, HR2; + return BearingRange(b, r); + } + + void print(const std::string& str = "") const { + std::cout << str; + traits::Print(this->first, "bearing "); + traits::Print(this->second, "range "); + } + bool equals(const BearingRange& m2, double tol = 1e-8) const { + return traits::Equals(this->first, m2.first, tol) && + traits::Equals(this->second, m2.second, tol); + } + + private: + /// Serialization function + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("bearing", this->first); + ar& boost::serialization::make_nvp("range", this->second); + } + + friend class boost::serialization::access; +}; + +// Declare this to be both Testable and a Manifold +template +struct traits > + : Testable >, + internal::ManifoldTraits > {}; + +// Helper class for to implement Range traits for classes with a bearing method +// For example, to specialize Bearing to Pose3 and Point3, using Pose3::bearing, it suffices to say +// template <> struct Bearing : HasBearing {}; +// where the third argument is used to indicate the return type +template +struct HasBearing { + typedef RT result_type; + RT operator()( + const A1& a1, const A2& a2, + OptionalJacobian::dimension, traits::dimension> H1, + OptionalJacobian::dimension, traits::dimension> H2) { + return a1.bearing(a2, H1, H2); + } +}; + +// Similar helper class for to implement Range traits for classes with a range method +// For classes with overloaded range methods, such as SimpleCamera, this can even be templated: +// template struct Range : HasRange {}; +template +struct HasRange { + typedef RT result_type; + RT operator()( + const A1& a1, const A2& a2, + OptionalJacobian::dimension, traits::dimension> H1, + OptionalJacobian::dimension, traits::dimension> H2) { + return a1.range(a2, H1, H2); + } +}; + +} // namespace gtsam diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index e90ae32a4..d95c47f7b 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -18,7 +18,6 @@ #pragma once -#include #include namespace gtsam { @@ -159,7 +158,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_); diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 6ae8ec14b..81463ac06 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -18,7 +18,6 @@ #pragma once -#include #include namespace gtsam { @@ -109,7 +108,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("Cal3DS2", boost::serialization::base_object(*this)); diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index d4f4cabe5..cfbdde07c 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -153,7 +153,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_); diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 2127fb200..9982cec47 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -23,7 +23,6 @@ #pragma once #include -#include namespace gtsam { @@ -134,7 +133,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("Cal3Unified", boost::serialization::base_object(*this)); diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index 3ec29bbd2..c131d46f7 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -83,10 +83,21 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal, } /* ************************************************************************* */ -Point2 Cal3_S2::calibrate(const Point2& p) const { - const double u = p.x(), v = p.y(); - return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)), - (1 / fy_) * (v - v0_)); +Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal, + OptionalJacobian<2,2> Dp) const { + const double u = p.x(), v = p.y(); + double delta_u = u - u0_, delta_v = v - v0_; + double inv_fx = 1/ fx_, inv_fy = 1/fy_; + double inv_fy_delta_v = inv_fy * delta_v, inv_fx_s_inv_fy = inv_fx * s_ * inv_fy; + Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), + inv_fy_delta_v); + if(Dcal) + *Dcal << - inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v, -inv_fx * point.y(), + -inv_fx, inv_fx_s_inv_fy, + 0, -inv_fy * point.y(), 0, 0, -inv_fy; + if(Dp) + *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy; + return point; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index b9e2ef581..4e62ca7e9 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -21,7 +21,6 @@ #pragma once -#include #include namespace gtsam { @@ -157,9 +156,12 @@ public: /** * convert image coordinates uv to intrinsic coordinates xy * @param p point in image coordinates + * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in intrinsic coordinates */ - Point2 calibrate(const Point2& p) const; + Point2 calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, + OptionalJacobian<2,2> Dp = boost::none) const; /** * convert homogeneous image coordinates to intrinsic coordinates @@ -211,7 +213,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_); diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 651a7fabb..365a6c40e 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -144,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_); diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 1f5f1f8a5..2d27b4dc7 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -19,88 +19,168 @@ #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, - OptionalJacobian<2,3> H1) { - if (H1) { - double d = 1.0 / P.z(), d2 = d * d; - *H1 << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2; +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(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); +} + +/* ************************************************************************* */ +Point2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) { + if (Dpoint) { + Matrix32 Dpoint3_pc; + Matrix23 Duv_point3; + Point2 uv = Project(pc.point3(Dpoint3_pc), Duv_point3); + *Dpoint = Duv_point3 * Dpoint3_pc; + return uv; + } else + return Project(pc.point3()); +} + +/* ************************************************************************* */ +pair PinholeBase::projectSafe(const Point3& pw) const { + const Point3 pc = pose().transform_to(pw); + const Point2 pn = Project(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(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; +} + +/* ************************************************************************* */ +Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, 2> Dpoint) const { + + // world to camera coordinate + Matrix23 Dpc_rot; + Matrix2 Dpc_point; + const Unit3 pc = pose().rotation().unrotate(pw, Dpose ? &Dpc_rot : 0, + Dpose ? &Dpc_point : 0); + + // camera to normalized image coordinate + Matrix2 Dpn_pc; + const Point2 pn = Project(pc, Dpose || Dpoint ? &Dpn_pc : 0); + + // chain the Jacobian matrices + if (Dpose) { + // only rotation is important + Matrix26 Dpc_pose; + Dpc_pose.setZero(); + Dpc_pose.leftCols<3>() = Dpc_rot; + *Dpose = Dpn_pc * Dpc_pose; // 2x2 * 2x6 + } + if (Dpoint) + *Dpoint = Dpn_pc * Dpc_point; // 2x2 * 2*2 + 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, - OptionalJacobian<2,6> Dpose, OptionalJacobian<2,3> Dpoint) const { - -#ifdef CALIBRATEDCAMERA_CHAIN_RULE - Matrix36 Dpose_; - Matrix3 Dpoint_; - Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0); -#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 - if(Dpose && Dpoint) { - Matrix23 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 << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v), - -uv, -u, 0., -d, d * v; - if (Dpoint) { - const Matrix3 R(pose_.rotation().matrix()); - Matrix23 Dpoint_; - Dpoint_ << 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); - *Dpoint = d * Dpoint_; - } -#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 9e907f1d5..b1e5917b2 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -18,11 +18,18 @@ #pragma once +#include #include -#include +#include +#include +#include +#include +#include namespace gtsam { +class Point2; + class GTSAM_EXPORT CheiralityException: public ThreadsafeException< CheiralityException> { public: @@ -31,6 +38,199 @@ public: } }; +/** + * A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT PinholeBase { + +public: + + /** Pose Concept requirements */ + typedef Rot3 Rotation; + typedef Point3 Translation; + + /** + * Some classes template on either PinholeCamera or StereoCamera, + * and this typedef informs those classes what "project" returns. + */ + typedef Point2 Measurement; + +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_; + } + + /// get rotation + const Rot3& rotation() const { + return pose_.rotation(); + } + + /// get translation + const Point3& translation() const { + return pose_.translation(); + } + + /// 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(const Point3& pc, // + OptionalJacobian<2, 3> Dpoint = boost::none); + + /** + * Project from 3D point at infinity 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(const Unit3& pc, // + OptionalJacobian<2, 2> 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; + + /** Project point at infinity 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 Unit3& point, + OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 2> 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); + + /// @} + /// @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); + } + + /// @} + +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 @@ -38,13 +238,13 @@ public: * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT CalibratedCamera { -private: - Pose3 pose_; // 6DOF pose +class GTSAM_EXPORT CalibratedCamera: public PinholeBase { public: - enum { dimension = 6 }; + enum { + dimension = 6 + }; /// @name Standard Constructors /// @{ @@ -54,26 +254,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) { } /// @} @@ -84,19 +298,6 @@ public: virtual ~CalibratedCamera() { } - /// return pose - inline const Pose3& pose() const { - return pose_; - } - - /** - * 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 /// @{ @@ -107,105 +308,93 @@ 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 + /// @name Transformations and measurement 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, - OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 3> 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, - OptionalJacobian<2, 3> 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, OptionalJacobian<1, 6> H1 = boost::none, - OptionalJacobian<1, 3> 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, OptionalJacobian<1, 6> H1 = boost::none, - OptionalJacobian<1, 6> 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, OptionalJacobian<1, 6> H1 = - boost::none, OptionalJacobian<1, 6> 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)); } /// @} }; -template<> +// manifold traits +template <> struct traits : public internal::Manifold {}; -template<> +template <> struct traits : public internal::Manifold {}; -} +// range traits, used in RangeFactor +template +struct Range : HasRange {}; + +} // namespace gtsam diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h new file mode 100644 index 000000000..3208c6555 --- /dev/null +++ b/gtsam/geometry/CameraSet.h @@ -0,0 +1,340 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#include +#include + +namespace gtsam { + +/** + * @brief A set of cameras, all with their own 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 D = traits::dimension; ///< Camera dimension + static const int ZDim = traits::dimension; ///< Measurement dimension + + /// Make a vector of re-projection errors + static Vector ErrorVector(const std::vector& predicted, + const std::vector& measured) { + + // Check size + size_t m = predicted.size(); + if (measured.size() != m) + throw std::runtime_error("CameraSet::errors: size mismatch"); + + // Project and fill error vector + Vector b(ZDim * m); + for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { + Z e = predicted[i] - measured[i]; + b.segment(row) = e.vector(); + } + return b; + } + +public: + + /// Definitions for blocks of F + typedef Eigen::Matrix MatrixZD; + typedef std::vector FBlocks; + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + virtual 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(s); + } + + /// equals + 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 (possibly Unit3 at infinity), with derivatives + * Note that F is a sparse block-diagonal matrix, so instead of a large dense + * matrix this function returns the diagonal blocks. + * throws CheiralityException + */ + template + std::vector project2(const POINT& point, // + boost::optional Fs = boost::none, // + boost::optional E = boost::none) const { + + static const int N = FixedDimension::value; + + // Allocate result + size_t m = this->size(); + std::vector z(m); + + // Allocate derivatives + if (E) + E->resize(ZDim * m, N); + if (Fs) + Fs->resize(m); + + // Project and fill derivatives + for (size_t i = 0; i < m; i++) { + MatrixZD Fi; + Eigen::Matrix Ei; + z[i] = this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0); + if (Fs) + (*Fs)[i] = Fi; + if (E) + E->block(ZDim * i, 0) = Ei; + } + + return z; + } + + /// Calculate vector [project2(point)-z] of re-projection errors + template + Vector reprojectionError(const POINT& point, const std::vector& measured, + boost::optional Fs = boost::none, // + boost::optional E = boost::none) const { + return ErrorVector(project2(point, Fs, E), measured); + } + + /** + * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix + * G = F' * F - F' * E * P * E' * F + * g = F' * (b - E * P * E' * b) + * Fixed size version + */ + template // N = 2 or 3 + static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs, + const Matrix& E, const Eigen::Matrix& P, const Vector& b) { + + // a single point is observed in m cameras + size_t m = Fs.size(); + + // Create a SymmetricBlockMatrix + size_t M1 = D * m + 1; + std::vector dims(m + 1); // this also includes the b term + std::fill(dims.begin(), dims.end() - 1, D); + dims.back() = 1; + SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); + + // Blockwise Schur complement + for (size_t i = 0; i < m; i++) { // for each camera + + const MatrixZD& Fi = Fs[i]; + const Eigen::Matrix Ei_P = // + E.block(ZDim * i, 0, ZDim, N) * P; + + // D = (Dx2) * ZDim + augmentedHessian(i, m) = Fi.transpose() * b.segment(ZDim * i) // F' * b + - Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) + + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) + augmentedHessian(i, i) = Fi.transpose() + * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi); + + // upper triangular part of the hessian + for (size_t j = i + 1; j < m; j++) { // for each camera + const MatrixZD& Fj = Fs[j]; + + // (DxD) = (Dx2) * ( (2x2) * (2xD) ) + augmentedHessian(i, j) = -Fi.transpose() + * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj); + } + } // end of for over cameras + + augmentedHessian(m, m)(0, 0) += b.squaredNorm(); + return augmentedHessian; + } + + /// Computes Point Covariance P, with lambda parameter + template // N = 2 or 3 + static void ComputePointCovariance(Eigen::Matrix& P, + const Matrix& E, double lambda, bool diagonalDamping = false) { + + Matrix EtE = E.transpose() * E; + + if (diagonalDamping) { // diagonal of the hessian + EtE.diagonal() += lambda * EtE.diagonal(); + } else { + DenseIndex n = E.cols(); + EtE += lambda * Eigen::MatrixXd::Identity(n, n); + } + + P = (EtE).inverse(); + } + + /// Computes Point Covariance P, with lambda parameter, dynamic version + static Matrix PointCov(const Matrix& E, const double lambda = 0.0, + bool diagonalDamping = false) { + if (E.cols() == 2) { + Matrix2 P2; + ComputePointCovariance(P2, E, lambda, diagonalDamping); + return P2; + } else { + Matrix3 P3; + ComputePointCovariance(P3, E, lambda, diagonalDamping); + return P3; + } + } + + /** + * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix + * Dynamic version + */ + static SymmetricBlockMatrix SchurComplement(const FBlocks& Fblocks, + const Matrix& E, const Vector& b, const double lambda = 0.0, + bool diagonalDamping = false) { + SymmetricBlockMatrix augmentedHessian; + if (E.cols() == 2) { + Matrix2 P; + ComputePointCovariance(P, E, lambda, diagonalDamping); + augmentedHessian = SchurComplement(Fblocks, E, P, b); + } else { + Matrix3 P; + ComputePointCovariance(P, E, lambda, diagonalDamping); + augmentedHessian = SchurComplement(Fblocks, E, P, b); + } + return augmentedHessian; + } + + /** + * 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. + */ + template // N = 2 or 3 + static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E, + const Eigen::Matrix& P, const Vector& b, + const FastVector& allKeys, const FastVector& keys, + /*output ->*/SymmetricBlockMatrix& augmentedHessian) { + + assert(keys.size()==Fs.size()); + assert(keys.size()<=allKeys.size()); + + FastMap KeySlotMap; + for (size_t slot = 0; slot < allKeys.size(); slot++) + KeySlotMap.insert(std::make_pair(allKeys[slot], slot)); + + // Schur complement trick + // G = F' * F - F' * E * P * E' * F + // g = F' * (b - E * P * E' * b) + + Eigen::Matrix matrixBlock; + + // a single point is observed in m cameras + size_t m = Fs.size(); // cameras observing current point + size_t M = (augmentedHessian.rows() - 1) / D; // all cameras in the group + assert(allKeys.size()==M); + + // Blockwise Schur complement + for (size_t i = 0; i < m; i++) { // for each camera in the current factor + + const MatrixZD& Fi = Fs[i]; + const Eigen::Matrix Ei_P = E.template block( + ZDim * i, 0) * P; + + // 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_i = this->keys_[i]; + DenseIndex aug_i = KeySlotMap.at(keys[i]); + + // information vector - store previous vector + // vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal(); + // add contribution of current factor + augmentedHessian(aug_i, M) = augmentedHessian(aug_i, M).knownOffDiagonal() + + Fi.transpose() * b.segment(ZDim * i) // F' * b + - Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) + + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) + // main block diagonal - store previous block + matrixBlock = augmentedHessian(aug_i, aug_i); + // add contribution of current factor + augmentedHessian(aug_i, aug_i) = matrixBlock + + (Fi.transpose() + * (Fi - Ei_P * E.template block(ZDim * i, 0).transpose() * Fi)); + + // upper triangular part of the hessian + for (size_t j = i + 1; j < m; j++) { // for each camera + const MatrixZD& Fj = Fs[j]; + + DenseIndex aug_j = KeySlotMap.at(keys[j]); + + // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) ) + // off diagonal block - store previous block + // matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal(); + // add contribution of current factor + augmentedHessian(aug_i, aug_j) = + augmentedHessian(aug_i, aug_j).knownOffDiagonal() + - Fi.transpose() + * (Ei_P * E.template block(ZDim * j, 0).transpose() * Fj); + } + } // end of for over cameras + + augmentedHessian(M, M)(0, 0) += b.squaredNorm(); + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & (*this); + } +}; + +template +const int CameraSet::D; + +template +const int CameraSet::ZDim; + +template +struct traits > : public Testable > { +}; + +template +struct traits > : public Testable > { +}; + +} // \ namespace gtsam diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index 2c883707f..15d8154b8 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -16,10 +16,8 @@ **/ #include -#include -#include -#include -#include +#include +#include // for cout :-( namespace gtsam { @@ -33,10 +31,11 @@ public: i_(i) { assert(i < N); } - /// Idenity element - static Cyclic Identity() { - return Cyclic(0); + /// Default constructor yields identity + Cyclic():i_(0) { } + static Cyclic identity() { return Cyclic();} + /// Cast to size_t operator size_t() const { return i_; @@ -63,20 +62,10 @@ public: } }; -/// Define cyclic group traits to be a model of the Group concept +/// Define cyclic group to be a model of the Additive Group concept template -struct traits > { - typedef group_tag structure_category;GTSAM_ADDITIVE_GROUP(Cyclic) - static Cyclic Identity() { - return Cyclic::Identity(); - } - static bool Equals(const Cyclic& a, const Cyclic& b, - double tol = 1e-9) { - return a.equals(b, tol); - } - static void Print(const Cyclic& c, const std::string &s = "") { - c.print(s); - } +struct traits > : internal::AdditiveGroupTraits >, // + Testable > { }; } // \namespace gtsam diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 062178fea..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_, +EssentialMatrix EssentialMatrix::FromPose3(const Pose3& aPb, OptionalJacobian<5, 6> H) { - const Rot3& _1R2_ = _1P2_.rotation(); - const Point3& _1T2_ = _1P2_.translation(); + 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 Matrix23 D_direction_1T2; - Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2); + Unit3 direction = Unit3::FromPoint3(aTb, D_direction_1T2); *H << I_3x3, Z_3x3, // - Matrix23::Zero(), D_direction_1T2 * _1R2_.matrix(); - return EssentialMatrix(_1R2_, direction); + 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: "); -} - -/* ************************************************************************* */ -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); -} - -/* ************************************************************************* */ -Vector5 EssentialMatrix::localCoordinates(const EssentialMatrix& other) const { - Vector5 v; - v << aRb_.localCoordinates(other.aRb_), - aTb_.localCoordinates(other.aTb_); - return v; + rotation().print("R:\n"); + direction().print("d: "); } /* ************************************************************************* */ Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE, OptionalJacobian<3, 3> Dpoint) const { - Pose3 pose(aRb_, aTb_.point3()); + 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 Matrix35 H; - H << DE_.block < 3, 3 > (0, 0), -aRb_.transpose() * aTb_.basis(); + H << DE_.block < 3, 3 > (0, 0), -rotation().transpose() * direction().basis(); *DE = H; } return q; @@ -81,17 +63,17 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, 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 Matrix23 D_c1Tc2_cRb; // 2*3 Matrix2 D_c1Tc2_aTb; // 2*2 - Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); + Unit3 c1Tc2 = cRb.rotate(direction(), D_c1Tc2_cRb, D_c1Tc2_aTb); if (HE) *HE << cRb.matrix(), Matrix32::Zero(), // Matrix23::Zero(), D_c1Tc2_aTb; @@ -113,8 +95,8 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // if (H) { // See math.lyx Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB); - Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) - * aTb_.basis(); + 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 606f62f87..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,17 +21,18 @@ 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 -public: + /// Construct from Base + EssentialMatrix(const Base& base) : + Base(base), E_(direction().skew() * rotation().matrix()) { + } - enum { dimension = 5 }; +public: /// Static function to convert Point2 to homogeneous coordinates static Vector3 Homogeneous(const Point2& p) { @@ -42,12 +44,12 @@ 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) @@ -72,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); } /// @} @@ -80,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 - size_t dim() const { - return 5; - } + using Base::dimension; + using Base::dim; + using Base::Dim; /// Retract delta to manifold - EssentialMatrix retract(const Vector& xi) const; + EssentialMatrix retract(const TangentVector& v) const { + return Base::retract(v); + } /// Compute the coordinates in the tangent space - Vector5 localCoordinates(const EssentialMatrix& other) const; - + TangentVector localCoordinates(const EssentialMatrix& other) const { + return Base::localCoordinates(other); + } /// @} /// @name Essential matrix methods @@ -103,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 @@ -118,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()); } /** @@ -179,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)); @@ -195,7 +195,6 @@ private: } /// @} - }; template<> diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 088a84243..95290497d 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -13,6 +13,7 @@ * @file OrientedPlane3.cpp * @date Dec 19, 2013 * @author Alex Trevor + * @author Zhaoyang Lv * @brief A plane, represented by a normal direction and perpendicular distance */ @@ -25,79 +26,54 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -/// The print fuction -void OrientedPlane3::print(const std::string& s) const { - Vector coeffs = planeCoefficients(); +void OrientedPlane3::print(const string& s) const { + Vector4 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); +OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> Hp, + OptionalJacobian<3, 6> Hr) const { + Matrix23 D_rotated_plane; + Matrix22 D_rotated_pose; + Unit3 n_rotated = xr.rotation().unrotate(n_, D_rotated_plane, D_rotated_pose); - 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); + Vector3 unit_vec = n_rotated.unitVector(); + double pred_d = n_.unitVector().dot(xr.translation().vector()) + d_; if (Hr) { - *Hr = gtsam::zeros(3, 6); - (*Hr).block<2, 3>(0, 0) = n_hr; - (*Hr).block<1, 3>(2, 3) = unit_vec; + *Hr = zeros(3, 6); + Hr->block<2, 3>(0, 0) = D_rotated_plane; + 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; + Vector2 hpp = n_.basis().transpose() * xr.translation().vector(); + *Hp = Z_3x3; + Hp->block<2, 2>(0, 0) = D_rotated_pose; + Hp->block<1, 2>(2, 0) = hpp; (*Hp)(2, 2) = 1; } - return (transformed_plane); + return OrientedPlane3(unit_vec(0), unit_vec(1), unit_vec(2), pred_d); } + /* ************************************************************************* */ -Vector3 OrientedPlane3::error(const gtsam::OrientedPlane3& plane) const { +Vector3 OrientedPlane3::error(const 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); + return Vector3(n_error(0), n_error(1), d_ - plane.d_); } /* ************************************************************************* */ -OrientedPlane3 OrientedPlane3::retract(const Vector& 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); +OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const { + return OrientedPlane3(n_.retract(Vector2(v(0), v(1))), d_ + v(2)); } /* ************************************************************************* */ Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const { - Vector n_local = n_.localCoordinates(y.n_); + 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; + return Vector3(n_local(0), n_local(1), -d_local); } -/* ************************************************************************* */ -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 index 28b67cb4e..5c9a5cdef 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -14,6 +14,7 @@ * @date Dec 19, 2013 * @author Alex Trevor * @author Frank Dellaert + * @author Zhaoyang Lv * @brief An infinite plane, represented by a normal direction and perpendicular distance */ @@ -22,47 +23,57 @@ #include #include #include -#include namespace gtsam { -/// Represents an infinite plane in 3D. -class OrientedPlane3: public DerivedValue { +/** + * @brief Represents an infinite plane in 3D, which is composed of a planar normal and its + * perpendicular distance to the origin. + * Currently it provides a transform of the plane, and a norm 1 differencing of two planes. + * Refer to Trevor12iros for more math details. + */ +class GTSAM_EXPORT OrientedPlane3 { private: - Unit3 n_; /// The direction of the planar normal - double d_; /// The perpendicular distance to this plane + 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) { + n_(), d_(0.0) { } /// Construct from a Unit3 and a distance OrientedPlane3(const Unit3& s, double d) : - n_(s), d_(d) { + n_(s), d_(d) { } - OrientedPlane3(Vector vec) : - n_(vec(0), vec(1), vec(2)), d_(vec(3)) { + /// Construct from a vector of plane coefficients + OrientedPlane3(const Vector4& vec) : + n_(vec(0), vec(1), vec(2)), d_(vec(3)) { } - /// Construct from a, b, c, d + /// Construct from four numbers of plane coeffcients (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 fuction + /// @} + /// @name Testable + /// @{ + + /// The print function void print(const std::string& s = std::string()) const; /// The equals function with tolerance @@ -70,13 +81,38 @@ public: 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; + /** Transforms a plane to the specified pose + * @param xr a transformation in current coordiante + * @param Hp optional Jacobian wrpt the destination plane + * @param Hr optional jacobian wrpt the pose transformation + * @return the transformed plane + */ + OrientedPlane3 transform(const Pose3& xr, + OptionalJacobian<3, 3> Hp = boost::none, + OptionalJacobian<3, 6> Hr = boost::none) const; + + /** + * @deprecated the static method has wrong Jacobian order, + * please use the member method transform() + * @param The raw plane + * @param xr a transformation in current coordiante + * @param Hr optional jacobian wrpt the pose transformation + * @param Hp optional Jacobian wrpt the destination plane + * @return the transformed plane + */ + static OrientedPlane3 Transform(const OrientedPlane3& plane, + const Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none, + OptionalJacobian<3, 3> Hp = boost::none) { + return plane.transform(xr, Hp, Hr); + } + + /** Computes the error between two planes. + * The error is a norm 1 difference in tangent space. + * @param the other plane + */ + Vector3 error(const OrientedPlane3& plane) const; /// Dimensionality of tangent space = 3 DOF inline static size_t Dim() { @@ -89,27 +125,36 @@ public: } /// The retract function - OrientedPlane3 retract(const Vector& v) const; + 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; + /// Returns the plane coefficients + inline Vector4 planeCoefficients() const { + Vector3 unit_vec = n_.unitVector(); + return Vector4(unit_vec[0], unit_vec[1], unit_vec[2], d_); + } + /// Return the normal inline Unit3 normal() const { return n_; } + /// Return the perpendicular distance to the origin + inline double distance() const { + return d_; + } + /// @} }; template<> struct traits : public internal::Manifold< - OrientedPlane3> { +OrientedPlane3> { }; template<> struct traits : public internal::Manifold< - OrientedPlane3> { +OrientedPlane3> { }; } // namespace gtsam diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 46df47ecb..6177dec95 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -18,32 +18,33 @@ #pragma once -#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 { +class GTSAM_EXPORT PinholeCamera: public PinholeBaseK { private: - Pose3 pose_; - Calibration K_; - GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration) + 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 }; + enum { + dimension = 6 + DimK + }; ///< Dimension depends on calibration /// @name Standard Constructors /// @{ @@ -54,12 +55,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) { } /// @} @@ -75,12 +76,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 @@ -99,28 +95,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(6)); - if (v.size() > 6) { - K_ = Calibration(v.tail(DimK)); - } + /// 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) { } /// @} @@ -128,14 +119,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"); } @@ -147,31 +138,21 @@ public: } /// return pose - inline Pose3& pose() { - return pose_; - } - - /// return pose, constant version - inline const Pose3& pose() const { - return pose_; + const Pose3& pose() const { + return Base::pose(); } /// return pose, with derivative - inline const Pose3& getPose(gtsam::OptionalJacobian<6, dimension> H) const { + const Pose3& getPose(OptionalJacobian<6, dimension> H) const { if (H) { H->setZero(); - H->block(0, 0, 6, 6) = I_6x6; + H->template block<6, 6>(0, 0) = I_6x6; } - return pose_; + return Base::pose(); } /// return calibration - inline Calibration& calibration() { - return K_; - } - - /// return calibration - inline const Calibration& calibration() const { + const Calibration& calibration() const { return K_; } @@ -179,13 +160,13 @@ public: /// @name Manifold /// @{ - /// Manifold dimension - inline size_t dim() const { + /// @deprecated + size_t dim() const { return dimension; } - /// Manifold dimension - inline static size_t Dim() { + /// @deprecated + static size_t Dim() { return dimension; } @@ -194,18 +175,17 @@ public: /// move a cameras according to d PinholeCamera retract(const Vector& d) const { if ((size_t) d.size() == 6) - return PinholeCamera(pose().retract(d), calibration()); + return PinholeCamera(this->pose().retract(d), calibration()); else - return PinholeCamera(pose().retract(d.head(6)), + return PinholeCamera(this->pose().retract(d.head<6>()), calibration().retract(d.tail(calibration().dim()))); } /// 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()); - d.tail(DimK) = calibration().localCoordinates(T2.calibration()); + d.template head<6>() = this->pose().localCoordinates(T2.pose()); + d.template tail() = calibration().localCoordinates(T2.calibration()); return d; } @@ -218,170 +198,45 @@ public: /// @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 - */ - static Point2 project_to_camera(const Point3& P, // - OptionalJacobian<2, 3> 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; - if (Dpoint) - *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; - return Point2(u, v); - } - - /// 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); - } - 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 + /** Templated projection of a 3D point or a point at infinity into the image + * @param pw either a Point3 or a Unit3, in world coordinates */ - inline Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = - boost::none, OptionalJacobian<2, 3> Dpoint = boost::none, - OptionalJacobian<2, DimK> Dcal = boost::none) 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); - } - - /** project a point at infinity from world coordinate to the image - * @param pw is a point in the world coordinate (it is pw = lambda*[pw_x pw_y pw_z] with lambda->inf) - * @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 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 - return K_.uncalibrate(pn); - } - - // world to camera coordinate - Matrix3 Dpc_rot, Dpc_point; - const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point); - - 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); - - // uncalibration - Matrix2 Dpi_pn; // 2*2 - const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); - - // chain the Jacobian matrices - const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc; - if (Dpose) - *Dpose = Dpi_pc * Dpc_pose; - if (Dpoint) - *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only) + template + Point2 _project2(const POINT& pw, OptionalJacobian<2, dimension> Dcamera, + OptionalJacobian<2, FixedDimension::value> Dpoint) const { + // We just call 3-derivative version in Base + Matrix26 Dpose; + Eigen::Matrix Dcal; + Point2 pi = Base::project(pw, Dcamera ? &Dpose : 0, Dpoint, + Dcamera ? &Dcal : 0); + if (Dcamera) + *Dcamera << Dpose, Dcal; return pi; } - /** 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, // - 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); - - 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) { // TODO why does leftCols<6>() not compile ?? - calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6)); - (*Dcamera).rightCols(DimK) = Dcal; // Jacobian wrt calib - } - if (Dpoint) { - calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); - } - return pi; - } + /// project a 3D point from world coordinates into the image + Point2 project2(const Point3& pw, OptionalJacobian<2, dimension> Dcamera = + boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { + return _project2(pw, Dcamera, Dpoint); } - /// 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); + /// project a point at infinity from world coordinates into the image + Point2 project2(const Unit3& pw, OptionalJacobian<2, dimension> Dcamera = + boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const { + return _project2(pw, Dcamera, Dpoint); } /** * Calculate range to a landmark * @param point 3D location of landmark - * @param Dcamera 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, // - OptionalJacobian<1, dimension> Dcamera = boost::none, - OptionalJacobian<1, 3> Dpoint = boost::none) const { + double range(const Point3& point, OptionalJacobian<1, dimension> Dcamera = + boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const { Matrix16 Dpose_; - double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint); + double result = this->pose().range(point, Dcamera ? &Dpose_ : 0, Dpoint); if (Dcamera) *Dcamera << Dpose_, Eigen::Matrix::Zero(); return result; @@ -390,16 +245,12 @@ public: /** * Calculate range to another pose * @param pose Other SO(3) pose - * @param Dcamera 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, // - OptionalJacobian<1, dimension> Dcamera = boost::none, - OptionalJacobian<1, 6> Dpose = boost::none) const { + double range(const Pose3& pose, OptionalJacobian<1, dimension> Dcamera = + boost::none, OptionalJacobian<1, 6> Dpose = boost::none) const { Matrix16 Dpose_; - double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose); + double result = this->pose().range(pose, Dcamera ? &Dpose_ : 0, Dpose); if (Dcamera) *Dcamera << Dpose_, Eigen::Matrix::Zero(); return result; @@ -408,41 +259,31 @@ public: /** * Calculate range to another camera * @param camera Other camera - * @param Dcamera 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, // + double range(const PinholeCamera& camera, OptionalJacobian<1, dimension> Dcamera = boost::none, -// OptionalJacobian<1, 6 + traits::dimension::value> Dother = - boost::optional Dother = - boost::none) const { + OptionalJacobian<1, 6 + CalibrationB::dimension> Dother = boost::none) const { Matrix16 Dcamera_, Dother_; - double result = pose_.range(camera.pose(), Dcamera ? &Dcamera_ : 0, + 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) { - Dother->resize(1, 6+CalibrationB::dimension); Dother->setZero(); - Dother->block(0, 0, 1, 6) = Dother_; + Dother->template block<1, 6>(0, 0) = Dother_; } return result; } /** - * Calculate range to another camera + * Calculate range to a calibrated camera * @param camera Other camera - * @param Dcamera 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, // + double range(const CalibratedCamera& camera, OptionalJacobian<1, dimension> Dcamera = boost::none, OptionalJacobian<1, 6> Dother = boost::none) const { return range(camera.pose(), Dcamera, Dother); @@ -450,65 +291,30 @@ public: 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_); } }; +// manifold traits -template -struct traits< PinholeCamera > : public internal::Manifold > {}; +template +struct traits > + : public internal::Manifold > {}; -template -struct traits< const PinholeCamera > : public internal::Manifold > {}; +template +struct traits > + : public internal::Manifold > {}; -} // \ gtsam +// range traits, used in RangeFactor +template +struct Range, T> : HasRange, T, double> {}; + +} // \ gtsam diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h new file mode 100644 index 000000000..ac453e048 --- /dev/null +++ b/gtsam/geometry/PinholePose.h @@ -0,0 +1,408 @@ +/* ---------------------------------------------------------------------------- + + * 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 { + +private: + + GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION); + + // Get dimensions of calibration type at compile time + static const int DimK = FixedDimension::value; + +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 + * @param pw is a point in the world coordinates + */ + Point2 project(const Point3& pw) const { + const Point2 pn = PinholeBase::project2(pw); // project to normalized coordinates + return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates + } + + /** project a point from world coordinate to the image + * @param pw is a point at infinity in the world coordinates + */ + Point2 project(const Unit3& pw) const { + const Unit3 pc = pose().rotation().unrotate(pw); // convert to camera frame + const Point2 pn = PinholeBase::Project(pc); // project to normalized coordinates + return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates + } + + /** Templated projection of a point (possibly at infinity) from world coordinate to the image + * @param pw is a 3D point or aUnit3 (point at infinity) 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 + */ + template + Point2 _project(const POINT& pw, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, FixedDimension::value> Dpoint, + OptionalJacobian<2, DimK> Dcal) 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 = Dpi_pn * *Dpoint; + + return pi; + } + + /// project a 3D point from world coordinates into the image + Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, 3> Dpoint = boost::none, + OptionalJacobian<2, DimK> Dcal = boost::none) const { + return _project(pw, Dpose, Dpoint, Dcal); + } + + /// project a point at infinity from world coordinates into the image + Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, 2> Dpoint = boost::none, + OptionalJacobian<2, DimK> Dcal = boost::none) const { + return _project(pw, Dpose, Dpoint, Dcal); + } + + /// 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 + Unit3 backprojectPointAtInfinity(const Point2& p) const { + const Point2 pn = calibration().calibrate(p); + const Unit3 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_; + } + + /** project a point from world coordinate to the image, 2 derivatives only + * @param pw is a point in world coordinates + * @param Dpose is the Jacobian w.r.t. the whole camera (really only the pose) + * @param Dpoint is the Jacobian w.r.t. point3 + */ + Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 3> Dpoint = boost::none) const { + return Base::project(pw, Dpose, Dpoint); + } + + /// project2 version for point at infinity + Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 2> Dpoint = boost::none) const { + return Base::project(pw, Dpose, Dpoint); + } + + /// @} + /// @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/PinholeSet.h b/gtsam/geometry/PinholeSet.h new file mode 100644 index 000000000..5101e9fc8 --- /dev/null +++ b/gtsam/geometry/PinholeSet.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 PinholeSet.h + * @brief A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera + * @author Frank Dellaert + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** + * PinholeSet: triangulates point and keeps an estimate of it around. + */ +template +class PinholeSet: public CameraSet { + +private: + typedef CameraSet Base; + typedef PinholeSet This; + +protected: + +public: + + /** Virtual destructor */ + virtual ~PinholeSet() { + } + + /// @name Testable + /// @{ + + /// print + virtual void print(const std::string& s = "") const { + Base::print(s); + } + + /// equals + bool equals(const PinholeSet& p, double tol = 1e-9) const { + return Base::equals(p, tol); // TODO all flags + } + + /// @} + + /// triangulateSafe + TriangulationResult triangulateSafe( + const std::vector& measured, + const TriangulationParameters& params) const { + return gtsam::triangulateSafe(*this, measured, params); + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } +}; + +template +struct traits > : public Testable > { +}; + +template +struct traits > : public Testable > { +}; + +} // \ namespace gtsam diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 6f4f93cf9..56809ae53 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -148,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_;} @@ -185,16 +185,18 @@ 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;} diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index a87aeb650..bffda9fef 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -58,6 +58,22 @@ Point3 Point3::operator/(double s) const { return Point3(x_ / s, y_ / s, z_ / s); } +/* ************************************************************************* */ +double Point3::distance(const Point3 &p2, OptionalJacobian<1, 3> H1, + OptionalJacobian<1, 3> H2) const { + double d = (p2 - *this).norm(); + if (H1) { + *H1 << x_ - p2.x(), y_ - p2.y(), z_ - p2.z(); + *H1 = *H1 *(1. / d); + } + + if (H2) { + *H2 << -x_ + p2.x(), -y_ + p2.y(), -z_ + p2.z(); + *H2 = *H2 *(1. / d); + } + return d; +} + /* ************************************************************************* */ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { @@ -75,13 +91,27 @@ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, } /* ************************************************************************* */ -Point3 Point3::cross(const Point3 &q) const { +Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H_p, OptionalJacobian<3, 3> H_q) const { + if (H_p) { + *H_p << skewSymmetric(-q.vector()); + } + if (H_q) { + *H_q << skewSymmetric(vector()); + } + return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_, x_ * q.y_ - y_ * q.x_); } /* ************************************************************************* */ -double Point3::dot(const Point3 &q) const { +double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H_p, OptionalJacobian<1, 3> H_q) const { + if (H_p) { + *H_p << q.vector().transpose(); + } + if (H_q) { + *H_q << vector().transpose(); + } + return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_); } @@ -116,6 +146,12 @@ ostream &operator<<(ostream &os, const Point3& p) { return os; } +/* ************************************************************************* */ +ostream &operator<<(ostream &os, const gtsam::Point3Pair &p) { + os << p.first << " <-> " << p.second; + return os; +} + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 95f728e39..e19b74d1c 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -22,6 +22,7 @@ #pragma once #include +#include #include #include @@ -36,8 +37,8 @@ namespace gtsam { private: - double x_, y_, z_; - + double x_, y_, z_; + public: enum { dimension = 3 }; @@ -55,7 +56,7 @@ namespace gtsam { /// @{ /// Construct from 3-element vector - Point3(const Vector3& v) { + explicit Point3(const Vector3& v) { x_ = v(0); y_ = v(1); z_ = v(2); @@ -81,6 +82,11 @@ namespace gtsam { /// inverse Point3 operator - () const { return Point3(-x_,-y_,-z_);} + /// add vector on right + inline Point3 operator +(const Vector3& v) const { + return Point3(x_ + v[0], y_ + v[1], z_ + v[2]); + } + /// add Point3 operator + (const Point3& q) const; @@ -98,20 +104,8 @@ namespace gtsam { Point3 operator / (double s) const; /** distance between two points */ - inline double distance(const Point3& p2, - OptionalJacobian<1,3> H1 = boost::none, OptionalJacobian<1,3> H2 = boost::none) const { - double d = (p2 - *this).norm(); - if (H1) { - *H1 << x_-p2.x(), y_-p2.y(), z_-p2.z(); - *H1 = *H1 *(1./d); - } - - if (H2) { - *H2 << -x_+p2.x(), -y_+p2.y(), -z_+p2.z(); - *H2 << *H2 *(1./d); - } - return d; - } + double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none, + OptionalJacobian<1, 3> H2 = boost::none) const; /** @deprecated The following function has been deprecated, use distance above */ inline double dist(const Point3& p2) const { @@ -125,17 +119,19 @@ namespace gtsam { Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const; /** cross product @return this x q */ - Point3 cross(const Point3 &q) const; + Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, // + OptionalJacobian<3, 3> H_q = boost::none) const; /** dot product @return this * q*/ - double dot(const Point3 &q) const; + double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, // + OptionalJacobian<1, 3> H_q = boost::none) const; /// @} /// @name Standard Interface /// @{ /// equality - bool operator ==(const Point3& q) const; + bool operator ==(const Point3& q) const; /** return vectorized form (column-wise)*/ Vector3 vector() const { return Vector3(x_,y_,z_); } @@ -181,7 +177,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_); @@ -189,15 +185,33 @@ namespace gtsam { } /// @} - }; - /// Syntactic sugar for multiplying coordinates by a scalar s*p - inline Point3 operator*(double s, const Point3& p) { return p*s;} +// Convenience typedef +typedef std::pair Point3Pair; +std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); - template<> - struct traits : public internal::VectorSpace {}; +/// Syntactic sugar for multiplying coordinates by a scalar s*p +inline Point3 operator*(double s, const Point3& p) { return p*s;} + +template<> +struct traits : public internal::VectorSpace {}; + +template<> +struct traits : public internal::VectorSpace {}; + +template +struct Range; + +template <> +struct Range { + typedef double result_type; + double operator()(const Point3& p, const Point3& q, + OptionalJacobian<1, 3> H1 = boost::none, + OptionalJacobian<1, 3> H2 = boost::none) { + return p.distance(q, H1, H2); + } +}; + +} // namespace gtsam - template<> - struct traits : public internal::VectorSpace {}; -} diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 0b0172857..89f6b3754 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -59,7 +59,7 @@ bool Pose2::equals(const Pose2& q, double tol) const { } /* ************************************************************************* */ -Pose2 Pose2::Expmap(const Vector& xi, OptionalJacobian<3, 3> H) { +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)); @@ -130,7 +130,7 @@ Matrix3 Pose2::AdjointMap() const { } /* ************************************************************************* */ -Matrix3 Pose2::adjointMap(const Vector& v) { +Matrix3 Pose2::adjointMap(const Vector3& v) { // See Chirikjian12book2, vol.2, pg. 36 Matrix3 ad = zeros(3,3); ad(0,1) = -v[2]; @@ -201,95 +201,88 @@ Pose2 Pose2::inverse() const { /* ************************************************************************* */ // see doc/math.lyx, SE(2) section Point2 Pose2::transform_to(const Point2& point, - 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(); + OptionalJacobian<2, 3> Hpose, OptionalJacobian<2, 2> Hpoint) const { + OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0); + OptionalJacobian<2, 1> Hrotation = Hpose.cols<1>(2); + const Point2 q = r_.unrotate(point - t_, Hrotation, Hpoint); + if (Htranslation) *Htranslation << -1.0, 0.0, 0.0, -1.0; return q; } /* ************************************************************************* */ // see doc/math.lyx, SE(2) section -Point2 Pose2::transform_from(const Point2& p, - OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const { - const Point2 q = r_ * p; - if (H1 || H2) { - const Matrix2 R = r_.matrix(); - Matrix21 Drotate1; - Drotate1 << -q.y(), q.x(); - if (H1) *H1 << R, Drotate1; - if (H2) *H2 = R; // R - } +Point2 Pose2::transform_from(const Point2& point, + OptionalJacobian<2, 3> Hpose, OptionalJacobian<2, 2> Hpoint) const { + OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0); + OptionalJacobian<2, 1> Hrotation = Hpose.cols<1>(2); + const Point2 q = r_.rotate(point, Hrotation, Hpoint); + if (Htranslation) *Htranslation = Hpoint ? *Hpoint : r_.matrix(); return q + t_; } /* ************************************************************************* */ Rot2 Pose2::bearing(const Point2& point, - OptionalJacobian<1, 3> H1, OptionalJacobian<1, 2> H2) const { + OptionalJacobian<1, 3> Hpose, OptionalJacobian<1, 2> Hpoint) 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); + Matrix23 D_d_pose; Matrix2 D_d_point; + Point2 d = transform_to(point, Hpose ? &D_d_pose : 0, Hpoint ? &D_d_point : 0); + if (!Hpose && !Hpoint) return Rot2::relativeBearing(d); Matrix12 D_result_d; - Rot2 result = Rot2::relativeBearing(d, D_result_d); - if (H1) *H1 = D_result_d * (D1); - if (H2) *H2 = D_result_d * (D2); + Rot2 result = Rot2::relativeBearing(d, Hpose || Hpoint ? &D_result_d : 0); + if (Hpose) *Hpose = D_result_d * D_d_pose; + if (Hpoint) *Hpoint = D_result_d * D_d_point; return result; } /* ************************************************************************* */ Rot2 Pose2::bearing(const Pose2& pose, - OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const { + OptionalJacobian<1, 3> Hpose, OptionalJacobian<1, 3> Hother) const { Matrix12 D2; - Rot2 result = bearing(pose.t(), H1, H2 ? &D2 : 0); - if (H2) { + Rot2 result = bearing(pose.t(), Hpose, Hother ? &D2 : 0); + if (Hother) { Matrix12 H2_ = D2 * pose.r().matrix(); - *H2 << H2_, Z_1x1; + *Hother << H2_, Z_1x1; } return result; } /* ************************************************************************* */ double Pose2::range(const Point2& point, - OptionalJacobian<1,3> H1, OptionalJacobian<1,2> H2) const { + OptionalJacobian<1,3> Hpose, OptionalJacobian<1,2> Hpoint) const { Point2 d = point - t_; - if (!H1 && !H2) return d.norm(); - Matrix12 H; - double r = d.norm(H); - if (H1) { - Matrix23 H1_; - H1_ << -r_.c(), r_.s(), 0.0, - -r_.s(), -r_.c(), 0.0; - *H1 = H * H1_; + if (!Hpose && !Hpoint) return d.norm(); + Matrix12 D_r_d; + double r = d.norm(D_r_d); + if (Hpose) { + Matrix23 D_d_pose; + D_d_pose << -r_.c(), r_.s(), 0.0, + -r_.s(), -r_.c(), 0.0; + *Hpose = D_r_d * D_d_pose; } - if (H2) *H2 = H; + if (Hpoint) *Hpoint = D_r_d; return r; } /* ************************************************************************* */ double Pose2::range(const Pose2& pose, - OptionalJacobian<1,3> H1, - OptionalJacobian<1,3> H2) const { + OptionalJacobian<1,3> Hpose, + OptionalJacobian<1,3> Hother) const { Point2 d = pose.t() - t_; - if (!H1 && !H2) return d.norm(); - Matrix12 H; - double r = d.norm(H); - if (H1) { - Matrix23 H1_; - H1_ << + if (!Hpose && !Hother) return d.norm(); + Matrix12 D_r_d; + double r = d.norm(D_r_d); + if (Hpose) { + Matrix23 D_d_pose; + D_d_pose << -r_.c(), r_.s(), 0.0, -r_.s(), -r_.c(), 0.0; - *H1 = H * H1_; + *Hpose = D_r_d * D_d_pose; } - if (H2) { - Matrix23 H2_; - H2_ << + if (Hother) { + Matrix23 D_d_other; + D_d_other << pose.r_.c(), -pose.r_.s(), 0.0, pose.r_.s(), pose.r_.c(), 0.0; - *H2 = H * H2_; + *Hother = D_r_d * D_d_other; } return r; } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index d4b647949..31dfb479f 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -20,9 +20,11 @@ #pragma once +#include #include #include #include +#include namespace gtsam { @@ -118,7 +120,7 @@ public: /// @{ ///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$ - static Pose2 Expmap(const Vector& xi, ChartJacobian H = boost::none); + 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 Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none); @@ -128,15 +130,14 @@ public: * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi) */ Matrix3 AdjointMap() const; - inline Vector Adjoint(const Vector& xi) const { - assert(xi.size() == 3); + 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 Vector& v); + static Matrix3 adjointMap(const Vector3& v); /** * wedge for SE(2): @@ -271,7 +272,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_); } @@ -290,11 +291,18 @@ inline Matrix wedge(const Vector& xi) { typedef std::pair Point2Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); -template<> -struct traits : public internal::LieGroupTraits {}; +template <> +struct traits : public internal::LieGroup {}; -template<> -struct traits : public internal::LieGroupTraits {}; +template <> +struct traits : public internal::LieGroup {}; + +// bearing and range traits, used in RangeFactor +template +struct Bearing : HasBearing {}; + +template +struct Range : HasRange {}; } // namespace gtsam diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index be8e1bfed..f373e5ad4 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -32,7 +32,7 @@ GTSAM_CONCEPT_POSE_INST(Pose3); /* ************************************************************************* */ Pose3::Pose3(const Pose2& pose2) : - R_(Rot3::rodriguez(0, 0, pose2.theta())), t_( + R_(Rot3::Rodrigues(0, 0, pose2.theta())), t_( Point3(pose2.x(), pose2.y(), 0)) { } @@ -111,23 +111,21 @@ 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, OptionalJacobian<6, 6> H) { +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)); + Point3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); - double theta = w.norm(); - if (theta < 1e-10) { - static const Rot3 I; - return Pose3(I, v); - } else { - Point3 n(w / theta); // axis unit vector - Rot3 R = Rot3::rodriguez(n.vector(), theta); - double vn = n.dot(v); // translation parallel to n - Point3 n_cross_v = n.cross(v); // points towards axis - Point3 t = (n_cross_v - R * n_cross_v) / theta + vn * n; + Rot3 R = Rot3::Expmap(omega.vector()); + double theta2 = omega.dot(omega); + if (theta2 > std::numeric_limits::epsilon()) { + double omega_v = omega.dot(v); // translation parallel to axis + Point3 omega_cross_v = omega.cross(v); // points towards axis + Point3 t = (omega_cross_v - R * omega_cross_v + omega_v * omega) / theta2; return Pose3(R, t); + } else { + return Pose3(R, v); } } @@ -195,10 +193,10 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) { * 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); + const Vector3 w = xi.head<3>(); + const Vector3 v = xi.tail<3>(); + const Matrix3 V = skewSymmetric(v); + const Matrix3 W = skewSymmetric(w); Matrix3 Q; @@ -217,8 +215,8 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { // 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; + const double sinPhi = sin(phi), cosPhi = cos(phi); + const 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) @@ -236,32 +234,37 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { /* ************************************************************************* */ 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(); + const Vector3 w = xi.head<3>(); + const Matrix3 Jw = Rot3::ExpmapDerivative(w); + const Matrix3 Q = computeQforExpmapDerivative(xi); + Matrix6 J; + J << Jw, Z_3x3, Q, Jw; 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(); + const Vector6 xi = Logmap(pose); + const Vector3 w = xi.head<3>(); + const Matrix3 Jw = Rot3::LogmapDerivative(w); + const Matrix3 Q = computeQforExpmapDerivative(xi); + const Matrix3 Q2 = -Jw*Q*Jw; + Matrix6 J; + J << Jw, Z_3x3, Q2, Jw; 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(); - Matrix14 A14; - A14 << 0.0, 0.0, 0.0, 1.0; + static const Matrix14 A14 = (Matrix14() << 0.0, 0.0, 0.0, 1.0).finished(); Matrix4 mat; - mat << R, T, A14; + mat << R_.matrix(), t_.vector(), A14; return mat; } @@ -275,14 +278,17 @@ Pose3 Pose3::transform_to(const Pose3& pose) const { /* ************************************************************************* */ Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose, OptionalJacobian<3,3> Dpoint) const { + // Only get matrix once, to avoid multiple allocations, + // as well as multiple conversions in the Quaternion case + const Matrix3 R = R_.matrix(); if (Dpose) { - const Matrix3 R = R_.matrix(); - Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); - (*Dpose) << DR, R; + Dpose->leftCols<3>() = R * skewSymmetric(-p.x(), -p.y(), -p.z()); + Dpose->rightCols<3>() = R; } - if (Dpoint) - *Dpoint = R_.matrix(); - return R_ * p + t_; + if (Dpoint) { + *Dpoint = R; + } + return Point3(R * p.vector()) + t_; } /* ************************************************************************* */ @@ -299,42 +305,55 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> 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; } /* ************************************************************************* */ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, - OptionalJacobian<1, 3> H2) const { - if (!H1 && !H2) - return transform_to(point).norm(); - 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; + OptionalJacobian<1, 3> H2) const { + Matrix36 D_local_pose; + Matrix3 D_local_point; + Point3 local = transform_to(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0); + if (!H1 && !H2) { + return local.norm(); + } else { + Matrix13 D_r_local; + const double r = local.norm(D_r_local); + if (H1) *H1 = D_r_local * D_local_pose; + if (H2) *H2 = D_r_local * D_local_point; + return r; } } /* ************************************************************************* */ -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) { - Matrix13 H2_ = D2 * pose.rotation().matrix(); - *H2 << Matrix13::Zero(), H2_; - } +double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> H1, + OptionalJacobian<1, 6> H2) const { + Matrix13 D_local_point; + double r = range(pose.translation(), H1, H2 ? &D_local_point : 0); + if (H2) *H2 << Matrix13::Zero(), D_local_point * pose.rotation().matrix(); return r; } +/* ************************************************************************* */ +Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1, + OptionalJacobian<2, 3> H2) const { + Matrix36 D_local_pose; + Matrix3 D_local_point; + Point3 local = transform_to(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0); + if (!H1 && !H2) { + return Unit3(local); + } else { + Matrix23 D_b_local; + Unit3 b = Unit3::FromPoint3(local, D_b_local); + if (H1) *H1 = D_b_local * D_local_pose; + if (H2) *H2 = D_b_local * D_local_point; + return b; + } +} + /* ************************************************************************* */ boost::optional align(const vector& pairs) { const size_t n = pairs.size(); @@ -342,25 +361,25 @@ 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 - Matrix3 H = Eigen::Matrix3d::Zero(); - 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; + Matrix U, V; Vector S; svd(H, U, S, V); @@ -368,7 +387,7 @@ boost::optional align(const vector& pairs) { Matrix3 UVtranspose = U * V.transpose(); Matrix3 detWeighting = I_3x3; detWeighting(2, 2) = UVtranspose.determinant(); - Rot3 R(Matrix(V * detWeighting * U.transpose())); + Rot3 R(Matrix3(V * detWeighting * U.transpose())); Point3 t = Point3(cq) - R * Point3(cp); return Pose3(R, t); } diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 4130a252e..f82e25424 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -19,6 +19,7 @@ #include +#include #include #include #include @@ -110,7 +111,7 @@ public: /// @{ /// 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, OptionalJacobian<6, 6> H = boost::none); + 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); @@ -125,7 +126,7 @@ public: * 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 { + Vector6 Adjoint(const Vector6& xi_b) const { return AdjointMap() * xi_b; } /// FIXME Not tested - marked as incorrect @@ -223,9 +224,7 @@ public: } /// get translation - const Point3& translation() const { - return t_; - } + const Point3& translation(OptionalJacobian<3, 6> H = boost::none) const; /// get x double x() const { @@ -264,6 +263,14 @@ public: double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none, OptionalJacobian<1, 6> H2 = boost::none) const; + /** + * Calculate bearing to a landmark + * @param point 3D location of landmark + * @return bearing (Unit3) + */ + Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> H1 = boost::none, + OptionalJacobian<2, 3> H2 = boost::none) const; + /// @} /// @name Advanced Interface /// @{ @@ -294,7 +301,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(R_); ar & BOOST_SERIALIZATION_NVP(t_); } @@ -322,11 +329,20 @@ inline Matrix wedge(const Vector& xi) { typedef std::pair Point3Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); -template<> -struct traits : public internal::LieGroupTraits {}; +// For MATLAB wrapper +typedef std::vector Pose3Vector; -template<> -struct traits : public internal::LieGroupTraits {}; +template <> +struct traits : public internal::LieGroup {}; +template <> +struct traits : public internal::LieGroup {}; -} // namespace gtsam +// bearing and range traits, used in RangeFactor +template <> +struct Bearing : HasBearing {}; + +template +struct Range : HasRange {}; + +} // namespace gtsam diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index cd093ca61..0ab4a5ee6 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -20,6 +20,7 @@ #include #include #include // Logmap/Expmap derivatives +#include #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> @@ -73,14 +74,22 @@ struct traits { return g.inverse(); } - /// Exponential map, simply be converting omega to axis/angle representation + /// Exponential map, using the inlined code from Eigen's conversion from axis/angle 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)); + ChartJacobian H = boost::none) { + using std::cos; + using std::sin; + if (H) *H = SO3::ExpmapDerivative(omega.template cast()); + _Scalar theta2 = omega.dot(omega); + if (theta2 > std::numeric_limits<_Scalar>::epsilon()) { + _Scalar theta = std::sqrt(theta2); + _Scalar ha = _Scalar(0.5) * theta; + Vector3 vec = (sin(ha) / theta) * omega; + return Q(cos(ha), vec.x(), vec.y(), vec.z()); + } else { + // first order approximation sin(theta/2)/theta = 0.5 + Vector3 vec = _Scalar(0.5) * omega; + return Q(1.0, vec.x(), vec.y(), vec.z()); } } @@ -93,9 +102,9 @@ struct traits { static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10; - Vector3 omega; + TangentVector omega; - const double qw = q.w(); + const _Scalar qw = q.w(); // See Quaternion-Logmap.nb in doc for Taylor expansions if (qw > NearlyOne) { // Taylor expansion of (angle / s) at 1 @@ -107,7 +116,7 @@ struct traits { omega = (-8. / 3. - 2. / 3. * qw) * q.vec(); } else { // Normal, away from zero case - double angle = 2 * acos(qw), s = sqrt(1 - qw * qw); + _Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw); // Important: convert to [-pi,pi] to keep error continuous if (angle > M_PI) angle -= twoPi; @@ -116,7 +125,7 @@ struct traits { omega = (angle / s) * q.vec(); } - if(H) *H = SO3::LogmapDerivative(omega); + if(H) *H = SO3::LogmapDerivative(omega.template cast()); return omega; } diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index deae1f3a0..198cd5862 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -118,12 +118,12 @@ namespace gtsam { Matrix1 AdjointMap() const { return I_1x1; } /// Left-trivialized derivative of the exponential map - static Matrix ExpmapDerivative(const Vector& v) { + static Matrix ExpmapDerivative(const Vector& /*v*/) { return ones(1); } /// Left-trivialized derivative inverse of the exponential map - static Matrix LogmapDerivative(const Vector& v) { + static Matrix LogmapDerivative(const Vector& /*v*/) { return ones(1); } @@ -200,7 +200,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(c_); ar & BOOST_SERIALIZATION_NVP(s_); } @@ -208,9 +208,9 @@ namespace gtsam { }; template<> - struct traits : public internal::LieGroupTraits {}; + struct traits : public internal::LieGroup {}; template<> - struct traits : public internal::LieGroupTraits {}; + struct traits : public internal::LieGroup {}; } // gtsam diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index fa09ddc21..6b28f5125 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -34,29 +34,12 @@ void Rot3::print(const std::string& s) const { } /* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Point3& w, double theta) { - return rodriguez((Vector)w.vector(),theta); -} - -/* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Unit3& w, double theta) { - return rodriguez(w.point3(),theta); -} - -/* ************************************************************************* */ -Rot3 Rot3::Random(boost::mt19937 & rng) { +Rot3 Rot3::Random(boost::mt19937& rng) { // TODO allow any engine without including all of boost :-( - Unit3 w = Unit3::Random(rng); - boost::uniform_real randomAngle(-M_PI,M_PI); + Unit3 axis = Unit3::Random(rng); + boost::uniform_real randomAngle(-M_PI, M_PI); double angle = randomAngle(rng); - return rodriguez(w,angle); -} - -/* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Vector3& w) { - double t = w.norm(); - if (t < 1e-10) return Rot3(); - return rodriguez(w/t, t); + return AxisAngle(axis, angle); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index d35fa52f4..d8b8a4682 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -22,8 +22,9 @@ #pragma once -#include #include +#include +#include #include #include // Get GTSAM_USE_QUATERNIONS macro @@ -84,11 +85,33 @@ namespace gtsam { double R21, double R22, double R23, double R31, double R32, double R33); - /** constructor from a rotation matrix */ - Rot3(const Matrix3& R); + /** + * Constructor from a rotation matrix + * Version for generic matrices. Need casting to Matrix3 + * in quaternion mode, since Eigen's quaternion doesn't + * allow assignment/construction from a generic matrix. + * See: http://stackoverflow.com/questions/27094132/cannot-understand-if-this-is-circular-dependency-or-clang#tab-top + */ + template + inline Rot3(const Eigen::MatrixBase& R) { + #ifdef GTSAM_USE_QUATERNIONS + quaternion_=Matrix3(R); + #else + rot_ = R; + #endif + } - /** constructor from a rotation matrix */ - Rot3(const Matrix& R); + /** + * Constructor from a rotation matrix + * Overload version for Matrix3 to avoid casting in quaternion mode. + */ + inline Rot3(const Matrix3& R) { + #ifdef GTSAM_USE_QUATERNIONS + quaternion_=R; + #else + rot_ = R; + #endif + } /** Constructor from a quaternion. This can also be called using a plain * Vector, due to implicit conversion from Vector to Quaternion @@ -140,7 +163,7 @@ namespace gtsam { * as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf. * Assumes vehicle coordinate frame X forward, Y right, Z down. */ - static Rot3 ypr (double y, double p, double r) { return RzRyRx(r,p,y);} + static Rot3 ypr(double y, double p, double r) { return RzRyRx(r,p,y);} /** Create from Quaternion coefficients */ static Rot3 quaternion(double w, double x, double y, double z) { @@ -149,45 +172,58 @@ namespace gtsam { } /** - * Rodriguez' formula to compute an incremental rotation matrix - * @param w is the rotation axis, unit length - * @param theta rotation angle - * @return incremental rotation matrix + * Convert from axis/angle representation + * @param axis is the rotation axis, unit length + * @param angle rotation angle + * @return incremental rotation */ - static Rot3 rodriguez(const Vector3& w, double theta); + static Rot3 AxisAngle(const Vector3& axis, double angle) { +#ifdef GTSAM_USE_QUATERNIONS + return Quaternion(Eigen::AngleAxis(angle, axis)); +#else + return SO3::AxisAngle(axis,angle); +#endif + } /** - * Rodriguez' formula to compute an incremental rotation matrix - * @param w is the rotation axis, unit length - * @param theta rotation angle - * @return incremental rotation matrix + * Convert from axis/angle representation + * @param axisw is the rotation axis, unit length + * @param angle rotation angle + * @return incremental rotation */ - static Rot3 rodriguez(const Point3& w, double theta); + static Rot3 AxisAngle(const Point3& axis, double angle) { + return AxisAngle(axis.vector(),angle); + } /** - * Rodriguez' formula to compute an incremental rotation matrix - * @param w is the rotation axis - * @param theta rotation angle - * @return incremental rotation matrix + * Convert from axis/angle representation + * @param axis is the rotation axis + * @param angle rotation angle + * @return incremental rotation */ - static Rot3 rodriguez(const Unit3& w, double theta); + static Rot3 AxisAngle(const Unit3& axis, double angle) { + return AxisAngle(axis.unitVector(),angle); + } /** - * Rodriguez' formula to compute an incremental rotation matrix - * @param v a vector of incremental roll,pitch,yaw - * @return incremental rotation matrix + * Rodrigues' formula to compute an incremental rotation + * @param w a vector of incremental roll,pitch,yaw + * @return incremental rotation */ - static Rot3 rodriguez(const Vector3& v); + static Rot3 Rodrigues(const Vector3& w) { + return Rot3::Expmap(w); + } /** - * Rodriguez' formula to compute an incremental rotation matrix + * Rodrigues' formula to compute an incremental rotation * @param wx Incremental roll (about X) * @param wy Incremental pitch (about Y) * @param wz Incremental yaw (about Z) - * @return incremental rotation matrix + * @return incremental rotation */ - static Rot3 rodriguez(double wx, double wy, double wz) - { return rodriguez(Vector3(wx, wy, wz));} + static Rot3 Rodrigues(double wx, double wy, double wz) { + return Rodrigues(Vector3(wx, wy, wz)); + } /// @} /// @name Testable @@ -272,11 +308,15 @@ namespace gtsam { /** * Exponential map at identity - create a rotation from canonical coordinates - * \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula + * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula */ - static Rot3 Expmap(const Vector& v, OptionalJacobian<3,3> H = boost::none) { + static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = boost::none) { if(H) *H = Rot3::ExpmapDerivative(v); - if (zero(v)) return Rot3(); else return rodriguez(v); +#ifdef GTSAM_USE_QUATERNIONS + return traits::Expmap(v); +#else + return traits::Expmap(v); +#endif } /** @@ -312,6 +352,17 @@ namespace gtsam { Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, OptionalJacobian<3,3> H2 = boost::none) const; + /// operator* for Vector3 + inline Vector3 operator*(const Vector3& v) const { + return rotate(Point3(v)).vector(); + } + + /// rotate for Vector3 + Vector3 rotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const { + return rotate(Point3(v), H1, H2).vector(); + } + /// rotate point from rotated coordinate frame to world = R*p Point3 operator*(const Point3& p) const; @@ -319,6 +370,12 @@ namespace gtsam { Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, OptionalJacobian<3,3> H2=boost::none) const; + /// unrotate for Vector3 + Vector3 unrotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const { + return unrotate(Point3(v), H1, H2).vector(); + } + /// @} /// @name Group Action on Unit3 /// @{ @@ -422,13 +479,21 @@ namespace gtsam { GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p); /// @} + /// @name Deprecated + /// @{ + static Rot3 rodriguez(const Vector3& axis, double angle) { return AxisAngle(axis, angle); } + static Rot3 rodriguez(const Point3& axis, double angle) { return AxisAngle(axis, angle); } + static Rot3 rodriguez(const Unit3& axis, double angle) { return AxisAngle(axis, angle); } + static Rot3 rodriguez(const Vector3& w) { return Rodrigues(w); } + static Rot3 rodriguez(double wx, double wy, double wz) { return Rodrigues(wx, wy, wz); } + /// @} 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*/) { #ifndef GTSAM_USE_QUATERNIONS ar & boost::serialization::make_nvp("rot11", rot_(0,0)); @@ -463,9 +528,9 @@ namespace gtsam { GTSAM_EXPORT std::pair RQ(const Matrix3& A); template<> - struct traits : public internal::LieGroupTraits {}; + struct traits : public internal::LieGroup {}; template<> - struct traits : public internal::LieGroupTraits {}; + struct traits : public internal::LieGroup {}; } diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index b4c79de3b..a7b654070 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -50,18 +50,6 @@ Rot3::Rot3(double R11, double R12, double R13, R31, R32, R33; } -/* ************************************************************************* */ -Rot3::Rot3(const Matrix3& R) { - rot_ = R; -} - -/* ************************************************************************* */ -Rot3::Rot3(const Matrix& R) { - if (R.rows()!=3 || R.cols()!=3) - throw invalid_argument("Rot3 constructor expects 3*3 matrix"); - rot_ = R; -} - /* ************************************************************************* */ Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) { } @@ -117,11 +105,6 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) { ); } -/* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Vector3& w, double theta) { - return SO3::Rodrigues(w,theta); -} - /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { return Rot3(Matrix3(rot_*R2.rot_)); @@ -136,10 +119,8 @@ Matrix3 Rot3::transpose() const { /* ************************************************************************* */ Point3 Rot3::rotate(const Point3& p, 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_; - } + if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); + if (H2) *H2 = rot_; return Point3(rot_ * p.vector()); } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 52fb4f262..2497f6806 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -27,14 +27,12 @@ using namespace std; namespace gtsam { - static const Matrix I3 = eye(3); - /* ************************************************************************* */ Rot3::Rot3() : quaternion_(Quaternion::Identity()) {} /* ************************************************************************* */ Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) : - quaternion_((Eigen::Matrix3d() << + quaternion_((Matrix3() << col1.x(), col2.x(), col3.x(), col1.y(), col2.y(), col3.y(), col1.z(), col2.z(), col3.z()).finished()) {} @@ -43,19 +41,11 @@ namespace gtsam { Rot3::Rot3(double R11, double R12, double R13, double R21, double R22, double R23, double R31, double R32, double R33) : - quaternion_((Eigen::Matrix3d() << + quaternion_((Matrix3() << R11, R12, R13, R21, R22, R23, R31, R32, R33).finished()) {} - /* ************************************************************************* */ - Rot3::Rot3(const Matrix3& R) : - quaternion_(R) {} - - /* ************************************************************************* */ - Rot3::Rot3(const Matrix& R) : - quaternion_(Matrix3(R)) {} - /* ************************************************************************* */ Rot3::Rot3(const Quaternion& q) : quaternion_(q) { @@ -83,10 +73,6 @@ namespace gtsam { Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()))); } - /* ************************************************************************* */ - Rot3 Rot3::rodriguez(const Vector3& w, double theta) { - return Quaternion(Eigen::AngleAxis(theta, w)); - } /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { return Rot3(quaternion_ * R2.quaternion_); @@ -103,10 +89,10 @@ namespace gtsam { /* ************************************************************************* */ Point3 Rot3::rotate(const Point3& p, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { - Matrix R = matrix(); + const Matrix3 R = matrix(); if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H2) *H2 = R; - Eigen::Vector3d r = R * p.vector(); + const Vector3 r = R * p.vector(); return Point3(r.x(), r.y(), r.z()); } diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 7e755d680..a417164e4 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -19,47 +19,69 @@ #include #include #include - -using namespace std; +#include 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; - +// Near zero, we just have I + skew(omega) +static SO3 FirstOrder(const Vector3& omega) { Matrix3 R; - R << c + C00, -swz + C01, swy + C02, // - swz + C01, c + C11, -swx + C12, // - -swy + C02, swx + C12, c + C22; - + R(0, 0) = 1.; + R(1, 0) = omega.z(); + R(2, 0) = -omega.y(); + R(0, 1) = -omega.z(); + R(1, 1) = 1.; + R(2, 1) = omega.x(); + R(0, 2) = omega.y(); + R(1, 2) = -omega.x(); + R(2, 2) = 1.; return R; } +SO3 SO3::AxisAngle(const Vector3& axis, double theta) { + if (theta*theta > std::numeric_limits::epsilon()) { + using std::cos; + using std::sin; + + // get components of axis \omega, where is a unit vector + const double& wx = axis.x(), wy = axis.y(), wz = axis.z(); + + const double costheta = cos(theta), sintheta = sin(theta), c_1 = 1 - costheta; + const double wx_sintheta = wx * sintheta, wy_sintheta = wy * sintheta, + wz_sintheta = wz * sintheta; + + const double C00 = c_1 * wx * wx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz; + const double C11 = c_1 * wy * wy, C12 = c_1 * wy * wz; + const double C22 = c_1 * wz * wz; + + Matrix3 R; + R(0, 0) = costheta + C00; + R(1, 0) = wz_sintheta + C01; + R(2, 0) = -wy_sintheta + C02; + R(0, 1) = -wz_sintheta + C01; + R(1, 1) = costheta + C11; + R(2, 1) = wx_sintheta + C12; + R(0, 2) = wy_sintheta + C02; + R(1, 2) = -wx_sintheta + C12; + R(2, 2) = costheta + C22; + return R; + } else { + return FirstOrder(axis*theta); + } + +} + /// simply convert omega to axis/angle representation -SO3 SO3::Expmap(const Vector3& omega, - ChartJacobian H) { +SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { + if (H) *H = ExpmapDerivative(omega); - if (H) - *H = ExpmapDerivative(omega); - - if (omega.isZero()) - return Identity(); - else { - double angle = omega.norm(); - return Rodrigues(omega / angle, angle); + double theta2 = omega.dot(omega); + if (theta2 > std::numeric_limits::epsilon()) { + double theta = std::sqrt(theta2); + return AxisAngle(omega / theta, theta); + } else { + return FirstOrder(omega); } } @@ -111,8 +133,9 @@ Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { using std::cos; using std::sin; - if(zero(omega)) return I_3x3; - double theta = omega.norm(); // rotation angle + const double theta2 = omega.dot(omega); + if (theta2 <= std::numeric_limits::epsilon()) return I_3x3; + const double theta = std::sqrt(theta2); // rotation angle #ifdef DUY_VERSION /// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) Matrix3 X = skewSymmetric(omega); @@ -131,9 +154,15 @@ Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { * 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 + const double wx = omega.x(), wy = omega.y(), wz = omega.z(); + Matrix3 Y; + Y << 0.0, -wz, +wy, + +wz, 0.0, -wx, + -wy, +wx, 0.0; + const double s2 = sin(theta / 2.0); + const double a = (2.0 * s2 * s2 / theta2); + const double b = (1.0 - sin(theta) / theta) / theta2; + return I_3x3 - a * Y + b * Y * Y; #endif } @@ -142,8 +171,9 @@ Matrix3 SO3::LogmapDerivative(const Vector3& omega) { using std::cos; using std::sin; - if(zero(omega)) return I_3x3; - double theta = omega.norm(); + double theta2 = omega.dot(omega); + if (theta2 <= std::numeric_limits::epsilon()) return I_3x3; + double theta = std::sqrt(theta2); // rotation angle #ifdef DUY_VERSION /// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) Matrix3 X = skewSymmetric(omega); diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index a07c3601d..580287eac 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -59,7 +59,7 @@ public: } /// Static, named constructor TODO think about relation with above - static SO3 Rodrigues(const Vector3& axis, double theta); + static SO3 AxisAngle(const Vector3& axis, double theta); /// @} /// @name Testable @@ -93,7 +93,7 @@ public: /** * Exponential map at identity - create a rotation from canonical coordinates - * \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula + * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula */ static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none); @@ -129,11 +129,11 @@ public: }; template<> -struct traits : public internal::LieGroupTraits { +struct traits : public internal::LieGroup { }; template<> -struct traits : public internal::LieGroupTraits { +struct traits : public internal::LieGroup { }; } // end namespace gtsam diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index c6d33bcb3..fe493c075 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -18,14 +18,126 @@ #pragma once +#include #include #include namespace gtsam { /// A simple camera class with a Cal3_S2 calibration - typedef PinholeCamera SimpleCamera; +typedef gtsam::PinholeCamera PinholeCameraCal3_S2; - /// Recover camera from 3*4 camera matrix - GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P); -} +/** + * @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()))); + } + + /// @} + +}; + +/// Recover camera from 3*4 camera matrix +GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P); + +// manifold traits +template <> +struct traits : public internal::Manifold {}; + +template <> +struct traits : public internal::Manifold {}; + +// range traits, used in RangeFactor +template +struct Range : HasRange {}; + +} // namespace gtsam diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 9170f8282..5c6646454 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -29,16 +29,15 @@ namespace gtsam { } /* ************************************************************************* */ - StereoPoint2 StereoCamera::project(const Point3& point, - OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2, - OptionalJacobian<3,6> H3) 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(); @@ -56,12 +55,6 @@ namespace gtsam { // check if derivatives need to be computed if (H1 || H2) { -#ifdef STEREOCAMERA_CHAIN_RULE - // just implement chain rule - Matrix3 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; @@ -76,10 +69,6 @@ namespace gtsam { 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 - if (H3) - // TODO, this is set to zero, as Cal3_S2Stereo cannot be optimized yet - H3->setZero(); } // finally translate @@ -87,15 +76,57 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix3 StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const { - double d = 1.0 / P.z(), d2 = d*d; + 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 point = leftCamPose_.transform_from(Point3(X, Y, Z)); + return point; + } + + /* ************************************************************************* */ + Point3 StereoCamera::backproject2(const StereoPoint2& z, OptionalJacobian<3, 6> H1, + OptionalJacobian<3, 3> H2) const { const Cal3_S2Stereo& K = *K_; - double f_x = K.fx(), f_y = K.fy(), b = K.baseline(); - Matrix3 m; - m << 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(); - return m; + const double fx = K.fx(), fy = K.fy(), cx = K.px(), cy = K.py(), b = K.baseline(); + + double uL = z.uL(), uR = z.uR(), v = z.v(); + double disparity = uL - uR; + + double local_z = b * fx / disparity; + const Point3 local(local_z * (uL - cx)/ fx, local_z * (v - cy) / fy, local_z); + + if(H1 || H2) { + double z_partial_uR = local_z/disparity; + double x_partial_uR = local.x()/disparity; + double y_partial_uR = local.y()/disparity; + Matrix3 D_local_z; + D_local_z << -x_partial_uR + local.z()/fx, x_partial_uR, 0, + -y_partial_uR, y_partial_uR, local.z() / fy, + -z_partial_uR, z_partial_uR, 0; + + Matrix3 D_point_local; + const Point3 point = leftCamPose_.transform_from(local, H1, D_point_local); + + if(H2) { + *H2 = D_point_local * D_local_z; + } + + return point; + } + + return leftCamPose_.transform_from(local); } } diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 913b1eab3..ac32be7ae 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -17,9 +17,6 @@ #pragma once -#include - -#include #include #include #include @@ -28,60 +25,77 @@ 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 }; + 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); - const Cal3_S2Stereo::shared_ptr calibration() const { - return K_; + /// Return shared pointer to calibration + const Cal3_S2Stereo& calibration() const { + return *K_; } /// @} /// @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; } @@ -100,46 +114,58 @@ public: /// @name Standard Interface /// @{ + /// pose const Pose3& pose() const { return leftCamPose_; } + /// baseline double baseline() const { return K_->baseline(); } - /* - * project 3D point and compute optional derivatives + /// Project 3D point to StereoPoint2 (uL,uR,v) + StereoPoint2 project(const Point3& point) const; + + /** Project 3D point and compute optional derivatives + * @param H1 derivative with respect to pose + * @param H2 derivative with respect to point + */ + StereoPoint2 project2(const Point3& point, OptionalJacobian<3, 6> H1 = + boost::none, OptionalJacobian<3, 3> H2 = boost::none) const; + + /// back-project a measurement + Point3 backproject(const StereoPoint2& z) const; + + /** Back-project the 2D point and compute optional derivatives + * @param H1 derivative with respect to pose + * @param H2 derivative with respect to point + */ + Point3 backproject2(const StereoPoint2& z, + OptionalJacobian<3, 6> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) 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 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none, - OptionalJacobian<3, 6> H3 = 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; - } + 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 */ - Matrix3 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_); } @@ -147,8 +173,10 @@ private: }; template<> -struct traits : public internal::Manifold {}; +struct traits : public internal::Manifold { +}; template<> -struct traits : public internal::Manifold {}; +struct traits : public internal::Manifold { +}; } diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 803c59a4b..1b9559e67 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -18,165 +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) : + /** constructor */ + StereoPoint2(double uL, double uR, double v) : uL_(uL), uR_(uR), v_(v) { - } + } - /// @} - /// @name Testable - /// @{ + /// construct from 3D vector + StereoPoint2(const Vector3& v) : + uL_(v(0)), uR_(v(1)), v_(v(2)) {} - /** print */ - void print(const std::string& s="") const; + /// @} + /// @name Testable + /// @{ - /** 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); - } + /** print */ + void print(const std::string& s = "") const; - /// @} - /// @name Group - /// @{ + /** 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); + } - /// identity - inline static StereoPoint2 identity() { return StereoPoint2(); } + /// @} + /// @name Group + /// @{ - /// inverse - inline StereoPoint2 inverse() const { - return StereoPoint2()- (*this); - } + /// identity + inline static StereoPoint2 identity() { + return StereoPoint2(); + } - /// "Compose", just adds the coordinates of two points. - inline StereoPoint2 compose(const StereoPoint2& p1) const { - return *this + p1; - } + /// inverse + StereoPoint2 operator-() const { + return StereoPoint2(-uL_, -uR_, -v_); + } - /// add two stereo points - StereoPoint2 operator+(const StereoPoint2& b) const { - return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_); - } + /// add + inline 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 - /// @{ + /// subtract + inline StereoPoint2 operator -(const StereoPoint2& b) const { + return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); + } - /// dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } + /// @} + /// @name Standard Interface + /// @{ - /// return dimensionality of tangent space, DOF = 3 - inline size_t dim() const { return dimension; } + /// equality + inline bool operator ==(const StereoPoint2& q) const {return uL_== q.uL_ && uR_==q.uR_ && v_ == q.v_;} - /// Updates a with tangent space delta - inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); } + /// get uL + inline double uL() const {return uL_;} - /// Returns inverse retraction - inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); } + /// get uR + inline double uR() const {return uR_;} - /// @} - /// @name Lie Group - /// @{ + /// get v + inline double v() const {return v_;} - /** 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)); - } + /** convert to vector */ + Vector3 vector() const { + return Vector3(uL_, uR_, v_); + } - /** Log map around identity - just return the Point2 as a vector */ - static inline Vector Logmap(const StereoPoint2& p) { - return p.vector(); - } + /** convenient function to get a Point2 from the left image */ + Point2 point2() const { + return Point2(uL_, v_); + } - /** The difference between another point and this point */ - inline StereoPoint2 between(const StereoPoint2& p2) const { - return p2 - *this; - } + /** convenient function to get a Point2 from the right image */ + Point2 right() const { + return Point2(uR_, v_); + } - /// @} - /// @name Standard Interface - /// @{ + /// @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)); } + /// @} - /// get uL - inline double uL() const {return uL_;} + /// Streaming + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p); - /// get uR - inline double uR() const {return uR_;} +private: - /// get v - inline double v() const {return v_;} + /// @} + /// @name Advanced Interface + /// @{ - /** convert to vector */ - Vector3 vector() const { - return Vector3(uL_, uR_, v_); - } + /** 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_); + } - /** 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_); - } +}; - /// Streaming - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p); +template<> +struct traits : public internal::VectorSpace {}; - 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::Manifold {}; - - template<> - struct traits : public internal::Manifold {}; +template<> +struct traits : public internal::VectorSpace {}; } diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index be48aecc9..aaf0aa953 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -15,12 +15,13 @@ * @author Can Erdogan * @author Frank Dellaert * @author Alex Trevor + * @author Zhaoyang Lv * @brief The Unit3 class - basically a point on a unit sphere */ #include #include -#include +#include // for GTSAM_USE_TBB #ifdef __clang__ # pragma clang diagnostic push @@ -31,12 +32,9 @@ # pragma clang diagnostic pop #endif -#ifdef GTSAM_USE_TBB -#include -#endif - #include #include +#include using namespace std; @@ -44,14 +42,13 @@ namespace gtsam { /* ************************************************************************* */ 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: - Matrix3 D_p_point; - point.normalize(D_p_point); // TODO, this calculates norm a second time :-( - // Calculate the 2*3 Jacobian + // 3*3 Derivative of representation with respect to point is 3*3: + Matrix3 D_p_point; + Point3 normalized = point.normalize(H ? &D_p_point : 0); + Unit3 direction; + direction.p_ = normalized.vector(); + if (H) *H << direction.basis().transpose() * D_p_point; - } return direction; } @@ -61,54 +58,103 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { boost::uniform_on_sphere randomDirection(3); // This variate_generator object is required for versions of boost somewhere // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng). - boost::variate_generator > - generator(rng, randomDirection); + boost::variate_generator > generator( + rng, randomDirection); vector d = generator(); - Unit3 result; - result.p_ = Point3(d[0], d[1], d[2]); - return result; + return Unit3(d[0], d[1], d[2]); } -#ifdef GTSAM_USE_TBB -tbb::mutex unit3BasisMutex; -#endif - /* ************************************************************************* */ -const Matrix32& Unit3::basis() const { +const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { #ifdef GTSAM_USE_TBB - tbb::mutex::scoped_lock lock(unit3BasisMutex); + // NOTE(hayk): At some point it seemed like this reproducably resulted in deadlock. However, I + // can't see the reason why and I can no longer reproduce it. It may have been a red herring, or + // there is still a latent bug to watch out for. + tbb::mutex::scoped_lock lock(B_mutex_); #endif - // Return cached version if exists - if (B_) + // Return cached basis if available and the Jacobian isn't needed. + if (B_ && !H) { return *B_; + } + + // Return cached basis and derivatives if available. + if (B_ && H && H_B_) { + *H = *H_B_; + return *B_; + } + + // Get the unit vector and derivative wrt this. + // NOTE(hayk): We can't call point3(), because it would recursively call basis(). + const Point3 n(p_); // Get the axis of rotation with the minimum projected length of the point Point3 axis; - double mx = fabs(p_.x()), my = fabs(p_.y()), mz = fabs(p_.z()); - if ((mx <= my) && (mx <= mz)) + double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); + if ((mx <= my) && (mx <= mz)) { axis = Point3(1.0, 0.0, 0.0); - else if ((my <= mx) && (my <= mz)) + } else if ((my <= mx) && (my <= mz)) { axis = Point3(0.0, 1.0, 0.0); - else if ((mz <= mx) && (mz <= my)) + } else if ((mz <= mx) && (mz <= my)) { axis = Point3(0.0, 0.0, 1.0); - else + } else { assert(false); + } - // Create the two basis vectors - Point3 b1 = p_.cross(axis); - b1 = b1 / b1.norm(); - Point3 b2 = p_.cross(b1); - b2 = b2 / b2.norm(); + // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with + // the chosen axis. + Matrix33 H_B1_n; + Point3 B1 = n.cross(axis, H ? &H_B1_n : 0); - // Create the basis matrix + // Normalize result to get a unit vector: b1 = B1 / |B1|. + Matrix33 H_b1_B1; + Point3 b1 = B1.normalize(H ? &H_b1_B1 : 0); + + // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. + // No need to normalize this, p and b1 are orthogonal unit vectors. + Matrix33 H_b2_n, H_b2_b1; + Point3 b2 = n.cross(b1, H ? &H_b2_n : 0, H ? &H_b2_b1 : 0); + + // Create the basis by stacking b1 and b2. B_.reset(Matrix32()); (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); + + if (H) { + // Chain rule tomfoolery to compute the derivative. + const Matrix32& H_n_p = *B_; + Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; + Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; + + // Cache the derivative and fill the result. + H_B_.reset(Matrix62()); + (*H_B_) << H_b1_p, H_b2_p; + *H = *H_B_; + } + return *B_; } /* ************************************************************************* */ -/// The print fuction +Point3 Unit3::point3(OptionalJacobian<3, 2> H) const { + if (H) + *H = basis(); + return Point3(p_); +} + +/* ************************************************************************* */ +Vector3 Unit3::unitVector(OptionalJacobian<3, 2> H) const { + if (H) + *H = basis(); + return p_; +} + +/* ************************************************************************* */ +std::ostream& operator<<(std::ostream& os, const Unit3& pair) { + os << pair.p_ << endl; + return os; +} + +/* ************************************************************************* */ void Unit3::print(const std::string& s) const { cout << s << ":" << p_ << endl; } @@ -119,12 +165,72 @@ Matrix3 Unit3::skew() const { } /* ************************************************************************* */ -Vector Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const { +double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,2> H_q) const { + // Get the unit vectors of each, and the derivative. + Matrix32 H_pn_p; + Point3 pn = point3(H_p ? &H_pn_p : 0); + + Matrix32 H_qn_q; + Point3 qn = q.point3(H_q ? &H_qn_q : 0); + + // Compute the dot product of the Point3s. + Matrix13 H_dot_pn, H_dot_qn; + double d = pn.dot(qn, H_p ? &H_dot_pn : 0, H_q ? &H_dot_qn : 0); + + if (H_p) { + (*H_p) << H_dot_pn * H_pn_p; + } + + if (H_q) { + (*H_q) = H_dot_qn * H_qn_q; + } + + return d; +} + +/* ************************************************************************* */ +Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const { // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1 Matrix23 Bt = basis().transpose(); - Vector2 xi = Bt * q.p_.vector(); - if (H) - *H = Bt * q.basis(); + Vector2 xi = Bt * q.p_; + if (H_q) { + *H_q = Bt * q.basis(); + } + return xi; +} + +/* ************************************************************************* */ +Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJacobian<2, 2> H_q) const { + // Get the point3 of this, and the derivative. + Matrix32 H_qn_q; + Point3 qn = q.point3(H_q ? &H_qn_q : 0); + + // 2D error here is projecting q into the tangent plane of this (p). + Matrix62 H_B_p; + Matrix23 Bt = basis(H_p ? &H_B_p : 0).transpose(); + Vector2 xi = Bt * qn.vector(); + + if (H_p) { + // Derivatives of each basis vector. + const Matrix32& H_b1_p = H_B_p.block<3, 2>(0, 0); + const Matrix32& H_b2_p = H_B_p.block<3, 2>(3, 0); + + // Derivatives of the two entries of xi wrt the basis vectors. + Matrix13 H_xi1_b1 = qn.vector().transpose(); + Matrix13 H_xi2_b2 = qn.vector().transpose(); + + // Assemble dxi/dp = dxi/dB * dB/dp. + Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p; + Matrix12 H_xi2_p = H_xi2_b2 * H_b2_p; + *H_p << H_xi1_p, H_xi2_p; + } + + if (H_q) { + // dxi/dq is given by dxi/dqu * dqu/dq, where qu is the unit vector of q. + Matrix23 H_xi_qu = Bt; + *H_q = H_xi_qu * H_qn_q; + } + return xi; } @@ -140,47 +246,39 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const { /* ************************************************************************* */ Unit3 Unit3::retract(const Vector2& v) const { - - // Get the vector form of the point and the basis matrix - Vector3 p = p_.vector(); - Matrix32 B = basis(); - // Compute the 3D xi_hat vector - Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1); + Vector3 xi_hat = basis() * v; + double theta = xi_hat.norm(); - double xi_hat_norm = xi_hat.norm(); - - // Avoid nan - if (xi_hat_norm == 0.0) { - if (v.norm() == 0.0) - return Unit3(point3()); - else - return Unit3(-point3()); + // Treat case of very small v differently + if (theta < std::numeric_limits::epsilon()) { + return Unit3(cos(theta) * p_ + xi_hat); } - Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p - + sin(xi_hat_norm) * (xi_hat / xi_hat_norm); + Vector3 exp_p_xi_hat = + cos(theta) * p_ + xi_hat * (sin(theta) / theta); return Unit3(exp_p_xi_hat); - } /* ************************************************************************* */ -Vector2 Unit3::localCoordinates(const Unit3& y) const { - - 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 Vector2(M_PI, 0); - else { +Vector2 Unit3::localCoordinates(const Unit3& other) const { + const double x = p_.dot(other.p_); + // Crucial quantity here is y = theta/sin(theta) with theta=acos(x) + // Now, y = acos(x) / sin(acos(x)) = acos(x)/sqrt(1-x^2) + // We treat the special case 1 and -1 below + const double x2 = x * x; + const double z = 1 - x2; + double y; + if (z < std::numeric_limits::epsilon()) { + if (x > 0) // first order expansion at x=1 + y = 1.0 - (x - 1.0) / 3.0; + else // cop out + return Vector2(M_PI, 0.0); + } else { // no special case - double theta = acos(dot); - Vector3 result_hat = (theta / sin(theta)) * (q - p * dot); - return basis().transpose() * result_hat; + y = acos(x) / sqrt(z); } + return basis().transpose() * y * (other.p_ - x * p_); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 7d145c213..428211c6b 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -20,11 +20,21 @@ #pragma once -#include +#include #include -#include -#include +#include +#include +#include + #include +#include +#include + +#include + +#ifdef GTSAM_USE_TBB +#include +#endif namespace gtsam { @@ -33,8 +43,13 @@ class GTSAM_EXPORT Unit3 { private: - Point3 p_; ///< The location of the point on the unit sphere + Vector3 p_; ///< The location of the point on the unit sphere mutable boost::optional B_; ///< Cached basis + mutable boost::optional H_B_; ///< Cached basis derivative + +#ifdef GTSAM_USE_TBB + mutable tbb::mutex B_mutex_; ///< Mutex to protect the cached basis. +#endif public: @@ -52,18 +67,35 @@ public: /// Construct from point explicit Unit3(const Point3& p) : - p_(p / p.norm()) { + p_(p.vector().normalized()) { } /// Construct from a vector3 explicit Unit3(const Vector3& p) : - p_(p / p.norm()) { + p_(p.normalized()) { } /// Construct from x,y,z Unit3(double x, double y, double z) : p_(x, y, z) { - p_ = p_ / p_.norm(); + p_.normalize(); + } + + /// Construct from 2D point in plane at focal length f + /// Unit3(p,1) can be viewed as normalized homogeneous coordinates of 2D point + explicit Unit3(const Point2& p, double f = 1.0) : p_(p.x(), p.y(), f) { + p_.normalize(); + } + + /// Copy constructor + Unit3(const Unit3& u) { + p_ = u.p_; + } + + /// Copy assignment + Unit3& operator=(const Unit3 & u) { + p_ = u.p_; + return *this; } /// Named constructor from Point3 with optional Jacobian @@ -78,12 +110,14 @@ public: /// @name Testable /// @{ + friend std::ostream& operator<<(std::ostream& os, const Unit3& pair); + /// The print fuction void print(const std::string& s = std::string()) const; /// The equals function with tolerance bool equals(const Unit3& s, double tol = 1e-9) const { - return p_.equals(s.p_, tol); + return equal_with_abs_tol(p_, s.p_, tol); } /// @} @@ -94,37 +128,50 @@ public: * Returns the local coordinate frame to tangent plane * It is a 3*2 matrix [b1 b2] composed of two orthogonal directions * tangent to the sphere at the current direction. + * Provides derivatives of the basis with the two basis vectors stacked up as a 6x1. */ - const Matrix32& basis() const; + const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const; /// Return skew-symmetric associated with 3D point on unit sphere Matrix3 skew() const; /// Return unit-norm Point3 - const Point3& point3(OptionalJacobian<3, 2> H = boost::none) const { - if (H) - *H = basis(); - return p_; - } + Point3 point3(OptionalJacobian<3, 2> H = boost::none) const; /// Return unit-norm Vector - Vector unitVector(boost::optional H = boost::none) const { - if (H) - *H = basis(); - return (p_.vector()); - } + Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const; /// Return scaled direction as Point3 friend Point3 operator*(double s, const Unit3& d) { - return s * d.p_; + return Point3(s * d.p_); } + /// Return dot product with q + double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, // + OptionalJacobian<1,2> H2 = boost::none) const; + /// Signed, vector-valued error between two directions - Vector error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const; + /// @deprecated, errorVector has the proper derivatives, this confusingly has only the second. + Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const; + + /// Signed, vector-valued error between two directions + /// NOTE(hayk): This method has zero derivatives if this (p) and q are orthogonal. + Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, // + OptionalJacobian<2, 2> H_q = boost::none) const; /// Distance between two directions double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const; + /// Cross-product between two Unit3s + Unit3 cross(const Unit3& q) const { + return Unit3(p_.cross(q.p_)); + } + + /// Cross-product w Point3 + Point3 cross(const Point3& q) const { + return point3().cross(q); + } + /// @} /// @name Manifold @@ -142,7 +189,7 @@ public: enum CoordinatesMode { EXPMAP, ///< Use the exponential map to retract - RENORM ///< Retract with vector addtion and renormalize. + RENORM ///< Retract with vector addition and renormalize. }; /// The retract function @@ -160,15 +207,8 @@ 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(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)); } /// @} diff --git a/gtsam/geometry/concepts.h b/gtsam/geometry/concepts.h index 621f04592..207b48f56 100644 --- a/gtsam/geometry/concepts.h +++ b/gtsam/geometry/concepts.h @@ -59,24 +59,6 @@ private: } }; -/** - * Range measurement concept - * Given a pair of Lie variables, there must exist a function to calculate - * range with derivatives. - */ -template -class RangeMeasurementConcept { -private: - static double checkRangeMeasurement(const V1& x, const V2& p) { - return x.range(p); - } - - static double checkRangeMeasurementDerivatives(const V1& x, const V2& p) { - boost::optional H1, H2; - return x.range(p, H1, H2); - } -}; - } // \namespace gtsam /** @@ -92,7 +74,3 @@ private: #define GTSAM_CONCEPT_POSE_INST(T) template class gtsam::PoseConcept; #define GTSAM_CONCEPT_POSE_TYPE(T) typedef gtsam::PoseConcept _gtsam_PoseConcept##T; -/** Range Measurement macros */ -#define GTSAM_CONCEPT_RANGE_MEASUREMENT_INST(V1,V2) template class gtsam::RangeMeasurementConcept; -#define GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(V1,V2) typedef gtsam::RangeMeasurementConcept _gtsam_RangeMeasurementConcept##V1##V2; - diff --git a/gtsam/geometry/tests/testBearingRange.cpp b/gtsam/geometry/tests/testBearingRange.cpp new file mode 100644 index 000000000..8c5d2208e --- /dev/null +++ b/gtsam/geometry/tests/testBearingRange.cpp @@ -0,0 +1,79 @@ +/* ---------------------------------------------------------------------------- + + * 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 testBearingRange.cpp + * @brief Unit tests for BearingRange Class + * @author Frank Dellaert + * @date July 2015 + */ + +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; +using namespace serializationTestHelpers; + +typedef BearingRange BearingRange2D; +BearingRange2D br2D(1, 2); + +typedef BearingRange BearingRange3D; +BearingRange3D br3D(Pose3().bearing(Point3(1, 0, 0)), 1); + +//****************************************************************************** +TEST(BearingRange2D, Concept) { + BOOST_CONCEPT_ASSERT((IsManifold)); +} + +/* ************************************************************************* */ +TEST(BearingRange, 2D) { + BearingRange2D expected(0, 1); + BearingRange2D actual = BearingRange2D::Measure(Pose2(), Point2(1, 0)); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(BearingRange, Serialization2D) { + EXPECT(equalsObj(br2D)); + EXPECT(equalsXML(br2D)); + EXPECT(equalsBinary(br2D)); +} + +//****************************************************************************** +TEST(BearingRange3D, Concept) { + BOOST_CONCEPT_ASSERT((IsManifold)); +} + +/* ************************************************************************* */ +TEST(BearingRange, 3D) { + BearingRange3D expected(Unit3(), 1); + BearingRange3D actual = BearingRange3D::Measure(Pose3(), Point3(1, 0, 0)); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(BearingRange, Serialization3D) { + EXPECT(equalsObj(br3D)); + EXPECT(equalsXML(br3D)); + EXPECT(equalsBinary(br3D)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index aa06a2e29..3e93dedc1 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -26,6 +26,8 @@ GTSAM_CONCEPT_MANIFOLD_INST(Cal3_S2) static Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2); static Point2 p(1, -2); +static Point2 p_uv(1320.3, 1740); +static Point2 p_xy(2, 3); /* ************************************************************************* */ TEST( Cal3_S2, easy_constructor) @@ -73,6 +75,27 @@ TEST( Cal3_S2, Duncalibrate2) CHECK(assert_equal(numerical,computed,1e-9)); } +Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {return k.calibrate(pt); } +/* ************************************************************************* */ +TEST(Cal3_S2, Dcalibrate1) +{ + Matrix computed; + Point2 expected = K.calibrate(p_uv, computed, boost::none); + Matrix numerical = numericalDerivative21(calibrate_, K, p_uv); + CHECK(assert_equal(expected, p_xy, 1e-8)); + CHECK(assert_equal(numerical, computed, 1e-8)); +} + +/* ************************************************************************* */ +TEST(Cal3_S2, Dcalibrate2) +{ + Matrix computed; + Point2 expected = K.calibrate(p_uv, boost::none, computed); + Matrix numerical = numericalDerivative22(calibrate_, K, p_uv); + CHECK(assert_equal(expected, p_xy, 1e-8)); + CHECK(assert_equal(numerical, computed, 1e-8)); +} + /* ************************************************************************* */ TEST( Cal3_S2, assert_equal) { diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 6a990e08e..3b0906b44 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -29,7 +29,8 @@ using namespace gtsam; GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera) -static const Pose3 pose1((Matrix)(Matrix(3,3) << +// Camera situated at 0.5 meters high, looking down +static const Pose3 pose1((Matrix3() << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. @@ -87,34 +88,94 @@ TEST( CalibratedCamera, project) } /* ************************************************************************* */ -TEST( CalibratedCamera, Dproject_to_camera1) { - Point3 pp(155,233,131); +static Point2 Project1(const Point3& point) { + return PinholeBase::Project(point); +} + +TEST( CalibratedCamera, DProject1) { + Point3 pp(155, 233, 131); Matrix test1; - CalibratedCamera::project_to_camera(pp, test1); - Matrix test2 = numericalDerivative11( - boost::bind(CalibratedCamera::project_to_camera, _1, boost::none), pp); + CalibratedCamera::Project(pp, test1); + Matrix test2 = numericalDerivative11(Project1, pp); CHECK(assert_equal(test1, test2)); } /* ************************************************************************* */ -static Point2 project2(const Pose3& pose, const Point3& point) { - return CalibratedCamera(pose).project(point); +static Point2 Project2(const Unit3& point) { + return PinholeBase::Project(point); +} + +Unit3 pointAtInfinity(0, 0, 1000); +TEST( CalibratedCamera, DProjectInfinity) { + Matrix test1; + CalibratedCamera::Project(pointAtInfinity, test1); + Matrix test2 = numericalDerivative11(Project2, + pointAtInfinity); + CHECK(assert_equal(test1, test2)); +} + +/* ************************************************************************* */ +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)); +} + +/* ************************************************************************* */ +static Point2 projectAtInfinity(const CalibratedCamera& camera, const Unit3& point) { + return camera.project2(point); +} + +TEST( CalibratedCamera, Dproject_point_pose_infinity) +{ + Matrix Dpose, Dpoint; + Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint); + Matrix numerical_pose = numericalDerivative21(projectAtInfinity, camera, pointAtInfinity); + Matrix numerical_point = numericalDerivative22(projectAtInfinity, camera, pointAtInfinity); + CHECK(assert_equal(Point2(), 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_infinity) +{ + 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.project2(pointAtInfinity, Dpose, Dpoint); + Matrix numerical_pose = numericalDerivative21(projectAtInfinity, camera, pointAtInfinity); + Matrix numerical_point = numericalDerivative22(projectAtInfinity, camera, pointAtInfinity); + 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..0afa04411 --- /dev/null +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -0,0 +1,177 @@ +/* ---------------------------------------------------------------------------- + + * 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 CameraSet Set; + typedef vector ZZ; + Set set; + Camera camera; + set.push_back(camera); + set.push_back(camera); + Point3 p(0, 0, 1); + EXPECT(assert_equal(set, set)); + Set set2 = set; + set2.push_back(camera); + EXPECT(!set.equals(set2)); + + // Check measurements + Point2 expected; + ZZ z = set.project2(p); + EXPECT(assert_equal(expected, z[0])); + EXPECT(assert_equal(expected, z[1])); + + // Calculate expected derivatives using Pinhole + Matrix actualE; + Matrix29 F1; + { + Matrix23 E1; + camera.project2(p, F1, E1); + actualE.resize(4,3); + actualE << E1, E1; + } + + // Check computed derivatives + Set::FBlocks Fs; + Matrix E; + set.project2(p, Fs, E); + LONGS_EQUAL(2, Fs.size()); + EXPECT(assert_equal(F1, Fs[0])); + EXPECT(assert_equal(F1, Fs[1])); + EXPECT(assert_equal(actualE, E)); + + // Check errors + ZZ measured; + measured.push_back(Point2(1, 2)); + measured.push_back(Point2(3, 4)); + Vector4 expectedV; + + // reprojectionError + expectedV << -1, -2, -3, -4; + Vector actualV = set.reprojectionError(p, measured); + EXPECT(assert_equal(expectedV, actualV)); + + // Check Schur complement + Matrix F(4, 18); + F << F1, Matrix29::Zero(), Matrix29::Zero(), F1; + Matrix Ft = F.transpose(); + Matrix34 Et = E.transpose(); + Matrix3 P = Et * E; + Matrix schur(19, 19); + Vector4 b = actualV; + Vector v = Ft * (b - E * P * Et * b); + schur << Ft * F - Ft * E * P * Et * F, v, v.transpose(), 30; + SymmetricBlockMatrix actualReduced = Set::SchurComplement(Fs, E, P, b); + EXPECT(assert_equal(schur, actualReduced.matrix())); + + // Check Schur complement update, same order, should just double + FastVector allKeys, keys; + allKeys.push_back(1); + allKeys.push_back(2); + keys.push_back(1); + keys.push_back(2); + Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys, actualReduced); + EXPECT(assert_equal((Matrix )(2.0 * schur), actualReduced.matrix())); + + // Check Schur complement update, keys reversed + FastVector keys2; + keys2.push_back(2); + keys2.push_back(1); + Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys2, actualReduced); + Vector4 reverse_b; + reverse_b << b.tail<2>(), b.head<2>(); + Vector reverse_v = Ft * (reverse_b - E * P * Et * reverse_b); + Matrix A(19, 19); + A << Ft * F - Ft * E * P * Et * F, reverse_v, reverse_v.transpose(), 30; + EXPECT(assert_equal((Matrix )(2.0 * schur + A), actualReduced.matrix())); + + // reprojectionErrorAtInfinity + Unit3 pointAtInfinity(0, 0, 1000); + EXPECT( + assert_equal(pointAtInfinity, + camera.backprojectPointAtInfinity(Point2()))); + actualV = set.reprojectionError(pointAtInfinity, measured, Fs, E); + EXPECT(assert_equal(expectedV, actualV)); + LONGS_EQUAL(2, Fs.size()); + { + Matrix22 E1; + camera.project2(pointAtInfinity, F1, E1); + actualE.resize(4,2); + actualE << E1, E1; + } + EXPECT(assert_equal(F1, Fs[0])); + EXPECT(assert_equal(F1, Fs[1])); + EXPECT(assert_equal(actualE, E)); +} + +/* ************************************************************************* */ +#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.project2(p); + EXPECT(assert_equal(expected, z[0])); + EXPECT(assert_equal(expected, z[1])); + + // Calculate expected derivatives using Pinhole + Matrix63 actualE; + Matrix F1; + { + Matrix33 E1; + camera.project2(p, F1, E1); + actualE << E1, E1; + } + + // Check computed derivatives + CameraSet::FBlocks Fs; + Matrix E; + set.project2(p, Fs, E); + LONGS_EQUAL(2, Fs.size()); + EXPECT(assert_equal(F1, Fs[0])); + EXPECT(assert_equal(F1, Fs[1])); + EXPECT(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 index a15d7e2c2..7becfc75f 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -22,52 +22,114 @@ using namespace std; using namespace gtsam; -typedef Cyclic<3> G; // Let's use the cyclic group of order 3 +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()); + BOOST_CONCEPT_ASSERT((IsGroup)); + EXPECT_LONGS_EQUAL(0, traits::Identity()); } //****************************************************************************** TEST(Cyclic, Constructor) { - G g(0); + Z3 g(0); } //****************************************************************************** TEST(Cyclic, Compose) { - EXPECT_LONGS_EQUAL(0, traits::Compose(G(0),G(0))); - EXPECT_LONGS_EQUAL(1, traits::Compose(G(0),G(1))); - EXPECT_LONGS_EQUAL(2, traits::Compose(G(0),G(2))); + 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(G(2),G(0))); - EXPECT_LONGS_EQUAL(0, traits::Compose(G(2),G(1))); - EXPECT_LONGS_EQUAL(1, traits::Compose(G(2),G(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(G(0),G(0))); - EXPECT_LONGS_EQUAL(1, traits::Between(G(0),G(1))); - EXPECT_LONGS_EQUAL(2, traits::Between(G(0),G(2))); + 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(G(2),G(0))); - EXPECT_LONGS_EQUAL(2, traits::Between(G(2),G(1))); - EXPECT_LONGS_EQUAL(0, traits::Between(G(2),G(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, Ivnverse) { - EXPECT_LONGS_EQUAL(0, traits::Inverse(G(0))); - EXPECT_LONGS_EQUAL(2, traits::Inverse(G(1))); - EXPECT_LONGS_EQUAL(1, traits::Inverse(G(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) { - G g(2), h(1); - check_group_invariants(g,h); + 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)); } //****************************************************************************** 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 index b2b4ecc43..11931449f 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -13,6 +13,7 @@ * @file testOrientedPlane3.cpp * @date Dec 19, 2012 * @author Alex Trevor + * @author Zhaoyang Lv * @brief Tests the OrientedPlane3 class */ @@ -30,39 +31,67 @@ GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) //******************************************************************************* +TEST (OrientedPlane3, getMethods) { + Vector4 c; + c << -1, 0, 0, 5; + OrientedPlane3 plane1(c); + OrientedPlane3 plane2(c[0], c[1], c[2], c[3]); + Vector4 coefficient1 = plane1.planeCoefficients(); + double distance1 = plane1.distance(); + EXPECT(assert_equal(coefficient1, c, 1e-8)); + EXPECT(assert_equal(Unit3(-1,0,0).unitVector(), plane1.normal().unitVector())); + EXPECT_DOUBLES_EQUAL(distance1, 5, 1e-8); + Vector4 coefficient2 = plane2.planeCoefficients(); + double distance2 = plane2.distance(); + EXPECT(assert_equal(coefficient2, c, 1e-8)); + EXPECT_DOUBLES_EQUAL(distance2, 5, 1e-8); + EXPECT(assert_equal(Unit3(-1,0,0).unitVector(), plane2.normal().unitVector())); +} + + +//******************************************************************************* +OrientedPlane3 Transform_(const OrientedPlane3& plane, const Pose3& xr) { + return OrientedPlane3::Transform(plane, xr); +} + +OrientedPlane3 transform_(const OrientedPlane3& plane, const Pose3& xr) { + return plane.transform(xr); +} + 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, + OrientedPlane3 expectedPlane(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3); + OrientedPlane3 transformedPlane1 = OrientedPlane3::Transform(plane, pose, none, none); - EXPECT(assert_equal(expected_meas, transformed_plane, 1e-9)); + OrientedPlane3 transformedPlane2 = plane.transform(pose, none, none); + EXPECT(assert_equal(expectedPlane, transformedPlane1, 1e-9)); + EXPECT(assert_equal(expectedPlane, transformedPlane2, 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); + // because the Transform class uses a wrong order of Jacobians in interface + OrientedPlane3::Transform(plane, pose, actualH1, none); + expectedH1 = numericalDerivative22(Transform_, plane, pose); 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); + OrientedPlane3::Transform(plane, pose, none, actualH2); + expectedH2 = numericalDerivative21(Transform_, plane, pose); + EXPECT(assert_equal(expectedH2, actualH2, 1e-9)); + } + { + plane.transform(pose, actualH1, none); + expectedH1 = numericalDerivative21(transform_, plane, pose); + EXPECT(assert_equal(expectedH1, actualH1, 1e-9)); + plane.transform(pose, none, actualH2); + expectedH2 = numericalDerivative22(Transform_, plane, pose); EXPECT(assert_equal(expectedH2, actualH2, 1e-9)); } - } //******************************************************************************* -// Returns a random vector -- copied from testUnit3.cpp +// Returns a any size random vector inline static Vector randomVector(const Vector& minLimits, const Vector& maxLimits) { @@ -82,11 +111,11 @@ inline static Vector randomVector(const Vector& minLimits, TEST(OrientedPlane3, localCoordinates_retract) { size_t numIterations = 10000; - gtsam::Vector minPlaneLimit(4), maxPlaneLimit(4); + Vector4 minPlaneLimit, maxPlaneLimit; minPlaneLimit << -1.0, -1.0, -1.0, 0.01; maxPlaneLimit << 1.0, 1.0, 1.0, 10.0; - Vector minXiLimit(3), maxXiLimit(3); + Vector3 minXiLimit, maxXiLimit; minXiLimit << -M_PI, -M_PI, -10.0; maxXiLimit << M_PI, M_PI, 10.0; for (size_t i = 0; i < numIterations; i++) { @@ -98,15 +127,15 @@ TEST(OrientedPlane3, localCoordinates_retract) { 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; + if (v12.head<3>().norm() > M_PI) + v12.head<3>() = v12.head<3>() / 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)); + EXPECT(assert_equal(v12, actual_v12, 1e-6)); OrientedPlane3 actual_p2 = p1.retract(actual_v12); - EXPECT(assert_equal(p2, actual_p2, 1e-3)); + EXPECT(assert_equal(p2, actual_p2, 1e-6)); } } diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 20f7a3231..74bc4ca2a 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -15,12 +15,14 @@ * @brief test PinholeCamera class */ -#include -#include -#include #include #include #include +#include +#include +#include + +#include #include #include @@ -43,10 +45,10 @@ 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); +static const Unit3 point1_inf(-0.16,-0.16, -1.0); +static const Unit3 point2_inf(-0.16, 0.16, -1.0); +static const Unit3 point3_inf( 0.16, 0.16, -1.0); +static const Unit3 point4_inf( 0.16,-0.16, -1.0); /* ************************************************************************* */ TEST( PinholeCamera, constructor) @@ -152,9 +154,9 @@ TEST( PinholeCamera, backprojectInfinity2) 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); + Unit3 actual = camera.backprojectPointAtInfinity(Point2()); + Unit3 expected(0., 1., 0.); + Point2 x = camera.project(expected); EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(Point2(), x)); @@ -167,9 +169,9 @@ TEST( PinholeCamera, backprojectInfinity3) Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity Camera camera(Pose3(rot, origin), K); - Point3 actual = camera.backprojectPointAtInfinity(Point2()); - Point3 expected(0., 0., 1.); - Point2 x = camera.projectPointAtInfinity(expected); + Unit3 actual = camera.backprojectPointAtInfinity(Point2()); + Unit3 expected(0., 0., 1.); + Point2 x = camera.project(expected); EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(Point2(), x)); @@ -195,17 +197,17 @@ TEST( PinholeCamera, Dproject) } /* ************************************************************************* */ -static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const Cal3_S2& cal) { - return Camera(pose,cal).projectPointAtInfinity(point3D); +static Point2 projectInfinity3(const Pose3& pose, const Unit3& point3D, const Cal3_S2& cal) { + return Camera(pose,cal).project(point3D); } TEST( PinholeCamera, Dproject_Infinity) { Matrix Dpose, Dpoint, Dcal; - Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1 + Unit3 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 actual = camera.project(point3D, Dpose, Dpoint, Dcal); Point2 expected(-5.0, 5.0); EXPECT(assert_equal(actual, expected, 1e-7)); @@ -236,6 +238,20 @@ TEST( PinholeCamera, Dproject2) 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); diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp new file mode 100644 index 000000000..dc294899f --- /dev/null +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -0,0 +1,278 @@ +/* ---------------------------------------------------------------------------- + + * 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 Unit3 point1_inf(-0.16,-0.16, -1.0); +static const Unit3 point2_inf(-0.16, 0.16, -1.0); +static const Unit3 point3_inf( 0.16, 0.16, -1.0); +static const Unit3 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 expectedDcamera = numericalDerivative31(project3, pose, point1, K); + Matrix expectedDpoint = numericalDerivative32(project3, pose, point1, K); + EXPECT(assert_equal(Point2(-100, 100), result)); + EXPECT(assert_equal(expectedDcamera, Dpose, 1e-7)); + EXPECT(assert_equal(expectedDpoint, 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 expectedDcamera = numericalDerivative21(project4, camera, point1); + Matrix expectedDpoint = numericalDerivative22(project4, camera, point1); + EXPECT(assert_equal(result, Point2(-100, 100) )); + EXPECT(assert_equal(expectedDcamera, Dcamera, 1e-7)); + EXPECT(assert_equal(expectedDpoint, 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 expectedDcamera = numericalDerivative21(project4, camera, point1); + Matrix numerical_point = numericalDerivative22(project4, camera, point1); + CHECK(assert_equal(expectedDcamera, Dpose, 1e-7)); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); +} + +/* ************************************************************************* */ +static Point2 project(const Pose3& pose, const Unit3& pointAtInfinity, + const Cal3_S2::shared_ptr& cal) { + return Camera(pose, cal).project(pointAtInfinity); +} + +/* ************************************************************************* */ +TEST( PinholePose, DprojectAtInfinity2) +{ + Unit3 pointAtInfinity(0,0,1000); + Matrix Dpose, Dpoint; + Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint); + Matrix expectedDcamera = numericalDerivative31(project, pose, pointAtInfinity, K); + Matrix expectedDpoint = numericalDerivative32(project, pose, pointAtInfinity, K); + EXPECT(assert_equal(Point2(0,0), result)); + EXPECT(assert_equal(expectedDcamera, Dpose, 1e-7)); + EXPECT(assert_equal(expectedDpoint, 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 expectedDcamera = numericalDerivative21(range0, camera, point1); + Matrix expectedDpoint = numericalDerivative22(range0, camera, point1); + EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result, + 1e-9); + EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); + EXPECT(assert_equal(expectedDpoint, 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 expectedDcamera = numericalDerivative21(range1, camera, pose1); + Matrix expectedDpoint = numericalDerivative22(range1, camera, pose1); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); + EXPECT(assert_equal(expectedDpoint, 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 expectedDcamera = numericalDerivative21(range2, camera, camera2); + Matrix expectedDpoint = numericalDerivative22(range2, camera, camera2); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); + EXPECT(assert_equal(expectedDpoint, 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 expectedDcamera = numericalDerivative21(range3, camera, camera3); + Matrix expectedDpoint = numericalDerivative22(range3, camera, camera3); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); + EXPECT(assert_equal(expectedDpoint, D2, 1e-7)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + + diff --git a/gtsam/geometry/tests/testPinholeSet.cpp b/gtsam/geometry/tests/testPinholeSet.cpp new file mode 100644 index 000000000..b8f001f1c --- /dev/null +++ b/gtsam/geometry/tests/testPinholeSet.cpp @@ -0,0 +1,158 @@ +/* ---------------------------------------------------------------------------- + + * 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; + +/* ************************************************************************* */ +#include +TEST(PinholeSet, Stereo) { + typedef vector ZZ; + PinholeSet set; + CalibratedCamera camera; + set.push_back(camera); + set.push_back(camera); + // set.print("set: "); + Point3 p(0, 0, 1); + EXPECT_LONGS_EQUAL(6, traits::dimension); + + // Check measurements + Point2 expected(0, 0); + ZZ z = set.project2(p); + EXPECT(assert_equal(expected, z[0])); + EXPECT(assert_equal(expected, z[1])); + + // Calculate expected derivatives using Pinhole + Matrix43 actualE; + Matrix F1; + { + Matrix23 E1; + camera.project2(p, F1, E1); + actualE << E1, E1; + } + + // Check computed derivatives + PinholeSet::FBlocks Fs; + Matrix E; + set.project2(p, Fs, E); + LONGS_EQUAL(2, Fs.size()); + EXPECT(assert_equal(F1, Fs[0])); + EXPECT(assert_equal(F1, Fs[1])); + EXPECT(assert_equal(actualE, E)); + + // Instantiate triangulateSafe + // TODO triangulation does not work yet for CalibratedCamera + // PinholeSet::Result actual = set.triangulateSafe(z); +} + +/* ************************************************************************* */ +// Cal3Bundler test +#include +#include +TEST(PinholeSet, Pinhole) { + typedef PinholeCamera Camera; + typedef vector ZZ; + PinholeSet set; + Camera camera; + set.push_back(camera); + set.push_back(camera); + Point3 p(0, 0, 1); + EXPECT(assert_equal(set, set)); + PinholeSet set2 = set; + set2.push_back(camera); + EXPECT(!set.equals(set2)); + + // Check measurements + Point2 expected; + ZZ z = set.project2(p); + EXPECT(assert_equal(expected, z[0])); + EXPECT(assert_equal(expected, z[1])); + + // Calculate expected derivatives using Pinhole + Matrix actualE; + Matrix F1; + { + Matrix23 E1; + camera.project2(p, F1, E1); + actualE.resize(4, 3); + actualE << E1, E1; + } + + // Check computed derivatives + { + PinholeSet::FBlocks Fs; + Matrix E; + set.project2(p, Fs, E); + EXPECT(assert_equal(actualE, E)); + LONGS_EQUAL(2, Fs.size()); + EXPECT(assert_equal(F1, Fs[0])); + EXPECT(assert_equal(F1, Fs[1])); + } + + // Check errors + ZZ measured; + measured.push_back(Point2(1, 2)); + measured.push_back(Point2(3, 4)); + Vector4 expectedV; + + // reprojectionError + expectedV << -1, -2, -3, -4; + Vector actualV = set.reprojectionError(p, measured); + EXPECT(assert_equal(expectedV, actualV)); + + // reprojectionErrorAtInfinity + Unit3 pointAtInfinity(0, 0, 1000); + { + Matrix22 E1; + camera.project2(pointAtInfinity, F1, E1); + actualE.resize(4, 2); + actualE << E1, E1; + } + EXPECT( + assert_equal(pointAtInfinity, + camera.backprojectPointAtInfinity(Point2()))); + { + PinholeSet::FBlocks Fs; + Matrix E; + actualV = set.reprojectionError(pointAtInfinity, measured, Fs, E); + EXPECT(assert_equal(actualE, E)); + LONGS_EQUAL(2, Fs.size()); + EXPECT(assert_equal(F1, Fs[0])); + EXPECT(assert_equal(F1, Fs[1])); + } + EXPECT(assert_equal(expectedV, actualV)); + + // Instantiate triangulateSafe + TriangulationParameters params; + TriangulationResult actual = set.triangulateSafe(z, params); + CHECK(actual.degenerate()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index fce7955e9..6f8d45b3e 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -37,8 +37,8 @@ TEST(Double , Concept) { //****************************************************************************** TEST(Double , Invariants) { double p1(2), p2(5); - check_group_invariants(p1, p2); - check_manifold_invariants(p1, p2); + EXPECT(check_group_invariants(p1, p2)); + EXPECT(check_manifold_invariants(p1, p2)); } //****************************************************************************** @@ -51,8 +51,8 @@ TEST(Point2 , Concept) { //****************************************************************************** TEST(Point2 , Invariants) { Point2 p1(1, 2), p2(4, 5); - check_group_invariants(p1, p2); - check_manifold_invariants(p1, p2); + EXPECT(check_group_invariants(p1, p2)); + EXPECT(check_manifold_invariants(p1, p2)); } /* ************************************************************************* */ @@ -61,6 +61,12 @@ TEST(Point2, constructor) { 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); diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 5c6b32dca..6811ed122 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -36,8 +36,8 @@ TEST(Point3 , Concept) { //****************************************************************************** TEST(Point3 , Invariants) { Point3 p1(1, 2, 3), p2(4, 5, 6); - check_group_invariants(p1, p2); - check_manifold_invariants(p1, p2); + EXPECT(check_group_invariants(p1, p2)); + EXPECT(check_manifold_invariants(p1, p2)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 803bb7c3f..5b835200a 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -775,15 +775,15 @@ Pose2 T2(M_PI / 2.0, Point2(0.0, 2.0)); TEST(Pose2 , Invariants) { Pose2 id; - check_group_invariants(id,id); - check_group_invariants(id,T1); - check_group_invariants(T2,id); - check_group_invariants(T2,T1); + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T1)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T1)); - check_manifold_invariants(id,id); - check_manifold_invariants(id,T1); - check_manifold_invariants(T2,id); - check_manifold_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)); } diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index a716406a4..9007ce1bd 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -32,10 +32,10 @@ GTSAM_CONCEPT_TESTABLE_INST(Pose3) GTSAM_CONCEPT_LIE_INST(Pose3) static Point3 P(0.2,0.7,-2); -static Rot3 R = Rot3::rodriguez(0.3,0,0); +static Rot3 R = Rot3::Rodrigues(0.3,0,0); static Pose3 T(R,Point3(3.5,-8.2,4.2)); -static Pose3 T2(Rot3::rodriguez(0.3,0.2,0.1),Point3(3.5,-8.2,4.2)); -static Pose3 T3(Rot3::rodriguez(-90, 0, 0), Point3(1, 2, 3)); +static Pose3 T2(Rot3::Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2)); +static Pose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3)); const double tol=1e-5; /* ************************************************************************* */ @@ -50,7 +50,7 @@ TEST( Pose3, equals) /* ************************************************************************* */ TEST( Pose3, constructors) { - Pose3 expected(Rot3::rodriguez(0,0,3),Point3(1,2,0)); + Pose3 expected(Rot3::Rodrigues(0,0,3),Point3(1,2,0)); Pose2 pose2(1,2,3); EXPECT(assert_equal(expected,Pose3(pose2))); } @@ -103,7 +103,7 @@ TEST(Pose3, expmap_b) { Pose3 p1(Rot3(), Point3(100, 0, 0)); Pose3 p2 = p1.retract((Vector(6) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0).finished()); - Pose3 expected(Rot3::rodriguez(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0)); + Pose3 expected(Rot3::Rodrigues(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0)); EXPECT(assert_equal(expected, p2,1e-2)); } @@ -182,8 +182,19 @@ TEST(Pose3, expmaps_galore_full) xi = (Vector(6) << 0.2, 0.3, -0.8, 100.0, 120.0, -60.0).finished(); actual = Pose3::Expmap(xi); EXPECT(assert_equal(expm(xi,10), actual,1e-5)); - EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); - EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-6)); + EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-9)); + EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-9)); +} + +/* ************************************************************************* */ +// 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)); } /* ************************************************************************* */ @@ -255,7 +266,7 @@ TEST( Pose3, inverse) /* ************************************************************************* */ TEST( Pose3, inverseDerivatives2) { - Rot3 R = Rot3::rodriguez(0.3,0.4,-0.5); + Rot3 R = Rot3::Rodrigues(0.3,0.4,-0.5); Point3 t(3.5,-8.2,4.2); Pose3 T(R,t); @@ -377,7 +388,7 @@ TEST( Pose3, transform_to_translate) /* ************************************************************************* */ TEST( Pose3, transform_to_rotate) { - Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3()); + Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3()); Point3 actual = transform.transform_to(Point3(2,1,10)); Point3 expected(-1,2,10); EXPECT(assert_equal(expected, actual, 0.001)); @@ -386,7 +397,7 @@ TEST( Pose3, transform_to_rotate) /* ************************************************************************* */ TEST( Pose3, transform_to) { - Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3(2,4, 0)); + Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3(2,4, 0)); Point3 actual = transform.transform_to(Point3(3,2,10)); Point3 expected(2,1,10); EXPECT(assert_equal(expected, actual, 0.001)); @@ -428,7 +439,7 @@ TEST( Pose3, transformPose_to_itself) TEST( Pose3, transformPose_to_translation) { // transform translation only - Rot3 r = Rot3::rodriguez(-1.570796,0,0); + Rot3 r = Rot3::Rodrigues(-1.570796,0,0); Pose3 pose2(r, Point3(21.,32.,13.)); Pose3 actual = pose2.transform_to(Pose3(Rot3(), Point3(1,2,3))); Pose3 expected(r, Point3(20.,30.,10.)); @@ -439,7 +450,7 @@ TEST( Pose3, transformPose_to_translation) TEST( Pose3, transformPose_to_simple_rotate) { // transform translation only - Rot3 r = Rot3::rodriguez(0,0,-1.570796); + Rot3 r = Rot3::Rodrigues(0,0,-1.570796); Pose3 pose2(r, Point3(21.,32.,13.)); Pose3 transform(r, Point3(1,2,3)); Pose3 actual = pose2.transform_to(transform); @@ -451,12 +462,12 @@ TEST( Pose3, transformPose_to_simple_rotate) TEST( Pose3, transformPose_to) { // transform to - Rot3 r = Rot3::rodriguez(0,0,-1.570796); //-90 degree yaw - Rot3 r2 = Rot3::rodriguez(0,0,0.698131701); //40 degree yaw + Rot3 r = Rot3::Rodrigues(0,0,-1.570796); //-90 degree yaw + Rot3 r2 = Rot3::Rodrigues(0,0,0.698131701); //40 degree yaw Pose3 pose2(r2, Point3(21.,32.,13.)); Pose3 transform(r, Point3(1,2,3)); Pose3 actual = pose2.transform_to(transform); - Pose3 expected(Rot3::rodriguez(0,0,2.26892803), Point3(-30.,20.,10.)); + Pose3 expected(Rot3::Rodrigues(0,0,2.26892803), Point3(-30.,20.,10.)); EXPECT(assert_equal(expected, actual, 0.001)); } @@ -622,6 +633,22 @@ TEST( Pose3, range_pose ) EXPECT(assert_equal(expectedH2,actualH2)); } +/* ************************************************************************* */ +Unit3 bearing_proxy(const Pose3& pose, const Point3& point) { + return pose.bearing(point); +} +TEST( Pose3, bearing ) +{ + Matrix expectedH1, actualH1, expectedH2, actualH2; + EXPECT(assert_equal(Unit3(1,0,0),x1.bearing(l1, actualH1, actualH2),1e-9)); + + // Check numerical derivatives + expectedH1 = numericalDerivative21(bearing_proxy, x1, l1); + expectedH2 = numericalDerivative22(bearing_proxy, x1, l1); + EXPECT(assert_equal(expectedH1,actualH1)); + EXPECT(assert_equal(expectedH2,actualH2)); +} + /* ************************************************************************* */ TEST( Pose3, unicycle ) { @@ -749,15 +776,15 @@ TEST( Pose3, stream) TEST(Pose3 , Invariants) { Pose3 id; - check_group_invariants(id,id); - check_group_invariants(id,T3); - check_group_invariants(T2,id); - check_group_invariants(T2,T3); + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T3)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T3)); - check_manifold_invariants(id,id); - check_manifold_invariants(id,T3); - check_manifold_invariants(T2,id); - check_manifold_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)); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 82d3283bd..0421e1e44 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -107,15 +107,15 @@ TEST(Quaternion , Inverse) { //****************************************************************************** TEST(Quaternion , Invariants) { - check_group_invariants(id, id); - check_group_invariants(id, R1); - check_group_invariants(R2, id); - check_group_invariants(R2, R1); + EXPECT(check_group_invariants(id, id)); + EXPECT(check_group_invariants(id, R1)); + EXPECT(check_group_invariants(R2, id)); + EXPECT(check_group_invariants(R2, R1)); - check_manifold_invariants(id, id); - check_manifold_invariants(id, R1); - check_manifold_invariants(R2, id); - check_manifold_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)); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 37054fd83..6ead22860 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -163,15 +163,15 @@ Rot2 T2(0.2); TEST(Rot2 , Invariants) { Rot2 id; - check_group_invariants(id,id); - check_group_invariants(id,T1); - check_group_invariants(T2,id); - check_group_invariants(T2,T1); + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T1)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T1)); - check_manifold_invariants(id,id); - check_manifold_invariants(id,T1); - check_manifold_invariants(T2,id); - check_manifold_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)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 7e0dcc43f..a61467b82 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -33,7 +33,7 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Rot3) GTSAM_CONCEPT_LIE_INST(Rot3) -static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); +static Rot3 R = Rot3::Rodrigues(0.1, 0.4, 0.2); static Point3 P(0.2, 0.7, -2.0); static double error = 1e-9, epsilon = 0.001; @@ -95,7 +95,7 @@ TEST( Rot3, equals) /* ************************************************************************* */ // Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + .... -Rot3 slow_but_correct_rodriguez(const Vector& w) { +Rot3 slow_but_correct_Rodrigues(const Vector& w) { double t = norm_2(w); Matrix J = skewSymmetric(w / t); if (t < 1e-5) return Rot3(); @@ -104,20 +104,20 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) { } /* ************************************************************************* */ -TEST( Rot3, rodriguez) +TEST( Rot3, Rodrigues) { - Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0); + Rot3 R1 = Rot3::Rodrigues(epsilon, 0, 0); Vector w = (Vector(3) << epsilon, 0., 0.).finished(); - Rot3 R2 = slow_but_correct_rodriguez(w); + Rot3 R2 = slow_but_correct_Rodrigues(w); CHECK(assert_equal(R2,R1)); } /* ************************************************************************* */ -TEST( Rot3, rodriguez2) +TEST( Rot3, Rodrigues2) { Vector axis = Vector3(0., 1., 0.); // rotation around Y double angle = 3.14 / 4.0; - Rot3 actual = Rot3::rodriguez(axis, angle); + Rot3 actual = Rot3::AxisAngle(axis, angle); Rot3 expected(0.707388, 0, 0.706825, 0, 1, 0, -0.706825, 0, 0.707388); @@ -125,26 +125,26 @@ TEST( Rot3, rodriguez2) } /* ************************************************************************* */ -TEST( Rot3, rodriguez3) +TEST( Rot3, Rodrigues3) { Vector w = Vector3(0.1, 0.2, 0.3); - Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w)); - Rot3 R2 = slow_but_correct_rodriguez(w); + Rot3 R1 = Rot3::AxisAngle(w / norm_2(w), norm_2(w)); + Rot3 R2 = slow_but_correct_Rodrigues(w); CHECK(assert_equal(R2,R1)); } /* ************************************************************************* */ -TEST( Rot3, rodriguez4) +TEST( Rot3, Rodrigues4) { Vector axis = Vector3(0., 0., 1.); // rotation around Z double angle = M_PI/2.0; - Rot3 actual = Rot3::rodriguez(axis, angle); + Rot3 actual = Rot3::AxisAngle(axis, angle); double c=cos(angle),s=sin(angle); Rot3 expected(c,-s, 0, s, c, 0, 0, 0, 1); CHECK(assert_equal(expected,actual)); - CHECK(assert_equal(slow_but_correct_rodriguez(axis*angle),actual)); + CHECK(assert_equal(slow_but_correct_Rodrigues(axis*angle),actual)); } /* ************************************************************************* */ @@ -168,7 +168,7 @@ TEST(Rot3, log) #define CHECK_OMEGA(X,Y,Z) \ w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \ - R = Rot3::rodriguez(w); \ + R = Rot3::Rodrigues(w); \ EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12)); // Check zero @@ -201,7 +201,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).finished(); - R = Rot3::rodriguez(w); + R = Rot3::Rodrigues(w); EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R),1e-12)); #else CHECK_OMEGA(x*PI,y*PI,z*PI) @@ -210,7 +210,7 @@ TEST(Rot3, log) // Check 360 degree rotations #define CHECK_OMEGA_ZERO(X,Y,Z) \ w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \ - R = Rot3::rodriguez(w); \ + R = Rot3::Rodrigues(w); \ EXPECT(assert_equal(zero(3), Rot3::Logmap(R))); CHECK_OMEGA_ZERO( 2.0*PI, 0, 0) @@ -312,8 +312,8 @@ TEST( Rot3, jacobianLogmap ) /* ************************************************************************* */ TEST(Rot3, manifold_expmap) { - Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); - Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); + Rot3 gR1 = Rot3::Rodrigues(0.1, 0.4, 0.2); + Rot3 gR2 = Rot3::Rodrigues(0.3, 0.1, 0.7); Rot3 origin; // log behaves correctly @@ -399,8 +399,8 @@ TEST( Rot3, unrotate) /* ************************************************************************* */ TEST( Rot3, compose ) { - Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); - Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5); + Rot3 R1 = Rot3::Rodrigues(0.1, 0.2, 0.3); + Rot3 R2 = Rot3::Rodrigues(0.2, 0.3, 0.5); Rot3 expected = R1 * R2; Matrix actualH1, actualH2; @@ -419,7 +419,7 @@ TEST( Rot3, compose ) /* ************************************************************************* */ TEST( Rot3, inverse ) { - Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); + Rot3 R = Rot3::Rodrigues(0.1, 0.2, 0.3); Rot3 I; Matrix3 actualH; @@ -444,13 +444,13 @@ TEST( Rot3, between ) 0.0, 0.0, 1.0).finished(); EXPECT(assert_equal(expectedr1, r1.matrix())); - Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); + Rot3 R = Rot3::Rodrigues(0.1, 0.4, 0.2); Rot3 origin; EXPECT(assert_equal(R, origin.between(R))); EXPECT(assert_equal(R.inverse(), R.between(origin))); - Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); - Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5); + Rot3 R1 = Rot3::Rodrigues(0.1, 0.2, 0.3); + Rot3 R2 = Rot3::Rodrigues(0.2, 0.3, 0.5); Rot3 expected = R1.inverse() * R2; Matrix actualH1, actualH2; @@ -652,24 +652,24 @@ TEST( Rot3, slerp) } //****************************************************************************** -Rot3 T1(Rot3::rodriguez(Vector3(0, 0, 1), 1)); -Rot3 T2(Rot3::rodriguez(Vector3(0, 1, 0), 2)); +Rot3 T1(Rot3::AxisAngle(Vector3(0, 0, 1), 1)); +Rot3 T2(Rot3::AxisAngle(Vector3(0, 1, 0), 2)); //****************************************************************************** TEST(Rot3 , Invariants) { Rot3 id; - check_group_invariants(id,id); - check_group_invariants(id,T1); - check_group_invariants(T2,id); - check_group_invariants(T2,T1); - check_group_invariants(T1,T2); + 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)); - check_manifold_invariants(id,id); - check_manifold_invariants(id,T1); - check_manifold_invariants(T2,id); - check_manifold_invariants(T2,T1); - check_manifold_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)); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index 12fb94bbc..d05356271 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -28,14 +28,14 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Rot3) GTSAM_CONCEPT_LIE_INST(Rot3) -static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); +static Rot3 R = Rot3::Rodrigues(0.1, 0.4, 0.2); static Point3 P(0.2, 0.7, -2.0); /* ************************************************************************* */ TEST(Rot3, manifold_cayley) { - Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); - Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); + Rot3 gR1 = Rot3::Rodrigues(0.1, 0.4, 0.2); + Rot3 gR2 = Rot3::Rodrigues(0.3, 0.1, 0.7); Rot3 origin; // log behaves correctly diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index acbf3bcf5..bc32e0df0 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -57,15 +57,15 @@ TEST(SO3 , Retract) { //****************************************************************************** TEST(SO3 , Invariants) { - check_group_invariants(id,id); - check_group_invariants(id,R1); - check_group_invariants(R2,id); - check_group_invariants(R2,R1); + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,R1)); + EXPECT(check_group_invariants(R2,id)); + EXPECT(check_group_invariants(R2,R1)); - check_manifold_invariants(id,id); - check_manifold_invariants(id,R1); - check_manifold_invariants(R2,id); - check_manifold_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)); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testSerializationGeometry.cpp b/gtsam/geometry/tests/testSerializationGeometry.cpp index 84fe5980e..dfef0a9c5 100644 --- a/gtsam/geometry/tests/testSerializationGeometry.cpp +++ b/gtsam/geometry/tests/testSerializationGeometry.cpp @@ -16,8 +16,6 @@ * @date Feb 7, 2012 */ -#include -#include #include #include #include diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 002cbe51b..515542298 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -15,20 +15,20 @@ * @brief test SimpleCamera class */ -#include -#include - -#include +#include +#include #include #include -#include +#include +#include +#include using namespace std; using namespace gtsam; static const Cal3_S2 K(625, 625, 0, 0, 0); -static const Pose3 pose1((Matrix)(Matrix(3,3) << +static const Pose3 pose1((Matrix3() << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index c77509b91..3adf2257d 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -96,20 +96,16 @@ static StereoPoint2 project3(const Pose3& pose, const Point3& point, const Cal3_ TEST( StereoCamera, Dproject) { Matrix expected1 = numericalDerivative31(project3, camPose, landmark, *K); - Matrix actual1; stereoCam.project(landmark, actual1, boost::none, boost::none); + 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.project(landmark, boost::none, actual2, boost::none); + Matrix expected2 = numericalDerivative32(project3, camPose, landmark, *K); + Matrix actual2; stereoCam.project2(landmark, boost::none, actual2); CHECK(assert_equal(expected2,actual2,1e-7)); - - Matrix expected3 = numericalDerivative33(project3,camPose, landmark, *K); - Matrix actual3; stereoCam.project(landmark, boost::none, boost::none, actual3); -// CHECK(assert_equal(expected3,actual3,1e-8)); } /* ************************************************************************* */ -TEST( StereoCamera, backproject) +TEST( StereoCamera, backproject_case1) { Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); StereoCamera stereoCam2(Pose3(), K2); @@ -121,7 +117,7 @@ TEST( StereoCamera, backproject) } /* ************************************************************************* */ -TEST( StereoCamera, backproject2) +TEST( StereoCamera, backproject_case2) { Rot3 R(0.589511291, -0.804859792, 0.0683931805, -0.804435942, -0.592650676, -0.0405925523, @@ -136,6 +132,53 @@ TEST( StereoCamera, backproject2) CHECK(assert_equal(z, actual, 1e-3)); } +static Point3 backproject3(const Pose3& pose, const StereoPoint2& point, const Cal3_S2Stereo& K) { + return StereoCamera(pose, boost::make_shared(K)).backproject(point); +} + +/* ************************************************************************* */ +TEST( StereoCamera, backproject2_case1) +{ + Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); + StereoCamera stereoCam2(Pose3(), K2); + + Point3 expected_point(1.2, 2.3, 4.5); + StereoPoint2 stereo_point = stereoCam2.project(expected_point); + + Matrix actual_jacobian_1, actual_jacobian_2; + Point3 actual_point = stereoCam2.backproject2(stereo_point, actual_jacobian_1, actual_jacobian_2); + CHECK(assert_equal(expected_point, actual_point, 1e-8)); + + Matrix expected_jacobian_to_pose = numericalDerivative31(backproject3, Pose3(), stereo_point, *K2); + CHECK(assert_equal(expected_jacobian_to_pose, actual_jacobian_1, 1e-6)); + + Matrix expected_jacobian_to_point = numericalDerivative32(backproject3, Pose3(), stereo_point, *K2); + CHECK(assert_equal(expected_jacobian_to_point, actual_jacobian_2, 1e-6)); +} + +TEST( StereoCamera, backproject2_case2) +{ + Rot3 R(0.589511291, -0.804859792, 0.0683931805, + -0.804435942, -0.592650676, -0.0405925523, + 0.0732045588, -0.0310882277, -0.996832359); + Point3 t(53.5239823, 23.7866016, -4.42379876); + Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612)); + StereoCamera camera(Pose3(R,t), K); + StereoPoint2 z(184.812, 129.068, 714.768); + + Matrix actual_jacobian_1, actual_jacobian_2; + Point3 l = camera.backproject2(z, actual_jacobian_1, actual_jacobian_2); + + StereoPoint2 actual = camera.project(l); + CHECK(assert_equal(z, actual, 1e-3)); + + Matrix expected_jacobian_to_pose = numericalDerivative31(backproject3, Pose3(R,t), z, *K); + CHECK(assert_equal(expected_jacobian_to_pose, actual_jacobian_1, 1e-6)); + + Matrix expected_jacobian_to_point = numericalDerivative32(backproject3, Pose3(R,t), z, *K); + CHECK(assert_equal(expected_jacobian_to_point, actual_jacobian_2, 1e-6)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testStereoPoint2.cpp b/gtsam/geometry/tests/testStereoPoint2.cpp index 5496b8aac..ddcc9238a 100644 --- a/gtsam/geometry/tests/testStereoPoint2.cpp +++ b/gtsam/geometry/tests/testStereoPoint2.cpp @@ -31,7 +31,9 @@ GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2) //****************************************************************************** TEST(StereoPoint2 , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index f986cca1d..bd18143cb 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -17,9 +17,16 @@ */ #include +#include +#include +#include #include +#include +#include +#include #include + #include #include @@ -49,6 +56,7 @@ Point2 z1 = camera1.project(landmark); Point2 z2 = camera2.project(landmark); //****************************************************************************** +// Simple test with a well-behaved two camera situation TEST( triangulation, twoPoses) { vector poses; @@ -57,24 +65,37 @@ TEST( triangulation, twoPoses) { poses += pose1, pose2; measurements += z1, z2; - bool optimize = true; double rank_tol = 1e-9; - boost::optional triangulated_landmark = triangulatePoint3(poses, - sharedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); + // 1. Test simple DLT, perfect in no noise situation + bool optimize = false; + boost::optional actual1 = // + triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(landmark, *actual1, 1e-7)); - // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) + // 2. test with optimization on, same answer + optimize = true; + boost::optional actual2 = // + triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(landmark, *actual2, 1e-7)); + + // 3. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); + optimize = false; + boost::optional actual3 = // + triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4)); - boost::optional triangulated_landmark_noise = triangulatePoint3(poses, - sharedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); + // 4. Now with optimization on + optimize = true; + boost::optional actual4 = // + triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); } //****************************************************************************** - +// Similar, but now with Bundler calibration TEST( triangulation, twoPosesBundler) { boost::shared_ptr bundlerCal = // @@ -95,17 +116,17 @@ TEST( triangulation, twoPosesBundler) { bool optimize = true; double rank_tol = 1e-9; - boost::optional triangulated_landmark = triangulatePoint3(poses, - bundlerCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); + boost::optional actual = // + triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(landmark, *actual, 1e-7)); - // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) + // Add some noise and try again measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional triangulated_landmark_noise = triangulatePoint3(poses, - bundlerCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); + boost::optional actual2 = // + triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-4)); } //****************************************************************************** @@ -116,17 +137,17 @@ TEST( triangulation, fourPoses) { poses += pose1, pose2; measurements += z1, z2; - boost::optional triangulated_landmark = triangulatePoint3(poses, - sharedCal, measurements); - EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); + boost::optional actual = triangulatePoint3(poses, sharedCal, + measurements); + EXPECT(assert_equal(landmark, *actual, 1e-2)); // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional triangulated_landmark_noise = // + boost::optional actual2 = // triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); + EXPECT(assert_equal(landmark, *actual2, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); @@ -150,7 +171,7 @@ TEST( triangulation, fourPoses) { SimpleCamera camera4(pose4, *sharedCal); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException); + CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); poses += pose4; measurements += Point2(400, 400); @@ -180,17 +201,17 @@ TEST( triangulation, fourPoses_distinct_Ks) { cameras += camera1, camera2; measurements += z1, z2; - boost::optional triangulated_landmark = // + boost::optional actual = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); + EXPECT(assert_equal(landmark, *actual, 1e-2)); // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional triangulated_landmark_noise = // + boost::optional actual2 = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); + EXPECT(assert_equal(landmark, *actual2, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); @@ -216,7 +237,7 @@ TEST( triangulation, fourPoses_distinct_Ks) { SimpleCamera camera4(pose4, K4); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException); + CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); cameras += camera4; measurements += Point2(400, 400); @@ -244,23 +265,119 @@ TEST( triangulation, twoIdenticalPoses) { } //****************************************************************************** -/* - TEST( triangulation, onePose) { - // we expect this test to fail with a TriangulationUnderconstrainedException - // because there's only one camera observation +TEST( triangulation, onePose) { + // we expect this test to fail with a TriangulationUnderconstrainedException + // because there's only one camera observation - Cal3_S2 *sharedCal(1500, 1200, 0, 640, 480); + vector poses; + vector measurements; - vector poses; - vector measurements; + poses += Pose3(); + measurements += Point2(); - poses += Pose3(); - measurements += Point2(); + CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), + TriangulationUnderconstrainedException); +} - CHECK_EXCEPTION(triangulatePoint3(poses, measurements, *sharedCal), - TriangulationUnderconstrainedException); - } - */ +//****************************************************************************** +TEST( triangulation, StereotriangulateNonlinear ) { + + Cal3_S2Stereo::shared_ptr stereoK(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612)); + + // two camera poses m1, m2 + Matrix4 m1, m2; + m1 << 0.796888717, 0.603404026, -0.0295271487, 46.6673779, + 0.592783835, -0.77156583, 0.230856632, 66.2186159, + 0.116517574, -0.201470143, -0.9725393, -4.28382528, + 0, 0, 0, 1; + + m2 << -0.955959025, -0.29288915, -0.0189328569, 45.7169799, + -0.29277519, 0.947083213, 0.131587097, 65.843136, + -0.0206094928, 0.131334858, -0.991123524, -4.3525033, + 0, 0, 0, 1; + + typedef CameraSet Cameras; + Cameras cameras; + cameras.push_back(StereoCamera(Pose3(m1), stereoK)); + cameras.push_back(StereoCamera(Pose3(m2), stereoK)); + + vector measurements; + measurements += StereoPoint2(226.936, 175.212, 424.469); + measurements += StereoPoint2(339.571, 285.547, 669.973); + + Point3 initial = Point3(46.0536958, 66.4621179, -6.56285929); // error: 96.5715555191 + + Point3 actual = triangulateNonlinear(cameras, measurements, initial); + + Point3 expected(46.0484569, 66.4710686, -6.55046613); // error: 0.763510644187 + + EXPECT(assert_equal(expected, actual, 1e-4)); + + + // regular stereo factor comparison - expect very similar result as above + { + typedef GenericStereoFactor StereoFactor; + + Values values; + values.insert(Symbol('x', 1), Pose3(m1)); + values.insert(Symbol('x', 2), Pose3(m2)); + values.insert(Symbol('l', 1), initial); + + NonlinearFactorGraph graph; + static SharedNoiseModel unit(noiseModel::Unit::Create(3)); + graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK))); + graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK))); + + const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9); + graph.push_back(PriorFactor(Symbol('x',1), Pose3(m1), posePrior)); + graph.push_back(PriorFactor(Symbol('x',2), Pose3(m2), posePrior)); + + LevenbergMarquardtOptimizer optimizer(graph, values); + Values result = optimizer.optimize(); + + EXPECT(assert_equal(expected, result.at(Symbol('l',1)), 1e-4)); + } + + // use Triangulation Factor directly - expect same result as above + { + Values values; + values.insert(Symbol('l', 1), initial); + + NonlinearFactorGraph graph; + static SharedNoiseModel unit(noiseModel::Unit::Create(3)); + + graph.push_back(TriangulationFactor(cameras[0], measurements[0], unit, Symbol('l',1))); + graph.push_back(TriangulationFactor(cameras[1], measurements[1], unit, Symbol('l',1))); + + LevenbergMarquardtOptimizer optimizer(graph, values); + Values result = optimizer.optimize(); + + EXPECT(assert_equal(expected, result.at(Symbol('l',1)), 1e-4)); + } + + // use ExpressionFactor - expect same result as above + { + Values values; + values.insert(Symbol('l', 1), initial); + + NonlinearFactorGraph graph; + static SharedNoiseModel unit(noiseModel::Unit::Create(3)); + + Expression point_(Symbol('l',1)); + Expression camera0_(cameras[0]); + Expression camera1_(cameras[1]); + Expression project0_(camera0_, &StereoCamera::project2, point_); + Expression project1_(camera1_, &StereoCamera::project2, point_); + + graph.addExpressionFactor(unit, measurements[0], project0_); + graph.addExpressionFactor(unit, measurements[1], project1_); + + LevenbergMarquardtOptimizer optimizer(graph, values); + Values result = optimizer.optimize(); + + EXPECT(assert_equal(expected, result.at(Symbol('l',1)), 1e-4)); + } +} //****************************************************************************** int main() { diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 1d0c28ded..3cfffa0da 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -18,21 +18,30 @@ * @brief Tests the Unit3 class */ -#include -#include #include #include +#include +#include +#include +#include +#include +#include +#include +#include + + #include + #include #include #include -//#include #include #include using namespace boost::assign; using namespace gtsam; using namespace std; +using gtsam::symbol_shorthand::U; GTSAM_CONCEPT_TESTABLE_INST(Unit3) GTSAM_CONCEPT_MANIFOLD_INST(Unit3) @@ -41,6 +50,7 @@ GTSAM_CONCEPT_MANIFOLD_INST(Unit3) Point3 point3_(const Unit3& p) { return p.point3(); } + TEST(Unit3, point3) { vector ps; ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0) @@ -90,6 +100,7 @@ TEST(Unit3, unrotate) { Unit3 expected = Unit3(1, 1, 0); Unit3 actual = R.unrotate(p); EXPECT(assert_equal(expected, actual, 1e-8)); + Matrix actualH, expectedH; // Use numerical derivatives to calculate the expected Jacobian { @@ -104,6 +115,37 @@ TEST(Unit3, unrotate) { } } +TEST(Unit3, dot) { + Unit3 p(1, 0.2, 0.3); + Unit3 q = p.retract(Vector2(0.5, 0)); + Unit3 r = p.retract(Vector2(0.8, 0)); + Unit3 t = p.retract(Vector2(0, 0.3)); + EXPECT(assert_equal(1.0, p.dot(p), 1e-8)); + EXPECT(assert_equal(0.877583, p.dot(q), 1e-5)); + EXPECT(assert_equal(0.696707, p.dot(r), 1e-5)); + EXPECT(assert_equal(0.955336, p.dot(t), 1e-5)); + + // Use numerical derivatives to calculate the expected Jacobians + Matrix H1, H2; + boost::function f = boost::bind(&Unit3::dot, _1, _2, // + boost::none, boost::none); + { + p.dot(q, H1, H2); + EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-9)); + EXPECT(assert_equal(numericalDerivative22(f, p, q), H2, 1e-9)); + } + { + p.dot(r, H1, H2); + EXPECT(assert_equal(numericalDerivative21(f, p, r), H1, 1e-9)); + EXPECT(assert_equal(numericalDerivative22(f, p, r), H2, 1e-9)); + } + { + p.dot(t, H1, H2); + EXPECT(assert_equal(numericalDerivative21(f, p, t), H1, 1e-9)); + EXPECT(assert_equal(numericalDerivative22(f, p, t), H2, 1e-9)); + } +} + //******************************************************************************* TEST(Unit3, error) { Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), // @@ -128,6 +170,45 @@ TEST(Unit3, error) { } } +//******************************************************************************* +TEST(Unit3, error2) { + Unit3 p(0.1, -0.2, 0.8); + Unit3 q = p.retract(Vector2(0.2, -0.1)); + Unit3 r = p.retract(Vector2(0.8, 0)); + + // Hard-coded as simple regression values + EXPECT(assert_equal((Vector)(Vector2(0.0, 0.0)), p.errorVector(p), 1e-8)); + EXPECT(assert_equal((Vector)(Vector2(0.198337495, -0.0991687475)), p.errorVector(q), 1e-5)); + EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.errorVector(r), 1e-5)); + + Matrix actual, expected; + // Use numerical derivatives to calculate the expected Jacobian + { + expected = numericalDerivative21( + boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q); + p.errorVector(q, actual, boost::none); + EXPECT(assert_equal(expected, actual, 1e-9)); + } + { + expected = numericalDerivative21( + boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r); + p.errorVector(r, actual, boost::none); + EXPECT(assert_equal(expected, actual, 1e-9)); + } + { + expected = numericalDerivative22( + boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q); + p.errorVector(q, boost::none, actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + } + { + expected = numericalDerivative22( + boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r); + p.errorVector(r, boost::none, actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + } +} + //******************************************************************************* TEST(Unit3, distance) { Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), // @@ -159,172 +240,139 @@ TEST(Unit3, localCoordinates0) { EXPECT(assert_equal(zero(2), actual, 1e-8)); } -//******************************************************************************* -TEST(Unit3, localCoordinates1) { - Unit3 p, q(1, 6.12385e-21, 0); - Vector actual = p.localCoordinates(q); - CHECK(assert_equal(zero(2), actual, 1e-8)); +TEST(Unit3, localCoordinates) { + { + Unit3 p, q; + Vector2 expected = Vector2::Zero(); + Vector2 actual = p.localCoordinates(q); + EXPECT(assert_equal(zero(2), actual, 1e-8)); + EXPECT(assert_equal(q, p.retract(expected), 1e-8)); + } + { + Unit3 p, q(1, 6.12385e-21, 0); + Vector2 expected = Vector2::Zero(); + Vector2 actual = p.localCoordinates(q); + EXPECT(assert_equal(zero(2), actual, 1e-8)); + EXPECT(assert_equal(q, p.retract(expected), 1e-8)); + } + { + Unit3 p, q(-1, 0, 0); + Vector2 expected(M_PI, 0); + Vector2 actual = p.localCoordinates(q); + EXPECT(assert_equal(expected, actual, 1e-8)); + EXPECT(assert_equal(q, p.retract(expected), 1e-8)); + } + { + Unit3 p, q(0, 1, 0); + Vector2 expected(0,-M_PI_2); + Vector2 actual = p.localCoordinates(q); + EXPECT(assert_equal(expected, actual, 1e-8)); + EXPECT(assert_equal(q, p.retract(expected), 1e-8)); + } + { + Unit3 p, q(0, -1, 0); + Vector2 expected(0, M_PI_2); + Vector2 actual = p.localCoordinates(q); + EXPECT(assert_equal(expected, actual, 1e-8)); + EXPECT(assert_equal(q, p.retract(expected), 1e-8)); + } + { + Unit3 p(0,1,0), q(0,-1,0); + Vector2 actual = p.localCoordinates(q); + EXPECT(assert_equal(q, p.retract(actual), 1e-8)); + } + { + Unit3 p(0,0,1), q(0,0,-1); + Vector2 actual = p.localCoordinates(q); + EXPECT(assert_equal(q, p.retract(actual), 1e-8)); + } + + double twist = 1e-4; + { + Unit3 p(0, 1, 0), q(0 - twist, -1 + twist, 0); + Vector2 actual = p.localCoordinates(q); + EXPECT(actual(0) < 1e-2); + EXPECT(actual(1) > M_PI - 1e-2) + } + { + Unit3 p(0, 1, 0), q(0 + twist, -1 - twist, 0); + Vector2 actual = p.localCoordinates(q); + EXPECT(actual(0) < 1e-2); + EXPECT(actual(1) < -M_PI + 1e-2) + } } //******************************************************************************* -TEST(Unit3, localCoordinates2) { - Unit3 p, q(-1, 0, 0); - Vector expected = (Vector(2) << M_PI, 0).finished(); - Vector actual = p.localCoordinates(q); - CHECK(assert_equal(expected, actual, 1e-8)); +// Wrapper to make basis return a vector6 so we can test numerical derivatives. +Vector6 BasisTest(const Unit3& p, OptionalJacobian<6, 2> H) { + Matrix32 B = p.basis(H); + Vector6 B_vec; + B_vec << B.col(0), B.col(1); + return B_vec; } -//******************************************************************************* TEST(Unit3, basis) { - Unit3 p; + Unit3 p(0.1, -0.2, 0.9); + Matrix expected(3, 2); - expected << 0, 0, 0, -1, 1, 0; - Matrix actual = p.basis(); - EXPECT(assert_equal(expected, actual, 1e-8)); + expected << 0.0, -0.994169047, 0.97618706, + -0.0233922129, 0.216930458, 0.105264958; + + Matrix62 actualH; + Matrix actual = p.basis(actualH); + EXPECT(assert_equal(expected, actual, 1e-6)); + + Matrix62 expectedH = numericalDerivative11( + boost::bind(BasisTest, _1, boost::none), p); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + +//******************************************************************************* +/// Check the basis derivatives of a bunch of random Unit3s. +TEST(Unit3, basis_derivatives) { + int num_tests = 100; + boost::mt19937 rng(42); + for (int i = 0; i < num_tests; i++) { + Unit3 p = Unit3::Random(rng); + + Matrix62 actualH; + p.basis(actualH); + + Matrix62 expectedH = numericalDerivative11( + boost::bind(BasisTest, _1, boost::none), p); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); + } } //******************************************************************************* TEST(Unit3, retract) { - Unit3 p; - Vector v(2); - v << 0.5, 0; - Unit3 expected(0.877583, 0, 0.479426); - Unit3 actual = p.retract(v); - EXPECT(assert_equal(expected, actual, 1e-6)); - EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); + { + Unit3 p; + Vector2 v(0.5, 0); + Unit3 expected(0.877583, 0, 0.479426); + Unit3 actual = p.retract(v); + EXPECT(assert_equal(expected, actual, 1e-6)); + EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); + } + { + Unit3 p; + Vector2 v(0, 0); + Unit3 actual = p.retract(v); + EXPECT(assert_equal(p, actual, 1e-6)); + EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); + } } //******************************************************************************* TEST(Unit3, retract_expmap) { Unit3 p; - Vector v(2); - v << (M_PI / 2.0), 0; + Vector2 v((M_PI / 2.0), 0); Unit3 expected(Point3(0, 0, 1)); Unit3 actual = p.retract(v); EXPECT(assert_equal(expected, actual, 1e-8)); EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); } -//******************************************************************************* -/// Returns a random vector -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; -} - -//******************************************************************************* -// Let x and y be two Unit3's. -// The equality x.localCoordinates(x.retract(v)) == v should hold. -TEST(Unit3, localCoordinates_retract) { - - size_t numIterations = 10000; - Vector minSphereLimit = Vector3(-1.0, -1.0, -1.0), maxSphereLimit = - Vector3(1.0, 1.0, 1.0); - Vector minXiLimit = Vector2(-1.0, -1.0), maxXiLimit = Vector2(1.0, 1.0); - for (size_t i = 0; i < numIterations; i++) { - - // Sleep for the random number generator (TODO?: Better create all of them first). -// boost::this_thread::sleep(boost::posix_time::milliseconds(0)); - - // Create the two Unit3s. - // NOTE: You can not create two totally random Unit3's because you cannot always compute - // between two any Unit3's. (For instance, they might be at the different sides of the circle). - Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit))); -// Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit))); - Vector v12 = randomVector(minXiLimit, maxXiLimit); - Unit3 s2 = s1.retract(v12); - - // Check if the local coordinates and retract return the same results. - Vector actual_v12 = s1.localCoordinates(s2); - EXPECT(assert_equal(v12, actual_v12, 1e-3)); - Unit3 actual_s2 = s1.retract(actual_v12); - EXPECT(assert_equal(s2, actual_s2, 1e-3)); - } -} - -//******************************************************************************* -// Let x and y be two Unit3's. -// The equality x.localCoordinates(x.retract(v)) == v should hold. -TEST(Unit3, localCoordinates_retract_expmap) { - - size_t numIterations = 10000; - Vector minSphereLimit = Vector3(-1.0, -1.0, -1.0), maxSphereLimit = - Vector3(1.0, 1.0, 1.0); - Vector minXiLimit = (Vector(2) << -M_PI, -M_PI).finished(), maxXiLimit = (Vector(2) << M_PI, M_PI).finished(); - for (size_t i = 0; i < numIterations; i++) { - - // Sleep for the random number generator (TODO?: Better create all of them first). -// boost::this_thread::sleep(boost::posix_time::milliseconds(0)); - - // Create the two Unit3s. - // Unlike the above case, we can use any two Unit3's. - Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit))); -// Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit))); - Vector v12 = randomVector(minXiLimit, maxXiLimit); - - // Magnitude of the rotation can be at most pi - if (v12.norm() > M_PI) - v12 = v12 / M_PI; - Unit3 s2 = s1.retract(v12); - - // Check if the local coordinates and retract return the same results. - Vector actual_v12 = s1.localCoordinates(s2); - EXPECT(assert_equal(v12, actual_v12, 1e-3)); - Unit3 actual_s2 = s1.retract(actual_v12); - EXPECT(assert_equal(s2, actual_s2, 1e-3)); - } -} - -//******************************************************************************* -//TEST( Pose2, between ) -//{ -// // < -// // -// // ^ -// // -// // *--0--*--* -// Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y -// Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x -// -// Matrix actualH1,actualH2; -// Pose2 expected(M_PI/2.0, Point2(2,2)); -// Pose2 actual1 = gT1.between(gT2); -// Pose2 actual2 = gT1.between(gT2,actualH1,actualH2); -// EXPECT(assert_equal(expected,actual1)); -// EXPECT(assert_equal(expected,actual2)); -// -// Matrix expectedH1 = (Matrix(3,3) << -// 0.0,-1.0,-2.0, -// 1.0, 0.0,-2.0, -// 0.0, 0.0,-1.0 -// ); -// Matrix numericalH1 = numericalDerivative21(testing::between, gT1, gT2); -// EXPECT(assert_equal(expectedH1,actualH1)); -// EXPECT(assert_equal(numericalH1,actualH1)); -// // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx -// EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1)); -// -// Matrix expectedH2 = (Matrix(3,3) << -// 1.0, 0.0, 0.0, -// 0.0, 1.0, 0.0, -// 0.0, 0.0, 1.0 -// ); -// Matrix numericalH2 = numericalDerivative22(testing::between, gT1, gT2); -// EXPECT(assert_equal(expectedH2,actualH2)); -// EXPECT(assert_equal(numericalH2,actualH2)); -// -//} - //******************************************************************************* TEST(Unit3, Random) { boost::mt19937 rng(42); @@ -336,6 +384,26 @@ TEST(Unit3, Random) { EXPECT(assert_equal(expectedMean,actualMean,0.1)); } +//******************************************************************************* +// New test that uses Unit3::Random +TEST(Unit3, localCoordinates_retract) { + boost::mt19937 rng(42); + size_t numIterations = 10000; + + for (size_t i = 0; i < numIterations; i++) { + // Create two random Unit3s + const Unit3 s1 = Unit3::Random(rng); + const Unit3 s2 = Unit3::Random(rng); + // Check that they are not at opposite ends of the sphere, which is ill defined + if (s1.unitVector().dot(s2.unitVector())<-0.9) continue; + + // Check if the local coordinates and retract return consistent results. + Vector v12 = s1.localCoordinates(s2); + Unit3 actual_s2 = s1.retract(v12); + EXPECT(assert_equal(s2, actual_s2, 1e-9)); + } +} + //************************************************************************* TEST (Unit3, FromPoint3) { Matrix actualH; @@ -347,6 +415,54 @@ TEST (Unit3, FromPoint3) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } +//******************************************************************************* +TEST(Unit3, ErrorBetweenFactor) { + std::vector data; + data.push_back(Unit3(1.0, 0.0, 0.0)); + data.push_back(Unit3(0.0, 0.0, 1.0)); + + NonlinearFactorGraph graph; + Values initial_values; + + // Add prior factors. + SharedNoiseModel R_prior = noiseModel::Unit::Create(2); + for (size_t i = 0; i < data.size(); i++) { + graph.add(PriorFactor(U(i), data[i], R_prior)); + } + + // Add process factors using the dot product error function. + SharedNoiseModel R_process = noiseModel::Isotropic::Sigma(2, 0.01); + for (size_t i = 0; i < data.size() - 1; i++) { + Expression exp(Expression(U(i)), &Unit3::errorVector, Expression(U(i + 1))); + graph.addExpressionFactor(R_process, Vector2::Zero(), exp); + } + + // Add initial values. Since there is no identity, just pick something. + for (size_t i = 0; i < data.size(); i++) { + initial_values.insert(U(i), Unit3(0.0, 1.0, 0.0)); + } + + Values values = GaussNewtonOptimizer(graph, initial_values).optimize(); + + // Check that the y-value is very small for each. + for (size_t i = 0; i < data.size(); i++) { + EXPECT(assert_equal(0.0, values.at(U(i)).unitVector().y(), 1e-3)); + } + + // Check that the dot product between variables is close to 1. + for (size_t i = 0; i < data.size() - 1; i++) { + EXPECT(assert_equal(1.0, values.at(U(i)).dot(values.at(U(i + 1))), 1e-2)); + } +} + +/* ************************************************************************* */ +TEST(actualH, Serialization) { + Unit3 p(0, 1, 0); + EXPECT(serializationTestHelpers::equalsObj(p)); + EXPECT(serializationTestHelpers::equalsXML(p)); + EXPECT(serializationTestHelpers::equalsBinary(p)); +} + /* ************************************************************************* */ int main() { srand(time(NULL)); diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 474689525..c92aa8237 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 @@ -52,12 +46,20 @@ Point3 triangulateDLT(const std::vector& projection_matrices, double error; Vector v; boost::tie(rank, error, v) = DLT(A, rank_tol); - // std::cout << "s " << s.transpose() << std:endl; 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 +91,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 ce83f780b..bec901830 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -18,7 +18,7 @@ #pragma once - +#include #include #include #include @@ -43,6 +43,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) @@ -52,13 +63,12 @@ public: */ GTSAM_EXPORT Point3 triangulateDLT( const std::vector& projection_matrices, - const std::vector& measurements, double rank_tol); + const std::vector& measurements, double rank_tol = 1e-9); -/// /** * Create a factor graph with projection factors from poses and one calibration * @param poses Camera poses - * @param sharedCal shared pointer to single calibration object + * @param sharedCal shared pointer to single calibration object (monocular only!) * @param measurements 2D measurements * @param landmarkKey to refer to landmark * @param initialEstimate @@ -76,8 +86,9 @@ std::pair triangulationGraph( static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); for (size_t i = 0; i < measurements.size(); i++) { const Pose3& pose_i = poses[i]; - PinholeCamera camera_i(pose_i, *sharedCal); - graph.push_back(TriangulationFactor // + typedef PinholePose Camera; + Camera camera_i(pose_i, sharedCal); + graph.push_back(TriangulationFactor // (camera_i, measurements[i], unit2, landmarkKey)); } return std::make_pair(graph, values); @@ -86,31 +97,39 @@ std::pair triangulationGraph( /** * Create a factor graph with projection factors from pinhole cameras * (each camera has a pose and calibration) - * @param cameras pinhole cameras + * @param cameras pinhole cameras (monocular or stereo) * @param measurements 2D measurements * @param landmarkKey to refer to landmark * @param initialEstimate * @return graph and initial values */ +template +std::pair triangulationGraph( + const std::vector& cameras, + const std::vector& measurements, Key landmarkKey, + const Point3& initialEstimate) { + Values values; + values.insert(landmarkKey, initialEstimate); // Initial landmark value + NonlinearFactorGraph graph; + static SharedNoiseModel unit(noiseModel::Unit::Create(CAMERA::Measurement::dimension)); + for (size_t i = 0; i < measurements.size(); i++) { + const CAMERA& camera_i = cameras[i]; + graph.push_back(TriangulationFactor // + (camera_i, measurements[i], unit, landmarkKey)); + } + return std::make_pair(graph, values); +} + +/// PinholeCamera specific version // TODO: (chris) why does this exist? template std::pair triangulationGraph( const std::vector >& cameras, const std::vector& measurements, Key landmarkKey, const Point3& initialEstimate) { - Values values; - values.insert(landmarkKey, initialEstimate); // Initial landmark value - NonlinearFactorGraph graph; - 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 // - (camera_i, measurements[i], unit2, landmarkKey)); - } - return std::make_pair(graph, values); + return triangulationGraph > // + (cameras, measurements, landmarkKey, initialEstimate); } -/// /** * Optimize for triangulation * @param graph nonlinear factors for projection @@ -137,33 +156,61 @@ Point3 triangulateNonlinear(const std::vector& poses, // Create a factor graph and initial values Values values; NonlinearFactorGraph graph; - boost::tie(graph, values) = triangulationGraph(poses, sharedCal, measurements, - Symbol('p', 0), initialEstimate); + boost::tie(graph, values) = triangulationGraph // + (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate); return optimize(graph, values, Symbol('p', 0)); } /** * Given an initial estimate , refine a point using measurements in several cameras - * @param cameras pinhole cameras + * @param cameras pinhole cameras (monocular or stereo) * @param measurements 2D measurements * @param initialEstimate * @return refined Point3 */ -template +template Point3 triangulateNonlinear( - const std::vector >& cameras, - const std::vector& measurements, const Point3& initialEstimate) { + const std::vector& cameras, + const std::vector& measurements, const Point3& initialEstimate) { // Create a factor graph and initial values Values values; NonlinearFactorGraph graph; - boost::tie(graph, values) = triangulationGraph(cameras, measurements, - Symbol('p', 0), initialEstimate); + boost::tie(graph, values) = triangulationGraph // + (cameras, measurements, Symbol('p', 0), initialEstimate); return optimize(graph, values, Symbol('p', 0)); } +/// PinholeCamera specific version // TODO: (chris) why does this exist? +template +Point3 triangulateNonlinear( + const std::vector >& cameras, + const std::vector& measurements, const Point3& initialEstimate) { + return triangulateNonlinear > // + (cameras, measurements, initialEstimate); +} + +/** + * 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 @@ -188,23 +235,24 @@ Point3 triangulatePoint3(const std::vector& poses, // construct projection matrices from poses & calibration std::vector projection_matrices; - BOOST_FOREACH(const Pose3& pose, poses) { - projection_matrices.push_back( - sharedCal->K() * (pose.inverse().matrix()).block<3,4>(0,0)); - } + 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); - // The n refine using non-linear optimization + // Then refine using non-linear optimization if (optimize) - point = triangulateNonlinear(poses, sharedCal, measurements, point); + point = triangulateNonlinear // + (poses, sharedCal, measurements, point); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - // verify that the triangulated point lies infront of all cameras + // verify that the triangulated point lies in front of all cameras BOOST_FOREACH(const Pose3& pose, poses) { const Point3& p_local = pose.transform_to(point); if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + throw(TriangulationCheiralityException()); } #endif @@ -223,42 +271,205 @@ 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; - BOOST_FOREACH(const Camera& camera, cameras) { - Matrix P_ = (camera.pose().inverse().matrix()); - projection_matrices.push_back(camera.calibration().K()* (P_.block<3,4>(0,0)) ); - } + BOOST_FOREACH(const CAMERA& camera, cameras) + projection_matrices.push_back( + CameraProjectionMatrix(camera.calibration())( + camera.pose())); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); // The n refine using non-linear optimization if (optimize) - point = triangulateNonlinear(cameras, measurements, point); + point = triangulateNonlinear(cameras, measurements, point); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - // verify that the triangulated point lies infront of all cameras - BOOST_FOREACH(const Camera& camera, cameras) { + // verify that the triangulated point lies in front of all cameras + 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 return point; } +/// Pinhole-specific version +template +Point3 triangulatePoint3( + const std::vector >& cameras, + const std::vector& measurements, double rank_tol = 1e-9, + bool optimize = false) { + return triangulatePoint3 > // + (cameras, measurements, rank_tol, optimize); +} + +struct TriangulationParameters { + + double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate + bool enableEPI; ///< if set to true, will refine triangulation using LM + + /** + * if the landmark is triangulated at distance larger than this, + * result is flagged as degenerate. + */ + double landmarkDistanceThreshold; // + + /** + * If this is nonnegative the we will check if the average reprojection error + * is smaller than this threshold after triangulation, otherwise result is + * flagged as degenerate. + */ + double dynamicOutlierRejectionThreshold; + + /** + * Constructor + * @param rankTol tolerance used to check if point triangulation is degenerate + * @param enableEPI if true refine triangulation with embedded LM iterations + * @param landmarkDistanceThreshold flag as degenerate if point further than this + * @param dynamicOutlierRejectionThreshold or if average error larger than this + * + */ + TriangulationParameters(const double _rankTolerance = 1.0, + const bool _enableEPI = false, double _landmarkDistanceThreshold = -1, + double _dynamicOutlierRejectionThreshold = -1) : + rankTolerance(_rankTolerance), enableEPI(_enableEPI), // + landmarkDistanceThreshold(_landmarkDistanceThreshold), // + dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold) { + } + + // stream to output + friend std::ostream &operator<<(std::ostream &os, + const TriangulationParameters& p) { + os << "rankTolerance = " << p.rankTolerance << std::endl; + os << "enableEPI = " << p.enableEPI << std::endl; + os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold + << std::endl; + os << "dynamicOutlierRejectionThreshold = " + << p.dynamicOutlierRejectionThreshold << std::endl; + return os; + } +}; + +/** + * TriangulationResult is an optional point, along with the reasons why it is invalid. + */ +class TriangulationResult: public boost::optional { + enum Status { + VALID, DEGENERATE, BEHIND_CAMERA + }; + Status status_; + TriangulationResult(Status s) : + status_(s) { + } +public: + + /** + * Default constructor, only for serialization + */ + TriangulationResult() {} + + /** + * Constructor + */ + TriangulationResult(const Point3& p) : + status_(VALID) { + reset(p); + } + static TriangulationResult Degenerate() { + return TriangulationResult(DEGENERATE); + } + static TriangulationResult BehindCamera() { + return TriangulationResult(BEHIND_CAMERA); + } + bool degenerate() const { + return status_ == DEGENERATE; + } + bool behindCamera() const { + return status_ == BEHIND_CAMERA; + } + // stream to output + friend std::ostream &operator<<(std::ostream &os, + const TriangulationResult& result) { + if (result) + os << "point = " << *result << std::endl; + else + os << "no point, status = " << result.status_ << std::endl; + return os; + } +}; + +/// triangulateSafe: extensive checking of the outcome +template +TriangulationResult triangulateSafe(const std::vector& cameras, + const std::vector& measured, + const TriangulationParameters& params) { + + size_t m = cameras.size(); + + // if we have a single pose the corresponding factor is uninformative + if (m < 2) + return TriangulationResult::Degenerate(); + else + // We triangulate the 3D position of the landmark + try { + Point3 point = triangulatePoint3(cameras, measured, + params.rankTolerance, params.enableEPI); + + // Check landmark distance and re-projection errors to avoid outliers + size_t i = 0; + double totalReprojError = 0.0; + BOOST_FOREACH(const CAMERA& camera, cameras) { + const Pose3& pose = camera.pose(); + if (params.landmarkDistanceThreshold > 0 + && pose.translation().distance(point) + > params.landmarkDistanceThreshold) + return TriangulationResult::Degenerate(); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + // verify that the triangulated point lies in front of all cameras + // Only needed if this was not yet handled by exception + const Point3& p_local = pose.transform_to(point); + if (p_local.z() <= 0) + return TriangulationResult::BehindCamera(); +#endif + // Check reprojection error + if (params.dynamicOutlierRejectionThreshold > 0) { + const Point2& zi = measured.at(i); + Point2 reprojectionError(camera.project(point) - zi); + totalReprojError += reprojectionError.vector().norm(); + } + i += 1; + } + // Flag as degenerate if average reprojection error is too large + if (params.dynamicOutlierRejectionThreshold > 0 + && totalReprojError / m > params.dynamicOutlierRejectionThreshold) + return TriangulationResult::Degenerate(); + + // all good! + return TriangulationResult(point); + } catch (TriangulationUnderconstrainedException&) { + // This exception is thrown if + // 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) + return TriangulationResult::Degenerate(); + } catch (TriangulationCheiralityException&) { + // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers + return TriangulationResult::BehindCamera(); + } +} + } // \namespace gtsam diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 281648409..3a3e1317c 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -344,7 +344,7 @@ namespace gtsam { gttic(Full_root_factoring); boost::shared_ptr p_C1_B; { FastVector C1_minus_B; { - FastSet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); + KeySet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); BOOST_FOREACH(const Key j, *B->conditional()) { C1_minus_B_set.erase(j); } C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end()); @@ -356,7 +356,7 @@ namespace gtsam { } boost::shared_ptr p_C2_B; { FastVector C2_minus_B; { - FastSet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); + KeySet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); BOOST_FOREACH(const Key j, *B->conditional()) { C2_minus_B_set.erase(j); } C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end()); diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 830ddd3ec..4d68acb5b 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -19,13 +19,13 @@ #pragma once -#include - -#include +#include #include #include #include +#include + namespace gtsam { // Forward declarations @@ -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-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index 274886c21..256ff983d 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -44,8 +44,8 @@ namespace gtsam { FastVector BayesTreeCliqueBase::separator_setminus_B(const derived_ptr& B) const { - FastSet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents()); - FastSet indicesB(B->conditional()->begin(), B->conditional()->end()); + KeySet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents()); + KeySet indicesB(B->conditional()->begin(), B->conditional()->end()); FastVector S_setminus_B; std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(), indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B)); @@ -58,8 +58,8 @@ namespace gtsam { const derived_ptr& B, const FactorGraphType& p_Cp_B) const { gttic(shortcut_indices); - FastSet allKeys = p_Cp_B.keys(); - FastSet indicesB(B->conditional()->begin(), B->conditional()->end()); + KeySet allKeys = p_Cp_B.keys(); + KeySet indicesB(B->conditional()->begin(), B->conditional()->end()); FastVector S_setminus_B = separator_setminus_B(B); FastVector keep; // keep = S\B intersect allKeys (S_setminus_B is already sorted) 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/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index 13130bf2e..6b28f2dbf 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -7,180 +7,233 @@ * @brief Collects factorgraph fragments defined on variable clusters, arranged in a tree */ -#include -#include #include #include #include +#include +#include #include #include -namespace gtsam -{ - namespace - { - /* ************************************************************************* */ - // Elimination traversal data - stores a pointer to the parent data and collects the factors - // resulting from elimination of the children. Also sets up BayesTree cliques with parent and - // child pointers. - template - struct EliminationData { - EliminationData* const parentData; - size_t myIndexInParent; - FastVector childFactors; - boost::shared_ptr bayesTreeNode; - EliminationData(EliminationData* _parentData, size_t nChildren) : - parentData(_parentData), - bayesTreeNode(boost::make_shared()) - { - if(parentData) { - myIndexInParent = parentData->childFactors.size(); - parentData->childFactors.push_back(typename CLUSTERTREE::sharedFactor()); - } else { - myIndexInParent = 0; - } - // Set up BayesTree parent and child pointers - if(parentData) { - if(parentData->parentData) // If our parent is not the dummy node - bayesTreeNode->parent_ = parentData->bayesTreeNode; - parentData->bayesTreeNode->children.push_back(bayesTreeNode); - } - } - }; +namespace gtsam { - /* ************************************************************************* */ - // Elimination pre-order visitor - just creates the EliminationData structure for the visited - // node. - template - EliminationData eliminationPreOrderVisitor( - const typename CLUSTERTREE::sharedNode& node, EliminationData& parentData) - { - EliminationData myData(&parentData, node->children.size()); - myData.bayesTreeNode->problemSize_ = node->problemSize(); - return myData; +/* ************************************************************************* */ +// Elimination traversal data - stores a pointer to the parent data and collects +// the factors resulting from elimination of the children. Also sets up BayesTree +// cliques with parent and child pointers. +template +struct EliminationData { + // Typedefs + typedef typename CLUSTERTREE::sharedFactor sharedFactor; + typedef typename CLUSTERTREE::FactorType FactorType; + typedef typename CLUSTERTREE::FactorGraphType FactorGraphType; + typedef typename CLUSTERTREE::ConditionalType ConditionalType; + typedef typename CLUSTERTREE::BayesTreeType::Node BTNode; + + EliminationData* const parentData; + size_t myIndexInParent; + FastVector childFactors; + boost::shared_ptr bayesTreeNode; + EliminationData(EliminationData* _parentData, size_t nChildren) : + parentData(_parentData), bayesTreeNode(boost::make_shared()) { + if (parentData) { + myIndexInParent = parentData->childFactors.size(); + parentData->childFactors.push_back(sharedFactor()); + } else { + myIndexInParent = 0; } + // Set up BayesTree parent and child pointers + if (parentData) { + if (parentData->parentData) // If our parent is not the dummy node + bayesTreeNode->parent_ = parentData->bayesTreeNode; + parentData->bayesTreeNode->children.push_back(bayesTreeNode); + } + } - /* ************************************************************************* */ - // Elimination post-order visitor - combine the child factors with our own factors, add the - // resulting conditional to the BayesTree, and add the remaining factor to the parent. - template - struct EliminationPostOrderVisitor - { - const typename CLUSTERTREE::Eliminate& eliminationFunction; - typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex; - EliminationPostOrderVisitor(const typename CLUSTERTREE::Eliminate& eliminationFunction, + // Elimination pre-order visitor - creates the EliminationData structure for the visited node. + static EliminationData EliminationPreOrderVisitor( + const typename CLUSTERTREE::sharedNode& node, + EliminationData& parentData) { + assert(node); + EliminationData myData(&parentData, node->children.size()); + myData.bayesTreeNode->problemSize_ = node->problemSize(); + return myData; + } + + // Elimination post-order visitor - combine the child factors with our own factors, add the + // resulting conditional to the BayesTree, and add the remaining factor to the parent. + class EliminationPostOrderVisitor { + const typename CLUSTERTREE::Eliminate& eliminationFunction_; + typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex_; + + public: + // Construct functor + EliminationPostOrderVisitor( + const typename CLUSTERTREE::Eliminate& eliminationFunction, typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex) : - eliminationFunction(eliminationFunction), nodesIndex(nodesIndex) {} - void operator()(const typename CLUSTERTREE::sharedNode& node, EliminationData& myData) - { - // Typedefs - typedef typename CLUSTERTREE::sharedFactor sharedFactor; - typedef typename CLUSTERTREE::FactorType FactorType; - typedef typename CLUSTERTREE::FactorGraphType FactorGraphType; - typedef typename CLUSTERTREE::ConditionalType ConditionalType; - typedef typename CLUSTERTREE::BayesTreeType::Node BTNode; - - // Gather factors - FactorGraphType gatheredFactors; - gatheredFactors.reserve(node->factors.size() + node->children.size()); - gatheredFactors += node->factors; - gatheredFactors += myData.childFactors; - - // Check for Bayes tree orphan subtrees, and add them to our children - BOOST_FOREACH(const sharedFactor& f, node->factors) - { - if(const BayesTreeOrphanWrapper* asSubtree = dynamic_cast*>(f.get())) - { - myData.bayesTreeNode->children.push_back(asSubtree->clique); - asSubtree->clique->parent_ = myData.bayesTreeNode; - } - } - - // Do dense elimination step - std::pair, boost::shared_ptr > eliminationResult = - eliminationFunction(gatheredFactors, Ordering(node->keys)); - - // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor - myData.bayesTreeNode->setEliminationResult(eliminationResult); - - // Fill nodes index - we do this here instead of calling insertRoot at the end to avoid - // putting orphan subtrees in the index - they'll already be in the index of the ISAM2 - // object they're added to. - BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals()) - nodesIndex.insert(std::make_pair(j, myData.bayesTreeNode)); - - // Store remaining factor in parent's gathered factors - if(!eliminationResult.second->empty()) - myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second; - } - }; - } - - /* ************************************************************************* */ - template - void ClusterTree::Cluster::print( - const std::string& s, const KeyFormatter& keyFormatter) const - { - std::cout << s; - BOOST_FOREACH(Key j, keys) - std::cout << j << " "; - std::cout << "problemSize = " << problemSize_ << std::endl; - } - - /* ************************************************************************* */ - template - void ClusterTree::print( - const std::string& s, const KeyFormatter& keyFormatter) const - { - treeTraversal::PrintForest(*this, s, keyFormatter); - } - - /* ************************************************************************* */ - template - ClusterTree& ClusterTree::operator=(const This& other) - { - // Start by duplicating the tree. - roots_ = treeTraversal::CloneForest(other); - - // Assign the remaining factors - these are pointers to factors in the original factor graph and - // we do not clone them. - remainingFactors_ = other.remainingFactors_; - - return *this; - } - - /* ************************************************************************* */ - template - std::pair, boost::shared_ptr > - ClusterTree::eliminate(const Eliminate& function) const - { - gttic(ClusterTree_eliminate); - // Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node - // that contains all of the roots as its children. rootsContainer also stores the remaining - // uneliminated factors passed up from the roots. - boost::shared_ptr result = boost::make_shared(); - EliminationData rootsContainer(0, roots_.size()); - EliminationPostOrderVisitor visitorPost(function, result->nodes_); - { - TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP - treeTraversal::DepthFirstForestParallel(*this, rootsContainer, - eliminationPreOrderVisitor, visitorPost, 10); + eliminationFunction_(eliminationFunction), nodesIndex_(nodesIndex) { } - // Create BayesTree from roots stored in the dummy BayesTree node. - result->roots_.insert(result->roots_.end(), rootsContainer.bayesTreeNode->children.begin(), rootsContainer.bayesTreeNode->children.end()); + // Function that does the HEAVY lifting + void operator()(const typename CLUSTERTREE::sharedNode& node, + EliminationData& myData) { + assert(node); - // Add remaining factors that were not involved with eliminated variables - boost::shared_ptr allRemainingFactors = boost::make_shared(); - allRemainingFactors->reserve(remainingFactors_.size() + rootsContainer.childFactors.size()); - allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end()); - BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors) - if(factor) - allRemainingFactors->push_back(factor); + // Gather factors + FactorGraphType gatheredFactors; + gatheredFactors.reserve(node->factors.size() + node->children.size()); + gatheredFactors += node->factors; + gatheredFactors += myData.childFactors; - // Return result - return std::make_pair(result, allRemainingFactors); + // Check for Bayes tree orphan subtrees, and add them to our children + BOOST_FOREACH(const sharedFactor& f, node->factors) { + if (const BayesTreeOrphanWrapper* asSubtree = + dynamic_cast*>(f.get())) { + myData.bayesTreeNode->children.push_back(asSubtree->clique); + asSubtree->clique->parent_ = myData.bayesTreeNode; + } + } + + // >>>>>>>>>>>>>> Do dense elimination step >>>>>>>>>>>>>>>>>>>>>>>>>>>>> + std::pair, + boost::shared_ptr > eliminationResult = + eliminationFunction_(gatheredFactors, node->orderedFrontalKeys); + // <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< + + // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor + myData.bayesTreeNode->setEliminationResult(eliminationResult); + + // Fill nodes index - we do this here instead of calling insertRoot at the end to avoid + // putting orphan subtrees in the index - they'll already be in the index of the ISAM2 + // object they're added to. + BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals()) + nodesIndex_.insert(std::make_pair(j, myData.bayesTreeNode)); + + // Store remaining factor in parent's gathered factors + if (!eliminationResult.second->empty()) + myData.parentData->childFactors[myData.myIndexInParent] = + eliminationResult.second; + } + }; +}; + +/* ************************************************************************* */ +template +void ClusterTree::Cluster::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + std::cout << s << " (" << problemSize_ << ")"; + PrintKeyVector(orderedFrontalKeys); +} + +/* ************************************************************************* */ +template +void ClusterTree::Cluster::mergeChildren( + const std::vector& merge) { + gttic(Cluster_mergeChildren); + + // Count how many keys, factors and children we'll end up with + size_t nrKeys = orderedFrontalKeys.size(); + size_t nrFactors = factors.size(); + size_t nrNewChildren = 0; + // Loop over children + size_t i = 0; + BOOST_FOREACH(const sharedNode& child, children) { + if (merge[i]) { + nrKeys += child->orderedFrontalKeys.size(); + nrFactors += child->factors.size(); + nrNewChildren += child->children.size(); + } else { + nrNewChildren += 1; // we keep the child + } + ++i; } + // now reserve space, and really merge + orderedFrontalKeys.reserve(nrKeys); + factors.reserve(nrFactors); + typename Node::Children newChildren; + newChildren.reserve(nrNewChildren); + i = 0; + BOOST_FOREACH(const sharedNode& child, children) { + if (merge[i]) { + // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. + orderedFrontalKeys.insert(orderedFrontalKeys.end(), + child->orderedFrontalKeys.rbegin(), child->orderedFrontalKeys.rend()); + // Merge keys, factors, and children. + factors.insert(factors.end(), child->factors.begin(), + child->factors.end()); + newChildren.insert(newChildren.end(), child->children.begin(), + child->children.end()); + // Increment problem size + problemSize_ = std::max(problemSize_, child->problemSize_); + // Increment number of frontal variables + } else { + newChildren.push_back(child); // we keep the child + } + ++i; + } + children = newChildren; + std::reverse(orderedFrontalKeys.begin(), orderedFrontalKeys.end()); +} + +/* ************************************************************************* */ +template +void ClusterTree::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + treeTraversal::PrintForest(*this, s, keyFormatter); +} + +/* ************************************************************************* */ +template +ClusterTree& ClusterTree::operator=( + const This& other) { + // Start by duplicating the tree. + roots_ = treeTraversal::CloneForest(other); + + // Assign the remaining factors - these are pointers to factors in the original factor graph and + // we do not clone them. + remainingFactors_ = other.remainingFactors_; + + return *this; +} + +/* ************************************************************************* */ +template +std::pair, boost::shared_ptr > ClusterTree< + BAYESTREE, GRAPH>::eliminate(const Eliminate& function) const { + gttic(ClusterTree_eliminate); + // Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node + // that contains all of the roots as its children. rootsContainer also stores the remaining + // uneliminated factors passed up from the roots. + boost::shared_ptr result = boost::make_shared(); + typedef EliminationData Data; + Data rootsContainer(0, roots_.size()); + typename Data::EliminationPostOrderVisitor visitorPost(function, + result->nodes_); + { + TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP + treeTraversal::DepthFirstForestParallel(*this, rootsContainer, + Data::EliminationPreOrderVisitor, visitorPost, 10); + } + + // Create BayesTree from roots stored in the dummy BayesTree node. + result->roots_.insert(result->roots_.end(), + rootsContainer.bayesTreeNode->children.begin(), + rootsContainer.bayesTreeNode->children.end()); + + // Add remaining factors that were not involved with eliminated variables + boost::shared_ptr remaining = boost::make_shared< + FactorGraphType>(); + remaining->reserve( + remainingFactors_.size() + rootsContainer.childFactors.size()); + remaining->push_back(remainingFactors_.begin(), remainingFactors_.end()); + BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors) { + if (factor) + remaining->push_back(factor); + } + // Return result + return std::make_pair(result, remaining); +} + } diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index 5a412a79e..b00532d22 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -11,115 +11,124 @@ #include #include -#include +#include -namespace gtsam -{ +namespace gtsam { - /** - * A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: - * each node k represents a subset \f$ C_k \sub X \f$, and the tree is family preserving, in that - * each factor \f$ f_i \f$ is associated with a single cluster and \f$ scope(f_i) \sub C_k \f$. - * \nosubgrouping - */ - template - class ClusterTree - { - public: - typedef GRAPH FactorGraphType; ///< The factor graph type - typedef typename GRAPH::FactorType FactorType; ///< The type of factors - typedef ClusterTree This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor - typedef BAYESTREE BayesTreeType; ///< The BayesTree type produced by elimination - typedef typename BayesTreeType::ConditionalType ConditionalType; ///< The type of conditionals - typedef boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional - typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine +/** + * A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: + * each node k represents a subset \f$ C_k \sub X \f$, and the tree is family preserving, in that + * each factor \f$ f_i \f$ is associated with a single cluster and \f$ scope(f_i) \sub C_k \f$. + * \nosubgrouping + */ +template +class ClusterTree { +public: + typedef GRAPH FactorGraphType; ///< The factor graph type + typedef typename GRAPH::FactorType FactorType; ///< The type of factors + typedef ClusterTree This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor + typedef BAYESTREE BayesTreeType; ///< The BayesTree type produced by elimination + typedef typename BayesTreeType::ConditionalType ConditionalType; ///< The type of conditionals + typedef boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional + typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine - struct Cluster { - typedef FastVector Keys; - typedef FastVector Factors; - typedef FastVector > Children; + struct Cluster { + typedef Ordering Keys; + typedef FastVector Factors; + typedef FastVector > Children; - Keys keys; ///< Frontal keys of this node - Factors factors; ///< Factors associated with this node - Children children; ///< sub-trees - int problemSize_; + Cluster() { + } + Cluster(Key key, const Factors& factors) : + factors(factors) { + orderedFrontalKeys.push_back(key); + } - int problemSize() const { return problemSize_; } + Keys orderedFrontalKeys; ///< Frontal keys of this node + Factors factors; ///< Factors associated with this node + Children children; ///< sub-trees + int problemSize_; - /** print this node */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - }; + int problemSize() const { + return problemSize_; + } - typedef boost::shared_ptr sharedCluster; ///< Shared pointer to Cluster - typedef Cluster Node; ///< Define Node=Cluster for compatibility with tree traversal functions - typedef sharedCluster sharedNode; ///< Define Node=Cluster for compatibility with tree traversal functions - - /** concept check */ - GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); - - protected: - FastVector roots_; - FastVector remainingFactors_; - - /// @name Standard Constructors - /// @{ - - /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are - * copied, factors are not cloned. */ - ClusterTree(const This& other) { *this = other; } - - /// @} - - public: - /// @name Testable - /// @{ - - /** Print the cluster tree */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - - /// @} - - /// @name Standard Interface - /// @{ - - /** Eliminate the factors to a Bayes tree and remaining factor graph - * @param function The function to use to eliminate, see the namespace functions - * in GaussianFactorGraph.h - * @return The Bayes tree and factor graph resulting from elimination - */ - std::pair, boost::shared_ptr > - eliminate(const Eliminate& function) const; - - /// @} - - /// @name Advanced Interface - /// @{ - - /** Return the set of roots (one for a tree, multiple for a forest) */ - const FastVector& roots() const { return roots_; } - - /** Return the remaining factors that are not pulled into elimination */ - const FastVector& remainingFactors() const { return remainingFactors_; } - - /// @} - - protected: - /// @name Details - - /// Assignment operator - makes a deep copy of the tree structure, but only pointers to factors - /// are copied, factors are not cloned. - This& operator=(const This& other); - - /// Default constructor to be used in derived classes - ClusterTree() {} - - /// @} + /// print this node + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; + /// Merge all children for which bit is set into this node + void mergeChildren(const std::vector& merge); }; + typedef boost::shared_ptr sharedCluster; ///< Shared pointer to Cluster + typedef Cluster Node; ///< Define Node=Cluster for compatibility with tree traversal functions + typedef sharedCluster sharedNode; ///< Define Node=Cluster for compatibility with tree traversal functions + + /** concept check */ + GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); + +protected: + FastVector roots_; + FastVector remainingFactors_; + + /// @name Standard Constructors + /// @{ + + /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are + * copied, factors are not cloned. */ + ClusterTree(const This& other) {*this = other;} + + /// @} + +public: + /// @name Testable + /// @{ + + /** Print the cluster tree */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// @} + + /// @name Standard Interface + /// @{ + + /** Eliminate the factors to a Bayes tree and remaining factor graph + * @param function The function to use to eliminate, see the namespace functions + * in GaussianFactorGraph.h + * @return The Bayes tree and factor graph resulting from elimination + */ + std::pair, boost::shared_ptr > + eliminate(const Eliminate& function) const; + + /// @} + + /// @name Advanced Interface + /// @{ + + /** Return the set of roots (one for a tree, multiple for a forest) */ + const FastVector& roots() const {return roots_;} + + /** Return the remaining factors that are not pulled into elimination */ + const FastVector& remainingFactors() const {return remainingFactors_;} + + /// @} + +protected: + /// @name Details + + /// Assignment operator - makes a deep copy of the tree structure, but only pointers to factors + /// are copied, factors are not cloned. + This& operator=(const This& other); + + /// Default constructor to be used in derived classes + ClusterTree() {} + + /// @} + +}; } - 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 5e261e200..b4fc3b3a6 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -55,9 +55,9 @@ namespace gtsam { // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. if (orderingType == Ordering::METIS) - return eliminateSequential(Ordering::metis(asDerived()), function, variableIndex, orderingType); - else - return eliminateSequential(Ordering::colamd(*variableIndex), function, variableIndex, orderingType); + return eliminateSequential(Ordering::Metis(asDerived()), function, variableIndex, orderingType); + else + return eliminateSequential(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType); } } @@ -93,9 +93,9 @@ namespace gtsam { // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. if (orderingType == Ordering::METIS) - return eliminateMultifrontal(Ordering::metis(asDerived()), function, variableIndex, orderingType); + return eliminateMultifrontal(Ordering::Metis(asDerived()), function, variableIndex, orderingType); else - return eliminateMultifrontal(Ordering::colamd(*variableIndex), function, variableIndex, orderingType); + return eliminateMultifrontal(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType); } } @@ -125,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()); @@ -163,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()); @@ -216,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(); @@ -275,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(); @@ -301,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 5fb5fbdb1..f5431db3d 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -128,7 +128,8 @@ namespace gtsam { 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. + * provided, the ordering will be computed using either COLAMD or METIS, dependeing on + * the parameter orderingType (Ordering::COLAMD or Ordering::METIS) * * Example - Full Cholesky elimination in COLAMD order: * \code diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h index 70b0dd393..68e623b68 100644 --- a/gtsam/inference/EliminationTree-inst.h +++ b/gtsam/inference/EliminationTree-inst.h @@ -101,10 +101,12 @@ namespace gtsam { { // Retrieve the factors involving this variable and create the current node const VariableIndex::Factors& factors = structure[order[j]]; - nodes[j] = boost::make_shared(); - nodes[j]->key = order[j]; + const sharedNode node = boost::make_shared(); + node->key = order[j]; // for row i \in Struct[A*j] do + node->children.reserve(factors.size()); + node->factors.reserve(factors.size()); BOOST_FOREACH(const size_t i, factors) { // If we already hit a variable in this factor, make the subtree containing the previous // variable in this factor a child of the current node. This means that the variables @@ -123,16 +125,16 @@ namespace gtsam { if (r != j) { // Now that we found the root, hook up parent and child pointers in the nodes. parents[r] = j; - nodes[j]->children.push_back(nodes[r]); + node->children.push_back(nodes[r]); } } else { - // Add the current factor to the current node since we are at the first variable in this - // factor. - nodes[j]->factors.push_back(graph[i]); + // Add the factor to the current node since we are at the first variable in this factor. + node->factors.push_back(graph[i]); factorUsed[i] = true; } prevCol[i] = j; } + nodes[j] = node; } } catch(std::invalid_argument& e) { // If this is thrown from structure[order[j]] above, it means that it was requested to 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-inst.h b/gtsam/inference/FactorGraph-inst.h index 8c19d4ff4..c629d336a 100644 --- a/gtsam/inference/FactorGraph-inst.h +++ b/gtsam/inference/FactorGraph-inst.h @@ -28,6 +28,7 @@ #include #include +#include // for cout :-( namespace gtsam { @@ -72,8 +73,8 @@ namespace gtsam { /* ************************************************************************* */ template - FastSet FactorGraph::keys() const { - FastSet allKeys; + KeySet FactorGraph::keys() const { + KeySet allKeys; BOOST_FOREACH(const sharedFactor& factor, factors_) if (factor) allKeys.insert(factor->begin(), factor->end()); diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index adb1c0aad..e97860eaa 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -332,7 +332,7 @@ namespace gtsam { size_t nrFactors() const; /** Potentially very slow function to return all keys involved */ - FastSet keys() const; + KeySet keys() const; /** MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer */ inline bool exists(size_t idx) const { return idx < size() && at(idx); } @@ -342,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 7cb3d9817..99f668591 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -29,7 +29,7 @@ namespace gtsam { // Remove the contaminated part of the Bayes tree BayesNetType bn; if (!this->empty()) { - const FastSet newFactorKeys = newFactors.keys(); + const KeySet newFactorKeys = newFactors.keys(); this->removeTop(std::vector(newFactorKeys.begin(), newFactorKeys.end()), bn, orphans); } @@ -44,9 +44,9 @@ namespace gtsam { // eliminate into a Bayes net const VariableIndex varIndex(factors); - const FastSet newFactorKeys = newFactors.keys(); + const KeySet 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/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 9d96c5b9c..352a8dded 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -26,123 +26,132 @@ #include namespace gtsam { - - namespace { - /* ************************************************************************* */ - template - struct ConstructorTraversalData { - ConstructorTraversalData* const parentData; - typename JunctionTree::sharedNode myJTNode; - FastVector childSymbolicConditionals; - FastVector childSymbolicFactors; - ConstructorTraversalData(ConstructorTraversalData* _parentData) : parentData(_parentData) {} - }; - /* ************************************************************************* */ - // Pre-order visitor function - template - ConstructorTraversalData ConstructorTraversalVisitorPre( +template +struct ConstructorTraversalData { + typedef typename JunctionTree::Node Node; + typedef typename JunctionTree::sharedNode sharedNode; + + ConstructorTraversalData* const parentData; + sharedNode myJTNode; + FastVector childSymbolicConditionals; + FastVector childSymbolicFactors; + + // Small inner class to store symbolic factors + class SymbolicFactors: public FactorGraph { + }; + + ConstructorTraversalData(ConstructorTraversalData* _parentData) : + parentData(_parentData) { + } + + // Pre-order visitor function + static ConstructorTraversalData ConstructorTraversalVisitorPre( const boost::shared_ptr& node, - ConstructorTraversalData& parentData) - { - // On the pre-order pass, before children have been visited, we just set up a traversal data - // structure with its own JT node, and create a child pointer in its parent. - ConstructorTraversalData myData = ConstructorTraversalData(&parentData); - myData.myJTNode = boost::make_shared::Node>(); - myData.myJTNode->keys.push_back(node->key); - myData.myJTNode->factors.insert(myData.myJTNode->factors.begin(), node->factors.begin(), node->factors.end()); - parentData.myJTNode->children.push_back(myData.myJTNode); - return myData; - } + ConstructorTraversalData& parentData) { + // On the pre-order pass, before children have been visited, we just set up + // a traversal data structure with its own JT node, and create a child + // pointer in its parent. + ConstructorTraversalData myData = ConstructorTraversalData(&parentData); + myData.myJTNode = boost::make_shared(node->key, node->factors); + parentData.myJTNode->children.push_back(myData.myJTNode); + return myData; + } - /* ************************************************************************* */ - // Post-order visitor function - template - void ConstructorTraversalVisitorPostAlg2( + // Post-order visitor function + static void ConstructorTraversalVisitorPostAlg2( const boost::shared_ptr& ETreeNode, - const ConstructorTraversalData& myData) - { - // In this post-order visitor, we combine the symbolic elimination results from the - // elimination tree children and symbolically eliminate the current elimination tree node. We - // then check whether each of our elimination tree child nodes should be merged with us. The - // check for this is that our number of symbolic elimination parents is exactly 1 less than - // our child's symbolic elimination parents - this condition indicates that eliminating the - // current node did not introduce any parents beyond those already in the child. + const ConstructorTraversalData& myData) { + // In this post-order visitor, we combine the symbolic elimination results + // from the elimination tree children and symbolically eliminate the current + // elimination tree node. We then check whether each of our elimination + // tree child nodes should be merged with us. The check for this is that + // our number of symbolic elimination parents is exactly 1 less than + // our child's symbolic elimination parents - this condition indicates that + // eliminating the current node did not introduce any parents beyond those + // already in the child-> - // Do symbolic elimination for this node - class : public FactorGraph {} symbolicFactors; - symbolicFactors.reserve(ETreeNode->factors.size() + myData.childSymbolicFactors.size()); - // Add ETree node factors - symbolicFactors += ETreeNode->factors; - // Add symbolic factors passed up from children - symbolicFactors += myData.childSymbolicFactors; + // Do symbolic elimination for this node + SymbolicFactors symbolicFactors; + symbolicFactors.reserve( + ETreeNode->factors.size() + myData.childSymbolicFactors.size()); + // Add ETree node factors + symbolicFactors += ETreeNode->factors; + // Add symbolic factors passed up from children + symbolicFactors += myData.childSymbolicFactors; - Ordering keyAsOrdering; keyAsOrdering.push_back(ETreeNode->key); - std::pair symbolicElimResult = - internal::EliminateSymbolic(symbolicFactors, keyAsOrdering); + Ordering keyAsOrdering; + keyAsOrdering.push_back(ETreeNode->key); + SymbolicConditional::shared_ptr myConditional; + SymbolicFactor::shared_ptr mySeparatorFactor; + boost::tie(myConditional, mySeparatorFactor) = internal::EliminateSymbolic( + symbolicFactors, keyAsOrdering); - // Store symbolic elimination results in the parent - myData.parentData->childSymbolicConditionals.push_back(symbolicElimResult.first); - myData.parentData->childSymbolicFactors.push_back(symbolicElimResult.second); + // Store symbolic elimination results in the parent + myData.parentData->childSymbolicConditionals.push_back(myConditional); + myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor); - // Merge our children if they are in our clique - if our conditional has exactly one fewer - // parent than our child's conditional. - size_t myNrFrontals = 1; - const size_t myNrParents = symbolicElimResult.first->nrParents(); - size_t nrMergedChildren = 0; - assert(myData.myJTNode->children.size() == myData.childSymbolicConditionals.size()); - // Loop over children - int combinedProblemSize = (int) (symbolicElimResult.first->size() * symbolicFactors.size()); - for(size_t child = 0; child < myData.childSymbolicConditionals.size(); ++child) { - // Check if we should merge the child - if(myNrParents + myNrFrontals == myData.childSymbolicConditionals[child]->nrParents()) { - // Get a reference to the child, adjusting the index to account for children previously - // merged and removed from the child list. - const typename JunctionTree::Node& childToMerge = - *myData.myJTNode->children[child - nrMergedChildren]; - // Merge keys, factors, and children. - myData.myJTNode->keys.insert(myData.myJTNode->keys.begin(), childToMerge.keys.begin(), childToMerge.keys.end()); - myData.myJTNode->factors.insert(myData.myJTNode->factors.end(), childToMerge.factors.begin(), childToMerge.factors.end()); - myData.myJTNode->children.insert(myData.myJTNode->children.end(), childToMerge.children.begin(), childToMerge.children.end()); - // Increment problem size - combinedProblemSize = std::max(combinedProblemSize, childToMerge.problemSize_); - // Increment number of frontal variables - myNrFrontals += childToMerge.keys.size(); - // Remove child from list. - myData.myJTNode->children.erase(myData.myJTNode->children.begin() + (child - nrMergedChildren)); - // Increment number of merged children - ++ nrMergedChildren; - } + sharedNode node = myData.myJTNode; + const FastVector& childConditionals = + myData.childSymbolicConditionals; + node->problemSize_ = (int) (myConditional->size() * symbolicFactors.size()); + + // Merge our children if they are in our clique - if our conditional has + // exactly one fewer parent than our child's conditional. + const size_t myNrParents = myConditional->nrParents(); + const size_t nrChildren = node->children.size(); + assert(childConditionals.size() == nrChildren); + + // decide which children to merge, as index into children + std::vector merge(nrChildren, false); + size_t myNrFrontals = 1, i = 0; + BOOST_FOREACH(const sharedNode& child, node->children) { + // Check if we should merge the i^th child + if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { + // Increment number of frontal variables + myNrFrontals += child->orderedFrontalKeys.size(); + merge[i] = true; } - myData.myJTNode->problemSize_ = combinedProblemSize; + ++i; } + + // now really merge + node->mergeChildren(merge); } +}; - /* ************************************************************************* */ - template - template - JunctionTree::JunctionTree(const EliminationTree& eliminationTree) - { - gttic(JunctionTree_FromEliminationTree); - // Here we rely on the BayesNet having been produced by this elimination tree, such that the - // conditionals are arranged in DFS post-order. We traverse the elimination tree, and inspect - // the symbolic conditional corresponding to each node. The elimination tree node is added to - // the same clique with its parent if it has exactly one more Bayes net conditional parent than - // does its elimination tree parent. +/* ************************************************************************* */ +template +template +JunctionTree::JunctionTree( + const EliminationTree& eliminationTree) { + gttic(JunctionTree_FromEliminationTree); + // Here we rely on the BayesNet having been produced by this elimination tree, + // such that the conditionals are arranged in DFS post-order. We traverse the + // elimination tree, and inspect the symbolic conditional corresponding to + // each node. The elimination tree node is added to the same clique with its + // parent if it has exactly one more Bayes net conditional parent than + // does its elimination tree parent. - // Traverse the elimination tree, doing symbolic elimination and merging nodes as we go. Gather - // the created junction tree roots in a dummy Node. - typedef typename EliminationTree::Node ETreeNode; - ConstructorTraversalData rootData(0); - rootData.myJTNode = boost::make_shared(); // Make a dummy node to gather the junction tree roots - treeTraversal::DepthFirstForest(eliminationTree, rootData, - ConstructorTraversalVisitorPre, ConstructorTraversalVisitorPostAlg2); + // Traverse the elimination tree, doing symbolic elimination and merging nodes + // as we go. Gather the created junction tree roots in a dummy Node. + typedef typename EliminationTree::Node ETreeNode; + typedef ConstructorTraversalData Data; + Data rootData(0); + rootData.myJTNode = boost::make_shared(); // Make a dummy node to gather + // the junction tree roots + treeTraversal::DepthFirstForest(eliminationTree, rootData, + Data::ConstructorTraversalVisitorPre, + Data::ConstructorTraversalVisitorPostAlg2); - // Assign roots from the dummy node - Base::roots_ = rootData.myJTNode->children; + // Assign roots from the dummy node + typedef typename JunctionTree::Node Node; + const typename Node::Children& children = rootData.myJTNode->children; + Base::roots_.reserve(children.size()); + Base::roots_.insert(Base::roots_.begin(), children.begin(), children.end()); - // Transfer remaining factors from elimination tree - Base::remainingFactors_ = eliminationTree.remainingFactors(); - } + // Transfer remaining factors from elimination tree + Base::remainingFactors_ = eliminationTree.remainingFactors(); +} -} //namespace gtsam +} // namespace gtsam diff --git a/gtsam/inference/Key.cpp b/gtsam/inference/Key.cpp index 0b9be2f1c..a2a6906d1 100644 --- a/gtsam/inference/Key.cpp +++ b/gtsam/inference/Key.cpp @@ -17,57 +17,72 @@ * @date Feb 20, 2012 */ -#include - -#include -#include - #include #include +#include +#include +#include + +using namespace std; + namespace gtsam { /* ************************************************************************* */ -std::string _multirobotKeyFormatter(Key key) { +string _defaultKeyFormatter(Key key) { + const Symbol asSymbol(key); + if (asSymbol.chr() > 0) + return (string) asSymbol; + else + return boost::lexical_cast(key); +} + +/* ************************************************************************* */ +void PrintKey(Key key, const string& s, const KeyFormatter& keyFormatter) { + cout << s << keyFormatter(key); +} + +/* ************************************************************************* */ +string _multirobotKeyFormatter(Key key) { const LabeledSymbol asLabeledSymbol(key); if (asLabeledSymbol.chr() > 0 && asLabeledSymbol.label() > 0) - return (std::string) asLabeledSymbol; + return (string) asLabeledSymbol; const Symbol asSymbol(key); if (asLabeledSymbol.chr() > 0) - return (std::string) asSymbol; + return (string) asSymbol; else - return boost::lexical_cast(key); + return boost::lexical_cast(key); } /* ************************************************************************* */ template -static void print(const CONTAINER& keys, const std::string& s, +static void Print(const CONTAINER& keys, const string& s, const KeyFormatter& keyFormatter) { - std::cout << s << " "; + cout << s << " "; if (keys.empty()) - std::cout << "(none)" << std::endl; + cout << "(none)" << endl; else { BOOST_FOREACH(const Key& key, keys) - std::cout << keyFormatter(key) << " "; - std::cout << std::endl; + cout << keyFormatter(key) << " "; + cout << endl; } } /* ************************************************************************* */ -void printKeyList(const KeyList& keys, const std::string& s, +void PrintKeyList(const KeyList& keys, const string& s, const KeyFormatter& keyFormatter) { - print(keys, s, keyFormatter); + Print(keys, s, keyFormatter); } /* ************************************************************************* */ -void printKeyVector(const KeyVector& keys, const std::string& s, +void PrintKeyVector(const KeyVector& keys, const string& s, const KeyFormatter& keyFormatter) { - print(keys, s, keyFormatter); + Print(keys, s, keyFormatter); } /* ************************************************************************* */ -void printKeySet(const KeySet& keys, const std::string& s, +void PrintKeySet(const KeySet& keys, const string& s, const KeyFormatter& keyFormatter) { - print(keys, s, keyFormatter); + Print(keys, s, keyFormatter); } /* ************************************************************************* */ diff --git a/gtsam/inference/Key.h b/gtsam/inference/Key.h index e2be79dc7..d2b342575 100644 --- a/gtsam/inference/Key.h +++ b/gtsam/inference/Key.h @@ -17,45 +17,75 @@ */ #pragma once -#include -#include - -#include -#include #include -#include #include +#include +#include +#include +#include +#include + +#include namespace gtsam { +/// Typedef for a function to format a key, i.e. to convert it to a string +typedef boost::function KeyFormatter; - // Helper function for Multi-robot Key Formatter - GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key); +// Helper function for DefaultKeyFormatter +GTSAM_EXPORT std::string _defaultKeyFormatter(Key key); - /// - /// A KeyFormatter that will check for LabeledSymbol keys, as well as Symbol and plain - /// integer keys. This keyformatter will need to be passed in to override the default - /// formatter in print functions. - /// - /// Checks for LabeledSymbol, Symbol and then plain keys, in order. - static const gtsam::KeyFormatter MultiRobotKeyFormatter = &_multirobotKeyFormatter; +/// The default KeyFormatter, which is used if no KeyFormatter is passed to +/// a nonlinear 'print' function. Automatically detects plain integer keys +/// and Symbol keys. +static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter; - /// Useful typedefs for operations with Values - allow for matlab interfaces - typedef FastList KeyList; - typedef FastVector KeyVector; - typedef FastSet KeySet; - typedef FastMap KeyGroupMap; +// Helper function for Multi-robot Key Formatter +GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key); - /// Utility function to print sets of keys with optional prefix - GTSAM_EXPORT void printKeyList(const KeyList& keys, const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter); +/// +/// A KeyFormatter that will check for LabeledSymbol keys, as well as Symbol and plain +/// integer keys. This keyformatter will need to be passed in to override the default +/// formatter in print functions. +/// +/// Checks for LabeledSymbol, Symbol and then plain keys, in order. +static const gtsam::KeyFormatter MultiRobotKeyFormatter = + &_multirobotKeyFormatter; - /// Utility function to print sets of keys with optional prefix - GTSAM_EXPORT void printKeyVector(const KeyVector& keys, const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter); +/// Useful typedef for operations with Values - allows for matlab interface +typedef FastVector KeyVector; - /// Utility function to print sets of keys with optional prefix - GTSAM_EXPORT void printKeySet(const KeySet& keys, const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter); -} +// TODO(frank): Nothing fast about these :-( +typedef FastList KeyList; +typedef FastSet KeySet; +typedef FastMap KeyGroupMap; + +/// Utility function to print one key with optional prefix +GTSAM_EXPORT void PrintKey(Key key, const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter); + +/// Utility function to print sets of keys with optional prefix +GTSAM_EXPORT void PrintKeyList(const KeyList& keys, const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter); + +/// Utility function to print sets of keys with optional prefix +GTSAM_EXPORT void PrintKeyVector(const KeyVector& keys, const std::string& s = + "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); + +/// Utility function to print sets of keys with optional prefix +GTSAM_EXPORT void PrintKeySet(const KeySet& keys, const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter); + +// Define Key to be Testable by specializing gtsam::traits +template struct traits; +template<> struct traits { + static void Print(const Key& key, const std::string& str = "") { + PrintKey(key, str); + } + static bool Equals(const Key& key1, const Key& key2, double tol = 1e-8) { + return key1 == key2; + } +}; + +} // namespace gtsam diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h index 23936afed..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_); diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h index 7299d7c8a..06e5ddeec 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -26,8 +26,8 @@ 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::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 ********** */ diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 5ae25d543..9f4a81d05 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -29,242 +29,236 @@ using namespace std; namespace gtsam { - /* ************************************************************************* */ - FastMap Ordering::invert() const - { - FastMap inverted; - for(size_t pos = 0; pos < this->size(); ++pos) - inverted.insert(make_pair((*this)[pos], pos)); - return inverted; +/* ************************************************************************* */ +FastMap Ordering::invert() const { + FastMap inverted; + for (size_t pos = 0; pos < this->size(); ++pos) + inverted.insert(make_pair((*this)[pos], pos)); + return inverted; +} + +/* ************************************************************************* */ +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); +} + +/* ************************************************************************* */ +Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, + std::vector& cmember) { + gttic(Ordering_COLAMDConstrained); + + gttic(Prepare); + size_t nEntries = variableIndex.nEntries(), nFactors = + variableIndex.nFactors(), nVars = variableIndex.size(); + // Convert to compressed column major format colamd wants it in (== MATLAB format!) + size_t Alen = ccolamd_recommended((int) nEntries, (int) nFactors, + (int) nVars); /* colamd arg 3: size of the array A */ + vector A = vector(Alen); /* colamd arg 4: row indices of A, of size Alen */ + vector p = vector(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */ + + // Fill in input data for COLAMD + p[0] = 0; + int count = 0; + vector keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order + size_t index = 0; + BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) { + // Arrange factor indices into COLAMD format + const VariableIndex::Factors& column = key_factors.second; + size_t lastFactorId = numeric_limits::max(); + BOOST_FOREACH(size_t factorIndex, column) { + if (lastFactorId != numeric_limits::max()) + assert(factorIndex > lastFactorId); + A[count++] = (int) factorIndex; // copy sparse column + } + p[index + 1] = count; // column j (base 1) goes from A[j-1] to A[j]-1 + // Store key in array and increment index + keys[index] = key_factors.first; + ++index; } - /* ************************************************************************* */ - 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); + assert((size_t)count == variableIndex.nEntries()); + + //double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */ + double knobs[CCOLAMD_KNOBS]; + ccolamd_set_defaults(knobs); + knobs[CCOLAMD_DENSE_ROW] = -1; + knobs[CCOLAMD_DENSE_COL] = -1; + + int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */ + + gttoc(Prepare); + + // call colamd, result will be in p + /* returns (1) if successful, (0) otherwise*/ + if (nVars > 0) { + gttic(ccolamd); + int rv = ccolamd((int) nFactors, (int) nVars, (int) Alen, &A[0], &p[0], + knobs, stats, &cmember[0]); + if (rv != 1) + throw runtime_error( + (boost::format("ccolamd failed with return value %1%") % rv).str()); } - /* ************************************************************************* */ - Ordering Ordering::colamdConstrained( - const VariableIndex& variableIndex, std::vector& cmember) - { - gttic(Ordering_COLAMDConstrained); + // ccolamd_report(stats); - gttic(Prepare); - size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size(); - // Convert to compressed column major format colamd wants it in (== MATLAB format!) - size_t Alen = ccolamd_recommended((int)nEntries, (int)nFactors, (int)nVars); /* colamd arg 3: size of the array A */ - vector A = vector(Alen); /* colamd arg 4: row indices of A, of size Alen */ - vector p = vector(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */ + gttic(Fill_Ordering); + // Convert elimination ordering in p to an ordering + Ordering result; + result.resize(nVars); + for (size_t j = 0; j < nVars; ++j) + result[j] = keys[p[j]]; + gttoc(Fill_Ordering); - // Fill in input data for COLAMD - p[0] = 0; - int count = 0; - vector keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order - size_t index = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) { - // Arrange factor indices into COLAMD format - const VariableIndex::Factors& column = key_factors.second; - size_t lastFactorId = numeric_limits::max(); - BOOST_FOREACH(size_t factorIndex, column) { - if(lastFactorId != numeric_limits::max()) - assert(factorIndex > lastFactorId); - A[count++] = (int)factorIndex; // copy sparse column - } - p[index+1] = count; // column j (base 1) goes from A[j-1] to A[j]-1 - // Store key in array and increment index - keys[index] = key_factors.first; - ++ index; - } + return result; +} - assert((size_t)count == variableIndex.nEntries()); +/* ************************************************************************* */ +Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex, + const std::vector& constrainLast, bool forceOrder) { + gttic(Ordering_COLAMDConstrainedLast); - //double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */ - double knobs[CCOLAMD_KNOBS]; - ccolamd_set_defaults(knobs); - knobs[CCOLAMD_DENSE_ROW]=-1; - knobs[CCOLAMD_DENSE_COL]=-1; + size_t n = variableIndex.size(); + std::vector cmember(n, 0); - int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */ + // Build a mapping to look up sorted Key indices by Key + FastMap keyIndices; + size_t j = 0; + BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) + keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); - gttoc(Prepare); + // If at least some variables are not constrained to be last, constrain the + // ones that should be constrained. + int group = (constrainLast.size() != n ? 1 : 0); + BOOST_FOREACH(Key key, constrainLast) { + cmember[keyIndices.at(key)] = group; + if (forceOrder) + ++group; + } - // call colamd, result will be in p - /* returns (1) if successful, (0) otherwise*/ - if(nVars > 0) { - gttic(ccolamd); - int rv = ccolamd((int)nFactors, (int)nVars, (int)Alen, &A[0], &p[0], knobs, stats, &cmember[0]); - if(rv != 1) - throw runtime_error((boost::format("ccolamd failed with return value %1%")%rv).str()); - } + return Ordering::ColamdConstrained(variableIndex, cmember); +} - // ccolamd_report(stats); +/* ************************************************************************* */ +Ordering Ordering::ColamdConstrainedFirst(const VariableIndex& variableIndex, + const std::vector& constrainFirst, bool forceOrder) { + gttic(Ordering_COLAMDConstrainedFirst); - gttic(Fill_Ordering); - // Convert elimination ordering in p to an ordering - Ordering result; - result.resize(nVars); - for(size_t j = 0; j < nVars; ++j) - result[j] = keys[p[j]]; - gttoc(Fill_Ordering); + const int none = -1; + size_t n = variableIndex.size(); + std::vector cmember(n, none); + // Build a mapping to look up sorted Key indices by Key + FastMap keyIndices; + size_t j = 0; + BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) + keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); + + // If at least some variables are not constrained to be last, constrain the + // ones that should be constrained. + int group = 0; + BOOST_FOREACH(Key key, constrainFirst) { + cmember[keyIndices.at(key)] = group; + if (forceOrder) + ++group; + } + + if (!forceOrder && !constrainFirst.empty()) + ++group; + BOOST_FOREACH(int& c, cmember) + if (c == none) + c = group; + + return Ordering::ColamdConstrained(variableIndex, cmember); +} + +/* ************************************************************************* */ +Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, + const FastMap& groups) { + gttic(Ordering_COLAMDConstrained); + size_t n = variableIndex.size(); + std::vector cmember(n, 0); + + // Build a mapping to look up sorted Key indices by Key + FastMap keyIndices; + size_t j = 0; + BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) + keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); + + // Assign groups + typedef FastMap::value_type key_group; + BOOST_FOREACH(const key_group& p, groups) { + // FIXME: check that no groups are skipped + cmember[keyIndices.at(p.first)] = p.second; + } + + 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; } - /* ************************************************************************* */ - Ordering Ordering::colamdConstrainedLast( - const VariableIndex& variableIndex, const std::vector& constrainLast, bool forceOrder) - { - gttic(Ordering_COLAMDConstrainedLast); - - size_t n = variableIndex.size(); - std::vector cmember(n, 0); - - // Build a mapping to look up sorted Key indices by Key - FastMap keyIndices; - size_t j = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) - keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); - - // If at least some variables are not constrained to be last, constrain the - // ones that should be constrained. - int group = (constrainLast.size() != n ? 1 : 0); - BOOST_FOREACH(Key key, constrainLast) { - cmember[keyIndices.at(key)] = group; - if(forceOrder) - ++ group; - } - - return Ordering::colamdConstrained(variableIndex, cmember); + 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; +} - /* ************************************************************************* */ - Ordering Ordering::colamdConstrainedFirst( - const VariableIndex& variableIndex, const std::vector& constrainFirst, bool forceOrder) - { - gttic(Ordering_COLAMDConstrainedFirst); - - const int none = -1; - size_t n = variableIndex.size(); - std::vector cmember(n, none); - - // Build a mapping to look up sorted Key indices by Key - FastMap keyIndices; - size_t j = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) - keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); - - // If at least some variables are not constrained to be last, constrain the - // ones that should be constrained. - int group = 0; - BOOST_FOREACH(Key key, constrainFirst) { - cmember[keyIndices.at(key)] = group; - if(forceOrder) - ++ group; - } - - if(!forceOrder && !constrainFirst.empty()) - ++ group; - BOOST_FOREACH(int& c, cmember) - if(c == none) - c = group; - - return Ordering::colamdConstrained(variableIndex, cmember); - } - - /* ************************************************************************* */ - Ordering Ordering::colamdConstrained(const VariableIndex& variableIndex, - const FastMap& groups) - { - gttic(Ordering_COLAMDConstrained); - size_t n = variableIndex.size(); - std::vector cmember(n, 0); - - // Build a mapping to look up sorted Key indices by Key - FastMap keyIndices; - size_t j = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) - keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); - - // Assign groups - typedef FastMap::value_type key_group; - BOOST_FOREACH(const key_group& p, groups) { - // FIXME: check that no groups are skipped - cmember[keyIndices.at(p.first)] = p.second; - } - - 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; - } - - /* ************************************************************************* */ - void Ordering::print(const std::string& str, const KeyFormatter& keyFormatter) const - { - cout << str; - // Print ordering in index order - // Print the ordering with varsPerLine ordering entries printed on each line, - // for compactness. - static const size_t varsPerLine = 10; - bool endedOnNewline = false; - for(size_t i = 0; i < size(); ++i) { - if(i % varsPerLine == 0) - cout << "Position " << i << ": "; - if(i % varsPerLine != 0) - cout << ", "; - cout << keyFormatter(at(i)); - if(i % varsPerLine == varsPerLine - 1) { - cout << "\n"; - endedOnNewline = true; - } else { - endedOnNewline = false; - } - } - if(!endedOnNewline) +/* ************************************************************************* */ +void Ordering::print(const std::string& str, + const KeyFormatter& keyFormatter) const { + cout << str; + // Print ordering in index order + // Print the ordering with varsPerLine ordering entries printed on each line, + // for compactness. + static const size_t varsPerLine = 10; + bool endedOnNewline = false; + for (size_t i = 0; i < size(); ++i) { + if (i % varsPerLine == 0) + cout << "Position " << i << ": "; + if (i % varsPerLine != 0) + cout << ", "; + cout << keyFormatter(at(i)); + if (i % varsPerLine == varsPerLine - 1) { cout << "\n"; - cout.flush(); + endedOnNewline = true; + } else { + endedOnNewline = false; + } } + if (!endedOnNewline) + cout << "\n"; + cout.flush(); +} - /* ************************************************************************* */ - bool Ordering::equals(const Ordering& other, double tol) const - { - return (*this) == other; - } +/* ************************************************************************* */ +bool Ordering::equals(const Ordering& other, double tol) const { + return (*this) == other; +} } diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index e25590578..8af2649c5 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -18,182 +18,224 @@ #pragma once -#include -#include -#include - -#include #include #include #include #include +#include + +#include +#include +#include namespace gtsam { - class Ordering : public std::vector { - protected: - typedef std::vector Base; +class Ordering: public std::vector { +protected: + typedef std::vector Base; - public: +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 - - /// Create an empty ordering - GTSAM_EXPORT Ordering() {} - - /// Create from a container - template - explicit Ordering(const KEYS& keys) : Base(keys.begin(), keys.end()) {} - - /// Create an ordering using iterators over keys - template - Ordering(ITERATOR firstKey, ITERATOR lastKey) : Base(firstKey, lastKey) {} - - /// Add new variables to the ordering as ordering += key1, key2, ... Equivalent to calling - /// push_back. - boost::assign::list_inserter > - operator+=(Key key) { - return boost::assign::make_list_inserter(boost::assign_detail::call_push_back(*this))(key); - } - - /// Invert (not reverse) the ordering - returns a map from key to order position - FastMap invert() const; - - /// @name Fill-reducing Orderings @{ - - /// Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on - /// 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)); } - - /// Compute a fill-reducing ordering using COLAMD from a 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 - /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains - /// the variables in \c constrainLast to the end of the ordering, and orders all other variables - /// before in a fill-reducing ordering. If \c forceOrder is true, the 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. - template - static Ordering colamdConstrainedLast(const FactorGraph& graph, - const std::vector& constrainLast, bool forceOrder = false) { - 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 - /// all other variables before in a fill-reducing ordering. If \c forceOrder is true, the - /// 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, - const std::vector& constrainLast, bool forceOrder = false); - - /// 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 - /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains - /// the variables in \c constrainLast to the end of the ordering, and orders all other variables - /// before in a fill-reducing ordering. If \c forceOrder is true, the 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 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, - const std::vector& constrainFirst, bool forceOrder = false) { - 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 - /// orders all other variables after in a fill-reducing ordering. If \c forceOrder is true, the - /// variables in \c constrainFirst will be ordered in the same order specified in the - /// 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, - const std::vector& constrainFirst, bool forceOrder = false); - - /// 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 - /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). In this function, a group - /// for each variable should be specified in \c groups, and each group of variables will appear - /// in the ordering in group index order. \c groups should be a map from Key to group index. - /// The group indices used should be consecutive starting at 0, but may 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. - template - static Ordering colamdConstrained(const FactorGraph& graph, - const FastMap& 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 - /// variables will appear in the ordering in group index order. \c groups should be a map from - /// Key to group index. The group indices used should be consecutive starting at 0, but may - /// 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, - const FastMap& groups); - - /// Return a natural Ordering. Typically used by iterative solvers - template - static Ordering Natural(const FactorGraph &fg) { - FastSet src = fg.keys(); - std::vector keys(src.begin(), src.end()); - std::stable_sort(keys.begin(), keys.end()); - 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 @{ - - GTSAM_EXPORT void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - - GTSAM_EXPORT bool equals(const Ordering& other, double tol = 1e-9) const; - - /// @} - - private: - /// Internal COLAMD function - 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) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - } + /// Type of ordering to use + enum OrderingType { + COLAMD, METIS, NATURAL, CUSTOM }; - /// traits - template<> struct traits : public Testable {}; + typedef Ordering This; ///< Typedef to this class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + + /// Create an empty ordering + GTSAM_EXPORT + Ordering() { + } + + /// Create from a container + template + explicit Ordering(const KEYS& keys) : + Base(keys.begin(), keys.end()) { + } + + /// Create an ordering using iterators over keys + template + Ordering(ITERATOR firstKey, ITERATOR lastKey) : + Base(firstKey, lastKey) { + } + + /// Add new variables to the ordering as ordering += key1, key2, ... Equivalent to calling + /// push_back. + boost::assign::list_inserter > operator+=( + Key key) { + return boost::assign::make_list_inserter( + boost::assign_detail::call_push_back(*this))(key); + } + + /// Invert (not reverse) the ordering - returns a map from key to order position + FastMap invert() const; + + /// @name Fill-reducing Orderings @{ + + /// Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on + /// 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)); + } + + /// Compute a fill-reducing ordering using COLAMD from a 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 + /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains + /// the variables in \c constrainLast to the end of the ordering, and orders all other variables + /// before in a fill-reducing ordering. If \c forceOrder is true, the 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. + template + static Ordering ColamdConstrainedLast(const FactorGraph& graph, + const std::vector& constrainLast, bool forceOrder = false) { + 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 + /// all other variables before in a fill-reducing ordering. If \c forceOrder is true, the + /// 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, const std::vector& constrainLast, + bool forceOrder = false); + + /// 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 + /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains + /// the variables in \c constrainLast to the end of the ordering, and orders all other variables + /// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c + /// constrainFirst will be ordered in the same order specified in the vector \c + /// constrainFirst. If \c forceOrder is false, the variables in \c constrainFirst will be + /// ordered before all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. + template + static Ordering ColamdConstrainedFirst(const FactorGraph& graph, + const std::vector& constrainFirst, bool forceOrder = false) { + 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 + /// orders all other variables after in a fill-reducing ordering. If \c forceOrder is true, the + /// variables in \c constrainFirst will be ordered in the same order specified in the + /// vector \c constrainFirst. If \c forceOrder is false, the variables in \c + /// constrainFirst will be ordered before all the others, but will be rearranged by CCOLAMD to + /// reduce fill-in as well. + 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 + /// for note on performance). This internally builds a VariableIndex so if you already have a + /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). In this function, a group + /// for each variable should be specified in \c groups, and each group of variables will appear + /// in the ordering in group index order. \c groups should be a map from Key to group index. + /// The group indices used should be consecutive starting at 0, but may 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. + template + static Ordering ColamdConstrained(const FactorGraph& graph, + const FastMap& 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 + /// variables will appear in the ordering in group index order. \c groups should be a map from + /// Key to group index. The group indices used should be consecutive starting at 0, but may + /// 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, const FastMap& groups); + + /// Return a natural Ordering. Typically used by iterative solvers + template + static Ordering Natural(const FactorGraph &fg) { + KeySet src = fg.keys(); + std::vector keys(src.begin(), src.end()); + std::stable_sort(keys.begin(), keys.end()); + 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 Named Constructors @{ + + template + static Ordering Create(OrderingType orderingType, + const FactorGraph& graph) { + + switch (orderingType) { + case COLAMD: + return Colamd(graph); + case METIS: + return Metis(graph); + case NATURAL: + return Natural(graph); + case CUSTOM: + throw std::runtime_error( + "Ordering::Create error: called with CUSTOM ordering type."); + default: + throw std::runtime_error( + "Ordering::Create error: called with unknown ordering type."); + } + } + + /// @} + + /// @name Testable @{ + + GTSAM_EXPORT + void print(const std::string& str = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; + + GTSAM_EXPORT + bool equals(const Ordering& other, double tol = 1e-9) const; + + /// @} + +private: + /// Internal COLAMD function + 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) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } +}; + +/// traits +template<> struct traits : public Testable { +}; } diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 4f9734639..68d927baf 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -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_); } diff --git a/gtsam/inference/VariableIndex-inl.h b/gtsam/inference/VariableIndex-inl.h index 82eb85c76..c00a3633e 100644 --- a/gtsam/inference/VariableIndex-inl.h +++ b/gtsam/inference/VariableIndex-inl.h @@ -18,6 +18,8 @@ #pragma once #include +#include +#include namespace gtsam { diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index bcaec6ee4..3b80f0d01 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -18,16 +18,14 @@ #pragma once #include -#include #include -#include -#include -#include +#include +#include -#include +#include +#include -#include -#include +#include #include namespace gtsam { @@ -45,7 +43,7 @@ class GTSAM_EXPORT VariableIndex { public: typedef boost::shared_ptr shared_ptr; - typedef FastList Factors; + typedef FastVector Factors; typedef Factors::iterator Factor_iterator; typedef Factors::const_iterator Factor_const_iterator; diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp index 1033c0cc9..b0708e660 100644 --- a/gtsam/inference/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -14,6 +14,7 @@ * @author Alex Cunningham */ +#include #include #include #include diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 013f569e0..d789da9b0 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -28,45 +28,47 @@ using namespace std; using namespace gtsam; using namespace boost::assign; +namespace example { +SymbolicFactorGraph symbolicChain() { + 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(4, 5); + return sfg; +} +} /* ************************************************************************* */ TEST(Ordering, constrained_ordering) { - SymbolicFactorGraph sfg; // create graph with wanted variable set = 2, 4 - sfg.push_factor(0,1); - sfg.push_factor(1,2); - sfg.push_factor(2,3); - sfg.push_factor(3,4); - sfg.push_factor(4,5); + SymbolicFactorGraph sfg = example::symbolicChain(); // 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)); } /* ************************************************************************* */ TEST(Ordering, grouped_constrained_ordering) { - SymbolicFactorGraph sfg; // create graph with constrained groups: // 1: 2, 4 // 2: 5 - sfg.push_factor(0,1); - sfg.push_factor(1,2); - sfg.push_factor(2,3); - sfg.push_factor(3,4); - sfg.push_factor(4,5); + SymbolicFactorGraph sfg = example::symbolicChain(); // constrained version - push one set to the end FastMap constraints; @@ -74,50 +76,48 @@ 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); + // 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); + 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); + 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 ; + 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()); + EXPECT(xadjExpected == mi.xadj()); + EXPECT(adjExpected.size() == mi.adj().size()); + EXPECT(adjExpected == mi.adj()); } /* ************************************************************************* */ @@ -135,12 +135,11 @@ TEST(Ordering, csr_format_2) { vector xadjExpected, adjExpected; xadjExpected += 0, 1, 4, 6, 8, 10; - adjExpected += 1, 0, 2, 4, 1, 3, 2, 4, 1, 3; + 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()); - } /* ************************************************************************* */ @@ -170,7 +169,6 @@ TEST(Ordering, csr_format_3) { EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == adjAcutal); - } /* ************************************************************************* */ @@ -197,7 +195,7 @@ TEST(Ordering, csr_format_4) { EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == adjAcutal); - Ordering metOrder = Ordering::metis(sfg); + Ordering metOrder = Ordering::Metis(sfg); // Test different symbol types sfg.push_factor(Symbol('l', 1)); @@ -206,8 +204,7 @@ TEST(Ordering, csr_format_4) { sfg.push_factor(Symbol('x', 3), Symbol('l', 1)); sfg.push_factor(Symbol('x', 4), Symbol('l', 1)); - Ordering metOrder2 = Ordering::metis(sfg); - + Ordering metOrder2 = Ordering::Metis(sfg); } /* ************************************************************************* */ @@ -229,8 +226,77 @@ TEST(Ordering, metis) { EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == mi.adj()); - Ordering metis = Ordering::metis(sfg); + Ordering metis = Ordering::Metis(sfg); +} + +/* ************************************************************************* */ +TEST(Ordering, MetisLoop) { + + // create linear graph + SymbolicFactorGraph sfg = example::symbolicChain(); + + // add loop closure + sfg.push_factor(0, 5); + + // METIS +#if !defined(__APPLE__) + { + Ordering actual = Ordering::Create(Ordering::METIS, sfg); + // - P( 0 4 1) + // | - P( 2 | 4 1) + // | | - P( 3 | 4 2) + // | - P( 5 | 0 1) + Ordering expected = Ordering(list_of(3)(2)(5)(0)(4)(1)); + EXPECT(assert_equal(expected, actual)); + } +#else + { + Ordering actual = Ordering::Create(Ordering::METIS, sfg); + // - P( 1 0 3) + // | - P( 4 | 0 3) + // | | - P( 5 | 0 4) + // | - P( 2 | 1 3) + Ordering expected = Ordering(list_of(5)(4)(2)(1)(0)(3)); + EXPECT(assert_equal(expected, actual)); + } +#endif +} + +/* ************************************************************************* */ +TEST(Ordering, Create) { + + // create chain graph + SymbolicFactorGraph sfg = example::symbolicChain(); + + // COLAMD + { + //- P( 4 5) + //| - P( 3 | 4) + //| | - P( 2 | 3) + //| | | - P( 1 | 2) + //| | | | - P( 0 | 1) + Ordering actual = Ordering::Create(Ordering::COLAMD, sfg); + Ordering expected = Ordering(list_of(0)(1)(2)(3)(4)(5)); + EXPECT(assert_equal(expected, actual)); + } + + // METIS + { + Ordering actual = Ordering::Create(Ordering::METIS, sfg); + //- P( 1 0 2) + //| - P( 3 4 | 2) + //| | - P( 5 | 4) + Ordering expected = Ordering(list_of(5)(3)(4)(1)(0)(2)); + EXPECT(assert_equal(expected, actual)); + } + + // CUSTOM + CHECK_EXCEPTION(Ordering::Create(Ordering::CUSTOM, sfg), runtime_error); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/linear/BinaryJacobianFactor.h b/gtsam/linear/BinaryJacobianFactor.h new file mode 100644 index 000000000..23d11964c --- /dev/null +++ b/gtsam/linear/BinaryJacobianFactor.h @@ -0,0 +1,91 @@ +/* ---------------------------------------------------------------------------- + + * 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 BinaryJacobianFactor.h + * + * @brief A binary JacobianFactor specialization that uses fixed matrix math for speed + * + * @date June 2015 + * @author Frank Dellaert + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * A binary JacobianFactor specialization that uses fixed matrix math for speed + */ +template +struct BinaryJacobianFactor: JacobianFactor { + + /// Constructor + BinaryJacobianFactor(Key key1, const Eigen::Matrix& A1, + Key key2, const Eigen::Matrix& A2, + const Eigen::Matrix& b, // + const SharedDiagonal& model = SharedDiagonal()) : + JacobianFactor(key1, A1, key2, A2, b, model) { + } + + inline Key key1() const { + return keys_[0]; + } + inline Key key2() const { + return keys_[1]; + } + + // Fixed-size matrix update + void updateHessian(const FastVector& infoKeys, + SymmetricBlockMatrix* info) const { + gttic(updateHessian_BinaryJacobianFactor); + // Whiten the factor if it has a noise model + const SharedDiagonal& model = get_model(); + if (model && !model->isUnit()) { + if (model->isConstrained()) + throw std::invalid_argument( + "BinaryJacobianFactor::updateHessian: cannot update information with " + "constrained noise model"); + BinaryJacobianFactor whitenedFactor(key1(), model->Whiten(getA(begin())), + key2(), model->Whiten(getA(end())), model->whiten(getb())); + whitenedFactor.updateHessian(infoKeys, info); + } else { + // First build an array of slots + DenseIndex slot1 = Slot(infoKeys, key1()); + DenseIndex slot2 = Slot(infoKeys, key2()); + DenseIndex slotB = info->nBlocks() - 1; + + const Matrix& Ab = Ab_.matrix(); + Eigen::Block A1(Ab, 0, 0); + Eigen::Block A2(Ab, 0, N1); + Eigen::Block b(Ab, 0, N1 + N2); + + // We perform I += A'*A to the upper triangle + (*info)(slot1, slot1).selfadjointView().rankUpdate(A1.transpose()); + (*info)(slot1, slot2).knownOffDiagonal() += A1.transpose() * A2; + (*info)(slot1, slotB).knownOffDiagonal() += A1.transpose() * b; + (*info)(slot2, slot2).selfadjointView().rankUpdate(A2.transpose()); + (*info)(slot2, slotB).knownOffDiagonal() += A2.transpose() * b; + (*info)(slotB, slotB)(0, 0) += b.transpose() * b; + } + } +}; + +template +struct traits > : Testable< + BinaryJacobianFactor > { +}; + +} //namespace gtsam diff --git a/gtsam/linear/ConjugateGradientSolver.cpp b/gtsam/linear/ConjugateGradientSolver.cpp index 5e7e08f61..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: Yong-Dian Jian + * @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 20e0c5262..2596a7403 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -20,7 +20,6 @@ #pragma once #include -#include namespace gtsam { @@ -87,9 +86,8 @@ public: static BLASKernel blasTranslator(const std::string &s) ; }; -/*********************************************************************************************/ /* - * A template of linear preconditioned conjugate gradient method. + * 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. @@ -98,8 +96,9 @@ public: * [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; diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index a89e7e21c..401583bbf 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -166,7 +166,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); } }; diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index e0a820819..d9b75c69f 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -135,7 +135,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(BaseFactor); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional); } diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index c67636341..7f9c5ea3c 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -28,6 +28,8 @@ namespace gtsam { // Forward declarations class VectorValues; + class Scatter; + class SymmetricBlockMatrix; /** * An abstract virtual base class for JacobianFactor and HessianFactor. A GaussianFactor has a @@ -119,6 +121,14 @@ namespace gtsam { */ virtual GaussianFactor::shared_ptr negate() const = 0; + /** Update an information matrix by adding the information corresponding to this factor + * (used internally during elimination). + * @param scatter A mapping from variable index to slot index in this HessianFactor + * @param info The information matrix to be updated + */ + virtual void updateHessian(const FastVector& keys, + SymmetricBlockMatrix* info) const = 0; + /// y += alpha * A'*A*x virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const = 0; @@ -131,16 +141,22 @@ namespace gtsam { /// Gradient wrt a key at any values virtual Vector gradient(Key key, const VectorValues& x) const = 0; + // Determine position of a given key + template + static DenseIndex Slot(const CONTAINER& keys, Key key) { + return std::find(keys.begin(), keys.end(), key) - keys.begin(); + } + 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 - + /// traits template<> struct traits : public Testable { diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 6953d2969..a39b1d91e 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -47,7 +47,7 @@ namespace gtsam { /* ************************************************************************* */ GaussianFactorGraph::Keys GaussianFactorGraph::keys() const { - FastSet keys; + KeySet keys; BOOST_FOREACH(const sharedFactor& factor, *this) if (factor) keys.insert(factor->begin(), factor->end()); @@ -123,26 +123,32 @@ namespace gtsam { /* ************************************************************************* */ vector > GaussianFactorGraph::sparseJacobian() const { // First find dimensions of each variable - vector dims; + typedef std::map KeySizeMap; + KeySizeMap dims; BOOST_FOREACH(const sharedFactor& factor, *this) { - for (GaussianFactor::const_iterator pos = factor->begin(); - pos != factor->end(); ++pos) { - if (dims.size() <= *pos) - dims.resize(*pos + 1, 0); - dims[*pos] = factor->getDim(pos); + if (!static_cast(factor)) continue; + + for (GaussianFactor::const_iterator key = factor->begin(); + key != factor->end(); ++key) { + dims[*key] = factor->getDim(key); } } // Compute first scalar column of each variable - vector columnIndices(dims.size() + 1, 0); - for (size_t j = 1; j < dims.size() + 1; ++j) - columnIndices[j] = columnIndices[j - 1] + dims[j - 1]; + size_t currentColIndex = 0; + KeySizeMap columnIndices = dims; + BOOST_FOREACH(const KeySizeMap::value_type& col, dims) { + columnIndices[col.first] = currentColIndex; + currentColIndex += dims[col.first]; + } // Iterate over all factors, adding sparse scalar entries typedef boost::tuple triplet; vector entries; size_t row = 0; BOOST_FOREACH(const sharedFactor& factor, *this) { + if (!static_cast(factor)) continue; + // Convert to JacobianFactor if necessary JacobianFactor::shared_ptr jacobianFactor( boost::dynamic_pointer_cast(factor)); @@ -159,11 +165,11 @@ namespace gtsam { // Whiten the factor and add entries for it // iterate over all variables in the factor const JacobianFactor whitened(jacobianFactor->whiten()); - for (JacobianFactor::const_iterator pos = whitened.begin(); - pos < whitened.end(); ++pos) { - JacobianFactor::constABlock whitenedA = whitened.getA(pos); + for (JacobianFactor::const_iterator key = whitened.begin(); + key < whitened.end(); ++key) { + JacobianFactor::constABlock whitenedA = whitened.getA(key); // find first column index for this key - size_t column_start = columnIndices[*pos]; + size_t column_start = columnIndices[*key]; for (size_t i = 0; i < (size_t) whitenedA.rows(); i++) for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) { double s = whitenedA(i, j); @@ -173,7 +179,7 @@ namespace gtsam { } JacobianFactor::constBVector whitenedb(whitened.getb()); - size_t bcolumn = columnIndices.back(); + size_t bcolumn = currentColIndex; for (size_t i = 0; i < (size_t) whitenedb.size(); i++) entries.push_back(boost::make_tuple(row + i, bcolumn, whitenedb(i))); diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 811c0f8b0..02bc95511 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -135,7 +135,7 @@ namespace gtsam { * Return the set of variables involved in the factors (computes a set * union). */ - typedef FastSet Keys; + typedef KeySet Keys; Keys keys() const; /* return a map of (Key, dimension) */ @@ -322,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); } diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index f2bebcab9..2b275b60f 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -15,17 +15,19 @@ * @date Dec 8, 2010 */ -#include -#include -#include -#include -#include -#include +#include + #include #include -#include -#include #include +#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -49,232 +51,185 @@ using namespace std; using namespace boost::assign; -namespace br { using namespace boost::range; using namespace boost::adaptors; } +namespace br { +using namespace boost::range; +using namespace boost::adaptors; +} namespace gtsam { -/* ************************************************************************* */ -string SlotEntry::toString() const { - ostringstream oss; - oss << "SlotEntry: slot=" << slot << ", dim=" << dimension; - return oss.str(); -} - -/* ************************************************************************* */ -Scatter::Scatter(const GaussianFactorGraph& gfg, - boost::optional ordering) { - gttic(Scatter_Constructor); - static const size_t none = std::numeric_limits::max(); - - // First do the set union. - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) { - if (factor) { - for (GaussianFactor::const_iterator variable = factor->begin(); - variable != factor->end(); ++variable) { - // TODO: Fix this hack to cope with zero-row Jacobians that come from BayesTreeOrphanWrappers - const JacobianFactor* asJacobian = - dynamic_cast(factor.get()); - if (!asJacobian || asJacobian->cols() > 1) - this->insert( - make_pair(*variable, SlotEntry(none, factor->getDim(variable)))); - } - } - } - - // If we have an ordering, pre-fill the ordered variables first - size_t slot = 0; - if (ordering) { - BOOST_FOREACH(Key key, *ordering) { - const_iterator entry = find(key); - if (entry == end()) - throw std::invalid_argument( - "The ordering provided to the HessianFactor Scatter constructor\n" - "contained extra variables that did not appear in the factors to combine."); - at(key).slot = (slot++); - } - } - - // Next fill in the slot indices (we can only get these after doing the set - // union. - BOOST_FOREACH(value_type& var_slot, *this) { - if (var_slot.second.slot == none) - var_slot.second.slot = (slot++); - } -} - /* ************************************************************************* */ HessianFactor::HessianFactor() : - info_(cref_list_of<1>(1)) -{ + info_(cref_list_of<1>(1)) { linearTerm().setZero(); constantTerm() = 0.0; } /* ************************************************************************* */ HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) : - GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1)) -{ - if(G.rows() != G.cols() || G.rows() != g.size()) throw invalid_argument( - "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); - info_(0,0) = G; - info_(0,1) = g; - info_(1,1)(0,0) = f; + GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1)) { + if (G.rows() != G.cols() || G.rows() != g.size()) + throw invalid_argument( + "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); + info_(0, 0) = G; + info_(0, 1) = g; + info_(1, 1)(0, 0) = f; } /* ************************************************************************* */ // error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu) // where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) : - GaussianFactor(cref_list_of<1>(j)), - info_(cref_list_of<2> (Sigma.cols()) (1) ) -{ - if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument( - "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); - info_(0,0) = Sigma.inverse(); // G - info_(0,1) = info_(0,0).selfadjointView() * mu; // g - info_(1,1)(0,0) = mu.dot(info_(0,1).knownOffDiagonal().col(0)); // f + GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(Sigma.cols())(1)) { + if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) + throw invalid_argument( + "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); + info_(0, 0) = Sigma.inverse(); // G + info_(0, 1) = info_(0, 0).selfadjointView() * mu; // g + info_(1, 1)(0, 0) = mu.dot(info_(0, 1).knownOffDiagonal().col(0)); // f } /* ************************************************************************* */ -HessianFactor::HessianFactor(Key j1, Key j2, - const Matrix& G11, const Matrix& G12, const Vector& g1, - const Matrix& G22, const Vector& g2, double f) : - GaussianFactor(cref_list_of<2>(j1)(j2)), - info_(cref_list_of<3> (G11.cols()) (G22.cols()) (1) ) -{ - info_(0,0) = G11; - info_(0,1) = G12; - info_(0,2) = g1; - info_(1,1) = G22; - info_(1,2) = g2; - info_(2,2)(0,0) = f; +HessianFactor::HessianFactor(Key j1, Key j2, const Matrix& G11, + const Matrix& G12, const Vector& g1, const Matrix& G22, const Vector& g2, + double f) : + GaussianFactor(cref_list_of<2>(j1)(j2)), info_( + cref_list_of<3>(G11.cols())(G22.cols())(1)) { + info_(0, 0) = G11; + info_(0, 1) = G12; + info_(0, 2) = g1; + info_(1, 1) = G22; + info_(1, 2) = g2; + info_(2, 2)(0, 0) = f; } /* ************************************************************************* */ -HessianFactor::HessianFactor(Key j1, Key j2, Key j3, - const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, - const Matrix& G22, const Matrix& G23, const Vector& g2, - const Matrix& G33, const Vector& g3, double f) : - GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), - info_(cref_list_of<4> (G11.cols()) (G22.cols()) (G33.cols()) (1) ) -{ - if(G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != G13.rows() || G11.rows() != g1.size() || - G22.cols() != G12.cols() || G33.cols() != G13.cols() || G22.cols() != g2.size() || G33.cols() != g3.size()) - throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); - info_(0,0) = G11; - info_(0,1) = G12; - info_(0,2) = G13; - info_(0,3) = g1; - info_(1,1) = G22; - info_(1,2) = G23; - info_(1,3) = g2; - info_(2,2) = G33; - info_(2,3) = g3; - info_(3,3)(0,0) = f; +HessianFactor::HessianFactor(Key j1, Key j2, Key j3, const Matrix& G11, + const Matrix& G12, const Matrix& G13, const Vector& g1, const Matrix& G22, + const Matrix& G23, const Vector& g2, const Matrix& G33, const Vector& g3, + double f) : + GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), info_( + cref_list_of<4>(G11.cols())(G22.cols())(G33.cols())(1)) { + if (G11.rows() != G11.cols() || G11.rows() != G12.rows() + || G11.rows() != G13.rows() || G11.rows() != g1.size() + || G22.cols() != G12.cols() || G33.cols() != G13.cols() + || G22.cols() != g2.size() || G33.cols() != g3.size()) + throw invalid_argument( + "Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + info_(0, 0) = G11; + info_(0, 1) = G12; + info_(0, 2) = G13; + info_(0, 3) = g1; + info_(1, 1) = G22; + info_(1, 2) = G23; + info_(1, 3) = g2; + info_(2, 2) = G33; + info_(2, 3) = g3; + info_(3, 3)(0, 0) = f; } /* ************************************************************************* */ namespace { -DenseIndex _getSizeHF(const Vector& m) { return m.size(); } +DenseIndex _getSizeHF(const Vector& m) { + return m.size(); +} } /* ************************************************************************* */ -HessianFactor::HessianFactor(const std::vector& js, const std::vector& Gs, - const std::vector& gs, double f) : - GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) -{ +HessianFactor::HessianFactor(const std::vector& js, + const std::vector& Gs, const std::vector& gs, double f) : + GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) { // Get the number of variables size_t variable_count = js.size(); // Verify the provided number of entries in the vectors are consistent - if(gs.size() != variable_count || Gs.size() != (variable_count*(variable_count+1))/2) - throw invalid_argument("Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \ + if (gs.size() != variable_count + || Gs.size() != (variable_count * (variable_count + 1)) / 2) + throw invalid_argument( + "Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \ in js must match the number of linear vector pieces in gs. The number of upper-diagonal blocks in Gs must be n*(n+1)/2"); // Verify the dimensions of each provided matrix are consistent // Note: equations for calculating the indices derived from the "sum of an arithmetic sequence" formula - for(size_t i = 0; i < variable_count; ++i){ + for (size_t i = 0; i < variable_count; ++i) { DenseIndex block_size = gs[i].size(); // Check rows - for(size_t j = 0; j < variable_count-i; ++j){ - size_t index = i*(2*variable_count - i + 1)/2 + j; - if(Gs[index].rows() != block_size){ - throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + for (size_t j = 0; j < variable_count - i; ++j) { + size_t index = i * (2 * variable_count - i + 1) / 2 + j; + if (Gs[index].rows() != block_size) { + throw invalid_argument( + "Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); } } // Check cols - for(size_t j = 0; j <= i; ++j){ - size_t index = j*(2*variable_count - j + 1)/2 + (i-j); - if(Gs[index].cols() != block_size){ - throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + for (size_t j = 0; j <= i; ++j) { + size_t index = j * (2 * variable_count - j + 1) / 2 + (i - j); + if (Gs[index].cols() != block_size) { + throw invalid_argument( + "Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); } } } // Fill in the blocks size_t index = 0; - for(size_t i = 0; i < variable_count; ++i){ - for(size_t j = i; j < variable_count; ++j){ + for (size_t i = 0; i < variable_count; ++i) { + for (size_t j = i; j < variable_count; ++j) { info_(i, j) = Gs[index++]; } info_(i, variable_count) = gs[i]; } - info_(variable_count, variable_count)(0,0) = f; + info_(variable_count, variable_count)(0, 0) = f; } /* ************************************************************************* */ namespace { -void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) -{ +void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) { gttic(HessianFactor_fromJacobian); const SharedDiagonal& jfModel = jf.get_model(); - if(jfModel) - { - if(jf.get_model()->isConstrained()) - throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model"); - info.full().triangularView() = jf.matrixObject().full().transpose() * - (jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal() * - jf.matrixObject().full(); + if (jfModel) { + if (jf.get_model()->isConstrained()) + throw invalid_argument( + "Cannot construct HessianFactor from JacobianFactor with constrained noise model"); + info.full().triangularView() = + jf.matrixObject().full().transpose() + * (jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal() + * jf.matrixObject().full(); } else { - info.full().triangularView() = jf.matrixObject().full().transpose() * jf.matrixObject().full(); + info.full().triangularView() = jf.matrixObject().full().transpose() + * jf.matrixObject().full(); } } } /* ************************************************************************* */ HessianFactor::HessianFactor(const JacobianFactor& jf) : - GaussianFactor(jf), info_(SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject())) -{ + GaussianFactor(jf), info_( + SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject())) { _FromJacobianHelper(jf, info_); } /* ************************************************************************* */ HessianFactor::HessianFactor(const GaussianFactor& gf) : - GaussianFactor(gf) -{ + GaussianFactor(gf) { // Copy the matrix data depending on what type of factor we're copying from - if(const JacobianFactor* jf = dynamic_cast(&gf)) - { + if (const JacobianFactor* jf = dynamic_cast(&gf)) { info_ = SymmetricBlockMatrix::LikeActiveViewOf(jf->matrixObject()); _FromJacobianHelper(*jf, info_); - } - else if(const HessianFactor* hf = dynamic_cast(&gf)) - { + } else if (const HessianFactor* hf = dynamic_cast(&gf)) { info_ = hf->info_; - } - else - { - throw std::invalid_argument("In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor"); + } else { + throw std::invalid_argument( + "In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor"); } } /* ************************************************************************* */ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, - boost::optional scatter) -{ + boost::optional scatter) { gttic(HessianFactor_MergeConstructor); boost::optional computedScatter; - if(!scatter) { + if (!scatter) { computedScatter = Scatter(factors); scatter = computedScatter; } @@ -282,11 +237,14 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, // Allocate and copy keys gttic(allocate); // Allocate with dimensions for each variable plus 1 at the end for the information vector - keys_.resize(scatter->size()); - FastVector dims(scatter->size() + 1); - BOOST_FOREACH(const Scatter::value_type& key_slotentry, *scatter) { - keys_[key_slotentry.second.slot] = key_slotentry.first; - dims[key_slotentry.second.slot] = key_slotentry.second.dimension; + const size_t n = scatter->size(); + keys_.resize(n); + FastVector dims(n + 1); + DenseIndex slot = 0; + BOOST_FOREACH(const SlotEntry& slotentry, *scatter) { + keys_[slot] = slotentry.key; + dims[slot] = slotentry.dimension; + ++slot; } dims.back() = 1; info_ = SymmetricBlockMatrix(dims); @@ -296,64 +254,50 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, // Form A' * A gttic(update); BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) - { - if(factor) { - if(const HessianFactor* hessian = dynamic_cast(factor.get())) - updateATA(*hessian, *scatter); - else if(const JacobianFactor* jacobian = dynamic_cast(factor.get())) - updateATA(*jacobian, *scatter); - else - throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian"); - } - } + if (factor) + factor->updateHessian(keys_, &info_); gttoc(update); } /* ************************************************************************* */ -void HessianFactor::print(const std::string& s, const KeyFormatter& formatter) const { +void HessianFactor::print(const std::string& s, + const KeyFormatter& formatter) const { cout << s << "\n"; cout << " keys: "; - for(const_iterator key=this->begin(); key!=this->end(); ++key) - cout << formatter(*key) << "(" << this->getDim(key) << ") "; + for (const_iterator key = begin(); key != end(); ++key) + cout << formatter(*key) << "(" << getDim(key) << ") "; cout << "\n"; - gtsam::print(Matrix(info_.full().selfadjointView()), "Augmented information matrix: "); + gtsam::print(Matrix(info_.full().selfadjointView()), + "Augmented information matrix: "); } /* ************************************************************************* */ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const { - if(!dynamic_cast(&lf)) + const HessianFactor* rhs = dynamic_cast(&lf); + if (!rhs || !Factor::equals(lf, tol)) return false; - else { - if(!Factor::equals(lf, tol)) - return false; - Matrix thisMatrix = this->info_.full().selfadjointView(); - thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0; - Matrix rhsMatrix = static_cast(lf).info_.full().selfadjointView(); - rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0; - return equal_with_abs_tol(thisMatrix, rhsMatrix, tol); - } + return equal_with_abs_tol(augmentedInformation(), rhs->augmentedInformation(), + tol); } /* ************************************************************************* */ -Matrix HessianFactor::augmentedInformation() const -{ +Matrix HessianFactor::augmentedInformation() const { return info_.full().selfadjointView(); } /* ************************************************************************* */ -Matrix HessianFactor::information() const -{ - return info_.range(0, this->size(), 0, this->size()).selfadjointView(); +Matrix HessianFactor::information() const { + return info_.range(0, size(), 0, size()).selfadjointView(); } /* ************************************************************************* */ VectorValues HessianFactor::hessianDiagonal() const { VectorValues d; // Loop over all variables - for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // Get the diagonal block, and insert its diagonal Matrix B = info_(j, j).selfadjointView(); - d.insert(keys_[j],B.diagonal()); + d.insert(keys_[j], B.diagonal()); } return d; } @@ -366,26 +310,24 @@ void HessianFactor::hessianDiagonal(double* d) const { } /* ************************************************************************* */ -map HessianFactor::hessianBlockDiagonal() const { - map blocks; +map HessianFactor::hessianBlockDiagonal() const { + map blocks; // Loop over all variables - for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // Get the diagonal block, and insert it Matrix B = info_(j, j).selfadjointView(); - blocks.insert(make_pair(keys_[j],B)); + blocks.insert(make_pair(keys_[j], B)); } return blocks; } /* ************************************************************************* */ -Matrix HessianFactor::augmentedJacobian() const -{ +Matrix HessianFactor::augmentedJacobian() const { return JacobianFactor(*this).augmentedJacobian(); } /* ************************************************************************* */ -std::pair HessianFactor::jacobian() const -{ +std::pair HessianFactor::jacobian() const { return JacobianFactor(*this).jacobian(); } @@ -396,103 +338,34 @@ double HessianFactor::error(const VectorValues& c) const { double xtg = 0, xGx = 0; // extract the relevant subset of the VectorValues // NOTE may not be as efficient - const Vector x = c.vector(this->keys()); + const Vector x = c.vector(keys()); xtg = x.dot(linearTerm()); - xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * x; - return 0.5 * (f - 2.0 * xtg + xGx); + xGx = x.transpose() * info_.range(0, size(), 0, size()).selfadjointView() * x; + return 0.5 * (f - 2.0 * xtg + xGx); } /* ************************************************************************* */ -void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter) -{ - gttic(updateATA); - // This function updates 'combined' with the information in 'update'. 'scatter' maps variables in - // the update factor to slots in the combined factor. - - // 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); - +void HessianFactor::updateHessian(const FastVector& infoKeys, + SymmetricBlockMatrix* info) const { + gttic(updateHessian_HessianFactor); // Apply updates to the upper triangle - gttic(update); - size_t nrInfoBlocks = this->info_.nBlocks(); - for(DenseIndex j2=0; j2nBlocks() - 1; + vector slots(n + 1); + for (DenseIndex j = 0; j <= n; ++j) { + const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]); + slots[j] = J; + for (DenseIndex i = 0; i <= j; ++i) { + const DenseIndex I = slots[i]; // because i<=j, slots[i] is valid. + (*info)(I, J) += info_(i, j); } } - gttoc(update); -} - -/* ************************************************************************* */ -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 - // factor. - - gttic(updateATA); - - if (update.rows() > 0) { - gttic(whiten); - // Whiten the factor if it has a noise model - boost::optional _whitenedFactor; - 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); - } - gttoc(whiten); - - // 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); - // 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); - } } /* ************************************************************************* */ -GaussianFactor::shared_ptr HessianFactor::negate() const -{ +GaussianFactor::shared_ptr HessianFactor::negate() const { shared_ptr result = boost::make_shared(*this); - result->info_.full().triangularView() = -result->info_.full().triangularView().nestedExpression(); // Negate the information matrix of the result + result->info_.full().triangularView() = + -result->info_.full().triangularView().nestedExpression(); // Negate the information matrix of the result return result; } @@ -509,7 +382,7 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, // 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) { + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // xj is the input vector Vector xj = x.at(keys_[j]); DenseIndex i = 0; @@ -518,13 +391,13 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, // blocks on the diagonal are only half y[i] += info_(j, j).selfadjointView() * xj; // for below diagonal, we take transpose block from upper triangular part - for (i = j + 1; i < (DenseIndex)size(); ++i) + for (i = j + 1; i < (DenseIndex) size(); ++i) y[i] += info_(i, j).knownOffDiagonal() * xj; } // copy to yvalues - for(DenseIndex i = 0; i < (DenseIndex)size(); ++i) { - bool didNotExist; + for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) { + bool didNotExist; VectorValues::iterator it; boost::tie(it, didNotExist) = yvalues.tryInsert(keys_[i], Vector()); if (didNotExist) @@ -539,7 +412,7 @@ VectorValues HessianFactor::gradientAtZero() const { VectorValues g; size_t n = size(); for (size_t j = 0; j < n; ++j) - g.insert(keys_[j], -info_(j,n).knownOffDiagonal()); + g.insert(keys_[j], -info_(j, n).knownOffDiagonal()); return g; } @@ -562,8 +435,7 @@ Vector HessianFactor::gradient(Key key, const VectorValues& x) const { if (i > j) { Matrix Gji = info(j, i); Gij = Gji.transpose(); - } - else { + } else { Gij = info(i, j); } // Accumulate Gij*xj to gradf @@ -575,30 +447,34 @@ Vector HessianFactor::gradient(Key key, const VectorValues& x) const { } /* ************************************************************************* */ -std::pair, boost::shared_ptr > -EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) -{ +std::pair, + boost::shared_ptr > EliminateCholesky( + const GaussianFactorGraph& factors, const Ordering& keys) { gttic(EliminateCholesky); // Build joint factor HessianFactor::shared_ptr jointFactor; try { - jointFactor = boost::make_shared(factors, Scatter(factors, keys)); - } catch(std::invalid_argument&) { + jointFactor = boost::make_shared(factors, + Scatter(factors, keys)); + } catch (std::invalid_argument&) { throw InvalidDenseElimination( "EliminateCholesky was called with a request to eliminate variables that are not\n" - "involved in the provided factors."); + "involved in the provided factors."); } // Do dense elimination GaussianConditional::shared_ptr conditional; try { size_t numberOfKeysToEliminate = keys.size(); - VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial(numberOfKeysToEliminate); - conditional = boost::make_shared(jointFactor->keys(), numberOfKeysToEliminate, Ab); + VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial( + numberOfKeysToEliminate); + conditional = boost::make_shared(jointFactor->keys(), + numberOfKeysToEliminate, Ab); // Erase the eliminated keys in the remaining factor - jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + numberOfKeysToEliminate); - } catch(CholeskyFailed&) { + jointFactor->keys_.erase(jointFactor->begin(), + jointFactor->begin() + numberOfKeysToEliminate); + } catch (const CholeskyFailed& e) { throw IndeterminantLinearSystemException(keys.front()); } @@ -607,9 +483,9 @@ EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) } /* ************************************************************************* */ -std::pair, boost::shared_ptr > -EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys) -{ +std::pair, + boost::shared_ptr > EliminatePreferCholesky( + const GaussianFactorGraph& factors, const Ordering& keys) { gttic(EliminatePreferCholesky); // If any JacobianFactors have constrained noise models, we have to convert diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 553978e37..b74d557ea 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -18,10 +18,10 @@ #pragma once +#include +#include #include #include -#include -#include #include @@ -41,30 +41,6 @@ namespace gtsam { GTSAM_EXPORT std::pair, boost::shared_ptr > EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys); - /** - * One SlotEntry stores the slot index for a variable, as well its dimension. - */ - struct GTSAM_EXPORT SlotEntry { - size_t slot, dimension; - SlotEntry(size_t _slot, size_t _dimension) - : slot(_slot), dimension(_dimension) {} - std::string toString() const; - }; - - /** - * Scatter is an intermediate data structure used when building a HessianFactor - * incrementally, to get the keys in the right order. The "scatter" is a map from - * global variable indices to slot indices in the union of involved variables. - * We also include the dimensionality of the variable. - */ - class Scatter: public FastMap { - public: - Scatter() { - } - Scatter(const GaussianFactorGraph& gfg, - boost::optional ordering = boost::none); - }; - /** * @brief A Gaussian factor using the canonical parameters (information form) * @@ -363,19 +339,12 @@ namespace gtsam { /** Return the full augmented Hessian matrix of this factor as a SymmetricBlockMatrix object. */ const SymmetricBlockMatrix& matrixObject() const { return info_; } - /** Update the factor by adding the information from the JacobianFactor + /** Update an information matrix by adding the information corresponding to this factor * (used internally during elimination). - * @param update The JacobianFactor containing the new information to add * @param scatter A mapping from variable index to slot index in this HessianFactor + * @param info The information matrix to be updated */ - void updateATA(const JacobianFactor& update, const Scatter& scatter); - - /** Update the factor by adding the information from the HessianFactor - * (used internally during elimination). - * @param update The HessianFactor containing the new information to add - * @param scatter A mapping from variable index to slot index in this HessianFactor - */ - void updateATA(const HessianFactor& update, const Scatter& scatter); + void updateHessian(const FastVector& keys, SymmetricBlockMatrix* info) const; /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; @@ -434,7 +403,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(GaussianFactor); ar & BOOST_SERIALIZATION_NVP(info_); } 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 d6e1b6e98..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) {} - size_t index() const { return this->get<0>(); } - size_t dim() const { return this->get<1>(); } - 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 eba06f99a..d4df57298 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include @@ -66,10 +66,10 @@ JacobianFactor::JacobianFactor() : /* ************************************************************************* */ JacobianFactor::JacobianFactor(const GaussianFactor& gf) { // Copy the matrix data depending on what type of factor we're copying from - if (const JacobianFactor* rhs = dynamic_cast(&gf)) - *this = JacobianFactor(*rhs); - else if (const HessianFactor* rhs = dynamic_cast(&gf)) - *this = JacobianFactor(*rhs); + if (const JacobianFactor* asJacobian = dynamic_cast(&gf)) + *this = JacobianFactor(*asJacobian); + else if (const HessianFactor* asHessian = dynamic_cast(&gf)) + *this = JacobianFactor(*asHessian); else throw std::invalid_argument( "In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor"); @@ -347,13 +347,21 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, /* ************************************************************************* */ void JacobianFactor::print(const string& s, const KeyFormatter& formatter) const { + static const Eigen::IOFormat matlab( + Eigen::StreamPrecision, // precision + 0, // flags + " ", // coeffSeparator + ";\n", // rowSeparator + "\t", // rowPrefix + "", // rowSuffix + "[\n", // matPrefix + "\n ]" // matSuffix + ); if (!s.empty()) cout << s << "\n"; for (const_iterator key = begin(); key != end(); ++key) { - cout - << formatMatrixIndented( - (boost::format(" A[%1%] = ") % formatter(*key)).str(), getA(key)) - << endl; + cout << boost::format(" A[%1%] = ") % formatter(*key); + cout << getA(key).format(matlab) << endl; } cout << formatMatrixIndented(" b = ", getb(), true) << "\n"; if (model_) @@ -365,32 +373,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; } @@ -414,8 +440,6 @@ Vector JacobianFactor::error_vector(const VectorValues& c) const { /* ************************************************************************* */ double JacobianFactor::error(const VectorValues& c) const { - if (empty()) - return 0; Vector weighted = error_vector(c); return 0.5 * weighted.dot(weighted); } @@ -479,6 +503,43 @@ map JacobianFactor::hessianBlockDiagonal() const { return blocks; } +/* ************************************************************************* */ +void JacobianFactor::updateHessian(const FastVector& infoKeys, + SymmetricBlockMatrix* info) const { + gttic(updateHessian_JacobianFactor); + + if (rows() == 0) return; + + // Whiten the factor if it has a noise model + const SharedDiagonal& model = get_model(); + if (model && !model->isUnit()) { + if (model->isConstrained()) + throw invalid_argument( + "JacobianFactor::updateHessian: cannot update information with " + "constrained noise model"); + JacobianFactor whitenedFactor = whiten(); + whitenedFactor.updateHessian(infoKeys, info); + } else { + // Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below + DenseIndex n = Ab_.nBlocks() - 1, N = info->nBlocks() - 1; + + // Apply updates to the upper triangle + // Loop over blocks of A, including RHS with j==n + vector slots(n+1); + for (DenseIndex j = 0; j <= n; ++j) { + const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]); + slots[j] = J; + // Fill off-diagonal blocks with Ai'*Aj + for (DenseIndex i = 0; i < j; ++i) { + const DenseIndex I = slots[i]; // because i& 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; + } +} + /* ************************************************************************* */ VectorValues JacobianFactor::gradientAtZero() const { VectorValues g; @@ -629,8 +735,8 @@ std::pair, jointFactor->Ab_.matrix().triangularView().setZero(); // Split elimination result into conditional and remaining factor - GaussianConditional::shared_ptr conditional = jointFactor->splitConditional( - keys.size()); + GaussianConditional::shared_ptr conditional = // + jointFactor->splitConditional(keys.size()); return make_pair(conditional, jointFactor); } @@ -659,11 +765,11 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional( } GaussianConditional::shared_ptr conditional = boost::make_shared< GaussianConditional>(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel); - const DenseIndex maxRemainingRows = std::min(Ab_.cols() - 1, originalRowEnd) + const DenseIndex maxRemainingRows = std::min(Ab_.cols(), originalRowEnd) - Ab_.rowStart() - frontalDim; const DenseIndex remainingRows = - model_ ? - std::min(model_->sigmas().size() - frontalDim, maxRemainingRows) : + model_ ? std::min(model_->sigmas().size() - frontalDim, + maxRemainingRows) : maxRemainingRows; Ab_.rowStart() += frontalDim; Ab_.rowEnd() = Ab_.rowStart() + remainingRows; diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 02ae21566..ea9958474 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -273,6 +273,13 @@ namespace gtsam { /** Get a view of the A matrix */ ABlock getA() { return Ab_.range(0, size()); } + /** Update an information matrix by adding the information corresponding to this factor + * (used internally during elimination). + * @param scatter A mapping from variable index to slot index in this HessianFactor + * @param info The information matrix to be updated + */ + void updateHessian(const FastVector& keys, SymmetricBlockMatrix* info) const; + /** Return A*x */ Vector operator*(const VectorValues& x) const; @@ -283,6 +290,17 @@ namespace gtsam { /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; + /** + * Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x + * 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): we should probably kill this if no longer needed + */ + void multiplyHessianAdd(double alpha, const double* x, double* y, + const std::vector& accumulatedDims) const; + /// A'*b for Jacobian VectorValues gradientAtZero() const; @@ -349,7 +367,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(Ab_); ar & BOOST_SERIALIZATION_NVP(model_); diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 2031a4b73..cb77902d0 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -28,6 +29,7 @@ #include #include #include +#include using namespace std; @@ -70,6 +72,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(); @@ -79,24 +86,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)); } } @@ -111,13 +119,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"); } /* ************************************************************************* */ @@ -129,6 +139,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; @@ -221,9 +237,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()) { } /* ************************************************************************* */ @@ -262,12 +280,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_); } /* ************************************************************************* */ @@ -293,7 +311,7 @@ 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 (size_t i = 0; i < sigmas.size(); ++i) + for (Vector::Index i = 0; i < sigmas.size(); ++i) if (!std::isfinite(1. / sigmas[i])) { precisions[i] = 0.0; invsigmas[i] = 0.0; @@ -342,7 +360,7 @@ Vector Constrained::whiten(const Vector& v) const { assert (b.size()==a.size()); Vector c(n); for( size_t i = 0; i < n; i++ ) { - const double& ai = a(i), &bi = b(i); + const double& ai = a(i), bi = b(i); c(i) = (bi==0.0) ? ai : ai/bi; // NOTE: not ediv_() } return c; @@ -359,8 +377,11 @@ double Constrained::distance(const Vector& v) const { /* ************************************************************************* */ Matrix Constrained::Whiten(const Matrix& H) const { - // selective scaling - return vector_scale(invsigmas(), H, true); + Matrix A = H; + for (DenseIndex i=0; i<(DenseIndex)dim_; ++i) + if (!constrained(i)) // if constrained, leave row of A as is + A.row(i) *= invsigmas_(i); + return A; } /* ************************************************************************* */ @@ -404,8 +425,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 @@ -486,7 +507,7 @@ Isotropic::shared_ptr Isotropic::Variance(size_t dim, double variance, bool smar /* ************************************************************************* */ void Isotropic::print(const string& name) const { - cout << name << "isotropic sigma " << " " << sigma_ << endl; + cout << boost::format("isotropic dim=%1% sigma=%2%") % dim() % sigma_ << endl; } /* ************************************************************************* */ @@ -514,6 +535,11 @@ void Isotropic::WhitenInPlace(Matrix& H) const { H *= invsigma_; } +/* ************************************************************************* */ +void Isotropic::whitenInPlace(Vector& v) const { + v *= invsigma_; +} + /* ************************************************************************* */ void Isotropic::WhitenInPlace(Eigen::Block H) const { H *= invsigma_; @@ -542,16 +568,6 @@ Vector Base::weight(const Vector &error) const { return w; } -/** square root version of the weight function */ -Vector Base::sqrtWeight(const Vector &error) const { - const size_t n = error.rows(); - Vector w(n); - for ( size_t i = 0 ; i < n ; ++i ) - w(i) = sqrtWeight(error(i)); - return w; -} - - /** The following three functions reweight block matrices and a vector * according to their weight implementation */ @@ -560,8 +576,7 @@ void Base::reweight(Vector& error) const { const double w = sqrtWeight(error.norm()); error *= w; } else { - const Vector w = sqrtWeight(error); - error.array() *= w.array(); + error.array() *= weight(error).cwiseSqrt().array(); } } @@ -579,7 +594,7 @@ void Base::reweight(vector &A, Vector &error) const { BOOST_FOREACH(Matrix& Aj, A) { vector_scale_inplace(W,Aj); } - error = emul(W, error); + error = W.cwiseProduct(error); } } @@ -593,7 +608,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); } } @@ -609,7 +624,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); } } @@ -627,7 +642,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); } } @@ -641,7 +656,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; @@ -653,26 +668,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; @@ -680,21 +695,21 @@ Huber::Huber(const double k, const ReweightScheme reweight) } } -double Huber::weight(const double &error) const { - return (error < k_) ? (1.0) : (k_ / fabs(error)); +double Huber::weight(double error) const { + return (fabs(error) > k_) ? k_ / fabs(error) : 1.0; } 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)); } @@ -702,7 +717,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; @@ -710,7 +725,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); } @@ -718,24 +733,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); @@ -748,24 +763,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); } @@ -774,16 +789,76 @@ 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)); } +/* ************************************************************************* */ +// GemanMcClure +/* ************************************************************************* */ +GemanMcClure::GemanMcClure(double c, const ReweightScheme reweight) + : Base(reweight), c_(c) { +} + +double GemanMcClure::weight(double error) const { + const double c2 = c_*c_; + const double c4 = c2*c2; + const double c2error = c2 + error*error; + return c4/(c2error*c2error); +} + +void GemanMcClure::print(const std::string &s="") const { + std::cout << s << ": Geman-McClure (" << c_ << ")" << std::endl; +} + +bool GemanMcClure::equals(const Base &expected, double tol) const { + const GemanMcClure* p = dynamic_cast(&expected); + if (p == NULL) return false; + return fabs(c_ - p->c_) < tol; +} + +GemanMcClure::shared_ptr GemanMcClure::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new GemanMcClure(c, reweight)); +} + +/* ************************************************************************* */ +// DCS +/* ************************************************************************* */ +DCS::DCS(double c, const ReweightScheme reweight) + : Base(reweight), c_(c) { +} + +double DCS::weight(double error) const { + const double e2 = error*error; + if (e2 > c_) + { + const double w = 2.0*c_/(c_ + e2); + return w*w; + } + + return 1.0; +} + +void DCS::print(const std::string &s="") const { + std::cout << s << ": DCS (" << c_ << ")" << std::endl; +} + +bool DCS::equals(const Base &expected, double tol) const { + const DCS* p = dynamic_cast(&expected); + if (p == NULL) return false; + return fabs(c_ - p->c_) < tol; +} + +DCS::shared_ptr DCS::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new DCS(c, reweight)); +} + } // namespace mEstimator /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 098af8b6d..db01152f6 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -20,13 +20,13 @@ #include #include +#include #include #include #include #include #include -#include namespace gtsam { @@ -59,14 +59,15 @@ 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 isConstrained() const { - return false; // default false - } + /// true if a constrained noise model, saves slow/clumsy dynamic casting + virtual bool isConstrained() const { return false; } // default false + + /// true if a unit noise model, saves slow/clumsy dynamic casting + virtual bool isUnit() const { return false; } // default false /// Dimensionality inline size_t dim() const { return dim_;} @@ -75,14 +76,16 @@ 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. - */ + /// Whiten a matrix. + virtual Matrix Whiten(const Matrix& H) const = 0; + + /// Unwhiten an error vector. virtual Vector unwhiten(const Vector& v) const = 0; virtual double distance(const Vector& v) const = 0; @@ -116,7 +119,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_); } }; @@ -189,6 +192,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; @@ -220,9 +224,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; /** @@ -234,16 +238,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();} + /// Compute information matrix + virtual Matrix information() const { return R().transpose() * R(); } + + /// 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_); } @@ -303,6 +311,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; @@ -312,7 +321,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); } /** @@ -338,7 +346,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_); @@ -387,9 +395,7 @@ namespace gtsam { virtual ~Constrained() {} /// true if a constrained noise mode, saves slow/clumsy dynamic casting - virtual bool isConstrained() const { - return true; - } + virtual bool isConstrained() const { return true; } /// Return true if a particular dimension is free or constrained bool constrained(size_t i) const; @@ -489,7 +495,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_); } @@ -545,6 +551,7 @@ namespace gtsam { virtual Vector unwhiten(const Vector& v) const; virtual Matrix Whiten(const Matrix& H) const; virtual void WhitenInPlace(Matrix& H) const; + virtual void whitenInPlace(Vector& v) const; virtual void WhitenInPlace(Eigen::Block H) const; /** @@ -556,7 +563,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_); @@ -587,23 +594,26 @@ namespace gtsam { return shared_ptr(new Unit(dim)); } + /// true if a unit noise model, saves slow/clumsy dynamic casting + virtual bool isUnit() const { return true; } + virtual void print(const std::string& name) const; virtual double Mahalanobis(const Vector& v) const {return v.dot(v); } 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); } }; @@ -629,20 +639,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; @@ -655,7 +668,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_); } }; @@ -667,16 +680,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); } }; @@ -686,12 +699,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_; @@ -700,7 +713,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_); } @@ -712,11 +725,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_; @@ -725,7 +738,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_); } @@ -741,11 +754,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_; @@ -754,7 +767,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_); } @@ -765,12 +778,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_; @@ -779,7 +792,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_); } @@ -790,12 +803,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_; @@ -804,7 +817,66 @@ 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_); + } + }; + + /// GemanMcClure implements the "Geman-McClure" robust error model + /// (Zhang97ivc). + /// + /// Note that Geman-McClure weight function uses the parameter c == 1.0, + /// but here it's allowed to use different values, so we actually have + /// the generalized Geman-McClure from (Agarwal15phd). + class GTSAM_EXPORT GemanMcClure : public Base { + public: + typedef boost::shared_ptr shared_ptr; + + GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); + virtual ~GemanMcClure() {} + 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_; + + 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(c_); + } + }; + + /// DCS implements the Dynamic Covariance Scaling robust error model + /// from the paper Robust Map Optimization (Agarwal13icra). + /// + /// Under the special condition of the parameter c == 1.0 and not + /// forcing the output weight s <= 1.0, DCS is similar to Geman-McClure. + class GTSAM_EXPORT DCS : public Base { + public: + typedef boost::shared_ptr shared_ptr; + + DCS(double c = 1.0, const ReweightScheme reweight = Block); + virtual ~DCS() {} + 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_; + + 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(c_); } @@ -848,7 +920,9 @@ 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 Matrix Whiten(const Matrix& A) const + { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; } + 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(); } @@ -867,7 +941,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_)); diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 3698edc2f..caf7d0095 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -1,16 +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: Yong-Dian Jian - * Author: Sungtae An + * @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 @@ -21,7 +36,7 @@ namespace gtsam { /*****************************************************************************/ void PCGSolverParameters::print(ostream &os) const { Base::print(os); - os << "PCGSolverParameters:" << endl; + os << "PCGSolverParameters:" << endl; preconditioner_->print(os); } @@ -32,30 +47,27 @@ PCGSolver::PCGSolver(const PCGSolverParameters &p) { } /*****************************************************************************/ -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 { @@ -67,7 +79,7 @@ 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; } /*****************************************************************************/ @@ -75,7 +87,7 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const { /* implement A^T*(A*x), assume x and AtAx are pre-allocated */ // Build a VectorValues for Vector x - VectorValues vvX = buildVectorValues(x,keyInfo_); + VectorValues vvX = buildVectorValues(x, keyInfo_); // VectorValues form of A'Ax for multiplyHessianAdd VectorValues vvAtAx; @@ -99,31 +111,33 @@ void GaussianFactorGraphSystem::getb(Vector &b) const { } /**********************************************************************************/ -void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const { +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 { +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)); @@ -137,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/RegularHessianFactor.h b/gtsam/linear/RegularHessianFactor.h new file mode 100644 index 000000000..e5e545c36 --- /dev/null +++ b/gtsam/linear/RegularHessianFactor.h @@ -0,0 +1,214 @@ +/* ---------------------------------------------------------------------------- + + * 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 RegularHessianFactor.h + * @brief HessianFactor class with constant sized blocks + * @author Sungtae An + * @date March 2014 + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +template +class RegularHessianFactor: public HessianFactor { + +public: + + typedef Eigen::Matrix VectorD; + typedef Eigen::Matrix MatrixD; + + /** Construct an n-way factor. Gs contains the upper-triangle blocks of the + * quadratic term (the Hessian matrix) provided in row-order, gs the pieces + * of the linear vector term, and f the constant term. + */ + RegularHessianFactor(const std::vector& js, + const std::vector& Gs, const std::vector& gs, double f) : + HessianFactor(js, Gs, gs, f) { + checkInvariants(); + } + + /** 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 MatrixD& G11, const MatrixD& G12, + const VectorD& g1, const MatrixD& G22, const VectorD& g2, double f) : + HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) { + } + + /** Construct a ternary 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, Key j3, + const MatrixD& G11, const MatrixD& G12, const MatrixD& G13, const VectorD& g1, + const MatrixD& G22, const MatrixD& G23, const VectorD& g2, + const MatrixD& G33, const VectorD& g3, double f) : + HessianFactor(j1, j2, j3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f) { + } + + /** Constructor with an arbitrary number of keys and with the augmented information matrix + * specified as a block matrix. */ + template + RegularHessianFactor(const KEYS& keys, + const SymmetricBlockMatrix& augmentedInformation) : + HessianFactor(keys, augmentedInformation) { + checkInvariants(); + } + + /// Construct from RegularJacobianFactor + RegularHessianFactor(const RegularJacobianFactor& jf) + : HessianFactor(jf) {} + + /// Construct from a GaussianFactorGraph + RegularHessianFactor(const GaussianFactorGraph& factors, + boost::optional scatter = boost::none) + : HessianFactor(factors, scatter) { + checkInvariants(); + } + +private: + + /// Check invariants after construction + void checkInvariants() { + if (info_.cols() != 1 + (info_.nBlocks()-1) * (DenseIndex)D) + throw std::invalid_argument( + "RegularHessianFactor constructor was given non-regular factors"); + } + + // Use Eigen magic to access raw memory + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + + // Scratch space for multiplyHessianAdd + // According to link below this is thread-safe. + // http://stackoverflow.com/questions/11160964/multiple-copies-of-the-same-object-c-thread-safe + mutable std::vector y_; + +public: + + /** y += alpha * A'*A*x */ + virtual void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const { + HessianFactor::multiplyHessianAdd(alpha, x, 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 + y_.resize(size()); + BOOST_FOREACH(VectorD & yi, y_) + yi.setZero(); + + // 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 + VectorD xj(D); + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { + Key key = keys_[j]; + const double* xj = x + key * D; + DenseIndex i = 0; + for (; i < j; ++i) + y_[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); + // blocks on the diagonal are only half + y_[i] += info_(j, j).selfadjointView() * ConstDMap(xj); + // 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(xj); + } + + // copy to yvalues + for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) { + Key key = keys_[i]; + DMap(yvalues + key * D) += alpha * y_[i]; + } + } + + /// 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 + y_.resize(size()); + BOOST_FOREACH(VectorD & yi, y_) + yi.setZero(); + + // 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 + VectorD 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/RegularJacobianFactor.h b/gtsam/linear/RegularJacobianFactor.h similarity index 55% rename from gtsam/slam/RegularJacobianFactor.h rename to gtsam/linear/RegularJacobianFactor.h index bb275ef3f..f954cba88 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/linear/RegularJacobianFactor.h @@ -21,25 +21,34 @@ #include #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 */ + // 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. */ + * 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()) : @@ -49,91 +58,68 @@ public: /** 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. */ + * factor. + * TODO Verify complies to regular + */ template RegularJacobianFactor(const KEYS& keys, - const VerticalBlockMatrix& augmentedMatrix, - const SharedDiagonal& sigmas = SharedDiagonal()) : + const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = + SharedDiagonal()) : JacobianFactor(keys, augmentedMatrix, sigmas) { } - /** 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!! */ - void multiplyHessianAdd(double alpha, const double* x, double* y, - const std::vector& accumulatedDims) const { + using JacobianFactor::multiplyHessianAdd; - /// Use eigen magic to access raw memory - typedef Eigen::Matrix VectorD; - typedef Eigen::Map MapD; - typedef Eigen::Map ConstMapD; + /** 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 (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) + // Just iterate over all A matrices and multiply in correct config part for (size_t pos = 0; pos < size(); ++pos) - { - Ax += Ab_(pos) - * ConstMapD(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); - } - /// Deal with noise properly, need to Double* whiten as we are dividing by variance + 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^T into y - for (size_t pos = 0; pos < size(); ++pos){ - MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( - pos).transpose() * Ax; - } - } - - /** Raw memory access version of multiplyHessianAdd */ - 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; 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; poswhiten(column_k); dj(k) = dot(column_k, column_k); - }else{ + } else { dj(k) = Ab_(j).col(k).squaredNorm(); } } @@ -141,10 +127,13 @@ public: } } - /** Raw memory access version of gradientAtZero - * TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n - */ - virtual void gradientAtZero(double* d) const { + /// 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(); @@ -156,19 +145,46 @@ public: } // Just iterate over all A matrices - for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + 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){ + for (size_t k = 0; k < D; ++k) { Vector column_k = Ab_(j).col(k); - dj(k) = -1.0*dot(b, column_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; + } + }; +// end class RegularJacobianFactor -} - +}// end namespace gtsam diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp index 48972a953..bfbc222ba 100644 --- a/gtsam/linear/Sampler.cpp +++ b/gtsam/linear/Sampler.cpp @@ -14,9 +14,6 @@ * @author Alex Cunningham */ -#include -#include - #include namespace gtsam { @@ -51,7 +48,7 @@ Vector Sampler::sampleDiagonal(const Vector& sigmas) { } else { typedef boost::normal_distribution Normal; Normal dist(0.0, sigma); - boost::variate_generator norm(generator_, dist); + boost::variate_generator norm(generator_, dist); result(i) = norm(); } } diff --git a/gtsam/linear/Sampler.h b/gtsam/linear/Sampler.h index f2632308b..6c84bfda2 100644 --- a/gtsam/linear/Sampler.h +++ b/gtsam/linear/Sampler.h @@ -20,7 +20,7 @@ #include -#include +#include namespace gtsam { @@ -37,7 +37,7 @@ protected: noiseModel::Diagonal::shared_ptr model_; /** generator */ - boost::minstd_rand generator_; + boost::mt19937_64 generator_; public: typedef boost::shared_ptr shared_ptr; diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp new file mode 100644 index 000000000..ed2af529f --- /dev/null +++ b/gtsam/linear/Scatter.cpp @@ -0,0 +1,91 @@ +/* ---------------------------------------------------------------------------- + + * 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 Scatter.cpp + * @author Richard Roberts + * @author Frank Dellaert + * @date June 2015 + */ + +#include +#include +#include + +#include +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +string SlotEntry::toString() const { + ostringstream oss; + oss << "SlotEntry: key=" << key << ", dim=" << dimension; + return oss.str(); +} + +/* ************************************************************************* */ +Scatter::Scatter(const GaussianFactorGraph& gfg, + boost::optional ordering) { + gttic(Scatter_Constructor); + + // If we have an ordering, pre-fill the ordered variables first + if (ordering) { + BOOST_FOREACH (Key key, *ordering) { + push_back(SlotEntry(key, 0)); + } + } + + // Now, find dimensions of variables and/or extend + BOOST_FOREACH (const GaussianFactor::shared_ptr& factor, gfg) { + if (!factor) continue; + + // TODO: Fix this hack to cope with zero-row Jacobians that come from BayesTreeOrphanWrappers + const JacobianFactor* asJacobian = dynamic_cast(factor.get()); + if (asJacobian && asJacobian->cols() <= 1) continue; + + // loop over variables + for (GaussianFactor::const_iterator variable = factor->begin(); + variable != factor->end(); ++variable) { + const Key key = *variable; + iterator it = find(key); // theoretically expensive, yet cache friendly + if (it!=end()) + it->dimension = factor->getDim(variable); + else + push_back(SlotEntry(key, factor->getDim(variable))); + } + } + + // To keep the same behavior as before, sort the keys after the ordering + iterator first = begin(); + if (ordering) first += ordering->size(); + if (first != end()) std::sort(first, end()); + + // Filter out keys with zero dimensions (if ordering had more keys) + erase(std::remove_if(begin(), end(), SlotEntry::Zero), end()); +} + +/* ************************************************************************* */ +FastVector::iterator Scatter::find(Key key) { + iterator it = begin(); + while(it != end()) { + if (it->key == key) + return it; + ++it; + } + return it; // end() +} + +/* ************************************************************************* */ + +} // gtsam diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h new file mode 100644 index 000000000..39bfa6b8d --- /dev/null +++ b/gtsam/linear/Scatter.h @@ -0,0 +1,63 @@ +/* ---------------------------------------------------------------------------- + + * 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 Scatter.h + * @brief Maps global variable indices to slot indices + * @author Richard Roberts + * @author Frank Dellaert + * @date June 2015 + */ + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +class GaussianFactorGraph; +class Ordering; + +/// One SlotEntry stores the slot index for a variable, as well its dim. +struct GTSAM_EXPORT SlotEntry { + Key key; + size_t dimension; + SlotEntry(Key _key, size_t _dimension) : key(_key), dimension(_dimension) {} + std::string toString() const; + friend bool operator<(const SlotEntry& p, const SlotEntry& q) { + return p.key < q.key; + } + static bool Zero(const SlotEntry& p) { return p.dimension==0;} +}; + +/** + * Scatter is an intermediate data structure used when building a HessianFactor + * incrementally, to get the keys in the right order. In spirit, it is a map + * from global variable indices to slot indices in the union of involved + * variables. We also include the dimensionality of the variable. + */ +class Scatter : public FastVector { + public: + /// Constructor + Scatter(const GaussianFactorGraph& gfg, + boost::optional ordering = boost::none); + + private: + + /// Find the SlotEntry with the right key (linear time worst case) + iterator find(Key key); +}; + +} // \ namespace gtsam diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index f5ee4ddfc..ee2447d2f 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -15,20 +15,40 @@ * @author: Frank Dellaert */ -#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 +#include +#include // accumulate #include #include +#include +#include #include #include diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index 86bee2188..e74b92df1 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -17,13 +17,31 @@ #pragma once -#include #include +#include +#include #include #include #include +#include +#include +#include +#include +#include + +#include #include +#include +#include +#include + +namespace boost { +namespace serialization { +class access; +} /* namespace serialization */ +} /* namespace boost */ + namespace gtsam { // Forward declarations @@ -44,7 +62,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 +88,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 +103,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.cpp b/gtsam/linear/VectorValues.cpp index 664fcf3b7..33c62cfb6 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -66,6 +66,18 @@ namespace gtsam { return result; } + /* ************************************************************************* */ + VectorValues::iterator VectorValues::insert(const std::pair& key_value) { + // Note that here we accept a pair with a reference to the Vector, but the Vector is copied as + // it is inserted into the values_ map. + std::pair result = values_.insert(key_value); + if(!result.second) + throw std::invalid_argument( + "Requested to insert variable '" + DefaultKeyFormatter(key_value.first) + + "' already in this VectorValues."); + return result.first; + } + /* ************************************************************************* */ void VectorValues::update(const VectorValues& values) { diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index a599e7269..ce7d46411 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -181,23 +181,14 @@ namespace gtsam { * @param value The vector to be inserted. * @param j The index with which the value will be associated. */ iterator insert(Key j, const Vector& value) { - return insert(std::make_pair(j, value)); // Note only passing a reference to the Vector + return insert(std::make_pair(j, value)); } /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c * j is already used. * @param value The vector to be inserted. * @param j The index with which the value will be associated. */ - iterator insert(const std::pair& key_value) { - // Note that here we accept a pair with a reference to the Vector, but the Vector is copied as - // it is inserted into the values_ map. - std::pair result = values_.insert(key_value); - if(!result.second) - throw std::invalid_argument( - "Requested to insert variable '" + DefaultKeyFormatter(key_value.first) - + "' already in this VectorValues."); - return result.first; - } + iterator insert(const std::pair& key_value); /** Insert all values from \c values. Throws an invalid_argument exception if any keys to be * inserted are already used. */ @@ -376,7 +367,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_); } }; // VectorValues definition diff --git a/gtsam/linear/linearExceptions.cpp b/gtsam/linear/linearExceptions.cpp index 88758ae7a..a4168af2d 100644 --- a/gtsam/linear/linearExceptions.cpp +++ b/gtsam/linear/linearExceptions.cpp @@ -19,6 +19,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/linear/linearExceptions.h b/gtsam/linear/linearExceptions.h index 63bc61e80..32db27fc9 100644 --- a/gtsam/linear/linearExceptions.h +++ b/gtsam/linear/linearExceptions.h @@ -17,9 +17,8 @@ */ #pragma once -#include -#include -#include +#include +#include namespace gtsam { diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 05f8c3d2a..4f7c5c335 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -83,13 +83,30 @@ TEST( GaussianBayesTree, eliminate ) { GaussianBayesTree bt = *chain.eliminateMultifrontal(chainOrdering); + Scatter scatter(chain); + EXPECT_LONGS_EQUAL(4, scatter.size()); + EXPECT_LONGS_EQUAL(1, scatter.at(0).key); + EXPECT_LONGS_EQUAL(2, scatter.at(1).key); + EXPECT_LONGS_EQUAL(3, scatter.at(2).key); + EXPECT_LONGS_EQUAL(4, scatter.at(3).key); + Matrix two = (Matrix(1, 1) << 2.).finished(); Matrix one = (Matrix(1, 1) << 1.).finished(); GaussianBayesTree bayesTree_expected; bayesTree_expected.insertRoot( - MakeClique(GaussianConditional(pair_list_of(x3, (Matrix(2, 1) << 2., 0.).finished()) (x4, (Matrix(2, 1) << 2., 2.).finished()), 2, Vector2(2., 2.)), list_of - (MakeClique(GaussianConditional(pair_list_of(x2, (Matrix(2, 1) << -2.*sqrt(2.), 0.).finished()) (x1, (Matrix(2, 1) << -sqrt(2.), -sqrt(2.)).finished()) (x3, (Matrix(2, 1) << -sqrt(2.), sqrt(2.)).finished()), 2, (Vector(2) << -2.*sqrt(2.), 0.).finished()))))); + MakeClique( + GaussianConditional( + pair_list_of(x3, (Matrix21() << 2., 0.).finished())( + x4, (Matrix21() << 2., 2.).finished()), 2, Vector2(2., 2.)), + list_of( + MakeClique( + GaussianConditional( + pair_list_of(x2, + (Matrix21() << -2. * sqrt(2.), 0.).finished())(x1, + (Matrix21() << -sqrt(2.), -sqrt(2.)).finished())(x3, + (Matrix21() << -sqrt(2.), sqrt(2.)).finished()), 2, + (Vector(2) << -2. * sqrt(2.), 0.).finished()))))); EXPECT(assert_equal(bayesTree_expected, bt)); } diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 3a2cd0fd4..10fb34988 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testCholeskyFactor.cpp + * @file testHessianFactor.cpp * @author Richard Roberts * @date Dec 15, 2010 */ @@ -38,6 +38,16 @@ using namespace gtsam; const double tol = 1e-5; +/* ************************************************************************* */ +TEST(HessianFactor, Slot) +{ + FastVector keys = list_of(2)(4)(1); + EXPECT_LONGS_EQUAL(0, GaussianFactor::Slot(keys,2)); + EXPECT_LONGS_EQUAL(1, GaussianFactor::Slot(keys,4)); + EXPECT_LONGS_EQUAL(2, GaussianFactor::Slot(keys,1)); + EXPECT_LONGS_EQUAL(3, GaussianFactor::Slot(keys,5)); // does not exist +} + /* ************************************************************************* */ TEST(HessianFactor, emptyConstructor) { @@ -270,30 +280,69 @@ TEST(HessianFactor, ConstructorNWay) } /* ************************************************************************* */ -TEST(HessianFactor, CombineAndEliminate) -{ - Matrix A01 = (Matrix(3,3) << - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0).finished(); +TEST(HessianFactor, CombineAndEliminate1) { + Matrix3 A01 = 3.0 * I_3x3; + Vector3 b0(1, 0, 0); + + Matrix3 A21 = 4.0 * I_3x3; + Vector3 b2 = Vector3::Zero(); + + GaussianFactorGraph gfg; + gfg.add(1, A01, b0); + gfg.add(1, A21, b2); + + Matrix63 A1; + A1 << A01, A21; + Vector6 b; + b << b0, b2; + + // create a full, uneliminated version of the factor + JacobianFactor jacobian(1, A1, b); + + // Make sure combining works + HessianFactor hessian(gfg); + VectorValues v; + v.insert(1, Vector3(1, 0, 0)); + EXPECT_DOUBLES_EQUAL(jacobian.error(v), hessian.error(v), 1e-9); + EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6)); + EXPECT(assert_equal(25.0 * I_3x3, hessian.information(), 1e-9)); + EXPECT( + assert_equal(jacobian.augmentedInformation(), + hessian.augmentedInformation(), 1e-9)); + + // perform elimination on jacobian + Ordering ordering = list_of(1); + GaussianConditional::shared_ptr expectedConditional; + JacobianFactor::shared_ptr expectedFactor; + boost::tie(expectedConditional, expectedFactor) = // + jacobian.eliminate(ordering); + + // Eliminate + GaussianConditional::shared_ptr actualConditional; + HessianFactor::shared_ptr actualHessian; + boost::tie(actualConditional, actualHessian) = // + EliminateCholesky(gfg, ordering); + + EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); + EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9); + EXPECT( + assert_equal(expectedFactor->augmentedInformation(), + actualHessian->augmentedInformation(), 1e-9)); + EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6)); +} + +/* ************************************************************************* */ +TEST(HessianFactor, CombineAndEliminate2) { + Matrix A01 = I_3x3; Vector3 b0(1.5, 1.5, 1.5); Vector3 s0(1.6, 1.6, 1.6); - Matrix A10 = (Matrix(3,3) << - 2.0, 0.0, 0.0, - 0.0, 2.0, 0.0, - 0.0, 0.0, 2.0).finished(); - Matrix A11 = (Matrix(3,3) << - -2.0, 0.0, 0.0, - 0.0, -2.0, 0.0, - 0.0, 0.0, -2.0).finished(); + Matrix A10 = 2.0 * I_3x3; + Matrix A11 = -2.0 * I_3x3; Vector3 b1(2.5, 2.5, 2.5); Vector3 s1(2.6, 2.6, 2.6); - Matrix A21 = (Matrix(3,3) << - 3.0, 0.0, 0.0, - 0.0, 3.0, 0.0, - 0.0, 0.0, 3.0).finished(); + Matrix A21 = 3.0 * I_3x3; Vector3 b2(3.5, 3.5, 3.5); Vector3 s2(3.6, 3.6, 3.6); @@ -302,27 +351,45 @@ TEST(HessianFactor, CombineAndEliminate) gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); - Matrix zero3x3 = zeros(3,3); - Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); - Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); - Vector9 b; b << b1, b0, b2; - Vector9 sigmas; sigmas << s1, s0, s2; + Matrix93 A0, A1; + A0 << A10, Z_3x3, Z_3x3; + A1 << A11, A01, A21; + Vector9 b, sigmas; + b << b1, b0, b2; + sigmas << s1, s0, s2; // create a full, uneliminated version of the factor - JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); + JacobianFactor jacobian(0, A0, 1, A1, b, + noiseModel::Diagonal::Sigmas(sigmas, true)); + + // Make sure combining works + HessianFactor hessian(gfg); + EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6)); + EXPECT( + assert_equal(jacobian.augmentedInformation(), + hessian.augmentedInformation(), 1e-9)); // perform elimination on jacobian + Ordering ordering = list_of(0); GaussianConditional::shared_ptr expectedConditional; - JacobianFactor::shared_ptr expectedRemainingFactor; - boost::tie(expectedConditional, expectedRemainingFactor) = expectedFactor.eliminate(Ordering(list_of(0))); + JacobianFactor::shared_ptr expectedFactor; + boost::tie(expectedConditional, expectedFactor) = // + jacobian.eliminate(ordering); // Eliminate GaussianConditional::shared_ptr actualConditional; - HessianFactor::shared_ptr actualCholeskyFactor; - boost::tie(actualConditional, actualCholeskyFactor) = EliminateCholesky(gfg, Ordering(list_of(0))); + HessianFactor::shared_ptr actualHessian; + boost::tie(actualConditional, actualHessian) = // + EliminateCholesky(gfg, ordering); EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); - EXPECT(assert_equal(HessianFactor(*expectedRemainingFactor), *actualCholeskyFactor, 1e-6)); + VectorValues v; + v.insert(1, Vector3(1, 2, 3)); + EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9); + EXPECT( + assert_equal(expectedFactor->augmentedInformation(), + actualHessian->augmentedInformation(), 1e-9)); + EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6)); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 7b2d59171..17ceaf5f0 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -540,9 +540,9 @@ TEST(JacobianFactor, EliminateQR) EXPECT(assert_equal(size_t(2), actualJF.keys().size())); EXPECT(assert_equal(Key(9), actualJF.keys()[0])); EXPECT(assert_equal(Key(11), actualJF.keys()[1])); - EXPECT(assert_equal(Matrix(R.block(6, 6, 4, 2)), actualJF.getA(actualJF.begin()), 0.001)); - EXPECT(assert_equal(Matrix(R.block(6, 8, 4, 2)), actualJF.getA(actualJF.begin()+1), 0.001)); - EXPECT(assert_equal(Vector(R.col(10).segment(6, 4)), actualJF.getb(), 0.001)); + EXPECT(assert_equal(Matrix(R.block(6, 6, 5, 2)), actualJF.getA(actualJF.begin()), 0.001)); + EXPECT(assert_equal(Matrix(R.block(6, 8, 5, 2)), actualJF.getA(actualJF.begin()+1), 0.001)); + EXPECT(assert_equal(Vector(R.col(10).segment(6, 5)), actualJF.getb(), 0.001)); EXPECT(!actualJF.get_model()); } diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 5c6b2729d..d6857c6ff 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -33,17 +33,11 @@ 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) << - 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 double inf = numeric_limits::infinity(); +static const double kSigma = 2, kInverseSigma = 1.0 / kSigma, + kVariance = kSigma * kSigma, prc = 1.0 / kVariance; +static const Matrix R = Matrix3::Identity() * kInverseSigma; +static const Matrix kCovariance = Matrix3::Identity() * kVariance; +static const Vector3 kSigmas(kSigma, kSigma, kSigma); /* ************************************************************************* */ TEST(NoiseModel, constructors) @@ -53,15 +47,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((Vector3(kVariance, kVariance, kVariance)),false)); + m.push_back(Diagonal::Precisions(Vector3(prc, prc, prc),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) @@ -77,25 +75,23 @@ TEST(NoiseModel, constructors) DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9); // test R matrix - Matrix expectedR((Matrix(3, 3) << - s_1, 0.0, 0.0, - 0.0, s_1, 0.0, - 0.0, 0.0, s_1).finished()); - BOOST_FOREACH(Gaussian::shared_ptr mi, m) - EXPECT(assert_equal(expectedR,mi->R())); + EXPECT(assert_equal(R,mi->R())); + + // test covariance + BOOST_FOREACH(Gaussian::shared_ptr mi, m) + EXPECT(assert_equal(kCovariance,mi->covariance())); + + // test covariance + BOOST_FOREACH(Gaussian::shared_ptr mi, m) + EXPECT(assert_equal(kCovariance.inverse(),mi->information())); // test Whiten operator Matrix H((Matrix(3, 4) << 0.0, 0.0, 1.0, 1.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 0.0, 1.0).finished()); - - Matrix expected((Matrix(3, 4) << - 0.0, 0.0, s_1, s_1, - 0.0, s_1, 0.0, s_1, - s_1, 0.0, 0.0, s_1).finished()); - + Matrix expected = kInverseSigma * H; BOOST_FOREACH(Gaussian::shared_ptr mi, m) EXPECT(assert_equal(expected,mi->Whiten(H))); @@ -117,9 +113,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(Vector3(kSigma, kSigma, kSigma)), 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)); @@ -136,7 +132,7 @@ TEST(NoiseModel, equals) ///* ************************************************************************* */ //TEST(NoiseModel, ConstrainedSmart ) //{ -// Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas((Vector(3) << sigma, 0.0, sigma), true); +// Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas((Vector3(sigma, 0.0, sigma), true); // Diagonal::shared_ptr n1 = boost::dynamic_pointer_cast(nonconstrained); // Constrained::shared_ptr n2 = boost::dynamic_pointer_cast(nonconstrained); // EXPECT(n1); @@ -155,8 +151,8 @@ 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 mu = Vector3(200.0, 300.0, 400.0); + Vector3 sigmas(kSigma, 0.0, 0.0); + Vector3 mu(200.0, 300.0, 400.0); actual = Constrained::All(d); // TODO: why should this be a thousand ??? Dummy variable? EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu())); @@ -182,7 +178,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(Vector3(kSigma, 0.0, kSigma)); // 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))); @@ -326,7 +322,7 @@ TEST(NoiseModel, WhitenInPlace) } /* ************************************************************************* */ -TEST(NoiseModel, robustFunction) +TEST(NoiseModel, robustFunctionHuber) { const double k = 5.0, error1 = 1.0, error2 = 10.0; const mEstimator::Huber::shared_ptr huber = mEstimator::Huber::Create(k); @@ -336,17 +332,37 @@ TEST(NoiseModel, robustFunction) DOUBLES_EQUAL(0.5, weight2, 1e-8); } +TEST(NoiseModel, robustFunctionGemanMcClure) +{ + const double k = 1.0, error1 = 1.0, error2 = 10.0; + const mEstimator::GemanMcClure::shared_ptr gmc = mEstimator::GemanMcClure::Create(k); + const double weight1 = gmc->weight(error1), + weight2 = gmc->weight(error2); + DOUBLES_EQUAL(0.25 , weight1, 1e-8); + DOUBLES_EQUAL(9.80296e-5, weight2, 1e-8); +} + +TEST(NoiseModel, robustFunctionDCS) +{ + const double k = 1.0, error1 = 1.0, error2 = 10.0; + const mEstimator::DCS::shared_ptr dcs = mEstimator::DCS::Create(k); + const double weight1 = dcs->weight(error1), + weight2 = dcs->weight(error2); + DOUBLES_EQUAL(1.0 , weight1, 1e-8); + DOUBLES_EQUAL(0.00039211, weight2, 1e-8); +} + /* ************************************************************************* */ -TEST(NoiseModel, robustNoise) +TEST(NoiseModel, robustNoiseHuber) { const double k = 10.0, error1 = 1.0, error2 = 100.0; Matrix A = (Matrix(2, 2) << 1.0, 10.0, 100.0, 1000.0).finished(); - Vector b = (Vector(2) << error1, error2).finished(); + Vector b = Vector2(error1, error2); const Robust::shared_ptr robust = Robust::Create( mEstimator::Huber::Create(k, mEstimator::Huber::Scalar), Unit::Create(2)); - robust->WhitenSystem(A,b); + robust->WhitenSystem(A, b); DOUBLES_EQUAL(error1, b(0), 1e-8); DOUBLES_EQUAL(sqrt(k*error2), b(1), 1e-8); @@ -357,6 +373,95 @@ TEST(NoiseModel, robustNoise) DOUBLES_EQUAL(sqrt(k/100.0)*1000.0, A(1,1), 1e-8); } +TEST(NoiseModel, robustNoiseGemanMcClure) +{ + const double k = 1.0, error1 = 1.0, error2 = 100.0; + const double a00 = 1.0, a01 = 10.0, a10 = 100.0, a11 = 1000.0; + Matrix A = (Matrix(2, 2) << a00, a01, a10, a11).finished(); + Vector b = Vector2(error1, error2); + const Robust::shared_ptr robust = Robust::Create( + mEstimator::GemanMcClure::Create(k, mEstimator::GemanMcClure::Scalar), + Unit::Create(2)); + + robust->WhitenSystem(A, b); + + const double k2 = k*k; + const double k4 = k2*k2; + const double k2error = k2 + error2*error2; + + const double sqrt_weight_error1 = sqrt(0.25); + const double sqrt_weight_error2 = sqrt(k4/(k2error*k2error)); + + DOUBLES_EQUAL(sqrt_weight_error1*error1, b(0), 1e-8); + DOUBLES_EQUAL(sqrt_weight_error2*error2, b(1), 1e-8); + + DOUBLES_EQUAL(sqrt_weight_error1*a00, A(0,0), 1e-8); + DOUBLES_EQUAL(sqrt_weight_error1*a01, A(0,1), 1e-8); + DOUBLES_EQUAL(sqrt_weight_error2*a10, A(1,0), 1e-8); + DOUBLES_EQUAL(sqrt_weight_error2*a11, A(1,1), 1e-8); +} + +TEST(NoiseModel, robustNoiseDCS) +{ + const double k = 1.0, error1 = 1.0, error2 = 100.0; + const double a00 = 1.0, a01 = 10.0, a10 = 100.0, a11 = 1000.0; + Matrix A = (Matrix(2, 2) << a00, a01, a10, a11).finished(); + Vector b = Vector2(error1, error2); + const Robust::shared_ptr robust = Robust::Create( + mEstimator::DCS::Create(k, mEstimator::DCS::Scalar), + Unit::Create(2)); + + robust->WhitenSystem(A, b); + + const double sqrt_weight = 2.0*k/(k + error2*error2); + + DOUBLES_EQUAL(error1, b(0), 1e-8); + DOUBLES_EQUAL(sqrt_weight*error2, b(1), 1e-8); + + DOUBLES_EQUAL(a00, A(0,0), 1e-8); + DOUBLES_EQUAL(a01, A(0,1), 1e-8); + DOUBLES_EQUAL(sqrt_weight*a10, A(1,0), 1e-8); + DOUBLES_EQUAL(sqrt_weight*a11, 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/linear/tests/testRegularHessianFactor.cpp b/gtsam/linear/tests/testRegularHessianFactor.cpp new file mode 100644 index 000000000..f53803dd1 --- /dev/null +++ b/gtsam/linear/tests/testRegularHessianFactor.cpp @@ -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 testRegularHessianFactor.cpp + * @author Frank Dellaert + * @date March 4, 2014 + */ + +#include +#include +#include + +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +/* ************************************************************************* */ +TEST(RegularHessianFactor, Constructors) +{ + // First construct a regular JacobianFactor + // 0.5*|x0 + x1 + x3 - [1;2]|^2 = 0.5*|A*x-b|^2, with A=[I I I] + Matrix A1 = I_2x2, A2 = I_2x2, A3 = I_2x2; + Vector2 b(1,2); + vector > terms; + terms += make_pair(0, A1), make_pair(1, A2), make_pair(3, A3); + RegularJacobianFactor<2> jf(terms, b); + + // Test conversion from JacobianFactor + RegularHessianFactor<2> factor(jf); + + // 0.5*|A*x-b|^2 = 0.5*(Ax-b)'*(Ax-b) = 0.5*x'*A'A*x - x'*A'b + 0.5*b'*b + // Compare with comment in HessianFactor: E(x) = 0.5 x^T G x - x^T g + 0.5 f + // Hence G = I6, g A'*b = [b;b;b], and f = b'*b = 1+4 = 5 + Matrix G11 = I_2x2; + Matrix G12 = I_2x2; + Matrix G13 = I_2x2; + + Matrix G22 = I_2x2; + Matrix G23 = I_2x2; + + Matrix G33 = I_2x2; + + Vector2 g1 = b, g2 = b, g3 = b; + + double f = 5; + + // Test ternary constructor + RegularHessianFactor<2> factor2(0, 1, 3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f); + EXPECT(assert_equal(factor,factor2)); + + // Test n-way constructor + vector keys; keys += 0, 1, 3; + vector Gs; Gs += G11, G12, G13, G22, G23, G33; + vector gs; gs += g1, g2, g3; + RegularHessianFactor<2> factor3(keys, Gs, gs, f); + EXPECT(assert_equal(factor, factor3)); + + // Test constructor from Gaussian Factor Graph + GaussianFactorGraph gfg; + gfg += jf; + RegularHessianFactor<2> factor4(gfg); + EXPECT(assert_equal(factor, factor4)); + GaussianFactorGraph gfg2; + gfg2 += factor; + RegularHessianFactor<2> factor5(gfg); + EXPECT(assert_equal(factor, factor5)); + + // Test constructor from Information matrix + Matrix info = factor.augmentedInformation(); + vector dims; dims += 2, 2, 2; + SymmetricBlockMatrix sym(dims, info, true); + RegularHessianFactor<2> factor6(keys, sym); + EXPECT(assert_equal(factor, factor6)); + + // 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 << 9, 12, 9, 12, 9, 12; + EXPECT(assert_equal(Y,AtA*X)); + + VectorValues x = map_list_of + (0, Vector2(1,2)) + (1, Vector2(3,4)) + (3, Vector2(5,6)); + + 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, Vector2::Zero()); + actualVV.insert(1, Vector2::Zero()); + actualVV.insert(3, Vector2::Zero()); + factor.multiplyHessianAdd(alpha, x, actualVV); + EXPECT(assert_equal(expected, actualVV)); + + // RAW ACCESS + Vector expected_y(8); expected_y << 9, 12, 9, 12, 0, 0, 9, 12; + Vector fast_y = Vector8::Zero(); + 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/testRegularJacobianFactor.cpp b/gtsam/linear/tests/testRegularJacobianFactor.cpp similarity index 95% rename from gtsam/slam/tests/testRegularJacobianFactor.cpp rename to gtsam/linear/tests/testRegularJacobianFactor.cpp index b56b65d7b..b8c4aa689 100644 --- a/gtsam/slam/tests/testRegularJacobianFactor.cpp +++ b/gtsam/linear/tests/testRegularJacobianFactor.cpp @@ -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 testRegularJacobianFactor.cpp * @brief unit test regular jacobian factors @@ -5,13 +16,13 @@ * @date Nov 12, 2014 */ -#include -#include - -#include +#include #include #include #include +#include + +#include #include #include diff --git a/gtsam/linear/tests/testScatter.cpp b/gtsam/linear/tests/testScatter.cpp new file mode 100644 index 000000000..575f29c26 --- /dev/null +++ b/gtsam/linear/tests/testScatter.cpp @@ -0,0 +1,65 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testScatter.cpp + * @author Frank Dellaert + * @date June, 2015 + */ + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::X; + +/* ************************************************************************* */ +TEST(HessianFactor, CombineAndEliminate) { + static const size_t m = 3, n = 3; + Matrix A01 = + (Matrix(m, n) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(); + Vector3 b0(1.5, 1.5, 1.5); + Vector3 s0(1.6, 1.6, 1.6); + + Matrix A10 = + (Matrix(m, n) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0).finished(); + Matrix A11 = (Matrix(m, n) << -2.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0, -2.0) + .finished(); + Vector3 b1(2.5, 2.5, 2.5); + Vector3 s1(2.6, 2.6, 2.6); + + Matrix A21 = + (Matrix(m, n) << 3.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 3.0).finished(); + Vector3 b2(3.5, 3.5, 3.5); + Vector3 s2(3.6, 3.6, 3.6); + + GaussianFactorGraph gfg; + gfg.add(X(1), A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); + gfg.add(X(0), A10, X(1), A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); + gfg.add(X(1), A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); + + Scatter scatter(gfg); + EXPECT_LONGS_EQUAL(2, scatter.size()); + EXPECT(assert_equal(X(0), scatter.at(0).key)); + EXPECT(assert_equal(X(1), scatter.at(1).key)); + EXPECT_LONGS_EQUAL(n, scatter.at(0).dimension); + EXPECT_LONGS_EQUAL(n, scatter.at(1).dimension); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index f93788af9..71fdbe6a6 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -38,6 +38,7 @@ using namespace gtsam::serializationTestHelpers; /* ************************************************************************* */ // Export Noisemodels +// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 2f03d72a4..0d7e05515 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -27,83 +27,50 @@ namespace gtsam { //------------------------------------------------------------------------------ // Inner class PreintegratedMeasurements //------------------------------------------------------------------------------ -AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements( - const Vector3& bias, const Matrix3& measuredOmegaCovariance) : - PreintegratedRotation(measuredOmegaCovariance), biasHat_(bias) -{ - preintMeasCov_.setZero(); -} - -//------------------------------------------------------------------------------ -AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() : - PreintegratedRotation(I_3x3), biasHat_(Vector3()) -{ - preintMeasCov_.setZero(); -} - -//------------------------------------------------------------------------------ -void AHRSFactor::PreintegratedMeasurements::print(const string& s) const { +void PreintegratedAhrsMeasurements::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 PreintegratedRotation::equals(other, tol) - && equal_with_abs_tol(biasHat_, other.biasHat_, tol); +bool PreintegratedAhrsMeasurements::equals( + const PreintegratedAhrsMeasurements& other, double tol) const { + return PreintegratedRotation::equals(other, tol) && + equal_with_abs_tol(biasHat_, other.biasHat_, tol); } //------------------------------------------------------------------------------ -void AHRSFactor::PreintegratedMeasurements::resetIntegration() { +void PreintegratedAhrsMeasurements::resetIntegration() { PreintegratedRotation::resetIntegration(); preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ -void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( - const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor) { +void PreintegratedAhrsMeasurements::integrateMeasurement( + const Vector3& measuredOmega, double deltaT) { - // First we compensate the measurements for the bias - Vector3 correctedOmega = measuredOmega - biasHat_; - - // 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(); - // rotation rate vector in the body frame - correctedOmega = body_R_sensor * correctedOmega; - } - - // 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 !! - - // Update Jacobian - update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT); - - // Update rotation and deltaTij. - Matrix3 Fr; // Jacobian of the update - updateIntegratedRotationAndDeltaT(incrR, deltaT, Fr); + Matrix3 D_incrR_integratedOmega, Fr; + PreintegratedRotation::integrateMeasurement(measuredOmega, + biasHat_, deltaT, &D_incrR_integratedOmega, &Fr); // first order uncertainty propagation // the deltaT allows to pass from continuous time noise to discrete time noise - preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() - + gyroscopeCovariance() * deltaT; + preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() + p().gyroscopeCovariance * deltaT; } //------------------------------------------------------------------------------ -Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias, - boost::optional H) const { +Vector3 PreintegratedAhrsMeasurements::predict(const Vector3& bias, + OptionalJacobian<3,3> H) const { const Vector3 biasOmegaIncr = bias - biasHat_; - return biascorrectedThetaRij(biasOmegaIncr, H); + const Rot3 biascorrected = biascorrectedDeltaRij(biasOmegaIncr, H); + Matrix3 D_omega_biascorrected; + const Vector3 omega = Rot3::Logmap(biascorrected, H ? &D_omega_biascorrected : 0); + if (H) (*H) = D_omega_biascorrected * (*H); + return omega; } //------------------------------------------------------------------------------ -Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles( +Vector PreintegratedAhrsMeasurements::DeltaAngles( const Vector& msr_gyro_t, const double msr_dt, const Vector3& delta_angles) { @@ -121,22 +88,16 @@ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles( //------------------------------------------------------------------------------ // AHRSFactor methods //------------------------------------------------------------------------------ -AHRSFactor::AHRSFactor() : - _PIM_(Vector3(), Z_3x3) { -} +AHRSFactor::AHRSFactor( + Key rot_i, Key rot_j, Key bias, + const PreintegratedAhrsMeasurements& preintegratedMeasurements) + : Base(noiseModel::Gaussian::Covariance( + preintegratedMeasurements.preintMeasCov_), + rot_i, rot_j, bias), + _PIM_(preintegratedMeasurements) {} -AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, - const PreintegratedMeasurements& preintegratedMeasurements, - const Vector3& omegaCoriolis, boost::optional body_P_sensor) : - Base( - noiseModel::Gaussian::Covariance( - preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), _PIM_( - preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_( - body_P_sensor) { -} - -//------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const { +//------------------------------------------------------------------------------ return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -147,20 +108,13 @@ void AHRSFactor::print(const string& s, cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","; _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: "); } //------------------------------------------------------------------------------ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { const This *e = dynamic_cast(&other); - 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_ - && body_P_sensor_->equals(*e->body_P_sensor_))); + return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol); } //------------------------------------------------------------------------------ @@ -172,8 +126,7 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, const Vector3 biascorrectedOmega = _PIM_.predict(bias, H3); // Coriolis term - const Vector3 coriolis = _PIM_.integrateCoriolis(Ri, omegaCoriolis_); - const Matrix3 coriolisHat = skewSymmetric(coriolis); + const Vector3 coriolis = _PIM_.integrateCoriolis(Ri); const Vector3 correctedOmega = biascorrectedOmega - coriolis; // Prediction @@ -191,7 +144,7 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, if (H1) { // dfR/dRi H1->resize(3, 3); - Matrix3 D_coriolis = -D_cDeltaRij_cOmega * coriolisHat; + Matrix3 D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis); (*H1) << D_fR_fRrot * (-actualRij.transpose() - fRrot.transpose() * D_coriolis); } @@ -199,7 +152,7 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, if (H2) { // dfR/dPosej H2->resize(3, 3); - (*H2) << D_fR_fRrot * Matrix3::Identity(); + (*H2) << D_fR_fRrot; } if (H3) { @@ -215,15 +168,13 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, } //------------------------------------------------------------------------------ -Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, - const PreintegratedMeasurements preintegratedMeasurements, - const Vector3& omegaCoriolis, boost::optional body_P_sensor) { - +Rot3 AHRSFactor::Predict( + const Rot3& rot_i, const Vector3& bias, + const PreintegratedAhrsMeasurements preintegratedMeasurements) { const Vector3 biascorrectedOmega = preintegratedMeasurements.predict(bias); // Coriolis term - const Vector3 coriolis = // - preintegratedMeasurements.integrateCoriolis(rot_i, omegaCoriolis); + const Vector3 coriolis = preintegratedMeasurements.integrateCoriolis(rot_i); const Vector3 correctedOmega = biascorrectedOmega - coriolis; const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); @@ -231,4 +182,31 @@ Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, return rot_i.compose(correctedDeltaRij); } -} //namespace gtsam +//------------------------------------------------------------------------------ +AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, + const PreintegratedMeasurements& pim, + const Vector3& omegaCoriolis, + const boost::optional& body_P_sensor) + : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), rot_i, rot_j, bias), + _PIM_(pim) { + boost::shared_ptr p = + boost::make_shared(pim.p()); + p->body_P_sensor = body_P_sensor; + _PIM_.p_ = p; +} + +//------------------------------------------------------------------------------ +Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, + const PreintegratedMeasurements pim, + const Vector3& omegaCoriolis, + const boost::optional& body_P_sensor) { + boost::shared_ptr p = + boost::make_shared(pim.p()); + p->omegaCoriolis = omegaCoriolis; + p->body_P_sensor = body_P_sensor; + PreintegratedMeasurements newPim = pim; + newPim.p_ = p; + return Predict(rot_i, bias, newPim); +} + +} // namespace gtsam diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 42bcb8027..c2a88bd44 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -26,91 +26,94 @@ namespace gtsam { -class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3 { -public: +/** + * PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope + * measurements (rotation rates) and the corresponding covariance matrix. + * Can be built incrementally so as to avoid costly integration at time of factor construction. + */ +class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation { + + protected: + + Vector3 biasHat_; ///< Angular rate bias values used during preintegration. + Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) + + /// Default constructor, only for serialization + PreintegratedAhrsMeasurements() {} + + friend class AHRSFactor; + + public: /** - * CombinedPreintegratedMeasurements accumulates (integrates) the Gyroscope - * measurements (rotation rates) and the corresponding covariance matrix. - * The measurements are then used to build the Preintegrated AHRS factor. - * Can be built incrementally so as to avoid costly integration at time of - * factor construction. + * Default constructor, initialize with no measurements + * @param bias Current estimate of acceleration and rotation rate biases */ - class GTSAM_EXPORT PreintegratedMeasurements : public PreintegratedRotation { + PreintegratedAhrsMeasurements(const boost::shared_ptr& p, + const Vector3& biasHat) : + PreintegratedRotation(p), biasHat_(biasHat) { + resetIntegration(); + } - friend class AHRSFactor; + const Params& p() const { return *boost::static_pointer_cast(p_);} + const Vector3& biasHat() const { return biasHat_; } + const Matrix3& preintMeasCov() const { return preintMeasCov_; } - protected: - Vector3 biasHat_; ///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer - Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) + /// print + void print(const std::string& s = "Preintegrated Measurements: ") const; - public: + /// equals + bool equals(const PreintegratedAhrsMeasurements&, double tol = 1e-9) const; - /// Default constructor - PreintegratedMeasurements(); + /// Reset inetgrated quantities to zero + void resetIntegration(); - /** - * Default constructor, initialize with no measurements - * @param bias Current estimate of acceleration and rotation rate biases - * @param measuredOmegaCovariance Covariance matrix of measured angular rate - */ - PreintegratedMeasurements(const Vector3& bias, - const Matrix3& measuredOmegaCovariance); + /** + * Add a single Gyroscope measurement to the preintegration. + * @param measureOmedga Measured angular velocity (in body frame) + * @param deltaT Time step + */ + void integrateMeasurement(const Vector3& measuredOmega, double deltaT); - Vector3 biasHat() const { - return biasHat_; - } - const Matrix3& preintMeasCov() const { - return preintMeasCov_; - } + /// Predict bias-corrected incremental rotation + /// TODO: The matrix Hbias is the derivative of predict? Unit-test? + Vector3 predict(const Vector3& bias, OptionalJacobian<3,3> H = boost::none) const; - /// print - void print(const std::string& s = "Preintegrated Measurements: ") const; + // 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, + const Vector3& delta_angles); - /// equals - bool equals(const PreintegratedMeasurements&, double tol = 1e-9) const; - - /// TODO: Document - void resetIntegration(); - - /** - * Add a single Gyroscope measurement to the preintegration. - * @param measureOmedga Measured angular velocity (in body frame) - * @param deltaT Time step - * @param body_P_sensor Optional sensor frame - */ - void integrateMeasurement(const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor = boost::none); - - /// Predict bias-corrected incremental rotation - /// TODO: The matrix Hbias is the derivative of predict? Unit-test? - Vector3 predict(const Vector3& bias, boost::optional H = - boost::none) const; - - // 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, - const Vector3& delta_angles); - - 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_); - } - }; + /// @deprecated constructor + PreintegratedAhrsMeasurements(const Vector3& biasHat, + const Matrix3& measuredOmegaCovariance) + : PreintegratedRotation(boost::make_shared()), + biasHat_(biasHat) { + p_->gyroscopeCovariance = measuredOmegaCovariance; + resetIntegration(); + } 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(p_); + ar & BOOST_SERIALIZATION_NVP(biasHat_); + } +}; + +class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3 { + typedef AHRSFactor This; typedef NoiseModelFactor3 Base; - 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 + PreintegratedAhrsMeasurements _PIM_; + + /** Default constructor - only use for serialization */ + AHRSFactor() {} public: @@ -121,22 +124,15 @@ public: typedef boost::shared_ptr shared_ptr; #endif - /** Default constructor - only use for serialization */ - AHRSFactor(); - /** * Constructor * @param rot_i previous rot key * @param rot_j current rot key * @param bias previous bias key * @param preintegratedMeasurements preintegrated measurements - * @param omegaCoriolis rotation rate of the inertial frame - * @param body_P_sensor Optional pose of the sensor frame in the body frame */ AHRSFactor(Key rot_i, Key rot_j, Key bias, - const PreintegratedMeasurements& preintegratedMeasurements, - const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none); + const PreintegratedAhrsMeasurements& preintegratedMeasurements); virtual ~AHRSFactor() { } @@ -152,14 +148,10 @@ public: virtual bool equals(const NonlinearFactor&, double tol = 1e-9) const; /// Access the preintegrated measurements. - const PreintegratedMeasurements& preintegratedMeasurements() const { + const PreintegratedAhrsMeasurements& preintegratedMeasurements() const { return _PIM_; } - const Vector3& omegaCoriolis() const { - return omegaCoriolis_; - } - /** implement functions needed to derive from Factor */ /// vector of errors @@ -169,28 +161,39 @@ public: boost::none) const; /// predicted states from IMU + /// TODO(frank): relationship with PIM predict ?? + static Rot3 Predict( + const Rot3& rot_i, const Vector3& bias, + const PreintegratedAhrsMeasurements preintegratedMeasurements); + + /// @deprecated name + typedef PreintegratedAhrsMeasurements PreintegratedMeasurements; + + /// @deprecated constructor + AHRSFactor(Key rot_i, Key rot_j, Key bias, + const PreintegratedMeasurements& preintegratedMeasurements, + const Vector3& omegaCoriolis, + const boost::optional& body_P_sensor = boost::none); + + /// @deprecated static function static Rot3 predict(const Rot3& rot_i, const Vector3& bias, const PreintegratedMeasurements preintegratedMeasurements, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none); + const boost::optional& body_P_sensor = boost::none); 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(_PIM_); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } }; // AHRSFactor -typedef AHRSFactor::PreintegratedMeasurements AHRSFactorPreintegratedMeasurements; - } //namespace gtsam diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 08b75c00d..b61a861d6 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -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)); @@ -206,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)); diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 1c22bab9a..4dbbfda27 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -29,143 +29,125 @@ namespace gtsam { using namespace std; //------------------------------------------------------------------------------ -// Inner class CombinedPreintegratedMeasurements +// Inner class PreintegratedCombinedMeasurements //------------------------------------------------------------------------------ -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{ +void PreintegratedCombinedMeasurements::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); +bool PreintegratedCombinedMeasurements::equals( + const PreintegratedCombinedMeasurements& other, double tol) const { + return PreintegrationBase::equals(other, tol) && + equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); } //------------------------------------------------------------------------------ -void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){ +void PreintegratedCombinedMeasurements::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) { +// sugar for derivative blocks +#define D_R_R(H) (H)->block<3,3>(0,0) +#define D_R_t(H) (H)->block<3,3>(0,3) +#define D_R_v(H) (H)->block<3,3>(0,6) +#define D_t_R(H) (H)->block<3,3>(3,0) +#define D_t_t(H) (H)->block<3,3>(3,3) +#define D_t_v(H) (H)->block<3,3>(3,6) +#define D_v_R(H) (H)->block<3,3>(6,0) +#define D_v_t(H) (H)->block<3,3>(6,3) +#define D_v_v(H) (H)->block<3,3>(6,6) +#define D_a_a(H) (H)->block<3,3>(9,9) +#define D_g_g(H) (H)->block<3,3>(12,12) - // 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). +//------------------------------------------------------------------------------ +void PreintegratedCombinedMeasurements::integrateMeasurement( + const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT) { - Vector3 correctedAcc, correctedOmega; - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); + const Matrix3 dRij = deltaXij_.R(); // expensive when quaternion - 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. + Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr + Matrix9 F_9x9; // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix93 G1,G2; + PreintegrationBase::update(measuredAcc, measuredOmega, deltaT, + &D_incrR_integratedOmega, &F_9x9, &G1, &G2); // 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; + Matrix3 H_vel_biasacc = -dRij * deltaT; + Matrix3 H_angles_biasomega = -D_incrR_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; + Eigen::Matrix F; 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; + F.block<9, 9>(0, 0) = F_9x9; + F.block<3, 3>(0, 12) = H_angles_biasomega; + F.block<3, 3>(6, 9) = H_vel_biasacc; + F.block<6, 6>(9, 9) = I_6x6; // first order uncertainty propagation // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() - Matrix G_measCov_Gt = Matrix::Zero(15,15); + Eigen::Matrix G_measCov_Gt; + G_measCov_Gt.setZero(15, 15); + + // BLOCK DIAGONAL TERMS + D_t_t(&G_measCov_Gt) = deltaT * p().integrationCovariance; + D_v_v(&G_measCov_Gt) = (1 / deltaT) * (H_vel_biasacc) + * (p().accelerometerCovariance + p().biasAccOmegaInit.block<3, 3>(0, 0)) + * (H_vel_biasacc.transpose()); + D_R_R(&G_measCov_Gt) = (1 / deltaT) * (H_angles_biasomega) + * (p().gyroscopeCovariance + p().biasAccOmegaInit.block<3, 3>(3, 3)) + * (H_angles_biasomega.transpose()); + D_a_a(&G_measCov_Gt) = deltaT * p().biasAccCovariance; + D_g_g(&G_measCov_Gt) = deltaT * p().biasOmegaCovariance; -// 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(); + Matrix3 temp = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0) + * H_angles_biasomega.transpose(); + D_v_R(&G_measCov_Gt) = temp; + D_R_v(&G_measCov_Gt) = temp.transpose(); preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; - - // 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; - } } +//------------------------------------------------------------------------------ +PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements( + const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, + const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInit, + const bool use2ndOrderIntegration) { + if (!use2ndOrderIntegration) + throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity"); + biasHat_ = biasHat; + boost::shared_ptr p = Params::MakeSharedD(); + p->gyroscopeCovariance = measuredOmegaCovariance; + p->accelerometerCovariance = measuredAccCovariance; + p->integrationCovariance = integrationErrorCovariance; + p->biasAccCovariance = biasAccCovariance; + p->biasOmegaCovariance = biasOmegaCovariance; + p->biasAccOmegaInit = biasAccOmegaInit; + p_ = p; + resetIntegration(); + preintMeasCov_.setZero(); +} //------------------------------------------------------------------------------ // 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) {} +CombinedImuFactor::CombinedImuFactor( + Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, + const PreintegratedCombinedMeasurements& pim) + : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias_i, bias_j), + _PIM_(pim) {} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { @@ -174,25 +156,20 @@ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { } //------------------------------------------------------------------------------ -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()) << "," +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); +bool CombinedImuFactor::equals(const NonlinearFactor& other, double tol) const { + const This* e = dynamic_cast(&other); + return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol); } //------------------------------------------------------------------------------ @@ -212,53 +189,85 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_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_, + Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, 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; + H1->block<9, 6>(0, 0) = D_r_pose_i; // adding: [dBiasAcc/dPi ; dBiasOmega/dPi] - H1->block < 6, 6 > (9, 0) = Z_6x6; + H1->block<6, 6>(9, 0).setZero(); } if (H2) { H2->resize(15, 3); - H2->block < 9, 3 > (0, 0) = D_r_vel_i; + 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); + H2->block<6, 3>(9, 0).setZero(); } if (H3) { H3->resize(15, 6); - H3->block < 9, 6 > (0, 0) = D_r_pose_j; + H3->block<9, 6>(0, 0) = D_r_pose_j; // adding: [dBiasAcc/dPj ; dBiasOmega/dPj] - H3->block < 6, 6 > (9, 0) = Z_6x6; + H3->block<6, 6>(9, 0).setZero(); } if (H4) { H4->resize(15, 3); - H4->block < 9, 3 > (0, 0) = D_r_vel_j; + 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); + H4->block<6, 3>(9, 0).setZero(); } if (H5) { H5->resize(15, 6); - H5->block < 9, 6 > (0, 0) = D_r_bias_i; + 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; + H5->block<6, 6>(9, 0) = Hbias_i; } if (H6) { H6->resize(15, 6); - H6->block < 9, 6 > (0, 0) = Matrix::Zero(9, 6); + H6->block<9, 6>(0, 0).setZero(); // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j] - H6->block < 6, 6 > (9, 0) = Hbias_j; + H6->block<6, 6>(9, 0) = Hbias_j; } // overall error Vector r(15); - r << r_pvR, fbias; // vector of size 15 + r << r_Rpv, fbias; // vector of size 15 return r; } +//------------------------------------------------------------------------------ +CombinedImuFactor::CombinedImuFactor( + Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, + const CombinedPreintegratedMeasurements& pim, const Vector3& n_gravity, + const Vector3& omegaCoriolis, const boost::optional& body_P_sensor, + const bool use2ndOrderCoriolis) + : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias_i, bias_j), + _PIM_(pim) { + boost::shared_ptr p = + boost::make_shared(pim.p()); + p->n_gravity = n_gravity; + p->omegaCoriolis = omegaCoriolis; + p->body_P_sensor = body_P_sensor; + p->use2ndOrderCoriolis = use2ndOrderCoriolis; + _PIM_.p_ = p; +} +//------------------------------------------------------------------------------ +void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, + Pose3& pose_j, Vector3& vel_j, + const imuBias::ConstantBias& bias_i, + CombinedPreintegratedMeasurements& pim, + const Vector3& n_gravity, + const Vector3& omegaCoriolis, + const bool use2ndOrderCoriolis) { + // use deprecated predict + PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity, + omegaCoriolis, use2ndOrderCoriolis); + pose_j = pvb.pose; + vel_j = pvb.velocity; +} + } /// namespace gtsam + diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index a7c6ecd39..737275759 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -24,21 +24,17 @@ /* GTSAM includes */ #include #include -#include -#include +#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. * - ** REFERENCES: + * 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 @@ -46,14 +42,139 @@ namespace gtsam { * TRO, 28(1):61-76, 2012. * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: * Computation of the Jacobian Matrices", Tech. Report, 2013. + * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, IMU Preintegration on + * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation, + * Robotics: Science and Systems (RSS), 2015. */ +/** + * PreintegratedCombinedMeasurements 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. + * + * @addtogroup SLAM + */ +class PreintegratedCombinedMeasurements : public PreintegrationBase { + +public: + + /// Parameters for pre-integration: + /// Usage: Create just a single Params and pass a shared pointer to the constructor + struct Params : PreintegrationBase::Params { + 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 + + /// See two named constructors below for good values of n_gravity in body frame + Params(const Vector3& n_gravity) : + PreintegrationBase::Params(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance( + I_3x3), biasAccOmegaInit(I_6x6) { + } + + // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis + static boost::shared_ptr MakeSharedD(double g = 9.81) { + return boost::make_shared(Vector3(0, 0, g)); + } + + // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis + static boost::shared_ptr MakeSharedU(double g = 9.81) { + return boost::make_shared(Vector3(0, 0, -g)); + } + + private: + /// Default constructor makes unitialized params struct + Params() {} + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params); + ar& BOOST_SERIALIZATION_NVP(biasAccCovariance); + ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance); + ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInit); + } + }; + + protected: + /* Covariance matrix of the preintegrated measurements + * COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] + * (first-order propagation from *measurementCovariance*). + * PreintegratedCombinedMeasurements also include the biases and keep the correlation + * between the preintegrated measurements and the biases + */ + Eigen::Matrix preintMeasCov_; + + PreintegratedCombinedMeasurements() {} + + friend class CombinedImuFactor; + + public: + /** + * Default constructor, initializes the class with no measurements + * @param bias Current estimate of acceleration and rotation rate biases + */ + PreintegratedCombinedMeasurements(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat) + : PreintegrationBase(p, biasHat) { + preintMeasCov_.setZero(); + } + + Params& p() const { return *boost::static_pointer_cast(p_);} + + /// print + void print(const std::string& s = "Preintegrated Measurements:") const; + + /// equals + bool equals(const PreintegratedCombinedMeasurements& expected, + double tol = 1e-9) const; + + /// Re-initialize PreintegratedCombinedMeasurements + 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); + + /// methods to access class variables + Matrix preintMeasCov() const { return preintMeasCov_; } + + /// deprecated constructor + /// NOTE(frank): assumes Z-Down convention, only second order integration supported + PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat, + const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, + const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, + const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = true); + + 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_); + } +}; + /** * 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 + * measurements, which are "summarized" using the PreintegratedCombinedMeasurements * 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 @@ -61,95 +182,26 @@ namespace gtsam { * "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 + * 3) The covariance matrix of the PreintegratedCombinedMeasurements preserves * the correlation between the bias uncertainty and the preintegrated * measurements uncertainty. + * + * @addtogroup SLAM */ -class CombinedImuFactor: public NoiseModelFactor6, public ImuFactorBase{ +class CombinedImuFactor: public NoiseModelFactor6 { 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_); - } - }; - private: typedef CombinedImuFactor This; - typedef NoiseModelFactor6 Base; + typedef NoiseModelFactor6 Base; - CombinedPreintegratedMeasurements _PIM_; + PreintegratedCombinedMeasurements _PIM_; + + /** Default constructor - only use for serialization */ + CombinedImuFactor() {} public: @@ -160,9 +212,6 @@ public: typedef boost::shared_ptr shared_ptr; #endif - /** Default constructor - only use for serialization */ - CombinedImuFactor(); - /** * Constructor * @param pose_i Previous pose key @@ -171,16 +220,11 @@ public: * @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 + * @param PreintegratedCombinedMeasurements Combined IMU measurements */ - 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); + CombinedImuFactor( + Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, + const PreintegratedCombinedMeasurements& preintegratedMeasurements); virtual ~CombinedImuFactor() {} @@ -190,43 +234,57 @@ public: /** implement functions needed for Testable */ /// print - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; /// equals - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; /** Access the preintegrated measurements. */ - const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { - return _PIM_; } + const PreintegratedCombinedMeasurements& 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, + 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; + 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 typename + typedef gtsam::PreintegratedCombinedMeasurements CombinedPreintegratedMeasurements; + + /// @deprecated constructor + CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, + Key bias_j, const CombinedPreintegratedMeasurements& pim, + const Vector3& n_gravity, const Vector3& omegaCoriolis, + const boost::optional& body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false); + + // @deprecated use PreintegrationBase::predict + static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, + Vector3& vel_j, const imuBias::ConstantBias& bias_i, + CombinedPreintegratedMeasurements& pim, + const Vector3& n_gravity, const Vector3& omegaCoriolis, + const bool use2ndOrderCoriolis = false); 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("NoiseModelFactor6", - boost::serialization::base_object(*this)); + 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 - -typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements; +}; +// class CombinedImuFactor } /// namespace gtsam 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 ce93bd740..7664c7fd5 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -17,8 +17,10 @@ #pragma once +#include +#include +#include #include -#include /* * NOTES: @@ -36,57 +38,61 @@ 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): + 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; - } + /** 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 accelerometer bias */ + const Vector3& accelerometer() const { + return biasAcc_; + } - /** get gyroscope bias */ - const Vector3& gyroscope() const { return biasGyro_; } + /** 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 an accelerometer measurement using this bias model, and optionally compute Jacobians */ + Vector3 correctAccelerometer(const Vector3& measurement, + OptionalJacobian<3, 6> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const { + if (H1) (*H1) << -I_3x3, Z_3x3; + if (H2) (*H2) << I_3x3; + 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, + OptionalJacobian<3, 6> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const { + if (H1) (*H1) << Z_3x3, -I_3x3; + if (H2) (*H2) << I_3x3; + return measurement - biasGyro_; + } // // H1: Jacobian w.r.t. IMUBias // // H2: Jacobian w.r.t. pose @@ -118,76 +124,90 @@ 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 Group - /// @{ + /// @} + /// @name Group + /// @{ - /** identity for group operation */ - static ConstantBias identity() { return ConstantBias(); } + /** identity for group operation */ + static ConstantBias identity() { + return ConstantBias(); + } - /** inverse */ - inline ConstantBias operator-() const { - return ConstantBias(-biasAcc_, -biasGyro_); - } + /** inverse */ + inline ConstantBias operator-() const { + return ConstantBias(-biasAcc_, -biasGyro_); + } - /** addition */ - ConstantBias operator+(const ConstantBias& b) const { - return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_); - } + /** addition */ + ConstantBias operator+(const ConstantBias& b) const { + return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_); + } - /** subtraction */ - ConstantBias operator-(const ConstantBias& b) const { - return ConstantBias(b.biasAcc_ - biasAcc_, b.biasGyro_ - biasGyro_); - } + /** subtraction */ + ConstantBias operator-(const ConstantBias& b) const { + return ConstantBias(biasAcc_ - b.biasAcc_, biasGyro_ - b.biasGyro_); + } - /// @} + /// @} - /// @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);} - /// @} + /// @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); + } + /// @} - private: +private: - /// @name Advanced Interface - /// @{ + /// @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_); - } + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(biasAcc_); + ar & BOOST_SERIALIZATION_NVP(biasGyro_); + } - /// @} + /// @} - }; // ConstantBias class +}; // ConstantBias class } // namespace imuBias template<> @@ -197,4 +217,3 @@ struct traits : public internal::VectorSpace< } // namespace gtsam - diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 81120b7c6..70527d91d 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -31,110 +31,92 @@ 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 { +void PreintegratedImuMeasurements::print(const string& s) const { PreintegrationBase::print(s); - cout << " preintMeasCov = \n [ " << preintMeasCov_ << " ]" << endl; + cout << " preintMeasCov \n[" << preintMeasCov_ << "]" << endl; } //------------------------------------------------------------------------------ -bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const { - return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) - && PreintegrationBase::equals(expected, tol); +bool PreintegratedImuMeasurements::equals( + const PreintegratedImuMeasurements& other, double tol) const { + return PreintegrationBase::equals(other, tol) + && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); } //------------------------------------------------------------------------------ -void ImuFactor::PreintegratedMeasurements::resetIntegration(){ +void PreintegratedImuMeasurements::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) { +void PreintegratedImuMeasurements::integrateMeasurement( + const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { - 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); + static const Matrix93 Gi = (Matrix93() << Z_3x3, I_3x3, Z_3x3).finished(); // 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); + Matrix93 G1, G2; + Matrix3 D_incrR_integratedOmega; + PreintegrationBase::update(measuredAcc, measuredOmega, dt, + &D_incrR_integratedOmega, &F, &G1, &G2); // 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(); + // as in [2] we consider a first order propagation that can be seen as a prediction phase in EKF + /* --------------------------------------------------------------------------------------------*/ + // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G' // NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) - // NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done blockwise, - // as G and measurementCovariance are blockdiagonal 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; +#ifdef OLD_JACOBIAN_CALCULATION + Matrix9 G; + G << G1, Gi, G2; + Matrix9 Cov; + Cov << p().accelerometerCovariance / dt, Z_3x3, Z_3x3, + Z_3x3, p().integrationCovariance * dt, Z_3x3, + Z_3x3, Z_3x3, p().gyroscopeCovariance / dt; + preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G * Cov * G.transpose(); +#else + preintMeasCov_ = F * preintMeasCov_ * F.transpose() + + Gi * (p().integrationCovariance * dt) * Gi.transpose() // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt) + + G1 * (p().accelerometerCovariance / dt) * G1.transpose() + + G2 * (p().gyroscopeCovariance / dt) * G2.transpose(); +#endif +} - 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(); - } +//------------------------------------------------------------------------------ +PreintegratedImuMeasurements::PreintegratedImuMeasurements( + const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration) { + if (!use2ndOrderIntegration) + throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity"); + biasHat_ = biasHat; + boost::shared_ptr p = Params::MakeSharedD(); + p->gyroscopeCovariance = measuredOmegaCovariance; + p->accelerometerCovariance = measuredAccCovariance; + p->integrationCovariance = integrationErrorCovariance; + p_ = p; + resetIntegration(); +} - // 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 - } +//------------------------------------------------------------------------------ +void PreintegratedImuMeasurements::integrateMeasurement( + const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, + boost::optional body_P_sensor) { + // modify parameters to accommodate deprecated method:-( + p_->body_P_sensor = body_P_sensor; + integrateMeasurement(measuredAcc, measuredOmega, deltaT); } //------------------------------------------------------------------------------ // 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) {} +ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, + const PreintegratedImuMeasurements& pim) : + Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias), _PIM_(pim) { +} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { @@ -144,23 +126,22 @@ gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { //------------------------------------------------------------------------------ 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(""); + cout << s << "ImuFactor(" << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) + << ")\n"; + Base::print(""); _PIM_.print(" preintegrated measurements:"); - this->noiseModel_->print(" noise model: "); + // 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); +bool ImuFactor::equals(const NonlinearFactor& other, double tol) const { + const This *e = dynamic_cast(&other); + return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) + && Base::equals(*e, tol); } //------------------------------------------------------------------------------ @@ -169,9 +150,36 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, 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); + H1, H2, H3, H4, H5); } -} /// namespace gtsam +//------------------------------------------------------------------------------ +ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, + const PreintegratedMeasurements& pim, const Vector3& n_gravity, + const Vector3& omegaCoriolis, const boost::optional& body_P_sensor, + const bool use2ndOrderCoriolis) : + Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias), _PIM_(pim) { + boost::shared_ptr p = boost::make_shared< + PreintegratedMeasurements::Params>(pim.p()); + p->n_gravity = n_gravity; + p->omegaCoriolis = omegaCoriolis; + p->body_P_sensor = body_P_sensor; + p->use2ndOrderCoriolis = use2ndOrderCoriolis; + _PIM_.p_ = p; +} + +//------------------------------------------------------------------------------ +void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, + Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, + PreintegratedMeasurements& pim, const Vector3& n_gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) { + // use deprecated predict + PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity, + omegaCoriolis, use2ndOrderCoriolis); + pose_j = pvb.pose; + vel_j = pvb.velocity; +} + +} // namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index b91c76ade..46306f8c7 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -24,21 +24,21 @@ /* GTSAM includes */ #include #include -#include #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. + * 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: + * C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on + * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", + * Robotics: Science and Systems (RSS), 2015. + * + * 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 @@ -46,96 +46,119 @@ namespace gtsam { * TRO, 28(1):61-76, 2012. * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: * Computation of the Jacobian Matrices", Tech. Report, 2013. + * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on + * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", + * Robotics: Science and Systems (RSS), 2015. */ +/** + * PreintegratedIMUMeasurements 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. + * + * @addtogroup SLAM + */ +class PreintegratedImuMeasurements: public PreintegrationBase { + + friend class ImuFactor; + +protected: + + Matrix9 preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] + ///< (first-order propagation from *measurementCovariance*). + + /// Default constructor for serialization + PreintegratedImuMeasurements() {} + +public: + + /** + * Constructor, initializes the class with no measurements + * @param bias Current estimate of acceleration and rotation rate biases + * @param p Parameters, typically fixed in a single application + */ + PreintegratedImuMeasurements(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat) : + PreintegrationBase(p, biasHat) { + preintMeasCov_.setZero(); + } + + /// print + void print(const std::string& s = "Preintegrated Measurements:") const; + + /// equals + bool equals(const PreintegratedImuMeasurements& expected, + double tol = 1e-9) const; + + /// Re-initialize PreintegratedIMUMeasurements + 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 dt Time interval between this and the last IMU measurement + */ + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, double dt); + + /// Return pre-integrated measurement covariance + Matrix preintMeasCov() const { return preintMeasCov_; } + + /// @deprecated constructor + /// NOTE(frank): assumes Z-Down convention, only second order integration supported + PreintegratedImuMeasurements(const imuBias::ConstantBias& biasHat, + const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, + bool use2ndOrderIntegration = true); + + /// @deprecated version of integrateMeasurement with body_P_sensor + /// Use parameters instead + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, double dt, + boost::optional body_P_sensor); + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar & bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), preintMeasCov_.size())); + } +}; + /** * 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. + * are "summarized" using the PreintegratedIMUMeasurements 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. + * + * @addtogroup SLAM */ -class ImuFactor: public NoiseModelFactor5, public ImuFactorBase { +class ImuFactor: public NoiseModelFactor5 { public: - /** - * 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 { - - friend class ImuFactor; - - protected: - - Eigen::Matrix preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] - ///< (first-order propagation from *measurementCovariance*). - - 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 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); - - /// print - void print(const std::string& s = "Preintegrated Measurements:") const; - - /// 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 two consecutive IMU measurements - * @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_;} - - 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_); - } - }; - - private: +private: typedef ImuFactor This; - typedef NoiseModelFactor5 Base; + typedef NoiseModelFactor5 Base; - PreintegratedMeasurements _PIM_; + PreintegratedImuMeasurements _PIM_; - public: +public: /** Shorthand for a smart pointer to a factor */ #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 @@ -145,7 +168,7 @@ public: #endif /** Default constructor - only use for serialization */ - ImuFactor(); + ImuFactor() {} /** * Constructor @@ -154,18 +177,12 @@ public: * @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); + const PreintegratedImuMeasurements& preintegratedMeasurements); - virtual ~ImuFactor() {} + virtual ~ImuFactor() { + } /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const; @@ -173,42 +190,59 @@ public: /** implement functions needed for Testable */ /// print - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; /// equals - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; /** Access the preintegrated measurements. */ - const PreintegratedMeasurements& preintegratedMeasurements() const { - return _PIM_; } + const PreintegratedImuMeasurements& 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; + 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; - private: + /// @deprecated typename + typedef PreintegratedImuMeasurements PreintegratedMeasurements; + + /// @deprecated constructor, in the new one gravity, coriolis settings are in Params + ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, + const PreintegratedMeasurements& preintegratedMeasurements, + const Vector3& n_gravity, const Vector3& omegaCoriolis, + const boost::optional& body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false); + + /// @deprecated use PreintegrationBase::predict, + /// in the new one gravity, coriolis settings are in Params + static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, + Vector3& vel_j, const imuBias::ConstantBias& bias_i, + PreintegratedMeasurements& pim, const Vector3& n_gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); + +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("NoiseModelFactor5", - boost::serialization::base_object(*this)); + 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 +}; +// class ImuFactor -typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements; +/// traits +template<> struct traits : public Testable {}; } /// namespace gtsam diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h deleted file mode 100644 index 1b7919a82..000000000 --- a/gtsam/navigation/ImuFactorBase.h +++ /dev/null @@ -1,84 +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 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/NavState.cpp b/gtsam/navigation/NavState.cpp new file mode 100644 index 000000000..6266c328f --- /dev/null +++ b/gtsam/navigation/NavState.cpp @@ -0,0 +1,358 @@ +/* ---------------------------------------------------------------------------- + + * 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 NavState.h + * @brief Navigation state composing of attitude, position, and velocity + * @author Frank Dellaert + * @date July 2015 + **/ + +#include + +using namespace std; + +namespace gtsam { + +#define TIE(R,t,v,x) const Rot3& R = (x).R_;const Point3& t = (x).t_;const Velocity3& v = (x).v_; + +//------------------------------------------------------------------------------ +NavState NavState::FromPoseVelocity(const Pose3& pose, const Vector3& vel, + OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2) { + if (H1) + *H1 << I_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3; + if (H2) + *H2 << Z_3x3, Z_3x3, pose.rotation().transpose(); + return NavState(pose, vel); +} + +//------------------------------------------------------------------------------ +const Rot3& NavState::attitude(OptionalJacobian<3, 9> H) const { + if (H) + *H << I_3x3, Z_3x3, Z_3x3; + return R_; +} + +//------------------------------------------------------------------------------ +const Point3& NavState::position(OptionalJacobian<3, 9> H) const { + if (H) + *H << Z_3x3, R(), Z_3x3; + return t_; +} + +//------------------------------------------------------------------------------ +const Vector3& NavState::velocity(OptionalJacobian<3, 9> H) const { + if (H) + *H << Z_3x3, Z_3x3, R(); + return v_; +} + +//------------------------------------------------------------------------------ +Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const { + const Rot3& nRb = R_; + const Vector3& n_v = v_; + Matrix3 D_bv_nRb; + Vector3 b_v = nRb.unrotate(n_v, H ? &D_bv_nRb : 0); + if (H) + *H << D_bv_nRb, Z_3x3, I_3x3; + return b_v; +} + +//------------------------------------------------------------------------------ +Matrix7 NavState::matrix() const { + Matrix3 R = this->R(); + Matrix7 T; + T << R, Z_3x3, t(), Z_3x3, R, v(), Vector6::Zero().transpose(), 1.0; + return T; +} + +//------------------------------------------------------------------------------ +void NavState::print(const string& s) const { + attitude().print(s + ".R"); + position().print(s + ".p"); + gtsam::print((Vector) v_, s + ".v"); +} + +//------------------------------------------------------------------------------ +bool NavState::equals(const NavState& other, double tol) const { + return R_.equals(other.R_, tol) && t_.equals(other.t_, tol) + && equal_with_abs_tol(v_, other.v_, tol); +} + +//------------------------------------------------------------------------------ +NavState NavState::inverse() const { + TIE(nRb, n_t, n_v, *this); + const Rot3 bRn = nRb.inverse(); + return NavState(bRn, -(bRn * n_t), -(bRn * n_v)); +} + +//------------------------------------------------------------------------------ +NavState NavState::operator*(const NavState& bTc) const { + TIE(nRb, n_t, n_v, *this); + TIE(bRc, b_t, b_v, bTc); + return NavState(nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v); +} + +//------------------------------------------------------------------------------ +NavState::PositionAndVelocity // +NavState::operator*(const PositionAndVelocity& b_tv) const { + TIE(nRb, n_t, n_v, *this); + const Point3& b_t = b_tv.first; + const Velocity3& b_v = b_tv.second; + return PositionAndVelocity(nRb * b_t + n_t, nRb * b_v + n_v); +} + +//------------------------------------------------------------------------------ +Point3 NavState::operator*(const Point3& b_t) const { + return Point3(R_ * b_t + t_); +} + +//------------------------------------------------------------------------------ +Velocity3 NavState::operator*(const Velocity3& b_v) const { + return Velocity3(R_ * b_v + v_); +} + +//------------------------------------------------------------------------------ +NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, + OptionalJacobian<9, 9> H) { + Matrix3 D_R_xi; + const Rot3 R = Rot3::Expmap(dR(xi), H ? &D_R_xi : 0); + const Point3 p = Point3(dP(xi)); + const Vector v = dV(xi); + const NavState result(R, p, v); + if (H) { + *H << D_R_xi, Z_3x3, Z_3x3, // + Z_3x3, R.transpose(), Z_3x3, // + Z_3x3, Z_3x3, R.transpose(); + } + return result; +} + +//------------------------------------------------------------------------------ +Vector9 NavState::ChartAtOrigin::Local(const NavState& x, + OptionalJacobian<9, 9> H) { + Vector9 xi; + Matrix3 D_xi_R; + xi << Rot3::Logmap(x.R_, H ? &D_xi_R : 0), x.t(), x.v(); + if (H) { + *H << D_xi_R, Z_3x3, Z_3x3, // + Z_3x3, x.R(), Z_3x3, // + Z_3x3, Z_3x3, x.R(); + } + return xi; +} + +//------------------------------------------------------------------------------ +NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> H) { + if (H) + throw runtime_error("NavState::Expmap derivative not implemented yet"); + + Eigen::Block n_omega_nb = dR(xi); + Eigen::Block v = dP(xi); + Eigen::Block a = dV(xi); + + // NOTE(frank): See Pose3::Expmap + Rot3 nRb = Rot3::Expmap(n_omega_nb); + double theta2 = n_omega_nb.dot(n_omega_nb); + if (theta2 > numeric_limits::epsilon()) { + // Expmap implements a "screw" motion in the direction of n_omega_nb + Vector3 n_t_parallel = n_omega_nb * n_omega_nb.dot(v); // component along rotation axis + Vector3 omega_cross_v = n_omega_nb.cross(v); // points towards axis + Point3 n_t = Point3(omega_cross_v - nRb * omega_cross_v + n_t_parallel) + / theta2; + Vector3 n_v_parallel = n_omega_nb * n_omega_nb.dot(a); // component along rotation axis + Vector3 omega_cross_a = n_omega_nb.cross(a); // points towards axis + Vector3 n_v = (omega_cross_a - nRb * omega_cross_a + n_v_parallel) / theta2; + return NavState(nRb, n_t, n_v); + } else { + return NavState(nRb, Point3(v), a); + } +} + +//------------------------------------------------------------------------------ +Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) { + if (H) + throw runtime_error("NavState::Logmap derivative not implemented yet"); + + TIE(nRb, n_p, n_v, nTb); + Vector3 n_t = n_p.vector(); + + // NOTE(frank): See Pose3::Logmap + Vector9 xi; + Vector3 n_omega_nb = Rot3::Logmap(nRb); + double theta = n_omega_nb.norm(); + if (theta * theta <= numeric_limits::epsilon()) { + xi << n_omega_nb, n_t, n_v; + } else { + Matrix3 W = skewSymmetric(n_omega_nb / theta); + // Formula from Agrawal06iros, equation (14) + // simplified with Mathematica, and multiplying in n_t to avoid matrix math + double factor = (1 - theta / (2. * tan(0.5 * theta))); + Vector3 n_x = W * n_t; + Vector3 v = n_t - (0.5 * theta) * n_x + factor * (W * n_x); + Vector3 n_y = W * n_v; + Vector3 a = n_v - (0.5 * theta) * n_y + factor * (W * n_y); + xi << n_omega_nb, v, a; + } + return xi; +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::AdjointMap() const { + // NOTE(frank): See Pose3::AdjointMap + const Matrix3 nRb = R(); + Matrix3 pAr = skewSymmetric(t()) * nRb; + Matrix3 vAr = skewSymmetric(v()) * nRb; + Matrix9 adj; + // nR/bR nR/bP nR/bV nP/bR nP/bP nP/bV nV/bR nV/bP nV/bV + adj << nRb, Z_3x3, Z_3x3, pAr, nRb, Z_3x3, vAr, Z_3x3, nRb; + return adj; +} + +//------------------------------------------------------------------------------ +Matrix7 NavState::wedge(const Vector9& xi) { + const Matrix3 Omega = skewSymmetric(dR(xi)); + Matrix7 T; + T << Omega, Z_3x3, dP(xi), Z_3x3, Omega, dV(xi), Vector6::Zero().transpose(), 1.0; + return T; +} + +//------------------------------------------------------------------------------ +// sugar for derivative blocks +#define D_R_R(H) (H)->block<3,3>(0,0) +#define D_R_t(H) (H)->block<3,3>(0,3) +#define D_R_v(H) (H)->block<3,3>(0,6) +#define D_t_R(H) (H)->block<3,3>(3,0) +#define D_t_t(H) (H)->block<3,3>(3,3) +#define D_t_v(H) (H)->block<3,3>(3,6) +#define D_v_R(H) (H)->block<3,3>(6,0) +#define D_v_t(H) (H)->block<3,3>(6,3) +#define D_v_v(H) (H)->block<3,3>(6,6) + +//------------------------------------------------------------------------------ +NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, + const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, + OptionalJacobian<9, 3> G2) const { + + Vector9 xi; + Matrix39 D_xiP_state; + Vector3 b_v = bodyVelocity(F ? &D_xiP_state : 0); + double dt22 = 0.5 * dt * dt; + + // Integrate on tangent space. TODO(frank): coriolis? + dR(xi) << dt * b_omega; + dP(xi) << dt * b_v + dt22 * b_acceleration; + dV(xi) << dt * b_acceleration; + + // Bring back to manifold + Matrix9 D_newState_xi; + NavState newState = retract(xi, F, G1 || G2 ? &D_newState_xi : 0); + + // Derivative wrt state is computed by retract directly + // However, as dP(xi) also depends on state, we need to add that contribution + if (F) { + F->middleRows<3>(3) += dt * D_t_t(F) * D_xiP_state; + } + // derivative wrt acceleration + if (G1) { + // D_newState_dPxi = D_newState_xi.middleCols<3>(3) + // D_dPxi_acc = dt22 * I_3x3 + // D_newState_dVxi = D_newState_xi.rightCols<3>() + // D_dVxi_acc = dt * I_3x3 + // *G2 = D_newState_acc = D_newState_dPxi * D_dPxi_acc + D_newState_dVxi * D_dVxi_acc + *G1 = D_newState_xi.middleCols<3>(3) * dt22 + + D_newState_xi.rightCols<3>() * dt; + } + // derivative wrt omega + if (G2) { + // D_newState_dRxi = D_newState_xi.leftCols<3>() + // D_dRxi_omega = dt * I_3x3 + // *G1 = D_newState_omega = D_newState_dRxi * D_dRxi_omega + *G2 = D_newState_xi.leftCols<3>() * dt; + } + return newState; +} + +//------------------------------------------------------------------------------ +Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, + OptionalJacobian<9, 9> H) const { + TIE(nRb, n_t, n_v, *this); + const double dt2 = dt * dt; + const Vector3 omega_cross_vel = omega.cross(n_v); + + Vector9 xi; + Matrix3 D_dP_R; + dR(xi) << nRb.unrotate((-dt) * omega, H ? &D_dP_R : 0); + dP(xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper + dV(xi) << ((-2.0 * dt) * omega_cross_vel); + if (secondOrder) { + const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t.vector())); + dP(xi) -= (0.5 * dt2) * omega_cross2_t; + dV(xi) -= dt * omega_cross2_t; + } + if (H) { + H->setZero(); + const Matrix3 Omega = skewSymmetric(omega); + const Matrix3 D_cross_state = Omega * R(); + H->setZero(); + D_R_R(H) << D_dP_R; + D_t_v(H) << (-dt2) * D_cross_state; + D_v_v(H) << (-2.0 * dt) * D_cross_state; + if (secondOrder) { + const Matrix3 D_cross2_state = Omega * D_cross_state; + D_t_t(H) -= (0.5 * dt2) * D_cross2_state; + D_v_t(H) -= dt * D_cross2_state; + } + } + return xi; +} + +//------------------------------------------------------------------------------ +Vector9 NavState::correctPIM(const Vector9& pim, double dt, + const Vector3& n_gravity, const boost::optional& omegaCoriolis, + bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 9> H2) const { + const Rot3& nRb = R_; + const Velocity3& n_v = v_; // derivative is Ri ! + const double dt22 = 0.5 * dt * dt; + + Vector9 xi; + Matrix3 D_dP_Ri1, D_dP_Ri2, D_dP_nv, D_dV_Ri; + dR(xi) = dR(pim); + dP(xi) = dP(pim) + + dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri1 : 0, H2 ? &D_dP_nv : 0) + + dt22 * nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0); + dV(xi) = dV(pim) + dt * nRb.unrotate(n_gravity, H1 ? &D_dV_Ri : 0); + + if (omegaCoriolis) { + xi += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1); + } + + if (H1 || H2) { + Matrix3 Ri = nRb.matrix(); + + if (H1) { + if (!omegaCoriolis) + H1->setZero(); // if coriolis H1 is already initialized + D_t_R(H1) += dt * D_dP_Ri1 + dt22 * D_dP_Ri2; + D_t_v(H1) += dt * D_dP_nv * Ri; + D_v_R(H1) += dt * D_dV_Ri; + } + if (H2) { + H2->setIdentity(); + } + } + + return xi; +} +//------------------------------------------------------------------------------ + +}/// namespace gtsam diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h new file mode 100644 index 000000000..9561aa77b --- /dev/null +++ b/gtsam/navigation/NavState.h @@ -0,0 +1,257 @@ +/* ---------------------------------------------------------------------------- + + * 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 NavState.h + * @brief Navigation state composing of attitude, position, and velocity + * @author Frank Dellaert + * @date July 2015 + **/ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/// Velocity is currently typedef'd to Vector3 +typedef Vector3 Velocity3; + +/** + * Navigation state: Pose (rotation, translation) + velocity + * Implements semi-direct Lie group product of SO(3) and R^6, where R^6 is position/velocity + */ +class NavState: public LieGroup { +private: + + // TODO(frank): + // - should we rename t_ to p_? if not, we should rename dP do dT + Rot3 R_; ///< Rotation nRb, rotates points/velocities in body to points/velocities in nav + Point3 t_; ///< position n_t, in nav frame + Velocity3 v_; ///< velocity n_v in nav frame + +public: + + typedef std::pair PositionAndVelocity; + + /// @name Constructors + /// @{ + + /// Default constructor + NavState() : + v_(Vector3::Zero()) { + } + /// Construct from attitude, position, velocity + NavState(const Rot3& R, const Point3& t, const Velocity3& v) : + R_(R), t_(t), v_(v) { + } + /// Construct from pose and velocity + NavState(const Pose3& pose, const Velocity3& v) : + R_(pose.rotation()), t_(pose.translation()), v_(v) { + } + /// Construct from Matrix group representation (no checking) + NavState(const Matrix7& T) : + R_(T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 6)), v_(T.block<3, 1>(3, 6)) { + } + /// Construct from SO(3) and R^6 + NavState(const Matrix3& R, const Vector9 tv) : + R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) { + } + /// Named constructor with derivatives + static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel, + OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2); + + /// @} + /// @name Component Access + /// @{ + + inline const Rot3& attitude() const { + return R_; + } + inline const Point3& position() const { + return t_; + } + inline const Velocity3& velocity() const { + return v_; + } + const Rot3& attitude(OptionalJacobian<3, 9> H) const; + const Point3& position(OptionalJacobian<3, 9> H) const; + const Velocity3& velocity(OptionalJacobian<3, 9> H) const; + + const Pose3 pose() const { + return Pose3(attitude(), position()); + } + + /// @} + /// @name Derived quantities + /// @{ + + /// Return rotation matrix. Induces computation in quaternion mode + Matrix3 R() const { + return R_.matrix(); + } + /// Return quaternion. Induces computation in matrix mode + Quaternion quaternion() const { + return R_.toQuaternion(); + } + /// Return position as Vector3 + Vector3 t() const { + return t_.vector(); + } + /// Return velocity as Vector3. Computation-free. + const Vector3& v() const { + return v_; + } + // Return velocity in body frame + Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = boost::none) const; + + /// Return matrix group representation, in MATLAB notation: + /// nTb = [nRb 0 n_t; 0 nRb n_v; 0 0 1] + /// With this embedding in GL(3), matrix product agrees with compose + Matrix7 matrix() const; + + /// @} + /// @name Testable + /// @{ + + /// print + void print(const std::string& s = "") const; + + /// equals + bool equals(const NavState& other, double tol = 1e-8) const; + + /// @} + /// @name Group + /// @{ + + /// identity for group operation + static NavState identity() { + return NavState(); + } + + /// inverse transformation with derivatives + NavState inverse() const; + + /// Group compose is the semi-direct product as specified below: + /// nTc = nTb * bTc = (nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v) + NavState operator*(const NavState& bTc) const; + + /// Native group action is on position/velocity pairs *in the body frame* as follows: + /// nTb * (b_t,b_v) = (nRb * b_t + n_t, nRb * b_v + n_v) + PositionAndVelocity operator*(const PositionAndVelocity& b_tv) const; + + /// Act on position alone, n_t = nRb * b_t + n_t + Point3 operator*(const Point3& b_t) const; + + /// Act on velocity alone, n_v = nRb * b_v + n_v + Velocity3 operator*(const Velocity3& b_v) const; + + /// @} + /// @name Manifold + /// @{ + + // Tangent space sugar. + // TODO(frank): move to private navstate namespace in cpp + static Eigen::Block dR(Vector9& v) { + return v.segment<3>(0); + } + static Eigen::Block dP(Vector9& v) { + return v.segment<3>(3); + } + static Eigen::Block dV(Vector9& v) { + return v.segment<3>(6); + } + static Eigen::Block dR(const Vector9& v) { + return v.segment<3>(0); + } + static Eigen::Block dP(const Vector9& v) { + return v.segment<3>(3); + } + static Eigen::Block dV(const Vector9& v) { + return v.segment<3>(6); + } + + // Chart at origin, constructs components separately (as opposed to Expmap) + struct ChartAtOrigin { + static NavState Retract(const Vector9& xi, // + OptionalJacobian<9, 9> H = boost::none); + static Vector9 Local(const NavState& x, // + OptionalJacobian<9, 9> H = boost::none); + }; + + /// @} + /// @name Lie Group + /// @{ + + /// Exponential map at identity - create a NavState from canonical coordinates + static NavState Expmap(const Vector9& xi, // + OptionalJacobian<9, 9> H = boost::none); + + /// Log map at identity - return the canonical coordinates for this NavState + static Vector9 Logmap(const NavState& p, // + OptionalJacobian<9, 9> H = boost::none); + + /// Calculate Adjoint map, a 9x9 matrix that takes a tangent vector in the body frame, and transforms + /// it to a tangent vector at identity, such that Exmap(AdjointMap()*xi) = (*this) * Exmpap(xi); + Matrix9 AdjointMap() const; + + /// wedge creates Lie algebra element from tangent vector + static Matrix7 wedge(const Vector9& xi); + + /// @} + /// @name Dynamics + /// @{ + + /// Integrate forward in time given angular velocity and acceleration in body frame + /// Uses second order integration for position, returns derivatives except dt. + NavState update(const Vector3& b_acceleration, const Vector3& b_omega, + const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, + OptionalJacobian<9, 3> G2) const; + + /// Compute tangent space contribution due to Coriolis forces + Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, + OptionalJacobian<9, 9> H = boost::none) const; + + /// Correct preintegrated tangent vector with our velocity and rotated gravity, + /// taking into account Coriolis forces if omegaCoriolis is given. + Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity, + const boost::optional& omegaCoriolis, bool use2ndOrderCoriolis = + false, OptionalJacobian<9, 9> H1 = boost::none, + OptionalJacobian<9, 9> H2 = boost::none) const; + +private: + /// @{ + /// serialization + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(R_); + ar & BOOST_SERIALIZATION_NVP(t_); + ar & BOOST_SERIALIZATION_NVP(v_); + } + /// @} +}; + +// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors +template<> +struct traits : Testable, internal::LieGroupTraits { +}; + +// Partial specialization of wedge +// TODO: deprecate, make part of traits +template<> +inline Matrix wedge(const Vector& xi) { + return NavState::wedge(xi); +} + +} // namespace gtsam diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp new file mode 100644 index 000000000..708d1a3f6 --- /dev/null +++ b/gtsam/navigation/PreintegratedRotation.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 + + * -------------------------------------------------------------------------- */ + +/** + * @file PreintegratedRotation.cpp + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#include "PreintegratedRotation.h" + +using namespace std; + +namespace gtsam { + +void PreintegratedRotation::Params::print(const string& s) const { + cout << s << endl; + cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl; + if (omegaCoriolis) + cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")" + << endl; + if (body_P_sensor) + body_P_sensor->print("body_P_sensor"); +} + +void PreintegratedRotation::resetIntegration() { + deltaTij_ = 0.0; + deltaRij_ = Rot3(); + delRdelBiasOmega_ = Z_3x3; +} + +void PreintegratedRotation::print(const string& s) const { + cout << s << endl; + cout << " deltaTij [" << deltaTij_ << "]" << endl; + cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << endl; +} + +bool PreintegratedRotation::equals(const PreintegratedRotation& other, + double tol) const { + return deltaRij_.equals(other.deltaRij_, tol) + && fabs(deltaTij_ - other.deltaTij_) < tol + && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); +} + +Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega, + const Vector3& biasHat, double deltaT, + OptionalJacobian<3, 3> D_incrR_integratedOmega) const { + + // First we compensate the measurements for the bias + Vector3 correctedOmega = measuredOmega - biasHat; + + // Then compensate for sensor-body displacement: we express the quantities + // (originally in the IMU frame) into the body frame + if (p_->body_P_sensor) { + Matrix3 body_R_sensor = p_->body_P_sensor->rotation().matrix(); + // rotation rate vector in the body frame + correctedOmega = body_R_sensor * correctedOmega; + } + + // rotation vector describing rotation increment computed from the + // current rotation rate measurement + const Vector3 integratedOmega = correctedOmega * deltaT; + return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! +} + +void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega, + const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega, + Matrix3* F) { + + const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT, + D_incrR_integratedOmega); + + // Update deltaTij and rotation + deltaTij_ += deltaT; + deltaRij_ = deltaRij_.compose(incrR, F); + + // Update Jacobian + const Matrix3 incrRt = incrR.transpose(); + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + - *D_incrR_integratedOmega * deltaT; +} + +Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr, + OptionalJacobian<3, 3> H) const { + const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasOmegaIncr; + const Rot3 deltaRij_biascorrected = deltaRij_.expmap(biasInducedOmega, + boost::none, H); + if (H) + (*H) *= delRdelBiasOmega_; + return deltaRij_biascorrected; +} + +Vector3 PreintegratedRotation::integrateCoriolis(const Rot3& rot_i) const { + if (!p_->omegaCoriolis) + return Vector3::Zero(); + return rot_i.transpose() * (*p_->omegaCoriolis) * deltaTij_; +} + +} // namespace gtsam diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 67deb2d99..eb25b35e2 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -21,7 +21,8 @@ #pragma once -#include +#include +#include namespace gtsam { @@ -31,111 +32,105 @@ namespace gtsam { * 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) {} + /// Parameters for pre-integration: + /// Usage: Create just a single Params and pass a shared pointer to the constructor + struct Params { + Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements + boost::optional omegaCoriolis; ///< Coriolis constant + boost::optional body_P_sensor; ///< The pose of the sensor in the body frame - /// 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_;} + Params() : + gyroscopeCovariance(I_3x3) { + } - /// Needed for testable - void print(const std::string& s) const { - std::cout << s << std::endl; - std::cout << "deltaTij [" << deltaTij_ << "]" << std::endl; - deltaRij_.print(" deltaRij "); - std::cout << "delRdelBiasOmega [" << delRdelBiasOmega_ << "]" << std::endl; - std::cout << "gyroscopeCovariance [" << gyroscopeCovariance_ << "]" << std::endl; + virtual void print(const std::string& s) const; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size())); + ar & BOOST_SERIALIZATION_NVP(omegaCoriolis); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor); + } + }; + +protected: + double deltaTij_; ///< Time interval from i to j + Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) + Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + + /// Parameters + boost::shared_ptr p_; + + /// Default constructor for serialization + PreintegratedRotation() { } - /// 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); +public: + /// Default constructor, resets integration to zero + explicit PreintegratedRotation(const boost::shared_ptr& p) : + p_(p) { + resetIntegration(); } /// Re-initialize PreintegratedMeasurements - void resetIntegration(){ - deltaRij_ = Rot3(); - deltaTij_ = 0.0; - delRdelBiasOmega_ = Z_3x3; - } + void resetIntegration(); - /// 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; + /// @name Access instance variables + /// @{ + const double& deltaTij() const { + return deltaTij_; } - - /** - * 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; + const Rot3& deltaRij() const { + return deltaRij_; } - - /// Return a bias corrected version of the integrated rotation - expensive - Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr) const { - return deltaRij_*Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr); + const Matrix3& delRdelBiasOmega() const { + return delRdelBiasOmega_; } + /// @} - /// 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); + /// @name Testable + /// @{ - 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); - } + void print(const std::string& s) const; + bool equals(const PreintegratedRotation& other, double tol) const; + + /// @} + + /// Take the gyro measurement, correct it using the (constant) bias estimate + /// and possibly the sensor pose, and then integrate it forward in time to yield + /// an incremental rotation. + Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, + double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega) const; + + /// Calculate an incremental rotation given the gyro measurement and a time interval, + /// and update both deltaTij_ and deltaRij_. + void integrateMeasurement(const Vector3& measuredOmega, + const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega, + Matrix3* F); + + /// Return a bias corrected version of the integrated rotation, with optional Jacobian + Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr, + OptionalJacobian<3, 3> 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(); - } + Vector3 integrateCoriolis(const Rot3& rot_i) const; private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(deltaRij_); + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { // NOLINT + ar & BOOST_SERIALIZATION_NVP(p_); ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & BOOST_SERIALIZATION_NVP(deltaRij_); ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); } }; -} /// namespace gtsam +} // namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp new file mode 100644 index 000000000..bb01971e6 --- /dev/null +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -0,0 +1,312 @@ +/* ---------------------------------------------------------------------------- + + * 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" +#include + +using namespace std; + +namespace gtsam { + +//------------------------------------------------------------------------------ +void PreintegrationBase::Params::print(const string& s) const { + PreintegratedRotation::Params::print(s); + cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]" + << endl; + cout << "integrationCovariance:\n[\n" << accelerometerCovariance << "\n]" + << endl; + if (omegaCoriolis && use2ndOrderCoriolis) + cout << "Using 2nd-order Coriolis" << endl; + if (body_P_sensor) + body_P_sensor->print(" "); + cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl; +} + +//------------------------------------------------------------------------------ +void PreintegrationBase::resetIntegration() { + deltaTij_ = 0.0; + deltaXij_ = NavState(); + delRdelBiasOmega_ = Z_3x3; + delPdelBiasAcc_ = Z_3x3; + delPdelBiasOmega_ = Z_3x3; + delVdelBiasAcc_ = Z_3x3; + delVdelBiasOmega_ = Z_3x3; +} + +//------------------------------------------------------------------------------ +void PreintegrationBase::print(const string& s) const { + cout << s << endl; + cout << " deltaTij [" << deltaTij_ << "]" << endl; + cout << " deltaRij.ypr = (" << deltaRij().ypr().transpose() << ")" << endl; + cout << " deltaPij [ " << deltaPij().transpose() << " ]" << endl; + cout << " deltaVij [ " << deltaVij().transpose() << " ]" << endl; + biasHat_.print(" biasHat"); +} + +//------------------------------------------------------------------------------ +bool PreintegrationBase::equals(const PreintegrationBase& other, + double tol) const { + return fabs(deltaTij_ - other.deltaTij_) < tol + && deltaXij_.equals(other.deltaXij_, tol) + && biasHat_.equals(other.biasHat_, tol) + && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) + && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) + && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) + && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) + && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol); +} + +//------------------------------------------------------------------------------ +pair PreintegrationBase::correctMeasurementsByBiasAndSensorPose( + const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, + OptionalJacobian<3, 3> D_correctedAcc_measuredAcc, + OptionalJacobian<3, 3> D_correctedAcc_measuredOmega, + OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const { + + // Correct for bias in the sensor frame + Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc); + Vector3 j_correctedOmega = biasHat_.correctGyroscope(j_measuredOmega); + + // Compensate for sensor-body displacement if needed: we express the quantities + // (originally in the IMU frame) into the body frame + // Equations below assume the "body" frame is the CG + if (p().body_P_sensor) { + // Correct omega to rotation rate vector in the body frame + const Matrix3 bRs = p().body_P_sensor->rotation().matrix(); + j_correctedOmega = bRs * j_correctedOmega; + + // Correct acceleration + j_correctedAcc = bRs * j_correctedAcc; + + // Jacobians + if (D_correctedAcc_measuredAcc) *D_correctedAcc_measuredAcc = bRs; + if (D_correctedAcc_measuredOmega) *D_correctedAcc_measuredOmega = Matrix3::Zero(); + if (D_correctedOmega_measuredOmega) *D_correctedOmega_measuredOmega = bRs; + + // Centrifugal acceleration + const Vector3 b_arm = p().body_P_sensor->translation().vector(); + if (!b_arm.isZero()) { + // Subtract out the the centripetal acceleration from the measured one + // to get linear acceleration vector in the body frame: + const Matrix3 body_Omega_body = skewSymmetric(j_correctedOmega); + const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm + j_correctedAcc -= body_Omega_body * b_velocity_bs; + // Update derivative: centrifugal causes the correlation between acc and omega!!! + if (D_correctedAcc_measuredOmega) { + double wdp = j_correctedOmega.dot(b_arm); + *D_correctedAcc_measuredOmega = -(diag(Vector3::Constant(wdp)) + + j_correctedOmega * b_arm.transpose()) * bRs.matrix() + + 2 * b_arm * j_measuredOmega.transpose(); + } + } + } + + // Do update in one fell swoop + return make_pair(j_correctedAcc, j_correctedOmega); +} + +//------------------------------------------------------------------------------ +NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc, + const Vector3& j_measuredOmega, const double dt, + OptionalJacobian<9, 9> D_updated_current, + OptionalJacobian<9, 3> D_updated_measuredAcc, + OptionalJacobian<9, 3> D_updated_measuredOmega) const { + + Vector3 j_correctedAcc, j_correctedOmega; + Matrix3 D_correctedAcc_measuredAcc, // + D_correctedAcc_measuredOmega, // + D_correctedOmega_measuredOmega; + bool needDerivs = D_updated_measuredAcc && D_updated_measuredOmega && p().body_P_sensor; + boost::tie(j_correctedAcc, j_correctedOmega) = + correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega, + (needDerivs ? &D_correctedAcc_measuredAcc : 0), + (needDerivs ? &D_correctedAcc_measuredOmega : 0), + (needDerivs ? &D_correctedOmega_measuredOmega : 0)); + // Do update in one fell swoop + Matrix93 D_updated_correctedAcc, D_updated_correctedOmega; + NavState updated = deltaXij_.update(j_correctedAcc, j_correctedOmega, dt, D_updated_current, + (needDerivs ? D_updated_correctedAcc : D_updated_measuredAcc), + (needDerivs ? D_updated_correctedOmega : D_updated_measuredOmega)); + if (needDerivs) { + *D_updated_measuredAcc = D_updated_correctedAcc * D_correctedAcc_measuredAcc; + *D_updated_measuredOmega = D_updated_correctedOmega * D_correctedOmega_measuredOmega; + if (!p().body_P_sensor->translation().vector().isZero()) { + *D_updated_measuredOmega += D_updated_correctedAcc * D_correctedAcc_measuredOmega; + } + } + return updated; +} + +//------------------------------------------------------------------------------ +void PreintegrationBase::update(const Vector3& j_measuredAcc, + const Vector3& j_measuredOmega, const double dt, + Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current, + Matrix93* D_updated_measuredAcc, Matrix93* D_updated_measuredOmega) { + + // Save current rotation for updating Jacobians + const Rot3 oldRij = deltaXij_.attitude(); + + // Do update + deltaTij_ += dt; + deltaXij_ = updatedDeltaXij(j_measuredAcc, j_measuredOmega, dt, + D_updated_current, D_updated_measuredAcc, D_updated_measuredOmega); // functional + + // Update Jacobians + // TODO(frank): we are repeating some computation here: accessible in F ? + Vector3 j_correctedAcc, j_correctedOmega; + boost::tie(j_correctedAcc, j_correctedOmega) = + correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega); + + Matrix3 D_acc_R; + oldRij.rotate(j_correctedAcc, D_acc_R); + const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_; + + const Vector3 integratedOmega = j_correctedOmega * dt; + const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! + const Matrix3 incrRt = incrR.transpose(); + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + - *D_incrR_integratedOmega * dt; + + double dt22 = 0.5 * dt * dt; + const Matrix3 dRij = oldRij.matrix(); // expensive + delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij; + delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega; + delVdelBiasAcc_ += -dRij * dt; + delVdelBiasOmega_ += D_acc_biasOmega * dt; +} + +//------------------------------------------------------------------------------ +Vector9 PreintegrationBase::biasCorrectedDelta( + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { + // Correct deltaRij, derivative is delRdelBiasOmega_ + const imuBias::ConstantBias biasIncr = bias_i - biasHat_; + Matrix3 D_correctedRij_bias; + const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope(); + const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none, + H ? &D_correctedRij_bias : 0); + if (H) + D_correctedRij_bias *= delRdelBiasOmega_; + + Vector9 xi; + Matrix3 D_dR_correctedRij; + // TODO(frank): could line below be simplified? It is equivalent to + // LogMap(deltaRij_.compose(Expmap(biasInducedOmega))) + NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0); + NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer() + + delPdelBiasOmega_ * biasIncr.gyroscope(); + NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer() + + delVdelBiasOmega_ * biasIncr.gyroscope(); + + if (H) { + Matrix36 D_dR_bias, D_dP_bias, D_dV_bias; + D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias; + D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_; + D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_; + (*H) << D_dR_bias, D_dP_bias, D_dV_bias; + } + return xi; +} + +//------------------------------------------------------------------------------ +NavState PreintegrationBase::predict(const NavState& state_i, + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 6> H2) const { + // correct for bias + Matrix96 D_biasCorrected_bias; + Vector9 biasCorrected = biasCorrectedDelta(bias_i, + H2 ? &D_biasCorrected_bias : 0); + + // integrate on tangent space + Matrix9 D_delta_state, D_delta_biasCorrected; + Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity, + p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0, + H2 ? &D_delta_biasCorrected : 0); + + // Use retract to get back to NavState manifold + Matrix9 D_predict_state, D_predict_delta; + NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta); + if (H1) + *H1 = D_predict_state + D_predict_delta * D_delta_state; + if (H2) + *H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias; + return state_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, OptionalJacobian<9, 6> H1, + OptionalJacobian<9, 3> H2, OptionalJacobian<9, 6> H3, + OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5) const { + + // Note that derivative of constructors below is not identity for velocity, but + // a 9*3 matrix == Z_3x3, Z_3x3, state.R().transpose() + NavState state_i(pose_i, vel_i); + NavState state_j(pose_j, vel_j); + + /// Predict state at time j + Matrix99 D_predict_state_i; + Matrix96 D_predict_bias_i; + NavState predictedState_j = predict(state_i, bias_i, + H1 || H2 ? &D_predict_state_i : 0, H5 ? &D_predict_bias_i : 0); + + Matrix9 D_error_state_j, D_error_predict; + Vector9 error = state_j.localCoordinates(predictedState_j, + H3 || H4 ? &D_error_state_j : 0, H1 || H2 || H5 ? &D_error_predict : 0); + + // Separate out derivatives in terms of 5 arguments + // Note that doing so requires special treatment of velocities, as when treated as + // separate variables the retract applied will not be the semi-direct product in NavState + // Instead, the velocities in nav are updated using a straight addition + // This is difference is accounted for by the R().transpose calls below + if (H1) + *H1 << D_error_predict * D_predict_state_i.leftCols<6>(); + if (H2) + *H2 + << D_error_predict * D_predict_state_i.rightCols<3>() + * state_i.R().transpose(); + if (H3) + *H3 << D_error_state_j.leftCols<6>(); + if (H4) + *H4 << D_error_state_j.rightCols<3>() * state_j.R().transpose(); + if (H5) + *H5 << D_error_predict * D_predict_bias_i; + + return error; +} + +//------------------------------------------------------------------------------ +PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, + const Vector3& vel_i, const imuBias::ConstantBias& bias_i, + const Vector3& n_gravity, const Vector3& omegaCoriolis, + const bool use2ndOrderCoriolis) { + // NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility + boost::shared_ptr q = boost::make_shared(p()); + q->n_gravity = n_gravity; + q->omegaCoriolis = omegaCoriolis; + q->use2ndOrderCoriolis = use2ndOrderCoriolis; + p_ = q; + return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i); +} + +//------------------------------------------------------------------------------ + +}/// namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 29b9b6e66..c53a8c878 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -22,21 +22,26 @@ #pragma once #include +#include #include +#include namespace gtsam { -/** - * Struct to hold all state variables of returned by Predict function - */ +/// @deprecated 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) { + pose(_pose), velocity(_velocity), bias(_bias) { + } + PoseVelocityBias(const NavState& navState, const imuBias::ConstantBias _bias) : + pose(navState.pose()), velocity(navState.velocity()), bias(_bias) { + } + NavState navState() const { + return NavState(pose, velocity); } }; @@ -46,387 +51,214 @@ struct PoseVelocityBias { * It includes the definitions of the preintegrated variables and the methods * to access, print, and compare them. */ -class PreintegrationBase : public PreintegratedRotation { +class PreintegrationBase { - imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration - bool use2ndOrderIntegration_; ///< Controls the order of integration +public: - 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) + /// Parameters for pre-integration: + /// Usage: Create just a single Params and pass a shared pointer to the constructor + struct Params: PreintegratedRotation::Params { + Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer + Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty + bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration + Vector3 n_gravity; ///< Gravity vector in nav frame + /// The Params constructor insists on getting the navigation frame gravity vector + /// For convenience, two commonly used conventions are provided by named constructors below + Params(const Vector3& n_gravity) : + accelerometerCovariance(I_3x3), integrationCovariance(I_3x3), use2ndOrderCoriolis( + false), n_gravity(n_gravity) { + } + + // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis + static boost::shared_ptr MakeSharedD(double g = 9.81) { + return boost::make_shared(Vector3(0, 0, g)); + } + + // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis + static boost::shared_ptr MakeSharedU(double g = 9.81) { + return boost::make_shared(Vector3(0, 0, -g)); + } + + void print(const std::string& s) const; + + protected: + /// Default constructor for serialization only: uninitialized! + Params() {} + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & boost::serialization::make_nvp("PreintegratedRotation_Params", + boost::serialization::base_object(*this)); + ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size())); + ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size())); + ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis); + ar & BOOST_SERIALIZATION_NVP(n_gravity); + } + }; + +protected: + + double deltaTij_; ///< Time interval from i to j + + /** + * Preintegrated navigation state, from frame i to frame j + * Note: relative position does not take into account velocity at time i, see deltap+, in [2] + * Note: velocity is now also in frame i, as opposed to deltaVij in [2] + */ + NavState deltaXij_; + + /// Parameters + boost::shared_ptr p_; + + /// Acceleration and gyro bias used for preintegration + imuBias::ConstantBias biasHat_; + + Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias 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 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements - Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty - /// (to compensate errors in Euler integration) + /// Default constructor for serialization + PreintegrationBase() { + } public: /** - * Default constructor, initializes the variables in the base class + * 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) + * @param p Parameters, typically fixed in a single application */ - 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) {} - - /// 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_;} - - /// Needed for testable - void print(const std::string& s) const { - PreintegratedRotation::print(s); - std::cout << " accelerometerCovariance [ " << accelerometerCovariance_ << " ]" << std::endl; - std::cout << " integrationCovariance [ " << integrationCovariance_ << " ]" << std::endl; - std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; - std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; - biasHat_.print(" biasHat"); - } - - /// Needed for testable - bool equals(const PreintegrationBase& expected, double tol) const { - return PreintegratedRotation::equals(expected, tol) - && biasHat_.equals(expected.biasHat_, tol) - && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) - && equal_with_abs_tol(deltaVij_, expected.deltaVij_, 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(accelerometerCovariance_, expected.accelerometerCovariance_, tol) - && equal_with_abs_tol(integrationCovariance_, expected.integrationCovariance_, tol); + PreintegrationBase(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat) : + p_(p), biasHat_(biasHat) { + resetIntegration(); } /// Re-initialize PreintegratedMeasurements - void resetIntegration(){ - PreintegratedRotation::resetIntegration(); - deltaPij_ = Vector3::Zero(); - deltaVij_ = Vector3::Zero(); - delPdelBiasAcc_ = Z_3x3; - delPdelBiasOmega_ = Z_3x3; - delVdelBiasAcc_ = Z_3x3; - delVdelBiasOmega_ = Z_3x3; + void resetIntegration(); + + const Params& p() const { + return *boost::static_pointer_cast(p_); } - /// Update preintegrated measurements - void updatePreintegratedMeasurements(const Vector3& correctedAcc, - const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F) { - - Matrix3 dRij = deltaRij(); // expensive - 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){ - 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 - } + void set_body_P_sensor(const Pose3& body_P_sensor) { + p_->body_P_sensor = body_P_sensor; } - /// Update Jacobians to be used during preintegration - void updatePreintegratedJacobians(const Vector3& correctedAcc, - const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT){ - Matrix3 dRij = deltaRij(); // expensive - 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); + /// getters + const NavState& deltaXij() const { + return deltaXij_; + } + const double& deltaTij() const { + return deltaTij_; + } + const Rot3& deltaRij() const { + return deltaXij_.attitude(); + } + Vector3 deltaPij() const { + return deltaXij_.position().vector(); + } + Vector3 deltaVij() const { + return deltaXij_.velocity(); + } + const imuBias::ConstantBias& biasHat() const { + return biasHat_; + } + const Matrix3& delRdelBiasOmega() const { + return delRdelBiasOmega_; + } + const Matrix3& delPdelBiasAcc() const { + return delPdelBiasAcc_; + } + const Matrix3& delPdelBiasOmega() const { + return delPdelBiasOmega_; + } + const Matrix3& delVdelBiasAcc() const { + return delVdelBiasAcc_; + } + const Matrix3& delVdelBiasOmega() const { + return delVdelBiasOmega_; } - void 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 - } + // Exposed for MATLAB + Vector6 biasHatVector() const { + return biasHat_.vector(); } + /// print + void print(const std::string& s) const; + + /// check equality + bool equals(const PreintegrationBase& other, double tol) const; + + /// Subtract estimate and correct for sensor pose + /// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc) + /// Ignore D_correctedOmega_measuredAcc as it is trivially zero + std::pair correctMeasurementsByBiasAndSensorPose( + const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, + OptionalJacobian<3, 3> D_correctedAcc_measuredAcc = boost::none, + OptionalJacobian<3, 3> D_correctedAcc_measuredOmega = boost::none, + OptionalJacobian<3, 3> D_correctedOmega_measuredOmega = boost::none) const; + + /// Calculate the updated preintegrated measurement, does not modify + /// It takes measured quantities in the j frame + NavState updatedDeltaXij(const Vector3& j_measuredAcc, + const Vector3& j_measuredOmega, const double dt, + OptionalJacobian<9, 9> D_updated_current = boost::none, + OptionalJacobian<9, 3> D_updated_measuredAcc = boost::none, + OptionalJacobian<9, 3> D_updated_measuredOmega = boost::none) const; + + /// Update preintegrated measurements and get derivatives + /// It takes measured quantities in the j frame + void update(const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, + const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current, + Matrix93* D_udpated_measuredAcc, Matrix93* D_updated_measuredOmega); + + /// Given the estimate of the bias, return a NavState tangent vector + /// summarizing the preintegrated IMU measurements so far + Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 6> H = boost::none) const; + /// 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 { - - const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); - - const Rot3& Rot_i = pose_i.rotation(); - const Vector3& pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr; - if(deltaPij_biascorrected_out)// if desired, store this - *deltaPij_biascorrected_out = deltaPij_biascorrected; - - Vector3 pos_j = pos_i + Rot_i.matrix() * deltaPij_biascorrected - + vel_i * deltaTij() - - omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij()*deltaTij(); - - 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) * deltaTij() // Coriolis term - + gravity * deltaTij()); - - if(use2ndOrderCoriolis){ - pos_j += - 0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij()*deltaTij(); // 2nd order coriolis term for position - vel_j += - omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij(); // 2nd order term for velocity - } - - const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); - // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr) - - Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected); - Vector3 correctedOmega = biascorrectedOmega - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term - const Rot3 correctedDeltaRij = - Rot3::Expmap( correctedOmega ); - const Rot3 Rot_j = Rot_i.compose( correctedDeltaRij ); - - Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); - return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant - } + NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 = + 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 { + const imuBias::ConstantBias& bias_i, 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; - // We need the mismatch w.r.t. the biases used for preintegration - // const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); // this is not necessary - const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); - - // we give some shorter name to rotations and translations - const Rot3& Ri = pose_i.rotation(); - 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() * ( 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() * ( vel_j - predictedState_j.velocity ); - - // fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j) - - // Jacobian computation - /* ---------------------------------------------------------------------------------------------------- */ - // 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; - Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5 ? &D_cThetaRij_biasOmegaIncr : 0); - - // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration - const Matrix3 Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis); - const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); - Vector3 correctedOmega = biascorrectedOmega - coriolis; - - Rot3 correctedDeltaRij, fRrot; - Vector3 fR; - - // Accessory matrix, used to build the jacobians - Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot; - - // This is done to save computation: we ask for the jacobians only when they are needed - if(H1 || H2 || H3 || H4 || H5){ - correctedDeltaRij = Rot3::Expmap( correctedOmega, D_cDeltaRij_cOmega); - // Residual rotation error - fRrot = correctedDeltaRij.between(Ri.between(Rj)); - fR = Rot3::Logmap(fRrot, D_fR_fRrot); - D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis); - }else{ - correctedDeltaRij = Rot3::Expmap( correctedOmega); - // Residual rotation error - fRrot = correctedDeltaRij.between(Ri.between(Rj)); - fR = Rot3::Logmap(fRrot); - } - if(H1) { - H1->resize(9,6); - Matrix3 dfPdPi = -I_3x3; - Matrix3 dfVdPi = Z_3x3; - if(use2ndOrderCoriolis){ - // this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() - Matrix3 temp = Ritranspose_omegaCoriolisHat * (-Ritranspose_omegaCoriolisHat.transpose()); - dfPdPi += 0.5 * temp * deltaTij()*deltaTij(); - dfVdPi += temp * deltaTij(); - } - (*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->resize(9,3); - (*H2) << - // dfP/dVi - - Ri.transpose() * deltaTij() - + Ritranspose_omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - - Ri.transpose() - + 2 * Ritranspose_omegaCoriolisHat * deltaTij(), // Coriolis term - // dfR/dVi - Z_3x3; - } - if(H3) { - H3->resize(9,6); - (*H3) << - // dfP/dPosej - Z_3x3, Ri.transpose() * Rj.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - D_fR_fRrot, Z_3x3; - } - if(H4) { - H4->resize(9,3); - (*H4) << - // dfP/dVj - Z_3x3, - // dfV/dVj - Ri.transpose(), - // 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->resize(9,6); - (*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; - } + /// @deprecated predict + PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation); + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & BOOST_SERIALIZATION_NVP(p_); + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & BOOST_SERIALIZATION_NVP(deltaXij_); 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_); + ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size())); + ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size())); + ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size())); + ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size())); + ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size())); } }; -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: - - 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(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) {} - - 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 25b3518bc..2121eda35 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -35,6 +35,12 @@ using symbol_shorthand::X; using symbol_shorthand::V; using symbol_shorthand::B; +Vector3 kZeroOmegaCoriolis(0,0,0); + +// Define covariance matrices +double accNoiseVar = 0.01; +const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3; + //****************************************************************************** namespace { Vector callEvaluateError(const AHRSFactor& factor, const Rot3 rot_i, @@ -50,8 +56,8 @@ Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i, AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( const Vector3& bias, const list& measuredOmegas, const list& deltaTs, - const Vector3& initialRotationRate = Vector3()) { - AHRSFactor::PreintegratedMeasurements result(bias, Matrix3::Identity()); + const Vector3& initialRotationRate = Vector3::Zero()) { + AHRSFactor::PreintegratedMeasurements result(bias, I_3x3); list::const_iterator itOmega = measuredOmegas.begin(); list::const_iterator itDeltaT = deltaTs.begin(); @@ -95,7 +101,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) { double expectedDeltaT1(0.5); // Actual preintegrated values - AHRSFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero()); + AHRSFactor::PreintegratedMeasurements actual1(bias, Z_3x3); actual1.integrateMeasurement(measuredOmega, deltaT); EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6)); @@ -121,18 +127,14 @@ TEST(AHRSFactor, Error) { Rot3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0)); // Measurements - Vector3 gravity; - gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; - omegaCoriolis << 0, 0, 0; Vector3 measuredOmega; measuredOmega << M_PI / 100, 0, 0; double deltaT = 1.0; - AHRSFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero()); - pre_int_data.integrateMeasurement(measuredOmega, deltaT); + AHRSFactor::PreintegratedMeasurements pim(bias, Z_3x3); + pim.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), pim, kZeroOmegaCoriolis, boost::none); Vector3 errorActual = factor.evaluateError(x1, x2, bias); @@ -182,18 +184,16 @@ TEST(AHRSFactor, ErrorWithBiases) { Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0))); // Measurements - Vector3 omegaCoriolis; - omegaCoriolis << 0, 0.0, 0.0; Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0 + 0.3; double deltaT = 1.0; - AHRSFactor::PreintegratedMeasurements pre_int_data(Vector3(0,0,0), - Matrix3::Zero()); - pre_int_data.integrateMeasurement(measuredOmega, deltaT); + AHRSFactor::PreintegratedMeasurements pim(Vector3(0,0,0), + Z_3x3); + pim.integrateMeasurement(measuredOmega, deltaT); // Create factor - AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis); + AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis); Vector errorActual = factor.evaluateError(x1, x2, bias); @@ -269,7 +269,7 @@ TEST( AHRSFactor, PartialDerivativeLogmap ) { 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 + const Matrix3 actualDelFdeltheta = I_3x3 + 0.5 * X + (1 / (normx * normx) - (1 + cos(normx)) / (2 * normx * sin(normx))) * X * X; @@ -359,8 +359,6 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0))); // Measurements - Vector3 gravity; - gravity << 0, 0, 9.81; Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; Vector3 measuredOmega; @@ -370,13 +368,15 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), Point3(1, 0, 0)); - AHRSFactor::PreintegratedMeasurements pre_int_data(Vector3::Zero(), - Matrix3::Zero()); + AHRSFactor::PreintegratedMeasurements pim(Vector3::Zero(), kMeasuredAccCovariance); - pre_int_data.integrateMeasurement(measuredOmega, deltaT); + pim.integrateMeasurement(measuredOmega, deltaT); + + // Check preintegrated covariance + EXPECT(assert_equal(kMeasuredAccCovariance, pim.preintMeasCov())); // Create factor - AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis); + AHRSFactor factor(X(1), X(2), B(1), pim, omegaCoriolis); // Expected Jacobians Matrix H1e = numericalDerivative11( @@ -407,34 +407,35 @@ TEST (AHRSFactor, predictTest) { Vector3 bias(0,0,0); // Measurements - Vector3 gravity; - gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; - omegaCoriolis << 0, 0.0, 0.0; Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0; - double deltaT = 0.001; - AHRSFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero()); + double deltaT = 0.2; + AHRSFactor::PreintegratedMeasurements pim(bias, kMeasuredAccCovariance); for (int i = 0; i < 1000; ++i) { - pre_int_data.integrateMeasurement(measuredOmega, deltaT); + pim.integrateMeasurement(measuredOmega, deltaT); } - AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis); + // Check preintegrated covariance + Matrix expectedMeasCov(3,3); + expectedMeasCov = 200*kMeasuredAccCovariance; + EXPECT(assert_equal(expectedMeasCov, pim.preintMeasCov())); + + AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis); // Predict Rot3 x; - Rot3 expectedRot = Rot3().ypr(M_PI / 10, 0, 0); - Rot3 actualRot = factor.predict(x, bias, pre_int_data, omegaCoriolis); + Rot3 expectedRot = Rot3().ypr(20*M_PI, 0, 0); + Rot3 actualRot = factor.predict(x, bias, pim, kZeroOmegaCoriolis); EXPECT(assert_equal(expectedRot, actualRot, 1e-6)); // AHRSFactor::PreintegratedMeasurements::predict Matrix expectedH = numericalDerivative11( boost::bind(&AHRSFactor::PreintegratedMeasurements::predict, - &pre_int_data, _1, boost::none), bias); + &pim, _1, boost::none), bias); // Actual Jacobians Matrix H; - (void) pre_int_data.predict(bias,H); - EXPECT(assert_equal(expectedH, H)); + (void) pim.predict(bias,H); + EXPECT(assert_equal(expectedH, H, 1e-8)); } //****************************************************************************** #include @@ -448,12 +449,7 @@ TEST (AHRSFactor, graphTest) { // PreIntegrator Vector3 biasHat(0, 0, 0); - Vector3 gravity; - gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; - omegaCoriolis << 0, 0, 0; - AHRSFactor::PreintegratedMeasurements pre_int_data(biasHat, - Matrix3::Identity()); + AHRSFactor::PreintegratedMeasurements pim(biasHat, kMeasuredAccCovariance); // Pre-integrate measurements Vector3 measuredOmega(0, M_PI / 20, 0); @@ -461,15 +457,15 @@ TEST (AHRSFactor, graphTest) { // Create Factor noiseModel::Base::shared_ptr model = // - noiseModel::Gaussian::Covariance(pre_int_data.preintMeasCov()); + noiseModel::Gaussian::Covariance(pim.preintMeasCov()); NonlinearFactorGraph graph; Values values; for (size_t i = 0; i < 5; ++i) { - pre_int_data.integrateMeasurement(measuredOmega, deltaT); + pim.integrateMeasurement(measuredOmega, deltaT); } - // pre_int_data.print("Pre integrated measurementes"); - AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis); + // pim.print("Pre integrated measurementes"); + AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis); values.insert(X(1), x1); values.insert(X(2), x2); values.insert(B(1), bias); diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 3f7109543..d473207da 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -40,173 +40,131 @@ 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) { - - 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); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, I_3x3, + I_3x3, I_3x3, I_3x3, I_3x3, I_6x6); 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){ - 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){ - 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){ - 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 ) -{ +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; // Actual preintegrated values - ImuFactor::PreintegratedMeasurements expected1(bias, Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero()); + ImuFactor::PreintegratedMeasurements expected1(bias, Z_3x3, Z_3x3, Z_3x3); expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias, - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)); + CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias, Z_3x3, + Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_6x6); 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(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()), tol)); + EXPECT(assert_equal(Vector(expected1.deltaPij()), actual1.deltaPij(), tol)); + EXPECT(assert_equal(Vector(expected1.deltaVij()), actual1.deltaVij(), tol)); + EXPECT(assert_equal(expected1.deltaRij(), actual1.deltaRij(), tol)); DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol); } /* ************************************************************************* */ -TEST( CombinedImuFactor, ErrorWithBiases ) -{ +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; - Matrix I6x6(6,6); - I6x6 = Matrix::Identity(6,6); + ImuFactor::PreintegratedMeasurements pim( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + I_3x3, I_3x3, I_3x3); - 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()); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + CombinedImuFactor::CombinedPreintegratedMeasurements combined_pim( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + I_3x3, I_3x3, I_3x3, I_3x3, 2 * I_3x3, I_6x6); - 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_pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pim, 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); + noiseModel::Gaussian::shared_ptr Combinedmodel = + noiseModel::Gaussian::Covariance(combined_pim.preintMeasCov()); + CombinedImuFactor combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), + combined_pim, gravity, omegaCoriolis); + Vector errorExpected = imuFactor.evaluateError(x1, v1, x2, v2, bias); + Vector errorActual = combinedfactor.evaluateError(x1, v1, x2, v2, bias, + bias2); 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); + (void) imuFactor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e); // 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))); @@ -216,258 +174,135 @@ TEST( CombinedImuFactor, ErrorWithBiases ) } /* ************************************************************************* */ -TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) -{ +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 - CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs); + CombinedImuFactor::CombinedPreintegratedMeasurements pim = + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs); // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs), 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), 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), 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(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(expectedDelPdelBiasAcc, pim.delPdelBiasAcc())); + EXPECT(assert_equal(expectedDelPdelBiasOmega, pim.delPdelBiasOmega())); + EXPECT(assert_equal(expectedDelVdelBiasAcc, pim.delVdelBiasAcc())); + EXPECT(assert_equal(expectedDelVdelBiasOmega, pim.delVdelBiasOmega())); + EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); + EXPECT(assert_equal(expectedDelRdelBiasOmega, pim.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } /* ************************************************************************* */ -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); + CombinedImuFactor::CombinedPreintegratedMeasurements pim(bias, I_3x3, I_3x3, + I_3x3, I_3x3, 2 * I_3x3, I_6x6); - CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( - 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); + for (int i = 0; i < 1000; ++i) + pim.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); + noiseModel::Gaussian::shared_ptr combinedmodel = + noiseModel::Gaussian::Covariance(pim.preintMeasCov()); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim, + gravity, omegaCoriolis); // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); + PoseVelocityBias poseVelocityBias = pim.predict(x1, v1, bias, gravity, + omegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); - Vector3 expectedVelocity; expectedVelocity<<0,1,0; + Vector3 expectedVelocity; + expectedVelocity << 0, 1, 0; EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity))); + 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); - 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; + CombinedImuFactor::CombinedPreintegratedMeasurements pim(bias, I_3x3, I_3x3, + I_3x3, I_3x3, 2 * I_3x3, I_6x6); + 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) - Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, - deltaT); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), - Combined_pre_int_data, gravity, omegaCoriolis); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim, + gravity, omegaCoriolis); // Predict - Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0)); - Vector3 v(0,0,0); - PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x,v,bias, 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, pim, gravity, omegaCoriolis); + Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); + EXPECT(assert_equal(expectedPose, x2, tol)); } /* ************************************************************************* */ -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< Rot3, Vector3>(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..596b76f6a 100644 --- a/gtsam/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -16,28 +16,138 @@ */ #include +#include + #include +#include using namespace std; using namespace gtsam; +typedef imuBias::ConstantBias Bias; + +Vector biasAcc1(Vector3(0.2, -0.1, 0)); +Vector biasGyro1(Vector3(0.1, -0.3, -0.2)); +Bias bias1(biasAcc1, biasGyro1); + +Vector biasAcc2(Vector3(0.1, 0.2, 0.04)); +Vector biasGyro2(Vector3(-0.002, 0.005, 0.03)); +Bias 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; + Bias bias1; // Acc + Gyro Constructor - gtsam::imuBias::ConstantBias bias2(bias_acc, bias_gyro); + Bias bias2(biasAcc2, biasGyro2); // Copy Constructor - gtsam::imuBias::ConstantBias bias3(bias2); + Bias bias3(bias2); } /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +TEST(ImuBias, inverse) { + Bias biasActual = bias1.inverse(); + Bias biasExpected = Bias(-biasAcc1, -biasGyro1); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST(ImuBias, compose) { + Bias biasActual = bias2.compose(bias1); + Bias biasExpected = Bias(biasAcc1 + biasAcc2, biasGyro1 + biasGyro2); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST(ImuBias, between) { + // p.between(q) == q - p + Bias biasActual = bias2.between(bias1); + Bias biasExpected = Bias(biasAcc1 - biasAcc2, biasGyro1 - biasGyro2); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST(ImuBias, localCoordinates) { + Vector deltaActual = Vector(bias2.localCoordinates(bias1)); + Vector deltaExpected = + (Bias(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; + Bias biasActual = bias2.retract(delta); + Bias biasExpected = Bias(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; + Bias biasActual = bias2.Expmap(delta); + Bias biasExpected = Bias(delta); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST(ImuBias, operatorSub) { + Bias biasActual = -bias1; + Bias biasExpected(-biasAcc1, -biasGyro1); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST(ImuBias, operatorAdd) { + Bias biasActual = bias2 + bias1; + Bias biasExpected(biasAcc2 + biasAcc1, biasGyro2 + biasGyro1); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST(ImuBias, operatorSubB) { + Bias biasActual = bias2 - bias1; + Bias biasExpected(biasAcc2 - biasAcc1, biasGyro2 - biasGyro1); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST(ImuBias, Correct1) { + Matrix aH1, aH2; + const Vector3 measurement(1, 2, 3); + boost::function f = boost::bind( + &Bias::correctAccelerometer, _1, _2, boost::none, boost::none); + bias1.correctAccelerometer(measurement, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2)); +} + +/* ************************************************************************* */ +TEST(ImuBias, Correct2) { + Matrix aH1, aH2; + const Vector3 measurement(1, 2, 3); + boost::function f = + boost::bind(&Bias::correctGyroscope, _1, _2, boost::none, boost::none); + bias1.correctGyroscope(measurement, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 9c82a7dfa..25a6e732c 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -12,20 +12,23 @@ /** * @file testImuFactor.cpp * @brief Unit test for ImuFactor - * @author Luca Carlone, Stephen Williams, Richard Roberts + * @author Luca Carlone, Stephen Williams, Richard Roberts, Frank Dellaert */ #include -#include -#include #include #include +#include +#include +#include +#include #include #include -#include +#include #include #include +#include using namespace std; using namespace gtsam; @@ -35,385 +38,479 @@ using symbol_shorthand::X; using symbol_shorthand::V; using symbol_shorthand::B; +static const Vector3 kGravityAlongNavZDown(0, 0, 9.81); +static const Vector3 kZeroOmegaCoriolis(0, 0, 0); +static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1); +static const imuBias::ConstantBias kZeroBiasHat, kZeroBias; + /* ************************************************************************* */ namespace { // 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).head(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; -} - -// Auxiliary functions to test preintegrated Jacobians -// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ +// Define covariance matrices /* ************************************************************************* */ double accNoiseVar = 0.01; double omegaNoiseVar = 0.03; double intNoiseVar = 0.0001; -ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs, - const bool use2ndOrderIntegration = false){ - ImuFactor::PreintegratedMeasurements result(bias, accNoiseVar * Matrix3::Identity(), - omegaNoiseVar *Matrix3::Identity(), intNoiseVar * Matrix3::Identity(),use2ndOrderIntegration); +const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3; +const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3; +const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; +const Vector3 accNoiseVar2(0.01, 0.02, 0.03); +const Vector3 omegaNoiseVar2(0.03, 0.01, 0.02); +int32_t accSamplerSeed = 29284, omegaSamplerSeed = 10; + +// Auxiliary functions to test preintegrated Jacobians +// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ +/* ************************************************************************* */ +PreintegratedImuMeasurements evaluatePreintegratedMeasurements( + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + PreintegratedImuMeasurements result(bias, kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); 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){ - 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) -{ - 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){ - 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()); } -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))); } +} // namespace + +/* ************************************************************************* */ +bool MonteCarlo(const PreintegratedImuMeasurements& pim, + const NavState& state, const imuBias::ConstantBias& bias, double dt, + boost::optional body_P_sensor, const Vector3& measuredAcc, + const Vector3& measuredOmega, const Vector3& accNoiseVar, + const Vector3& omegaNoiseVar, size_t N = 10000, + size_t M = 1) { + // Get mean prediction from "ground truth" measurements + PreintegratedImuMeasurements pim1 = pim; + for (size_t k = 0; k < M; k++) + pim1.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor); + NavState prediction = pim1.predict(state, bias); + Matrix9 actual = pim1.preintMeasCov(); + + // Do a Monte Carlo analysis to determine empirical density on the predicted state + Sampler sampleAccelerationNoise((accNoiseVar/dt).cwiseSqrt(), accSamplerSeed); + Sampler sampleOmegaNoise((omegaNoiseVar/dt).cwiseSqrt(), omegaSamplerSeed); + Matrix samples(9, N); + Vector9 sum = Vector9::Zero(); + for (size_t i = 0; i < N; i++) { + PreintegratedImuMeasurements pim2 = pim; + for (size_t k = 0; k < M; k++) { + Vector3 perturbedAcc = measuredAcc + sampleAccelerationNoise.sample(); + Vector3 perturbedOmega = measuredOmega + sampleOmegaNoise.sample(); + pim2.integrateMeasurement(perturbedAcc, perturbedOmega, dt, + body_P_sensor); + } + NavState sampled = pim2.predict(state, bias); + Vector9 xi = sampled.localCoordinates(prediction); + samples.col(i) = xi; + sum += xi; + } + + Vector9 sampleMean = sum / N; + Matrix9 Q; + Q.setZero(); + for (size_t i = 0; i < N; i++) { + Vector9 xi = samples.col(i); + xi -= sampleMean; + Q += xi * (xi.transpose() / (N - 1)); + } + + // Compare MonteCarlo value with actual (computed) value + return assert_equal(Matrix(Q), actual, 1e-4); } /* ************************************************************************* */ -TEST( ImuFactor, PreintegratedMeasurements ) -{ - // Linearization point - imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases +TEST(ImuFactor, StraightLine) { + // Set up IMU measurements + static const double g = 10; // make gravity 10 to make this easy to check + static const double v = 50.0, a = 0.2, dt = 3.0; + const double dt22 = dt * dt / 2; + // Set up body pointing towards y axis, and start at 10,20,0 with velocity going in X + // The body itself has Z axis pointing down + static const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); + static const Point3 initial_position(10, 20, 0); + static const Vector3 initial_velocity(v, 0, 0); + static const NavState state1(nRb, initial_position, initial_velocity); + + // set up acceleration in X direction, no angular velocity. + // Since body Z-axis is pointing down, accelerometer measures table exerting force in negative Z + Vector3 measuredAcc(a, 0, -g), measuredOmega(0, 0, 0); + + // Create parameters assuming nav frame has Z up + boost::shared_ptr p = + PreintegratedImuMeasurements::Params::MakeSharedU(g); + p->accelerometerCovariance = kMeasuredAccCovariance; + p->gyroscopeCovariance = kMeasuredOmegaCovariance; + + // Check G1 and G2 derivatives of pim.update + + // Now, preintegrate for 3 seconds, in 10 steps + PreintegratedImuMeasurements pim(p, kZeroBiasHat); + for (size_t i = 0; i < 10; i++) + pim.integrateMeasurement(measuredAcc, measuredOmega, dt / 10); + + Matrix93 aG1,aG2; + boost::function f = + boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, dt/10, + boost::none, boost::none, boost::none); + pim.updatedDeltaXij(measuredAcc, measuredOmega, dt / 10, boost::none, aG1, aG2); + EXPECT(assert_equal(numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7)); + EXPECT(assert_equal(numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), aG2, 1e-7)); + + // Do Monte-Carlo analysis + PreintegratedImuMeasurements pimMC(kZeroBiasHat, p->accelerometerCovariance, + p->gyroscopeCovariance, Z_3x3, true); // MonteCarlo does not sample integration noise + EXPECT( + MonteCarlo(pimMC, state1, kZeroBias, dt / 10, boost::none, measuredAcc, + measuredOmega, Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000)); + + // Check integrated quantities in body frame: gravity measured by IMU is integrated! + Vector3 b_deltaP(a * dt22, 0, -g * dt22); + Vector3 b_deltaV(a * dt, 0, -g * dt); + EXPECT(assert_equal(Rot3(), pim.deltaRij())); + EXPECT(assert_equal(b_deltaP, pim.deltaPij())); + EXPECT(assert_equal(b_deltaV, pim.deltaVij())); + + // Check bias-corrected delta: also in body frame + Vector9 expectedBC; + expectedBC << Vector3(0, 0, 0), b_deltaP, b_deltaV; + EXPECT(assert_equal(expectedBC, pim.biasCorrectedDelta(kZeroBias))); + + // Check prediction in nav, note we move along x in body, along y in nav + Point3 expected_position(10 + v * dt, 20 + a * dt22, 0); + Velocity3 expected_velocity(v, a * dt, 0); + NavState expected(nRb, expected_position, expected_velocity); + EXPECT(assert_equal(expected, pim.predict(state1, kZeroBias))); +} + +/* ************************************************************************* */ +TEST(ImuFactor, PreintegratedMeasurements) { // 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); + PreintegratedImuMeasurements actual1(kZeroBiasHat, kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); 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(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6)); - DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6); + EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()))); + EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()))); + EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()))); + DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-9); // 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; + PreintegratedImuMeasurements 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(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6)); - DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6); + EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()))); + EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()))); + EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()))); + DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-9); } /* ************************************************************************* */ -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)); - 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)); - Vector3 v2(Vector3(0.5, 0.0, 0.0)); +// Common linearization point and measurements for tests +namespace common { +static const Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0), + Point3(5.0, 1.0, 0)); +static const Vector3 v1(Vector3(0.5, 0.0, 0.0)); +static const NavState state1(x1, v1); - // Measurements - 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); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); +// Measurements +static const double w = M_PI / 100; +static const Vector3 measuredOmega(w, 0, 0); +static const Vector3 measuredAcc = x1.rotation().unrotate( + -kGravityAlongNavZDown); +static const double deltaT = 1.0; + +static const Pose3 x2(Rot3::RzRyRx(M_PI / 12.0 + w, M_PI / 6.0, M_PI / 4.0), + Point3(5.5, 1.0, 0)); +static const Vector3 v2(Vector3(0.5, 0.0, 0.0)); +static const NavState state2(x2, v2); +} // namespace common + +/* ************************************************************************* */ +TEST(ImuFactor, PreintegrationBaseMethods) { + using namespace common; + boost::shared_ptr p = + PreintegratedImuMeasurements::Params::MakeSharedD(); + p->gyroscopeCovariance = kMeasuredOmegaCovariance; + p->omegaCoriolis = Vector3(0.02, 0.03, 0.04); + p->accelerometerCovariance = kMeasuredAccCovariance; + p->integrationCovariance = kIntegrationErrorCovariance; + p->use2ndOrderCoriolis = true; + + PreintegratedImuMeasurements pim(p, kZeroBiasHat); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // biasCorrectedDelta + Matrix96 actualH; + pim.biasCorrectedDelta(kZeroBias, actualH); + Matrix expectedH = numericalDerivative11( + boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, + boost::none), kZeroBias); + EXPECT(assert_equal(expectedH, actualH)); + + Matrix9 aH1; + Matrix96 aH2; + NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2); + Matrix eH1 = numericalDerivative11( + boost::bind(&PreintegrationBase::predict, pim, _1, kZeroBias, boost::none, + boost::none), state1); + EXPECT(assert_equal(eH1, aH1)); + Matrix eH2 = numericalDerivative11( + boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none, + boost::none), kZeroBias); + EXPECT(assert_equal(eH2, aH2)); + return; + +} + +/* ************************************************************************* */ +TEST(ImuFactor, ErrorAndJacobians) { + using namespace common; + PreintegratedImuMeasurements pim(kZeroBiasHat, kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); + + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + EXPECT(assert_equal(state2, pim.predict(state1, kZeroBias))); // Create factor - 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); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, + kZeroOmegaCoriolis); // Expected error - Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; - EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + Vector expectedError(9); + expectedError << 0, 0, 0, 0, 0, 0, 0, 0, 0; + EXPECT( + assert_equal(expectedError, factor.evaluateError(x1, v1, x2, v2, kZeroBias))); + + Values values; + values.insert(X(1), x1); + values.insert(V(1), v1); + values.insert(X(2), x2); + values.insert(V(2), v2); + values.insert(B(1), kZeroBias); + EXPECT(assert_equal(expectedError, factor.unwhitenedError(values))); + + // Make sure linearization is correct + double diffDelta = 1e-7; + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); // Actual Jacobians Matrix H1a, H2a, H3a, H4a, H5a; - (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); + (void) factor.evaluateError(x1, v1, x2, v2, kZeroBias, H1a, H2a, H3a, H4a, H5a); - // Expected Jacobians - /////////////////// H1 /////////////////////////// - Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); + // Make sure rotation part is correct when error is interpreted as axis-angle // 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)); + Matrix H1Rot3 = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, kZeroBias), x1); + EXPECT(assert_equal(H1Rot3, H1a.topRows(3))); - /////////////////// H2 /////////////////////////// - Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - EXPECT(assert_equal(H2e, H2a)); + Matrix H3Rot3 = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, kZeroBias), x2); + EXPECT(assert_equal(H3Rot3, H3a.topRows(3))); - /////////////////// 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)); + // Evaluate error with wrong values + Vector3 v2_wrong = v2 + Vector3(0.1, 0.1, 0.1); + values.update(V(2), v2_wrong); + expectedError << 0, 0, 0, 0, 0, 0, -0.0724744871, -0.040715657, -0.151952901; + EXPECT( + assert_equal(expectedError, + factor.evaluateError(x1, v1, x2, v2_wrong, kZeroBias), 1e-2)); + EXPECT(assert_equal(expectedError, factor.unwhitenedError(values), 1e-2)); - /////////////////// H4 /////////////////////////// - Matrix H4e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - EXPECT(assert_equal(H4e, H4a)); + // Make sure the whitening is done correctly + Matrix cov = pim.preintMeasCov(); + Matrix R = RtR(cov.inverse()); + Vector whitened = R * expectedError; + EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-5)); - /////////////////// H5 /////////////////////////// - Matrix H5e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); - EXPECT(assert_equal(H5e, H5a)); + // Make sure linearization is correct + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); } /* ************************************************************************* */ -TEST( ImuFactor, ErrorAndJacobianWithBiases ) -{ +TEST(ImuFactor, ErrorAndJacobianWithBiases) { + using common::x1; + using common::v1; + using common::v2; 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)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), + Point3(5.5, 1.0, -50.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 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown) + + 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)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)); + PreintegratedImuMeasurements pim(biasHat, kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // Make sure of biasCorrectedDelta + Matrix96 actualH; + pim.biasCorrectedDelta(bias, actualH); + Matrix expectedH = numericalDerivative11( + boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, + boost::none), bias); + EXPECT(assert_equal(expectedH, actualH)); // 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), pim, kGravityAlongNavZDown, + kNonZeroOmegaCoriolis); - SETDEBUG("ImuFactor evaluateError", false); - Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); - SETDEBUG("ImuFactor evaluateError", false); + Values values; + values.insert(X(1), x1); + values.insert(V(1), v1); + values.insert(X(2), x2); + values.insert(V(2), v2); + values.insert(B(1), bias); - // 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)); + // Make sure linearization is correct + double diffDelta = 1e-7; + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); } /* ************************************************************************* */ -TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) -{ +TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) { + using common::x1; + using common::v1; + using common::v2; 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)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), + Point3(5.5, 1.0, -50.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 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown) + + 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)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + PreintegratedImuMeasurements pim( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), + kMeasuredAccCovariance, kMeasuredOmegaCovariance, + kIntegrationErrorCovariance); + pim.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); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, + kNonZeroOmegaCoriolis, boost::none, use2ndOrderCoriolis); - SETDEBUG("ImuFactor evaluateError", false); - Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); - SETDEBUG("ImuFactor evaluateError", false); + Values values; + values.insert(X(1), x1); + values.insert(V(1), v1); + values.insert(X(2), x2); + values.insert(V(2), v2); + values.insert(B(1), bias); - // 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)); + // Make sure linearization is correct + double diffDelta = 1e-7; + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); } /* ************************************************************************* */ -TEST( ImuFactor, PartialDerivative_wrt_Bias ) -{ +TEST(ImuFactor, PartialDerivative_wrt_Bias) { // Linearization point - Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias + Vector3 biasOmega(0, 0, 0); // Current estimate of rotation rate bias // Measurements - Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; + Vector3 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::ExpmapDerivative((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-9)); } /* ************************************************************************* */ -TEST( ImuFactor, PartialDerivativeLogmap ) -{ +TEST(ImuFactor, PartialDerivativeLogmap) { // Linearization point - Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias + Vector3 thetahat(0.1, 0.1, 0); // Current estimate of rotation rate bias // Measurements - Vector3 deltatheta; deltatheta << 0, 0, 0; + Vector3 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)); Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat); @@ -422,472 +519,269 @@ TEST( ImuFactor, PartialDerivativeLogmap ) } /* ************************************************************************* */ -TEST( ImuFactor, fistOrderExponential ) -{ +TEST(ImuFactor, fistOrderExponential) { // Linearization point - Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias + Vector3 biasOmega(0, 0, 0); // Current estimate of rotation rate bias // Measurements - Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; + Vector3 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; + Vector3 deltabiasOmega; + deltabiasOmega << alpha, alpha, alpha; - const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::ExpmapDerivative( + (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 * (I_3x3 + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); // This is a first order expansion so the equality is only an approximation EXPECT(assert_equal(expectedRot, actualRot)); } /* ************************************************************************* */ -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)); +TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { + 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); + PreintegratedImuMeasurements preintegrated = + evaluatePreintegratedMeasurements(kZeroBias, measuredAccs, measuredOmegas, + deltaTs); // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); + Matrix expectedDelPdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, + measuredOmegas, deltaTs), kZeroBias); + Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - Matrix expectedDelVdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); + Matrix expectedDelVdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, + measuredOmegas, deltaTs), kZeroBias); + Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); - Matrix expectedDelRdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); + Matrix expectedDelRdelBias = + numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, + measuredAccs, measuredOmegas, deltaTs), kZeroBias); + 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())); } /* ************************************************************************* */ -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)); +Vector3 correctedAcc(const PreintegratedImuMeasurements& pim, const Vector3& measuredAcc, const Vector3& measuredOmega) { + return pim.correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega).first; } -/* ************************************************************************* */ -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 -///* ************************************************************************* */ -//TEST( ImuFactor, LinearizeTiming) -//{ -// // Linearization point -// 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)); -// Vector3 v2(Vector3(0.5, 0.0, 0.0)); -// imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012)); -// -// // Pre-integrator -// imuBias::ConstantBias biasHat(Vector3(0, 0, 0.10), Vector3(0, 0, 0.10)); -// Vector3 gravity; gravity << 0, 0, 9.81; -// Vector3 omegaCoriolis; omegaCoriolis << 0.0001, 0, 0.01; -// ImuFactor::PreintegratedMeasurements pre_int_data(biasHat, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); -// -// // Pre-integrate Measurements -// Vector3 measuredAcc(0.1, 0.0, 0.0); -// Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); -// double deltaT = 0.5; -// for(size_t i = 0; i < 50; ++i) { -// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); -// } -// -// // Create factor -// noiseModel::Base::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.MeasurementCovariance()); -// ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); -// -// Values values; -// values.insert(X(1), x1); -// values.insert(X(2), x2); -// values.insert(V(1), v1); -// values.insert(V(2), v2); -// values.insert(B(1), bias); -// -// Ordering ordering; -// ordering.push_back(X(1)); -// ordering.push_back(V(1)); -// ordering.push_back(X(2)); -// ordering.push_back(V(2)); -// ordering.push_back(B(1)); -// -// GaussianFactorGraph graph; -// gttic_(LinearizeTiming); -// for(size_t i = 0; i < 100000; ++i) { -// GaussianFactor::shared_ptr g = factor.linearize(values, ordering); -// graph.push_back(g); -// } -// gttoc_(LinearizeTiming); -// tictoc_finishedIteration_(); -// std::cout << "Linear Error: " << graph.error(values.zeroVectors(ordering)) << std::endl; -// 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); - double deltaT = 1.0; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown) + + Vector3(0.2, 0.0, 0.0); + double dt = 0.1; - const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.10,0.10)), Point3(1,0,0)); + Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, M_PI/2, 0)), Point3(0.1, 0.05, 0.01)); + imuBias::ConstantBias biasHat(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::Zero(), Matrix3::Zero(), Matrix3::Zero()); + // Get mean prediction from "ground truth" measurements + PreintegratedImuMeasurements pim(biasHat, accNoiseVar2.asDiagonal(), + omegaNoiseVar2.asDiagonal(), Z_3x3, true); // MonteCarlo does not sample integration noise + pim.set_body_P_sensor(body_P_sensor); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + // Check updatedDeltaXij derivatives + Matrix3 D_correctedAcc_measuredOmega = Matrix3::Zero(); + pim.correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, boost::none, D_correctedAcc_measuredOmega, boost::none); + Matrix3 expectedD = numericalDerivative11(boost::bind(correctedAcc, pim, measuredAcc, _1), measuredOmega, 1e-6); + EXPECT(assert_equal(expectedD, D_correctedAcc_measuredOmega, 1e-5)); - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + Matrix93 G1, G2; + NavState preint = pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2); +// Matrix9 preintCov = G1*((accNoiseVar2/dt).asDiagonal())*G1.transpose() + G2*((omegaNoiseVar2/dt).asDiagonal())*G2.transpose(); - // 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); + Matrix93 expectedG1 = numericalDerivative21( + boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2, + dt, boost::none, boost::none, boost::none), measuredAcc, measuredOmega, + 1e-6); + EXPECT(assert_equal(expectedG1, G1, 1e-5)); - // 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); + Matrix93 expectedG2 = numericalDerivative22( + boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2, + dt, boost::none, boost::none, boost::none), measuredAcc, measuredOmega, + 1e-6); + EXPECT(assert_equal(expectedG2, G2, 1e-5)); - // Actual Jacobians - Matrix H1a, H2a, H3a, H4a, H5a; - (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); +#if 0 + /* + * This code is to verify the quality of the generated samples + * by checking if the covariance of the generated noises matches + * with the input covariance, and visualizing the nonlinearity of + * the sample set using the following matlab script: + * + noises = dlmread('noises.txt'); + cov(noises) - 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)); + samples = dlmread('noises.txt'); + figure(1); + for i=1:9 + subplot(3,3,i) + hist(samples(:,i), 500) + end + */ + size_t N = 100000; + Matrix samples(9,N); + Sampler sampleAccelerationNoise((accNoiseVar2/dt).cwiseSqrt(), 29284); + Sampler sampleOmegaNoise((omegaNoiseVar2/dt).cwiseSqrt(), 10); + ofstream samplesOut("preintSamples.txt"); + ofstream noiseOut("noises.txt"); + for (size_t i = 0; iprint("noise: "); // Make sure the noise is valid + + // Make sure linearization is correct + double diffDelta = 1e-8; + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); } /* ************************************************************************* */ -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 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); + PreintegratedImuMeasurements pim( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + kMeasuredAccCovariance, kMeasuredOmegaCovariance, + kIntegrationErrorCovariance, true); - 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); + for (int i = 0; i < 1000; ++i) + pim.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), pim, kGravityAlongNavZDown, + kZeroOmegaCoriolis); - // 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))); + // Predict + Pose3 x1; + Vector3 v1(0, 0.0, 0.0); + PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, + kGravityAlongNavZDown, kZeroOmegaCoriolis); + 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))); } /* ************************************************************************* */ @@ -895,33 +789,276 @@ 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 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); + PreintegratedImuMeasurements pim( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + kMeasuredAccCovariance, kMeasuredOmegaCovariance, + kIntegrationErrorCovariance, true); - 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); + for (int i = 0; i < 1000; ++i) + pim.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), pim, kGravityAlongNavZDown, + kZeroOmegaCoriolis); - // 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().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, pim, kGravityAlongNavZDown, + kZeroOmegaCoriolis); + 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);} +TEST(ImuFactor, PredictArbitrary) { + imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)); + + // Measurements + Vector3 measuredOmega(M_PI / 10, M_PI / 10, M_PI / 10); + Vector3 measuredAcc(0.1, 0.2, -9.81); + double dt = 0.001; + + ImuFactor::PreintegratedMeasurements pim(biasHat, kMeasuredAccCovariance, + kMeasuredOmegaCovariance, Z_3x3, true); // MonteCarlo does not sample integration noise + Pose3 x1; + Vector3 v1 = Vector3(0, 0, 0); + imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); + EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, boost::none, measuredAcc, measuredOmega, + Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000)); + + for (int i = 0; i < 1000; ++i) + pim.integrateMeasurement(measuredAcc, measuredOmega, dt); + +// Matrix expected(9,9); +// expected << // +// 0.0299999995, 2.46739898e-10, 2.46739896e-10, -0.0144839494, 0.044978128, 0.0100471195, -0.0409843415, 0.134423822, 0.0383280513, // +// 2.46739898e-10, 0.0299999995, 2.46739902e-10, -0.0454268484, -0.0149428917, 0.00609093775, -0.13554868, -0.0471183681, 0.0247643646, // +// 2.46739896e-10, 2.46739902e-10, 0.0299999995, 0.00489577218, 0.00839301168, 0.000448720395, 0.00879031682, 0.0162199769, 0.00112485862, // +// -0.0144839494, -0.0454268484, 0.00489577218, 0.142448905, 0.00345595825, -0.0225794125, 0.34774305, 0.0119449979, -0.075667905, // +// 0.044978128, -0.0149428917, 0.00839301168, 0.00345595825, 0.143318431, 0.0200549262, 0.0112877167, 0.351503176, 0.0629164907, // +// 0.0100471195, 0.00609093775, 0.000448720395, -0.0225794125, 0.0200549262, 0.0104041905, -0.0580647212, 0.051116506, 0.0285371399, // +// -0.0409843415, -0.13554868, 0.00879031682, 0.34774305, 0.0112877167, -0.0580647212, 0.911721561, 0.0412249672, -0.205920425, // +// 0.134423822, -0.0471183681, 0.0162199769, 0.0119449979, 0.351503176, 0.051116506, 0.0412249672, 0.928013807, 0.169935105, // +// 0.0383280513, 0.0247643646, 0.00112485862, -0.075667905, 0.0629164907, 0.0285371399, -0.205920425, 0.169935105, 0.09407764; +// EXPECT(assert_equal(expected, pim.preintMeasCov(), 1e-7)); + + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, + kZeroOmegaCoriolis); + + // Predict + Pose3 x2; + Vector3 v2; + ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravityAlongNavZDown, + kZeroOmegaCoriolis); + + // Regression test for Imu Refactor + Rot3 expectedR( // + +0.903715275, -0.250741668, 0.347026393, // + +0.347026393, 0.903715275, -0.250741668, // + -0.250741668, 0.347026393, 0.903715275); + Point3 expectedT(-0.505517319, 0.569413747, 0.0861035711); + Vector3 expectedV(-1.59121524, 1.55353139, 0.3376838540); + Pose3 expectedPose(expectedR, expectedT); + EXPECT(assert_equal(expectedPose, x2, 1e-7)); + EXPECT(assert_equal(Vector(expectedV), v2, 1e-7)); +} + +/* ************************************************************************* */ +TEST(ImuFactor, bodyPSensorNoBias) { + imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) + + // Measurements + Vector3 n_gravity(0, 0, -9.81); // z-up nav frame + Vector3 omegaCoriolis(0, 0, 0); + // Sensor frame is z-down + // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame + Vector3 s_omegaMeas_ns(0, 0.1, M_PI / 10); + // Acc measurement is acceleration of sensor in the sensor frame, when stationary, + // table exerts an equal and opposite force w.r.t gravity + Vector3 s_accMeas(0, 0, -9.81); + double dt = 0.001; + + // Rotate sensor (z-down) to body (same as navigation) i.e. z-up + Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3(0, 0, 0)); + + ImuFactor::PreintegratedMeasurements pim(bias, Z_3x3, Z_3x3, Z_3x3, true); + + for (int i = 0; i < 1000; ++i) + pim.integrateMeasurement(s_accMeas, s_omegaMeas_ns, dt, body_P_sensor); + + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, n_gravity, omegaCoriolis); + + // Predict + Pose3 x1; + Vector3 v1(0, 0, 0); + PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, n_gravity, + omegaCoriolis); + + Pose3 expectedPose(Rot3().ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0)); + EXPECT(assert_equal(expectedPose, poseVelocity.pose)); + + Vector3 expectedVelocity(0, 0, 0); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); +} + +/* ************************************************************************* */ +#include +#include +#include +#include +#include + +TEST(ImuFactor, bodyPSensorWithBias) { + using noiseModel::Diagonal; + typedef imuBias::ConstantBias Bias; + + int numFactors = 80; + Vector6 noiseBetweenBiasSigma; + noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6, + 3.0e-6, 3.0e-6); + SharedDiagonal biasNoiseModel = Diagonal::Sigmas(noiseBetweenBiasSigma); + + // Measurements + Vector3 n_gravity(0, 0, -9.81); + Vector3 omegaCoriolis(0, 0, 0); + + // Sensor frame is z-down + // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame + Vector3 measuredOmega(0, 0.01, 0); + // Acc measurement is acceleration of sensor in the sensor frame, when stationary, + // table exerts an equal and opposite force w.r.t gravity + Vector3 measuredAcc(0, 0, -9.81); + + Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3()); + + Matrix3 accCov = 1e-7 * I_3x3; + Matrix3 gyroCov = 1e-8 * I_3x3; + Matrix3 integrationCov = 1e-9 * I_3x3; + double deltaT = 0.005; + + // Specify noise values on priors + Vector6 priorNoisePoseSigmas( + (Vector(6) << 0.001, 0.001, 0.001, 0.01, 0.01, 0.01).finished()); + Vector3 priorNoiseVelSigmas((Vector(3) << 0.1, 0.1, 0.1).finished()); + Vector6 priorNoiseBiasSigmas( + (Vector(6) << 0.1, 0.1, 0.1, 0.5e-1, 0.5e-1, 0.5e-1).finished()); + SharedDiagonal priorNoisePose = Diagonal::Sigmas(priorNoisePoseSigmas); + SharedDiagonal priorNoiseVel = Diagonal::Sigmas(priorNoiseVelSigmas); + SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas); + Vector3 zeroVel(0, 0, 0); + + // Create a factor graph with priors on initial pose, vlocity and bias + NonlinearFactorGraph graph; + Values values; + + PriorFactor priorPose(X(0), Pose3(), priorNoisePose); + graph.add(priorPose); + values.insert(X(0), Pose3()); + + PriorFactor priorVel(V(0), zeroVel, priorNoiseVel); + graph.add(priorVel); + values.insert(V(0), zeroVel); + + // The key to this test is that we specify the bias, in the sensor frame, as known a priori + // We also create factors below that encode our assumption that this bias is constant over time + // In theory, after optimization, we should recover that same bias estimate + Bias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot) + PriorFactor priorBiasFactor(B(0), priorBias, priorNoiseBias); + graph.add(priorBiasFactor); + values.insert(B(0), priorBias); + + // Now add IMU factors and bias noise models + Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0)); + for (int i = 1; i < numFactors; i++) { + ImuFactor::PreintegratedMeasurements pim = + ImuFactor::PreintegratedMeasurements(priorBias, accCov, gyroCov, + integrationCov, true); + for (int j = 0; j < 200; ++j) + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT, + body_P_sensor); + + // Create factors + graph.add( + ImuFactor(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim, n_gravity, + omegaCoriolis)); + graph.add(BetweenFactor(B(i - 1), B(i), zeroBias, biasNoiseModel)); + + values.insert(X(i), Pose3()); + values.insert(V(i), zeroVel); + values.insert(B(i), priorBias); + } + + // Finally, optimize, and get bias at last time step + Values results = LevenbergMarquardtOptimizer(graph, values).optimize(); + Bias biasActual = results.at(B(numFactors - 1)); + + // And compare it with expected value (our prior) + Bias biasExpected(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); + EXPECT(assert_equal(biasExpected, biasActual, 1e-3)); +} + +/* ************************************************************************** */ +#include + +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +TEST(ImuFactor, serialization) { + using namespace gtsam::serializationTestHelpers; + + Vector3 n_gravity(0, 0, -9.81); + Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3()); + Matrix3 accCov = 1e-7 * I_3x3; + Matrix3 gyroCov = 1e-8 * I_3x3; + Matrix3 integrationCov = 1e-9 * I_3x3; + double deltaT = 0.005; + imuBias::ConstantBias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot) + + ImuFactor::PreintegratedMeasurements pim = + ImuFactor::PreintegratedMeasurements(priorBias, accCov, gyroCov, + integrationCov, true); + + // measurements are needed for non-inf noise model, otherwise will throw err when deserialize + + // Sensor frame is z-down + // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame + Vector3 measuredOmega(0, 0.01, 0); + // Acc measurement is acceleration of sensor in the sensor frame, when stationary, + // table exerts an equal and opposite force w.r.t gravity + Vector3 measuredAcc(0, 0, -9.81); + + for (int j = 0; j < 200; ++j) + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT, + body_P_sensor); + + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp new file mode 100644 index 000000000..a62ca06a8 --- /dev/null +++ b/gtsam/navigation/tests/testNavState.cpp @@ -0,0 +1,272 @@ +/* ---------------------------------------------------------------------------- + + * 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 testNavState.cpp + * @brief Unit tests for NavState + * @author Frank Dellaert + * @date July 2015 + */ + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +static const Rot3 kAttitude = Rot3::RzRyRx(0.1, 0.2, 0.3); +static const Point3 kPosition(1.0, 2.0, 3.0); +static const Pose3 kPose(kAttitude, kPosition); +static const Velocity3 kVelocity(0.4, 0.5, 0.6); +static const NavState kIdentity; +static const NavState kState1(kAttitude, kPosition, kVelocity); +static const Vector3 kOmegaCoriolis(0.02, 0.03, 0.04); +static const Vector3 kGravity(0, 0, 9.81); +static const Vector9 kZeroXi = Vector9::Zero(); + +/* ************************************************************************* */ +TEST(NavState, Constructor) { + boost::function construct = + boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none, + boost::none); + Matrix aH1, aH2; + EXPECT( + assert_equal(kState1, + NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(construct, kPose, kVelocity), aH1)); + EXPECT(assert_equal(numericalDerivative22(construct, kPose, kVelocity), aH2)); +} + +/* ************************************************************************* */ +TEST( NavState, Attitude) { + Matrix39 aH, eH; + Rot3 actual = kState1.attitude(aH); + EXPECT(assert_equal(actual, kAttitude)); + eH = numericalDerivative11( + boost::bind(&NavState::attitude, _1, boost::none), kState1); + EXPECT(assert_equal((Matrix )eH, aH)); +} + +/* ************************************************************************* */ +TEST( NavState, Position) { + Matrix39 aH, eH; + Point3 actual = kState1.position(aH); + EXPECT(assert_equal(actual, kPosition)); + eH = numericalDerivative11( + boost::bind(&NavState::position, _1, boost::none), kState1); + EXPECT(assert_equal((Matrix )eH, aH)); +} + +/* ************************************************************************* */ +TEST( NavState, Velocity) { + Matrix39 aH, eH; + Velocity3 actual = kState1.velocity(aH); + EXPECT(assert_equal(actual, kVelocity)); + eH = numericalDerivative11( + boost::bind(&NavState::velocity, _1, boost::none), kState1); + EXPECT(assert_equal((Matrix )eH, aH)); +} + +/* ************************************************************************* */ +TEST( NavState, BodyVelocity) { + Matrix39 aH, eH; + Velocity3 actual = kState1.bodyVelocity(aH); + EXPECT(assert_equal(actual, kAttitude.unrotate(kVelocity))); + eH = numericalDerivative11( + boost::bind(&NavState::bodyVelocity, _1, boost::none), kState1); + EXPECT(assert_equal((Matrix )eH, aH)); +} + +/* ************************************************************************* */ +TEST( NavState, MatrixGroup ) { + // check roundtrip conversion to 7*7 matrix representation + Matrix7 T = kState1.matrix(); + EXPECT(assert_equal(kState1, NavState(T))); + + // check group product agrees with matrix product + NavState state2 = kState1 * kState1; + Matrix T2 = T * T; + EXPECT(assert_equal(state2, NavState(T2))); + EXPECT(assert_equal(T2, state2.matrix())); +} + +/* ************************************************************************* */ +TEST( NavState, Manifold ) { + // Check zero xi + EXPECT(assert_equal(kIdentity, kIdentity.retract(kZeroXi))); + EXPECT(assert_equal(kZeroXi, kIdentity.localCoordinates(kIdentity))); + EXPECT(assert_equal(kState1, kState1.retract(kZeroXi))); + EXPECT(assert_equal(kZeroXi, kState1.localCoordinates(kState1))); + + // Check definition of retract as operating on components separately + Vector9 xi; + xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; + Rot3 drot = Rot3::Expmap(xi.head<3>()); + Point3 dt = Point3(xi.segment<3>(3)); + Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3); + NavState state2 = kState1 * NavState(drot, dt, dvel); + EXPECT(assert_equal(state2, kState1.retract(xi))); + EXPECT(assert_equal(xi, kState1.localCoordinates(state2))); + + // roundtrip from state2 to state3 and back + NavState state3 = state2.retract(xi); + EXPECT(assert_equal(xi, state2.localCoordinates(state3))); + + // Check derivatives for ChartAtOrigin::Retract + Matrix9 aH; + // For zero xi + boost::function Retract = boost::bind( + NavState::ChartAtOrigin::Retract, _1, boost::none); + NavState::ChartAtOrigin::Retract(kZeroXi, aH); + EXPECT(assert_equal(numericalDerivative11(Retract, kZeroXi), aH)); + // For non-zero xi + NavState::ChartAtOrigin::Retract(xi, aH); + EXPECT(assert_equal(numericalDerivative11(Retract, xi), aH)); + + // Check derivatives for ChartAtOrigin::Local + // For zero xi + boost::function Local = boost::bind( + NavState::ChartAtOrigin::Local, _1, boost::none); + NavState::ChartAtOrigin::Local(kIdentity, aH); + EXPECT(assert_equal(numericalDerivative11(Local, kIdentity), aH)); + // For non-zero xi + NavState::ChartAtOrigin::Local(kState1, aH); + EXPECT(assert_equal(numericalDerivative11(Local, kState1), aH)); + + // Check retract derivatives + Matrix9 aH1, aH2; + kState1.retract(xi, aH1, aH2); + boost::function retract = + boost::bind(&NavState::retract, _1, _2, boost::none, boost::none); + EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1)); + EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2)); + + // Check localCoordinates derivatives + boost::function local = + boost::bind(&NavState::localCoordinates, _1, _2, boost::none, + boost::none); + // from state1 to state2 + kState1.localCoordinates(state2, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1)); + EXPECT(assert_equal(numericalDerivative22(local, kState1, state2), aH2)); + // from identity to state2 + kIdentity.localCoordinates(state2, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(local, kIdentity, state2), aH1)); + EXPECT(assert_equal(numericalDerivative22(local, kIdentity, state2), aH2)); + // from state2 to identity + state2.localCoordinates(kIdentity, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(local, state2, kIdentity), aH1)); + EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2)); +} + +/* ************************************************************************* */ +TEST( NavState, Lie ) { + // Check zero xi + EXPECT(assert_equal(kIdentity, kIdentity.expmap(kZeroXi))); + EXPECT(assert_equal(kZeroXi, kIdentity.logmap(kIdentity))); + EXPECT(assert_equal(kState1, kState1.expmap(kZeroXi))); + EXPECT(assert_equal(kZeroXi, kState1.logmap(kState1))); + + // Expmap/Logmap roundtrip + Vector xi(9); + xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; + NavState state2 = NavState::Expmap(xi); + EXPECT(assert_equal(xi, NavState::Logmap(state2))); + + // roundtrip from state2 to state3 and back + NavState state3 = state2.expmap(xi); + EXPECT(assert_equal(xi, state2.logmap(state3))); + + // For the expmap/logmap (not necessarily expmap/local) -xi goes other way + EXPECT(assert_equal(state2, state3.expmap(-xi))); + EXPECT(assert_equal(xi, -state3.logmap(state2))); +} + +/* ************************************************************************* */ +TEST(NavState, Update) { + Vector3 omega(M_PI / 100.0, 0.0, 0.0); + Vector3 acc(0.1, 0.0, 0.0); + double dt = 10; + Matrix9 aF; + Matrix93 aG1, aG2; + boost::function update = + boost::bind(&NavState::update, _1, _2, _3, dt, boost::none, + boost::none, boost::none); + Vector3 b_acc = kAttitude * acc; + NavState expected(kAttitude.expmap(dt * omega), + kPosition + Point3((kVelocity + b_acc * dt / 2) * dt), + kVelocity + b_acc * dt); + NavState actual = kState1.update(acc, omega, dt, aF, aG1, aG2); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(numericalDerivative31(update, kState1, acc, omega, 1e-7), aF, 1e-7)); + EXPECT(assert_equal(numericalDerivative32(update, kState1, acc, omega, 1e-7), aG1, 1e-7)); + EXPECT(assert_equal(numericalDerivative33(update, kState1, acc, omega, 1e-7), aG2, 1e-7)); + + // Try different values + omega = Vector3(0.1, 0.2, 0.3); + acc = Vector3(0.4, 0.5, 0.6); + kState1.update(acc, omega, dt, aF, aG1, aG2); + EXPECT(assert_equal(numericalDerivative31(update, kState1, acc, omega, 1e-7), aF, 1e-7)); + EXPECT(assert_equal(numericalDerivative32(update, kState1, acc, omega, 1e-7), aG1, 1e-7)); + EXPECT(assert_equal(numericalDerivative33(update, kState1, acc, omega, 1e-7), aG2, 1e-7)); +} + +/* ************************************************************************* */ +static const double dt = 2.0; +boost::function coriolis = boost::bind( + &NavState::coriolis, _1, dt, kOmegaCoriolis, _2, boost::none); + +TEST(NavState, Coriolis) { + Matrix9 aH; + + // first-order + kState1.coriolis(dt, kOmegaCoriolis, false, aH); + EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, false), aH)); + // second-order + kState1.coriolis(dt, kOmegaCoriolis, true, aH); + EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, true), aH)); +} + +TEST(NavState, Coriolis2) { + Matrix9 aH; + NavState state2(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0), + Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0)); + + // first-order + state2.coriolis(dt, kOmegaCoriolis, false, aH); + EXPECT(assert_equal(numericalDerivative21(coriolis, state2, false), aH)); + // second-order + state2.coriolis(dt, kOmegaCoriolis, true, aH); + EXPECT(assert_equal(numericalDerivative21(coriolis, state2, true), aH)); +} + +/* ************************************************************************* */ +TEST(NavState, CorrectPIM) { + Vector9 xi; + xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; + double dt = 0.5; + Matrix9 aH1, aH2; + boost::function correctPIM = + boost::bind(&NavState::correctPIM, _1, _2, dt, kGravity, kOmegaCoriolis, + false, boost::none, boost::none); + kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1)); + EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testPoseVelocityBias.cpp b/gtsam/navigation/tests/testPoseVelocityBias.cpp new file mode 100644 index 000000000..ce419fdd0 --- /dev/null +++ b/gtsam/navigation/tests/testPoseVelocityBias.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 testPoseVelocityBias.cpp + * @brief Unit test for PoseVelocityBias + * @author Frank Dellaert + */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// Should be seen as between(pvb1,pvb2), i.e., written as pvb2 \omin pvb1 +Vector9 error(const PoseVelocityBias& pvb1, const PoseVelocityBias& pvb2) { + Matrix3 R1 = pvb1.pose.rotation().matrix(); + // Ri.transpose() translate the error from the global frame into pose1's frame + const Vector3 fp = R1.transpose() * (pvb2.pose.translation() - pvb1.pose.translation()).vector(); + const Vector3 fv = R1.transpose() * (pvb2.velocity - pvb1.velocity); + const Rot3 R1BetweenR2 = pvb1.pose.rotation().between(pvb2.pose.rotation()); + const Vector3 fR = Rot3::Logmap(R1BetweenR2); + Vector9 r; + r << fp, fv, fR; + return r; +} + +/* ************************************************************************************************/ +TEST(PoseVelocityBias, error) { + Point3 i1(0, 1, 0), j1(-1, 0, 0), k1(0, 0, 1); + Pose3 x1(Rot3(i1, j1, k1), Point3(5.0, 1.0, 0.0)); + Vector3 v1(Vector3(0.5, 0.0, 0.0)); + imuBias::ConstantBias bias1(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); + + Pose3 x2(Rot3(i1, j1, k1).expmap(Vector3(0.1, 0.2, 0.3)), Point3(5.5, 1.0, 6.0)); + Vector3 v2(Vector3(0.5, 4.0, 3.0)); + imuBias::ConstantBias bias2(Vector3(0.1, 0.2, -0.3), Vector3(0.2, 0.3, 0.1)); + + PoseVelocityBias pvb1(x1, v1, bias1), pvb2(x2, v2, bias2); + + Vector9 expected, actual = error(pvb1, pvb2); + expected << 0.0, -0.5, 6.0, 4.0, 0.0, 3.0, 0.1, 0.2, 0.3; + EXPECT(assert_equal(expected, actual, 1e-9)); +} + +/* ************************************************************************************************/ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************************************/ diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h index 7295f3160..ff059ef78 100644 --- a/gtsam/nonlinear/AdaptAutoDiff.h +++ b/gtsam/nonlinear/AdaptAutoDiff.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -27,92 +27,46 @@ 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. + * The AdaptAutoDiff class uses ceres-style autodiff to adapt a ceres-style + * Function evaluation, i.e., a function FUNCTOR that defines an operator + * template bool operator()(const T* const, const T* const, T* + * predicted) const; + * For now only binary operators are supported. */ -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 +template class AdaptAutoDiff { + typedef Eigen::Matrix RowMajor1; + typedef Eigen::Matrix RowMajor2; - static const int N = traits::dimension; - static const int M1 = traits::dimension; - static const int M2 = traits::dimension; + typedef Eigen::Matrix VectorT; + typedef Eigen::Matrix Vector1; + typedef Eigen::Matrix Vector2; - 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) { + FUNCTOR f; + public: + VectorT operator()(const Vector1& v1, const Vector2& v2, + 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); + const double* parameters[] = {v1.data(), v2.data()}; + double rowMajor1[M * N1], rowMajor2[M * N2]; // on the stack + double* jacobians[] = {rowMajor1, rowMajor2}; + success = AutoDiff::Differentiate( + f, parameters, M, 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); + // TODO: if this is a bottleneck (probably not!) fix Autodiff to be + // Column-Major + if (H1) *H1 = Eigen::Map(rowMajor1); + if (H2) *H2 = Eigen::Map(rowMajor2); } else { // Apply the mapping, to get result @@ -121,9 +75,8 @@ public: if (!success) throw std::runtime_error( "AdaptAutoDiff: function call resulted in failure"); - return CanonicalT::Retract(result); + return result; } - }; -} +} // namespace gtsam 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 e148bd65d..a91515e9c 100644 --- a/gtsam/nonlinear/DoglegOptimizer.cpp +++ b/gtsam/nonlinear/DoglegOptimizer.cpp @@ -95,8 +95,8 @@ void DoglegOptimizer::iterate(void) { /* ************************************************************************* */ DoglegParams DoglegOptimizer::ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph) const { - if(!params.ordering) - params.ordering = Ordering::colamd(graph); + if (!params.ordering) + params.ordering = Ordering::Create(params.orderingType, graph); return params; } diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index a86e7312a..70a872d15 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -19,773 +19,244 @@ #pragma once -#include -#include -#include +#include -#include #include -#include -#include - -// template meta-programming headers -#include -namespace MPL = boost::mpl::placeholders; - -#include // operator typeid -#include - -class ExpressionFactorBinaryTest; -// Forward declare for testing +#include +#include namespace gtsam { -const unsigned TraceAlignment = 16; - 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); +void Expression::print(const std::string& s) const { + root_->print(s); } 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); - } -}; - -//----------------------------------------------------------------------------- - -namespace internal { - -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; - } -}; +Expression::Expression(const T& value) : + root_(new internal::ConstantExpression(value)) { } -/// Handle Leaf Case: reverse AD ends here, by writing a matrix into Jacobians -template -void handleLeafCase(const Eigen::MatrixBase& dTdA, - JacobianMap& jacobians, Key key) { - internal::UseBlockIf< - Derived::RowsAtCompileTime != Eigen::Dynamic - && Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian( - dTdA, jacobians, key); +template +Expression::Expression(const Key& key) : + root_(new internal::LeafExpression(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 { +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 KeyVector& 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; - 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); - } + VerticalBlockMatrix Ab(dims, Dim); + Ab.matrix().setZero(); + internal::JacobianMap jacobianMap(keys, Ab); - /// Define type so we can apply it as a meta-function - typedef ExecutionTrace type; -}; + // Call unsafe version + T result = valueAndJacobianMap(values, jacobianMap); -/// 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. -typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage; + // Copy blocks into the vector of jacobians passed in + for (DenseIndex i = 0; i < static_cast(keys.size()); i++) + H[i] = Ab(i); -//----------------------------------------------------------------------------- -/** - * 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 { + return result; +} -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: - - /// 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; +T Expression::traceExecution(const Values& values, + internal::ExecutionTrace& trace, void* traceStorage) const { + return root_->traceExecution(values, trace, + static_cast(traceStorage)); +} - /// The key into values - Key key_; +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(); - /// 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; - } - - /// 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_); - } - -}; - -//----------------------------------------------------------------------------- -// 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::dimension, - traits::dimension> type; -}; - -/// meta-function to generate JacobianTA optional reference -template -struct MakeOptionalJacobian { - typedef OptionalJacobian::dimension, - traits::dimension> 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 startReverseAD4(JacobianMap& jacobians) const { - } - template - void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { - } - }; - /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, - ExecutionTraceStorage*& traceStorage) 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 -// The reason we inherit from Argument is because we can then -// case to this unique signature to retrieve the expression at any level -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 - // The reason we inherit from JacobianTrace is because we can then - // case to this unique signature to retrieve the value/trace at any level - 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 startReverseAD4(JacobianMap& jacobians) const { - Base::Record::startReverseAD4(jacobians); - // 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. - This::trace.reverseAD1(This::dTdA, jacobians); - } - - /// Given df/dT, multiply in dT/dA and continue reverse AD process - // Cols is always known at compile time - template - void reverseAD4(const SomeMatrix & dFdT, - JacobianMap& jacobians) const { - Base::Record::reverseAD4(dFdT, jacobians); - This::trace.reverseAD1(dFdT * This::dTdA, jacobians); - } - }; - - /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, - ExecutionTraceStorage*& traceStorage) const { - Base::trace(values, record, traceStorage); // recurse - // 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->Record::This::value = This::expression->traceExecution(values, - record->Record::This::trace, traceStorage); - // traceStorage is never modified by traceExecution, but if traceExecution has - // written in the buffer, the next caller expects we advance the pointer - traceStorage += 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; + // 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 - /// Reset expression shared pointer - template - void reset(const boost::shared_ptr >& ptr) { - static_cast &>(*this).expression = ptr; - } + internal::ExecutionTrace trace; + T value(this->traceExecution(values, trace, traceStorage)); + trace.startReverseAD1(jacobians); - /// Access Expression shared pointer - template - boost::shared_ptr > expression() const { - return static_cast const &>(*this).expression; - } +#ifdef _MSC_VER + delete[] traceStorage; +#endif - /// Provide convenience access to Record storage and implement - /// the virtual function based interface of CallRecord using the CallRecordImplementor - struct Record: public internal::CallRecordImplementor::dimension>, public Base::Record { - using Base::Record::print; - using Base::Record::startReverseAD4; - using Base::Record::reverseAD4; - - 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, - ExecutionTraceStorage* traceStorage) const { - assert(reinterpret_cast(traceStorage) % TraceAlignment == 0); - - // Create the record and advance the pointer - Record* record = new (traceStorage) Record(); - traceStorage += upAligned(sizeof(Record)); - - // Record the traces for all arguments - // After this, the traceStorage pointer is set to after what was written - Base::trace(values, record, traceStorage); - - // Return the record for this function evaluation - return record; - } - }; -}; -//----------------------------------------------------------------------------- - -/// Unary Function Expression -template -class UnaryExpression: public FunctionalNode >::type { - - typedef typename MakeOptionalJacobian::type OJ1; - -public: - - typedef boost::function 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_ = upAligned(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, - ExecutionTraceStorage* traceStorage) const { - - Record* record = Base::trace(values, traceStorage); - trace.setFunction(record); - - return function_(record->template value(), - record->template jacobian()); - } -}; - -//----------------------------------------------------------------------------- -/// Binary Expression - -template -class BinaryExpression: - public FunctionalNode >::type { - - typedef typename MakeOptionalJacobian::type OJ1; - typedef typename MakeOptionalJacobian::type OJ2; - -public: - - typedef boost::function 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_ = // - upAligned(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, - ExecutionTraceStorage* traceStorage) const { - - Record* record = Base::trace(values, traceStorage); - 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 { - - typedef typename MakeOptionalJacobian::type OJ1; - typedef typename MakeOptionalJacobian::type OJ2; - typedef typename MakeOptionalJacobian::type OJ3; - -public: - - typedef boost::function 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_ = // - upAligned(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, - ExecutionTraceStorage* traceStorage) const { - - Record* record = Base::trace(values, traceStorage); - trace.setFunction(record); - - return function_( - record->template value(), record->template value(), - record->template value(), record->template jacobian(), - record->template jacobian(), record->template jacobian()); - } - -}; -//----------------------------------------------------------------------------- + return value; } + +template +typename Expression::KeysAndDims Expression::keysAndDims() const { + std::map map; + dims(map); + size_t n = map.size(); + KeysAndDims pair = std::make_pair(KeyVector(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 index a39b1557c..0b348ece9 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -19,21 +19,28 @@ #pragma once -#include +#include #include -#include +#include #include -#include -#include +#include +#include +// Forward declare tests class ExpressionFactorShallowTest; namespace gtsam { -// Forward declare +// Forward declares +class Values; template class ExpressionFactor; +namespace internal { +template class ExecutionTrace; +template class ExpressionNode; +} + /** * Expression class that supports automatic differentiation */ @@ -48,110 +55,97 @@ public: private: // Paul's trick shared pointer, polymorphic root of entire expression tree - boost::shared_ptr > root_; + 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 { - std::cout << s << *root_ << std::endl; - } + void print(const std::string& s) const; - // Construct a constant expression - Expression(const T& value) : - root_(new ConstantExpression(value)) { - } + /// Construct a constant expression + Expression(const T& value); - // Construct a leaf expression, with Key - Expression(const Key& key) : - root_(new LeafExpression(key)) { - } + /// Construct a leaf expression, with Key + Expression(const Key& key); - // Construct a leaf expression, with Symbol - Expression(const Symbol& symbol) : - root_(new LeafExpression(symbol)) { - } + /// Construct a leaf expression, with Symbol + Expression(const Symbol& symbol); - // Construct a leaf expression, creating Symbol - Expression(unsigned char c, size_t j) : - root_(new LeafExpression(Symbol(c, j))) { - } + /// 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) : - 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)) { - } + 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) : - 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)) { - } + const Expression& expression2); /// Construct a binary method expression template Expression(const Expression& expression1, T (A1::*method)(const A2&, const A3&, - typename TernaryExpression::OJ1, - typename TernaryExpression::OJ2, - typename TernaryExpression::OJ3) const, - const Expression& expression2, const Expression& expression3) : - root_( - new TernaryExpression( - boost::bind(method, _1, _2, _3, _4, _5, _6), expression1, - expression2, expression3)) { - } + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) const, + const Expression& expression2, const Expression& expression3); - /// 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 root - const boost::shared_ptr >& root() const { - return root_; - } - - // Return size needed for memory buffer in traceExecution - size_t traceSize() const { - return root_->traceSize(); + /// Destructor + virtual ~Expression() { } /// Return keys that play in this expression - std::set keys() const { - return root_->keys(); - } + std::set keys() const; /// Return dimensions for each argument, as a map - void dims(std::map& map) const { - root_->dims(map); - } + void dims(std::map& map) const; /** * @brief Return value and optional derivatives, reverse AD version @@ -159,16 +153,7 @@ public: * 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 { - - if (H) { - // Call private version that returns derivatives in H - KeysAndDims pair = keysAndDims(); - return value(values, pair.first, pair.second, *H); - } else - // no derivatives needed, just return value - return root_->value(values); - } + boost::none) const; /** * @return a "deep" copy of this Expression @@ -179,116 +164,58 @@ public: 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: - /// Vaguely unsafe keys and dimensions in same order - typedef std::pair, FastVector > KeysAndDims; - KeysAndDims 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; - } + /// Default constructor, for serialization + Expression() {} + + /// Keys and dimensions in same order + typedef std::pair > KeysAndDims; + KeysAndDims keysAndDims() const; /// private version that takes keys and dimensions, returns derivatives - T value(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(); - JacobianMap jacobianMap(keys, Ab); - - // Call unsafe version - T result = value(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; - } + T valueAndDerivatives(const Values& values, const KeyVector& keys, + const FastVector& dims, std::vector& H) const; /// trace execution, very unsafe - T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - return root_->traceExecution(values, trace, traceStorage); - } + T traceExecution(const Values& values, internal::ExecutionTrace& trace, + void* traceStorage) const; - /** - * @brief Return value and derivatives, reverse AD version - * This very unsafe method needs a JacobianMap with correctly allocated - * and initialized VerticalBlockMatrix, hence is declared private. - */ - 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(); - - // 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 - ExecutionTraceStorage* traceStorage = new ExecutionTraceStorage[size]; -#else - ExecutionTraceStorage traceStorage[size]; -#endif - - ExecutionTrace trace; - T value(traceExecution(values, trace, traceStorage)); - trace.startReverseAD1(jacobians); - -#ifdef _MSC_VER - delete[] traceStorage; -#endif - - return value; - } + /// 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; - }; -// 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); - } -}; - -/// Construct a product expression, assumes T::compose(T) -> T +/** + * Construct a product expression, assumes T::compose(T) -> T + * Example: + * Expression a(0), b(1), c = a*b; + */ template -Expression operator*(const Expression& expression1, - const Expression& expression2) { - return Expression(boost::bind(apply_compose(), _1, _2, _3, _4), - expression1, expression2); -} +Expression operator*(const Expression& e1, const Expression& e2); -/// Construct an array of leaves +/** + * 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) { - std::vector > unknowns; - unknowns.reserve(n); - for (size_t i = start; i < start + n; i++) - unknowns.push_back(Expression(c, i)); - return unknowns; -} +std::vector > createUnknowns(size_t n, char c, size_t start = 0); -} +} // namespace gtsam + +#include diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 4769e5048..21e17a648 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -27,50 +27,68 @@ namespace gtsam { /** + * Factor that supports arbitrary expressions via AD */ -template +template class ExpressionFactor: public NoiseModelFactor { + BOOST_CONCEPT_ASSERT((IsTestable)); protected: - T measurement_; ///< the measurement to be compared with the expression - Expression expression_; ///< the expression that is AD enabled - FastVector dims_; ///< dimensions of the Jacobian matrices - + typedef ExpressionFactor This; static const int Dim = traits::dimension; + T measured_; ///< the measurement to be compared with the expression + Expression expression_; ///< the expression that is AD enabled + FastVector dims_; ///< dimensions of the Jacobian matrices + public: - /// Constructor - ExpressionFactor(const SharedNoiseModel& noiseModel, // - const T& measurement, const Expression& expression) : - measurement_(measurement), expression_(expression) { - if (!noiseModel) - throw std::invalid_argument("ExpressionFactor: no NoiseModel."); - if (noiseModel->dim() != Dim) - throw std::invalid_argument( - "ExpressionFactor was created with a NoiseModel of incorrect dimension."); - noiseModel_ = noiseModel; + typedef boost::shared_ptr > shared_ptr; - // Get keys and dimensions for Jacobian matrices - // An Expression is assumed unmutable, so we do this now - boost::tie(keys_, dims_) = expression_.keysAndDims(); + /// Constructor + ExpressionFactor(const SharedNoiseModel& noiseModel, // + const T& measurement, const Expression& expression) + : NoiseModelFactor(noiseModel), measured_(measurement) { + initialize(expression); + } + + /// Destructor + virtual ~ExpressionFactor() {} + + /** return the measurement */ + const T& measured() const { return measured_; } + + /// print relies on Testable traits being defined for T + void print(const std::string& s, const KeyFormatter& keyFormatter) const { + NoiseModelFactor::print(s, keyFormatter); + traits::Print(measured_, s + ".measured_"); + } + + /// equals relies on Testable traits being defined for T + bool equals(const NonlinearFactor& f, double tol) const { + const ExpressionFactor* p = dynamic_cast(&f); + return p && NoiseModelFactor::equals(f, tol) && + traits::Equals(measured_, p->measured_, tol) && + dims_ == p->dims_; } /** - * Error function *without* the NoiseModel, \f$ h(x)-z \f$. + * Error function *without* the NoiseModel, \f$ z-h(x) -> Local(h(x),z) \f$. * We override this method to provide * both the function evaluation and its derivative(s) in H. */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { - if (H) { - const T value = expression_.value(x, keys_, dims_, *H); - return traits::Local(measurement_, value); + if (H) { + const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H); + // NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here + // because it would use the tangent space of the measurement instead of the value. + return -traits::Local(value, measured_); } else { const T value = expression_.value(x); - return traits::Local(measurement_, value); + return -traits::Local(value, measured_); } } @@ -79,35 +97,154 @@ public: 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_->isConstrained(); + SharedDiagonal noiseModel; + if (noiseModel_ && noiseModel_->isConstrained()) { + noiseModel = boost::static_pointer_cast( + noiseModel_)->unit(); + } + + // Create a writeable JacobianFactor in advance boost::shared_ptr factor( - constrained ? new JacobianFactor(keys_, dims_, Dim, - boost::static_pointer_cast(noiseModel_)->unit()) : - new JacobianFactor(keys_, dims_, Dim)); + new JacobianFactor(keys_, dims_, Dim, noiseModel)); // Wrap keys and VerticalBlockMatrix into structure passed to expression_ VerticalBlockMatrix& Ab = factor->matrixObject(); - JacobianMap jacobianMap(keys_, Ab); + internal::JacobianMap jacobianMap(keys_, Ab); // Zero out Jacobian so we can simply add to it Ab.matrix().setZero(); // Get value and Jacobians, writing directly into JacobianFactor - T value = expression_.value(x, jacobianMap); // <<< Reverse AD happens here ! + 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); + Ab(size()).col(0) = traits::Local(value, measured_); // Whiten the corresponding system, Ab already contains RHS - Vector dummy(Dim); - noiseModel_->WhitenSystem(Ab.matrix(), dummy); + if (noiseModel_) { + 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))); + } + +protected: + ExpressionFactor() {} + /// Default constructor, for serialization + + /// Constructor for serializable derived classes + ExpressionFactor(const SharedNoiseModel& noiseModel, const T& measurement) + : NoiseModelFactor(noiseModel), measured_(measurement) { + // Not properly initialized yet, need to call initialize + } + + /// Initialize with constructor arguments + void initialize(const Expression& expression) { + if (!noiseModel_) + throw std::invalid_argument("ExpressionFactor: no NoiseModel."); + if (noiseModel_->dim() != Dim) + throw std::invalid_argument( + "ExpressionFactor was created with a NoiseModel of incorrect dimension."); + expression_ = expression; + + // Get keys and dimensions for Jacobian matrices + // An Expression is assumed unmutable, so we do this now + boost::tie(keys_, dims_) = expression_.keysAndDims(); + } + + /// Recreate expression from keys_ and measured_, used in load below. + /// Needed to deserialize a derived factor + virtual Expression expression() const { + throw std::runtime_error("ExpressionFactor::expression not provided: cannot deserialize."); + } + +private: + /// Save to an archive: just saves the base class + template + void save(Archive& ar, const unsigned int /*version*/) const { + ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor); + ar << boost::serialization::make_nvp("measured_", this->measured_); + } + + /// Load from an archive, creating a valid expression using the overloaded + /// [expression] method + template + void load(Archive& ar, const unsigned int /*version*/) { + ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor); + ar >> boost::serialization::make_nvp("measured_", this->measured_); + this->initialize(expression()); + } + + // Indicate that we implement save/load separately, and be friendly to boost + BOOST_SERIALIZATION_SPLIT_MEMBER() + + friend class boost::serialization::access; }; // ExpressionFactor -} // \ namespace gtsam +/// traits +template +struct traits > : public Testable > {}; + +/** + * Binary specialization of ExpressionFactor meant as a base class for binary factors + * Enforces expression method with two keys, and provides evaluateError + * Derived needs to call initialize. + */ +template +class ExpressionFactor2 : public ExpressionFactor { + public: + /// Destructor + virtual ~ExpressionFactor2() {} + + /// Backwards compatible evaluateError, to make existing tests compile + Vector evaluateError(const A1& a1, const A2& a2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + Values values; + values.insert(this->keys_[0], a1); + values.insert(this->keys_[1], a2); + std::vector H(2); + Vector error = this->unwhitenedError(values, H); + if (H1) (*H1) = H[0]; + if (H2) (*H2) = H[1]; + return error; + } + + /// Recreate expression from given keys_ and measured_, used in load + /// Needed to deserialize a derived factor + virtual Expression expression(Key key1, Key key2) const { + throw std::runtime_error("ExpressionFactor2::expression not provided: cannot deserialize."); + } + + protected: + /// Default constructor, for serialization + ExpressionFactor2() {} + + /// Constructor takes care of keys, but still need to call initialize + ExpressionFactor2(Key key1, Key key2, + const SharedNoiseModel& noiseModel, + const T& measurement) + : ExpressionFactor(noiseModel, measurement) { + this->keys_.push_back(key1); + this->keys_.push_back(key2); + } + + private: + /// Return an expression that predicts the measurement given Values + virtual Expression expression() const { + return expression(this->keys_[0], this->keys_[1]); + } +}; +// ExpressionFactor2 + +}// \ namespace gtsam diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.cpp b/gtsam/nonlinear/GaussNewtonOptimizer.cpp index 7a115e1c4..e420567ec 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.cpp +++ b/gtsam/nonlinear/GaussNewtonOptimizer.cpp @@ -46,10 +46,9 @@ void GaussNewtonOptimizer::iterate() { /* ************************************************************************* */ GaussNewtonParams GaussNewtonOptimizer::ensureHasOrdering( - GaussNewtonParams params, const NonlinearFactorGraph& graph) const -{ - if(!params.ordering) - params.ordering = Ordering::colamd(graph); + GaussNewtonParams params, const NonlinearFactorGraph& graph) const { + if (!params.ordering) + params.ordering = Ordering::Create(params.orderingType, graph); return params; } diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 41ee52b3a..4e8c0e303 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -19,6 +19,8 @@ #include #include // for selective linearization thresholds #include +#include // for GTSAM_USE_TBB + #include #include @@ -84,11 +86,11 @@ void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool u } /* ************************************************************************* */ -void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const FastVector& roots, +void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, const FastVector& roots, Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd, - FastSet& replacedKeys, Base::Nodes& nodes, - FastSet& fixedVariables) + KeySet& replacedKeys, Base::Nodes& nodes, + KeySet& fixedVariables) { variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); BOOST_FOREACH(Key key, unusedKeys) { @@ -103,10 +105,10 @@ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const FastVect } /* ************************************************************************* */ -FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, +KeySet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { - FastSet relinKeys; + KeySet relinKeys; if(const double* threshold = boost::get(&relinearizeThreshold)) { @@ -131,7 +133,7 @@ FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, } /* ************************************************************************* */ -void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double threshold, +void CheckRelinearizationRecursiveDouble(KeySet& relinKeys, double threshold, const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) { // Check the current clique for relinearization @@ -153,7 +155,7 @@ void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double thresho } /* ************************************************************************* */ -void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap& thresholds, +void CheckRelinearizationRecursiveMap(KeySet& relinKeys, const FastMap& thresholds, const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) { @@ -185,11 +187,11 @@ void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap ISAM2::Impl::CheckRelinearizationPartial(const FastVector& roots, +KeySet ISAM2::Impl::CheckRelinearizationPartial(const FastVector& roots, const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { - FastSet relinKeys; + KeySet relinKeys; BOOST_FOREACH(const ISAM2::sharedClique& root, roots) { if(relinearizeThreshold.type() == typeid(double)) CheckRelinearizationRecursiveDouble(relinKeys, boost::get(relinearizeThreshold), delta, root); @@ -200,7 +202,7 @@ FastSet ISAM2::Impl::CheckRelinearizationPartial(const FastVector& keys, const FastSet& markedMask) +void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask) { static const bool debug = false; // does the separator contain any of the variables? @@ -224,7 +226,7 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, co /* ************************************************************************* */ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, - const FastSet& mask, boost::optional invalidateIfDebug, const KeyFormatter& keyFormatter) + const KeySet& mask, boost::optional invalidateIfDebug, const KeyFormatter& keyFormatter) { // If debugging, invalidate if requested, otherwise do not invalidate. // Invalidating means setting expmapped entries to Inf, to trigger assertions @@ -272,7 +274,7 @@ inline static void optimizeInPlace(const boost::shared_ptr& clique, /* ************************************************************************* */ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector& roots, - const FastSet& replacedKeys, VectorValues& delta, double wildfireThreshold) { + const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold) { size_t lastBacksubVariableCount; @@ -300,7 +302,7 @@ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector /* ************************************************************************* */ namespace internal { -void updateRgProd(const boost::shared_ptr& clique, const FastSet& replacedKeys, +void updateRgProd(const boost::shared_ptr& clique, const KeySet& replacedKeys, const VectorValues& grad, VectorValues& RgProd, size_t& varsUpdated) { // Check if any frontal or separator keys were recalculated, if so, we need @@ -344,7 +346,7 @@ void updateRgProd(const boost::shared_ptr& clique, const FastSet& replacedKeys, +size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, const VectorValues& gradAtZero, VectorValues& RgProd) { // Update variables diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index a8d03df13..d34480144 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -57,10 +57,10 @@ struct GTSAM_EXPORT ISAM2::Impl { /** * Remove variables from the ISAM2 system. */ - static void RemoveVariables(const FastSet& unusedKeys, const FastVector& roots, + static void RemoveVariables(const KeySet& unusedKeys, const FastVector& roots, Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, - VectorValues& RgProd, FastSet& replacedKeys, Base::Nodes& nodes, - FastSet& fixedVariables); + VectorValues& RgProd, KeySet& replacedKeys, Base::Nodes& nodes, + KeySet& fixedVariables); /** * Find the set of variables to be relinearized according to relinearizeThreshold. @@ -71,7 +71,7 @@ struct GTSAM_EXPORT ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static FastSet CheckRelinearizationFull(const VectorValues& delta, + static KeySet CheckRelinearizationFull(const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); /** @@ -85,7 +85,7 @@ struct GTSAM_EXPORT ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static FastSet CheckRelinearizationPartial(const FastVector& roots, + static KeySet CheckRelinearizationPartial(const FastVector& roots, const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); /** @@ -103,7 +103,7 @@ struct GTSAM_EXPORT ISAM2::Impl { * * Alternatively could we trace up towards the root for each variable here? */ - static void FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, const FastSet& markedMask); + static void FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask); /** * Apply expmap to the given values, but only for indices appearing in @@ -119,7 +119,7 @@ struct GTSAM_EXPORT ISAM2::Impl { * @param keyFormatter Formatter for printing nonlinear keys during debugging */ static void ExpmapMasked(Values& values, const VectorValues& delta, - const FastSet& mask, + const KeySet& mask, boost::optional invalidateIfDebug = boost::none, const KeyFormatter& keyFormatter = DefaultKeyFormatter); @@ -127,13 +127,13 @@ struct GTSAM_EXPORT ISAM2::Impl { * Update the Newton's method step point, using wildfire */ static size_t UpdateGaussNewtonDelta(const FastVector& roots, - const FastSet& replacedKeys, VectorValues& delta, double wildfireThreshold); + const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold); /** * Update the RgProd (R*g) incrementally taking into account which variables * have been recalculated in \c replacedKeys. Only used in Dogleg. */ - static size_t UpdateRgProd(const ISAM2::Roots& roots, const FastSet& replacedKeys, + static size_t UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, const VectorValues& gradAtZero, VectorValues& RgProd); /** diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index f7c6d0345..538c66068 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -36,7 +36,7 @@ VALUE ISAM2::calculateEstimate(Key key) const { namespace internal { template void optimizeWildfire(const boost::shared_ptr& clique, double threshold, - FastSet& changed, const FastSet& replaced, VectorValues& delta, size_t& count) + KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& count) { // if none of the variables in this clique (frontal and separator!) changed // significantly, then by the running intersection property, none of the @@ -113,7 +113,7 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, template bool optimizeWildfireNode(const boost::shared_ptr& clique, double threshold, - FastSet& changed, const FastSet& replaced, VectorValues& delta, size_t& count) + KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& count) { // if none of the variables in this clique (frontal and separator!) changed // significantly, then by the running intersection property, none of the @@ -245,8 +245,8 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh /* ************************************************************************* */ template -size_t optimizeWildfire(const boost::shared_ptr& root, double threshold, const FastSet& keys, VectorValues& delta) { - FastSet changed; +size_t optimizeWildfire(const boost::shared_ptr& root, double threshold, const KeySet& keys, VectorValues& delta) { + KeySet changed; int count = 0; // starting from the root, call optimize on each conditional if(root) @@ -256,9 +256,9 @@ size_t optimizeWildfire(const boost::shared_ptr& root, double threshold, /* ************************************************************************* */ template -size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, double threshold, const FastSet& keys, VectorValues& delta) +size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, double threshold, const KeySet& keys, VectorValues& delta) { - FastSet changed; + KeySet changed; size_t count = 0; if (root) { diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 3b3d76285..52af4fee3 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; @@ -194,7 +194,7 @@ FastSet ISAM2::getAffectedFactors(const FastList& keys) const { // (note that the remaining stuff is summarized in the cached factors) GaussianFactorGraph::shared_ptr -ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const +ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySet& relinKeys) const { gttic(getAffectedFactors); FastSet candidates = getAffectedFactors(affectedKeys); @@ -204,7 +204,7 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const FastS gttic(affectedKeysSet); // for fast lookup below - FastSet affectedKeysSet; + KeySet affectedKeysSet; affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); gttoc(affectedKeysSet); @@ -260,9 +260,9 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { } /* ************************************************************************* */ -boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKeys, const FastSet& relinKeys, +boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const KeySet& relinKeys, const vector& observedKeys, - const FastSet& unusedIndices, + const KeySet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result) { @@ -326,7 +326,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals()); gttoc(affectedKeys); - boost::shared_ptr > affectedKeysSet(new FastSet()); // Will return this result + boost::shared_ptr affectedKeysSet(new KeySet()); // Will return this result if(affectedKeys.size() >= theta_.size() * batchThreshold) { @@ -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( @@ -566,17 +566,17 @@ ISAM2Result ISAM2::update( variableIndex_.remove(removeFactorIndices.begin(), removeFactorIndices.end(), removeFactors); // Compute unused keys and indices - FastSet unusedKeys; - FastSet unusedIndices; + KeySet unusedKeys; + KeySet unusedIndices; { // Get keys from removed factors and new factors, and compute unused keys, // i.e., keys that are empty now and do not appear in the new factors. - FastSet removedAndEmpty; + KeySet removedAndEmpty; BOOST_FOREACH(Key key, removeFactors.keys()) { if(variableIndex_[key].empty()) removedAndEmpty.insert(removedAndEmpty.end(), key); } - FastSet newFactorSymbKeys = newFactors.keys(); + KeySet newFactorSymbKeys = newFactors.keys(); std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(), newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(unusedKeys, unusedKeys.end())); @@ -602,10 +602,10 @@ ISAM2Result ISAM2::update( gttic(gather_involved_keys); // 3. Mark linear update - FastSet markedKeys = newFactors.keys(); // Get keys from new factors + KeySet markedKeys = newFactors.keys(); // Get keys from new factors // Also mark keys involved in removed factors { - FastSet markedRemoveKeys = removeFactors.keys(); // Get keys involved in removed factors + KeySet markedRemoveKeys = removeFactors.keys(); // Get keys involved in removed factors markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys } // Also mark any provided extra re-eliminate keys @@ -632,7 +632,7 @@ ISAM2Result ISAM2::update( gttoc(gather_involved_keys); // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold - FastSet relinKeys; + KeySet relinKeys; if (relinearizeThisStep) { gttic(gather_relinearize_keys); // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. @@ -659,7 +659,7 @@ ISAM2Result ISAM2::update( result.detail->variableStatus[key].isRelinearized = true; } } // Add the variables being relinearized to the marked keys - FastSet markedRelinMask; + KeySet markedRelinMask; BOOST_FOREACH(const Key key, relinKeys) markedRelinMask.insert(key); markedKeys.insert(relinKeys.begin(), relinKeys.end()); @@ -674,7 +674,7 @@ ISAM2Result ISAM2::update( // Relin involved keys for detailed results if(params_.enableDetailedResults) { - FastSet involvedRelinKeys; + KeySet involvedRelinKeys; BOOST_FOREACH(const sharedClique& root, roots_) Impl::FindAll(root, involvedRelinKeys, markedRelinMask); BOOST_FOREACH(Key key, involvedRelinKeys) { @@ -726,7 +726,7 @@ ISAM2Result ISAM2::update( gttic(recalculate); // 8. Redo top of Bayes tree - boost::shared_ptr > replacedKeys; + boost::shared_ptr replacedKeys; if(!markedKeys.empty() || !observedKeys.empty()) replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, unusedIndices, constrainedKeys, result); @@ -758,7 +758,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, boost::optional&> deletedFactorsIndices) { // Convert to ordered set - FastSet leafKeys(leafKeysList.begin(), leafKeysList.end()); + KeySet leafKeys(leafKeysList.begin(), leafKeysList.end()); // Keep track of marginal factors - map from clique to the marginal factors // that should be incorporated into it, passed up from it's children. @@ -769,7 +769,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, FastSet factorIndicesToRemove; // Keep track of variables removed in subtrees - FastSet leafKeysRemoved; + KeySet leafKeysRemoved; // Remove each variable and its subtrees BOOST_FOREACH(Key j, leafKeys) { @@ -881,7 +881,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, graph.push_back(nonlinearFactors_[i]->linearize(theta_)); } // Reeliminate the linear graph to get the marginal and discard the conditional - const FastSet cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); + const KeySet cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); FastVector cliqueFrontalsToEliminate; std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(), std::back_inserter(cliqueFrontalsToEliminate)); @@ -957,7 +957,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, deletedFactorsIndices->assign(factorIndicesToRemove.begin(), factorIndicesToRemove.end()); // Remove the marginalized variables - Impl::RemoveVariables(FastSet(leafKeys.begin(), leafKeys.end()), roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, + Impl::RemoveVariables(KeySet(leafKeys.begin(), leafKeys.end()), roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, nodes_, fixedVariables_); } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 9adceee6a..fdc0021e5 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_); @@ -450,7 +460,7 @@ protected: * This is \c mutable because it is used internally to not update delta_ * until it is needed. */ - mutable FastSet deltaReplacedMask_; // TODO: Make sure accessed in the right way + mutable KeySet deltaReplacedMask_; // TODO: Make sure accessed in the right way /** All original nonlinear factors are stored here to use during relinearization */ NonlinearFactorGraph nonlinearFactors_; @@ -466,7 +476,7 @@ protected: /** Set of variables that are involved with linear factors from marginalized * variables and thus cannot have their linearization points changed. */ - FastSet fixedVariables_; + KeySet fixedVariables_; int update_count_; ///< Counter incremented every update(), used to determine periodic relinearization @@ -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 @@ -597,7 +614,7 @@ public: const VariableIndex& getVariableIndex() const { return variableIndex_; } /** Access the nonlinear variable index */ - const FastSet& getFixedVariables() const { return fixedVariables_; } + const KeySet& getFixedVariables() const { return fixedVariables_; } size_t lastAffectedVariableCount; size_t lastAffectedFactorCount; @@ -624,11 +641,11 @@ public: protected: FastSet getAffectedFactors(const FastList& keys) const; - GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const; + GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const KeySet& relinKeys) const; GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); - virtual boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& relinKeys, - const std::vector& observedKeys, const FastSet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result); + virtual boost::shared_ptr recalculate(const KeySet& markedKeys, const KeySet& relinKeys, + const std::vector& observedKeys, const KeySet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result); void updateDelta(bool forceFullSolve = false) const; }; // ISAM2 @@ -649,11 +666,11 @@ template<> struct traits : public Testable {}; /// @return The number of variables that were solved for template size_t optimizeWildfire(const boost::shared_ptr& root, - double threshold, const FastSet& replaced, VectorValues& delta); + double threshold, const KeySet& replaced, VectorValues& delta); template size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, - double threshold, const FastSet& replaced, VectorValues& delta); + double threshold, const KeySet& replaced, VectorValues& delta); /// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix) template diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 473caa35e..9e42afa33 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -22,12 +22,16 @@ #include #include #include +#include #include #include +#include + +#include +#include #include #include -#include using namespace std; @@ -42,6 +46,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 +71,9 @@ std::string LevenbergMarquardtParams::verbosityLMTranslator( case LevenbergMarquardtParams::SILENT: s = "SILENT"; break; + case LevenbergMarquardtParams::SUMMARY: + s = "SUMMARY"; + break; case LevenbergMarquardtParams::TERMINATION: s = "TERMINATION"; break; @@ -99,8 +108,8 @@ void LevenbergMarquardtParams::print(const std::string& str) const { std::cout << " lambdaLowerBound: " << lambdaLowerBound << "\n"; std::cout << " minModelFidelity: " << minModelFidelity << "\n"; std::cout << " diagonalDamping: " << diagonalDamping << "\n"; - std::cout << " min_diagonal: " << min_diagonal_ << "\n"; - std::cout << " max_diagonal: " << max_diagonal_ << "\n"; + std::cout << " minDiagonal: " << minDiagonal << "\n"; + std::cout << " maxDiagonal: " << maxDiagonal << "\n"; std::cout << " verbosityLM: " << verbosityLMTranslator(verbosityLM) << "\n"; std::cout.flush(); @@ -113,19 +122,19 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::linearize() const { /* ************************************************************************* */ void LevenbergMarquardtOptimizer::increaseLambda() { - if (params_.useFixedLambdaFactor_) { + if (params_.useFixedLambdaFactor) { state_.lambda *= params_.lambdaFactor; } else { state_.lambda *= params_.lambdaFactor; params_.lambdaFactor *= 2.0; } - params_.reuse_diagonal_ = true; + state_.reuseDiagonal = true; } /* ************************************************************************* */ void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality) { - if (params_.useFixedLambdaFactor_) { + if (params_.useFixedLambdaFactor) { state_.lambda /= params_.lambdaFactor; } else { // CHECK_GT(step_quality, 0.0); @@ -133,7 +142,7 @@ void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality) { params_.lambdaFactor = 2.0; } state_.lambda = std::max(params_.lambdaLowerBound, state_.lambda); - params_.reuse_diagonal_ = false; + state_.reuseDiagonal = false; } @@ -146,12 +155,12 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( cout << "building damped system with lambda " << state_.lambda << endl; // Only retrieve diagonal vector when reuse_diagonal = false - if (params_.diagonalDamping && params_.reuse_diagonal_ == false) { + if (params_.diagonalDamping && state_.reuseDiagonal == false) { state_.hessianDiagonal = linear.hessianDiagonal(); BOOST_FOREACH(Vector& v, state_.hessianDiagonal | map_values) { for (int aa = 0; aa < v.size(); aa++) { - v(aa) = std::min(std::max(v(aa), params_.min_diagonal_), - params_.max_diagonal_); + v(aa) = std::min(std::max(v(aa), params_.minDiagonal), + params_.maxDiagonal); v(aa) = sqrt(v(aa)); } } @@ -173,19 +182,31 @@ 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; } } } else { // Straightforward damping: + + // initialize noise model cache to a reasonable default size + NoiseCacheVector noises(6); BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) { size_t dim = key_value.value.dim(); - Matrix A = Matrix::Identity(dim, dim); - Vector b = Vector::Zero(dim); - SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); - damped += boost::make_shared(key_value.key, A, b, model); + + if (dim > noises.size()) + noises.resize(dim); + + NoiseCacheItem& item = noises[dim-1]; + + // Initialize noise model, A and b if we haven't done so already + if(!item.model) { + item.A = Matrix::Identity(dim, dim); + item.b = Vector::Zero(dim); + item.model = noiseModel::Isotropic::Sigma(dim, sigma); + } + damped += boost::make_shared(key_value.key, item.A, item.b, item.model); } } gttoc(damp); @@ -219,12 +240,26 @@ 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) { +#ifdef GTSAM_USING_NEW_BOOST_TIMERS + boost::timer::cpu_timer lamda_iteration_timer; + lamda_iteration_timer.start(); +#else + boost::timer lamda_iteration_timer; + lamda_iteration_timer.restart(); +#endif + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "trying lambda = " << state_.lambda << endl; @@ -236,20 +271,21 @@ void LevenbergMarquardtOptimizer::iterate() { double modelFidelity = 0.0; bool step_is_successful = false; bool stopSearchingLambda = false; - double newError; + double newError = numeric_limits::infinity(), costChange; Values newValues; VectorValues delta; bool systemSolvedSuccessfully; try { + // ============ Solve is where most computation happens !! ================= delta = solve(dampedSystem, state_.values, params_); systemSolvedSuccessfully = true; - } catch (IndeterminantLinearSystemException) { + } catch (const IndeterminantLinearSystemException& e) { systemSolvedSuccessfully = false; } if (systemSolvedSuccessfully) { - params_.reuse_diagonal_ = true; + state_.reuseDiagonal = true; if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.norm() << endl; @@ -267,7 +303,9 @@ void LevenbergMarquardtOptimizer::iterate() { if (linearizedCostChange >= 0) { // step is valid // update values gttic(retract); + // ============ This is where the solution is updated ==================== newValues = state_.values.retract(delta); + // ======================================================================= gttoc(retract); // compute new error @@ -282,7 +320,7 @@ void LevenbergMarquardtOptimizer::iterate() { << ") new (tentative) error (" << newError << ")" << endl; // cost change in the original, nonlinear system (old - new) - double costChange = state_.error - newError; + costChange = state_.error - newError; if (linearizedCostChange > 1e-20) { // the (linear) error has to decrease to satisfy this condition // fidelity of linearized model VS original system between @@ -304,6 +342,21 @@ void LevenbergMarquardtOptimizer::iterate() { } } + if (lmVerbosity == LevenbergMarquardtParams::SUMMARY) { + // do timing +#ifdef GTSAM_USING_NEW_BOOST_TIMERS + double iterationTime = 1e-9 * lamda_iteration_timer.elapsed().wall; +#else + double iterationTime = lamda_iteration_timer.elapsed(); +#endif + if (state_.iterations == 0) + cout << "iter cost cost_change lambda success iter_time" << endl; + + cout << boost::format("% 4d % 8e % 3.2e % 3.2e % 4d % 3.2e") % + state_.iterations % newError % costChange % state_.lambda % + systemSolvedSuccessfully % iterationTime << endl; + } + ++state_.totalNumberInnerIterations; if (step_is_successful) { // we have successfully decreased the cost and we have good modelFidelity @@ -320,7 +373,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,12 +394,8 @@ void LevenbergMarquardtOptimizer::iterate() { /* ************************************************************************* */ LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering( LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const { - if (!params.ordering){ - if (params.orderingType == Ordering::METIS) - params.ordering = Ordering::metis(graph); - else - params.ordering = Ordering::colamd(graph); - } + if (!params.ordering) + params.ordering = Ordering::Create(params.orderingType, graph); return params; } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 20f9dec3c..2be4a218e 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); @@ -52,70 +52,84 @@ public: double lambdaLowerBound; ///< The minimum lambda used in LM (default: 0) VerbosityLM verbosityLM; ///< The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::verbosity double minModelFidelity; ///< Lower bound for the modelFidelity to accept the result of an LM iteration - std::string logFile; ///< an optional CSV log file, with [iteration, time, error, labda] + std::string logFile; ///< an optional CSV log file, with [iteration, time, error, lambda] bool diagonalDamping; ///< if true, use diagonal of Hessian - bool reuse_diagonal_; ///< an additional option in Ceres for diagonalDamping (related to efficiency) - bool useFixedLambdaFactor_; ///< if true applies constant increase (or decrease) to lambda according to lambdaFactor - double min_diagonal_; ///< when using diagonal damping saturates the minimum diagonal entries (default: 1e-6) - double max_diagonal_; ///< when using diagonal damping saturates the maximum diagonal entries (default: 1e32) + bool useFixedLambdaFactor; ///< if true applies constant increase (or decrease) to lambda according to lambdaFactor + double minDiagonal; ///< when using diagonal damping saturates the minimum diagonal entries (default: 1e-6) + double maxDiagonal; ///< when using diagonal damping saturates the maximum diagonal entries (default: 1e32) - LevenbergMarquardtParams() : - lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), lambdaLowerBound( - 0.0), verbosityLM(SILENT), minModelFidelity(1e-3), - diagonalDamping(false), reuse_diagonal_(false), useFixedLambdaFactor_(true), - min_diagonal_(1e-6), max_diagonal_(1e32) { - } - virtual ~LevenbergMarquardtParams() { + LevenbergMarquardtParams() + : verbosityLM(SILENT), + diagonalDamping(false), + minDiagonal(1e-6), + maxDiagonal(1e32) { + SetLegacyDefaults(this); } + static void SetLegacyDefaults(LevenbergMarquardtParams* p) { + // Relevant NonlinearOptimizerParams: + p->maxIterations = 100; + p->relativeErrorTol = 1e-5; + p->absoluteErrorTol = 1e-5; + // LM-specific: + p->lambdaInitial = 1e-5; + p->lambdaFactor = 10.0; + p->lambdaUpperBound = 1e5; + p->lambdaLowerBound = 0.0; + p->minModelFidelity = 1e-3; + p->diagonalDamping = false; + p->useFixedLambdaFactor = true; + } + + // these do seem to work better for SFM + static void SetCeresDefaults(LevenbergMarquardtParams* p) { + // Relevant NonlinearOptimizerParams: + p->maxIterations = 50; + p->absoluteErrorTol = 0; // No corresponding option in CERES + p->relativeErrorTol = 1e-6; // This is function_tolerance + // LM-specific: + p->lambdaUpperBound = 1e32; + p->lambdaLowerBound = 1e-16; + p->lambdaInitial = 1e-04; + p->lambdaFactor = 2.0; + p->minModelFidelity = 1e-3; // options.min_relative_decrease in CERES + p->diagonalDamping = true; + p->useFixedLambdaFactor = false; // This is important + } + + static LevenbergMarquardtParams LegacyDefaults() { + LevenbergMarquardtParams p; + SetLegacyDefaults(&p); + return p; + } + + static LevenbergMarquardtParams CeresDefaults() { + LevenbergMarquardtParams p; + SetCeresDefaults(&p); + return p; + } + + virtual ~LevenbergMarquardtParams() {} virtual void print(const std::string& str = "") const; - inline double getlambdaInitial() const { - return lambdaInitial; - } - inline double getlambdaFactor() const { - return lambdaFactor; - } - inline double getlambdaUpperBound() const { - return lambdaUpperBound; - } - inline double getlambdaLowerBound() const { - return lambdaLowerBound; - } - inline std::string getVerbosityLM() const { - return verbosityLMTranslator(verbosityLM); - } - inline std::string getLogFile() const { - return logFile; - } - inline bool getDiagonalDamping() const { - return diagonalDamping; - } - - inline void setlambdaInitial(double value) { - lambdaInitial = value; - } - inline void setlambdaFactor(double value) { - lambdaFactor = value; - } - inline void setlambdaUpperBound(double value) { - lambdaUpperBound = value; - } - inline void setlambdaLowerBound(double value) { - lambdaLowerBound = value; - } - inline void setVerbosityLM(const std::string &s) { - verbosityLM = verbosityLMTranslator(s); - } - inline void setLogFile(const std::string &s) { - logFile = s; - } - inline void setDiagonalDamping(bool flag) { - diagonalDamping = flag; - } - inline void setUseFixedLambdaFactor(bool flag) { - useFixedLambdaFactor_ = flag; - } + /// @name Getters/Setters, mainly for MATLAB. Use fields above in C++. + /// @{ + bool getDiagonalDamping() const { return diagonalDamping; } + double getlambdaFactor() const { return lambdaFactor; } + double getlambdaInitial() const { return lambdaInitial; } + double getlambdaLowerBound() const { return lambdaLowerBound; } + double getlambdaUpperBound() const { return lambdaUpperBound; } + std::string getLogFile() const { return logFile; } + std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM);} + void setDiagonalDamping(bool flag) { diagonalDamping = flag; } + void setlambdaFactor(double value) { lambdaFactor = value; } + void setlambdaInitial(double value) { lambdaInitial = value; } + void setlambdaLowerBound(double value) { lambdaLowerBound = value; } + void setlambdaUpperBound(double value) { lambdaUpperBound = value; } + void setLogFile(const std::string& s) { logFile = s; } + void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor = flag;} + void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s);} + // @} }; /** @@ -125,13 +139,12 @@ class GTSAM_EXPORT LevenbergMarquardtState: public NonlinearOptimizerState { public: double lambda; - int totalNumberInnerIterations; // The total number of inner iterations in the optimization (for each iteration, LM may try multiple iterations with different lambdas) boost::posix_time::ptime startTime; - VectorValues hessianDiagonal; //only update hessianDiagonal when reuse_diagonal_ = false + int totalNumberInnerIterations; //< The total number of inner iterations in the optimization (for each iteration, LM may try multiple iterations with different lambdas) + VectorValues hessianDiagonal; //< we only update hessianDiagonal when reuseDiagonal = false + bool reuseDiagonal; ///< an additional option in Ceres for diagonalDamping - LevenbergMarquardtState() { - initTime(); - } + LevenbergMarquardtState() {} // used in LM constructor but immediately overwritten void initTime() { startTime = boost::posix_time::microsec_clock::universal_time(); @@ -145,7 +158,7 @@ protected: const Values& initialValues, const LevenbergMarquardtParams& params, unsigned int iterations = 0) : NonlinearOptimizerState(graph, initialValues, iterations), lambda( - params.lambdaInitial), totalNumberInnerIterations(0) { + params.lambdaInitial), totalNumberInnerIterations(0),reuseDiagonal(false) { initTime(); } @@ -175,12 +188,12 @@ public: * @param initialValues The initial variable assignments * @param params The optimization parameters */ - LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, - const Values& initialValues, const LevenbergMarquardtParams& params = - LevenbergMarquardtParams()) : - NonlinearOptimizer(graph), params_(ensureHasOrdering(params, graph)), state_( - graph, initialValues, params_) { - } + LevenbergMarquardtOptimizer( + const NonlinearFactorGraph& graph, const Values& initialValues, + const LevenbergMarquardtParams& params = LevenbergMarquardtParams()) + : NonlinearOptimizer(graph), + params_(ensureHasOrdering(params, graph)), + state_(graph, initialValues, params_) {} /** Standard constructor, requires a nonlinear factor graph, initial * variable assignments, and optimization parameters. For convenience this @@ -189,9 +202,11 @@ public: * @param graph The nonlinear factor graph to optimize * @param initialValues The initial variable assignments */ - LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, - const Values& initialValues, const Ordering& ordering) : - NonlinearOptimizer(graph) { + LevenbergMarquardtOptimizer( + const NonlinearFactorGraph& graph, const Values& initialValues, + const Ordering& ordering, + const LevenbergMarquardtParams& params = LevenbergMarquardtParams()) + : NonlinearOptimizer(graph), params_(params) { params_.ordering = ordering; state_ = LevenbergMarquardtState(graph, initialValues, params_); } @@ -257,6 +272,17 @@ public: GaussianFactorGraph::shared_ptr buildDampedSystem(const GaussianFactorGraph& linear); friend class ::NonlinearOptimizerMoreOptimizationTest; + /** Small struct to cache objects needed for damping. + * This is used in buildDampedSystem */ + struct NoiseCacheItem { + Matrix A; + Vector b; + SharedDiagonal model; + }; + + /// Noise model Cache + typedef std::vector NoiseCacheVector; + void writeLogFile(double currentError); /// @} diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index 676b3fb85..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_); 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 6e348fb58..86fb34fe0 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -21,21 +21,14 @@ #include #include +#include + #include #include #include namespace gtsam { -/** - * Template default compare function that assumes a testable T - */ -template -bool compare(const T& a, const T& b) { - GTSAM_CONCEPT_TESTABLE_TYPE(T); - return a.equals(b); -} - /** * An equality factor that forces either one variable to a constant, * or a set of variables to be equal to each other. @@ -76,7 +69,9 @@ public: /** * Function that compares two values */ - bool (*compare_)(const T& a, const T& b); + typedef boost::function CompareFunction; + CompareFunction compare_; +// bool (*compare_)(const T& a, const T& b); /** default constructor - only for serialization */ NonlinearEquality() { @@ -92,7 +87,7 @@ public: * Constructor - forces exact evaluation */ NonlinearEquality(Key j, const T& feasible, - bool (*_compare)(const T&, const T&) = compare) : + 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) { @@ -102,7 +97,7 @@ public: * Constructor - allows inexact evaluation */ NonlinearEquality(Key j, const T& feasible, double error_gain, - bool (*_compare)(const T&, const T&) = compare) : + 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) { @@ -122,7 +117,7 @@ public: /** 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) + return e && Base::equals(f) && traits::Equals(feasible_,e->feasible_, tol) && std::abs(error_gain_ - e->error_gain_) < tol; } @@ -185,7 +180,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)); @@ -273,7 +268,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)); @@ -337,7 +332,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/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 5d9f176b3..5b46742e7 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -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 @@ -247,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_); @@ -325,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)); } @@ -401,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)); } @@ -478,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)); } @@ -559,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)); } @@ -644,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)); } @@ -733,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 238d3bc56..b81332f5e 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -17,22 +17,24 @@ * @author Christian Potthast */ -#include -#include -#include #include #include -#include -#include #include -#include #include #include +#include +#include +#include +#include // for GTSAM_USE_TBB #ifdef GTSAM_USE_TBB # include #endif +#include +#include +#include + using namespace std; namespace gtsam { @@ -47,11 +49,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,85 +65,51 @@ 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(); + KeySet keys = this->keys(); // Local utility function to extract x and y coordinates 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 +117,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 +132,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) { + std::set > structure; + BOOST_FOREACH(const sharedFactor& factor, *this){ if(factor) { vector factorKeys = factor->keys(); std::sort(factorKeys.begin(), factorKeys.end()); @@ -199,57 +157,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; - } - } } } } @@ -270,8 +236,8 @@ double NonlinearFactorGraph::error(const Values& c) const { } /* ************************************************************************* */ -FastSet NonlinearFactorGraph::keys() const { - FastSet keys; +KeySet NonlinearFactorGraph::keys() const { + KeySet keys; BOOST_FOREACH(const sharedFactor& factor, this->factors_) { if(factor) keys.insert(factor->begin(), factor->end()); @@ -282,13 +248,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); } /* ************************************************************************* */ @@ -312,23 +278,26 @@ SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic() const namespace { #ifdef GTSAM_USE_TBB - struct _LinearizeOneFactor { - const NonlinearFactorGraph& graph; - const Values& linearizationPoint; - GaussianFactorGraph& result; - _LinearizeOneFactor(const NonlinearFactorGraph& graph, const Values& linearizationPoint, GaussianFactorGraph& result) : - graph(graph), linearizationPoint(linearizationPoint), result(result) {} - void operator()(const tbb::blocked_range& r) const - { - for(size_t i = r.begin(); i != r.end(); ++i) - { - if(graph[i]) - result[i] = graph[i]->linearize(linearizationPoint); - else - result[i] = GaussianFactor::shared_ptr(); - } +class _LinearizeOneFactor { + const NonlinearFactorGraph& nonlinearGraph_; + const Values& linearizationPoint_; + GaussianFactorGraph& result_; +public: + // Create functor with constant parameters + _LinearizeOneFactor(const NonlinearFactorGraph& graph, + const Values& linearizationPoint, GaussianFactorGraph& result) : + nonlinearGraph_(graph), linearizationPoint_(linearizationPoint), result_(result) { + } + // Operator that linearizes a given range of the factors + void operator()(const tbb::blocked_range& blocked_range) const { + for (size_t i = blocked_range.begin(); i != blocked_range.end(); ++i) { + if (nonlinearGraph_[i]) + result_[i] = nonlinearGraph_[i]->linearize(linearizationPoint_); + else + result_[i] = GaussianFactor::shared_ptr(); } - }; + } +}; #endif } @@ -357,7 +326,7 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li if(factor) { (*linearFG) += factor->linearize(linearizationPoint); } else - (*linearFG) += GaussianFactor::shared_ptr(); + (*linearFG) += GaussianFactor::shared_ptr(); } #endif diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 28f72d57d..ecd3b9b56 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) {} }; @@ -101,7 +106,7 @@ namespace gtsam { const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** return keys as an ordered set - ordering is by key value */ - FastSet keys() const; + KeySet keys() const; /** unnormalized error, \f$ 0.5 \sum_i (h_i(X_i)-z)^2/\sigma^2 \f$ in the most common case */ double error(const Values& c) const; @@ -150,12 +155,24 @@ 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)); } diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 00746d9b7..77d26d361 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -68,6 +68,7 @@ void NonlinearOptimizer::defaultOptimize() { // Do next iteration currentError = this->error(); this->iterate(); + tictoc_finishedIteration(); // Maybe show output if(params.verbosity >= NonlinearOptimizerParams::VALUES) this->values().print("newValues"); diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index fe2c3f3ca..05e58accb 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -49,8 +49,12 @@ namespace gtsam { const Key key; ///< The key const ValueType& value; ///< The value - _ValuesConstKeyValuePair(Key _key, const ValueType& _value) : key(_key), value(_value) {} - _ValuesConstKeyValuePair(const _ValuesKeyValuePair& rhs) : key(rhs.key), value(rhs.value) {} + _ValuesConstKeyValuePair(Key _key, const ValueType& _value) : + key(_key), value(_value) { + } + _ValuesConstKeyValuePair(const _ValuesKeyValuePair& rhs) : + key(rhs.key), value(rhs.value) { + } }; /* ************************************************************************* */ @@ -59,17 +63,20 @@ namespace gtsam { // need to use a struct here for later partial specialization template struct ValuesCastHelper { - static CastedKeyValuePairType cast(KeyValuePairType key_value) { - // Static cast because we already checked the type during filtering - return CastedKeyValuePairType(key_value.key, const_cast&>(static_cast&>(key_value.value)).value()); - } + static CastedKeyValuePairType cast(KeyValuePairType key_value) { + // Static cast because we already checked the type during filtering + return CastedKeyValuePairType(key_value.key, + const_cast&>(static_cast&>(key_value.value)).value()); + } }; // partial specialized version for ValueType == Value template struct ValuesCastHelper { static CastedKeyValuePairType cast(KeyValuePairType key_value) { // Static cast because we already checked the type during filtering - // in this case the casted and keyvalue pair are essentially the same type (key, Value&) so perhaps this could be done with just a cast of the key_value? + // in this case the casted and keyvalue pair are essentially the same type + // (key, Value&) so perhaps this could be done with just a cast of the key_value? return CastedKeyValuePairType(key_value.key, key_value.value); } }; @@ -78,7 +85,8 @@ namespace gtsam { struct ValuesCastHelper { static CastedKeyValuePairType cast(KeyValuePairType key_value) { // Static cast because we already checked the type during filtering - // in this case the casted and keyvalue pair are essentially the same type (key, Value&) so perhaps this could be done with just a cast of the key_value? + // in this case the casted and keyvalue pair are essentially the same type + // (key, Value&) so perhaps this could be done with just a cast of the key_value? return CastedKeyValuePairType(key_value.key, key_value.value); } }; @@ -126,23 +134,29 @@ namespace gtsam { } private: - Filtered(const boost::function& filter, Values& values) : - begin_(boost::make_transform_iterator( - boost::make_filter_iterator( - filter, values.begin(), values.end()), - &ValuesCastHelper::cast)), - end_(boost::make_transform_iterator( - boost::make_filter_iterator( - filter, values.end(), values.end()), - &ValuesCastHelper::cast)), - constBegin_(boost::make_transform_iterator( - boost::make_filter_iterator( - filter, ((const Values&)values).begin(), ((const Values&)values).end()), - &ValuesCastHelper::cast)), - constEnd_(boost::make_transform_iterator( - boost::make_filter_iterator( - filter, ((const Values&)values).end(), ((const Values&)values).end()), - &ValuesCastHelper::cast)) {} + Filtered( + const boost::function& filter, + Values& values) : + begin_( + boost::make_transform_iterator( + boost::make_filter_iterator(filter, values.begin(), values.end()), + &ValuesCastHelper::cast)), end_( + boost::make_transform_iterator( + boost::make_filter_iterator(filter, values.end(), values.end()), + &ValuesCastHelper::cast)), constBegin_( + boost::make_transform_iterator( + boost::make_filter_iterator(filter, + ((const Values&) values).begin(), + ((const Values&) values).end()), + &ValuesCastHelper::cast)), constEnd_( + boost::make_transform_iterator( + boost::make_filter_iterator(filter, + ((const Values&) values).end(), + ((const Values&) values).end()), + &ValuesCastHelper::cast)) { + } friend class Values; iterator begin_; @@ -191,7 +205,9 @@ namespace gtsam { friend class Values; const_iterator begin_; const_iterator end_; - ConstFiltered(const boost::function& filter, const Values& values) { + ConstFiltered( + const boost::function& filter, + const Values& values) { // We remove the const from values to create a non-const Filtered // view, then pull the const_iterators out of it. const Filtered filtered(filter, const_cast(values)); @@ -247,7 +263,8 @@ namespace gtsam { /* ************************************************************************* */ template<> - inline bool Values::filterHelper(const boost::function filter, const ConstKeyValuePair& key_value) { + inline bool Values::filterHelper(const boost::function filter, + const ConstKeyValuePair& key_value) { // Filter and check the type return filter(key_value.key); } @@ -263,10 +280,11 @@ namespace gtsam { throw ValuesKeyDoesNotExist("retrieve", j); // Check the type and throw exception if incorrect + const Value& value = *item->second; try { - return dynamic_cast&>(*item->second).value(); + return dynamic_cast&>(value).value(); } catch (std::bad_cast &) { - throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType)); + throw ValuesIncorrectType(j, typeid(value), typeid(ValueType)); } } @@ -278,10 +296,11 @@ namespace gtsam { if(item != values_.end()) { // dynamic cast the type and throw exception if incorrect + const Value& value = *item->second; try { - return dynamic_cast&>(*item->second).value(); + return dynamic_cast&>(value).value(); } catch (std::bad_cast &) { - throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType)); + throw ValuesIncorrectType(j, typeid(value), typeid(ValueType)); } } else { return boost::none; diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 372bced8e..07757062c 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -58,18 +58,19 @@ namespace gtsam { /* ************************************************************************* */ bool Values::equals(const Values& other, double tol) const { - if(this->size() != other.size()) + if (this->size() != other.size()) return false; - for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2) { - if(typeid(it1->value) != typeid(it2->value)) - return false; - if(it1->key != it2->key) - return false; - if(!it1->value.equals_(it2->value, tol)) + for (const_iterator it1 = this->begin(), it2 = other.begin(); + it1 != this->end(); ++it1, ++it2) { + const Value& value1 = it1->value; + const Value& value2 = it2->value; + if (typeid(value1) != typeid(value2) || it1->key != it2->key + || !value1.equals_(value2, tol)) { return false; + } } return true; // We return false earlier if we find anything that does not match - } +} /* ************************************************************************* */ bool Values::exists(Key j) const { @@ -85,7 +86,6 @@ namespace gtsam { VectorValues::const_iterator vector_item = delta.find(key_value->key); Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument if(vector_item != delta.end()) { -// const Vector& singleDelta = delta[key_value->key]; // Delta for this value const Vector& singleDelta = vector_item->second; Value* retractedValue(key_value->value.retract_(singleDelta)); // Retract result.values_.insert(key, retractedValue); // Add retracted result directly to result values @@ -184,12 +184,13 @@ namespace gtsam { void Values::update(Key j, const Value& val) { // Find the value to update KeyValueMap::iterator item = values_.find(j); - if(item == values_.end()) + if (item == values_.end()) throw ValuesKeyDoesNotExist("update", j); // Cast to the derived type - if(typeid(*item->second) != typeid(val)) - throw ValuesIncorrectType(j, typeid(*item->second), typeid(val)); + const Value& old_value = *item->second; + if (typeid(old_value) != typeid(val)) + throw ValuesIncorrectType(j, typeid(old_value), typeid(val)); values_.replace(item, val.clone_()); } @@ -210,8 +211,9 @@ namespace gtsam { } /* ************************************************************************* */ - FastList Values::keys() const { - FastList result; + KeyVector Values::keys() const { + KeyVector result; + result.reserve(size()); for(const_iterator key_value = begin(); key_value != end(); ++key_value) result.push_back(key_value->key); return result; diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 73d0711be..70aadfa06 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -26,24 +26,19 @@ #include #include -#include #include - -#include -#include #include #include -#include #ifdef __GNUC__ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" +#pragma GCC diagnostic ignored "-Wunused-local-typedefs" #endif #include #ifdef __GNUC__ #pragma GCC diagnostic pop #endif #include -#include #include #include @@ -297,7 +292,7 @@ namespace gtsam { * Returns a set of keys in the config * Note: by construction, the list is ordered */ - KeyList keys() const; + KeyVector keys() const; /** Replace all keys and variables */ Values& operator=(const Values& rhs); @@ -404,7 +399,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_); } diff --git a/gtsam/nonlinear/expressionTesting.h b/gtsam/nonlinear/expressionTesting.h index ab6703f3a..b15592c00 100644 --- a/gtsam/nonlinear/expressionTesting.h +++ b/gtsam/nonlinear/expressionTesting.h @@ -20,99 +20,24 @@ #pragma once #include -#include -#include +#include #include -#include -#include - -#include -#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 map of Jacobians - std::map 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[key] = J; - } - - // Next step...return JacobianFactor - return JacobianFactor(jacobians, -e); -} - -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_, +bool 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); -} + return testFactorJacobians(result_, name_, f, values, nd_step, tolerance); } +} // namespace internal } // namespace gtsam /// \brief Check the Jacobians produced by an expression against finite differences. @@ -121,4 +46,4 @@ void testExpressionJacobians(TestResult& result_, const std::string& name_, /// \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); } + { EXPECT(gtsam::internal::testExpressionJacobians(result_, name_, expression, values, numerical_derivative_step, tolerance)); } diff --git a/gtsam/nonlinear/expressions.h b/gtsam/nonlinear/expressions.h index a549517e5..2f46d6d74 100644 --- a/gtsam/nonlinear/expressions.h +++ b/gtsam/nonlinear/expressions.h @@ -18,6 +18,12 @@ 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_; diff --git a/gtsam/nonlinear/factorTesting.h b/gtsam/nonlinear/factorTesting.h new file mode 100644 index 000000000..2a9d70cb4 --- /dev/null +++ b/gtsam/nonlinear/factorTesting.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 factorTesting.h + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief Evaluate derivatives of a nonlinear factor numerically + */ + +#pragma once + +#include +#include + +#include +#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. + * NOTE(frank): factor that have non vector-space measurements use between or LocalCoordinates + * to evaluate the error, and their derivatives will only be correct for near-zero errors. + * This is fixable but expensive, and does not matter in practice as most factors will sit near + * zero errors anyway. However, it means that below will only be exact for the correct measurement. + */ +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 internal { +// CPPUnitLite-style test for linearization of a factor +bool 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)); + if (!actual) return false; + + // Check cast result and then equality + bool equal = assert_equal(expected, *actual, tolerance); + + // if not equal, test individual jacobians: + if (!equal) { + for (size_t i = 0; i < actual->size(); i++) { + bool i_good = assert_equal((Matrix) (expected.getA(expected.begin() + i)), + (Matrix) (actual->getA(actual->begin() + i)), tolerance); + if (!i_good) { + std::cout << "Mismatch in Jacobian " << i+1 << " (base 1), as shown above" << std::endl; + } + } + } + + return equal; +} +} + +/// \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) \ + { EXPECT(gtsam::internal::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance)); } + +} // namespace gtsam diff --git a/gtsam/nonlinear/CallRecord.h b/gtsam/nonlinear/internal/CallRecord.h similarity index 92% rename from gtsam/nonlinear/CallRecord.h rename to gtsam/nonlinear/internal/CallRecord.h index 159a841e5..90aa8a8be 100644 --- a/gtsam/nonlinear/CallRecord.h +++ b/gtsam/nonlinear/internal/CallRecord.h @@ -20,18 +20,9 @@ #pragma once -#include -#include -#include - -#include +#include namespace gtsam { - -class JacobianMap; -// forward declaration - -//----------------------------------------------------------------------------- namespace internal { /** @@ -64,8 +55,6 @@ struct ConvertToVirtualFunctionSupportedMatrixType { } }; -} // namespace internal - /** * 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, @@ -94,9 +83,8 @@ struct CallRecord { inline void reverseAD2(const Eigen::MatrixBase & dFdT, JacobianMap& jacobians) const { _reverseAD3( - internal::ConvertToVirtualFunctionSupportedMatrixType< - (Derived::RowsAtCompileTime > 5)>::convert(dFdT), - jacobians); + ConvertToVirtualFunctionSupportedMatrixType< + (Derived::RowsAtCompileTime > 5)>::convert(dFdT), jacobians); } // This overload supports matrices with both rows and columns dynamically sized. @@ -140,7 +128,6 @@ private: */ const int CallRecordMaxVirtualStaticRows = 5; -namespace internal { /** * The CallRecordImplementor implements the CallRecord interface for a Derived class by * delegating to its corresponding (templated) non-virtual methods. @@ -193,5 +180,4 @@ private: }; } // namespace internal - } // gtsam diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h new file mode 100644 index 000000000..315261293 --- /dev/null +++ b/gtsam/nonlinear/internal/ExecutionTrace.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 ExecutionTrace.h + * @date May 11, 2015 + * @author Frank Dellaert + * @brief Execution trace for expressions + */ + +#pragma once +#include // Configuration from CMake +#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) { + 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..928387eb9 --- /dev/null +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -0,0 +1,551 @@ +/* ---------------------------------------------------------------------------- + + * 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() { + } + + /// Print + virtual void print(const std::string& indent = "") const = 0; + + /// 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() { + } + + /// Print + virtual void print(const std::string& indent = "") const { + std::cout << indent << "Constant" << std::endl; + } + + /// 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() { + } + + /// Print + virtual void print(const std::string& indent = "") const { + std::cout << indent << "Leaf, key = " << key_ << std::endl; + } + + /// 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; +}; + +// Helper function for printing Jacobians with compact Eigen format, and trace +template +static void PrintJacobianAndTrace(const std::string& indent, + const typename Jacobian::type& dTdA, + const ExecutionTrace trace) { + static const Eigen::IOFormat kMatlabFormat(0, 1, " ", "; ", "", "", "[", "]"); + std::cout << indent << "D(" << typeid(T).name() << ")/D(" << typeid(A).name() + << ") = " << dTdA.format(kMatlabFormat) << std::endl; + trace.print(indent); +} + +//----------------------------------------------------------------------------- +/// 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() { + } + + /// Print + virtual void print(const std::string& indent = "") const { + std::cout << indent << "UnaryExpression" << std::endl; + expression1_->print(indent + " "); + } + + /// 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; + PrintJacobianAndTrace(indent, dTdA1, trace1); + 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); + + // We have written in the buffer, the next caller expects we advance the pointer + ptr += expression1_->traceSize(); + trace.setFunction(record); + + // Finally, the function call fills in the Jacobian dTdA1 + 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() { + } + + /// Print + virtual void print(const std::string& indent = "") const { + std::cout << indent << "BinaryExpression" << std::endl; + expression1_->print(indent + " "); + expression2_->print(indent + " "); + } + + /// 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; + PrintJacobianAndTrace(indent, dTdA1, trace1); + PrintJacobianAndTrace(indent, dTdA2, trace2); + 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); + ptr += expression1_->traceSize(); + record->value2 = expression2_->traceExecution(values, record->trace2, ptr); + ptr += 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() { + } + + /// Print + virtual void print(const std::string& indent = "") const { + std::cout << indent << "TernaryExpression" << std::endl; + expression1_->print(indent + " "); + expression2_->print(indent + " "); + expression3_->print(indent + " "); + } + + /// 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; + PrintJacobianAndTrace(indent, dTdA1, trace1); + PrintJacobianAndTrace(indent, dTdA2, trace2); + PrintJacobianAndTrace(indent, dTdA3, trace3); + 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); + ptr += expression1_->traceSize(); + record->value2 = expression2_->traceExecution(values, record->trace2, ptr); + ptr += expression2_->traceSize(); + record->value3 = expression3_->traceExecution(values, record->trace3, ptr); + ptr += 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..c99f05b7d --- /dev/null +++ b/gtsam/nonlinear/internal/JacobianMap.h @@ -0,0 +1,53 @@ +/* ---------------------------------------------------------------------------- + + * 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: + const KeyVector& keys_; + VerticalBlockMatrix& Ab_; + +public: + /// Construct a JacobianMap for writing into a VerticalBlockMatrix Ab + JacobianMap(const KeyVector& keys, VerticalBlockMatrix& Ab) : + keys_(keys), Ab_(Ab) { + } + + /// Access blocks of via key + VerticalBlockMatrix::Block operator()(Key key) { + KeyVector::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/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index f3858c818..377d6cd34 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -36,81 +36,43 @@ 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) { +struct Cal3Bundler0 : public Cal3Bundler { + Cal3Bundler0(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0, + double v0 = 0) + : Cal3Bundler(f, k1, k2, u0, v0) {} + Cal3Bundler0 retract(const Vector& d) const { + return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), 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 { + Vector3 localCoordinates(const Cal3Bundler0& T2) const { return T2.vector() - vector(); } }; -template<> -struct traits : public internal::Manifold {}; +template <> +struct traits : public internal::Manifold {}; // With that, camera below behaves like Snavely's 9-dim vector -typedef PinholeCamera Camera; - +typedef PinholeCamera Camera; } using namespace std; using namespace gtsam; /* ************************************************************************* */ -// 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)); +// Check that ceres rotation convention is the same +TEST(AdaptAutoDiff, Rotation) { + Vector3 axisAngle(0.1, 0.2, 0.3); + Matrix3 expected = Rot3::rodriguez(axisAngle).matrix(); + Matrix3 actual; + ceres::AngleAxisToRotationMatrix(axisAngle.data(), actual.data()); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ // Some Ceres Snippets copied for testing // Copyright 2010, 2011, 2012 Google Inc. All rights reserved. -template inline T &RowMajorAccess(T *base, int rows, int cols, - int i, int j) { +template +inline T& RowMajorAccess(T* base, int rows, int cols, int i, int j) { return base[cols * i + j]; } @@ -124,14 +86,14 @@ inline double RandDouble() { struct Projective { // Function that takes P and X as separate vectors: // P, X -> x - template + template bool operator()(A const P[12], A const X[4], A x[2]) const { A PX[3]; for (int i = 0; i < 3; ++i) { - PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0] - + RowMajorAccess(P, 3, 4, i, 1) * X[1] - + RowMajorAccess(P, 3, 4, i, 2) * X[2] - + RowMajorAccess(P, 3, 4, i, 3) * X[3]; + PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0] + + RowMajorAccess(P, 3, 4, i, 1) * X[1] + + RowMajorAccess(P, 3, 4, i, 2) * X[2] + + RowMajorAccess(P, 3, 4, i, 3) * X[3]; } if (PX[2] != 0.0) { x[0] = PX[0] / PX[2]; @@ -160,29 +122,31 @@ TEST(AdaptAutoDiff, AutoDiff) { Projective projective; // Make arguments - typedef Eigen::Matrix M; - M P; + typedef Eigen::Matrix RowMajorMatrix34; + RowMajorMatrix34 P; P << 1, 0, 0, 0, 0, 1, 0, 5, 0, 0, 1, 0; Vector4 X(10, 0, 5, 1); // Apply the mapping, to get image point b_x. Vector expected = Vector2(2, 1); Vector2 actual = projective(P, X); - EXPECT(assert_equal(expected,actual,1e-9)); + EXPECT(assert_equal(expected, actual, 1e-9)); // Get expected derivatives - Matrix E1 = numericalDerivative21(Projective(), P, X); - Matrix E2 = numericalDerivative22(Projective(), P, X); + Matrix E1 = numericalDerivative21( + Projective(), P, X); + Matrix E2 = numericalDerivative22( + Projective(), P, X); // Get derivatives with AutoDiff Vector2 actual2; MatrixRowMajor H1(2, 12), H2(2, 4); - double *parameters[] = { P.data(), X.data() }; - double *jacobians[] = { H1.data(), H2.data() }; - CHECK( - (AutoDiff::Differentiate( projective, parameters, 2, actual2.data(), jacobians))); - EXPECT(assert_equal(E1,H1,1e-8)); - EXPECT(assert_equal(E2,H2,1e-8)); + double* parameters[] = {P.data(), X.data()}; + double* jacobians[] = {H1.data(), H2.data()}; + CHECK((AutoDiff::Differentiate( + projective, parameters, 2, actual2.data(), jacobians))); + EXPECT(assert_equal(E1, H1, 1e-8)); + EXPECT(assert_equal(E2, H2, 1e-8)); } /* ************************************************************************* */ @@ -197,77 +161,85 @@ Vector2 adapted(const Vector9& P, const Vector3& X) { throw std::runtime_error("Snavely fail"); } +/* ************************************************************************* */ +namespace example { +Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)), + Cal3Bundler0(1, 0, 0)); +Point3 point(10, 0, -5); // negative Z-axis convention of Snavely! +Vector9 P = Camera().localCoordinates(camera); +Vector3 X = Point3::Logmap(point); +Vector2 expectedMeasurement(1.2431567, 1.2525694); +Matrix E1 = numericalDerivative21(adapted, P, X); +Matrix E2 = numericalDerivative22(adapted, P, X); +} + +/* ************************************************************************* */ +// Check that Local worked as expected +TEST(AdaptAutoDiff, Local) { + using namespace example; + Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0, 5, 0, 1, 0, 0).finished(); + EXPECT(equal_with_abs_tol(expectedP, P)); + Vector3 expectedX(10, 0, -5); // negative Z-axis convention of Snavely! + EXPECT(equal_with_abs_tol(expectedX, X)); +} + +/* ************************************************************************* */ +// Test Ceres AutoDiff TEST(AdaptAutoDiff, AutoDiff2) { + using namespace example; using ceres::internal::AutoDiff; + // Apply the mapping, to get image point b_x. + Vector2 actual = adapted(P, X); + EXPECT(assert_equal(expectedMeasurement, actual, 1e-6)); + // Instantiate function SnavelyProjection snavely; - // Make arguments - Vector9 P; // zero rotation, (0,5,0) translation, focal length 1 - P << 0, 0, 0, 0, 5, 0, 1, 0, 0; - Vector3 X(10, 0, -5); // negative Z-axis convention of Snavely! - - // Apply the mapping, to get image point b_x. - Vector expected = Vector2(2, 1); - Vector2 actual = adapted(P, X); - EXPECT(assert_equal(expected,actual,1e-9)); - - // Get expected derivatives - Matrix E1 = numericalDerivative21(adapted, P, X); - Matrix E2 = numericalDerivative22(adapted, P, X); - // Get derivatives with AutoDiff Vector2 actual2; MatrixRowMajor H1(2, 9), H2(2, 3); - double *parameters[] = { P.data(), X.data() }; - double *jacobians[] = { H1.data(), H2.data() }; - CHECK( - (AutoDiff::Differentiate( snavely, parameters, 2, actual2.data(), jacobians))); - EXPECT(assert_equal(E1,H1,1e-8)); - EXPECT(assert_equal(E2,H2,1e-8)); + double* parameters[] = {P.data(), X.data()}; + double* jacobians[] = {H1.data(), H2.data()}; + CHECK((AutoDiff::Differentiate( + snavely, parameters, 2, actual2.data(), jacobians))); + EXPECT(assert_equal(E1, H1, 1e-8)); + EXPECT(assert_equal(E2, H2, 1e-8)); } /* ************************************************************************* */ // Test AutoDiff wrapper Snavely -TEST(AdaptAutoDiff, AutoDiff3) { +TEST(AdaptAutoDiff, AdaptAutoDiff) { + using namespace example; - // Make arguments - 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; + typedef AdaptAutoDiff Adaptor; Adaptor snavely; // Apply the mapping, to get image point b_x. - Point2 expected(2, 1); - Point2 actual = snavely(P, X); - EXPECT(assert_equal(expected,actual,1e-9)); - -// // Get expected derivatives - Matrix E1 = numericalDerivative21(Adaptor(), P, X); - Matrix E2 = numericalDerivative22(Adaptor(), P, X); + Vector2 actual = snavely(P, X); + EXPECT(assert_equal(expectedMeasurement, actual, 1e-6)); // Get derivatives with AutoDiff, not gives RowMajor results! Matrix29 H1; Matrix23 H2; - 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)); + Vector2 actual2 = snavely(P, X, H1, H2); + EXPECT(assert_equal(expectedMeasurement, actual2, 1e-6)); + EXPECT(assert_equal(E1, H1, 1e-8)); + EXPECT(assert_equal(E2, H2, 1e-8)); } /* ************************************************************************* */ // Test AutoDiff wrapper in an expression -TEST(AdaptAutoDiff, Snavely) { - Expression P(1); - Expression X(2); - typedef AdaptAutoDiff Adaptor; - Expression expression(Adaptor(), P, X); +TEST(AdaptAutoDiff, SnavelyExpression) { + Expression P(1); + Expression X(2); + typedef AdaptAutoDiff Adaptor; + Expression expression(Adaptor(), P, X); + EXPECT_LONGS_EQUAL( + sizeof(internal::BinaryExpression::Record), + expression.traceSize()); #ifdef GTSAM_USE_QUATERNIONS - EXPECT_LONGS_EQUAL(400,expression.traceSize()); // Todo, should be zero -#else - EXPECT_LONGS_EQUAL(432,expression.traceSize()); // Todo, should be zero + EXPECT_LONGS_EQUAL(336, expression.traceSize()); #endif set expected = list_of(1)(2); EXPECT(expected == expression.keys()); @@ -279,4 +251,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp index 483b5ffa9..208f0b284 100644 --- a/gtsam/nonlinear/tests/testCallRecord.cpp +++ b/gtsam/nonlinear/tests/testCallRecord.cpp @@ -18,7 +18,7 @@ * @brief unit tests for CallRecord class */ -#include +#include #include #include @@ -32,7 +32,7 @@ static const int Cols = 3; int dynamicIfAboveMax(int i){ - if(i > CallRecordMaxVirtualStaticRows){ + if(i > internal::CallRecordMaxVirtualStaticRows){ return Eigen::Dynamic; } else return i; @@ -80,13 +80,13 @@ struct Record: public internal::CallRecordImplementor { } void print(const std::string& indent) const { } - void startReverseAD4(JacobianMap& jacobians) const { + void startReverseAD4(internal::JacobianMap& jacobians) const { } mutable CallConfig cc; private: template - void reverseAD4(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(); @@ -97,7 +97,7 @@ struct Record: public internal::CallRecordImplementor { friend struct internal::CallRecordImplementor; }; -JacobianMap & NJM= *static_cast(NULL); +internal::JacobianMap & NJM= *static_cast(NULL); /* ************************************************************************* */ typedef Eigen::Matrix DynRowMat; 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/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 80100af5e..75af5f634 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -42,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); @@ -68,24 +68,27 @@ 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 f0(const Point3& p, OptionalJacobian<2,3> H) { +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, Unary0) { +TEST(Expression, Unary1) { using namespace unary; - Expression e(f0, p); + Expression e(f1, p); EXPECT(expected == e.keys()); } TEST(Expression, Unary2) { @@ -94,6 +97,12 @@ TEST(Expression, Unary2) { EXPECT(expected == e.keys()); } /* ************************************************************************* */ +// Unary(Leaf), dynamic +TEST(Expression, Unary3) { + using namespace unary; +// Expression e(f3, p); +} +/* ************************************************************************* */ //Nullary Method TEST(Expression, NullaryMethod) { @@ -118,7 +127,7 @@ TEST(Expression, NullaryMethod) { 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])); + EXPECT(assert_equal(expected, H[0])); } /* ************************************************************************* */ // Binary(Leaf,Leaf) @@ -149,12 +158,12 @@ TEST(Expression, BinaryKeys) { TEST(Expression, BinaryDimensions) { 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()); } @@ -166,7 +175,8 @@ using namespace binary; Expression K(3); // Create expression tree -Expression projection(PinholeCamera::project_to_camera, p_cam); +Point2 (*f)(const Point3&, OptionalJacobian<2, 3>) = &PinholeBase::Project; +Expression projection(f, p_cam); Expression uv_hat(uncalibrate, K, projection); } /* ************************************************************************* */ @@ -180,17 +190,26 @@ TEST(Expression, TreeKeys) { TEST(Expression, TreeDimensions) { 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()); } /* ************************************************************************* */ @@ -234,7 +253,8 @@ TEST(Expression, compose3) { /* ************************************************************************* */ // Test with ternary function Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, - OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> 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/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index 2bdc04d5b..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 diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index faa285cd5..dfc14e1e6 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -26,6 +26,7 @@ #include #include // for operator += +#include #include using namespace boost::assign; #include @@ -308,12 +309,12 @@ TEST(Values, extract_keys) config.insert(key3, Pose2()); config.insert(key4, Pose2()); - FastList expected, actual; + KeyVector expected, actual; expected += key1, key2, key3, key4; actual = config.keys(); CHECK(actual.size() == expected.size()); - FastList::const_iterator itAct = actual.begin(), itExp = expected.begin(); + KeyVector::const_iterator itAct = actual.begin(), itExp = expected.begin(); for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) { EXPECT(*itExp == *itAct); } diff --git a/gtsam/sam/BearingFactor.h b/gtsam/sam/BearingFactor.h new file mode 100644 index 000000000..f190e683c --- /dev/null +++ b/gtsam/sam/BearingFactor.h @@ -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 + + * -------------------------------------------------------------------------- */ + +/** + * @file BearingFactor.h + * @brief Serializable factor induced by a bearing measurement + * @date July 2015 + * @author Frank Dellaert + **/ + +#pragma once + +#include + +namespace gtsam { + +// forward declaration of Bearing functor, assumed partially specified +template +struct Bearing; + +/** + * Binary factor for a bearing measurement + * Works for any two types A1,A2 for which the functor Bearing() is + * defined + * @addtogroup SAM + */ +template ::result_type> +struct BearingFactor : public ExpressionFactor2 { + typedef ExpressionFactor2 Base; + + /// default constructor + BearingFactor() {} + + /// primary constructor + BearingFactor(Key key1, Key key2, const T& measured, + const SharedNoiseModel& model) + : Base(key1, key2, model, measured) { + this->initialize(expression(key1, key2)); + } + + // Return measurement expression + virtual Expression expression(Key key1, Key key2) const { + Expression a1_(key1); + Expression a2_(key2); + return Expression(Bearing(), a1_, a2_); + } + + /// print + void print(const std::string& s = "", + const KeyFormatter& kf = DefaultKeyFormatter) const { + std::cout << s << "BearingFactor" << std::endl; + Base::print(s, kf); + } +}; // BearingFactor + +/// traits +template +struct traits > + : public Testable > {}; + +} // namespace gtsam diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h new file mode 100644 index 000000000..2dd1fecb8 --- /dev/null +++ b/gtsam/sam/BearingRangeFactor.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 BearingRangeFactor.h + * @date Apr 1, 2010 + * @author Kai Ni + * @brief a single factor contains both the bearing and the range to prevent + * handle to pair bearing and range factors + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * Binary factor for a bearing/range measurement + * @addtogroup SLAM + */ +template ::result_type, + typename R = typename Range::result_type> +class BearingRangeFactor + : public ExpressionFactor2, A1, A2> { + private: + typedef BearingRange T; + typedef ExpressionFactor2 Base; + typedef BearingRangeFactor This; + + public: + typedef boost::shared_ptr shared_ptr; + + /// default constructor + BearingRangeFactor() {} + + /// primary constructor + BearingRangeFactor(Key key1, Key key2, const B& measuredBearing, + const R& measuredRange, const SharedNoiseModel& model) + : Base(key1, key2, model, T(measuredBearing, measuredRange)) { + this->initialize(expression(key1, key2)); + } + + virtual ~BearingRangeFactor() {} + + /// @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 measurement expression + virtual Expression expression(Key key1, Key key2) const { + return Expression(T::Measure, Expression(key1), + Expression(key2)); + } + + /// print + virtual void print(const std::string& s = "", + const KeyFormatter& kf = DefaultKeyFormatter) const { + std::cout << s << "BearingRangeFactor" << std::endl; + Base::print(s, kf); + } + +}; // BearingRangeFactor + +/// traits +template +struct traits > + : public Testable > {}; + +} // namespace gtsam diff --git a/gtsam/sam/CMakeLists.txt b/gtsam/sam/CMakeLists.txt new file mode 100644 index 000000000..bf20b751c --- /dev/null +++ b/gtsam/sam/CMakeLists.txt @@ -0,0 +1,6 @@ +# Install headers +file(GLOB sam_headers "*.h") +install(FILES ${sam_headers} DESTINATION include/gtsam/sam) + +# Build tests +add_subdirectory(tests) diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h new file mode 100644 index 000000000..1ad27865d --- /dev/null +++ b/gtsam/sam/RangeFactor.h @@ -0,0 +1,142 @@ +/* ---------------------------------------------------------------------------- + + * 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 RangeFactor.h + * @brief Serializable factor induced by a range measurement + * @date July 2015 + * @author Frank Dellaert + **/ + +#pragma once + +#include + +namespace gtsam { + +// forward declaration of Range functor, assumed partially specified +template +struct Range; + +/** + * Binary factor for a range measurement + * Works for any two types A1,A2 for which the functor Range() is defined + * @addtogroup SAM + */ +template ::result_type> +class RangeFactor : public ExpressionFactor2 { + private: + typedef RangeFactor This; + typedef ExpressionFactor2 Base; + + public: + /// default constructor + RangeFactor() {} + + RangeFactor(Key key1, Key key2, T measured, const SharedNoiseModel& model) + : Base(key1, key2, model, measured) { + this->initialize(expression(key1, key2)); + } + + /// @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 measurement expression + virtual Expression expression(Key key1, Key key2) const { + Expression a1_(key1); + Expression a2_(key2); + return Expression(Range(), a1_, a2_); + } + + /// print + void print(const std::string& s = "", + const KeyFormatter& kf = DefaultKeyFormatter) const { + std::cout << s << "RangeFactor" << std::endl; + Base::print(s, kf); + } +}; // \ RangeFactor + +/// traits +template +struct traits > + : public Testable > {}; + +/** + * Binary factor for a range measurement, with a transform applied + * @addtogroup SAM + */ +template ::result_type> +class RangeFactorWithTransform : public ExpressionFactor2 { + private: + typedef RangeFactorWithTransform This; + typedef ExpressionFactor2 Base; + + A1 body_T_sensor_; ///< The pose of the sensor in the body frame + + public: + //// Default constructor + RangeFactorWithTransform() {} + + RangeFactorWithTransform(Key key1, Key key2, T measured, + const SharedNoiseModel& model, + const A1& body_T_sensor) + : Base(key1, key2, model, measured), body_T_sensor_(body_T_sensor) { + this->initialize(expression(key1, key2)); + } + + 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))); + } + + // Return measurement expression + virtual Expression expression(Key key1, Key key2) const { + Expression body_T_sensor__(body_T_sensor_); + Expression nav_T_body_(key1); + Expression nav_T_sensor_(traits::Compose, nav_T_body_, + body_T_sensor__); + Expression a2_(key2); + return Expression(Range(), nav_T_sensor_, a2_); + } + + /** print contents */ + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "RangeFactorWithTransform" << std::endl; + this->body_T_sensor_.print(" sensor pose in body frame: "); + Base::print(s, keyFormatter); + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Base", boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(body_T_sensor_); + } +}; // \ RangeFactorWithTransform + +/// traits +template +struct traits > + : public Testable > {}; + +} // \ namespace gtsam diff --git a/gtsam/sam/tests/CMakeLists.txt b/gtsam/sam/tests/CMakeLists.txt new file mode 100644 index 000000000..127c619ee --- /dev/null +++ b/gtsam/sam/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(sam "test*.cpp" "" "gtsam") diff --git a/gtsam/sam/tests/testBearingFactor.cpp b/gtsam/sam/tests/testBearingFactor.cpp new file mode 100644 index 000000000..12635a7e5 --- /dev/null +++ b/gtsam/sam/tests/testBearingFactor.cpp @@ -0,0 +1,117 @@ +/* ---------------------------------------------------------------------------- + + * 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 testBearingFactor.cpp + * @brief Unit tests for BearingFactor Class + * @author Frank Dellaert + * @date July 2015 + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +Key poseKey(1); +Key pointKey(2); + +typedef BearingFactor BearingFactor2D; +double measurement2D(10.0); +static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(1, 0.5)); +BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model2D); + +typedef BearingFactor BearingFactor3D; +Unit3 measurement3D = Pose3().bearing(Point3(1, 0, 0)); // has to match values! +static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(2, 0.5)); +BearingFactor3D factor3D(poseKey, pointKey, measurement3D, model3D); + +/* ************************************************************************* */ +// Export Noisemodels +// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html +BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic); + +/* ************************************************************************* */ +TEST(BearingFactor, Serialization2D) { + EXPECT(serializationTestHelpers::equalsObj(factor2D)); + EXPECT(serializationTestHelpers::equalsXML(factor2D)); + EXPECT(serializationTestHelpers::equalsBinary(factor2D)); +} + +/* ************************************************************************* */ +TEST(BearingFactor, 2D) { + // Serialize the factor + std::string serialized = serializeXML(factor2D); + + // And de-serialize it + BearingFactor2D factor; + deserializeXML(serialized, factor); + + // Set the linearization point + Values values; + values.insert(poseKey, Pose2(1.0, 2.0, 0.57)); + values.insert(pointKey, Point2(-4.0, 11.0)); + + EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey), + values, 1e-7, 1e-5); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +TEST(BearingFactor, Serialization3D) { + EXPECT(serializationTestHelpers::equalsObj(factor3D)); + EXPECT(serializationTestHelpers::equalsXML(factor3D)); + EXPECT(serializationTestHelpers::equalsBinary(factor3D)); +} + +/* ************************************************************************* */ +// TODO(frank): this test is disabled (for now) because the macros below are +// incompatible with the Unit3 localCoordinates. The issue is the following: +// For factors, we want to use Local(value, measured), because we need the error +// to be expressed in the tangent space of value. This surfaced in the Unit3 case +// where the tangent space can be radically didfferent from one value to the next. +// For derivatives, we want Local(constant, varying), because we need a derivative +// in a constant tangent space. But since the macros below call whitenedError +// which calls Local(value,measured), we actually call the reverse. This does not +// matter for types with a commutative Local, but matters a lot for Unit3. +// More thinking needed about what the right thing is, here... +//TEST(BearingFactor, 3D) { +// // Serialize the factor +// std::string serialized = serializeXML(factor3D); +// +// // And de-serialize it +// BearingFactor3D factor; +// deserializeXML(serialized, factor); +// +// // Set the linearization point +// Values values; +// values.insert(poseKey, Pose3()); +// values.insert(pointKey, Point3(1, 0, 0)); +// +// EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey), +// values, 1e-7, 1e-5); +// EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +//} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/sam/tests/testBearingRangeFactor.cpp b/gtsam/sam/tests/testBearingRangeFactor.cpp new file mode 100644 index 000000000..4c7a9ab91 --- /dev/null +++ b/gtsam/sam/tests/testBearingRangeFactor.cpp @@ -0,0 +1,108 @@ +/* ---------------------------------------------------------------------------- + + * 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 testBearingRangeFactor.cpp + * @brief Unit tests for BearingRangeFactor Class + * @author Frank Dellaert + * @date July 2015 + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +Key poseKey(1); +Key pointKey(2); + +typedef BearingRangeFactor BearingRangeFactor2D; +static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(2, 0.5)); +BearingRangeFactor2D factor2D(poseKey, pointKey, 1, 2, model2D); + +typedef BearingRangeFactor BearingRangeFactor3D; +static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5)); +BearingRangeFactor3D factor3D(poseKey, pointKey, + Pose3().bearing(Point3(1, 0, 0)), 1, model3D); + +/* ************************************************************************* */ +// Export Noisemodels +// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html +BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic); + +/* ************************************************************************* */ +TEST(BearingRangeFactor, Serialization2D) { + EXPECT(serializationTestHelpers::equalsObj(factor2D)); + EXPECT(serializationTestHelpers::equalsXML(factor2D)); + EXPECT(serializationTestHelpers::equalsBinary(factor2D)); +} + +/* ************************************************************************* */ +TEST(BearingRangeFactor, 2D) { + // Serialize the factor + std::string serialized = serializeXML(factor2D); + + // And de-serialize it + BearingRangeFactor2D factor; + deserializeXML(serialized, factor); + + // Set the linearization point + Values values; + values.insert(poseKey, Pose2(1.0, 2.0, 0.57)); + values.insert(pointKey, Point2(-4.0, 11.0)); + + EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey), + values, 1e-7, 1e-5); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +TEST(BearingRangeFactor, Serialization3D) { + EXPECT(serializationTestHelpers::equalsObj(factor3D)); + EXPECT(serializationTestHelpers::equalsXML(factor3D)); + EXPECT(serializationTestHelpers::equalsBinary(factor3D)); +} + +/* ************************************************************************* */ +// TODO(frank): this test is disabled (for now) because the macros below are +// incompatible with the Unit3 localCoordinates. See testBearingFactor... +//TEST(BearingRangeFactor, 3D) { +// // Serialize the factor +// std::string serialized = serializeXML(factor3D); +// +// // And de-serialize it +// BearingRangeFactor3D factor; +// deserializeXML(serialized, factor); +// +// // Set the linearization point +// Values values; +// values.insert(poseKey, Pose3()); +// values.insert(pointKey, Point3(1, 0, 0)); +// +// EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey), +// values, 1e-7, 1e-5); +// EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +//} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp similarity index 79% rename from gtsam/slam/tests/testRangeFactor.cpp rename to gtsam/sam/tests/testRangeFactor.cpp index a8455d685..706c20e78 100644 --- a/gtsam/slam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -16,14 +16,15 @@ * @date Oct 2012 */ -#include -#include +#include #include -#include #include -#include +#include #include +#include #include + +#include #include using namespace std; @@ -37,6 +38,10 @@ typedef RangeFactor RangeFactor3D; typedef RangeFactorWithTransform RangeFactorWithTransform2D; typedef RangeFactorWithTransform RangeFactorWithTransform3D; +Key poseKey(1); +Key pointKey(2); +double measurement(10.0); + /* ************************************************************************* */ Vector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) { @@ -63,19 +68,33 @@ Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point, /* ************************************************************************* */ TEST( RangeFactor, Constructor) { - Key poseKey(1); - Key pointKey(2); - double measurement(10.0); - RangeFactor2D factor2D(poseKey, pointKey, measurement, model); RangeFactor3D factor3D(poseKey, pointKey, measurement, model); } +/* ************************************************************************* */ +// Export Noisemodels +// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html +BOOST_CLASS_EXPORT(gtsam::noiseModel::Unit); + +/* ************************************************************************* */ +TEST(RangeFactor, Serialization2D) { + RangeFactor2D factor2D(poseKey, pointKey, measurement, model); + EXPECT(serializationTestHelpers::equalsObj(factor2D)); + EXPECT(serializationTestHelpers::equalsXML(factor2D)); + EXPECT(serializationTestHelpers::equalsBinary(factor2D)); +} + +/* ************************************************************************* */ +TEST(RangeFactor, Serialization3D) { + RangeFactor3D factor3D(poseKey, pointKey, measurement, model); + EXPECT(serializationTestHelpers::equalsObj(factor3D)); + EXPECT(serializationTestHelpers::equalsXML(factor3D)); + EXPECT(serializationTestHelpers::equalsBinary(factor3D)); +} + /* ************************************************************************* */ TEST( RangeFactor, ConstructorWithTransform) { - Key poseKey(1); - 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)); @@ -89,10 +108,6 @@ TEST( RangeFactor, ConstructorWithTransform) { /* ************************************************************************* */ TEST( RangeFactor, Equals ) { // Create two identical factors and make sure they're equal - Key poseKey(1); - Key pointKey(2); - double measurement(10.0); - RangeFactor2D factor2D_1(poseKey, pointKey, measurement, model); RangeFactor2D factor2D_2(poseKey, pointKey, measurement, model); CHECK(assert_equal(factor2D_1, factor2D_2)); @@ -105,9 +120,6 @@ TEST( RangeFactor, Equals ) { /* ************************************************************************* */ TEST( RangeFactor, EqualsWithTransform ) { // Create two identical factors and make sure they're equal - Key poseKey(1); - 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)); @@ -128,9 +140,6 @@ TEST( RangeFactor, EqualsWithTransform ) { /* ************************************************************************* */ TEST( RangeFactor, Error2D ) { // Create a factor - Key poseKey(1); - Key pointKey(2); - double measurement(10.0); RangeFactor2D factor(poseKey, pointKey, measurement, model); // Set the linearization point @@ -150,9 +159,6 @@ TEST( RangeFactor, Error2D ) { /* ************************************************************************* */ TEST( RangeFactor, Error2DWithTransform ) { // Create a factor - Key poseKey(1); - Key pointKey(2); - double measurement(10.0); Pose2 body_P_sensor(0.25, -0.10, -M_PI_2); RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model, body_P_sensor); @@ -176,9 +182,6 @@ TEST( RangeFactor, Error2DWithTransform ) { /* ************************************************************************* */ TEST( RangeFactor, Error3D ) { // Create a factor - Key poseKey(1); - Key pointKey(2); - double measurement(10.0); RangeFactor3D factor(poseKey, pointKey, measurement, model); // Set the linearization point @@ -198,9 +201,6 @@ TEST( RangeFactor, Error3D ) { /* ************************************************************************* */ TEST( RangeFactor, Error3DWithTransform ) { // Create a factor - 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)); RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model, @@ -225,9 +225,6 @@ TEST( RangeFactor, Error3DWithTransform ) { /* ************************************************************************* */ TEST( RangeFactor, Jacobian2D ) { // Create a factor - Key poseKey(1); - Key pointKey(2); - double measurement(10.0); RangeFactor2D factor(poseKey, pointKey, measurement, model); // Set the linearization point @@ -253,9 +250,6 @@ TEST( RangeFactor, Jacobian2D ) { /* ************************************************************************* */ TEST( RangeFactor, Jacobian2DWithTransform ) { // Create a factor - Key poseKey(1); - Key pointKey(2); - double measurement(10.0); Pose2 body_P_sensor(0.25, -0.10, -M_PI_2); RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model, body_P_sensor); @@ -285,9 +279,6 @@ TEST( RangeFactor, Jacobian2DWithTransform ) { /* ************************************************************************* */ TEST( RangeFactor, Jacobian3D ) { // Create a factor - Key poseKey(1); - Key pointKey(2); - double measurement(10.0); RangeFactor3D factor(poseKey, pointKey, measurement, model); // Set the linearization point @@ -313,9 +304,6 @@ TEST( RangeFactor, Jacobian3D ) { /* ************************************************************************* */ TEST( RangeFactor, Jacobian3DWithTransform ) { // Create a factor - 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)); RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model, @@ -343,6 +331,64 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { CHECK(assert_equal(H2Expected, H2Actual, 1e-9)); } +/* ************************************************************************* */ +// Do a test with Point3 +TEST(RangeFactor, Point3) { + // Create a factor + RangeFactor factor(poseKey, pointKey, measurement, model); + + // Set the linearization point + Point3 pose(1.0, 2.0, 00); + Point3 point(-4.0, 11.0, 0); + + // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance + Vector expectedError = (Vector(1) << 0.295630141).finished(); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, factor.evaluateError(pose, point), 1e-9)); +} + +/* ************************************************************************* */ +// Do tests with SimpleCamera +TEST( RangeFactor, Camera) { + RangeFactor factor1(poseKey, pointKey, measurement, model); + RangeFactor factor2(poseKey, pointKey, measurement, model); + RangeFactor factor3(poseKey, pointKey, measurement, model); +} + +/* ************************************************************************* */ +// Do a test with non GTSAM types + +namespace gtsam{ +template <> +struct Range { + typedef double result_type; + double operator()(const Vector3& v1, const Vector3& v2, + OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) { + return (v2 - v1).norm(); + // derivatives not implemented + } +}; +} + +TEST(RangeFactor, NonGTSAM) { + // Create a factor + Key poseKey(1); + Key pointKey(2); + double measurement(10.0); + RangeFactor factor(poseKey, pointKey, measurement, model); + + // Set the linearization point + Vector3 pose(1.0, 2.0, 00); + Vector3 point(-4.0, 11.0, 0); + + // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance + Vector expectedError = (Vector(1) << 0.295630141).finished(); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, factor.evaluateError(pose, point), 1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/sfm/CMakeLists.txt b/gtsam/sfm/CMakeLists.txt new file mode 100644 index 000000000..fde997840 --- /dev/null +++ b/gtsam/sfm/CMakeLists.txt @@ -0,0 +1,6 @@ +# Install headers +file(GLOB sfm_headers "*.h") +install(FILES ${sfm_headers} DESTINATION include/gtsam/sfm) + +# Build tests +add_subdirectory(tests) diff --git a/gtsam/sfm/tests/CMakeLists.txt b/gtsam/sfm/tests/CMakeLists.txt new file mode 100644 index 000000000..22245dffe --- /dev/null +++ b/gtsam/sfm/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(sfm "test*.cpp" "" "gtsam") 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..1a1fac922 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -9,94 +9,13 @@ * -------------------------------------------------------------------------- */ -/** - * @file BearingFactor.H - * @author Frank Dellaert - **/ - #pragma once -#include -#include -#include +#ifdef _MSC_VER +#pragma message("BearingFactor is now an ExpressionFactor in SAM directory") +#else +#warning "BearingFactor is now an ExpressionFactor in SAM directory" +#endif -namespace gtsam { - /** - * Binary factor for a bearing measurement - * @addtogroup SLAM - */ - template - class BearingFactor: public NoiseModelFactor2 { - private: - - typedef POSE Pose; - typedef ROTATION Rot; - typedef POINT Point; - - typedef BearingFactor This; - typedef NoiseModelFactor2 Base; - - Rot measured_; /** measurement */ - - /** concept check by type */ - GTSAM_CONCEPT_TESTABLE_TYPE(Rot) - GTSAM_CONCEPT_POSE_TYPE(Pose) - - public: - - /** default constructor for serialization/testing only */ - BearingFactor() {} - - /** primary constructor */ - BearingFactor(Key poseKey, Key pointKey, const Rot& measured, - const SharedNoiseModel& model) : - Base(model, poseKey, pointKey), measured_(measured) { - } - - virtual ~BearingFactor() {} - - /// @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 -> between(z,h(x)) for Rot2 manifold */ - Vector evaluateError(const Pose& pose, const Point& point, - boost::optional H1, boost::optional H2) const { - Rot hx = pose.bearing(point, H1, H2); - return Rot::Logmap(measured_.between(hx)); - } - - /** return the measured */ - const Rot& measured() const { - return measured_; - } - - /** 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); - } - - /** print contents */ - void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "BearingFactor, bearing = "; - measured_.print(); - 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_); - } - - }; // BearingFactor - -} // namespace gtsam +#include diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index ef02b5cb1..901860015 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -9,116 +9,13 @@ * -------------------------------------------------------------------------- */ -/** - * @file BearingRangeFactor.h - * @date Apr 1, 2010 - * @author Kai Ni - * @brief a single factor contains both the bearing and the range to prevent handle to pair bearing and range factors - */ - #pragma once -#include -#include -#include +#ifdef _MSC_VER +#pragma message( \ + "BearingRangeFactor is now an ExpressionFactor in SAM directory") +#else +#warning "BearingRangeFactor is now an ExpressionFactor in SAM directory" +#endif -namespace gtsam { - - /** - * Binary factor for a bearing measurement - * @addtogroup SLAM - */ - template - class BearingRangeFactor: public NoiseModelFactor2 - { - public: - typedef BearingRangeFactor This; - typedef NoiseModelFactor2 Base; - typedef boost::shared_ptr shared_ptr; - - private: - typedef POSE Pose; - typedef ROTATION Rot; - typedef POINT Point; - - // the measurement - Rot measuredBearing_; - double measuredRange_; - - /** concept check by type */ - GTSAM_CONCEPT_TESTABLE_TYPE(Rot) - GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(Pose, Point) - GTSAM_CONCEPT_POSE_TYPE(Pose) - - public: - - BearingRangeFactor() {} /* Default constructor */ - BearingRangeFactor(Key poseKey, Key pointKey, const Rot& measuredBearing, const double measuredRange, - const SharedNoiseModel& model) : - Base(model, poseKey, pointKey), measuredBearing_(measuredBearing), measuredRange_(measuredRange) { - } - - virtual ~BearingRangeFactor() {} - - /// @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))); } - - /** Print */ - virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "BearingRangeFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << ")\n"; - measuredBearing_.print("measured bearing: "); - std::cout << "measured range: " << measuredRange_ << std::endl; - this->noiseModel_->print("noise model:\n"); - } - - /** 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) && - fabs(this->measuredRange_ - e->measuredRange_) < tol && - this->measuredBearing_.equals(e->measuredBearing_, tol); - } - - /** h(x)-z -> between(z,h(x)) for Rot manifold */ - Vector evaluateError(const Pose& pose, const Point& point, - boost::optional H1, boost::optional H2) const { - Matrix H11, H21, H12, H22; - boost::optional H11_ = H1 ? boost::optional(H11) : boost::optional(); - boost::optional H21_ = H1 ? boost::optional(H21) : boost::optional(); - boost::optional H12_ = H2 ? boost::optional(H12) : boost::optional(); - boost::optional H22_ = H2 ? boost::optional(H22) : boost::optional(); - - Rot y1 = pose.bearing(point, H11_, H12_); - Vector e1 = Rot::Logmap(measuredBearing_.between(y1)); - - double y2 = pose.range(point, H21_, H22_); - Vector e2 = (Vector(1) << y2 - measuredRange_).finished(); - - if (H1) *H1 = gtsam::stack(2, &H11, &H21); - if (H2) *H2 = gtsam::stack(2, &H12, &H22); - return concatVectors(2, &e1, &e2); - } - - /** return the measured */ - const std::pair measured() const { - return std::make_pair(measuredBearing_, measuredRange_); - } - - 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(measuredBearing_); - ar & BOOST_SERIALIZATION_NVP(measuredRange_); - } - }; // BearingRangeFactor - -} // namespace gtsam +#include diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 319c74cd5..bfd7d4e34 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -117,7 +117,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_); @@ -149,7 +149,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("BetweenFactor", boost::serialization::base_object >(*this)); } 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/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 9d4a8e6e5..da22225e5 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -173,7 +173,7 @@ public: Point3 _1T2 = E.direction().point3(); Point3 d1T2 = d * _1T2; Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2) - pn = SimpleCamera::project_to_camera(dP2); + pn = PinholeBase::Project(dP2); } else { @@ -186,7 +186,7 @@ public: Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point); Matrix23 Dpn_dP2; - pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2); + pn = PinholeBase::Project(dP2, Dpn_dP2); if (DE) { Matrix DdP2_E(3, 5); diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 2f8dd601f..d2b042fed 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -20,218 +20,280 @@ #pragma once -#include -#include #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include #include +#include + +namespace boost { +namespace serialization { +class access; +} /* namespace serialization */ +} /* namespace boost */ namespace gtsam { - /** - * Non-linear factor for a constraint derived from a 2D measurement. - * The calibration is unknown here compared to GenericProjectionFactor - * @addtogroup SLAM - */ - template - class GeneralSFMFactor: public NoiseModelFactor2 { +/** + * Non-linear factor for a constraint derived from a 2D measurement. + * The calibration is unknown here compared to GenericProjectionFactor + * @addtogroup SLAM + */ +template +class GeneralSFMFactor: public NoiseModelFactor2 { - GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA) - GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK) + GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA); + GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK); - static const int DimC = FixedDimension::value; - static const int DimL = FixedDimension::value; + static const int DimC = FixedDimension::value; + static const int DimL = FixedDimension::value; + typedef Eigen::Matrix JacobianC; + typedef Eigen::Matrix JacobianL; - protected: +protected: - Point2 measured_; ///< the 2D measurement + Point2 measured_; ///< the 2D measurement - public: +public: - typedef GeneralSFMFactor This; ///< typedef for this object - typedef NoiseModelFactor2 Base; ///< typedef for the base class + typedef GeneralSFMFactor This;///< typedef for this object + typedef NoiseModelFactor2 Base;///< typedef for the base class - // shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; - - /** - * Constructor - * @param measured is the 2 dimensional location of point in image (the measurement) - * @param model is the standard deviation of the measurements - * @param cameraKey is the index of the camera - * @param landmarkKey is the index of the landmark - */ - GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) : - Base(model, cameraKey, landmarkKey), measured_(measured) {} - - GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor - GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2 - GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2 - - virtual ~GeneralSFMFactor() {} ///< destructor - - /// @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))); } - - /** - * print - * @param s optional string naming the factor - * @param keyFormatter optional formatter for printing out Symbols - */ - void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - Base::print(s, keyFormatter); - measured_.print(s + ".z"); - } - - /** - * equals - */ - bool equals(const NonlinearFactor &p, double tol = 1e-9) const { - const This* e = dynamic_cast(&p); - return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ; - } - - /** h(x)-z */ - Vector evaluateError(const CAMERA& camera, const LANDMARK& point, - boost::optional H1=boost::none, boost::optional H2=boost::none) const { - - try { - Point2 reprojError(camera.project2(point,H1,H2) - measured_); - return reprojError.vector(); - } - catch( CheiralityException& e) { - 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); - } - } - - /** return the measured */ - inline const Point2 measured() const { - return measured_; - } - - 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_); - } - }; - - template - struct traits > : Testable< - GeneralSFMFactor > { - }; + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; /** - * 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.. + * Constructor + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param model is the standard deviation of the measurements + * @param cameraKey is the index of the camera + * @param landmarkKey is the index of the landmark */ - template - class GeneralSFMFactor2: public NoiseModelFactor3 { + GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) : + Base(model, cameraKey, landmarkKey), measured_(measured) {} - GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) - static const int DimK = FixedDimension::value; + GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor + GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2 + GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2 - protected: + virtual ~GeneralSFMFactor() {} ///< destructor - Point2 measured_; ///< the 2D measurement + /// @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)));} - public: + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter for printing out Symbols + */ + void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s, keyFormatter); + measured_.print(s + ".z"); + } - typedef GeneralSFMFactor2 This; - typedef PinholeCamera Camera; ///< typedef for camera type - typedef NoiseModelFactor3 Base; ///< typedef for the base class + /** + * equals + */ + bool equals(const NonlinearFactor &p, double tol = 1e-9) const { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol); + } - // shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; - - /** - * Constructor - * @param measured is the 2 dimensional location of point in image (the measurement) - * @param model is the standard deviation of the measurements - * @param poseKey is the index of the camera - * @param landmarkKey is the index of the landmark - * @param calibKey is the index of the calibration - */ - GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) : - Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {} - GeneralSFMFactor2():measured_(0.0,0.0) {} ///< default constructor - - virtual ~GeneralSFMFactor2() {} ///< destructor - - /// @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))); } - - /** - * print - * @param s optional string naming the factor - * @param keyFormatter optional formatter useful for printing Symbols - */ - void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - Base::print(s, keyFormatter); - measured_.print(s + ".z"); + /** h(x)-z */ + Vector evaluateError(const CAMERA& camera, const LANDMARK& point, + boost::optional H1=boost::none, boost::optional H2=boost::none) const { + try { + Point2 reprojError(camera.project2(point,H1,H2) - measured_); + return reprojError.vector(); } - - /** - * equals - */ - bool equals(const NonlinearFactor &p, double tol = 1e-9) const { - const This* e = dynamic_cast(&p); - return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ; - } - - /** h(x)-z */ - Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib, - boost::optional H1=boost::none, - boost::optional H2=boost::none, - boost::optional H3=boost::none) const - { - try { - Camera camera(pose3,calib); - Point2 reprojError(camera.project(point, H1, H2, H3) - measured_); - return reprojError.vector(); - } - catch( CheiralityException& e) { - 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; - } + catch( CheiralityException& e) { + if (H1) *H1 = JacobianC::Zero(); + if (H2) *H2 = JacobianL::Zero(); + // TODO warn if verbose output asked for return zero(2); } + } - /** return the measured */ - inline const Point2 measured() const { - return measured_; + /// Linearize using fixed-size matrices + boost::shared_ptr linearize(const Values& values) const { + // Only linearize if the factor is active + if (!this->active(values)) return boost::shared_ptr(); + + const Key key1 = this->key1(), key2 = this->key2(); + JacobianC H1; + JacobianL H2; + Vector2 b; + try { + const CAMERA& camera = values.at(key1); + const LANDMARK& point = values.at(key2); + Point2 reprojError(camera.project2(point, H1, H2) - measured()); + b = -reprojError.vector(); + } catch (CheiralityException& e) { + H1.setZero(); + H2.setZero(); + b.setZero(); + // TODO warn if verbose output asked for } - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor3", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); + // Whiten the system if needed + const SharedNoiseModel& noiseModel = this->get_noiseModel(); + if (noiseModel && !noiseModel->isUnit()) { + // TODO: implement WhitenSystem for fixed size matrices and include + // above + H1 = noiseModel->Whiten(H1); + H2 = noiseModel->Whiten(H2); + b = noiseModel->Whiten(b); } - }; - template - struct traits > : Testable< - GeneralSFMFactor2 > { - }; + // Create new (unit) noiseModel, preserving constraints if applicable + SharedDiagonal model; + if (noiseModel && noiseModel->isConstrained()) { + model = boost::static_pointer_cast(noiseModel)->unit(); + } + + return boost::make_shared >(key1, H1, key2, H2, b, model); + } + + /** return the measured */ + inline const Point2 measured() const { + return measured_; + } + +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_); + } +}; + +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 + +public: + + typedef GeneralSFMFactor2 This; + typedef PinholeCamera Camera;///< typedef for camera type + typedef NoiseModelFactor3 Base;///< typedef for the base class + + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** + * Constructor + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param model is the standard deviation of the measurements + * @param poseKey is the index of the camera + * @param landmarkKey is the index of the landmark + * @param calibKey is the index of the calibration + */ + GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) : + Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {} + GeneralSFMFactor2():measured_(0.0,0.0) {} ///< default constructor + + virtual ~GeneralSFMFactor2() {} ///< destructor + + /// @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)));} + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s, keyFormatter); + measured_.print(s + ".z"); + } + + /** + * equals + */ + bool equals(const NonlinearFactor &p, double tol = 1e-9) const { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol); + } + + /** h(x)-z */ + Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib, + boost::optional H1=boost::none, + boost::optional H2=boost::none, + boost::optional H3=boost::none) const + { + try { + Camera camera(pose3,calib); + Point2 reprojError(camera.project(point, H1, H2, H3) - measured_); + return reprojError.vector(); + } + catch( CheiralityException& e) { + 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; + } + return zero(2); + } + + /** return the measured */ + inline const Point2 measured() const { + return measured_; + } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + 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 923edddb7..16560a43e 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 namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQ: public JacobianSchurFactor { +class JacobianFactorQ: public RegularJacobianFactor { - typedef JacobianSchurFactor Base; + typedef RegularJacobianFactor Base; + typedef Eigen::Matrix MatrixZD; + typedef std::pair KeyMatrix; 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() { + JacobianFactorQ(const FastVector& keys, + 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 mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D) - BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) - QF.push_back(KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second)); + for (size_t k = 0; k < FBlocks.size(); ++k) { + Key key = keys[k]; + QF.push_back( + KeyMatrix(key, - Q.block(0, ZDim * j++, m2, ZDim) * FBlocks[k])); + } // Which is then passed to the normal JacobianFactor constructor - JacobianFactor::fillTerms(QF, Q * b, model); + 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 ccd6e8756..77102c24b 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -6,41 +6,47 @@ */ #pragma once -#include +#include +#include +#include namespace gtsam { + +class GaussianBayesNet; + /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQR: public JacobianSchurFactor { +class JacobianFactorQR: public RegularJacobianFactor { - typedef JacobianSchurFactor Base; + typedef RegularJacobianFactor Base; + typedef Eigen::Matrix MatrixZD; public: /** * Constructor */ - JacobianFactorQR(const std::vector& Fblocks, - const Matrix& E, const Matrix3& P, const Vector& b, + JacobianFactorQR(const FastVector& keys, + 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(ZDim * i, 0), it.first, it.second, - b.segment(ZDim * i), model); - i += 1; + for (size_t k = 0; k < FBlocks.size(); ++k) { + Key key = keys[k]; + gfg.add(pointKey, E.block(ZDim * k, 0), key, FBlocks[k], + b.segment < ZDim > (ZDim * k), model); } //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 +54,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 e0a5f4566..86636c38f 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -5,50 +5,66 @@ */ #pragma once -#include "gtsam/slam/JacobianSchurFactor.h" +#include namespace gtsam { /** - * JacobianFactor for Schur complement that uses Q noise model + * JacobianFactor for Schur complement */ template -class JacobianFactorSVD: public JacobianSchurFactor { +class JacobianFactorSVD: public RegularJacobianFactor { + + typedef RegularJacobianFactor Base; + typedef Eigen::Matrix MatrixZD; // e.g 2 x 6 with Z=Point2 + typedef std::pair KeyMatrix; public: - typedef Eigen::Matrix Matrix2D; // e.g 2 x 6 with Z=Point2 - 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() { + /** + * @brief Constructor + * Takes the CameraSet derivatives (as ZDim*D blocks of block-diagonal F) + * and a reduced point derivative, Enull + * and creates a reduced-rank Jacobian factor on the CameraSet + * + * @Fblocks: + */ + JacobianFactorSVD(const FastVector& keys, + 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; + size_t 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, ZDim * j++, m2, ZDim) * it.second)); + for (size_t k = 0; k < Fblocks.size(); ++k) { + Key key = keys[k]; + QF.push_back( + KeyMatrix(key, + (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k])); + } JacobianFactor::fillTerms(QF, Enull.transpose() * b, model); } }; diff --git a/gtsam/slam/JacobianSchurFactor.h b/gtsam/slam/JacobianSchurFactor.h deleted file mode 100644 index 5d28bbada..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; pos - 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 6689653aa..e9914955c 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -90,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 eee14f9f2..8305fce12 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( @@ -80,11 +85,11 @@ namespace gtsam { /** 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(traits::GetDimension(p)); - // manifold equivalent of h(x)-z -> log(z,h(x)) + Vector evaluateError(const T& x, boost::optional H = boost::none) const { + if (H) (*H) = eye(traits::GetDimension(x)); + // manifold equivalent of z-x -> Local(x,z) // TODO(ASL) Add Jacobians. - return traits::Local(prior_,p); + return -traits::Local(x, prior_); } const VALUE & prior() const { return prior_; } @@ -94,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 52e28beaf..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_); diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index 216b9320e..d50674d75 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -9,189 +9,14 @@ * -------------------------------------------------------------------------- */ -/** - * @file RangeFactor.H - * @author Frank Dellaert - **/ - #pragma once -#include -#include -#include +#ifdef _MSC_VER +#pragma message("RangeFactor is now an ExpressionFactor in SAM directory") +#else +#warning "RangeFactor is now an ExpressionFactor in SAM directory" +#endif -namespace gtsam { -/** - * Binary factor for a range measurement - * @addtogroup SLAM - */ -template -class RangeFactor: public NoiseModelFactor2 { -private: +#include - double measured_; /** measurement */ - - typedef RangeFactor This; - typedef NoiseModelFactor2 Base; - - // Concept requirements for this factor - GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(T1, T2) - -public: - - RangeFactor() { - } /* Default constructor */ - - RangeFactor(Key key1, Key key2, double measured, - const SharedNoiseModel& model) : - Base(model, key1, key2), measured_(measured) { - } - - 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(); - } - - /** 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 - && body_P_sensor_.equals(e->body_P_sensor_); - } - - /** 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); - } - -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_); - } -}; // \ RangeFactorWithTransform - -/// traits -template -struct traits > : public Testable > {}; - -} // \ namespace gtsam diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 6a70d58b4..90ceeafc8 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -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("NonlinearFactor3", boost::serialization::base_object(*this)); } diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h deleted file mode 100644 index 8c3df87cf..000000000 --- a/gtsam/slam/RegularHessianFactor.h +++ /dev/null @@ -1,178 +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 RegularHessianFactor.h - * @brief HessianFactor class with constant sized blcoks - * @author Richard Roberts - * @date Dec 8, 2010 - */ - -#pragma once - -#include -#include -#include - -namespace gtsam { - -template -class RegularHessianFactor: public HessianFactor { - -private: - - typedef Eigen::Matrix MatrixDD; // camera hessian block - typedef Eigen::Matrix VectorD; - -public: - - /** Construct an n-way factor. Gs contains the upper-triangle blocks of the - * quadratic term (the Hessian matrix) provided in row-order, gs the pieces - * of the linear vector term, and f the constant term. - */ - RegularHessianFactor(const std::vector& js, - const std::vector& Gs, const std::vector& gs, double f) : - HessianFactor(js, Gs, gs, f) { - } - - /** Constructor with an arbitrary number of keys and with the augmented information matrix - * specified as a block matrix. */ - template - RegularHessianFactor(const KEYS& keys, - const SymmetricBlockMatrix& augmentedInformation) : - HessianFactor(keys, augmentedInformation) { - } - - // Scratch space for multiplyHessianAdd - typedef Eigen::Matrix DVector; - mutable std::vector y; - - /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const{ - throw std::runtime_error( - "RegularHessianFactor::forbidden use of multiplyHessianAdd without raw memory access, use HessianFactor instead"); - } - - /** 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 - y.resize(size()); - 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 - DVector xj(D); - for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { - Key key = keys_[j]; - const double* xj = x + key * D; - DenseIndex i = 0; - for (; i < j; ++i) - y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); - // blocks on the diagonal are only half - y[i] += info_(j, j).selfadjointView() * ConstDMap(xj); - // 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(xj); - } - - // copy to yvalues - for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) { - Key key = keys_[i]; - DMap(yvalues + key * D) += alpha * y[i]; - } - } - - void multiplyHessianAdd(double alpha, const double* x, double* yvalues, - std::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 - 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 { - - // Use eigen magic to access raw memory - //typedef Eigen::Matrix DVector; - 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(); - DMap(d + D * j) += B.diagonal(); - } - } - - /* ************************************************************************* */ - // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n - virtual void gradientAtZero(double* d) const { - - // Use eigen magic to access raw memory - //typedef Eigen::Matrix DVector; - 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 - VectorD dj = -info_(pos,size()).knownOffDiagonal(); - //DMap(d + 9 * j) += dj; - DMap(d + D * j) += dj; - } - } - -}; - -} - diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 75b2d44ba..c713eff72 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -7,19 +7,18 @@ #pragma once -#include +#include #include #include #include -#include -#include +#include namespace gtsam { /** * RegularImplicitSchurFactor */ -template // +template class RegularImplicitSchurFactor: public GaussianFactor { public: @@ -28,14 +27,20 @@ public: protected: - typedef Eigen::Matrix Matrix2D; ///< type of an F block - typedef Eigen::Matrix MatrixDD; ///< camera hessian - typedef std::pair KeyMatrix2D; ///< named F block + // This factor is closely related to a CameraSet + typedef CameraSet Set; - std::vector Fblocks_; ///< All 2*D F blocks (one for each camera) - Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate) - Matrix E_; ///< The 2m*3 E Jacobian with respect to the point - Vector b_; ///< 2m-dimensional RHS vector + typedef typename CAMERA::Measurement Z; + static const int D = traits::dimension; ///< Camera dimension + static const int ZDim = traits::dimension; ///< Measurement dimension + + typedef Eigen::Matrix MatrixZD; ///< type of an F block + typedef Eigen::Matrix MatrixDD; ///< camera hessian + + const std::vector FBlocks_; ///< All ZDim*D F blocks (one for each camera) + const Matrix PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate) + const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point + const Vector b_; ///< 2m-dimensional RHS vector public: @@ -43,64 +48,60 @@ public: RegularImplicitSchurFactor() { } - /// Construct from blcoks of F, E, inv(E'*E), and RHS vector b - RegularImplicitSchurFactor(const std::vector& Fblocks, const Matrix& E, - const Matrix3& P, const Vector& b) : - Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) { - initKeys(); - } - - /// initialize keys from Fblocks - void initKeys() { - keys_.reserve(Fblocks_.size()); - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) - keys_.push_back(it.first); + /// Construct from blocks of F, E, inv(E'*E), and RHS vector b + RegularImplicitSchurFactor(const FastVector& keys, + const std::vector& FBlocks, const Matrix& E, const Matrix& P, + const Vector& b) : + GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) { } /// Destructor virtual ~RegularImplicitSchurFactor() { } - // Write access, only use for construction! - - inline std::vector& Fblocks() { - return Fblocks_; + std::vector& FBlocks() const { + return FBlocks_; } - inline Matrix3& PointCovariance() { - return PointCovariance_; - } - - inline Matrix& E() { + const Matrix& E() const { return E_; } - inline Vector& b() { + const Vector& b() const { return b_; } - /// Get matrix P - inline const Matrix3& getPointCovariance() const { + const Matrix& getPointCovariance() const { return PointCovariance_; } /// print - void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "", 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] << 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 k = 0; k < FBlocks_.size(); ++k) { + if (keys_[k] != f->keys_[k]) + return false; + if (!equal_with_abs_tol(FBlocks_[k], f->FBlocks_[k], 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 @@ -108,24 +109,37 @@ public: return D; } + virtual void updateHessian(const FastVector& keys, + SymmetricBlockMatrix* info) const { + throw std::runtime_error( + "RegularImplicitSchurFactor::updateHessian non implemented"); + } virtual Matrix augmentedJacobian() const { throw std::runtime_error( "RegularImplicitSchurFactor::augmentedJacobian non implemented"); return Matrix(); } virtual std::pair jacobian() const { - throw std::runtime_error("RegularImplicitSchurFactor::jacobian non implemented"); + throw std::runtime_error( + "RegularImplicitSchurFactor::jacobian non implemented"); return std::make_pair(Matrix(), Vector()); } + + /// *Compute* full augmented information matrix virtual Matrix augmentedInformation() const { - throw std::runtime_error( - "RegularImplicitSchurFactor::augmentedInformation non implemented"); - return Matrix(); + + // Do the Schur complement + SymmetricBlockMatrix augmentedHessian = // + Set::SchurComplement(FBlocks_, E_, b_); + return augmentedHessian.matrix(); } + + /// *Compute* full information matrix virtual Matrix information() const { - throw std::runtime_error( - "RegularImplicitSchurFactor::information non implemented"); - return Matrix(); + Matrix augmented = augmentedInformation(); + int m = this->keys_.size(); + size_t M = D * m; + return augmented.block(0, 0, M, M); } /// Return the diagonal of the Hessian for this factor @@ -133,17 +147,17 @@ public: // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); VectorValues d; - for (size_t pos = 0; pos < size(); ++pos) { // for each camera - Key j = keys_[pos]; + for (size_t k = 0; k < size(); ++k) { // for each camera + Key j = keys_[k]; // Calculate Fj'*Ej for the current camera (observing a single point) - // D x 3 = (D x 2) * (2 x 3) - const Matrix2D& Fj = Fblocks_[pos].second; - Eigen::Matrix FtE = Fj.transpose() - * E_.block<2, 3>(2 * pos, 0); + // D x 3 = (D x ZDim) * (ZDim x 3) + const MatrixZD& Fj = FBlocks_[k]; + Eigen::Matrix FtE = Fj.transpose() + * E_.block(ZDim * k, 0); Eigen::Matrix dj; - for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian + for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian // Vector column_k_Fj = Fj.col(k); dj(k) = Fj.col(k).squaredNorm(); // dot(column_k_Fj, column_k_Fj); // Vector column_k_FtE = FtE.row(k); @@ -169,13 +183,13 @@ public: Key j = keys_[pos]; // Calculate Fj'*Ej for the current camera (observing a single point) - // D x 3 = (D x 2) * (2 x 3) - const Matrix2D& Fj = Fblocks_[pos].second; + // D x 3 = (D x ZDim) * (ZDim x 3) + const MatrixZD& Fj = FBlocks_[pos]; Eigen::Matrix FtE = Fj.transpose() - * E_.block<2, 3>(2 * pos, 0); + * E_.block(ZDim * pos, 0); DVector dj; - for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian + for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian dj(k) = Fj.col(k).squaredNorm(); // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1) dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose(); @@ -190,38 +204,41 @@ public: // F'*(I - E*P*E')*F for (size_t pos = 0; pos < size(); ++pos) { Key j = keys_[pos]; - // F'*F - F'*E*P*E'*F (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9) - const Matrix2D& Fj = Fblocks_[pos].second; + // F'*F - F'*E*P*E'*F e.g. (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9) + const MatrixZD& Fj = FBlocks_[pos]; // Eigen::Matrix FtE = Fj.transpose() - // * E_.block<2, 3>(2 * pos, 0); + // * E_.block(ZDim * pos, 0); // blocks[j] = Fj.transpose() * Fj // - FtE * PointCovariance_ * FtE.transpose(); - const Matrix23& Ej = E_.block<2, 3>(2 * pos, 0); - blocks[j] = Fj.transpose() * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj); + const Matrix23& Ej = E_.block(ZDim * pos, 0); + blocks[j] = Fj.transpose() + * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj); // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-( - // static const Eigen::Matrix I2 = eye(2); + // static const Eigen::Matrix I2 = eye(ZDim); // Matrix2 Q = // - // I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose(); + // I2 - E_.block(ZDim * pos, 0) * PointCovariance_ * E_.block(ZDim * pos, 0).transpose(); // blocks[j] = Fj.transpose() * Q * Fj; } return blocks; } virtual GaussianFactor::shared_ptr clone() const { - return boost::make_shared >(Fblocks_, - PointCovariance_, E_, b_); - throw std::runtime_error("RegularImplicitSchurFactor::clone non implemented"); + return boost::make_shared >(keys_, + FBlocks_, PointCovariance_, E_, b_); + throw std::runtime_error( + "RegularImplicitSchurFactor::clone non implemented"); } virtual bool empty() const { return false; } virtual GaussianFactor::shared_ptr negate() const { - return boost::make_shared >(Fblocks_, - PointCovariance_, E_, b_); - throw std::runtime_error("RegularImplicitSchurFactor::negate non implemented"); + return boost::make_shared >(keys_, + FBlocks_, PointCovariance_, E_, b_); + throw std::runtime_error( + "RegularImplicitSchurFactor::negate non implemented"); } // Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing @@ -239,22 +256,24 @@ public: typedef std::vector Error2s; /** - * @brief Calculate corrected error Q*(e-2*b) = (I - E*P*E')*(e-2*b) + * @brief Calculate corrected error Q*(e-ZDim*b) = (I - E*P*E')*(e-ZDim*b) */ void projectError2(const Error2s& e1, Error2s& e2) const { - // d1 = E.transpose() * (e1-2*b) = (3*2m)*2m + // d1 = E.transpose() * (e1-ZDim*b) = (3*2m)*2m Vector3 d1; d1.setZero(); for (size_t k = 0; k < size(); k++) - d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * (e1[k] - 2 * b_.segment < 2 > (k * 2)); + d1 += E_.block(ZDim * k, 0).transpose() + * (e1[k] - ZDim * b_.segment(k * ZDim)); // d2 = E.transpose() * e1 = (3*2m)*2m Vector3 d2 = PointCovariance_ * d1; // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] for (size_t k = 0; k < size(); k++) - e2[k] = e1[k] - 2 * b_.segment < 2 > (k * 2) - E_.block < 2, 3 > (2 * k, 0) * d2; + e2[k] = e1[k] - ZDim * b_.segment(k * ZDim) + - E_.block(ZDim * k, 0) * d2; } /* @@ -274,7 +293,7 @@ public: // e1 = F * x - b = (2m*dm)*dm for (size_t k = 0; k < size(); ++k) - e1[k] = Fblocks_[k].second * x.at(keys_[k]); + e1[k] = FBlocks_[k] * x.at(keys_[k]); projectError2(e1, e2); double result = 0; @@ -296,7 +315,7 @@ public: // e1 = F * x - b = (2m*dm)*dm for (size_t k = 0; k < size(); ++k) - e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2); + e1[k] = FBlocks_[k] * x.at(keys_[k]) - b_.segment(k * ZDim); projectError(e1, e2); double result = 0; @@ -309,21 +328,21 @@ public: /** * @brief Calculate corrected error Q*e = (I - E*P*E')*e */ - void projectError(const Error2s& e1, Error2s& e2) const { + void projectError(const Error2s& e1, Error2s& e2) const { - // d1 = E.transpose() * e1 = (3*2m)*2m - Vector3 d1; - d1.setZero(); - for (size_t k = 0; k < size(); k++) - d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * e1[k]; + // d1 = E.transpose() * e1 = (3*2m)*2m + Vector3 d1; + d1.setZero(); + for (size_t k = 0; k < size(); k++) + d1 += E_.block(ZDim * k, 0).transpose() * e1[k]; - // d2 = E.transpose() * e1 = (3*2m)*2m - Vector3 d2 = PointCovariance_ * d1; + // d2 = E.transpose() * e1 = (3*2m)*2m + Vector3 d2 = PointCovariance_ * d1; - // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] - for (size_t k = 0; k < size(); k++) - e2[k] = e1[k] - E_.block < 2, 3 > (2 * k, 0) * d2; - } + // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] + for (size_t k = 0; k < size(); k++) + e2[k] = e1[k] - E_.block(ZDim * k, 0) * d2; + } /// Scratch space for multiplyHessianAdd mutable Error2s e1, e2; @@ -344,19 +363,17 @@ public: e2.resize(size()); // e1 = F * x = (2m*dm)*dm - size_t k = 0; - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) { - Key key = it.first; - e1[k++] = it.second * ConstDMap(x + D * key); + for (size_t k = 0; k < size(); ++k) { + Key key = keys_[k]; + e1[k] = FBlocks_[k] * ConstDMap(x + D * key); } projectError(e1, e2); // y += F.transpose()*e2 = (2d*2m)*2m - k = 0; - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) { - Key key = it.first; - DMap(y + D * key) += it.second.transpose() * alpha * e2[k++]; + for (size_t k = 0; k < size(); ++k) { + Key key = keys_[k]; + DMap(y + D * key) += FBlocks_[k].transpose() * alpha * e2[k]; } } @@ -377,7 +394,7 @@ public: // e1 = F * x = (2m*dm)*dm for (size_t k = 0; k < size(); ++k) - e1[k] = Fblocks_[k].second * x.at(keys_[k]); + e1[k] = FBlocks_[k] * x.at(keys_[k]); projectError(e1, e2); @@ -389,8 +406,8 @@ public: Vector& yi = it.first->second; // Create the value as a zero vector if it does not exist. if (it.second) - yi = Vector::Zero(Fblocks_[k].second.cols()); - yi += Fblocks_[k].second.transpose() * alpha * e2[k]; + yi = Vector::Zero(FBlocks_[k].cols()); + yi += FBlocks_[k].transpose() * alpha * e2[k]; } } @@ -400,9 +417,9 @@ public: void multiplyHessianDummy(double alpha, const VectorValues& x, VectorValues& y) const { - BOOST_FOREACH(const KeyMatrix2D& Fi, Fblocks_) { + for (size_t k = 0; k < size(); ++k) { static const Vector empty; - Key key = Fi.first; + Key key = keys_[k]; std::pair it = y.tryInsert(key, empty); Vector& yi = it.first->second; yi = x.at(key); @@ -417,14 +434,14 @@ public: e1.resize(size()); e2.resize(size()); for (size_t k = 0; k < size(); k++) - e1[k] = b_.segment < 2 > (2 * k); + e1[k] = b_.segment(ZDim * k); projectError(e1, e2); // g = F.transpose()*e2 VectorValues g; for (size_t k = 0; k < size(); ++k) { Key key = keys_[k]; - g.insert(key, -Fblocks_[k].second.transpose() * e2[k]); + g.insert(key, -FBlocks_[k].transpose() * e2[k]); } // return it @@ -444,23 +461,34 @@ public: e1.resize(size()); e2.resize(size()); for (size_t k = 0; k < size(); k++) - e1[k] = b_.segment < 2 > (2 * k); + e1[k] = b_.segment(ZDim * k); projectError(e1, e2); for (size_t k = 0; k < size(); ++k) { // for each camera in the factor Key j = keys_[k]; - DMap(d + D * j) += -Fblocks_[k].second.transpose() * e2[k]; + DMap(d + D * j) += -FBlocks_[k].transpose() * e2[k]; } } /// 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"); + throw std::runtime_error( + "gradient for RegularImplicitSchurFactor is not implemented yet"); } - }; -// RegularImplicitSchurFactor +// end class RegularImplicitSchurFactor + +template +const int RegularImplicitSchurFactor::D; + +template +const int RegularImplicitSchurFactor::ZDim; + +// traits +template struct traits > : public Testable< + RegularImplicitSchurFactor > { +}; } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 44d6902fa..01a8fcf8d 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -15,6 +15,7 @@ * @author Luca Carlone * @author Zsolt Kira * @author Frank Dellaert + * @author Chris Beall */ #pragma once @@ -22,111 +23,118 @@ #include #include #include -#include #include -#include // for Cheirality exception -#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) - - 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 std::pair KeyMatrix2D; // Fblocks - typedef Eigen::Matrix MatrixDD; // camera hessian block - typedef Eigen::Matrix Matrix23; - typedef Eigen::Matrix VectorD; - typedef Eigen::Matrix Matrix2; - - /// shorthand for base class type +private: typedef NonlinearFactor Base; + typedef SmartFactorBase This; + typedef typename CAMERA::Measurement Z; - /// shorthand for this class - typedef SmartFactorBase This; +protected: + /** + * As of Feb 22, 2015, the noise model is the same for all measurements and + * is isotropic. This allows for moving most calculations of Schur complement + * etc to be moved to CameraSet very easily, and also agrees pragmatically + * with what is normally done. + */ + SharedIsotropic noiseModel_; + /** + * 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_; + + /// @name Pose of the camera in the body frame + const boost::optional body_P_sensor_; ///< Pose of the camera in the body frame + /// @} + + static const int Dim = traits::dimension; ///< Camera dimension + static const int ZDim = traits::dimension; ///< Measurement dimension + + // Definitions for block matrices used internally + typedef Eigen::Matrix MatrixD2; // F' + typedef Eigen::Matrix MatrixDD; // camera hessian block + typedef Eigen::Matrix Matrix23; + typedef Eigen::Matrix VectorD; + typedef Eigen::Matrix Matrix2; public: - + // Definitions for blocks of F, externally visible + typedef Eigen::Matrix MatrixZD; // F 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 CAMERA Camera; - typedef std::vector Cameras; + /// We use the new CameraSte data structure to refer to a set of 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) : - body_P_sensor_(body_P_sensor) { + /// Default Constructor, for serialization + SmartFactorBase() {} + + /// Constructor + SmartFactorBase(const SharedNoiseModel& sharedNoiseModel, + boost::optional body_P_sensor = boost::none) : + body_P_sensor_(body_P_sensor){ + + if (!sharedNoiseModel) + throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required"); + + SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast< + noiseModel::Isotropic>(sharedNoiseModel); + + if (!sharedIsotropic) + throw std::runtime_error("SmartFactorBase: needs isotropic"); + + noiseModel_ = sharedIsotropic; } - /** Virtual destructor */ + /// Virtual destructor, subclasses from NonlinearFactor virtual ~SmartFactorBase() { } /** - * 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 + * @param sharedNoiseModel is the measurement noise */ - void add(const Z& measured_i, const Key& poseKey_i, - const SharedNoiseModel& noise_i) { + void add(const Z& measured_i, const Key& cameraKey_i) { this->measured_.push_back(measured_i); - this->keys_.push_back(poseKey_i); - this->noise_.push_back(noise_i); + this->keys_.push_back(cameraKey_i); } /** - * 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 */ - // **************************************************************************************************** - void add(std::vector& measurements, std::vector& poseKeys, - std::vector& noises) { + void add(std::vector& measurements, std::vector& cameraKeys) { for (size_t i = 0; i < measurements.size(); i++) { this->measured_.push_back(measurements.at(i)); - this->keys_.push_back(poseKeys.at(i)); - this->noise_.push_back(noises.at(i)); - } - } - - /** - * variant of the previous add: adds a bunch of measurements and uses the same noise model for all of them - */ - // **************************************************************************************************** - 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)); - this->keys_.push_back(poseKeys.at(i)); - this->noise_.push_back(noise); + this->keys_.push_back(cameraKeys.at(i)); } } @@ -134,24 +142,30 @@ public: * Adds an entire SfM_track (collection of cameras observing a single point). * The noise is assumed to be the same for all measurements */ - // **************************************************************************************************** template - void add(const SFM_TRACK& trackToAdd, const SharedNoiseModel& noise) { + void add(const SFM_TRACK& trackToAdd) { 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); - this->noise_.push_back(noise); } } + /// 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 { return measured_; } - /** return the noise model */ - const std::vector& noise() const { - return noise_; + /// Collect all cameras: important that in key order + virtual Cameras cameras(const Values& values) const { + Cameras cameras; + BOOST_FOREACH(const Key& k, this->keys_) + cameras.push_back(values.at(k)); + return cameras; } /** @@ -164,10 +178,10 @@ public: std::cout << s << "SmartFactorBase, z = \n"; for (size_t k = 0; k < measured_.size(); ++k) { std::cout << "measurement, p = " << measured_[k] << "\t"; - noise_[k]->print("noise model = "); + noiseModel_->print("noise model = "); } - if (this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); + if(body_P_sensor_) + body_P_sensor_->print("body_P_sensor_:\n"); Base::print("", keyFormatter); } @@ -181,511 +195,210 @@ public: areMeasurementsEqual = false; break; } - return e && Base::equals(p, tol) && areMeasurementsEqual - && ((!body_P_sensor_ && !e->body_P_sensor_) - || (body_P_sensor_ && e->body_P_sensor_ - && body_P_sensor_->equals(*e->body_P_sensor_))); + return e && Base::equals(p, tol) && areMeasurementsEqual; } - // **************************************************************************************************** -// /// Calculate vector of re-projection errors, before applying noise model - Vector reprojectionError(const Cameras& cameras, const Point3& point) const { - - Vector b = zero(ZDim * cameras.size()); - - size_t i = 0; - BOOST_FOREACH(const CAMERA& camera, cameras) { - const Z& zi = this->measured_.at(i); - try { - Z e(camera.project(point) - zi); - b[ZDim * i] = e.x(); - b[ZDim * i + 1] = e.y(); - } catch (CheiralityException& e) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); + /// Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives + template + Vector unwhitenedError(const Cameras& cameras, const POINT& point, + boost::optional Fs = boost::none, // + boost::optional E = boost::none) const { + Vector ue = cameras.reprojectionError(point, measured_, Fs, E); + if(body_P_sensor_){ + for(size_t i=0; i < Fs->size(); i++){ + 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); + Fs->at(i) = Fs->at(i) * J; } - i += 1; } - - return b; + return ue; } - // **************************************************************************************************** /** - * Calculate the error of the factor. + * Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z] + * Noise model applied + */ + template + Vector whitenedError(const Cameras& cameras, const POINT& point) const { + Vector e = cameras.reprojectionError(point, measured_); + if (noiseModel_) + noiseModel_->whitenInPlace(e); + return e; + } + + /** 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. - * This is different from reprojectionError(cameras,point) as each point is whitened + * Will be used in "error(Values)" function required by NonlinearFactor base class */ + template double totalReprojectionError(const Cameras& cameras, - const Point3& point) const { - - double overallError = 0; - - size_t i = 0; - BOOST_FOREACH(const CAMERA& camera, cameras) { - const Z& zi = this->measured_.at(i); - try { - Z 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; + const POINT& point) const { + Vector e = whitenedError(cameras, point); + return 0.5 * e.dot(e); } - // **************************************************************************************************** - /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras, - const Point3& point) const { - - int numKeys = this->keys_.size(); - E = zeros(ZDim * numKeys, 3); - Vector b = zero(2 * numKeys); - - Matrix Ei(ZDim, 3); - for (size_t i = 0; i < this->measured_.size(); i++) { - try { - cameras[i].project(point, boost::none, Ei, boost::none); - } catch (CheiralityException& e) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); - } - this->noise_.at(i)->WhitenSystem(Ei, b); - E.block(ZDim * i, 0) = Ei; - } - - // Matrix PointCov; - PointCov.noalias() = (E.transpose() * E).inverse(); + /// Computes Point Covariance P from E + static Matrix PointCov(Matrix& E) { + return (E.transpose() * E).inverse(); } - // **************************************************************************************************** - /// 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 { - - size_t numKeys = this->keys_.size(); - E = zeros(ZDim * numKeys, 3); - b = zero(ZDim * numKeys); - double f = 0; - - Matrix Fi(ZDim, 6), Ei(ZDim, 3), Hcali(ZDim, D - 6), Hcam(ZDim, D); - for (size_t i = 0; i < this->measured_.size(); i++) { - - 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; - } - } catch (CheiralityException&) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); - } - 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(0, 0) = Fi; // ZDim x 6 block for the cameras - Hcam.block(0, 6) = Hcali; // ZDim x nrCal block for the cameras - Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam)); - } - E.block(ZDim * i, 0) = Ei; - subInsert(b, bi, ZDim * i); - } - return f; + /** + * 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. + * TODO: Kill this obsolete method + */ + template + void computeJacobians(std::vector& Fblocks, Matrix& E, Vector& b, + const Cameras& cameras, const POINT& point) const { + // Project into Camera set and calculate derivatives + // As in expressionFactor, RHS vector b = - (h(x_bar) - z) = z-h(x_bar) + // Indeed, nonlinear error |h(x_bar+dx)-z| ~ |h(x_bar) + A*dx - z| + // = |A*dx - (z-h(x_bar))| + b = -unwhitenedError(cameras, point, Fblocks, E); } - // **************************************************************************************************** - /// 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 { - - double f = computeJacobians(Fblocks, E, b, cameras, point); - - // 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{ - EtE(0, 0) += lambda; - EtE(1, 1) += lambda; - EtE(2, 2) += lambda; - } - - PointCov.noalias() = (EtE).inverse(); - - return f; - } - - // **************************************************************************************************** - // 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 { - - size_t numKeys = this->keys_.size(); - std::vector Fblocks; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, - lambda); - F = zeros(This::ZDim * numKeys, D * numKeys); - - for (size_t i = 0; i < this->keys_.size(); ++i) { - F.block(This::ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras - } - 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 { + template + void computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, + Vector& b, const Cameras& cameras, const POINT& 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) + computeJacobians(Fblocks, E, b, cameras, point); + + static const int N = FixedDimension::value; // 2 (Unit3) or 3 (Point3) // Do SVD on A Eigen::JacobiSVD svd(E, Eigen::ComputeFullU); Vector s = svd.singularValues(); - // Enull = zeros(ZDim * numKeys, ZDim * numKeys - 3); - size_t numKeys = this->keys_.size(); - Enull = svd.matrixU().block(0, 3, ZDim * numKeys, ZDim * numKeys - 3); // last ZDimm-3 columns - - return f; + size_t m = this->keys_.size(); + Enull = svd.matrixU().block(0, N, ZDim * m, ZDim * m - N); // last ZDim*m-N columns } - // **************************************************************************************************** - /// 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(ZDim * numKeys, D * numKeys); - F.setZero(); - - for (size_t i = 0; i < this->keys_.size(); ++i) - F.block(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras - - return f; - } - - // **************************************************************************************************** - /// linearize returns a Hessianfactor that is an approximation of error(p) - boost::shared_ptr > createHessianFactor( + /// Linearize to a Hessianfactor + boost::shared_ptr > createHessianFactor( const Cameras& cameras, const Point3& point, const double lambda = 0.0, bool diagonalDamping = false) const { - int numKeys = this->keys_.size(); - - std::vector Fblocks; + std::vector Fblocks; Matrix E; - Matrix3 PointCov; Vector b; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); + computeJacobians(Fblocks, E, b, cameras, point); -//#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix -#ifdef HESSIAN_BLOCKS - // Create structures for Hessian Factors - std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); - std::vector < Vector > gs(numKeys); + // build augmented hessian + SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fblocks, E, + b); - sparseSchurComplement(Fblocks, E, PointCov, b, Gs, gs); - // schurComplement(Fblocks, E, PointCov, 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); -#else // we create directly a SymmetricBlockMatrix - size_t n1 = D * numKeys + 1; - std::vector dims(numKeys + 1); // this also includes the b term - std::fill(dims.begin(), dims.end() - 1, D); - 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) = ... - augmentedHessian(numKeys, numKeys)(0, 0) = f; - return boost::make_shared >(this->keys_, + return boost::make_shared >(keys_, augmentedHessian); -#endif } - // **************************************************************************************************** - // slow version - works on full (sparse) matrices - void schurComplement(const std::vector& Fblocks, const Matrix& E, - const Matrix& PointCov, const Vector& b, - /*output ->*/std::vector& Gs, std::vector& gs) const { - // Schur complement trick - // Gs = F' * F - F' * E * inv(E'*E) * E' * F - // gs = F' * (b - E * inv(E'*E) * E' * b) - // This version uses full matrices - - int numKeys = this->keys_.size(); - - /// Compute Full F ???? - Matrix F = zeros(ZDim * numKeys, D * numKeys); - for (size_t i = 0; i < this->keys_.size(); ++i) - F.block(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras - - 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)))); - - // Populate Gs and gs - int GsCount2 = 0; - for (DenseIndex i1 = 0; i1 < numKeys; i1++) { // for each camera - DenseIndex i1D = i1 * D; - gs.at(i1) = gs_vector.segment(i1D); - for (DenseIndex i2 = 0; i2 < numKeys; i2++) { - if (i2 >= i1) { - Gs.at(GsCount2) = H.block(i1D, i2 * D); - GsCount2++; - } - } - } - } - - // **************************************************************************************************** - void sparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, - /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { - // Schur complement trick - // Gs = F' * F - F' * E * P * E' * F - // gs = F' * (b - E * P * E' * b) - - // a single point is observed in numKeys cameras - size_t numKeys = this->keys_.size(); - - // Blockwise Schur complement - for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera - - const Matrix2D& Fi1 = Fblocks.at(i1).second; - 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(ZDim * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - - // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - augmentedHessian(i1, i1) = 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 - const Matrix2D& Fi2 = Fblocks.at(i2).second; - - // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - augmentedHessian(i1, i2) = -Fi1.transpose() - * (Ei1_P * E.block(ZDim * i2, 0).transpose() * Fi2); - } - } // end of for over cameras - } - - // **************************************************************************************************** - void sparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix& 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 - // gs = F' * (b - E * P * E' * b) - - // a single point is observed in numKeys cameras - size_t numKeys = this->keys_.size(); - - int GsIndex = 0; - // Blockwise Schur complement - for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera - // GsIndex points to the upper triangular blocks - // 0 1 2 3 4 - // X 5 6 7 8 - // X X 9 10 11 - // X X X 12 13 - // X X X X 14 - const Matrix2D& Fi1 = Fblocks.at(i1).second; - - const Matrix23 Ei1_P = E.block(ZDim * i1, 0) * P; - - { // for i1 = i2 - // D = (Dx2) * (2) - 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) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - Gs.at(GsIndex) = Fi1.transpose() - * (Fi1 - Ei1_P * E.block(ZDim * i1, 0).transpose() * Fi1); - GsIndex++; - } - // upper triangular part of the hessian - for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera - const Matrix2D& Fi2 = Fblocks.at(i2).second; - - // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - Gs.at(GsIndex) = -Fi1.transpose() - * (Ei1_P * E.block(ZDim * i2, 0).transpose() * Fi2); - GsIndex++; - } - } // end of for over cameras - } - - // **************************************************************************************************** + /** + * 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; - 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) = ... + std::vector Fblocks; + computeJacobians(Fblocks, E, b, cameras, point); + Cameras::UpdateSchurComplement(Fblocks, E, b, allKeys, keys_, + augmentedHessian); } - // **************************************************************************************************** - void updateSparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, - const double f, const FastVector allKeys, - /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { - // Schur complement trick - // Gs = F' * F - F' * E * P * E' * F - // gs = F' * (b - E * P * E' * b) - - 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)); - - // a single point is observed in numKeys cameras - size_t numKeys = this->keys_.size(); // cameras observing current point - size_t aug_numKeys = (augmentedHessian.rows() - 1) / D; // all cameras in the group - - // Blockwise Schur complement - 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(ZDim * i1, 0) * P; - - // 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]; - DenseIndex aug_i1 = KeySlotMap[this->keys_[i1]]; - - // 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(ZDim * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - - // (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(ZDim * i1, 0).transpose() * Fi1) ); - - // upper triangular part of the hessian - for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera - const Matrix2D& Fi2 = Fblocks.at(i2).second; - - //Key cameraKey_i2 = this->keys_[i2]; - DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]]; - - // (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(ZDim * i2, 0).transpose() * Fi2); - } - } // end of for over cameras - - augmentedHessian(aug_numKeys, aug_numKeys)(0, 0) += f; + /// Whiten the Jacobians computed by computeJacobians using noiseModel_ + void whitenJacobians(std::vector& F, Matrix& E, Vector& b) const { + noiseModel_->WhitenSystem(E, b); + // TODO make WhitenInPlace work with any dense matrix type + for (size_t i = 0; i < F.size(); i++) + F[i] = noiseModel_->Whiten(F[i]); } - // **************************************************************************************************** - boost::shared_ptr > createRegularImplicitSchurFactor( + /// 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 { + Matrix E; + Vector b; + std::vector F; + computeJacobians(F, E, b, cameras, point); + whitenJacobians(F, E, b); + Matrix P = Cameras::PointCov(E, lambda, diagonalDamping); + return boost::make_shared >(keys_, F, E, + P, b); + } + + /** + * Return Jacobians as JacobianFactorQ + */ + boost::shared_ptr > createJacobianQFactor( 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); - f->initKeys(); - return f; - } - - // **************************************************************************************************** - 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); + std::vector F; + computeJacobians(F, E, b, cameras, point); + const size_t M = b.size(); + Matrix P = Cameras::PointCov(E, lambda, diagonalDamping); + SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma()); + return boost::make_shared >(keys_, F, E, P, b, n); } - // **************************************************************************************************** + /** + * Return Jacobians as JacobianFactorSVD + * 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 Fblocks; + size_t m = this->keys_.size(); + std::vector F; Vector b; - Matrix Enull(ZDim*numKeys, ZDim*numKeys-3); - computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); - return boost::make_shared< JacobianFactorSVD<6, ZDim> >(Fblocks, Enull, b); + const size_t M = ZDim * m; + Matrix E0(M, M - 3); + computeJacobiansSVD(F, E0, b, cameras, point); + SharedIsotropic n = noiseModel::Isotropic::Sigma(M - 3, + noiseModel_->sigma()); + return boost::make_shared >(keys_, F, E0, b, n); + } + + /// 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, Dim * m); + F.setZero(); + for (size_t i = 0; i < m; ++i) + F.block(ZDim * i, Dim * i) = Fblocks.at(i); + } + + + Pose3 body_P_sensor() const{ + if(body_P_sensor_) + return *body_P_sensor_; + else + return Pose3(); // if unspecified, the transformation is the identity } private: - /// Serialization function +/// 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_); } }; +// end class SmartFactorBase -template -const int SmartFactorBase::ZDim; +// Definitions need to avoid link errors (above are only declarations) +template const int SmartFactorBase::Dim; +template const int SmartFactorBase::ZDim; } // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 2bfcbfaa0..09fe84caa 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -11,7 +11,7 @@ /** * @file SmartProjectionFactor.h - * @brief Base class to create smart factors on poses or cameras + * @brief Smart factor on cameras (pose + calibration) * @author Luca Carlone * @author Zsolt Kira * @author Frank Dellaert @@ -22,7 +22,6 @@ #include #include -#include #include #include @@ -32,110 +31,144 @@ namespace gtsam { -/** - * Structure for storing some state memory, used to speed up optimization - * @addtogroup SLAM - */ -class SmartProjectionFactorState { +/// Linearization mode: what factor to linearize to +enum LinearizationMode { + HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD +}; -protected: +/// How to manage degeneracy +enum DegeneracyMode { + IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY +}; + +/* + * Parameters for the smart projection factors + */ +struct GTSAM_EXPORT SmartProjectionParams { + + LinearizationMode linearizationMode; ///< How to linearize the factor + DegeneracyMode degeneracyMode; ///< How to linearize the factor + + /// @name Parameters governing the triangulation + /// @{ + TriangulationParameters triangulation; + double retriangulationThreshold; ///< threshold to decide whether to re-triangulate + /// @} + + /// @name Parameters governing how triangulation result is treated + /// @{ + bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false) + bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false) + /// @} + + // Constructor + SmartProjectionParams(LinearizationMode linMode = HESSIAN, + DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false, + bool verboseCheirality = false) : + linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold( + 1e-5), throwCheirality(throwCheirality), verboseCheirality( + verboseCheirality) { + } + + virtual ~SmartProjectionParams() { + } + + void print(const std::string& str) const { + std::cout << "linearizationMode: " << linearizationMode << "\n"; + std::cout << " degeneracyMode: " << degeneracyMode << "\n"; + std::cout << triangulation << std::endl; + } + + LinearizationMode getLinearizationMode() const { + return linearizationMode; + } + DegeneracyMode getDegeneracyMode() const { + return degeneracyMode; + } + TriangulationParameters getTriangulationParameters() const { + return triangulation; + } + bool getVerboseCheirality() const { + return verboseCheirality; + } + bool getThrowCheirality() const { + return throwCheirality; + } + void setLinearizationMode(LinearizationMode linMode) { + linearizationMode = linMode; + } + void setDegeneracyMode(DegeneracyMode degMode) { + degeneracyMode = degMode; + } + void setRankTolerance(double rankTol) { + triangulation.rankTolerance = rankTol; + } + void setEnableEPI(bool enableEPI) { + triangulation.enableEPI = enableEPI; + } + void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) { + triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold; + } + void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) { + triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; + } +}; + +/** + * SmartProjectionFactor: triangulates point and keeps an estimate of it around. + * This factor operates with monocular cameras, where a camera is expected to + * behave like PinholeCamera or PinholePose. This factor is intended + * to be used directly with PinholeCamera, which optimizes the camera pose + * and calibration. This also requires that values contains the involved + * cameras (instead of poses and calibrations separately). + * If the calibration is fixed use SmartProjectionPoseFactor instead! + */ +template +class SmartProjectionFactor: public SmartFactorBase { public: - SmartProjectionFactorState() { - } - // Hessian representation (after Schur complement) - bool calculatedHessian; - Matrix H; - Vector gs_vector; - std::vector Gs; - std::vector gs; - double f; -}; +private: + typedef SmartFactorBase Base; + typedef SmartProjectionFactor This; + typedef SmartProjectionFactor SmartProjectionCameraFactor; -enum LinearizationMode { - HESSIAN, JACOBIAN_SVD, JACOBIAN_Q -}; - -/** - * SmartProjectionFactor: triangulates point - * TODO: why LANDMARK parameter? - */ -template -class SmartProjectionFactor: public SmartFactorBase, D> { protected: - // Some triangulation parameters - const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_ - const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate + /// @name Parameters + /// @{ + SmartProjectionParams params_; + /// @} + + /// @name Caching triangulation + /// @{ + mutable TriangulationResult result_; ///< result from triangulateSafe 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, D> 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 SmartProjectionFactor This; - - static const int ZDim = 2; ///< Measurement dimension + /// @} public: /// 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; + /// shorthand for a set of cameras + typedef CameraSet Cameras; + + /** + * Default constructor, only for serialization + */ + SmartProjectionFactor() {} /** * 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 sensor to body frame (default identity) + * @param body_P_sensor pose of the camera in the body frame + * @param params internal parameters of the smart factors */ - SmartProjectionFactor(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 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) { + SmartProjectionFactor(const SharedNoiseModel& sharedNoiseModel, + const boost::optional body_P_sensor = boost::none, + const SmartProjectionParams& params = SmartProjectionParams()) : + Base(sharedNoiseModel, body_P_sensor), params_(params), // + result_(TriangulationResult::Degenerate()) { } /** Virtual destructor */ @@ -149,24 +182,33 @@ public: */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "SmartProjectionFactor, z = \n"; - std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl; - std::cout << "degenerate_ = " << degenerate_ << std::endl; - std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl; + std::cout << s << "SmartProjectionFactor\n"; + std::cout << "linearizationMode:\n" << params_.linearizationMode + << std::endl; + std::cout << "triangulationParameters:\n" << params_.triangulation + << std::endl; + std::cout << "result:\n" << result_ << std::endl; Base::print("", keyFormatter); } - /// Check if the new linearization point_ is the same as the one used for previous triangulation + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + return e && params_.linearizationMode == e->params_.linearizationMode + && Base::equals(p, tol); + } + + /// 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 + // 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_ + // 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 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; @@ -174,7 +216,7 @@ public: if (!retriangulate) { for (size_t i = 0; i < cameras.size(); i++) { if (!cameras[i].pose().equals(cameraPosesTriangulation_[i], - retriangulationThreshold_)) { + params_.retriangulationThreshold)) { retriangulate = true; // at least two poses are different, hence we retriangulate break; } @@ -189,141 +231,39 @@ public: 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 + return retriangulate; // if we arrive to this point all poses are the same and we don't need re-triangulation } /// triangulateSafe - size_t triangulateSafe(const Values& values) const { - return triangulateSafe(this->cameras(values)); - } - - /// triangulateSafe - size_t triangulateSafe(const Cameras& cameras) const { + TriangulationResult 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; - } + if (m < 2) // if we have a single pose the corresponding factor is uninformative + return TriangulationResult::Degenerate(); + bool retriangulate = decideIfTriangulate(cameras); - - if (retriangulate) { - // We triangulate the 3D position of the landmark - try { - // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; - 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; - 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 Point2& zi = this->measured_.at(i); - try { - Point2 reprojectionError(camera.project(point_) - zi); - totalReprojError += reprojectionError.vector().norm(); - } catch (CheiralityException) { - cheiralityException_ = true; - } - i += 1; - } - // 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; + if (retriangulate) + result_ = gtsam::triangulateSafe(cameras, this->measured_, + params_.triangulation); + return result_; } /// 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 << "createRegularImplicitSchurFactor: 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; - } + triangulateSafe(cameras); // imperative, might reset result_ + return bool(result_); } /// linearize returns a Hessianfactor that is an approximation of error(p) - boost::shared_ptr > createHessianFactor( - const Cameras& cameras, const double lambda = 0.0) const { + boost::shared_ptr > createHessianFactor( + const Cameras& cameras, const double lambda = 0.0, bool diagonalDamping = + false) 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); + std::vector js; + std::vector Gs(numKeys * (numKeys + 1) / 2); + std::vector gs(numKeys); if (this->measured_.size() != cameras.size()) { std::cout @@ -332,278 +272,198 @@ public: exit(1); } - this->triangulateSafe(cameras); + triangulateSafe(cameras); - if (numKeys < 2 - || (!this->manageDegeneracy_ - && (this->cheiralityException_ || this->degenerate_))) { - // std::cout << "In linearize: exception" << std::endl; - BOOST_FOREACH(gtsam::Matrix& m, Gs) - m = zeros(D, D); + if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) { + // failed: return"empty" Hessian + BOOST_FOREACH(Matrix& m, Gs) + m = zeros(Base::Dim, Base::Dim); BOOST_FOREACH(Vector& v, gs) - v = zero(D); - return boost::make_shared >(this->keys_, Gs, gs, - 0.0); + v = zero(Base::Dim); + 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; - } - - bool doLinearize = this->decideIfLinearize(cameras); - - 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; - Matrix3 PointCov; + // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols(). + std::vector Fblocks; + Matrix E; Vector b; - double f = computeJacobians(F, E, PointCov, b, cameras, lambda); + computeJacobiansWithTriangulatedPoint(Fblocks, 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; + // Whiten using noise model + Base::whitenJacobians(Fblocks, E, b); - H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); - gs_vector.noalias() = F.transpose() - * (b - (E * (PointCov * (E.transpose() * b)))); - if (isDebug) - std::cout << "gs_vector size " << gs_vector.size() << std::endl; + // build augmented hessian + SymmetricBlockMatrix augmentedHessian = // + Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping); - // 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); + return boost::make_shared >(this->keys_, + augmentedHessian); } // create factor - boost::shared_ptr > createRegularImplicitSchurFactor( + boost::shared_ptr > createRegularImplicitSchurFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) - return Base::createRegularImplicitSchurFactor(cameras, point_, lambda); + return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda); else - return boost::shared_ptr >(); + // failed: return empty + return boost::shared_ptr >(); } /// 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); + return Base::createJacobianQFactor(cameras, *result_, lambda); else - return boost::make_shared< JacobianFactorQ >(this->keys_); + // failed: return empty + 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 ?? - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); - if (nonDegenerate) - return createJacobianQFactor(myCameras, lambda); - else - return boost::make_shared< JacobianFactorQ >(this->keys_); + return createJacobianQFactor(this->cameras(values), lambda); } /// 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); + return Base::createJacobianSVDFactor(cameras, *result_, lambda); else - return boost::make_shared< JacobianFactorSVD >(this->keys_); + // failed: return empty + return boost::make_shared >(this->keys_); } - /// Returns true if nonDegenerate - bool computeCamerasAndTriangulate(const Values& values, - Cameras& myCameras) const { - Values valuesFactor; + /// linearize to a Hessianfactor + virtual boost::shared_ptr > linearizeToHessian( + const Values& values, double lambda = 0.0) const { + return createHessianFactor(this->cameras(values), lambda); + } - // Select only the cameras - BOOST_FOREACH(const Key key, this->keys_) - valuesFactor.insert(key, values.at(key)); + /// linearize to an Implicit Schur factor + virtual boost::shared_ptr > linearizeToImplicit( + const Values& values, double lambda = 0.0) const { + return createRegularImplicitSchurFactor(this->cameras(values), lambda); + } - myCameras = this->cameras(valuesFactor); - size_t nrCameras = this->triangulateSafe(myCameras); + /// linearize to a JacobianfactorQ + virtual boost::shared_ptr > linearizeToJacobian( + const Values& values, double lambda = 0.0) const { + return createJacobianQFactor(this->cameras(values), lambda); + } - 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 << "SmartProjectionFactor: this is not ready" << std::endl; - std::cout << "this->cheiralityException_ " << this->cheiralityException_ - << std::endl; - std::cout << "this->degenerate_ " << this->degenerate_ << std::endl; + /** + * Linearize to Gaussian Factor + * @param values Values structure which must contain camera poses for this factor + * @return a Gaussian factor + */ + boost::shared_ptr linearizeDamped(const Cameras& cameras, + const double lambda = 0.0) const { + // depending on flag set on construction we may linearize to different linear factors + switch (params_.linearizationMode) { + case HESSIAN: + return createHessianFactor(cameras, lambda); + case IMPLICIT_SCHUR: + return createRegularImplicitSchurFactor(cameras, lambda); + case JACOBIAN_SVD: + return createJacobianSVDFactor(cameras, lambda); + case JACOBIAN_Q: + return createJacobianQFactor(cameras, lambda); + default: + throw std::runtime_error("SmartFactorlinearize: unknown mode"); } - return true; } - /// Takes values - bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + /** + * Linearize to Gaussian Factor + * @param values Values structure which must contain camera poses for this factor + * @return a Gaussian factor + */ + boost::shared_ptr linearizeDamped(const Values& values, + const double lambda = 0.0) const { + // depending on flag set on construction we may linearize to different linear factors + Cameras cameras = this->cameras(values); + return linearizeDamped(cameras, lambda); + } + + /// linearize + virtual boost::shared_ptr linearize( + const Values& values) const { + return linearizeDamped(values); + } + + /** + * Triangulate and compute derivative of error with respect to point + * @return whether triangulation worked + */ + bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const { + bool nonDegenerate = triangulateForLinearize(cameras); if (nonDegenerate) - computeEP(E, PointCov, myCameras); + cameras.project2(*result_, boost::none, E); 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 { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); - if (nonDegenerate) - computeJacobians(Fblocks, E, PointCov, b, myCameras, 0.0); - return nonDegenerate; + /** + * Triangulate and compute derivative of error with respect to point + * @return whether triangulation worked + */ + bool triangulateAndComputeE(Matrix& E, const Values& values) const { + Cameras cameras = this->cameras(values); + return triangulateAndComputeE(E, cameras); } /// 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 { + void computeJacobiansWithTriangulatedPoint( + std::vector& Fblocks, Matrix& E, Vector& b, + const Cameras& cameras) const { - if (this->degenerate_) { - std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl; - std::cout << "point " << point_ << std::endl; - std::cout - << "SmartProjectionFactor: 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; + if (!result_) { + // Handle degeneracy + // TODO check flag whether we should do this + Unit3 backProjected = cameras[0].backprojectPointAtInfinity( + this->measured_.at(0)); + Base::computeJacobians(Fblocks, E, b, cameras, backProjected); } else { - // nondegenerate: just return Base version - return Base::computeJacobians(Fblocks, E, b, cameras, point_); - } // end else + // valid result: just return Base version + Base::computeJacobians(Fblocks, E, b, cameras, *result_); + } } - /// 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; + /// Version that takes values, and creates the point + bool triangulateAndComputeJacobians( + std::vector& Fblocks, Matrix& E, Vector& b, + const Values& values) const { + Cameras cameras = this->cameras(values); + bool nonDegenerate = triangulateForLinearize(cameras); + if (nonDegenerate) + computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); + return nonDegenerate; } /// 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, Matrix3& PointCov, Vector& b, - const Cameras& cameras, const double lambda) const { - return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda); - } - - /// 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); + bool triangulateAndComputeJacobiansSVD( + std::vector& Fblocks, Matrix& Enull, Vector& b, + const Values& values) const { + Cameras cameras = this->cameras(values); + bool nonDegenerate = triangulateForLinearize(cameras); if (nonDegenerate) - return reprojectionError(myCameras); + Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_); + return nonDegenerate; + } + + /// Calculate vector of re-projection errors, before applying noise model + Vector reprojectionErrorAfterTriangulation(const Values& values) const { + Cameras cameras = this->cameras(values); + bool nonDegenerate = triangulateForLinearize(cameras); + if (nonDegenerate) + return Base::unwhitenedError(cameras, *result_); else - return zero(myCameras.size() * 2); + return zero(cameras.size() * 2); } /** @@ -615,86 +475,57 @@ public: 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 (externalPoint) + result_ = TriangulationResult(*externalPoint); + else + result_ = triangulateSafe(cameras); - if (nrCameras < 2 - || (!this->manageDegeneracy_ - && (this->cheiralityException_ || this->degenerate_))) { + if (result_) + // All good, just use version in base class + return Base::totalReprojectionError(cameras, *result_); + else if (params_.degeneracyMode == HANDLE_INFINITY) { + // Otherwise, manage the exceptions with rotation-only factors + const Point2& z0 = this->measured_.at(0); + Unit3 backprojected = cameras.front().backprojectPointAtInfinity(z0); + return Base::totalReprojectionError(cameras, backprojected); + } else // 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 Point2& zi = this->measured_.at(i); - if (i == 0) // first pose - this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity - Point2 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_); + /// Calculate total reprojection error + virtual double error(const Values& values) const { + if (this->active(values)) { + return totalReprojectionError(Base::cameras(values)); + } else { // else of active flag + return 0.0; } } - /// Cameras are computed in derived class - virtual Cameras cameras(const Values& values) const = 0; - /** return the landmark */ - boost::optional point() const { - return point_; + TriangulationResult point() const { + return result_; } /** COMPUTE the landmark */ - boost::optional point(const Values& values) const { - triangulateSafe(values); - return point_; + TriangulationResult point(const Values& values) const { + Cameras cameras = this->cameras(values); + return triangulateSafe(cameras); + } + + /// Is result valid? + bool isValid() const { + return result_; } /** return the degenerate state */ - inline bool isDegenerate() const { - return (cheiralityException_ || degenerate_); + bool isDegenerate() const { + return result_.degenerate(); } /** return the cheirality status flag */ - inline bool isPointBehindCamera() const { - return cheiralityException_; - } - /** return cheirality verbosity */ - inline bool verboseCheirality() const { - return verboseCheirality_; - } - - /** return flag for throwing cheirality exceptions */ - inline bool throwCheirality() const { - return throwCheirality_; + bool isPointBehindCamera() const { + return result_.behindCamera(); } private: @@ -704,12 +535,16 @@ private: 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_); + ar & BOOST_SERIALIZATION_NVP(params_.throwCheirality); + ar & BOOST_SERIALIZATION_NVP(params_.verboseCheirality); } +} +; + +/// traits +template +struct traits > : public Testable< + SmartProjectionFactor > { }; -template -const int SmartProjectionFactor::ZDim; - } // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 0801d141f..455e0de87 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -11,7 +11,7 @@ /** * @file SmartProjectionPoseFactor.h - * @brief Produces an Hessian factors on POSES from monocular measurements of a single landmark + * @brief Smart factor on poses, assuming camera calibration is fixed * @author Luca Carlone * @author Chris Beall * @author Zsolt Kira @@ -34,91 +34,52 @@ namespace gtsam { */ /** - * The calibration is known here. The factor only constraints poses (variable dimension is 6) + * This factor assumes that camera calibration is fixed, and that + * the calibration is the same for all cameras involved in this factor. + * The factor only constrains poses (variable dimension is 6). + * This factor requires that values contains the involved poses (Pose3). + * If the calibration should be optimized, as well, use SmartProjectionFactor instead! * @addtogroup SLAM */ -template -class SmartProjectionPoseFactor: public SmartProjectionFactor { +template +class SmartProjectionPoseFactor: public SmartProjectionFactor< + PinholePose > { + +private: + typedef PinholePose Camera; + typedef SmartProjectionFactor Base; + typedef SmartProjectionPoseFactor This; + 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) + boost::shared_ptr K_; ///< calibration object (one for all cameras) public: - /// shorthand for base class type - typedef SmartProjectionFactor Base; - - /// shorthand for this class - typedef SmartProjectionPoseFactor 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 sensor to body frame (default identity) + * Default constructor, only for serialization */ - SmartProjectionPoseFactor(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) {} + SmartProjectionPoseFactor() {} + + /** + * Constructor + * @param Isotropic measurement noise + * @param K (fixed) calibration, assumed to be the same for all cameras + * @param body_P_sensor pose of the camera in the body frame + * @param params internal parameters of the smart factors + */ + SmartProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel, + const boost::shared_ptr K, + const boost::optional body_P_sensor = boost::none, + const SmartProjectionParams& params = SmartProjectionParams()) : + Base(sharedNoiseModel, body_P_sensor, params), K_(K) { + } /** Virtual destructor */ - virtual ~SmartProjectionPoseFactor() {} - - /** - * 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 Point2 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); - } + virtual ~SmartProjectionPoseFactor() { } /** @@ -129,64 +90,15 @@ public: void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "SmartProjectionPoseFactor, 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); } - /// 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 - * 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); - if(Base::body_P_sensor_) - pose = pose.compose(*(Base::body_P_sensor_)); - - 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 : - return this->createJacobianQFactor(cameras(values), 0.0); - break; - default: - return this->createHessianFactor(cameras(values)); - break; - } - } - /** * error calculates the error of the factor. */ @@ -198,9 +110,28 @@ public: } } - /** return the calibration object */ - inline const std::vector > calibration() const { - return K_all_; + /** return calibration shared pointers */ + inline const boost::shared_ptr calibration() const { + return K_; + } + + /** + * 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; + BOOST_FOREACH(const Key& k, this->keys_) { + Pose3 pose = values.at(k); + if (Base::body_P_sensor_) + pose = pose.compose(*(Base::body_P_sensor_)); + + Camera camera(pose, K_); + cameras.push_back(camera); + } + return cameras; } private: @@ -208,15 +139,18 @@ 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_); + ar & BOOST_SERIALIZATION_NVP(K_); } -}; // end of class declaration +}; +// end of class declaration /// traits -template -struct traits > : public Testable > {}; +template +struct traits > : public Testable< + SmartProjectionPoseFactor > { +}; } // \ namespace gtsam diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 7b21e044f..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_); diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index b7f6a20dc..aa50929a5 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -16,9 +16,9 @@ */ #include -#include -#include +#include #include +#include namespace gtsam { @@ -27,19 +27,28 @@ namespace gtsam { * The calibration and pose are assumed known. * @addtogroup SLAM */ -template +template class TriangulationFactor: public NoiseModelFactor1 { public: - /// Camera type - typedef PinholeCamera Camera; + /// CAMERA type + typedef CAMERA Camera; protected: + /// shorthand for base class type + typedef NoiseModelFactor1 Base; + + /// shorthand for this class + typedef TriangulationFactor This; + + /// shorthand for measurement type, e.g. Point2 or StereoPoint2 + typedef typename CAMERA::Measurement Measurement; + // Keep a copy of measurement and calibration for I/O - const Camera camera_; ///< Camera in which this landmark was seen - const Point2 measured_; ///< 2D measurement + const CAMERA camera_; ///< CAMERA in which this landmark was seen + const Measurement measured_; ///< 2D measurement // verbosity handling for Cheirality Exceptions const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) @@ -47,12 +56,6 @@ protected: public: - /// shorthand for base class type - typedef NoiseModelFactor1 Base; - - /// shorthand for this class - typedef TriangulationFactor This; - /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -70,14 +73,17 @@ public: * @param throwCheirality determines whether Cheirality exceptions are rethrown * @param verboseCheirality determines whether exceptions are printed for Cheirality */ - TriangulationFactor(const Camera& camera, const Point2& measured, + TriangulationFactor(const CAMERA& camera, const Measurement& measured, const SharedNoiseModel& model, Key pointKey, bool throwCheirality = false, bool verboseCheirality = false) : Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_( throwCheirality), verboseCheirality_(verboseCheirality) { - if (model && model->dim() != 2) + if (model && model->dim() != Measurement::dimension) throw std::invalid_argument( - "TriangulationFactor must be created with 2-dimensional noise model."); + "TriangulationFactor must be created with " + + boost::lexical_cast((int) Measurement::dimension) + + "-dimensional noise model."); + } /** Virtual destructor */ @@ -114,18 +120,18 @@ public: Vector evaluateError(const Point3& point, boost::optional H2 = boost::none) const { try { - Point2 error(camera_.project(point, boost::none, H2, boost::none) - measured_); + Measurement error(camera_.project2(point, boost::none, H2) - measured_); return error.vector(); } catch (CheiralityException& e) { if (H2) - *H2 = zeros(2, 3); + *H2 = zeros(Measurement::dimension, 3); if (verboseCheirality_) std::cout << e.what() << ": Landmark " << DefaultKeyFormatter(this->key()) << " moved behind camera" << std::endl; if (throwCheirality_) throw e; - return ones(2) * 2.0 * camera_.calibration().fx(); + return ones(Measurement::dimension) * 2.0 * camera_.calibration().fx(); } } @@ -147,14 +153,14 @@ public: // Allocate memory for Jacobian factor, do only once if (Ab.rows() == 0) { std::vector dimensions(1, 3); - Ab = VerticalBlockMatrix(dimensions, 2, true); - A.resize(2,3); - b.resize(2); + Ab = VerticalBlockMatrix(dimensions, Measurement::dimension, true); + A.resize(Measurement::dimension,3); + b.resize(Measurement::dimension); } // Would be even better if we could pass blocks to project const Point3& point = x.at(key()); - b = -(camera_.project(point, boost::none, A, boost::none) - measured_).vector(); + b = -(camera_.project2(point, boost::none, A) - measured_).vector(); if (noiseModel_) this->noiseModel_->WhitenSystem(A, b); @@ -165,7 +171,7 @@ public: } /** return the measurement */ - const Point2& measured() const { + const Measurement& measured() const { return measured_; } @@ -184,7 +190,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 d3a7c1e84..68c27e76b 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -15,19 +15,34 @@ * @brief utility functions for loading datasets */ -#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 using namespace std; namespace fs = boost::filesystem; @@ -64,8 +79,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"); } @@ -232,6 +247,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, // Parse the pose constraints Key id1, id2; bool haveLandmark = false; + const bool useModelInFile = !model; while (!is.eof()) { if (!(is >> tag)) break; @@ -252,7 +268,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, if (maxID && (id1 >= maxID || id2 >= maxID)) continue; - if (!model) + if (useModelInFile) model = modelInFile; if (addNoise) @@ -670,6 +686,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); @@ -704,10 +721,10 @@ bool readBAL(const string& filename, SfM_data &data) { // Get the information for the camera poses for (size_t i = 0; i < nrPoses; i++) { - // Get the rodriguez vector + // Get the Rodrigues vector float wx, wy, wz; is >> wx >> wy >> wz; - Rot3 R = Rot3::rodriguez(wx, wy, wz); // BAL-OpenGL rotation matrix + Rot3 R = Rot3::Rodrigues(wx, wy, wz); // BAL-OpenGL rotation matrix // Get the translation vector float tx, ty, tz; diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 3a0f8edb7..54e27229c 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -18,16 +18,21 @@ #pragma once -#include -#include -#include -#include #include #include +#include +#include +#include +#include +#include +#include +#include +#include -#include -#include // for pair +#include #include +#include // for pair +#include namespace gtsam { @@ -137,11 +142,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/slam/expressions.h b/gtsam/slam/expressions.h index f7314c73f..e81c76bd6 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -8,6 +8,7 @@ #pragma once #include +#include #include #include #include @@ -27,6 +28,7 @@ inline Point2_ transform_to(const Pose2_& x, const Point2_& p) { // 3D Geometry typedef Expression Point3_; +typedef Expression Unit3_; typedef Expression Rot3_; typedef Expression Pose3_; @@ -34,38 +36,61 @@ inline Point3_ transform_to(const Pose3_& x, const Point3_& p) { return Point3_(x, &Pose3::transform_to, p); } +inline Point3_ transform_from(const Pose3_& x, const Point3_& p) { + return Point3_(x, &Pose3::transform_from, p); +} + // Projection typedef Expression Cal3_S2_; typedef Expression Cal3Bundler_; +/// Expression version of PinholeBase::Project inline Point2_ project(const Point3_& p_cam) { - return Point2_(PinholeCamera::project_to_camera, p_cam); + Point2 (*f)(const Point3&, OptionalJacobian<2, 3>) = &PinholeBase::Project; + return Point2_(f, p_cam); } -template -Point2 project4(const CAMERA& camera, const Point3& p, - OptionalJacobian<2, CAMERA::dimension> Dcam, OptionalJacobian<2, 3> Dpoint) { +inline Point2_ project(const Unit3_& p_cam) { + Point2 (*f)(const Unit3&, OptionalJacobian<2, 2>) = &PinholeBase::Project; + return Point2_(f, p_cam); +} + +namespace internal { +// Helper template for project2 expression below +template +Point2 project4(const CAMERA& camera, const POINT& p, + OptionalJacobian<2, CAMERA::dimension> Dcam, + OptionalJacobian<2, FixedDimension::value> Dpoint) { return camera.project2(p, Dcam, Dpoint); } - -template -Point2_ project2(const Expression& camera_, const Point3_& p_) { - return Point2_(project4, camera_, p_); } +template +Point2_ project2(const Expression& camera_, + const Expression& p_) { + return Point2_(internal::project4, camera_, p_); +} + +namespace internal { +// Helper template for project3 expression below +template 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) { + OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, + OptionalJacobian<2, 5> Dcal) { return PinholeCamera(x, K).project(p, Dpose, Dpoint, Dcal); } - -inline Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) { - return Point2_(project6, x, p, K); } -template -Point2_ uncalibrate(const Expression& K, const Point2_& xy_hat) { - return Point2_(K, &CAL::uncalibrate, xy_hat); +template +inline Point2_ project3(const Pose3_& x, const Expression& p, + const Expression& K) { + return Point2_(internal::project6, x, p, K); +} + +template +Point2_ uncalibrate(const Expression& K, const Point2_& xy_hat) { + return Point2_(K, &CALIBRATION::uncalibrate, xy_hat); } } // \namespace gtsam diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h new file mode 100644 index 000000000..8e83ec503 --- /dev/null +++ b/gtsam/slam/tests/smartFactorScenarios.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 + + * -------------------------------------------------------------------------- */ + +/** + * @file SmartFactorScenarios.h + * @brief Scenarios for testSmart*.cpp + * @author Frank Dellaert + * @date Feb 2015 + */ + +#pragma once +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// 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(10, 0.5, 1.2); +Point3 landmark5(10, -0.5, 1.2); + +// First camera pose, 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)); +// Second camera 1 meter to the right of first camera +Pose3 pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); +// Third camera 1 meter above the first camera +Pose3 pose_above = level_pose * Pose3(Rot3(), Point3(0, -1, 0)); + +// Create a noise unit2 for the pixel error +static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); + +static double fov = 60; // degrees +static size_t w = 640, h = 480; + +/* ************************************************************************* */ +// default Cal3_S2 cameras +namespace vanilla { +typedef PinholeCamera Camera; +typedef SmartProjectionFactor SmartFactor; +static Cal3_S2 K(fov, w, h); +static Cal3_S2 K2(1500, 1200, 0, w, h); +Camera level_camera(level_pose, K2); +Camera level_camera_right(pose_right, K2); +Point2 level_uv = level_camera.project(landmark1); +Point2 level_uv_right = level_camera_right.project(landmark1); +Camera cam1(level_pose, K2); +Camera cam2(pose_right, K2); +Camera cam3(pose_above, K2); +typedef GeneralSFMFactor SFMFactor; +SmartProjectionParams params; +} + +/* ************************************************************************* */ +// default Cal3_S2 poses +namespace vanillaPose { +typedef PinholePose Camera; +typedef SmartProjectionPoseFactor SmartFactor; +static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); +Camera level_camera(level_pose, sharedK); +Camera level_camera_right(pose_right, sharedK); +Camera cam1(level_pose, sharedK); +Camera cam2(pose_right, sharedK); +Camera cam3(pose_above, sharedK); +} + +/* ************************************************************************* */ +// default Cal3_S2 poses +namespace vanillaPose2 { +typedef PinholePose Camera; +typedef SmartProjectionPoseFactor SmartFactor; +static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); +Camera level_camera(level_pose, sharedK2); +Camera level_camera_right(pose_right, sharedK2); +Camera cam1(level_pose, sharedK2); +Camera cam2(pose_right, sharedK2); +Camera cam3(pose_above, sharedK2); +} + +/* *************************************************************************/ +// Cal3Bundler cameras +namespace bundler { +typedef PinholeCamera Camera; +typedef SmartProjectionFactor SmartFactor; +static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0); +Camera level_camera(level_pose, K); +Camera level_camera_right(pose_right, K); +Point2 level_uv = level_camera.project(landmark1); +Point2 level_uv_right = level_camera_right.project(landmark1); +Pose3 pose1 = level_pose; +Camera cam1(level_pose, K); +Camera cam2(pose_right, K); +Camera cam3(pose_above, K); +typedef GeneralSFMFactor SFMFactor; +} +/* *************************************************************************/ +// Cal3Bundler poses +namespace bundlerPose { +typedef PinholePose Camera; +typedef SmartProjectionPoseFactor SmartFactor; +static boost::shared_ptr sharedBundlerK( + new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); +Camera level_camera(level_pose, sharedBundlerK); +Camera level_camera_right(pose_right, sharedBundlerK); +Camera cam1(level_pose, sharedBundlerK); +Camera cam2(pose_right, sharedBundlerK); +Camera cam3(pose_above, sharedBundlerK); +} +/* *************************************************************************/ + +template +CAMERA perturbCameraPose(const CAMERA& camera) { + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + Pose3 cameraPose = camera.pose(); + Pose3 perturbedCameraPose = cameraPose.compose(noise_pose); + return CAMERA(perturbedCameraPose, camera.calibration()); +} + +template +void projectToMultipleCameras(const CAMERA& cam1, const CAMERA& cam2, + const CAMERA& cam3, Point3 landmark, vector& measurements_cam) { + Point2 cam1_uv1 = cam1.project(landmark); + Point2 cam2_uv1 = cam2.project(landmark); + Point2 cam3_uv1 = cam3.project(landmark); + measurements_cam.push_back(cam1_uv1); + measurements_cam.push_back(cam2_uv1); + measurements_cam.push_back(cam3_uv1); +} + +/* ************************************************************************* */ + diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index adb759dc0..1e8b330c3 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -19,9 +19,9 @@ using namespace gtsam::noiseModel; * This TEST should fail. If you want it to pass, change noise to 0. */ TEST(BetweenFactor, Rot3) { - Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); - Rot3 R2 = Rot3::rodriguez(0.4, 0.5, 0.6); - Rot3 noise = Rot3(); // Rot3::rodriguez(0.01, 0.01, 0.01); // Uncomment to make unit test fail + Rot3 R1 = Rot3::Rodrigues(0.1, 0.2, 0.3); + Rot3 R2 = Rot3::Rodrigues(0.4, 0.5, 0.6); + Rot3 noise = Rot3(); // Rot3::Rodrigues(0.01, 0.01, 0.01); // Uncomment to make unit test fail Rot3 measured = R1.between(R2)*noise ; BetweenFactor factor(R(1), R(2), measured, Isotropic::Sigma(3, 0.05)); 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/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index e0e26ecff..3bcc3eccd 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -160,7 +160,7 @@ TEST (EssentialMatrixFactor2, factor) { // Check evaluation Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1); - const Point2 pi = SimpleCamera::project_to_camera(P2); + const Point2 pi = PinholeBase::Project(P2); Point2 reprojectionError(pi - pB(i)); Vector expected = reprojectionError.vector(); diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 240b19a46..a634928dc 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -17,20 +17,22 @@ */ #include -#include +#include #include +#include +#include +#include #include -#include #include #include #include -#include -#include +#include #include +#include #include #include -using namespace boost; +using namespace boost::assign; #include #include @@ -49,7 +51,8 @@ typedef NonlinearEquality Point3Constraint; class Graph: public NonlinearFactorGraph { public: - void addMeasurement(int i, int j, const Point2& z, const SharedNoiseModel& model) { + void addMeasurement(int i, int j, const Point2& z, + const SharedNoiseModel& model) { push_back(boost::make_shared(z, model, X(i), L(j))); } @@ -65,98 +68,99 @@ public: }; -static double getGaussian() -{ - double S,V1,V2; - // Use Box-Muller method to create gauss noise from uniform noise - do - { - double U1 = rand() / (double)(RAND_MAX); - double U2 = rand() / (double)(RAND_MAX); - V1 = 2 * U1 - 1; /* V1=[-1,1] */ - V2 = 2 * U2 - 1; /* V2=[-1,1] */ - S = V1 * V1 + V2 * V2; - } while(S>=1); - return sqrt(-2.0f * (double)log(S) / S) * V1; +static double getGaussian() { + double S, V1, V2; + // Use Box-Muller method to create gauss noise from uniform noise + do { + double U1 = rand() / (double) (RAND_MAX); + double U2 = rand() / (double) (RAND_MAX); + V1 = 2 * U1 - 1; /* V1=[-1,1] */ + V2 = 2 * U2 - 1; /* V2=[-1,1] */ + S = V1 * V1 + V2 * V2; + } while (S >= 1); + return sqrt(-2.f * (double) log(S) / S) * V1; } static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); +static const double baseline = 5.; /* ************************************************************************* */ -TEST( GeneralSFMFactor, equals ) -{ - // Create two identical factors and make sure they're equal - Point2 z(323.,240.); - const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); - const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr - factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); +static vector genPoint3() { + const double z = 5; + vector landmarks; + landmarks.push_back(Point3(-1., -1., z)); + landmarks.push_back(Point3(-1., 1., z)); + landmarks.push_back(Point3(1., 1., z)); + landmarks.push_back(Point3(1., -1., z)); + landmarks.push_back(Point3(-1.5, -1.5, 1.5 * z)); + landmarks.push_back(Point3(-1.5, 1.5, 1.5 * z)); + landmarks.push_back(Point3(1.5, 1.5, 1.5 * z)); + landmarks.push_back(Point3(1.5, -1.5, 1.5 * z)); + landmarks.push_back(Point3(-2., -2., 2 * z)); + landmarks.push_back(Point3(-2., 2., 2 * z)); + landmarks.push_back(Point3(2., 2., 2 * z)); + landmarks.push_back(Point3(2., -2., 2 * z)); + return landmarks; +} - boost::shared_ptr - factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); +static vector genCameraDefaultCalibration() { + vector X; + X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.)))); + X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.)))); + return X; +} + +static vector genCameraVariableCalibration() { + const Cal3_S2 K(640, 480, 0.1, 320, 240); + vector X; + X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.)), K)); + X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.)), K)); + return X; +} + +static boost::shared_ptr getOrdering( + const vector& cameras, const vector& landmarks) { + boost::shared_ptr ordering(new Ordering); + for (size_t i = 0; i < landmarks.size(); ++i) + ordering->push_back(L(i)); + for (size_t i = 0; i < cameras.size(); ++i) + ordering->push_back(X(i)); + return ordering; +} + +/* ************************************************************************* */ +TEST( GeneralSFMFactor, equals ) { + // Create two identical factors and make sure they're equal + Point2 z(323., 240.); + const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1); + const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); + boost::shared_ptr factor1( + new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + + boost::shared_ptr factor2( + new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); EXPECT(assert_equal(*factor1, *factor2)); } /* ************************************************************************* */ TEST( GeneralSFMFactor, error ) { - Point2 z(3.,0.); - const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr factor(new Projection(z, sigma, X(1), L(1))); + Point2 z(3., 0.); + const SharedNoiseModel sigma(noiseModel::Unit::Create(2)); + Projection factor(z, sigma, X(1), L(1)); // For the following configuration, the factor predicts 320,240 Values values; Rot3 R; - Point3 t1(0,0,-6); - Pose3 x1(R,t1); + Point3 t1(0, 0, -6); + Pose3 x1(R, t1); values.insert(X(1), GeneralCamera(x1)); - Point3 l1; values.insert(L(1), l1); - EXPECT(assert_equal(((Vector) Vector2(-3.0, 0.0)), factor->unwhitenedError(values))); + Point3 l1; + values.insert(L(1), l1); + EXPECT( + assert_equal(((Vector ) Vector2(-3., 0.)), + factor.unwhitenedError(values))); } -static const double baseline = 5.0 ; - -/* ************************************************************************* */ -static vector genPoint3() { - const double z = 5; - vector landmarks ; - landmarks.push_back(Point3 (-1.0,-1.0, z)); - landmarks.push_back(Point3 (-1.0, 1.0, z)); - landmarks.push_back(Point3 ( 1.0, 1.0, z)); - landmarks.push_back(Point3 ( 1.0,-1.0, z)); - landmarks.push_back(Point3 (-1.5,-1.5, 1.5*z)); - landmarks.push_back(Point3 (-1.5, 1.5, 1.5*z)); - landmarks.push_back(Point3 ( 1.5, 1.5, 1.5*z)); - landmarks.push_back(Point3 ( 1.5,-1.5, 1.5*z)); - landmarks.push_back(Point3 (-2.0,-2.0, 2*z)); - landmarks.push_back(Point3 (-2.0, 2.0, 2*z)); - landmarks.push_back(Point3 ( 2.0, 2.0, 2*z)); - landmarks.push_back(Point3 ( 2.0,-2.0, 2*z)); - return landmarks ; -} - -static vector genCameraDefaultCalibration() { - vector X ; - X.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)))); - X.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)))); - return X ; -} - -static vector genCameraVariableCalibration() { - const Cal3_S2 K(640,480,0.01,320,240); - vector X ; - X.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)), K)); - X.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)), K)); - return X ; -} - -static boost::shared_ptr getOrdering(const vector& cameras, const vector& landmarks) { - boost::shared_ptr ordering(new Ordering); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ; - return ordering ; -} - - /* ************************************************************************* */ TEST( GeneralSFMFactor, optimize_defaultK ) { @@ -165,32 +169,32 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); } graph.addCameraConstraint(0, cameras[0]); // Create an ordering of the variables - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements); @@ -202,38 +206,37 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { vector cameras = genCameraVariableCalibration(); // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); // add noise only to the first landmark - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - if ( i == 0 ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; - } - else { - values.insert(L(i), landmarks[i]) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + if (i == 0) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); + } else { + values.insert(L(i), landmarks[i]); } } graph.addCameraConstraint(0, cameras[0]); const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -246,35 +249,34 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { vector cameras = genCameraVariableCalibration(); // add measurement with noise - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; - graph.addMeasurement(j, i, pt, sigma1); + for (size_t i = 0; i < cameras.size(); ++i) { + for (size_t j = 0; j < landmarks.size(); ++j) { + Point2 z = cameras[i].project(landmarks[j]); + graph.addMeasurement(i, j, z, sigma1); } } - const size_t nMeasurements = landmarks.size()*cameras.size(); + const size_t nMeasurements = landmarks.size() * cameras.size(); Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - //Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z()); - values.insert(L(i), pt) ; + for (size_t j = 0; j < landmarks.size(); ++j) { + Point3 pt(landmarks[j].x() + noise * getGaussian(), + landmarks[j].y() + noise * getGaussian(), + landmarks[j].z() + noise * getGaussian()); + values.insert(L(j), pt); } - for ( size_t i = 0 ; i < cameras.size() ; ++i ) + for (size_t i = 0; i < cameras.size(); ++i) graph.addCameraConstraint(i, cameras[i]); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -288,50 +290,45 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = landmarks.size()*cameras.size(); + const size_t nMeasurements = landmarks.size() * cameras.size(); Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) { - const double - rot_noise = 1e-5, - trans_noise = 1e-3, - focal_noise = 1, - skew_noise = 1e-5; - if ( i == 0 ) { - values.insert(X(i), cameras[i]) ; - } - else { + for (size_t i = 0; i < cameras.size(); ++i) { + const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1, + skew_noise = 1e-5; + if (i == 0) { + values.insert(X(i), cameras[i]); + } else { - Vector delta = (Vector(11) << - rot_noise, rot_noise, rot_noise, // rotation - trans_noise, trans_noise, trans_noise, // translation - focal_noise, focal_noise, // f_x, f_y - skew_noise, // s - trans_noise, trans_noise // ux, uy + Vector delta = (Vector(11) << rot_noise, rot_noise, rot_noise, // rotation + trans_noise, trans_noise, trans_noise, // translation + focal_noise, focal_noise, // f_x, f_y + skew_noise, // s + trans_noise, trans_noise // ux, uy ).finished(); - values.insert(X(i), cameras[i].retract(delta)) ; + values.insert(X(i), cameras[i].retract(delta)); } } - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - values.insert(L(i), landmarks[i]) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + values.insert(L(i), landmarks[i]); } // fix X0 and all landmarks, allow only the cameras[1] to move graph.addCameraConstraint(0, cameras[0]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) + for (size_t i = 0; i < landmarks.size(); ++i) graph.addPoint3Constraint(i, landmarks[i]); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -344,38 +341,40 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); // add noise only to the first landmark - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); } // Constrain position of system with the first camera constrained to the origin graph.addCameraConstraint(0, cameras[0]); // Constrain the scale of the problem with a soft range factor of 1m between the cameras - graph.push_back(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0))); + graph.push_back( + RangeFactor(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 10.))); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -386,17 +385,21 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { // Tests range factor between a GeneralCamera and a Pose3 Graph graph; graph.addCameraConstraint(0, GeneralCamera()); - graph.push_back(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0))); - graph.push_back(PriorFactor(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0))); + graph.push_back( + RangeFactor(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 1.))); + graph.push_back( + PriorFactor(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), + noiseModel::Isotropic::Sigma(6, 1.))); Values init; init.insert(X(0), GeneralCamera()); - init.insert(X(1), Pose3(Rot3(), Point3(1.0,1.0,1.0))); + init.insert(X(1), Pose3(Rot3(), Point3(1., 1., 1.))); // The optimal value between the 2m range factor and 1m prior is 1.5m Values expected; expected.insert(X(0), GeneralCamera()); - expected.insert(X(1), Pose3(Rot3(), Point3(1.5,0.0,0.0))); + expected.insert(X(1), Pose3(Rot3(), Point3(1.5, 0., 0.))); LevenbergMarquardtParams params; params.absoluteErrorTol = 1e-9; @@ -410,16 +413,23 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { // Tests range factor between a CalibratedCamera and a Pose3 NonlinearFactorGraph graph; - graph.push_back(PriorFactor(X(0), CalibratedCamera(), noiseModel::Isotropic::Sigma(6, 1.0))); - graph.push_back(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0))); - graph.push_back(PriorFactor(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0))); + graph.push_back( + PriorFactor(X(0), CalibratedCamera(), + noiseModel::Isotropic::Sigma(6, 1.))); + graph.push_back( + RangeFactor(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 1.))); + graph.push_back( + PriorFactor(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), + noiseModel::Isotropic::Sigma(6, 1.))); Values init; init.insert(X(0), CalibratedCamera()); - init.insert(X(1), Pose3(Rot3(), Point3(1.0,1.0,1.0))); + init.insert(X(1), Pose3(Rot3(), Point3(1., 1., 1.))); Values expected; - expected.insert(X(0), CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0)))); + expected.insert(X(0), + CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0)))); expected.insert(X(1), Pose3(Rot3(), Point3(1.333333333333, 0, 0))); LevenbergMarquardtParams params; @@ -431,5 +441,95 @@ TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +// Frank created these tests after switching to a custom LinearizedFactor +TEST(GeneralSFMFactor, BinaryJacobianFactor) { + Point2 measurement(3., -1.); + + // Create Values + Values values; + Rot3 R; + Point3 t1(0, 0, -6); + Pose3 x1(R, t1); + values.insert(X(1), GeneralCamera(x1)); + Point3 l1; + values.insert(L(1), l1); + + vector models; + { + // Create various noise-models to test all cases + using namespace noiseModel; + Rot2 R = Rot2::fromAngle(0.3); + Matrix2 cov = R.matrix() * R.matrix().transpose(); + models += SharedNoiseModel(), Unit::Create(2), // + Isotropic::Sigma(2, 0.5), Constrained::All(2), Gaussian::Covariance(cov); + } + + // Now loop over all these noise models + BOOST_FOREACH(SharedNoiseModel model, models) { + Projection factor(measurement, model, X(1), L(1)); + + // Test linearize + GaussianFactor::shared_ptr expected = // + factor.NoiseModelFactor::linearize(values); + GaussianFactor::shared_ptr actual = factor.linearize(values); + EXPECT(assert_equal(*expected, *actual, 1e-9)); + + // Test methods that rely on updateHessian + if (model && !model->isConstrained()) { + // Construct HessianFactor from single JacobianFactor + HessianFactor expectedHessian(*expected), actualHessian(*actual); + EXPECT(assert_equal(expectedHessian, actualHessian, 1e-9)); + + // Convert back + JacobianFactor actualJacobian(actualHessian); + // Note we do not expect the actualJacobian to match *expected + // Just that they have the same information on the variable. + EXPECT( + assert_equal(expected->augmentedInformation(), + actualJacobian.augmentedInformation(), 1e-9)); + + // Construct from GaussianFactorGraph + GaussianFactorGraph gfg1; + gfg1 += expected; + GaussianFactorGraph gfg2; + gfg2 += actual; + HessianFactor hessian1(gfg1), hessian2(gfg2); + EXPECT(assert_equal(hessian1, hessian2, 1e-9)); + } + } +} + +/* ************************************************************************* */ +// Do a thorough test of BinaryJacobianFactor +TEST( GeneralSFMFactor, BinaryJacobianFactor2 ) { + + vector landmarks = genPoint3(); + vector cameras = genCameraVariableCalibration(); + + Values values; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); + for (size_t j = 0; j < landmarks.size(); ++j) + values.insert(L(j), landmarks[j]); + + for (size_t i = 0; i < cameras.size(); ++i) { + for (size_t j = 0; j < landmarks.size(); ++j) { + Point2 z = cameras[i].project(landmarks[j]); + Projection::shared_ptr nonlinear = // + boost::make_shared(z, sigma1, X(i), L(j)); + GaussianFactor::shared_ptr factor = nonlinear->linearize(values); + HessianFactor hessian(*factor); + JacobianFactor jacobian(hessian); + EXPECT( + assert_equal(factor->augmentedInformation(), + jacobian.augmentedInformation(), 1e-9)); + } + } +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index df56f5260..a349a4992 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include @@ -49,7 +49,8 @@ typedef NonlinearEquality Point3Constraint; /* ************************************************************************* */ class Graph: public NonlinearFactorGraph { public: - void addMeasurement(const int& i, const int& j, const Point2& z, const SharedNoiseModel& model) { + void addMeasurement(const int& i, const int& j, const Point2& z, + const SharedNoiseModel& model) { push_back(boost::make_shared(z, model, X(i), L(j))); } @@ -65,97 +66,101 @@ public: }; -static double getGaussian() -{ - double S,V1,V2; - // Use Box-Muller method to create gauss noise from uniform noise - do - { - double U1 = rand() / (double)(RAND_MAX); - double U2 = rand() / (double)(RAND_MAX); - V1 = 2 * U1 - 1; /* V1=[-1,1] */ - V2 = 2 * U2 - 1; /* V2=[-1,1] */ - S = V1 * V1 + V2 * V2; - } while(S>=1); - return sqrt(-2.0f * (double)log(S) / S) * V1; +static double getGaussian() { + double S, V1, V2; + // Use Box-Muller method to create gauss noise from uniform noise + do { + double U1 = rand() / (double) (RAND_MAX); + double U2 = rand() / (double) (RAND_MAX); + V1 = 2 * U1 - 1; /* V1=[-1,1] */ + V2 = 2 * U2 - 1; /* V2=[-1,1] */ + S = V1 * V1 + V2 * V2; + } while (S >= 1); + return sqrt(-2.f * (double) log(S) / S) * V1; } static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); /* ************************************************************************* */ -TEST( GeneralSFMFactor_Cal3Bundler, equals ) -{ +TEST( GeneralSFMFactor_Cal3Bundler, equals ) { // Create two identical factors and make sure they're equal - Point2 z(323.,240.); - const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); + Point2 z(323., 240.); + const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr - factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + boost::shared_ptr factor1( + new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); - boost::shared_ptr - factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + boost::shared_ptr factor2( + new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); EXPECT(assert_equal(*factor1, *factor2)); } /* ************************************************************************* */ TEST( GeneralSFMFactor_Cal3Bundler, error ) { - Point2 z(3.,0.); + Point2 z(3., 0.); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr - factor(new Projection(z, sigma, X(1), L(1))); + boost::shared_ptr factor(new Projection(z, sigma, X(1), L(1))); // For the following configuration, the factor predicts 320,240 Values values; Rot3 R; - Point3 t1(0,0,-6); - Pose3 x1(R,t1); + Point3 t1(0, 0, -6); + Pose3 x1(R, t1); values.insert(X(1), GeneralCamera(x1)); - Point3 l1; values.insert(L(1), l1); - EXPECT(assert_equal(Vector2(-3.0, 0.0), factor->unwhitenedError(values))); + Point3 l1; + values.insert(L(1), l1); + EXPECT(assert_equal(Vector2(-3., 0.), factor->unwhitenedError(values))); } - -static const double baseline = 5.0 ; +static const double baseline = 5.; /* ************************************************************************* */ static vector genPoint3() { const double z = 5; - vector landmarks ; - landmarks.push_back(Point3 (-1.0,-1.0, z)); - landmarks.push_back(Point3 (-1.0, 1.0, z)); - landmarks.push_back(Point3 ( 1.0, 1.0, z)); - landmarks.push_back(Point3 ( 1.0,-1.0, z)); - landmarks.push_back(Point3 (-1.5,-1.5, 1.5*z)); - landmarks.push_back(Point3 (-1.5, 1.5, 1.5*z)); - landmarks.push_back(Point3 ( 1.5, 1.5, 1.5*z)); - landmarks.push_back(Point3 ( 1.5,-1.5, 1.5*z)); - landmarks.push_back(Point3 (-2.0,-2.0, 2*z)); - landmarks.push_back(Point3 (-2.0, 2.0, 2*z)); - landmarks.push_back(Point3 ( 2.0, 2.0, 2*z)); - landmarks.push_back(Point3 ( 2.0,-2.0, 2*z)); - return landmarks ; + vector landmarks; + landmarks.push_back(Point3(-1., -1., z)); + landmarks.push_back(Point3(-1., 1., z)); + landmarks.push_back(Point3(1., 1., z)); + landmarks.push_back(Point3(1., -1., z)); + landmarks.push_back(Point3(-1.5, -1.5, 1.5 * z)); + landmarks.push_back(Point3(-1.5, 1.5, 1.5 * z)); + landmarks.push_back(Point3(1.5, 1.5, 1.5 * z)); + landmarks.push_back(Point3(1.5, -1.5, 1.5 * z)); + landmarks.push_back(Point3(-2., -2., 2 * z)); + landmarks.push_back(Point3(-2., 2., 2 * z)); + landmarks.push_back(Point3(2., 2., 2 * z)); + landmarks.push_back(Point3(2., -2., 2 * z)); + return landmarks; } static vector genCameraDefaultCalibration() { - vector cameras ; - cameras.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)))); - cameras.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)))); - return cameras ; + vector cameras; + cameras.push_back( + GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)))); + cameras.push_back( + GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)))); + return cameras; } static vector genCameraVariableCalibration() { - const Cal3Bundler K(500,1e-3,1e-3); - vector cameras ; - cameras.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)), K)); - cameras.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)), K)); - return cameras ; + const Cal3Bundler K(500, 1e-3, 1e-3); + vector cameras; + cameras.push_back( + GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)), K)); + cameras.push_back( + GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)), K)); + return cameras; } -static boost::shared_ptr getOrdering(const std::vector& cameras, const std::vector& landmarks) { +static boost::shared_ptr getOrdering( + const std::vector& cameras, + const std::vector& landmarks) { boost::shared_ptr ordering(new Ordering); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ; - return ordering ; + for (size_t i = 0; i < landmarks.size(); ++i) + ordering->push_back(L(i)); + for (size_t i = 0; i < cameras.size(); ++i) + ordering->push_back(X(i)); + return ordering; } /* ************************************************************************* */ @@ -166,32 +171,32 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_defaultK ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); } graph.addCameraConstraint(0, cameras[0]); // Create an ordering of the variables - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements); @@ -203,38 +208,37 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_SingleMeasurementError ) { vector cameras = genCameraVariableCalibration(); // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); // add noise only to the first landmark - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - if ( i == 0 ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; - } - else { - values.insert(L(i), landmarks[i]) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + if (i == 0) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); + } else { + values.insert(L(i), landmarks[i]); } } graph.addCameraConstraint(0, cameras[0]); const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -247,35 +251,35 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixCameras ) { vector cameras = genCameraVariableCalibration(); // add measurement with noise - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = landmarks.size()*cameras.size(); + const size_t nMeasurements = landmarks.size() * cameras.size(); Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); + for (size_t i = 0; i < landmarks.size(); ++i) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); //Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z()); - values.insert(L(i), pt) ; + values.insert(L(i), pt); } - for ( size_t i = 0 ; i < cameras.size() ; ++i ) + for (size_t i = 0; i < cameras.size(); ++i) graph.addCameraConstraint(i, cameras[i]); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -289,46 +293,43 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixLandmarks ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = landmarks.size()*cameras.size(); + const size_t nMeasurements = landmarks.size() * cameras.size(); Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) { - const double - rot_noise = 1e-5, trans_noise = 1e-3, - focal_noise = 1, distort_noise = 1e-3; - if ( i == 0 ) { - values.insert(X(i), cameras[i]) ; - } - else { + for (size_t i = 0; i < cameras.size(); ++i) { + const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1, + distort_noise = 1e-3; + if (i == 0) { + values.insert(X(i), cameras[i]); + } else { - Vector delta = (Vector(9) << - rot_noise, rot_noise, rot_noise, // rotation - trans_noise, trans_noise, trans_noise, // translation - focal_noise, distort_noise, distort_noise // f, k1, k2 + Vector delta = (Vector(9) << rot_noise, rot_noise, rot_noise, // rotation + trans_noise, trans_noise, trans_noise, // translation + focal_noise, distort_noise, distort_noise // f, k1, k2 ).finished(); - values.insert(X(i), cameras[i].retract(delta)) ; + values.insert(X(i), cameras[i].retract(delta)); } } - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - values.insert(L(i), landmarks[i]) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + values.insert(L(i), landmarks[i]); } // fix X0 and all landmarks, allow only the cameras[1] to move graph.addCameraConstraint(0, cameras[0]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) + for (size_t i = 0; i < landmarks.size(); ++i) graph.addPoint3Constraint(i, landmarks[i]); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -341,43 +342,48 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); // add noise only to the first landmark - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); } // Constrain position of system with the first camera constrained to the origin graph.addCameraConstraint(0, cameras[0]); // Constrain the scale of the problem with a soft range factor of 1m between the cameras - graph.push_back(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0))); + graph.push_back( + RangeFactor(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 10.))); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index 61c4566bf..9fd8474eb 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -246,7 +246,7 @@ TEST( InitializePose3, initializePoses ) inputGraph->add(PriorFactor(0, Pose3(), priorModel)); Values initial = InitializePose3::initialize(*inputGraph); - EXPECT(assert_equal(*expectedValues,initial,1e-4)); + EXPECT(assert_equal(*expectedValues,initial,0.1)); // TODO(frank): very loose !! } 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/testRegularHessianFactor.cpp b/gtsam/slam/tests/testRegularHessianFactor.cpp deleted file mode 100644 index 8dfb753f4..000000000 --- a/gtsam/slam/tests/testRegularHessianFactor.cpp +++ /dev/null @@ -1,100 +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 testRegularHessianFactor.cpp - * @author Frank Dellaert - * @date March 4, 2014 - */ - -#include -#include - -//#include -#include - -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace boost::assign; - -const double tol = 1e-5; - -/* ************************************************************************* */ -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)); - - // 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(1, xvalues, fast_y.data()); - EXPECT(assert_equal(expected_y, fast_y)); - - // now, do it with non-zero y - factor.multiplyHessianAdd(1, 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 29eaf2ac1..77944da83 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -1,21 +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 + + * -------------------------------------------------------------------------- */ + /** - * @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 -#include #include #include #include +#include #include #include @@ -31,8 +41,8 @@ using namespace gtsam; const Matrix26 F0 = Matrix26::Ones(); const Matrix26 F1 = 2 * Matrix26::Ones(); const Matrix26 F3 = 3 * Matrix26::Ones(); -const vector > Fblocks = list_of > // - (make_pair(0, F0))(make_pair(1, F1))(make_pair(3, F3)); +const vector FBlocks = list_of(F0)(F1)(F3); +const FastVector keys = list_of(0)(1)(3); // RHS and sigmas const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished(); @@ -43,7 +53,7 @@ TEST( regularImplicitSchurFactor, creation ) { E.block<2,2>(0, 0) = eye(2); E.block<2,3>(2, 0) = 2 * ones(2, 3); Matrix3 P = (E.transpose() * E).inverse(); - RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); + RegularImplicitSchurFactor expected(keys, FBlocks, E, P, b); Matrix expectedP = expected.getPointCovariance(); EXPECT(assert_equal(expectedP, P)); } @@ -76,15 +86,15 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { F << F0, zeros(2, d * 3), zeros(2, d), F1, zeros(2, d*2), zeros(2, d * 3), F3; // Calculate expected result F'*alpha*(I - E*P*E')*F*x - FastVector keys; - keys += 0,1,2,3; - Vector x = xvalues.vector(keys); + FastVector keys2; + keys2 += 0,1,2,3; + Vector x = xvalues.vector(keys2); Vector expected = zero(24); - RegularImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected); - EXPECT(assert_equal(expected, yExpected.vector(keys), 1e-8)); + RegularImplicitSchurFactor::multiplyHessianAdd(F, E, P, alpha, x, expected); + EXPECT(assert_equal(expected, yExpected.vector(keys2), 1e-8)); // Create ImplicitSchurFactor - RegularImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b); + RegularImplicitSchurFactor implicitFactor(keys, FBlocks, E, P, b); VectorValues zero = 0 * yExpected;// quick way to get zero w right structure { // First Version @@ -114,32 +124,34 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { // Create JacobianFactor with same error const SharedDiagonal model; - JacobianFactorQ<6, 2> jf(Fblocks, E, P, b, model); + JacobianFactorQ<6, 2> jfQ(keys, FBlocks, E, P, b, model); - { // error - double expectedError = jf.error(xvalues); - double actualError = implicitFactor.errorJF(xvalues); - DOUBLES_EQUAL(expectedError,actualError,1e-7) + // error + double expectedError = 11875.083333333334; + { + EXPECT_DOUBLES_EQUAL(expectedError,jfQ.error(xvalues),1e-7) + EXPECT_DOUBLES_EQUAL(expectedError,implicitFactor.errorJF(xvalues),1e-7) + EXPECT_DOUBLES_EQUAL(11903.500000000007,implicitFactor.error(xvalues),1e-7) } - { // JacobianFactor with same error + { VectorValues yActual = zero; - jf.multiplyHessianAdd(alpha, xvalues, yActual); + jfQ.multiplyHessianAdd(alpha, xvalues, yActual); EXPECT(assert_equal(yExpected, yActual, 1e-8)); - jf.multiplyHessianAdd(alpha, xvalues, yActual); + jfQ.multiplyHessianAdd(alpha, xvalues, yActual); EXPECT(assert_equal(2 * yExpected, yActual, 1e-8)); - jf.multiplyHessianAdd(-1, xvalues, yActual); + jfQ.multiplyHessianAdd(-1, xvalues, yActual); EXPECT(assert_equal(zero, yActual, 1e-8)); } { // check hessian Diagonal - VectorValues diagExpected = jf.hessianDiagonal(); + VectorValues diagExpected = jfQ.hessianDiagonal(); VectorValues diagActual = implicitFactor.hessianDiagonal(); EXPECT(assert_equal(diagExpected, diagActual, 1e-8)); } { // check hessian Block Diagonal - map BD = jf.hessianBlockDiagonal(); + map BD = jfQ.hessianBlockDiagonal(); map actualBD = implicitFactor.hessianBlockDiagonal(); LONGS_EQUAL(3,actualBD.size()); EXPECT(assert_equal(BD[0],actualBD[0])); @@ -149,40 +161,46 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { { // Raw memory Version std::fill(y, y + 24, 0);// zero y ! - jf.multiplyHessianAdd(alpha, xdata, y); + jfQ.multiplyHessianAdd(alpha, xdata, y); EXPECT(assert_equal(expected, XMap(y), 1e-8)); - jf.multiplyHessianAdd(alpha, xdata, y); + jfQ.multiplyHessianAdd(alpha, xdata, y); EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8)); - jf.multiplyHessianAdd(-1, xdata, y); + jfQ.multiplyHessianAdd(-1, xdata, y); EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8)); } + VectorValues expectedVV; + expectedVV.insert(0,-3.5*ones(6)); + expectedVV.insert(1,10*ones(6)/3); + expectedVV.insert(3,-19.5*ones(6)); { // Check gradientAtZero - VectorValues expected = jf.gradientAtZero(); VectorValues actual = implicitFactor.gradientAtZero(); - EXPECT(assert_equal(expected, actual, 1e-8)); + EXPECT(assert_equal(expectedVV, jfQ.gradientAtZero(), 1e-8)); + EXPECT(assert_equal(expectedVV, implicitFactor.gradientAtZero(), 1e-8)); } // Create JacobianFactorQR - JacobianFactorQR<6, 2> jfq(Fblocks, E, P, b, model); + JacobianFactorQR<6, 2> jfQR(keys, FBlocks, E, P, b, model); + EXPECT_DOUBLES_EQUAL(expectedError, jfQR.error(xvalues),1e-7) + EXPECT(assert_equal(expectedVV, jfQR.gradientAtZero(), 1e-8)); { const SharedDiagonal model; VectorValues yActual = zero; - jfq.multiplyHessianAdd(alpha, xvalues, yActual); + jfQR.multiplyHessianAdd(alpha, xvalues, yActual); EXPECT(assert_equal(yExpected, yActual, 1e-8)); - jfq.multiplyHessianAdd(alpha, xvalues, yActual); + jfQR.multiplyHessianAdd(alpha, xvalues, yActual); EXPECT(assert_equal(2 * yExpected, yActual, 1e-8)); - jfq.multiplyHessianAdd(-1, xvalues, yActual); + jfQR.multiplyHessianAdd(-1, xvalues, yActual); EXPECT(assert_equal(zero, yActual, 1e-8)); } { // Raw memory Version std::fill(y, y + 24, 0);// zero y ! - jfq.multiplyHessianAdd(alpha, xdata, y); + jfQR.multiplyHessianAdd(alpha, xdata, y); EXPECT(assert_equal(expected, XMap(y), 1e-8)); - jfq.multiplyHessianAdd(alpha, xdata, y); + jfQR.multiplyHessianAdd(alpha, xdata, y); EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8)); - jfq.multiplyHessianAdd(-1, xdata, y); + jfQR.multiplyHessianAdd(-1, xdata, y); EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8)); } delete [] y; @@ -206,7 +224,7 @@ TEST(regularImplicitSchurFactor, hessianDiagonal) E.block<2,3>(2, 0) << 1,2,3,4,5,6; E.block<2,3>(4, 0) << 0.5,1,2,3,4,5; Matrix3 P = (E.transpose() * E).inverse(); - RegularImplicitSchurFactor<6> factor(Fblocks, E, P, b); + RegularImplicitSchurFactor factor(keys, FBlocks, E, P, b); // hessianDiagonal VectorValues expected; @@ -247,6 +265,18 @@ TEST(regularImplicitSchurFactor, hessianDiagonal) EXPECT(assert_equal(F0t*(I2-E0*P*E0.transpose())*F0,actualBD[0])); EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1])); EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3])); + + // augmentedInformation (test just checks diagonals) + Matrix actualInfo = factor.augmentedInformation(); + EXPECT(assert_equal(actualBD[0],actualInfo.block<6,6>(0,0))); + EXPECT(assert_equal(actualBD[1],actualInfo.block<6,6>(6,6))); + EXPECT(assert_equal(actualBD[3],actualInfo.block<6,6>(12,12))); + + // information (test just checks diagonals) + Matrix actualInfo2 = factor.information(); + EXPECT(assert_equal(actualBD[0],actualInfo2.block<6,6>(0,0))); + EXPECT(assert_equal(actualBD[1],actualInfo2.block<6,6>(6,6))); + EXPECT(assert_equal(actualBD[3],actualInfo2.block<6,6>(12,12))); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index 9c99628e6..1283987d4 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -29,9 +29,9 @@ Rot3 iRc(cameraX, cameraY, cameraZ); // Now, let's create some rotations around IMU frame Unit3 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1); -Rot3 i1Ri2 = Rot3::rodriguez(p1, 1), // -i2Ri3 = Rot3::rodriguez(p2, 1), // -i3Ri4 = Rot3::rodriguez(p3, 1); +Rot3 i1Ri2 = Rot3::AxisAngle(p1, 1), // +i2Ri3 = Rot3::AxisAngle(p2, 1), // +i3Ri4 = Rot3::AxisAngle(p3, 1); // The corresponding rotations in the camera frame Rot3 c1Zc2 = iRc.inverse() * i1Ri2 * iRc, // @@ -47,9 +47,9 @@ typedef noiseModel::Isotropic::shared_ptr Model; //************************************************************************* TEST (RotateFactor, checkMath) { - EXPECT(assert_equal(c1Zc2, Rot3::rodriguez(z1, 1))); - EXPECT(assert_equal(c2Zc3, Rot3::rodriguez(z2, 1))); - EXPECT(assert_equal(c3Zc4, Rot3::rodriguez(z3, 1))); + EXPECT(assert_equal(c1Zc2, Rot3::AxisAngle(z1, 1))); + EXPECT(assert_equal(c2Zc3, Rot3::AxisAngle(z2, 1))); + EXPECT(assert_equal(c3Zc4, Rot3::AxisAngle(z3, 1))); } //************************************************************************* diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp new file mode 100644 index 000000000..96052bd0f --- /dev/null +++ b/gtsam/slam/tests/testSmartFactorBase.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 testSmartFactorBase.cpp + * @brief Unit tests for testSmartFactorBase Class + * @author Frank Dellaert + * @date Feb 2015 + */ + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); +static SharedNoiseModel unit3(noiseModel::Unit::Create(3)); + +/* ************************************************************************* */ +#include +#include + +namespace gtsam { + +class PinholeFactor: public SmartFactorBase > { +public: + typedef SmartFactorBase > Base; + PinholeFactor() {} + PinholeFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) { + } + 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()); + } +}; + +/// traits +template<> +struct traits : public Testable {}; +} + +TEST(SmartFactorBase, Pinhole) { + PinholeFactor f= PinholeFactor(unit2); + f.add(Point2(), 1); + f.add(Point2(), 2); + EXPECT_LONGS_EQUAL(2 * 2, f.dim()); +} + +/* ************************************************************************* */ +#include + +namespace gtsam { + +class StereoFactor: public SmartFactorBase { +public: + typedef SmartFactorBase Base; + StereoFactor() {} + StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) { + } + 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()); + } +}; + +/// traits +template<> +struct traits : public Testable {}; +} + +TEST(SmartFactorBase, Stereo) { + StereoFactor f(unit3); + f.add(StereoPoint2(), 1); + f.add(StereoPoint2(), 2); + EXPECT_LONGS_EQUAL(2 * 3, f.dim()); +} + +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +TEST(SmartFactorBase, serialize) { + using namespace gtsam::serializationTestHelpers; + PinholeFactor factor(unit2); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp new file mode 100644 index 000000000..54bbd6c22 --- /dev/null +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -0,0 +1,873 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSmartProjectionCameraFactor.cpp + * @brief Unit tests for SmartProjectionCameraFactor Class + * @author Chris Beall + * @author Luca Carlone + * @author Zsolt Kira + * @author Frank Dellaert + * @date Sept 2013 + */ + +#include "smartFactorScenarios.h" +#include +#include +#include +#include +#include +#include + +using namespace boost::assign; + +static bool isDebugTest = false; + +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; + +static Key x1(1); +Symbol l1('l', 1), l2('l', 2), l3('l', 3); +Key c1 = 1, c2 = 2, c3 = 3; + +static Point2 measurement1(323.0, 240.0); + +static double rankTol = 1.0; + +template +PinholeCamera perturbCameraPoseAndCalibration( + const PinholeCamera& camera) { + GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + Pose3 cameraPose = camera.pose(); + Pose3 perturbedCameraPose = cameraPose.compose(noise_pose); + typename gtsam::traits::TangentVector d; + d.setRandom(); + d *= 0.1; + CALIBRATION perturbedCalibration = camera.calibration().retract(d); + return PinholeCamera(perturbedCameraPose, perturbedCalibration); +} + +/* ************************************************************************* */ +TEST( SmartProjectionCameraFactor, perturbCameraPose) { + using namespace vanilla; + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + Pose3 perturbed_level_pose = level_pose.compose(noise_pose); + Camera actualCamera(perturbed_level_pose, K2); + + Camera expectedCamera = perturbCameraPose(level_camera); + CHECK(assert_equal(expectedCamera, actualCamera)); +} + +/* ************************************************************************* */ +TEST( SmartProjectionCameraFactor, Constructor) { + using namespace vanilla; + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); +} + +/* ************************************************************************* */ +TEST( SmartProjectionCameraFactor, Constructor2) { + using namespace vanilla; + params.setRankTolerance(rankTol); + SmartFactor factor1(unit2, boost::none, params); +} + +/* ************************************************************************* */ +TEST( SmartProjectionCameraFactor, Constructor3) { + using namespace vanilla; + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(measurement1, x1); +} + +/* ************************************************************************* */ +TEST( SmartProjectionCameraFactor, Constructor4) { + using namespace vanilla; + params.setRankTolerance(rankTol); + SmartFactor factor1(unit2, boost::none, params); + factor1.add(measurement1, x1); +} + +/* ************************************************************************* */ +TEST( SmartProjectionCameraFactor, Equals ) { + using namespace vanilla; + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(measurement1, x1); + + SmartFactor::shared_ptr factor2(new SmartFactor(unit2)); + factor2->add(measurement1, x1); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, noiseless ) { + using namespace vanilla; + + Values values; + values.insert(c1, level_camera); + values.insert(c2, level_camera_right); + + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(level_uv, c1); + factor1->add(level_uv_right, c2); + + double expectedError = 0.0; + DOUBLES_EQUAL(expectedError, factor1->error(values), 1e-7); + CHECK( + assert_equal(zero(4), + factor1->reprojectionErrorAfterTriangulation(values), 1e-7)); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, noisy ) { + + using namespace vanilla; + + // Project one landmark into two cameras and add noise on first + Point2 level_uv = level_camera.project(landmark1) + Point2(0.2, 0.2); + Point2 level_uv_right = level_camera_right.project(landmark1); + + Values values; + values.insert(c1, level_camera); + Camera perturbed_level_camera_right = perturbCameraPose(level_camera_right); + values.insert(c2, perturbed_level_camera_right); + + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(level_uv, c1); + factor1->add(level_uv_right, c2); + + // Point is now uninitialized before a triangulation event + EXPECT(!factor1->point()); + + double expectedError = 58640; + double actualError1 = factor1->error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError1, 1); + + // Check triangulated point + CHECK(factor1->point()); + EXPECT( + assert_equal(Point3(13.7587, 1.43851, -1.14274), *factor1->point(), 1e-4)); + + // Check whitened errors + Vector expected(4); + expected << -7, 235, 58, -242; + SmartFactor::Cameras cameras1 = factor1->cameras(values); + Point3 point1 = *factor1->point(); + Vector actual = factor1->whitenedError(cameras1, point1); + EXPECT(assert_equal(expected, actual, 1)); + + SmartFactor::shared_ptr factor2(new SmartFactor(unit2)); + vector measurements; + measurements.push_back(level_uv); + measurements.push_back(level_uv_right); + + vector views; + views.push_back(c1); + views.push_back(c2); + + factor2->add(measurements, views); + + double actualError2 = factor2->error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) { + + using namespace vanilla; + + // Project three landmarks into three cameras + vector measurements_cam1, measurements_cam2, measurements_cam3; + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + // Create and fill smartfactors + SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2)); + vector views; + views.push_back(c1); + views.push_back(c2); + views.push_back(c3); + smartFactor1->add(measurements_cam1, views); + smartFactor2->add(measurements_cam2, views); + smartFactor3->add(measurements_cam3, views); + + // Create factor graph and add priors on two cameras + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); + graph.push_back(PriorFactor(c1, cam1, noisePrior)); + graph.push_back(PriorFactor(c2, cam2, noisePrior)); + + // Create initial estimate + Values initial; + initial.insert(c1, cam1); + initial.insert(c2, cam2); + initial.insert(c3, perturbCameraPose(cam3)); + if (isDebugTest) + initial.at(c3).print("Smart: Pose3 before optimization: "); + + // Points are now uninitialized before a triangulation event + EXPECT(!smartFactor1->point()); + EXPECT(!smartFactor2->point()); + EXPECT(!smartFactor3->point()); + + EXPECT_DOUBLES_EQUAL(75711, smartFactor1->error(initial), 1); + EXPECT_DOUBLES_EQUAL(58524, smartFactor2->error(initial), 1); + EXPECT_DOUBLES_EQUAL(77564, smartFactor3->error(initial), 1); + + // Error should trigger this and initialize the points, abort if not so + CHECK(smartFactor1->point()); + CHECK(smartFactor2->point()); + CHECK(smartFactor3->point()); + + EXPECT( + assert_equal(Point3(2.57696, -0.182566, 1.04085), *smartFactor1->point(), + 1e-4)); + EXPECT( + assert_equal(Point3(2.80114, -0.702153, 1.06594), *smartFactor2->point(), + 1e-4)); + EXPECT( + assert_equal(Point3(1.82593, -0.289569, 2.13438), *smartFactor3->point(), + 1e-4)); + + // Check whitened errors + Vector expected(6); + expected << 256, 29, -26, 29, -206, -202; + Point3 point1 = *smartFactor1->point(); + SmartFactor::Cameras cameras1 = smartFactor1->cameras(initial); + Vector reprojectionError = cameras1.reprojectionError(point1, + measurements_cam1); + EXPECT(assert_equal(expected, reprojectionError, 1)); + Vector actual = smartFactor1->whitenedError(cameras1, point1); + EXPECT(assert_equal(expected, actual, 1)); + + // Optimize + LevenbergMarquardtParams lmParams; + if (isDebugTest) { + lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + lmParams.verbosity = NonlinearOptimizerParams::ERROR; + } + LevenbergMarquardtOptimizer optimizer(graph, initial, lmParams); + Values result = optimizer.optimize(); + + EXPECT(assert_equal(landmark1, *smartFactor1->point(), 1e-5)); + EXPECT(assert_equal(landmark2, *smartFactor2->point(), 1e-5)); + EXPECT(assert_equal(landmark3, *smartFactor3->point(), 1e-5)); + + // GaussianFactorGraph::shared_ptr GFG = graph.linearize(initial); + // VectorValues delta = GFG->optimize(); + + if (isDebugTest) + result.at(c3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(cam1, result.at(c1))); + EXPECT(assert_equal(cam2, result.at(c2))); + EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-4)); + EXPECT( + assert_equal(result.at(c3).calibration(), cam3.calibration(), 2)); + if (isDebugTest) + tictoc_print_(); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) { + + using namespace vanilla; + + // Project three landmarks into three cameras + vector measurements_cam1, measurements_cam2, measurements_cam3; + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + vector views; + views.push_back(c1); + views.push_back(c2); + views.push_back(c3); + + SfM_Track track1; + for (size_t i = 0; i < 3; ++i) { + SfM_Measurement measures; + measures.first = i + 1; // cameras are from 1 to 3 + measures.second = measurements_cam1.at(i); + track1.measurements.push_back(measures); + } + SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); + smartFactor1->add(track1); + + SfM_Track track2; + for (size_t i = 0; i < 3; ++i) { + SfM_Measurement measures; + measures.first = i + 1; // cameras are from 1 to 3 + measures.second = measurements_cam2.at(i); + track2.measurements.push_back(measures); + } + SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); + smartFactor2->add(track2); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2)); + smartFactor3->add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(c1, cam1, noisePrior)); + graph.push_back(PriorFactor(c2, cam2, noisePrior)); + + Values values; + values.insert(c1, cam1); + values.insert(c2, cam2); + values.insert(c3, perturbCameraPose(cam3)); + if (isDebugTest) + values.at(c3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams lmParams; + if (isDebugTest) + lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + lmParams.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + + // GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); + // VectorValues delta = GFG->optimize(); + + if (isDebugTest) + result.at(c3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(cam1, result.at(c1))); + EXPECT(assert_equal(cam2, result.at(c2))); + EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-4)); + EXPECT( + assert_equal(result.at(c3).calibration(), cam3.calibration(), 2)); + if (isDebugTest) + tictoc_print_(); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) { + + using namespace vanilla; + + vector measurements_cam1, measurements_cam2, measurements_cam3, + measurements_cam4, measurements_cam5; + + // 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); + projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5); + + vector views; + views.push_back(c1); + views.push_back(c2); + views.push_back(c3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2)); + smartFactor3->add(measurements_cam3, views); + + SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2)); + smartFactor4->add(measurements_cam4, views); + + SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2)); + smartFactor5->add(measurements_cam5, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(smartFactor4); + graph.push_back(smartFactor5); + graph.push_back(PriorFactor(c1, cam1, noisePrior)); + graph.push_back(PriorFactor(c2, cam2, noisePrior)); + + Values values; + values.insert(c1, cam1); + values.insert(c2, cam2); + values.insert(c3, perturbCameraPoseAndCalibration(cam3)); + if (isDebugTest) + values.at(c3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams lmParams; + lmParams.relativeErrorTol = 1e-8; + lmParams.absoluteErrorTol = 0; + lmParams.maxIterations = 20; + if (isDebugTest) + lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + lmParams.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + + // GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); + // VectorValues delta = GFG->optimize(); + + if (isDebugTest) + result.at(c3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(cam1, result.at(c1))); + EXPECT(assert_equal(cam2, result.at(c2))); + EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-1)); + EXPECT( + assert_equal(result.at(c3).calibration(), cam3.calibration(), 20)); + if (isDebugTest) + tictoc_print_(); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, Cal3Bundler ) { + + using namespace bundler; + + vector measurements_cam1, measurements_cam2, measurements_cam3, + measurements_cam4, measurements_cam5; + + // 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); + projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5); + + vector views; + views.push_back(c1); + views.push_back(c2); + views.push_back(c3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2)); + smartFactor3->add(measurements_cam3, views); + + SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2)); + smartFactor4->add(measurements_cam4, views); + + SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2)); + smartFactor5->add(measurements_cam5, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(c1, cam1, noisePrior)); + graph.push_back(PriorFactor(c2, cam2, noisePrior)); + + Values values; + values.insert(c1, cam1); + values.insert(c2, cam2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(c3, perturbCameraPose(cam3)); + if (isDebugTest) + values.at(c3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams lmParams; + lmParams.relativeErrorTol = 1e-8; + lmParams.absoluteErrorTol = 0; + lmParams.maxIterations = 20; + if (isDebugTest) + lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + lmParams.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + + if (isDebugTest) + result.at(c3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(cam1, result.at(c1), 1e-4)); + EXPECT(assert_equal(cam2, result.at(c2), 1e-4)); + EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-1)); + EXPECT( + assert_equal(result.at(c3).calibration(), cam3.calibration(), 1)); + if (isDebugTest) + tictoc_print_(); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) { + + using namespace bundler; + + vector measurements_cam1, measurements_cam2, measurements_cam3, + measurements_cam4, measurements_cam5; + + // 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); + projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5); + + vector views; + views.push_back(c1); + views.push_back(c2); + views.push_back(c3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2)); + smartFactor3->add(measurements_cam3, views); + + SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2)); + smartFactor4->add(measurements_cam4, views); + + SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2)); + smartFactor5->add(measurements_cam5, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(c1, cam1, noisePrior)); + graph.push_back(PriorFactor(c2, cam2, noisePrior)); + + Values values; + values.insert(c1, cam1); + values.insert(c2, cam2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(c3, perturbCameraPoseAndCalibration(cam3)); + if (isDebugTest) + values.at(c3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams lmParams; + lmParams.relativeErrorTol = 1e-8; + lmParams.absoluteErrorTol = 0; + lmParams.maxIterations = 20; + if (isDebugTest) + lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + lmParams.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + + if (isDebugTest) + result.at(c3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(cam1, result.at(c1), 1e-4)); + EXPECT(assert_equal(cam2, result.at(c2), 1e-4)); + EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-1)); + EXPECT( + assert_equal(result.at(c3).calibration(), cam3.calibration(), 2)); + if (isDebugTest) + tictoc_print_(); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, noiselessBundler ) { + + using namespace bundler; + Values values; + values.insert(c1, level_camera); + values.insert(c2, level_camera_right); + + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(level_uv, c1); + factor1->add(level_uv_right, c2); + + double actualError = factor1->error(values); + + double expectedError = 0.0; + DOUBLES_EQUAL(expectedError, actualError, 1e-3); + + Point3 oldPoint; // this takes the point stored in the factor (we are not interested in this) + if (factor1->point()) + oldPoint = *(factor1->point()); + + Point3 expectedPoint; + if (factor1->point(values)) + expectedPoint = *(factor1->point(values)); + + EXPECT(assert_equal(expectedPoint, landmark1, 1e-3)); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) { + + using namespace bundler; + Values values; + values.insert(c1, level_camera); + values.insert(c2, level_camera_right); + + NonlinearFactorGraph smartGraph; + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(level_uv, c1); + factor1->add(level_uv_right, c2); + smartGraph.push_back(factor1); + double expectedError = factor1->error(values); + double expectedErrorGraph = smartGraph.error(values); + Point3 expectedPoint; + if (factor1->point()) + expectedPoint = *(factor1->point()); + // cout << "expectedPoint " << expectedPoint.vector() << endl; + + // COMMENTS: + // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as + // value in the generalGrap + NonlinearFactorGraph generalGraph; + SFMFactor sfm1(level_uv, unit2, c1, l1); + SFMFactor sfm2(level_uv_right, unit2, c2, l1); + generalGraph.push_back(sfm1); + generalGraph.push_back(sfm2); + values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation + Vector e1 = sfm1.evaluateError(values.at(c1), values.at(l1)); + Vector e2 = sfm2.evaluateError(values.at(c2), values.at(l1)); + double actualError = 0.5 + * (norm_2(e1) * norm_2(e1) + norm_2(e2) * norm_2(e2)); + double actualErrorGraph = generalGraph.error(values); + + DOUBLES_EQUAL(expectedErrorGraph, actualErrorGraph, 1e-7); + DOUBLES_EQUAL(expectedErrorGraph, expectedError, 1e-7); + DOUBLES_EQUAL(actualErrorGraph, actualError, 1e-7); + DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) { + + using namespace bundler; + Values values; + values.insert(c1, level_camera); + values.insert(c2, level_camera_right); + + NonlinearFactorGraph smartGraph; + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(level_uv, c1); + factor1->add(level_uv_right, c2); + smartGraph.push_back(factor1); + Matrix expectedHessian = smartGraph.linearize(values)->hessian().first; + Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second; + Point3 expectedPoint; + if (factor1->point()) + expectedPoint = *(factor1->point()); + + // COMMENTS: + // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as + // value in the generalGrap + NonlinearFactorGraph generalGraph; + SFMFactor sfm1(level_uv, unit2, c1, l1); + SFMFactor sfm2(level_uv_right, unit2, c2, l1); + generalGraph.push_back(sfm1); + generalGraph.push_back(sfm2); + values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation + Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first; + Matrix actualFullInfoVector = generalGraph.linearize(values)->hessian().second; + Matrix actualHessian = actualFullHessian.block(0, 0, 18, 18) + - actualFullHessian.block(0, 18, 18, 3) + * (actualFullHessian.block(18, 18, 3, 3)).inverse() + * actualFullHessian.block(18, 0, 3, 18); + Vector actualInfoVector = actualFullInfoVector.block(0, 0, 18, 1) + - actualFullHessian.block(0, 18, 18, 3) + * (actualFullHessian.block(18, 18, 3, 3)).inverse() + * actualFullInfoVector.block(18, 0, 3, 1); + + EXPECT(assert_equal(expectedHessian, actualHessian, 1e-7)); + EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-7)); +} + +/* *************************************************************************/ +// Have to think about how to compare multiplyHessianAdd in generalSfMFactor and smartFactors +//TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor2 ){ +// +// Values values; +// values.insert(c1, level_camera); +// values.insert(c2, level_camera_right); +// +// NonlinearFactorGraph smartGraph; +// SmartFactor::shared_ptr factor1(new SmartFactor()); +// factor1->add(level_uv, c1, unit2); +// factor1->add(level_uv_right, c2, unit2); +// smartGraph.push_back(factor1); +// GaussianFactorGraph::shared_ptr gfgSmart = smartGraph.linearize(values); +// +// Point3 expectedPoint; +// if(factor1->point()) +// expectedPoint = *(factor1->point()); +// +// // COMMENTS: +// // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as +// // value in the generalGrap +// NonlinearFactorGraph generalGraph; +// SFMFactor sfm1(level_uv, unit2, c1, l1); +// SFMFactor sfm2(level_uv_right, unit2, c2, l1); +// generalGraph.push_back(sfm1); +// generalGraph.push_back(sfm2); +// values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation +// GaussianFactorGraph::shared_ptr gfg = generalGraph.linearize(values); +// +// double alpha = 1.0; +// +// VectorValues yExpected, yActual, ytmp; +// VectorValues xtmp = map_list_of +// (c1, (Vec(9) << 0,0,0,0,0,0,0,0,0)) +// (c2, (Vec(9) << 0,0,0,0,0,0,0,0,0)) +// (l1, (Vec(3) << 5.5, 0.5, 1.2)); +// gfg ->multiplyHessianAdd(alpha, xtmp, ytmp); +// +// VectorValues x = map_list_of +// (c1, (Vec(9) << 1,2,3,4,5,6,7,8,9)) +// (c2, (Vec(9) << 11,12,13,14,15,16,17,18,19)) +// (l1, (Vec(3) << 5.5, 0.5, 1.2)); +// +// gfgSmart->multiplyHessianAdd(alpha, ytmp + x, yActual); +// gfg ->multiplyHessianAdd(alpha, x, yExpected); +// +// EXPECT(assert_equal(yActual,yExpected, 1e-7)); +//} +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) { + + using namespace bundler; + Values values; + values.insert(c1, level_camera); + values.insert(c2, level_camera_right); + + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(level_uv, c1); + factor1->add(level_uv_right, c2); + Matrix expectedE; + Vector expectedb; + + CameraSet cameras; + cameras.push_back(level_camera); + cameras.push_back(level_camera_right); + + factor1->error(values); // this is important to have a triangulation of the point + Point3 point; + if (factor1->point()) + point = *(factor1->point()); + vector Fblocks; + factor1->computeJacobians(Fblocks, expectedE, expectedb, cameras, point); + + NonlinearFactorGraph generalGraph; + SFMFactor sfm1(level_uv, unit2, c1, l1); + SFMFactor sfm2(level_uv_right, unit2, c2, l1); + generalGraph.push_back(sfm1); + generalGraph.push_back(sfm2); + values.insert(l1, point); // note: we get rid of possible errors in the triangulation + Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first; + Matrix actualVinv = (actualFullHessian.block(18, 18, 3, 3)).inverse(); + + Matrix3 expectedVinv = factor1->PointCov(expectedE); + EXPECT(assert_equal(expectedVinv, actualVinv, 1e-7)); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { + + using namespace bundler; + + Values values; + values.insert(c1, level_camera); + values.insert(c2, level_camera_right); + double rankTol = 1; + bool useEPI = false; + + // Hessian version + SmartProjectionParams params; + params.setRankTolerance(rankTol); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setEnableEPI(useEPI); + + SmartFactor::shared_ptr explicitFactor( + new SmartFactor(unit2, boost::none, params)); + explicitFactor->add(level_uv, c1); + explicitFactor->add(level_uv_right, c2); + + GaussianFactor::shared_ptr gaussianHessianFactor = explicitFactor->linearize( + values); + HessianFactor& hessianFactor = + dynamic_cast(*gaussianHessianFactor); + + // Implicit Schur version + params.setLinearizationMode(gtsam::IMPLICIT_SCHUR); + SmartFactor::shared_ptr implicitFactor( + new SmartFactor(unit2, boost::none, params)); + implicitFactor->add(level_uv, c1); + implicitFactor->add(level_uv_right, c2); + GaussianFactor::shared_ptr gaussianImplicitSchurFactor = + implicitFactor->linearize(values); + CHECK(gaussianImplicitSchurFactor); + typedef RegularImplicitSchurFactor Implicit9; + Implicit9& implicitSchurFactor = + dynamic_cast(*gaussianImplicitSchurFactor); + + VectorValues x = map_list_of(c1, + (Vector(9) << 1, 2, 3, 4, 5, 6, 7, 8, 9).finished())(c2, + (Vector(9) << 11, 12, 13, 14, 15, 16, 17, 18, 19).finished()); + + VectorValues yExpected, yActual; + double alpha = 1.0; + hessianFactor.multiplyHessianAdd(alpha, x, yActual); + implicitSchurFactor.multiplyHessianAdd(alpha, x, yExpected); + EXPECT(assert_equal(yActual, yExpected, 1e-7)); +} + + +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +TEST( SmartProjectionCameraFactor, serialize) { + using namespace vanilla; + using namespace gtsam::serializationTestHelpers; + SmartFactor factor(unit2); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index c2855f09b..0e2429840 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -10,290 +10,190 @@ * -------------------------------------------------------------------------- */ /** - * @file TestSmartProjectionPoseFactor.cpp + * @file testSmartProjectionPoseFactor.cpp * @brief Unit tests for ProjectionFactor Class * @author Chris Beall * @author Luca Carlone * @author Zsolt Kira + * @author Frank Dellaert * @date Sept 2013 */ -#include "../SmartProjectionPoseFactor.h" - -#include -#include +#include "smartFactorScenarios.h" #include -#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 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 double rankTol = 1.0; -static double linThreshold = -1.0; -static bool manageDegeneracy = true; +static const double rankTol = 1.0; // Create a noise model for the pixel error -static SharedNoiseModel model(noiseModel::Unit::Create(2)); +static const double sigma = 0.1; +static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma)); // 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)); -typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionPoseFactor SmartFactorBundler; - -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); - Point2 cam3_uv1 = cam3.project(landmark); - measurements_cam.push_back(cam1_uv1); - measurements_cam.push_back(cam2_uv1); - measurements_cam.push_back(cam3_uv1); -} +LevenbergMarquardtParams lmParams; +// Make more verbose like so (in tests): +// params.verbosityLM = LevenbergMarquardtParams::SUMMARY; /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor) { - SmartFactor::shared_ptr factor1(new SmartFactor()); + using namespace vanillaPose; + SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor2) { - SmartFactor factor1(rankTol, linThreshold); + using namespace vanillaPose; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactor factor1(model, sharedK, boost::none, params); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor3) { - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(measurement1, poseKey1, model, K); + using namespace vanillaPose; + SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); + factor1->add(measurement1, x1); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor4) { - SmartFactor factor1(rankTol, linThreshold); - factor1.add(measurement1, poseKey1, model, K); -} - -/* ************************************************************************* */ -TEST( SmartProjectionPoseFactor, ConstructorWithTransform) { - bool manageDegeneracy = true; - bool enableEPI = false; - SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor1); - factor1.add(measurement1, poseKey1, model, K); + using namespace vanillaPose; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactor factor1(model, sharedK, boost::none, params); + factor1.add(measurement1, x1); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Equals ) { - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(measurement1, poseKey1, model, K); + using namespace vanillaPose; + SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); + factor1->add(measurement1, x1); - SmartFactor::shared_ptr factor2(new SmartFactor()); - factor2->add(measurement1, poseKey1, model, K); + SmartFactor::shared_ptr factor2(new SmartFactor(model,sharedK)); + factor2->add(measurement1, x1); CHECK(assert_equal(*factor1, *factor2)); } /* *************************************************************************/ -TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ){ - // cout << " ************************ SmartProjectionPoseFactor: noisy ****************************" << endl; +TEST( SmartProjectionPoseFactor, noiseless ) { - // 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)); - SimpleCamera level_camera(level_pose, *K2); + using namespace vanillaPose; - // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera level_camera_right(level_pose_right, *K2); + // Project two landmarks into two cameras + Point2 level_uv = level_camera.project(landmark1); + Point2 level_uv_right = level_camera_right.project(landmark1); - // landmark ~5 meters infront of camera - Point3 landmark(5, 0.5, 1.2); + SmartFactor factor(model, sharedK); + factor.add(level_uv, x1); + factor.add(level_uv_right, x2); - // 1. Project two landmarks into two cameras and triangulate - Point2 level_uv = level_camera.project(landmark); - Point2 level_uv_right = level_camera_right.project(landmark); + Values values; // it's a pose factor, hence these are poses + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); - Values values; - 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); - - 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); - //EXPECT(assert_equal(zero(4),actual,1e-8)); + // Calculate expected derivative for point (easiest to check) + boost::function f = // + boost::bind(&SmartFactor::whitenedError, factor, cameras, _1); + + // Calculate using computeEP + Matrix actualE; + factor.triangulateAndComputeE(actualE, values); + + // get point + boost::optional point = factor.point(); + CHECK(point); + + // calculate numerical derivative with triangulated point + Matrix expectedE = sigma * numericalDerivative11(f, *point); + EXPECT(assert_equal(expectedE, actualE, 1e-7)); + + // Calculate using reprojectionError + SmartFactor::Cameras::FBlocks F; + Matrix E; + Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); + EXPECT(assert_equal(expectedE, E, 1e-7)); + + EXPECT(assert_equal(zero(4), actualErrors, 1e-7)); + + // Calculate using computeJacobians + Vector b; + vector Fblocks; + factor.computeJacobians(Fblocks, E, b, cameras, *point); + double actualError3 = b.squaredNorm(); + EXPECT(assert_equal(expectedE, E, 1e-7)); + EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-8); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, noisy ){ - // cout << " ************************ SmartProjectionPoseFactor: noisy ****************************" << endl; +TEST( SmartProjectionPoseFactor, noisy ) { - // 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)); - SimpleCamera level_camera(level_pose, *K2); + using namespace vanillaPose; - // create second camera 1 meter to the right of first camera - 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 level_uv = level_camera.project(landmark) + pixelError; - Point2 level_uv_right = level_camera_right.project(landmark); + // Project two landmarks into two cameras + Point2 pixelError(0.2, 0.2); + Point2 level_uv = level_camera.project(landmark1) + pixelError; + Point2 level_uv_right = level_camera_right.project(landmark1); 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)); - values.insert(x2, level_pose_right.compose(noise_pose)); + values.insert(x1, cam1.pose()); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + values.insert(x2, 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(model, sharedK)); + factor->add(level_uv, x1); + factor->add(level_uv_right, x2); - double actualError1= factor1->error(values); + double actualError1 = factor->error(values); - SmartFactor::shared_ptr factor2(new SmartFactor()); + SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK)); vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); - std::vector< SharedNoiseModel > noises; - noises.push_back(model); - noises.push_back(model); - - std::vector< boost::shared_ptr > 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); - + factor2->add(measurements, views); + double actualError2 = factor2->error(values); DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } - -/* *************************************************************************/ -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)); - SimpleCamera cam1(pose1, *K2); - - // create second camera 1 meter to the right of first camera - 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)); - SimpleCamera 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); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 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); - - std::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), 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 - 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_(SmartProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - gttoc_(SmartProjectionPoseFactor); - 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),1e-8)); - if(isDebugTest) tictoc_print_(); -} - /* *************************************************************************/ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ + // make a realistic calibration matrix + double fov = 60; // degrees + size_t w=640,h=480; + + Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); // 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 @@ -330,19 +230,19 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ views.push_back(x2); views.push_back(x3); - double rankTol = 1; - double linThreshold = -1; - bool manageDegeneracy = false; - bool enableEPI = false; + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setEnableEPI(false); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartProjectionPoseFactor smartFactor1(model, K, sensor_to_body, params); + smartFactor1.add(measurements_cam1, views); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartProjectionPoseFactor smartFactor2(model, K, sensor_to_body, params); + smartFactor2.add(measurements_cam2, views); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartProjectionPoseFactor smartFactor3(model, K, sensor_to_body, params); + smartFactor3.add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -370,58 +270,37 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(x3, bodyPose3*noise_pose); - LevenbergMarquardtParams params; + LevenbergMarquardtParams lmParams; Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); 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))); } - /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ - // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; - - std::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)); - SimpleCamera cam1(pose1, *K); - - // create second camera 1 meter to the right of first camera - 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)); - SimpleCamera 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); +TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { + using namespace vanillaPose2; vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate + // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1, views, model, K); + vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - smartFactor2->add(measurements_cam2, views, model, K); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); + smartFactor1->add(measurements_cam1, views); - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); - smartFactor3->add(measurements_cam3, views, model, K); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK2)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK2)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -429,73 +308,227 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ 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)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), 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 + Values groundTruth; + groundTruth.insert(x1, cam1.pose()); + groundTruth.insert(x2, cam2.pose()); + groundTruth.insert(x3, cam3.pose()); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // 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.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * 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))); Values result; - gttic_(SmartProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - 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-7)); - if(isDebugTest) tictoc_print_(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-8)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, jacobianSVD ){ +TEST( SmartProjectionPoseFactor, Factors ) { - std::vector views; + using namespace vanillaPose; + + // Default cameras for simple derivatives + Rot3 R; + static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0)); + Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2( + Pose3(R, Point3(1, 0, 0)), sharedK); + + // 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 + vector views; + views.push_back(x1); + views.push_back(x2); + + SmartFactor::shared_ptr smartFactor1 = boost::make_shared(model, sharedK); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::Cameras cameras; + cameras.push_back(cam1); + cameras.push_back(cam2); + + // Make sure triangulation works + CHECK(smartFactor1->triangulateSafe(cameras)); + CHECK(!smartFactor1->isDegenerate()); + CHECK(!smartFactor1->isPointBehindCamera()); + boost::optional p = smartFactor1->point(); + CHECK(p); + EXPECT(assert_equal(landmark1, *p)); + + VectorValues zeroDelta; + Vector6 delta; + delta.setZero(); + zeroDelta.insert(x1, delta); + zeroDelta.insert(x2, delta); + + VectorValues perturbedDelta; + delta.setOnes(); + perturbedDelta.insert(x1, delta); + perturbedDelta.insert(x2, delta); + double expectedError = 2500; + + // After eliminating the point, A1 and A2 contain 2-rank information on cameras: + Matrix16 A1, A2; + A1 << -10, 0, 0, 0, 1, 0; + A2 << 10, 0, 1, 0, -1, 0; + A1 *= 10. / sigma; + A2 *= 10. / sigma; + Matrix expectedInformation; // filled below + { + // 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); + expectedInformation = expected.information(); + + boost::shared_ptr > actual = + smartFactor1->createHessianFactor(cameras, 0.0); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8)); + EXPECT(assert_equal(expected, *actual, 1e-8)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-8); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 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; + Matrix E(4, 3); + E.setZero(); + E(0, 0) = 10; + E(1, 1) = 10; + E(2, 0) = 10; + E(2, 2) = 1; + E(3, 1) = 10; + vector Fblocks = list_of(F1)(F2); + Vector b(4); + b.setZero(); + + // Create smart factors + FastVector keys; + keys.push_back(x1); + keys.push_back(x2); + + // createJacobianQFactor + SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma); + Matrix3 P = (E.transpose() * E).inverse(); + JacobianFactorQ<6, 2> expectedQ(keys, Fblocks, E, P, b, n); + EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-8)); + + boost::shared_ptr > actualQ = + smartFactor1->createJacobianQFactor(cameras, 0.0); + CHECK(actualQ); + EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-8)); + EXPECT(assert_equal(expectedQ, *actualQ)); + EXPECT_DOUBLES_EQUAL(0, actualQ->error(zeroDelta), 1e-8); + EXPECT_DOUBLES_EQUAL(expectedError, actualQ->error(perturbedDelta), 1e-8); + + // Whiten for RegularImplicitSchurFactor (does not have noise model) + model->WhitenSystem(E, b); + Matrix3 whiteP = (E.transpose() * E).inverse(); + Fblocks[0] = model->Whiten(Fblocks[0]); + Fblocks[1] = model->Whiten(Fblocks[1]); + + // createRegularImplicitSchurFactor + RegularImplicitSchurFactor expected(keys, Fblocks, E, whiteP, b); + + boost::shared_ptr > actual = + smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); + CHECK(actual); + EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8)); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8)); + EXPECT(assert_equal(expected, *actual)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-8); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-8); + } + + { + // createJacobianSVDFactor + Vector1 b; + b.setZero(); + double s = sigma * sin(M_PI_4); + SharedIsotropic n = noiseModel::Isotropic::Sigma(4 - 3, sigma); + JacobianFactor expected(x1, s * A1, x2, s * A2, b, n); + EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8)); + + boost::shared_ptr actual = + smartFactor1->createJacobianSVDFactor(cameras, 0.0); + CHECK(actual); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8)); + EXPECT(assert_equal(expected, *actual)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-8); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-8); + } +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { + + using namespace vanillaPose; + + 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)); - SimpleCamera cam1(pose1, *K); - // create second camera 1 meter to the right of first camera - 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)); - SimpleCamera 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 + // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); 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)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK)); + smartFactor1->add(measurements_cam1, views); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK)); + smartFactor2->add(measurements_cam2, views); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -503,63 +536,127 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ){ 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)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), 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(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * 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; Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3), 1e-8)); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, landmarkDistance ){ +TEST( SmartProjectionPoseFactor, jacobianSVD ) { + + using namespace vanillaPose; + + vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setEnableEPI(false); + SmartFactor factor1(model, sharedK, boost::none, params); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views); + + 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, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), 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, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, pose_above * noise_pose); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-8)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, landmarkDistance ) { + + using namespace vanillaPose; 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)); - SimpleCamera cam1(pose1, *K); - // create second camera 1 meter to the right of first camera - 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)); - SimpleCamera 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 + // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); 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)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(false); - 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 smartFactor1( + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -567,75 +664,70 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ){ 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)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), 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(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, pose_above * noise_pose); // All factors are disabled and pose should remain where it is - LevenbergMarquardtParams params; Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); 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 ) { + + using namespace vanillaPose; 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)); - SimpleCamera cam1(pose1, *K); - // create second camera 1 meter to the right of first camera - 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)); - SimpleCamera 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); + // add fourth landmark 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 + // Project 4 landmarks into three cameras 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)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartProjectionParams params; + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setDynamicOutlierRejectionThreshold(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 smartFactor1( + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views); - 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 smartFactor2( + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views); - SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor4->add(measurements_cam4, views, model, K); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views); + + SmartFactor::shared_ptr smartFactor4( + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor4->add(measurements_cam4, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -644,61 +736,52 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ 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)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, cam3.pose()); // All factors are disabled and pose should remain where it is - LevenbergMarquardtParams params; Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(cam3.pose(), result.at(x3))); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, jacobianQ ){ +TEST( SmartProjectionPoseFactor, jacobianQ ) { - std::vector views; + using namespace vanillaPose; + + 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)); - SimpleCamera cam1(pose1, *K); - // create second camera 1 meter to the right of first camera - 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)); - SimpleCamera 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 + // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); 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)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartProjectionParams params; + params.setLinearizationMode(gtsam::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 smartFactor1( + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -706,565 +789,485 @@ TEST( SmartProjectionPoseFactor, jacobianQ ){ 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)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), 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 / 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(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, pose_above * noise_pose); - LevenbergMarquardtParams params; Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3), 1e-8)); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-8)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){ - // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; +TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { - std::vector views; + using namespace vanillaPose2; + + 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)); - SimpleCamera cam1(pose1, *K2); - - // create second camera 1 meter to the right of first camera - 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)); - SimpleCamera 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 GenericProjectionFactor 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)); + // Project three landmarks into three cameras + graph.push_back( + ProjectionFactor(cam1.project(landmark1), model, x1, L(1), sharedK2)); + graph.push_back( + ProjectionFactor(cam2.project(landmark1), model, x2, L(1), sharedK2)); + graph.push_back( + ProjectionFactor(cam3.project(landmark1), model, x3, L(1), sharedK2)); - 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), sharedK2)); + graph.push_back( + ProjectionFactor(cam2.project(landmark2), model, x2, L(2), sharedK2)); + graph.push_back( + ProjectionFactor(cam3.project(landmark2), model, x3, L(2), sharedK2)); - 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), sharedK2)); + graph.push_back( + ProjectionFactor(cam2.project(landmark3), model, x2, L(3), sharedK2)); + graph.push_back( + ProjectionFactor(cam3.project(landmark3), model, x3, L(3), sharedK2)); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.push_back(PriorFactor(x1, level_pose, noisePrior)); + graph.push_back(PriorFactor(x2, pose_right, 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(x1, level_pose); + values.insert(x2, pose_right); + values.insert(x3, pose_above * 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); + DOUBLES_EQUAL(48406055, graph.error(values), 1); + + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); Values result = optimizer.optimize(); - if(isDebugTest) result.at(x3).print("Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3), 1e-7)); + DOUBLES_EQUAL(0, graph.error(result), 1e-9); + + EXPECT(assert_equal(pose_above, 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)); - SimpleCamera cam1(pose1, *K); + using namespace vanillaPose; - // 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)); - 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)); - SimpleCamera 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); + // Two slightly different cameras + Pose3 pose2 = level_pose + * 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)); + Camera cam2(pose2, sharedK); + Camera cam3(pose3, sharedK); vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate + // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - double rankTol = 10; + SmartProjectionParams params; + params.setRankTolerance(10); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default + smartFactor1->add(measurements_cam1, views); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default + smartFactor2->add(measurements_cam2, views); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default + smartFactor3->add(measurements_cam3, views); NonlinearFactorGraph graph; graph.push_back(smartFactor1); 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(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + 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 factor1 = smartFactor1->linearize(values); + boost::shared_ptr factor2 = smartFactor2->linearize(values); + boost::shared_ptr factor3 = smartFactor3->linearize(values); - Matrix CumulativeInformation = hessianFactor1->information() + hessianFactor2->information() + hessianFactor3->information(); + Matrix CumulativeInformation = factor1->information() + factor2->information() + + factor3->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 = factor1->augmentedInformation() + + factor2->augmentedInformation() + factor3->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 ){ - // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; +TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) { + using namespace vanillaPose2; - 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)); - SimpleCamera cam1(pose1, *K2); + // Two different cameras, at the same position, but different rotations + Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3()); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3()); + Camera cam2(pose2, sharedK2); + Camera cam3(pose3, sharedK2); - // 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)); - SimpleCamera cam2(pose2, *K2); + vector measurements_cam1, measurements_cam2; - // 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)); - SimpleCamera 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 + // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); 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); + SmartProjectionParams params; + params.setRankTolerance(50); + params.setDegeneracyMode(gtsam::HANDLE_INFINITY); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor2->add(measurements_cam2, views, model, K2); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK2, boost::none, params)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK2, boost::none, params)); + smartFactor2->add(measurements_cam2, views); 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); + 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(PriorFactor(x1, cam1.pose(), 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), 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*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(x1, cam1.pose()); + values.insert(x2, pose2 * noise_pose); + values.insert(x3, pose3 * noise_pose); - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; - - Values result; - gttic_(SmartProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - gttoc_(SmartProjectionPoseFactor); - 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_(); + // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + Values result = optimizer.optimize(); + EXPECT(assert_equal(pose3, result.at(x3))); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ - // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; +TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) { - std::vector views; + using namespace vanillaPose; + + 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)); - 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)); - 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)); - SimpleCamera 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); + // Two different cameras, at the same position, but different rotations + Pose3 pose2 = level_pose + * 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)); + Camera cam2(pose2, sharedK); + Camera cam3(pose3, sharedK); vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate + // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - double rankTol = 10; + SmartProjectionParams params; + params.setRankTolerance(10); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views); 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(PriorFactor(x1, cam1.pose(), 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), 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: "); - - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + 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))); Values result; - gttic_(SmartProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - gttoc_(SmartProjectionPoseFactor); - 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_(); + // Since we do not do anything on degenerate instances (ZERO_ON_DEGENERACY) + // rotation remains the same as the initial guess, but position is fixed by PoseTranslationPrior + EXPECT(assert_equal(Pose3(values.at(x3).rotation(), + Point3(0,0,1)), result.at(x3))); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Hessian ){ - // cout << " ************************ SmartProjectionPoseFactor: Hessian **********************" << endl; +TEST( SmartProjectionPoseFactor, Hessian ) { - std::vector views; + using namespace vanillaPose2; + + 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)); - SimpleCamera cam1(pose1, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera 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 + // Project three landmarks into 2 cameras Point2 cam1_uv1 = cam1.project(landmark1); Point2 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); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); + smartFactor1->add(measurements_cam1, views); - 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(x1, cam1.pose()); + values.insert(x2, cam2.pose()); - boost::shared_ptr hessianFactor = smartFactor1->linearize(values); - if(isDebugTest) hessianFactor->print("Hessian factor \n"); + boost::shared_ptr factor = smartFactor1->linearize(values); // 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) + // compare with factor.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( SmartProjectionPoseFactor, HessianWithRotation ){ +TEST( SmartProjectionPoseFactor, HessianWithRotation ) { // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian **********************" << endl; - std::vector views; + using namespace vanillaPose; + + 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)); - SimpleCamera cam1(pose1, *K); - - // create second camera 1 meter to the right of first camera - 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)); - SimpleCamera cam3(pose3, *K); - - Point3 landmark1(5, 0.5, 1.2); - vector measurements_cam1, measurements_cam2, measurements_cam3; projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); - smartFactorInstance->add(measurements_cam1, views, model, K); + SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(model, sharedK)); + smartFactorInstance->add(measurements_cam1, views); Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, cam3.pose()); - boost::shared_ptr hessianFactor = smartFactorInstance->linearize(values); - // hessianFactor->print("Hessian factor \n"); + boost::shared_ptr factor = smartFactorInstance->linearize( + values); - 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)); + rotValues.insert(x1, poseDrift.compose(level_pose)); + rotValues.insert(x2, poseDrift.compose(pose_right)); + rotValues.insert(x3, poseDrift.compose(pose_above)); - boost::shared_ptr hessianFactorRot = smartFactorInstance->linearize(rotValues); - // hessianFactorRot->print("Hessian factor \n"); + boost::shared_ptr factorRot = smartFactorInstance->linearize( + rotValues); // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + EXPECT(assert_equal(factor->information(), factorRot->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)); + tranValues.insert(x1, poseDrift2.compose(level_pose)); + tranValues.insert(x2, poseDrift2.compose(pose_right)); + tranValues.insert(x3, poseDrift2.compose(pose_above)); - boost::shared_ptr hessianFactorRotTran = smartFactorInstance->linearize(tranValues); + boost::shared_ptr factorRotTran = + 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(factor->information(), factorRotTran->information(), 1e-7)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ){ - // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; +TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { - std::vector views; + using namespace vanillaPose2; + + 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)); - SimpleCamera cam1(pose1, *K2); - - // create second camera 1 meter to the right of first camera - 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)); - SimpleCamera cam3(pose3, *K2); - - Point3 landmark1(5, 0.5, 1.2); - - vector measurements_cam1, measurements_cam2, measurements_cam3; + // All cameras have the same pose so will be degenerate ! + Camera cam2(level_pose, sharedK2); + Camera cam3(level_pose, sharedK2); + vector measurements_cam1; projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - SmartFactor::shared_ptr smartFactor(new SmartFactor()); - smartFactor->add(measurements_cam1, views, model, K2); - + SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedK2)); + smartFactor->add(measurements_cam1, views); Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, cam3.pose()); - boost::shared_ptr hessianFactor = smartFactor->linearize(values); - if(isDebugTest) hessianFactor->print("Hessian factor \n"); + boost::shared_ptr factor = smartFactor->linearize(values); - 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)); + rotValues.insert(x1, poseDrift.compose(level_pose)); + rotValues.insert(x2, poseDrift.compose(level_pose)); + rotValues.insert(x3, poseDrift.compose(level_pose)); - boost::shared_ptr hessianFactorRot = smartFactor->linearize(rotValues); - if(isDebugTest) hessianFactorRot->print("Hessian factor \n"); + boost::shared_ptr factorRot = // + smartFactor->linearize(rotValues); // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + EXPECT(assert_equal(factor->information(), factorRot->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)); + tranValues.insert(x1, poseDrift2.compose(level_pose)); + tranValues.insert(x2, poseDrift2.compose(level_pose)); + tranValues.insert(x3, poseDrift2.compose(level_pose)); - boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); + boost::shared_ptr factorRotTran = 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(factor->information(), factorRotTran->information(), 1e-7)); } /* ************************************************************************* */ 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); + using namespace bundlerPose; + SmartProjectionParams params; + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + SmartFactor factor(model, sharedBundlerK, boost::none, params); + factor.add(measurement1, x1); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Cal3Bundler ){ - // cout << " ************************ SmartProjectionPoseFactor: Cal3Bundler **********************" << endl; +TEST( SmartProjectionPoseFactor, Cal3Bundler ) { - // 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)); - PinholeCamera cam1(pose1, *Kbundler); + using namespace bundlerPose; - // create second camera 1 meter to the right of first camera - 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)); - PinholeCamera cam3(pose3, *Kbundler); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); + // three landmarks ~5 meters in front of camera Point3 landmark3(3, 0, 3.0); vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1.push_back(cam1_uv1); - measurements_cam1.push_back(cam2_uv1); - measurements_cam1.push_back(cam3_uv1); + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - Point2 cam1_uv2 = cam1.project(landmark2); - Point2 cam2_uv2 = cam2.project(landmark2); - Point2 cam3_uv2 = cam3.project(landmark2); - measurements_cam2.push_back(cam1_uv2); - measurements_cam2.push_back(cam2_uv2); - measurements_cam2.push_back(cam3_uv2); - - Point2 cam1_uv3 = cam1.project(landmark3); - Point2 cam2_uv3 = cam2.project(landmark3); - Point2 cam3_uv3 = cam3.project(landmark3); - measurements_cam3.push_back(cam1_uv3); - 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); - SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler()); - smartFactor1->add(measurements_cam1, views, model, Kbundler); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedBundlerK)); + smartFactor1->add(measurements_cam1, views); - SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler()); - smartFactor2->add(measurements_cam2, views, model, Kbundler); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedBundlerK)); + smartFactor2->add(measurements_cam2, views); - SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler()); - smartFactor3->add(measurements_cam3, views, model, Kbundler); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedBundlerK)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1272,136 +1275,144 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ){ 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)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), 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: "); - - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * 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))); Values result; - gttic_(SmartProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - 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(cam3.pose(), result.at(x3), 1e-6)); +} /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ){ +TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { - std::vector views; + using namespace bundlerPose; + + 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)); - PinholeCamera cam1(pose1, *Kbundler); + // Two different cameras + Pose3 pose2 = level_pose + * 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)); + Camera cam2(pose2, sharedBundlerK); + Camera cam3(pose3, sharedBundlerK); - // 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)); - 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)); - PinholeCamera cam3(pose3, *Kbundler); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); + // landmark3 at 3 meters now Point3 landmark3(3, 0, 3.0); vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1.push_back(cam1_uv1); - measurements_cam1.push_back(cam2_uv1); - measurements_cam1.push_back(cam3_uv1); + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - Point2 cam1_uv2 = cam1.project(landmark2); - Point2 cam2_uv2 = cam2.project(landmark2); - Point2 cam3_uv2 = cam3.project(landmark2); - measurements_cam2.push_back(cam1_uv2); - measurements_cam2.push_back(cam2_uv2); - measurements_cam2.push_back(cam3_uv2); + SmartProjectionParams params; + params.setRankTolerance(10); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - Point2 cam1_uv3 = cam1.project(landmark3); - Point2 cam2_uv3 = cam2.project(landmark3); - Point2 cam3_uv3 = cam3.project(landmark3); - measurements_cam3.push_back(cam1_uv3); - measurements_cam3.push_back(cam2_uv3); - measurements_cam3.push_back(cam3_uv3); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedBundlerK, boost::none, params)); + smartFactor1->add(measurements_cam1, views); - double rankTol = 10; + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedBundlerK, boost::none, params)); + smartFactor2->add(measurements_cam2, views); - 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)); - smartFactor2->add(measurements_cam2, views, model, Kbundler); - - SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); - smartFactor3->add(measurements_cam3, views, model, Kbundler); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedBundlerK, boost::none, params)); + smartFactor3->add(measurements_cam3, views); 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(PriorFactor(x1, cam1.pose(), 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), 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: "); - - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + 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))); Values result; - gttic_(SmartProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - gttoc_(SmartProjectionPoseFactor); - 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_(); + 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))); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +TEST(SmartProjectionPoseFactor, serialize) { + using namespace vanillaPose; + using namespace gtsam::serializationTestHelpers; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactor factor(model, sharedK, boost::none, params); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ - diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp index 6b79171df..5ac92b4a9 100644 --- a/gtsam/slam/tests/testTriangulationFactor.cpp +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -17,7 +17,11 @@ */ #include +#include +#include #include +#include +#include #include #include @@ -35,7 +39,7 @@ static const boost::shared_ptr sharedCal = // // 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); +SimpleCamera camera1(pose1, *sharedCal); // landmark ~5 meters infront of camera static const Point3 landmark(5, 0.5, 1.2); @@ -45,10 +49,10 @@ 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; + typedef TriangulationFactor Factor; Factor factor(camera1, z1, model, pointKey); // Use the factor to calculate the Jacobians @@ -62,6 +66,46 @@ TEST( triangulation, TriangulationFactor ) { CHECK(assert_equal(HExpected, HActual, 1e-3)); } +//****************************************************************************** +TEST( triangulation, TriangulationFactorStereo ) { + + Key pointKey(1); + SharedNoiseModel model=noiseModel::Isotropic::Sigma(3,0.5); + Cal3_S2Stereo::shared_ptr stereoCal(new Cal3_S2Stereo(1500, 1200, 0, 640, 480, 0.5)); + StereoCamera camera2(pose1, stereoCal); + + StereoPoint2 z2 = camera2.project(landmark); + + typedef TriangulationFactor Factor; + Factor factor(camera2, z2, 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)); + + // compare same problem against expression factor + Expression::UnaryFunction::type f = boost::bind(&StereoCamera::project2, camera2, _1, boost::none, _2); + Expression point_(pointKey); + Expression project2_(f, point_); + + ExpressionFactor eFactor(model, z2, project2_); + + Values values; + values.insert(pointKey, landmark); + + vector HActual1(1), HActual2(1); + Vector error1 = factor.unwhitenedError(values, HActual1); + Vector error2 = eFactor.unwhitenedError(values, HActual2); + EXPECT(assert_equal(error1, error2)); + EXPECT(assert_equal(HActual1[0], HActual2[0])); +} + //****************************************************************************** int main() { TestResult tr; diff --git a/gtsam/smart/CMakeLists.txt b/gtsam/smart/CMakeLists.txt new file mode 100644 index 000000000..53c18fe96 --- /dev/null +++ b/gtsam/smart/CMakeLists.txt @@ -0,0 +1,6 @@ +# Install headers +file(GLOB smart_headers "*.h") +install(FILES ${smart_headers} DESTINATION include/gtsam/smart) + +# Build tests +add_subdirectory(tests) diff --git a/gtsam/smart/tests/CMakeLists.txt b/gtsam/smart/tests/CMakeLists.txt new file mode 100644 index 000000000..caa3164fa --- /dev/null +++ b/gtsam/smart/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(smart "test*.cpp" "" "gtsam") diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index 5dda02350..4db265036 100644 --- a/gtsam/symbolic/SymbolicBayesNet.h +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -76,7 +76,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); } }; diff --git a/gtsam/symbolic/SymbolicBayesTree.h b/gtsam/symbolic/SymbolicBayesTree.h index cac711043..c4f09c71c 100644 --- a/gtsam/symbolic/SymbolicBayesTree.h +++ b/gtsam/symbolic/SymbolicBayesTree.h @@ -65,7 +65,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); } }; diff --git a/gtsam/symbolic/SymbolicConditional.h b/gtsam/symbolic/SymbolicConditional.h index 4b89a4b60..0530af556 100644 --- a/gtsam/symbolic/SymbolicConditional.h +++ b/gtsam/symbolic/SymbolicConditional.h @@ -116,7 +116,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(BaseFactor); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional); } diff --git a/gtsam/symbolic/SymbolicFactor-inst.h b/gtsam/symbolic/SymbolicFactor-inst.h index 56850e991..f1e2b48c2 100644 --- a/gtsam/symbolic/SymbolicFactor-inst.h +++ b/gtsam/symbolic/SymbolicFactor-inst.h @@ -17,15 +17,17 @@ #pragma once -#include +#include +#include +#include +#include +#include + #include #include #include -#include -#include -#include -#include +#include namespace gtsam { @@ -37,8 +39,10 @@ namespace gtsam std::pair, boost::shared_ptr > EliminateSymbolic(const FactorGraph& factors, const Ordering& keys) { + gttic(EliminateSymbolic); + // Gather all keys - FastSet allKeys; + KeySet allKeys; BOOST_FOREACH(const boost::shared_ptr& factor, factors) { allKeys.insert(factor->begin(), factor->end()); } @@ -50,7 +54,7 @@ namespace gtsam } // Sort frontal keys - FastSet frontals(keys); + KeySet frontals(keys); const size_t nFrontals = keys.size(); // Build a key vector with the frontals followed by the separator diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h index f30f88935..aca1ba043 100644 --- a/gtsam/symbolic/SymbolicFactor.h +++ b/gtsam/symbolic/SymbolicFactor.h @@ -144,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 diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index 9813df331..0a5427ba3 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -111,7 +111,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); } }; diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 3c02f6dbd..93c38b324 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -691,6 +691,76 @@ TEST(SymbolicBayesTree, complicatedMarginal) } } + +/* ************************************************************************* */ +TEST(SymbolicBayesTree, COLAMDvsMETIS) { + + // create circular graph + 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(4, 5); + sfg.push_factor(0, 5); + + // COLAMD + { + Ordering ordering = Ordering::Create(Ordering::COLAMD, sfg); + EXPECT(assert_equal(Ordering(list_of(0)(5)(1)(4)(2)(3)), ordering)); + + // - P( 4 2 3) + // | - P( 1 | 2 4) + // | | - P( 5 | 1 4) + // | | | - P( 0 | 1 5) + SymbolicBayesTree expected; + expected.insertRoot( + MakeClique(list_of(4)(2)(3), 3, + list_of( + MakeClique(list_of(1)(2)(4), 1, + list_of( + MakeClique(list_of(5)(1)(4), 1, + list_of(MakeClique(list_of(0)(1)(5), 1)))))))); + + SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); + EXPECT(assert_equal(expected, actual)); + } + + // METIS + { + Ordering ordering = Ordering::Create(Ordering::METIS, sfg); +// Linux and Mac split differently when using mettis +#if !defined(__APPLE__) + EXPECT(assert_equal(Ordering(list_of(3)(2)(5)(0)(4)(1)), ordering)); +#else + EXPECT(assert_equal(Ordering(list_of(5)(4)(2)(1)(0)(3)), ordering)); +#endif + + // - P( 1 0 3) + // | - P( 4 | 0 3) + // | | - P( 5 | 0 4) + // | - P( 2 | 1 3) + SymbolicBayesTree expected; +#if !defined(__APPLE__) + expected.insertRoot( + MakeClique(list_of(2)(4)(1), 3, + list_of( + MakeClique(list_of(0)(1)(4), 1, + list_of(MakeClique(list_of(5)(0)(4), 1))))( + MakeClique(list_of(3)(2)(4), 1)))); +#else + expected.insertRoot( + MakeClique(list_of(1)(0)(3), 3, + list_of( + MakeClique(list_of(4)(0)(3), 1, + list_of(MakeClique(list_of(5)(0)(4), 1))))( + MakeClique(list_of(2)(1)(3), 1)))); +#endif + SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); + EXPECT(assert_equal(expected, actual)); + } +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp index 49b14bc07..f2d891827 100644 --- a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp @@ -46,10 +46,10 @@ TEST( JunctionTree, constructor ) frontal1 = list_of(2)(3), frontal2 = list_of(0)(1), sep1, sep2 = list_of(2); - EXPECT(assert_container_equality(frontal1, actual.roots().front()->keys)); + EXPECT(assert_container_equality(frontal1, actual.roots().front()->orderedFrontalKeys)); //EXPECT(assert_equal(sep1, actual.roots().front()->separator)); LONGS_EQUAL(1, (long)actual.roots().front()->factors.size()); - EXPECT(assert_container_equality(frontal2, actual.roots().front()->children.front()->keys)); + EXPECT(assert_container_equality(frontal2, actual.roots().front()->children.front()->orderedFrontalKeys)); //EXPECT(assert_equal(sep2, actual.roots().front()->children.front()->separator)); LONGS_EQUAL(2, (long)actual.roots().front()->children.front()->factors.size()); EXPECT(assert_equal(*simpleChain[2], *actual.roots().front()->factors[0])); 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/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp index b579364b6..37a4fe5a4 100644 --- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp +++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp @@ -193,7 +193,7 @@ private: // add the belief factor for each neighbor variable to this star graph // also record the factor index for later modification - FastSet neighbors = star->keys(); + KeySet neighbors = star->keys(); neighbors.erase(key); CorrectedBeliefIndices correctedBeliefIndices; BOOST_FOREACH(Key neighbor, neighbors) { diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 818266d4c..51737285a 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 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,31 @@ 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(); 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) + && equal_with_abs_tol(velocity(), other.velocity(), tol); } /* ************************************************************************* */ @@ -53,73 +53,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, ChartJacobian H) { - if (H) CONCEPT_NOT_IMPLEMENTED; - Pose3 newPose = Pose3::Expmap(v.head<6>()); - Velocity3 newVel = Velocity3(v.tail<3>()); - return PoseRTV(newPose, newVel); -} - -/* ************************************************************************* */ -Vector9 PoseRTV::Logmap(const PoseRTV& p, ChartJacobian H) { - if (H) CONCEPT_NOT_IMPLEMENTED; - Vector6 Lx = Pose3::Logmap(p.Rt_); - Vector3 Lv = p.v_.vector(); - return (Vector9() << Lx, Lv).finished(); -} - -/* ************************************************************************* */ -PoseRTV PoseRTV::retract(const Vector& v, - ChartJacobian Horigin, - ChartJacobian Hv) const { - if (Horigin || Hv) CONCEPT_NOT_IMPLEMENTED; - 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, - ChartJacobian Horigin, - ChartJacobian Hp) const { - if (Horigin || Hp) CONCEPT_NOT_IMPLEMENTED; - 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(ChartJacobian H1) const { - if (H1) *H1 = numericalDerivative11(inverse_, *this, 1e-5); - return PoseRTV(Rt_.inverse(), - v_); -} - -/* ************************************************************************* */ -PoseRTV compose_(const PoseRTV& p1, const PoseRTV& p2) { return p1.compose(p2); } -PoseRTV PoseRTV::compose(const PoseRTV& p, ChartJacobian H1, - ChartJacobian 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_+p.v_); -} - -/* ************************************************************************* */ -PoseRTV between_(const PoseRTV& p1, const PoseRTV& p2) { return p1.between(p2); } -PoseRTV PoseRTV::between(const PoseRTV& p, - ChartJacobian H1, - ChartJacobian H2) const { - if (H1) *H1 = numericalDerivative21(between_, *this, p, 1e-5); - if (H2) *H2 = numericalDerivative22(between_, *this, p, 1e-5); - return inverse().compose(p); + gtsam::print((Vector)velocity(), " V"); } /* ************************************************************************* */ @@ -187,7 +121,7 @@ PoseRTV PoseRTV::generalDynamics( Rot3 r2 = rotation().retract(gyro * dt); // Integrate Velocity Equations - Velocity3 v2 = v_ + (Velocity3(dt * (r2.matrix() * accel + g))); + Velocity3 v2 = velocity() + Velocity3(dt * (r2.matrix() * accel + kGravity)); // Integrate Position Equations Point3 t2 = translationIntegration(r2, v2, dt); @@ -204,8 +138,8 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { Vector6 imu; // acceleration - Vector3 accel = (v2-v1).vector() / dt; - imu.head<3>() = r2.transpose() * (accel - g); + Vector3 accel = (v2-v1) / dt; + imu.head<3>() = r2.transpose() * (accel - kGravity); // rotation rates // just using euler angles based on matlab code @@ -227,68 +161,54 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const { // predict point for constraint // NOTE: uses simple Euler approach for prediction - Point3 pred_t2 = t() + v2 * dt; + Point3 pred_t2 = t().retract(v2 * dt); return pred_t2; } /* ************************************************************************* */ -double range_(const PoseRTV& A, const PoseRTV& B) { return A.range(B); } double PoseRTV::range(const PoseRTV& other, OptionalJacobian<1,9> H1, OptionalJacobian<1,9> 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()); + 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, ChartJacobian Dglobal, OptionalJacobian<9, 6> 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); // 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(Point3(velocity()), D_newvel_R, D_newvel_v).vector(); 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); } /* ************************************************************************* */ -Matrix PoseRTV::RRTMbn(const Vector& euler) { +Matrix PoseRTV::RRTMbn(const Vector3& euler) { assert(euler.size() == 3); - const double s1 = sin(euler(1-1)), c1 = cos(euler(1-1)); - const double t2 = tan(euler(2-1)), c2 = cos(euler(2-1)); + const double s1 = sin(euler.x()), c1 = cos(euler.x()); + const double t2 = tan(euler.y()), c2 = cos(euler.y()); Matrix Ebn(3,3); Ebn << 1.0, s1 * t2, c1 * t2, 0.0, c1, -s1, @@ -302,11 +222,10 @@ Matrix PoseRTV::RRTMbn(const Rot3& att) { } /* ************************************************************************* */ -Matrix PoseRTV::RRTMnb(const Vector& euler) { - assert(euler.size() == 3); +Matrix PoseRTV::RRTMnb(const Vector3& euler) { Matrix Enb(3,3); - const double s1 = sin(euler(1-1)), c1 = cos(euler(1-1)); - const double s2 = sin(euler(2-1)), c2 = cos(euler(2-1)); + const double s1 = sin(euler.x()), c1 = cos(euler.x()); + const double s2 = sin(euler.y()), c2 = cos(euler.y()); Enb << 1.0, 0.0, -s2, 0.0, c1, s1*c2, 0.0, -s1, c1*c2; diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index ea7a2c9ff..b59ea4614 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -7,41 +7,43 @@ #pragma once #include -#include #include +#include namespace gtsam { /// Syntactic sugar to clarify components -typedef Point3 Velocity3; +typedef Vector3 Velocity3; /** * Robot state for use with IMU measurements * - contains translation, translational velocity and rotation + * TODO(frank): Alex should deprecate/move to project */ -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: - enum { dimension = 9 }; // 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),Vector3::Zero()) {} PoseRTV(const Pose3& pose, const Velocity3& vel) - : Rt_(pose), v_(vel) {} + : Base(pose, vel) {} explicit PoseRTV(const Pose3& pose) - : Rt_(pose) {} + : Base(pose,Vector3::Zero()) {} + + // 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, @@ -51,72 +53,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(); } + const Velocity3& velocityVec() const { return velocity(); } // 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, ChartJacobian Horigin=boost::none, ChartJacobian Hv=boost::none) const; - Vector localCoordinates(const PoseRTV& p, ChartJacobian Horigin=boost::none,ChartJacobian Hp=boost::none) const; + /// @name measurement functions + /// @{ - // Lie TODO IS this a Lie group or just a Manifold???? - /** - * expmap/logmap are poor approximations that assume independence of components - * Currently implemented using the poor retract/unretract approximations - */ - static PoseRTV Expmap(const Vector9& v, ChartJacobian H = boost::none); - static Vector9 Logmap(const PoseRTV& p, ChartJacobian H = boost::none); - - static PoseRTV identity() { return PoseRTV(); } - - /** Derivatives calculated numerically */ - PoseRTV inverse(ChartJacobian H1=boost::none) const; - - /** Derivatives calculated numerically */ - PoseRTV compose(const PoseRTV& p, - ChartJacobian H1=boost::none, - ChartJacobian H2=boost::none) const; - - PoseRTV operator*(const PoseRTV& p) const { return compose(p); } - - /** Derivatives calculated numerically */ - PoseRTV between(const PoseRTV& p, - ChartJacobian H1=boost::none, - ChartJacobian H2=boost::none) const; - - // measurement functions - /** Derivatives calculated numerically */ + /** range between translations */ double range(const PoseRTV& other, 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 @@ -164,32 +140,39 @@ public: 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 - static Matrix RRTMbn(const Vector& euler); - + static Matrix RRTMbn(const Vector3& euler); static Matrix RRTMbn(const Rot3& att); /// RRTMnb - Function computes the rotation rate transformation matrix from /// euler angle rates to body axis rates - static Matrix RRTMnb(const Vector& euler); - + static Matrix RRTMnb(const Vector3& 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); } }; template<> -struct traits : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroup {}; + +// Define Range functor specializations that are used in RangeFactor +template struct Range; + +template<> +struct Range : HasRange {}; } // \namespace gtsam diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index c9db449f9..c41ea220c 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -106,12 +106,13 @@ private: static gtsam::Vector evaluateError_(const PoseRTV& x1, const PoseRTV& x2, double dt, const dynamics::IntegrationMode& mode) { - const Velocity3& v1 = x1.v(), v2 = x2.v(), p1 = x1.t(), p2 = x2.t(); - Velocity3 hx; + const Velocity3& v1 = x1.v(), v2 = x2.v(); + const Point3& p1 = x1.t(), p2 = x2.t(); + Point3 hx; switch(mode) { - case dynamics::TRAPEZOIDAL: hx = p1 + (v1 + v2) * dt *0.5; break; - case dynamics::EULER_START: hx = p1 + v1 * dt; break; - case dynamics::EULER_END : hx = p1 + v2 * dt; break; + case dynamics::TRAPEZOIDAL: hx = p1.retract((v1 + v2) * dt *0.5); break; + case dynamics::EULER_START: hx = p1.retract(v1 * dt); break; + case dynamics::EULER_END : hx = p1.retract(v2 * dt); break; default: assert(false); break; } return (p2 - hx).vector(); 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 d97f185e7..3fff06de1 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -9,7 +9,7 @@ #include #include -#include +#include #include #include #include @@ -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), 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)); + pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)), + pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Velocity3(0.0, 0.0, 0.0)), + pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Velocity3(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), 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)); + pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)), + pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)), + pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(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..0386d8bcd 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -15,44 +15,43 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(PoseRTV) GTSAM_CONCEPT_LIE_INST(PoseRTV) -const double tol=1e-5; - -Rot3 rot = Rot3::RzRyRx(0.1, 0.2, 0.3); -Point3 pt(1.0, 2.0, 3.0); -Velocity3 vel(0.4, 0.5, 0.6); +static const Rot3 rot = Rot3::RzRyRx(0.1, 0.2, 0.3); +static const Point3 pt(1.0, 2.0, 3.0); +static const Velocity3 vel(0.4, 0.5, 0.6); +static const Vector3 kZero3 = Vector3::Zero(); /* ************************************************************************* */ TEST( testPoseRTV, constructors ) { PoseRTV state1(pt, rot, vel); - EXPECT(assert_equal(pt, state1.t(), tol)); - EXPECT(assert_equal(rot, state1.R(), tol)); - EXPECT(assert_equal(vel, state1.v(), tol)); - EXPECT(assert_equal(Pose3(rot, pt), state1.pose(), tol)); + EXPECT(assert_equal(pt, state1.t())); + EXPECT(assert_equal(rot, state1.R())); + EXPECT(assert_equal(vel, state1.v())); + EXPECT(assert_equal(Pose3(rot, pt), state1.pose())); PoseRTV state2; - EXPECT(assert_equal(Point3(), state2.t(), tol)); - EXPECT(assert_equal(Rot3(), state2.R(), tol)); - EXPECT(assert_equal(Velocity3(), state2.v(), tol)); - EXPECT(assert_equal(Pose3(), state2.pose(), tol)); + EXPECT(assert_equal(Point3(), state2.t())); + EXPECT(assert_equal(Rot3(), state2.R())); + EXPECT(assert_equal(kZero3, state2.v())); + EXPECT(assert_equal(Pose3(), state2.pose())); PoseRTV state3(Pose3(rot, pt), vel); - EXPECT(assert_equal(pt, state3.t(), tol)); - EXPECT(assert_equal(rot, state3.R(), tol)); - EXPECT(assert_equal(vel, state3.v(), tol)); - EXPECT(assert_equal(Pose3(rot, pt), state3.pose(), tol)); + EXPECT(assert_equal(pt, state3.t())); + EXPECT(assert_equal(rot, state3.R())); + EXPECT(assert_equal(vel, state3.v())); + EXPECT(assert_equal(Pose3(rot, pt), state3.pose())); PoseRTV state4(Pose3(rot, pt)); - EXPECT(assert_equal(pt, state4.t(), tol)); - EXPECT(assert_equal(rot, state4.R(), tol)); - EXPECT(assert_equal(Velocity3(), state4.v(), tol)); - EXPECT(assert_equal(Pose3(rot, pt), state4.pose(), tol)); + EXPECT(assert_equal(pt, state4.t())); + EXPECT(assert_equal(rot, state4.R())); + EXPECT(assert_equal(kZero3, state4.v())); + EXPECT(assert_equal(Pose3(rot, pt), state4.pose())); Vector vec_init = (Vector(9) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0, 0.4, 0.5, 0.6).finished(); PoseRTV state5(vec_init); - EXPECT(assert_equal(pt, state5.t(), tol)); - EXPECT(assert_equal(rot, state5.R(), tol)); - EXPECT(assert_equal(vel, state5.v(), tol)); - EXPECT(assert_equal(vec_init, state5.vector(), tol)); + EXPECT(assert_equal(pt, state5.t())); + EXPECT(assert_equal(rot, state5.R())); + EXPECT(assert_equal(vel, state5.v())); + EXPECT(assert_equal(vec_init, state5.vector())); } /* ************************************************************************* */ @@ -65,32 +64,44 @@ TEST( testPoseRTV, dim ) { /* ************************************************************************* */ TEST( testPoseRTV, equals ) { PoseRTV state1, state2(pt, rot, vel), state3(state2), state4(Pose3(rot, pt)); - EXPECT(assert_equal(state1, state1, tol)); - EXPECT(assert_equal(state2, state3, tol)); - EXPECT(assert_equal(state3, state2, tol)); - EXPECT(assert_inequal(state1, state2, tol)); - EXPECT(assert_inequal(state2, state1, tol)); - EXPECT(assert_inequal(state2, state4, tol)); + EXPECT(assert_equal(state1, state1)); + EXPECT(assert_equal(state2, state3)); + EXPECT(assert_equal(state3, state2)); + EXPECT(assert_inequal(state1, state2)); + EXPECT(assert_inequal(state2, state1)); + EXPECT(assert_inequal(state2, state4)); } /* ************************************************************************* */ 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)))); + EXPECT(assert_equal(zero(9), identity.localCoordinates(identity))); PoseRTV state1(pt, rot, vel); - EXPECT(assert_equal(state1, state1.retract(zero(9)), tol)); - EXPECT(assert_equal(zero(9), state1.localCoordinates(state1), tol)); + EXPECT(assert_equal(state1, (PoseRTV)state1.retract(zero(9)))); + EXPECT(assert_equal(zero(9), state1.localCoordinates(state1))); - Vector delta = (Vector(9) << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3).finished(); - Rot3 rot2 = rot.retract(repeat(3, 0.1)); - 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(delta, state1.localCoordinates(state2), 1e-1)); - EXPECT(assert_equal(-delta, state2.localCoordinates(state1), 1e-1)); // loose tolerance due to retract approximation + Vector delta(9); + delta << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3; + Pose3 pose2 = Pose3(rot, pt).retract(delta.head<6>()); + Velocity3 vel2 = vel + Velocity3(-0.1, -0.2, -0.3); + PoseRTV state2(pose2.translation(), pose2.rotation(), vel2); + EXPECT(assert_equal(state2, (PoseRTV)state1.retract(delta))); + EXPECT(assert_equal(delta, state1.localCoordinates(state2))); + + // roundtrip from state2 to state3 and back + PoseRTV state3 = state2.retract(delta); + EXPECT(assert_equal(delta, state2.localCoordinates(state3))); + + // roundtrip from state3 to state4 and back, with expmap. + PoseRTV state4 = state3.expmap(delta); + EXPECT(assert_equal(delta, state3.logmap(state4))); + + // For the expmap/logmap (not necessarily retract/local) -delta goes other way + EXPECT(assert_equal(state3, (PoseRTV)state4.expmap(-delta))); + EXPECT(assert_equal(delta, -state4.logmap(state3))); } /* ************************************************************************* */ @@ -107,33 +118,70 @@ TEST( testPoseRTV, dynamics_identities ) { x3 = x2.generalDynamics(accel, gyro, dt); x4 = x3.generalDynamics(accel, gyro, dt); -// EXPECT(assert_equal(imu01, x0.imuPrediction(x1, dt).first, tol)); -// EXPECT(assert_equal(imu12, x1.imuPrediction(x2, dt).first, tol)); -// EXPECT(assert_equal(imu23, x2.imuPrediction(x3, dt).first, tol)); -// EXPECT(assert_equal(imu34, x3.imuPrediction(x4, dt).first, tol)); +// EXPECT(assert_equal(imu01, x0.imuPrediction(x1, dt).first)); +// EXPECT(assert_equal(imu12, x1.imuPrediction(x2, dt).first)); +// EXPECT(assert_equal(imu23, x2.imuPrediction(x3, dt).first)); +// EXPECT(assert_equal(imu34, x3.imuPrediction(x4, dt).first)); // -// EXPECT(assert_equal(x1.translation(), x0.imuPrediction(x1, dt).second, tol)); -// EXPECT(assert_equal(x2.translation(), x1.imuPrediction(x2, dt).second, tol)); -// EXPECT(assert_equal(x3.translation(), x2.imuPrediction(x3, dt).second, tol)); -// EXPECT(assert_equal(x4.translation(), x3.imuPrediction(x4, dt).second, tol)); +// EXPECT(assert_equal(x1.translation(), x0.imuPrediction(x1, dt).second)); +// EXPECT(assert_equal(x2.translation(), x1.imuPrediction(x2, dt).second)); +// EXPECT(assert_equal(x3.translation(), x2.imuPrediction(x3, dt).second)); +// EXPECT(assert_equal(x4.translation(), x3.imuPrediction(x4, dt).second)); } +/* ************************************************************************* */ +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)); + EXPECT(assert_equal(numericH2, actH2)); +} + +/* ************************************************************************* */ +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)); + EXPECT(assert_equal(numericH2, actH2)); +} + +/* ************************************************************************* */ +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)); +} + /* ************************************************************************* */ double range_proxy(const PoseRTV& A, const PoseRTV& B) { return A.range(B); } TEST( testPoseRTV, range ) { Point3 tA(1.0, 2.0, 3.0), tB(3.0, 2.0, 3.0); PoseRTV rtvA(tA), rtvB(tB); - EXPECT_DOUBLES_EQUAL(0.0, rtvA.range(rtvA), tol); - EXPECT_DOUBLES_EQUAL(2.0, rtvA.range(rtvB), tol); - EXPECT_DOUBLES_EQUAL(2.0, rtvB.range(rtvA), tol); + EXPECT_DOUBLES_EQUAL(0.0, rtvA.range(rtvA), 1e-9); + EXPECT_DOUBLES_EQUAL(2.0, rtvA.range(rtvB), 1e-9); + EXPECT_DOUBLES_EQUAL(2.0, rtvB.range(rtvA), 1e-9); Matrix actH1, actH2; rtvA.range(rtvB, actH1, actH2); Matrix numericH1 = numericalDerivative21(range_proxy, rtvA, rtvB); Matrix numericH2 = numericalDerivative22(range_proxy, rtvA, rtvB); - EXPECT(assert_equal(numericH1, actH1, tol)); - EXPECT(assert_equal(numericH2, actH2, tol)); + EXPECT(assert_equal(numericH1, actH1)); + EXPECT(assert_equal(numericH2, actH2)); } /* ************************************************************************* */ @@ -141,7 +189,7 @@ PoseRTV transformed_from_proxy(const PoseRTV& a, const Pose3& trans) { return a.transformed_from(trans); } TEST( testPoseRTV, transformed_from_1 ) { - Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); + Rot3 R = Rot3::Rodrigues(0.1, 0.2, 0.3); Point3 T(1.0, 2.0, 3.0); Velocity3 V(2.0, 3.0, 4.0); PoseRTV start(R, T, V); @@ -149,13 +197,13 @@ TEST( testPoseRTV, transformed_from_1 ) { Matrix actDTrans, actDGlobal; PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans); - PoseRTV expected(transform.compose(start.pose()), transform.rotation().rotate(V)); - EXPECT(assert_equal(expected, actual, tol)); + PoseRTV expected(transform.compose(start.pose()), transform.rotation().matrix() * V); + EXPECT(assert_equal(expected, actual)); Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-5); // At 1e-8, fails Matrix numDTrans = numericalDerivative22(transformed_from_proxy, start, transform, 1e-8); // Sensitive to step size - EXPECT(assert_equal(numDGlobal, actDGlobal, tol)); - EXPECT(assert_equal(numDTrans, actDTrans, tol)); // FIXME: still needs analytic derivative + EXPECT(assert_equal(numDGlobal, actDGlobal)); + EXPECT(assert_equal(numDTrans, actDTrans, 1e-5)); // FIXME: still needs analytic derivative } /* ************************************************************************* */ @@ -168,27 +216,27 @@ TEST( testPoseRTV, transformed_from_2 ) { Matrix actDTrans, actDGlobal; PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans); - PoseRTV expected(transform.compose(start.pose()), transform.rotation().rotate(V)); - EXPECT(assert_equal(expected, actual, tol)); + PoseRTV expected(transform.compose(start.pose()), transform.rotation().matrix() * V); + EXPECT(assert_equal(expected, actual)); Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-5); // At 1e-8, fails Matrix numDTrans = numericalDerivative22(transformed_from_proxy, start, transform, 1e-8); // Sensitive to step size - EXPECT(assert_equal(numDGlobal, actDGlobal, tol)); - EXPECT(assert_equal(numDTrans, actDTrans, tol)); // FIXME: still needs analytic derivative + EXPECT(assert_equal(numDGlobal, actDGlobal)); + EXPECT(assert_equal(numDTrans, actDTrans, 1e-5)); // FIXME: still needs analytic derivative } /* ************************************************************************* */ TEST(testPoseRTV, RRTMbn) { - EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(zero(3)), tol)); - EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3()), tol)); - EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)), tol)); + EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(kZero3))); + EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3()))); + EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)))); } /* ************************************************************************* */ TEST(testPoseRTV, RRTMnb) { - EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(zero(3)), tol)); - EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3()), tol)); - EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)), tol)); + EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(kZero3))); + EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3()))); + EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)))); } /* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp index 56d38714a..f253be371 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(), Point3(1.0, 0.0, 0.0)), + pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0)), pose1a(Point3(0.5, 0.0, 0.0)), - pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Point3(1.0, 0.0, 0.0)); + pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0)); /* ************************************************************************* */ TEST( testVelocityConstraint, trapezoidal ) { diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index c8a4e7123..de1d3f77b 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -26,16 +26,14 @@ * -makes monocular observations of many landmarks */ -#include +#include +#include #include #include #include #include #include #include -#include - -#include #include #include @@ -46,7 +44,8 @@ using namespace gtsam; int main(int argc, char** argv){ - typedef SmartProjectionPoseFactor SmartFactor; + typedef PinholePose Camera; + typedef SmartProjectionPoseFactor SmartFactor; Values initial_estimate; NonlinearFactorGraph graph; @@ -55,7 +54,7 @@ int main(int argc, char** argv){ 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; @@ -68,18 +67,17 @@ int main(int argc, char** argv){ cout << "Reading camera poses" << endl; ifstream pose_file(pose_loc.c_str()); - int pose_id; + int pose_index; 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++) { + while (pose_file >> pose_index) { + for (int i = 0; i < 16; i++) pose_file >> m.data()[i]; - } - initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); + initial_estimate.insert(pose_index, Pose3(m)); } - - // camera and landmark keys - size_t x, l; + + // landmark keys + size_t landmark_key; // pixel coordinates uL, uR, v (same for left/right images due to rectification) // landmark coordinates X, Y, Z in camera frame, resulting from triangulation @@ -89,24 +87,24 @@ int main(int argc, char** argv){ //read stereo measurements and construct smart factors - SmartFactor::shared_ptr factor(new SmartFactor()); + SmartFactor::shared_ptr factor(new SmartFactor(model, K)); size_t current_l = 3; // hardcoded landmark ID from first measurement - while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { + while (factor_file >> pose_index >> landmark_key >> uL >> uR >> v >> X >> Y >> Z) { - if(current_l != l) { + if(current_l != landmark_key) { graph.push_back(factor); - factor = SmartFactor::shared_ptr(new SmartFactor()); - current_l = l; + factor = SmartFactor::shared_ptr(new SmartFactor(model, K)); + current_l = landmark_key; } - factor->add(Point2(uL,v), Symbol('x',x), model, K); + factor->add(Point2(uL,v), pose_index); } - Pose3 first_pose = initial_estimate.at(Symbol('x',1)); + Pose3 firstPose = initial_estimate.at(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)); + graph.push_back(NonlinearEquality(1,firstPose)); LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp index 6cc8a7b78..4062d0659 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp @@ -39,7 +39,7 @@ // have been provided with the library for solving robotics SLAM problems. #include #include -#include +#include #include // Standard headers, added last, so we know headers above work on their own diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp index b689179a2..90d92897e 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp @@ -39,7 +39,7 @@ // have been provided with the library for solving robotics SLAM problems. #include #include -#include +#include // Standard headers, added last, so we know headers above work on their own #include diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index 770c5f18c..d3680460f 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -46,7 +46,7 @@ using namespace gtsam; int main(int argc, char** argv){ - typedef SmartStereoProjectionPoseFactor SmartFactor; + typedef SmartStereoProjectionPoseFactor SmartFactor; bool output_poses = true; string poseOutput("../../../examples/data/optimized_poses.txt"); @@ -88,11 +88,13 @@ int main(int argc, char** argv){ } Values initial_pose_values = initial_estimate.filter(); - if(output_poses){ - init_pose3Out.open(init_poseOutput.c_str(),ios::out); - for(int 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; + 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; } } @@ -107,17 +109,17 @@ int main(int argc, char** argv){ //read stereo measurements and construct smart factors - SmartFactor::shared_ptr factor(new SmartFactor()); + SmartFactor::shared_ptr factor(new SmartFactor(model)); 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()); + factor = SmartFactor::shared_ptr(new SmartFactor(model)); current_l = l; } - factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), model, K); + factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), K); } Pose3 first_pose = initial_estimate.at(Symbol('x',1)); @@ -141,7 +143,7 @@ int main(int argc, char** argv){ if(output_poses){ pose3Out.open(poseOutput.c_str(),ios::out); - for(int i = 1; i<=pose_values.size(); i++){ + 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; } diff --git a/gtsam_unstable/geometry/BearingS2.h b/gtsam_unstable/geometry/BearingS2.h index 3c9247048..70a22b9a5 100644 --- a/gtsam_unstable/geometry/BearingS2.h +++ b/gtsam_unstable/geometry/BearingS2.h @@ -92,7 +92,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(azimuth_); ar & BOOST_SERIALIZATION_NVP(elevation_); } diff --git a/gtsam_unstable/geometry/Event.cpp b/gtsam_unstable/geometry/Event.cpp new file mode 100644 index 000000000..68d4d60ce --- /dev/null +++ b/gtsam_unstable/geometry/Event.cpp @@ -0,0 +1,29 @@ +/* ---------------------------------------------------------------------------- + + * 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 + */ + +#include + +namespace gtsam { + +const double Event::Speed = 330; +const Matrix14 Event::JacobianZ = (Matrix14() << 0,0,0,1).finished(); + +} + + diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 9d35331bb..3c622924a 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -99,9 +99,6 @@ public: } }; -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 {}; 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 c48508fa0..9d01e20a5 100644 --- a/gtsam_unstable/geometry/Pose3Upright.h +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -133,7 +133,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(z_); } 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/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/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp new file mode 100644 index 000000000..14dfb8f1a --- /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::Rodrigues(0.3,0,0); +static Similarity3 T(R,Point3(3.5,-8.2,4.2),1); +static Similarity3 T2(Rot3::Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2),1); +static Similarity3 T3(Rot3::Rodrigues(-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::Rodrigues(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..99b33182f 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; @@ -52,9 +53,9 @@ class Dummy { class PoseRTV { PoseRTV(); PoseRTV(Vector rtv); - PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Point3& vel); - PoseRTV(const gtsam::Rot3& rot, const gtsam::Point3& pt, const gtsam::Point3& vel); - PoseRTV(const gtsam::Pose3& pose, const gtsam::Point3& vel); + PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const Vector& vel); + PoseRTV(const gtsam::Rot3& rot, const gtsam::Point3& pt, const Vector& vel); + PoseRTV(const gtsam::Pose3& pose, const Vector& vel); PoseRTV(const gtsam::Pose3& pose); PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, double vx, double vy, double vz); @@ -65,7 +66,7 @@ class PoseRTV { // access gtsam::Point3 translation() const; gtsam::Rot3 rotation() const; - gtsam::Point3 velocity() const; + Vector velocity() const; gtsam::Pose3 pose() const; // Vector interfaces @@ -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(); @@ -392,7 +367,7 @@ virtual class SmartRangeFactor : gtsam::NoiseModelFactor { }; -#include +#include template virtual class RangeFactor : gtsam::NonlinearFactor { RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); @@ -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/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index 05b0bb49e..d74a0c302 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) << " "; @@ -370,11 +407,11 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { std::set removedFactorSlots; VariableIndex variableIndex(factors_); BOOST_FOREACH(Key key, marginalizeKeys) { - const FastList& slots = variableIndex[key]; + const FastVector& slots = variableIndex[key]; 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::KeySet& 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: "); + KeySet allKeys(graph.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: "); + KeySet 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: "); - 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 5c66d411f..605f3a5e8 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -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,13 +158,10 @@ 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); - static void PrintKeySet(const gtsam::FastSet& keys, const std::string& label); + static void PrintKeySet(const gtsam::KeySet& keys, const std::string& label); static void PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor); static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor); static void PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label); diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index fcaae9626..c3e307010 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -221,7 +221,7 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSumm if(debug) { PrintNonlinearFactorGraph(smootherSummarization_, "ConcurrentBatchFilter::synchronize ", "Updated Smoother Summarization:", DefaultKeyFormatter); } // Find the set of new separator keys - FastSet newSeparatorKeys; + KeySet newSeparatorKeys; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { newSeparatorKeys.insert(key_value.key); } @@ -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_); } } @@ -536,7 +536,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { std::vector removedFactorSlots; VariableIndex variableIndex(factors_); BOOST_FOREACH(Key key, keysToMove) { - const FastList& slots = variableIndex[key]; + const FastVector& slots = variableIndex[key]; removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); } @@ -573,7 +573,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { } // Calculate the set of new separator keys: AffectedKeys + PreviousSeparatorKeys - KeysToMove - FastSet newSeparatorKeys = removedFactors.keys(); + KeySet newSeparatorKeys = removedFactors.keys(); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { newSeparatorKeys.insert(key_value.key); } @@ -584,7 +584,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { if(debug) { PrintKeys(newSeparatorKeys, "ConcurrentBatchFilter::synchronize ", "New Separator Keys:", DefaultKeyFormatter); } // Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys - FastSet shortcutKeys = newSeparatorKeys; + KeySet shortcutKeys = newSeparatorKeys; BOOST_FOREACH(Key key, smootherSummarization_.keys()) { shortcutKeys.insert(key); } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 0f6515056..29ee14aee 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -230,8 +230,8 @@ void ConcurrentBatchSmoother::reorder() { // Recalculate the variable index variableIndex_ = VariableIndex(factors_); - FastList separatorKeys = separatorValues_.keys(); - ordering_ = Ordering::colamdConstrainedLast(variableIndex_, std::vector(separatorKeys.begin(), separatorKeys.end())); + KeyVector separatorKeys = separatorValues_.keys(); + ordering_ = Ordering::ColamdConstrainedLast(variableIndex_, std::vector(separatorKeys.begin(), separatorKeys.end())); } @@ -371,7 +371,7 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() { } // Get the set of separator keys - gtsam::FastSet separatorKeys; + gtsam::KeySet separatorKeys; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { separatorKeys.insert(key_value.key); } diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp index 3b6b884f6..b893860cc 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp @@ -52,16 +52,16 @@ namespace internal { /* ************************************************************************* */ NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, - const FastSet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) { + const KeySet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) { // Calculate the set of RootKeys = AllKeys \Intersect RemainingKeys - FastSet rootKeys; - FastSet allKeys(graph.keys()); + KeySet rootKeys; + KeySet allKeys(graph.keys()); std::set_intersection(allKeys.begin(), allKeys.end(), remainingKeys.begin(), remainingKeys.end(), std::inserter(rootKeys, rootKeys.end())); // Calculate the set of MarginalizeKeys = AllKeys - RemainingKeys - FastSet marginalizeKeys; + KeySet marginalizeKeys; std::set_difference(allKeys.begin(), allKeys.end(), remainingKeys.begin(), remainingKeys.end(), std::inserter(marginalizeKeys, marginalizeKeys.end())); if(marginalizeKeys.size() == 0) { diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h index fb4b78fc2..b8a9941ad 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h @@ -152,7 +152,7 @@ namespace internal { /** Calculate the marginal on the specified keys, returning a set of LinearContainerFactors. * Unlike other GTSAM functions with similar purposes, this version can operate on disconnected graphs. */ NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, - const FastSet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction); + const KeySet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction); } diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index 9742aefd7..4c4442141 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -184,7 +184,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth previousSmootherSummarization_ = smootherSummarization; // Find the set of new separator keys - const FastSet& newSeparatorKeys = isam2_.getFixedVariables(); + const KeySet& newSeparatorKeys = isam2_.getFixedVariables(); // Use the shortcut to calculate an updated marginal on the current separator // Combine just the shortcut and the previousSmootherSummarization @@ -291,7 +291,7 @@ std::vector ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2 std::vector removedFactorSlots; const VariableIndex& variableIndex = isam2.getVariableIndex(); BOOST_FOREACH(Key key, keys) { - const FastList& slots = variableIndex[key]; + const FastVector& slots = variableIndex[key]; removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); } @@ -312,7 +312,7 @@ std::vector ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2 void ConcurrentIncrementalFilter::updateShortcut(const NonlinearFactorGraph& removedFactors) { // Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys - FastSet shortcutKeys; + KeySet shortcutKeys; BOOST_FOREACH(size_t slot, currentSmootherSummarizationSlots_) { const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot); if(factor) { @@ -343,7 +343,7 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization() // variables that result from marginalizing out all of the other variables // Find the set of current separator keys - const FastSet& separatorKeys = isam2_.getFixedVariables(); + const KeySet& separatorKeys = isam2_.getFixedVariables(); // Find all cliques that contain any separator variables std::set separatorCliques; diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp index 16a336695..b87409aae 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp @@ -245,7 +245,7 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() { } // Get the set of separator keys - gtsam::FastSet separatorKeys; + gtsam::KeySet separatorKeys; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { separatorKeys.insert(key_value.key); } 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 72c7c66f5..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,7 +149,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("LinearizedJacobianFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(Ab_); @@ -264,7 +264,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("LinearizedHessianFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(info_); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index 51c2a55a2..558654367 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -559,7 +559,7 @@ TEST( ConcurrentBatchSmoother, synchronize_3 ) ordering = smoother.getOrdering(); // I'm really hoping this is an acceptable ordering... GaussianFactorGraph::shared_ptr linearFactors = allFactors.linearize(allValues); - FastSet eliminateKeys = linearFactors->keys(); + KeySet eliminateKeys = linearFactors->keys(); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) { eliminateKeys.erase(key_value.key); } diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp index 5f91527e6..22dd0ccaa 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp @@ -579,7 +579,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 ) // GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph); // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate(); - FastSet allkeys = LinFactorGraph->keys(); + KeySet allkeys = LinFactorGraph->keys(); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) { allkeys.erase(key_value.key); } diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index 9708eedb5..c372577ca 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -581,7 +581,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 ) // GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph); // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate(); - FastSet allkeys = LinFactorGraph->keys(); + KeySet allkeys = LinFactorGraph->keys(); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) allkeys.erase(key_value.key); std::vector variables(allkeys.begin(), allkeys.end()); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp deleted file mode 100644 index 7c2f9d9b9..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, - OptionalJacobian<3,6> Dpose, - OptionalJacobian<3,3> 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/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 857c07761..7f9564ee3 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.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(dt_); ar & BOOST_SERIALIZATION_NVP(tau_); diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 6b111b759..d8fceeb68 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -386,7 +386,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/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 4218a5561..f6f1c4a5c 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -69,7 +69,7 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, Mechanization_bRn2 Mechanization_bRn2::correct(const Vector3& dx) const { Vector3 rho = sub(dx, 0, 3); - Rot3 delta_nRn = Rot3::rodriguez(rho); + Rot3 delta_nRn = Rot3::Rodrigues(rho); Rot3 bRn = bRn_ * delta_nRn; Vector3 x_g = x_g_ + sub(dx, 3, 6); @@ -101,10 +101,10 @@ Mechanization_bRn2 Mechanization_bRn2::integrate(const Vector3& u, // convert to navigation frame Rot3 nRb = bRn_.inverse(); - Vector3 n_omega_bn = (nRb*b_omega_bn).vector(); + Vector3 n_omega_bn = nRb * b_omega_bn; // integrate bRn using exponential map, assuming constant over dt - Rot3 delta_nRn = Rot3::rodriguez(n_omega_bn*dt); + Rot3 delta_nRn = Rot3::Rodrigues(n_omega_bn*dt); Rot3 bRn = bRn_.compose(delta_nRn); // implicit updating of biases (variables just do not change) diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index fa33ce5b9..4c85614d4 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -42,7 +42,7 @@ public: /// gravity in the body frame Vector3 b_g(double g_e) const { Vector3 n_g(0, 0, g_e); - return (bRn_ * n_g).vector(); + return bRn_ * n_g; } const Rot3& bRn() const {return bRn_; } diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index 088bfd2ea..f1487f562 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -70,7 +70,7 @@ namespace gtsam { * @param body_P_sensor is the transform from body to sensor frame (default identity) */ MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model, - FastSet poseKeys, Key pointKey, const boost::shared_ptr& K, + KeySet poseKeys, Key pointKey, const boost::shared_ptr& K, boost::optional body_P_sensor = boost::none) : Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor), throwCheirality_(false), verboseCheirality_(false) { @@ -91,7 +91,7 @@ namespace gtsam { * @param body_P_sensor is the transform from body to sensor frame (default identity) */ MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model, - FastSet poseKeys, Key pointKey, const boost::shared_ptr& K, + KeySet poseKeys, Key pointKey, const boost::shared_ptr& K, bool throwCheirality, bool verboseCheirality, boost::optional body_P_sensor = boost::none) : Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor), @@ -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 5f6d0ef92..e3080466f 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -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 991aae1fd..2de060302 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -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 0426c3ba4..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_); diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index 5906a6664..069274065 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -161,7 +161,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(throwCheirality_); 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 index 5d2ba3323..6651c005f 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -11,10 +11,11 @@ /** * @file SmartStereoProjectionFactor.h - * @brief Base class to create smart factors on poses or cameras + * @brief Smart stereo factor on StereoCameras (pose + calibration) * @author Luca Carlone * @author Zsolt Kira * @author Frank Dellaert + * @author Chris Beall */ #pragma once @@ -24,6 +25,7 @@ #include #include #include +#include #include #include @@ -33,114 +35,136 @@ namespace gtsam { +/// Linearization mode: what factor to linearize to + enum LinearizationMode { + HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD + }; + +/// How to manage degeneracy +enum DegeneracyMode { + IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY + }; + + /* + * Parameters for the smart stereo projection factors + */ + struct GTSAM_EXPORT SmartStereoProjectionParams { + + LinearizationMode linearizationMode; ///< How to linearize the factor + DegeneracyMode degeneracyMode; ///< How to linearize the factor + + /// @name Parameters governing the triangulation + /// @{ + TriangulationParameters triangulation; + double retriangulationThreshold; ///< threshold to decide whether to re-triangulate + /// @} + + /// @name Parameters governing how triangulation result is treated + /// @{ + bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false) + bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false) + /// @} + + + /// Constructor + SmartStereoProjectionParams(LinearizationMode linMode = HESSIAN, + DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false, + bool verboseCheirality = false) : + linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold( + 1e-5), throwCheirality(throwCheirality), verboseCheirality( + verboseCheirality) { + } + + virtual ~SmartStereoProjectionParams() { + } + + void print(const std::string& str) const { + std::cout << "linearizationMode: " << linearizationMode << "\n"; + std::cout << " degeneracyMode: " << degeneracyMode << "\n"; + std::cout << triangulation << std::endl; + } + + LinearizationMode getLinearizationMode() const { + return linearizationMode; + } + DegeneracyMode getDegeneracyMode() const { + return degeneracyMode; + } + TriangulationParameters getTriangulationParameters() const { + return triangulation; + } + bool getVerboseCheirality() const { + return verboseCheirality; + } + bool getThrowCheirality() const { + return throwCheirality; + } + void setLinearizationMode(LinearizationMode linMode) { + linearizationMode = linMode; + } + void setDegeneracyMode(DegeneracyMode degMode) { + degeneracyMode = degMode; + } + void setRankTolerance(double rankTol) { + triangulation.rankTolerance = rankTol; + } + void setEnableEPI(bool enableEPI) { + triangulation.enableEPI = enableEPI; + } + void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) { + triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold; + } + void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) { + triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; + } + }; + + + /** - * Structure for storing some state memory, used to speed up optimization - * @addtogroup SLAM - */ -class SmartStereoProjectionFactorState { + * SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around. + * This factor operates with StereoCamera. This factor requires that values + * contains the involved StereoCameras. Calibration is assumed to be fixed, as this + * is also assumed in StereoCamera. + * If you'd like to store poses in values instead of cameras, use + * SmartStereoProjectionPoseFactor instead +*/ +class SmartStereoProjectionFactor: public SmartFactorBase { +private: + + typedef SmartFactorBase Base; protected: -public: + /// @name Parameters + /// @{ + const SmartStereoProjectionParams params_; + /// @} - 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 - * TODO: why LANDMARK parameter? - */ -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 + /// @name Caching triangulation + /// @{ + mutable TriangulationResult result_; ///< result from triangulateSafe 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; + typedef boost::shared_ptr shared_ptr; /// Vector of cameras - typedef std::vector 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) + * @param params internal parameters of the smart factors */ - 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) { + SmartStereoProjectionFactor(const SharedNoiseModel& sharedNoiseModel, + const SmartStereoProjectionParams& params = + SmartStereoProjectionParams()) : + Base(sharedNoiseModel), // + params_(params), // + result_(TriangulationResult::Degenerate()) { } /** Virtual destructor */ @@ -154,14 +178,21 @@ public: */ 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; + std::cout << s << "SmartStereoProjectionFactor\n"; + std::cout << "linearizationMode:\n" << params_.linearizationMode << std::endl; + std::cout << "triangulationParameters:\n" << params_.triangulation << std::endl; + std::cout << "result:\n" << result_ << std::endl; Base::print("", keyFormatter); } + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const SmartStereoProjectionFactor *e = + dynamic_cast(&p); + return e && params_.linearizationMode == e->params_.linearizationMode + && Base::equals(p, tol); + } + /// 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 @@ -180,7 +211,7 @@ public: if (!retriangulate) { for (size_t i = 0; i < cameras.size(); i++) { if (!cameras[i].pose().equals(cameraPosesTriangulation_[i], - retriangulationThreshold_)) { + params_.retriangulationThreshold)) { retriangulate = true; // at least two poses are different, hence we retriangulate break; } @@ -198,448 +229,306 @@ public: 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 Values& values) const { - return triangulateSafe(this->cameras(values)); - } - - /// triangulateSafe - size_t triangulateSafe(const Cameras& cameras) const { + TriangulationResult 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) +// std::cout << "retriangulate = false" << std::endl; +// +// bool retriangulate = true; + 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; +// std::cout << "Retriangulate " << std::endl; + std::vector reprojections; + reprojections.reserve(m); + for(size_t i = 0; i < m; i++) { + reprojections.push_back(cameras[i].backproject(measured_[i])); } - } - return m; + + Point3 pw_sum; + BOOST_FOREACH(const Point3& pw, reprojections) { + pw_sum = pw_sum + pw; + } + // average reprojected landmark + Point3 pw_avg = pw_sum / double(m); + + double totalReprojError = 0; + + // check if it lies in front of all cameras + for(size_t i = 0; i < m; i++) { + const Pose3& pose = cameras[i].pose(); + const Point3& pl = pose.transform_to(pw_avg); + if (pl.z() <= 0) { + result_ = TriangulationResult::BehindCamera(); + return result_; + } + + // check landmark distance + if (params_.triangulation.landmarkDistanceThreshold > 0 && + pl.norm() > params_.triangulation.landmarkDistanceThreshold) { + result_ = TriangulationResult::Degenerate(); + return result_; + } + + if (params_.triangulation.dynamicOutlierRejectionThreshold > 0) { + const StereoPoint2& zi = measured_[i]; + StereoPoint2 reprojectionError(cameras[i].project(pw_avg) - zi); + totalReprojError += reprojectionError.vector().norm(); + } + } // for + + if (params_.triangulation.dynamicOutlierRejectionThreshold > 0 + && totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) { + result_ = TriangulationResult::Degenerate(); + return result_; + } + + if(params_.triangulation.enableEPI) { + try { + pw_avg = triangulateNonlinear(cameras, measured_, pw_avg); + } catch(StereoCheiralityException& e) { + if(params_.verboseCheirality) + std::cout << "Cheirality Exception in SmartStereoProjectionFactor" << std::endl; + if(params_.throwCheirality) + throw; + result_ = TriangulationResult::BehindCamera(); + return TriangulationResult::BehindCamera(); + } + } + + result_ = TriangulationResult(pw_avg); + + } // if retriangulate + return result_; + } /// 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; - } + triangulateSafe(cameras); // imperative, might reset result_ + return bool(result_); } /// linearize returns a Hessianfactor that is an approximation of error(p) - boost::shared_ptr > createHessianFactor( - const Cameras& cameras, const double lambda = 0.0) const { + boost::shared_ptr > createHessianFactor( + const Cameras& cameras, const double lambda = 0.0, bool diagonalDamping = + false) 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); + std::vector js; + std::vector Gs(numKeys * (numKeys + 1) / 2); + std::vector gs(numKeys); if (this->measured_.size() != cameras.size()) { std::cout - << "SmartProjectionHessianFactor: this->measured_.size() inconsistent with input" + << "SmartStereoProjectionHessianFactor: this->measured_.size() inconsistent with input" << std::endl; exit(1); } - this->triangulateSafe(cameras); - if (isDebug) std::cout << "point_ = " << point_ << std::endl; + triangulateSafe(cameras); - if (numKeys < 2 - || (!this->manageDegeneracy_ - && (this->cheiralityException_ || this->degenerate_))) { - if (isDebug) std::cout << "In linearize: exception" << std::endl; - BOOST_FOREACH(gtsam::Matrix& m, Gs) - m = zeros(D, D); + if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) { + // failed: return"empty" Hessian + BOOST_FOREACH(Matrix& m, Gs) + m = zeros(Base::Dim, Base::Dim); BOOST_FOREACH(Vector& v, gs) - v = zero(D); - return boost::make_shared >(this->keys_, Gs, gs, - 0.0); + v = zero(Base::Dim); + 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); - } - - // ================================================================== + // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols(). + std::vector Fblocks; Matrix F, E; - Matrix3 PointCov; Vector b; - double f = computeJacobians(F, E, PointCov, b, cameras, lambda); + computeJacobiansWithTriangulatedPoint(Fblocks, 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; + // Whiten using noise model + Base::whitenJacobians(Fblocks, E, b); - H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); - gs_vector.noalias() = F.transpose() - * (b - (E * (PointCov * (E.transpose() * b)))); + // build augmented hessian + SymmetricBlockMatrix augmentedHessian = // + Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping); - 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); + return boost::make_shared >(this->keys_, + augmentedHessian); } -// // create factor -// boost::shared_ptr > createImplicitSchurFactor( + // create factor +// boost::shared_ptr > createRegularImplicitSchurFactor( // const Cameras& cameras, double lambda) const { // if (triangulateForLinearize(cameras)) -// return Base::createImplicitSchurFactor(cameras, point_, lambda); +// return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda); // else -// return boost::shared_ptr >(); +// // failed: return empty +// return boost::shared_ptr >(); // } // // /// 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); +// return Base::createJacobianQFactor(cameras, *result_, lambda); // else -// return boost::make_shared< JacobianFactorQ >(this->keys_); +// // failed: return empty +// 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 ?? -// bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); -// if (nonDegenerate) -// return createJacobianQFactor(myCameras, lambda); -// else -// return boost::make_shared< JacobianFactorQ >(this->keys_); +// return createJacobianQFactor(this->cameras(values), lambda); +// } + + /// different (faster) way to compute Jacobian factor + boost::shared_ptr createJacobianSVDFactor( + const Cameras& cameras, double lambda) const { + if (triangulateForLinearize(cameras)) + return Base::createJacobianSVDFactor(cameras, *result_, lambda); + else + return boost::make_shared >(this->keys_); + } + +// /// linearize to a Hessianfactor +// virtual boost::shared_ptr > linearizeToHessian( +// const Values& values, double lambda = 0.0) const { +// return createHessianFactor(this->cameras(values), lambda); +// } + +// /// linearize to an Implicit Schur factor +// virtual boost::shared_ptr > linearizeToImplicit( +// const Values& values, double lambda = 0.0) const { +// return createRegularImplicitSchurFactor(this->cameras(values), lambda); // } // - /// 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_); - } +// /// linearize to a JacobianfactorQ +// virtual boost::shared_ptr > linearizeToJacobian( +// const Values& values, double lambda = 0.0) const { +// return createJacobianQFactor(this->cameras(values), lambda); +// } - /// 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; + /** + * Linearize to Gaussian Factor + * @param values Values structure which must contain camera poses for this factor + * @return a Gaussian factor + */ + boost::shared_ptr linearizeDamped(const Cameras& cameras, + const double lambda = 0.0) const { + // depending on flag set on construction we may linearize to different linear factors + switch (params_.linearizationMode) { + case HESSIAN: + return createHessianFactor(cameras, lambda); +// case IMPLICIT_SCHUR: +// return createRegularImplicitSchurFactor(cameras, lambda); + case JACOBIAN_SVD: + return createJacobianSVDFactor(cameras, lambda); +// case JACOBIAN_Q: +// return createJacobianQFactor(cameras, lambda); + default: + throw std::runtime_error("SmartStereoFactorlinearize: unknown mode"); } - return true; } - /// Takes values - bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + /** + * Linearize to Gaussian Factor + * @param values Values structure which must contain camera poses for this factor + * @return a Gaussian factor + */ + boost::shared_ptr linearizeDamped(const Values& values, + const double lambda = 0.0) const { + // depending on flag set on construction we may linearize to different linear factors + Cameras cameras = this->cameras(values); + return linearizeDamped(cameras, lambda); + } + + /// linearize + virtual boost::shared_ptr linearize( + const Values& values) const { + return linearizeDamped(values); + } + + /** + * Triangulate and compute derivative of error with respect to point + * @return whether triangulation worked + */ + bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const { + bool nonDegenerate = triangulateForLinearize(cameras); if (nonDegenerate) - computeEP(E, PointCov, myCameras); + cameras.project2(*result_, boost::none, E); return nonDegenerate; } - /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const { - return Base::computeEP(E, PointCov, cameras, point_); + /** + * Triangulate and compute derivative of error with respect to point + * @return whether triangulation worked + */ + bool triangulateAndComputeE(Matrix& E, const Values& values) const { + Cameras cameras = this->cameras(values); + return triangulateAndComputeE(E, cameras); } - /// Version that takes values, and creates the point - bool computeJacobians(std::vector& Fblocks, - Matrix& E, Matrix& PointCov, Vector& b, const Values& values) const { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); - if (nonDegenerate) - computeJacobians(Fblocks, E, PointCov, 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; -// } + void computeJacobiansWithTriangulatedPoint( + std::vector& Fblocks, Matrix& E, Vector& b, + const Cameras& cameras) const { + + if (!result_) { + throw ("computeJacobiansWithTriangulatedPoint"); +// // Handle degeneracy +// // TODO check flag whether we should do this +// Unit3 backProjected; /* = cameras[0].backprojectPointAtInfinity( +// this->measured_.at(0)); */ // -// 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; +// Base::computeJacobians(Fblocks, E, b, cameras, backProjected); } else { - // nondegenerate: just return Base version - return Base::computeJacobians(Fblocks, E, b, cameras, point_); - } // end else + // valid result: just return Base version + Base::computeJacobians(Fblocks, E, b, cameras, *result_); + } } -// /// 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, Matrix3& PointCov, Vector& b, - const Cameras& cameras, const double lambda) const { - return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda); - } - - /// 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); + /// Version that takes values, and creates the point + bool triangulateAndComputeJacobians( + std::vector& Fblocks, Matrix& E, Vector& b, + const Values& values) const { + Cameras cameras = this->cameras(values); + bool nonDegenerate = triangulateForLinearize(cameras); if (nonDegenerate) - return reprojectionError(myCameras); + computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); + return nonDegenerate; + } + + /// takes values + bool triangulateAndComputeJacobiansSVD( + std::vector& Fblocks, Matrix& Enull, Vector& b, + const Values& values) const { + Cameras cameras = this->cameras(values); + bool nonDegenerate = triangulateForLinearize(cameras); + if (nonDegenerate) + Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_); + return nonDegenerate; + } + + /// Calculate vector of re-projection errors, before applying noise model + Vector reprojectionErrorAfterTriangulation(const Values& values) const { + Cameras cameras = this->cameras(values); + bool nonDegenerate = triangulateForLinearize(cameras); + if (nonDegenerate) + return Base::unwhitenedError(cameras, *result_); else - return zero(myCameras.size() * 3); + return zero(cameras.size() * Base::ZDim); } /** @@ -651,104 +540,78 @@ public: 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 (externalPoint) + result_ = TriangulationResult(*externalPoint); + else + result_ = triangulateSafe(cameras); - if (nrCameras < 2 - || (!this->manageDegeneracy_ - && (this->cheiralityException_ || this->degenerate_))) { + if (result_) + // All good, just use version in base class + return Base::totalReprojectionError(cameras, *result_); + else if (params_.degeneracyMode == HANDLE_INFINITY) { + throw(std::runtime_error("Backproject at infinity not implemented for SmartStereo.")); +// // Otherwise, manage the exceptions with rotation-only factors +// const StereoPoint2& z0 = this->measured_.at(0); +// Unit3 backprojected; //= cameras.front().backprojectPointAtInfinity(z0); +// +// return Base::totalReprojectionError(cameras, backprojected); + } else { // 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; + /// Calculate total reprojection error + virtual double error(const Values& values) const { + if (this->active(values)) { + return totalReprojectionError(Base::cameras(values)); + } else { // else of active flag + return 0.0; + } + } /** return the landmark */ - boost::optional point() const { - return point_; - } + TriangulationResult point() const { + return result_; + } - /** COMPUTE the landmark */ - boost::optional point(const Values& values) const { - triangulateSafe(values); - return point_; - } + /** COMPUTE the landmark */ + TriangulationResult point(const Values& values) const { + Cameras cameras = this->cameras(values); + return triangulateSafe(cameras); + } - /** return the degenerate state */ - inline bool isDegenerate() const { - return (cheiralityException_ || degenerate_); - } + /// Is result valid? + bool isValid() const { + return bool(result_); + } - /** return the cheirality status flag */ - inline bool isPointBehindCamera() const { - return cheiralityException_; - } - /** return chirality verbosity */ - inline bool verboseCheirality() const { - return verboseCheirality_; - } + /** return the degenerate state */ + bool isDegenerate() const { + return result_.degenerate(); + } - /** return flag for throwing cheirality exceptions */ - inline bool throwCheirality() const { - return throwCheirality_; - } + /** return the cheirality status flag */ + bool isPointBehindCamera() const { + return result_.behindCamera(); + } 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_); + ar & BOOST_SERIALIZATION_NVP(params_.throwCheirality); + ar & BOOST_SERIALIZATION_NVP(params_.verboseCheirality); } }; /// traits -template -struct traits > : - public Testable > { +template<> +struct traits : public Testable< + SmartStereoProjectionFactor> { }; } // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 2e3bc866b..c2526fd74 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -11,10 +11,11 @@ /** * @file SmartStereoProjectionPoseFactor.h - * @brief Produces an Hessian factors on POSES from monocular measurements of a single landmark + * @brief Smart stereo factor on poses, assuming camera calibration is fixed * @author Luca Carlone * @author Chris Beall * @author Zsolt Kira + * @author Frank Dellaert */ #pragma once @@ -34,46 +35,41 @@ namespace gtsam { */ /** - * The calibration is known here. The factor only constraints poses (variable dimension is 6) + * This factor assumes that camera calibration is fixed, but each camera + * has its own calibration. + * The factor only constrains poses (variable dimension is 6). + * This factor requires that values contains the involved poses (Pose3). * @addtogroup SLAM */ -template -class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { +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) + 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; + typedef SmartStereoProjectionFactor Base; /// shorthand for this class - typedef SmartStereoProjectionPoseFactor This; + 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) + * @param Isotropic measurement noise + * @param params internal parameters of the smart factors */ - 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) {} + SmartStereoProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel, + const SmartStereoProjectionParams& params = + SmartStereoProjectionParams()) : + Base(sharedNoiseModel, params) { + } /** Virtual destructor */ virtual ~SmartStereoProjectionPoseFactor() {} @@ -82,27 +78,23 @@ public: * 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 + * @param K is the (fixed) 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); + void add(const StereoPoint2 measured, const Key poseKey, + const boost::shared_ptr K) { + Base::add(measured, poseKey); + K_all_.push_back(K); } /** * 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); + std::vector > Ks) { + Base::add(measurements, poseKeys); for (size_t i = 0; i < measurements.size(); i++) { K_all_.push_back(Ks.at(i)); } @@ -112,13 +104,12 @@ public: * 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) { + const boost::shared_ptr K) { for (size_t i = 0; i < measurements.size(); i++) { - Base::add(measurements.at(i), poseKeys.at(i), noise); + Base::add(measurements.at(i), poseKeys.at(i)); K_all_.push_back(K); } } @@ -131,62 +122,19 @@ public: 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_) + 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); + const SmartStereoProjectionPoseFactor *e = + dynamic_cast(&p); 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 - * 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. */ @@ -199,16 +147,33 @@ public: } /** return the calibration object */ - inline const std::vector > calibration() const { + inline const std::vector > calibration() const { return K_all_; } + /** + * 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 + */ + Base::Cameras cameras(const Values& values) const { + Base::Cameras cameras; + size_t i=0; + BOOST_FOREACH(const Key& k, this->keys_) { + const Pose3& pose = values.at(k); + StereoCamera camera(pose, K_all_[i++]); + cameras.push_back(camera); + } + return cameras; + } + 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_); } @@ -216,9 +181,9 @@ private: }; // end of class declaration /// traits -template -struct traits > : - public Testable > { +template<> +struct traits : public Testable< + SmartStereoProjectionPoseFactor> { }; } // \ namespace gtsam diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index 2abc49fa1..156ac242e 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -217,7 +217,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("NonlinearFactor", boost::serialization::base_object(*this)); //ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index adfb9ae36..bf10ae531 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -415,7 +415,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("NonlinearFactor", boost::serialization::base_object(*this)); //ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index 31995e769..7928a2aac 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -5,26 +5,24 @@ * @author Alex Cunningham */ +#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 @@ -81,9 +79,6 @@ typedef RangeFactor RangeFactorSimpleCameraP typedef RangeFactor RangeFactorCalibratedCamera; typedef RangeFactor RangeFactorSimpleCamera; -typedef BearingFactor BearingFactor2D; -typedef BearingFactor BearingFactor3D; - typedef BearingRangeFactor BearingRangeFactor2D; typedef BearingRangeFactor BearingRangeFactor3D; @@ -186,8 +181,6 @@ BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleC BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera"); BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera"); -BOOST_CLASS_EXPORT_GUID(BearingFactor2D, "gtsam::BearingFactor2D"); - BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D"); BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2"); diff --git a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp index d8a3ba0c1..c85187d5f 100644 --- a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp +++ b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp @@ -21,7 +21,7 @@ using symbol_shorthand::B; TEST(BiasedGPSFactor, errorNoiseless) { - Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); + Rot3 R = Rot3::Rodrigues(0.1, 0.2, 0.3); Point3 t(1.0, 0.5, 0.2); Pose3 pose(R,t); Point3 bias(0.0,0.0,0.0); @@ -36,7 +36,7 @@ TEST(BiasedGPSFactor, errorNoiseless) { TEST(BiasedGPSFactor, errorNoisy) { - Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); + Rot3 R = Rot3::Rodrigues(0.1, 0.2, 0.3); Point3 t(1.0, 0.5, 0.2); Pose3 pose(R,t); Point3 bias(0.0,0.0,0.0); @@ -51,7 +51,7 @@ TEST(BiasedGPSFactor, errorNoisy) { TEST(BiasedGPSFactor, jacobian) { - Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); + Rot3 R = Rot3::Rodrigues(0.1, 0.2, 0.3); Point3 t(1.0, 0.5, 0.2); Pose3 pose(R,t); Point3 bias(0.0,0.0,0.0); diff --git a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp index ff7307840..209326672 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/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/testMultiProjectionFactor.cpp b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp index 2794e5707..ca91d4cb5 100644 --- a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp @@ -69,7 +69,7 @@ TEST( MultiProjectionFactor, create ){ n_measPixel << 10, 10, 10, 10, 10, 10; const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); - FastSet views; + KeySet views; views.insert(x1); views.insert(x2); views.insert(x3); diff --git a/gtsam_unstable/slam/tests/testSerialization.cpp b/gtsam_unstable/slam/tests/testSerialization.cpp index a5e09515c..1d8b30850 100644 --- a/gtsam_unstable/slam/tests/testSerialization.cpp +++ b/gtsam_unstable/slam/tests/testSerialization.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 4691da384..2981f675d 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -18,8 +18,8 @@ * @date Sept 2013 */ +// TODO #include #include - #include #include #include @@ -32,42 +32,38 @@ using namespace std; using namespace boost::assign; using namespace gtsam; -static bool isDebugTest = true; - // 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 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 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 SmartStereoProjectionParams params; -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(3)); +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 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)); +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 stereo_projectToMultipleCameras(const StereoCamera& cam1, + const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { vector measurements_cam; @@ -81,58 +77,50 @@ vector stereo_projectToMultipleCameras( return measurements_cam; } +LevenbergMarquardtParams lm_params; + /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor) { - fprintf(stderr,"Test 1 Complete"); - SmartFactor::shared_ptr factor1(new SmartFactor()); + SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model)); } /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor2) { - SmartFactor factor1(rankTol, linThreshold); + SmartStereoProjectionPoseFactor factor1(model, params); } /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor3) { - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(measurement1, poseKey1, model, K); + SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model)); + factor1->add(measurement1, poseKey1, 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); + SmartStereoProjectionPoseFactor factor1(model, params); + factor1.add(measurement1, poseKey1, K); } /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Equals ) { - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(measurement1, poseKey1, model, K); + SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model)); + factor1->add(measurement1, poseKey1, K); - SmartFactor::shared_ptr factor2(new SmartFactor()); - factor2->add(measurement1, poseKey1, model, K); + SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor(model)); + factor2->add(measurement1, poseKey1, K); CHECK(assert_equal(*factor1, *factor2)); } /* *************************************************************************/ -TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ){ - // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl; - +TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { // 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)); 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)); + 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 @@ -146,15 +134,15 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ){ 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); + SmartStereoProjectionPoseFactor factor1(model); + factor1.add(level_uv, x1, K2); + factor1.add(level_uv_right, x2, K2); double actualError = factor1.error(values); double expectedError = 0.0; EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); - SmartFactor::Cameras cameras = factor1.cameras(values); + SmartStereoProjectionPoseFactor::Cameras cameras = factor1.cameras(values); double actualError2 = factor1.totalReprojectionError(cameras); EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); @@ -164,75 +152,69 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ){ } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, noisy ){ - // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl; - +TEST( SmartStereoProjectionPoseFactor, noisy ) { // 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)); 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)); + 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 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), 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); + SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model)); + factor1->add(level_uv, x1, K); + factor1->add(level_uv_right, x2, K); - double actualError1= factor1->error(values); + double actualError1 = factor1->error(values); - SmartFactor::shared_ptr factor2(new SmartFactor()); + SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor(model)); vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); - std::vector< SharedNoiseModel > 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); + factor2->add(measurements, views, Ks); - double actualError2= factor2->error(values); + double actualError2 = factor2->error(values); DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } - /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ - cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; +TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { // 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)); StereoCamera 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)); StereoCamera 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)); StereoCamera cam3(pose3, K2); // three landmarks ~5 meters infront of camera @@ -241,26 +223,33 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ 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 measurements_l1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_l2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_l3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); - std::vector views; + 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); + SmartStereoProjectionParams smart_params; + smart_params.triangulation.enableEPI = true; + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, smart_params)); + smartFactor1->add(measurements_l1, views, K2); - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - smartFactor2->add(measurements_cam2, views, model, K2); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, smart_params)); + smartFactor2->add(measurements_l2, views, K2); - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); - smartFactor3->add(measurements_cam3, views, model, K2); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, smart_params)); + smartFactor3->add(measurements_l3, views, K2); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); @@ -268,52 +257,110 @@ TEST( SmartStereoProjectionPoseFactor, 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; + // cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl; + EXPECT_DOUBLES_EQUAL(797312.95069157204, graph.error(values), 1e-7); + + // get triangulated landmarks from smart factors + Point3 landmark1_smart = *smartFactor1->point(); + Point3 landmark2_smart = *smartFactor2->point(); + Point3 landmark3_smart = *smartFactor3->point(); Values result; gttic_(SmartStereoProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); result = optimizer.optimize(); gttoc_(SmartStereoProjectionPoseFactor); tictoc_finishedIteration_(); -// GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); -// VectorValues delta = GFG->optimize(); + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5); + +// cout << std::setprecision(10) << "SmartStereoFactor graph optimized error: " << graph.error(result) << endl; + + GaussianFactorGraph::shared_ptr GFG = graph.linearize(result); + VectorValues delta = GFG->optimize(); + VectorValues expected = VectorValues::Zero(delta); + EXPECT(assert_equal(expected, delta, 1e-6)); // 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_(); + EXPECT(assert_equal(pose3, result.at(x3))); + + /* *************************************************************** + * Same problem with regular Stereo factors, expect same error! + * ****************************************************************/ + +// landmark1_smart.print("landmark1_smart"); +// landmark2_smart.print("landmark2_smart"); +// landmark3_smart.print("landmark3_smart"); + + // add landmarks to values + values.insert(L(1), landmark1_smart); + values.insert(L(2), landmark2_smart); + values.insert(L(3), landmark3_smart); + + // add factors + NonlinearFactorGraph graph2; + + graph2.push_back(PriorFactor(x1, pose1, noisePrior)); + graph2.push_back(PriorFactor(x2, pose2, noisePrior)); + + typedef GenericStereoFactor ProjectionFactor; + + bool verboseCheirality = true; + + graph2.push_back(ProjectionFactor(measurements_l1[0], model, x1, L(1), K2, false, verboseCheirality)); + graph2.push_back(ProjectionFactor(measurements_l1[1], model, x2, L(1), K2, false, verboseCheirality)); + graph2.push_back(ProjectionFactor(measurements_l1[2], model, x3, L(1), K2, false, verboseCheirality)); + + graph2.push_back(ProjectionFactor(measurements_l2[0], model, x1, L(2), K2, false, verboseCheirality)); + graph2.push_back(ProjectionFactor(measurements_l2[1], model, x2, L(2), K2, false, verboseCheirality)); + graph2.push_back(ProjectionFactor(measurements_l2[2], model, x3, L(2), K2, false, verboseCheirality)); + + graph2.push_back(ProjectionFactor(measurements_l3[0], model, x1, L(3), K2, false, verboseCheirality)); + graph2.push_back(ProjectionFactor(measurements_l3[1], model, x2, L(3), K2, false, verboseCheirality)); + graph2.push_back(ProjectionFactor(measurements_l3[2], model, x3, L(3), K2, false, verboseCheirality)); + +// cout << std::setprecision(10) << "\n----StereoFactor graph initial error: " << graph2.error(values) << endl; + EXPECT_DOUBLES_EQUAL(797312.95069157204, graph2.error(values), 1e-7); + + LevenbergMarquardtOptimizer optimizer2(graph2, values, lm_params); + Values result2 = optimizer2.optimize(); + EXPECT_DOUBLES_EQUAL(0, graph2.error(result2), 1e-5); + +// cout << std::setprecision(10) << "StereoFactor graph optimized error: " << graph2.error(result2) << endl; + } - /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ +TEST( SmartStereoProjectionPoseFactor, 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)); StereoCamera 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)); StereoCamera 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)); StereoCamera cam3(pose3, K); // three landmarks ~5 meters infront of camera @@ -322,18 +369,24 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ 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 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); + SmartStereoProjectionParams params; + params.setLinearizationMode(JACOBIAN_SVD); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(model, params)); + smartFactor1->add(measurements_cam1, views, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor2->add(measurements_cam2, views, K); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor3->add(measurements_cam3, views, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -344,38 +397,38 @@ TEST( SmartStereoProjectionPoseFactor, 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); + LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3, result.at(x3))); } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ +TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { - double excludeLandmarksFutherThanDist = 2; +// 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)); StereoCamera 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)); StereoCamera 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)); StereoCamera cam3(pose3, K); // three landmarks ~5 meters infront of camera @@ -384,19 +437,25 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ 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 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); + SmartStereoProjectionParams params; + params.setLinearizationMode(JACOBIAN_SVD); + params.setLandmarkDistanceThreshold(2); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor1->add(measurements_cam1, views, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor2->add(measurements_cam2, views, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor3->add(measurements_cam3, views, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -407,40 +466,37 @@ TEST( SmartStereoProjectionPoseFactor, 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); + LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); result = optimizer.optimize(); - EXPECT(assert_equal(values.at(x3),result.at(x3))); + EXPECT(assert_equal(values.at(x3), result.at(x3))); } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ +TEST( SmartStereoProjectionPoseFactor, 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)); StereoCamera 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)); StereoCamera 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)); StereoCamera cam3(pose3, K); // three landmarks ~5 meters infront of camera @@ -450,29 +506,37 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ 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); + 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 + + SmartStereoProjectionParams params; + params.setLinearizationMode(JACOBIAN_SVD); + params.setDynamicOutlierRejectionThreshold(1); - measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10,10,1); // add outlier + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor1->add(measurements_cam1, views, K); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, - JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor2->add(measurements_cam2, views, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor3->add(measurements_cam3, views, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor4(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor4->add(measurements_cam4, views, K); - SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor4->add(measurements_cam4, views, model, K); + // same as factor 4, but dynamic outlier rejection is off + SmartStereoProjectionPoseFactor::shared_ptr smartFactor4b(new SmartStereoProjectionPoseFactor(model)); + smartFactor4b->add(measurements_cam4, views, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -484,30 +548,46 @@ TEST( SmartStereoProjectionPoseFactor, 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); values.insert(x3, pose3); - // All factors are disabled and pose should remain where it is - LevenbergMarquardtParams params; + EXPECT_DOUBLES_EQUAL(0, smartFactor1->error(values), 1e-9); + EXPECT_DOUBLES_EQUAL(0, smartFactor2->error(values), 1e-9); + EXPECT_DOUBLES_EQUAL(0, smartFactor3->error(values), 1e-9); + // zero error due to dynamic outlier rejection + EXPECT_DOUBLES_EQUAL(0, smartFactor4->error(values), 1e-9); + + // dynamic outlier rejection is off + EXPECT_DOUBLES_EQUAL(6700, smartFactor4b->error(values), 1e-9); + + // Factors 1-3 should have valid point, factor 4 should not + EXPECT(smartFactor1->point()); + EXPECT(smartFactor2->point()); + EXPECT(smartFactor3->point()); + EXPECT(smartFactor4->point().degenerate()); + EXPECT(smartFactor4b->point()); + + // Factor 4 is disabled, pose 3 stays put Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3, result.at(x3))); } // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, 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)); // StereoCamera cam1(pose1, K); // // create second camera 1 meter to the right of first camera // Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); @@ -528,13 +608,13 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ // 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)); +// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(1, -1, false, false, JACOBIAN_Q)); // smartFactor1->add(measurements_cam1, views, model, K); // -// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); +// SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(1, -1, false, false, JACOBIAN_Q)); // smartFactor2->add(measurements_cam2, views, model, K); // -// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); +// SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(1, -1, false, false, JACOBIAN_Q)); // smartFactor3->add(measurements_cam3, views, model, K); // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -546,31 +626,29 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ // 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); // -// LevenbergMarquardtParams params; -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, params); +//// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); // result = optimizer.optimize(); // EXPECT(assert_equal(pose3,result.at(x3))); //} // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, 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)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -606,7 +684,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ // 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); @@ -614,36 +692,31 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ // 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); +// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); // Values result = optimizer.optimize(); // -// if(isDebugTest) result.at(x3).print("Pose3 after optimization: "); // EXPECT(assert_equal(pose3,result.at(x3))); //} // /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, CheckHessian){ +TEST( SmartStereoProjectionPoseFactor, 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)); 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)); + // 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 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + // 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 @@ -651,55 +724,64 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ 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); + // 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); + SmartStereoProjectionParams params; + params.setRankTolerance(10); - double rankTol = 10; + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor1->add(measurements_cam1, views, K); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor2->add(measurements_cam2, views, 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); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor3->add(measurements_cam3, views, K); + // Create graph to optimize NonlinearFactorGraph graph; graph.push_back(smartFactor1); 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 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: "); + 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); - boost::shared_ptr hessianFactor1 = smartFactor1->linearize(values); - boost::shared_ptr hessianFactor2 = smartFactor2->linearize(values); - boost::shared_ptr hessianFactor3 = smartFactor3->linearize(values); + // 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(); + 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)); @@ -707,15 +789,14 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){ -// // cout << " ************************ SmartStereoProjectionPoseFactor: 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)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -737,15 +818,15 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); // // double rankTol = 50; -// SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy)); // smartFactor1->add(measurements_cam1, views, model, K2); // -// SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(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); +// Point3 positionPrior = Point3(0,0,1); // // NonlinearFactorGraph graph; // graph.push_back(smartFactor1); @@ -754,43 +835,34 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // 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); // // 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); +// LevenbergMarquardtOptimizer optimizer(graph, values, lm_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: "); -// 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_(); //} // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ -// // cout << " ************************ SmartStereoProjectionPoseFactor: 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)); // StereoCamera cam1(pose1, K); // // // create second camera 1 meter to the right of first camera @@ -815,18 +887,18 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // // double rankTol = 10; // -// SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy)); // smartFactor1->add(measurements_cam1, views, model, K); // -// SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy)); // smartFactor2->add(measurements_cam2, views, model, K); // -// SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(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); +// Point3 positionPrior = Point3(0,0,1); // // NonlinearFactorGraph graph; // graph.push_back(smartFactor1); @@ -836,43 +908,34 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // 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: "); -// -// LevenbergMarquardtParams params; -// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; -// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; // // Values result; // gttic_(SmartStereoProjectionPoseFactor); -// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// LevenbergMarquardtOptimizer optimizer(graph, values, lm_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: "); -// 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_(); //} // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, Hessian ){ -// // cout << " ************************ SmartStereoProjectionPoseFactor: 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)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -889,16 +952,15 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // measurements_cam1.push_back(cam1_uv1); // measurements_cam1.push_back(cam2_uv1); // -// SmartFactor::shared_ptr smartFactor1(new SmartFactor()); +// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor()); // 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"); // // // compute triangulation from linearization point // // compute reprojection errors (sum squared) @@ -908,78 +970,84 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ){ - // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian **********************" << endl; - - std::vector views; +TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { + 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)); StereoCamera 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)); StereoCamera 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)); StereoCamera cam3(pose3, K); Point3 landmark1(5, 0.5, 1.2); - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); - SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); - smartFactorInstance->add(measurements_cam1, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactorInstance(new SmartStereoProjectionPoseFactor(model)); + smartFactorInstance->add(measurements_cam1, views, K); Values values; values.insert(x1, pose1); 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-6)); } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ){ - // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; +TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { - 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)); StereoCamera cam1(pose1, K2); // Second and third cameras in same place, which is a degenerate configuration @@ -990,49 +1058,56 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ){ 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); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor(new SmartStereoProjectionPoseFactor(model)); + smartFactor->add(measurements_cam1, views, 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); - 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); // 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-6)); - 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-6)); } - /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ - diff --git a/matlab.h b/matlab.h index 349cdeea4..4a9ac2309 100644 --- a/matlab.h +++ b/matlab.h @@ -71,16 +71,16 @@ FastVector createKeyVector(string s, const Vector& I) { } // Create a KeySet from indices -FastSet createKeySet(const Vector& I) { - FastSet set; +KeySet createKeySet(const Vector& I) { + KeySet set; for (int i = 0; i < I.size(); i++) set.insert(I[i]); return set; } // Create a KeySet from indices using symbol -FastSet createKeySet(string s, const Vector& I) { - FastSet set; +KeySet createKeySet(string s, const Vector& I) { + KeySet set; char c = s[0]; for (int i = 0; i < I.size(); i++) set.insert(symbol(c, I[i])); @@ -229,7 +229,7 @@ Values localToWorld(const Values& local, const Pose2& base, // if no keys given, get all keys from local values FastVector keys(user_keys); if (keys.size()==0) - keys = FastVector(local.keys()); + keys = local.keys(); // Loop over all keys BOOST_FOREACH(Key key, keys) { 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_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/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index 4cbe42364..e43fbcf76 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -48,8 +48,10 @@ testVisualISAMExample 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/tests/simulated2D.h b/tests/simulated2D.h index 37d2455eb..a1080a6de 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -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_); } diff --git a/gtsam/nonlinear/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp similarity index 75% rename from gtsam/nonlinear/tests/testExpressionFactor.cpp rename to tests/testExpressionFactor.cpp index 99b8120e3..48cf03f8c 100644 --- a/gtsam/nonlinear/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -21,7 +21,9 @@ #include #include #include +#include #include +#include #include #include @@ -36,6 +38,10 @@ using namespace gtsam; Point2 measured(-17, 30); SharedNoiseModel model = noiseModel::Unit::Create(2); +// This deals with the overload problem and makes the expressions factor +// understand that we work on Point3 +Point2 (*Project)(const Point3&, OptionalJacobian<2, 3>) = &PinholeBase::Project; + namespace leaf { // Create some values struct MyValues: public Values { @@ -169,7 +175,7 @@ static Point2 myUncal(const Cal3_S2& K, const Point2& p, // Binary(Leaf,Leaf) TEST(ExpressionFactor, Binary) { - typedef BinaryExpression Binary; + typedef internal::BinaryExpression Binary; Cal3_S2_ K_(1); Point2_ p_(2); @@ -184,11 +190,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 @@ -197,8 +207,8 @@ 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 - ExecutionTraceStorage traceStorage[size]; - ExecutionTrace trace; + internal::ExecutionTraceStorage traceStorage[size]; + internal::ExecutionTrace trace; Point2 value = binary.traceExecution(values, trace, traceStorage); EXPECT(assert_equal(Point2(),value, 1e-9)); // trace.print(); @@ -212,9 +222,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)) @@ -250,22 +259,22 @@ TEST(ExpressionFactor, Shallow) { 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); - ExecutionTraceStorage traceStorage[size]; - ExecutionTrace trace; + internal::ExecutionTraceStorage traceStorage[size]; + internal::ExecutionTrace trace; Point2 value = expression.traceExecution(values, trace, traceStorage); EXPECT(assert_equal(Point2(),value, 1e-9)); // trace.print(); @@ -277,7 +286,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); @@ -309,7 +318,7 @@ TEST(ExpressionFactor, tree) { // Create expression tree Point3_ p_cam(x, &Pose3::transform_to, p); - Point2_ xy_hat(PinholeCamera::project_to_camera, p_cam); + Point2_ xy_hat(Project, p_cam); Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); // Create factor and check value, dimension, linearization @@ -327,8 +336,6 @@ TEST(ExpressionFactor, tree) { boost::shared_ptr gf2 = f2.linearize(values); EXPECT( assert_equal(*expected, *gf2, 1e-9)); - TernaryExpression::Function fff = project6; - // Try ternary version ExpressionFactor f3(model, measured, project3(x, p, K)); EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9); @@ -485,7 +492,7 @@ TEST(ExpressionFactor, tree_finite_differences) { // Create expression tree Point3_ p_cam(x, &Pose3::transform_to, p); - Point2_ xy_hat(PinholeCamera::project_to_camera, p_cam); + Point2_ xy_hat(Project, p_cam); Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); const double fd_step = 1e-5; @@ -493,6 +500,106 @@ TEST(ExpressionFactor, tree_finite_differences) { EXPECT_CORRECT_EXPRESSION_JACOBIANS(uv_hat, values, fd_step, tolerance); } +TEST(ExpressionFactor, push_back) { + NonlinearFactorGraph graph; + graph.addExpressionFactor(model, Point2(0, 0), leaf::p); +} + +/* ************************************************************************* */ +// Test with multiple compositions on duplicate keys +struct Combine { + double a, b; + Combine(double a, double b) : a(a), b(b) {} + double operator()(const double& x, const double& y, OptionalJacobian<1, 1> H1, + OptionalJacobian<1, 1> H2) { + if (H1) (*H1) << a; + if (H2) (*H2) << b; + return a * x + b * y; + } +}; + +TEST(Expression, testMultipleCompositions) { + const double tolerance = 1e-5; + const double fd_step = 1e-5; + + Values values; + values.insert(1, 10.0); + values.insert(2, 20.0); + + Expression v1_(Key(1)); + Expression v2_(Key(2)); + + // BinaryExpression(1,2) + // Leaf, key = 1 + // Leaf, key = 2 + Expression sum1_(Combine(1, 2), v1_, v2_); + EXPECT(sum1_.keys() == list_of(1)(2)); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum1_, values, fd_step, tolerance); + + // BinaryExpression(3,4) + // BinaryExpression(1,2) + // Leaf, key = 1 + // Leaf, key = 2 + // Leaf, key = 1 + Expression sum2_(Combine(3, 4), sum1_, v1_); + EXPECT(sum2_.keys() == list_of(1)(2)); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance); + + // BinaryExpression(5,6) + // BinaryExpression(3,4) + // BinaryExpression(1,2) + // Leaf, key = 1 + // Leaf, key = 2 + // Leaf, key = 1 + // BinaryExpression(1,2) + // Leaf, key = 1 + // Leaf, key = 2 + Expression sum3_(Combine(5, 6), sum1_, sum2_); + EXPECT(sum3_.keys() == list_of(1)(2)); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance); +} + +/* ************************************************************************* */ +// Another test, with Ternary Expressions +static double combine3(const double& x, const double& y, const double& z, + OptionalJacobian<1, 1> H1, OptionalJacobian<1, 1> H2, + OptionalJacobian<1, 1> H3) { + if (H1) (*H1) << 1.0; + if (H2) (*H2) << 2.0; + if (H3) (*H3) << 3.0; + return x + 2.0 * y + 3.0 * z; +} + +TEST(Expression, testMultipleCompositions2) { + const double tolerance = 1e-5; + const double fd_step = 1e-5; + + Values values; + values.insert(1, 10.0); + values.insert(2, 20.0); + values.insert(3, 30.0); + + Expression v1_(Key(1)); + Expression v2_(Key(2)); + Expression v3_(Key(3)); + + Expression sum1_(Combine(4,5), v1_, v2_); + EXPECT(sum1_.keys() == list_of(1)(2)); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum1_, values, fd_step, tolerance); + + Expression sum2_(combine3, v1_, v2_, v3_); + EXPECT(sum2_.keys() == list_of(1)(2)(3)); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance); + + Expression sum3_(combine3, v3_, v2_, v1_); + EXPECT(sum3_.keys() == list_of(1)(2)(3)); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance); + + Expression sum4_(combine3, sum1_, sum2_, sum3_); + EXPECT(sum4_.keys() == list_of(1)(2)(3)); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum4_, values, fd_step, tolerance); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 3a0081bdb..88dc91ce7 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -532,7 +532,7 @@ TEST(GaussianFactorGraph, hasConstraints) #include #include #include -#include +#include /* ************************************************************************* */ TEST( GaussianFactorGraph, conditional_sigma_failure) { diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index b43f0d3ef..58c718726 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -9,7 +9,7 @@ #include #include #include -#include +#include #include #include #include @@ -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; } @@ -187,12 +186,12 @@ done: // Permuted permuted(permutation, values); // // // After permutation, the indices above the threshold are 2 and 2 -// FastSet expected; +// KeySet expected; // expected.insert(2); // expected.insert(3); // // // Indices checked by CheckRelinearization -// FastSet actual = Impl::CheckRelinearization(permuted, 0.1); +// KeySet actual = Impl::CheckRelinearization(permuted, 0.1); // // EXPECT(assert_equal(expected, actual)); //} diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index cdb1509b6..e877e5a9d 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -15,29 +15,42 @@ * @author nikai */ -#include - -#if 0 - #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 -#include // for operator += -#include // for operator += +#include + +#include +#include #include + +#include +#include +#include +#include + using namespace boost::assign; #include @@ -51,190 +64,218 @@ using symbol_shorthand::L; /* ************************************************************************* * Bayes tree for smoother with "nested dissection" ordering: - C1 x5 x6 x4 - C2 x3 x2 : x4 - C3 x1 : x2 - C4 x7 : x6 -*/ -TEST( GaussianJunctionTreeB, constructor2 ) -{ + C1 x5 x6 x4 + C2 x3 x2 : x4 + C3 x1 : x2 + C4 x7 : x6 + */ +TEST( GaussianJunctionTreeB, constructor2 ) { // create a graph - Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); - GaussianFactorGraph fg = createSmoother(7, ordering).first; + NonlinearFactorGraph nlfg; + Values values; + boost::tie(nlfg, values) = createNonlinearSmoother(7); + SymbolicFactorGraph::shared_ptr symbolic = nlfg.symbolic(); + + // linearize + GaussianFactorGraph::shared_ptr fg = nlfg.linearize(values); + + Ordering ordering; + ordering += X(1), X(3), X(5), X(7), X(2), X(6), X(4); // create an ordering - GaussianJunctionTree actual(fg); + GaussianEliminationTree etree(*fg, ordering); + SymbolicEliminationTree stree(*symbolic, ordering); + GaussianJunctionTree actual(etree); - vector frontal1; frontal1 += ordering[X(5)], ordering[X(6)], ordering[X(4)]; - vector frontal2; frontal2 += ordering[X(3)], ordering[X(2)]; - vector frontal3; frontal3 += ordering[X(1)]; - vector frontal4; frontal4 += ordering[X(7)]; - vector sep1; - vector sep2; sep2 += ordering[X(4)]; - vector sep3; sep3 += ordering[X(2)]; - vector sep4; sep4 += ordering[X(6)]; - EXPECT(assert_equal(frontal1, actual.root()->frontal)); - EXPECT(assert_equal(sep1, actual.root()->separator)); - LONGS_EQUAL(5, actual.root()->size()); - list::const_iterator child0it = actual.root()->children().begin(); - list::const_iterator child1it = child0it; ++child1it; - GaussianJunctionTree::sharedClique child0 = *child0it; - GaussianJunctionTree::sharedClique child1 = *child1it; - EXPECT(assert_equal(frontal2, child0->frontal)); - EXPECT(assert_equal(sep2, child0->separator)); - LONGS_EQUAL(4, child0->size()); - EXPECT(assert_equal(frontal3, child0->children().front()->frontal)); - EXPECT(assert_equal(sep3, child0->children().front()->separator)); - LONGS_EQUAL(2, child0->children().front()->size()); - EXPECT(assert_equal(frontal4, child1->frontal)); - EXPECT(assert_equal(sep4, child1->separator)); - LONGS_EQUAL(2, child1->size()); + Ordering o324; + o324 += X(3), X(2), X(4); + Ordering o56; + o56 += X(5), X(6); + Ordering o7; + o7 += X(7); + Ordering o1; + o1 += X(1); + + // Can no longer test these: +// Ordering sep1; +// Ordering sep2; sep2 += X(4); +// Ordering sep3; sep3 += X(6); +// Ordering sep4; sep4 += X(2); + + GaussianJunctionTree::sharedNode x324 = actual.roots().front(); + LONGS_EQUAL(2, x324->children.size()); + GaussianJunctionTree::sharedNode x1 = x324->children.front(); + GaussianJunctionTree::sharedNode x56 = x324->children.back(); + if (x1->children.size() > 0) + x1.swap(x56); // makes it work with different tie-breakers + + LONGS_EQUAL(0, x1->children.size()); + LONGS_EQUAL(1, x56->children.size()); + GaussianJunctionTree::sharedNode x7 = x56->children[0]; + LONGS_EQUAL(0, x7->children.size()); + + EXPECT(assert_equal(o324, x324->orderedFrontalKeys)); + EXPECT_LONGS_EQUAL(5, x324->factors.size()); + EXPECT_LONGS_EQUAL(9, x324->problemSize_); + + EXPECT(assert_equal(o56, x56->orderedFrontalKeys)); + EXPECT_LONGS_EQUAL(4, x56->factors.size()); + EXPECT_LONGS_EQUAL(9, x56->problemSize_); + + EXPECT(assert_equal(o7, x7->orderedFrontalKeys)); + EXPECT_LONGS_EQUAL(2, x7->factors.size()); + EXPECT_LONGS_EQUAL(4, x7->problemSize_); + + EXPECT(assert_equal(o1, x1->orderedFrontalKeys)); + EXPECT_LONGS_EQUAL(2, x1->factors.size()); + EXPECT_LONGS_EQUAL(4, x1->problemSize_); } +///* ************************************************************************* */ +//TEST( GaussianJunctionTreeB, optimizeMultiFrontal ) +//{ +// // create a graph +// GaussianFactorGraph fg; +// Ordering ordering; +// boost::tie(fg,ordering) = createSmoother(7); +// +// // optimize the graph +// GaussianJunctionTree tree(fg); +// VectorValues actual = tree.optimize(&EliminateQR); +// +// // verify +// VectorValues expected(vector(7,2)); // expected solution +// Vector v = Vector2(0., 0.); +// for (int i=1; i<=7; i++) +// expected[ordering[X(i)]] = v; +// EXPECT(assert_equal(expected,actual)); +//} +// +///* ************************************************************************* */ +//TEST( GaussianJunctionTreeB, optimizeMultiFrontal2) +//{ +// // create a graph +// example::Graph nlfg = createNonlinearFactorGraph(); +// Values noisy = createNoisyValues(); +// Ordering ordering; ordering += X(1),X(2),L(1); +// GaussianFactorGraph fg = *nlfg.linearize(noisy, ordering); +// +// // optimize the graph +// GaussianJunctionTree tree(fg); +// VectorValues actual = tree.optimize(&EliminateQR); +// +// // verify +// VectorValues expected = createCorrectDelta(ordering); // expected solution +// EXPECT(assert_equal(expected,actual)); +//} +// +///* ************************************************************************* */ +//TEST(GaussianJunctionTreeB, slamlike) { +// Values init; +// NonlinearFactorGraph newfactors; +// NonlinearFactorGraph fullgraph; +// SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, M_PI/100.0)); +// SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0, 0.1)); +// +// size_t i = 0; +// +// newfactors = NonlinearFactorGraph(); +// newfactors.add(PriorFactor(X(0), Pose2(0.0, 0.0, 0.0), odoNoise)); +// init.insert(X(0), Pose2(0.01, 0.01, 0.01)); +// fullgraph.push_back(newfactors); +// +// for( ; i<5; ++i) { +// newfactors = NonlinearFactorGraph(); +// newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); +// init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); +// fullgraph.push_back(newfactors); +// } +// +// newfactors = NonlinearFactorGraph(); +// newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); +// newfactors.add(BearingRangeFactor(X(i), L(0), Rot2::fromAngle(M_PI/4.0), 5.0, brNoise)); +// newfactors.add(BearingRangeFactor(X(i), L(1), Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise)); +// init.insert(X(i+1), Pose2(1.01, 0.01, 0.01)); +// init.insert(L(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); +// init.insert(L(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); +// fullgraph.push_back(newfactors); +// ++ i; +// +// for( ; i<5; ++i) { +// newfactors = NonlinearFactorGraph(); +// newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); +// init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); +// fullgraph.push_back(newfactors); +// } +// +// newfactors = NonlinearFactorGraph(); +// newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); +// newfactors.add(BearingRangeFactor(X(i), L(0), Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); +// newfactors.add(BearingRangeFactor(X(i), L(1), Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); +// init.insert(X(i+1), Pose2(6.9, 0.1, 0.01)); +// fullgraph.push_back(newfactors); +// ++ i; +// +// // Compare solutions +// Ordering ordering = *fullgraph.orderingCOLAMD(init); +// GaussianFactorGraph linearized = *fullgraph.linearize(init, ordering); +// +// GaussianJunctionTree gjt(linearized); +// VectorValues deltaactual = gjt.optimize(&EliminateQR); +// Values actual = init.retract(deltaactual, ordering); +// +// GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); +// VectorValues delta = optimize(gbn); +// Values expected = init.retract(delta, ordering); +// +// EXPECT(assert_equal(expected, actual)); +//} +// +///* ************************************************************************* */ +//TEST(GaussianJunctionTreeB, simpleMarginal) { +// +// typedef BayesTree GaussianBayesTree; +// +// // Create a simple graph +// NonlinearFactorGraph fg; +// fg.add(PriorFactor(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0))); +// fg.add(BetweenFactor(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector3(10.0, 1.0, 1.0)))); +// +// Values init; +// init.insert(X(0), Pose2()); +// init.insert(X(1), Pose2(1.0, 0.0, 0.0)); +// +// Ordering ordering; +// ordering += X(1), X(0); +// +// GaussianFactorGraph gfg = *fg.linearize(init, ordering); +// +// // Compute marginals with both sequential and multifrontal +// Matrix expected = GaussianSequentialSolver(gfg).marginalCovariance(1); +// +// Matrix actual1 = GaussianMultifrontalSolver(gfg).marginalCovariance(1); +// +// // Compute marginal directly from marginal factor +// GaussianFactor::shared_ptr marginalFactor = GaussianMultifrontalSolver(gfg).marginalFactor(1); +// JacobianFactor::shared_ptr marginalJacobian = boost::dynamic_pointer_cast(marginalFactor); +// Matrix actual2 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin())); +// +// // Compute marginal directly from BayesTree +// GaussianBayesTree gbt; +// gbt.insert(GaussianJunctionTree(gfg).eliminate(EliminateCholesky)); +// marginalFactor = gbt.marginalFactor(1, EliminateCholesky); +// marginalJacobian = boost::dynamic_pointer_cast(marginalFactor); +// Matrix actual3 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin())); +// +// EXPECT(assert_equal(expected, actual1)); +// EXPECT(assert_equal(expected, actual2)); +// EXPECT(assert_equal(expected, actual3)); +//} + /* ************************************************************************* */ -TEST( GaussianJunctionTreeB, optimizeMultiFrontal ) -{ - // create a graph - GaussianFactorGraph fg; - Ordering ordering; - boost::tie(fg,ordering) = createSmoother(7); - - // optimize the graph - GaussianJunctionTree tree(fg); - VectorValues actual = tree.optimize(&EliminateQR); - - // verify - VectorValues expected(vector(7,2)); // expected solution - Vector v = Vector2(0., 0.); - for (int i=1; i<=7; i++) - expected[ordering[X(i)]] = v; - EXPECT(assert_equal(expected,actual)); +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); } - -/* ************************************************************************* */ -TEST( GaussianJunctionTreeB, optimizeMultiFrontal2) -{ - // create a graph - example::Graph nlfg = createNonlinearFactorGraph(); - Values noisy = createNoisyValues(); - Ordering ordering; ordering += X(1),X(2),L(1); - GaussianFactorGraph fg = *nlfg.linearize(noisy, ordering); - - // optimize the graph - GaussianJunctionTree tree(fg); - VectorValues actual = tree.optimize(&EliminateQR); - - // verify - VectorValues expected = createCorrectDelta(ordering); // expected solution - EXPECT(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST(GaussianJunctionTreeB, slamlike) { - Values init; - NonlinearFactorGraph newfactors; - NonlinearFactorGraph fullgraph; - SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, M_PI/100.0)); - SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0, 0.1)); - - size_t i = 0; - - newfactors = NonlinearFactorGraph(); - newfactors.add(PriorFactor(X(0), Pose2(0.0, 0.0, 0.0), odoNoise)); - init.insert(X(0), Pose2(0.01, 0.01, 0.01)); - fullgraph.push_back(newfactors); - - for( ; i<5; ++i) { - newfactors = NonlinearFactorGraph(); - newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); - init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullgraph.push_back(newfactors); - } - - newfactors = NonlinearFactorGraph(); - newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); - newfactors.add(BearingRangeFactor(X(i), L(0), Rot2::fromAngle(M_PI/4.0), 5.0, brNoise)); - newfactors.add(BearingRangeFactor(X(i), L(1), Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise)); - init.insert(X(i+1), Pose2(1.01, 0.01, 0.01)); - init.insert(L(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - init.insert(L(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - fullgraph.push_back(newfactors); - ++ i; - - for( ; i<5; ++i) { - newfactors = NonlinearFactorGraph(); - newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); - init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullgraph.push_back(newfactors); - } - - newfactors = NonlinearFactorGraph(); - newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); - newfactors.add(BearingRangeFactor(X(i), L(0), Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); - newfactors.add(BearingRangeFactor(X(i), L(1), Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); - init.insert(X(i+1), Pose2(6.9, 0.1, 0.01)); - fullgraph.push_back(newfactors); - ++ i; - - // Compare solutions - Ordering ordering = *fullgraph.orderingCOLAMD(init); - GaussianFactorGraph linearized = *fullgraph.linearize(init, ordering); - - GaussianJunctionTree gjt(linearized); - VectorValues deltaactual = gjt.optimize(&EliminateQR); - Values actual = init.retract(deltaactual, ordering); - - GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); - VectorValues delta = optimize(gbn); - Values expected = init.retract(delta, ordering); - - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(GaussianJunctionTreeB, simpleMarginal) { - - typedef BayesTree GaussianBayesTree; - - // Create a simple graph - NonlinearFactorGraph fg; - fg.add(PriorFactor(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0))); - fg.add(BetweenFactor(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector3(10.0, 1.0, 1.0)))); - - Values init; - init.insert(X(0), Pose2()); - init.insert(X(1), Pose2(1.0, 0.0, 0.0)); - - Ordering ordering; - ordering += X(1), X(0); - - GaussianFactorGraph gfg = *fg.linearize(init, ordering); - - // Compute marginals with both sequential and multifrontal - Matrix expected = GaussianSequentialSolver(gfg).marginalCovariance(1); - - Matrix actual1 = GaussianMultifrontalSolver(gfg).marginalCovariance(1); - - // Compute marginal directly from marginal factor - GaussianFactor::shared_ptr marginalFactor = GaussianMultifrontalSolver(gfg).marginalFactor(1); - JacobianFactor::shared_ptr marginalJacobian = boost::dynamic_pointer_cast(marginalFactor); - Matrix actual2 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin())); - - // Compute marginal directly from BayesTree - GaussianBayesTree gbt; - gbt.insert(GaussianJunctionTree(gfg).eliminate(EliminateCholesky)); - marginalFactor = gbt.marginalFactor(1, EliminateCholesky); - marginalJacobian = boost::dynamic_pointer_cast(marginalFactor); - Matrix actual3 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin())); - - EXPECT(assert_equal(expected, actual1)); - EXPECT(assert_equal(expected, actual2)); - EXPECT(assert_equal(expected, actual3)); -} - -#endif - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp new file mode 100644 index 000000000..aea18c90d --- /dev/null +++ b/tests/testGeneralSFMFactorB.cpp @@ -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 testGeneralSFMFactorB.cpp + * @author Frank Dellaert + * @brief test general SFM class, with nonlinear optimization and BAL files + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +typedef GeneralSFMFactor, Point3> 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-5); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ 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..c153adf5f --- /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.expmap(d); + EXPECT(assert_equal(expected, pair2, 1e-9)); + EXPECT(assert_equal(d, pair1.logmap(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 f6180fb73..89b296824 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -10,16 +10,16 @@ * -------------------------------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 @@ -149,6 +149,32 @@ TEST(Manifold, DefaultChart) { EXPECT(assert_equal(zero(3), traits::Local(R, R))); } +//****************************************************************************** +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; + } +}; +} + +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)); +} + //****************************************************************************** int main() { TestResult tr; diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index dd22ae738..392b84deb 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -31,7 +31,7 @@ // add in headers for specific factors #include #include -#include +#include #include @@ -226,7 +226,8 @@ TEST(Marginals, order) { vals.at(3).range(vals.at(101)), noiseModel::Unit::Create(2)); Marginals marginals(fg, vals); - FastVector keys(fg.keys()); + KeySet set = fg.keys(); + FastVector keys(set.begin(), set.end()); JointMarginal joint = marginals.jointMarginalCovariance(keys); LONGS_EQUAL(3, (long)joint(0,0).rows()); diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index f398a33fe..ee60f9714 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -66,9 +66,9 @@ TEST( NonlinearFactorGraph, error ) TEST( NonlinearFactorGraph, keys ) { NonlinearFactorGraph fg = createNonlinearFactorGraph(); - FastSet actual = fg.keys(); + KeySet actual = fg.keys(); LONGS_EQUAL(3, (long)actual.size()); - FastSet::const_iterator it = actual.begin(); + KeySet::const_iterator it = actual.begin(); LONGS_EQUAL((long)L(1), (long)*(it++)); LONGS_EQUAL((long)X(1), (long)*(it++)); LONGS_EQUAL((long)X(2), (long)*(it++)); @@ -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/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index e9e266bb3..3c49d54af 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -6,7 +6,7 @@ #include #include -#include +#include #include #include #include diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 6d8a056fc..f927f45ae 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -187,7 +187,9 @@ TEST( NonlinearOptimizer, Factorization ) ordering.push_back(X(1)); ordering.push_back(X(2)); - LevenbergMarquardtOptimizer optimizer(graph, config, ordering); + LevenbergMarquardtParams params; + LevenbergMarquardtParams::SetLegacyDefaults(¶ms); + LevenbergMarquardtOptimizer optimizer(graph, config, ordering, params); optimizer.iterate(); Values expected; @@ -260,13 +262,13 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { expectedGradient.insert(2,zero(3)); // Try LM and Dogleg - LevenbergMarquardtParams params; -// params.setVerbosityLM("TRYDELTA"); -// params.setVerbosity("TERMINATION"); - params.setlambdaUpperBound(1e9); -// params.setRelativeErrorTol(0); -// params.setAbsoluteErrorTol(0); - //params.setlambdaInitial(10); + LevenbergMarquardtParams params = LevenbergMarquardtParams::LegacyDefaults(); + // params.setVerbosityLM("TRYDELTA"); + // params.setVerbosity("TERMINATION"); + params.lambdaUpperBound = 1e9; +// params.relativeErrorTol = 0; +// params.absoluteErrorTol = 0; + //params.lambdaInitial = 10; { LevenbergMarquardtOptimizer optimizer(fg, init, params); @@ -290,7 +292,7 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { initBetter.insert(2, Pose2(11,7,M_PI/2)); { - params.setDiagonalDamping(true); + params.diagonalDamping = true; LevenbergMarquardtOptimizer optimizer(fg, initBetter, params); // test the diagonal @@ -399,7 +401,7 @@ public: /// Constructor IterativeLM(const NonlinearFactorGraph& graph, const Values& initialValues, const ConjugateGradientParameters &p, - const LevenbergMarquardtParams& params = LevenbergMarquardtParams()) : + const LevenbergMarquardtParams& params = LevenbergMarquardtParams::LegacyDefaults()) : LevenbergMarquardtOptimizer(graph, initialValues, params), cgParams_(p) { } @@ -446,8 +448,7 @@ TEST( NonlinearOptimizer, logfile ) // Levenberg-Marquardt LevenbergMarquardtParams lmParams; static const string filename("testNonlinearOptimizer.log"); - lmParams.setLogFile(filename); - CHECK(lmParams.getLogFile()==filename); + lmParams.logFile = filename; LevenbergMarquardtOptimizer(fg, c0, lmParams).optimize(); // stringstream expected,actual; diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index c90b09db1..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 diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index 89c3d5cf8..f00916475 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -18,11 +18,12 @@ #include -#include -#include #include -#include +#include +#include +#include #include +#include using namespace std; using namespace gtsam; diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 2514c80d9..33453d7d3 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -22,15 +22,14 @@ #include //#include -#include -#include +#include #include //#include #include //#include #include #include -#include +#include #include #include #include @@ -106,9 +105,6 @@ typedef RangeFactor RangeFactorSimpleCameraP typedef RangeFactor RangeFactorCalibratedCamera; typedef RangeFactor RangeFactorSimpleCamera; -typedef BearingFactor BearingFactor2D; -typedef BearingFactor BearingFactor3D; - typedef BearingRangeFactor BearingRangeFactor2D; typedef BearingRangeFactor BearingRangeFactor3D; @@ -217,8 +213,6 @@ BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleC BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera"); BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera"); -BOOST_CLASS_EXPORT_GUID(BearingFactor2D, "gtsam::BearingFactor2D"); - BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D"); BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2"); @@ -393,8 +387,6 @@ TEST (testSerializationSLAM, factors) { RangeFactorCalibratedCamera rangeFactorCalibratedCamera(a12, b12, 2.0, model1); RangeFactorSimpleCamera rangeFactorSimpleCamera(a13, b13, 2.0, model1); - BearingFactor2D bearingFactor2D(a08, a03, rot2, model1); - BearingRangeFactor2D bearingRangeFactor2D(a08, a03, rot2, 2.0, model2); GenericProjectionFactorCal3_S2 genericProjectionFactorCal3_S2(point2, model2, a09, a05, boost::make_shared(cal3_s2)); @@ -456,8 +448,6 @@ TEST (testSerializationSLAM, factors) { graph += rangeFactorCalibratedCamera; graph += rangeFactorSimpleCamera; - graph += bearingFactor2D; - graph += bearingRangeFactor2D; graph += genericProjectionFactorCal3_S2; @@ -524,8 +514,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsObj(rangeFactorCalibratedCamera)); EXPECT(equalsObj(rangeFactorSimpleCamera)); - EXPECT(equalsObj(bearingFactor2D)); - EXPECT(equalsObj(bearingRangeFactor2D)); EXPECT(equalsObj(genericProjectionFactorCal3_S2)); @@ -592,8 +580,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsXML(rangeFactorCalibratedCamera)); EXPECT(equalsXML(rangeFactorSimpleCamera)); - EXPECT(equalsXML(bearingFactor2D)); - EXPECT(equalsXML(bearingRangeFactor2D)); EXPECT(equalsXML(genericProjectionFactorCal3_S2)); @@ -660,8 +646,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsBinary(rangeFactorCalibratedCamera)); EXPECT(equalsBinary(rangeFactorSimpleCamera)); - EXPECT(equalsBinary(bearingFactor2D)); - EXPECT(equalsBinary(bearingRangeFactor2D)); EXPECT(equalsBinary(genericProjectionFactorCal3_S2)); diff --git a/timing/DummyFactor.h b/timing/DummyFactor.h new file mode 100644 index 000000000..08e9d8f4b --- /dev/null +++ b/timing/DummyFactor.h @@ -0,0 +1,56 @@ +/** + * @file DummyFactor.h + * @brief Just to help in timing overhead + * @author Frank Dellaert + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * DummyFactor + */ +template // +class DummyFactor: public RegularImplicitSchurFactor { + +public: + + typedef Eigen::Matrix Matrix2D; + typedef std::pair KeyMatrix2D; + + DummyFactor() { + } + + DummyFactor(const std::vector& Fblocks, const Matrix& E, + const Matrix3& P, const Vector& b) : + RegularImplicitSchurFactor(Fblocks, E, P, b) { + } + + virtual ~DummyFactor() { + } + +public: + + /** + * @brief Dummy version to measure overhead of key access + */ + void multiplyHessian(double alpha, const VectorValues& x, + VectorValues& y) const { + + BOOST_FOREACH(const KeyMatrix2D& Fi, this->Fblocks_) { + static const Vector empty; + Key key = Fi.first; + std::pair it = y.tryInsert(key, empty); + Vector& yi = it.first->second; + yi = x.at(key); + } + } + +}; +// DummyFactor + +} + diff --git a/timing/timeAdaptAutoDiff.cpp b/timing/timeAdaptAutoDiff.cpp index edfd420ef..3a9b5297a 100644 --- a/timing/timeAdaptAutoDiff.cpp +++ b/timing/timeAdaptAutoDiff.cpp @@ -57,16 +57,19 @@ int main() { f1 = boost::make_shared >(z, model, 1, 2); time("GeneralSFMFactor : ", f1, values); - // AdaptAutoDiff - typedef AdaptAutoDiff AdaptedSnavely; - Point2_ expression(AdaptedSnavely(), camera, point); - f2 = boost::make_shared >(model, z, expression); - time("Point2_(AdaptedSnavely(), camera, point): ", f2, values); - // ExpressionFactor Point2_ expression2(camera, &Camera::project2, point); f3 = boost::make_shared >(model, z, expression2); time("Point2_(camera, &Camera::project, point): ", f3, values); + // AdaptAutoDiff + values.clear(); + values.insert(1,Vector9(Vector9::Zero())); + values.insert(2,Vector3(0,0,1)); + typedef AdaptAutoDiff AdaptedSnavely; + Expression expression(AdaptedSnavely(), Expression(1), Expression(2)); + f2 = boost::make_shared >(model, z.vector(), expression); + time("Point2_(AdaptedSnavely(), camera, point): ", f2, values); + return 0; } 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/timing/timeIncremental.cpp b/timing/timeIncremental.cpp index d82ef9442..b5dd37516 100644 --- a/timing/timeIncremental.cpp +++ b/timing/timeIncremental.cpp @@ -17,7 +17,7 @@ #include #include #include -#include +#include #include #include #include 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/timing/timeRot2.cpp b/timing/timeRot2.cpp index 4ad9d7d47..52dfdcc08 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; @@ -92,11 +92,8 @@ int main() int n = 50000000; cout << "NOTE: Times are reported for " << n << " calls" << endl; - // create a random direction: - double norm=sqrt(16.0+4.0); - double x=4.0/norm, y=2.0/norm; - Vector v = (Vector(2) << x, y).finished(); - Rot2 R = Rot2(0.4), R2 = R.retract(v), R3(0.6); + Vector1 v; v << 0.1; + Rot2 R = Rot2(0.4), R2(0.5), R3(0.6); TEST(Rot2_Expmap, Rot2::Expmap(v)); TEST(Rot2_Retract, R.retract(v)); diff --git a/timing/timeRot3.cpp b/timing/timeRot3.cpp index 4977fba86..5806149f3 100644 --- a/timing/timeRot3.cpp +++ b/timing/timeRot3.cpp @@ -40,10 +40,10 @@ int main() double norm=sqrt(1.0+16.0+4.0); double x=1.0/norm, y=4.0/norm, z=2.0/norm; Vector v = (Vector(3) << x, y, z).finished(); - Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2), R2 = R.retract(v); + Rot3 R = Rot3::Rodrigues(0.1, 0.4, 0.2), R2 = R.retract(v); - TEST("Rodriguez formula given axis angle", Rot3::rodriguez(v,0.001)) - TEST("Rodriguez formula given canonical coordinates", Rot3::rodriguez(v)) + TEST("Rodriguez formula given axis angle", Rot3::AxisAngle(v,0.001)) + TEST("Rodriguez formula given canonical coordinates", Rot3::Rodrigues(v)) TEST("Expmap", R*Rot3::Expmap(v)) TEST("Retract", R.retract(v)) TEST("Logmap", Rot3::Logmap(R.between(R2))) diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp new file mode 100644 index 000000000..6308a1d61 --- /dev/null +++ b/timing/timeSFMBAL.cpp @@ -0,0 +1,56 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file timeSFMBAL.cpp + * @brief time SFM with BAL file, conventional GeneralSFMFactor + * @author Frank Dellaert + * @date June 6, 2015 + */ + +#include "timeSFMBAL.h" + +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +typedef PinholeCamera Camera; +typedef GeneralSFMFactor SfmFactor; + +int main(int argc, char* argv[]) { + // parse options and read BAL file + SfM_data db = preamble(argc, argv); + + // Build graph using conventional GeneralSFMFactor + NonlinearFactorGraph graph; + for (size_t j = 0; j < db.number_tracks(); j++) { + BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) { + size_t i = m.first; + Point2 z = m.second; + graph.push_back(SfmFactor(z, gNoiseModel, C(i), P(j))); + } + } + + Values initial; + size_t i = 0, j = 0; + BOOST_FOREACH (const SfM_Camera& camera, db.cameras) + initial.insert(C(i++), camera); + BOOST_FOREACH (const SfM_Track& track, db.tracks) + initial.insert(P(j++), track.p); + + return optimize(db, graph, initial); +} diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h new file mode 100644 index 000000000..e9449af7b --- /dev/null +++ b/timing/timeSFMBAL.h @@ -0,0 +1,87 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeSFMBAL.h + * @brief Common code for timeSFMBAL scripts + * @author Frank Dellaert + * @date July 5, 2015 + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::C; +using symbol_shorthand::K; +using symbol_shorthand::P; + +static bool gUseSchur = true; +static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2); + +// parse options and read BAL file +SfM_data preamble(int argc, char* argv[]) { + // primitive argument parsing: + if (argc > 2) { + if (strcmp(argv[1], "--colamd")) + gUseSchur = false; + else + throw runtime_error("Usage: timeSFMBALxxx [--colamd] [BALfile]"); + } + + // Load BAL file + SfM_data db; + string defaultFilename = findExampleDataFile("dubrovnik-16-22106-pre"); + bool success = readBAL(argc > 1 ? argv[argc - 1] : defaultFilename, db); + if (!success) throw runtime_error("Could not access file!"); + return db; +} + +// Create ordering and optimize +int optimize(const SfM_data& db, const NonlinearFactorGraph& graph, + const Values& initial, bool separateCalibration = false) { + using symbol_shorthand::P; + + // Set parameters to be similar to ceres + LevenbergMarquardtParams params; + LevenbergMarquardtParams::SetCeresDefaults(¶ms); + params.setVerbosityLM("SUMMARY"); + + if (gUseSchur) { + // Create Schur-complement ordering + Ordering ordering; + for (size_t j = 0; j < db.number_tracks(); j++) ordering.push_back(P(j)); + for (size_t i = 0; i < db.number_cameras(); i++) { + ordering.push_back(C(i)); + if (separateCalibration) ordering.push_back(K(i)); + } + params.setOrdering(ordering); + } + + // Optimize + LevenbergMarquardtOptimizer lm(graph, initial, params); + Values result = lm.optimize(); + + tictoc_finishedIteration_(); + tictoc_print_(); + + return 0; +} diff --git a/timing/timeSFMBALautodiff.cpp b/timing/timeSFMBALautodiff.cpp new file mode 100644 index 000000000..4545abc21 --- /dev/null +++ b/timing/timeSFMBALautodiff.cpp @@ -0,0 +1,75 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeSFMBALautodiff.cpp + * @brief time SFM with BAL file, Ceres autodiff version + * @author Frank Dellaert + * @date July 5, 2015 + */ + +#include "timeSFMBAL.h" +#include +#include +#include +#include + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// See http://www.cs.cornell.edu/~snavely/bundler/bundler-v0.3-manual.html +// as to why so much gymnastics is needed to massage the initial estimates and +// measurements: basically, Snavely does not use computer vision conventions +// but OpenGL conventions :-( + +typedef PinholeCamera Camera; + +int main(int argc, char* argv[]) { + // parse options and read BAL file + SfM_data db = preamble(argc, argv); + + AdaptAutoDiff snavely; + + // Build graph + NonlinearFactorGraph graph; + for (size_t j = 0; j < db.number_tracks(); j++) { + BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) { + size_t i = m.first; + Point2 z = m.second; + Expression camera_(C(i)); + Expression point_(P(j)); + // Expects measurements in OpenGL format, with y increasing upwards + graph.addExpressionFactor(gNoiseModel, Vector2(z.x(), -z.y()), + Expression(snavely, camera_, point_)); + } + } + + Values initial; + size_t i = 0, j = 0; + BOOST_FOREACH (const SfM_Camera& camera, db.cameras) { + // readBAL converts to GTSAM format, so we need to convert back ! + Pose3 openGLpose = gtsam2openGL(camera.pose()); + Vector9 v9; + v9 << Pose3::Logmap(openGLpose), camera.calibration().vector(); + initial.insert(C(i++), v9); + } + BOOST_FOREACH (const SfM_Track& track, db.tracks) { + Vector3 v3 = track.p.vector(); + initial.insert(P(j++), v3); + } + + return optimize(db, graph, initial); +} diff --git a/timing/timeSFMBALcamTnav.cpp b/timing/timeSFMBALcamTnav.cpp new file mode 100644 index 000000000..32fae4ac2 --- /dev/null +++ b/timing/timeSFMBALcamTnav.cpp @@ -0,0 +1,63 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeSFMBALcamTnav.cpp + * @brief time SFM with BAL file, expressions with camTnav pose + * @author Frank Dellaert + * @date July 5, 2015 + */ + +#include "timeSFMBAL.h" + +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +int main(int argc, char* argv[]) { + // parse options and read BAL file + SfM_data db = preamble(argc, argv); + + // Build graph using conventional GeneralSFMFactor + NonlinearFactorGraph graph; + for (size_t j = 0; j < db.number_tracks(); j++) { + BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) { + size_t i = m.first; + Point2 z = m.second; + Pose3_ camTnav_(C(i)); + Cal3Bundler_ calibration_(K(i)); + Point3_ nav_point_(P(j)); + graph.addExpressionFactor( + gNoiseModel, z, + uncalibrate(calibration_, // now using transform_from !!!: + project(transform_from(camTnav_, nav_point_)))); + } + } + + Values initial; + size_t i = 0, j = 0; + BOOST_FOREACH (const SfM_Camera& camera, db.cameras) { + initial.insert(C(i), camera.pose().inverse()); // inverse !!! + initial.insert(K(i), camera.calibration()); + i += 1; + } + BOOST_FOREACH (const SfM_Track& track, db.tracks) + initial.insert(P(j++), track.p); + + bool separateCalibration = true; + return optimize(db, graph, initial, separateCalibration); +} diff --git a/timing/timeSFMBALnavTcam.cpp b/timing/timeSFMBALnavTcam.cpp new file mode 100644 index 000000000..e370a5a67 --- /dev/null +++ b/timing/timeSFMBALnavTcam.cpp @@ -0,0 +1,63 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeSFMBALnavTcam.cpp + * @brief time SFM with BAL file, expressions with navTcam pose + * @author Frank Dellaert + * @date July 5, 2015 + */ + +#include "timeSFMBAL.h" + +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +int main(int argc, char* argv[]) { + // parse options and read BAL file + SfM_data db = preamble(argc, argv); + + // Build graph using conventional GeneralSFMFactor + NonlinearFactorGraph graph; + for (size_t j = 0; j < db.number_tracks(); j++) { + BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) { + size_t i = m.first; + Point2 z = m.second; + Pose3_ navTcam_(C(i)); + Cal3Bundler_ calibration_(K(i)); + Point3_ nav_point_(P(j)); + graph.addExpressionFactor( + gNoiseModel, z, + uncalibrate(calibration_, + project(transform_to(navTcam_, nav_point_)))); + } + } + + Values initial; + size_t i = 0, j = 0; + BOOST_FOREACH (const SfM_Camera& camera, db.cameras) { + initial.insert(C(i), camera.pose()); + initial.insert(K(i), camera.calibration()); + i += 1; + } + BOOST_FOREACH (const SfM_Track& track, db.tracks) + initial.insert(P(j++), track.p); + + bool separateCalibration = true; + return optimize(db, graph, initial, separateCalibration); +} diff --git a/timing/timeSchurFactors.cpp b/timing/timeSchurFactors.cpp new file mode 100644 index 000000000..e08924400 --- /dev/null +++ b/timing/timeSchurFactors.cpp @@ -0,0 +1,157 @@ +/** + * @file timeSchurFactors.cpp + * @brief Time various Schur-complement Jacobian factors + * @author Frank Dellaert + * @date Oct 27, 2013 + */ + +#include "DummyFactor.h" +#include + +#include +#include "gtsam/slam/JacobianFactorQR.h" +#include +#include +#include + +#include +#include +#include + +using namespace std; +using namespace boost::assign; +using namespace gtsam; + +#define SLOW +#define RAW +#define HESSIAN +#define NUM_ITERATIONS 1000 + +// Create CSV file for results +ofstream os("timeSchurFactors.csv"); + +/*************************************************************************************/ +template +void timeAll(size_t m, size_t N) { + + cout << m << endl; + + // create F + static const int D = CAMERA::dimension; + typedef Eigen::Matrix Matrix2D; + FastVector keys; + vector Fblocks; + for (size_t i = 0; i < m; i++) { + keys.push_back(i); + Fblocks.push_back((i + 1) * Matrix::Ones(2, D)); + } + + // create E + Matrix E(2 * m, 3); + for (size_t i = 0; i < m; i++) + E.block < 2, 3 > (2 * i, 0) = Matrix::Ones(2, 3); + + // Calculate point covariance + Matrix P = (E.transpose() * E).inverse(); + + // RHS and sigmas + const Vector b = gtsam::repeat(2 * m, 1); + const SharedDiagonal model; + + // parameters for multiplyHessianAdd + double alpha = 0.5; + VectorValues xvalues, yvalues; + for (size_t i = 0; i < m; i++) + xvalues.insert(i, gtsam::repeat(D, 2)); + + // Implicit + RegularImplicitSchurFactor implicitFactor(keys, Fblocks, E, P, b); + // JacobianFactor with same error + JacobianFactorQ jf(keys, Fblocks, E, P, b, model); + // JacobianFactorQR with same error + JacobianFactorQR jqr(keys, Fblocks, E, P, b, model); + // Hessian + HessianFactor hessianFactor(jqr); + +#define TIME(label,factor,xx,yy) {\ + for (size_t t = 0; t < N; t++) \ + factor.multiplyHessianAdd(alpha, xx, yy);\ + gttic_(label);\ + for (size_t t = 0; t < N; t++) {\ + factor.multiplyHessianAdd(alpha, xx, yy);\ + }\ + gttoc_(label);\ + tictoc_getNode(timer, label)\ + os << timer->secs()/NUM_ITERATIONS << ", ";\ + } + +#ifdef SLOW + TIME(Implicit, implicitFactor, xvalues, yvalues) + TIME(Jacobian, jf, xvalues, yvalues) + TIME(JacobianQR, jqr, xvalues, yvalues) +#endif + +#ifdef HESSIAN + TIME(Hessian, hessianFactor, xvalues, yvalues) +#endif + +#ifdef OVERHEAD + DummyFactor dummy(Fblocks, E, P, b); + TIME(Overhead,dummy,xvalues,yvalues) +#endif + +#ifdef RAW + { // Raw memory Version + FastVector < Key > keys; + for (size_t i = 0; i < m; i++) + keys += i; + Vector x = xvalues.vector(keys); + double* xdata = x.data(); + + // create a y + Vector y = zero(m * D); + TIME(RawImplicit, implicitFactor, xdata, y.data()) + TIME(RawJacobianQ, jf, xdata, y.data()) + TIME(RawJacobianQR, jqr, xdata, y.data()) + } +#endif + + os << m << endl; + +} // timeAll + +/*************************************************************************************/ +int main(void) { +#ifdef SLOW + os << "Implicit,"; + os << "JacobianQ,"; + os << "JacobianQR,"; +#endif +#ifdef HESSIAN + os << "Hessian,"; +#endif +#ifdef OVERHEAD + os << "Overhead,"; +#endif +#ifdef RAW + os << "RawImplicit,"; + os << "RawJacobianQ,"; + os << "RawJacobianQR,"; +#endif + os << "m" << endl; + // define images + vector < size_t > ms; + // ms += 2; + // ms += 3, 4, 5, 6, 7, 8, 9, 10; + // ms += 11,12,13,14,15,16,17,18,19; + // ms += 20, 30, 40, 50; + // ms += 20,30,40,50,60,70,80,90,100; + for (size_t m = 2; m <= 50; m += 2) + ms += m; + //for (size_t m=10;m<=100;m+=10) ms += m; + // loop over number of images + BOOST_FOREACH(size_t m,ms) + timeAll >(m, NUM_ITERATIONS); +} + +//************************************************************************************* diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 1a9c82143..c03c40444 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -41,10 +41,6 @@ static string topdir = "TOPSRCDIR_NOT_CONFIGURED"; // If TOPSRCDIR is not define typedef vector strvec; -// NOTE: as this path is only used to generate makefiles, it is hardcoded here for testing -// In practice, this path will be an absolute system path, which makes testing it more annoying -static const std::string headerPath = "/not_really_a_real_path/borg/gtsam/wrap"; - /* ************************************************************************* */ TEST( wrap, ArgumentList ) { ArgumentList args; diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 51dc6f4c3..57a5bce29 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -91,7 +91,8 @@ bool files_equal(const string& expected, const string& actual, bool skipheader) cerr << "<<< DIFF OUTPUT (if none, white-space differences only):\n"; stringstream command; command << "diff --ignore-all-space " << expected << " " << actual << endl; - system(command.str().c_str()); + if (system(command.str().c_str())<0) + throw "command '" + command.str() + "' failed"; cerr << ">>>\n"; } return equal;