Explorer/RaspberryPi/Visualprocessing/Motionvector

The Raspberry Pi is based on a BCM2835 System on a Chip (SoC), which provides an hardware (GPU) motion estimation block to the H264 encoder.
The Coarse Motion Block (CME) compares macroblock (16×16 pixels) from two consecutive frames and output a vector where the macroblock came from.
The vector is encoded in a single 32-bit value:
- the most significant 16 bits represent a Sum of Absolute Differences (SAD = quality of the estimation, the lower is the better)
- the others 16 bits represents (8-bit signed integer) the horizontal and vertical directions 
The code below, patch "raspivid" to provide a video buffer (gray scale image) with motion vectors.
The new code compiled as "raspicv", with a custom "cv.cpp" (see below), output motion vector display to /tmp/camera3 
The "cv.cpp" is limited to 640x480 grayscale image in this sample.
(ref. Adam Heinrich thesis: An Optical Flow Odometry Sensor Based on the Raspberry Pi Computer)
 git clone https://github.com/raspberrypi/userland.git
 git clone https://github.com/adamheinrich/RaspiCV.git
Path RaspiCV/src/Makefile
 < USERLAND_DIR
 > USERLAND_DIR = $(HOME)/userland
 ...
 > $(USERLAND_DIR)/host_applications/linux/apps/raspicam/RaspiHelpers.c \
 > $(USERLAND_DIR)/host_applications/linux/apps/raspicam/RaspiCommonSettings.c \
 > $(USERLAND_DIR)/host_applications/linux/apps/raspicam/RaspiGPS.c \
 > $(USERLAND_DIR)/host_applications/linux/apps/raspicam/libgps_loader.c
 ...
 < CXXFLAGS = $(ARCHFLAGS) $(DBGFLAGS) $(OPTFLAGS) `pkg-config --cflags opencv` \
 > CXXFLAGS = $(ARCHFLAGS) $(DBGFLAGS) $(OPTFLAGS) `pkg-config --cflags opencv4` \
 ...
 < LDFLAGS += `pkg-config --libs opencv`
 > LDFLAGS += `pkg-config --libs opencv4`
 ...
 < LDLIBFLAGS = -Wl, ...
 > LDLIBFLAGS = -ldl -Wl, ...
Update RaspiCV.c
 cp ~/userland/host_applications/linux/apps/raspicam/RaspiVid.c ~/RaspiCV/src/RaspiCV.c
