Saving point cloud as ply including normals

[philianeles]

Required Info
Camera Model	D435i
Operating System & Version	Win 10
Platform	PC
SDK Version	2.0
Language	C
Segment	Robot

I have built the librealsense sdk from source - I am looking for a C++ example for saving point cloud as ply, including normals, would there be one?

[MartyG-RealSense]

The link below has a C++ script submitted by a RealSense user that they said exports a ply with mesh and faces:

#6411

That RealSense user and another one also took the alternative approach of computing a point cloud with voxels instead of normals:

#6802 (comment)

If using C++ is not an absolute requirement, the link below has a guide for exporting a ply from the RealSense Viewer and then importing it into a program called MeshLab and computing the normals:

https://www.andreasjakl.com/capturing-3d-point-cloud-intel-realsense-converting-mesh-meshlab/

[philianeles]

Thanks @MartyG-RealSense!
Now I found save_to_plyin librealsense\include\librealsense2\hpp\rs_export.hpp which should ideally work:

rs2::save_to_ply exporter("Test ", filtered_pc);
exporter.set_option(rs2::save_to_ply::OPTION_IGNORE_COLOR, 0.f);
exporter.set_option(rs2::save_to_ply::OPTION_PLY_MESH, 0.f);
exporter.set_option(rs2::save_to_ply::OPTION_PLY_NORMALS, 0.f);

