Skip to content
Snippets Groups Projects
Commit bc56f919 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

improved efficiency of publisher_laser_map

parent d2802eed
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/publisher_laser_map.h
+ 18
− 1
View file @ bc56f919
......@@ -59,6 +59,7 @@ class PublisherLaserMap: public Publisher
protected:
double update_dist_th_, update_angle_th_;
bool recompute_on_frame_removal_;
SensorBaseConstPtr sensor_;
// OCCUPANCY MAP parameters and auxiliar variables
......@@ -79,7 +80,8 @@ class PublisherLaserMap: public Publisher
int pc_r_, pc_g_, pc_b_;
// std::maps
std::map<FrameBaseConstPtr,std::list<ScanData>> scans_;
std::map<FrameBaseConstPtr,std::map<CaptureLaser2dConstPtr, ScanData>> scans_;
std::set<CaptureLaser2dConstPtr> last_captures_occ_, last_captures_pc_;
std::map<FrameBaseConstPtr,Eigen::Vector3d> current_trajectory_, last_trajectory_occ_, last_trajectory_pc_;
......@@ -99,6 +101,9 @@ class PublisherLaserMap: public Publisher
const std::map<FrameBaseConstPtr,Eigen::Vector3d>& _current_trajectory) const;
void storeScan(FrameBaseConstPtr frame, CaptureLaser2dConstPtr capture);
bool renderingOccMap() const;
bool renderingPcMap() const;
// occmap
void resetOccMap();
bool updateOccMap();
......@@ -129,5 +134,17 @@ inline Eigen::Vector2i PublisherLaserMap::vectorToCell(const Eigen::DenseBase<T>
return cell;
}
inline bool PublisherLaserMap::renderingOccMap() const
{
return publisher_.getNumSubscribers() == 0;
}
inline bool PublisherLaserMap::renderingPcMap() const
{
return publisher_pc_.getNumSubscribers() == 0;
}
}
#endif
src/publisher_laser_map.cpp
+ 90
− 48
View file @ bc56f919
......@@ -30,6 +30,7 @@
#include <pcl_ros/transforms.h>
#include <limits>
#include <set>
namespace wolf
{
......@@ -55,6 +56,7 @@ PublisherLaserMap::PublisherLaserMap(const std::string& _unique_name,
sensor_ = _problem->findSensor(sensor_name);
WOLF_WARN_COND(not sensor_, "PublisherLaserMap: Sensor name not found: ", sensor_name);
}
recompute_on_frame_removal_ = getParamWithDefault<bool> (_server, prefix_ + "/recompute_on_frame_removal", false);
// occmap params
occ_msg_.header.frame_id = _server.getParam<std::string> (prefix_ + "/map_frame_id");
......@@ -138,8 +140,10 @@ void PublisherLaserMap::publishDerived()
void PublisherLaserMap::updateTrajectory()
{
current_trajectory_.clear();
scans_.clear();
// store old frames to know which were removed
std::set<FrameBaseConstPtr> removed_frames;
for (auto frame_pos : current_trajectory_)
removed_frames.insert(frame_pos.first);
// copy new trajectory poses and scans
auto frame_map = problem_->getTrajectory()->getFrameMap();
......@@ -148,32 +152,56 @@ void PublisherLaserMap::updateTrajectory()
if (frame_pair.second->isRemoving())
continue;
// insert frame to maps
// frame not removed
removed_frames.erase(frame_pair.second);
// insert/modify frame pose
current_trajectory_[frame_pair.second] = frame_pair.second->getState("PO").vector("PO");
scans_[frame_pair.second] = std::list<ScanData>();
// search all CaptureLaser2d
auto cap_list = frame_pair.second->getCaptureList();
for (auto cap : cap_list)
{
auto cap_laser = std::dynamic_pointer_cast<const CaptureLaser2d>(cap);
if (cap_laser and
not cap_laser->isRemoving() and
(not sensor_ or cap_laser->getSensor() == sensor_))
// add scan if...
if (// ...not null
cap_laser and
// ...not removing
not cap_laser->isRemoving() and
// ...from specified sensor (if specified)
(not sensor_ or cap_laser->getSensor() == sensor_) and
// ...not stored already
(not scans_.count(frame_pair.second) or not scans_.at(frame_pair.second).count(cap_laser)))
storeScan(frame_pair.second, cap_laser);
}
// remove frames without scans
if (scans_.at(frame_pair.second).empty())
if (scans_.count(frame_pair.second) and scans_.at(frame_pair.second).empty())
{
scans_.erase(frame_pair.second);
current_trajectory_.erase(frame_pair.second);
}
}
// remove removed frames from scans and trajectory
for (auto removed_frm : removed_frames)
{
current_trajectory_.erase(removed_frm);
scans_.erase(removed_frm);
}
}
void PublisherLaserMap::storeScan(FrameBaseConstPtr frame, CaptureLaser2dConstPtr capture)
{
if (not capture or
not capture->getSensor() or
not capture->getSensor()->getP() or
not capture->getSensor()->getO() or
capture->getSensor()->getP()->getState().size() != 2 or
capture->getSensor()->getO()->getState().size() != 1)
return;
ScanData scan_data;
scan_data.capture_ = capture;
scan_data.params_ = std::static_pointer_cast<const SensorLaser2d>(capture->getSensor())->getScanParams();
......@@ -197,13 +225,6 @@ void PublisherLaserMap::storeScan(FrameBaseConstPtr frame, CaptureLaser2dConstPt
scan_data.local_points_.row(0) = orientations.cos() * ranges_filtered + laser_extrinsics_p(0);
scan_data.local_points_.row(1) = orientations.sin() * ranges_filtered + laser_extrinsics_p(1);
// // discard max range (optionally)
// if (discard_max_range_)
// {
// scan_data.local_points_.row(0) = (ranges < scan_data.params_.range_max_-1e-3).select(scan_data.local_points_.row(0), 0.0);
// scan_data.local_points_.row(1) = (ranges < scan_data.params_.range_max_-1e-3).select(scan_data.local_points_.row(1), 0.0);
// }
// local pointcloud
scan_data.local_pc_ = pcl::PointCloud<pcl::PointXYZRGB>(ranges.size(),
1,
......@@ -220,8 +241,11 @@ void PublisherLaserMap::storeScan(FrameBaseConstPtr frame, CaptureLaser2dConstPt
}
}
// insert scan if not exist
if (not scans_.count(frame))
scans_[frame] = std::map<CaptureLaser2dConstPtr, ScanData>();
// store data
scans_.at(frame).push_back(scan_data);
scans_.at(frame).emplace(capture, scan_data);
}
bool PublisherLaserMap::trajectoryChanged(const std::map<FrameBaseConstPtr,Eigen::Vector3d>& _last_trajectory,
......@@ -233,17 +257,17 @@ bool PublisherLaserMap::trajectoryChanged(const std::map<FrameBaseConstPtr,Eigen
// any old frame was removed -> recompute
if (_current_trajectory.count(frame_pos.first) == 0)
{
WOLF_INFO("PublisherLaserMap::trajectoryChanged: Any old frame was removed. Recomputing from scratch");
return true;
WOLF_INFO_COND(recompute_on_frame_removal_, "PublisherLaserMap::trajectoryChanged: Any old frame was removed. Recomputing from scratch");
if (recompute_on_frame_removal_)
return true;
else
continue;
}
// any old frame moved significantly (angle or position) -> recompute occupancy map
const Eigen::Vector3d& old_frame_pos = frame_pos.second;
const Eigen::Vector3d& new_frame_pos = _current_trajectory.at(frame_pos.first);
//std::cout << "old_frame_pos " << old_frame_pos.transpose() << std::endl;
//std::cout << "new_frame_pos " << new_frame_pos.transpose() << std::endl;
if ((old_frame_pos-new_frame_pos).head<2>().norm() > update_dist_th_ or
std::abs(old_frame_pos(2)-new_frame_pos(2)) > update_angle_th_)
{
......@@ -261,30 +285,39 @@ void PublisherLaserMap::resetOccMap()
// Reset logodds_grid_
logodds_array_ = Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>::Zero(n_cells_(0),n_cells_(1));
// reset last trajectory poses
// reset last trajectory poses and captures
last_trajectory_occ_.clear();
last_captures_occ_.clear();
}
bool PublisherLaserMap::updateOccMap()
{
bool updated = false;
// Update the map with captures of frames in trajectory_poses_ that are not in last_trajectory_occ_
for (auto frame_pose : current_trajectory_)
// Update the map with captures in scans_ that are not in last_captures_occ_
for (auto scan_pair : scans_)
{
if (last_trajectory_occ_.count(frame_pose.first) == 1)
continue;
// add frame to last_trajectory_occ_
if (not last_trajectory_occ_.count(scan_pair.first))
{
WOLF_DEBUG("Frame ", scan_pair.first->id(), " not in last_trajectory_occ_, adding its pose");
WOLF_DEBUG("Frame ", frame_pose.first->id(), " not in last_trajectory_occ_, adding ", scans_.at(frame_pose.first).size(), " scans to map");
// add trajectory pose
last_trajectory_occ_[frame_pose.first] = frame_pose.second;
// add all scans of the frame the map
for (auto scan_data : scans_.at(frame_pose.first))
addScanToOccMap(scan_data, frame_pose.second);
last_trajectory_occ_[scan_pair.first] = current_trajectory_.at(scan_pair.first);
updated = true;
}
updated = true;
// update occmap with new scans
for (auto capture_pair : scan_pair.second)
{
if (last_captures_occ_.count(capture_pair.first))
continue;
WOLF_DEBUG("Capture ", capture_pair.first->id(), " not in last_captures_occ_, adding it to map");
last_captures_occ_.insert(capture_pair.first);
addScanToOccMap(capture_pair.second, last_trajectory_occ_.at(scan_pair.first));
updated = true;
}
}
WOLF_DEBUG("PublisherLaserMap::updateOccMap updated = ", (updated ? "true" : "false"));
......@@ -481,28 +514,37 @@ void PublisherLaserMap::resetPcMap()
// reset last trajectory poses
last_trajectory_pc_.clear();
last_captures_pc_.clear();
}
bool PublisherLaserMap::updatePcMap()
{
bool updated = false;
// Update the map with captures of frames in trajectory_poses_ that are not in last_trajectory_pc_
for (auto frame_pose : current_trajectory_)
// Update the map with captures in scans_ that are not in last_captures_pc_
for (auto scan_pair : scans_)
{
if (last_trajectory_pc_.count(frame_pose.first) == 1)
continue;
// add frame to last_trajectory_pc_
if (not last_trajectory_pc_.count(scan_pair.first))
{
WOLF_DEBUG("Frame ", scan_pair.first->id(), " not in last_trajectory_pc_, adding its pose");
WOLF_DEBUG("Frame ", frame_pose.first->id(), " not in last_trajectory_pc_, adding ", scans_.at(frame_pose.first).size(), " scans to pointcloud");
// add trajectory pose
last_trajectory_pc_[frame_pose.first] = frame_pose.second;
// add all scans of the frame the map
for (auto cap_laser : scans_.at(frame_pose.first))
addScanToPcMap(cap_laser, frame_pose.second);
last_trajectory_pc_[scan_pair.first] = current_trajectory_.at(scan_pair.first);
updated = true;
}
updated = true;
// update aggregated pointcloud with new scans
for (auto capture_pair : scan_pair.second)
{
if (last_captures_pc_.count(capture_pair.first))
continue;
WOLF_DEBUG("Capture ", capture_pair.first->id(), " not in last_captures_pc_, adding it to map");
last_captures_pc_.insert(capture_pair.first);
addScanToPcMap(capture_pair.second, last_trajectory_pc_.at(scan_pair.first));
updated = true;
}
}
WOLF_DEBUG("PublisherLaserMap::updatePcMap updated = ", (updated ? "true" : "false"));
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment