From 76abe000958cae2bf19e0867c4ecff360b2d9ac8 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Wed, 3 Feb 2021 18:11:22 +0100
Subject: [PATCH] added extrinsics to the pointcloud and occgrid computation
---
src/publisher_laser_map.cpp | 14 +++++++++++---
1 file changed, 11 insertions(+), 3 deletions(-)
diff --git a/src/publisher_laser_map.cpp b/src/publisher_laser_map.cpp
index cf08a4b..021a6ee 100644
--- a/src/publisher_laser_map.cpp
+++ b/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++)
{
--
GitLab