pinholeTools.hpp

Go to the documentation of this file.

#ifndef PINHOLE_HPP_
#define PINHOLE_HPP_

#include "jmath/jblas.hpp"
#include "jmath/linearSolvers.hpp"
#include "jmath/ublasExtra.hpp"

#include "jmath/matlab.hpp"

namespace jafar {
  namespace rtslam {

    using namespace jblas;


    namespace pinhole {


      template<class V>
      vec2 projectPointToNormalizedPlane(const V & v) {

        vec2 up;
        up(0) = v(0) / v(2);
        up(1) = v(1) / v(2);
        return up;
      }

      template<class V, class P>
      void projectPointToNormalizedPlane(const V & v, P & up, double & dist) {

        up(0) = v(0) / v(2);
        up(1) = v(1) / v(2);
        dist = ublas::norm_2(v);
      }


      template<class V, class U, class MU_v>
      void projectPointToNormalizedPlane(const V & v, U & up, MU_v & UP_v) {

        up = projectPointToNormalizedPlane(v);

        UP_v(0, 0) = 1.0 / v(2);
        UP_v(0, 1) = 0.0;
        UP_v(0, 2) = -v(0) / (v(2) * v(2));
        UP_v(1, 0) = 0.0;
        UP_v(1, 1) = 1.0 / v(2);
        UP_v(1, 2) = -v(1) / (v(2) * v(2));

      }

      template<class V, class U, class MU_v>
      void projectPointToNormalizedPlane(const V & v, U & up, double & dist, MU_v & UP_v) {

        projectPointToNormalizedPlane(v, up, dist);

        UP_v(0, 0) = 1.0 / v(2);
        UP_v(0, 1) = 0.0;
        UP_v(0, 2) = -v(0) / (v(2) * v(2));
        UP_v(1, 0) = 0.0;
        UP_v(1, 1) = 1.0 / v(2);
        UP_v(1, 2) = -v(1) / (v(2) * v(2));

      }

      template<class U>
      vec3 backprojectPointFromNormalizedPlane(const U & u, double depth = 1) {

        vec3 p;
        p(0) = depth * u(0);
        p(1) = depth * u(1);
        p(2) = depth;
        return p;
      }

      template<class U, class P, class MP_u, class MP_depth>
      void backprojectPointFromNormalizedPlane(const U & u, const double depth, P & p, MP_u & P_u, MP_depth & P_depth) {
        p = backprojectPointFromNormalizedPlane(u, depth);

        P_u(0, 0) = depth;
        P_u(0, 1) = 0.0;
        P_u(1, 0) = 0.0;
        P_u(1, 1) = depth;
        P_u(2, 0) = 0.0;
        P_u(2, 1) = 0.0;

        P_depth(0, 0) = u(0);
        P_depth(1, 0) = u(1);
        P_depth(2, 0) = 1.0;
      }

      template<class VD>
      double distortFactor(const VD & d, double r2){
        if (d.size() == 0) return 1.0;
        double s = 1.0;
        double r2i = 1.0;
        for (size_t i = 0; i < d.size(); i++) { //   here we are doing:
          r2i = r2i * r2; //                    r2i = r^(2*(i+1))
          s += d(i) * r2i; //                   s = 1 + d_0 * r^2 + d_1 * r^4 + d_2 * r^6 + ...
        }
        return s;
      }


      template<class VD, class VU>
      jblas::vec2 distortPoint(const VD & d, const VU & up, double r2max) {
        size_t n = d.size();
        if (n == 0)
          return up;
        else {
          double r2 = up(0) * up(0) + up(1) * up(1); // this is the norm squared: r2 = ||u||^2
          if (r2 > r2max) return up;
          return distortFactor(d, r2) * up;
//          double s = 1.0;
//          double r2i = 1.0;
//          for (size_t i = 0; i < n; i++) { //   here we are doing:
//            r2i = r2i * r2; //                    r2i = r^(2*(i+1))
//            s += d(i) * r2i; //                   s = 1 + d_0 * r^2 + d_1 * r^4 + d_2 * r^6 + ...
//          }
//          return s * up; //                     finally: ud = (1 + d_0 * r^2 + d_1 * r^4 + d_2 * r^6 + ...) * u;
        }
      }


