small bugfix and higher variance

src/LidarOdometry.cpp

@@ -202,7 +202,7 @@ void LidarOdometry::callbackFeatures(const pole_based_localization::PoleArray& p
     if(mPoleMatchingPublisher.getNumSubscribers())
     {
         PoleMatchArray matchArray;
-        matchArray.header.frame_id = "map";
+        matchArray.header.frame_id = "utm";
         matchArray.header.stamp = ros::Time::now();
         for(auto match : matchings)
         {
@@ -382,12 +382,12 @@ void LidarOdometry::insertPrints(std::map<long, Pole> &poleMap, FingerprintMap &
         geometry_msgs::Point utmpos = map2UTM(pole.pose.position, pole.header.stamp);        
 
         if(isEdgeType(pole)){            
-            geometry_msgs::Point start, end;
-            start.x = pole.pose.position.x;
-            start.y = pole.pose.position.y;
-            end = edgeToPoints(pole).second;
+            geometry_msgs::Point end;
+            Pole utmPole = pole;
+            utmPole.pose.position = utmpos;
+            end = edgeToPoints(utmPole).second;
 
-            geometry_msgs::Point utmEndpos = map2UTM(end, pole.header.stamp);
+            //geometry_msgs::Point utmEndpos = map2UTM(end, pole.header.stamp);
             for(auto print : fingerprints.at(id)){
                 if(lastPoleMap.find(id) != lastPoleMap.end() && lastPoleMap.find(print.first) != lastPoleMap.end())
                     continue;
@@ -403,8 +403,8 @@ void LidarOdometry::insertPrints(std::map<long, Pole> &poleMap, FingerprintMap &
                 sqlite3_bind_int64(mWallInsertStmt, 1, id);
                 sqlite3_bind_double(mWallInsertStmt, 2, utmpos.x);
                 sqlite3_bind_double(mWallInsertStmt, 3, utmpos.y);
-                sqlite3_bind_double(mWallInsertStmt, 4, utmEndpos.x);
-                sqlite3_bind_double(mWallInsertStmt, 5, utmEndpos.y);
+                sqlite3_bind_double(mWallInsertStmt, 4, end.x);
+                sqlite3_bind_double(mWallInsertStmt, 5, end.y);
                 sqlite3_bind_int64(mWallInsertStmt, 6, print.first);
                 sqlite3_bind_double(mWallInsertStmt, 7, print.second.first);
                 sqlite3_bind_double(mWallInsertStmt, 8, print.second.second);
@@ -413,8 +413,8 @@ void LidarOdometry::insertPrints(std::map<long, Pole> &poleMap, FingerprintMap &
                 sqlite3_bind_int(mWallInsertStmt, 11, pole.type);
                 sqlite3_bind_double(mWallInsertStmt, 12, utmpos.x);
                 sqlite3_bind_double(mWallInsertStmt, 13, utmpos.y);
-                sqlite3_bind_double(mWallInsertStmt, 14, utmEndpos.x);
-                sqlite3_bind_double(mWallInsertStmt, 15, utmEndpos.y);
+                sqlite3_bind_double(mWallInsertStmt, 14, end.x);
+                sqlite3_bind_double(mWallInsertStmt, 15, end.y);
 
                 //ROS_ERROR_STREAM("Inserting wall " << id << " angle " << print.second.second);
 
@@ -565,14 +565,17 @@ 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)){
-        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)));
-    }
+//    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)));
+//    }
 
     if(pole1.header.frame_id == "map")
         p1UTM = map2UTM(pole1.pose.position, pole1.header.stamp);
@@ -1048,9 +1051,9 @@ std::array<double, 3> LidarOdometry::calcVariances(const std::vector<std::pair<P
 
     std::array<double, 3> res;
     measurementUncertainty = measurementUncertainty + std::min(mMaxSearchRadius/2.0, 0.3/(matchings.size()+1.0));
-    res[0] = var.x + measurementUncertainty;
-    res[1] = var.y + measurementUncertainty;
-    res[2] = std::min(0.00001, varZ + measurementUncertainty/10.0);
+    res[0] = var.x + measurementUncertainty * 2.0;
+    res[1] = var.y + measurementUncertainty * 2.0;
+    res[2] = std::min(0.00001, varZ + measurementUncertainty/10.0) * 2.0;
     return res;
 }