Export point cloud

[UcefMountacer]

Hi,

Has anyone tried to export the point cloud after processing data? there are some functions to export the trajectory but nothing for point cloud.

It would be great to export it like the trajectory that already have a function for that:

SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");

[catproof]

I am also interested in this.

[ghost]

Also interested.

[NJNischal]

Hello everyone, have any of you figured out how to save the point cloud points from ORBSLAM3?

[WFram]

For example, you can use ROS and PCL library for this purpose.

Just use functions GetCurrentMap() and GetMapPoints(), it will give a set of landmarks. Which can be then converted to PointCloudXYZ (data structure in PCL library to store a point cloud). Once the mapping has finished, the point cloud can be saved to .pcd file using PCL I/O interface.

There is a raw example of how to do it: https://github.com/WFram/vslam2/blob/98ddf36524b162664ec71cc9269eaaf83a560769/ORB_SLAM3_ROS/src/viewer.cpp#L216

[altaykacan]

Hey all,

a bit late to the discussion but I implemented a function inspired by @WFram 's example that works similarly to the SaveTrajectory functions.

Here's my code:

void System::SavePointCloud(const string &filename){
    // Code is based on MapDrawer::DrawMapPoints()
    cout << endl << "Saving map point coordinates to " << filename << " ..." << endl;
    cout << endl << "Number of maps is: " << mpAtlas->CountMaps() << endl;

    // TODO Get all maps or is the current active map is enough?
    // vector<Map*> vpAllMaps = mpAtlas->GetAllMaps()

    Map* pActiveMap = mpAtlas->GetCurrentMap();
    if(!pActiveMap) {
        cout << endl << "There is no active map (pActiveMap is null)" << endl;
        return;
    }

    // Vectors containing pointers to MapPoint objects contained in the maps
    // Vector of pointers for Map Points -- vpMPs
    // Vector of pointers for Reference Map Points -- vpRefMPs
    // TODO figure out the difference between Reference Map Points and normal Map Points
    const vector<MapPoint*> &vpMPs = pActiveMap->GetAllMapPoints();
    const vector<MapPoint*> &vpRefMPs = pActiveMap->GetReferenceMapPoints();

    if(vpMPs.empty()){
        cout << endl << "Vector of map points vpMPs is empty!" << endl;
        return;
    }

    // Use a set for fast lookup of reference frames
    set<MapPoint*> spRefMPs(vpRefMPs.begin(), vpRefMPs.end());

    // Get the output file stream in fixed-point format for map points
    ofstream f;
    f << "pos_x, pos_y, pos_z";
    f.open(filename.c_str());
    f << fixed;

    // TODO figure out if we need to consider whether the presence of IMU
    // requires some transforms/exceptions

    // Iterate over map points, skip "bad" ones and reference map points
    for (size_t i=0, iend=vpMPs.size(); i<iend;i++)
    {
        if (vpMPs[i]->isBad() || spRefMPs.count(vpMPs[i])){
            continue;
        }
        Eigen::Matrix<float,3,1> pos = vpMPs[i]->GetWorldPos();
        f << pos(0) << ", " << pos(1) << ", " << pos(2) << endl;
    }

    // Close the output stream
    f.close();

    // Get the output file stream in fixed-point format for reference map points
    f.open(("ref_" + filename).c_str());
    f << "pos_x, pos_y, pos_z" << endl;
    f << fixed;

    // Iterate over reference map points, skip if bad
    for (set<MapPoint*>::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++)
    {
        if((*sit)->isBad()){
            continue;
        }
        Eigen::Matrix<float,3,1> pos = (*sit)->GetWorldPos();
        f << pos(0) << ", " << pos(1) << ", " << pos(2) << endl;
    }

    // Close the output stream
    f.close();
}

Copy pasting this in System.cc and the function signature into System.h, and finally calling the function in whatever program you are using after SLAM.Shutdown() should hopefully work. In my experience the RefMapPoints seem to work better, i.e. the output file with the ref_ prefix. It seems to have way more points but I'm not sure about the exact difference.

There were a couple of things I was unsure about, e.g. the difference between "map points" and "reference points", whether we need to transform the points if we have an IMU, and whether taking the active map from the atlas is enough. I marked them with TODO's and will get back to them when I have time but according to my preliminary experiments with custom monocular data the results seem reasonable.

