subroutine rd_calibration(filename,cc,cp)
use calibration
character(256)::filename
type(ccT)::cc
type(cpT)::cp
real(8)::sa,ca,sb,cb,sg,cg
open(9,file=filename,form='formatted',status='old')
read(9,*) cp%Ncx !
read(9,*) cp%Nfx !
read(9,*) cp%dx !
read(9,*) cp%dy !
read(9,*) cp%dpx !
read(9,*) cp%dpy !
read(9,*) cp%Cx !
read(9,*) cp%Cy !
read(9,*) cp%sx !
read(9,*) cc%f !
read(9,*) cc%kappa1 !
read(9,*) cc%Tx !
read(9,*) cc%Ty !
read(9,*) cc%Tz !
read(9,*) cc%Rx !
read(9,*) cc%Ry !
read(9,*) cc%Rz !
read(9,*) cc%p1 !
read(9,*) cc%p2 !
close(9)
!print*,cp%Ncx,cp%Nfx,cp%dx
sa=sin(cc%Rx)
ca=cos(cc%Rx)
sb=sin(cc%Ry)
cb=cos(cc%Ry)
sg=sin(cc%Rz)
cg=cos(cc%Rz)
cc%r1=cb*cg
cc%r2 = cg * sa * sb - ca * sg
cc%r3 = sa * sg + ca * cg * sb
cc%r4 = cb * sg
cc%r5 = sa * sb * sg + ca * cg
cc%r6 = ca * sb * sg - cg * sa
cc%r7 = -sb
cc%r8 = cb * sa
cc%r9 = ca * cb
! print*,"R"
! print*,cc%r1,cc%r2,cc%r3
! print*,cc%r4,cc%r5,cc%r6
! print*,cc%r7,cc%r8,cc%r9
end subroutine
subroutine image_coord_to_world_coord(Xfd,Yfd,zw,xw,yw,cc,cp)
use calibration
type(ccT)::cc
type(cpT)::cp
real(8)::Xfd,Yfd,zw,xw,yw
real(8)::Xd,Yd,Xu,Yu,common_denominator
real(8)::distortion_factor
! /* convert from image to distorted sensor coordinates */
Xd = cp%dpx * (Xfd - cp%Cx) / cp%sx
Yd = cp%dpy * (Yfd - cp%Cy)
! /* convert from distorted sensor to undistorted sensor plane coordinates */
!distorted_to_undistorted_sensor_coord (Xd, Yd, Xu, Yu,cc,cp);
distortion_factor= 1 + cc%kappa1 * (Xd*Xd + Yd*Yd)
! print*,'distortion_factor',distortion_factor,cc%kappa1,sqrt (abs(Xd)),sqrt (abs(Yd)),Xd,Yd
Xu = Xd * distortion_factor
Yu = Yd * distortion_factor
! /* calculate the corresponding xw and yw world coordinates */
! /* (these equations were derived by simply inverting */
! /* the perspective projection equations using Macsyma) */
common_denominator = ((cc%r1 * cc%r8 - cc%r2 * cc%r7) * Yu + &
(cc%r5 * cc%r7 - cc%r4 * cc%r8) * Xu - &
cc%f * cc%r1 * cc%r5 + cc%f * cc%r2 * cc%r4)
!
xw = (((cc%r2 * cc%r9 - cc%r3 * cc%r8) * Yu + &
(cc%r6 * cc%r8 - cc%r5 * cc%r9) * Xu - &
cc%f * cc%r2 * cc%r6 + cc%f * cc%r3 * cc%r5) * zw + &
(cc%r2 * cc%Tz - cc%r8 * cc%Tx) * Yu + &
(cc%r8 * cc%Ty - cc%r5 * cc%Tz) * Xu - &
cc%f * cc%r2 * cc%Ty + cc%f * cc%r5 * cc%Tx) / common_denominator
yw = -(((cc%r1 * cc%r9 - cc%r3 * cc%r7) * Yu + &
(cc%r6 * cc%r7 - cc%r4 * cc%r9) * Xu - &
cc%f * cc%r1 * cc%r6 + cc%f * cc%r3 * cc%r4) * zw + &
(cc%r1 * cc%Tz - cc%r7 * cc%Tx) * Yu + &
(cc%r7 * cc%Ty - cc%r4 * cc%Tz) * Xu - &
cc%f * cc%r1 * cc%Ty + cc%f * cc%r4 * cc%Tx) / common_denominator
print*,Xfd,Yfd
print*,xw,yw
print*,' '
end subroutine
subroutine world_coord_to_image_coord(xw,yw,zw,Xf,Yf,cc,cp)
use calibration
implicit none
type(ccT)::cc
type(cpT)::cp
real(8)::xw,yw,zw,Xf,Yf,Xd,Yd,xc,yc,zc,Xu,Yu
real(8),parameter::SQRT3=1.732050807568877293527446341505872366943
real(8)::Ru,Rd,lambda,c,d,Q,R,D1,S,T,sinT,cosT
real(8)::SQRT_,CBRT_
! /* convert from world coordinates to camera coordinates */
xc = cc%r1 * xw + cc%r2 * yw + cc%r3 * zw + cc%Tx
yc = cc%r4 * xw + cc%r5 * yw + cc%r6 * zw + cc%Ty
zc = cc%r7 * xw + cc%r8 * yw + cc%r9 * zw + cc%Tz
! /* convert from camera coordinates to undistorted sensor plane coordinates */
Xu = cc%f * xc / zc
Yu = cc%f * yc / zc
! print*,"Xu,Yu",Xu,Yu
! /* convert from undistorted to distorted sensor plane coordinates */
!! undistorted_to_distorted_sensor_coord (Xu, Yu, &Xd, &Yd);
if (( (Xu==0).and. (Yu == 0)) .or. (cc%kappa1 == 0)) then
Xd = Xu
Yd = Yu
goto 100
endif
Ru = SQRT_(Xu*Xu+Yu*Yu)
c = 1. / cc%kappa1
d = -c * Ru
Q = c / 3.
R = -d / 2.
D1 = Q**3 + R**2
! print*,"D",D1
! /* convert from distorted sensor plane coordinates to image coordinates */
if (D1 >= 0) then !/* one real root */
D1 = SQRT_(D1)
S = CBRT_(R + D1)
T = CBRT_(R - D1)
Rd = S + T
if (Rd < 0) then
Rd = SQRT_(-1. / (3. * cc%kappa1))
print*, "Warning: undistorted image point to distorted image point mapping limited by"
print*, " maximum barrel distortion radius of ", Rd
! print*," (Xu = %lf, Yu = %lf) -> (Xd = %lf, Yd = %lf)\n\n",
! Xu, Yu, Xu * Rd / Ru, Yu * Rd / Ru);
endif
else ! /* three real roots */
D1 = SQRT_(-D1)
S = CBRT_(sqrt(R**2+D1**2))
T = atan2 (D1, R) / 3.
sinT=sin(T)
cosT=cos(T)
! /* the larger positive root is 2*S*cos(T) */
! /* the smaller positive root is -S*cos(T) + SQRT(3)*S*sin(T) */
! /* the negative root is -S*cos(T) - SQRT(3)*S*sin(T) */
Rd = -S * cosT + SQRT3 * S * sinT !/* use the smaller positive root */
endif
lambda = Rd / Ru
! print*,"Rd,Ru",Rd,Ru
Xd = Xu * lambda
Yd = Yu * lambda
! print*,"Xd,Yd",Xd,Yd
100 continue
Xf = Xd * cp%sx / cp%dpx + cp%Cx
Yf = Yd / cp%dpy + cp%Cy
end subroutine
function SQRT_(Q)
real(8)::Q,SQRT_
SQRT_=sqrt(abs(Q))
end function SQRT_
function CBRT_(x)
real(8)::x,CBRT_
if (x == 0) then
CBRT_=0
else
if (x > 0) then
CBRT_=x**(1.0 / 3.0)
else
CBRT_= -(-x)**(1.0 / 3.0)
endif
endif
!print*,"x,CBRT(x)",x,CBRT_
end function CBRT_