/ *   
 
   *   P R O J E C T :   N y A R T o o l k i t 
 
   *   - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 
 
   *   T h i s   w o r k   i s   b a s e d   o n   t h e   o r i g i n a l   A R T o o l K i t   d e v e l o p e d   b y 
 
   *       H i r o k a z u   K a t o 
 
   *       M a r k   B i l l i n g h u r s t 
 
   *       H I T L a b ,   U n i v e r s i t y   o f   W a s h i n g t o n ,   S e a t t l e 
 
   *   h t t p : / / w w w . h i t l . w a s h i n g t o n . e d u / a r t o o l k i t / 
 
   * 
 
   *   T h e   N y A R T o o l k i t   i s   J a v a   e d i t i o n   A R T o o l K i t   c l a s s   l i b r a r y . 
 
   *   C o p y r i g h t   ( C ) 2 0 0 8 - 2 0 0 9   R y o   I i z u k a 
 
   * 
 
   *   T h i s   p r o g r a m   i s   f r e e   s o f t w a r e :   y o u   c a n   r e d i s t r i b u t e   i t   a n d / o r   m o d i f y 
 
   *   i t   u n d e r   t h e   t e r m s   o f   t h e   G N U   G e n e r a l   P u b l i c   L i c e n s e   a s   p u b l i s h e d   b y 
 
   *   t h e   F r e e   S o f t w a r e   F o u n d a t i o n ,   e i t h e r   v e r s i o n   3   o f   t h e   L i c e n s e ,   o r 
 
   *   ( a t   y o u r   o p t i o n )   a n y   l a t e r   v e r s i o n . 
 
   *   
 
   *   T h i s   p r o g r a m   i s   d i s t r i b u t e d   i n   t h e   h o p e   t h a t   i t   w i l l   b e   u s e f u l , 
 
   *   b u t   W I T H O U T   A N Y   W A R R A N T Y ;   w i t h o u t   e v e n   t h e   i m p l i e d   w a r r a n t y   o f 
 
   *   M E R C H A N T A B I L I T Y   o r   F I T N E S S   F O R   A   P A R T I C U L A R   P U R P O S E .     S e e   t h e 
 
   *   G N U   G e n e r a l   P u b l i c   L i c e n s e   f o r   m o r e   d e t a i l s . 
 
   * 
 
   *   Y o u   s h o u l d   h a v e   r e c e i v e d   a   c o p y   o f   t h e   G N U   G e n e r a l   P u b l i c   L i c e n s e 
 
   *   a l o n g   w i t h   t h i s   p r o g r a m .     I f   n o t ,   s e e   < h t t p : / / w w w . g n u . o r g / l i c e n s e s / > . 
 
   *   
 
   *   F o r   f u r t h e r   i n f o r m a t i o n   p l e a s e   c o n t a c t . 
 
   * 	 h t t p : / / n y a t l a . j p / n y a t o o l k i t / 
 
   * 	 < a i r m a i l ( a t ) e b o n y . p l a l a . o r . j p >   o r   < n y a t l a ( a t ) n y a t l a . j p > 
 
   *   
 
   * / 
 
 u s i n g   S y s t e m ; 
 
 n a m e s p a c e   j p . n y a t l a . n y a r t o o l k i t . c s . c o r e 
 
 { 
 
 
 
 
 
         / * * 
 
           *   S0n0000o00O p e n C V   d i s t o r t i o n   m o d e l n0=jWjk0-[/ d000g0Y00
 
           * / 
 
         p u b l i c   c l a s s   N y A R C a m e r a D i s t o r t i o n F a c t o r V 4   :   I N y A R C a m e r a D i s t o r t i o n F a c t o r 
 
         { 
 
                 p u b l i c   c o n s t   i n t   N U M _ O F _ F A C T O R   =   9 ; 
 
                 p r i v a t e   d o u b l e   _ k 1 ; 
 
                 p r i v a t e   d o u b l e   _ k 2 ; 
 
                 p r i v a t e   d o u b l e   _ p 1 ; 
 
                 p r i v a t e   d o u b l e   _ p 2 ; 
 
                 p r i v a t e   d o u b l e   _ f x ; 
 
                 p r i v a t e   d o u b l e   _ f y ; 
 
                 p r i v a t e   d o u b l e   _ x 0 ; 
 
                 p r i v a t e   d o u b l e   _ y 0 ; 
 
                 p r i v a t e   d o u b l e   _ s ; 
 
                 p r i v a t e   d o u b l e   g e t S i z e F a c t o r ( d o u b l e   x 0 ,   d o u b l e   y 0 ,   i n t   x s i z e ,   i n t   y s i z e ) 
 
                 { 
 
                         d o u b l e   o x ,   o y ; 
 
                         d o u b l e   o l e n ,   i l e n ; 
 
                         d o u b l e   s f 1 ; 
 
 
 
                         d o u b l e   s f   =   1 0 0 . 0 ; 
 
 
 
                         o x   =   0 . 0 ; 
 
                         o y   =   y 0 ; 
 
                         o l e n   =   x 0 ; 
 
                         N y A R D o u b l e P o i n t 2 d   i t m p   =   n e w   N y A R D o u b l e P o i n t 2 d ( ) ; 
 
                         t h i s . o b s e r v 2 I d e a l ( o x ,   o y ,   i t m p ) ; 
 
                         i l e n   =   x 0   -   i t m p . x ; 
 
                         / / p r i n t f ( " O l e n   =   % f ,   I l e n   =   % f ,   s   =   % f \ n " ,   o l e n ,   i l e n ,   i l e n   /   o l e n ) ; 
 
                         i f   ( i l e n   >   0 ) 
 
                         { 
 
                                 s f 1   =   i l e n   /   o l e n ; 
 
                                 i f   ( s f 1   <   s f )   s f   =   s f 1 ; 
 
                         } 
 
 
 
                         o x   =   x s i z e ; 
 
                         o y   =   y 0 ; 
 
                         o l e n   =   x s i z e   -   x 0 ; 
 
                         t h i s . o b s e r v 2 I d e a l ( o x ,   o y ,   i t m p ) ; 
 
                         i l e n   =   i t m p . x   -   x 0 ; 
 
                         / / p r i n t f ( " O l e n   =   % f ,   I l e n   =   % f ,   s   =   % f \ n " ,   o l e n ,   i l e n ,   i l e n   /   o l e n ) ; 
 
                         i f   ( i l e n   >   0 ) 
 
                         { 
 
                                 s f 1   =   i l e n   /   o l e n ; 
 
                                 i f   ( s f 1   <   s f )   s f   =   s f 1 ; 
 
                         } 
 
 
 
                         o x   =   x 0 ; 
 
                         o y   =   0 . 0 ; 
 
                         o l e n   =   y 0 ; 
 
                         t h i s . o b s e r v 2 I d e a l ( o x ,   o y ,   i t m p ) ; 
 
                         i l e n   =   y 0   -   i t m p . y ; 
 
                         / / p r i n t f ( " O l e n   =   % f ,   I l e n   =   % f ,   s   =   % f \ n " ,   o l e n ,   i l e n ,   i l e n   /   o l e n ) ; 
 
                         i f   ( i l e n   >   0 ) 
 
                         { 
 
                                 s f 1   =   i l e n   /   o l e n ; 
 
                                 i f   ( s f 1   <   s f )   s f   =   s f 1 ; 
 
                         } 
 
 
 
                         o x   =   x 0 ; 
 
                         o y   =   y s i z e ; 
 
                         o l e n   =   y s i z e   -   y 0 ; 
 
                         t h i s . o b s e r v 2 I d e a l ( o x ,   o y ,   i t m p ) ; 
 
                         i l e n   =   i t m p . y   -   y 0 ; 
 
                         / / p r i n t f ( " O l e n   =   % f ,   I l e n   =   % f ,   s   =   % f \ n " ,   o l e n ,   i l e n ,   i l e n   /   o l e n ) ; 
 
                         i f   ( i l e n   >   0 ) 
 
                         { 
 
                                 s f 1   =   i l e n   /   o l e n ; 
 
                                 i f   ( s f 1   <   s f )   s f   =   s f 1 ; 
 
                         } 
 
 
 
 
 
                         o x   =   0 . 0 ; 
 
                         o y   =   0 . 0 ; 
 
                         t h i s . o b s e r v 2 I d e a l ( o x ,   o y ,   i t m p ) ; 
 
                         i l e n   =   x 0   -   i t m p . x ; 
 
                         o l e n   =   x 0 ; 
 
                         i f   ( i l e n   >   0 ) 
 
                         { 
 
                                 s f 1   =   i l e n   /   o l e n ; 
 
                                 i f   ( s f 1   <   s f )   s f   =   s f 1 ; 
 
                         } 
 
                         i l e n   =   y 0   -   i t m p . y ; 
 
                         o l e n   =   y 0 ; 
 
                         i f   ( i l e n   >   0 ) 
 
                         { 
 
                                 s f 1   =   i l e n   /   o l e n ; 
 
                                 i f   ( s f 1   <   s f )   s f   =   s f 1 ; 
 
                         } 
 
 
 
                         o x   =   x s i z e ; 
 
                         o y   =   0 . 0 ; 
 
                         t h i s . o b s e r v 2 I d e a l ( o x ,   o y ,   i t m p ) ; 
 
                         i l e n   =   i t m p . x   -   x 0 ; 
 
                         o l e n   =   x s i z e   -   x 0 ; 
 
                         / / p r i n t f ( " O l e n   =   % f ,   I l e n   =   % f ,   s   =   % f \ n " ,   o l e n ,   i l e n ,   i l e n   /   o l e n ) ; 
 
                         i f   ( i l e n   >   0 ) 
 
                         { 
 
                                 s f 1   =   i l e n   /   o l e n ; 
 
                                 i f   ( s f 1   <   s f )   s f   =   s f 1 ; 
 
                         } 
 
                         i l e n   =   y 0   -   i t m p . y ; 
 
                         o l e n   =   y 0 ; 
 
                         / / p r i n t f ( " O l e n   =   % f ,   I l e n   =   % f ,   s   =   % f \ n " ,   o l e n ,   i l e n ,   i l e n   /   o l e n ) ; 
 
                         i f   ( i l e n   >   0 ) 
 
                         { 
 
                                 s f 1   =   i l e n   /   o l e n ; 
 
                                 i f   ( s f 1   <   s f )   s f   =   s f 1 ; 
 
                         } 
 
 
 
                         o x   =   0 . 0 ; 
 
                         o y   =   y s i z e ; 
 
                         t h i s . o b s e r v 2 I d e a l ( o x ,   o y ,   i t m p ) ; 
 
                         i l e n   =   x 0   -   i t m p . x ; 
 
                         o l e n   =   x 0 ; 
 
                         / / p r i n t f ( " O l e n   =   % f ,   I l e n   =   % f ,   s   =   % f \ n " ,   o l e n ,   i l e n ,   i l e n   /   o l e n ) ; 
 
                         i f   ( i l e n   >   0 ) 
 
                         { 
 
                                 s f 1   =   i l e n   /   o l e n ; 
 
                                 i f   ( s f 1   <   s f )   s f   =   s f 1 ; 
 
                         } 
 
                         i l e n   =   i t m p . y   -   y 0 ; 
 
                         o l e n   =   y s i z e   -   y 0 ; 
 
                         / / p r i n t f ( " O l e n   =   % f ,   I l e n   =   % f ,   s   =   % f \ n " ,   o l e n ,   i l e n ,   i l e n   /   o l e n ) ; 
 
                         i f   ( i l e n   >   0 ) 
 
                         { 
 
                                 s f 1   =   i l e n   /   o l e n ; 
 
                                 i f   ( s f 1   <   s f )   s f   =   s f 1 ; 
 
                         } 
 
 
 
                         o x   =   x s i z e ; 
 
                         o y   =   y s i z e ; 
 
                         t h i s . o b s e r v 2 I d e a l ( o x ,   o y ,   i t m p ) ; 
 
                         i l e n   =   i t m p . x   -   x 0 ; 
 
                         o l e n   =   x s i z e   -   x 0 ; 
 
                         / / p r i n t f ( " O l e n   =   % f ,   I l e n   =   % f ,   s   =   % f \ n " ,   o l e n ,   i l e n ,   i l e n   /   o l e n ) ; 
 
                         i f   ( i l e n   >   0 ) 
 
                         { 
 
                                 s f 1   =   i l e n   /   o l e n ; 
 
                                 i f   ( s f 1   <   s f )   s f   =   s f 1 ; 
 
                         } 
 
                         i l e n   =   i t m p . y   -   y 0 ; 
 
                         o l e n   =   y s i z e   -   y 0 ; 
 
                         / / p r i n t f ( " O l e n   =   % f ,   I l e n   =   % f ,   s   =   % f \ n " ,   o l e n ,   i l e n ,   i l e n   /   o l e n ) ; 
 
                         i f   ( i l e n   >   0 ) 
 
                         { 
 
                                 s f 1   =   i l e n   /   o l e n ; 
 
                                 i f   ( s f 1   <   s f ) 
 
                                 { 
 
                                         s f   =   s f 1 ; 
 
                                 } 
 
                         } 
 
 
 
                         i f   ( s f   = =   1 0 0 . 0 ) 
 
                         { 
 
                                 s f   =   1 . 0 ; 
 
                         } 
 
 
 
                         r e t u r n   s f ; 
 
                 } 
 
 
 
                 p u b l i c   d o u b l e   g e t S ( ) 
 
                 { 
 
                         r e t u r n   t h i s . _ s ; 
 
                 }               / * * 
 
                   *   S0n0peo00SgqCQK00jk000000$P0000W0~0Y00
 
                   *   @ p a r a m   i _ r e f 
 
                   *   000CQn00000000
 
                   * / 
 
                 p u b l i c   v o i d   c o p y F r o m ( I N y A R C a m e r a D i s t o r t i o n F a c t o r   i _ r e f ) 
 
                 { 
 
                         N y A R C a m e r a D i s t o r t i o n F a c t o r V 4   s r c   =   ( N y A R C a m e r a D i s t o r t i o n F a c t o r V 4 ) i _ r e f ; 
 
                         t h i s . _ k 1   =   s r c . _ k 1 ; 
 
                         t h i s . _ k 2   =   s r c . _ k 2 ; 
 
                         t h i s . _ p 1   =   s r c . _ p 1 ; 
 
                         t h i s . _ p 2   =   s r c . _ p 2 ; 
 
                         t h i s . _ f x   =   s r c . _ f x ; 
 
                         t h i s . _ f y   =   s r c . _ f y ; 
 
                         t h i s . _ x 0   =   s r c . _ x 0 ; 
 
                         t h i s . _ y 0   =   s r c . _ y 0 ; 
 
                         t h i s . _ s   =   s r c . _ s ; 
 
                 } 
 
                 / * * 
 
                   *   @ p a r a m   i _ s i z e 
 
                   *   @ p a r a m   i _ i n t r i n s i c _ m a t r i x 
 
                   *   3 x 3   m a t r i x 
 
                   *   S0n000000o00O p e n C V n0c v C a l i b r a t e C a m e r a 2 peL0QRY00i n t r i n s i c _ m a t r i x n0$Ph0TW0~0Y00
 
                   *   @ p a r a m   i _ d i s t o r t i o n _ c o e f f s 
 
                   *   4 x 1   v e c t o r 
 
                   *   S0n000000o00O p e n C V n0c v C a l i b r a t e C a m e r a 2 peL0QRY00d i s t o r t i o n _ c o e f f s n0$Ph0TW0~0Y00
 
                   * / 
 
                 p u b l i c   v o i d   s e t V a l u e ( N y A R I n t S i z e   i _ s i z e ,   d o u b l e [ ]   i _ i n t r i n s i c _ m a t r i x ,   d o u b l e [ ]   i _ d i s t o r t i o n _ c o e f f s ) 
 
                 { 
 
                         t h i s . _ k 1   =   i _ d i s t o r t i o n _ c o e f f s [ 0 ] ; 
 
                         t h i s . _ k 2   =   i _ d i s t o r t i o n _ c o e f f s [ 1 ] ; 
 
                         t h i s . _ p 1   =   i _ d i s t o r t i o n _ c o e f f s [ 2 ] ; 
 
                         t h i s . _ p 2   =   i _ d i s t o r t i o n _ c o e f f s [ 3 ] ; 
 
                         t h i s . _ f x   =   i _ i n t r i n s i c _ m a t r i x [ 0   *   3   +   0 ] ; / / 0 , 0 
 
                         t h i s . _ f y   =   i _ i n t r i n s i c _ m a t r i x [ 1   *   3   +   1 ] ; / / 1 , 1 
 
                         t h i s . _ x 0   =   i _ i n t r i n s i c _ m a t r i x [ 0   *   3   +   2 ] ; / / 0 , 2 
 
                         t h i s . _ y 0   =   i _ i n t r i n s i c _ m a t r i x [ 1   *   3   +   2 ] ; / / 1 , 2 
 
                         t h i s . _ s   =   1 . 0 ; 
 
                         / / u p d a t e   s 
 
                         t h i s . _ s   =   t h i s . g e t S i z e F a c t o r ( t h i s . _ x 0 ,   t h i s . _ y 0 ,   i _ s i z e . w ,   i _ s i z e . h ) ; 
 
                 } 
 
                 / * * 
 
                   *   S0n0peo00MRn0$P0jk000000$Ph0W0f00S0n0000000k0000W0~0Y00
 
                   *   @ p a r a m   i _ f a c t o r 
 
                   *   jk000000$P0<h
}W0_0MR0
 
                   * / 
 
                 p u b l i c   v o i d   s e t V a l u e ( d o u b l e [ ]   i _ f a c t o r ) 
 
                 { 
 
                         t h i s . _ k 1   =   i _ f a c t o r [ 0 ] ; 
 
                         t h i s . _ k 2   =   i _ f a c t o r [ 1 ] ; 
 
                         t h i s . _ p 1   =   i _ f a c t o r [ 2 ] ; 
 
                         t h i s . _ p 2   =   i _ f a c t o r [ 3 ] ; 
 
                         t h i s . _ f x   =   i _ f a c t o r [ 4 ] ; 
 
                         t h i s . _ f y   =   i _ f a c t o r [ 5 ] ; 
 
                         t h i s . _ x 0   =   i _ f a c t o r [ 6 ] ; 
 
                         t h i s . _ y 0   =   i _ f a c t o r [ 7 ] ; 
 
                         t h i s . _ s   =   i _ f a c t o r [ 8 ] ; 
 
                 } 
 
 
 
                 / * * 
 
                   *   S0n0peo0000000$P0MRx0ԏW0~0Y00
 
                   *   o _ f a c t o r k0o0 }pe{ @ l i n k   # N U M _ O F _ F A C T O R } n0
 
                   *   @ p a r a m   o _ f a c t o r 
 
                   *   jk000000$Pn0QRHQMR0
 
                   * / 
 
                 p u b l i c   v o i d   g e t V a l u e ( d o u b l e [ ]   o _ f a c t o r ) 
 
                 { 
 
                         o _ f a c t o r [ 0 ]   =   t h i s . _ k 1 ; 
 
                         o _ f a c t o r [ 1 ]   =   t h i s . _ k 2 ; 
 
                         o _ f a c t o r [ 2 ]   =   t h i s . _ p 1 ; 
 
                         o _ f a c t o r [ 3 ]   =   t h i s . _ p 2 ; 
 
                         o _ f a c t o r [ 4 ]   =   t h i s . _ f x ; 
 
                         o _ f a c t o r [ 5 ]   =   t h i s . _ f y ; 
 
                         o _ f a c t o r [ 6 ]   =   t h i s . _ x 0 ; 
 
                         o _ f a c t o r [ 7 ]   =   t h i s . _ y 0 ; 
 
                         o _ f a c t o r [ 8 ]   =   t h i s . _ s ; 
 
                 } 
 
 
 
                 / * * 
 
                   * / 
 
                 p u b l i c   v o i d   c h a n g e S c a l e ( d o u b l e   i _ x _ s c a l e ,   d o u b l e   i _ y _ s c a l e ) 
 
                 { 
 
                         t h i s . _ f x   =   t h i s . _ f x   *   i _ x _ s c a l e ;       / *     f x     * / 
 
                         t h i s . _ f y   =   t h i s . _ f y   *   i _ y _ s c a l e ;       / *     f y     * / 
 
                         t h i s . _ x 0   =   t h i s . _ x 0   *   i _ x _ s c a l e ;       / *     x 0     * / 
 
                         t h i s . _ y 0   =   t h i s . _ y 0   *   i _ y _ s c a l e ;       / *     y 0     * / 
 
                         r e t u r n ; 
 
                 } 
 
 
 
                 p u b l i c   v o i d   i d e a l 2 O b s e r v ( N y A R D o u b l e P o i n t 2 d   i _ i n ,   N y A R D o u b l e P o i n t 2 d   o _ o u t ) 
 
                 { 
 
                         t h i s . i d e a l 2 O b s e r v ( i _ i n . x ,   i _ i n . y ,   o _ o u t ) ; 
 
                 } 
 
 
 
                 p u b l i c   v o i d   i d e a l 2 O b s e r v ( N y A R D o u b l e P o i n t 2 d   i _ i n ,   N y A R I n t P o i n t 2 d   o _ o u t ) 
 
                 { 
 
                         t h i s . i d e a l 2 O b s e r v ( i _ i n . x ,   i _ i n . y ,   o _ o u t ) ; 
 
                 } 
 
 
 
                 / * * 
 
                   *   S0n0peo00^jp0t`^j|K00[^j|x0	YcW0~0Y00
 
                   *   @ p a r a m   i _ i n 
 
                   *   	YcCQn0^j
 
                   *   @ p a r a m   o _ o u t 
 
                   *   	Yc_n0^j0SQ0S0000000
 
                   * / 
 
                 p u b l i c   v o i d   i d e a l 2 O b s e r v ( d o u b l e   i _ x ,   d o u b l e   i _ y ,   N y A R D o u b l e P o i n t 2 d   o _ o u t ) 
 
                 { 
 
                         d o u b l e   k 1   =   t h i s . _ k 1 ; 
 
                         d o u b l e   k 2   =   t h i s . _ k 2 ; 
 
                         d o u b l e   p 1   =   t h i s . _ p 1 ; 
 
                         d o u b l e   p 2   =   t h i s . _ p 2 ; 
 
                         d o u b l e   f x   =   t h i s . _ f x ; 
 
                         d o u b l e   f y   =   t h i s . _ f y ; 
 
                         d o u b l e   x 0   =   t h i s . _ x 0 ; 
 
                         d o u b l e   y 0   =   t h i s . _ y 0 ; 
 
                         d o u b l e   s   =   t h i s . _ s ; 
 
 
 
                         d o u b l e   x   =   ( i _ x   -   x 0 )   *   s   /   f x ; 
 
                         d o u b l e   y   =   ( i _ y   -   y 0 )   *   s   /   f y ; 
 
                         d o u b l e   l   =   x   *   x   +   y   *   y ; 
 
                         o _ o u t . x   =   ( x   *   ( 1 . 0   +   k 1   *   l   +   k 2   *   l   *   l )   +   2 . 0   *   p 1   *   x   *   y   +   p 2   *   ( l   +   2 . 0   *   x   *   x ) )   *   f x   +   x 0 ; 
 
                         o _ o u t . y   =   ( y   *   ( 1 . 0   +   k 1   *   l   +   k 2   *   l   *   l )   +   p 1   *   ( l   +   2 . 0   *   y   *   y )   +   2 . 0   *   p 2   *   x   *   y )   *   f y   +   y 0 ; 
 
                 } 
 
 
 
 
 
                 p u b l i c   v o i d   i d e a l 2 O b s e r v ( d o u b l e   i _ x ,   d o u b l e   i _ y ,   N y A R I n t P o i n t 2 d   o _ o u t ) 
 
                 { 
 
                         d o u b l e   k 1   =   t h i s . _ k 1 ; 
 
                         d o u b l e   k 2   =   t h i s . _ k 2 ; 
 
                         d o u b l e   p 1   =   t h i s . _ p 1 ; 
 
                         d o u b l e   p 2   =   t h i s . _ p 2 ; 
 
                         d o u b l e   f x   =   t h i s . _ f x ; 
 
                         d o u b l e   f y   =   t h i s . _ f y ; 
 
                         d o u b l e   x 0   =   t h i s . _ x 0 ; 
 
                         d o u b l e   y 0   =   t h i s . _ y 0 ; 
 
                         d o u b l e   s   =   t h i s . _ s ; 
 
 
 
                         d o u b l e   x   =   ( i _ x   -   x 0 )   *   s   /   f x ; 
 
                         d o u b l e   y   =   ( i _ y   -   y 0 )   *   s   /   f y ; 
 
                         d o u b l e   l   =   x   *   x   +   y   *   y ; 
 
                         o _ o u t . x   =   ( i n t ) ( ( x   *   ( 1 . 0   +   k 1   *   l   +   k 2   *   l   *   l )   +   2 . 0   *   p 1   *   x   *   y   +   p 2   *   ( l   +   2 . 0   *   x   *   x ) )   *   f x   +   x 0 ) ; 
 
                         o _ o u t . y   =   ( i n t ) ( ( y   *   ( 1 . 0   +   k 1   *   l   +   k 2   *   l   *   l )   +   p 1   *   ( l   +   2 . 0   *   y   *   y )   +   2 . 0   *   p 2   *   x   *   y )   *   f y   +   y 0 ) ; 
 
                 } 
 
 
 
                 / * * 
 
                   *   S0n0peo00pen0^jp00 NbW0f0t`^j|K00[^j|x0	YcW0~0Y00
 
                   *   i _ i n h0o _ o u t k0o00TX00000000c[g0M0~0Y00
 
                   *   @ p a r a m   i _ i n 
 
                   *   	YcCQn0^jMR
 
                   *   @ p a r a m   o _ o u t 
 
                   *   	Yc_n0^j0SQ0S0MR
 
                   *   @ p a r a m   i _ s i z e 
 
                   *   	YcY00^jn0Ppe0
 
                   *   @ t o d o   s h o u l d   o p t i m i z e ! 
 
                   * / 
 
                 p u b l i c   v o i d   i d e a l 2 O b s e r v B a t c h ( N y A R D o u b l e P o i n t 2 d [ ]   i _ i n ,   N y A R D o u b l e P o i n t 2 d [ ]   o _ o u t ,   i n t   i _ s i z e ) 
 
                 { 
 
                         d o u b l e   k 1   =   t h i s . _ k 1 ; 
 
                         d o u b l e   k 2   =   t h i s . _ k 2 ; 
 
                         d o u b l e   p 1   =   t h i s . _ p 1 ; 
 
                         d o u b l e   p 2   =   t h i s . _ p 2 ; 
 
                         d o u b l e   f x   =   t h i s . _ f x ; 
 
                         d o u b l e   f y   =   t h i s . _ f y ; 
 
                         d o u b l e   x 0   =   t h i s . _ x 0 ; 
 
                         d o u b l e   y 0   =   t h i s . _ y 0 ; 
 
                         d o u b l e   s   =   t h i s . _ s ; 
 
 
 
                         f o r   ( i n t   i   =   0 ;   i   <   i _ s i z e ;   i + + ) 
 
                         { 
 
                                 d o u b l e   x   =   ( i _ i n [ i ] . x   -   x 0 )   *   s   /   f x ; 
 
                                 d o u b l e   y   =   ( i _ i n [ i ] . y   -   y 0 )   *   s   /   f y ; 
 
                                 d o u b l e   l   =   x   *   x   +   y   *   y ; 
 
                                 o _ o u t [ i ] . x   =   ( ( x   *   ( 1 . 0   +   k 1   *   l   +   k 2   *   l   *   l )   +   2 . 0   *   p 1   *   x   *   y   +   p 2   *   ( l   +   2 . 0   *   x   *   x ) )   *   f x   +   x 0 ) ; 
 
                                 o _ o u t [ i ] . y   =   ( ( y   *   ( 1 . 0   +   k 1   *   l   +   k 2   *   l   *   l )   +   p 1   *   ( l   +   2 . 0   *   y   *   y )   +   2 . 0   *   p 2   *   x   *   y )   *   f y   +   y 0 ) ; 
 
                         } 
 
                         r e t u r n ; 
 
                 } 
 
 
 
                 / * * 
 
                   *   S0n0peo00pen0^jp00 NbW0f0t`^j|K00[^j|x0	YcW0~0Y00
 
                   *   i _ i n h0o _ o u t k0o00TX00000000c[g0M0~0Y00
 
                   *   @ p a r a m   i _ i n 
 
                   *   	YcCQn0^jMR
 
                   *   @ p a r a m   o _ o u t 
 
                   *   	Yc_n0^j0SQ0S0MR
 
                   *   @ p a r a m   i _ s i z e 
 
                   *   	YcY00^jn0Ppe0
 
                   *   @ t o d o   s h o u l d   o p t i m i z e ! 
 
                   * / 
 
                 p u b l i c   v o i d   i d e a l 2 O b s e r v B a t c h ( N y A R D o u b l e P o i n t 2 d [ ]   i _ i n ,   N y A R I n t P o i n t 2 d [ ]   o _ o u t ,   i n t   i _ s i z e ) 
 
                 { 
 
                         d o u b l e   k 1   =   t h i s . _ k 1 ; 
 
                         d o u b l e   k 2   =   t h i s . _ k 2 ; 
 
                         d o u b l e   p 1   =   t h i s . _ p 1 ; 
 
                         d o u b l e   p 2   =   t h i s . _ p 2 ; 
 
                         d o u b l e   f x   =   t h i s . _ f x ; 
 
                         d o u b l e   f y   =   t h i s . _ f y ; 
 
                         d o u b l e   x 0   =   t h i s . _ x 0 ; 
 
                         d o u b l e   y 0   =   t h i s . _ y 0 ; 
 
                         d o u b l e   s   =   t h i s . _ s ; 
 
 
 
                         f o r   ( i n t   i   =   0 ;   i   <   i _ s i z e ;   i + + ) 
 
                         { 
 
                                 d o u b l e   x   =   ( i _ i n [ i ] . x   -   x 0 )   *   s   /   f x ; 
 
                                 d o u b l e   y   =   ( i _ i n [ i ] . y   -   y 0 )   *   s   /   f y ; 
 
                                 d o u b l e   l   =   x   *   x   +   y   *   y ; 
 
                                 o _ o u t [ i ] . x   =   ( i n t ) ( ( x   *   ( 1 . 0   +   k 1   *   l   +   k 2   *   l   *   l )   +   2 . 0   *   p 1   *   x   *   y   +   p 2   *   ( l   +   2 . 0   *   x   *   x ) )   *   f x   +   x 0 ) ; 
 
                                 o _ o u t [ i ] . y   =   ( i n t ) ( ( y   *   ( 1 . 0   +   k 1   *   l   +   k 2   *   l   *   l )   +   p 1   *   ( l   +   2 . 0   *   y   *   y )   +   2 . 0   *   p 2   *   x   *   y )   *   f y   +   y 0 ) ; 
 
                         } 
 
                         r e t u r n ; 
 
                 } 
 
                 p r i v a t e   c o n s t   i n t   P D _ L O O P 2   =   4 ; 
 
                 / * * 
 
                   *   S0n0peo00^j0[^j|K00t`^j|x0	YcW0~0Y00
 
                   *   @ p a r a m   i x 
 
                   *   	YcCQn0^j
 
                   *   @ p a r a m   i y 
 
                   *   	YcCQn0^j
 
                   *   @ p a r a m   o _ p o i n t 
 
                   *   	Yc_n0^j0SQ0S0000000
 
                   *   @ t o d o   s h o u l d   o p t i m i z e ! 
 
                   * / 
 
                 p u b l i c   v o i d   o b s e r v 2 I d e a l ( d o u b l e   i x ,   d o u b l e   i y ,   N y A R D o u b l e P o i n t 2 d   o _ p o i n t ) 
 
                 { 
 
                         / /   O p e n C V   d i s t o r t i o n   m o d e l ,   w i t h   a d d i t i o n   o f   a   s c a l e   f a c t o r   s o   t h a t 
 
                         / /   e n t i r e   i m a g e   f i t s   o n s c r e e n . 
 
 
 
                         d o u b l e   k 1   =   t h i s . _ k 1 ; 
 
                         d o u b l e   k 2   =   t h i s . _ k 2 ; 
 
                         d o u b l e   p 1   =   t h i s . _ p 1 ; 
 
                         d o u b l e   p 2   =   t h i s . _ p 2 ; 
 
                         d o u b l e   f x   =   t h i s . _ f x ; 
 
                         d o u b l e   f y   =   t h i s . _ f y ; 
 
                         d o u b l e   x 0   =   t h i s . _ x 0 ; 
 
                         d o u b l e   y 0   =   t h i s . _ y 0 ; 
 
 
 
                         d o u b l e   p x   =   ( i x   -   x 0 )   /   f x ; 
 
                         d o u b l e   p y   =   ( i y   -   y 0 )   /   f y ; 
 
 
 
                         d o u b l e   x 0 2   =   p x   *   p x ; 
 
                         d o u b l e   y 0 2   =   p y   *   p y ; 
 
 
 
                         f o r   ( i n t   i   =   1 ;   ;   i + + ) 
 
                         { 
 
                                 i f   ( x 0 2   ! =   0 . 0   | |   y 0 2   ! =   0 . 0 ) 
 
                                 { 
 
                                         p x   =   p x   -   ( ( 1 . 0   +   k 1   *   ( x 0 2   +   y 0 2 )   +   k 2   *   ( x 0 2   +   y 0 2 )   *   ( x 0 2   +   y 0 2 ) )   *   p x   +   2 . 0   *   p 1   *   p x   *   p y   +   p 2   *   ( x 0 2   +   y 0 2   +   2 . 0   *   x 0 2 )   -   ( ( i x   -   x 0 )   /   f x ) )   /   ( 1 . 0   +   k 1   *   ( 3 . 0   *   x 0 2   +   y 0 2 )   +   k 2   *   ( 5 . 0   *   x 0 2   *   x 0 2   +   3 . 0   *   x 0 2   *   y 0 2   +   y 0 2   *   y 0 2 )   +   2 . 0   *   p 1   *   p y   +   6 . 0   *   p 2   *   p x ) ; 
 
                                         p y   =   p y   -   ( ( 1 . 0   +   k 1   *   ( x 0 2   +   y 0 2 )   +   k 2   *   ( x 0 2   +   y 0 2 )   *   ( x 0 2   +   y 0 2 ) )   *   p y   +   p 1   *   ( x 0 2   +   y 0 2   +   2 . 0   *   y 0 2 )   +   2 . 0   *   p 2   *   p x   *   p y   -   ( ( i y   -   y 0 )   /   f y ) )   /   ( 1 . 0   +   k 1   *   ( x 0 2   +   3 . 0   *   y 0 2 )   +   k 2   *   ( x 0 2   *   x 0 2   +   3 . 0   *   x 0 2   *   y 0 2   +   5 . 0   *   y 0 2   *   y 0 2 )   +   6 . 0   *   p 1   *   p y   +   2 . 0   *   p 2   *   p x ) ; 
 
                                 } 
 
                                 e l s e 
 
                                 { 
 
                                         p x   =   0 . 0 ; 
 
                                         p y   =   0 . 0 ; 
 
                                         b r e a k ; 
 
                                 } 
 
                                 i f   ( i   = =   P D _ L O O P 2 )   b r e a k ; 
 
 
 
                                 x 0 2   =   p x   *   p x ; 
 
                                 y 0 2   =   p y   *   p y ; 
 
                         } 
 
 
 
                         o _ p o i n t . x   =   p x   *   f x   /   t h i s . _ s   +   x 0 ; 
 
                         o _ p o i n t . y   =   p y   *   f y   /   t h i s . _ s   +   y 0 ; 
 
 
 
                         r e t u r n ; 
 
                 } 
 
 
 
                 p u b l i c   v o i d   o b s e r v 2 I d e a l ( N y A R D o u b l e P o i n t 2 d   i _ i n ,   N y A R D o u b l e P o i n t 2 d   o _ p o i n t ) 
 
                 { 
 
                         t h i s . o b s e r v 2 I d e a l ( i _ i n . x ,   i _ i n . y ,   o _ p o i n t ) ; 
 
                 } 
 
 
 
                 / * * 
 
                   *   ^jMRhQf0k0[W0f00{ @ l i n k   # o b s e r v 2 I d e a l ( d o u b l e ,   d o u b l e ,   N y A R D o u b l e P o i n t 2 d ) } 0i_W0~0Y00
 
                   *   @ p a r a m   i _ i n 
 
                   *   @ p a r a m   o _ o u t 
 
                   *   @ p a r a m   i _ s i z e 
 
                   * / 
 
                 p u b l i c   v o i d   o b s e r v 2 I d e a l B a t c h ( N y A R D o u b l e P o i n t 2 d [ ]   i _ i n ,   N y A R D o u b l e P o i n t 2 d [ ]   o _ o u t ,   i n t   i _ s i z e ) 
 
                 { 
 
                         f o r   ( i n t   i   =   i _ s i z e   -   1 ;   i   > =   0 ;   i - - ) 
 
                         { 
 
                                 t h i s . o b s e r v 2 I d e a l ( i _ i n [ i ] . x ,   i _ i n [ i ] . y ,   o _ o u t [ i ] ) ; 
 
                         } 
 
                         r e t u r n ; 
 
                 } 
 
 
 
 
 
         } 
 
 } 