implemented spatialite approach

include/LidarOdometry.h

@@ -4,6 +4,7 @@
 #include <ros/ros.h>
 #include <nodelet/nodelet.h>
 #include <tf2_ros/transform_listener.h>
+#include <sqlite3.h>
 
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
 #include <nav_msgs/Odometry.h>
@@ -20,20 +21,75 @@ namespace fub_lidar_odometry {
 
 class LidarOdometry : public nodelet::Nodelet{
 public:
+    typedef std::map<long, std::map<long, std::pair<double, double> > > FingerprintMap;
+
     LidarOdometry();
     virtual ~LidarOdometry();
     virtual void onInit() override;
 
+    struct {
+        bool operator()(const std::tuple<double, double, double, double, pole_based_localization::Pole, unsigned int, double>& i, const std::tuple<double, double, double, double, pole_based_localization::Pole, unsigned int, double>& j) const
+        {
+            // squared x,y dist is enough...
+            double iDist = std::pow(std::get<1>(i)-std::get<4>(i).pose.position.x, 2.0) + std::pow(std::get<2>(i)-std::get<4>(i).pose.position.y, 2.0);
+            double jDist = std::pow(std::get<1>(j)-std::get<4>(j).pose.position.x, 2.0) + std::pow(std::get<2>(j)-std::get<4>(j).pose.position.y, 2.0);
+            unsigned int distCounti = std::get<5>(i), distCountj = std::get<5>(j);
+            // if dist count equal sort by distance (close to far)
+            if(distCounti == distCountj)
+                return iDist <= jDist;
+
+            return std::get<5>(i) > std::get<5>(j);
+        }
+    } distCompare;
+
 protected:
     //void callbackFeatures(const fub_feature_common::Features& features);
     void callbackFeatures(const pole_based_localization::PoleArray& features);
+    void callbackPosition(const nav_msgs::Odometry &odom);
+
+    void updateFingerprints(const pole_based_localization::PoleArray &poles, FingerprintMap& fingerprints, std::map<long, pole_based_localization::Pole>& poleMap) const;
+    void cleanPolesNPrints(std::map<long, pole_based_localization::Pole> &poleMap, FingerprintMap &fingerprints) const;
+    void insertPrints(std::map<long, pole_based_localization::Pole> &poleMap, FingerprintMap& fingerprints) const;
+    std::vector<std::tuple<pole_based_localization::Pole, std::vector<std::tuple<double, double, double, double, pole_based_localization::Pole, unsigned int, double> > > > dbAssociation(const std::vector<long>::const_iterator fpbegin, const std::vector<long>::const_iterator fpend, const FingerprintMap& fpMap, const std::map<long, pole_based_localization::Pole> &poleMap, sqlite3* db, sqlite3_stmt* poleStmt, sqlite3_stmt* wallStmt) const;
 
-    ros::Subscriber mFeaturesSubscriber;
-    ros::Publisher mOdometryPublisher;
+    std::vector<std::pair<pole_based_localization::Pole, pole_based_localization::Pole> > associate(const FingerprintMap& fingerprints, const std::map<long, pole_based_localization::Pole> &poleMap) const;
+    geometry_msgs::Pose calculateOffset(const std::vector<std::pair<pole_based_localization::Pole, pole_based_localization::Pole> >& associations) const;
+    std::array<double, 3> calcVariances(const std::vector<std::pair<pole_based_localization::Pole, pole_based_localization::Pole> >& matchings) const;
+
+    double calcDistSquared(const geometry_msgs::Point &point1, const geometry_msgs::Point &point2) const;
+    double calcRangeDistSquared(const pole_based_localization::Pole &feat1, const pole_based_localization::Pole &feat2) const;
+    double calcLineDist(const pole_based_localization::Pole &line, const geometry_msgs::Point & point) const;
+    double calcDist(const pole_based_localization::Pole &pole1, const pole_based_localization::Pole &pole2) const;
+    double calcDist(const geometry_msgs::Point &point1, const geometry_msgs::Point &point2) const;
+    double calcPointEdgeAngle(const pole_based_localization::Pole &pole1, const pole_based_localization::Pole &pole2) const;
+    double calcAngle(const pole_based_localization::Pole& pole1, const pole_based_localization::Pole& pole2) const;
+    double calcAngle(const tf2::Vector3& direction) const;
+    geometry_msgs::Point getClosestPointOnLine(const pole_based_localization::Pole &line, const geometry_msgs::Point &point) const;
+    std::pair<geometry_msgs::Point, geometry_msgs::Point> edgeToPoints(const pole_based_localization::Pole& edge) const;
+
+    std::vector<long> getPolesInRange(const pole_based_localization::Pole &origin, const std::map<long, pole_based_localization::Pole>& poleMap, double range) const;
+    bool isEdgeType(const pole_based_localization::Pole& feature) const;
+    geometry_msgs::Point map2UTM(const geometry_msgs::Point &point, ros::Time stamp) const;
+
+    ros::Subscriber mFeaturesSubscriber, mPosSubscriber;
+    ros::Publisher mOdometryPublisher, mPositionDifferencePublisher,mPoleMatchingPublisher;
     std::map<std::string, pole_based_localization::Pole> mLastPoles;
     //std::map<long, fub_feature_common::Pole> mLastPoles;
     std::map<std::string, fub_feature_common::Corner> mLastCorners;
     std::map<std::string, fub_feature_common::Edge> mLastEdges;
+
+    static const unsigned short mMaxDists = 100;
+    bool mDetectWalls = true;
+    float mMaxDistTolerance = 0.05f, mMaxDistAbsTolerance = 0.8f, mMinDistAbsTolerance = 0.2;
+    float mMaxAngleTolerance = 0.02f;
+    float mMaxWallAngleTolerance = 0.02f;
+    float mMaxSearchRadius = 2.5f;
+    sqlite3* mDatabase;
+    sqlite3_stmt* mStatement, *mWallStatement;
+    geometry_msgs::Point mLastPos;
+
+    tf2_ros::Buffer mTfBuffer;
+    tf2_ros::TransformListener mTfListener;
 };
 }
 

src/LidarOdometry.cpp

 #include "LidarOdometry.h"
+
+#include <chrono>
+
+#include <std_msgs/Header.h>
+#include <geometry_msgs/PointStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
 #include <pcl_ros/point_cloud.h>
 #include <pcl/registration/transformation_estimation_svd.h>
 #include <pcl/registration/correspondence_rejection_sample_consensus.h>
@@ -6,6 +14,8 @@
 #include <pcl/common/transformation_from_correspondences.h>
 #include <pcl/common/poses_from_matches.h>
 