The code above basically saves the coordinates of the map points in a comma-separated file. I'm more of a Python person so I used the following Python code to save the results as PLY files. It needs the output of the function above and a custom formatted trajectory file. Just putting it here for reference and in case it helps someone:

import os
from pathlib import Path
from typing import Union

import numpy as np

def save_pointcloud_from_ORB_SLAM(input_file: Union[Path, str], output_file: Union[Path,str] = "out.ply"):
    """Converts a comma separated list of map point coordinates into
    PLY format for viewing the generated map.

    Args:
        input_file (str or Path): Path to the input file which is expected to
        be a .csv file with the columns pos_x, pos_y, pos_z designating the
        coordinates of the points in the world reference frame.

        output_file (str or Path): Path to the output .ply file, format is
        described here: https://paulbourke.net/dataformats/ply/
    """

    coords = np.genfromtxt(input_file, delimiter=", ", skip_header=1)

    x = coords[:, 0]
    y = coords[:, 1]
    z = coords[:, 2]

    ply_header = 'ply\n' \
                'format ascii 1.0\n' \
                'element vertex %d\n' \
                'property float x\n' \
                'property float y\n' \
                'property float z\n' \
                'end_header' % x.shape[0]

    np.savetxt(output_file, np.column_stack((x, y, z)), fmt='%f %f %f', header=ply_header, comments='')

def save_trajectory_from_ORB_SLAM(input_file: Union[Path, str], output_file: Union[Path,str] = "out_trajectory.ply"):
    """Converts the saved trajectory file from ORB-SLAM3 to a point cloud to then
    show alongside the mapped cloud.

    The input file is expected to be in the format (KITTI format with image path prepended, can ignore it):
    /path/to/image0.png R_00 R_01 R_02 t_0 R_10 R_11 R_12 t_1 R_20 R_21 R_22 t_2
    /path/to/image1.png R_00 R_01 R_02 t_0 R_10 R_11 R_12 t_1 R_20 R_21 R_22 t_2

    Where the R terms are the rotation and t terms are the translation terms
    of the homogeneous transformation matrix T_w_cam0.
    """
    x = []
    y = []
    z = []

    with open(input_file, "r") as file:
        lines = file.readlines()

    for line in lines:
        cols = line.strip().split(" ")

        x.append(float(cols[4]))
        y.append(float(cols[8]))
        z.append(float(cols[12]))

    # Each trajectory point is shown as a "point" in the "point cloud" which is the trajectory
    x = np.array(x)
    y = np.array(y)
    z = np.array(z)

    # RGB values for each point on the trajectory, set to be light green
    r = np.ones_like(x) * 144
    g = np.ones_like(x) * 238
    b = np.ones_like(x) * 144

    ply_header = 'ply\n' \
                'format ascii 1.0\n' \
                'element vertex %d\n' \
                'property float x\n' \
                'property float y\n' \
                'property float z\n' \
                'property uchar red\n' \
                'property uchar green\n' \
                'property uchar blue\n' \
                'end_header' % x.shape

    np.savetxt(output_file, np.column_stack((x, y, z, r, g, b)), fmt='%f %f %f %d %d %d', header=ply_header, comments='')

if __name__ == "__main__":
    input_file = "/path/to/point_cloud.txt" # ReferenceMapPoints seem to work better, use that file
    output_file = "./output.ply"

    input_trajectory = "/path/to/trajectory_file.txt"
    output_trajectory = "./output_trajectory.ply"

    save_pointcloud_from_ORB_SLAM(input_file, output_file)
    save_trajectory_from_ORB_SLAM(input_trajectory, output_trajectory)

After plotting the two .ply files on CloudCompare I got results like this on custom data, which shows a camera moving straight. So I believe the code works:

I know the answer a bit long and I'm no C++ expert but I really hope this helps! Please let me know if you notice any mistakes, we can maybe create a pull request later on. It seems like a very popular issue.

[jonaug88]

Hey all,

a bit late to the discussion but I implemented a function inspired by @WFram 's example that works similarly to the SaveTrajectory functions.

Here's my code:

