added extrinsics to the pointcloud and occgrid computation

src/publisher_laser_map.cpp

@@ -19,6 +19,8 @@ PublisherLaserMap::PublisherLaserMap(const std::string& _unique_name,
     logodds_array_(Eigen::ArrayXXd::Zero(n_cells_(0),n_cells_(1))),
     agg_pc_(0,0)
 {
+    assert(_problem->getDim() == 2 && "PublisherLaserMap only implemented for 2D problems");
+
     // common params
     update_dist_th_             = _server.getParam<double>      (prefix_ + "/update_dist_th");
     update_angle_th_            = _server.getParam<double>      (prefix_ + "/update_angle_th");
@@ -186,7 +188,7 @@ void PublisherLaserMap::storeScan(FrameBasePtr frame, CaptureLaser2dPtr capture)
 }
 
 bool PublisherLaserMap::trajectoryChanged(const std::map<FrameBasePtr,Eigen::Vector3d>& _last_trajectory,
-                                              const std::map<FrameBasePtr,Eigen::Vector3d>& _current_trajectory) const
+                                          const std::map<FrameBasePtr,Eigen::Vector3d>& _current_trajectory) const
 {
     // check if trajectory changed enough
     for (auto frame_pos : _last_trajectory)
@@ -254,8 +256,14 @@ bool PublisherLaserMap::updateOccMap()
 
 void PublisherLaserMap::addScanToOccMap(const ScanData& scan_data, const Eigen::Vector3d& pose)
 {
+    // Get laser extrinsics
+    Eigen::Vector3d laser_pose;
+    laser_pose.head<2>() = Eigen::Rotation2Dd(pose(2)).matrix() * scan_data.capture_->getSensor()->getP()->getState() + pose.head<2>();
+    laser_pose(2) = scan_data.capture_->getSensor()->getO()->getState()(0) + pose(2);
+
+
     // Compute global points
-    Eigen::MatrixXf points = Eigen::Rotation2Df(pose(2)).matrix() * scan_data.local_points_ + pose.cast<float>().head(2).replicate(1,scan_data.local_points_.cols());
+    Eigen::MatrixXf points = Eigen::Rotation2Df(laser_pose(2)).matrix() * scan_data.local_points_ + laser_pose.cast<float>().head(2).replicate(1,scan_data.local_points_.cols());
 
     // Bounds
     Eigen::Vector2i min_cell = vectorToCell(Eigen::Vector2d(points.row(0).minCoeff(),
@@ -280,7 +288,7 @@ void PublisherLaserMap::addScanToOccMap(const ScanData& scan_data, const Eigen::
     }
 
     // Apply log odds for each ray
-    Eigen::Vector2i origin_cell = vectorToCell(pose.head<2>());
+    Eigen::Vector2i origin_cell = vectorToCell(laser_pose.head<2>());
     Eigen::Vector2i point_cell;
     for (auto i=0; i < points.cols(); i++)
     {