[pointcloud]: Let's see if this is good for python

NOSALRO · Jul 18, 2023 · 1fa1d25 · 1fa1d25
1 parent 2d13c36
commit 1fa1d25
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 9 deletions.
diff --git a/src/examples/cameras.cpp b/src/examples/cameras.cpp
@@ -99,11 +99,12 @@ int main()
     auto depth_image = camera->depth_array();
 
     auto small_box = robot_dart::Robot::create_box({0.01, 0.01, 0.01}, Eigen::Vector6d::Zero(), "fixed", 1., dart::Color::Blue(1.), "box_pc");
-    std::vector<Eigen::Vector3d> point_cloud = robot_dart::gui::point_cloud_from_depth_array(depth_image, camera->camera_intrinsic_matrix(), camera->camera_extrinsic_matrix(), camera->camera().far_plane());
-    for (size_t i = 0; i < point_cloud.size(); i++) {
+    std::vector<double> data = robot_dart::gui::point_cloud_from_depth_array(depth_image, camera->camera_intrinsic_matrix(), camera->camera_extrinsic_matrix(), camera->camera().far_plane());
+    Eigen::MatrixXd point_cloud = Eigen::MatrixXd::Map(data.data(), 3, data.size() / 3);
+    for (int i = 0; i < point_cloud.cols(); i++) {
         if (i % 30 == 0) { // do not display all the points in the cloud
             Eigen::Vector6d pose;
-            pose << 0., 0., 0., point_cloud[i];
+            pose << 0., 0., 0., point_cloud.col(i);
             auto bbox = small_box->clone_ghost("box_" + std::to_string(i), dart::Color::Blue(1.));
             bbox->set_base_pose(pose);
             bbox->set_cast_shadows(false);
@@ -119,8 +120,8 @@ int main()
             break;
         if (simu.schedule(simu.graphics_freq())) {
             auto depth_image = camera->depth_array();
-            std::vector<Eigen::Vector3d> point_cloud = robot_dart::gui::point_cloud_from_depth_array(depth_image, camera->camera_intrinsic_matrix(), camera->camera_extrinsic_matrix(), camera->camera().far_plane());
-            std::cout << simu.scheduler().current_time() << ": " << point_cloud.size() << std::endl;
+            std::vector<double> data = robot_dart::gui::point_cloud_from_depth_array(depth_image, camera->camera_intrinsic_matrix(), camera->camera_extrinsic_matrix(), camera->camera().far_plane());
+            std::cout << simu.scheduler().current_time() << ": " << (data.size() / 3) << std::endl;
         }
     }
 

diff --git a/src/robot_dart/gui/helper.cpp b/src/robot_dart/gui/helper.cpp
@@ -58,7 +58,7 @@ namespace robot_dart {
             return gray;
         }
 
-        std::vector<Eigen::Vector3d> point_cloud_from_depth_array(const DepthImage& depth_image, const Eigen::Matrix3d& intrinsic_matrix, const Eigen::Matrix4d& tf, double far_plane)
+        std::vector<double> point_cloud_from_depth_array(const DepthImage& depth_image, const Eigen::Matrix3d& intrinsic_matrix, const Eigen::Matrix4d& tf, double far_plane)
         {
             // This is assuming that K is normal intrisinc matrix (i.e., camera pointing to +Z),
             // but an OpenGL camera (i.e., pointing to -Z). Thus it transforms the points accordingly
@@ -80,7 +80,7 @@ namespace robot_dart {
                 return p;
             };
 
-            std::vector<Eigen::Vector3d> point_cloud;
+            std::vector<double> point_cloud;
 
             size_t height = depth_image.height;
             size_t width = depth_image.width;
@@ -94,7 +94,9 @@ namespace robot_dart {
                     pp.tail(1) << 1.;
                     pp = tf.inverse() * pp;
 
-                    point_cloud.push_back(pp.head(3));
+                    point_cloud.push_back(pp[0]);
+                    point_cloud.push_back(pp[1]);
+                    point_cloud.push_back(pp[2]);
                 }
             }
 

diff --git a/src/robot_dart/gui/helper.hpp b/src/robot_dart/gui/helper.hpp
@@ -29,7 +29,7 @@ namespace robot_dart {
 
         GrayscaleImage convert_rgb_to_grayscale(const Image& rgb);
 
-        std::vector<Eigen::Vector3d> point_cloud_from_depth_array(const DepthImage& depth_image, const Eigen::Matrix3d& intrinsic_matrix, const Eigen::Matrix4d& tf, double far_plane = 1000.);
+        std::vector<double> point_cloud_from_depth_array(const DepthImage& depth_image, const Eigen::Matrix3d& intrinsic_matrix, const Eigen::Matrix4d& tf, double far_plane = 1000.);
     } // namespace gui
 } // namespace robot_dart