
mJC[c           @   s   d  d l  Z d  d l m Z d  d l j Z d  d l Z d  d l Z d  d l	 m
 Z
 d  d l	 m Z d   Z d   Z d   Z d   Z d S(	   iN(   t   inv(   t   random_quaternion(   t   quaternion_slerpc         C   s  t  |   } d t |  d GHg  |  D] } | d ^ q& } t d   | D  | } t d   | D  | } t d   | D  | } | | | f } g  |  D] } | d ^ q } d }	 t   }
 x. | D]& } t |
 | d |	  }
 |	 d }	 q Wt |
  }
 | |
 f S(	   Ns   Computing the average of s    transformsi    c         s   s   |  ] } | d  Vq d S(   i    N(    (   t   .0t   v(    (    s\   /home/filipe/Documents/Dissertacao/Optimization/OpenConstructorOptimization/costFunctions.pys	   <genexpr>   s    c         s   s   |  ] } | d  Vq d S(   i   N(    (   R   R   (    (    s\   /home/filipe/Documents/Dissertacao/Optimization/OpenConstructorOptimization/costFunctions.pys	   <genexpr>   s    c         s   s   |  ] } | d  Vq d S(   i   N(    (   R   R   (    (    s\   /home/filipe/Documents/Dissertacao/Optimization/OpenConstructorOptimization/costFunctions.pys	   <genexpr>    s    i   g      ?(   t   lent   strt   sumR   R   t   tuple(   t   l_transformst   Nt   it   l_tt   tmp1t   tmp2t   tmp3t   avg_tt   l_qt   acct   avg_qt   q(    (    s\   /home/filipe/Documents/Dissertacao/Optimization/OpenConstructorOptimization/costFunctions.pyt   averageTransforms   s    	c         C   s  g  } | d } | d } | d } | d	 } | d
 }	 | d }
 | d } | d } | d } | d  d   d d  f } xW| D]O} t  j |  d d  d d  f  } | j |  |  d d  d f } | d | d } | d | d } | d | d } | d |	 | |
 | d | | d d | | | | | d | d } | d |	 | |
 | d | | d | | d | d d | | | } | | | } | | | } | j | | g  q Wt  j |  S(   Ni    i   i   i   i   (   i    i    (   i   i   (   i    i   (   i   i   (   i    i    (   i    i   (   i    i   (   i    i   (   i    i   (   i    i    (   i    i   (   i    i   (   i    i   (   t   npt   matrixt   dott   appendt   array(   t   Tt   Kt   Pt   distt   xypixt   fxt   fyt   cxt   cyt   k1t   k2t   p1t   p2t   k3t   pt   rott   xyzt   xlt   ylt   r_squaret   xllt   yllt   uR   (    (    s\   /home/filipe/Documents/Dissertacao/Optimization/OpenConstructorOptimization/costFunctions.pyt   points2imageFromT9   s.    








%#NNc
         C   s  | j  t |   |  |	 d j j \ }
 } } g  } t j d 7_ g  t d d  D] } d | ^ qT } | sy | } n  x!t | |  D]\ } } g  | j D]" } | j	 | j
 d k r | ^ q d } g  | j D]" } | j	 | j d k r | ^ q d } | j   } | j   } t j t |  |  } t | | | |  } d } | d d k rx-t t |   D]h } | j d | d f | | d f d | j d | d f | | d f d d d } | | } qoWn | j d d | j d d | j d d | j d d d	 } | j d d | j d d | j d d | j d d d	 } | | d d | | d d d d } | j |  q W| d
 rt j | k r| r| j |  n  t j   t j d  n  | S(   s   Cost function
    i    i   id   t   option1t   cornersi   g       @i   i   t   dog{Gz?ie   (   i    i    (   i   i    (   i   i    (   i   i    (   i    i   (   i   i   (   i   i   (   i   i   (   i    i    (   i    i   (   t
   fromVectort   listt   rawt   shapet   costFunctiont   countert   ranget   zipt   camerast   idt   camerat   arucost   arucot   getTR   t   matmulR    R2   R   t   cornerR   t	   set_ydatat   pltt   drawt   pause(   t   xR   t
   intrinsicst   Xt   Pct
   detectionst   argst   handlest
   handle_funt   st   widtht   heightt   _t   costt   nt	   multiplest	   detectiont   handleR@   RB   t   Tat   TcR   R   t   distanceFourPointsR   t   distancet   xcmt   ycm(    (    s\   /home/filipe/Documents/Dissertacao/Optimization/OpenConstructorOptimization/costFunctions.pyR:   b   s@    &	  "6BB*
c   
      C   s   d } g  | D] } | j  d k r | ^ q d } | j } | j } | j } x |  D] } g  | D] } | j  | j  k ra | ^ qa d } | | j | | j d | j | | j d | j | | j d d d } qT W| t |   }	 |	 S(   s   Compute the ground truth
    i    t   0i   i   g       @(   R?   RJ   t   yt   zR   (
   t   realPtst   compPtst   Errort   compA0t   x0t   y0t   z0t   realPtt   compPtt   AverageError(    (    s\   /home/filipe/Documents/Dissertacao/Optimization/OpenConstructorOptimization/costFunctions.pyt   computeError   s    			+&(   t   numpyR   t   numpy.linalgR    t   matplotlib.pyplott   pyplotRG   t   randomt   cv2t   transformationsR   R   R   R2   R:   Rn   (    (    (    s\   /home/filipe/Documents/Dissertacao/Optimization/OpenConstructorOptimization/costFunctions.pyt   <module>   s   	$	)	P