+#include <pole_based_localization/PoleMatchArray.h>
+
 /* -------------------------------------------------------------------------- */
 
 /** Lidar odometry for velodyne sensors
@@ -14,25 +24,174 @@
  */
 
 namespace fub_lidar_odometry{
+using namespace pole_based_localization;
 
-LidarOdometry::LidarOdometry()
+LidarOdometry::LidarOdometry() : mTfListener(mTfBuffer)
 {}
 
 LidarOdometry::~LidarOdometry()
 {    
+    sqlite3_finalize(mStatement);
+    sqlite3_finalize(mWallStatement);
+    sqlite3_close(mDatabase);
 }
 
 void LidarOdometry::onInit()
 {
     ros::NodeHandle nh;    
-  //  mFeaturesSubscriber = nh.subscribe(nh.resolveName("/pole_based_localization/detected_features"), 11, &LidarOdometry::callbackFeatures, this);
-    mOdometryPublisher = nh.advertise<nav_msgs::Odometry>("/localization/lidar_odom",1);
-    mFeaturesSubscriber = nh.subscribe(nh.resolveName("/pole_recognition/rawDetections"), 11, &LidarOdometry::callbackFeatures, this);
+   // mOdometryPublisher = nh.advertise<nav_msgs::Odometry>("/localization/lidar_odom",1);
+
+    mPositionDifferencePublisher = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("/lidar_odometry/PositionDifference",1);
+    mPoleMatchingPublisher = nh.advertise<PoleMatchArray>("/lidar_odometry/PoleMatchings",1);
+
+
+    sqlite3* db;
+    // Open database connection
+    if(sqlite3_open_v2("file::memory:", &db, SQLITE_OPEN_URI | SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, NULL))
+        ROS_ERROR_STREAM("Can't open database: " << sqlite3_errmsg(db));
+
+    // Open Spatialite extension
+    sqlite3_enable_load_extension(db, 1);
+    if(sqlite3_exec(db, "SELECT load_extension('mod_spatialite.so'); SELECT InitSpatialMetaData();", NULL, NULL, NULL))
+        ROS_ERROR_STREAM("Can't load spatialite lib: " << sqlite3_errmsg(db));
+
+    mDatabase = db;
+
+    sqlite3_exec(db, "DROP TABLE fingerprints", NULL, NULL, NULL);
+    sqlite3_exec(db, "DROP TABLE fingerprintsWalls", NULL, NULL, NULL);
+
+    // Prepare Database
+    const std::string printTableCreate = "CREATE TABLE 'fingerprints' ('poleId' INTEGER, 'x' DOUBLE, 'y' DOUBLE, 'z' DOUBLE, 'otherId' INTEGER, 'dist' DOUBLE, 'angle' DOUBLE, 'type' INTEGER )";
+    const std::string wallPrintTableCreate = "CREATE TABLE 'fingerprintsWalls' ( 'id' INTEGER, 'x1' DOUBLE, 'y1' DOUBLE, 'x2' DOUBLE, 'y2' DOUBLE, 'otherId' TEXT, 'dist' DOUBLE, 'angle' DOUBLE, 'oX' DOUBLE, 'oY' DOUBLE, 'type' INTEGER)";
+    if(sqlite3_exec(db, printTableCreate.c_str(), NULL, NULL, NULL))
+        ROS_ERROR_STREAM("Could not create fingerprints table: " << sqlite3_errmsg(db));
+
+    if(sqlite3_exec(db, wallPrintTableCreate.c_str(), NULL, NULL, NULL))
+        ROS_ERROR("Could not create fingerprints walls table");
+
+    if(sqlite3_exec(db, "SELECT AddGeometryColumn ('fingerprints', 'Geometry', 32633, 'POINT', 2);", NULL, NULL, NULL))
+        ROS_ERROR("Could not create fingerprints geometry column");
+
+    if(sqlite3_exec(db, "SELECT AddGeometryColumn ('fingerprintsWalls', 'Geometry', 32633, 'LINESTRING', 2);", NULL, NULL, NULL))
+        ROS_ERROR("Could not create fingerprints walls geomtry column");
+
+    sqlite3_exec(db, "SELECT CreateSpatialIndex('fingerprints', 'Geometry')", NULL, NULL, NULL);
+    sqlite3_exec(db, "SELECT CreateSpatialIndex('fingerprintsWalls', 'Geometry')", NULL, NULL, NULL);
+    sqlite3_exec(db, "CREATE INDEX dist_angle ON 'fingerprints' (dist,angle)", NULL, NULL, NULL);
+    sqlite3_exec(db, "CREATE INDEX dist_angle_walls ON 'fingerprintsWalls' (dist,angle)", NULL, NULL, NULL);
+
+    // Build prepared statements
+    std::ostringstream query, wallquery, debug_query, debug_wall_query;
+    query << "SELECT count(poleID) as c, poleID, type as why, x, y, angle FROM (SELECT poleID, dist, angle, type, x, y FROM fingerprints WHERE fingerprints.ROWID IN (SELECT ROWID FROM SpatialIndex WHERE f_table_name='fingerprints' AND search_frame=BuildMbr(?,?,?,?,32633)) AND type = ? AND ((dist BETWEEN ? AND ?) AND (angle BETWEEN ? AND ?)";
+    wallquery << "SELECT count(id) as c, id, type as why, x1, y1, angle, x2, y2 FROM (SELECT id, dist, angle, type, x1, y1, x2, y2 FROM fingerprintsWalls WHERE fingerprintsWalls.ROWID IN (SELECT ROWID FROM SpatialIndex WHERE f_table_name='fingerprintsWalls' AND search_frame=BuildMbr(?,?,?,?,32633)) AND type = ? AND ((dist BETWEEN ? AND ? AND angle BETWEEN ? AND ? AND (oX BETWEEN ? AND ? AND oY BETWEEN ? AND ?))";
+    debug_query << "SELECT otherId,dist,angle FROM fingerprints where poleId = ? AND ((dist BETWEEN ? AND ? AND angle BETWEEN ? AND ?)";
+    debug_wall_query << "SELECT otherId,dist,angle,oX,oY FROM fingerprintsWalls where id = ? AND ((dist BETWEEN ? AND ? AND angle BETWEEN ? AND ? AND oX BETWEEN ? AND ? AND oY BETWEEN ? AND ?)";
+
+    for(int i=1; i < mMaxDists; ++i){
+        query << " OR (dist BETWEEN ? AND ? AND angle BETWEEN ? AND ?)";
+        wallquery << " OR (dist BETWEEN ? AND ? AND angle BETWEEN ? AND ? AND oX BETWEEN ? AND ? AND oY BETWEEN ? AND ?)";
+        debug_query << " OR (dist BETWEEN ? AND ? AND angle BETWEEN ? AND ?)";
+        debug_wall_query << " OR (dist BETWEEN ? AND ? AND angle BETWEEN ? AND ? AND oX BETWEEN ? AND ? AND oY BETWEEN ? AND ?)";
+
+        query << ") GROUP BY poleID, dist, angle, x, y, type) as t1 GROUP BY poleID, type, x, y HAVING count(poleID) > 3 ORDER BY c DESC LIMIT 10;";
+        wallquery << ") GROUP BY id, dist, angle, x1, y1, x2, y2, type) as t1 GROUP BY id, type, x1, y1, x2, y2 HAVING count(id) > 3 ORDER BY c DESC LIMIT 10;";
+        debug_query << ")";
+        debug_wall_query << ")";
+
+    }
+
+    sqlite3_stmt* poleStmt = NULL, *wallStmt = NULL;
+    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, wallquery.str().c_str(), -1, SQLITE_PREPARE_PERSISTENT, &wallStmt, 0))){
+        mDetectWalls = false;
+        ROS_ERROR_STREAM("ERROR while preparing statment: " << sqlite3_errmsg(db));
+    }
+
+        mStatement = poleStmt;
+        mWallStatement = wallStmt;
+
+    ROS_ERROR("Finished init");
+
+    mFeaturesSubscriber = nh.subscribe(nh.resolveName("/pole_recognition/trackedPolesArray"), 11, &LidarOdometry::callbackFeatures, this);
+    mPosSubscriber = nh.subscribe(nh.resolveName("/localization/odometry/filtered_map"), 1, &LidarOdometry::callbackPosition, this);
 }
 
 
