12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

still debugging

release/4.3a0
Luca 2014-09-02 20:56:36 -04:00
parent 8384c37179
commit 35d5b56b65
2 changed files with 46 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/slam/InitializePose3.cpp
View File

@ -183,13 +183,14 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
double rho = 2*a*b;
double mu_max = maxNodeDeg * rho;
double stepsize = 2/mu_max; // = 1/(a b dG)
size_t maxIter = 1000;
size_t maxIter = 1;
// gradient iterations
vector<double> reshapedCost;
for(size_t it=0; it < maxIter; it++){
//////////////////////////////////////////////////////////////////////////
// compute the gradient at each node
std::cout << "it " << it <<" b " << b <<" f0 " << f0 <<" a " << a
<<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
double maxGrad = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) {
Key key = key_value.key;
@ -213,6 +214,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
} // end of i-th gradient computation
double normGradKey = (grad.at(key)).norm();
std::cout << "key " << DefaultKeyFormatter(key) <<" grad " << grad.at(key) << std::endl;
if(normGradKey>maxGrad)
maxGrad = normGradKey;
} // end of loop over nodes
@ -274,7 +276,7 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) {
Vector3 logRot = Rot3::Logmap(R1.between(R2));
if(logRot.norm()<1e-4){ // some tolerance
Rot3 R1pert = R1.compose( Rot3::Expmap((Vector(3)<< 0.0, 0.0, 0.0)) ); // some perturbation
Rot3 R1pert = R1.compose( Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01)) ); // some perturbation
logRot = Rot3::Logmap(R1pert.between(R2));
}
double th = logRot.norm();

41
gtsam/slam/tests/testInitializePose3.cpp
View File

@ -123,6 +123,42 @@ TEST( InitializePose3, orientationsGradientSymbolicGraph ) {
EXPECT_DOUBLES_EQUAL(adjEdgesMap.size(), 5, 1e-9);
}
/* *************************************************************************** *
TEST( InitializePose3, orientationsCheckGradient ) {
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
// Wrong initial guess - initialization should fix the rotations
Values givenPoses;
givenPoses.insert(x0,simple::pose0);
givenPoses.insert(x1,simple::pose0);
givenPoses.insert(x2,simple::pose0);
givenPoses.insert(x3,simple::pose0);
Values initial = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal(simple::R0, initial.at<Rot3>(x0), 1e-6));
EXPECT(assert_equal(simple::R1, initial.at<Rot3>(x1), 1e-6));
EXPECT(assert_equal(simple::R2, initial.at<Rot3>(x2), 1e-6));
EXPECT(assert_equal(simple::R3, initial.at<Rot3>(x3), 1e-6));
}
/* *************************************************************************** */
TEST( InitializePose3, singleGradient ) {
Rot3 R1 = Rot3();
Matrix M = Matrix3::Zero();
M(0,1) = -1; M(1,0) = 1; M(2,2) = 1;
Rot3 R2 = Rot3(M);
double a = 6.010534238540223;
double b = 1.0;
Vector actual = InitializePose3::gradientTron(R1, R2, a, b);
Vector expected = Vector3::Zero();
expected(2) = 1.962658662803917;
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal(expected, actual, 1e-6));
}
/* *************************************************************************** */
TEST( InitializePose3, orientationsGradient ) {
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
@ -135,6 +171,11 @@ TEST( InitializePose3, orientationsGradient ) {
givenPoses.insert(x3,simple::pose0);
Values initial = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses);
const Key keyAnchor = symbol('Z', 9999999);
givenPoses.insert(keyAnchor,simple::pose0);
string g2oFile = "/home/aspn/Desktop/toyExample.g2o";
writeG2o(pose3Graph, givenPoses, g2oFile);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal(simple::R0, initial.at<Rot3>(x0), 1e-6));
EXPECT(assert_equal(simple::R1, initial.at<Rot3>(x1), 1e-6));