​ 声明头文件;

​ 实例化对象;

​ 打开文件;

​ 进行执写操作;

​ 关闭。

  1. 定义读取文件对象 std::ifstream pose_ifs(pose_path);
  2. 判断文件是否打开,若未打开,报错;
  3. 定义字符串line;
  4. 定义点云指针cloud;pcl::PointCloudpcl::PointXYZI::Ptr cloud(new pcl::PointCloudpcl::PointXYZI);
  5. 读取每行字符串;
  6. 读取每行中的每个数字;
  7. 追加到预先定义的点云中;
  8. 声明kdtree, 设置kdtree输入参数;
  9. 遍历每个点,设置搜索半径,定义两个向量ixi和ixf,用于存储搜索到的点的索引和对应的距离。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
std::vector<vector<int>> getGTFromPose(const string& pose_path)
{
std::ifstream pose_ifs(pose_path);


if (!pose_ifs.is_open())
{
std::cerr << "Error opening pose file: " << pose_path << std::endl;
return std::vector<std::vector<int>>();
}


std::string line;
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);

int index = 1;
while(getline(pose_ifs, line))
{
if(line.empty()) {
cout<<"空"<<endl;
break;
}

stringstream ss(line);
float r1,r2,r3,t1,r4,r5,r6,t2,r7,r8,r9,t3;
ss >> r1 >> r2 >> r3 >> t1 >> r4 >> r5 >> r6 >> t2 >> r7 >> r8 >> r9 >> t3;
pcl::PointXYZI p;
p.x = t1;
p.y = 0;
p.z = t3;
p.intensity = index++;
cout << p << endl;
cloud->push_back(p);
}
cout<<cloud->points.size()<<endl;
pcl::io::savePCDFileASCII(seq + ".pcd", *cloud);
pcl::KdTreeFLANN<pcl::PointXYZI> kdtree;
kdtree.setInputCloud(cloud);
std::vector<vector<int>> res(N+1);
for(int i = 0; i < cloud->points.size(); i++)
{
float radius = 4;
std::vector<int> ixi;
std::vector<float> ixf;
pcl::PointXYZI p = cloud->points[i];
int cur = p.intensity;
std::vector<int> nrs;
kdtree.radiusSearch(p,radius,ixi,ixf);
for(int j = 0; j < ixi.size(); j++)
{
if(cloud->points[ixi[j]].intensity == cur) continue;
nrs.push_back(cloud->points[ixi[j]].intensity);
}
sort(nrs.begin(), nrs.end());
res[cur] = nrs;
}

std::ofstream gt_ofs(seq + "gt" + ".txt");

for(int i =1; i < res.size(); i++)
{
gt_ofs << i << " ";
for(int j = 0; j < res[i].size(); j++)
{
gt_ofs << res[i][j] << " ";
}
gt_ofs << endl;
}
return res;
}