diff --git a/examples/cpp/demo_sequential.cpp b/examples/cpp/demo_sequential.cpp
index 0cb9dcd..3454987 100644
--- a/examples/cpp/demo_sequential.cpp
+++ b/examples/cpp/demo_sequential.cpp
@@ -64,8 +64,8 @@ int main() {
Patchworkpp.estimateGround(cloud);
// Get Ground and Nonground
- Eigen::MatrixX3f ground = Patchworkpp.getGround();
- Eigen::MatrixX3f nonground = Patchworkpp.getNonground();
+ Eigen::MatrixX4f ground = Patchworkpp.getGround();
+ Eigen::MatrixX4f nonground = Patchworkpp.getNonground();
double time_taken = Patchworkpp.getTimeTaken();
// Get centers and normals for patches
diff --git a/examples/cpp/demo_visualize.cpp b/examples/cpp/demo_visualize.cpp
index 9278555..259e331 100644
--- a/examples/cpp/demo_visualize.cpp
+++ b/examples/cpp/demo_visualize.cpp
@@ -62,8 +62,8 @@ int main() {
Patchworkpp.estimateGround(cloud);
// Get Ground and Nonground
- Eigen::MatrixX3f ground = Patchworkpp.getGround();
- Eigen::MatrixX3f nonground = Patchworkpp.getNonground();
+ Eigen::MatrixX4f ground = Patchworkpp.getGround();
+ Eigen::MatrixX4f nonground = Patchworkpp.getNonground();
double time_taken = Patchworkpp.getTimeTaken();
// Get centers and normals for patches
diff --git a/examples/python/demo_sequential.py b/examples/python/demo_sequential.py
index eaff330..20b4b70 100644
--- a/examples/python/demo_sequential.py
+++ b/examples/python/demo_sequential.py
@@ -42,6 +42,9 @@ def read_bin(bin_path):
nonground = PatchworkPLUSPLUS.getNonground()
time_taken = PatchworkPLUSPLUS.getTimeTaken()
+ print("\nGround\n", ground)
+ print("\nNonground\n", nonground)
+
# Get centers and normals for patches
centers = PatchworkPLUSPLUS.getCenters()
normals = PatchworkPLUSPLUS.getNormals()
@@ -50,39 +53,39 @@ def read_bin(bin_path):
print("Ground Points #: ", ground.shape[0])
print("Nonground Points #: ", nonground.shape[0])
print("Time Taken : ", time_taken / 1000000, "(sec)")
- print("Press ... \n")
- print("\t H : help")
- print("\t N : visualize the surface normals")
- print("\tESC : close the Open3D window")
+ # print("Press ... \n")
+ # print("\t H : help")
+ # print("\t N : visualize the surface normals")
+ # print("\tESC : close the Open3D window")
- vis = o3d.visualization.VisualizerWithKeyCallback()
- vis.create_window(width = 600, height = 400)
-
- mesh = o3d.geometry.TriangleMesh.create_coordinate_frame()
-
- ground_o3d = o3d.geometry.PointCloud()
- ground_o3d.points = o3d.utility.Vector3dVector(ground)
- ground_o3d.colors = o3d.utility.Vector3dVector(
- np.array([[0.0, 1.0, 0.0] for _ in range(ground.shape[0])], dtype=float) # RGB
- )
-
- nonground_o3d = o3d.geometry.PointCloud()
- nonground_o3d.points = o3d.utility.Vector3dVector(nonground)
- nonground_o3d.colors = o3d.utility.Vector3dVector(
- np.array([[1.0, 0.0, 0.0] for _ in range(nonground.shape[0])], dtype=float) #RGB
- )
-
- centers_o3d = o3d.geometry.PointCloud()
- centers_o3d.points = o3d.utility.Vector3dVector(centers)
- centers_o3d.normals = o3d.utility.Vector3dVector(normals)
- centers_o3d.colors = o3d.utility.Vector3dVector(
- np.array([[1.0, 1.0, 0.0] for _ in range(centers.shape[0])], dtype=float) #RGB
- )
-
- vis.add_geometry(mesh)
- vis.add_geometry(ground_o3d)
- vis.add_geometry(nonground_o3d)
- vis.add_geometry(centers_o3d)
-
- vis.run()
- vis.destroy_window()
+ # vis = o3d.visualization.VisualizerWithKeyCallback()
+ # vis.create_window(width = 600, height = 400)
+
+ # mesh = o3d.geometry.TriangleMesh.create_coordinate_frame()
+
+ # ground_o3d = o3d.geometry.PointCloud()
+ # ground_o3d.points = o3d.utility.Vector3dVector(ground)
+ # ground_o3d.colors = o3d.utility.Vector3dVector(
+ # np.array([[0.0, 1.0, 0.0] for _ in range(ground.shape[0])], dtype=float) # RGB
+ # )
+
+ # nonground_o3d = o3d.geometry.PointCloud()
+ # nonground_o3d.points = o3d.utility.Vector3dVector(nonground)
+ # nonground_o3d.colors = o3d.utility.Vector3dVector(
+ # np.array([[1.0, 0.0, 0.0] for _ in range(nonground.shape[0])], dtype=float) #RGB
+ # )
+
+ # centers_o3d = o3d.geometry.PointCloud()
+ # centers_o3d.points = o3d.utility.Vector3dVector(centers)
+ # centers_o3d.normals = o3d.utility.Vector3dVector(normals)
+ # centers_o3d.colors = o3d.utility.Vector3dVector(
+ # np.array([[1.0, 1.0, 0.0] for _ in range(centers.shape[0])], dtype=float) #RGB
+ # )
+
+ # vis.add_geometry(mesh)
+ # vis.add_geometry(ground_o3d)
+ # vis.add_geometry(nonground_o3d)
+ # vis.add_geometry(centers_o3d)
+
+ # vis.run()
+ # vis.destroy_window()
diff --git a/examples/python/demo_visualize.py b/examples/python/demo_visualize.py
index eb92f17..6f5b30e 100644
--- a/examples/python/demo_visualize.py
+++ b/examples/python/demo_visualize.py
@@ -40,6 +40,9 @@ def read_bin(bin_path):
nonground = PatchworkPLUSPLUS.getNonground()
time_taken = PatchworkPLUSPLUS.getTimeTaken()
+ print("\nGround\n", ground)
+ print("\nNonground\n", nonground)
+
# Get centers and normals for patches
centers = PatchworkPLUSPLUS.getCenters()
normals = PatchworkPLUSPLUS.getNormals()
@@ -48,40 +51,40 @@ def read_bin(bin_path):
print("Ground Points #: ", ground.shape[0])
print("Nonground Points #: ", nonground.shape[0])
print("Time Taken : ", time_taken / 1000000, "(sec)")
- print("Press ... \n")
- print("\t H : help")
- print("\t N : visualize the surface normals")
- print("\tESC : close the Open3D window")
-
- # Visualize
- vis = o3d.visualization.VisualizerWithKeyCallback()
- vis.create_window(width = 600, height = 400)
-
- mesh = o3d.geometry.TriangleMesh.create_coordinate_frame()
-
- ground_o3d = o3d.geometry.PointCloud()
- ground_o3d.points = o3d.utility.Vector3dVector(ground)
- ground_o3d.colors = o3d.utility.Vector3dVector(
- np.array([[0.0, 1.0, 0.0] for _ in range(ground.shape[0])], dtype=float) # RGB
- )
-
- nonground_o3d = o3d.geometry.PointCloud()
- nonground_o3d.points = o3d.utility.Vector3dVector(nonground)
- nonground_o3d.colors = o3d.utility.Vector3dVector(
- np.array([[1.0, 0.0, 0.0] for _ in range(nonground.shape[0])], dtype=float) #RGB
- )
-
- centers_o3d = o3d.geometry.PointCloud()
- centers_o3d.points = o3d.utility.Vector3dVector(centers)
- centers_o3d.normals = o3d.utility.Vector3dVector(normals)
- centers_o3d.colors = o3d.utility.Vector3dVector(
- np.array([[1.0, 1.0, 0.0] for _ in range(centers.shape[0])], dtype=float) #RGB
- )
-
- vis.add_geometry(mesh)
- vis.add_geometry(ground_o3d)
- vis.add_geometry(nonground_o3d)
- vis.add_geometry(centers_o3d)
-
- vis.run()
- vis.destroy_window()
+ # print("Press ... \n")
+ # print("\t H : help")
+ # print("\t N : visualize the surface normals")
+ # print("\tESC : close the Open3D window")
+
+ # # Visualize
+ # vis = o3d.visualization.VisualizerWithKeyCallback()
+ # vis.create_window(width = 600, height = 400)
+
+ # mesh = o3d.geometry.TriangleMesh.create_coordinate_frame()
+
+ # ground_o3d = o3d.geometry.PointCloud()
+ # ground_o3d.points = o3d.utility.Vector3dVector(ground)
+ # ground_o3d.colors = o3d.utility.Vector3dVector(
+ # np.array([[0.0, 1.0, 0.0] for _ in range(ground.shape[0])], dtype=float) # RGB
+ # )
+
+ # nonground_o3d = o3d.geometry.PointCloud()
+ # nonground_o3d.points = o3d.utility.Vector3dVector(nonground)
+ # nonground_o3d.colors = o3d.utility.Vector3dVector(
+ # np.array([[1.0, 0.0, 0.0] for _ in range(nonground.shape[0])], dtype=float) #RGB
+ # )
+
+ # centers_o3d = o3d.geometry.PointCloud()
+ # centers_o3d.points = o3d.utility.Vector3dVector(centers)
+ # centers_o3d.normals = o3d.utility.Vector3dVector(normals)
+ # centers_o3d.colors = o3d.utility.Vector3dVector(
+ # np.array([[1.0, 1.0, 0.0] for _ in range(centers.shape[0])], dtype=float) #RGB
+ # )
+
+ # vis.add_geometry(mesh)
+ # vis.add_geometry(ground_o3d)
+ # vis.add_geometry(nonground_o3d)
+ # vis.add_geometry(centers_o3d)
+
+ # vis.run()
+ # vis.destroy_window()
diff --git a/patchworkpp/include/patchworkpp.h b/patchworkpp/include/patchworkpp.h
index 1ff57d1..2c37a06 100755
--- a/patchworkpp/include/patchworkpp.h
+++ b/patchworkpp/include/patchworkpp.h
@@ -17,8 +17,10 @@ struct PointXYZ {
float x;
float y;
float z;
+ float intensity;
PointXYZ(float _x, float _y, float _z) : x(_x), y(_y), z(_z) {}
+ PointXYZ(float _x, float _y, float _z, float _intensity) : x(_x), y(_y), z(_z), intensity(_intensity) {}
};
struct RevertCandidate
@@ -149,8 +151,8 @@ class PatchWorkpp {
double getHeight() { return params_.sensor_height; }
double getTimeTaken() { return time_taken_; }
- Eigen::MatrixX3f getGround() { return toEigenCloud(cloud_ground_); }
- Eigen::MatrixX3f getNonground() { return toEigenCloud(cloud_nonground_); }
+ Eigen::MatrixX4f getGround() { return toEigenX4f(cloud_ground_); }
+ Eigen::MatrixX4f getNonground() { return toEigenX4f(cloud_nonground_); }
Eigen::MatrixX3f getCenters() { return toEigenCloud(centers_); }
Eigen::MatrixX3f getNormals() { return toEigenCloud(normals_); }
@@ -188,6 +190,7 @@ class PatchWorkpp {
vector<PointXYZ> centers_, normals_;
Eigen::MatrixX3f toEigenCloud(vector<PointXYZ> cloud);
+ Eigen::MatrixX4f toEigenX4f(vector<PointXYZ> cloud);
void addCloud(vector<PointXYZ> &cloud, vector<PointXYZ> &add);
diff --git a/patchworkpp/src/patchworkpp.cpp b/patchworkpp/src/patchworkpp.cpp
index d093bda..68412ff 100755
--- a/patchworkpp/src/patchworkpp.cpp
+++ b/patchworkpp/src/patchworkpp.cpp
@@ -15,6 +15,16 @@ Eigen::MatrixX3f PatchWorkpp::toEigenCloud(vector<PointXYZ> cloud)
return dst;
}
+Eigen::MatrixX4f PatchWorkpp::toEigenX4f(vector<PointXYZ> cloud)
+{
+ Eigen::MatrixX4f dst(cloud.size(), 4);
+ int j=0;
+ for (auto &p: cloud) {
+ dst.row(j++) << p.x, p.y, p.z, p.intensity;
+ }
+ return dst;
+}
+
void PatchWorkpp::addCloud(vector<PointXYZ> &cloud, vector<PointXYZ> &add)
{
cloud.insert(cloud.end(), add.begin(), add.end());
@@ -373,7 +383,7 @@ void PatchWorkpp::reflected_noise_removal(Eigen::MatrixXf &cloud_in)
if ( ver_angle_in_deg < params_.RNR_ver_angle_thr && z < -params_.sensor_height-0.8 && cloud_in.row(i)(3) < params_.RNR_intensity_thr)
{
- cloud_nonground_.push_back(PointXYZ(cloud_in.row(i)(0), cloud_in.row(i)(1), cloud_in.row(i)(2)));
+ cloud_nonground_.push_back(PointXYZ(cloud_in.row(i)(0), cloud_in.row(i)(1), cloud_in.row(i)(2), cloud_in.row(i)(3)));
cloud_in.row(i)(2) = std::numeric_limits<float>::min();
cnt++;
}
@@ -569,7 +579,7 @@ void PatchWorkpp::pc2czm(const Eigen::MatrixXf &src, std::vector<Zone> &czm) {
for (int i=0; i<src.rows(); i++) {
- float x = src.row(i)(0), y = src.row(i)(1), z = src.row(i)(2);
+ float x = src.row(i)(0), y = src.row(i)(1), z = src.row(i)(2), intensity = src.row(i)(3);
if ( z == std::numeric_limits<float>::min() ) continue;
@@ -582,23 +592,23 @@ void PatchWorkpp::pc2czm(const Eigen::MatrixXf &src, std::vector<Zone> &czm) {
if (r < min_range_1) { // In First rings
ring_idx = min(static_cast<int>(((r - min_range_0) / ring_sizes_[0])), num_ring_0 - 1);
sector_idx = min(static_cast<int>((theta / sector_sizes_[0])), num_sector_0 - 1);
- czm[0][ring_idx][sector_idx].emplace_back(PointXYZ(x, y, z));
+ czm[0][ring_idx][sector_idx].emplace_back(PointXYZ(x, y, z, intensity));
} else if (r < min_range_2) {
ring_idx = min(static_cast<int>(((r - min_range_1) / ring_sizes_[1])), num_ring_1 - 1);
sector_idx = min(static_cast<int>((theta / sector_sizes_[1])), num_sector_1 - 1);
- czm[1][ring_idx][sector_idx].emplace_back(PointXYZ(x, y, z));
+ czm[1][ring_idx][sector_idx].emplace_back(PointXYZ(x, y, z, intensity));
} else if (r < min_range_3) {
ring_idx = min(static_cast<int>(((r - min_range_2) / ring_sizes_[2])), num_ring_2 - 1);
sector_idx = min(static_cast<int>((theta / sector_sizes_[2])), num_sector_2 - 1);
- czm[2][ring_idx][sector_idx].emplace_back(PointXYZ(x, y, z));
+ czm[2][ring_idx][sector_idx].emplace_back(PointXYZ(x, y, z, intensity));
} else { // Far!
ring_idx = min(static_cast<int>(((r - min_range_3) / ring_sizes_[3])), num_ring_3 - 1);
sector_idx = min(static_cast<int>((theta / sector_sizes_[3])), num_sector_3 - 1);
- czm[3][ring_idx][sector_idx].emplace_back(PointXYZ(x, y, z));
+ czm[3][ring_idx][sector_idx].emplace_back(PointXYZ(x, y, z, intensity));
}
} else {
- cloud_nonground_.push_back(PointXYZ(x, y, z));
+ cloud_nonground_.push_back(PointXYZ(x, y, z, intensity));
}
}
if (params_.verbose) cout << "\033[1;33m" << "PatchWorkpp::pc2czm() - Divides pointcloud into the concentric zone model successfully" << "\033[0m" << endl;