After plotting the two .ply files on CloudCompare I got results like this on custom data, which shows a camera moving straight. So I believe the code works:

I know the answer a bit long and I'm no C++ expert but I really hope this helps! Please let me know if you notice any mistakes, we can maybe create a pull request later on. It seems like a very popular issue.

I am terribly sorry for ressurecting this topic, but i have a question regarding your solution. I implemented your changes to both System.cc and System.hm but when i stop the Map Viewer it saves the trajectory to CameraTrajectory.txt aswell as the keyframe trajectory. The point cloud itself does not save. Is there a way to add saving the point cloud in the same manner, or is there another way you'd rather enforce the SLAM.Shutdown() ?

Here is the output when stopping: " Shutdown

Saving trajectory to CameraTrajectory.txt ... There are 2 maps in the atlas Map 0 has 42 KFs Map 1 has 86 KFs

End of saving trajectory to CameraTrajectory.txt ...

Saving keyframe trajectory to KeyFrameTrajectory.txt ... "

I've made sure that the placement of "SavePointCloud" function is between the "SaveTrajectoryEuRoC" and SaveKeyFrameTrajectoryEuRoC functions as it seems that the output is originating from there, it seems like it doesnt initiate the Savepointcloud, as i dont even get the "couts". Do you have any ideas?

Thanks again for your hard work.

[altaykacan]

Hey all, a bit late to the discussion but I implemented a function inspired by @WFram 's example that works similarly to the SaveTrajectory functions. Here's my code: After plotting the two .ply files on CloudCompare I got results like this on custom data, which shows a camera moving straight. So I believe the code works:

I know the answer a bit long and I'm no C++ expert but I really hope this helps! Please let me know if you notice any mistakes, we can maybe create a pull request later on. It seems like a very popular issue.

I am terribly sorry for ressurecting this topic, but i have a question regarding your solution. I implemented your changes to both System.cc and System.hm but when i stop the Map Viewer it saves the trajectory to CameraTrajectory.txt aswell as the keyframe trajectory. The point cloud itself does not save. Is there a way to add saving the point cloud in the same manner, or is there another way you'd rather enforce the SLAM.Shutdown() ?

Here is the output when stopping: " Shutdown

Saving trajectory to CameraTrajectory.txt ... There are 2 maps in the atlas Map 0 has 42 KFs Map 1 has 86 KFs

End of saving trajectory to CameraTrajectory.txt ...

Saving keyframe trajectory to KeyFrameTrajectory.txt ... "

I've made sure that the placement of "SavePointCloud" function is between the "SaveTrajectoryEuRoC" and SaveKeyFrameTrajectoryEuRoC functions as it seems that the output is originating from there, it seems like it doesnt initiate the Savepointcloud, as i dont even get the "couts". Do you have any ideas?

Thanks again for your hard work.

Hey! Thank you for your question, I'd be very happy if I can help. It seems weird that even the first cout prints don't show up. Can you try adding your own print statements before and after you call SavePointCloud()? If those don't show up that means some other code is being executed and you should have a look there.

I think even if there is some bug in my hacky solution the first cout << endl << "Saving map point coordinates to " << filename << " ..." << endl; should definitely show up on the terminal.

Also, I know this is very simple but did you build the project again after making your adjustments? Otherwise your executable wouldn't get updated as far as I know. I had to learn that the hard way when I switched to C++ from coding in Python for years :D

[jonaug88]

Hey all, a bit late to the discussion but I implemented a function inspired by @WFram 's example that works similarly to the SaveTrajectory functions. Here's my code: After plotting the two .ply files on CloudCompare I got results like this on custom data, which shows a camera moving straight. So I believe the code works:

I know the answer a bit long and I'm no C++ expert but I really hope this helps! Please let me know if you notice any mistakes, we can maybe create a pull request later on. It seems like a very popular issue.

