wall only version

src/LidarOdometry.cpp

@@ -123,7 +123,7 @@ void LidarOdometry::onInit()
     if((sqlite3_prepare_v3(db, query.str().c_str(), -1, SQLITE_PREPARE_PERSISTENT, &poleStmt, 0)))
         ROS_ERROR_STREAM("ERROR while preparing statment: " << sqlite3_errmsg(db));
 
-    if(mDetectWalls && (sqlite3_prepare_v3(db, wallquery2.str().c_str(), -1, SQLITE_PREPARE_PERSISTENT, &wallStmt, 0))){
+    if(mDetectWalls && (sqlite3_prepare_v3(db, wallquery.str().c_str(), -1, SQLITE_PREPARE_PERSISTENT, &wallStmt, 0))){
         mDetectWalls = false;
         ROS_ERROR_STREAM("ERROR while preparing statment: " << sqlite3_errmsg(db));
     }
@@ -397,11 +397,8 @@ void LidarOdometry::insertPrints(std::map<long, Pole> &poleMap, FingerprintMap &
 
                 Pole other = poleMap[print.first];
                 geometry_msgs::Point otherutmpos;
-                if(isEdgeType(other))
-                    otherutmpos = map2UTM(other.pose.position, other.header.stamp);
-                else
-                    continue;
-
+                if(!isEdgeType(other))
+                    continue; //otherutmpos = map2UTM(other.pose.position, other.header.stamp);
 
                 sqlite3_bind_int64(mWallInsertStmt, 1, id);
                 sqlite3_bind_double(mWallInsertStmt, 2, utmpos.x);
@@ -433,6 +430,7 @@ void LidarOdometry::insertPrints(std::map<long, Pole> &poleMap, FingerprintMap &
                 if(lastPoleMap.find(id) != lastPoleMap.end() && lastPoleMap.find(print.first) != lastPoleMap.end())
                     continue;
 
+                continue;
                 sqlite3_bind_int64(mPoleInsertStmt, 1, id);
                 sqlite3_bind_double(mPoleInsertStmt, 2, utmpos.x);
                 sqlite3_bind_double(mPoleInsertStmt, 3, utmpos.y);
@@ -741,7 +739,7 @@ std::vector<std::tuple<pole_based_localization::Pole, std::vector<std::tuple<dou
         double x1=coords.x - mMaxSearchRadius, y1=coords.y - mMaxSearchRadius, x2 = coords.x + mMaxSearchRadius, y2 = coords.y + mMaxSearchRadius;
 
         if(isEdge){
-            auto endPoint = edgeToPoints(trackedPole).second;
+            auto endPoint = map2UTM(edgeToPoints(trackedPole).second, trackedPole.header.stamp);
             x2 = endPoint.x + mMaxSearchRadius;
             y2 = endPoint.y + mMaxSearchRadius;
         }
@@ -750,16 +748,16 @@ std::vector<std::tuple<pole_based_localization::Pole, std::vector<std::tuple<dou
         sqlite3_bind_double(stmt, 3, x2);
         sqlite3_bind_double(stmt, 4, y2);
 
-        sqlite3_bind_double(stmt, 5, x1);
-        sqlite3_bind_double(stmt, 6, y1);
-        sqlite3_bind_double(stmt, 7, x2);
-        sqlite3_bind_double(stmt, 8, y2);
+        //sqlite3_bind_double(stmt, 5, x1);
+        //sqlite3_bind_double(stmt, 6, y1);
+       // sqlite3_bind_double(stmt, 7, x2);
+       // sqlite3_bind_double(stmt, 8, y2);
         //ROS_ERROR_STREAM("from " << std::fixed << coords.x - mMaxSearchRadius << ", " << std::fixed << coords.x + mMaxSearchRadius << " to " << std::fixed << coords.y - mMaxSearchRadius << ", " << std::fixed << coords.y + mMaxSearchRadius);
-        //sqlite3_bind_int(stmt, 5, trackedPole.type);
-        sqlite3_bind_int(stmt, 9, trackedPole.type);
+        sqlite3_bind_int(stmt, 5, trackedPole.type);
+     //   sqlite3_bind_int(stmt, 9, trackedPole.type);
 
         int i=5;
-     /*   for(auto it = dists.begin(); it != dists.end(); ++it)
+        for(auto it = dists.begin(); it != dists.end(); ++it)
         {
             if(isEdge && poleMap.find(it->first) == poleMap.end())
                 continue;
@@ -769,7 +767,7 @@ std::vector<std::tuple<pole_based_localization::Pole, std::vector<std::tuple<dou
             sqlite3_bind_double(stmt, ++i, it->second.first + dist_tol);
 
             double minAngle = it->second.second - ((angleTolerance*10.0)/it->second.first), maxAngle = it->second.second + ((angleTolerance*10.0)/it->second.first);
-            sqlite3_bind_double(stmt, ++i, -minAngle);
+            sqlite3_bind_double(stmt, ++i, minAngle);
             sqlite3_bind_double(stmt, ++i, maxAngle);
 
             if(isEdge){
@@ -788,9 +786,9 @@ std::vector<std::tuple<pole_based_localization::Pole, std::vector<std::tuple<dou
 
         ++i;
         int paramsPerPrint = isEdge ? 8 : 4;
-        for(; i<5+mMaxDists*paramsPerPrint; ++i){
+        for(; i<6+mMaxDists*paramsPerPrint; ++i){
             sqlite3_bind_double(stmt, i, -5.0);
-        }*/
+        }
 
         std::string curPole = "";
         unsigned int distCount = 0;
@@ -801,7 +799,8 @@ std::vector<std::tuple<pole_based_localization::Pole, std::vector<std::tuple<dou
             elapsed_seconds = std::chrono::system_clock::now()-afterStep;
             //std::cout << "Database statement step for poleId took " << elapsed_seconds.count() * 1000 << "ms (id: " << entry.first << ")" << std::endl;
 
-            distCount = std::round(sqlite3_column_double(stmt, 0) * 1000); // TODO HACK FOR TESTING
+            //distCount = std::round(sqlite3_column_double(stmt, 0) * 1000); // TODO HACK FOR TESTING
+            distCount = sqlite3_column_int(stmt, 0);
             if(curPole.compare(reinterpret_cast<const char*>(sqlite3_column_text(stmt, 1))) && distCount > 0)
             {
                 curPole = reinterpret_cast<const char*>(sqlite3_column_text(stmt, 1));