Skip to content
Snippets Groups Projects
Commit 99e91f17 authored by echelon's avatar echelon
Browse files

implemented spatialite approach

parent 412bc681
Branches
Tags
No related merge requests found
......@@ -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;
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;
ros::Subscriber mFeaturesSubscriber;
ros::Publisher mOdometryPublisher;
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;
};
}
......
This diff is collapsed.
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment