diff --git a/src/LidarOdometry.cpp b/src/LidarOdometry.cpp
index a8f19518d53fd5bbf38a0b0f44426efee30e892c..0ac1f82799be35d76b7ad1a6cdd39ef597c8a1ab 100644
--- a/src/LidarOdometry.cpp
+++ b/src/LidarOdometry.cpp
@@ -114,7 +114,7 @@ void LidarOdometry::onInit()
     debug_wall_query << ")";
 
     std::ostringstream wallquery2;
-    wallquery2 << "Select DISTINCT d, id, type as why, x1, y1, angle, x2, y2 FROM (SELECT Distance(Geometry, MakeLine(MakePoint(?,?,32633),MakePoint(?,?,32633))) as d, id, dist, angle, type, x1, y1, x2, y2, Geometry FROM fingerprintsWalls WHERE fingerprintsWalls.ROWID IN (SELECT ROWID FROM SpatialIndex WHERE f_table_name='fingerprintsWalls' AND search_frame=BuildMbr(?,?,?,?,32633)) AND type = ? AND d < 5.0 GROUP BY id, dist, angle, x1, y1, x2, y2, type) as t1 GROUP BY id, type, x1, y1, x2, y2 ORDER BY d ASC;";
+    wallquery2 << "Select DISTINCT 1.0 - d, id, type as why, x1, y1, angle, x2, y2 FROM (SELECT Distance(Geometry, MakeLine(MakePoint(?,?,32633),MakePoint(?,?,32633))) as d, id, dist, angle, type, x1, y1, x2, y2, Geometry FROM fingerprintsWalls WHERE fingerprintsWalls.ROWID IN (SELECT ROWID FROM SpatialIndex WHERE f_table_name='fingerprintsWalls' AND search_frame=BuildMbr(?,?,?,?,32633)) AND type = ? AND d < 1.0 GROUP BY id, dist, angle, x1, y1, x2, y2, type) as t1 GROUP BY id, type, x1, y1, x2, y2 ORDER BY d ASC;";
 
 
 //    ROS_ERROR_STREAM("sql: " << query.str());
@@ -399,6 +399,8 @@ void LidarOdometry::insertPrints(std::map<long, Pole> &poleMap, FingerprintMap &
                 geometry_msgs::Point otherutmpos;
                 if(isEdgeType(other))
                     otherutmpos = map2UTM(other.pose.position, other.header.stamp);
+                else
+                    continue;
 
 
                 sqlite3_bind_int64(mWallInsertStmt, 1, id);
@@ -566,18 +568,29 @@ double LidarOdometry::calcPointEdgeAngle(const pole_based_localization::Pole& po
 
 double LidarOdometry::calcAngle(const Pole& pole1, const Pole& pole2) const
 {
-    if(isEdgeType(pole1) && isEdgeType(pole2)) return pole1.pose.orientation.z; // for walls just use their own angle
+    geometry_msgs::Point other, p1UTM = pole1.pose.position, p2UTM = pole2.pose.position;
 
-    if(isEdgeType(pole1) || isEdgeType(pole2))
-        return calcPointEdgeAngle(pole1, pole2);
+    if(isEdgeType(pole1) && isEdgeType(pole2)){
+        auto points = edgeToPoints(pole1);
+        geometry_msgs::Point startUTM = map2UTM(points.first, pole1.header.stamp);
+        geometry_msgs::Point endUTM = map2UTM(points.second, pole1.header.stamp);
+        return calcAngle((tf2::Vector3(endUTM.x-startUTM.x, endUTM.y-startUTM.y, 0.0)));
+    }
 
-    geometry_msgs::Point other, p1UTM = pole1.pose.position, p2UTM = pole2.pose.position;
     if(pole1.header.frame_id == "map")
         p1UTM = map2UTM(pole1.pose.position, pole1.header.stamp);
     if(pole2.header.frame_id == "map")
         p2UTM = map2UTM(pole2.pose.position, pole2.header.stamp);
     other.x = p2UTM.x - p1UTM.x;
     other.y = p2UTM.y - p1UTM.y;
+
+    Pole pole1UTM = pole1, pole2UTM = pole2;
+    pole1UTM.pose.position = p1UTM;
+    pole2UTM.pose.position = p2UTM;
+
+    if(isEdgeType(pole1) || isEdgeType(pole2))
+        return calcPointEdgeAngle(pole1UTM, pole2UTM);
+
     double result = std::acos(other.y/(std::sqrt(std::pow(other.x, 2.0) + std::pow(other.y, 2.0))));
     if(other.x < 0.0) result *= -1.0;
     return result;
@@ -654,6 +667,8 @@ std::vector<std::pair<pole_based_localization::Pole, pole_based_localization::Po
             Pole trackedPole = poleMap.at(entry.first);
             trackedPole.header.frame_id = "utm";
             trackedPole.pose.position = map2UTM(trackedPole.pose.position, trackedPole.header.stamp);
+            if(isEdgeType(trackedPole)) // recalculate angle for edges in utm
+                trackedPole.pose.orientation.z = calcAngle(trackedPole, trackedPole);
             allMatches.push_back(std::make_tuple(trackedPole, std::get<1>(lastMatches[entry.first])));
         }
     }
@@ -786,7 +801,7 @@ 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 = sqlite3_column_int(stmt, 0);
+            distCount = std::round(sqlite3_column_double(stmt, 0) * 1000); // TODO HACK FOR TESTING
             if(curPole.compare(reinterpret_cast<const char*>(sqlite3_column_text(stmt, 1))) && distCount > 0)
             {
                 curPole = reinterpret_cast<const char*>(sqlite3_column_text(stmt, 1));
@@ -801,7 +816,7 @@ std::vector<std::tuple<pole_based_localization::Pole, std::vector<std::tuple<dou
                     tf2::Vector3 wallVec(sqlite3_column_double(stmt, 6)-pole.pose.position.x, sqlite3_column_double(stmt, 7) - pole.pose.position.y, 0.0);
                     pole.pose.orientation.x = wallVec.length();
                     //ROS_ERROR_STREAM("x1y1: " << std::fixed << pole.pose.position.x << " " << std::fixed << pole.pose.position.y <<  " x2y2 " << std::fixed << sqlite3_column_double(stmt, 6) << " " << std::fixed << sqlite3_column_double(stmt, 7));
-                    pole.pose.orientation.z = calcAngle(wallVec) - 0.022; //TODO hack for testing
+                    pole.pose.orientation.z = calcAngle(wallVec);
                     if(std::abs(trackedPole.pose.orientation.z-pole.pose.orientation.z) > M_PI_2/2.0) // 45 degrees orientation error? I don't think so.
                         continue;
                 }
@@ -824,6 +839,7 @@ std::vector<std::tuple<pole_based_localization::Pole, std::vector<std::tuple<dou
             Pole pole;
             pole.pose.position = coords;
             pole.pose.orientation = trackedPole.pose.orientation;
+            pole.pose.orientation.z = calcAngle(trackedPole, trackedPole);
 
             pole.ID = std::to_string(curId);
             pole.type = trackedPole.type;