diff --git a/include/publisher_laser_map.h b/include/publisher_laser_map.h
index 98d26ea8ddd083d4467013bec89864af8ed4d067..c0919820f4372e195f305e2568c3d7366c5457c8 100644
--- a/include/publisher_laser_map.h
+++ b/include/publisher_laser_map.h
@@ -24,6 +24,14 @@
using namespace wolf;
+struct ScanData
+{
+ CaptureLaser2dPtr capture_;
+ laserscanutils::LaserScanParams params_;
+ Eigen::MatrixXf local_points_;
+ pcl::PointCloud<pcl::PointXYZRGB> local_pc_;
+};
+
class PublisherLaserMap: public Publisher
{
protected:
@@ -34,8 +42,7 @@ class PublisherLaserMap: public Publisher
double Lfree_, Lobst_, Lobst_th_, Lfree_th_;
int max_n_cells_;
Eigen::Vector2i n_cells_;
- double grid_size_, laser_ray_incr_;
- int n_obstacle_samples_;
+ double grid_size_;
bool resized_;
bool discard_max_range_;
Eigen::Vector2d map_origin_;
@@ -49,7 +56,7 @@ class PublisherLaserMap: public Publisher
int pc_r_, pc_g_, pc_b_;
// std::maps
- std::map<FrameBasePtr,std::list<CaptureLaser2dPtr>> scans_;
+ std::map<FrameBasePtr,std::list<ScanData>> scans_;
std::map<FrameBasePtr,Eigen::Vector3d> current_trajectory_, last_trajectory_occ_, last_trajectory_pc_;
@@ -67,21 +74,36 @@ class PublisherLaserMap: public Publisher
void updateTrajectory();
bool trajectoryChanged(const std::map<FrameBasePtr,Eigen::Vector3d>& _last_trajectory,
const std::map<FrameBasePtr,Eigen::Vector3d>& _current_trajectory) const;
+ void storeScan(FrameBasePtr frame, CaptureLaser2dPtr capture);
// occmap
void resetOccMap();
bool updateOccMap();
- void addScanToOccMap(const CaptureLaser2dPtr& capture_laser, const Eigen::Vector3d& pose);
- void addRayToOccMap(const double& theta, const double& range, const Eigen::Vector3d& laser_pose, bool _obstacle);
- Eigen::Vector2i vectorToCell(const Eigen::Vector2d& p);
- void resizeOccMap(const int& dim, const unsigned int& oversize, const bool& back);
+ void addScanToOccMap(const ScanData& scan_data, const Eigen::Vector3d& pose);
+ template<typename T>
+ Eigen::Vector2i vectorToCell(const Eigen::DenseBase<T>& p);
+ void drawFreeLine(const Eigen::Vector2i& cell_0, const Eigen::Vector2i& cell_1);
+ void resizeOccMap(const Eigen::Vector4i& oversize);
void updateOccMsg();
void resizeOccMsg();
// pointcloud
void resetPcMap();
bool updatePcMap();
- void addScanToPcMap(const CaptureLaser2dPtr& capture_laser, const Eigen::Vector3d& pose);
+ void addScanToPcMap(const ScanData& scan_data, const Eigen::Vector3d& pose);
void updatePcMsg();
};
+
+
+template<typename T>
+inline Eigen::Vector2i PublisherLaserMap::vectorToCell(const Eigen::DenseBase<T>& p)
+{
+ MatrixSizeCheck<2, 1>::check(p);
+ Eigen::Vector2i cell;
+ cell(0) = int( std::floor((p(0) - map_origin_(0)) / grid_size_ ));
+ cell(1) = int( std::floor((p(1) - map_origin_(1)) / grid_size_ ));
+
+ return cell;
+}
+
#endif
diff --git a/src/publisher_laser_map.cpp b/src/publisher_laser_map.cpp
index d173c05977e6d61bf70be59acf3eab177e99657b..288ba6c586af48a8b3864000ca0b792002e0d150 100644
--- a/src/publisher_laser_map.cpp
+++ b/src/publisher_laser_map.cpp
@@ -20,36 +20,24 @@ PublisherLaserMap::PublisherLaserMap(const std::string& _unique_name,
agg_pc_(0,0)
{
// common params
- update_dist_th_ = _server.getParam<double> (prefix_ + "/update_dist_th");
- update_angle_th_ = _server.getParam<double> (prefix_ + "/update_angle_th");
+ update_dist_th_ = _server.getParam<double> (prefix_ + "/update_dist_th");
+ update_angle_th_ = _server.getParam<double> (prefix_ + "/update_angle_th");
// occmap params
- occ_msg_.header.frame_id = _server.getParam<std::string> (prefix_ + "/map_frame_id");
- max_n_cells_ = _server.getParam<int> (prefix_ + "/max_n_cells");
- grid_size_ = _server.getParam<double> (prefix_ + "/grid_size");
- laser_ray_incr_ = _server.getParam<double> (prefix_ + "/laser_ray_incr");
- n_obstacle_samples_ = _server.getParam<int> (prefix_ + "/n_obstacle_samples");
- discard_max_range_ = _server.getParam<bool> (prefix_ + "/discard_max_range");
- double p_free = _server.getParam<double> (prefix_ + "/p_free");
- double p_obst = _server.getParam<double> (prefix_ + "/p_obst");
- double p_free_th = _server.getParam<double> (prefix_ + "/p_free_th");
- double p_obst_th = _server.getParam<double> (prefix_ + "/p_obst_th");
+ occ_msg_.header.frame_id = _server.getParam<std::string> (prefix_ + "/map_frame_id");
+ max_n_cells_ = _server.getParam<int> (prefix_ + "/max_n_cells");
+ grid_size_ = _server.getParam<double> (prefix_ + "/grid_size");
+ discard_max_range_ = _server.getParam<bool> (prefix_ + "/discard_max_range");
+ double p_free = _server.getParam<double> (prefix_ + "/p_free");
+ double p_obst = _server.getParam<double> (prefix_ + "/p_obst");
+ double p_free_th = _server.getParam<double> (prefix_ + "/p_free_th");
+ double p_obst_th = _server.getParam<double> (prefix_ + "/p_obst_th");
// pointcloud params
- pc_msg_.header.frame_id = _server.getParam<std::string> (prefix_ + "/map_frame_id");
- try
- {
- pc_r_ = (int)(_server.getParam<double> (prefix_ + "/pointcloud_color_r") * 255.0);
- pc_g_ = (int)(_server.getParam<double> (prefix_ + "/pointcloud_color_g") * 255.0);
- pc_b_ = (int)(_server.getParam<double> (prefix_ + "/pointcloud_color_b") * 255.0);
- }
- catch(...)
- {
- WOLF_INFO("PublisherLaserMap: taking default pointcloud colors...");
- pc_r_ = 100;
- pc_g_ = 200;
- pc_b_ = 50;
- }
+ pc_msg_.header.frame_id = _server.getParam<std::string> (prefix_ + "/map_frame_id");
+ pc_r_ = (int)(getParamWithDefault<double> (_server, prefix_ + "/pointcloud_color_r", 100) * 255.0);
+ pc_g_ = (int)(getParamWithDefault<double> (_server, prefix_ + "/pointcloud_color_g", 200) * 255.0);
+ pc_b_ = (int)(getParamWithDefault<double> (_server, prefix_ + "/pointcloud_color_b", 50) * 255.0);
// preprocessing
Lfree_ = log(p_free / (1 - p_free));
@@ -124,7 +112,7 @@ void PublisherLaserMap::updateTrajectory()
// insert frame to maps
current_trajectory_[frame_pair.second] = frame_pair.second->getState("PO").vector("PO");
- scans_[frame_pair.second] = std::list<CaptureLaser2dPtr>();
+ scans_[frame_pair.second] = std::list<ScanData>();
// search all CaptureLaser2d
auto cap_list = frame_pair.second->getCaptureList();
@@ -132,9 +120,7 @@ void PublisherLaserMap::updateTrajectory()
{
auto cap_laser = std::dynamic_pointer_cast<CaptureLaser2d>(cap);
if (cap_laser and not cap_laser->isRemoving())
- {
- scans_.at(frame_pair.second).push_back(cap_laser);
- }
+ storeScan(frame_pair.second, cap_laser);
}
// remove frames without scans
@@ -145,6 +131,55 @@ void PublisherLaserMap::updateTrajectory()
}
}
}
+
+void PublisherLaserMap::storeScan(FrameBasePtr frame, CaptureLaser2dPtr capture)
+{
+ ScanData scan_data;
+ scan_data.capture_ = capture;
+ scan_data.params_ = std::static_pointer_cast<SensorLaser2d>(capture->getSensor())->getScanParams();
+
+ // local points
+ Eigen::Vector2d laser_extrinsics_p = capture->getSensor()->getP()->getState();
+ double laser_extrinsics_o = capture->getSensor()->getO()->getState()(0);
+
+ Eigen::Map<const Eigen::ArrayXf> ranges(capture->getScan().ranges_raw_.data(), capture->getScan().ranges_raw_.size());
+
+ Eigen::ArrayXf orientations = Eigen::ArrayXf::LinSpaced(ranges.size(),
+ scan_data.params_.angle_min_ + laser_extrinsics_o,
+ scan_data.params_.angle_max_ + laser_extrinsics_o);
+
+ scan_data.local_points_.resize(2,ranges.size());
+ if (discard_max_range_)
+ {
+ scan_data.local_points_.row(0) = orientations.cos() * (ranges < scan_data.params_.range_max_ -0.1).select(ranges, 0) + laser_extrinsics_p(0);
+ scan_data.local_points_.row(1) = orientations.sin() * (ranges < scan_data.params_.range_max_ -0.1).select(ranges, 0) + laser_extrinsics_p(1);
+ }
+ else
+ {
+ scan_data.local_points_.row(0) = orientations.cos() * ranges + laser_extrinsics_p(0);
+ scan_data.local_points_.row(1) = orientations.sin() * ranges + laser_extrinsics_p(1);
+ }
+
+ // local pointcloud
+ scan_data.local_pc_ = pcl::PointCloud<pcl::PointXYZRGB>(ranges.size(),
+ 1,
+ pcl::PointXYZRGB(pc_r_,pc_g_,pc_b_));
+ for (auto i=0; i < ranges.size(); i++)
+ {
+ if (ranges[i] > scan_data.params_.range_max_ - 0.1)
+ scan_data.local_pc_.at(i).x = scan_data.local_pc_.at(i).y = scan_data.local_pc_.at(i).z = std::numeric_limits<float>::quiet_NaN();
+ else
+ {
+ scan_data.local_pc_.at(i).x = scan_data.local_points_(0,i);
+ scan_data.local_pc_.at(i).y = scan_data.local_points_(1,i);
+ scan_data.local_pc_.at(i).z = 0;
+ }
+ }
+
+ // store data
+ scans_.at(frame).push_back(scan_data);
+}
+
bool PublisherLaserMap::trajectoryChanged(const std::map<FrameBasePtr,Eigen::Vector3d>& _last_trajectory,
const std::map<FrameBasePtr,Eigen::Vector3d>& _current_trajectory) const
{
@@ -202,8 +237,8 @@ bool PublisherLaserMap::updateOccMap()
last_trajectory_occ_[frame_pose.first] = frame_pose.second;
// add all scans of the frame the map
- for (auto cap_laser : scans_.at(frame_pose.first))
- addScanToOccMap(cap_laser, frame_pose.second);
+ for (auto scan_data : scans_.at(frame_pose.first))
+ addScanToOccMap(scan_data, frame_pose.second);
updated = true;
}
@@ -212,141 +247,151 @@ bool PublisherLaserMap::updateOccMap()
return updated;
}
-void PublisherLaserMap::addScanToOccMap(const CaptureLaser2dPtr& capture_laser, const Eigen::Vector3d& pose)
+void PublisherLaserMap::addScanToOccMap(const ScanData& scan_data, const Eigen::Vector3d& pose)
{
- // scan pose (considering extrinsics)
- Eigen::Vector3d laser_pose = pose;
- laser_pose.head<2>() += Eigen::Rotation2Dd(pose(2)) * capture_laser->getSensor()->getP()->getState();
- laser_pose(2) += capture_laser->getSensor()->getO()->getState()(0);
-
- // Apply log odds for each beam
- const std::vector<float>& ranges = capture_laser->getScan().ranges_raw_;
- auto scan_params = std::static_pointer_cast<SensorLaser2d>(capture_laser->getSensor())->getScanParams();
- double theta = scan_params.angle_min_;
- for (unsigned int i=0; i < ranges.size(); i++, theta+=scan_params.angle_step_)
+ // 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());
+
+ // Bounds
+ Eigen::Vector2i min_cell = vectorToCell(Eigen::Vector2d(points.row(0).minCoeff(),
+ points.row(1).minCoeff()));
+ Eigen::Vector2i max_cell = vectorToCell(Eigen::Vector2d(points.row(0).maxCoeff(),
+ points.row(1).maxCoeff()));
+
+ // Resize
+ Eigen::Vector4i oversize;
+ oversize << std::max(0, -min_cell(0)),
+ std::max(0, -min_cell(1)),
+ std::max(0, max_cell(0) - n_cells_(0) + 1),
+ std::max(0, max_cell(1) - n_cells_(1) + 1);
+
+ if (oversize.maxCoeff() != 0)
+ {
+ WOLF_DEBUG("PublisherLaserMap::addScanToOccMap: resizing... "
+ "\nmin_cell: ", min_cell.transpose() ,
+ "\nmax_cell: ", max_cell.transpose() ,
+ "\noversize: ", oversize.transpose());
+ resizeOccMap(oversize);
+ }
+
+ // Apply log odds for each ray
+ Eigen::Vector2i origin_cell = vectorToCell(pose.head<2>());
+ Eigen::Vector2i point_cell;
+ for (auto i=0; i < points.cols(); i++)
{
- assert(theta <= scan_params.angle_max_ + 1e-3 && "PublisherLaserMap::addScanToOccMap: Ray orientation higher than angle_max ");
- if (ranges[i] > scan_params.range_min_ and
- (ranges[i] < scan_params.range_max_ - 0.1 or not discard_max_range_))
- // Add the logodds ray
- addRayToOccMap(theta, ranges[i], laser_pose, ranges[i] < scan_params.range_max_-0.1);
+ point_cell = vectorToCell(points.col(i));
+ if (point_cell == origin_cell)
+ continue;
+ assert(point_cell(0) >= 0 and point_cell(0) < n_cells_(0));
+ assert(point_cell(1) >= 0 and point_cell(1) < n_cells_(1));
+
+ // Free cells
+ drawFreeLine(origin_cell, point_cell);
+
+ // Obstacle cell
+ logodds_array_(point_cell(0), point_cell(1)) += Lobst_ - Lfree_; // and undo last free
}
}
-void PublisherLaserMap::addRayToOccMap(const double& theta, const double& range, const Eigen::Vector3d& laser_pose, bool _obstacle)
+void PublisherLaserMap::drawFreeLine(const Eigen::Vector2i& cell_1, const Eigen::Vector2i& cell_2)
{
- double d = 0;
- Eigen::Vector2d origin, point, displ;
-
- double orientation = theta + laser_pose(2);
- origin = laser_pose.head<2>();
+ // Bresenham's line algorithm inspired in https://github.com/JesusHF/bresenham/blob/master/src/main.cpp
- displ(0) = cos(orientation) * laser_ray_incr_;
- displ(1) = sin(orientation) * laser_ray_incr_;
- point = origin;// + displ;
+ int dx, dy, x, y, d, sx, sy;
+ bool swap = false;
- assert(std::isfinite(point(0)) && std::isfinite(point(1)) && "PublisherLaserMap::addRayToOccMap: Not finite point");
+ dx = abs(cell_2(0) - cell_1(0));
+ dy = abs(cell_2(1) - cell_1(1));
+ sx = (cell_2(0) - cell_1(0)) > 0 ? 1 : -1;
+ sy = (cell_2(1) - cell_1(1)) > 0 ? 1 : -1;
- // free cells log odds
- Eigen::Vector2i cell_prev(-1,-1);
+ x = cell_1(0);
+ y = cell_1(1);
- while (d < range)
+ /* Check if dx or dy has a greater range */
+ /* if dy has a greater range than dx swap */
+ if (dy > dx)
{
- Eigen::Vector2i cell = vectorToCell(point);
+ int temp = dx;
+ dx = dy;
+ dy = temp;
- if (cell != cell_prev)
- logodds_array_(cell(0), cell(1)) += Lfree_;
+ temp = sx;
+ sx = sy;
+ sy = temp;
- cell_prev = cell;
+ temp = x;
+ x = y;
+ y = temp;
- // next point
- point += displ;
- d += laser_ray_incr_;
+ swap = true;
}
- // obstacle cell log odd
- if (_obstacle)
+ /* Set the initial decision parameter and the initial point */
+ d = 2 * dy - dx;
+
+ for (int i = 1; i <= dx; i++)
{
- double range_boundary = range;
- for (auto j = 0; j < n_obstacle_samples_; j++, range_boundary += laser_ray_incr_)
- {
- point(0) = origin(0) + cos(orientation) * range_boundary;
- point(1) = origin(1) + sin(orientation) * range_boundary;
+ if (swap)
+ logodds_array_(y, x) += Lfree_;
+ else
+ logodds_array_(x, y) += Lfree_;
- Eigen::Vector2i cell = vectorToCell(point);
- logodds_array_(cell(0), cell(1)) += Lobst_;
+ while (d >= 0)
+ {
+ y = y + sy;
+ d = d - 2 * dx;
}
+ x = x + sx;
+ d = d + 2 * dy;
}
}
-Eigen::Vector2i PublisherLaserMap::vectorToCell(const Eigen::Vector2d& p)
+void PublisherLaserMap::resizeOccMap(const Eigen::Vector4i& oversize)
{
- Eigen::Vector2i cell;
- cell(0) = int( std::floor((p(0) - map_origin_(0)) / grid_size_ ));
- cell(1) = int( std::floor((p(1) - map_origin_(1)) / grid_size_ ));
+ WOLF_DEBUG("PublisherLaserMap::resizeOccMap: oversize = ", oversize.transpose(),
+ "\nCurrent size: ", n_cells_.transpose() , " cells | ", (grid_size_ * n_cells_.cast<double>()).transpose(), " m ",
+ "\nCurrent origin: ", map_origin_.transpose());
- for (unsigned int i = 0; i<2; i++)
+ assert(oversize.minCoeff() >= 0);
+ if (oversize.maxCoeff() == 0)
{
- int oversize = std::max( 0 - cell(i), cell(i) - n_cells_(i) + 1 );
- while ( oversize > 0 && n_cells_(i) < max_n_cells_ )
- {
- bool back = !(cell(i) < 0);
- ROS_DEBUG("PublisherLaserMap: Point out of the map in dimension %i: \n\tpoint = %f, %f\n\tlower = %f, %f\n\tupper = %f, %f\n\tcell = %i, %i\n\tn cells = %i, %i", i,
- p(0), p(1), map_origin_(0), map_origin_(1), map_origin_(0) + grid_size_ * n_cells_(1), map_origin_(1) + grid_size_ * n_cells_(0),
- cell(1), cell(0), n_cells_(1), n_cells_(0));
- resizeOccMap(i, oversize, back);
- ROS_DEBUG("PublisherLaserMap: Map resized in dimension %i: new limits:\n\tlower = %f, %f\n\tupper = %f, %f\n\tn cells = %i, %i", i,
- map_origin_(0), map_origin_(1), map_origin_(0) + grid_size_ * n_cells_(1), map_origin_(1) + grid_size_ * n_cells_(0), n_cells_(1), n_cells_(0));
-
- // Recompute current cell and oversize
- cell(i) = int( (p(i) - map_origin_(i)) / grid_size_ );
- oversize = std::max( 0 - cell(i), cell(i) - n_cells_(i) + 1 );
- }
+ ROS_WARN("called resizeOccMap with zero oversize!");
+ return;
}
- assert(cell(0) >= 0 && cell(1) >= 0 && cell(0) < n_cells_(0) && cell(1) < n_cells_(1) && "Resultinc cell out of the map. Resizing whent wrong somehow...");
- return cell;
-}
+ // 0 and 1 oversize (x, y) at the beginning
+ // 2 and 3 oversize (x, y) at the end
-void PublisherLaserMap::resizeOccMap(const int& dim, const unsigned int& oversize, const bool& back)
-{
- Eigen::Vector2i new_n_cells = n_cells_;
- new_n_cells(dim) = n_cells_(dim) + oversize;
+ Eigen::Vector2i new_n_cells = n_cells_ + oversize.head<2>() + oversize.tail<2>();
+ WOLF_DEBUG("Resizing to ", new_n_cells.transpose(), " cells");
// store old logodds array
Eigen::ArrayXXd old_logodds_array = logodds_array_;
+
// resize with zeros
logodds_array_ = Eigen::ArrayXXd::Zero(new_n_cells(0), new_n_cells(1));
- // Adding at the end of the array
- if (back)
- {
- // Origin doesn't change
- // Copy old values
- logodds_array_.block(0, 0, n_cells_(0), n_cells_(1)) = old_logodds_array;
- }
- // Adding at the beggining of the array
- else
- {
- // Change origin
- map_origin_(dim) -= oversize * grid_size_;
- // Copy old values
- logodds_array_.block((dim == 0 ? oversize : 0),
- (dim == 1 ? oversize : 0),
- n_cells_(0),
- n_cells_(1)) = old_logodds_array;
- }
+ // Copy back the old array
+ logodds_array_.block(oversize(0), oversize(1), n_cells_(0), n_cells_(1)) = old_logodds_array;
+
+ // Change origin
+ map_origin_(0) -= oversize(0) * grid_size_;
+ map_origin_(1) -= oversize(1) * grid_size_;
+
// Change in the occupancy grid msg
n_cells_ = new_n_cells;
resized_= true;
+ WOLF_INFO("PublisherLaserMap::resizeOccMap: resized occupancy map!",
+ "\nCurrent size: ", n_cells_.transpose() , " cells | ", (grid_size_ * n_cells_.cast<double>()).transpose(), " m ",
+ "\nCurrent origin: ", map_origin_.transpose());
+
}
void PublisherLaserMap::updateOccMsg()
{
Eigen::ArrayXXd occupancy_probability(Eigen::ArrayXXd::Ones(n_cells_(0), n_cells_(1)));
- //std::cout << logodds_array_<< std::endl;
-
occupancy_probability = (logodds_array_ >= Lobst_th_).select(100,occupancy_probability);
occupancy_probability = (logodds_array_ <= Lfree_th_).select(0,occupancy_probability);
occupancy_probability = (logodds_array_ < Lobst_th_ && logodds_array_ > Lfree_th_).select(-1,occupancy_probability);
@@ -421,31 +466,17 @@ bool PublisherLaserMap::updatePcMap()
}
-void PublisherLaserMap::addScanToPcMap(const CaptureLaser2dPtr& capture_laser, const Eigen::Vector3d& pose)
+void PublisherLaserMap::addScanToPcMap(const ScanData& scan_data, const Eigen::Vector3d& pose)
{
- // scan global pose (considering extrinsics)
- Eigen::Vector3d laser_pose = pose;
- laser_pose.head<2>() += Eigen::Rotation2Dd(pose(2)) * capture_laser->getSensor()->getP()->getState();
- laser_pose(2) += capture_laser->getSensor()->getO()->getState()(0);
-
- // get scan necessary data
- const std::vector<float>& ranges = capture_laser->getScan().ranges_raw_;
- auto scan_params = std::static_pointer_cast<SensorLaser2d>(capture_laser->getSensor())->getScanParams();
-
- // Generate scan pointcloud
- pcl::PointCloud<pcl::PointXYZRGB> scan_pc(ranges.size(), 1, pcl::PointXYZRGB(pc_r_,pc_g_,pc_b_));
- double theta = laser_pose(2) + scan_params.angle_min_;
- for (auto i=0; i < ranges.size(); i++, theta+=scan_params.angle_step_)
- {
- if (ranges[i] > scan_params.range_max_ - 0.1)
- scan_pc.at(i).x = scan_pc.at(i).y = scan_pc.at(i).z = std::numeric_limits<float>::quiet_NaN();
- else
- {
- scan_pc.at(i).x = laser_pose(0) + cos(theta) * ranges[i];
- scan_pc.at(i).y = laser_pose(1) + sin(theta) * ranges[i];
- scan_pc.at(i).z = 0;
- }
- }
+ // transformation
+ Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
+ T.topLeftCorner<2,2>() = Eigen::Rotation2Dd(pose(2)).matrix();
+ T.topRightCorner<2,1>() = pose.head<2>();
+ WOLF_DEBUG("addScanToPcMap T:\n", T);
+
+ // global scan pointcloud
+ pcl::PointCloud<pcl::PointXYZRGB> scan_pc(scan_data.local_pc_);
+ pcl::transformPointCloud (scan_data.local_pc_, scan_pc, T);
// Aggregate scan
agg_pc_ += scan_pc;