Exemplo n.º 1
0
  void Rectify(
      const LookupTable& lut,
      const unsigned char* pInputImageData,
      unsigned char* pOutputRectImageData,
      int w,
      int h
      )
  {
    // Make the most of the continuous block of memory!
    const BilinearLutPoint* ptr   = &lut.m_vLutPixels[0];

    const int nHeight = lut.Height();
    const int nWidth  = lut.Width();

    // Make sure we have been given a correct lookup table.
    assert(w== nWidth && h == nHeight);

    for( int nRow = 0; nRow < nHeight; nRow++ ) {
      for( int nCol = 0; nCol < nWidth; nCol++ ) {
        *pOutputRectImageData++ =
          (char) ( ptr->w00 * pInputImageData[ ptr->idx0 ] +
              ptr->w01 * pInputImageData[ ptr->idx0 + 1 ] +
              ptr->w10 * pInputImageData[ ptr->idx1 ] +
              ptr->w11 * pInputImageData[ ptr->idx1 + 1 ] );
        ptr++;
      }
    }
  }
Exemplo n.º 2
0
  void CreateLookupTable(
      const std::shared_ptr<calibu::CameraInterface<double>>& cam_from,
      const Eigen::Matrix3d& R_onKinv,
      LookupTable& lut,
	  int lookup_width,
	  int lookup_height
      )
  {
    const int cam_width = cam_from->Width();
    const int cam_height = cam_from->Height();

    if(lookup_width < 1 || lookup_height < 1){
    	if(lut.Height() == 0){
			lookup_width = cam_width;
			lookup_height = cam_height;
			// make sure we have mem in the look up table
			lut.m_vLutPixels.resize( lookup_width*lookup_height );
			lut.m_nWidth = lookup_width;
    	}else{
    		lookup_width = lut.Width();
			lookup_height = lut.Height();
    	}
    }

    double x_offset = (lookup_width - cam_width) / 2.0;
    double y_offset = (lookup_height - cam_height) / 2.0;

    for( int r = 0; r < lookup_height; ++r) {
      for( int c = 0; c < lookup_width; ++c) {
        // Remap
        const Eigen::Vector3d p_o = R_onKinv * Eigen::Vector3d(c - x_offset,r - y_offset,1);
        Eigen::Vector2d p_warped = cam_from->Project(p_o);

        // Clamp to valid image coords. This will cause out of image
        // data to be stretched from nearest valid coords with
        // no branching in rectify function.
        p_warped[0] = std::min(std::max(0.0, p_warped[0]), cam_width - 1.0 );
        p_warped[1] = std::min(std::max(0.0, p_warped[1]), cam_height - 1.0 );

        // Truncates the values for the left image
        int u  = (int) p_warped[0];
        int v  = (int) p_warped[1];
        float su = p_warped[0] - (double)u;
        float sv = p_warped[1] - (double)v;

        // Fix pixel access for last row/column to ensure all accesses are in bounds
        if(u == (cam_width-1)) {
          u -= 1;
          su = 1.0;
        }
        if(v == (cam_height-1)) {
          v -= 1;
          sv = 1.0;
        }

        // Pre-compute the bilinear interpolation weights
        BilinearLutPoint p;
        p.idx0 = u + v*cam_width;
        p.idx1 = u + v*cam_width + cam_width;
        p.w00  = (1-su)*(1-sv);
        p.w01  =    su *(1-sv);
        p.w10  = (1-su)*sv;
        p.w11  =     su*sv;

        lut.SetPoint( r, c, p );
      }
    }
  }