slam定位练习笔记(二)
本篇文章主要是学习笔记三里面的一些疑问和对于它们的理解，还有在关于一些创新点的练习。
问题部分
第一个问题：front_end部分与front_end_flow之间的关系？
front_end_flow和front_end之间的关系类似前一个版本的node文件和front_end之间的关系，而front_end和front_end_flow之间是通过在front_end_flow.hpp中写一个智能指针来调用front_end的模块。
//front_end_flow.hpp
std::shared_ptr front_end_ptr_; 第二个问题：设置了interface的基类，如何扩展出ndt、icp？有关使用make_shared智能指针的相关设置。
是一种智能指针和类的继承的简单的联合使用方式，首先使用shared_ptr
#include
#include
class interface
{
public:
interface()
{
std::cout << "调用了interface基类" << std::endl;
}
~interface()
{
std::cout <<"析构interface基类"<< std::endl;
}
};
class ndt : public interface
{
public:
ndt()
{
std::cout << "调用了ndt类" << std::endl;
}
~ndt()
{
std::cout << "析构ndt类" < inter = std::make_shared();
std::shared_ptr inter1(inter);
std::shared_ptr inter2 = std::make_shared();
inter2 = inter;
std::cout << inter.use_count() << std::endl;
} 输出：
调用了interface基类
调用了ndt类
调用了interface基类
调用icp类
析构icp类
析构interface基类
3
析构ndt类
析构interface基类第三个问题：为什么要把ndt的配准过程拆分为两个函数？SetInputTarget和ScanMatch。
先从ndt配准的基本流程来看：
把ndt->setInputTarget放在了SetInputTarget函数中，把其它三个放在了ScanMatch函数里面。
在具体的实施代码中，第一步setInputTarget和其它三步并不在一起使用。setInputTarget出现在UpdataWithNewFrame中：
//UpdatawithNewFrame函数：
if (local_map_frames_.size() < 10) {
registration_ptr_->SetInputTarget(local_map_ptr_);
} else {
CloudData::CLOUD_PTR filtered_local_map_ptr(new CloudData::CLOUD());
local_map_filter_ptr_->Filter(local_map_ptr_, filtered_local_map_ptr);
registration_ptr_-> SetInputTarget(filtered_local_map_ptr);
}而ScanMatch函数出现在Updata函数里面：
//Updata函数：
registration_ptr_->ScanMatch(filtered_cloud_ptr, predict_pose, result_cloud_ptr_, current_frame_.pose);
cloud_pose = current_frame_.pose;所以分开来写相当与把ndt配准的流程按功能区分开了，封装了功能，利于工程实现，代码更简洁了。
第四个问题：为什么ndt和voxel在使用中将初始化参数的步骤独立封装成了一个函数？SetRegistrationParam和SetFilterParam，然后使用了不同的构造函数调用相同的参数初始化函数？
我认为是因为工程采用了yaml格式的参数设置的方式，所以设置了两个不同的构造函数。一个对应常规的初始化形式，另一个对应于使用yaml文件进行node初始化。然后把参数设置独立封装成一个函数，然后使用不同的构造函数调用这一个函数。
第五个问题：为什么在UpdataWithNewFrame函数里面将点云的关键帧存在硬盘中使用的是global_map_frames_，而不使用local_map_frames_。
如果使用local_map_frames的话，会限制在20帧以内，而使用global_map_frames不会有这个影响。并且这里使用不同的file_path，然后另一个函数Savemap就使用相同的路径来将所有的关键帧叠加起来。
//函数UpdatawithNewFrame
std::string file_path = data_path_ + "/key_frames/key_frame_" + std::to_string(global_map_frames_.size()) + ".pcd";
pcl::io::savePCDFileBinary(file_path, *new_key_frame.cloud_data.cloud_ptr);
//函数Scan_match
bool FrontEnd::SaveMap() {
global_map_ptr_.reset(new CloudData::CLOUD());
std::string key_frame_path = "";
CloudData::CLOUD_PTR key_frame_cloud_ptr(new CloudData::CLOUD());
CloudData::CLOUD_PTR transformed_cloud_ptr(new CloudData::CLOUD());
for (size_t i = 0; i < global_map_frames_.size(); ++i) {
key_frame_path = data_path_ + "/key_frames/key_frame_" + std::to_string(i) + ".pcd";
pcl::io::loadPCDFile(key_frame_path, *key_frame_cloud_ptr);
pcl::transformPointCloud(*key_frame_cloud_ptr,
*transformed_cloud_ptr,
global_map_frames_.at(i).pose);
*global_map_ptr_ += *transformed_cloud_ptr;
}
std::string map_file_path = data_path_ + "/map.pcd";
pcl::io::savePCDFileBinary(map_file_path, *global_map_ptr_);
has_new_global_map_ = true;
return true;
}有两个第一次遇见的函数：
pcl::io::loadPCDFile(key_frame_path, *key_frame_cloud_ptr);
pcl::io::savePCDFileBinary(map_file_path, *global_map_ptr_);值得学习一下。
练习部分：
第一个练习：关于ndt和voxel的学习。
直接使用学习笔记里面引用的文章。
有两个错误，
第一个是ndt.setInputCLoud()这个函数在现在的库中被摒弃了，得使用ndt.setInputSource();
第二个是使用多线程的时候，需要将boost_thread写进target_link_libraries里面。
#include
#include
#include
#include
#include
#include
#include
int main (int argc, char** argv)
{
//加载房间的第一次扫描
pcl::PointCloud::Ptr target_cloud (new pcl::PointCloud);
if (pcl::io::loadPCDFile ("room_scan1.pcd", *target_cloud) == -1)
{
PCL_ERROR ("Couldn't read file room_scan1.pcd \n");
return (-1);
}
std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << std::endl;
//加载从新视角得到的房间的第二次扫描
pcl::PointCloud::Ptr input_cloud (new pcl::PointCloud);
if (pcl::io::loadPCDFile ("room_scan2.pcd", *input_cloud) == -1)
{
PCL_ERROR ("Couldn't read file room_scan2.pcd \n");
return (-1);
}
std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl;
//将输入的扫描过滤到原始尺寸的大概 10% 以提高匹配的速度。
pcl::PointCloud::Ptr filtered_cloud (new pcl::PointCloud);
pcl::ApproximateVoxelGrid approximate_voxel_filter;
approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2);
approximate_voxel_filter.setInputCloud (input_cloud);
approximate_voxel_filter.filter (*filtered_cloud);
std::cout << "Filtered cloud contains " << filtered_cloud->size ()
<< " data points from room_scan2.pcd" << std::endl;
//初始化正态分布变换（NDT）
pcl::NormalDistributionsTransform ndt;
//设置依赖尺度NDT参数
//为终止条件设置最小转换差异
ndt.setTransformationEpsilon (0.01);
//为More-Thuente线搜索设置最大步长
ndt.setStepSize (0.1);
//设置NDT网格结构的分辨率（VoxelGridCovariance）
ndt.setResolution (1.0);
//设置匹配迭代的最大次数
ndt.setMaximumIterations (50);
// 设置要配准的点云
// ndt.setInputCloud (filtered_cloud);
ndt.setInputSource (filtered_cloud);
//设置点云配准目标
ndt.setInputTarget (target_cloud);
//设置使用机器人测距法得到的初始对准估计结果
Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());
Eigen::Translation3f init_translation (1.79387, 0, 0);
Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();
//计算需要的刚体变换以便将输入的点云匹配到目标点云
pcl::PointCloud::Ptr output_cloud (new pcl::PointCloud);
ndt.align (*output_cloud, init_guess);
std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()
<< " score: " << ndt.getFitnessScore () << std::endl;
//使用创建的变换对未过滤的输入点云进行变换
pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());
//保存转换的输入点云
pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);
// 初始化点云可视化界面
boost::shared_ptr
viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer_final->setBackgroundColor (0, 0, 0);
//对目标点云着色（红色）并可视化
pcl::visualization::PointCloudColorHandlerCustom
target_color (target_cloud, 255, 0, 0);
// viewer_final->addPointCloud (target_cloud, target_color, "target cloud");
viewer_final->addPointCloud (input_cloud, target_color, "target cloud");
viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "target cloud");
//对转换后的目标点云着色（绿色）并可视化
pcl::visualization::PointCloudColorHandlerCustom
output_color (output_cloud, 0, 255, 0);
viewer_final->addPointCloud (output_cloud, output_color, "output cloud");
viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "output cloud");
// 启动可视化
viewer_final->addCoordinateSystem (1.0);
viewer_final->initCameraParameters ();
//等待直到可视化窗口关闭。
while (!viewer_final->wasStopped ())
{
viewer_final->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
return (0);
} 注释写很清楚了，看注释就行了。
简单修改了下代码，输出了变换后的pcd文件。
第二个练习：关于yaml格式的学习。
关于yaml有两个练习，一个是读取已经存在yaml文件，一个是写一个yaml文件。首先在cmakelists.txt文件里面加入：
link_directories(/usr/local/lib)
include_directories(/usr/local/include/yaml-cpp)首先是读取一个yaml文件，
#include
#include
#include
#include
int main(int argc, char **argv)
{
std::string fin = "/media/hhh/75c0c2a9-f565-4a05-b2a5-5599a918e2f0/hhh/STUDY/yaml_study/src/study_yaml/config/param.yaml"; //yaml文件所在的路径
YAML::Node yamlConfig = YAML::LoadFile(fin);
int int_param = yamlConfig["int_param"].as();
std::cout << " node size: " << yamlConfig.size() << std::endl;
std::cout << yamlConfig["bool_param"].as() << std::endl;
yamlConfig["bool_param"] = !yamlConfig["bool_param"].as();
std::cout << yamlConfig["bool_param"].as() << std::endl;
std::cout << "str_param: " << yamlConfig["str_param"].as() << std::endl;
std::ofstream file;
file.open(fin);
file.flush();
file << yamlConfig;
file.close();
return 0;
} param.yaml文件：
bool_param: true
int_param: 2
double_param: 0.5
str_param: "123"
然后是生成一个yaml文件：
#include
#include
#include
#include
int main(int argc, char **argv)
{
std::ofstream fout("/media/hhh/75c0c2a9-f565-4a05-b2a5-5599a918e2f0/hhh/STUDY/yaml_study/src/study_yaml/config/p111.yaml");
YAML::Emitter out(fout);
out << YAML::BeginMap;
out << YAML::Key << "int_param";
out << YAML::Value << 1;
out << YAML::Key << "double_param";
out << YAML::Value << 0.5;
out << YAML::Key << "bool_param";
out << YAML::Value << false;
out << YAML::Comment("bool parameter");
out << YAML::Key << "str_param";
out << YAML::Value << "test";
out << YAML::EndMap;
return 0;
} 生成的yaml文件：
int_param: 1
double_param: 0.5
bool_param: false # bool parameter
str_param: test第三个练习：关于ros-services的学习。
利用ros-service来生成一只海龟，这里使用了ros::ServiceClient来创建client
#include "ros/ros.h"
#include "turtlesim/Spawn.h"
int main(int argc, char **argv)
{
setlocale(LC_ALL,"");
/* 初始化ROS节点 */
ros::init(argc,argv,"turtle_spawn_client");
/* 为这个节点创建句柄 */
ros::NodeHandle nh;
/* 创建service client 指定服务类型为turtlesim::Spawn */
ros::ServiceClient client = nh.serviceClient("/spawn");
/* 等待服务器启动 */
ros::service::waitForService("/spawn");
/* 实例化服务数据类型，并给成员request赋值 */
turtlesim::Spawn spawn;
spawn.request.x = 2;
spawn.request.y = 8;
spawn.request.theta = 1.8;
spawn.request.name = "new_turtle1";
/* 服务调用 */
if(client.call(spawn))
{
ROS_INFO("小乌龟[%s]已孵化", spawn.response.name.c_str());
}
else
{
ROS_INFO("小乌龟孵化失败！");
}
return 0;
}
这里就直接生成一个new_tuetle1
还有一个对于ros::ServiceServer的使用。
这个和topic订阅类似，但返回的是bool值，代码如下：
#include
#include
#include
ros::Publisher turtle_vel_pub;
bool pubCommand = false;
// service回调函数，输入参数req，输出参数res
bool commandCallback(std_srvs::Trigger::Request &req,
std_srvs::Trigger::Response &res)
{
pubCommand = !pubCommand;
// 显示请求数据
ROS_INFO("Publish turtle velocity command [%s]", pubCommand==true?"Yes":"No");
// 设置反馈数据
res.success = true;
res.message = "Change turtle command state!";
return true;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "turtle_command_server");
ros::NodeHandle n;
ros::ServiceServer command_service = n.advertiseService("/turtle_command", commandCallback);
turtle_vel_pub = n.advertise("/turtle1/cmd_vel", 10);
ROS_INFO("Ready to receive turtle command.");
ros::Rate loop_rate(10);
while(ros::ok())
{
ros::spinOnce();
if(pubCommand)
{
geometry_msgs::Twist vel_msg;
vel_msg.linear.x = 0.5;
vel_msg.angular.z = 0.2;
turtle_vel_pub.publish(vel_msg);
}
loop_rate.sleep();
}
return 0;
}
就是等待一个"/turtle_command"的命令后就调用后面的commandCallback函数，两个参数，第一个是输入，第二个是输出，一般是用来表示是否接受到消息，比较简单。
这段代码的核心在while里面那个判断，只有当使用这个命令的时候，pubCommand才会变成true
rosservice call /turtle_command第四个练习：关于boost库中关于filesystem的学习。
比较简单，实现了检测是否存在这个文件和创建这个文件的演示。
#include
#include
#include
int main(int argc, char** argv)
{
std::string file_one = "./";
file_one += "test_one";
if(boost::filesystem::is_directory(file_one))
{
boost::filesystem::remove_all(file_one);
}
boost::filesystem::create_directory(file_one);
if(boost::filesystem::is_directory(file_one))
{
std::cout << 1 << std::endl;
}
return 0;
}
第五个练习：关于pcl::io::PCD相关函数的学习。
使用的pcd文件就直接是前面学习ndt、voxel的文件，简单的使用了下pcl::io::loadPCDFile()函数和pcl::io::savePCDFile()函数的功能。
#include
#include
#include
#include
#include
#include
int main(int argc, char** argv)
{
std::string file_path = "../";
file_path += "pcl_document/room_scan1.pcd";
pcl::PointCloud::Ptr p1(new pcl::PointCloud);
pcl::io::loadPCDFile(file_path, *p1);
std::string file_path1 = "../pcl_document/one.pcd";
pcl::io::savePCDFile(file_path1, *p1);
return 0;
} 一定要注意执行文件和代码中选择存储的文件的区别，运行执行文件的时候是按执行文件的位置来判断代码中文件的路径的。