Skip to content
Snippets Groups Projects
Commit 7c4c754b authored by echelon's avatar echelon
Browse files

small bugfix and higher variance

parent 2fe7b121
No related branches found
No related tags found
No related merge requests found
......@@ -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;
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment