Merging in remaining missing contents for laser_piple that svn ignored on the first merge.
git-svn-id: https://code.ros.org/svn/ros-pkg/pkg/trunk/stacks/laser_pipeline@23510 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
This commit is contained in:
Jeremy Leibs
288
src/laser_processor.cpp
Normal file
288
src/laser_processor.cpp
Normal file
@@ -0,0 +1,288 @@
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2008, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Willow Garage nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
#include "laser_geometry/laser_processor.h"
|
||||
|
||||
#include <stdexcept>
|
||||
|
||||
using namespace ros;
|
||||
using namespace std;
|
||||
using namespace laser_geometry;
|
||||
|
||||
Sample* Sample::Extract(int ind, const sensor_msgs::LaserScan& scan)
|
||||
{
|
||||
Sample* s = new Sample;
|
||||
|
||||
s->index = ind;
|
||||
s->range = scan.ranges[ind];
|
||||
s->intensity = scan.intensities[ind];
|
||||
s->x = cos( scan.angle_min + ind*scan.angle_increment ) * s->range;
|
||||
s->y = sin( scan.angle_min + ind*scan.angle_increment ) * s->range;
|
||||
if (s->range > scan.range_min && s->range < scan.range_max)
|
||||
return s;
|
||||
else
|
||||
{
|
||||
delete s;
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
void SampleSet::clear()
|
||||
{
|
||||
for (SampleSet::iterator i = begin();
|
||||
i != end();
|
||||
i++)
|
||||
{
|
||||
delete (*i);
|
||||
}
|
||||
set<Sample*, CompareSample>::clear();
|
||||
}
|
||||
|
||||
void SampleSet::appendToCloud(sensor_msgs::PointCloud& cloud, int r, int g, int b)
|
||||
{
|
||||
float color_val = 0;
|
||||
|
||||
int rgb = (r << 16) | (g << 8) | b;
|
||||
color_val = *(float*)&(rgb);
|
||||
|
||||
for (iterator sample_iter = begin();
|
||||
sample_iter != end();
|
||||
sample_iter++)
|
||||
{
|
||||
geometry_msgs::Point32 point;
|
||||
point.x = (*sample_iter)->x;
|
||||
point.y = (*sample_iter)->y;
|
||||
point.z = 0;
|
||||
|
||||
cloud.points.push_back(point);
|
||||
|
||||
if (cloud.channels[0].name == "rgb")
|
||||
cloud.channels[0].values.push_back(color_val);
|
||||
|
||||
if (cloud.channels[0].name == "intensity")
|
||||
cloud.channels[0].values.push_back((*sample_iter)->intensity);
|
||||
}
|
||||
}
|
||||
|
||||
tf::Point SampleSet::center()
|
||||
{
|
||||
float x_mean = 0.0;
|
||||
float y_mean = 0.0;
|
||||
for (iterator i = begin();
|
||||
i != end();
|
||||
i++)
|
||||
|
||||
{
|
||||
x_mean += ((*i)->x)/size();
|
||||
y_mean += ((*i)->y)/size();
|
||||
}
|
||||
|
||||
return tf::Point (x_mean, y_mean, 0.0);
|
||||
}
|
||||
|
||||
|
||||
void ScanMask::addScan(sensor_msgs::LaserScan& scan)
|
||||
{
|
||||
if (!filled)
|
||||
{
|
||||
angle_min = scan.angle_min;
|
||||
angle_max = scan.angle_max;
|
||||
size = scan.ranges.size();
|
||||
filled = true;
|
||||
} else if (angle_min != scan.angle_min ||
|
||||
angle_max != scan.angle_max ||
|
||||
size != scan.ranges.size())
|
||||
{
|
||||
throw std::runtime_error("laser_scan::ScanMask::addScan: inconsistantly sized scans added to mask");
|
||||
}
|
||||
|
||||
for (uint32_t i = 0; i < scan.ranges.size(); i++)
|
||||
{
|
||||
Sample* s = Sample::Extract(i, scan);
|
||||
|
||||
if (s != NULL)
|
||||
{
|
||||
SampleSet::iterator m = mask_.find(s);
|
||||
|
||||
if (m != mask_.end())
|
||||
{
|
||||
if ((*m)->range > s->range)
|
||||
{
|
||||
delete (*m);
|
||||
mask_.erase(m);
|
||||
mask_.insert(s);
|
||||
} else {
|
||||
delete s;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
mask_.insert(s);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool ScanMask::hasSample(Sample* s, float thresh)
|
||||
{
|
||||
if (s != NULL)
|
||||
{
|
||||
SampleSet::iterator m = mask_.find(s);
|
||||
if ( m != mask_.end())
|
||||
if (((*m)->range - thresh) < s->range)
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
|
||||
ScanProcessor::ScanProcessor(const sensor_msgs::LaserScan& scan, ScanMask& mask_, float mask_threshold)
|
||||
{
|
||||
scan_ = scan;
|
||||
|
||||
SampleSet* cluster = new SampleSet;
|
||||
|
||||
for (uint32_t i = 0; i < scan.ranges.size(); i++)
|
||||
{
|
||||
Sample* s = Sample::Extract(i, scan);
|
||||
|
||||
if (s != NULL)
|
||||
{
|
||||
if (!mask_.hasSample(s, mask_threshold))
|
||||
{
|
||||
cluster->insert(s);
|
||||
} else {
|
||||
delete s;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
clusters_.push_back(cluster);
|
||||
|
||||
}
|
||||
|
||||
ScanProcessor::~ScanProcessor()
|
||||
{
|
||||
for ( list<SampleSet*>::iterator c = clusters_.begin();
|
||||
c != clusters_.end();
|
||||
c++)
|
||||
delete (*c);
|
||||
}
|
||||
|
||||
void
|
||||
ScanProcessor::removeLessThan(uint32_t num)
|
||||
{
|
||||
list<SampleSet*>::iterator c_iter = clusters_.begin();
|
||||
while (c_iter != clusters_.end())
|
||||
{
|
||||
if ( (*c_iter)->size() < num )
|
||||
{
|
||||
delete (*c_iter);
|
||||
clusters_.erase(c_iter++);
|
||||
} else {
|
||||
++c_iter;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
ScanProcessor::splitConnected(float thresh)
|
||||
{
|
||||
list<SampleSet*> tmp_clusters;
|
||||
|
||||
list<SampleSet*>::iterator c_iter = clusters_.begin();
|
||||
|
||||
// For each cluster
|
||||
while (c_iter != clusters_.end())
|
||||
{
|
||||
// Go through the entire list
|
||||
while ((*c_iter)->size() > 0 )
|
||||
{
|
||||
// Take the first element
|
||||
SampleSet::iterator s_first = (*c_iter)->begin();
|
||||
|
||||
// Start a new queue
|
||||
list<Sample*> sample_queue;
|
||||
sample_queue.push_back(*s_first);
|
||||
|
||||
(*c_iter)->erase(s_first);
|
||||
|
||||
// Grow until we get to the end of the queue
|
||||
list<Sample*>::iterator s_q = sample_queue.begin();
|
||||
while (s_q != sample_queue.end())
|
||||
{
|
||||
int expand = (int)(asin( thresh / (*s_q)->range ) / scan_.angle_increment);
|
||||
|
||||
SampleSet::iterator s_rest = (*c_iter)->begin();
|
||||
|
||||
while ( (s_rest != (*c_iter)->end() &&
|
||||
(*s_rest)->index < (*s_q)->index + expand ) )
|
||||
{
|
||||
if ( (*s_rest)->range - (*s_q)->range > thresh)
|
||||
{
|
||||
break;
|
||||
}
|
||||
else if (sqrt( pow( (*s_q)->x - (*s_rest)->x, 2.0f) + pow( (*s_q)->y - (*s_rest)->y, 2.0f)) < thresh)
|
||||
{
|
||||
sample_queue.push_back(*s_rest);
|
||||
(*c_iter)->erase(s_rest++);
|
||||
break;
|
||||
} else {
|
||||
++s_rest;
|
||||
}
|
||||
}
|
||||
s_q++;
|
||||
}
|
||||
|
||||
// Move all the samples into the new cluster
|
||||
SampleSet* c = new SampleSet;
|
||||
for (s_q = sample_queue.begin(); s_q != sample_queue.end(); s_q++)
|
||||
c->insert(*s_q);
|
||||
|
||||
// Store the temporary clusters
|
||||
tmp_clusters.push_back(c);
|
||||
}
|
||||
|
||||
//Now that c_iter is empty, we can delete
|
||||
delete (*c_iter);
|
||||
|
||||
//And remove from the map
|
||||
clusters_.erase(c_iter++);
|
||||
}
|
||||
|
||||
clusters_.insert(clusters_.begin(), tmp_clusters.begin(), tmp_clusters.end());
|
||||
}
|
||||
337
src/laser_scan.cc
Normal file
337
src/laser_scan.cc
Normal file
@@ -0,0 +1,337 @@
|
||||
/*
|
||||
* Copyright (c) 2008, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
* * Neither the name of the Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived from
|
||||
* this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "laser_geometry/laser_geometry.h"
|
||||
#include <algorithm>
|
||||
|
||||
namespace laser_geometry
|
||||
{
|
||||
|
||||
void
|
||||
LaserProjection::projectLaser (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud & cloud_out, double range_cutoff,
|
||||
bool preservative, int mask)
|
||||
{
|
||||
boost::numeric::ublas::matrix<double> ranges(2, scan_in.get_ranges_size());
|
||||
|
||||
// Fill the ranges matrix
|
||||
for (unsigned int index = 0; index < scan_in.get_ranges_size(); index++)
|
||||
{
|
||||
ranges(0,index) = (double) scan_in.ranges[index];
|
||||
ranges(1,index) = (double) scan_in.ranges[index];
|
||||
}
|
||||
|
||||
|
||||
//Do the projection
|
||||
// NEWMAT::Matrix output = NEWMAT::SP(ranges, getUnitVectors(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment));
|
||||
boost::numeric::ublas::matrix<double> output = element_prod(ranges, getUnitVectors(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment, scan_in.get_ranges_size()));
|
||||
|
||||
//Stuff the output cloud
|
||||
cloud_out.header = scan_in.header;
|
||||
cloud_out.set_points_size (scan_in.get_ranges_size ());
|
||||
|
||||
// Define 4 indices in the channel array for each possible value type
|
||||
int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1;
|
||||
|
||||
cloud_out.set_channels_size(0);
|
||||
|
||||
// Check if the intensity bit is set
|
||||
if ((mask & MASK_INTENSITY) && scan_in.get_intensities_size () > 0)
|
||||
{
|
||||
int chan_size = cloud_out.get_channels_size ();
|
||||
cloud_out.set_channels_size (chan_size + 1);
|
||||
cloud_out.channels[0].name = "intensities";
|
||||
cloud_out.channels[0].set_values_size (scan_in.get_intensities_size ());
|
||||
idx_intensity = 0;
|
||||
}
|
||||
|
||||
// Check if the index bit is set
|
||||
if (mask & MASK_INDEX)
|
||||
{
|
||||
int chan_size = cloud_out.get_channels_size ();
|
||||
cloud_out.set_channels_size (chan_size +1);
|
||||
cloud_out.channels[chan_size].name = "index";
|
||||
cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
|
||||
idx_index = chan_size;
|
||||
}
|
||||
|
||||
// Check if the distance bit is set
|
||||
if (mask & MASK_DISTANCE)
|
||||
{
|
||||
int chan_size = cloud_out.get_channels_size ();
|
||||
cloud_out.set_channels_size (chan_size + 1);
|
||||
cloud_out.channels[chan_size].name = "distances";
|
||||
cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
|
||||
idx_distance = chan_size;
|
||||
}
|
||||
|
||||
if (mask & MASK_TIMESTAMP)
|
||||
{
|
||||
int chan_size = cloud_out.get_channels_size ();
|
||||
cloud_out.set_channels_size (chan_size + 1);
|
||||
cloud_out.channels[chan_size].name = "stamps";
|
||||
cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
|
||||
idx_timestamp = chan_size;
|
||||
}
|
||||
|
||||
if (range_cutoff < 0)
|
||||
range_cutoff = scan_in.range_max;
|
||||
else
|
||||
range_cutoff = std::min(range_cutoff, (double)scan_in.range_max);
|
||||
|
||||
unsigned int count = 0;
|
||||
for (unsigned int index = 0; index< scan_in.get_ranges_size(); index++)
|
||||
{
|
||||
if (preservative || ((ranges(0,index) < range_cutoff) && (ranges(0,index) >= scan_in.range_min))) //if valid or preservative
|
||||
{
|
||||
|
||||
cloud_out.points[count].x = output(0,index);
|
||||
cloud_out.points[count].y = output(1,index);
|
||||
cloud_out.points[count].z = 0.0;
|
||||
|
||||
//double x = cloud_out.points[count].x;
|
||||
//double y = cloud_out.points[count].y;
|
||||
//if(x*x + y*y < scan_in.range_min * scan_in.range_min){
|
||||
// ROS_INFO("(%.2f, %.2f)", cloud_out.points[count].x, cloud_out.points[count].y);
|
||||
//}
|
||||
|
||||
// Save the original point index
|
||||
if (idx_index != -1)
|
||||
cloud_out.channels[idx_index].values[count] = index;
|
||||
|
||||
// Save the original point distance
|
||||
if (idx_distance != -1)
|
||||
cloud_out.channels[idx_distance].values[count] = ranges (0, index);
|
||||
|
||||
// Save intensities channel
|
||||
if (scan_in.get_intensities_size() >= index)
|
||||
{ /// \todo optimize and catch length difference better
|
||||
if (idx_intensity != -1)
|
||||
cloud_out.channels[idx_intensity].values[count] = scan_in.intensities[index];
|
||||
}
|
||||
|
||||
// Save timestamps to seperate channel if asked for
|
||||
if( idx_timestamp != -1)
|
||||
cloud_out.channels[idx_timestamp].values[count] = (float)index*scan_in.time_increment;
|
||||
|
||||
count++;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//downsize if necessary
|
||||
cloud_out.set_points_size (count);
|
||||
for (unsigned int d = 0; d < cloud_out.get_channels_size (); d++)
|
||||
cloud_out.channels[d].set_values_size(count);
|
||||
};
|
||||
|
||||
const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors(float angle_min, float angle_max, float angle_increment, unsigned int length)
|
||||
{
|
||||
if (angle_min >= angle_max)
|
||||
{
|
||||
std::stringstream ss;
|
||||
ss << "LaserProjection min angle " << angle_min << " greater than max angle "<< angle_max;
|
||||
ROS_ERROR("%s", ss.str().c_str());
|
||||
throw std::runtime_error(ss.str()); //This would result in a bad alloc anyway so throwing instead
|
||||
}
|
||||
//construct string for lookup in the map
|
||||
std::stringstream anglestring;
|
||||
anglestring <<angle_min<<","<<angle_max<<","<<angle_increment<<","<<length;
|
||||
std::map<std::string, boost::numeric::ublas::matrix<double>* >::iterator it;
|
||||
it = unit_vector_map_.find(anglestring.str());
|
||||
//check the map for presense
|
||||
if (it != unit_vector_map_.end())
|
||||
return *((*it).second); //if present return
|
||||
|
||||
boost::numeric::ublas::matrix<double> * tempPtr = new boost::numeric::ublas::matrix<double>(2,length);
|
||||
for (unsigned int index = 0;index < length; index++)
|
||||
{
|
||||
(*tempPtr)(0,index) = cos(angle_min + (double) index * angle_increment);
|
||||
(*tempPtr)(1,index) = sin(angle_min + (double) index * angle_increment);
|
||||
}
|
||||
//store
|
||||
unit_vector_map_[anglestring.str()] = tempPtr;
|
||||
//and return
|
||||
return *tempPtr;
|
||||
};
|
||||
|
||||
|
||||
LaserProjection::~LaserProjection()
|
||||
{
|
||||
std::map<std::string, boost::numeric::ublas::matrix<double>*>::iterator it;
|
||||
it = unit_vector_map_.begin();
|
||||
while (it != unit_vector_map_.end())
|
||||
{
|
||||
delete (*it).second;
|
||||
it++;
|
||||
}
|
||||
};
|
||||
|
||||
void
|
||||
LaserProjection::transformLaserScanToPointCloud (const std::string &target_frame, sensor_msgs::PointCloud &cloud_out, const sensor_msgs::LaserScan &scan_in,
|
||||
tf::Transformer& tf, int mask, bool preservative)
|
||||
{
|
||||
cloud_out.header = scan_in.header;
|
||||
cloud_out.header.frame_id = target_frame;
|
||||
cloud_out.set_points_size (scan_in.get_ranges_size());
|
||||
|
||||
// Define 4 indices in the channel array for each possible value type
|
||||
int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1;
|
||||
|
||||
|
||||
cloud_out.set_channels_size(0);
|
||||
|
||||
// Check if the intensity bit is set
|
||||
if ((mask & MASK_INTENSITY) && scan_in.get_intensities_size () > 0)
|
||||
{
|
||||
int chan_size = cloud_out.get_channels_size ();
|
||||
cloud_out.set_channels_size (chan_size + 1);
|
||||
cloud_out.channels[0].name = "intensities";
|
||||
cloud_out.channels[0].set_values_size (scan_in.get_intensities_size ());
|
||||
idx_intensity = 0;
|
||||
}
|
||||
|
||||
// Check if the index bit is set
|
||||
if (mask & MASK_INDEX)
|
||||
{
|
||||
int chan_size = cloud_out.get_channels_size ();
|
||||
cloud_out.set_channels_size (chan_size + 1);
|
||||
cloud_out.channels[chan_size].name = "index";
|
||||
cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
|
||||
idx_index = chan_size;
|
||||
}
|
||||
|
||||
// Check if the distance bit is set
|
||||
if (mask & MASK_DISTANCE)
|
||||
{
|
||||
int chan_size = cloud_out.get_channels_size ();
|
||||
cloud_out.set_channels_size (chan_size + 1);
|
||||
cloud_out.channels[chan_size].name = "distances";
|
||||
cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
|
||||
idx_distance = chan_size;
|
||||
}
|
||||
|
||||
if (mask & MASK_TIMESTAMP)
|
||||
{
|
||||
int chan_size = cloud_out.get_channels_size ();
|
||||
cloud_out.set_channels_size (chan_size + 1);
|
||||
cloud_out.channels[chan_size].name = "stamps";
|
||||
cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
|
||||
idx_timestamp = chan_size;
|
||||
}
|
||||
|
||||
tf::Stamped<tf::Point> pointIn;
|
||||
tf::Stamped<tf::Point> pointOut;
|
||||
|
||||
pointIn.frame_id_ = scan_in.header.frame_id;
|
||||
|
||||
///\todo this can be optimized
|
||||
sensor_msgs::PointCloud intermediate; //optimize out
|
||||
|
||||
projectLaser (scan_in, intermediate, -1.0, true, mask);
|
||||
|
||||
// Extract transforms for the beginning and end of the laser scan
|
||||
ros::Time start_time = scan_in.header.stamp ;
|
||||
ros::Time end_time = scan_in.header.stamp + ros::Duration().fromSec(scan_in.get_ranges_size()*scan_in.time_increment) ;
|
||||
|
||||
tf::Stamped<tf::Transform> start_transform ;
|
||||
tf::Stamped<tf::Transform> end_transform ;
|
||||
tf::Stamped<tf::Transform> cur_transform ;
|
||||
|
||||
tf.lookupTransform(target_frame, scan_in.header.frame_id, start_time, start_transform) ;
|
||||
tf.lookupTransform(target_frame, scan_in.header.frame_id, end_time, end_transform) ;
|
||||
|
||||
unsigned int count = 0;
|
||||
for (unsigned int i = 0; i < scan_in.get_ranges_size(); i++)
|
||||
{
|
||||
if (preservative || (scan_in.ranges[i] < scan_in.range_max && scan_in.ranges[i] > scan_in.range_min)) //only when valid
|
||||
{
|
||||
// Looking up transforms in tree is too expensive. Need more optimized way
|
||||
/*
|
||||
pointIn = tf::Stamped<tf::Point>(btVector3(intermediate.points[i].x, intermediate.points[i].y, intermediate.points[i].z),
|
||||
ros::Time(scan_in.header.stamp.to_ull() + (uint64_t) (scan_in.time_increment * 1000000000)),
|
||||
pointIn.frame_id_ = scan_in.header.frame_id);///\todo optimize to no copy
|
||||
transformPoint(target_frame, pointIn, pointOut);
|
||||
*/
|
||||
|
||||
// Instead, assume constant motion during the laser-scan, and use slerp to compute intermediate transforms
|
||||
btScalar ratio = i / ( (double) scan_in.get_ranges_size() - 1.0) ;
|
||||
|
||||
//! \todo Make a function that performs both the slerp and linear interpolation needed to interpolate a Full Transform (Quaternion + Vector)
|
||||
|
||||
//Interpolate translation
|
||||
btVector3 v (0, 0, 0);
|
||||
v.setInterpolate3(start_transform.getOrigin(), end_transform.getOrigin(), ratio) ;
|
||||
cur_transform.setOrigin(v) ;
|
||||
|
||||
//Interpolate rotation
|
||||
btQuaternion q1, q2 ;
|
||||
start_transform.getBasis().getRotation(q1) ;
|
||||
end_transform.getBasis().getRotation(q2) ;
|
||||
|
||||
// Compute the slerp-ed rotation
|
||||
cur_transform.setRotation( slerp( q1, q2 , ratio) ) ;
|
||||
|
||||
// Apply the transform to the current point
|
||||
btVector3 pointIn(intermediate.points[i].x, intermediate.points[i].y, intermediate.points[i].z) ;
|
||||
btVector3 pointOut = cur_transform * pointIn ;
|
||||
|
||||
// Copy transformed point into cloud
|
||||
cloud_out.points[count].x = pointOut.x();
|
||||
cloud_out.points[count].y = pointOut.y();
|
||||
cloud_out.points[count].z = pointOut.z();
|
||||
|
||||
// Copy index over from projected point cloud
|
||||
if (idx_index != -1)
|
||||
cloud_out.channels[idx_index].values[count] = intermediate.channels[idx_index].values[i];
|
||||
|
||||
// Save the original point distance
|
||||
if (idx_distance != -1)
|
||||
cloud_out.channels[idx_distance].values[count] = scan_in.ranges[i];
|
||||
|
||||
if (scan_in.get_intensities_size() >= i)
|
||||
{ /// \todo optimize and catch length difference better
|
||||
if (idx_intensity != -1)
|
||||
cloud_out.channels[idx_intensity].values[count] = scan_in.intensities[i];
|
||||
}
|
||||
|
||||
if (idx_timestamp != -1)
|
||||
cloud_out.channels[idx_timestamp].values[count] = (float)i * scan_in.time_increment;
|
||||
|
||||
count++;
|
||||
}
|
||||
|
||||
}
|
||||
//downsize if necessary
|
||||
cloud_out.set_points_size (count);
|
||||
for (unsigned int d = 0; d < cloud_out.get_channels_size (); d++)
|
||||
cloud_out.channels[d].set_values_size (count);
|
||||
}
|
||||
|
||||
|
||||
} //laser_scan
|
||||
Reference in New Issue
Block a user