18 #include "slam6d/globals.icc"
21 double *source_alignxf,
22 double *
const *q_points,
unsigned int startindex,
unsigned int endindex,
24 int rnd,
double max_dist_match2,
double &sum,
25 double *centroid_m,
double *centroid_d)
30 double local_alignxf_inv[16];
31 M4inv(source_alignxf, local_alignxf_inv);
36 for (
unsigned int i = startindex; i < endindex; i++) {
37 if (rnd > 1 && rand(rnd) != 0)
continue;
39 t[0] = q_points[i][0];
40 t[1] = q_points[i][1];
41 t[2] = q_points[i][2];
43 transform3(local_alignxf_inv, t, s);
45 double *closest = this->
FindClosest(s, max_dist_match2, thread_num);
47 transform3(source_alignxf, closest, s);
50 centroid_m[0] += s[0];
51 centroid_m[1] += s[1];
52 centroid_m[2] += s[2];
53 centroid_d[0] += t[0];
54 centroid_d[1] += t[1];
55 centroid_d[2] += t[2];
61 myPair.
p1.
z - myPair.
p2.
z };
64 pairs->push_back(myPair);
83 double *source_alignxf,
84 const DataXYZ& xyz_r,
unsigned int startindex,
unsigned int endindex,
86 int rnd,
double max_dist_match2,
double &sum,
87 double *centroid_m,
double *centroid_d)
92 double local_alignxf_inv[16];
93 M4inv(source_alignxf, local_alignxf_inv);
98 for (
unsigned int i = startindex; i < endindex; i++) {
99 if (rnd > 1 && rand(rnd) != 0)
continue;
105 transform3(local_alignxf_inv, t, s);
107 double *closest = this->
FindClosest(s, max_dist_match2, thread_num);
109 transform3(source_alignxf, closest, s);
112 centroid_m[0] += s[0];
113 centroid_m[1] += s[1];
114 centroid_m[2] += s[2];
115 centroid_d[0] += t[0];
116 centroid_d[1] += t[1];
117 centroid_d[2] += t[2];
121 myPair.
p1.
x - myPair.
p2.
x,
122 myPair.
p1.
y - myPair.
p2.
y,
123 myPair.
p1.
z - myPair.
p2.
z };
126 pairs->push_back(myPair);