I am terribly sorry for ressurecting this topic, but i have a question regarding your solution. I implemented your changes to both System.cc and System.hm but when i stop the Map Viewer it saves the trajectory to CameraTrajectory.txt aswell as the keyframe trajectory. The point cloud itself does not save. Is there a way to add saving the point cloud in the same manner, or is there another way you'd rather enforce the SLAM.Shutdown() ? Here is the output when stopping: " Shutdown Saving trajectory to CameraTrajectory.txt ... There are 2 maps in the atlas Map 0 has 42 KFs Map 1 has 86 KFs End of saving trajectory to CameraTrajectory.txt ... Saving keyframe trajectory to KeyFrameTrajectory.txt ... " I've made sure that the placement of "SavePointCloud" function is between the "SaveTrajectoryEuRoC" and SaveKeyFrameTrajectoryEuRoC functions as it seems that the output is originating from there, it seems like it doesnt initiate the Savepointcloud, as i dont even get the "couts". Do you have any ideas? Thanks again for your hard work.

Hey! Thank you for your question, I'd be very happy if I can help. It seems weird that even the first cout prints don't show up. Can you try adding your own print statements before and after you call SavePointCloud()? If those don't show up that means some other code is being executed and you should have a look there.

I think even if there is some bug in my hacky solution the first cout << endl << "Saving map point coordinates to " << filename << " ..." << endl; should definitely show up on the terminal.

Also, I know this is very simple but did you build the project again after making your adjustments? Otherwise your executable wouldn't get updated as far as I know. I had to learn that the hard way when I switched to C++ from coding in Python for years :D

Hello and thank you for getting back to me! First of, i would like to say, I almost kicked myself in anger from the possibility of it being such an easy fix as rebuilding the workspace... That being said, it seemed to bring me closer to my goal =) When i stop the orb slam, i still get the same output, however, when i killed the process using CTRL + C, it followed up by actually involving the SavePointCloud call.

"^C[orb_slam3_ros/trajectory_server_orb_slam3-3] killing on exit [rviz-2] killing on exit [orb_slam3-1] killing on exit

Saving point cloud #My own cout before the SavePointCloud() call.

Saving map point coordinates to /home/jonaug/orb_master/maps ...

Number of maps is: 1

Point cloud saved #My own added cout after SavePointCloud() call Shutdown " The map didnt get saved to that path, but i am under the impression that it cant "track" the pointcloud data after the process is killed as it does not have any "memory" regarding the cloud, and it has to be called before the process termination ?

Is it the placement of the call that is the issue or what am i missing?

Thanks again

[altaykacan]

Hmmm it seems like you're using ROS, I have never used that before so can't really help you there. From what I can tell by looking at WFram's comment you have to publish the cloud somehow but I am not sure?

Maybe you can look for a loop like the viewer::run() like WFram has in his example and figure out where to put the pointcloud saving code. His implementation is probably a better reference if you're using ROS.

You might be right with the process being killed and the function not being able to find the memory, maybe somewhere in the code related to ROS (I have no idea) the map or the pointers get cleared. I guess you can also try putting more print statements in SavePointCloud() to see what exactly is missing.

Good luck! Hopefully it works out

[jonaug88]

Hmmm it seems like you're using ROS, I have never used that before so can't really help you there. From what I can tell by looking at WFram's comment you have to publish the cloud somehow but I am not sure?

Maybe you can look for a loop like the viewer::run() like WFram has in his example and figure out where to put the pointcloud saving code. His implementation is probably a better reference if you're using ROS.

You might be right with the process being killed and the function not being able to find the memory, maybe somewhere in the code related to ROS (I have no idea) the map or the pointers get cleared. I guess you can also try putting more print statements in SavePointCloud() to see what exactly is missing.

Good luck! Hopefully it works out

Thanks for taking your time to reply man, much appreciated, ill see if i can hook WFram for some more specifics as i am not that well versed in this myself =)

[jonaug88]

For example, you can use ROS and PCL library for this purpose.

Just use functions GetCurrentMap() and GetMapPoints(), it will give a set of landmarks. Which can be then converted to PointCloudXYZ (data structure in PCL library to store a point cloud). Once the mapping has finished, the point cloud can be saved to .pcd file using PCL I/O interface.

There is a raw example of how to do it: https://github.com/WFram/vslam2/blob/98ddf36524b162664ec71cc9269eaaf83a560769/ORB_SLAM3_ROS/src/viewer.cpp#L216

I am sorry for pulling you back into this again a year after your contribution. Are you able to expand abit on this please? I have not been able to replicate your example...

[RIPGuy]

Hey all,

