Program Listing for File point-cloud.h¶
↰ Return to documentation for file (common/resources-common/include/resources-common/point-cloud.h
)
#ifndef RESOURCES_COMMON_POINT_CLOUD_H_
#define RESOURCES_COMMON_POINT_CLOUD_H_
#include <cstdio>
#include <fstream> // NOLINT
#include <string>
#include <vector>
#include <Eigen/Core>
#include <aslam/common/pose-types.h>
#include <maplab-common/file-system-tools.h>
#include <maplab-common/parallel-process.h>
#include <maplab-common/threading-helpers.h>
#include "resources-common/tinyply/tinyply.h"
namespace resources {
typedef Eigen::Matrix<uint8_t, 4, 1> RgbaColor;
struct PointCloud {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
std::vector<float> xyz;
std::vector<float> normals;
std::vector<unsigned char> colors;
std::vector<float> scalars;
std::vector<uint32_t> labels;
// Apply transformation T_A_B to pointcloud, assuming the pointcloud is
// currently expressed in the B frame.
inline void applyTransformation(const aslam::Transformation& T_A_B) {
std::function<void(const std::vector<size_t>&)> transform_points_function =
[this, &T_A_B](const std::vector<size_t>& batch) {
for (size_t point_idx : batch) {
const size_t idx = point_idx * 3;
const Eigen::Vector3d point(xyz[idx], xyz[idx + 1u], xyz[idx + 2u]);
const Eigen::Vector3f& transformed_point =
(T_A_B * point).cast<float>();
xyz[idx] = transformed_point.x();
xyz[idx + 1u] = transformed_point.y();
xyz[idx + 2u] = transformed_point.z();
}
// Rotate normals if present.
if (normals.size() == xyz.size()) {
const aslam::Quaternion& R_A_B = T_A_B.getRotation();
for (size_t point_idx : batch) {
const size_t idx = point_idx * 3;
const Eigen::Vector3d normal(
normals[idx], normals[idx + 1u], normals[idx + 2u]);
const Eigen::Vector3f& transformed_normal =
R_A_B.rotate(normal).cast<float>();
normals[idx] = transformed_normal.x();
normals[idx + 1u] = transformed_normal.y();
normals[idx + 2u] = transformed_normal.z();
}
}
};
const size_t num_threads = common::getNumHardwareThreads();
static constexpr bool kAlwaysParallelize = false;
common::ParallelProcess(
size(), transform_points_function, kAlwaysParallelize, num_threads);
}
inline void resize(
const size_t size, const bool has_normals = true,
const bool has_colors = true, const bool has_scalars = true,
const bool has_labels = true) {
xyz.resize(3 * size);
if (has_normals) {
normals.resize(3 * size);
}
if (has_colors) {
colors.resize(3 * size);
}
if (has_scalars) {
scalars.resize(1 * size);
}
if (has_labels) {
labels.resize(1 * size);
}
}
inline size_t size() const {
CHECK_EQ(xyz.size() % 3, 0u);
return (xyz.size() / 3);
}
inline bool empty() const {
return xyz.empty();
}
inline bool hasNormals() const {
return normals.size() == xyz.size() && !normals.empty();
}
inline bool hasColor() const {
return colors.size() == xyz.size() && !colors.empty();
}
inline bool hasScalars() const {
return (scalars.size() == xyz.size() / 3u) && !scalars.empty();
}
inline bool hasLabels() const {
return (labels.size() == xyz.size() / 3u) && !labels.empty();
}
inline bool checkConsistency(const bool verbose = false) const {
bool consistent = true;
consistent &= (normals.size() == xyz.size()) || normals.empty();
consistent &= (colors.size() == xyz.size()) || colors.empty();
consistent &= (scalars.size() == xyz.size() / 3u) || scalars.empty();
consistent &= (labels.size() == xyz.size() / 3u) || labels.empty();
LOG_IF(ERROR, verbose && !consistent)
<< "\nInconsistent point cloud:"
<< "\n - Point vector size: " << xyz.size()
<< "\n - Normal vector size: " << normals.size()
<< "\n - Color vector size: " << colors.size()
<< "\n - Scalar vector size: " << scalars.size()
<< "\n - Label vector size: " << labels.size();
return consistent;
}
inline void append(const PointCloud& other) {
if (other.empty()) {
return;
}
xyz.reserve(xyz.size() + other.xyz.size());
normals.reserve(normals.size() + other.normals.size());
colors.reserve(colors.size() + other.colors.size());
scalars.reserve(scalars.size() + other.scalars.size());
labels.reserve(labels.size() + other.labels.size());
xyz.insert(xyz.end(), other.xyz.begin(), other.xyz.end());
normals.insert(normals.end(), other.normals.begin(), other.normals.end());
colors.insert(colors.end(), other.colors.begin(), other.colors.end());
scalars.insert(scalars.end(), other.scalars.begin(), other.scalars.end());
labels.insert(labels.end(), other.labels.begin(), other.labels.end());
CHECK(checkConsistency(true)) << "Point cloud is not consistent!";
}
// Transforms the other point cloud and appends it to the current one. Assumes
// the other point cloud is in B frame.
inline void appendTransformed(
const PointCloud& other, const aslam::Transformation& T_A_B) {
if (other.empty()) {
return;
}
// Remember how many points there wrere before.
const size_t old_size = size();
append(other);
const size_t new_size = size();
for (size_t point_idx = old_size; point_idx < new_size; ++point_idx) {
const size_t idx = point_idx * 3;
const Eigen::Vector3d point(xyz[idx], xyz[idx + 1u], xyz[idx + 2u]);
const Eigen::Vector3f& transformed_point = (T_A_B * point).cast<float>();
xyz[idx] = transformed_point.x();
xyz[idx + 1u] = transformed_point.y();
xyz[idx + 2u] = transformed_point.z();
}
// Rotate normals if present.
if (normals.size() == xyz.size()) {
const aslam::Quaternion& R_A_B = T_A_B.getRotation();
for (size_t point_idx = old_size; point_idx < new_size; ++point_idx) {
const size_t idx = point_idx * 3;
const Eigen::Vector3d normal(
normals[idx], normals[idx + 1u], normals[idx + 2u]);
const Eigen::Vector3f& transformed_normal =
R_A_B.rotate(normal).cast<float>();
normals[idx] = transformed_normal.x();
normals[idx + 1u] = transformed_normal.y();
normals[idx + 2u] = transformed_normal.z();
}
}
CHECK(checkConsistency(true)) << "Point cloud is not consistent!";
}
inline void removeInvalidPoints() {
auto it_xyz = xyz.begin();
auto it_normals = normals.begin();
auto it_colors = colors.begin();
auto it_scalars = scalars.begin();
auto it_labels = labels.begin();
size_t removed_points = 0u;
const size_t initial_number_of_points = xyz.size() / 3u;
while (it_xyz != xyz.end()) {
const float x = *it_xyz;
const float y = *(it_xyz + 1);
const float z = *(it_xyz + 2);
if (((x * x + y * y + z * z) < 1e-10) || !std::isfinite(x) ||
!std::isfinite(y) || !std::isfinite(z)) {
++removed_points;
it_xyz = xyz.erase(it_xyz, it_xyz + 3);
if (!normals.empty()) {
it_normals = normals.erase(it_normals, it_normals + 3);
}
if (!colors.empty()) {
it_colors = colors.erase(it_colors, it_colors + 3);
}
if (!scalars.empty()) {
it_scalars = scalars.erase(it_scalars);
}
if (!labels.empty()) {
it_labels = labels.erase(it_labels);
}
} else {
it_xyz += 3;
if (!normals.empty()) {
it_normals += 3;
}
if (!colors.empty()) {
it_colors += 3;
}
if (!scalars.empty()) {
++it_scalars;
}
if (!labels.empty()) {
++it_labels;
}
}
}
LOG_IF(WARNING, removed_points > 0)
<< "Removed " << removed_points << "/" << initial_number_of_points
<< " invalid points from point cloud!";
CHECK(checkConsistency(true)) << "Point cloud is not consistent!";
}
bool operator==(const PointCloud& other) const {
bool is_same = xyz == other.xyz;
is_same &= normals == other.normals;
is_same &= colors == other.colors;
is_same &= scalars == other.scalars;
is_same &= labels == other.labels;
return is_same;
}
inline void writeToFile(const std::string& file_path) const {
CHECK(common::createPathToFile(file_path));
std::filebuf filebuf;
filebuf.open(file_path, std::ios::out | std::ios::binary);
CHECK(filebuf.is_open());
std::ostream output_stream(&filebuf);
tinyply::PlyFile ply_file;
// Const-casting is necessary as tinyply requires non-const access to the
// vectors for reading.
ply_file.add_properties_to_element(
"vertex", {"x", "y", "z"}, const_cast<std::vector<float>&>(xyz));
if (!normals.empty()) {
ply_file.add_properties_to_element(
"vertex", {"nx", "ny", "nz"},
const_cast<std::vector<float>&>(normals));
}
if (!colors.empty()) {
ply_file.add_properties_to_element(
"vertex", {"red", "green", "blue"},
const_cast<std::vector<unsigned char>&>(colors));
}
if (!scalars.empty()) {
ply_file.add_properties_to_element(
"vertex", {"scalar"}, const_cast<std::vector<float>&>(scalars));
}
if (!labels.empty()) {
ply_file.add_properties_to_element(
"vertex", {"label"}, const_cast<std::vector<uint32_t>&>(labels));
}
ply_file.comments.push_back("generated by tinyply from maplab");
ply_file.write(output_stream, true);
filebuf.close();
}
inline bool loadFromFile(const std::string& file_path) {
if (!common::fileExists(file_path)) {
VLOG(1) << "Point cloud file does not exist! Path: " << file_path;
return false;
}
std::ifstream stream_ply(file_path);
if (stream_ply.is_open()) {
tinyply::PlyFile ply_file(stream_ply);
const int xyz_point_count = ply_file.request_properties_from_element(
"vertex", {"x", "y", "z"}, xyz);
const int colors_count = ply_file.request_properties_from_element(
"vertex", {"nx", "ny", "nz"}, normals);
const int normals_count = ply_file.request_properties_from_element(
"vertex", {"red", "green", "blue"}, colors);
const int scalar_count = ply_file.request_properties_from_element(
"vertex", {"scalar"}, scalars);
const int label_count =
ply_file.request_properties_from_element("vertex", {"label"}, labels);
if (xyz_point_count > 0) {
if (colors_count > 0) {
// If colors are present, their count should match the point count.
CHECK_EQ(xyz_point_count, colors_count);
}
if (normals_count > 0) {
// If normals are present, their count should match the point count.
CHECK_EQ(xyz_point_count, normals_count);
}
if (scalar_count > 0) {
// If a value attribute is present, its count should match the point
// count.
CHECK_EQ(xyz_point_count, scalar_count);
}
if (label_count > 0) {
// If a label attribute is present, its count should match the point
// count.
CHECK_EQ(xyz_point_count, label_count);
}
ply_file.read(stream_ply);
}
stream_ply.close();
return true;
}
return false;
}
inline bool colorizePointCloud(
const size_t start_point_idx, const size_t end_point_idx, const uint8_t r,
const uint8_t g, const uint8_t b) {
if (colors.size() != xyz.size()) {
return false;
}
CHECK_LT(end_point_idx, size());
CHECK_LE(start_point_idx, end_point_idx);
auto it_rgb = colors.begin() + start_point_idx;
auto end_it = colors.begin() + end_point_idx;
while (it_rgb != end_it && it_rgb != colors.end()) {
*it_rgb = r;
*(it_rgb + 1) = g;
*(it_rgb + 2) = b;
it_rgb += 3;
}
return true;
}
inline bool colorizePointCloud(
const uint8_t r, const uint8_t g, const uint8_t b) {
return colorizePointCloud(0, size(), r, g, b);
}
};
} // namespace resources
#endif // RESOURCES_COMMON_POINT_CLOUD_H_