void callback_imu(const sensor_msgs::Imu::ConstPtr& imu_reading)
{
_accel = imu_reading->linear_acceleration.y;
float h_filter_out = hipass(_accel,_cutoff());
if((h_filter_out >_accel_th()) && _active)
{
_peak.data = ros::Time::now();
_pub_imu.publish(_peak);
}
}
static int filter_get_image( mlt_frame frame, uint8_t **image, mlt_image_format *format, int *width, int *height, int writable )
{
mlt_filter filter = mlt_frame_pop_service( frame );
*format = mlt_image_rgb24;
mlt_properties_set_int( MLT_FRAME_PROPERTIES(frame), "consumer_deinterlace", 1 );
int error = mlt_frame_get_image( frame, image, format, width, height, 1 );
if ( !error && *image )
{
videostab self = filter->child;
mlt_position length = mlt_filter_get_length2( filter, frame );
int h = *height;
int w = *width;
// Service locks are for concurrency control
mlt_service_lock( MLT_FILTER_SERVICE( filter ) );
if ( !self->initialized )
{
// Initialize our context
self->initialized = 1;
self->es = es_init( w, h );
self->pos_i = (vc*) malloc( length * sizeof(vc) );
self->pos_h = (vc*) malloc( length * sizeof(vc) );
self->pos_y = (vc*) malloc( h * sizeof(vc) );
self->rs = rs_init( w, h );
}
char *vectors = mlt_properties_get( MLT_FILTER_PROPERTIES(filter), "vectors" );
if ( !vectors )
{
// Analyse
int pos = (int) mlt_filter_get_position( filter, frame );
self->pos_i[pos] = vc_add( pos == 0 ? vc_zero() : self->pos_i[pos - 1], es_estimate( self->es, *image ) );
// On last frame
if ( pos == length - 1 )
{
mlt_profile profile = mlt_service_profile( MLT_FILTER_SERVICE(filter) );
double fps = mlt_profile_fps( profile );
// Filter and store the results
hipass( self->pos_i, self->pos_h, length, fps );
serialize_vectors( self, length );
}
} else {
// Apply
if ( self->initialized != 2 )
{
// Load analysis results from property
self->initialized = 2;
deserialize_vectors( self, vectors, length );
}
if ( self->initialized == 2 )
{
// Stabilize
float shutter_angle = mlt_properties_get_double( MLT_FRAME_PROPERTIES(frame) , "shutterangle" );
float pos = mlt_filter_get_position( filter, frame );
int i;
for (i = 0; i < h; i ++)
self->pos_y[i] = interp( self->lanc_kernels,self->pos_h, length, pos + (i - h / 2.0) * shutter_angle / (h * 360.0) );
rs_resample( self->lanc_kernels,self->rs, *image, self->pos_y );
}
}
mlt_service_unlock( MLT_FILTER_SERVICE( filter ) );
}
return error;
}
int main(int argc, char *argv[]) {
int opt_shutter_angle = 0;
int opt_mjpeg_quality = 100;
int nf, i, nc, nr;
int tfs, fps;
vc *pos_i, *pos_h, *pos_y;
es_ctx *es;
rs_ctx *rs;
opterr = 0;
while ((i = getopt(argc, argv, "r:q:")) != -1) {
switch (i) {
case 'r':
opt_shutter_angle = atoi(optarg);
break;
case 'q':
opt_mjpeg_quality = atoi(optarg);
break;
default:
print_help(argv);
}
}
if (argc < optind + 2)
print_help(argv);
if (AVI_open_movie(argv[optind], &mv_in) != AVI_ERROR_NONE) {
printf("error: can't read from %s\n", argv[optind]);
return EXIT_FAILURE;
}
if (mv_in.header->Streams < 1 || mv_in.streams[0].sh.Type != AVIST_VIDEO) {
printf("error: video stream not found on %s\n", argv[optind]);
return EXIT_FAILURE;
}
if (AVI_open_compress(argv[optind + 1], &mv_out, 1, AVI_FORMAT_MJPEG) != AVI_ERROR_NONE) {
printf("error: can't write to %s\n", argv[optind + 1]);
return EXIT_FAILURE;
}
printf("status: setup\n");
prepare_lanc_kernels();
nc = mv_in.header->Width;
nr = mv_in.header->Height;
tfs = mv_in.header->TotalFrames;
fps = 1000000 / mv_in.header->MicroSecPerFrame;
pos_i = (vc *)malloc(tfs * sizeof(vc));
pos_h = (vc *)malloc(tfs * sizeof(vc));
pos_y = (vc *)malloc(nr * sizeof(vc));
AVI_set_compress_option(&mv_out, AVI_OPTION_TYPE_MAIN, 0, AVI_OPTION_WIDTH, &nc);
AVI_set_compress_option(&mv_out, AVI_OPTION_TYPE_MAIN, 0, AVI_OPTION_HEIGHT, &nr);
AVI_set_compress_option(&mv_out, AVI_OPTION_TYPE_MAIN, 0, AVI_OPTION_FRAMERATE, &fps);
AVI_set_compress_option(&mv_out, AVI_OPTION_TYPE_MAIN, 0, AVI_OPTION_QUALITY, &opt_mjpeg_quality);
es = es_init(nc, nr);
rs = rs_init(nc, nr);
printf("status: estimating\n");
for (nf = 0; nf < tfs; nf ++) {
unsigned char *fr = (unsigned char *)AVI_read_frame(&mv_in, AVI_FORMAT_RGB24, nf, 0);
pos_i[nf] = vc_add(
nf > 0 ? pos_i[nf - 1] : vc_set(0.0, 0.0),
es_estimate(es, fr)
);
free(fr);
if ((nf + 1) % 10 == 0) {
printf(".");
fflush(stdout);
}
}
printf("\nstatus: filtering\n");
hipass(pos_i, pos_h, tfs, fps / 2);
printf("status: resampling\n");
for (nf = 0; nf < tfs; nf ++) {
unsigned char *fr = (unsigned char *)AVI_read_frame(&mv_in, AVI_FORMAT_RGB24, nf, 0);
for (i = 0; i < nr; i ++) {
pos_y[i] = interp(
pos_h, tfs,
nf + (i - nr / 2.0) * opt_shutter_angle / (nr * 360.0)
);
}
rs_resample(rs, fr, pos_y);
AVI_write_frame(&mv_out, nf, AVI_FORMAT_RGB24, fr, nc * nr * 3 * sizeof(unsigned char));
if ((nf + 1) % 10 == 0) {
printf(".");
fflush(stdout);
}
}
printf("\nstatus: closing\n");
es_free(es);
rs_free(rs);
free_lanc_kernels();
AVI_close(&mv_in);
AVI_close_compress(&mv_out);
return EXIT_SUCCESS;
}