hotfix: Extrinsics were already taken into account...!

src/publisher_laser_map.cpp

@@ -256,14 +256,8 @@ 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(laser_pose(2)).matrix() * scan_data.local_points_ + laser_pose.cast<float>().head(2).replicate(1,scan_data.local_points_.cols());
+    Eigen::MatrixXf points = Eigen::Rotation2Df(pose(2)).matrix() * scan_data.local_points_ + pose.cast<float>().head(2).replicate(1,scan_data.local_points_.cols());
 
     // Bounds
     Eigen::Vector2i min_cell = vectorToCell(Eigen::Vector2d(points.row(0).minCoeff(),
@@ -288,7 +282,7 @@ void PublisherLaserMap::addScanToOccMap(const ScanData& scan_data, const Eigen::
     }
 
     // Apply log odds for each ray
-    Eigen::Vector2i origin_cell = vectorToCell(laser_pose.head<2>());
+    Eigen::Vector2i origin_cell = vectorToCell(pose.head<2>());
     Eigen::Vector2i point_cell;
     for (auto i=0; i < points.cols(); i++)
     {