Difference between revisions of "Explorer/RaspberryPi/Visualprocessing/Motionvector"

From PaparazziUAV
Jump to navigation Jump to search
 
(9 intermediate revisions by the same user not shown)
Line 6: Line 6:


The code below, patch "raspivid" to provide a video buffer (gray scale image) with motion vectors.<br />
The code below, patch "raspivid" to provide a video buffer (gray scale image) with motion vectors.<br />
The new code compile as "raspicv", with a custom "cv.cpp" (see below), output video stream with motion vector display to /tmp/camera3  
The new code compiled as "raspicv", with a custom "cv.cpp" (see below), output motion vector display to /tmp/camera3 <br />
The "cv.cpp" is limited to 640x480 grayscale image in this sample.
The "cv.cpp" is limited to 640x480 grayscale image in this sample.<br />


Adam Heinrich thesis
(ref. Adam Heinrich thesis: An Optical Flow Odometry Sensor Based on the Raspberry Pi Computer)
An Optical Flow Odometry Sensor Based on the Raspberry Pi Computer


  git clone https://github.com/raspberrypi/userland.git
  git clone https://github.com/raspberrypi/userland.git
Line 34: Line 33:


Update RaspiCV.c
Update RaspiCV.c
  cp userland/host_applications/linux/apps/raspicam/RaspiVid.c ~/RaspiCV/src/RaspiCV.c
  cp ~/userland/host_applications/linux/apps/raspicam/RaspiVid.c ~/RaspiCV/src/RaspiCV.c
and patch it
and patch it
  < #include <semaphore.h>
  < #include <semaphore.h>
Line 71: Line 70:
   
   
  /*****************************************************************************/
  /*****************************************************************************/
  #define SAD_LIMIT 150
  #define SAD_LIMIT 2000
#define SCALE 3/2
  #define streamOutGstStr "appsrc ! shmsink socket-path=/tmp/camera3 wait-for-connection=false async=false sync=false"
  #define streamOutGstStr "appsrc ! shmsink socket-path=/tmp/camera3 wait-for-connection=false async=false sync=false"
   
   
Line 82: Line 80:
  };
  };
  static motion_elt_t *motionIn;
  static motion_elt_t *motionIn;
  static Mat imageIn;
  static Mat1b gray;
  static int width,height,fps;  
  static int width,height,fps;  
   
   
Line 99: Line 97:
  {
  {
   VideoWriter strOut = VideoWriter(streamOutGstStr,0,fps/1,Size(width,height),true);  
   VideoWriter strOut = VideoWriter(streamOutGstStr,0,fps/1,Size(width,height),true);  
   Mat3b grayBGR;
   unsigned int imageSize = ((width * height * sizeof(uint8_t))*SCALE);
   Mat colormap;
   Mat imageOut(width, height, CV_8UC3,Scalar(0,0,0));
   int32_t sum_x,sum_y;
   char* imgptr = imageOut.ptr<char>(0);  
   
   
   int mbx = width/16;
   int mbx = width/16;
   int mby = height/16;
   int mby = height/16;
  int mbxy = (8 * mbx * mby);
   unsigned int motionSize = ((mbx+1)*mby) * sizeof(struct motion_elt_t);  
   unsigned int motionSize = ((mbx+1)*mby) * sizeof(struct motion_elt_t);  
   motion_elt_t *motionOut = new motion_elt_t[(mbx+1)*mby];
   motion_elt_t *motionOut = new motion_elt_t[(mbx+1)*mby];
   unsigned int uoffset=(width * height);
  unsigned int voffset=uoffset*5/4;  
   char buff[20];int lg;
    
    
  unsigned int k,l,m,s;
  int x,y,p,q,a,b;
  int8_t r;
  int8_t mline[17][17];
  for(int y=0;y<=16;y++) {
    for(int x=0;x<=16;x++) {
      mline[x][y]=floor((x*y/16.0)+0.5);
    }
  }
   while (true) {
   while (true) {
     pthread_mutex_lock(&img_mutex);
     pthread_mutex_lock(&img_mutex);
     while (!img_ready) pthread_cond_wait(&img_condv, &img_mutex);
     while (!img_ready) pthread_cond_wait(&img_condv, &img_mutex);
     memcpy(imageOut.data,imageIn.data,imageSize);
     cvtColor(gray, grayBGR, COLOR_GRAY2BGR);
    applyColorMap(grayBGR, colormap, COLORMAP_JET);
     img_ready=false;
     img_ready=false;
     pthread_mutex_unlock(&img_mutex);  
     pthread_mutex_unlock(&img_mutex);  
Line 134: Line 123:
       imv_ready=false;
       imv_ready=false;
       pthread_mutex_unlock(&imv_mutex);
       pthread_mutex_unlock(&imv_mutex);
     }  
     }   
   
    for (int j=1;j<(mby-1);j++) {
      for (int i=1;i< (mbx-1);i++) {
    x=((i-1)*8)+12;
        y=((j-1)*8)+12;
        k=(2*((y*(mbx)*16)+x));
        imgptr[k]=0;
 
        l=(y*(mbx)*8)+x;
        m = (l & 1) ? 0xFF : 0x00;
        imgptr[uoffset+l] = imgptr[uoffset+l] & m;
        imgptr[voffset+l] = imgptr[voffset+l] & m;
   
   
    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);  
         motion_elt_t *vec = motionOut + (i+(mbx+1)*j);  
         if (vec->x == 0 && vec->y == 0) continue;
         if (vec->x == 0 && vec->y == 0) continue;
         if (vec->sad > SAD_LIMIT) 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);
   
   
        p = ((vec->x)>>2);
    drawAxes(&grayBGR);
        if(p<0) {a=-1;p=-p;} else a=1;
        if(p>15) p=15;
        q = ((vec->y)>>2);
        if(q<0) {b=-1;q=-q;} else b=1;
        if(q>15) q=15;
   
   
        if(p<q) {
     strOut.write(grayBGR);
          for (int n=0;n<=q;n++) {
            r=mline[n][p];
            s=(b*n*width)-(a*r);
            imgptr[k+s]=0;
          }
        } else {
          for (int n=0;n<=p;n++) {
            r=mline[n][q];
            s=(b*r*width)-(a*n);
            imgptr[k+s]=0;
          }
        }
      }
    }
     strOut.write(imageOut);
   }
   }
   return((void *)0);
   return((void *)0);
  }
  }
 
  /*****************************************************************************/
  /*****************************************************************************/
  void cv_init(int w, int h, int f, int fmt)
  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;
   width=w;height=h;fps=f;
   
   
   imageIn.create(width*SCALE, height, CV_8UC1);
   gray = Mat(height, width, CV_8UC1);
   motionIn = new motion_elt_t[((width/16)+1) * (height/16)];  
   motionIn = new motion_elt_t[((width/16)+1) * (height/16)];  
   
   
Line 198: Line 181:
   init_ready=true;
   init_ready=true;
  }
  }
 
  /*****************************************************************************/
  /*****************************************************************************/
  void cv_process_img(uint8_t *p_buffer, int length, int64_t timestamp)
  void cv_process_img(uint8_t *p_buffer, int length, int64_t timestamp)
Line 204: Line 187:
   if (init_ready) {
   if (init_ready) {
     pthread_mutex_lock(&img_mutex);
     pthread_mutex_lock(&img_mutex);
     memcpy(imageIn.data, p_buffer, length);
     memcpy(gray.data, p_buffer, length);
     img_ready=true;
     img_ready=true;
     pthread_cond_signal(&img_condv);
     pthread_cond_signal(&img_condv);

Latest revision as of 06:20, 12 February 2021

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