-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmain.cpp
148 lines (121 loc) · 4.95 KB
/
main.cpp
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
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
#include <QApplication>
#include <iostream>
#include "core/registration/LasFile_copy.h"
#include "core/registration/icp.h"
#include "core/registration/arti_registration.h"
int main(int argc, char *argv[])
{
QApplication a(argc, argv);
Matrix4f final_transform_matrix = Matrix4f::Identity();
//las数据读取
std::string strFilePathName = "C:\\Users\\jiaqy11\\Desktop\\registration_las\\";
std::string las1 = "039-10011014200924-154853-00377.las";
std::string las2 = "039-10011014200924-155253-00389.las";
PointCloud_T::Ptr pointCloud_target;
pointCloud_target.reset(new PointCloud_T);
PointCloud_T::Ptr pointCloud_source;
pointCloud_source.reset(new PointCloud_T);
LasFile_copy::ReadLas(strFilePathName+las1, pointCloud_source);
LasFile_copy::ReadLas(strFilePathName+las2, pointCloud_target);
std::cout << pointCloud_source->points[0].x<<std::endl;
std::cout << pointCloud_source->points[0].y<<std::endl;
registration_icp(pointCloud_source,pointCloud_target,final_transform_matrix);
//手动配准
std::vector<Vector3d> source_input_point;
std::vector<Vector3d> target_input_point;
Vector3d p1(100,100,100);
Vector3d p2(110,100,110);
Vector3d p3(110,110,100);
Vector3d p4(100,110,110);
// Vector3d p1(198185.7896,2510754.467,72.158012);
// Vector3d p2(198199.7971,2510761.976,72.217613);
// Vector3d p3(198150.5581,2510796.427,71.690308);
// Vector3d p4(197957.8393,2510942.516,69.409859);
// Vector3d p5(197874.3099,2511007.762,68.30162);
// Vector3d p6(197466.1709,2511231.661,62.068573);
// Vector3d p7(197333.267,2511272.127,60.209713);
// Vector3d p8(197245.8874,2511208.406,59.116413);
// Vector3d p9(197661.4316,2511133.433,64.967628);
// Vector3d p10(197848.3018,2511018.094,67.95533);
// Vector3d p11(198106.7492,2510817.743,71.240059);
// Vector3d p12(197241.6241,2510933.692,58.310509);
// Vector3d p13(198237.0484,2510668.271,73.287132);
// Vector3d p14(198099.4792,2510513.97,70.109764);
// Vector3d p15(197763.3733,2510663.936,66.121788);
// Vector3d p16(197675.2731,2510726.628,65.350227);
Vector3d test(10,3,2);
Matrix3d R1,R2;
R1<< 0.866,0.5,0,
-0.5,0.866,0,
0,0,1;
R2<< 1,0.001,0,
-0.001,1,0,
0,0,1;
Vector3d t1 =p1 + test;
Vector3d t2 =p2 + test;
Vector3d t3 =p3 + test;
Vector3d t4 =p4 + test;
// Vector3d t1(198185.5138,2510754.849,72.900024);
// Vector3d t2(198199.9969,2510761.985,72.987228);
// Vector3d t3(198150.8439,2510796.195,72.479225);
// Vector3d t4(197957.684,2510942.419,70.217026);
// Vector3d t5(197874.2676,2511007.65,69.100029);
// Vector3d t6(197466.4156,2511231.628,62.853928);
// Vector3d t7(197333.0673,2511272.079,60.900028);
// Vector3d t8(197245.9148,2511208.437,59.901028);
// Vector3d t9(197661.4426,2511133.387,65.733528);
// Vector3d t10(197848.3054,2511018.309,68.700027);
// Vector3d t11(198106.7282,2510817.876,72.093025);
// Vector3d t12(197241.5316,2510933.669,59.055729);
// Vector3d t13(198237.3199,2510668.536,74.10083);
// Vector3d t14(198099.5552,2510514.097,71.037727);
// Vector3d t15(197763.4707,2510663.934,67.072029);
// Vector3d t16(197675.0571,2510726.636,65.990028);
source_input_point.push_back(p1);
source_input_point.push_back(p2);
source_input_point.push_back(p3);
source_input_point.push_back(p4);
// source_input_point.push_back(p5);
// source_input_point.push_back(p6);
// source_input_point.push_back(p7);
// source_input_point.push_back(p8);
// source_input_point.push_back(p9);
// source_input_point.push_back(p10);
// source_input_point.push_back(p11);
// source_input_point.push_back(p12);
// source_input_point.push_back(p13);
// source_input_point.push_back(p14);
// source_input_point.push_back(p15);
// source_input_point.push_back(p16);
target_input_point.push_back(t1);
target_input_point.push_back(t2);
target_input_point.push_back(t3);
target_input_point.push_back(t4);
// target_input_point.push_back(t5);
// target_input_point.push_back(t6);
// target_input_point.push_back(t7);
// target_input_point.push_back(t8);
// target_input_point.push_back(t9);
// target_input_point.push_back(t10);
// target_input_point.push_back(t11);
// target_input_point.push_back(t12);
// target_input_point.push_back(t13);
// target_input_point.push_back(t14);
// target_input_point.push_back(t15);
// target_input_point.push_back(t16);
//设置区域中心点
//Vector3d center(197241.5316,2510933.669,59.055729);
Vector3d center(0,0,0);
for(auto &item:source_input_point)
{
item = item - center;
}
for(auto &item:target_input_point)
{
item = R2*(item - center);
}
//target为控制点/目标点
final_transform_matrix = Matrix4f::Identity();
calculateMatrix(source_input_point,target_input_point,5,final_transform_matrix);
return a.exec();
}