kindred/create
1
1
Fork 0
Code Issues 37 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity

Points: fix many linter warnings

Browse Source
This commit is contained in:
wmayer
2024-03-12 14:09:17 +01:00
committed by wwmayer
parent be5dce2e9e
commit 0b8990c96c
14 changed files with 298 additions and 242 deletions
Download Patch File Download Diff File Expand all files Collapse all files

259
src/Mod/Points/App/PointsAlgos.cpp
View File

@@ -124,11 +124,7 @@ void PointsAlgos::LoadAscii(PointKernel& points, const char* FileName)
// ----------------------------------------------------------------------------
Reader::Reader()
{
width = 0;
height = 0;
}
Reader::Reader() = default;
Reader::~Reader() = default;
@@ -253,11 +249,8 @@ class DataStreambuf: public std::streambuf
public:
explicit DataStreambuf(const std::vector<char>& data)
: _buffer(data)
{
_beg = 0;
_end = data.size();
_cur = 0;
}
, _end(int(data.size()))
{}
~DataStreambuf() override = default;
protected:
@@ -291,8 +284,9 @@ protected:
}
pos_type seekoff(std::streambuf::off_type off,
std::ios_base::seekdir way,
std::ios_base::openmode = std::ios::in | std::ios::out) override
std::ios_base::openmode mode = std::ios::in | std::ios::out) override
{
(void)mode;
int p_pos = -1;
if (way == std::ios_base::beg) {
p_pos = _beg;
@@ -331,9 +325,10 @@ public:
private:
const std::vector<char>& _buffer;
int _beg, _end, _cur;
int _beg {0}, _end {0}, _cur {0};
};
// NOLINTBEGIN
// Taken from https://github.com/PointCloudLibrary/pcl/blob/master/io/src/lzf.cpp
unsigned int
lzfDecompress(const void* const in_data, unsigned int in_len, void* out_data, unsigned int out_len)
@@ -544,6 +539,7 @@ lzfDecompress(const void* const in_data, unsigned int in_len, void* out_data, un
return (static_cast<unsigned int>(op - static_cast<unsigned char*>(out_data)));
}
} // namespace Points
// NOLINTEND
PlyReader::PlyReader() = default;
@@ -561,7 +557,7 @@ void PlyReader::read(const std::string& filename)
std::vector<std::string> types;
std::vector<int> sizes;
std::size_t offset = 0;
std::size_t numPoints = readHeader(inp, format, offset, fields, types, sizes);
Eigen::Index numPoints = Eigen::Index(readHeader(inp, format, offset, fields, types, sizes));
Eigen::MatrixXd data(numPoints, fields.size());
if (format == "ascii") {
@@ -575,31 +571,31 @@ void PlyReader::read(const std::string& filename)
}
std::vector<std::string>::iterator it;
std::size_t max_size = std::numeric_limits<std::size_t>::max();
Eigen::Index max_size = std::numeric_limits<Eigen::Index>::max();
// x field
std::size_t x = max_size;
Eigen::Index x = max_size;
it = std::find(fields.begin(), fields.end(), "x");
if (it != fields.end()) {
x = std::distance(fields.begin(), it);
}
// y field
std::size_t y = max_size;
Eigen::Index y = max_size;
it = std::find(fields.begin(), fields.end(), "y");
if (it != fields.end()) {
y = std::distance(fields.begin(), it);
}
// z field
std::size_t z = max_size;
Eigen::Index z = max_size;
it = std::find(fields.begin(), fields.end(), "z");
if (it != fields.end()) {
z = std::distance(fields.begin(), it);
}
// normal x field
std::size_t normal_x = max_size;
Eigen::Index normal_x = max_size;
it = std::find(fields.begin(), fields.end(), "normal_x");
if (it == fields.end()) {
it = std::find(fields.begin(), fields.end(), "nx");
@@ -609,7 +605,7 @@ void PlyReader::read(const std::string& filename)
}
// normal y field
std::size_t normal_y = max_size;
Eigen::Index normal_y = max_size;
it = std::find(fields.begin(), fields.end(), "normal_y");
if (it == fields.end()) {
it = std::find(fields.begin(), fields.end(), "ny");
@@ -619,7 +615,7 @@ void PlyReader::read(const std::string& filename)
}
// normal z field
std::size_t normal_z = max_size;
Eigen::Index normal_z = max_size;
it = std::find(fields.begin(), fields.end(), "normal_z");
if (it == fields.end()) {
it = std::find(fields.begin(), fields.end(), "nz");
@@ -629,14 +625,17 @@ void PlyReader::read(const std::string& filename)
}
// intensity field
std::size_t greyvalue = max_size;
Eigen::Index greyvalue = max_size;
it = std::find(fields.begin(), fields.end(), "intensity");
if (it != fields.end()) {
greyvalue = std::distance(fields.begin(), it);
}
// rgb(a) field
std::size_t red = max_size, green = max_size, blue = max_size, alpha = max_size;
Eigen::Index red = max_size;
Eigen::Index green = max_size;
Eigen::Index blue = max_size;
Eigen::Index alpha = max_size;
it = std::find(fields.begin(), fields.end(), "red");
if (it != fields.end()) {
red = std::distance(fields.begin(), it);
@@ -665,22 +664,22 @@ void PlyReader::read(const std::string& filename)
if (hasData) {
points.reserve(numPoints);
for (std::size_t i = 0; i < numPoints; i++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
points.push_back(Base::Vector3d(data(i, x), data(i, y), data(i, z)));
}
}
if (hasData && hasNormal) {
normals.reserve(numPoints);
for (std::size_t i = 0; i < numPoints; i++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
normals.emplace_back(data(i, normal_x), data(i, normal_y), data(i, normal_z));
}
}
if (hasData && hasIntensity) {
intensity.reserve(numPoints);
for (std::size_t i = 0; i < numPoints; i++) {
intensity.push_back(data(i, greyvalue));
for (Eigen::Index i = 0; i < numPoints; i++) {
intensity.push_back(static_cast<float>(data(i, greyvalue)));
}
}
@@ -688,26 +687,26 @@ void PlyReader::read(const std::string& filename)
colors.reserve(numPoints);
float a = 1.0;
if (types[red] == "uchar") {
for (std::size_t i = 0; i < numPoints; i++) {
float r = data(i, red);
float g = data(i, green);
float b = data(i, blue);
for (Eigen::Index i = 0; i < numPoints; i++) {
float r = static_cast<float>(data(i, red));
float g = static_cast<float>(data(i, green));
float b = static_cast<float>(data(i, blue));
if (alpha != max_size) {
a = data(i, alpha);
a = static_cast<float>(data(i, alpha));
}
colors.emplace_back(static_cast<float>(r) / 255.0f,
static_cast<float>(g) / 255.0f,
static_cast<float>(b) / 255.0f,
static_cast<float>(a) / 255.0f);
colors.emplace_back(static_cast<float>(r) / 255.0F,
static_cast<float>(g) / 255.0F,
static_cast<float>(b) / 255.0F,
static_cast<float>(a) / 255.0F);
}
}
else if (types[red] == "float") {
for (std::size_t i = 0; i < numPoints; i++) {
float r = data(i, red);
float g = data(i, green);
float b = data(i, blue);
for (Eigen::Index i = 0; i < numPoints; i++) {
float r = static_cast<float>(data(i, red));
float g = static_cast<float>(data(i, green));
float b = static_cast<float>(data(i, blue));
if (alpha != max_size) {
a = data(i, alpha);
a = static_cast<float>(data(i, alpha));
}
colors.emplace_back(r, g, b, a);
}
@@ -722,7 +721,8 @@ std::size_t PlyReader::readHeader(std::istream& in,
std::vector<std::string>& types,
std::vector<int>& sizes)
{
std::string line, element;
std::string line;
std::string element;
std::vector<std::string> list;
std::size_t numPoints = 0;
// a pair of numbers of elements and the total size of the properties
@@ -887,9 +887,9 @@ std::size_t PlyReader::readHeader(std::istream& in,
void PlyReader::readAscii(std::istream& inp, std::size_t offset, Eigen::MatrixXd& data)
{
std::string line;
std::size_t row = 0;
std::size_t numPoints = data.rows();
std::size_t numFields = data.cols();
Eigen::Index row = 0;
Eigen::Index numPoints = Eigen::Index(data.rows());
Eigen::Index numFields = Eigen::Index(data.cols());
std::vector<std::string> list;
while (std::getline(inp, line) && row < numPoints) {
if (line.empty()) {
@@ -907,7 +907,8 @@ void PlyReader::readAscii(std::istream& inp, std::size_t offset, Eigen::MatrixXd
std::istringstream str(line);
for (std::size_t col = 0; col < list.size() && col < numFields; col++) {
Eigen::Index size = Eigen::Index(list.size());
for (Eigen::Index col = 0; col < size && col < numFields; col++) {
double value = boost::lexical_cast<double>(list[col]);
data(row, col) = value;
}
@@ -923,8 +924,8 @@ void PlyReader::readBinary(bool swapByteOrder,
const std::vector<int>& sizes,
Eigen::MatrixXd& data)
{
std::size_t numPoints = data.rows();
std::size_t numFields = data.cols();
Eigen::Index numPoints = data.rows();
Eigen::Index numFields = data.cols();
int neededSize = 0;
ConverterPtr convert_float32(new ConverterT<float>);
@@ -937,8 +938,8 @@ void PlyReader::readBinary(bool swapByteOrder,
ConverterPtr convert_uint32(new ConverterT<uint32_t>);
std::vector<ConverterPtr> converters;
for (std::size_t j = 0; j < numFields; j++) {
std::string t = types[j];
for (Eigen::Index j = 0; j < numFields; j++) {
const std::string& t = types[j];
switch (sizes[j]) {
case 1:
if (t == "char" || t == "int8") {
@@ -1005,8 +1006,8 @@ void PlyReader::readBinary(bool swapByteOrder,
Base::InputStream str(inp);
str.setByteOrder(swapByteOrder ? Base::Stream::BigEndian : Base::Stream::LittleEndian);
for (std::size_t i = 0; i < numPoints; i++) {
for (std::size_t j = 0; j < numFields; j++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
for (Eigen::Index j = 0; j < numFields; j++) {
double value = converters[j]->toDouble(str);
data(i, j) = value;
}
@@ -1030,7 +1031,7 @@ void PcdReader::read(const std::string& filename)
std::vector<std::string> fields;
std::vector<std::string> types;
std::vector<int> sizes;
std::size_t numPoints = readHeader(inp, format, fields, types, sizes);
Eigen::Index numPoints = Eigen::Index(readHeader(inp, format, fields, types, sizes));
Eigen::MatrixXd data(numPoints, fields.size());
if (format == "ascii") {
@@ -1040,14 +1041,15 @@ void PcdReader::read(const std::string& filename)
readBinary(false, inp, types, sizes, data);
}
else if (format == "binary_compressed") {
unsigned int c, u;
unsigned int c {};
unsigned int u {};
Base::InputStream str(inp);
str >> c >> u;
std::vector<char> compressed(c);
inp.read(&compressed[0], c);
inp.read(compressed.data(), c);
std::vector<char> uncompressed(u);
if (lzfDecompress(&compressed[0], c, &uncompressed[0], u) == u) {
if (lzfDecompress(compressed.data(), c, uncompressed.data(), u) == u) {
DataStreambuf ibuf(uncompressed);
std::istream istr(nullptr);
istr.rdbuf(&ibuf);
@@ -1059,31 +1061,31 @@ void PcdReader::read(const std::string& filename)
}
std::vector<std::string>::iterator it;
std::size_t max_size = std::numeric_limits<std::size_t>::max();
Eigen::Index max_size = std::numeric_limits<Eigen::Index>::max();
// x field
std::size_t x = max_size;
Eigen::Index x = max_size;
it = std::find(fields.begin(), fields.end(), "x");
if (it != fields.end()) {
x = std::distance(fields.begin(), it);
}
// y field
std::size_t y = max_size;
Eigen::Index y = max_size;
it = std::find(fields.begin(), fields.end(), "y");
if (it != fields.end()) {
y = std::distance(fields.begin(), it);
}
// z field
std::size_t z = max_size;
Eigen::Index z = max_size;
it = std::find(fields.begin(), fields.end(), "z");
if (it != fields.end()) {
z = std::distance(fields.begin(), it);
}
// normal x field
std::size_t normal_x = max_size;
Eigen::Index normal_x = max_size;
it = std::find(fields.begin(), fields.end(), "normal_x");
if (it == fields.end()) {
it = std::find(fields.begin(), fields.end(), "nx");
@@ -1093,7 +1095,7 @@ void PcdReader::read(const std::string& filename)
}
// normal y field
std::size_t normal_y = max_size;
Eigen::Index normal_y = max_size;
it = std::find(fields.begin(), fields.end(), "normal_y");
if (it == fields.end()) {
it = std::find(fields.begin(), fields.end(), "ny");
@@ -1103,7 +1105,7 @@ void PcdReader::read(const std::string& filename)
}
// normal z field
std::size_t normal_z = max_size;
Eigen::Index normal_z = max_size;
it = std::find(fields.begin(), fields.end(), "normal_z");
if (it == fields.end()) {
it = std::find(fields.begin(), fields.end(), "nz");
@@ -1113,14 +1115,14 @@ void PcdReader::read(const std::string& filename)
}
// intensity field
std::size_t greyvalue = max_size;
Eigen::Index greyvalue = max_size;
it = std::find(fields.begin(), fields.end(), "intensity");
if (it != fields.end()) {
greyvalue = std::distance(fields.begin(), it);
}
// rgb(a) field
std::size_t rgba = max_size;
Eigen::Index rgba = max_size;
it = std::find(fields.begin(), fields.end(), "rgb");
if (it == fields.end()) {
it = std::find(fields.begin(), fields.end(), "rgba");
@@ -1137,21 +1139,21 @@ void PcdReader::read(const std::string& filename)
if (hasData) {
points.reserve(numPoints);
for (std::size_t i = 0; i < numPoints; i++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
points.push_back(Base::Vector3d(data(i, x), data(i, y), data(i, z)));
}
}
if (hasData && hasNormal) {
normals.reserve(numPoints);
for (std::size_t i = 0; i < numPoints; i++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
normals.emplace_back(data(i, normal_x), data(i, normal_y), data(i, normal_z));
}
}
if (hasData && hasIntensity) {
intensity.reserve(numPoints);
for (std::size_t i = 0; i < numPoints; i++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
intensity.push_back(data(i, greyvalue));
}
}
@@ -1159,7 +1161,7 @@ void PcdReader::read(const std::string& filename)
if (hasData && hasColor) {
colors.reserve(numPoints);
if (types[rgba] == "U") {
for (std::size_t i = 0; i < numPoints; i++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
uint32_t packed = static_cast<uint32_t>(data(i, rgba));
App::Color col;
col.setPackedARGB(packed);
@@ -1169,9 +1171,9 @@ void PcdReader::read(const std::string& filename)
else if (types[rgba] == "F") {
static_assert(sizeof(float) == sizeof(uint32_t),
"float and uint32_t have different sizes");
for (std::size_t i = 0; i < numPoints; i++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
float f = static_cast<float>(data(i, rgba));
uint32_t packed;
uint32_t packed {};
std::memcpy(&packed, &f, sizeof(packed));
App::Color col;
col.setPackedARGB(packed);
@@ -1255,9 +1257,9 @@ std::size_t PcdReader::readHeader(std::istream& in,
void PcdReader::readAscii(std::istream& inp, Eigen::MatrixXd& data)
{
std::string line;
std::size_t row = 0;
std::size_t numPoints = data.rows();
std::size_t numFields = data.cols();
Eigen::Index row = 0;
Eigen::Index numPoints = data.rows();
Eigen::Index numFields = data.cols();
std::vector<std::string> list;
while (std::getline(inp, line) && row < numPoints) {
if (line.empty()) {
@@ -1270,7 +1272,8 @@ void PcdReader::readAscii(std::istream& inp, Eigen::MatrixXd& data)
std::istringstream str(line);
for (std::size_t col = 0; col < list.size() && col < numFields; col++) {
Eigen::Index size = Eigen::Index(list.size());
for (Eigen::Index col = 0; col < size && col < numFields; col++) {
double value = boost::lexical_cast<double>(list[col]);
data(row, col) = value;
}
@@ -1285,8 +1288,8 @@ void PcdReader::readBinary(bool transpose,
const std::vector<int>& sizes,
Eigen::MatrixXd& data)
{
std::size_t numPoints = data.rows();
std::size_t numFields = data.cols();
Eigen::Index numPoints = data.rows();
Eigen::Index numFields = data.cols();
int neededSize = 0;
ConverterPtr convert_float32(new ConverterT<float>);
@@ -1299,7 +1302,7 @@ void PcdReader::readBinary(bool transpose,
ConverterPtr convert_uint32(new ConverterT<uint32_t>);
std::vector<ConverterPtr> converters;
for (std::size_t j = 0; j < numFields; j++) {
for (Eigen::Index j = 0; j < numFields; j++) {
char t = types[j][0];
switch (sizes[j]) {
case 1:
@@ -1367,16 +1370,16 @@ void PcdReader::readBinary(bool transpose,
Base::InputStream str(inp);
if (transpose) {
for (std::size_t j = 0; j < numFields; j++) {
for (std::size_t i = 0; i < numPoints; i++) {
for (Eigen::Index j = 0; j < numFields; j++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
double value = converters[j]->toDouble(str);
data(i, j) = value;
}
}
}
else {
for (std::size_t i = 0; i < numPoints; i++) {
for (std::size_t j = 0; j < numFields; j++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
for (Eigen::Index j = 0; j < numFields; j++) {
double value = converters[j]->toDouble(str);
data(i, j) = value;
}
@@ -1580,7 +1583,7 @@ private:
);
return true;
}
else if (node.elementName() == "colorGreen") {
if (node.elementName() == "colorGreen") {
proto.cnt_rgb++;
proto.sdb
.emplace_back(imfi, node.elementName(), proto.greenData.data(), buf_size, true, true
@@ -1588,7 +1591,7 @@ private:
);
return true;
}
else if (node.elementName() == "colorBlue") {
if (node.elementName() == "colorBlue") {
proto.cnt_rgb++;
proto.sdb
.emplace_back(imfi, node.elementName(), proto.blueData.data(), buf_size, true, true
@@ -1702,9 +1705,9 @@ private:
App::Color getColor(const Proto& proto, size_t index) const
{
App::Color c;
c.r = static_cast<float>(proto.redData[index]) / 255.0f;
c.g = static_cast<float>(proto.greenData[index]) / 255.0f;
c.b = static_cast<float>(proto.blueData[index]) / 255.0f;
c.r = static_cast<float>(proto.redData[index]) / 255.0F;
c.g = static_cast<float>(proto.greenData[index]) / 255.0F;
c.b = static_cast<float>(proto.blueData[index]) / 255.0F;
return c;
}
@@ -1798,10 +1801,9 @@ void E57Reader::read(const std::string& filename)
Writer::Writer(const PointKernel& p)
: points(p)
{
width = p.size();
height = 1;
}
, width(int(p.size()))
, height {1}
{}
Writer::~Writer() = default;
@@ -1903,10 +1905,10 @@ void PlyWriter::write(const std::string& filename)
converters.push_back(convert_float);
}
std::size_t numPoints = points.size();
std::size_t numValid = 0;
Eigen::Index numPoints = Eigen::Index(points.size());
Eigen::Index numValid = 0;
const std::vector<Base::Vector3f>& pts = points.getBasicPoints();
for (std::size_t i = 0; i < numPoints; i++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
const Base::Vector3f& p = pts[i];
if (!boost::math::isnan(p.x) && !boost::math::isnan(p.y) && !boost::math::isnan(p.z)) {
numValid++;
@@ -1916,7 +1918,7 @@ void PlyWriter::write(const std::string& filename)
Eigen::MatrixXf data(numPoints, properties.size());
if (placement.isIdentity()) {
for (std::size_t i = 0; i < numPoints; i++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
data(i, 0) = pts[i].x;
data(i, 1) = pts[i].y;
data(i, 2) = pts[i].z;
@@ -1924,7 +1926,7 @@ void PlyWriter::write(const std::string& filename)
}
else {
Base::Vector3d tmp;
for (std::size_t i = 0; i < numPoints; i++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
tmp = Base::convertTo<Base::Vector3d>(pts[i]);
placement.multVec(tmp, tmp);
data(i, 0) = static_cast<float>(tmp.x);
@@ -1933,14 +1935,14 @@ void PlyWriter::write(const std::string& filename)
}
}
std::size_t col = 3;
Eigen::Index col = 3;
if (hasNormals) {
int col0 = col;
int col1 = col + 1;
int col2 = col + 2;
Eigen::Index col0 = col;
Eigen::Index col1 = col + 1;
Eigen::Index col2 = col + 2;
Base::Rotation rot = placement.getRotation();
if (rot.isIdentity()) {
for (std::size_t i = 0; i < numPoints; i++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
data(i, col0) = normals[i].x;
data(i, col1) = normals[i].y;
data(i, col2) = normals[i].z;
@@ -1948,7 +1950,7 @@ void PlyWriter::write(const std::string& filename)
}
else {
Base::Vector3d tmp;
for (std::size_t i = 0; i < numPoints; i++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
tmp = Base::convertTo<Base::Vector3d>(normals[i]);
rot.multVec(tmp, tmp);
data(i, col0) = static_cast<float>(tmp.x);
@@ -1960,22 +1962,22 @@ void PlyWriter::write(const std::string& filename)
}
if (hasColors) {
int col0 = col;
int col1 = col + 1;
int col2 = col + 2;
int col3 = col + 3;
for (std::size_t i = 0; i < numPoints; i++) {
Eigen::Index col0 = col;
Eigen::Index col1 = col + 1;
Eigen::Index col2 = col + 2;
Eigen::Index col3 = col + 3;
for (Eigen::Index i = 0; i < numPoints; i++) {
App::Color c = colors[i];
data(i, col0) = (c.r * 255.0f + 0.5f);
data(i, col1) = (c.g * 255.0f + 0.5f);
data(i, col2) = (c.b * 255.0f + 0.5f);
data(i, col3) = (c.a * 255.0f + 0.5f);
data(i, col0) = (c.r * 255.0F + 0.5F);
data(i, col1) = (c.g * 255.0F + 0.5F);
data(i, col2) = (c.b * 255.0F + 0.5F);
data(i, col3) = (c.a * 255.0F + 0.5F);
}
col += 4;
}
if (hasIntensity) {
for (std::size_t i = 0; i < numPoints; i++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
data(i, col) = intensity[i];
}
col += 1;
@@ -1993,7 +1995,7 @@ void PlyWriter::write(const std::string& filename)
}
out << "end_header" << std::endl;
for (std::size_t r = 0; r < numPoints; r++) {
for (Eigen::Index r = 0; r < numPoints; r++) {
if (boost::math::isnan(data(r, 0))) {
continue;
}
@@ -2003,7 +2005,7 @@ void PlyWriter::write(const std::string& filename)
if (boost::math::isnan(data(r, 2))) {
continue;
}
for (std::size_t c = 0; c < col; c++) {
for (Eigen::Index c = 0; c < col; c++) {
float value = data(r, c);
out << converters[c]->toString(value) << " ";
}
@@ -2065,13 +2067,13 @@ void PcdWriter::write(const std::string& filename)
converters.push_back(convert_float);
}
std::size_t numPoints = points.size();
Eigen::Index numPoints = Eigen::Index(points.size());
const std::vector<Base::Vector3f>& pts = points.getBasicPoints();
Eigen::MatrixXd data(numPoints, fields.size());
if (placement.isIdentity()) {
for (std::size_t i = 0; i < numPoints; i++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
data(i, 0) = pts[i].x;
data(i, 1) = pts[i].y;
data(i, 2) = pts[i].z;
@@ -2079,7 +2081,7 @@ void PcdWriter::write(const std::string& filename)
}
else {
Base::Vector3d tmp;
for (std::size_t i = 0; i < numPoints; i++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
tmp = Base::convertTo<Base::Vector3d>(pts[i]);
placement.multVec(tmp, tmp);
data(i, 0) = static_cast<float>(tmp.x);
@@ -2088,14 +2090,14 @@ void PcdWriter::write(const std::string& filename)
}
}
std::size_t col = 3;
Eigen::Index col = 3;
if (hasNormals) {
int col0 = col;
int col1 = col + 1;
int col2 = col + 2;
Eigen::Index col0 = col;
Eigen::Index col1 = col + 1;
Eigen::Index col2 = col + 2;
Base::Rotation rot = placement.getRotation();
if (rot.isIdentity()) {
for (std::size_t i = 0; i < numPoints; i++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
data(i, col0) = normals[i].x;
data(i, col1) = normals[i].y;
data(i, col2) = normals[i].z;
@@ -2103,7 +2105,7 @@ void PcdWriter::write(const std::string& filename)
}
else {
Base::Vector3d tmp;
for (std::size_t i = 0; i < numPoints; i++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
tmp = Base::convertTo<Base::Vector3d>(normals[i]);
rot.multVec(tmp, tmp);
data(i, col0) = static_cast<float>(tmp.x);
@@ -2115,7 +2117,7 @@ void PcdWriter::write(const std::string& filename)
}
if (hasColors) {
for (std::size_t i = 0; i < numPoints; i++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
// http://docs.pointclouds.org/1.3.0/structpcl_1_1_r_g_b.html
data(i, col) = colors[i].getPackedARGB();
}
@@ -2123,7 +2125,7 @@ void PcdWriter::write(const std::string& filename)
}
if (hasIntensity) {
for (std::size_t i = 0; i < numPoints; i++) {
for (Eigen::Index i = 0; i < numPoints; i++) {
data(i, col) = intensity[i];
}
col += 1;
@@ -2167,15 +2169,18 @@ void PcdWriter::write(const std::string& filename)
Base::Placement plm;
Base::Vector3d p = plm.getPosition();
Base::Rotation o = plm.getRotation();
double x, y, z, w;
double x {};
double y {};
double z {};
double w {};
o.getValue(x, y, z, w);
out << "VIEWPOINT " << p.x << " " << p.y << " " << p.z << " " << w << " " << x << " " << y
<< " " << z << std::endl;
out << "POINTS " << numPoints << std::endl << "DATA ascii" << std::endl;
for (std::size_t r = 0; r < numPoints; r++) {
for (std::size_t c = 0; c < col; c++) {
for (Eigen::Index r = 0; r < numPoints; r++) {
for (Eigen::Index c = 0; c < col; c++) {
double value = data(r, c);
if (boost::math::isnan(value)) {
out << "nan ";