Closed faresfaresCS closed 5 years ago
Hi @faresfaresCS , thank you for sharing this bug.
Yes, osmap let you open previously saved map, no matter if you closed orb-slam2.
To load a map SLAM must be fully initialized (not the map, but the system). I open maps by pressing some key while orb-slam2 is already running.
Can you find a way to load the map after the first image is processed by orb-slam2?
Not sure this is the cause of this error. In a few days I'll analyze this case, I hope I'll come up with a solution.
Thanks for the quick reply. I managed to call "osmap.mapLoad()" right after the initialization of the camera in ROS, but the same error occurred.
@faresfaresCS , if you don't mind, I'd like you to share myFirstMap.yaml, or you can paste its content here.
I can see it is a small map, only 16 keyframes. By any chance do the map have a loop closing?
I'm thinking on turning on some console logs in the code. The problem is always in rebuild(), it is the key function to produce small files.
Yes its a small map with no loop closing (see content below). I"ll try to save a bigger map with loop closing and update you.
%YAML:1.0
---
mappointsFile: "myFirstMap.mappoints"
nMappoints: 1294
keyframesFile: "myFirstMap.keyframes"
nKeyframes: 24
featuresFile: "myFirstMap.features"
nFeatures: 26113
Options: 2
Options descriptions: [ FEATURES_FILE_NOT_DELIMITED ]
cameraMatrices:
- { fx:3.7849645996093750e+02, fy:3.8136294555664062e+02,
cx:2.1110339355468750e+02, cy:1.4478785705566406e+02 }
Update: I tried saving a bigger map with loop closing:
%YAML:1.0
---
mappointsFile: "myFirstMap.mappoints"
nMappoints: 6395
keyframesFile: "myFirstMap.keyframes"
nKeyframes: 153
featuresFile: "myFirstMap.features"
nFeatures: 155987
Options: 2
Options descriptions: [ FEATURES_FILE_NOT_DELIMITED ]
cameraMatrices:
- { fx:3.7849645996093750e+02, fy:3.8136294555664062e+02,
cx:2.1110339355468750e+02, cy:1.4478785705566406e+02 }
now Im getting a different error "Wrong initialization, resetting..." and ORB-SLAM2 window freezes before showing features:
Input sensor was set to: Monocular
Loading ORB Vocabulary. This could take a while...
Vocabulary loaded!
Camera Parameters:
- fx: 378.496
- fy: 381.363
- cx: 211.103
- cy: 144.788
- k1: 0
- k2: 0
- p1: 0
- p2: 0
- fps: 30
- color order: RGB (ignored if grayscale)
ORB Extractor Parameters:
- Number of Features: 1000
- Scale Levels: 8
- Scale Factor: 1.2
- Initial Fast Threshold: 20
- Minimum Fast Threshold: 7
New Map created with 94 points
Wrong initialization, reseting...
System Reseting
Reseting Local Mapper... done
Reseting Loop Closing... done
Reseting Database... done
Local Mapping RELEASE
System Reseting
Reseting Local Mapper... done
Reseting Loop Closing... done
Reseting Database... done
Local Mapping STOP
Mappoints loaded: 6395
Keyframes loaded: 153
Loading features from myFirstMap.features ...
Features loaded: 155987
Rebuilding map:
@faresfaresCS ,
@cugcsu found the source of the error in rebuild. I have a fix I can't test. Please take a look at https://github.com/AlejandroSilvestri/osmap/issues/5#issuecomment-477591769
@faresfaresCS , I fix this in Osmap.cpp. @cugcsu reported it worked and he was able to open a previously saved map. Please let me know.
Still failing after few seconds:
Input sensor was set to: Monocular
Loading ORB Vocabulary. This could take a while...
Vocabulary loaded!
Camera Parameters:
- fx: 378.496
- fy: 381.363
- cx: 211.103
- cy: 144.788
- k1: 0
- k2: 0
- p1: 0
- p2: 0
- fps: 30
- color order: RGB (ignored if grayscale)
ORB Extractor Parameters:
- Number of Features: 1000
- Scale Levels: 8
- Scale Factor: 1.2
- Initial Fast Threshold: 20
- Minimum Fast Threshold: 7
New Map created with 153 points
Track lost soon after initialisation, reseting...
System Reseting
Reseting Local Mapper... done
Reseting Loop Closing... done
Reseting Database... done
New Map created with 147 points
Local Mapping RELEASE
System Reseting
Reseting Local Mapper... done
Reseting Loop Closing... done
Reseting Database... done
Local Mapping STOP
Mappoints loaded: 9686
Keyframes loaded: 247
Loading features from myFirstMap.features ...
Features loaded: 250552
Rebuilding map:
Mono_Load: tpp.c:84: __pthread_tpp_change_priority: Assertion `new_prio == -1 || (new_prio >= fifo_min_prio && new_prio <= fifo_max_prio)' failed.
Aborted (core dumped)
This is my LOAD file:
/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <https://github.com/raulmur/ORB_SLAM2>
*
* ORB-SLAM2 is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM2 is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/
#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include<opencv2/core/core.hpp>
#include "../../../include/System.h"
#include <geometry_msgs/PoseStamped.h>
#include "geometry_msgs/TransformStamped.h"
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include "../../../include/Converter.h"
#include "../../../include/Osmap.h"
using namespace std;
using namespace tf;
//#include <chrono>
class ImageGrabber
{
public:
ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){}
void GrabImage(const sensor_msgs::ImageConstPtr& msg);
ORB_SLAM2::System* mpSLAM;
};
ORB_SLAM2::Osmap *osmap;
int main(int argc, char **argv)
{
ros::init(argc, argv, "Mono");
ros::start();
if(argc != 3)
{
cerr << endl << "Usage: rosrun ORB_SLAM2 Mono path_to_vocabulary path_to_settings" << endl;
ros::shutdown();
return 1;
}
// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);
osmap = new ORB_SLAM2::Osmap(SLAM);
ImageGrabber igb(&SLAM);
ros::NodeHandle nodeHandler;
ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb);
ros::spin();
// Stop all threads
SLAM.Shutdown();
// Save camera trajectory
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
// std::ofstream myfile;
// myfile.open("pointData.csv");
// string filename = "descriptorsData.xml";
// cv::FileStorage fs(filename, cv::FileStorage::WRITE);
// fs.open(filename, cv::FileStorage::WRITE);
// int i=0;
// std::vector<ORB_SLAM2::MapPoint*> allMapPoints = SLAM.GetMap()->GetAllMapPoints();
// for(auto p : allMapPoints) {
// Eigen::Matrix<double, 3, 1> v = ORB_SLAM2::Converter::toVector3d(p->GetWorldPos());
// cv::Mat desc = p->GetDescriptor();
// myfile << v.x() << "," << v.y() << "," << v.z() << std::endl;
// fs << "desc" + std::to_string(i++) << desc;
// }
// fs.release();
ros::shutdown();
return 0;
}
void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg)
{
static bool loadedmap = false;
// Copy the ros image message to cv::Mat.
cv_bridge::CvImageConstPtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvShare(msg);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::Mat pose = mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());
if (pose.empty())
return;
if (!loadedmap)
{
static int count_frames = 0;
count_frames++;
if (count_frames == 60){
osmap->mapLoad("myFirstMap.yaml");
loadedmap = true;
}
}
// transform into right handed camera frame
tf::Matrix3x3 rh_cameraPose( - pose.at<float>(0,0), pose.at<float>(0,1), pose.at<float>(0,2),
- pose.at<float>(1,0), pose.at<float>(1,1), pose.at<float>(1,2),
pose.at<float>(2,0), - pose.at<float>(2,1), - pose.at<float>(2,2));
tf::Vector3 rh_cameraTranslation( pose.at<float>(0,3),pose.at<float>(1,3), - pose.at<float>(2,3) );
//rotate 270deg about z and 270deg about x
tf::Matrix3x3 rotation270degZX( 0, 0, 1,
-1, 0, 0,
0,-1, 0);
//publish right handed, x forward, y right, z down (NED)
static tf::TransformBroadcaster br;
tf::Transform transformCoordSystem = tf::Transform(rotation270degZX,tf::Vector3(0.0, 0.0, 0.0));
br.sendTransform(tf::StampedTransform(transformCoordSystem, ros::Time::now(), "camera_link", "camera_pose"));
tf::Transform transformCamera = tf::Transform(rh_cameraPose,rh_cameraTranslation);
br.sendTransform(tf::StampedTransform(transformCamera, ros::Time::now(), "camera_pose", "pose"));
ofstream myfile;
using namespace std::chrono;
static milliseconds ms = duration_cast< milliseconds >(system_clock::now().time_since_epoch());
static std::string which_file = "1";
if(duration_cast< milliseconds >(system_clock::now().time_since_epoch()) - ms > milliseconds(10))
{
which_file = which_file == "1" ? "2" : "1";
ms = duration_cast< milliseconds >(system_clock::now().time_since_epoch());
myfile.open ("/home/fares/PycharmProjects/Point-Tracker/Data/realtimepos" + which_file + ".csv");
myfile << pose.at<float>(0,3) << " " << pose.at<float>(1,3) << " " << pose.at<float>(2,3);
myfile.close();
}
}
@faresfaresCS , Sorry I can't help you on that one. What is Mono_load? The error happens there:
Mono_Load: tpp.c:84:
I don't remember seeing this in ORB-SLAM2.
@AlejandroSilvestri , Im working with ROS option. So Im calling save and laod from ros_mono.cc file which serves me as main as you can see.
Its crashing at "Osmap.cpp":
pKF->ComputeBoW();
&
pMP->UpdateNormalAndDepth()
ros_mono.cc:
/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <https://github.com/raulmur/ORB_SLAM2>
*
* ORB-SLAM2 is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM2 is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/
#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include<opencv2/core/core.hpp>
#include "../../../include/System.h"
#include <geometry_msgs/PoseStamped.h>
#include "geometry_msgs/TransformStamped.h"
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include "../../../include/Converter.h"
#include "../../../include/Osmap.h"
using namespace std;
using namespace tf;
//#include <chrono>
class ImageGrabber
{
public:
ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){}
void GrabImage(const sensor_msgs::ImageConstPtr& msg);
ORB_SLAM2::System* mpSLAM;
};
ORB_SLAM2::Osmap *osmap;
int main(int argc, char **argv)
{
ros::init(argc, argv, "Mono");
ros::start();
if(argc != 3)
{
cerr << endl << "Usage: rosrun ORB_SLAM2 Mono path_to_vocabulary path_to_settings" << endl;
ros::shutdown();
return 1;
}
// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);
osmap = new ORB_SLAM2::Osmap(SLAM);
osmap->mapLoad("myFirstMap.yaml");
ImageGrabber igb(&SLAM);
ros::NodeHandle nodeHandler;
ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb);
ros::spin();
// Stop all threads
SLAM.Shutdown();
// Save camera trajectory
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
ros::shutdown();
return 0;
}
void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg)
{
// Copy the ros image message to cv::Mat.
cv_bridge::CvImageConstPtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvShare(msg);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::Mat pose = mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());
if (pose.empty())
return;
// transform into right handed camera frame
tf::Matrix3x3 rh_cameraPose( - pose.at<float>(0,0), pose.at<float>(0,1), pose.at<float>(0,2),
- pose.at<float>(1,0), pose.at<float>(1,1), pose.at<float>(1,2),
pose.at<float>(2,0), - pose.at<float>(2,1), - pose.at<float>(2,2));
tf::Vector3 rh_cameraTranslation( pose.at<float>(0,3),pose.at<float>(1,3), - pose.at<float>(2,3) );
//rotate 270deg about z and 270deg about x
tf::Matrix3x3 rotation270degZX( 0, 0, 1,
-1, 0, 0,
0,-1, 0);
//publish right handed, x forward, y right, z down (NED)
static tf::TransformBroadcaster br;
tf::Transform transformCoordSystem = tf::Transform(rotation270degZX,tf::Vector3(0.0, 0.0, 0.0));
br.sendTransform(tf::StampedTransform(transformCoordSystem, ros::Time::now(), "camera_link", "camera_pose"));
tf::Transform transformCamera = tf::Transform(rh_cameraPose,rh_cameraTranslation);
br.sendTransform(tf::StampedTransform(transformCamera, ros::Time::now(), "camera_pose", "pose"));
ofstream myfile;
using namespace std::chrono;
static milliseconds ms = duration_cast< milliseconds >(system_clock::now().time_since_epoch());
static std::string which_file = "1";
if(duration_cast< milliseconds >(system_clock::now().time_since_epoch()) - ms > milliseconds(10))
{
which_file = which_file == "1" ? "2" : "1";
ms = duration_cast< milliseconds >(system_clock::now().time_since_epoch());
myfile.open ("/home/fares/PycharmProjects/Point-Tracker/Data/realtimepos" + which_file + ".csv");
myfile << pose.at<float>(0,3) << " " << pose.at<float>(1,3) << " " << pose.at<float>(2,3);
myfile.close();
}
}
Mono_Load is a file which I get when building the nodes for mono as mentioned in ROS Examples in https://github.com/raulmur/ORB_SLAM2.git
Hi @AlejandroSilvestri, Thanks for adding map saving functionality to ORB-SLAM2 and sharing your code. I tried to use your map saving/loading code, but it gives segmentation fault (core dump) error: In fact, I run the same code (mono_tum example) which was used to save the map, except I load the saved map once it has processed initial 100 frames. ......... // Pass the image to the SLAM system SLAM.TrackMonocular(im,tframe); if (ni==100){ // Now you want to load the map osmap.mapLoad("myFirstMap.yaml"); } ..........
Could you please elaborate how to load a saved map?
@omair50 ,
I'm glad you are interested in oslam, I'm sorry it's not working for you. Please post the complete SegFault error, I need to trace the point of code that threw the error.
And if you don't mind, please post the content of your yaml map file.
Thank you
@AlejandroSilvestri , Thanks for a quick reply! The YAML file is like:
mappointsFile: "myFirstMap.mappoints" nMappoints: 1526 keyframesFile: "myFirstMap.keyframes" nKeyframes: 27 featuresFile: "myFirstMap.features" nFeatures: 29142 Options: 2 Options descriptions: [ FEATURES_FILE_NOT_DELIMITED ] cameraMatrices:
and the error (I loaded the map after 600 frames have been processed by ORB_SLAM2 from a total of 798 frames ):
Input sensor was set to: Monocular
Loading ORB Vocabulary. This could take a while... Vocabulary loaded!
Camera Parameters:
ORB Extractor Parameters:
Start processing sequence ... Images in the sequence: 798
Framebuffer with requested attributes not available. Using available framebuffer. You may see visual artifacts.New Map created with 96 points Wrong initialization, reseting... System Reseting Reseting Local Mapper... done Reseting Loop Closing... done Reseting Database... done New Map created with 69 points Wrong initialization, reseting... System Reseting Reseting Local Mapper... done Reseting Loop Closing... done Reseting Database... done New Map created with 96 points Wrong initialization, reseting... System Reseting Reseting Local Mapper... done Reseting Loop Closing... done Reseting Database... done New Map created with 98 points Wrong initialization, reseting... System Reseting Reseting Local Mapper... done Reseting Loop Closing... done Reseting Database... done New Map created with 118 points Local Mapping RELEASE System Reseting Reseting Local Mapper... done Reseting Loop Closing... done Reseting Database... done Local Mapping STOP Mappoints loaded: 1526 Keyframes loaded: 27 Loading features from myFirstMap.features ... Features loaded: 29142 Rebuilding map: Segmentation fault (core dumped)
@omair50
As far as I can tell, map is correctly saved, rebuilding step is failing. I can't reproduce this error, to go further I need to know the point in code producing this error. It'll help if you compile with -g flag and it'll better if you can run it with a debugger.
@AlejandroSilvestri, The program crashes at line 543 of OSmap.cpp ("pKF->UpdateConnections();"). I am unable to find the actual reason. Could you please provide a minimal working example to load a map, e.g. using of the original examples of ORB-SLAM2? As @faresfaresCS also faced the same problem, it would be really helpful.
@omair50 ,
Your code is fine, there is a bug in osmap I can't trace. I'll take a quick look into your problem right now, but don't expect much from it.
I must work into a debugger that prevents this errors and/or reports more info. And when I have time, I'm planning to do a stand alone map viewer, to visually inspect map files.
@omair50 ,
pKF->UpdateConnections(); in line 543 is a function, the error must happen inside it, can you trace it to the exact file and line? The error should happen in file KeyFrame.cc . Thank you.
@AlejandroSilvestri, I found a quick soln: set pauseThreads=false while calling osmap.mapLoad("myFirstMap.yaml", false). This settings does not cause crash and I can load the map and localize within it.
@omair50 , glad to hear that.
That's another piece of info to debug osmap. I still don't understand why this works.
pauseThreads
should be true, unless you already paused the threads yourself. So, beware for unexpected behaviour, and don't hesitate to tell me.
Thank you.
Hi, Thanks for sharing such a great work.
I've been trying to load a previously saved map which i saved with osmap.mapSave(). Here is the code that saves the map:
Then i have tried to load the map and use it with the code:
But i got the message "Segmentation fault (core dumped)":
is it possible to load a map from a previous orb slam run?