Saving a colorful pointcloud using sofware-device

[ilyak93]

---

Required Info
Camera Model	D465
Operating System & Version	Win 10
Platform	PC
SDK Version	2.48.0.3381
Language	C++

I'm creating a software-device in the following way:

    rs2::software_device dev;

    auto depth_sensor = dev.add_sensor("Depth");
    auto color_sensor = dev.add_sensor("Color");

    rs2_intrinsics depth_intrinsics{W, H, stof(depth[3]), stof(depth[4]),
                                    stof(depth[5]), stof(depth[6]),
                                    RS2_DISTORTION_BROWN_CONRADY,
                                    {stof(depth[7]), stof(depth[8]),
                                     stof(depth[9]), stof(depth[10]),
                                     stof(depth[11])}};
    rs2_intrinsics color_intrinsics{W, H, stof(color[3]), stof(color[4]),
                                    stof(color[5]), stof(color[6]),
                                    RS2_DISTORTION_INVERSE_BROWN_CONRADY,
                                    {stof(color[7]), stof(color[8]),
                                     stof(color[9]), stof(color[10]),
                                     stof(color[11])}};

    auto depth_stream = depth_sensor.add_video_stream(
            {RS2_STREAM_DEPTH, 0, 0, W, H, 30, 2, RS2_FORMAT_Z16,
             depth_intrinsics});
    auto color_stream = color_sensor.add_video_stream(
            {RS2_STREAM_COLOR, 0, 1, W, H, 30, 3, RS2_FORMAT_RGB8,
             color_intrinsics});

    depth_sensor.add_read_only_option(RS2_OPTION_DEPTH_UNITS, 0.001f);
    depth_sensor.add_read_only_option(RS2_OPTION_STEREO_BASELINE, 0.001f);

    depth_stream.register_extrinsics_to(color_stream,
                                        {
        {
            stof(depth_to_color[1]),stof(depth_to_color[2]),
            stof(depth_to_color[3]),stof(depth_to_color[4]),
            stof(depth_to_color[5]),stof(depth_to_color[6]),
            stof(depth_to_color[7]),stof(depth_to_color[8]),
            stof(depth_to_color[9])
            },
            {
            stof(depth_to_color[11]), stof(depth_to_color[12]),
            stof(depth_to_color[12])
            }
                                        });

    dev.create_matcher(RS2_MATCHER_DEFAULT);
    rs2::syncer sync;

    depth_sensor.open(depth_stream);
    color_sensor.open(color_stream);

    depth_sensor.start(sync);
    color_sensor.start(sync);

    rs2::align align(RS2_STREAM_COLOR);

    rs2::frameset fs;
    rs2::frame depth_frame;
    rs2::frame color_frame;

    cv::Mat color_image;
    cv::Mat depth_image;

and while looping over the frames:

        string color_image_path = string("G:/Vista_project/finish1/rs/") + string(ent_color.d_name);
        string depth_image_path = string("G:/Vista_project/finish1/rs/") + string(ent_depth.d_name);
        color_image = cv::imread(color_image_path.c_str(), cv::IMREAD_COLOR);
        depth_image = cv::imread(depth_image_path.c_str(),cv::IMREAD_UNCHANGED);

        color_sensor.on_video_frame({(void*) color_image.data, // Frame pixels from capture API
                                     [](void*) {}, // Custom deleter (if required)
                                     3 * 1280, 3, // Stride and Bytes-per-pixel
                                     double(idx * 30), RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME,
                                     idx, // Timestamp, Frame# for potential sync services
                                     color_stream});
        depth_sensor.on_video_frame({(void*) depth_image.data, // Frame pixels from capture API
                                     [](void*) {}, // Custom deleter (if required)
                                     2 * 1280, 2, // Stride and Bytes-per-pixel
                                     double(idx * 30), RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME,
                                     idx, // Timestamp, Frame# for potential sync services
                                     depth_stream});

        fs = sync.wait_for_frames();
        pc_idx++;
        if (fs.size() == 2) {

            rs2::frame depth_frame2 = fs.get_depth_frame();
            auto color_frame2 = fs.get_color_frame();

            rs2::pointcloud pc;
            rs2::points points;
            points = pc.calculate(depth_frame2);
            pc.map_to(color_frame2);
            //const rs2::texture_coordinate* coordinates =  points.get_texture_coordinates();
            points.export_to_ply("G:/Vista_project/finish1/rs-ptc/"
                                 + to_string(pc_idx) + "1.ply", color_frame2);
            rs2::save_to_ply exporter("G:/Vista_project/finish1/rs-ptc/"
                                      + to_string(pc_idx) + ".ply", pc);
            exporter.set_option(rs2::save_to_ply::OPTION_PLY_BINARY, 0.f);
            exporter.set_option(rs2::save_to_ply::OPTION_IGNORE_COLOR, 0.f);
            exporter.export_to_ply(points, color_frame2);


            fs = align.process(fs);
            rs2::frame depth_frame = fs.get_depth_frame();
            rs2::frame color_frame = fs.get_color_frame();

            cv::Mat aligned_image(720, 1280, CV_16UC1, (void*) (depth_frame.get_data()), 2 * 1280);
            cv::imwrite("G:/Vista_project/finish1//aligned_rs/" + string(ent_color.d_name), aligned_image);

        }
        idx++;