and patch it
 < #include <semaphore.h>
 > #include "cv.h"
 #include <semaphore.h>
 ...
 if(pData->pstate->inlineMotionVectors)
 {
 >		 cv_process_imv(buffer->data, buffer->length, buffer->pts);
 bytes_written = fwrite(buffer->data, 1, buffer->length, pData->imv_file_handle);
 ...
 if (bytes_to_write)
 {
 mmal_buffer_header_mem_lock(buffer);
 >	cv_process_img(buffer->data, bytes_to_write, buffer->pts);
 bytes_written = fwrite(buffer->data, 1, bytes_to_write, pData->raw_file_handle);
 ...
 > cv_init(state.common_settings.width, state.common_settings.height, state.framerate, state.raw_output_fmt);
 int initialCapturing=state.bCapturing;
 while (running)
 {
 ...
 ...
 destroy_splitter_component(&state);
 destroy_camera_component(&state);
 
 > cv_close();
Replace de file RaspiCV/src/cv.cpp with this
 #include "cv.h"
 #include <opencv2/opencv.hpp>
 #include <pthread.h>
 
 using namespace cv;
 using namespace std;
 
 /*****************************************************************************/
 #define SAD_LIMIT 2000
 #define streamOutGstStr "appsrc ! shmsink socket-path=/tmp/camera3 wait-for-connection=false async=false sync=false"
 
 /*****************************************************************************/
 struct motion_elt_t {
 int8_t x;
 int8_t y;
 uint16_t sad;
 };
 static motion_elt_t *motionIn;
 static Mat1b gray;
 static int width,height,fps; 
 
 static pthread_mutex_t imv_mutex;
 static bool imv_ready=false; 
 
 static pthread_t img_thread;
 static pthread_mutex_t img_mutex;
 static pthread_cond_t img_condv;
 static bool img_ready=false; 
 
 static bool init_ready=false; 
 
 /*****************************************************************************/
 static void *process_thread(void *ptr)
 {
 VideoWriter strOut = VideoWriter(streamOutGstStr,0,fps/1,Size(width,height),true); 
 Mat3b grayBGR;
 Mat colormap;
 int32_t sum_x,sum_y;
 
 int mbx = width/16;
 int mby = height/16;
 int mbxy = (8 * mbx * mby);
 unsigned int motionSize = ((mbx+1)*mby) * sizeof(struct motion_elt_t); 
 motion_elt_t *motionOut = new motion_elt_t[(mbx+1)*mby];
 
 char buff[20];int lg;
 
 while (true) {
 pthread_mutex_lock(&img_mutex);
 while (!img_ready) pthread_cond_wait(&img_condv, &img_mutex);
 cvtColor(gray, grayBGR, COLOR_GRAY2BGR);
 applyColorMap(grayBGR, colormap, COLORMAP_JET); 
 
 img_ready=false;
 pthread_mutex_unlock(&img_mutex); 
 
 if(imv_ready) { 
 pthread_mutex_lock(&imv_mutex);
 memcpy(motionOut ,motionIn, motionSize);
 imv_ready=false;
 pthread_mutex_unlock(&imv_mutex);
 } 
 
 sum_x=0;sum_y=0;
 for (int j=0;j<mby;j++) {
 for (int i=0;i<mbx;i++) { 
 motion_elt_t *vec = motionOut + (i+(mbx+1)*j); 
 if (vec->x == 0 && vec->y == 0) continue;
 if (vec->sad > SAD_LIMIT) continue;
 	 int x = i*16 + 8;
 int y = j*16 + 8;
 float intensity = vec->sad;
 intensity = round(255 * intensity / SAD_LIMIT);
 if (intensity > 255) intensity = 255;
 uint8_t *ptr = colormap.ptr<uchar>(0);
 uint8_t idx = 3*(uint8_t)intensity;
 arrowedLine(grayBGR, Point(x+vec->x, y+vec->y),
 Point(x, y),
 Scalar(ptr[idx], ptr[idx+1], ptr[idx+2])); 
 
 sum_x += (intensity * vec->x);
 sum_y += (intensity * vec->y);
 }
 } 
 
 sum_x = (sum_x/mbxy);
 sum_y = (sum_y/mbxy);
 arrowedLine(grayBGR, Point(320,240),Point(320+sum_x,240+sum_y),Scalar(0,255,0),5); 
 
 drawAxes(&grayBGR);
 
 strOut.write(grayBGR);
 }
 return((void *)0);
 }
 
 /*****************************************************************************/
 void cv_init(int w, int h, int f, int fmt)
 {
 // freopen( "/tmp/error.txt", "w", stderr );
 // cerr << length << endl;
 // fd << static_cast<int32_t>(sum_x) << endl;
 
 drawAxes_init(); 
 
 width=w;height=h;fps=f;
 
 gray = Mat(height, width, CV_8UC1);
 motionIn = new motion_elt_t[((width/16)+1) * (height/16)]; 
 
 pthread_mutex_init(&imv_mutex, NULL);
 
 pthread_mutex_init(&img_mutex, NULL);
 pthread_cond_init(&img_condv, NULL);
 
 pthread_create(&img_thread, NULL, process_thread, (void *)0);
 
 init_ready=true;
 }
 
 /*****************************************************************************/
 void cv_process_img(uint8_t *p_buffer, int length, int64_t timestamp)
 {
 if (init_ready) {
 pthread_mutex_lock(&img_mutex);
 memcpy(gray.data, p_buffer, length);		
 img_ready=true;
 pthread_cond_signal(&img_condv);
 pthread_mutex_unlock(&img_mutex);
 }
 }
 
 /*****************************************************************************/
 void cv_process_imv(uint8_t *p_buffer, int length, int64_t timestamp)
 {
 if (init_ready) {
 pthread_mutex_lock(&imv_mutex);
 memcpy(motionIn ,p_buffer, length);
 imv_ready=true;
 pthread_mutex_unlock(&imv_mutex);
 }
 }
 
 /*****************************************************************************/
 void cv_close(void)
 {
 }
 cd RaspiCV/src
 make
Explorer/RaspberryPi/Visualprocessing/Motionvector/Run