-void LidarOdometry::callbackFeatures(const pole_based_localization::PoleArray& features)
+void LidarOdometry::callbackPosition(const nav_msgs::Odometry &odom)
+{
+    mLastPos = odom.pose.pose.position;
+    //mLastSpeed = odom.twist.twist.linear;
+}
+
+
+void LidarOdometry::callbackFeatures(const pole_based_localization::PoleArray& poles)
+{
+
+    static FingerprintMap fingerprints;
+    static std::map<long, Pole> poleMap;
+    auto start = std::chrono::system_clock::now();
+    updateFingerprints(poles, fingerprints, poleMap);
+    auto afterPrints = std::chrono::system_clock::now();
+    std::chrono::duration<double> elapsed_seconds = afterPrints-start;
+    ROS_ERROR_STREAM("Print update took " <<  elapsed_seconds.count() * 1000 << "ms\n");
+
+    auto matchings = associate(fingerprints, poleMap);
+
+    auto endDetection = std::chrono::system_clock::now();
+    elapsed_seconds = endDetection-afterPrints;
+    //ROS_ERROR_STREAM("Found " << matchings.size() << " matchings in " << elapsed_seconds.count() * 1000 << "ms\n");
+
+    geometry_msgs::Pose offset;
+
+    if(matchings.size() >= 3){
+       offset = calculateOffset(matchings);
+    }
+    //ROS_ERROR_STREAM("Position difference is: (" << offset.position.x << ", " << offset.position.y << ")");
+    auto var = calcVariances(matchings);
+
+    // Publish position difference
+    if(mPositionDifferencePublisher.getNumSubscribers())
+    {
+        geometry_msgs::PoseWithCovarianceStamped ps;
+        boost::array<double, 36> cov;
+        for(int i=0; i<36; ++i) cov[i] = 0.0;
+        cov[0] = var[0];
+        cov[7] = var[1];
+        cov[35] = var[2];
+        ps.header.stamp = poles.header.stamp;
+        ps.header.frame_id = "map";
+        ps.pose.pose = offset;
+        ps.pose.covariance = cov;
+        mPositionDifferencePublisher.publish(ps);
+    }
+
+    // Publish pole match array
+    if(mPoleMatchingPublisher.getNumSubscribers())
     {
+        PoleMatchArray matchArray;
+        matchArray.header.frame_id = "map";
+        matchArray.header.stamp = ros::Time::now();
+        for(auto match : matchings)
+        {
+            auto now = ros::Time::now();
+            PoleMatch poleMatch;
+            poleMatch.header.frame_id = "utm";
+            poleMatch.header.stamp = poles.header.stamp;
+            poleMatch.local = match.first;
+            poleMatch.remote = match.second;
+            poleMatch.local.header.frame_id = "utm";
+            poleMatch.local.header.stamp = now;
+            poleMatch.remote.header.frame_id = "utm";
+            poleMatch.remote.header.stamp = now;
+            matchArray.matches.push_back(poleMatch);
+        }
+        mPoleMatchingPublisher.publish(matchArray);
+    }
+
+    //ROS_ERROR_STREAM("fp association of " << matchings.size() << " took " << es)
+
 /*
     // get own position
     std_msgs::Header header;
@@ -54,7 +213,7 @@ void LidarOdometry::callbackFeatures(const pole_based_localization::PoleArray& f
 
     geometry_msgs::PointStamped psIn, posUTM;
     psIn.header = header;
-    tf2::doTransform(psIn, posUTM, transformStamped);*/
+    tf2::doTransform(psIn, posUTM, transformStamped);
 
     pcl::PointCloud<pcl::PointXYZ> cloud_old;
     pcl::PointCloud<pcl::PointXYZ> cloud_new;
@@ -108,9 +267,772 @@ void LidarOdometry::callbackFeatures(const pole_based_localization::PoleArray& f
     odom.pose.covariance[7] += 0.1;
     odom.pose.covariance[35] += 0.1;
     mOdometryPublisher.publish(odom);
-    mLastPoles = newPoles;
+    mLastPoles = newPoles;*/
+}
+
+
+void LidarOdometry::updateFingerprints(const PoleArray &poles, FingerprintMap& fingerprints, std::map<long, Pole>& poleMap) const
+{
+    // add new poles
+    for(Pole pole : poles.poles){
+        if((pole.life >= 60 || poleMap.find(std::stol(pole.ID)) != poleMap.end()))
+            poleMap[std::stol(pole.ID)] = pole;
+    }
+
+    // remove old poles and prints
+    cleanPolesNPrints(poleMap, fingerprints);
+
+    // insert unupdated prints into db
+    insertPrints(poleMap, fingerprints);
+
+    // update fingerprints for all affected poles
+    for(Pole pole : poles.poles)
+    {
+        if(pole.life <= 60 || calcDistSquared(pole.pose.position, mLastPos) > 10000.0)
+            continue;
+
+        std::vector<long> polesInRange = getPolesInRange(pole, poleMap, 100.0);
+        for(long otherID : polesInRange)
+        {
+            if(otherID == std::stol(pole.ID)) continue; // Don't fingerprint self
+
+            Pole other = poleMap.at(otherID);
+            double dist = calcDist(pole, other);
+            double angle = calcAngle(pole, other);
+            double otherAngle = calcAngle(other, pole);
+
+            fingerprints[std::stol(pole.ID)][otherID] = std::make_pair(dist, angle);
+            fingerprints[otherID][std::stol(pole.ID)] = std::make_pair(dist, otherAngle);
+        }
+    }
+}
+
+
+void LidarOdometry::cleanPolesNPrints(std::map<long, Pole> &poleMap, FingerprintMap &fingerprints) const
+{
+    std::vector<long> toDelete;
+
+    for(auto entry : poleMap)
+    {
+        Pole pole = entry.second;
+
+        if(pole.life <= 0 || calcDistSquared(pole.pose.position, mLastPos) > 16900.0 || ros::Time::now() - pole.header.stamp > ros::Duration(15))
+        {
+            if(calcDistSquared(pole.pose.position, mLastPos) > 90000.0)
+                toDelete.push_back(entry.first);
+            fingerprints.erase(entry.first);
+        }
+    }
+
+    for(long ID : toDelete)
+        poleMap.erase(ID);
+}
+
+
+void LidarOdometry::insertPrints(std::map<long, Pole> &poleMap, FingerprintMap &fingerprints) const
+{
+    sqlite3_stmt* poleStmt = NULL, *wallStmt = NULL;
+
+    // Clean tables
+    sqlite3_exec(mDatabase, "DELETE FROM fingerprints", NULL, NULL, NULL);
+    sqlite3_exec(mDatabase, "DELETE FROM fingerprintsWalls", NULL, NULL, NULL);
+
+    if((sqlite3_prepare_v3(mDatabase, "INSERT INTO fingerprints (poleid,x,y,z,otherId,dist,angle,type,Geometry) VALUES (?,?,?,?,?,?,?,?,MakePoint(?,?,32633))", -1, SQLITE_PREPARE_PERSISTENT, &poleStmt, 0)))
+        ROS_ERROR_STREAM("ERROR while preparing statment: " << sqlite3_errmsg(mDatabase));
+    if((sqlite3_prepare_v3(mDatabase, "INSERT INTO fingerprintsWalls (id,x1,y1,x2,y2,otherId,dist,angle,oX,oY,type,Geometry) VALUES (?,?,?,?,?,?,?,?,?,?,?,MakeLine(MakePoint(?,?,32633),MakePoint(?,?,32633)))", -1, SQLITE_PREPARE_PERSISTENT, &wallStmt, 0)))
+        ROS_ERROR_STREAM("ERROR while preparing statment: " << sqlite3_errmsg(mDatabase));
+
+    for(auto entry : poleMap){
+        long id = entry.first;
+        Pole pole = entry.second;
+
+        geometry_msgs::Point utmpos = map2UTM(pole.pose.position, pole.header.stamp);
+
+        if(isEdgeType(pole)){
+            // TODO...
+            continue;
+        }
+        else{
+            if(fingerprints.find(id) == fingerprints.end())
+                continue;
+
+            for(auto print : fingerprints.at(id)){
+                sqlite3_bind_int(poleStmt, 1, id);
+                sqlite3_bind_double(poleStmt, 2, utmpos.x);
+                sqlite3_bind_double(poleStmt, 3, utmpos.y);
+                sqlite3_bind_double(poleStmt, 4, 0.0);
+                sqlite3_bind_int(poleStmt, 5, print.first);
+                sqlite3_bind_double(poleStmt, 6, print.second.first);
+                sqlite3_bind_double(poleStmt, 7, print.second.second);
+                sqlite3_bind_int(poleStmt, 8, pole.type);
+                sqlite3_bind_double(poleStmt, 9, utmpos.x);
+                sqlite3_bind_double(poleStmt, 10, utmpos.y);
+
+                sqlite3_step(poleStmt);
+
+                sqlite3_clear_bindings(poleStmt);
+                sqlite3_reset(poleStmt);
+            }
+        }
+    }
+}
+
+
+double LidarOdometry::calcDistSquared(const geometry_msgs::Point &point1, const geometry_msgs::Point &point2) const
+{
+    return std::pow(point1.x - point2.x, 2.0) + std::pow(point1.y - point2.y, 2.0);// + pow(point1.z - point2.z, 2.0)); no z right now
+}
+
+
+std::vector<long> LidarOdometry::getPolesInRange(const Pole &origin, const std::map<long, Pole> &poleMap, double range) const
+{
+    std::vector<long> result;
+    for(auto entry : poleMap){
+        Pole feat = entry.second;
+        if(calcRangeDistSquared(origin, feat) < (range*range))
+            result.push_back(entry.first);
+    }
+
+    return result;
+}
+
+
+double LidarOdometry::calcRangeDistSquared(const Pole &feat1, const Pole &feat2) const
+{
+    // Any walls or lines involved?
+    if(feat1.type == pole_based_localization::Pole::TYPE_WALL || feat2.type == pole_based_localization::Pole::TYPE_WALL ||
+       feat1.type == pole_based_localization::Pole::TYPE_LANELINE || feat2.type == pole_based_localization::Pole::TYPE_LANELINE){
+        // Both walls
+        if(feat1.type == feat2.type){
+            geometry_msgs::Point feat1Start = feat1.pose.position;
+            geometry_msgs::Point feat1End = feat1.pose.position;
+            feat1End.x += feat1.pose.orientation.x * -std::sin(-feat1.pose.orientation.z);
+            feat1End.y += feat1.pose.orientation.x * std::cos(-feat1.pose.orientation.z);
+
+            geometry_msgs::Point feat2Start = feat2.pose.position;
+            geometry_msgs::Point feat2End = feat2.pose.position;
+            feat2End.x += feat2.pose.orientation.x * -std::sin(-feat2.pose.orientation.z);
+            feat2End.y += feat2.pose.orientation.x * std::cos(-feat2.pose.orientation.z);
+
+            double feat1StartDist = std::min(calcDistSquared(feat1Start, feat2Start), calcDistSquared(feat1Start, feat2End));
+            double feat1EndDist = std::min(calcDistSquared(feat1End, feat2Start), calcDistSquared(feat1End, feat2End));
+
+            return std::min(feat1StartDist, feat1EndDist);
+        }
+        else{
+            // only one wall/line
+            const Pole& wall = feat1.type == pole_based_localization::Pole::TYPE_WALL || feat1.type == pole_based_localization::Pole::TYPE_LANELINE ? feat1 : feat2;
+            const Pole& feat = feat1.type == pole_based_localization::Pole::TYPE_WALL || feat1.type == pole_based_localization::Pole::TYPE_LANELINE ? feat2 : feat1;
+
+            geometry_msgs::Point wallStart = wall.pose.position;
+            geometry_msgs::Point wallEnd = wall.pose.position;
+            double wallLength = wall.pose.orientation.x;
+            wallEnd.x += wallLength * -std::sin(-wall.pose.orientation.z);
+            wallEnd.y += wallLength * std::cos(-wall.pose.orientation.z);
+
+            double startDist = calcDistSquared(feat.pose.position, wallStart);
+            double endDist = calcDistSquared(feat.pose.position, wallEnd);
+
+            // If the feature is in between the wall start and end, take the line dist
+            if(startDist < wallLength && endDist < wallLength)
+                return calcLineDist(wall, feat.pose.position);
+
+            // otherwise take the smaller dist to wall start or end
+            return std::min(startDist, endDist);
+        }
+    }
+
+    return calcDistSquared(feat1.pose.position, feat2.pose.position);
+}
+
+
+double LidarOdometry::calcLineDist(const Pole &line, const geometry_msgs::Point & point) const
+{
+    if(line.type != pole_based_localization::Pole::TYPE_WALL && line.type != pole_based_localization::Pole::TYPE_LANELINE){
+        ROS_ERROR("Got a non line feature as line in calcLineDist!");
+        return 0.0;
+    }
+    geometry_msgs::Point lineStart = line.pose.position;
+    geometry_msgs::Point lineEnd = line.pose.position;
+    lineEnd.x += -std::sin(-line.pose.orientation.z);
+    lineEnd.y += std::cos(-line.pose.orientation.z);
+
+    //https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line
+    return std::abs((lineEnd.y-lineStart.y)*point.x - (lineEnd.x-lineStart.x)*point.y + lineEnd.x*lineStart.y - lineEnd.y*lineStart.x)/std::sqrt(std::pow(lineEnd.y-lineStart.y, 2.0) + std::pow(lineEnd.x-lineStart.x, 2.0));
+}
+
+
+double LidarOdometry::calcPointEdgeAngle(const pole_based_localization::Pole& pole1, const pole_based_localization::Pole& pole2) const
+{
+    const Pole& line = isEdgeType(pole1) ? pole1 : pole2;
+    const Pole& point= isEdgeType(pole1) ? pole2 : pole1;
+
+    auto linePoints = edgeToPoints(line);
+    tf2::Vector3 lineVec(linePoints.second.x - linePoints.first.x, linePoints.second.y - linePoints.first.y, 0.0);
+    tf2::Vector3 pointVec(point.pose.position.x - linePoints.first.x, point.pose.position.y - linePoints.first.y, 0.0);
+
+    lineVec.normalize();
+
+    auto proj = pointVec.dot(lineVec);
+    tf2::Vector3 v_proj(proj * lineVec.x() + linePoints.first.x, proj * lineVec.y() + linePoints.first.y, 0.0);
+    tf2::Vector3 rej(v_proj.x() - point.pose.position.x, v_proj.y() - point.pose.position.y, 0.0);
+
+    double angle = std::acos(rej.y()/rej.length());
+    if(!isEdgeType(pole2)){ // Check
+        angle -= M_PI;
+        angle = angle < -M_PI ? angle + 2*M_PI : angle;
+    }
+
+    return rej.x() < 0 ? angle * -1 : angle;
+}
+
+
+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
+
+    if(isEdgeType(pole1) || isEdgeType(pole2))
+        return calcPointEdgeAngle(pole1, pole2);
+
+    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;
+    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;
+}
+
+
+double LidarOdometry::calcAngle(const tf2::Vector3& direction) const
+{
+    auto adjustedDir = direction.x() < 0.0 ? tf2::Vector3(direction.x()*-1.0,direction.y()*-1.0,direction.z()*-1.0) : direction;
+    return std::acos(adjustedDir.y()/std::hypot(adjustedDir.x(), adjustedDir.y()));
+}
+
+
+
+double LidarOdometry::calcDist(const Pole &pole1, const Pole &pole2) const
+{
+    if(pole1.type != pole_based_localization::Pole::TYPE_WALL && pole2.type != pole_based_localization::Pole::TYPE_WALL &&
+            pole1.type != pole_based_localization::Pole::TYPE_LANELINE && pole2.type != pole_based_localization::Pole::TYPE_LANELINE) // no walls or lines
+        return calcDist(pole1.pose.position, pole2.pose.position);
+
+    // two walls: For two walls their fingerprint dist is actually their angle (I'm sorry)
+    if(pole1.type == pole2.type)
+        return std::abs(pole1.pose.orientation.z - pole2.pose.orientation.z);
+
+    // wall to pole
+    if(pole1.type == pole_based_localization::Pole::TYPE_WALL || pole1.type == pole_based_localization::Pole::TYPE_LANELINE)
+        return calcLineDist(pole1, pole2.pose.position);
+    else // pole to wall (actually the same as wall to pole)
+        return calcLineDist(pole2, pole1.pose.position);
+}
+
+
+double LidarOdometry::calcDist(const geometry_msgs::Point &point1, const geometry_msgs::Point &point2) const
+{
+    return std::sqrt(std::pow(point1.x - point2.x, 2.0) + std::pow(point1.y - point2.y, 2.0));// + pow(point1.z - point2.z, 2.0)); no z right now
+}
+
+
+std::pair<geometry_msgs::Point, geometry_msgs::Point> LidarOdometry::edgeToPoints(const pole_based_localization::Pole& edge) const
+{
+    geometry_msgs::Point p1 = edge.pose.position, p2;
+
+    p2.x = p1.x + (edge.pose.orientation.x * -std::sin(-edge.pose.orientation.z));
+    p2.y = p1.y + (edge.pose.orientation.x * std::cos(-edge.pose.orientation.z));
+
+    if(p1.x > p2.x){
+        geometry_msgs::Point p3 = p1;
+        p1 = p2;
+        p2 = p3;
+    }
+
+    return std::make_pair(p1,p2);
+}
+
+
+std::vector<std::pair<pole_based_localization::Pole, pole_based_localization::Pole> > LidarOdometry::associate(const FingerprintMap& fingerprints, const std::map<long, Pole> &poleMap) const
+{
+    auto start = std::chrono::system_clock::now();
+
+    static std::map<long, ros::Time> trackedMatches;
+    static std::map<long, std::tuple<Pole, std::vector<std::tuple<double, double, double, double, Pole, unsigned int, double> > > > lastMatches;
+    std::vector<std::tuple<Pole, std::vector<std::tuple<double, double, double, double, Pole, unsigned int, double> > > > allMatches;
+
+    std::vector<std::pair<pole_based_localization::Pole, pole_based_localization::Pole> > associated;
+
+    std::vector<std::tuple<Pole, std::vector<std::tuple<double, double, double, double, Pole, unsigned int, double> > > > results;
+
+    std::vector<long> toMatch;
+
+    for(auto entry : fingerprints){
+        if(trackedMatches.find(entry.first) == trackedMatches.end() || ros::Time::now() - trackedMatches[entry.first] > ros::Duration(std::get<5>(std::get<1>(lastMatches[entry.first]).front())/(mMaxDists)+0.05))
+            toMatch.push_back(entry.first);
+        else{
+            Pole trackedPole = poleMap.at(entry.first);
+            trackedPole.header.frame_id = "utm";
+            trackedPole.pose.position = map2UTM(trackedPole.pose.position, trackedPole.header.stamp);
+            allMatches.push_back(std::make_tuple(trackedPole, std::get<1>(lastMatches[entry.first])));
+        }
+    }
+
+    results = dbAssociation(toMatch.cbegin(), toMatch.cend(), std::cref(fingerprints), std::cref(poleMap), mDatabase, mStatement, mWallStatement);
+
+
+    for(int i=0; i<results.size(); ++i)
+        if(!std::get<1>(results[i]).empty())
+            allMatches.push_back(results[i]);
+
+    for(auto match : allMatches)
+        associated.push_back(std::make_pair(std::get<0>(match), std::get<4>(std::get<1>(match)[0])));
+
+/*
+    if(allMatches.size() >= 3){
+        associated = evolution(allMatches);
+        associated = rejectOutliers(associated);
+    }*/
+
+    auto afterMatching = std::chrono::system_clock::now();
+    std::chrono::duration<double> elapsed_seconds = afterMatching-start;
+    ROS_ERROR_STREAM("Database matching took " << elapsed_seconds.count() * 1000 << "ms" << " got " << associated.size() << " pole associations");
+
+    for(auto match : allMatches){
+        long localID = std::stol(std::get<0>(match).ID);
+        if(std::find(toMatch.begin(), toMatch.end(), localID) != toMatch.end()){
+            trackedMatches[localID] = ros::Time::now();
+            lastMatches[localID] = match;
+        }
+    }
+
+    return associated;
+}
+
+
+std::vector<std::tuple<pole_based_localization::Pole, std::vector<std::tuple<double, double, double, double, pole_based_localization::Pole, unsigned int, double> > > > LidarOdometry::dbAssociation(const std::vector<long>::const_iterator fpbegin, const std::vector<long>::const_iterator fpend, const FingerprintMap& fpMap, const std::map<long, Pole> &poleMap, sqlite3* db, sqlite3_stmt* poleStmt, sqlite3_stmt* wallStmt) const
+{
+    auto start = std::chrono::system_clock::now();
+    std::chrono::duration<double> elapsed_seconds;
+    sqlite3_stmt *polePos = NULL, *matchedPrints = NULL, *stmt = NULL;
+
+    std::vector<std::tuple<Pole, std::vector<std::tuple<double, double, double, double, Pole, unsigned int, double> > > > allMatches; // don't ask...
+
+    float angleTolerance = mMaxAngleTolerance;
+
+    auto afterStep = start;
+
+    for(auto it=fpbegin; it != fpend; ++it){
+        long curId = *it;
+        auto trackedPole = poleMap.at(curId);
+        bool isEdge = isEdgeType(trackedPole);
+
+        if(ros::Time::now() - trackedPole.header.stamp > ros::Duration(.5)) continue; // ignore dead poles (older than .5 seconds)
+
+        if(isEdge){
+            if(!mDetectWalls) continue; // Skip, if wall detection is disabled
+
+            // switch the statement for the wall statement
+            stmt = wallStmt;
+            angleTolerance = mMaxWallAngleTolerance;
+        }
+        else stmt = poleStmt;
+
+        std::map<long, std::pair<double, double> > dists = fpMap.at(curId);
+        auto coords = map2UTM(trackedPole.pose.position, trackedPole.header.stamp);
+
+        if(dists.size() < 10) // min 10 prints per pole
+            continue;
+
+        sqlite3_bind_double(stmt, 1, coords.x - mMaxSearchRadius);
+        sqlite3_bind_double(stmt, 2, coords.y - mMaxSearchRadius);
+        sqlite3_bind_double(stmt, 3, coords.x + mMaxSearchRadius);
+        sqlite3_bind_double(stmt, 4, coords.y + mMaxSearchRadius);
+        //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);
+
+        int i=5;
+        for(auto it = dists.begin(); it != dists.end(); ++it)
+        {
+            if(isEdge && poleMap.find(it->first) == poleMap.end())
+                continue;
+
+            double dist_tol = std::max(std::min(it->second.first*mMaxDistTolerance, (double)mMaxDistAbsTolerance), (double)mMinDistAbsTolerance);
+            sqlite3_bind_double(stmt, ++i, it->second.first - dist_tol);
+            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);
+            if(minAngle < -M_PI) minAngle += 2.0*M_PI;
+            if(maxAngle > M_PI) maxAngle -= 2.0*M_PI;
+            sqlite3_bind_double(stmt, ++i, minAngle);
+            sqlite3_bind_double(stmt, ++i, maxAngle);
+
+            if(isEdge){
+                const Pole& other = poleMap.at(it->first);
+                auto otherCoords = map2UTM(other.pose.position, other.header.stamp);
+                double oX = isEdgeType(other) ? 0.0 : otherCoords.x;
+                double oY = isEdgeType(other) ? 0.0 : otherCoords.y;
+                //if(isEdgeType(other))
+                  //  ROS_ERROR_STREAM("from to " << curId << ", " << it->first << " dist angle " << it->second.first << ", " << it->second.second);
+                sqlite3_bind_double(stmt, ++i, oX - mMaxSearchRadius);
+                sqlite3_bind_double(stmt, ++i, oX + mMaxSearchRadius);
+                sqlite3_bind_double(stmt, ++i, oY - mMaxSearchRadius);
+                sqlite3_bind_double(stmt, ++i, oY + mMaxSearchRadius);
+            }
+        }
+
+        ++i;
+        int paramsPerPrint = isEdge ? 8 : 4;
+        for(; i<5+mMaxDists*paramsPerPrint; ++i){
+            sqlite3_bind_double(stmt, i, -5.0);
+        }
+
+        std::string curPole = "";
+        unsigned int distCount = 0;
+        std::vector<std::tuple<double, double, double, double, Pole, unsigned int, double> > results;
+
+        while(sqlite3_step(stmt) == SQLITE_ROW)
+        {
+            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);
+            if(curPole.compare(reinterpret_cast<const char*>(sqlite3_column_text(stmt, 1))) && distCount > 0)
+            {
+                curPole = reinterpret_cast<const char*>(sqlite3_column_text(stmt, 1));
+                double prec = distCount/(double)dists.size();
+                double rec  = distCount/(double)distCount; //TODO: recall calculation disabled
+                Pole pole;
+                pole.ID = curPole;
+                pole.pose.position.x = sqlite3_column_double(stmt, 3);
+                pole.pose.position.y = sqlite3_column_double(stmt, 4);
+                pole.pose.position.z = 0.0;
+                if(isEdgeType(trackedPole)){
+                    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);
+                    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;
+                }
+                //std::cout << "pole matching follow: " << entry.first << " - " << pole.ID << ": "  << pole.pose.orientation.z << std::endl;
+                pole.type = sqlite3_column_int(stmt, 2);
+                pole.life = prec*100;
+                results.push_back(std::make_tuple((1.3*prec*rec)/(0.3*prec+rec), pole.pose.position.x, pole.pose.position.y, pole.pose.position.z, pole, distCount, 0)); //last entry = dbFingerprint count
+            }
+            //std::cout << "pole matching first: " << entry.first << " - " << dbPole.ID << ": " << distCount << ", " << dbPole.pose.orientation.z << std::endl;
+        }
+
+        // reset statement
+        sqlite3_clear_bindings(stmt);
+        sqlite3_reset(stmt);
+
+        if(!results.empty())
+        {
+            //ROS_ERROR_STREAM("found " << results.size() << " pole matches.");
+            Pole pole;
+            pole.pose.position = coords;
+            pole.pose.orientation = trackedPole.pose.orientation;
+
+            pole.ID = std::to_string(curId);
+            pole.type = trackedPole.type;
+            pole.header.stamp = trackedPole.header.stamp;
+            pole.header.frame_id = "utm";
+            pole.life = trackedPole.life;
+            std::sort(results.begin(), results.end(), distCompare);
+        //    if(std::get<5>(results[0]) >= 2){
+                //ROS_ERROR_STREAM("matched " << pole.ID << " to " << std::get<4>(results[0]).ID << " dist: " << calcDist(pole.pose.position, std::get<4>(results[0]).pose.position) << " angle: " << std::get<4>(results[0]).pose.orientation.z << " certainty: " << std::get<0>(results[0]) << " fingerprint count: " << std::get<5>(results[0]));
+                allMatches.push_back(std::make_tuple(pole, results));
+         //   }
+        }
+        afterStep = std::chrono::system_clock::now();
+    }
+
+    if(polePos) sqlite3_finalize(polePos);
+    if(matchedPrints) sqlite3_finalize(matchedPrints);
+
+    auto afterClose = std::chrono::system_clock::now();
+    elapsed_seconds = afterClose-start;
+    //ROS_ERROR_STREAM("Database thread association took " << elapsed_seconds.count() * 1000 << "ms");
+
+    return allMatches;
+}
+
+
+geometry_msgs::Pose LidarOdometry::calculateOffset(const std::vector<std::pair<Pole, Pole> >& associations) const
+{
+    ros::Duration d(0.06);
+    auto stamp = ros::Time::now() - d;
+
+    geometry_msgs::TransformStamped transformStamped;
+    try{
+        //mTfBuffer.canTransform("velodyne_base_link", "utm", stamp, ros::Duration(0.00));
+        transformStamped = mTfBuffer.lookupTransform("velodyne_base_link", "utm", stamp, ros::Duration(0.0));
+    }
+    catch (tf2::TransformException &ex) {
+        ROS_WARN("%s",ex.what());
+    }
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_local(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_remote(new pcl::PointCloud<pcl::PointXYZ>);
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_wall_local(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_wall_remote(new pcl::PointCloud<pcl::PointXYZ>);
+
+    int wallCount = 0;
+
+    double angleDifference = 0.0;
+
+    for(const std::pair<Pole, Pole>& match : associations){
+        const Pole& poleLocal = match.first;
+        const Pole& poleRemote = match.second;
+
+
+        if(poleLocal.type == pole_based_localization::Pole::TYPE_WALL || poleLocal.type == pole_based_localization::Pole::TYPE_LANELINE){
+            ++wallCount;
+            //ROS_ERROR_STREAM("found matching walls: " << poleLocal.ID << " with " << poleRemote.ID << " angles ( " << poleLocal.pose.orientation.z << ", " << poleRemote.pose.orientation.z << " ) markerid: " << markerID << " life: " << (int)poleLocal.life << " length: " << poleLocal.pose.orientation.x);
+            angleDifference += poleLocal.pose.orientation.z - poleRemote.pose.orientation.z;
+
+            for(int i=1; i<std::ceil(poleLocal.pose.orientation.x/10.0); ++i){
+                geometry_msgs::Point wallLocalPoint;
+                wallLocalPoint.x = poleLocal.pose.position.x + i*10.0 * std::sin(poleLocal.pose.orientation.z);
+                wallLocalPoint.y = poleLocal.pose.position.y + i*10.0 * std::cos(-poleLocal.pose.orientation.z);
+
+                geometry_msgs::Point wallRemotePoint = getClosestPointOnLine(poleRemote, wallLocalPoint);
+
+                geometry_msgs::PointStamped psIn, psOut;
+                psIn.header = poleLocal.header;
+                psIn.point = wallLocalPoint;
+                psIn.header.stamp = stamp;
+                tf2::doTransform(psIn, psOut, transformStamped);
+                auto pLocal = psOut.point;
+                psIn.point = wallRemotePoint;
+                tf2::doTransform(psIn, psOut, transformStamped);
+                wallRemotePoint = psOut.point;
+                pcl::PointXYZ pl(pLocal.x, pLocal.y, 0);
+                pcl::PointXYZ pr(wallRemotePoint.x, wallRemotePoint.y, 0);
+
+                if(calcDistSquared(pLocal,wallRemotePoint) > (mMaxSearchRadius*mMaxSearchRadius)/8.0)
+                    continue; // don't add points that are too far
+
+                cloud_wall_local->push_back(pl);
+                cloud_wall_remote->push_back(pr);
+                cloud_local->push_back(pl);
+                cloud_remote->push_back(pr);
+            }
+            continue;
+        }
+
+        geometry_msgs::PointStamped psIn, psOut;
+        psIn.header = poleLocal.header;
+        psIn.point = poleLocal.pose.position;
+        psIn.header.stamp = stamp;
+        tf2::doTransform(psIn, psOut, transformStamped);
+        auto pLocal = psOut.point;
+        pcl::PointXYZ pl(pLocal.x, pLocal.y, 0);
+        cloud_local->push_back(pl);
+
+        psIn.point = poleRemote.pose.position;
+        tf2::doTransform(psIn, psOut, transformStamped);
+        auto pRemote = psOut.point;
+        pcl::PointXYZ pr(pRemote.x, pRemote.y, 0);
+        cloud_remote->push_back(pr);
+    }
+
+    geometry_msgs::Pose result;
+    if(cloud_remote->empty() && cloud_wall_remote->empty())
+        return result;
+
+    //pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
+    pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ, double> pclSVD;
+    pcl::registration::TransformationEstimation<pcl::PointXYZ, pcl::PointXYZ, double>::Matrix4 trans, transwall;
+    pclSVD.estimateRigidTransformation(*cloud_local, *cloud_remote, trans);
+    double rz = 0.0, rz2 = 0.0;
+    result.position.x = 0.0;
+    result.position.y = 0.0;
+    result.position.z = 0.0;
+
+    if(!std::isnan(trans(1)) && !std::isnan(trans(12)) && !std::isnan(trans(13)) && std::abs(trans(12)) < mMaxSearchRadius*2.0 && std::abs(trans(13)) < mMaxSearchRadius*2.0){
+        result.position.x = trans(12);
+        result.position.y = trans(13);
+        rz = trans(1);// * cloud_local->size();
+    }
+
+    result.orientation.x = 0.0;
+    result.orientation.y = 0.0;
+    result.orientation.z = 0.0;
+    result.orientation.w = 1.0;
+
+    pclSVD.estimateRigidTransformation(*cloud_wall_local, *cloud_wall_remote, transwall);
+    if(!std::isnan(transwall(13)) && !std::isnan(transwall(1)) && std::abs(transwall(13)) < mMaxSearchRadius){
+        //tmp_hack.pose.position.y += transwall(13) * wallCount;
+        rz2 = angleDifference; //wallCount >= 3 ? angleDifference/wallCount : 0.0;
+    }
+
+    try{
+        transformStamped = mTfBuffer.lookupTransform("map", "velodyne_base_link", stamp, ros::Duration(0.0));
+    }
+    catch (tf2::TransformException &ex) {
+        ROS_WARN("%s",ex.what());
+        return result; // Couldn't get transform...
     }
 
+    geometry_msgs::PoseStamped psIn, psOut, zero, zeroOut;
+    psIn.header.frame_id = "velodyne_base_link";
+    psIn.pose = result;
+    psIn.header.stamp = ros::Time::now();
+    tf2::doTransform(psIn, psOut, transformStamped);
+    zero.header = psIn.header;
+    //tmp_hack.header.frame_id = "map";
+    result = psOut.pose;
+    tf2::doTransform(zero, zeroOut, transformStamped);
+    result.position.x -= zeroOut.pose.position.x;
+    result.position.y -= zeroOut.pose.position.y;
+    result.orientation.z = -rz;// + rz2;
+    //result.orientation.z /= cloud_local->size() + wallCount;
+    result.orientation.x = -rz;
+    result.orientation.y = -rz2;
+    result.orientation.w = 1.0;
+
+    result.position.x *= -1.0;
+    result.position.y *= -1.0;
+
+    tf2::Matrix3x3 tf3d;
+    tf3d.setValue(trans(0), trans(4), trans(8), trans(1), trans(5), trans(9), trans(2), trans(6), trans(10));
+    tf2::Quaternion tfqt;
+    tf3d.getRotation(tfqt);
+
+    tf2::convert(tfqt, result.orientation);
+
+    return result;
+}
+
+
+std::array<double, 3> LidarOdometry::calcVariances(const std::vector<std::pair<Pole, Pole> >& matchings) const
+{
+    double varZ = 0.1;
+    double measurementUncertainty = 0.5;
+    geometry_msgs::Point p;
+    geometry_msgs::Point avg = std::accumulate(matchings.begin(), matchings.end(), p, [](geometry_msgs::Point p, std::pair<Pole, Pole> m) {
+        geometry_msgs::Point res;
+        res.x = p.x + (m.first.pose.position.x - m.second.pose.position.x);
+        res.y = p.y + (m.first.pose.position.y - m.second.pose.position.y);
+        return res;
+        });
+
+    avg.x /= matchings.size() + 1.0;
+    avg.y /= matchings.size() + 1.0;
+    avg.z = 0.0; // no z
+
+    geometry_msgs::Point var = std::accumulate(matchings.begin(), matchings.end(), p, [avg](geometry_msgs::Point p, std::pair<Pole, Pole> m) {
+            geometry_msgs::Point res;
+            res.x = m.first.pose.position.x - m.second.pose.position.x;
+            res.y = m.first.pose.position.y - m.second.pose.position.y;
+            res.z = 0.0; // no z
+
+            p.x = std::pow(res.x - avg.x, 2.0);
+            p.y = std::pow(res.y - avg.y, 2.0);
+            p.z = 0.0; // no z
+            return p;
+            });
+
+    var.x /= matchings.size()+1.0;
+    var.y /= matchings.size()+1.0;
+    varZ = (var.x + var.y)/20.0;
+
+
+    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);
+    return res;
+}
+
+
+geometry_msgs::Point LidarOdometry::map2UTM(const geometry_msgs::Point &point, ros::Time stamp) const
+{
+    std_msgs::Header header;
+    geometry_msgs::Point result;
+    header.stamp = stamp;
+    header.frame_id = "map";
+    static geometry_msgs::TransformStamped transformStamped; // map2utm is a static transform
+
+
+    if(transformStamped.header.stamp.sec == 0.0)
+    {
+        try{
+            mTfBuffer.canTransform("utm", "map", stamp, ros::Duration(5.0));
+            transformStamped = mTfBuffer.lookupTransform("utm", "map", stamp, ros::Duration(1.0));
+        }
+        catch (tf2::TransformException &ex) {
+            ROS_WARN("%s",ex.what());
+            ros::Duration(1.0).sleep();
+            return point;
+        }
+    }
+
+
+    geometry_msgs::PointStamped psIn, psOut;
+    psIn.header = header;
+    psIn.point = point;
+
+    tf2::doTransform(psIn, psOut, transformStamped);
+
+    result.x = psOut.point.x;
+    result.y = psOut.point.y;
+    result.z = psOut.point.z;
+
+    return result;
+}
+
+
+geometry_msgs::Point LidarOdometry::getClosestPointOnLine(const Pole &line, const geometry_msgs::Point &point) const
+{
+    geometry_msgs::Point result;
+    if(line.type != Pole::TYPE_WALL && line.type != Pole::TYPE_LANELINE){
+        ROS_ERROR("Got a non line feature as line in getClosestPointOnLine!");
+        return result;
+    }
+    geometry_msgs::Point lineStart = line.pose.position;
+    geometry_msgs::Point lineEnd = line.pose.position;
+    lineEnd.x += -std::sin(-line.pose.orientation.z);
+    lineEnd.y += std::cos(-line.pose.orientation.z);
+
+    geometry_msgs::Point lineVector, pointVector;
+    lineVector.x = lineEnd.x-lineStart.x;
+    lineVector.y = lineEnd.y-lineStart.y;
+    pointVector.x = point.x-lineStart.x;
+    pointVector.y = point.y-lineStart.y;
+    double dot = lineVector.x*pointVector.x + lineVector.y*pointVector.y;
+    dot /= lineVector.x*lineVector.x + lineVector.y*lineVector.y;
+
+    result.x = lineStart.x + lineVector.x*dot;
+    result.y = lineStart.y + lineVector.y*dot;
+
+    return result;
+}
+
+
+bool LidarOdometry::isEdgeType(const pole_based_localization::Pole& feature) const
+{
+    switch (feature.type) {
+    case Pole::TYPE_POLE:
+    case Pole::TYPE_CORNER:
+        return false;
+
+    case Pole::TYPE_WALL:
+    case Pole::TYPE_LANELINE:
+        return true;
+
+    default:
+        ROS_ERROR("Encountered unknown pole type. Can not decide if edge type!");
+        return false;
+    }
+}
 
 }
 #include <pluginlib/class_list_macros.h>