diff --git a/.cproject b/.cproject deleted file mode 100644 index 10b16f91c2..0000000000 --- a/.cproject +++ /dev/null @@ -1,3563 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - make - -j5 - SmartProjectionFactorExample_kitti_nonbatch.run - true - true - true - - - make - -j5 - SmartProjectionFactorExample_kitti.run - true - true - true - - - make - -j5 - SmartProjectionFactorTesting.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testSPQRUtil.run - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j4 - testSimilarity3.run - true - true - true - - - make - -j5 - testInvDepthCamera3.run - true - true - true - - - make - -j5 - testTriangulation.run - true - true - true - - - make - -j4 - testEvent.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -k - check - true - false - true - - - make - - tests/testBayesTree.run - true - false - true - - - make - - testBinaryBayesNet.run - true - false - true - - - make - -j2 - testFactorGraph.run - true - true - true - - - make - -j2 - testISAM.run - true - true - true - - - make - -j2 - testJunctionTree.run - true - true - true - - - make - -j2 - testKey.run - true - true - true - - - make - -j2 - testOrdering.run - true - true - true - - - make - - testSymbolicBayesNet.run - true - false - true - - - make - - tests/testSymbolicFactor.run - true - false - true - - - make - - testSymbolicFactorGraph.run - true - false - true - - - make - -j2 - timeSymbolMaps.run - true - true - true - - - make - - tests/testBayesTree - true - false - true - - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testAHRS.run - true - true - true - - - make - -j5 - testInvDepthFactor3.run - true - true - true - - - make - -j5 - testMultiProjectionFactor.run - true - true - true - - - make - -j5 - testPoseRotationPrior.run - true - true - true - - - make - -j5 - testPoseTranslationPrior.run - true - true - true - - - make - -j5 - testReferenceFrameFactor.run - true - true - true - - - make - -j5 - testSmartProjectionFactor.run - true - true - true - - - make - -j5 - testTSAMFactors.run - true - true - true - - - make - -j5 - testInertialNavFactor_GlobalVelocity.run - true - true - true - - - make - -j5 - testInvDepthFactorVariant3.run - true - true - true - - - make - -j5 - testInvDepthFactorVariant1.run - true - true - true - - - make - -j5 - testEquivInertialNavFactor_GlobalVel.run - true - true - true - - - make - -j5 - testInvDepthFactorVariant2.run - true - true - true - - - make - -j5 - testRelativeElevationFactor.run - true - true - true - - - make - -j5 - testPoseBetweenFactor.run - true - true - true - - - make - -j5 - testGaussMarkov1stOrderFactor.run - true - true - true - - - make - -j4 - testSmartStereoProjectionPoseFactor.run - true - true - true - - - make - -j4 - testTOAFactor.run - 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 - testCombinedImuFactor.run - true - true - true - - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testAHRSFactor.run - true - true - true - - - make - -j8 - testAttitudeFactor.run - true - true - true - - - make - -j5 - clean - true - true - true - - - make - -j5 - all - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - testGaussianConditional.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - timeGaussianFactor.run - true - true - true - - - make - -j2 - timeVectorConfig.run - true - true - true - - - make - -j2 - testVectorBTree.run - true - true - true - - - make - -j2 - testVectorMap.run - true - true - true - - - make - -j2 - testNoiseModel.run - true - true - true - - - make - -j2 - testBayesNetPreconditioner.run - true - true - true - - - make - - testErrors.run - true - false - true - - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testGaussianJunctionTree.run - true - true - true - - - make - -j2 - tests/testGaussianFactor.run - true - true - true - - - make - -j2 - tests/testGaussianConditional.run - true - true - true - - - make - -j2 - tests/timeSLAMlike.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testBTree.run - true - true - true - - - make - -j2 - testDSF.run - true - true - true - - - make - -j2 - testDSFVector.run - true - true - true - - - make - -j2 - testMatrix.run - true - true - true - - - make - -j2 - testSPQRUtil.run - true - true - true - - - make - -j2 - testVector.run - true - true - true - - - make - -j2 - timeMatrix.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - testClusterTree.run - true - true - true - - - make - -j2 - testJunctionTree.run - true - true - true - - - make - -j2 - tests/testEliminationTree.run - true - true - true - - - make - -j2 - tests/testSymbolicFactor.run - true - true - true - - - make - -j2 - tests/testVariableSlots.run - true - true - true - - - make - -j2 - tests/testConditional.run - true - true - true - - - make - -j2 - tests/testSymbolicFactorGraph.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - testNonlinearConstraint.run - true - true - true - - - make - -j2 - testLieConfig.run - true - true - true - - - make - -j2 - testConstraintOptimizer.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPlanarSLAM.run - true - true - true - - - make - -j2 - testPose2Config.run - true - true - true - - - make - -j2 - testPose2Factor.run - true - true - true - - - make - -j2 - testPose2Prior.run - true - true - true - - - make - -j2 - testPose2SLAM.run - true - true - true - - - make - -j2 - testPose3Config.run - true - true - true - - - make - -j2 - testPose3SLAM.run - true - true - true - - - make - testSimulated2DOriented.run - true - false - true - - - make - -j2 - testVSLAMConfig.run - true - true - true - - - make - -j2 - testVSLAMFactor.run - true - true - true - - - make - -j2 - testVSLAMGraph.run - true - true - true - - - make - -j2 - testPose3Factor.run - true - true - true - - - make - testSimulated2D.run - true - false - true - - - make - testSimulated3D.run - true - false - true - - - make - -j2 - tests/testGaussianISAM2 - true - true - true - - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - - - make - -j5 - testEliminationTree.run - true - true - true - - - make - -j5 - testInference.run - true - true - true - - - make - -j5 - testKey.run - true - true - true - - - make - -j1 - testSymbolicBayesTree.run - true - false - true - - - make - -j1 - testSymbolicSequentialSolver.run - true - false - true - - - make - -j4 - testLabeledSymbol.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testLieConfig.run - true - true - true - - - make - -j3 - install - true - false - true - - - make - -j2 - clean - true - true - true - - - make - -j1 - check - true - false - true - - - make - -j5 - all - true - true - true - - - cmake - .. - true - false - true - - - make - -j5 - gtsam-shared - true - true - true - - - make - -j5 - gtsam-static - true - true - true - - - make - -j5 - timing - true - true - true - - - make - -j5 - examples - true - true - true - - - make - -j5 - VERBOSE=1 all - true - true - true - - - make - -j5 - VERBOSE=1 check - true - true - true - - - make - -j5 - check.base - true - true - true - - - make - -j5 - timing.base - true - true - true - - - make - -j2 VERBOSE=1 - check.geometry - true - false - true - - - make - -j5 - timing.geometry - true - true - true - - - make - -j2 VERBOSE=1 - check.inference - true - false - true - - - make - -j5 - timing.inference - true - true - true - - - make - -j2 VERBOSE=1 - check.linear - true - false - true - - - make - -j5 - timing.linear - true - true - true - - - make - -j2 VERBOSE=1 - check.nonlinear - true - false - true - - - make - -j5 - timing.nonlinear - true - true - true - - - make - -j2 VERBOSE=1 - check.slam - true - false - true - - - make - -j5 - timing.slam - true - true - true - - - make - -j5 - wrap_gtsam - true - true - true - - - make - VERBOSE=1 - wrap_gtsam - true - false - true - - - cpack - - -G DEB - true - false - true - - - cpack - - -G RPM - true - false - true - - - cpack - - -G TGZ - true - false - true - - - cpack - - --config CPackSourceConfig.cmake - true - false - true - - - make - -j5 - check.discrete - true - true - true - - - make - -j5 - check.discrete_unstable - true - true - true - - - make - -j5 - check.base_unstable - true - true - true - - - make - -j5 - check.dynamics_unstable - true - true - true - - - make - -j5 - check.slam_unstable - true - true - true - - - make - -j5 - check.unstable - true - true - true - - - make - -j5 - wrap_gtsam_build - true - true - true - - - make - -j5 - wrap_gtsam_unstable_build - true - true - true - - - make - -j5 - wrap_gtsam_unstable - true - true - true - - - make - -j5 - wrap - true - true - true - - - make - -j5 - wrap_gtsam_distclean - true - true - true - - - make - -j5 - wrap_gtsam_unstable_distclean - true - true - true - - - make - -j5 - doc - true - true - true - - - make - -j5 - doc_clean - true - true - true - - - make - -j5 - check - true - true - true - - - make - -j5 - check.geometry_unstable - true - true - true - - - make - -j5 - check.linear_unstable - true - true - true - - - make - -j6 -j8 - gtsam_unstable-shared - true - true - true - - - make - -j6 -j8 - gtsam_unstable-static - true - true - true - - - make - -j6 -j8 - check.nonlinear_unstable - true - true - true - - - make - -j5 - check.tests - true - true - true - - - make - -j2 VERBOSE=1 - check.navigation - true - false - true - - - make - -j4 - check.sam - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - all - true - true - true - - - cmake - .. - true - false - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j5 - testCal3Bundler.run - true - true - true - - - make - -j5 - testCal3DS2.run - true - true - true - - - make - -j5 - testCalibratedCamera.run - true - true - true - - - make - -j5 - testEssentialMatrix.run - true - true - true - - - make - -j1 VERBOSE=1 - testHomography2.run - true - false - true - - - make - -j5 - testPinholeCamera.run - true - true - true - - - make - -j5 - testPoint2.run - true - true - true - - - make - -j5 - testPoint3.run - true - true - true - - - make - -j5 - testPose2.run - true - true - true - - - make - -j5 - testPose3.run - true - true - true - - - make - -j5 - testRot3M.run - true - true - true - - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testStereoCamera.run - true - true - true - - - make - -j5 - testCal3Unified.run - true - true - true - - - make - -j5 - testRot2.run - true - true - true - - - make - -j5 - testRot3Q.run - true - true - true - - - make - -j5 - testRot3.run - true - true - true - - - make - -j4 - testSO3.run - true - true - true - - - make - -j4 - testQuaternion.run - true - true - true - - - make - -j4 - testOrientedPlane3.run - true - true - true - - - make - -j4 - testPinholePose.run - true - true - true - - - make - -j4 - testCyclic.run - true - true - true - - - make - -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 - - - make - -j5 - schedulingExample.run - true - true - true - - - make - -j5 - schedulingQuals12.run - true - true - true - - - make - -j5 - schedulingQuals13.run - true - true - true - - - make - -j5 - testCSP.run - true - true - true - - - make - -j5 - testScheduler.run - true - true - true - - - make - -j5 - testSudoku.run - true - true - true - - - make - -j2 - 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 - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testAntiFactor.run - true - true - true - - - make - -j5 - testPriorFactor.run - true - true - true - - - make - -j5 - testDataset.run - true - true - true - - - make - -j5 - testEssentialMatrixFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testRotateFactor.run - true - true - true - - - make - -j5 - testPoseRotationPrior.run - true - true - true - - - make - -j5 - testImplicitSchurFactor.run - true - true - true - - - make - -j4 - testOrientedPlane3Factor.run - true - true - true - - - make - -j4 - testSmartProjectionPoseFactor.run - true - true - true - - - make - -j4 - testInitializePose3.run - true - true - true - - - make - -j2 - SimpleRotation.run - true - true - true - - - make - -j5 - CameraResectioning.run - true - true - true - - - make - -j5 - PlanarSLAMExample.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - easyPoint2KalmanFilter.run - true - true - true - - - make - -j2 - elaboratePoint2KalmanFilter.run - true - true - true - - - make - -j5 - Pose2SLAMExample.run - true - true - true - - - make - -j2 - Pose2SLAMwSPCG_easy.run - true - true - true - - - make - -j5 - UGM_small.run - true - true - true - - - make - -j5 - LocalizationExample.run - true - true - true - - - make - -j5 - OdometryExample.run - true - true - true - - - make - -j5 - RangeISAMExample_plaza2.run - true - true - true - - - make - -j5 - SelfCalibrationExample.run - true - true - true - - - make - -j5 - SFMExample.run - true - true - true - - - make - -j5 - VisualISAMExample.run - true - true - true - - - make - -j5 - VisualISAM2Example.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graphviz.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graph.run - true - true - true - - - make - -j5 - SFMExample_bal.run - true - true - true - - - make - -j5 - Pose2SLAMExample_lago.run - true - true - true - - - make - -j5 - Pose2SLAMExample_g2o.run - true - true - true - - - make - -j5 - SFMExample_SmartFactor.run - true - true - true - - - make - -j4 - Pose2SLAMExampleExpressions.run - true - true - true - - - make - -j4 - SFMExampleExpressions.run - true - true - true - - - make - -j4 - SFMExampleExpressions_bal.run - true - true - true - - - make - -j5 - testLago.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j5 - testOrdering.run - true - true - true - - - make - -j5 - testValues.run - true - true - true - - - make - -j5 - testWhiteNoiseFactor.run - true - true - true - - - make - -j4 - testExpression.run - true - true - true - - - make - -j4 - testAdaptAutoDiff.run - true - true - true - - - make - -j4 - testCallRecord.run - true - true - true - - - make - -j4 - testExpressionFactor.run - true - true - true - - - make - -j4 - testExecutionTrace.run - true - true - true - - - make - -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 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - - - make - -j4 - testBearingFactor.run - true - true - true - - - make - -j4 - testRangeFactor.run - true - true - true - - - make - -j4 - testBearingRangeFactor.run - true - true - true - - - make - -j5 - wrap - true - true - true - - - - diff --git a/examples/ShonanAveragingCLI.cpp b/examples/ShonanAveragingCLI.cpp index 09221fda26..c72a320178 100644 --- a/examples/ShonanAveragingCLI.cpp +++ b/examples/ShonanAveragingCLI.cpp @@ -25,6 +25,9 @@ * Read 3D dataset sphere25000.txt and output to shonan.g2o (default) * ./ShonanAveragingCLI -i spere2500.txt * + * If you prefer using a robust Huber loss, you can add the option "-h true", + * for instance + * ./ShonanAveragingCLI -i spere2500.txt -h true */ #include @@ -43,7 +46,8 @@ int main(int argc, char* argv[]) { string datasetName; string inputFile; string outputFile; - int d, seed; + int d, seed, pMin; + bool useHuberLoss; po::options_description desc( "Shonan Rotation Averaging CLI reads a *pose* graph, extracts the " "rotation constraints, and runs the Shonan algorithm."); @@ -58,6 +62,10 @@ int main(int argc, char* argv[]) { "Write solution to the specified file")( "dimension,d", po::value(&d)->default_value(3), "Optimize over 2D or 3D rotations")( + "useHuberLoss,h", po::value(&useHuberLoss)->default_value(false), + "set True to use Huber loss")("pMin,p", + po::value(&pMin)->default_value(3), + "set to use desired rank pMin")( "seed,s", po::value(&seed)->default_value(42), "Random seed for initial estimate"); po::variables_map vm; @@ -85,11 +93,14 @@ int main(int argc, char* argv[]) { NonlinearFactorGraph::shared_ptr inputGraph; Values::shared_ptr posesInFile; Values poses; + auto lmParams = LevenbergMarquardtParams::CeresDefaults(); if (d == 2) { cout << "Running Shonan averaging for SO(2) on " << inputFile << endl; - ShonanAveraging2 shonan(inputFile); + ShonanAveraging2::Parameters parameters(lmParams); + parameters.setUseHuber(useHuberLoss); + ShonanAveraging2 shonan(inputFile, parameters); auto initial = shonan.initializeRandomly(rng); - auto result = shonan.run(initial); + auto result = shonan.run(initial, pMin); // Parse file again to set up translation problem, adding a prior boost::tie(inputGraph, posesInFile) = load2D(inputFile); @@ -101,9 +112,11 @@ int main(int argc, char* argv[]) { poses = initialize::computePoses(result.first, &poseGraph); } else if (d == 3) { cout << "Running Shonan averaging for SO(3) on " << inputFile << endl; - ShonanAveraging3 shonan(inputFile); + ShonanAveraging3::Parameters parameters(lmParams); + parameters.setUseHuber(useHuberLoss); + ShonanAveraging3 shonan(inputFile, parameters); auto initial = shonan.initializeRandomly(rng); - auto result = shonan.run(initial); + auto result = shonan.run(initial, pMin); // Parse file again to set up translation problem, adding a prior boost::tie(inputGraph, posesInFile) = load3D(inputFile); @@ -118,7 +131,7 @@ int main(int argc, char* argv[]) { return 1; } cout << "Writing result to " << outputFile << endl; - writeG2o(NonlinearFactorGraph(), poses, outputFile); + writeG2o(*inputGraph, poses, outputFile); return 0; } diff --git a/gtsam/sfm/BinaryMeasurement.h b/gtsam/sfm/BinaryMeasurement.h index c525c1b9e6..99e553f7a2 100644 --- a/gtsam/sfm/BinaryMeasurement.h +++ b/gtsam/sfm/BinaryMeasurement.h @@ -45,10 +45,11 @@ template class BinaryMeasurement : public Factor { T measured_; ///< The measurement SharedNoiseModel noiseModel_; ///< Noise model -public: + public: BinaryMeasurement(Key key1, Key key2, const T &measured, const SharedNoiseModel &model = nullptr) - : Factor(std::vector({key1, key2})), measured_(measured), + : Factor(std::vector({key1, key2})), + measured_(measured), noiseModel_(model) {} /// @name Standard Interface @@ -80,4 +81,4 @@ template class BinaryMeasurement : public Factor { } /// @} }; -} // namespace gtsam \ No newline at end of file +} // namespace gtsam diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index df2d72c289..5957047a3b 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -53,7 +53,9 @@ ShonanAveragingParameters::ShonanAveragingParameters( optimalityThreshold(optimalityThreshold), alpha(alpha), beta(beta), - gamma(gamma) { + gamma(gamma), + useHuber(false), + certifyOptimality(true) { // By default, we will do conjugate gradient lm.linearSolverType = LevenbergMarquardtParams::Iterative; @@ -140,25 +142,23 @@ template NonlinearFactorGraph ShonanAveraging::buildGraphAt(size_t p) const { NonlinearFactorGraph graph; auto G = boost::make_shared(SO<-1>::VectorizedGenerators(p)); + for (const auto &measurement : measurements_) { const auto &keys = measurement.keys(); const auto &Rij = measurement.measured(); const auto &model = measurement.noiseModel(); graph.emplace_shared>(keys[0], keys[1], Rij, p, model, G); } - // Possibly add Karcher prior if (parameters_.beta > 0) { const size_t dim = SOn::Dimension(p); graph.emplace_shared>(graph.keys(), dim); } - // Possibly add gauge factors - they are probably useless as gradient is zero if (parameters_.gamma > 0 && p > d + 1) { for (auto key : graph.keys()) graph.emplace_shared(key, p, d, parameters_.gamma); } - return graph; } @@ -186,7 +186,6 @@ ShonanAveraging::createOptimizerAt(size_t p, const Values &initial) const { graph.emplace_shared>(i, SOn::Lift(p, value.matrix()), model); } - // Optimize return boost::make_shared(graph, initial, parameters_.lm); @@ -334,15 +333,33 @@ double ShonanAveraging::cost(const Values &values) const { /* ************************************************************************* */ // Get kappa from noise model -template -static double Kappa(const BinaryMeasurement &measurement) { +template +static double Kappa(const BinaryMeasurement &measurement, + const ShonanAveragingParameters ¶meters) { const auto &isotropic = boost::dynamic_pointer_cast( measurement.noiseModel()); - if (!isotropic) { - throw std::invalid_argument( - "Shonan averaging noise models must be isotropic."); + double sigma; + if (isotropic) { + sigma = isotropic->sigma(); + } else { + const auto &robust = boost::dynamic_pointer_cast( + measurement.noiseModel()); + // Check if noise model is robust + if (robust) { + // If robust, check if optimality certificate is expected + if (parameters.getCertifyOptimality()) { + throw std::invalid_argument( + "Certification of optimality does not work with robust cost."); + } else { + // Optimality certificate not required, so setting default sigma + sigma = 1; + } + } else { + throw std::invalid_argument( + "Shonan averaging noise models must be isotropic (but robust losses " + "are allowed)."); + } } - const double sigma = isotropic->sigma(); return 1.0 / (sigma * sigma); } @@ -362,7 +379,7 @@ Sparse ShonanAveraging::buildD() const { const auto &keys = measurement.keys(); // Get kappa from noise model - double kappa = Kappa(measurement); + double kappa = Kappa(measurement, parameters_); const size_t di = d * keys[0], dj = d * keys[1]; for (size_t k = 0; k < d; k++) { @@ -400,7 +417,7 @@ Sparse ShonanAveraging::buildQ() const { const auto Rij = measurement.measured().matrix(); // Get kappa from noise model - double kappa = Kappa(measurement); + double kappa = Kappa(measurement, parameters_); const size_t di = d * keys[0], dj = d * keys[1]; for (size_t r = 0; r < d; r++) { @@ -793,21 +810,30 @@ std::pair ShonanAveraging::run(const Values &initialEstimate, for (size_t p = pMin; p <= pMax; p++) { // Optimize until convergence at this level Qstar = tryOptimizingAt(p, initialSOp); - - // Check certificate of global optimzality - Vector minEigenVector; - double minEigenValue = computeMinEigenValue(Qstar, &minEigenVector); - if (minEigenValue > parameters_.optimalityThreshold) { - // If at global optimum, round and return solution + if (parameters_.getUseHuber() || !parameters_.getCertifyOptimality()) { + // in this case, there is no optimality certification + if (pMin != pMax) { + throw std::runtime_error( + "When using robust norm, Shonan only tests a single rank. Set pMin = pMax"); + } const Values SO3Values = roundSolution(Qstar); - return std::make_pair(SO3Values, minEigenValue); - } + return std::make_pair(SO3Values, 0); + } else { + // Check certificate of global optimality + Vector minEigenVector; + double minEigenValue = computeMinEigenValue(Qstar, &minEigenVector); + if (minEigenValue > parameters_.optimalityThreshold) { + // If at global optimum, round and return solution + const Values SO3Values = roundSolution(Qstar); + return std::make_pair(SO3Values, minEigenValue); + } - // Not at global optimimum yet, so check whether we will go to next level - if (p != pMax) { - // Calculate initial estimate for next level by following minEigenVector - initialSOp = - initializeWithDescent(p + 1, Qstar, minEigenVector, minEigenValue); + // Not at global optimimum yet, so check whether we will go to next level + if (p != pMax) { + // Calculate initial estimate for next level by following minEigenVector + initialSOp = + initializeWithDescent(p + 1, Qstar, minEigenVector, minEigenValue); + } } } throw std::runtime_error("Shonan::run did not converge for given pMax"); @@ -819,9 +845,13 @@ template class ShonanAveraging<2>; ShonanAveraging2::ShonanAveraging2(const Measurements &measurements, const Parameters ¶meters) - : ShonanAveraging<2>(measurements, parameters) {} + : ShonanAveraging<2>(maybeRobust(measurements, parameters.getUseHuber()), + parameters) {} + ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters ¶meters) - : ShonanAveraging<2>(parseMeasurements(g2oFile), parameters) {} + : ShonanAveraging<2>(maybeRobust(parseMeasurements(g2oFile), + parameters.getUseHuber()), + parameters) {} /* ************************************************************************* */ // Explicit instantiation for d=3 @@ -829,10 +859,13 @@ template class ShonanAveraging<3>; ShonanAveraging3::ShonanAveraging3(const Measurements &measurements, const Parameters ¶meters) - : ShonanAveraging<3>(measurements, parameters) {} + : ShonanAveraging<3>(maybeRobust(measurements, parameters.getUseHuber()), + parameters) {} ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters ¶meters) - : ShonanAveraging<3>(parseMeasurements(g2oFile), parameters) {} + : ShonanAveraging<3>(maybeRobust(parseMeasurements(g2oFile), + parameters.getUseHuber()), + parameters) {} // TODO(frank): Deprecate after we land pybind wrapper @@ -847,9 +880,11 @@ static BinaryMeasurement convert( "parseMeasurements can only convert Pose3 measurements " "with Gaussian noise models."); const Matrix6 M = gaussian->covariance(); - return BinaryMeasurement( - f->key1(), f->key2(), f->measured().rotation(), - noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true)); + auto model = noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), + noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3))); + return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), + model); } static ShonanAveraging3::Measurements extractRot3Measurements( @@ -862,7 +897,9 @@ static ShonanAveraging3::Measurements extractRot3Measurements( ShonanAveraging3::ShonanAveraging3(const BetweenFactorPose3s &factors, const Parameters ¶meters) - : ShonanAveraging<3>(extractRot3Measurements(factors), parameters) {} + : ShonanAveraging<3>(maybeRobust(extractRot3Measurements(factors), + parameters.getUseHuber()), + parameters) {} /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index edd9f33a22..a603dec98a 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -46,13 +46,17 @@ struct GTSAM_EXPORT ShonanAveragingParameters { using Rot = typename std::conditional::type; using Anchor = std::pair; - // Paremeters themselves: - LevenbergMarquardtParams lm; // LM parameters - double optimalityThreshold; // threshold used in checkOptimality - Anchor anchor; // pose to use as anchor if not Karcher - double alpha; // weight of anchor-based prior (default 0) - double beta; // weight of Karcher-based prior (default 1) - double gamma; // weight of gauge-fixing factors (default 0) + // Parameters themselves: + LevenbergMarquardtParams lm; ///< LM parameters + double optimalityThreshold; ///< threshold used in checkOptimality + Anchor anchor; ///< pose to use as anchor if not Karcher + double alpha; ///< weight of anchor-based prior (default 0) + double beta; ///< weight of Karcher-based prior (default 1) + double gamma; ///< weight of gauge-fixing factors (default 0) + /// if enabled, the Huber loss is used (default false) + bool useHuber; + /// if enabled solution optimality is certified (default true) + bool certifyOptimality; ShonanAveragingParameters(const LevenbergMarquardtParams &lm = LevenbergMarquardtParams::CeresDefaults(), @@ -67,16 +71,31 @@ struct GTSAM_EXPORT ShonanAveragingParameters { double getOptimalityThreshold() const { return optimalityThreshold; } void setAnchor(size_t index, const Rot &value) { anchor = {index, value}; } - std::pair getAnchor() { return anchor; } + std::pair getAnchor() const { return anchor; } void setAnchorWeight(double value) { alpha = value; } - double getAnchorWeight() { return alpha; } + double getAnchorWeight() const { return alpha; } void setKarcherWeight(double value) { beta = value; } - double getKarcherWeight() { return beta; } + double getKarcherWeight() const { return beta; } void setGaugesWeight(double value) { gamma = value; } - double getGaugesWeight() { return gamma; } + double getGaugesWeight() const { return gamma; } + + void setUseHuber(bool value) { useHuber = value; } + bool getUseHuber() const { return useHuber; } + + void setCertifyOptimality(bool value) { certifyOptimality = value; } + bool getCertifyOptimality() const { return certifyOptimality; } + + /// Print the parameters and flags used for rotation averaging. + void print() const { + std::cout << " ShonanAveragingParameters: " << std::endl; + std::cout << " alpha: " << alpha << std::endl; + std::cout << " beta: " << beta << std::endl; + std::cout << " gamma: " << gamma << std::endl; + std::cout << " useHuber: " << useHuber << std::endl; + } }; using ShonanAveragingParameters2 = ShonanAveragingParameters<2>; @@ -107,7 +126,6 @@ class GTSAM_EXPORT ShonanAveraging { using Rot = typename Parameters::Rot; // We store SO(d) BetweenFactors to get noise model - // TODO(frank): use BinaryMeasurement? using Measurements = std::vector>; private: @@ -151,6 +169,36 @@ class GTSAM_EXPORT ShonanAveraging { return measurements_[k]; } + /** + * Update factors to use robust Huber loss. + * + * @param measurements Vector of BinaryMeasurements. + * @param k Huber noise model threshold. + */ + Measurements makeNoiseModelRobust(const Measurements &measurements, + double k = 1.345) const { + Measurements robustMeasurements; + for (auto &measurement : measurements) { + auto model = measurement.noiseModel(); + const auto &robust = + boost::dynamic_pointer_cast(model); + + SharedNoiseModel robust_model; + // Check if the noise model is already robust + if (robust) { + robust_model = model; + } else { + // make robust + robust_model = noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(k), model); + } + BinaryMeasurement meas(measurement.key1(), measurement.key2(), + measurement.measured(), robust_model); + robustMeasurements.push_back(meas); + } + return robustMeasurements; + } + /// k^th measurement, as a Rot. const Rot &measured(size_t k) const { return measurements_[k].measured(); } @@ -345,6 +393,22 @@ class GTSAM_EXPORT ShonanAveraging { std::pair run(const Values &initialEstimate, size_t pMin = d, size_t pMax = 10) const; /// @} + + /** + * Helper function to convert measurements to robust noise model + * if flag is set. + * + * @tparam T the type of measurement, e.g. Rot3. + * @param measurements vector of BinaryMeasurements of type T. + * @param useRobustModel flag indicating whether use robust noise model + * instead. + */ + template + inline std::vector> maybeRobust( + const std::vector> &measurements, + bool useRobustModel = false) const { + return useRobustModel ? makeNoiseModelRobust(measurements) : measurements; + } }; // Subclasses for d=2 and d=3 that explicitly instantiate, as well as provide a diff --git a/gtsam/sfm/tests/testBinaryMeasurement.cpp b/gtsam/sfm/tests/testBinaryMeasurement.cpp index 3dd81c2c1f..ae13e54c47 100644 --- a/gtsam/sfm/tests/testBinaryMeasurement.cpp +++ b/gtsam/sfm/tests/testBinaryMeasurement.cpp @@ -36,6 +36,7 @@ static SharedNoiseModel rot3_model(noiseModel::Isotropic::Sigma(3, 0.05)); const Unit3 unit3Measured(Vector3(1, 1, 1)); const Rot3 rot3Measured; +/* ************************************************************************* */ TEST(BinaryMeasurement, Unit3) { BinaryMeasurement unit3Measurement(kKey1, kKey2, unit3Measured, unit3_model); @@ -48,6 +49,7 @@ TEST(BinaryMeasurement, Unit3) { EXPECT(unit3Measurement.equals(unit3MeasurementCopy)); } +/* ************************************************************************* */ TEST(BinaryMeasurement, Rot3) { // testing the accessors BinaryMeasurement rot3Measurement(kKey1, kKey2, rot3Measured, @@ -62,6 +64,21 @@ TEST(BinaryMeasurement, Rot3) { EXPECT(rot3Measurement.equals(rot3MeasurementCopy)); } +/* ************************************************************************* */ +TEST(BinaryMeasurement, Rot3MakeRobust) { + auto huber_model = noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), rot3_model); + BinaryMeasurement rot3Measurement(kKey1, kKey2, rot3Measured, + huber_model); + + EXPECT_LONGS_EQUAL(rot3Measurement.key1(), kKey1); + EXPECT_LONGS_EQUAL(rot3Measurement.key2(), kKey2); + EXPECT(rot3Measurement.measured().equals(rot3Measured)); + const auto &robust = boost::dynamic_pointer_cast( + rot3Measurement.noiseModel()); + EXPECT(robust); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index 1200c8ebb8..c2ad71dad1 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include #include @@ -321,6 +322,42 @@ TEST(ShonanAveraging2, noisyToyGraph) { EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate! } +/* ************************************************************************* */ +TEST(ShonanAveraging2, noisyToyGraphWithHuber) { + // Load 2D toy example + auto lmParams = LevenbergMarquardtParams::CeresDefaults(); + string g2oFile = findExampleDataFile("noisyToyGraph.txt"); + ShonanAveraging2::Parameters parameters(lmParams); + auto measurements = parseMeasurements(g2oFile); + parameters.setUseHuber(true); + parameters.setCertifyOptimality(false); + + string parameters_print = + " ShonanAveragingParameters: \n alpha: 0\n beta: 1\n gamma: 0\n " + "useHuber: 1\n"; + assert_print_equal(parameters_print, parameters); + + ShonanAveraging2 shonan(measurements, parameters); + EXPECT_LONGS_EQUAL(4, shonan.nrUnknowns()); + + // Check graph building + NonlinearFactorGraph graph = shonan.buildGraphAt(2); + EXPECT_LONGS_EQUAL(6, graph.size()); + + // test that each factor is actually robust + for (size_t i=0; i<=4; i++) { // note: last is the Gauge factor and is not robust + const auto &robust = boost::dynamic_pointer_cast( + boost::dynamic_pointer_cast(graph[i])->noiseModel()); + EXPECT(robust); // we expect the factors to be use a robust noise model (in particular, Huber) + } + + // test result + auto initial = shonan.initializeRandomly(kRandomNumberGenerator); + auto result = shonan.run(initial, 2,2); + EXPECT_DOUBLES_EQUAL(0.0008211, shonan.cost(result.first), 1e-6); + EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate! +} + /* ************************************************************************* */ // Test alpha/beta/gamma prior weighting. TEST(ShonanAveraging3, PriorWeights) { diff --git a/gtsam/slam/FrobeniusFactor.cpp b/gtsam/slam/FrobeniusFactor.cpp index 5697a0cd61..8c0baaf384 100644 --- a/gtsam/slam/FrobeniusFactor.cpp +++ b/gtsam/slam/FrobeniusFactor.cpp @@ -23,17 +23,25 @@ using namespace std; namespace gtsam { //****************************************************************************** -boost::shared_ptr +SharedNoiseModel ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) { double sigma = 1.0; + if (model != nullptr) { - auto sigmas = model->sigmas(); + const auto &robust = boost::dynamic_pointer_cast(model); + Vector sigmas; + if (robust) { + sigmas = robust->noise()->sigmas(); + } else { + sigmas = model->sigmas(); + } + size_t n = sigmas.size(); if (n == 1) { sigma = sigmas(0); // Rot2 goto exit; } - if (n == 3 || n == 6) { + else if (n == 3 || n == 6) { sigma = sigmas(2); // Pose2, Rot3, or Pose3 if (sigmas(0) != sigma || sigmas(1) != sigma) { if (!defaultToUnit) { @@ -46,8 +54,15 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) { throw std::runtime_error("Can only convert Pose2/Pose3 noise models"); } } -exit: - return noiseModel::Isotropic::Sigma(d, sigma); + exit: + auto isoModel = noiseModel::Isotropic::Sigma(d, sigma); + const auto &robust = boost::dynamic_pointer_cast(model); + if (robust) { + return noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), isoModel); + } else { + return isoModel; + } } //****************************************************************************** diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index 1fc37c7852..f17a9e4217 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -26,15 +26,20 @@ namespace gtsam { /** - * When creating (any) FrobeniusFactor we can convert a Rot/Pose - * BetweenFactor noise model into a n-dimensional isotropic noise - * model used to weight the Frobenius norm. If the noise model passed is - * null we return a n-dimensional isotropic noise model with sigma=1.0. If - * not, we we check if the d-dimensional noise model on rotations is + * When creating (any) FrobeniusFactor we can convert a Rot/Pose BetweenFactor + * noise model into a n-dimensional isotropic noise + * model used to weight the Frobenius norm. + * If the noise model passed is null we return a n-dimensional isotropic noise + * model with sigma=1.0. + * If not, we we check if the d-dimensional noise model on rotations is * isotropic. If it is, we extend to 'n' dimensions, otherwise we throw an - * error. If defaultToUnit == false throws an exception on unexepcted input. + * error. + * If the noise model is a robust error model, we use the sigmas of the + * underlying noise model. + * + * If defaultToUnit == false throws an exception on unexepcted input. */ -GTSAM_EXPORT boost::shared_ptr +GTSAM_EXPORT SharedNoiseModel ConvertNoiseModel(const SharedNoiseModel &model, size_t n, bool defaultToUnit = true);