diff --git a/include/LidarOdometry.h b/include/LidarOdometry.h
index 9dd3533de5c1cb8828fa0663e5bb66fdadf089c2..b600bca86cab8c30a6d20dfdb05860f0b3c29663 100644
--- a/include/LidarOdometry.h
+++ b/include/LidarOdometry.h
@@ -49,7 +49,7 @@ protected:
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;
+ void insertPrints(std::map<long, pole_based_localization::Pole> &poleMap, FingerprintMap& fingerprints, const std::vector<std::pair<pole_based_localization::Pole, pole_based_localization::Pole> > &matchings) 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;
@@ -82,7 +82,7 @@ protected:
bool mDetectWalls = true;
float mMaxDistTolerance = 0.05f, mMaxDistAbsTolerance = 0.8f, mMinDistAbsTolerance = 0.2f;
float mMaxAngleTolerance = 0.05f;
- float mMaxWallAngleTolerance = 0.03f;
+ float mMaxWallAngleTolerance = 0.05f;
float mMaxSearchRadius = 1.0f;
sqlite3* mDatabase;
sqlite3_stmt* mStatement, *mWallStatement, *mPoleInsertStmt, *mWallInsertStmt, *mDeleteStmt;
diff --git a/src/LidarOdometry.cpp b/src/LidarOdometry.cpp
index 1f62ee9fb3f90a3ffcf1c283c7e1f9a6866cef51..b39735aac6bc5a9fd8dcfa653c9a75e3f4e13e60 100644
--- a/src/LidarOdometry.cpp
+++ b/src/LidarOdometry.cpp
@@ -60,7 +60,7 @@ void LidarOdometry::onInit()
// if(sqlite3_open_v2("file::memory:", &db, SQLITE_OPEN_URI | SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE | SQLITE_OPEN_NOMUTEX, NULL))
// ROS_ERROR_STREAM("Can't open database: " << sqlite3_errmsg(db));
- if(sqlite3_open_v2("file::memory:", &db, SQLITE_OPEN_URI | SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE | SQLITE_OPEN_NOMUTEX, NULL))
+ if(sqlite3_open_v2("file:memdb1?mode=memory&cache=shared", &db, SQLITE_OPEN_URI | SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE | SQLITE_OPEN_NOMUTEX, NULL))
ROS_ERROR_STREAM("Can't open database: " << sqlite3_errmsg(db));
@@ -179,6 +179,11 @@ void LidarOdometry::callbackFeatures(const pole_based_localization::PoleArray& p
if(matchings.size() >= 3){
offset = calculateOffset(matchings);
}
+
+
+ // insert unupdated prints into db
+ insertPrints(poleMap, fingerprints, matchings);
+
//ROS_ERROR_STREAM("Position difference is: (" << offset.position.x << ", " << offset.position.y << ")");
auto var = calcVariances(matchings);
@@ -306,7 +311,7 @@ void LidarOdometry::updateFingerprints(const PoleArray &poles, FingerprintMap& f
{
// add new poles
for(Pole pole : poles.poles){
- if((pole.life >= 80 || poleMap.find(std::stol(pole.ID)) != poleMap.end()))
+ if((pole.life >= 95 || poleMap.find(std::stol(pole.ID)) != poleMap.end()))
poleMap[std::stol(pole.ID)] = pole;
}
@@ -316,7 +321,7 @@ void LidarOdometry::updateFingerprints(const PoleArray &poles, FingerprintMap& f
// update fingerprints for all affected poles
for(Pole pole : poles.poles)
{
- if(pole.life <= 60 || calcDistSquared(pole.pose.position, mLastPos) - std::pow(pole.pose.orientation.x,2.0) > 10000.0 || poleMap.find(std::stol(pole.ID)) == poleMap.end())
+ if(pole.life <= 80 || calcDistSquared(pole.pose.position, mLastPos) - std::pow(pole.pose.orientation.x,2.0) > 10000.0 || poleMap.find(std::stol(pole.ID)) == poleMap.end())
continue;
std::vector<long> polesInRange = getPolesInRange(pole, poleMap, 50.0);
@@ -332,9 +337,6 @@ void LidarOdometry::updateFingerprints(const PoleArray &poles, FingerprintMap& f
fingerprints[otherID][std::stol(pole.ID)] = std::make_pair(dist, otherAngle);
}
}
-
- // insert unupdated prints into db
- insertPrints(poleMap, fingerprints);
}
@@ -359,19 +361,23 @@ void LidarOdometry::cleanPolesNPrints(std::map<long, Pole> &poleMap, Fingerprint
}
-void LidarOdometry::insertPrints(std::map<long, Pole> &poleMap, FingerprintMap &fingerprints) const
+void LidarOdometry::insertPrints(std::map<long, Pole> &poleMap, FingerprintMap &fingerprints, const std::vector<std::pair<Pole,Pole>>& matchings) const
{
static std::map<long, Pole> lastPoleMap;
+ std::map<long, long> matchedMap;
sqlite3_exec(mDatabase, "BEGIN Transaction", NULL, NULL, NULL);
- // Clean tables
- // sqlite3_exec(mDatabase, "DELETE FROM fingerprints where Distance(MakePoint(?,?,32633), Geometry) > 300.0", NULL, NULL, NULL);
- //sqlite3_exec(mDatabase, "DELETE FROM fingerprintsWalls", NULL, NULL, NULL);
+ // Clean tables
geometry_msgs::Point utmEgoPos = map2UTM(mLastPos, ros::Time::now());
sqlite3_bind_double(mDeleteStmt, 1, utmEgoPos.x);
sqlite3_bind_double(mDeleteStmt, 2, utmEgoPos.y);
sqlite3_step(mDeleteStmt);
+ for(auto match : matchings)
+ matchedMap[std::stol(match.first.ID)] = std::stol(match.second.ID);
+
+ int added = 0;
+
for(auto entry : poleMap){
long id = entry.first;
Pole pole = entry.second;
@@ -379,6 +385,7 @@ void LidarOdometry::insertPrints(std::map<long, Pole> &poleMap, FingerprintMap &
if(fingerprints.find(id) == fingerprints.end())
continue;
+ ++added;
geometry_msgs::Point utmpos = map2UTM(pole.pose.position, pole.header.stamp);
if(isEdgeType(pole)){
@@ -447,6 +454,8 @@ void LidarOdometry::insertPrints(std::map<long, Pole> &poleMap, FingerprintMap &
sqlite3_reset(mPoleInsertStmt);
}
}
+ if(added > 5)
+ break; // TODO: testing: only one pole per callback
}
sqlite3_exec(mDatabase, "END Transaction", NULL, NULL, NULL);
@@ -891,10 +900,10 @@ geometry_msgs::Pose LidarOdometry::calculateOffset(const std::vector<std::pair<P
//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){
+ for(int i=1; i<std::ceil(poleLocal.pose.orientation.x/3.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);
+ wallLocalPoint.x = poleLocal.pose.position.x + i*3.0 * std::sin(poleLocal.pose.orientation.z);
+ wallLocalPoint.y = poleLocal.pose.position.y + i*3.0 * std::cos(-poleLocal.pose.orientation.z);
geometry_msgs::Point wallRemotePoint = getClosestPointOnLine(poleRemote, wallLocalPoint);
@@ -1011,7 +1020,7 @@ geometry_msgs::Pose LidarOdometry::calculateOffset(const std::vector<std::pair<P
std::array<double, 3> LidarOdometry::calcVariances(const std::vector<std::pair<Pole, Pole> >& matchings) const
{
double varZ = 0.1;
- double measurementUncertainty = 0.5;
+ double measurementUncertainty = 0.3;
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;
@@ -1045,7 +1054,7 @@ std::array<double, 3> LidarOdometry::calcVariances(const std::vector<std::pair<P
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[2] = std::min(0.00001, varZ + measurementUncertainty/100.0);
return res;
}