a bit late to the discussion but I implemented a function inspired by @WFram 's example that works similarly to the SaveTrajectory functions.

Here's my code:

void System::SavePointCloud(const string &filename){
    // Code is based on MapDrawer::DrawMapPoints()
    cout << endl << "Saving map point coordinates to " << filename << " ..." << endl;
    cout << endl << "Number of maps is: " << mpAtlas->CountMaps() << endl;

    // TODO Get all maps or is the current active map is enough?
    // vector<Map*> vpAllMaps = mpAtlas->GetAllMaps()

    Map* pActiveMap = mpAtlas->GetCurrentMap();
    if(!pActiveMap) {
        cout << endl << "There is no active map (pActiveMap is null)" << endl;
        return;
    }

    // Vectors containing pointers to MapPoint objects contained in the maps
    // Vector of pointers for Map Points -- vpMPs
    // Vector of pointers for Reference Map Points -- vpRefMPs
    // TODO figure out the difference between Reference Map Points and normal Map Points
    const vector<MapPoint*> &vpMPs = pActiveMap->GetAllMapPoints();
    const vector<MapPoint*> &vpRefMPs = pActiveMap->GetReferenceMapPoints();

    if(vpMPs.empty()){
        cout << endl << "Vector of map points vpMPs is empty!" << endl;
        return;
    }

    // Use a set for fast lookup of reference frames
    set<MapPoint*> spRefMPs(vpRefMPs.begin(), vpRefMPs.end());

    // Get the output file stream in fixed-point format for map points
    ofstream f;
    f << "pos_x, pos_y, pos_z";
    f.open(filename.c_str());
    f << fixed;

    // TODO figure out if we need to consider whether the presence of IMU
    // requires some transforms/exceptions

    // Iterate over map points, skip "bad" ones and reference map points
    for (size_t i=0, iend=vpMPs.size(); i<iend;i++)
    {
        if (vpMPs[i]->isBad() || spRefMPs.count(vpMPs[i])){
            continue;
        }
        Eigen::Matrix<float,3,1> pos = vpMPs[i]->GetWorldPos();
        f << pos(0) << ", " << pos(1) << ", " << pos(2) << endl;
    }

    // Close the output stream
    f.close();

    // Get the output file stream in fixed-point format for reference map points
    f.open(("ref_" + filename).c_str());
    f << "pos_x, pos_y, pos_z" << endl;
    f << fixed;

    // Iterate over reference map points, skip if bad
    for (set<MapPoint*>::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++)
    {
        if((*sit)->isBad()){
            continue;
        }
        Eigen::Matrix<float,3,1> pos = (*sit)->GetWorldPos();
        f << pos(0) << ", " << pos(1) << ", " << pos(2) << endl;
    }

    // Close the output stream
    f.close();
}

Copy pasting this in System.cc and the function signature into System.h, and finally calling the function in whatever program you are using after SLAM.Shutdown() should hopefully work. In my experience the RefMapPoints seem to work better, i.e. the output file with the ref_ prefix. It seems to have way more points but I'm not sure about the exact difference.

There were a couple of things I was unsure about, e.g. the difference between "map points" and "reference points", whether we need to transform the points if we have an IMU, and whether taking the active map from the atlas is enough. I marked them with TODO's and will get back to them when I have time but according to my preliminary experiments with custom monocular data the results seem reasonable.

The code above basically saves the coordinates of the map points in a comma-separated file. I'm more of a Python person so I used the following Python code to save the results as PLY files. It needs the output of the function above and a custom formatted trajectory file. Just putting it here for reference and in case it helps someone:

import os
from pathlib import Path
from typing import Union

import numpy as np

def save_pointcloud_from_ORB_SLAM(input_file: Union[Path, str], output_file: Union[Path,str] = "out.ply"):
    """Converts a comma separated list of map point coordinates into
    PLY format for viewing the generated map.

    Args:
        input_file (str or Path): Path to the input file which is expected to
        be a .csv file with the columns pos_x, pos_y, pos_z designating the
        coordinates of the points in the world reference frame.

        output_file (str or Path): Path to the output .ply file, format is
        described here: https://paulbourke.net/dataformats/ply/
    """

    coords = np.genfromtxt(input_file, delimiter=", ", skip_header=1)

    x = coords[:, 0]
    y = coords[:, 1]
    z = coords[:, 2]

    ply_header = 'ply\n' \
                'format ascii 1.0\n' \
                'element vertex %d\n' \
                'property float x\n' \
                'property float y\n' \
                'property float z\n' \
                'end_header' % x.shape[0]

    np.savetxt(output_file, np.column_stack((x, y, z)), fmt='%f %f %f', header=ply_header, comments='')

