// out := centroid of images in[n] from image ref
void centroid(float *out, float **in, int n, float *ref, int w, int h, int pd)
{
// storage for optical flows
float *flow[n];
for (int i = 0; i < n; i++)
flow[i] = xmalloc(w * h * 2 * sizeof(float));
float *flow_avg = xmalloc(w * h * 2 * sizeof(float));
float *flow_iavg= xmalloc(w * h * 2 * sizeof(float));
// compute all optical flows
for (int i = 0; i < n; i++)
{
fprintf(stderr, "flow %d/%d", i+1, n);
optical_flow(flow[i], ref, in[i], w, h, pd);
fprintf(stderr, "\n");
}
// compute average
for (int i = 0; i < 2*w*h; i++)
flow_avg[i] = 0;
for (int j = 0; j < n; j++)
for (int i = 0; i < 2*w*h; i++)
flow_avg[i] += flow[j][i] / n;
// push-forward the reference image by average flow
fprintf(stderr, "warping...\n");
field_invert(flow_iavg, flow_avg, w, h);
field_pull_back(out, flow_iavg, ref, w, h, pd);
// cleanup
free(flow_avg);
free(flow_iavg);
for (int i = 0; i < n; i++)
free(flow[i]);
}
void send_flow_cb(const mavros_extras::OpticalFlow::ConstPtr flow_msg) {
optical_flow(flow_msg->header.stamp.toNSec() / 1000,
0, /* maybe we need parameter? */
flow_msg->flow_y,
flow_msg->flow_x,
flow_msg->flow_comp_m_y,
flow_msg->flow_comp_m_x,
flow_msg->quality,
flow_msg->ground_distance);
}
void MainWindow::on_comboBox_currentIndexChanged(int index)
{
cbox_index=index;
if(cbox_index==0)
MainWindow::FrameDiff();
else
if(cbox_index==1)
MainWindow::BackgroundDiff();
else
if(cbox_index==2)
optical_flow();
// MainWindow::OpticalFlowDetect();
//MainWindow::OpticalFlow();
// MainWindow::OpencvOpticalFlow();
}
void mexFunction( int nl, mxArray *pl[], int nr, const mxArray *pr[] ) {
if( nr==0 ){
usage(MATLAB_OPTIONS);
return;
}
if ( nl != 1){
usage(MATLAB_OPTIONS);
mexErrMsgTxt("error: returns one output");
return;
}
if( nr < 2 || nr > 4){
usage(MATLAB_OPTIONS);
mexErrMsgTxt("error: takes two to four inputs");
return;
}
// The code is originally written for C-order arrays.
// We thus transpose all arrays in this mex-function which is not efficient...
const int *pDims;
if( mxGetNumberOfDimensions(pr[0]) != 3 ) mexErrMsgTxt("input images must have 3 dimensions");
if( !mxIsClass(pr[0], "single") ) mexErrMsgTxt("input images must be single");
pDims = mxGetDimensions(pr[0]);
if( pDims[2]!=3 ) mexErrMsgTxt("input images must have 3 channels");
const int h = pDims[0], w = pDims[1];
color_image_t *im1 = input3darray_to_color_image( pr[0] );
if( mxGetNumberOfDimensions(pr[1]) != 3 ) mexErrMsgTxt("input images must have 3 dimensions");
if( !mxIsClass(pr[1], "single") ) mexErrMsgTxt("input images must be single");
pDims = mxGetDimensions(pr[1]);
if( pDims[0]!=h || pDims[1]!=w || pDims[2]!=3) mexErrMsgTxt( "input images must have the same size" );
color_image_t *im2 = input3darray_to_color_image( pr[1] );
image_t *match_x = NULL, *match_y = NULL, *match_z = NULL;
if( nr>2 && !mxIsEmpty(pr[2]) ){
if( mxGetNumberOfDimensions(pr[2]) != 2 ) mexErrMsgTxt("input matches must be a 2d-matrix");
if( !mxIsClass(pr[2], "single")) mexErrMsgTxt("input matches must be single");
pDims = mxGetDimensions(pr[1]);
if( pDims[1]<4) mexErrMsgTxt( "input matches must have at least 4 columns: x1 y1 x2 y2" );
match_x = image_new(w, h); match_y = image_new(w, h); match_z = image_new(w, h);
input2darray_to_matches( match_x, match_y, match_z, pr[2]);
}
// set params to default
optical_flow_params_t* params = (optical_flow_params_t*) malloc(sizeof(optical_flow_params_t));
if(!params){
fprintf(stderr,"error deepflow2(): not enough memory\n");
exit(1);
}
optical_flow_params_default(params);
// read options
if( nr > 3 ){
char *options = mxArrayToString(pr[3]);
if( !options ) mexErrMsgTxt("Fourth parameter must be a string");
int argc=0;
char* argv[256];
argv[argc]=strtok(options," ");
while(argv[argc]!=NULL)
{
argv[++argc]=strtok(NULL," ");
}
parse_options(params, argc, argv, MATLAB_OPTIONS, w, h);
}
image_t *wx = image_new(im1->width,im1->height), *wy = image_new(im1->width,im1->height);
optical_flow(wx, wy, im1, im2, params, match_x, match_y, match_z);
int dims[3] = {h,w,2};
pl[0] = mxCreateNumericArray(3, dims, mxSINGLE_CLASS, mxREAL);
flow_to_output3darray(wx, wy, pl[0]);
image_delete(wx);
image_delete(wy);
image_delete(match_x); image_delete(match_y); image_delete(match_z);
color_image_delete(im1); color_image_delete(im2);
free(params);
}
int main(int argc, char **argv) {
/*********/
/* INPUT */
/*********/
if (argc < 3)
throw std::runtime_error(
"Usage: ./cmd_line_optical_flow <input.h5> <output.h5>");
// create files
util::HDF5File in_file(argv[1]);
util::HDF5File out_file(argv[2]);
std::vector<int> image0_size, image1_size;
std::vector<float> image0_data, image1_data;
in_file.readArray("image0", image0_data, image0_size);
in_file.readArray("image1", image1_data, image1_size);
if (image0_size.size() != 2)
throw std::runtime_error("Expecting 2D float image");
if (image0_size != image1_size)
throw std::runtime_error("Expecting equal size images");
// default arguments
vision::D_OpticalAndARFlow::Parameters parameters;
parameters.n_scales_ =
fetchScalar<int>(in_file, "n_scales", parameters.n_scales_);
parameters.median_filter_ = (bool)fetchScalar<int>(in_file, "median_filter",
parameters.median_filter_);
parameters.consistent_ =
(bool)fetchScalar<int>(in_file, "consistent", parameters.consistent_);
parameters.cons_thres_ =
fetchScalar<float>(in_file, "cons_thres", parameters.cons_thres_);
parameters.four_orientations_ = fetchScalar<int>(
in_file, "four_orientations", parameters.four_orientations_);
// time execution?
bool timing = (bool)fetchScalar<int>(in_file, "timing", false);
/***********/
/* PROCESS */
/***********/
int width = image0_size.at(1);
int height = image0_size.at(0);
// this run also serves as warm-up for the timing code
util::Device2D<float> d_image0(width, height);
d_image0.copyFrom(image0_data);
util::Device2D<float> d_image1(width, height);
d_image1.copyFrom(image1_data);
vision::D_OpticalAndARFlow optical_flow(d_image0, parameters);
optical_flow.addImageReal(d_image1);
optical_flow.updateOpticalFlowReal();
// output already since timing will overwrite
auto &flow_x = optical_flow.getOpticalFlowX();
std::vector<int> h_size{ height, width };
std::vector<float> h_data(h_size.at(0) * h_size.at(1));
flow_x.copyTo(h_data);
out_file.writeArray("optical_flow_x", h_data, h_size);
auto &flow_y = optical_flow.getOpticalFlowY();
flow_y.copyTo(h_data);
out_file.writeArray("optical_flow_y", h_data, h_size);
// timing
float image_copy_time, gabor_time, flow_time;
if (timing) {
int n_reps = 10;
util::TimerGPU timer;
// image copy
timer.reset();
for (int r = 0; r < n_reps; r++)
d_image1.copyFrom(image1_data);
image_copy_time = timer.read() / (float)n_reps;
// gabor filtering
timer.reset();
for (int r = 0; r < n_reps; r++)
optical_flow.addImageReal(d_image1);
gabor_time = timer.read() / (float)n_reps;
// optical flow
timer.reset();
for (int r = 0; r < n_reps; r++)
optical_flow.updateOpticalFlowReal();
flow_time = timer.read() / (float)n_reps;
// output timers
out_file.writeScalar("image_copy_time", image_copy_time);
out_file.writeScalar("gabor_time", gabor_time);
out_file.writeScalar("flow_time", flow_time);
}
return EXIT_SUCCESS;
}
int main(int argc, char ** argv){
image_t *match_x = NULL, *match_y = NULL, *match_z = NULL;
// load images
if(argc < 4){
fprintf(stderr,"Wrong command, require at least 3 arguments.\n\n");
usage();
exit(1);
}
color_image_t *im1 = color_image_load(argv[1]), *im2 = color_image_load(argv[2]);
if(im1->width != im2->width || im1->height != im2->height){
fprintf(stderr,"Image dimensions does not match\n");
exit(1);
}
// set params to default
optical_flow_params_t* params = (optical_flow_params_t*) malloc(sizeof(optical_flow_params_t));
if(!params){
fprintf(stderr,"error deepflow(): not enough memory\n");
exit(1);
}
optical_flow_params_default(params);
// parse options
int current_arg = 4;
while(1){
if( current_arg >= argc) break;
if(!strcmp(argv[current_arg],"-h") || !strcmp(argv[current_arg],"--help") ){
usage();
exit(1);
}else if(!strcmp(argv[current_arg],"-a") || !strcmp(argv[current_arg],"-alpha") ){
current_arg++;
if(current_arg >= argc) require_argument("alpha");
float alpha = atof(argv[current_arg++]);
if(alpha<0){
fprintf(stderr,"Alpha argument cannot be negative\n");
exit(1);
}
params->alpha = alpha;
}else if(!strcmp(argv[current_arg],"-b") || !strcmp(argv[current_arg],"-beta") ){
current_arg++;
if(current_arg >= argc) require_argument("beta");
float beta = atof(argv[current_arg++]);
if(beta<0){
fprintf(stderr,"Beta argument cannot be negative\n");
exit(1);
}
params->beta = beta;
}else if(!strcmp(argv[current_arg],"-g") || !strcmp(argv[current_arg],"-gamma") ){
current_arg++;
if(current_arg >= argc) require_argument("gamma");
float gamma = atof(argv[current_arg++]);
if(gamma<0){
fprintf(stderr,"Gamma argument cannot be negative\n");
exit(1);
}
params->gamma = gamma;
}else if(!strcmp(argv[current_arg],"-d") || !strcmp(argv[current_arg],"-delta") ){
current_arg++;
if(current_arg >= argc) require_argument("delta");
float delta = atof(argv[current_arg++]);
if(delta<0) {
fprintf(stderr,"Delta argument cannot be negative\n");
exit(1);
}
params->delta = delta;
}else if(!strcmp(argv[current_arg],"-s") || !strcmp(argv[current_arg],"-sigma") ){
current_arg++;
if(current_arg >= argc) require_argument("sigma");
float sigma = atof(argv[current_arg++]);
if(sigma<0){
fprintf(stderr,"Sigma argument is negative\n");
exit(1);
}
params->sigma = sigma;
}else if(!strcmp(argv[current_arg],"-bk")) {
current_arg++;
if(current_arg >= argc) require_argument("bk");
float betak = atof(argv[current_arg++]);
if(betak<0.0f){
fprintf(stderr,"Bk argument must be positive\n");
exit(1);
}
params->bk = betak;
}else if(!strcmp(argv[current_arg],"-e") || !strcmp(argv[current_arg],"-eta") ){
current_arg++;
if(current_arg >= argc) require_argument("eta");
float eta = atof(argv[current_arg++]);
if(eta<0.25 || eta>0.98){
fprintf(stderr,"Eta argument has to be between 0.25 and 0.98\n");
exit(1);
}
params->eta = eta;
}else if( !strcmp(argv[current_arg],"-minsize") ){
current_arg++;
if(current_arg >= argc) require_argument("minsize");
int minsize = atoi(argv[current_arg++]);
if(minsize < 10){
fprintf(stderr,"Minsize argument has to be higher than 10\n");
exit(1);
}
params->min_size = minsize;
}else if(!strcmp(argv[current_arg],"-inner") ){
current_arg++;
if(current_arg >= argc) require_argument("inner");
int inner = atoi(argv[current_arg++]);
if(inner<=0){
fprintf(stderr,"Inner argument must be strictly positive\n");
exit(1);
}
params->n_inner_iteration = inner;
}else if(!strcmp(argv[current_arg],"-iter") ){
current_arg++;
if(current_arg >= argc) require_argument("iter");
int iter = atoi(argv[current_arg++]);
if(iter<=0){
fprintf(stderr,"Iter argument must be strictly positive\n");
exit(1);
}
params->n_solver_iteration = iter;
}else if( !strcmp(argv[current_arg],"-match") || !strcmp(argv[current_arg],"-matchf")){
int wm = im1->width, hm = im1->height;
if( !strcmp(argv[current_arg++],"-match") ){
wm = 512;
hm = 256;
}
image_delete(match_x); image_delete(match_y); image_delete(match_z);
match_x = image_new(wm, hm); match_y = image_new(wm, hm); match_z = image_new(wm, hm);
image_erase(match_x); image_erase(match_y); image_erase(match_z);
FILE *fid = stdin;
if( current_arg<argc && argv[current_arg][0] != '-'){
fid = fopen(argv[current_arg++], "r");
if(fid==NULL){
fprintf(stderr, "Cannot read matches from file %s", argv[current_arg-1]);
exit(1);
}
}
int x1, x2, y1, y2;
float score;
while(!feof(fid) && fscanf(fid, "%d %d %d %d %f\n", &x1, &y1, &x2, &y2, &score)==5){
if( x1<0 || y1<0 || x2<0 || y2<0 || x1>=wm || y1>=hm || x2>=wm || y2>=hm){
fprintf(stderr, "Error while reading matches %d %d -> %d %d, out of bounds\n", x1, y1, x2, y2);
exit(1);
}
match_x->data[ y1*match_x->stride+x1 ] = (float) (x2-x1);
match_y->data[ y1*match_x->stride+x1 ] = (float) (y2-y1);
match_z->data[ y1*match_x->stride+x1 ] = score;
}
}else if ( !strcmp(argv[current_arg],"-sintel") ){
current_arg++;
optical_flow_params_sintel(params);
}else if ( !strcmp(argv[current_arg],"-middlebury") ){
current_arg++;
optical_flow_params_middlebury(params);
}else if ( !strcmp(argv[current_arg],"-kitti") ){
current_arg++;
optical_flow_params_kitti(params);
}else{
if(argv[current_arg][0] == '-'){
fprintf(stderr,"Unknow options %s\n",argv[current_arg]);
}else{
fprintf(stderr,"Error while reading options, %s\n",argv[current_arg]);
}
exit(1);
}
}
image_t *wx = image_new(im1->width,im1->height), *wy = image_new(im1->width,im1->height);
optical_flow(wx, wy, im1, im2, params, match_x, match_y, match_z);
writeFlowFile(argv[3], wx, wy);
image_delete(wx);
image_delete(wy);
image_delete(match_x); image_delete(match_y); image_delete(match_z);
color_image_delete(im1); color_image_delete(im2);
free(params);
return 0;
}
bool Classifier::run(const IplImage *frame, CObjectList *objects, bool scored)
{
double xDiff = 0, yDiff = 0;
optical_flow(frame, &xDiff, &yDiff);
totalXDiff += xDiff;
totalYDiff += yDiff;
if (!scored)
return true;
cout << "--------------------------------------" << endl;
cout << "\t\tRun" << endl;
assert((frame != NULL) && (objects != NULL));
printf("Let's go!\n");
for (int i = 0; i < (int)prevObjects.size(); ++i) {
if (prevObjects[i].rect.x > -20 && prevObjects[i].rect.x < frame->width
&& prevObjects[i].rect.y > -20 && prevObjects[i].rect.y < frame->height) {
objects->push_back(prevObjects[i]);
cout << prevObjects[i].label << " is now at (" << prevObjects[i].rect.x << ", " << prevObjects[i].rect.y << ")" << endl;
}
}
//printf("HEY OPTICAL FLOW!!!! %f %f\n", totalXDiff, totalYDiff);
// move old objects
for (int i = 0; i < (int)objects->size(); ++i) {
(*objects)[i].rect.x -= totalXDiff * 3;
(*objects)[i].rect.y -= totalYDiff * 3;
}
cout << "Flow: " << totalXDiff << " " << totalYDiff << endl;
totalYDiff = 0;
totalXDiff = 0;
// Convert to grayscale.
IplImage *gray = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 1);
cvCvtColor(frame, gray, CV_BGR2GRAY);
// Resize by half first, as per the handout.
double scale = 2.0;
IplImage *dst = cvCreateImage(cvSize(gray->width / scale, gray->height / scale), gray->depth, gray->nChannels);
cvResize(gray, dst);
printf("About to do SURF\n");
CvSeq *keypoints = 0, *descriptors = 0;
CvSURFParams params = cvSURFParams(100, SURF_SIZE == 128);
cvExtractSURF(dst, 0, &keypoints, &descriptors, storage, params);
cout << "desc: " << descriptors->total << endl;
if (descriptors->total == 0) return false;
vector<float> desc;
desc.resize(descriptors->total * descriptors->elem_size/sizeof(float));
cvCvtSeqToArray(descriptors, &desc[0]);
vector<CvSURFPoint> keypts;
keypts.resize(keypoints->total);
cvCvtSeqToArray(keypoints, &keypts[0]);
vector<float *> features;
int where = 0;
for (int pt = 0; pt < keypoints->total; ++pt) {
float *f = new float[SURF_SIZE];
for (int j = 0; j < SURF_SIZE; ++j) {
f[j] = desc[where];
++where;
}
features.push_back(f);
}
printf("Done SURF\n");
printf("Clustering...\n");
vector<int> cluster(features.size());
for (int i = 0; i < (int)features.size(); ++i) {
cluster[i] = best_cluster(centers, features[i]);
}
printf("Done clustering...\n");
vector<FoundObject> newObjects;
run_boxscan(dst, cluster, keypts, features, newObjects, objects);
for (int i = 0; i < (int)newObjects.size(); ++i) {
if (newObjects[i].object.rect.x > -20 && newObjects[i].object.rect.x < frame->width
&& newObjects[i].object.rect.y > -20 && newObjects[i].object.rect.y < frame->height) {
objects->push_back(newObjects[i].object);
cout << "Found object: " << newObjects[i].object.label << " at (" ;
cout << newObjects[i].object.rect.x << ", " << newObjects[i].object.rect.y << ")" << endl;
}
}
prevObjects = *objects;
cvReleaseImage(&gray);
return true;
}