But I don`t see the ply saved in the directory - any idea what could be the problem?

[MartyG-RealSense]

It looks as though your script is on the right track. Compared to the C++ script in the link below that is suggested by Dorodnic the RealSense SDK Manager though, your version does not have an exporter.export_to_ply statement at the end of it after the exporter.set_option statements.

#6677 (comment)

[philianeles]

after adding that too:

rs2::save_to_ply exporter("Test ", filtered_pc);
exporter.set_option(rs2::save_to_ply::OPTION_PLY_BINARY, 0.f);
exporter.set_option(rs2::save_to_ply::OPTION_IGNORE_COLOR, 1.f);
exporter.set_option(rs2::save_to_ply::OPTION_PLY_MESH, 1.f);
exporter.set_option(rs2::save_to_ply::OPTION_PLY_NORMALS, 1.f);
exporter.export_to_ply(filtered_points, colored_filtered);

I have this error:
'rs2::save_to_ply::export_to_ply': cannot access private member declared in class 'rs2::save_to_ply'

Any clues how to avoid this?

[MartyG-RealSense]

In the rs2::save_to_ply exporter line, I'm not sure that "Test " is a valid path for a location and filename to save the ply file to.

Try this please:

rs2::save_to_ply exporter('test.ply', filtered_pc);

[philianeles]

also tried that but the error persists (please see below):

[MartyG-RealSense]

Comparing your script to the earlier case with Dorodnic's advice that I linked to, it looks as though you should include this in your rs2:: list:

rs2::pointcloud pc;

And then set the pc definition in save_to_ply:

rs2::save_to_ply exporter('test.ply', pc);

I also suspect that by having more than one type of the same rs2:: reference (two rs2::frame and rs2::points) instead of one rs2:: for each type, it may be confusing the program.

[philianeles]

Even though I would include rs2::pointcloud pc in my rs2:: list for saving, the problem is rather I think the export_to_ply is a private function in rs_export.hpp --> the exporter object can't visit it. @dorodnic would you be able to provide some example code how to use the export_to_ply including normals?

[MartyG-RealSense]

Hi @philianeles Do you still require assistance with this case, please? Thanks!

[philianeles]

@MartyG-RealSense yes please, the latest state I am at is here: #6979 (comment)

[MartyG-RealSense]

Hi @philianeles I completely re-researched the case and turned up one new C++ lead, in which it is suggested that export_to_ply be called from rs2::points in a way that is a bit different from yours:

#2832 (comment)

[philianeles]

@MartyG-RealSense thank you! the issue that I had before (#6979 (comment)) seems to be gone with this new lead however I see this error now:

would you have an idea why this error is thrown?

I have this structure now:

    rs2::pointcloud pc_to_save;
    rs2::points points_to_save;
    rs2::frame depthFrame;
    rs2::frame colorFrame;

    points_to_save = pc_to_save.calculate(depthFrame);

    //rs2::save_to_ply exporter("test.ply ", pc_to_save);
    //exporter.set_option(rs2::save_to_ply::OPTION_PLY_BINARY, 0.f);
    //exporter.set_option(rs2::save_to_ply::OPTION_IGNORE_COLOR, 1.f);
    //exporter.set_option(rs2::save_to_ply::OPTION_PLY_MESH, 1.f);
    //exporter.set_option(rs2::save_to_ply::OPTION_PLY_NORMALS, 1.f);
    points_to_save.export_to_ply("test.ply ", colorFrame);

[MartyG-RealSense]

Assuming that the program got past the switch instruction okay, I would guess that it is failing at the CrtDbgBreak() instruction. It looks as though this is a call to check the contents of a function called CrtDbgBreak(). Do you actually have a function in the project with that exact name CrtDbgBreak() ?

[philianeles]

I am actually trying in an existing example project (librealsense2.sln), inside rs-post-processing
here is the whole thing: (it runs without throwing an error now but there is no test.ply saved anywhere? from this line: points_to_save.export_to_ply("test.ply", colored_filtered) at the very end)

// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2017 Intel Corporation. All Rights Reserved.

#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include "example.hpp"          // Include short list of convenience functions for rendering
#include <librealsense2/hpp/rs_export.hpp> // Include RealSense Cross Platform API

#include <map>
#include <string>
#include <thread>
#include <atomic>

#include <imgui.h>
#include "imgui_impl_glfw.h"

/**
Helper class for controlling the filter's GUI element
*/
struct filter_slider_ui
{
    std::string name;
    std::string label;
    std::string description;
    bool is_int;
    float value;
    rs2::option_range range;

    bool render(const float3& location, bool enabled);
    static bool is_all_integers(const rs2::option_range& range);
};

/**
Class to encapsulate a filter alongside its options
*/
class filter_options
{
public:
    filter_options(const std::string name, rs2::filter& filter);
    filter_options(filter_options&& other);
    std::string filter_name;                                   //Friendly name of the filter
    rs2::filter& filter;                                       //The filter in use
    std::map<rs2_option, filter_slider_ui> supported_options;  //maps from an option supported by the filter, to the corresponding slider
    std::atomic_bool is_enabled;                               //A boolean controlled by the user that determines whether to apply the filter or not
};

// Helper functions for rendering the UI
void render_ui(float w, float h, std::vector<filter_options>& filters);
// Helper function for getting data from the queues and updating the view
void update_data(rs2::frame_queue& data, rs2::frame& depth, rs2::points& points, rs2::pointcloud& pc, glfw_state& view, rs2::colorizer& color_map);

int main(int argc, char * argv[]) try
{
    // Create a simple OpenGL window for rendering:
    window app(1280, 720, "RealSense Post Processing Example");
    ImGui_ImplGlfw_Init(app, false);

    // Construct objects to manage view state
    glfw_state original_view_orientation{};
    glfw_state filtered_view_orientation{};

    // Declare pointcloud objects, for calculating pointclouds and texture mappings
    rs2::pointcloud original_pc;
    rs2::pointcloud filtered_pc;

    // Declare RealSense pipeline, encapsulating the actual device and sensors
    rs2::pipeline pipe;
    rs2::config cfg;
    // Use a configuration object to request only depth from the pipeline
    cfg.enable_stream(RS2_STREAM_DEPTH, 640, 0, RS2_FORMAT_Z16, 30);
    // Start streaming with the above configuration
    pipe.start(cfg);

    // Declare filters
    rs2::decimation_filter dec_filter;  // Decimation - reduces depth frame density
    rs2::threshold_filter thr_filter;   // Threshold  - removes values outside recommended range
    rs2::spatial_filter spat_filter;    // Spatial    - edge-preserving spatial smoothing
    rs2::temporal_filter temp_filter;   // Temporal   - reduces temporal noise

                                        // Declare disparity transform from depth to disparity and vice versa
    const std::string disparity_filter_name = "Disparity";
    rs2::disparity_transform depth_to_disparity(true);
    rs2::disparity_transform disparity_to_depth(false);

    // Initialize a vector that holds filters and their options
    std::vector<filter_options> filters;

    // The following order of emplacement will dictate the orders in which filters are applied
    filters.emplace_back("Decimate", dec_filter);
    filters.emplace_back("Threshold", thr_filter);
    filters.emplace_back(disparity_filter_name, depth_to_disparity);
    filters.emplace_back("Spatial", spat_filter);
    filters.emplace_back("Temporal", temp_filter);

    // Declaring two concurrent queues that will be used to enqueue and dequeue frames from different threads
    rs2::frame_queue original_data;
    rs2::frame_queue filtered_data;

    // Declare depth colorizer for pretty visualization of depth data
    rs2::colorizer color_map;

    // Atomic boolean to allow thread safe way to stop the thread
    std::atomic_bool stopped(false);

    // Create a thread for getting frames from the device and process them
    // to prevent UI thread from blocking due to long computations.
    std::thread processing_thread([&]() {
        while (!stopped) //While application is running
        {
            rs2::frameset data = pipe.wait_for_frames(); // Wait for next set of frames from the camera
            rs2::frame depth_frame = data.get_depth_frame(); //Take the depth frame from the frameset
            if (!depth_frame) // Should not happen but if the pipeline is configured differently
                return;       //  it might not provide depth and we don't want to crash

            rs2::frame filtered = depth_frame; // Does not copy the frame, only adds a reference

            /* Apply filters.
            The implemented flow of the filters pipeline is in the following order:
            1. apply decimation filter
            2. apply threshold filter
            3. transform the scene into disparity domain
            4. apply spatial filter
            5. apply temporal filter
            6. revert the results back (if step Disparity filter was applied
            to depth domain (each post processing block is optional and can be applied independantly).
            */
            bool revert_disparity = false;
            for (auto&& filter : filters)
            {
                if (filter.is_enabled)
                {
                    filtered = filter.filter.process(filtered);
                    if (filter.filter_name == disparity_filter_name)
                    {
                        revert_disparity = true;
                    }
                }
            }
            if (revert_disparity)
            {
                filtered = disparity_to_depth.process(filtered);
            }

            // Push filtered & original data to their respective queues
            // Note, pushing to two different queues might cause the application to display
            //  original and filtered pointclouds from different depth frames
            //  To make sure they are synchronized you need to push them together or add some
            //  synchronization mechanisms
            filtered_data.enqueue(filtered);
            original_data.enqueue(depth_frame);
        }
    });


    // Declare objects that will hold the calculated pointclouds and colored frames
    // We save the last set of data to minimize flickering of the view
    rs2::frame colored_depth;
    rs2::frame colored_filtered;
    rs2::points original_points;
    rs2::points filtered_points;

    // Save the time of last frame's arrival
    auto last_time = std::chrono::high_resolution_clock::now();
    // Maximum angle for the rotation of the pointcloud
    const double max_angle = 15.0;
    // We'll use rotation_velocity to rotate the pointcloud for a better view of the filters effects
    float rotation_velocity = 0.3f;

    while (app)
    {
        float w = static_cast<float>(app.width());
        float h = static_cast<float>(app.height());

        // Render the GUI
        render_ui(w, h, filters);

        // Try to get new data from the queues and update the view with new texture
        update_data(original_data, colored_depth, original_points, original_pc, original_view_orientation, color_map);
        update_data(filtered_data, colored_filtered, filtered_points, filtered_pc, filtered_view_orientation, color_map);

        draw_text(10, 50, "Original");
        draw_text(static_cast<int>(w / 2), 50, "Filtered");

        // Draw the pointclouds of the original and the filtered frames (if the are available already)
        if (colored_depth && original_points)
        {
            glViewport(0, int(h) / 2, int(w) / 2, int(h) / 2);
            draw_pointcloud(int(w) / 2, int(h) / 2, original_view_orientation, original_points);
        }
        if (colored_filtered && filtered_points)
        {
            glViewport(int(w) / 2, int(h) / 2, int(w) / 2, int(h) / 2);
            draw_pointcloud(int(w) / 2, int(h) / 2, filtered_view_orientation, filtered_points);
        }
        // Update time of current frame's arrival
        auto curr = std::chrono::high_resolution_clock::now();

        // Time interval which must pass between movement of the pointcloud
        const std::chrono::milliseconds rotation_delta(40);

        // In order to calibrate the velocity of the rotation to the actual displaying speed, rotate
        //  pointcloud only when enough time has passed between frames
        if (curr - last_time > rotation_delta)
        {
            if (fabs(filtered_view_orientation.yaw) > max_angle)
            {
                rotation_velocity = -rotation_velocity;
            }
            original_view_orientation.yaw += rotation_velocity;
            filtered_view_orientation.yaw += rotation_velocity;
            last_time = curr;
        }
    }

    rs2::pointcloud pc_to_save;
    rs2::points points_to_save;

    points_to_save = pc_to_save.calculate(colored_depth);

    //rs2::save_to_ply exporter("test.ply ", pc_to_save);
    //exporter.set_option(rs2::save_to_ply::OPTION_PLY_BINARY, 0.f);
    //exporter.set_option(rs2::save_to_ply::OPTION_IGNORE_COLOR, 1.f);
    //exporter.set_option(rs2::save_to_ply::OPTION_PLY_MESH, 1.f);
    //exporter.set_option(rs2::save_to_ply::OPTION_PLY_NORMALS, 1.f);
    points_to_save.export_to_ply("test.ply", colored_filtered);

    // Signal the processing thread to stop, and join
    // (Not the safest way to join a thread, please wrap your threads in some RAII manner)
    stopped = true;
    processing_thread.join();

    return EXIT_SUCCESS;
}

[MartyG-RealSense]

At the bottom of the script, most of the point cloud saving code is commented out, preventing it from running. This includes the definitions for the format that the data will be saved in.

If the ply file was successfully saved, I suspect that it would be saved in the same folder as the script since there is not a path defined for "test.ply".

[philianeles]

I am not sure how the settings would effect the actual export action (would it not take default settings if custom ones are not provided?) but just in case I have included them back. Still I do not know why I cannot see the ply in the same folder. As you pointed out, it should save it in the same folder if no path is given - or am I missing something?

[MartyG-RealSense]

Does it still error if you do this:

points_to_save.export_to_ply(points_to_save, colored_filtered);

[philianeles]

Yes then it throws this error: "no suitable user-defined conversion from "rs2::points" to "const std::string" exists"

[MartyG-RealSense]

At this point the best way forward would be to get a second opinion on your script from a developer so that we don't go round in circles. @dorodnic or @ev-mp could you take a look at the ply export script above please to see why a ply file is not being created? Thanks!

[MartyG-RealSense]

Reminder: @dorodnic or @ev-mp could you take a look at the ply export script above please to see why a ply file is not being created? Thanks!

[MartyG-RealSense]

Hi @philianeles I apologise that you have not received further comment about your ply export problem. I have highlighted the case with Intel internally to draw attention to it.

[RealSenseSupport]

The point cloud is calculated based on the depth frame. The rs-post-processing has an example of how to properly generate the point cloud from the depth frame, colorized the depth and merge it with the point could. Please check:
https://github.com/IntelRealSense/librealsense/blob/master/examples/post-processing/rs-post-processing.cpp#L237

Replacing the colored_depth with the actual depth frame would make your code work.
The ply file will be generated on exit in the same directory where the program run.

	rs2::frame f;
	if (filtered_data.poll_for_frame(&f))  // Try to take the depth and points from the queue
	{
		colorized_depth = color_map.process(f);     // Colorize the depth frame with a color map
		pc_to_save.map_to(colorized_depth);         // Map the colored depth to the point cloud
		points_to_save = pc_to_save.calculate(f); // Generate pointcloud from the depth data
		points_to_save.export_to_ply("test.ply", colorized_depth);
	}

    // Signal the processing thread to stop, and join
    // (Not the safest way to join a thread, please wrap your threads in some RAII manner)
    stopped = true;
    processing_thread.join();

[philianeles]

Thanks for the tips. There is no error:

However I still do not see any .ply generated on exit in the same directory where the program runs (or anywhere else):

[philianeles]

sorry it works now (somehow not in debugging mode from VS but only running the .exe)! Anyhow, thanks for the help!

[philianeles]

@RealSenseSupport and one more question how can I use options like OPTION_PLY_BINARY, OPTION_PLY_NORMALS or OPTION_PLY_MESH with export_to_ply?