def save_trajectory_from_ORB_SLAM(input_file: Union[Path, str], output_file: Union[Path,str] = "out_trajectory.ply"):
    """Converts the saved trajectory file from ORB-SLAM3 to a point cloud to then
    show alongside the mapped cloud.

    The input file is expected to be in the format (KITTI format with image path prepended, can ignore it):
    /path/to/image0.png R_00 R_01 R_02 t_0 R_10 R_11 R_12 t_1 R_20 R_21 R_22 t_2
    /path/to/image1.png R_00 R_01 R_02 t_0 R_10 R_11 R_12 t_1 R_20 R_21 R_22 t_2

    Where the R terms are the rotation and t terms are the translation terms
    of the homogeneous transformation matrix T_w_cam0.
    """
    x = []
    y = []
    z = []

    with open(input_file, "r") as file:
        lines = file.readlines()

    for line in lines:
        cols = line.strip().split(" ")

        x.append(float(cols[4]))
        y.append(float(cols[8]))
        z.append(float(cols[12]))

    # Each trajectory point is shown as a "point" in the "point cloud" which is the trajectory
    x = np.array(x)
    y = np.array(y)
    z = np.array(z)

    # RGB values for each point on the trajectory, set to be light green
    r = np.ones_like(x) * 144
    g = np.ones_like(x) * 238
    b = np.ones_like(x) * 144

    ply_header = 'ply\n' \
                'format ascii 1.0\n' \
                'element vertex %d\n' \
                'property float x\n' \
                'property float y\n' \
                'property float z\n' \
                'property uchar red\n' \
                'property uchar green\n' \
                'property uchar blue\n' \
                'end_header' % x.shape

    np.savetxt(output_file, np.column_stack((x, y, z, r, g, b)), fmt='%f %f %f %d %d %d', header=ply_header, comments='')

if __name__ == "__main__":
    input_file = "/path/to/point_cloud.txt" # ReferenceMapPoints seem to work better, use that file
    output_file = "./output.ply"

    input_trajectory = "/path/to/trajectory_file.txt"
    output_trajectory = "./output_trajectory.ply"

    save_pointcloud_from_ORB_SLAM(input_file, output_file)
    save_trajectory_from_ORB_SLAM(input_trajectory, output_trajectory)

After plotting the two .ply files on CloudCompare I got results like this on custom data, which shows a camera moving straight. So I believe the code works:

I know the answer a bit long and I'm no C++ expert but I really hope this helps! Please let me know if you notice any mistakes, we can maybe create a pull request later on. It seems like a very popular issue.

Hello! I am new to ORBSLAM and planning to export the mappoints after processing. You said that first code export a .csv file for the mappoints, what are the values I should expect from its output are they like xyz values that correspond to each point in the map?

[altaykacan]

Hello! I am new to ORBSLAM and planning to export the mappoints after processing. You said that first code export a .csv file for the mappoints, what are the values I should expect from its output are they like xyz values that correspond to each point in the map?

Yup exactly, this code writes the x, y, and z positions of each point to the output file you specify line-by-line:

  Eigen::Matrix<float,3,1> pos = vpMPs[i]->GetWorldPos();
  f << pos(0) << ", " << pos(1) << ", " << pos(2) << endl;

Each line of the output (except the first line which has the name of the columns) should contain the coordinates for a single point. It worked for me, I hope it works for you too! :)

[migsdigs]

Hi @altaykacan! Thank you so much for your code to save the point cloud. It looks as though it works (I am able to save txt with x,y,z positions of points). I was wondering if you made an attempt at also saving the RGB values of the points, or if you'd know how to approach this?

I was originally using a docker image that had Python wrapper of Orbslam. They were able to save the pointcloud along with the RGB values of the points to create a nice sparse pointcloud. Unfortunately that module doesnt seem to support Inertial assisted slam.

