From 0e0ca6a8c91b5765ba137c2aaac04c0569948294 Mon Sep 17 00:00:00 2001 From: Adrian Ratter Date: Fri, 12 Sep 2014 02:30:08 +1000 Subject: [PATCH] Minor changes to optimiser --- crosbot_graphslam/src/graphSlam.cpp | 4 ++-- crosbot_graphslam/src/graphSlamCPU.cpp | 14 ++++++++++---- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/crosbot_graphslam/src/graphSlam.cpp b/crosbot_graphslam/src/graphSlam.cpp index 0c449e7..fe17658 100644 --- a/crosbot_graphslam/src/graphSlam.cpp +++ b/crosbot_graphslam/src/graphSlam.cpp @@ -31,9 +31,9 @@ void GraphSlam::initialise(ros::NodeHandle &nh) { 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, 175); //paramNH.param("FinalMinGoodCount", FinalMinGoodCount, 225); - //paramNH.param("FinalMinGoodCount", FinalMinGoodCount, 200); + paramNH.param("FinalMinGoodCount", FinalMinGoodCount, 200); paramNH.param("MaxIterations", MaxIterations, 50); paramNH.param("MaxErrorTheta", MaxErrorTheta, 0.03); paramNH.param("MaxErrorDisp", MaxErrorDisp, 0.0005); diff --git a/crosbot_graphslam/src/graphSlamCPU.cpp b/crosbot_graphslam/src/graphSlamCPU.cpp index 1bb2fd6..8748941 100644 --- a/crosbot_graphslam/src/graphSlamCPU.cpp +++ b/crosbot_graphslam/src/graphSlamCPU.cpp @@ -231,9 +231,9 @@ void GraphSlamCPU::updateTrack(Pose icpPose, PointCloudPtr cloud, ros::Time stam double mapGradX = common->grid[ogIndex].gradX / length; double mapGradY = common->grid[ogIndex].gradY / length; if (length > 0) { - //double temp = x * mapGradX + y * mapGradY * 0.1; + double temp = x * mapGradX + y * mapGradY; //TODO: find a better way to estimate the angular covar of a scan - double temp = 0.5; + //double temp = 0.5; tempCovar[0][0] += mapGradX * mapGradX; tempCovar[0][1] += mapGradX * mapGradY; tempCovar[0][2] += mapGradX * temp; @@ -1357,9 +1357,12 @@ void GraphSlamCPU::prepareLocalMap() { //a[y][x] = localMaps[currentLocalMap].internalCovar[y][x] / (double)lastI; //a[y][x] = (localMaps[currentLocalMap].scans[lastI]->covar[y][x] * // localMaps[currentLocalMap].scans[lastI]->covar[y][x]) * 1000000; + //a[y][x] = ((localMaps[currentLocalMap].internalCovar[y][x] / (double) lastI) * // (localMaps[currentLocalMap].internalCovar[y][x] / (double) lastI)) * InformationScaleFactor; - a[y][x] = (localMaps[currentLocalMap].internalCovar[y][x] / (double) lastI) * 10; + //a[y][x] = (localMaps[currentLocalMap].internalCovar[y][x] / (double) lastI) * 100; + a[y][x] = ((localMaps[currentLocalMap].internalCovar[y][x] / (double) lastI) * + (localMaps[currentLocalMap].internalCovar[y][x] / (double) lastI)) * 100000; } } @@ -2923,11 +2926,14 @@ void GraphSlamCPU::optimiseGraph(int type) { cs_spfree(compH); if (retVal != 1) { cout << "Colesky failed. Can't optimise " << retVal << endl; - return; + break; } //b has the solution!! for (int x = 0; x < numMaps; x++) { + if (b[x * 3 + 2] > 100) { + cout << "AAAAAAAAAAAARGGGG angle is big: " << b[x * 3 + 2] << endl; + } ANGNORM(b[x * 3 + 2]); //cout << "Map " << x << " before: " << localMaps[x].currentGlobalPosX << " " << // localMaps[x].currentGlobalPosY << " " << localMaps[x].currentGlobalPosTh << -- GitLab