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;
 };
 }