mirror of
https://github.com/JHUAPL/Terrasaur.git
synced 2026-01-09 22:37:58 -05:00
68 lines
1.3 KiB
Plaintext
68 lines
1.3 KiB
Plaintext
# This script generates C code for the partial derivatives of the
|
|
# objective function used by the lidar point optimizer. The objective
|
|
# function is the sum of squared differences between the target points
|
|
# and the transformed source points. This script only computes the
|
|
# partial derivatives for one term of the sum. After running this
|
|
# script, the generated C code should be copied into the loop that
|
|
# computes the sum of squared difference between the points. See
|
|
# the file icp-gsl.cpp in the function 'grad' to see how this was
|
|
# done.
|
|
|
|
p0 = q0;
|
|
p1 = q1;
|
|
p2 = q2;
|
|
p3 = q3;
|
|
|
|
inv = 1.0 / sqrt(p0 * p0 + p1 * p1 + p2 * p2 + p3 * p3);
|
|
p0 = p0 * inv;
|
|
p1 = p1 * inv;
|
|
p2 = p2 * inv;
|
|
p3 = p3 * inv;
|
|
|
|
s0 = s0 - cor0;
|
|
s1 = s1 - cor1;
|
|
s2 = s2 - cor2;
|
|
|
|
a = s0;
|
|
b = s1;
|
|
c = s2;
|
|
|
|
w = p1 * a + p2 * b + p3 * c;
|
|
s0 = 2.0 * (p0 * (a * p0 - (p2 * c - p3 * b)) + w * p1) - a;
|
|
s1 = 2.0 * (p0 * (b * p0 - (p3 * a - p1 * c)) + w * p2) - b;
|
|
s2 = 2.0 * (p0 * (c * p0 - (p1 * b - p2 * a)) + w * p3) - c;
|
|
|
|
s0 = s0 + cor0;
|
|
s1 = s1 + cor1;
|
|
s2 = s2 + cor2;
|
|
|
|
s0 = s0 + x * m;
|
|
s1 = s1 + y * m;
|
|
s2 = s2 + z * m;
|
|
|
|
d2 = ( s0 - t0 )^2 + ( s1 - t1 )^2 + ( s2 - t2 )^2;
|
|
|
|
0;
|
|
0;
|
|
print_csrc(diff(d2,x));
|
|
0;
|
|
0;
|
|
print_csrc(diff(d2,y));
|
|
0;
|
|
0;
|
|
print_csrc(diff(d2,z));
|
|
0;
|
|
0;
|
|
print_csrc(diff(d2,q0));
|
|
0;
|
|
0;
|
|
print_csrc(diff(d2,q1));
|
|
0;
|
|
0;
|
|
print_csrc(diff(d2,q2));
|
|
0;
|
|
0;
|
|
print_csrc(diff(d2,q3));
|
|
0;
|
|
0;
|