If you're not sure, no worries, thank you very much for your code in any case :)

[AndreasGZ]

Thank you @altaykacan. I rewrote the Code, so that every Map gets saved and I changed the saving-format of the pointcloud to a ply-format based on your python code.

void System::SavePointCloud(const string &filename){
    cout << endl << "Saving pointclouds to " << filename << " ..." << endl;
    int size = mpAtlas->CountMaps();
    cout << endl << "Number of maps is: " << size << endl;
    vector<Map*> vpAllMaps = mpAtlas->GetAllMaps();
    for(size_t i=0; i<size; i++){
        Map* vpMap = vpAllMaps[i];
        if(!vpMap) continue;
        const vector<MapPoint*> &vpMPs = vpMap->GetAllMapPoints();
        const vector<MapPoint*> &vpRefMPs = vpMap->GetReferenceMapPoints();
        set<MapPoint*> spRefMPs(vpRefMPs.begin(), vpRefMPs.end());
        if(vpMPs.empty()) continue;
        string nr = to_string(i) + "_"; 
        int count = 0;
        stringstream mapss("");
        mapss << fixed;
        for (size_t i=0, iend=vpMPs.size(); i<iend;i++)
        {
            if (vpMPs[i]->isBad() || spRefMPs.count(vpMPs[i])){
                continue;
            }
            count++;
            Eigen::Matrix<float,3,1> pos = vpMPs[i]->GetWorldPos();
            mapss << pos(0) << " " << pos(1) << " " << pos(2) << "\n";
        }
        // Farbe für Punkte -> "property uchar red\nproperty uchar green\nproperty uchar blue\n";
        ofstream f;
        if(count > 0) {
            f.open((nr + filename).c_str());
            f << "ply\nformat ascii 1.0\nelement vertex " << count << "\n";
            f << "property float x\nproperty float y\nproperty float z\n";
            f << "end_header\n";
            f << fixed;
            f << mapss.str();
            f.close();
            mapss.str("");
            mapss.clear();
        }

        count = 0;
        mapss << fixed;
        for (set<MapPoint*>::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++)
        {
            if((*sit)->isBad()){
                continue;
            }
            count++;
            Eigen::Matrix<float,3,1> pos = (*sit)->GetWorldPos();
            mapss << pos(0) << " " << pos(1) << " " << pos(2)  << "\n";
        }
        if(count > 0){
            f.open((nr + "ref_" + filename).c_str());
            f << "ply\nformat ascii 1.0\nelement vertex " << count << "\n";
            f << "property float x\nproperty float y\nproperty float z\n";
            f << "end_header\n";
            f << fixed;
            f << mapss.str();
            f.close();
            mapss.str("");
            mapss.clear();
        }

    }
}

[altaykacan]

Hi @altaykacan! Thank you so much for your code to save the point cloud. It looks as though it works (I am able to save txt with x,y,z positions of points). I was wondering if you made an attempt at also saving the RGB values of the points, or if you'd know how to approach this?

I was originally using a docker image that had Python wrapper of Orbslam. They were able to save the pointcloud along with the RGB values of the points to create a nice sparse pointcloud. Unfortunately that module doesnt seem to support Inertial assisted slam.

If you're not sure, no worries, thank you very much for your code in any case :)

Hey @migsdigs

I'm glad my code helped you out! I didn't really work on getting the RGB values but it should be pretty straightforward if the MapPoint objects include the color as attributes. I guess ORB-SLAM uses gray-scale images though, so that might be an issue. If the RGB images are saved somewhere, you can probably take the pixel coordinates of the detected feature points and assign it to the 3D point and just add it to the final output when saving the map. What I recommend you to do would be to check the code from the docker image you were using before.

Another thing you could try would be to colorize the pointcloud after you get the sparse map and the poses, as a post-processing step. With a simple python script, I believe you can basically project your pointcloud to the RGB images, compare the projected points with your RGB images, and assign colors to each point. Open3D has lots of functions that would help you do this. This is probably not be best solution though and won't give you a real-time colored map.

I'm not working on this project anymore so I unfortunately can't check it or implement it myself. I hope this still helps you out though :)