float PathNode::calculateF(PathNode* end)
{
g = calculateG(parent);
h = calculateH(end);
return f = g + h;
}
double* ModelContext::calculateIntegratedG(int j)
{
int i,k;
int p_ir_idx = j;
int nLoc = *(S -> ncol);
int nTpt = *(S -> nrow);
if (j >= nTpt)
{
lssCout << "Invalid time point: " << j << "\n";
throw(-1);
}
calculateP_IR_CPU();
double* outG = calculateG(j);
double* newG;
double pIR;
double _1mpIR_cum = 1.0;
// Need to do prediction here for later time points?
for (i = j+1; ((i < nTpt) && (_1mpIR_cum >= 1e-8)); i++)
{
newG = calculateG(i);
pIR = (p_ir)[p_ir_idx];
_1mpIR_cum *= (1-pIR);
for (k = 0; k < nLoc*nLoc; k++)
{
outG[k] += _1mpIR_cum*newG[k];
}
p_ir_idx++;
delete[] newG;
}
// If we're truncated at end of time period
// do a simple extrapolation
i = nTpt - 1;
newG = calculateG(i);
pIR = (p_ir)[p_ir_idx];
while (_1mpIR_cum >= 1e-8)
{
_1mpIR_cum *= (1-pIR);
for (k = 0; k < nLoc*nLoc; k++)
{
outG[k] += _1mpIR_cum*newG[k];
}
}
delete[] newG;
return(outG);
}
//void OrthographicGridPathfinder::addNeighbors(bool *nodesToAdd, PathNode *centerNode, PathNode *destination, list<PathNode> *openList, list<PathNode> *closedList)
void OrthographicGridPathfinder::addNeighbors(bool *nodesToAdd, PathNode *centerNode, PathNode *destination, map<int, PathNode> *openNodes, map<int, PathNode> *closedNodes)
{
int adjacencyIndex = 0;
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
bool shouldBeAdded = nodesToAdd[adjacencyIndex];
if (shouldBeAdded)
{
int col = i + centerNode->column - 1;
int row = j + centerNode->row - 1;
PathNode testNode;
testNode.column = col;
testNode.row = row;
testNode.parentNode = centerNode;
testNode.G = calculateG(testNode);
testNode.H = calculateH(testNode, destination);
// BEFORE WE ADD IT, CHECK TO SEE IF WE'VE ALREADY
// FOUND A FASTER WAY OF GETTING TO THIS NODE THAT
// IS IN THE OPEN LIST
// if (containsPathNode(openList, testNode))
int index = getGridIndex(testNode.column, testNode.row);
if (openNodes->find(index) != openNodes->end())
{
// PathNode *dup = findDupNodeInList(testNode, openList);
PathNode *dup = &openNodes->at(index);
if ((testNode.G+testNode.H) < (dup->G + dup->H))
{
// IT'S BETTER THAN WHAT'S ALREADY THERE, SO
// UPDATE THE ONE THAT'S ALREADY THERE
dup->column = testNode.column;
dup->row = testNode.row;
dup->G = testNode.G;
dup->H = testNode.H;
dup->parentNode = testNode.parentNode;
}
}
else
// openList->push_back(testNode);
(*openNodes)[index] = testNode;
}
adjacencyIndex++;
}
}
}
// An implementation of the Pyramidal Lucas-Kanade Optical Flow algorithm.
// See http://robots.stanford.edu/cs223b04/algo_tracking.pdf for details.
bool OpticalFlow::findFlowAtPoint(const float32 u_x, const float32 u_y,
float32* final_x, float32* final_y) const {
const float32 threshold_squared = square(THRESHOLD);
// Initial guess.
float32 g_x = 0.0f;
float32 g_y = 0.0f;
// For every level in the pyramid, update the coordinates of the best match.
for (int32 l = NUM_LEVELS - 1; l >= 0; --l) {
// Shrink factor from original.
const int32 shrink_factor = (1 << l);
// Images I (prev) and J (next).
const Image<uint8>& img_I = *frame1_->pyramid_[l];
const Image<uint8>& img_J = *frame2_->pyramid_[l];
// Computed gradients.
const Image<int32>& I_x = *frame1_->spatial_x_[l];
const Image<int32>& I_y = *frame1_->spatial_y_[l];
// Image position vector (p := u^l), scaled for this level.
const float32 p_x = u_x / static_cast<float32>(shrink_factor);
const float32 p_y = u_y / static_cast<float32>(shrink_factor);
// LOGV("Level %d: (%d, %d) / %d -> (%d, %d)",
// l, u_x, u_y, shrink_factor, p_x, p_y);
// Get values for frame 1. They remain constant through the inner
// iteration loop.
float32 vals_I[ARRAY_SIZE];
float32 vals_I_x[ARRAY_SIZE];
float32 vals_I_y[ARRAY_SIZE];
int32 val_idx = 0;
for (int32 win_x = -WINDOW_SIZE; win_x <= WINDOW_SIZE; ++win_x) {
for (int32 win_y = -WINDOW_SIZE; win_y <= WINDOW_SIZE; ++win_y) {
const float32 x_pos = p_x + win_x;
const float32 y_pos = p_y + win_y;
if (!img_I.validInterpPixel(x_pos, y_pos)) {
return false;
}
vals_I[val_idx] = img_I.getPixelInterp(x_pos, y_pos);
vals_I_x[val_idx] = I_x.getPixelInterp(x_pos, y_pos);
vals_I_y[val_idx] = I_y.getPixelInterp(x_pos, y_pos);
++val_idx;
}
}
// Compute the spatial gradient matrix about point p.
float32 G[] = { 0, 0, 0, 0 };
calculateG(vals_I_x, vals_I_y, ARRAY_SIZE, G);
// Find the inverse of G.
float32 G_inv[4];
if (!invert2x2(G, G_inv)) {
// If we can't invert, hope that the next level will have better luck.
continue;
}
#ifdef NORMALIZE
const float32 mean_I = computeMean(vals_I, ARRAY_SIZE);
const float32 std_dev_I = computeStdDev(vals_I, ARRAY_SIZE, mean_I);
#endif
// Iterate NUM_ITERATIONS times or until we converge.
for (int32 iteration = 0; iteration < NUM_ITERATIONS; ++iteration) {
// Get values for frame 2.
float32 vals_J[ARRAY_SIZE];
int32 val_idx = 0;
for (int32 win_x = -WINDOW_SIZE; win_x <= WINDOW_SIZE; ++win_x) {
for (int32 win_y = -WINDOW_SIZE; win_y <= WINDOW_SIZE; ++win_y) {
const float32 x_pos = p_x + win_x + g_x;
const float32 y_pos = p_y + win_y + g_y;
if (!img_I.validInterpPixel(x_pos, y_pos)) {
return false;
}
vals_J[val_idx] = img_J.getPixelInterp(x_pos, y_pos);
++val_idx;
}
}
#ifdef NORMALIZE
const float32 mean_J = computeMean(vals_J, ARRAY_SIZE);
const float32 std_dev_J = computeStdDev(vals_J, ARRAY_SIZE, mean_J);
const float32 std_dev_ratio = std_dev_I / std_dev_J;
#endif
// Compute image mismatch vector.
float32 b_x = 0.0f;
float32 b_y = 0.0f;
val_idx = 0;
for (int32 win_x = -WINDOW_SIZE; win_x <= WINDOW_SIZE; ++win_x) {
for (int32 win_y = -WINDOW_SIZE; win_y <= WINDOW_SIZE; ++win_y) {
// Normalized Image difference.
#ifdef NORMALIZE
const float32 dI = (vals_I[val_idx] - mean_I) -
(vals_J[val_idx] - mean_J) * std_dev_ratio;
#else
const float32 dI = vals_I[val_idx] - vals_J[val_idx];
#endif
b_x += dI * vals_I_x[val_idx];
b_y += dI * vals_I_y[val_idx];
++val_idx;
}
}
// Optical flow... solve n = G^-1 * b
const float32 n_x = (G_inv[0] * b_x) + (G_inv[1] * b_y);
const float32 n_y = (G_inv[2] * b_x) + (G_inv[3] * b_y);
// Update best guess with residual displacement from this level and
// iteration.
g_x += n_x;
g_y += n_y;
// LOGV("Iteration %d: delta (%.3f, %.3f)", iteration, n_x, n_y);
// Abort early if we're already below the threshold.
if (square(n_x) + square(n_y) < threshold_squared) {
break;
}
} // Iteration.
if (l > 0) {
// Every lower level of the pyramid is 2x as large dimensionally.
g_x = 2.0f * g_x;
g_y = 2.0f * g_y;
}
} // Level.
// LOGV("Final displacement for feature %d was (%.2f, %.2f)",
// iFeat, g_x, g_y);
*final_x = u_x + g_x;
*final_y = u_y + g_y;
// Assign the best guess, if we're still in the image.
if (frame1_->pyramid_[0]->validInterpPixel(*final_x, *final_y)) {
return true;
} else {
return false;
}
}
void AStarNode::update() {
calculateH();
calculateG();
}
int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int *p_x, int *p_y, int n_found_points,
int imW, int imH, int *new_x, int *new_y, int *status, int half_window_size, int max_iterations)
{
// A straightforward one-level implementation of Lucas-Kanade.
// For all points:
// (1) determine the subpixel neighborhood in the old image
// (2) get the x- and y- gradients
// (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
// (4) iterate over taking steps in the image to minimize the error:
// [a] get the subpixel neighborhood in the new image
// [b] determine the image difference between the two neighborhoods
// [c] calculate the 'b'-vector
// [d] calculate the additional flow step and possibly terminate the iteration
int p, subpixel_factor, x, y, it, step_threshold, step_x, step_y, v_x, v_y, Det;
int b_x, b_y, patch_size, padded_patch_size, error;
unsigned int ix1, ix2;
int *I_padded_neighborhood; int *I_neighborhood; int *J_neighborhood;
int *DX; int *DY; int *ImDiff; int *IDDX; int *IDDY;
int G[4];
int error_threshold;
// set the image width and height
IMG_WIDTH = imW;
IMG_HEIGHT = imH;
// spatial resolution of flow is 1 / subpixel_factor
subpixel_factor = 10;
// determine patch sizes and initialize neighborhoods
patch_size = (2 * half_window_size + 1);
error_threshold = (25 * 25) * (patch_size * patch_size);
padded_patch_size = (2 * half_window_size + 3);
I_padded_neighborhood = (int *) malloc(padded_patch_size * padded_patch_size * sizeof(int));
I_neighborhood = (int *) malloc(patch_size * patch_size * sizeof(int));
J_neighborhood = (int *) malloc(patch_size * patch_size * sizeof(int));
if (I_padded_neighborhood == 0 || I_neighborhood == 0 || J_neighborhood == 0) {
return NO_MEMORY;
}
DX = (int *) malloc(patch_size * patch_size * sizeof(int));
DY = (int *) malloc(patch_size * patch_size * sizeof(int));
IDDX = (int *) malloc(patch_size * patch_size * sizeof(int));
IDDY = (int *) malloc(patch_size * patch_size * sizeof(int));
ImDiff = (int *) malloc(patch_size * patch_size * sizeof(int));
if (DX == 0 || DY == 0 || ImDiff == 0 || IDDX == 0 || IDDY == 0) {
return NO_MEMORY;
}
for (p = 0; p < n_found_points; p++) {
// status: point is not yet lost:
status[p] = 1;
// We want to be able to take steps in the image of 1 / subpixel_factor:
p_x[p] *= subpixel_factor;
p_y[p] *= subpixel_factor;
// if the pixel is outside the ROI in the image, do not track it:
if (!(p_x[p] > ((half_window_size + 1) * subpixel_factor) && p_x[p] < (IMG_WIDTH - half_window_size) * subpixel_factor
&& p_y[p] > ((half_window_size + 1) * subpixel_factor) && p_y[p] < (IMG_HEIGHT - half_window_size)*subpixel_factor)) {
status[p] = 0;
}
// (1) determine the subpixel neighborhood in the old image
// we determine a padded neighborhood with the aim of subsequent gradient processing:
getSubPixel_gray(I_padded_neighborhood, old_image_buf, p_x[p], p_y[p], half_window_size + 1, subpixel_factor);
// Also get the original-sized neighborhood
for (x = 1; x < padded_patch_size - 1; x++) {
for (y = 1; y < padded_patch_size - 1; y++) {
ix1 = (y * padded_patch_size + x);
ix2 = ((y - 1) * patch_size + (x - 1));
I_neighborhood[ix2] = I_padded_neighborhood[ix1];
}
}
// (2) get the x- and y- gradients
getGradientPatch(I_padded_neighborhood, DX, DY, half_window_size);
// (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
error = calculateG(G, DX, DY, half_window_size);
if (error == NO_MEMORY) { return NO_MEMORY; }
for (it = 0; it < 4; it++) {
G[it] /= 255; // to keep values in range
}
// calculate G's determinant:
Det = G[0] * G[3] - G[1] * G[2];
Det = Det / subpixel_factor; // so that the steps will be expressed in subpixel units
if (Det < 1) {
status[p] = 0;
}
// (4) iterate over taking steps in the image to minimize the error:
it = 0;
step_threshold = 2; // 0.2 as smallest step (L1)
v_x = 0;
v_y = 0;
step_x = step_threshold + 1;
step_y = step_threshold + 1;
while (status[p] == 1 && it < max_iterations && (abs(step_x) >= step_threshold || abs(step_y) >= step_threshold)) {
// if the pixel goes outside the ROI in the image, stop tracking:
if (!(p_x[p] + v_x > ((half_window_size + 1) * subpixel_factor)
&& p_x[p] + v_x < ((int)IMG_WIDTH - half_window_size) * subpixel_factor
&& p_y[p] + v_y > ((half_window_size + 1) * subpixel_factor)
&& p_y[p] + v_y < ((int)IMG_HEIGHT - half_window_size)*subpixel_factor)) {
status[p] = 0;
break;
}
// [a] get the subpixel neighborhood in the new image
// clear J:
for (x = 0; x < patch_size; x++) {
for (y = 0; y < patch_size; y++) {
ix2 = (y * patch_size + x);
J_neighborhood[ix2] = 0;
}
}
getSubPixel_gray(J_neighborhood, new_image_buf, p_x[p] + v_x, p_y[p] + v_y, half_window_size, subpixel_factor);
// [b] determine the image difference between the two neighborhoods
getImageDifference(I_neighborhood, J_neighborhood, ImDiff, patch_size, patch_size);
error = calculateError(ImDiff, patch_size, patch_size) / 255;
if (error > error_threshold && it > max_iterations / 2) {
status[p] = 0;
break;
}
multiplyImages(ImDiff, DX, IDDX, patch_size, patch_size);
b_x = getSumPatch(IDDX, patch_size) / 255;
b_y = getSumPatch(IDDY, patch_size) / 255;
//printf("b_x = %d; b_y = %d;\n\r", b_x, b_y);
// [d] calculate the additional flow step and possibly terminate the iteration
step_x = (G[3] * b_x - G[1] * b_y) / Det;
step_y = (G[0] * b_y - G[2] * b_x) / Det;
v_x += step_x;
v_y += step_y; // - (?) since the origin in the image is in the top left of the image, with y positive pointing down
// next iteration
it++;
} // iteration to find the right window in the new image
new_x[p] = (p_x[p] + v_x) / subpixel_factor;
new_y[p] = (p_y[p] + v_y) / subpixel_factor;
p_x[p] /= subpixel_factor;
p_y[p] /= subpixel_factor;
}
// free all allocated variables:
free((int *) I_padded_neighborhood);
free((int *) I_neighborhood);
free((int *) J_neighborhood);
free((int *) DX);
free((int *) DY);
free((int *) ImDiff);
free((int *) IDDX);
free((int *) IDDY);
// no errors:
return OK;
}
void NodeAstar::calculateScores(NodeAstar* finish)
{
G = calculateG(parent);
H = calculateH(finish);
F = G + H;
}
