ORB_SLAM
ORB_SLAM copied to clipboard
NVM Export
Hi Raul, in the last days I implemented the export of the scene into an NVM file. The goal is to use the ORB-Slam computations for further dense 3D reconstruction via openMVS. You can see the code I added to main.cc here The nvm file basically contains the list of keyframes and the list of observations.
When I apply ORB_SLAM on this dataset, the result look like this:
When I open the generated NVM file with VisualSFM, it looks like this:
However, the issue is that many cameras seem to have observations "behind them".
This can be seen in VisualSFM by clicking on one of the cameras with the right mouse button. Vsfm shows then all points being "observed" by the camera in red:
The reason Iam posting it here is that I dont understand how this can happen. Based on the code referenced above I refer to an observation via
std::vector<MapPoint*> all_points=World.GetAllMapPoints();
f << all_points.size() << "\n";
for(size_t i=0, iend=all_points.size(); i<iend;i++)
{
MapPoint* pMP = all_points[i];
cv::Mat pos=pMP->GetWorldPos();
f << pos.at<float>(0) << " " << pos.at<float>(1) << " " << pos.at<float>(2) << " " <<
//rgb
"0 0 0 ";
//now all the observations/measurements
std::map<KeyFrame*,size_t> observations=pMP->GetObservations();
//count good observations:
int num_good_observations=0;
for (std::map<KeyFrame*,size_t>::iterator ob_it=observations.begin(); ob_it!=observations.end(); ob_it++)
{
if (!(*ob_it).first->isBad())
num_good_observations+=1;
}
f << num_good_observations << " ";
for (std::map<KeyFrame*,size_t>::iterator ob_it=observations.begin(); ob_it!=observations.end(); ob_it++)
{
//skip if the key frame is "bad"
if ((*ob_it).first->isBad())
continue;
//<Measurement> = <Image index> <Feature Index> <xy>
std::vector<cv::KeyPoint> key_points=(*ob_it).first->GetKeyPoints();
f << kf_index[(*ob_it).first->mnFrameId] << " " << (*ob_it).second << " " <<
key_points[ob_it->second].pt.x << " " <<
key_points[ob_it->second].pt.y << " ";
}
f << "\n";
}
So it looks like
- I misunderstand how the observation structure in ORB_SLAM works,or
- There is something wrong with the observations.
would be really great if you can give me a hint here.
Thanks in advance!
ha, solved! :) https://github.com/mojovski/ORB_SLAM/commit/49451d089ac89a0044c5d066bbe4cee5cedfd90c
sometimes, it already helps to describe an issue systematically. :)
hmm, there is still an issue. When I check the image points
key_points[ob_it->second].pt.x
from
for (std::map<KeyFrame*,size_t>::iterator ob_it=observations.begin(); ob_it!=observations.end(); ob_it++)
{
//skip if the key frame is "bad"
if ((*ob_it).first->isBad())
continue;
//<Measurement> = <Image index> <Feature Index> <xy>
std::vector<cv::KeyPoint> key_points=(*ob_it).first->GetKeyPoints();
f << kf_index[(*ob_it).first->mnFrameId] << " " << (*ob_it).second << " " <<
key_points[ob_it->second].pt.x << " " <<
key_points[ob_it->second].pt.y << " ";
}
the position in different images is changing even if the camera stands still. You can see it from the first exported observation in the NVM file: Reformated to
x y z r g b num_observations
img_idx key_in_img_idx u v
-0.3520444 0.0611118 0.9319469 0 0 0 5
1 572 181.2000122 330.0000000
0 218 202.0000000 299.0000000
4 293 19.2000008 265.2000122
3 306 168.0000000 297.6000061
2 314 175.2000122 322.8000183
so you can see that the same 3D point has a strong variance along the horizontal image u-axis though the camera nearly fixed for the first 4 images.
Any ideas? :/
I am interested in exporting the map information to use outside of ORB-SLAM and manipulate the data using other software like Matlab etc.
Was wondering if you were able to solve your issue and have any code available to export the map data?
Thank you!
I have forked the repository and added this example: https://github.com/mojovski/ORB_SLAM2/blob/master/Examples/Monocular/mono_nvm.cc
It basically uses the new method in System.SaveNVM(path) https://github.com/mojovski/ORB_SLAM2/blob/master/src/System.cc#L435
Please note, that the NVM saving method has still one issue: It stores the name of the image as a full path. You will need to pen the file with your text editor and find&replace unnecessary parts.
If you need something similar for Matlab, I would suggest to use the SaveNVM method as example and then to store the data in a mat file. However, writing mat files in c++ is not that easy. http://stackoverflow.com/questions/14597733/how-to-save-a-c-readable-mat-file You might prefer to store your information in a simple csv/txt file and parse it then in matlab.
Hi mojovski, Thank you for your response. Much appreciated. I will try your solution soonest and appreciate your instructions and also advice regarding the Matlab implementation.
Will let you know how it goes. Thank you!
Hi mojovski, I wonder if you succeeded in reconstructing 3D model via openmvs. Recently I'm doing the similar work but the quality of the 3D model is poor I don't know the reason is bugs in my exporting data code or the accuracy of camera pose or 3d points in slam is not precise enough
It would be great if you can give me some hints. Thank you