DuongTD/laser_geometry
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix cpplint

Browse Source
This commit is contained in:
Martin Idel 2018-03-14 13:46:00 +01:00
parent 1dd3314e3b
commit 2783d803f8
3 changed files with 123 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
include/laser_geometry/laser_geometry.hpp
View File

@ -27,26 +27,20 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef LASER_SCAN_UTILS_LASERSCAN_H
#define LASER_SCAN_UTILS_LASERSCAN_H
#ifndef LASER_GEOMETRY__LASER_GEOMETRY_HPP_
#define LASER_GEOMETRY__LASER_GEOMETRY_HPP_
#include <map>
#include <iostream>
#include <sstream>
#include <string>
#include <Eigen/Core> // NOLINT (cpplint cannot handle include order here)
#include "tf2/buffer_core.h"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#ifndef ROS_DEBUG
#define ROS_DEBUG(...)
#endif // !ROS_DEBUG
#ifndef ROS_ASSERT
#define ROS_ASSERT(...)
#endif // !ROS_ASSERT
#include <Eigen/Core>
namespace laser_geometry
{
// NOTE: invalid scan errors (will be present in LaserScan.msg in D-Turtle)
@ -56,7 +50,7 @@ const float LASER_SCAN_MAX_RANGE = -3.0;
namespace channel_option
{
//! Enumerated output channels options.
// Enumerated output channels options.
/*!
* An OR'd set of these options is passed as the final argument of
* the projectLaser and transformLaserScanToPointCloud calls to
@ -72,7 +66,7 @@ enum ChannelOption
Viewpoint = 0x10, //!< Enable "viewpoint" channel
Default = (Intensity | Index) //!< Enable "intensities" and "index" channels
};
}
} // namespace channel_option
//! \brief A Class to Project Laser Scan
/*!
@ -98,12 +92,11 @@ enum ChannelOption
*/
class LaserProjection
{
public:
LaserProjection()
: angle_min_(0), angle_max_(0) {}
//! Project a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud2
// Project a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud2
/*!
* Project a single laser scan from a linear array into a 3D
* point cloud. The generated cloud will be in the same frame
@ -128,7 +121,7 @@ public:
projectLaser_(scan_in, cloud_out, range_cutoff, channel_options);
}
//! Transform a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud2 in target frame
// Transform a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud2 in target frame
/*!
* Transform a single laser scan from a linear array into a 3D
* point cloud, accounting for movement of the laser over the
@ -163,14 +156,14 @@ public:
}
private:
//! Internal hidden representation of projectLaser
// Internal hidden representation of projectLaser
void projectLaser_(
const sensor_msgs::msg::LaserScan & scan_in,
sensor_msgs::msg::PointCloud2 & cloud_out,
double range_cutoff,
int channel_options);
//! Internal hidden representation of transformLaserScanToPointCloud2
// Internal hidden representation of transformLaserScanToPointCloud2
void transformLaserScanToPointCloud_(
const std::string & target_frame,
const sensor_msgs::msg::LaserScan & scan_in,
@ -179,7 +172,7 @@ private:
double range_cutoff,
int channel_options);
//! Function used by the several forms of transformLaserScanToPointCloud_
// Function used by the several forms of transformLaserScanToPointCloud_
void transformLaserScanToPointCloud_(
const std::string & target_frame,
const sensor_msgs::msg::LaserScan & scan_in,
@ -191,12 +184,12 @@ private:
double range_cutoff,
int channel_options);
//! Internal map of pointers to stored values
// Internal map of pointers to stored values
float angle_min_;
float angle_max_;
Eigen::ArrayXXd co_sine_map_;
};
}
} // namespace laser_geometry
#endif //LASER_SCAN_UTILS_LASERSCAN_H
#endif // LASER_GEOMETRY__LASER_GEOMETRY_HPP_

100
src/laser_geometry.cpp
View File

@ -30,15 +30,17 @@
#include "laser_geometry/laser_geometry.hpp"
#include <algorithm>
#include <string>
#include "rclcpp/time.hpp"
#define TIME rclcpp::Time
#define POINT_FIELD sensor_msgs::msg::PointField
// TODO: fix definitions
typedef double tfScalar;
#include <tf2/LinearMath/Transform.h>
#include "tf2/LinearMath/Transform.h"
namespace laser_geometry
{
@ -54,22 +56,24 @@ void LaserProjection::projectLaser_(
// Get the ranges into Eigen format
for (size_t i = 0; i < n_pts; ++i) {
ranges(i, 0) = (double) scan_in.ranges[i];
ranges(i, 1) = (double) scan_in.ranges[i];
ranges(i, 0) = static_cast<double>(scan_in.ranges[i]);
ranges(i, 1) = static_cast<double>(scan_in.ranges[i]);
}
// Check if our existing co_sine_map is valid
if (co_sine_map_.rows() != (int)n_pts || angle_min_ != scan_in.angle_min ||
if (co_sine_map_.rows() != static_cast<int>(n_pts) || angle_min_ != scan_in.angle_min ||
angle_max_ != scan_in.angle_max)
{
ROS_DEBUG("[projectLaser] No precomputed map given. Computing one.");
// ROS_DEBUG("[projectLaser] No precomputed map given. Computing one.");
co_sine_map_ = Eigen::ArrayXXd(n_pts, 2);
angle_min_ = scan_in.angle_min;
angle_max_ = scan_in.angle_max;
// Spherical->Cartesian projection
for (size_t i = 0; i < n_pts; ++i) {
co_sine_map_(i, 0) = cos(scan_in.angle_min + (double) i * scan_in.angle_increment);
co_sine_map_(i, 1) = sin(scan_in.angle_min + (double) i * scan_in.angle_increment);
co_sine_map_(i, 0) =
cos(scan_in.angle_min + static_cast<double>(i) * scan_in.angle_increment);
co_sine_map_(i, 1) =
sin(scan_in.angle_min + static_cast<double>(i) * scan_in.angle_increment);
}
}
@ -97,7 +101,7 @@ void LaserProjection::projectLaser_(
int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1, idx_vpx = -1,
idx_vpy = -1, idx_vpz = -1;
//now, we need to check what fields we need to store
// now, we need to check what fields we need to store
int offset = 12;
if ((channel_options & channel_option::Intensity) && scan_in.intensities.size() > 0) {
int field_size = cloud_out.fields.size();
@ -181,10 +185,10 @@ void LaserProjection::projectLaser_(
unsigned int count = 0;
for (size_t i = 0; i < n_pts; ++i) {
//check to see if we want to keep the point
// check to see if we want to keep the point
const float range = scan_in.ranges[i];
if (range < range_cutoff && range >= scan_in.range_min) {
float * pstep = (float *)&cloud_out.data[count * cloud_out.point_step];
auto pstep = reinterpret_cast<float *>(&cloud_out.data[count * cloud_out.point_step]);
// Copy XYZ
pstep[0] = output(i, 0);
@ -196,9 +200,9 @@ void LaserProjection::projectLaser_(
pstep[idx_intensity] = scan_in.intensities[i];
}
//Copy index
// Copy index
if (idx_index != -1) {
((int *)(pstep))[idx_index] = i;
reinterpret_cast<int *>(pstep)[idx_index] = i;
}
// Copy distance
@ -218,12 +222,12 @@ void LaserProjection::projectLaser_(
pstep[idx_vpz] = 0;
}
//make sure to increment count
// make sure to increment count
++count;
}
/* TODO: Why was this done in this way, I don't get this at all, you end up with a ton of points with NaN values
* why can't you just leave them out?
/* TODO(anonymous): Why was this done in this way, I don't get this at all, you end up with a
* ton of points with NaN values why can't you just leave them out?
*
// Invalid measurement?
if (scan_in.ranges[i] >= range_cutoff || scan_in.ranges[i] <= scan_in.range_min)
@ -254,7 +258,7 @@ void LaserProjection::projectLaser_(
*/
}
//resize if necessary
// resize if necessary
cloud_out.width = count;
cloud_out.row_step = cloud_out.point_step * cloud_out.width;
cloud_out.data.resize(cloud_out.row_step * cloud_out.height);
@ -271,63 +275,64 @@ void LaserProjection::transformLaserScanToPointCloud_(
double range_cutoff,
int channel_options)
{
//check if the user has requested the index field
// check if the user has requested the index field
bool requested_index = false;
if ((channel_options & channel_option::Index)) {
requested_index = true;
}
//we'll enforce that we get index values for the laser scan so that we
//ensure that we use the correct timestamps
// we'll enforce that we get index values for the laser scan so that we
// ensure that we use the correct timestamps
channel_options |= channel_option::Index;
projectLaser_(scan_in, cloud_out, range_cutoff, channel_options);
//we'll assume no associated viewpoint by default
// we'll assume no associated viewpoint by default
bool has_viewpoint = false;
uint32_t vp_x_offset = 0;
//we need to find the offset of the intensity field in the point cloud
//we also know that the index field is guaranteed to exist since we
//set the channel option above. To be really safe, it might be worth
//putting in a check at some point, but I'm just going to put in an
//assert for now
// we need to find the offset of the intensity field in the point cloud
// we also know that the index field is guaranteed to exist since we
// set the channel option above. To be really safe, it might be worth
// putting in a check at some point, but I'm just going to put in an
// assert for now
uint32_t index_offset = 0;
for (unsigned int i = 0; i < cloud_out.fields.size(); ++i) {
if (cloud_out.fields[i].name == "index") {
index_offset = cloud_out.fields[i].offset;
}
//we want to check if the cloud has a viewpoint associated with it
//checking vp_x should be sufficient since vp_x, vp_y, and vp_z all
//get put in together
// we want to check if the cloud has a viewpoint associated with it
// checking vp_x should be sufficient since vp_x, vp_y, and vp_z all
// get put in together
if (cloud_out.fields[i].name == "vp_x") {
has_viewpoint = true;
vp_x_offset = cloud_out.fields[i].offset;
}
}
ROS_ASSERT(index_offset > 0);
assert(index_offset > 0);
cloud_out.header.frame_id = target_frame;
tf2::Transform cur_transform;
double ranges_norm = 1 / ((double) scan_in.ranges.size() - 1.0);
double ranges_norm = 1 / (static_cast<double>(scan_in.ranges.size()) - 1.0);
//we want to loop through all the points in the cloud
// we want to loop through all the points in the cloud
for (size_t i = 0; i < cloud_out.width; ++i) {
// Apply the transform to the current point
float * pstep = (float *)&cloud_out.data[i * cloud_out.point_step + 0];
float * pstep = reinterpret_cast<float *>(&cloud_out.data[i * cloud_out.point_step + 0]);
//find the index of the point
// find the index of the point
uint32_t pt_index;
memcpy(&pt_index, &cloud_out.data[i * cloud_out.point_step + index_offset], sizeof(uint32_t));
// Assume constant motion during the laser-scan, and use slerp to compute intermediate transforms
// Assume constant motion during the laser-scan and use slerp to compute intermediate transforms
tfScalar ratio = pt_index * ranges_norm;
//! \todo Make a function that performs both the slerp and linear interpolation needed to interpolate a Full Transform (Quaternion + Vector)
// TODO(anon): Make a function that performs both the slerp and linear interpolation needed to
// interpolate a Full Transform (Quaternion + Vector)
// Interpolate translation
tf2::Vector3 v(0, 0, 0);
v.setInterpolate3(origin_start, origin_end, ratio);
@ -346,7 +351,8 @@ void LaserProjection::transformLaserScanToPointCloud_(
// Convert the viewpoint as well
if (has_viewpoint) {
float * vpstep = (float *)&cloud_out.data[i * cloud_out.point_step + vp_x_offset];
auto vpstep =
reinterpret_cast<float *>(&cloud_out.data[i * cloud_out.point_step + vp_x_offset]);
point_in = tf2::Vector3(vpstep[0], vpstep[1], vpstep[2]);
point_out = cur_transform * point_in;
@ -357,18 +363,18 @@ void LaserProjection::transformLaserScanToPointCloud_(
}
}
//if the user didn't request the index field, then we need to copy the PointCloud and drop it
// if the user didn't request the index field, then we need to copy the PointCloud and drop it
if (!requested_index) {
sensor_msgs::msg::PointCloud2 cloud_without_index;
//copy basic meta data
// copy basic meta data
cloud_without_index.header = cloud_out.header;
cloud_without_index.width = cloud_out.width;
cloud_without_index.height = cloud_out.height;
cloud_without_index.is_bigendian = cloud_out.is_bigendian;
cloud_without_index.is_dense = cloud_out.is_dense;
//copy the fields
// copy the fields
cloud_without_index.fields.resize(cloud_out.fields.size());
unsigned int field_count = 0;
unsigned int offset_shift = 0;
@ -378,22 +384,22 @@ void LaserProjection::transformLaserScanToPointCloud_(
cloud_without_index.fields[field_count].offset -= offset_shift;
++field_count;
} else {
//once we hit the index, we'll set the shift
// once we hit the index, we'll set the shift
offset_shift = 4;
}
}
//resize the fields
// resize the fields
cloud_without_index.fields.resize(field_count);
//compute the size of the new data
// compute the size of the new data
cloud_without_index.point_step = cloud_out.point_step - offset_shift;
cloud_without_index.row_step = cloud_without_index.point_step * cloud_without_index.width;
cloud_without_index.data.resize(cloud_without_index.row_step * cloud_without_index.height);
uint32_t i = 0;
uint32_t j = 0;
//copy over the data from one cloud to the other
// copy over the data from one cloud to the other
while (i < cloud_out.data.size()) {
if ((i % cloud_out.point_step) < index_offset ||
(i % cloud_out.point_step) >= (index_offset + 4))
@ -403,7 +409,7 @@ void LaserProjection::transformLaserScanToPointCloud_(
i++;
}
//make sure to actually set the output
// make sure to actually set the output
cloud_out = cloud_without_index;
}
}
@ -418,7 +424,7 @@ void LaserProjection::transformLaserScanToPointCloud_(
{
TIME start_time = scan_in.header.stamp;
TIME end_time = scan_in.header.stamp;
// TODO: reconcile all the different time constructs
// TODO(anonymous): reconcile all the different time constructs
if (!scan_in.ranges.empty()) {
end_time = end_time + rclcpp::Duration((scan_in.ranges.size() - 1) * scan_in.time_increment, 0);
}
@ -456,4 +462,4 @@ void LaserProjection::transformLaserScanToPointCloud_(
channel_options);
}
} //laser_geometry
} // namespace laser_geometry

102
test/projection_test.cpp
View File

@ -30,6 +30,7 @@
#include <gtest/gtest.h>
#include <sys/time.h>
#include <math.h>
#include <vector>
#include "rclcpp/rclcpp.hpp"
@ -86,11 +87,17 @@ sensor_msgs::msg::LaserScan build_constant_scan(const ScanOptions & options)
scan.intensities.push_back(options.intensity_);
}
scan.time_increment = options.scan_time_.nanoseconds() / (double)i;
scan.time_increment = options.scan_time_.nanoseconds() / static_cast<double>(i);
return scan;
}
template<typename T>
T cloudData(sensor_msgs::msg::PointCloud2 cloud_out, uint32_t index)
{
return *reinterpret_cast<T *>(&cloud_out.data[index]);
}
TEST(laser_geometry, projectLaser2) {
double tolerance = 1e-12;
laser_geometry::LaserProjection projector;
@ -124,10 +131,10 @@ TEST(laser_geometry, projectLaser2) {
max_angles.push_back(M_PI / 4);
max_angles.push_back(M_PI / 8);
// angle_increments.push_back(-M_PI/180); // -one degree
angle_increments.push_back(M_PI / 180); // one degree
angle_increments.push_back(M_PI / 360); // half degree
angle_increments.push_back(M_PI / 720); // quarter degree
angle_increments.push_back(-M_PI / 180); // -one degree
angle_increments.push_back(M_PI / 180); // one degree
angle_increments.push_back(M_PI / 360); // half degree
angle_increments.push_back(M_PI / 720); // quarter degree
scan_times.push_back(rclcpp::Duration(1 / 40));
scan_times.push_back(rclcpp::Duration(1 / 20));
@ -156,28 +163,28 @@ TEST(laser_geometry, projectLaser2) {
sensor_msgs::msg::PointCloud2 cloud_out;
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
EXPECT_EQ(cloud_out.fields.size(), 4u);
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
EXPECT_EQ(cloud_out.fields.size(), 4u);
projector.projectLaser(scan, cloud_out, -1.0);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
EXPECT_EQ(cloud_out.fields.size(), 5u);
projector.projectLaser(scan, cloud_out, -1.0,
laser_geometry::channel_option::Intensity |
laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
EXPECT_EQ(cloud_out.fields.size(), 5u);
projector.projectLaser(scan, cloud_out, -1.0,
laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index |
laser_geometry::channel_option::Distance);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
EXPECT_EQ(cloud_out.fields.size(), 6u);
projector.projectLaser(scan, cloud_out, -1.0,
laser_geometry::channel_option::Intensity |
laser_geometry::channel_option::Index |
laser_geometry::channel_option::Distance |
laser_geometry::channel_option::Timestamp);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
EXPECT_EQ(cloud_out.fields.size(), 7u);
unsigned int valid_points = 0;
for (unsigned int i = 0; i < scan.ranges.size(); i++) {
@ -210,21 +217,22 @@ TEST(laser_geometry, projectLaser2) {
}
for (unsigned int i = 0; i < cloud_out.width; i++) {
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + x_offset],
(float)((double)(scan.ranges[i]) *
cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + y_offset],
(float)((double)(scan.ranges[i]) *
sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + z_offset], 0, tolerance);
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + intensity_offset],
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + x_offset),
static_cast<float>(static_cast<double>(scan.ranges[i]) *
cos(static_cast<double>(scan.angle_min) + i * static_cast<double>(scan.angle_increment))),
tolerance);
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + y_offset),
static_cast<float>(static_cast<double>(scan.ranges[i]) *
sin(static_cast<double>(scan.angle_min) + i * static_cast<double>(scan.angle_increment))),
tolerance);
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + z_offset), 0, tolerance);
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + intensity_offset),
scan.intensities[i], tolerance); // intensity
EXPECT_NEAR(*(uint32_t *)&cloud_out.data[i * cloud_out.point_step + index_offset], i,
EXPECT_NEAR(cloudData<uint32_t>(cloud_out, i * cloud_out.point_step + index_offset), i,
tolerance); // index
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + distance_offset],
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + distance_offset),
scan.ranges[i], tolerance); // ranges
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + stamps_offset],
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + stamps_offset),
(float)i * scan.time_increment, tolerance); // timestamps
}
} catch (BuildScanException & ex) {
@ -237,7 +245,6 @@ TEST(laser_geometry, projectLaser2) {
}
TEST(laser_geometry, transformLaserScanToPointCloud2) {
tf2::BufferCore tf2;
double tolerance = 1e-12;
@ -272,10 +279,10 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) {
max_angles.push_back(M_PI / 4);
max_angles.push_back(M_PI / 8);
angle_increments.push_back(-M_PI / 180); // -one degree
angle_increments.push_back(M_PI / 180); // one degree
angle_increments.push_back(M_PI / 360); // half degree
angle_increments.push_back(M_PI / 720); // quarter degree
angle_increments.push_back(-M_PI / 180); // -one degree
angle_increments.push_back(M_PI / 180); // one degree
angle_increments.push_back(M_PI / 360); // half degree
angle_increments.push_back(M_PI / 720); // quarter degree
scan_times.push_back(rclcpp::Duration(1 / 40));
scan_times.push_back(rclcpp::Duration(1 / 20));
@ -307,31 +314,31 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) {
sensor_msgs::msg::PointCloud2 cloud_out;
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0,
laser_geometry::channel_option::None);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3);
EXPECT_EQ(cloud_out.fields.size(), 3u);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0,
laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
EXPECT_EQ(cloud_out.fields.size(), 4u);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0,
laser_geometry::channel_option::Intensity);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
EXPECT_EQ(cloud_out.fields.size(), 4u);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
EXPECT_EQ(cloud_out.fields.size(), 5u);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0,
laser_geometry::channel_option::Intensity |
laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
EXPECT_EQ(cloud_out.fields.size(), 5u);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0,
laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index |
laser_geometry::channel_option::Distance);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
EXPECT_EQ(cloud_out.fields.size(), 6u);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0,
laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index |
laser_geometry::channel_option::Distance |
laser_geometry::channel_option::Timestamp);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
EXPECT_EQ(cloud_out.fields.size(), 7u);
EXPECT_EQ(cloud_out.is_dense, false);
@ -365,22 +372,23 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) {
}
for (unsigned int i = 0; i < cloud_out.width; i++) {
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + x_offset],
(float)((double)(scan.ranges[i]) *
cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + y_offset],
(float)((double)(scan.ranges[i]) *
sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + z_offset], 0, tolerance);
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + intensity_offset],
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + x_offset),
static_cast<float>(static_cast<double>(scan.ranges[i]) *
cos(static_cast<double>(scan.angle_min) + i * static_cast<double>(scan.angle_increment))),
tolerance);
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + y_offset),
static_cast<float>(static_cast<double>(scan.ranges[i]) *
sin(static_cast<double>(scan.angle_min) + i * static_cast<double>(scan.angle_increment))),
tolerance);
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + z_offset), 0, tolerance);
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + intensity_offset),
scan.intensities[i], tolerance); // intensity
EXPECT_NEAR(*(uint32_t *)&cloud_out.data[i * cloud_out.point_step + index_offset], i,
EXPECT_NEAR(cloudData<uint32_t>(cloud_out, i * cloud_out.point_step + index_offset), i,
tolerance); // index
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + distance_offset],
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + distance_offset),
scan.ranges[i], tolerance); // ranges
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + stamps_offset],
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + stamps_offset),
(float)i * scan.time_increment, tolerance); // timestamps
}
} catch (BuildScanException & ex) {
// make sure it is not a false exception