//callback method for operations that require a depth image
void depthCallback(const sensor_msgs::ImageConstPtr &msg)
{
//Load image into OpenCV
bridge_image_depth = cv_bridge::toCvCopy(msg, msg->encoding);
std::cout << "bridge_image created" << std::endl;
cvImage_depth = bridge_image_depth->image;
std::cout << "cvImage created" << std::endl;
//set cv_bridge image matrix to output and publish
getDepthThresholdImage(cvImage_depth, depthThresholdOut);
std::cout << "got depthThresholdImage" << std::endl;
bridge_image_depth->image = depthThresholdOut;
std::cout << "bridge_image->image = depthThresholdOut" << std::endl;
depthThresholdPub.publish(bridge_image_depth->toImageMsg());
//printImagePixels(depthThresholdOut, "output depth threshold");
//std::cout << depthThresholdOut << "\n*****\n" << std::endl;
//cv::Mat temp;
//depthThresholdOut.copyTo(temp);
//printImageData(depthThresholdOut, "depthThesholdOut");
if (lastImage.rows > 0) {
getImageDifference(cvImage_depth, lastImage);
bridge_image_depth->image = lastImage;
motionDetectPub.publish(bridge_image_depth->toImageMsg());
} else {
std::cout << "lastImage hasn't been created yet" << std::endl;
}
lastImage = cvImage_depth;
/*
cv::Mat depthThresholdBWOut;
//getDepthThresholdBWImage(depthThresholdOut, depthThresholdBWOut);
//printImageData(depthThresholdBWOut,"depthBW");
//depthThresholdBWOut = depthThresholdOut;
bridge_image->image = depthThresholdBWOut;
depthThresholdBWPub.publish(bridge_image->toImageMsg());
*/
//delete &depthThresholdOut;
//delete &cvImage;
/*cvReleaseMat(&cvImage);
cvReleaseMat(&depthThresholdOut);
*/
}
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;
}
