nice and clean unit tests for gradient initialization
Luca
parent
3d06a737bf
commit
7322a74bbd
24
.cproject
24
.cproject
|
|
@ -788,18 +788,18 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianFactorGraph.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testGaussianFactorGraphUnordered.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testGaussianFactorGraph.run</buildTarget>
|
||||
<buildTarget>testGaussianFactorGraphUnordered.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianBayesNet.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testGaussianBayesNetUnordered.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testGaussianBayesNet.run</buildTarget>
|
||||
<buildTarget>testGaussianBayesNetUnordered.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -2631,6 +2631,22 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGPSFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testGPSFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussMarkov1stOrderFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testGaussMarkov1stOrderFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testInitializePose3.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,11 @@
|
|||
VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.0008187 0.0011723 0.0895466 0.9959816
|
||||
VERTEX_SE3:QUAT 1 0.000000 -0.000000 0.000000 0.0010673 0.0015636 0.1606931 0.9870026
|
||||
VERTEX_SE3:QUAT 2 -0.388822 0.632954 0.001223 0.0029920 0.0014066 0.0258235 0.9996610
|
||||
VERTEX_SE3:QUAT 3 -1.143204 0.050638 0.006026 -0.0012800 -0.0002767 -0.2850291 0.9585180
|
||||
VERTEX_SE3:QUAT 4 -0.512416 0.486441 0.005171 0.0002681 0.0023574 0.0171476 0.9998502
|
||||
EDGE_SE3:QUAT 1 2 1.000000 2.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
|
||||
EDGE_SE3:QUAT 2 3 -0.000000 1.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
|
||||
EDGE_SE3:QUAT 3 4 1.000000 1.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
|
||||
EDGE_SE3:QUAT 3 1 0.000001 2.000000 0.000000 0.0000000 0.0000000 1.0000000 0.0000002 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
|
||||
EDGE_SE3:QUAT 1 4 -1.000000 1.000000 0.000000 0.0000000 0.0000000 -0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
|
||||
EDGE_SE3:QUAT 0 1 0.000000 0.000000 0.000000 0.0000000 0.0000000 0.0000000 1.0000000 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
|
||||
|
|
@ -147,7 +147,7 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// Return the orientations of a graph including only BetweenFactors<Pose3>
|
||||
Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter) {
|
||||
Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter, const bool setRefFrame) {
|
||||
gttic(InitializePose3_computeOrientationsGradient);
|
||||
|
||||
// this works on the inverse rotations, according to Tron&Vidal,2011
|
||||
|
|
@ -219,7 +219,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
|
|||
grad.at(key) = stepsize * gradKey;
|
||||
|
||||
double normGradKey = (gradKey).norm();
|
||||
// std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl;
|
||||
//std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl;
|
||||
if(normGradKey>maxGrad)
|
||||
maxGrad = normGradKey;
|
||||
} // end of loop over nodes
|
||||
|
|
@ -243,7 +243,10 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
|
|||
Key key = key_value.key;
|
||||
if (key != keyAnchor) {
|
||||
const Rot3& R = inverseRot.at<Rot3>(key);
|
||||
estimateRot.insert(key, Rref.compose(R.inverse()));
|
||||
if(setRefFrame)
|
||||
estimateRot.insert(key, Rref.compose(R.inverse()));
|
||||
else
|
||||
estimateRot.insert(key, R.inverse());
|
||||
}
|
||||
}
|
||||
return estimateRot;
|
||||
|
|
|
|||
|
|
@ -39,7 +39,8 @@ GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
|
|||
|
||||
GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph);
|
||||
|
||||
GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, size_t maxIter = 10000);
|
||||
GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph,
|
||||
const Values& givenGuess, size_t maxIter = 10000, const bool setRefFrame = true);
|
||||
|
||||
GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
|
||||
const NonlinearFactorGraph& pose3Graph);
|
||||
|
|
|
|||
|
|
@ -153,18 +153,12 @@ TEST( InitializePose3, iterationGradient ) {
|
|||
givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) ));
|
||||
|
||||
size_t maxIter = 1; // test gradient at the first iteration
|
||||
Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter);
|
||||
bool setRefFrame = false;
|
||||
Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame);
|
||||
|
||||
// const Key keyAnchor = symbol('Z', 9999999);
|
||||
// Matrix Mz = (Matrix(3,3) << 0.999993962808392, -0.002454045561375, 0.002460082752984,
|
||||
// 0.002460082752984, 0.999993962808392, -0.002454045561375,
|
||||
// -0.002454045561375, 0.002460082752984, 0.999993962808392);
|
||||
// Rot3 RzExpected = Rot3(Mz);
|
||||
// EXPECT(assert_equal(RzExpected, orientations.at<Rot3>(keyAnchor), 1e-6));
|
||||
|
||||
Matrix M0 = (Matrix(3,3) << 0.999344848920642, -0.036021919324717, 0.003506317718352,
|
||||
0.036032601656108, 0.999346013522419, -0.003032634950127,
|
||||
-0.003394783302464, 0.003156989865691, 0.999989254372924);
|
||||
Matrix M0 = (Matrix(3,3) << 0.999435813876064, -0.033571481675497, 0.001004768630281,
|
||||
0.033572116359134, 0.999436104312325, -0.000621610948719,
|
||||
-0.000983333645009, 0.000654992453817, 0.999999302019670);
|
||||
Rot3 R0Expected = Rot3(M0);
|
||||
EXPECT(assert_equal(R0Expected, orientations.at<Rot3>(x0), 1e-5));
|
||||
|
||||
|
|
@ -199,42 +193,30 @@ TEST( InitializePose3, orientationsGradient ) {
|
|||
givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) ));
|
||||
givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) ));
|
||||
// do 10 gradient iterations
|
||||
Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10);
|
||||
bool setRefFrame = false;
|
||||
Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame);
|
||||
|
||||
// const Key keyAnchor = symbol('Z', 9999999);
|
||||
// givenPoses.insert(keyAnchor,simple::pose0);
|
||||
// string g2oFile = "/home/aspn/Desktop/toyExample.g2o";
|
||||
// writeG2o(pose3Graph, givenPoses, g2oFile);
|
||||
|
||||
// const Key keyAnchor = symbol('Z', 9999999);
|
||||
// Matrix Mz = (Matrix(3,3) << 0.983348036379704, -0.181672808000167, 0.004650825895948,
|
||||
// 0.181688570817424, 0.983350839452522, -0.003223318529546,
|
||||
// -0.003987804220587, 0.004014645856811, 0.999983989889910);
|
||||
// Rot3 RzExpected = Rot3(Mz);
|
||||
// EXPECT(assert_equal(RzExpected, orientations.at<Rot3>(keyAnchor), 1e-4));
|
||||
const string matlabResultsfile = findExampleDataFile("simpleGraph10gradIter");
|
||||
NonlinearFactorGraph::shared_ptr matlabGraph;
|
||||
Values::shared_ptr matlabValues;
|
||||
bool is3D = true;
|
||||
boost::tie(matlabGraph, matlabValues) = readG2o(matlabResultsfile, is3D);
|
||||
|
||||
Matrix M0 = (Matrix(3,3) << 0.946965375724015, -0.321288672646614, 0.005492359133630,
|
||||
0.321308000189570, 0.946969747977338, -0.003076593882320,
|
||||
-0.004212623179851, 0.004678166811270, 0.999980184084280);
|
||||
Rot3 R0Expected = Rot3(M0);
|
||||
Rot3 R0Expected = matlabValues->at<Pose3>(1).rotation();
|
||||
EXPECT(assert_equal(R0Expected, orientations.at<Rot3>(x0), 1e-4));
|
||||
|
||||
Matrix M1 = (Matrix(3,3) << 0.998716651908197, -0.050557818750367, 0.002992685163299,
|
||||
0.050577444118653, 0.998696360370342, -0.006892164352146,
|
||||
-0.002640330984207, 0.007034681672788, 0.999971770554577);
|
||||
Rot3 R1Expected = Rot3(M1);
|
||||
Rot3 R1Expected = matlabValues->at<Pose3>(2).rotation();
|
||||
EXPECT(assert_equal(R1Expected, orientations.at<Rot3>(x1), 1e-4));
|
||||
|
||||
Matrix M2 = (Matrix(3,3) << 0.837033946147607, 0.547150466557869, 0.000734807505930,
|
||||
-0.547150098128722, 0.837029489230720, 0.002899012916604,
|
||||
0.000971140718506, -0.002828622220494, 0.999995527881019);
|
||||
Rot3 R2Expected = Rot3(M2);
|
||||
Rot3 R2Expected = matlabValues->at<Pose3>(3).rotation();
|
||||
EXPECT(assert_equal(R2Expected, orientations.at<Rot3>(x2), 1e-3));
|
||||
|
||||
Matrix M3 = (Matrix(3,3) << 0.999422151199602, -0.033471815469964, 0.005916186331164,
|
||||
0.033474965560969, 0.999439461971174, -0.000434206705656,
|
||||
-0.005898336397012, 0.000631999933520, 0.999982404947123);
|
||||
Rot3 R3Expected = Rot3(M3);
|
||||
Rot3 R3Expected = matlabValues->at<Pose3>(4).rotation();
|
||||
EXPECT(assert_equal(R3Expected, orientations.at<Rot3>(x3), 1e-4));
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue