Tutorial: Pose estimation from QRcode

Table of Contents

• Introduction
• Pose estimation of a QRcode
• Next tutorial

Introduction

This tutorial shows how to use Tutorial: Bar code detection and Tutorial: Pose estimation from points in order to estimate the pose of a QRcode. After a first step that enables QRcode detection, the pose estimation process is achieved from the location of the four QRcode corners. The 3D coordinates of the corners are set knowing the size of the QRcode while their 2D coordinates are extracted from the image and transformed in the image plane thanks to camera intrinsic parameters.

Note that all the material (source code and image) described in this tutorial is part of ViSP source code and could be downloaded using the following command:

$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/computer-vision

Note also that ViSP has to be built with zbar 3rd party library that enables barcode detection. See zbar quick installation guide.

Pose estimation of a QRcode

In this section we consider the case of an image that may contain a 12 by 12 cm square QRcode. The camera should be calibrated (see Tutorial: Camera calibration). For each QRcode that is detected thanks to vpDetectorQRCode class, we update the coordinates of the four corners as a vpPoint object and compute the pose from the four points thanks to vpPose class. This process is replicated in a while loop. The end of the loop is reached when the user click in the image that is displayed.

The corresponding source code also provided in tutorial-pose-from-qrcode-image.cpp is the following.

#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/vision/vpPose.h>
#include <visp3/detection/vpDetectorQRCode.h>

void computePose(std::vector<vpPoint> &point, const std::vector<vpImagePoint> &ip,
                 const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo)
{
  vpPose pose;     double x=0, y=0;
  for (unsigned int i=0; i < point.size(); i ++) {
    vpPixelMeterConversion::convertPoint(cam, ip[i], x, y);
    point[i].set_x(x);
    point[i].set_y(y);
    pose.addPoint(point[i]);
  }

  if (init == true) {
    vpHomogeneousMatrix cMo_dem;
    vpHomogeneousMatrix cMo_lag;
    pose.computePose(vpPose::DEMENTHON, cMo_dem);
    pose.computePose(vpPose::LAGRANGE, cMo_lag);
    double residual_dem = pose.computeResidual(cMo_dem);
    double residual_lag = pose.computeResidual(cMo_lag);
    if (residual_dem < residual_lag)
      cMo = cMo_dem;
    else
      cMo = cMo_lag;
  }
  pose.computePose(vpPose::VIRTUAL_VS, cMo);
}

int main()
{
#if defined(VISP_HAVE_ZBAR)
  try {
    vpImage<unsigned char> I;
    vpImageIo::read(I, "bar-code.pgm");

#if defined(VISP_HAVE_X11)
    vpDisplayX d(I);
#elif defined(VISP_HAVE_GDI)
    vpDisplayGDI d(I);
#elif defined(VISP_HAVE_OPENCV)
    vpDisplayOpenCV d(I);
#endif

    // Camera parameters should be adapted to your camera
    vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);

    // 3D model of the QRcode: here we consider a 12cm by 12cm QRcode
    std::vector<vpPoint> point;
    point.push_back( vpPoint(-0.06, -0.06, 0) ); // QCcode point 0 3D coordinates in plane Z=0
    point.push_back( vpPoint( 0.06, -0.06, 0) ); // QCcode point 1 3D coordinates in plane Z=0
    point.push_back( vpPoint( 0.06,  0.06, 0) ); // QCcode point 2 3D coordinates in plane Z=0
    point.push_back( vpPoint(-0.06,  0.06, 0) ); // QCcode point 3 3D coordinates in plane Z=0

    vpHomogeneousMatrix cMo;
    bool init = true;

    vpDetectorQRCode detector;

    while(1) {
      vpImageIo::read(I, "bar-code.pgm");
      vpDisplay::display(I);

      bool status = detector.detect(I);

      std::ostringstream legend;
      legend << detector.getNbObjects() << " bar code detected";
      vpDisplay::displayText(I, (int)I.getHeight()-30, 10, legend.str(), vpColor::red);

      if (status) { // true if at least one QRcode is detected
        for(size_t i=0; i < detector.getNbObjects(); i++) {

          std::vector<vpImagePoint> p = detector.getPolygon(i); // get the four corners location in the image

          for(size_t j=0; j < p.size(); j++) {
            vpDisplay::displayCross(I, p[j], 14, vpColor::red, 3);
            std::ostringstream number;
            number << j;
            vpDisplay::displayText(I, p[j]+vpImagePoint(15,5), number.str(), vpColor::blue);
          }

          computePose(point, p, cam, init, cMo); // resulting pose is available in cMo var
          std::cout << "Pose translation (meter): " << cMo.getTranslationVector().t() << std::endl
                    << "Pose rotation (quaternion): " << vpQuaternionVector(cMo.getRotationMatrix()).t() << std::endl;
          vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none, 3);
        }
      }
      vpDisplay::displayText(I, (int)I.getHeight()-15, 10, "A click to quit...", vpColor::red);
      vpDisplay::flush(I);

      if (vpDisplay::getClick(I, false)) break;

      vpTime::wait(40);
    }
  }
  catch(const vpException &e) {
    std::cout << "Catch an exception: " << e.getMessage() << std::endl;
  }
#else
  std::cout << "ViSP is not build with zbar 3rd party." << std::endl;
#endif
}

Here is a screen shot of the resulting program:

More source code explanations could be found in Tutorial: Bar code detection and Tutorial: Pose estimation from points.

Note that adapting this tutorial to process images coming from a camera live stream consists in replacing the lines

vpImageIo::read(I, "bar-code.pgm");

by

g.aquire(I);

where g is nothing more than a grabber instantiated as a vp1394TwoGrabber, vpFlyCaptureGrabber, vpV4l2Grabber or vpRealSense. How to implement framegrabbing is explained in Tutorial: Image frame grabbing.

Next tutorial

You are now ready to see the next Tutorial: Homography estimation from points that shows how to estimate an homography from points.