diff --git a/include/publisher_laser_map.h b/include/publisher_laser_map.h
index 9471795421100679fca08bde46583876776bc6a1..58fd1d79e2d71cacef4240e81c791e24d139e26b 100644
--- a/include/publisher_laser_map.h
+++ b/include/publisher_laser_map.h
@@ -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
diff --git a/src/publisher_laser_map.cpp b/src/publisher_laser_map.cpp
index 50c1452996e2ee6e5cc2427d365706cb641faf66..7d384c55d3ece8d9b1c2f073fa25a04fee544deb 100644
--- a/src/publisher_laser_map.cpp
+++ b/src/publisher_laser_map.cpp
@@ -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"));