      template<class VD, class VUp, class VUd, class MUD_up>
      void distortPoint(const VD & d, const VUp & up, VUd & ud, double r2max, MUD_up & UD_up) {
        size_t n = d.size();
        jblas::vec2 R2_up;
        jblas::vec2 S_up;

        if (n > 0)
        {
          double r2 = up(0) * up(0) + up(1) * up(1); // this is the norm squared: r2 = ||u||^2
          if (r2 <= r2max)
          {
            double s = 1.0;
            double r2i = 1.0;
            double r2im1 = 1.0; //r2*(i-1)
            double S_r2 = 0.0;

            for (size_t i = 0; i < n; i++) { //.. here we are doing:
              r2i = r2i * r2; //................. r2i = r^(2*(i+1))
              s += d(i) * r2i; //................ s = 1 + d_0 * r^2 + d_1 * r^4 + d_2 * r^6 + ...

              S_r2 = S_r2 + (i + 1) * d(i) * r2im1; //jacobian of s wrt r2 : S_r2 = d_0 + 2 * d1 * r^2 + 3 * d_2 * r^4 +  ...
              r2im1 = r2im1 * r2;
            }

            ud = s * up; // finally ud = (1 + d_0 * r^2 + d_1 * r^4 + d_2 * r^6 + ...) * u;

            R2_up(0) = 2 * up(0);
            R2_up(1) = 2 * up(1);

            S_up(0) = R2_up(0) * S_r2;
            S_up(1) = R2_up(1) * S_r2;

            UD_up(0, 0) = S_up(0) * up(0) + s;
            UD_up(0, 1) = S_up(1) * up(0);
            UD_up(1, 0) = S_up(0) * up(1);
            UD_up(1, 1) = S_up(1) * up(1) + s;

            return;
          }
        }
        // if n==0 or r2>r2max
        ud = up;
        UD_up = jblas::identity_mat(2);
      }

      template<class VC, class VU>
      jblas::vec2 undistortPoint(const VC & c, const VU & ud) {
        size_t n = c.size();
        if (n == 0)
          return ud;
        else {
          double r2 = ud(0) * ud(0) + ud(1) * ud(1); // this is the norm squared: r2 = ||u||^2
          return distortFactor(c, r2) * ud;
//          double s = 1.0;
//          double r2i = 1.0;
//          for (size_t i = 0; i < n; i++) { //   here we are doing:
//            r2i = r2i * r2; //                    r2i = r^(2*(i+1))
//            s += c(i) * r2i; //                   s = 1 + c_0 * r^2 + c_1 * r^4 + c_2 * r^6 + ...
//          }
//          return s * ud; //                     finally: up = (1 + c_0 * r^2 + c_1 * r^4 + c_2 * r^6 + ...) * u;
        }
      }

      template<class VC, class VUd, class VUp, class MUP_ud>
      void undistortPoint(const VC & c, const VUd & ud, VUp & up, MUP_ud & UP_ud) {
        size_t n = c.size();
        jblas::vec2 R2_ud;
        jblas::vec2 S_ud;

        if (n == 0) {
          up = ud;
          UP_ud = jblas::identity_mat(2);
        }

        else {
          double r2 = ud(0) * ud(0) + ud(1) * ud(1); // this is the norm squared: r2 = ||u||^2
          double s = 1.0;
          double r2i = 1.0;
          double r2im1 = 1.0; //r2*(i-1)
          double S_r2 = 0.0;

          for (size_t i = 0; i < n; i++) { //.. here we are doing:
            r2i = r2i * r2; //................. r2i = r^(2*(i+1))
            s += c(i) * r2i; //................ s = 1 + c_0 * r^2 + c_1 * r^4 + c_2 * r^6 + ...

            S_r2 = S_r2 + (i + 1) * c(i) * r2im1; //jacobian of s wrt r2 : S_r2 = c_0 + 2 * d1 * r^2 + 3 * c_2 * r^4 +  ...
            r2im1 = r2im1 * r2;
          }

          up = s * ud; // finally up = (1 + c_0 * r^2 + c_1 * r^4 + c_2 * r^6 + ...) * u;

          R2_ud(0) = 2 * ud(0);
          R2_ud(1) = 2 * ud(1);

          S_ud(0) = R2_ud(0) * S_r2;
          S_ud(1) = R2_ud(1) * S_r2;

          UP_ud(0, 0) = S_ud(0) * ud(0) + s;
          UP_ud(0, 1) = S_ud(1) * ud(0);
          UP_ud(1, 0) = S_ud(0) * ud(1);
          UP_ud(1, 1) = S_ud(1) * ud(1) + s;
        }

      }


