鱼眼图像转经纬图代码

 
 

  
  #include <iostream>

 
 
 
 

  
  #include <string>

 
 
 
 

  
  #include <opencv2\opencv.hpp>

 
 
 
 

  
  

 
 
 
 

  
  using 
  
  namespace 
  
  std
  
  ;

 
 
 
 

  
  using 
  
  namespace 
  
  cv
  
  ;

 
 
 
 


 
 
 
 

  
  const 
  
  float 
  
  PI 
  
  = 
  
  3.1415926
  
  ;

 
 
 
 


 
 
 
 

  
  void 
  
  rectifyMap
  
  (
  
  Mat 
  
  &
  
  mapImg
  
  , 
  
  const 
  
  int 
  
  inWidth
  
  , 
  
  const 
  
  int 
  
  inHeight
  
  ,
  
  const 
  
  float
  
  * 
  
  rot
  
  , 
  
  const 
  
  int 
  
  outWidth
  
  , 
  
  const 
  
  int 
  
  outHeight
  
  , 
  
  const 
  
  float 
  
  FOV
  
  , 
  
  const 
  
  float 
  
  radius
  
  )

 
 
 
 

  
  {

 
 
 
 
	
  
  float 
  
  cx 
  
  = 
  
  inWidth
  
  /
  
  2.0
  
  ;

 
 
 
 
	
  
  float 
  
  cy 
  
  = 
  
  inHeight
  
  /
  
  2.0
  
  ;

 
 
 
 


 
 
 
 
	
  
  float
  
  * 
  
  pMapData 
  
  = 
  
  (
  
  float
  
  *
  
  )
  
  mapImg
  
  .
  
  data
  
  ;

 
 
 
 
	
  
  for 
  
  (
  
  int 
  
  j 
  
  = 
  
  0
  
  ; 
  
  j 
  
  < 
  
  outHeight
  
  ; 
  
  j
  
  ++
  
  )

 
 
 
 
	
  
  {

 
 
 
 
		
  
  float 
  
  theta1 
  
  = 
  
  j
  
  *
  
  PI 
  
  / 
  
  outHeight
  
  ;

 
 
 
 
		
  
  float 
  
  sinTheta1 
  
  = 
  
  sin
  
  (
  
  theta1
  
  );

 
 
 
 
		
  
  float 
  
  z1 
  
  = 
  
  cos
  
  (
  
  theta1
  
  );

 
 
 
 


 
 
 
 
		
  
  for 
  
  (
  
  int 
  
  i 
  
  = 
  
  0
  
  ; 
  
  i 
  
  < 
  
  outWidth
  
  ; 
  
  i
  
  ++
  
  )

 
 
 
 
		
  
  {

 
 
 
 
			
  
  float 
  
  fi1 
  
  = 
  
  2 
  
  * 
  
  PI 
  
  - 
  
  i
  
  * 
  
  2
  
  *
  
  PI 
  
  / 
  
  outWidth
  
  ;

 
 
 
 
			
  
  float 
  
  x1 
  
  = 
  
  sinTheta1
  
  *
  
  cos
  
  (
  
  fi1
  
  );

 
 
 
 
			
  
  float 
  
  y1 
  
  = 
  
  sinTheta1
  
  *
  
  sin
  
  (
  
  fi1
  
  );

 
 
 
 


 
 
 
 
			
  
  //归一化三维坐标

 
 
 
 

  
  			
  
  float 
  
  x2 
  
  = 
  
  rot
  
  [
  
  0
  
  ] 
  
  * 
  
  x1 
  
  + 
  
  rot
  
  [
  
  1
  
  ] 
  
  * 
  
  y1 
  
  + 
  
  rot
  
  [
  
  2
  
  ] 
  
  * 
  
  z1
  
  ;

 
 
 
 

  
  			float y2 = rot[3] * x1 + rot[4] * y1 + rot[5] * z1;

 
 
 
 
			
  
  float 
  
  z2 
  
  = 
  
  rot
  
  [
  
  6
  
  ] 
  
  * 
  
  x1 
  
  + 
  
  rot
  
  [
  
  7
  
  ] 
  
  * 
  
  y1 
  
  + 
  
  rot
  
  [
  
  8
  
  ] 
  
  * 
  
  z1
  
  ;

 
 
 
 
			
  
  float 
  
  norm 
  
  = 
  
  sqrt
  
  (
  
  x2
  
  *
  
  x2 
  
  + 
  
  y2
  
  *
  
  y2 
  
  + 
  
  z2
  
  *
  
  z2
  
  );

 
 
 
 
			
  
  x2 
  
  /= 
  
  norm
  
  ;

 
 
 
 
			
  
  y2 
  
  /= 
  
  norm
  
  ;

 
 
 
 
			
  
  z2 
  
  /= 
  
  norm
  
  ;

 
 
 
 


 
 
 
 
			
  
  //球面坐标系转换

 
 
 
 

  
  			
  
  float 
  
  theta2 
  
  = 
  
  acos
  
  (
  
  z2
  
  )
  
  *
  
  180
  
  /
  
  PI
  
  ;

 
 
 
 

  
  			float fi2 = atan2(y2, x2);

 
 
 
 


 
 
 
 
			
  
  if 
  
  (
  
  theta2 
  
  <= 
  
  (
  
  FOV 
  
  / 
  
  2
  
  ) 
  
  && 
  
  theta2 
  
  >= 
  
  0
  
  )

 
 
 
 
			
  
  {

 
 
 
 
				
  
  //球面到鱼眼

 
 
 
 

  
  				
  
  float 
  
  radius2 
  
  =
  
  radius
  
  * 
  
  theta2 
  
  / 
  
  (
  
  FOV 
  
  / 
  
  2
  
  );

 
 
 
 

  
  				float u = (radius2*cos(fi2) + cx);

 
 
 
 
				
  
  float 
  
  v 
  
  = 
  
  (
  
  radius2
  
  *
  
  sin
  
  (
  
  fi2
  
  ) 
  
  + 
  
  cy
  
  );

 
 
 
 
				
  
  if 
  
  (
  
  u 
  
  >= 
  
  0 
  
  && 
  
  u 
  
  < 
  
  inWidth 
  
  - 
  
  1 
  
  && 
  
  v 
  
  >= 
  
  0 
  
  && 
  
  v 
  
  < 
  
  inHeight 
  
  - 
  
  1
  
  )

 
 
 
 
				
  
  {

 
 
 
 
					
  
  pMapData
  
  [
  
  j
  
  *
  
  outWidth 
  
  * 
  
  2 
  
  + 
  
  2 
  
  * 
  
  i 
  
  + 
  
  0
  
  ] 
  
  = 
  
  u
  
  ;

 
 
 
 
					
  
  pMapData
  
  [
  
  j
  
  *
  
  outWidth 
  
  * 
  
  2 
  
  + 
  
  2 
  
  * 
  
  i 
  
  + 
  
  1
  
  ] 
  
  = 
  
  v
  
  ;

 
 
 
 
				
  
  }

 
 
 
 
				
  
  else

 
 
 
 
				
  
  {

 
 
 
 
					
  
  pMapData
  
  [
  
  j
  
  *
  
  outWidth 
  
  * 
  
  2 
  
  + 
  
  2 
  
  * 
  
  i 
  
  + 
  
  0
  
  ] 
  
  = 
  
  0
  
  ;

 
 
 
 
					
  
  pMapData
  
  [
  
  j
  
  *
  
  outWidth 
  
  * 
  
  2 
  
  + 
  
  2 
  
  * 
  
  i 
  
  + 
  
  1
  
  ] 
  
  = 
  
  0
  
  ;

 
 
 
 
				
  
  }

 
 
 
 
			
  
  }

 
 
 
 
			
  
  else

 
 
 
 
			
  
  {

 
 
 
 
				
  
  pMapData
  
  [
  
  j
  
  *
  
  outWidth 
  
  * 
  
  2 
  
  + 
  
  2 
  
  * 
  
  i 
  
  + 
  
  0
  
  ] 
  
  = 
  
  0
  
  ;

 
 
 
 
				
  
  pMapData
  
  [
  
  j
  
  *
  
  outWidth 
  
  * 
  
  2 
  
  + 
  
  2 
  
  * 
  
  i 
  
  + 
  
  1
  
  ] 
  
  = 
  
  0
  
  ;

 
 
 
 
			
  
  }

 
 
 
 
		
  
  }

 
 
 
 
	
  
  }

 
 
 
 

  
  }

 
 
 
 


 
 
 
 

  
  void 
  
  remap
  
  (
  
  const 
  
  cv
  
  ::
  
  Mat
  
  & 
  
  srcImg
  
  , 
  
  cv
  
  ::
  
  Mat
  
  & 
  
  dstImg
  
  , 
  
  const 
  
  cv
  
  ::
  
  Mat
  
  & 
  
  mapImg
  
  , 
  
  int 
  
  inHeight
  
  , 
  
  int 
  
  inWidth
  
  , 
  
  int 
  
  outHeight
  
  , 
  
  int 
  
  outWidth
  
  )

 
 
 
 

  
  {

 
 
 
 
	
  
  uchar
  
  * 
  
  pSrcData 
  
  = 
  
  (
  
  uchar
  
  *
  
  )
  
  srcImg
  
  .
  
  data
  
  ;

 
 
 
 
	
  
  uchar
  
  * 
  
  pDstData 
  
  = 
  
  (
  
  uchar
  
  *
  
  )
  
  dstImg
  
  .
  
  data
  
  ;

 
 
 
 
	
  
  float
  
  * 
  
  pMapData 
  
  = 
  
  (
  
  float
  
  *
  
  )
  
  mapImg
  
  .
  
  data
  
  ;

 
 
 
 


 
 
 
 
	
  
  for 
  
  (
  
  int 
  
  j 
  
  = 
  
  0
  
  ; 
  
  j 
  
  < 
  
  outHeight
  
  ; 
  
  j
  
  ++
  
  )

 
 
 
 
	
  
  {

 
 
 
 
		
  
  for 
  
  (
  
  int 
  
  i 
  
  = 
  
  0
  
  ; 
  
  i 
  
  < 
  
  outWidth
  
  ; 
  
  i
  
  ++
  
  )

 
 
 
 
		
  
  {

 
 
 
 
			
  
  int 
  
  idx 
  
  = 
  
  j
  
  *
  
  outWidth 
  
  * 
  
  2 
  
  + 
  
  i 
  
  * 
  
  2
  
  ;

 
 
 
 
			
  
  float 
  
  u 
  
  = 
  
  pMapData
  
  [
  
  idx 
  
  + 
  
  0
  
  ];

 
 
 
 
			
  
  float 
  
  v 
  
  = 
  
  pMapData
  
  [
  
  idx 
  
  + 
  
  1
  
  ];

 
 
 
 


 
 
 
 
			
  
  int 
  
  u0 
  
  = 
  
  floor
  
  (
  
  u
  
  );

 
 
 
 
			
  
  int 
  
  v0 
  
  = 
  
  floor
  
  (
  
  v
  
  );

 
 
 
 
			
  
  float 
  
  dx 
  
  = 
  
  u 
  
  - 
  
  u0
  
  ;

 
 
 
 
			
  
  float 
  
  dy 
  
  = 
  
  v 
  
  - 
  
  v0
  
  ;

 
 
 
 
			
  
  float 
  
  weight1 
  
  = 
  
  (
  
  1 
  
  - 
  
  dx
  
  )
  
  *
  
  (
  
  1 
  
  - 
  
  dy
  
  );

 
 
 
 
			
  
  float 
  
  weight2 
  
  = 
  
  dx
  
  *
  
  (
  
  1 
  
  - 
  
  dy
  
  );

 
 
 
 
			
  
  float 
  
  weight3 
  
  = 
  
  (
  
  1 
  
  - 
  
  dx
  
  )
  
  *
  
  dy
  
  ;

 
 
 
 
			
  
  float 
  
  weight4 
  
  = 
  
  dx
  
  *
  
  dy
  
  ;

 
 
 
 


 
 
 
 
			
  
  if 
  
  (
  
  u0 
  
  >= 
  
  0 
  
  && 
  
  v0 
  
  >= 
  
  0 
  
  && 
  
  (
  
  u0 
  
  + 
  
  1
  
  ) 
  
  < 
  
  inWidth 
  
  && 
  
  (
  
  v0 
  
  + 
  
  1
  
  ) 
  
  < 
  
  inHeight
  
  )

 
 
 
 
			
  
  {

 
 
 
 
				
  
  float 
  
  B 
  
  = 
  
  weight1
  
  *
  
  pSrcData
  
  [
  
  v0
  
  *
  
  inWidth 
  
  * 
  
  3 
  
  + 
  
  u0 
  
  * 
  
  3 
  
  + 
  
  0
  
  ] 
  
  + 
  
  weight2
  
  *
  
  pSrcData
  
  [
  
  v0
  
  *
  
  inWidth 
  
  * 
  
  3 
  
  + 
  
  (
  
  u0 
  
  + 
  
  1
  
  ) 
  
  * 
  
  3 
  
  + 
  
  0
  
  ] 
  
  +

 
 
 
 
					
  
  weight3
  
  *
  
  pSrcData
  
  [(
  
  v0 
  
  + 
  
  1
  
  )
  
  *
  
  inWidth 
  
  * 
  
  3 
  
  + 
  
  u0 
  
  * 
  
  3 
  
  + 
  
  0
  
  ] 
  
  + 
  
  weight4
  
  *
  
  pSrcData
  
  [(
  
  v0 
  
  + 
  
  1
  
  )
  
  *
  
  inWidth 
  
  * 
  
  3 
  
  + 
  
  (
  
  u0 
  
  + 
  
  1
  
  ) 
  
  * 
  
  3 
  
  + 
  
  0
  
  ];

 
 
 
 


 
 
 
 
				
  
  float 
  
  G 
  
  = 
  
  weight1
  
  *
  
  pSrcData
  
  [
  
  v0
  
  *
  
  inWidth 
  
  * 
  
  3 
  
  + 
  
  u0 
  
  * 
  
  3 
  
  + 
  
  1
  
  ] 
  
  + 
  
  weight2
  
  *
  
  pSrcData
  
  [
  
  v0
  
  *
  
  inWidth 
  
  * 
  
  3 
  
  + 
  
  (
  
  u0 
  
  + 
  
  1
  
  ) 
  
  * 
  
  3 
  
  + 
  
  1
  
  ] 
  
  +

 
 
 
 
					
  
  weight3
  
  *
  
  pSrcData
  
  [(
  
  v0 
  
  + 
  
  1
  
  )
  
  *
  
  inWidth 
  
  * 
  
  3 
  
  + 
  
  u0 
  
  * 
  
  3 
  
  + 
  
  1
  
  ] 
  
  + 
  
  weight4
  
  *
  
  pSrcData
  
  [(
  
  v0 
  
  + 
  
  1
  
  )
  
  *
  
  inWidth 
  
  * 
  
  3 
  
  + 
  
  (
  
  u0 
  
  + 
  
  1
  
  ) 
  
  * 
  
  3 
  
  + 
  
  1
  
  ];

 
 
 
 


 
 
 
 
				
  
  float 
  
  R 
  
  = 
  
  weight1
  
  *
  
  pSrcData
  
  [
  
  v0
  
  *
  
  inWidth 
  
  * 
  
  3 
  
  + 
  
  u0 
  
  * 
  
  3 
  
  + 
  
  2
  
  ] 
  
  + 
  
  weight2
  
  *
  
  pSrcData
  
  [
  
  v0
  
  *
  
  inWidth 
  
  * 
  
  3 
  
  + 
  
  (
  
  u0 
  
  + 
  
  1
  
  ) 
  
  * 
  
  3 
  
  + 
  
  2
  
  ] 
  
  +

 
 
 
 
					
  
  weight3
  
  *
  
  pSrcData
  
  [(
  
  v0 
  
  + 
  
  1
  
  )
  
  *
  
  inWidth 
  
  * 
  
  3 
  
  + 
  
  u0 
  
  * 
  
  3 
  
  + 
  
  2
  
  ] 
  
  + 
  
  weight4
  
  *
  
  pSrcData
  
  [(
  
  v0 
  
  + 
  
  1
  
  )
  
  *
  
  inWidth 
  
  * 
  
  3 
  
  + 
  
  (
  
  u0 
  
  + 
  
  1
  
  ) 
  
  * 
  
  3 
  
  + 
  
  2
  
  ];

 
 
 
 


 
 
 
 
				
  
  int 
  
  idxResult 
  
  = 
  
  j
  
  *
  
  outWidth 
  
  * 
  
  3 
  
  + 
  
  i 
  
  * 
  
  3
  
  ;

 
 
 
 
				
  
  pDstData
  
  [
  
  idxResult 
  
  + 
  
  0
  
  ] 
  
  = 
  
  uchar
  
  (
  
  B
  
  );

 
 
 
 
				
  
  pDstData
  
  [
  
  idxResult 
  
  + 
  
  1
  
  ] 
  
  = 
  
  uchar
  
  (
  
  G
  
  );

 
 
 
 
				
  
  pDstData
  
  [
  
  idxResult 
  
  + 
  
  2
  
  ] 
  
  = 
  
  uchar
  
  (
  
  R
  
  );

 
 
 
 
			
  
  }

 
 
 
 
		
  
  }

 
 
 
 
	
  
  }

 
 
 
 

  
  }

 
 
 
 


 
 
 
 

  
  void 
  
  main
  
  ()

 
 
 
 

  
  {

 
 
 
 
	
  
  string 
  
  imgPath 
  
  = 
  
  "data/source_images/"
  
  ;

 
 
 
 
	
  
  Mat 
  
  srcImg 
  
  = 
  
  imread
  
  (
  
  imgPath 
  
  + 
  
  "animal.png"
  
  );

 
 
 
 
    
  
  //输入鱼眼图像尺寸

 
 
 
 

  
  	
  
  int 
  
  inHeight 
  
  = 
  
  srcImg
  
  .
  
  rows
  
  ;

 
 
 
 

  
  	int inWidth = srcImg.cols;

 
 
 
 
    
  
  //输出经纬度图像尺寸

 
 
 
 

  
  	
  
  int 
  
  outHeight 
  
  = 
  
  1000
  
  ;

 
 
 
 

  
  	int outWidth = 2000;

 
 
 
 
    
  
  //视场角

 
 
 
 

  
  	
  
  float 
  
  FOV 
  
  = 
  
  210
  
  ;

 
 
 
 

  
      //鱼眼半径

 
 
 
 

  
  	
  
  float 
  
  radius 
  
  = 
  
  inWidth 
  
  / 
  
  2.0
  
  ;

 
 
 
 

  
  

 
 
 
 

  
      //以图像中心为赤道

 
 
 
 

  
      
  
  float 
  
  rot
  
  [
  
  9
  
  ] 
  
  = 
  
  { 
  
  1
  
  ,
  
  0
  
  ,
  
  0
  
  ,
  
  0
  
  ,
  
  1
  
  ,
  
  0
  
  ,
  
  0
  
  ,
  
  0
  
  ,
  
  1 
  
  };

 
 
 
 

  
  	float angle = PI/2;

 
 
 
 
	
  
  rot
  
  [
  
  0
  
  ] 
  
  = 
  
  cos
  
  (
  
  angle
  
  );

 
 
 
 
	
  
  rot
  
  [
  
  2
  
  ] 
  
  = 
  
  sin
  
  (
  
  angle
  
  );

 
 
 
 
	
  
  rot
  
  [
  
  6
  
  ] 
  
  = 
  
  -
  
  sin
  
  (
  
  angle
  
  );

 
 
 
 
	
  
  rot
  
  [
  
  8
  
  ] 
  
  = 
  
  cos
  
  (
  
  angle
  
  );

 
 
 
 


 
 
 
 
    
  
  //求映射Map

 
 
 
 

  
  	
  
  cv
  
  ::
  
  Mat 
  
  mapImg 
  
  = 
  
  cv
  
  ::
  
  Mat
  
  ::
  
  zeros
  
  (
  
  outHeight
  
  , 
  
  outWidth
  
  , 
  
  CV_32FC2
  
  );

 
 
 
 

  
  	rectifyMap(mapImg, inWidth, inHeight,rot, outWidth, outHeight, FOV, radius);

 
 
 
 
    
  
  //remap得到经纬度图像

 
 
 
 

  
  	
  
  Mat 
  
  dstImg 
  
  = 
  
  Mat
  
  ::
  
  zeros
  
  (
  
  outHeight
  
  , 
  
  outWidth
  
  , 
  
  CV_8UC3
  
  );

 
 
 
 

  
  	remap(srcImg, dstImg, mapImg, inHeight, inWidth, outHeight, outWidth);

 
 
 
 


 
 
 
 
	
  
  imwrite
  
  (
  
  "dstImg.jpg"
  
  , 
  
  dstImg
  
  );

 
 
 
 

  
  }