Merge branch 'pc' of github.com:resibots/robot_dart into pc

Author: costashatz

src/examples/cameras.cpp

@@ -76,6 +76,8 @@ int main()
     simu.add_robot(robot_dart::Robot::create_ellipsoid({0.2, 0.2, 0.2}, pose, "free", 1., dart::Color::Red(1.), "sphere"));
     simu.add_sensor(camera);
 
+    simu.set_graphics_freq(20);
+
     simu.run(10.);
 
     // a nested std::vector (w*h*3) of the last image taken can be retrieved
@@ -99,10 +101,9 @@ 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<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);
+    Eigen::MatrixXd 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 (int i = 0; i < point_cloud.cols(); i++) {
-        if (i % 30 == 0) { // do not display all the points in the cloud
+        if (i % 100 == 0) { // do not display all the points in the cloud
             Eigen::Vector6d pose;
             pose << 0., 0., 0., point_cloud.col(i);
             auto bbox = small_box->clone_ghost("box_" + std::to_string(i), dart::Color::Blue(1.));
@@ -112,16 +113,18 @@ int main()
         }
     }
 
-    simu.set_graphics_freq(20);
     simu.world()->setTime(0.);
     simu.scheduler().reset(simu.timestep(), true);
     while (true) {
         if (simu.step())
             break;
         if (simu.schedule(simu.graphics_freq())) {
+            auto start = std::chrono::high_resolution_clock::now();
             auto depth_image = camera->depth_array();
-            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;
+            Eigen::MatrixXd data = robot_dart::gui::point_cloud_from_depth_array(depth_image, camera->camera_intrinsic_matrix(), camera->camera_extrinsic_matrix(), camera->camera().far_plane());
+            auto end = std::chrono::high_resolution_clock::now();
+            std::chrono::duration<double, std::milli> t_pc = (end - start);
+            std::cout << simu.scheduler().current_time() << ": " << (data.cols()) << " -> " << t_pc.count() << "ms" << std::endl;
         }
     }
 

src/python/gui.cpp

@@ -293,7 +293,7 @@ namespace robot_dart {
             sm.def("save_png_image", static_cast<void (*)(const std::string&, const gui::Image&)>(&gui::save_png_image));
             sm.def("save_png_image", static_cast<void (*)(const std::string&, const gui::GrayscaleImage&)>(&gui::save_png_image));
             sm.def("convert_rgb_to_grayscale", gui::convert_rgb_to_grayscale);
-            sm.def("point_cloud_from_depth_array", gui::point_cloud_from_depth_array,
+            sm.def("point_cloud_from_depth_array", gui::point_cloud_from_depth_array_vector,
                 py::arg("depth_image"),
                 py::arg("intrinsic_matrix"),
                 py::arg("tf"),

src/robot_dart/gui/helper.cpp

@@ -58,7 +58,7 @@ namespace robot_dart {
             return gray;
         }
 
-        std::vector<double> 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_vector(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
@@ -102,5 +102,11 @@ namespace robot_dart {
 
             return point_cloud;
         }
+
+        Eigen::MatrixXd point_cloud_from_depth_array(const DepthImage& depth_image, const Eigen::Matrix3d& intrinsic_matrix, const Eigen::Matrix4d& tf, double far_plane)
+        {
+            auto v = point_cloud_from_depth_array_vector(depth_image, intrinsic_matrix, tf, far_plane);
+            return Eigen::MatrixXd::Map(v.data(), 3, v.size() / 3);
+        }
     } // namespace gui
 } // namespace robot_dart

src/robot_dart/gui/helper.hpp

@@ -29,7 +29,8 @@ namespace robot_dart {
 
         GrayscaleImage convert_rgb_to_grayscale(const Image& rgb);
 
-        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.);
+        std::vector<double> point_cloud_from_depth_array_vector(const DepthImage& depth_image, const Eigen::Matrix3d& intrinsic_matrix, const Eigen::Matrix4d& tf, double far_plane = 1000.);
+        Eigen::MatrixXd 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