Closed jilinzhou closed 6 years ago
I noticed Function IntegratedTermMatrix() is called quite often with x = 1.0 (at least from the perspective of reference_line_provider.cc where the knot vector step is always 1.0). How about handle it specially inside this function to save some computation cycles since the element of the returned power matrix (P) is either 1.0 or 0?
Here is the code for the referencing function:
void SplineSegKernel::IntegratedTermMatrix(const uint32_t num_params,
const double x,
const std::string& type,
Eigen::MatrixXd* term_matrix) const {
if (term_matrix->rows() != term_matrix->cols() ||
term_matrix->rows() != static_cast<int>(num_params)) {
term_matrix->resize(num_params, num_params);
}
// The vector size should be 2 * num_params.
// x_pow[2*num_params] is never referenced.
std::vector<double> x_pow(2 * num_params + 1, 1.0);
for (uint32_t i = 1; i < 2 * num_params + 1; ++i) {
x_pow[i] = x_pow[i - 1] * x;
}
if (type == "fx") {
for (uint32_t r = 0; r < num_params; ++r) {
for (uint32_t c = 0; c < num_params; ++c) {
(*term_matrix)(r, c) = x_pow[r + c + 1];
}
}
} else if (type == "derivative") {
for (uint32_t r = 1; r < num_params; ++r) {
for (uint32_t c = 1; c < num_params; ++c) {
(*term_matrix)(r, c) = x_pow[r + c - 1];
}
}
(*term_matrix).block(0, 0, num_params, 1) =
Eigen::MatrixXd::Zero(num_params, 1);
(*term_matrix).block(0, 0, 1, num_params) =
Eigen::MatrixXd::Zero(1, num_params);
} else if (type == "second_order") {
for (uint32_t r = 2; r < num_params; ++r) {
for (uint32_t c = 2; c < num_params; ++c) {
(*term_matrix)(r, c) = x_pow[r + c - 3];
}
}
(*term_matrix).block(0, 0, num_params, 2) =
Eigen::MatrixXd::Zero(num_params, 2);
(*term_matrix).block(0, 0, 2, num_params) =
Eigen::MatrixXd::Zero(2, num_params);
} else {
for (uint32_t r = 3; r < num_params; ++r) {
for (uint32_t c = 3; c < num_params; ++c) {
(*term_matrix)(r, c) = x_pow[r + c - 5];
}
}
(*term_matrix).block(0, 0, num_params, 3) =
Eigen::MatrixXd::Zero(num_params, 3);
(*term_matrix).block(0, 0, 3, num_params) =
Eigen::MatrixXd::Zero(3, num_params);
}
}
As I read through the code, I found it might be helpful for developers to understand why those spline kernel matrices are initialized in such ways. The basic idea is to write them in quadratic form so quadratic programming algorithms can be applied, and at the same time they can be computed efficiently.
Here is a bit math (no guarantee of 100% correct though!)
Assume we have 5th order polynomial written in the form of f(x) = p_0 + p_1x + p_2x^2 + p_3x^3 + p_4x^4 + p_5x^5 and p = [p_0 p_1 p_2 p_3 p_4 p_5]^T (^T means transpose here)
Accordingly, the square of f(x) can be written in the matrix form of f(x)^2 = p^T A(x) p
with A(x) = [ 1 x x^2 x^3 x^4 x^5, x x^2 x^3 x^4 x^5 x^6, x^2 x^3 x^4 x^5 x^6 x^7, x^3 x^4 x^5 x^6 x^7 x^8, x^4 x^5 x^6 x^7 x^8 x^9, x^5 x^6 x^7 x^8 x^9 x^10]
Let's calculate integral of f(x)^2 now.
int (f(x)^2) = p^T int(A(x)) p
int(A(x)) = [ x x^2/2 x^3/3 x^4/4 x^5/5 x^6/6, x^2/2 x^3/3 x^4/4 x^5/5 x^6/6 x^7/7, x^3/3 x^4/4 x^5/5 x^6/6 x^7/7 x^8/8, x^4/4 x^5/5 x^6/6 x^7/7 x^8/8 x^9/9, x^5/5 x^6/6 x^7/7 x^8/8 x^9/9 x^10/10, x^6/6 x^7/7 x^8/8 x^9/9 x^10/10 x^11/11]
Now we split this matrix into two coeffcient-wise product matrices K and P where K = [ 1 1/2 1/3 1/4 1/5 1/6, 1/2 1/3 1/4 1/5 1/6 1/7, 1/3 1/4 1/5 1/6 1/7 1/8, 1/4 1/5 1/6 1/7 1/8 1/9, 1/5 1/6 1/7 1/8 1/9 1/10, 1/6 1/7 1/8 1/9 1/10 1/11]
and P = [ x x^2 x^3 x^4 x^5 x^6, x^2 x^3 x^4 x^5 x^6 x^7, x^3 x^4 x^5 x^6 x^7 x^8, x^4 x^5 x^6 x^7 x^8 x^9, x^5 x^6 x^7 x^8 x^9 x^10 x^6 x^7 x^8 x^9 x^10 x^11]
If we look at the code, K is just kernel_fx_. Similarly, we can figure out kernel_derivative_, kernel_second_derivative_, and kernel_third_derivative_.
Here is the code snippet to print to out those matrice:
And here is the output from the console: