From 0dc6b1a3f9fd8769b641a27737b95bdba2b07929 Mon Sep 17 00:00:00 2001 From: Timothy Wiley Date: Mon, 7 Aug 2017 18:56:09 +1000 Subject: [PATCH] starting tesing of graphslam changes --- crosbot_graphslam/src/graphSlam.cpp | 3 --- crosbot_graphslam/src/graphSlamCPU.cpp | 5 ++++- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/crosbot_graphslam/src/graphSlam.cpp b/crosbot_graphslam/src/graphSlam.cpp index 77ae88a..80db7c3 100644 --- a/crosbot_graphslam/src/graphSlam.cpp +++ b/crosbot_graphslam/src/graphSlam.cpp @@ -31,10 +31,7 @@ void GraphSlam::initialise(ros::NodeHandle &nh) { paramNH.param("InformationScaleFactor", InformationScaleFactor, 1000000); paramNH.param("MaxCovar", MaxCovar, 1.5); paramNH.param("CorrelationThreshold", CorrelationThreshold, 3.4); - //paramNH.param("CorrelationThreshold", CorrelationThreshold, 3.6); paramNH.param("MinGoodCount", MinGoodCount, 10); - //paramNH.param("FinalMinGoodCount", FinalMinGoodCount, 175); - //paramNH.param("FinalMinGoodCount", FinalMinGoodCount, 225); paramNH.param("FinalMinGoodCount", FinalMinGoodCount, 200); paramNH.param("MaxIterations", MaxIterations, 50); paramNH.param("MaxErrorTheta", MaxErrorTheta, 0.03); diff --git a/crosbot_graphslam/src/graphSlamCPU.cpp b/crosbot_graphslam/src/graphSlamCPU.cpp index 9f0fe32..520d43b 100644 --- a/crosbot_graphslam/src/graphSlamCPU.cpp +++ b/crosbot_graphslam/src/graphSlamCPU.cpp @@ -178,6 +178,7 @@ void GraphSlamCPU::updateTrack(Pose icpPose, PointCloudPtr cloud, ros::Time stam oldICPPose.getYPR(yo, po, ro); diffTh = yi - yo; ANGNORM(diffTh); + //Update the slam position double ys, ps, rs; slamPose.getYPR(ys, ps, rs); @@ -189,11 +190,13 @@ void GraphSlamCPU::updateTrack(Pose icpPose, PointCloudPtr cloud, ros::Time stam slamPose.position.x += cosTh * diffX - sinTh * diffY; slamPose.position.y += sinTh * diffX + cosTh * diffY; slamPose.setYPR(ys, pi, ri); + //Update the offset in the current local map offsetFromParentX += diffX; offsetFromParentY += diffY; offsetFromParentTh += diffTh; ANGNORM(offsetFromParentTh); + //Need to convert diffX and diffY to be relative to the current local map cosTh = cos(-common->startICPTh); sinTh = sin(-common->startICPTh); @@ -2699,7 +2702,7 @@ void GraphSlamCPU::optimiseGraph(int type) { int constraintIndex = common->constraintIndex[lastFullLoopIndex + 1]; //startingNode = common->loopConstraintJ[constraintIndex]; if (common->constraintType[lastFullLoopIndex + 1] == 1) { - cout << "Error: constraint after loop cloosing constraint is another loop closing one" << endl; + cout << "Error: constraint after loop closing constraint is another loop closing one" << endl; } startingNode = constraintIndex - 1; -- GitLab