      template<class VK, class VU>
      jblas::vec2 pixellizePoint(const VK & k, const VU & ud) {
        double u_0 = k(0);
        double v_0 = k(1);
        double a_u = k(2);
        double a_v = k(3);
        vec2 u;
        u(0) = u_0 + a_u * ud(0);
        u(1) = v_0 + a_v * ud(1);
        return u;
      }


      template<class VK, class VUd, class VU, class MU_ud>
      void pixellizePoint(const VK & k, const VUd & ud, VU & u, MU_ud & U_ud) {
        //double u_0 = k(0);
        //double v_0 = k(1);
        double a_u = k(2);
        double a_v = k(3);

        u = pixellizePoint(k, ud);

        U_ud(0, 0) = a_u;
        U_ud(0, 1) = 0;
        U_ud(1, 0) = 0;
        U_ud(1, 1) = a_v;
      }


      template<class VK, class VU>
      jblas::vec2 depixellizePoint(const VK & k, const VU & u) {
        double u_0 = k(0);
        double v_0 = k(1);
        double a_u = k(2);
        double a_v = k(3);
        vec2 ud;
        ud(0) = (u(0) - u_0) / a_u;
        ud(1) = (u(1) - v_0) / a_v;
        return ud;
      }


      template<class VK, class VUd, class VU, class MUD_u>
      void depixellizePoint(const VK & k, const VU & u, VUd & ud, MUD_u & UD_u) {
        //        double u_0 = k(0);
        //        double v_0 = k(1);
        double a_u = k(2);
        double a_v = k(3);

        ud = depixellizePoint(k, u);

        UD_u(0, 0) = 1.0 / a_u;
        UD_u(0, 1) = 0.0;
        UD_u(1, 0) = 0.0;
        UD_u(1, 1) = 1.0 / a_v;
      }


      template<class VK, class VD, class V>
      vec2 projectPoint(const VK & k, const VD & d, const V & v, double r2max) {
        return pixellizePoint(k, distortPoint(d, projectPointToNormalizedPlane(v), r2max));
      }

      template<class VK, class VD, class V, class U>
      void projectPoint(const VK & k, const VD & d, const V & v, U & u, double r2max, double & dist) {
        vec2 up;
        projectPointToNormalizedPlane(v, up, dist);
        u = pixellizePoint(k, distortPoint(d, up, r2max));
      }


      template<class VK, class VD, class V, class VU, class MU_v>
      void projectPoint(const VK & k, const VD & d, const V & v, VU & u, double r2max, MU_v & U_v) {
        vec2 up, ud;
        mat23 UP_v;
        mat22 UD_up, U_ud;
        projectPointToNormalizedPlane(v, up, UP_v);
        distortPoint(d, up, ud, r2max, UD_up);
        pixellizePoint(k, ud, u, U_ud);

        mat23 U_v1;
        U_v1 = ublas::prod(UD_up, UP_v);
        U_v = ublas::prod(U_ud, U_v1);
      }

      template<class VK, class VD, class V, class VU, class MU_v>
      void projectPoint(const VK & k, const VD & d, const V & v, VU & u, double r2max, double & dist, MU_v & U_v) {
        vec2 up, ud;
        mat23 UP_v;
        mat22 UD_up, U_ud;
        projectPointToNormalizedPlane(v, up, dist, UP_v);
        distortPoint(d, up, ud, r2max, UD_up);
        pixellizePoint(k, ud, u, U_ud);

        mat23 U_v1;
        U_v1 = ublas::prod(UD_up, UP_v);
        U_v = ublas::prod(U_ud, U_v1);
      }


      template<class VK, class VC, class U>
      vec3 backprojectPoint(const VK & k, const VC & c, const U & u, const double depth = 1.0) {
        return backprojectPointFromNormalizedPlane(undistortPoint(c, depixellizePoint(k, u)), depth);
      }

      template<class VK, class VC, class U, class P, class MP_u, class MP_depth>
      void backProjectPoint(const VK & k, const VC & c, const U & u, double depth, P & p, MP_u & P_u, MP_depth & P_depth) {
        vec2 up, ud;
        mat32 P_up;
        mat22 UP_ud, UD_u;
        depixellizePoint(k, u, ud, UD_u);
        undistortPoint(c, ud, up, UP_ud);
        backprojectPointFromNormalizedPlane(up, depth, p, P_up, P_depth);

        P_u = ublas::prod(P_up, ublas::prod<mat>(UP_ud, UD_u));
      }


      template<class VPix>
      bool isInRoi(const VPix & pix, const int x, const int y, const int width, const int height) {
        return ((pix(0) >= x) && (pix(0) <= x + width - 1) && (pix(1) >= y) && (pix(1) <= y + height - 1));
      }

      template<class VPix>
      bool isInImage(const VPix & pix, const int & width, const int & height) {
        return isInRoi(pix, 0, 0, width, height);
      }


      template<class Vk, class Vd, class Vc>
      void computeCorrectionModel(const int width, const int height, const Vk & k, const Vd & d, Vc & c, double & r_max) {
        size_t size = c.size();

        if (size != 0) {
#if 0
          double rd_max = sqrt(k(0)*k(0) / (k(2)*k(2)) + k(1)*k(1) / (k(3)*k(3)));
          double r_max = 1.1 * rd_max;
#else
          double rw = std::max(k(0), (width-k(0))), rh = std::max(k(1), (height-k(1)));
          double rd_max = sqrt(rw*rw / (k(2)*k(2)) + rh*rh / (k(3)*k(3))) * 0.95;

          // search when the distortion's slope is lower than 0.5
          double resolution = 0.1;
          double r_a = 0.0, r_b = 2.01;
          double r_mon, rd_mon; // monotonous
          for (int i = 0; i < 3; ++i)
          {
            double r, r2, prev_r = -1, ir, prev_ir = -1;
            bool decreased = false;
            for(r = r_a; r <= r_b; r += resolution)
            {
              r2 = (r*rd_max); r2 *= r2;
              ir = distortFactor(d, r2);
//              if (i != 0 && r*ir < prev_r*prev_ir) { decreased = true; break; }
              if (prev_r > -0.5 && (r*ir-prev_r*prev_ir)/(r-prev_r) < 0.5) { decreased = true; break; }
              prev_r = r; prev_ir = ir;
            }
            if (!decreased) { r_mon = r; rd_mon = ir; break; }
            r_a = r-2*resolution; r_b = r;
            r_mon = r-resolution; rd_mon = prev_ir;
            resolution /= 10;
          }
          // now search for r_max
          if (r_mon*rd_mon > 1)
          {
            r_a = 0.9; r_b = r_mon;
            while (r_b-r_a > 1e-3)
            {
              double r = (r_a+r_b)/2;
              double r2 = (r*rd_max); r2 *= r2;
              double ir = distortFactor(d, r2);
              if (r * ir < 1.0) r_a = r; else r_b = r;
              //std::cout << "dichotomy " << r_a << " ; " << r_b << " (ir " << ir << ")" << std::endl;
            }
            r_max = (r_b+r_a)/2 * rd_max;
          } else r_max = r_mon*rd_max;
#endif

          size_t N_samples = 200; // number of samples
          double iN_samples = 1 / (double) N_samples;
          double rd_n, rc_2, rd_2;
          vec rd(N_samples+1), rc(N_samples+1);
          mat Rd(N_samples+1, size);


          for (size_t sample = 0; sample <= N_samples; sample++) {

            rc(sample) = sample * r_max * iN_samples; // sample * rd_max / N_samples
            rc_2 = rc(sample) * rc(sample);
            rd(sample) = distortFactor(d, rc_2) * rc(sample);
            rd_2 = rd(sample) * rd(sample);

            rd_n = rd(sample); // start with rd

            for (size_t order = 0; order < size; order++) {
              rd_n *= rd_2; // increment:
              Rd(sample, order) = rd_n; // we have : rd^3, rd^5, rd^7, ...
            }
          }


          // solve Rd*c = (rc-rd) for c, with least-squares SVD method:
          // the result is c = pseudo_inv(Rd)*(rc-rd)
          //  with pseudo_inv(Rd) = (Rd'*Rd)^-1 * Rd'

          // this does not work:
          // jmath::LinearSolvers::solve_Cholesky(Rd, (rc - rd), c);

          // therefore we solve manually the pseudo-inverse:
          jblas::mat RdtRd(size,size);
          RdtRd = ublas::prod(ublas::trans(Rd), Rd);
          jblas::mat iRdtRd(size, size);
          jmath::ublasExtra::inv(RdtRd, iRdtRd);
          mat iRd = ublas::prod(iRdtRd, ublas::trans<jblas::mat>(Rd));

          c = ublas::prod(iRd,(rc-rd));
          //std::cout << "c " << c << std::endl;
          //std::cout << "d " << d << std::endl;
          //std::cout << "rd_max " << rd_max << " r_max " << r_max << std::endl;
        }
      }


    } // namespace pinhole

  } // namespace rtslam
} // namespace jafar

#endif /* PINHOLE_HPP_ */