DS-SLAM代码（动态环境下基于图像分割的SLAM）阅读笔记（一）

本人的毕业设计是基于动态环境下的SLAM，所以从开源代码DS-SLAM开始学习，如有理解不周到的地方，欢迎指正。

“备注：学习DS-SLAM需要先完整的学习ORBSLAM2，理清整个流程”

今天学习ros_tum_realtime.cc 这个主函数

这个文件和ORBSLAM2中的rgbd_tum.cc的文件内容基本上一样的，主要的目的就是构建整个SLAM系统，并且将数据集中的深度depth图和RGBD图在for循环中逐个发送到SLAM系统当中去，然后SLAM计算得到的位姿信息发布到ROS中的topic中去，接下来我们详细来看。
（1）首先，我们定义了一个ROS的节点的名称为 TUM

ros::init(argc, argv, "TUM");   // 定义了一个ROS的节点的名称为  TUM

接下来和ORBSLAM2一样调用LoadImages 加载图片：

LoadImages(strAssociationFilename, vstrImageFilenamesRGB, vstrImageFilenamesD, vTimestamps);
调用SLAM系统的构造函数去初始化整个SLAM系统，这一部分内容主要在System.cc文件里面
 ORB_SLAM2::System SLAM(argv[1],argv[2], argv[5],argv[6],argv[7],ORB_SLAM2::System::RGBD, viewer);

（2）之后我们创建了3个Publisher CamPose_Pub 、Camodom_Pub 、odom_pub。

 ros::NodeHandle nh;  //创建一个节点的句柄
    // 发布了消息类型为geometry_msgs::PoseStamped 的消息，话题为/Camera_Pose" ，消息队列的大小为1
    CamPose_Pub = nh.advertise<geometry_msgs::PoseStamped>("/Camera_Pose",1);
    //发布了话题  Camera_Odom ，消息队列大小为1
    Camodom_Pub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("/Camera_Odom", 1);

    //发布了话题  odom ，消息队列大小为50
    odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 50);

1. CamPose_Pub 发布器发布的消息类型为geometry_msgs::PoseStamped 的消息，将该消息发布到话题为**/Camera_Pose**" ，消息队列的大小为1
2. Camodom_Pub 发布了话题  **Camera_Odom** ，消息队列大小为1
3. odom_pub 布了话题  **odom** ，消息队列大小为50

关于geometry_msgs::PoseStamped，可以参考ROS官网的解释：

http://wiki.ros.org/geometry_msgs?distro=noetic

这个ROS的消息类型包括帧头Header和位姿信息Pose,用下面的一张图可以说明：

pose 里面含有X，Y，Z三轴平移信息和四个float64的变量x,y,z,w四元数信息可以表示旋转，我们一会要将SLAM的计算出来的位姿信息发送到上面这些topic供我们建图等环节使用。

（3）之后，我们可以在一个while循环里面遍历数据集里面的每一幅图片，并将深度图imD，RGB图imRGB逐个发送到SLAM系统之中，并可以获得当前帧的位姿矩阵Tcw，并且通过ROS将位姿矩阵T发送到前面的3个Topic

 // 当前帧的相机的位姿矩阵
	    Camera_Pose =  SLAM.TrackRGBD(imRGB,imD,tframe);
	    ........ 中间是计算时间的一些代码，不重要
	     // 每来一次新图片，都会发布一次相机的位姿
	    Pub_CamPose(Camera_Pose);

我们下来着重理清一下位姿发送的函数：

void Pub_CamPose(cv::Mat &pose)

首先我们要明确，位姿矩阵是一个4*4的矩阵，里面有平移和旋转信息。
定义了一个TF广播，一会我们要将我们的位姿信息通过这个广播到TF树：

orb_slam_broadcaster = new tf::TransformBroadcaster;

取出T矩阵里面的旋转矩阵R，并将其转化为四元数Q，和平移向量t。

		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);
		// 将旋转矩阵R 转化为四元数 Q 
		Eigen::Quaterniond Q(rotationMat);
		
		// 将位姿信息放到 Pose_quat  Pose_trans 数组里面
		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);

定义存放转换信息（平动，转动）的变量 tf::Transform orb_slam，这个变量可以表示两个坐标系之间的变换关系,实际上就是TF树里面表示平移和旋转的矩阵，接下来，为这个矩阵赋值平移信息和旋转信息：

		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()));

然后将这个变换矩阵广播到TF树上面去：

// 将变换广播出去 母坐标系是  "/map"    子坐标系是"/base_link"
		// 将父坐标系和子坐标系的变换矩阵存储在 orb_slam 变换矩阵里，然后广播出去，完善TF树
		orb_slam_broadcaster->sendTransform(
		        tf::StampedTransform(orb_slam,            // 存储的是变换矩阵
		                            ros::Time::now(),     //时间戳
		                              "/map",             // 父坐标系
		                              "/base_link"));     //子坐标系

我们不仅要把SLAM计算的位姿矩阵广播到TF树上面去，还要把这个位姿矩阵发布到topic上面去，因此我们需要将Transform的数据类型转换为ros的msg 消息类型

这里还要说明的一点就是，SLAM当中仅仅存在两个坐标系：世界坐标系和当前的相机坐标系，我们的位姿矩阵都是当前的相机坐标系（随着相机的运动不断地在变化，也就是这里的父坐标系“/map”）相对于世界坐标系（第一帧相机坐标系,也就是这里的“/base_link”）

1. 首先我们往第一个topic "/Camera_Pose"发送消息 ,消息类型为 geometry_msgs::PoseStamped

		geometry_msgs::PoseStamped Cam_Pose  是一个msgs的消息类型，Cam_Pose是消息的名称

		tf::pointTFToMsg(orb_slam.getOrigin(), Cam_Pose.pose.position);
		tf::quaternionTFToMsg(orb_slam.getRotation(), Cam_Pose.pose.orientation);
		Cam_Pose.header.stamp = ros::Time::now();
		Cam_Pose.header.frame_id = "/map";    // 这个消息对应的坐标系是 “/map”

消息的内容填充好之后，将这个消息发送出去：

CamPose_Pub.publish(Cam_Pose);   //发布消息

2. 往第二个topic “/Camera_Odom” 发布消息 ，这次的消息类型为 geometry_msgs::PoseWithCovarianceStamped，不仅包含位姿信息还包含 位姿的协方差

		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};  //位置的协方差矩阵
		Camodom_Pub.publish(Cam_odom);  //发布消息

3. 往第3个 topic “odom” 发布消息，这次的消息类型为nav_msgs::Odometry ，这个消息类型我们可以继续在ROS官网查看：

http://docs.ros.org/en/melodic/api/nav_msgs/html/msg/Odometry.html

可以看到 这个消息类型不仅有位姿信息还有 速度信息，我们对这个消息一一进行赋值：

		nav_msgs::Odometry odom;
		odom.header.stamp =ros::Time::now();  // 赋值 header 部分
		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);

至此，我们的发布相机的函数已经说完了，之后遍历完整个数据集，SLAM系统就可以关闭了。
（4）SLAM系统关闭SLAM.Shutdown();
（5）Save camera trajectory 保存相机的轨迹和关键帧的轨迹

	SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");	

注意最后这两个文件的存放路径，如果不改源代码的话，那么我的是存放于 /home/miao/.ros文件夹里面，所以 你需要按键Ctrl+H 可以显示隐藏文件。