ivipsourcecode/ds-slam

DS-SLAM

DS-SLAM is a complete robust semantic SLAM system, which could reduce the influence of dynamic objects on pose estimation, such as walking people and other moving robots. Meanwhile, DS-SLAM could also provide semantic presentation of the octo-tree map．DS-SLAM is a optimized SLAM system based on the famous ORB-SLAM2 (from https://github.com/raulmur/ORB_SLAM2 and https://github.com/gaoxiang12/ORBSLAM2_with_pointcloud_map, thanks for Raul's, Gao Xiang's and SegNet's great work!). It not only includes tracking, local mapping, loop closing threads, but also contains semantic segmentation, dense mapping threads. Currently, the system is integrated with Robot Operating System (ROS). In this Open Source project, we provide one example to run DS_SLAM in the TUM dataset with RGB-D sensors. The example of real-time implementation of DS-SLAM and hardware/software configurations would be coming soon.

As described in **DS-SLAM: A Semantic Visual SLAM towards Dynamic Environments **Chao Yu, Zuxin Liu, Xinjun Liu, Fugui Xie, Yi Yang, Qi Wei, Fei Qiao, Published in the Proceedings of the 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2018). The code is opened by Dongjiang Li.

Moreover, to download the pre-print version, you could be directed to [https://arxiv.org/abs/1809.08379v1].

DS-SLAM is developed by the joint research project of iVip Lab @ EE, THU (https://ivip-tsinghua.github.io/iViP-Homepage/) and Advanced Mechanism and Roboticized Equipment Lab.

If you have any questions or use DS_SLAM for commercial purposes, please contact: [email protected]

1. License

DS-SLAM is released under a GPLv3 license.

DS-SLAM allows personal and research use only. For a commercial license please contact: [email protected]

If you use DS-SLAM in an academic work, please cite their publications as below:

Chao Yu, Zuxin Liu, Xinjun Liu, Fugui Xie, Yi Yang, Qi Wei, Fei Qiao "DS-SLAM: A Semantic Visual SLAM towards Dynamic Environments." Published in the Proceedings of the 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2018).

2. Prerequisites

We have tested the library in Ubuntu 14.04 and 16.04, but it should be easy to compile in other platforms. The experiment is performed on a computer with Intel i7 CPU, P4000 GPU, and 32GB memory.

ORB_SLAM2 Prereguisites

DS-SLAM is a optimized SLAM system based on the famous ORB-SLAM2. In order to run DS_SLAM, you have to install environment needed by ORB_SLAM2(the section of 2. Prereguisites). We suggest that the path of the folder of Thirdparty and Vocabulary(provided at https://pan.baidu.com/s/1-zWXlzOn-X0tjoEF9XI6qA, extract code: 8t7x) to be DS-SLAM. Instructions can be found at: https://github.com/raulmur/ORB_SLAM2.

ROS

We provide one example to process the TUM dataset as RGB-D image. A version Hydro or newer is needed. You should create a ROS catkin workspace(in our case, catkin_ws).

SegNet

We adopt SegNet to provide pixel-wise semantic segmentation based on caffe in real-time. Download and decompress DS_SLAM library (provided at https://github.com/zoeyuchao/DS-SLAM) in catkin_ws/src. Then you can find the Example folder in the folder of DS-SLAM. We suggest the installation path of caffe-segnet to be /Examples/ROS/ORB_SLAM2_PointMap_SegNetM. The root of Download and install instructions can be found at: https://github.com/TimoSaemann/caffe-segnet-cudnn5

OctoMap and RVIZ

We provide semantic presentation of the octo-tree map by OctoMap. RViz display plugins for visualizing octomap messages. We suggest that the installation path of octomap_mapping and octomap_rviz_plugins to be catkin_ws/src. Add #define COLOR_OCTOMAP_SERVER into the OctomapServer.h at the folder of octomap_mapping/octomap_server/include/octomap_server Download and install instructions can be found at: https://github.com/OctoMap/octomap_mapping and https://github.com/OctoMap/octomap_rviz_plugins.

3. Building DS_SLAM library and the example

We provide a script DS_SLAM_BUILD.sh to build the third party libraries and DS-SLAM. Please make sure you have installed all previous required dependencies. Execute:

cd DS-SLAM
chmod +x DS_SLAM_BUILD.sh
./DS_SLAM_BUILD.sh

4. TUM example

1. Add the path including Examples/ROS/ORB_SLAM2_PointMap_SegNetM to the ROS_PACKAGE_PATH environment variable. Open .bashrc file and add at the end the following line. Replace PATH by the folder where you cloned ORB_SLAM2:

export  ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM

2. Download a sequence from http://vision.in.tum.de/data/datasets/rgbd-dataset/download and unzip it. We suggest you download rgbd_dataset_freiburg3_walking_xyz.
3. We provide DS_SLAM_TUM.launch script to run TUM example. Change PATH_TO_SEQUENCE and PATH_TO_SEQUENCE/associate.txt in the DS_SLAM_TUM.launch to the sequence directory that you download before, then execute the following command in a new terminal. Execute:

cd DS-SLAM
roslaunch DS_SLAM_TUM.launch 

5. Something about Folders

The function of folder in the catkin_ws/src/ORB_SLAM2_PointMap_SegNetM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM.

1. segmentation: the section of segmentation including source code, header file and dynamic link library created by Cmake.
2. launch: used for showing octomap.
3. prototxts and tools: containing some parameters associated with caffe net relating to the semantic segmentation thread of DS_SLAM. There is the folder of models(provided at https://pan.baidu.com/s/1gkI7nAvijF5Fjj0DTR0rEg extract code: fpa3), please download and place the folder in the same path with the folder of prototxts and tools.

ds-slam's Issues

Can not find segnet_pascal.caffemodel

Hi, should I download the segnet_pascal.caffemodel fron segnet? But when I click the URL "http://mi.eng.cam.ac.uk/~agk34/resources/SegNet/segnet_pascal.caffemodel", it show me that Object not found! Could you upload it through baidu cloud disk? Thank you.

About Frame::ProcessMovingObject

I think there is something wrong about the last part of this function. Take much time to calculate F_prepoint and F_nextpoint but still use prepoint and nextpoint in the last.

In addition to the case, can I use rgbd camera to run online in real time?

如何查看语义地图

博主您好，我刚刚接触语义slam。请问在Rviz中如何显示语义地图，我尝试打开ColorOccupancyGrid中的octomap_full和topic中的occupied_cells_vis_array,但这些看起来都不太像语义地图。并且在occupied_cells_vis_array里出现了状态警告，map/15 Uninitialized quanternion和map/16 Uninitialized quanternion .请问这两个警告会影响仿真结果吗

Launch only Octomap and transform

Hello,

I would like to employ implementation for my course project. In out project, the map is constructed sparse but I want to convert it to how you resulting map is. I have a whole SLAM pipeline which is derived from ORBSLAM, as yours. So, I do want to run your SLAM pipeline as a whole but only map construction part. However, I cannot find entry and exit points of topics that are published and subscribed. I only want to use Octomap and transform.launch files ( I think they are enough for me). I also imported pointcloudmapping.h and .cpp for pointcloud mapping. Could you explain where the info coming out and goes into Octomap.launch topics and the same for transform.launch.

I guess I manage to integrate my pipeline and the things that are needed from this repo.
However, on RVIZ I do not see anything. I can see images that are published /camera/depth/image and /kitti/left/image_raw but I cannot see any other results from any topics. I know /ORB_SLAM2_PointMap_SegNetM/Point_Clouds is published.

Could you image what is the problem? What topic I need to add on RVIZ to see octomap?

Thanks in advance.

How does it work on a real robot

Hello，Thank you for your work!

Now I want to transplant it to my real robot, how should I operate it?

Error :cannot launch node of type [octomap_server/octomap_server_node]:cannot locate node [octomap_server_node] in package [octomap_serverer]

Hello!When I ran roslaunch DS_SLAM_TUM. Launch, the following error occurred:
Error :cannot launch node of type [octomap_server/octomap_server_node]:cannot locate node [octomap_server_node] in package [octomap_serverer].But I have downloaded and compiled the octomap_mapping package, I hope to get your help, thank you!

CPU is fulled

May I ask you a question?
When I execute DS_SLAM_BUILD.sh the memory will be fulled.
How can I solve it?
The project's file tree as follow:
ros-workspace--src--DS-SLAM--Examples--ROS--ORB_SLAM2_PointMap_SegNetM--caffe-segnet-cudnn5
ros-workspace--src--octomap_mapping-kinetic
ros-workspace--src--octomap_rviz_plugins-kinetic
There is any problem?

Getting very poor estimation when reproducing the work on fr3_walking_rpy sequence

Hi, it seems that something was wrong in my reproducing your excellant work. I got almost the same estimation to your paper in any other four sequences except for fr3_walking_rpy one. The numerical RPE values of this sequence came even larger than that of original ORB-SLAM2. I am confused, could you tell me what is the cause of this ?

compared_pose_pairs 9695 pairs
translational_error.rmse 0.570246 m
translational_error.mean 0.453428 m
translational_error.median 0.362260 m
translational_error.std 0.345808 m
translational_error.min 0.000000 m
translational_error.max 1.396033 m
rotational_error.rmse 11.530595 deg
rotational_error.mean 9.154053 deg
rotational_error.median 7.271315 deg
rotational_error.std 7.011271 deg
rotational_error.min 0.000000 deg
rotational_error.max 28.179311 deg

Compilation fails

your work is excellent，thanks for your code sharing.But I compile it using a long time,at last,it still has errors and can not compile successfully. My computer is i7CPU,not has GPU,I install caffe with CPU only(I don't know whether GPU and CUDA are necessary for this work.)
I install octomap and octomap_rviz in my ros workspace(catkin_ws/src),and I put the caffe-segnet-cudnn5 in /home/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM.
when I run ./DS_SLAM_BUILD.sh,Error is as follows

not able to download thirdparty and vocab folders from baidu

hi. im almost done in the compilation but now im stuck since I cant download the fodders from baidu storage, it requires registration and blocks numbers outside china.
If its not too much to ask could any one please upload the folders anywhere else so I could download?

Bug in pointcloudmapping.cc PointCloudMapping::viewer()

A lock should be added when calling generatePointCloud, or the addresses of the vectors may shift when other threads execute the push_back operation.
This is especially likely to happen in a device where the memory is limited and the memory block allocated initially is not large enough. If it happens, a segmentation fault will result.
Wrong:

        for ( size_t i=lastKeyframeSize; i<N ; i++ )
        {
            PointCloud::Ptr p = generatePointCloud( keyframes[i],semanticImgs_color[i], semanticImgs[i],colorImgs[i], depthImgs[i] );
	        *KfMap += *p;
	        *globalMap += *p;	    
        }

A possible correction:

        for ( size_t i=lastKeyframeSize; i<N ; i++ )
        {
            unique_lock<mutex> lck(keyframeMutex);
            PointCloud::Ptr p = generatePointCloud( keyframes[i],semanticImgs_color[i], semanticImgs[i],colorImgs[i], depthImgs[i] );
	        *KfMap += *p;
	        *globalMap += *p;	    
        }

Nothing displayed in RViz and no image received

Hello,when i run roslaunch DS_SLAM_TUM.launch ,according to the terminal log ,it seems works perfectly. But RViz displays nothind. I added "image","map","ColorOccupancyGrid",but it's status is "No Image received","No map received"
What should i do? Thank you very much!

DS_SLAM_TUM.launch has problems after running for tens of seconds

DS_SLAM_TUM.launch has problems after running for tens of seconds，can you help me

[TUM-2] process has died [pid 6832, exit code -6, cmd /home/mm/catkin_ws3/src/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM/TUM /home/mm/catkin_ws3/src/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM/../../../Vocabulary/ORBvoc.bin /home/mm/catkin_ws3/src/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM/TUM3.yaml /home/mm/catkin_ws3/src/DS-SLAM/rgbd_dataset_freiburg3_walking_xyz /home/mm/catkin_ws3/src/DS-SLAM/rgbd_dataset_freiburg3_walking_xyz/associate.txt /home/mm/catkin_ws3/src/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM/prototxts/segnet_pascal.prototxt /home/mm/catkin_ws3/src/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM/models/segnet_pascal.caffemodel /home/mm/catkin_ws3/src/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM/tools/pascal.png __name:=TUM __log:=/home/mm/.ros/log/e6c14834-bd9a-11ea-b802-80fa5b6b2c8a/TUM-2.log].
log file: /home/mm/.ros/log/e6c14834-bd9a-11ea-b802-80fa5b6b2c8a/TUM-2*.log

#define COLOR_OCTOMAP_SERVER

When I uncomment"#define COLOR_OCTOMAP_SERVER" in OctomapServer.h at the folder of octomap_mapping/octomap_server/include/octomap_server, the octomapping and octomap_rviz_plugins is uncompiled. what is your operation about this? Thank you

运行报错

Welcome !

Input sensor was set to: RGB-D

Loading ORB Vocabulary. This could take a while...
loading duration: 2.57s
Vocabulary loaded!

• size: 640x480
• fx: 535.4
• fy: 539.2
• cx: 320.1
• cy: 247.6
• k1: 0
• k2: 0
• p1: 0
• p2: 0
• bf: 40
• 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

Depth Threshold (Close/Far Points): 3.73552
####in Viewer run

---

Start processing sequence ...
Images in the sequence: 827

Load model ...
Wait for new RGB img time =
F0423 20:34:39.230823 7374 math_functions.cu:26] Check failed: status == CUBLAS_STATUS_SUCCESS (13 vs. 0) CUBLAS_STATUS_EXECUTION_FAILED
*** Check failure stack trace: ***
@ 0x7fa1a380d0cd google::LogMessage::Fail()
@ 0x7fa1a380ef33 google::LogMessage::SendToLog()
@ 0x7fa1a380cc28 google::LogMessage::Flush()
@ 0x7fa1a380f999 google::LogMessageFatal::~LogMessageFatal()
@ 0x7fa186bb021e caffe::caffe_gpu_gemm<>()
@ 0x7fa186b44c4e caffe::BNLayer<>::Forward_gpu()
@ 0x7fa186ac5202 caffe::Net<>::ForwardFromTo()
@ 0x7fa186ac5317 caffe::Net<>::Forward()
@ 0x7fa198c3a7b9 Classifier::Predict()
@ 0x7fa1a3b609d6 ORB_SLAM2::Segment::Run()
@ 0x7fa1a01fb4c0 (unknown)
@ 0x7fa19f4236db start_thread
@ 0x7fa19fc3f61f clone
[TUM-2] process has died [pid 7246, exit code -6, cmd /home/h/catkin_ws/src/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM/TUM /home/h/catkin_ws/src/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM/../../../Vocabulary/ORBvoc.bin /home/h/catkin_ws/src/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM/TUM3.yaml /home/h/catkin_ws/src/DS-SLAM/TUM_DATA/rgbd_dataset_freiburg3_walking_xyz /home/h/catkin_ws/src/DS-SLAM/TUM_DATA/rgbd_dataset_freiburg3_walking_xyz/associate.txt /home/h/catkin_ws/src/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM/prototxts/segnet_pascal.prototxt /home/h/catkin_ws/src/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM/segnet_pascal.caffemodel /home/h/catkin_ws/src/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM/tools/pascal.png __name:=TUM __log:=/home/h/.ros/log/bb689dd6-c301-11ec-9024-6c92bf1cd756/TUM-2.log].
log file: /home/h/.ros/log/bb689dd6-c301-11ec-9024-6c92bf1cd756/TUM-2*.log

how is pose estimation generated ?

Hi,
As I am reading your paper, I dont seem to be able to find how you guys estimate the pose of the robot after your removed the outliers.
What happens next ? in the pipeline it says visual odometry ? and then octo-map generation.

Can you guys explain how you generate the pose ?

Question about some results in your paper

你好：
在您论文中关于fr3_walking_xyz的结果对比中，ORB-SLAM2的ATE的RMSE是0.7521，我用单目ORB-SLAM2验证了这个序列，发现两个问题：1.前面好多帧无法完成初始化；2.就算完成初始化后，最终的RMSE平均大小在0.1上下浮动。
请问您是在单目下还是RGB-D情况下完成的实验？对于初始化，你们是怎么处理的？

Stereo Camera Real-time

Hello, I'm trying to modify the main code "ros_tum_real-time.cc" since I want to obtain images in real-time from a Zed camera, but when I run the code the number of raws of the variable Camera_Pose is 0, therefore the code doesn't publish data. the code is down.

#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>

#include<ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <octomap/octomap.h>    
#include <octomap/ColorOcTree.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Pose.h>
#include "geometry_msgs/PoseWithCovarianceStamped.h"

#include<opencv2/core/core.hpp>

#include"../../../include/System.h"

using namespace std;

using namespace octomap;

ros::Publisher CamPose_Pub;
ros::Publisher Camodom_Pub;
ros::Publisher odom_pub;

geometry_msgs::PoseStamped Cam_Pose;
geometry_msgs::PoseWithCovarianceStamped Cam_odom;

cv::Mat Camera_Pose;
tf::Transform orb_slam;
tf::TransformBroadcaster * orb_slam_broadcaster;
std::vector<float> Pose_quat(4);
std::vector<float> Pose_trans(3);

ros::Time current_time, last_time;
double lastx=0,lasty=0,lastth=0;
unsigned int a =0,b=0; 
octomap::ColorOcTree tree( 0.05 );

void Pub_CamPose(cv::Mat &pose);

typedef octomap::ColorOcTree::leaf_iterator it_t;

class ImageGrabber
{
public:
    ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){}

    void GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight);

    ORB_SLAM2::System* mpSLAM;
    bool do_rectify;
    cv::Mat M1l,M2l,M1r,M2r;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "Stereo");
    ros::start();

    if(argc != 8)
    {
        cerr << endl << "Usage: TUM path_to_vocabulary path_to_settings path_to_sequence path_to_association path_to_prototxt path_to_caffemodel path_to_pascal.png" << endl;
        ros::shutdown();
        return 1;
    }    


     ORB_SLAM2::Viewer *viewer;
    viewer = new ORB_SLAM2::Viewer();

    // Create SLAM system. It initializes all system threads and gets ready to process frames.    
    ORB_SLAM2::System SLAM(argv[1],argv[2], argv[5],argv[6],argv[7],ORB_SLAM2::System::STEREO, viewer);
    usleep(50);

    ImageGrabber igb(&SLAM);

    ros::NodeHandle nh;

    CamPose_Pub = nh.advertise<geometry_msgs::PoseStamped>("/Camera_Pose",1);
    Camodom_Pub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("/Camera_Odom", 1);
    odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 50);

    message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/left/image_rect_color", 1);
    message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "/right/image_rect_color", 1);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
    message_filters::Synchronizer<sync_pol> sync(sync_pol(10), left_sub,right_sub);
    sync.registerCallback(boost::bind(&ImageGrabber::GrabStereo,&igb,_1,_2));

    ros::spin();

    // Stop all threads
    SLAM.Shutdown();

    // Save camera trajectory
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory_TUM_Format.txt");
    SLAM.SaveTrajectoryTUM("FrameTrajectory_TUM_Format.txt");
    SLAM.SaveTrajectoryKITTI("FrameTrajectory_KITTI_Format.txt");

    ros::shutdown();

    return 0;
}

