From 7c4c754be87b5b46a80a57ec6ac8d4e00951b5bd Mon Sep 17 00:00:00 2001
From: Nicolai Steinke <n.steinke@fu-berlin.de>
Date: Mon, 6 Jul 2020 18:26:44 +0200
Subject: [PATCH] small bugfix and higher variance
---
src/LidarOdometry.cpp | 41 ++++++++++++++++++++++-------------------
1 file changed, 22 insertions(+), 19 deletions(-)
diff --git a/src/LidarOdometry.cpp b/src/LidarOdometry.cpp
index 6897f03..6792970 100644
--- a/src/LidarOdometry.cpp
+++ b/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;
}
--
GitLab