I'm trying to save the pointcloud in a ply format.

I try both methods: using points.export_to_ply (many people wrote here in issues that it worked for them (probably on live streaming and not software-device)) and the method suggested on issue #6677 (by making it public and accessible in rs_export.hpp).

In both I get one gray color for all the points. Further I found that:

I debugged the function export_to_ply of the object rs2::save_to_ply and found that texcoords[i] for each i (all the points) are all set to 0 (the cout << "here" loop is my code injected into real sense rs_export.hpp, which why probably I get only one color (the color of (u=0,v=0)), which means my pointcloud somehow doesn't know what is his texture coordinates. Maybe it is somehow connected to the fact it is software-device, some data maybe lost, I think the implementation of this function rs2::points points = pc.calculate(depth_frame2); (calculate) should give the answers if you can look around it.

That's the whole peace of code above in two images) from rs_export.hpp

void export_to_ply(points p, video_frame color) {
            const bool use_texcoords  = color && !get_option(OPTION_IGNORE_COLOR);
            bool mesh = get_option(OPTION_PLY_MESH);
            bool binary = get_option(OPTION_PLY_BINARY);
            bool use_normals = get_option(OPTION_PLY_NORMALS);
            const auto verts = p.get_vertices();
            const auto texcoords = p.get_texture_coordinates();
			
			for (size_t i = 0; i < p.size(); ++i) { 
				if(texcoords[i].u !=0 || texcoords[i].v != 0){
					std::cout << "here";
				}
			}
			
            const uint8_t* texture_data;
            if (use_texcoords) // texture might be on the gpu, get pointer to data before for-loop to avoid repeated access
                texture_data = reinterpret_cast<const uint8_t*>(color.get_data());
            std::vector<rs2::vertex> new_verts;
            std::vector<vec3d> normals;
            std::vector<std::array<uint8_t, 3>> new_tex;
            std::map<size_t, size_t> idx_map;
            std::map<size_t, std::vector<vec3d>> index_to_normals;

            new_verts.reserve(p.size());
            if (use_texcoords) new_tex.reserve(p.size());

            static const auto min_distance = 1e-6;

            for (size_t i = 0; i < p.size(); ++i) {
                if (fabs(verts[i].x) >= min_distance || fabs(verts[i].y) >= min_distance ||
                    fabs(verts[i].z) >= min_distance)
                {
                    idx_map[int(i)] = int(new_verts.size());
                    new_verts.push_back({ verts[i].x, -1 * verts[i].y, -1 * verts[i].z });
                    if (use_texcoords)
                    {
                        auto rgb = get_texcolor(color, texture_data, texcoords[i].u, texcoords[i].v);
                        new_tex.push_back(rgb);
                    }
                }
            }

@MartyG-RealSense, that's the new issue from the #6677 discussion.

I've updated my SDK to the latest version my alignment code works, which is 2.48.0.
After that the alignment just gives black image (aligning depth to color) and both methods write 0 points, such that the calculate function doesn't creates any points.

The alignment which also kind of an assurance I'm not breaking the basic functionality of software-device and it's intrinsics/extrinsics registration. The alignment works fine while I cant get the ply of a colorful pointcloud.

Thank you in advance.

[MartyG-RealSense]

hI @ilyak93 When OPTION_PLY_BINARY is set to 0 / false, it exports the ply as ASCII text characters, as described in the save_to_ply documentation at #4906

So can you try using false / true instead of 0 / 1 and setting OPTION_PLY_BINARY to true to see whether it results in the export of a colored cloud, please?

exporter.set_option(rs2::save_to_ply::OPTION_PLY_BINARY, true);
exporter.set_option(rs2::save_to_ply::OPTION_IGNORE_COLOR, false);

[ilyak93]

@MartyG-RealSense, nope, that didn't changed the result, the texcoords are still all (0,0), same behavior.

Do you maybe where in the file system I can look at the implementation of calculate() function that creates the point cloud ?

Or even maybe instructions to linking the library without installing it, so I can debug it and see where is the problem.

[ilyak93]

@MartyG-RealSense,
I have found a solution (using points.export_to_ply) in issue #900 , this have worked for me:

Hi @tchapi

Can you try:

auto color = frames.get_color_frame();
pc.map_to(color);
points = pc.calculate(depth);

Is the rs-pointcloud demo working properly on your system?

[MartyG-RealSense]

That's great to hear! I was just going to show you that old case at #900 that was experiencing (0,0) texcoords. A RealSense user in that case found in #900 (comment) that calling pc.calculate(depth) twice worked for them. I'm very pleased that you were able to develop your own solution!

[ilyak93]

@MartyG-RealSense, actually the same solution worked for me, forgot to mention that the whole working code looks like:

points = pc.calculate(depth);
auto color = frames.get_color_frame();
pc.map_to(color);
points = pc.calculate(depth);

So the second calculation gives the proper colorful pointcloud.