void ImageGrabber::GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight)
{
    // Copy the ros image message to cv::Mat.
    cv_bridge::CvImageConstPtr cv_ptrLeft;
    try
    {
        cv_ptrLeft = cv_bridge::toCvShare(msgLeft);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv_bridge::CvImageConstPtr cv_ptrRight;
    try
    {
        cv_ptrRight = cv_bridge::toCvShare(msgRight);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
	double segmentationTime=0;
        Camera_Pose=mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec());
	double orbTimeTmp=mpSLAM->mpTracker->orbExtractTime;
	double movingTimeTmp=mpSLAM->mpTracker->movingDetectTime;
	segmentationTime=mpSLAM->mpSegment->mSegmentTime;
	Pub_CamPose(Camera_Pose);

}

void Pub_CamPose(cv::Mat &pose)
{
    cv::Mat Rwc(3,3,CV_32F);
	cv::Mat twc(3,1,CV_32F);
	Eigen::Matrix<double,3,3> rotationMat;
	orb_slam_broadcaster = new tf::TransformBroadcaster;
	cout << pose.dims << "        " << pose.rows << endl;
	if(pose.dims<2 || pose.rows < 3)
	{
        Rwc = Rwc;
		twc = twc;
	}
	else
	{
		Rwc = pose.rowRange(0,3).colRange(0,3).t();
		twc = -Rwc*pose.rowRange(0,3).col(3);
		
		rotationMat << Rwc.at<float>(0,0), Rwc.at<float>(0,1), Rwc.at<float>(0,2),
					Rwc.at<float>(1,0), Rwc.at<float>(1,1), Rwc.at<float>(1,2),
					Rwc.at<float>(2,0), Rwc.at<float>(2,1), Rwc.at<float>(2,2);
		Eigen::Quaterniond Q(rotationMat);

		Pose_quat[0] = Q.x(); Pose_quat[1] = Q.y();
		Pose_quat[2] = Q.z(); Pose_quat[3] = Q.w();
		
		Pose_trans[0] = twc.at<float>(0);
		Pose_trans[1] = twc.at<float>(1);
		Pose_trans[2] = twc.at<float>(2);
		
		orb_slam.setOrigin(tf::Vector3(Pose_trans[2], -Pose_trans[0], -Pose_trans[1]));
		orb_slam.setRotation(tf::Quaternion(Q.z(), -Q.x(), -Q.y(), Q.w()));
		orb_slam_broadcaster->sendTransform(tf::StampedTransform(orb_slam, ros::Time::now(), "/map", "/base_link"));
		
		Cam_Pose.header.stamp = ros::Time::now();
		Cam_Pose.header.frame_id = "/map";
		tf::pointTFToMsg(orb_slam.getOrigin(), Cam_Pose.pose.position);
		tf::quaternionTFToMsg(orb_slam.getRotation(), Cam_Pose.pose.orientation);
		
		Cam_odom.header.stamp = ros::Time::now();
		Cam_odom.header.frame_id = "/map";
		tf::pointTFToMsg(orb_slam.getOrigin(), Cam_odom.pose.pose.position);
		tf::quaternionTFToMsg(orb_slam.getRotation(), Cam_odom.pose.pose.orientation);
		Cam_odom.pose.covariance = {0.01, 0, 0, 0, 0, 0,
									0, 0.01, 0, 0, 0, 0,
									0, 0, 0.01, 0, 0, 0,
									0, 0, 0, 0.01, 0, 0,
									0, 0, 0, 0, 0.01, 0,
									0, 0, 0, 0, 0, 0.01};
		
		CamPose_Pub.publish(Cam_Pose);
		Camodom_Pub.publish(Cam_odom);
		
		nav_msgs::Odometry odom;
		odom.header.stamp =ros::Time::now();
		odom.header.frame_id = "/map";

		// Set the position
		odom.pose.pose.position = Cam_odom.pose.pose.position;
		odom.pose.pose.orientation = Cam_odom.pose.pose.orientation;

		// Set the velocity
		odom.child_frame_id = "/base_link";
		current_time = ros::Time::now();
		double dt = (current_time - last_time).toSec();
		double vx = (Cam_odom.pose.pose.position.x - lastx)/dt;
		double vy = (Cam_odom.pose.pose.position.y - lasty)/dt;
		double vth = (Cam_odom.pose.pose.orientation.z - lastth)/dt;
		
		odom.twist.twist.linear.x = vx;
		odom.twist.twist.linear.y = vy;
		odom.twist.twist.angular.z = vth;

		// Publish the message
		odom_pub.publish(odom);
		
		last_time = current_time;
		lastx = Cam_odom.pose.pose.position.x;
		lasty = Cam_odom.pose.pose.position.y;
		lastth = Cam_odom.pose.pose.orientation.z;
	}
}

How to save the semantic segmentation result for each frame?

Hi, your work is amazing. I want to save the semantic segmentation result for each frame. Please help me. Thank you very much!

How to get CameraTrajectory.txt and KeyFrameTrajectory.txt?

But I cannot find CameraTrajectory.txt and KeyFrameTrajectory.txt.

protobuf3 环境下运行失败

[libprotobuf FATAL google/protobuf/stubs/common.cc:78] This program was compiled against version 2.6.1 of the Protocol Buffer runtime library, which is not compatible with the installed version (3.3.0). Contact the program author for an update. If you compiled the program yourself, make sure that your headers are from the same version of Protocol Buffers as your link-time library. (Version verification failed in "/build/mir-O8_xaj/mir-0.26.3+16.04.20170605/obj-x86_64-linux-gnu/src/protobuf/mir_protobuf.pb.cc".)
terminate called after throwing an instance of 'google::protobuf::FatalException'
what(): This program was compiled against version 2.6.1 of the Protocol Buffer runtime library, which is not compatible with the installed version (3.3.0). Contact the program author for an update. If you compiled the program yourself, make sure that your headers are from the same version of Protocol Buffers as your link-time library. (Version verification failed in "/build/mir-O8_xaj/mir-0.26.3+16.04.20170605/obj-x86_64-linux-gnu/src/protobuf/mir_protobuf.pb.cc".)
已放弃 (核心已转储)

能不能在protobuf3环境下实现该项目？该怎么解决呢？

Program running crash

Hello, thanks for your great job and open source sharing. But I encountered the problem of program runing crash. Do you know the reason?

Compilation failed

[ 12%] Linking CXX executable ../RGBD_RealTime
[ 25%] Linking CXX executable ../KITTI
../../../../build/libORB_SLAM2_PointMap_SegNetM.so：对‘image_transport::ImageTransport::ImageTransport(ros::NodeHandle const&)’未定义的引用
../../../../build/libORB_SLAM2_PointMap_SegNetM.so：对‘image_transport::ImageTransport::~ImageTransport()’未定义的引用
collect2: error: ld returned 1 exit status
CMakeFiles/RGBD_RealTime.dir/build.make:490: recipe for target '../RGBD_RealTime' failed
make[2]: *** [../RGBD_RealTime] Error 1
CMakeFiles/Makefile2:67: recipe for target 'CMakeFiles/RGBD_RealTime.dir/all' failed
make[1]: *** [CMakeFiles/RGBD_RealTime.dir/all] Error 2
make[1]: *** 正在等待未完成的任务....
../../../../build/libORB_SLAM2_PointMap_SegNetM.so：对‘image_transport::ImageTransport::ImageTransport(ros::NodeHandle const&)’未定义的引用
../../../../build/libORB_SLAM2_PointMap_SegNetM.so：对‘image_transport::ImageTransport::~ImageTransport()’未定义的引用
collect2: error: ld returned 1 exit status
CMakeFiles/KITTI.dir/build.make:490: recipe for target '../KITTI' failed
make[2]: *** [../KITTI] Error 1
CMakeFiles/Makefile2:104: recipe for target 'CMakeFiles/KITTI.dir/all' failed
make[1]: *** [CMakeFiles/KITTI.dir/all] Error 2
Makefile:127: recipe for target 'all' failed
make: *** [all] Error 2

Who could help me with it, please? Thanks very much!

Compilation fails

your work is excellent，thanks for your code sharing.But I compile it using a long time,at last,it still has errors and can not compile successfully. My computer is i7CPU,not has GPU,I install caffe with CPU only(I don't know whether GPU and CUDA are necessary for this work.)
I install octomap and octomap_rviz in my ros workspace(catkin_ws/src),and I put the caffe-segnet-cudnn5 in /home/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM.
when I run ./DS_SLAM_BUILD.sh,Error is as follows：

Can you help me with these problems？Thank you very much！

No Image Received/No Map Received in RVIZ, Please Help!

Hello, when i roslaunched DS_SLAM_TUM.launch, there's no error reported and RVIZ was switched on successfully, but there were no results. I found some similar problems in the comments but i am still confused. i thought i added components by topic and sitll i got no image received or map received. Did i set the wrong dataset path? Could u please tell me the correct way to edit the launch.txt? I m a little confused by the sentence "Change PATH_TO_SEQUENCE and PATH_TO_SEQUENCE/associate.txt in the DS_SLAM_TUM.launch to the sequence directory that you download before", i looked it up and found that i need a script named associate.py, i did what i did when running TUM dataset in orbslam2. Please do reply me. THX!

关于CheckMovingKeyPoints( )函数的问题

学姐你好，在函数CheckMovingKeyPoints( )中，

不太明白这段代码是什么意思，并(int m=-15, m<15, m++)中的15是什么意思？
对您的工程很感兴趣，希望收到您的回复？
谢谢，祝好！

运行失败

您好，请问 在运行的时候出现这个情况，可能是什么原因呢？ 我使用的是opencv3.4.6 pcl1.8 ros_melodic，caffe是在Python2.7环境下编译安装的

The question about the result

After I had run the program, I got the result which is not satisfactory. First, I only got a octo-map without color, not semantic octo-tree map as described in the article. Second, in the fr3_walking_xyz sequence, the ATE of DS_SLAM is ten times bigger than that result in paper. As for the result, I want to know Where I may be wrong.
The result:
compared_pose_pairs 826 pairs
absolute_translational_error.rmse 0.216291 m
absolute_translational_error.mean 0.202078 m
absolute_translational_error.median 0.209339 m

Crashed while reading NO.116 frame

Hi @ivipsourcecode
May I ask you a question?
The program has not been initialized, and crashed while reading NO.116 frame.
What happend to it?
Thx.

Dependency conflict while making TUM example

The DS_SLAM_BUILD.sh file errors out while building the TUM example

CMake Warning at /opt/ros/kinetic/share/ros/core/rosbuild/public.cmake:509 (add_executable):
  Cannot generate a safe linker search path for target TUM because there is a
  cycle in the constraint graph:

    dir 0 is [/catkin_ws/devel/.private/tf/lib]
    dir 1 is [/catkin_ws/devel/.private/tf2_ros/lib]
    dir 2 is [/catkin_ws/devel/.private/tf2_py/lib]
    dir 3 is [/catkin_ws/devel/.private/tf2/lib]
    dir 4 is [/catkin_ws/devel/.private/tf2_msgs/lib]
    dir 5 is [/opt/ros/kinetic/lib]
    dir 6 is [/catkin_ws/src/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM/../../../Examples/ROS/ORB_SLAM2_PointMap_SegNetM/caffe-segnet-cudnn5/lib]
    dir 7 is [/opt/ros/kinetic/lib/x86_64-linux-gnu]
    dir 8 is [/catkin_ws/devel/lib]
    dir 9 is [/usr/lib/x86_64-linux-gnu/hdf5/serial/lib]
      dir 14 must precede it due to link library [libhdf5.so]
    dir 10 is [/catkin_ws/src/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM/../../../Thirdparty/DBoW2/lib]
    dir 11 is [/catkin_ws/src/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM/../../../Thirdparty/g2o/lib]
    dir 12 is [/catkin_ws/src/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM/../../../build]
    dir 13 is [/catkin_ws/src/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM/../../../Examples/ROS/ORB_SLAM2_PointMap_SegNetM/libsegmentation/build]
    dir 14 is [/usr/lib/x86_64-linux-gnu/hdf5/openmpi/lib]
      dir 9 must precede it due to link library [libhdf5.so]
    dir 15 is [/usr/lib/openmpi/lib]

It compiles anyway, but won't this cause problems down the line?

there is an error when I "Configuring and building libORB_SLAM2_PointMap_SegNetM.so ..."

/home/dasong/DS_catkin_ws/src/DS-SLAM/src/System.cc: In constructor ‘ORB_SLAM2::System::System(const string&, const string&, const string&, const string&, const string&, ORB_SLAM2::System::eSensor, ORB_SLAM2::Viewer*, ORB_SLAM2::Map*, ORB_SLAM2::ORBVocabulary*)’:
/home/dasong/DS_catkin_ws/src/DS-SLAM/src/System.cc:114:38: error: ‘ORB_SLAM2::ORBVocabulary’ has no member named ‘loadFromBinaryFile’
bVocLoad = mpVocabulary->loadFromBinaryFile(strVocFile);
^
make[2]: *** [CMakeFiles/ORB_SLAM2_PointMap_SegNetM.dir/src/System.cc.o] 错误 1
make[1]: *** [CMakeFiles/ORB_SLAM2_PointMap_SegNetM.dir/all] 错误 2
make: *** [all] 错误 2

when I launch DS_SLAM_TUM.launch, there are no results shown in rviz

Hi, your work is amazing! I have installed all the prerequisites and Built the DS_SLAM library. However, when I launch DS_SLAM_TUM.launch, there are no results shown in rviz. I have changed PATH_TO_SEQUENCE and PATH_TO_SEQUENCE/associate.txt in the DS_SLAM_TUM.launch to the sequence directory that I download. The following image is my running result, there is no errors in the terminal.

Could you help me out here? Thank you very much!

如何查看语义地图

博主您好，我刚刚接触语义slam。请问在Rviz中如何显示语义地图，我尝试打开ColorOccupancyGrid中的octomap_full和topic中的occupied_cells_vis_array,但这些看起来都不太像语义地图。并且在occupied_cells_vis_array里出现了状态警告，map/15 Uninitialized quanternion和map/16 Uninitialized quanternion .请问这两个警告会影响仿真结果吗

Compile error: no caffe/caffe.hpp

Thank you for your excellent work! When I was compiling, the above error occurred: no caffe file was found, but my caffe compilation was successful.
I hope to get your help

Question about Formula (6) in the paper published on "IROS 2018"

In the paper "DS-SLAM: A Semantic Visual SLAM towards Dynamic Environments", the iterative formula (6) about log odds score calculating seems to have a wrong subscript. I think it is more reasonable to be written as "L(n|z 1:t+1) = L(n|z 1:t) + L(n|z t+1)"

How is octomap_mapping and octomap_rviz installed ??

I just started studying in ROS

I have installed in catkin_ws/src with
git clone https://github.com/OctoMap/octomap_mapping and
git clone https://github.com/OctoMap/octomap_rviz_plugins
then, use command catkin_make

The result shows as follow :

1. How can I fix this problem ? Is there other octomap installation??
2. What is PATH_TO_SEQUENCE in the DS_SLAM_TUM.launch ?? It's a path to rgb.txt, depth.txt, rgb figure or depth figure folder ??

It's very good if you tell me how to do it in detail. Thank you

Compiling issues

Hello,

I am struggling with compiling DS-SLAM.
I have a folder uwsim_ws//install/share where I have ORB-SLAM2 which works fine.
I installed DS-SLAM in uwsim_ws/share/install, but when I ran ./DS_SLAM_BUILD.sh, I received errors:

Then I went to uwsim_ws/install/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM and ran
$ git clone https://github.com/TimoSaemann/caffe-segnet-cudnn5.git
$ cmake .

Then re-ran the ./DS_SLAM_BUILD.sh, and still got the same errors.

I am running Ubuntu 14.04, indigo distribution of ROS.

Publish Cam_Pose question

What's the meaning :

orb_slam.setOrigin(tf::Vector3(Pose_trans[2], -Pose_trans[0], -Pose_trans[1])); orb_slam.setRotation(tf::Quaternion(Q.z(), -Q.x(), -Q.y(), Q.w()));

Why you construct Quaternion by (Q.z(), -Q.x(), -Q.y(), Q.w())?

Thanks

How to save .pcd file?

Sorry, I would like to know if someone has saved the point cloud file (.pcd file) successful.

Questions about showing result

I'm trying to run
roslaunch DS_SLAM_TUM.launch
using rgbd_dataset_freiburg3_walking_xyz dataset

I got this result:


$ rostopic list /Camera_Odom /Camera_Pose /ORB_SLAM2_PointMap_SegNetM/Point_Clouds /clicked_point /free_cells_vis_array /initialpose /move_base_simple/goal /occupied_cells_vis_array /octomap_binary /octomap_full /octomap_point_cloud_centers /octomap_server/parameter_descriptions /octomap_server/parameter_updates /odom /point_cloud /projected_map /rosout /rosout_agg /tf /tf_static

`Wait for new RGB img time =
process time =16.7728
wait for new segment img time =0.000384
check time =0.039342
SLAM TrackRGBD all time =45.6625

Wait for new RGB img time =
process time =15.2025
wait for new segment img time =0.000377
check time =0.045316
SLAM TrackRGBD all time =49.9497

---

`

Then, I have some questions:

1. How can I check RGB images?
2. How can I check the segmentation result or the result after outlier removed by RGB images in real time?
3. How can I use the kinetic v2 camera or realsense camera to run it in real time? Is there any sample code?

Thanks

when I have finshed this DS-SLAM, I can't find the result "CameraTrajectory.txt" and "KeyFrameTrajectory.txt"

median tracking time: 6.08592
mean tracking time: 6.17377
mean orb extract time =0.0314097
mean moving detection time =0.114223
mean segmentation time =6.14007

Saving camera trajectory to CameraTrajectory.txt ...

trajectory saved!

Saving keyframe trajectory to KeyFrameTrajectory.txt ...

trajectory saved!