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; }