anisotropictensordistance.h.svn-base

fast marching method

/*------------------------------------------------------------------------------------------------------
  
  File        : AnisotropicTensorDistance.h     (GCM Library)

  Description : This class specifies the virtual methods of the class "GenericPradosSchemesForFastMarching_3D" for the special case of the 3D anisotropic Eikonal equation.

  Authors      : Emmanuel Prados (UCLA/INRIA), Christophe Lenglet (INRIA), Jean-Philippe Pons (INRIA)
  
  --------------

  License     : This software is governed by the CeCILL-C license under French law and abiding by the 
  rules of distribution of free software. 

  Users can use, modify and/ or redistribute the software under the terms of the CeCILL-C. In particular, 
  the exercising of this right is conditional upon the obligation to make available to the community the 
  modifications made to the source code of the software so as to contribute to its evolution (e.g. by the 
  mean of the web; i.e. by publishing a web page!).
  
  In this respect, the risks associated with loading, using, modifying and/or developing or reproducing 
  the software by the user are brought to the user's attention, given its Free Software status, which may 
  make it complicated to use, with the result that its use is reserved for developers and experienced 
  professionals having in-depth computer knowledge. Users are therefore encouraged to load and test the 
  suitability of the software as regards their requirements in conditions enabling the security of their
  systems and/or data to be ensured and, more generally, to use and operate it in the same conditions of 
  security. This Agreement may be freely reproduced and published, provided it is not altered, and that 
  no provisions are either added or removed herefrom. 
  
  CeCILL-C FREE SOFTWARE LICENSE AGREEMENT is available in the file 
                          Licence_CeCILL-C_V1-en.txt 
  or at  
                          http://www.cecill.info/index.en.html.

  This Agreement may apply to any or all software for which the holder of the economic rights decides to 
  submit the use thereof to its provisions.
  
  --------------

  Associated publications  : This C++ code corresponds to the implementation of the algorithm presented 
  in the following articles:  
  - E. Prados, C. Lenglet, J.P. Pons, N. Wotawa, R. Deriche, O. Faugeras, S. Soatto; 
    "Control Theory and Fast Marching Methods for Brain Connectivity Mapping"; 
    INRIA Research Report 5845 -- UCLA Computer Science Department Technical Report 060004, February 2006.
  - E. Prados, C. Lenglet, J.P. Pons, N. Wotawa, R. Deriche, O. Faugeras, S. Soatto; 
    "Control Theory and Fast Marching Methods for Brain Connectivity Mapping";
    Proc. IEEE Computer Society Conference on Computer Vision and Pattern Recognition, New York, NY, I: 1076-1083, June 17-22, 2006.  
  - For more references, we refer to the official web page of the GCM Library and to authors' web pages.

  Please, if you use the GCM library in you work, make sure you will include the reference to the work of 
  the authors in your publications.

  --------------
  
  Technical Comments and Detailled description:

  - This class specifies the virtual methods of the class "GenericPradosSchemesForFastMarching_3D" 
  for the special case of the 3D anisotropic Eikonal equation.
  The functions we overcharge are:
   * virtual bool eqSolverOnPart_with_s1s2s3_nonNul(...)
   * virtual bool eqSolverOnPart_withOne_si_Nul(...)
   * virtual bool eqSolverOnPart_withTwo_si_Nul(...)

  $(s_1,s_2,s_3)$ being fixed,  these methods allow to solve the equations   
  $$ sup_{a in B} g_s(a,t) = 0 $$
  where $g_s(a,t) = - f(x; a).P_{x,s,U}(t)-l(x; a)$ 
  and where, according to the different cases, 
  * $B$ is the interior set of the $As$   (eqSolverOnPart_with_s1s2s3_nonNu)
  * $B$ is the union of the 2D facets of $As$  (eqSolverOnPart_withOne_si_Nul)
  * $B$ is the union of the 1D edges of $As$  (eqSolverOnPart_withTwo_si_Nul)
  See section 4.3.2 of INRIA RR-5845 (page 11).
  Let us note that these methods compute the $t_s$ and the associated dynamics.

  - The goal of the class being to specify the virtual methods of the class  
  "GenericPradosSchemesForFastMarching_3D" for the special case of the 3D anisotropic Eikonal equation,
  we then need to describe the data defining the Hamiltonian.
  In this class, we then need to declare and construct all the associated structures.

  - Note about the optimal dynamics:
  The optimal dymamics (so its preservation and its transmission in the paramters) ARE NOT USEFULL IF we
  only want to compute the viscosity solution of the considered equation by the Fast Marching Method. 
  Nevertheless the computation of the optimal dymamics is necessary in order to be able to chose the good 
  simplex. The Preservation and the transmission of the optimal Dymamics can be usefull in many applications 
  as for example fibers tracking in DTI (see INRIA RR-5845).


  ----------------------------------------------------------------------------------------------------*/




#ifndef ANISOTROPICTENSORDISTANCE_H
#define ANISOTROPICTENSORDISTANCE_H

#include <iostream>
#include <cmath>
#include <cassert>
#include "GenericPradosSchemesForFastMarching_3D.h"

/**************************************************/
//
//  This class (AnisotropicTensorDistance) inherits of
//  the class: "PradosSchemesForFastMarching_3D"
//
//  Here we have to overcharge the functions
//   - virtual bool eqSolverOnPart_with_s1s2s3_nonNul(...)
//   - virtual bool eqSolverOnPart_withOne_si_Nul(...)
//   - virtual bool eqSolverOnPart_withTwo_si_Nul(...)
//
/**************************************************/




namespace FastLevelSet {


/**************************************************
  The class AnisotropicTensorDistance inherits of
  the class: "PradosSchemesForFastMarching_3D"
**************************************************/

    template <typename T = float>
    class AnisotropicTensorDistance : public PradosSchemesForFastMarching_3D<T> {

        /***************************************************************
           Let us declare all the structures describing the data
           charaterizing the 3D Eikonal equation considered.
           We also declare some variables required to describe the scheme.
        ****************************************************************/


    protected:
    T dx,dy,dz;                               // voxel sizes
        typedef T Vector3[3];
        typedef T Matrix3x3[3][3];
        typedef T Matrix2x2[2][2];

        // we start with the declaration of the class "Hamiltonian".

          /***************************************************************
            The Hamiltonian that we have implemented here is
                           $  |A_x p |^2 - 1  $.
            The Hamiltonian is equivalent to the Hamiltonian
                     $  H_{AEik}(x,p) = |A_x p | - 1 $
            given at page 7 of the  INRIA report RR-5845.
            The $ H_i(x,p) $  and $ H_{ij}(x,p) $ described at page 12 (section 4.3.3) of the INRIA report RR-5845
            are, in fact, the ones associated to $|A_x p |^2 - 1$.
            There are no differences, except that we need to renormalize the optimal dynamics. 
            We will explain better this point in our forthcomming journal paper...
          ****************************************************************/

        class Hamiltonian {
        public:
            Matrix3x3 B, invB;
            Matrix2x2 invinvB1, invinvB2, invinvB3;
            T invsqinvA1, invsqinvA2, invsqinvA3;

            // Initialization from B
            bool InitFromB(const Matrix3x3 &_B) {

                // Storage of B
                CopyMatrix3x3(_B,B);

                // Computation of the inverse of B
//                 if (!InverseSymmetricMatrix3x3(B,invB)) return false;
                if (Matrix3x3IsZero(B)) return false;
                else PseudoInverseSymmetricMatrix3x3(B,invB);

                // Computation of invinvB_i: the inverse of invB after removing the i^th row and i^th column
                for (int i=0;i<2;i++) for (int j=0;j<2;j++) {
                    invinvB1[i][j] = invB[i+1][j+1];
                    invinvB2[i][j] = invB[2*i][2*j];
                    invinvB3[i][j] = invB[i][j];
                }

                if (Matrix2x2IsZero(invinvB1)) return false;
                else PseudoInverseMatrix2x2(invinvB1);

                if (Matrix2x2IsZero(invinvB2)) return false;
                else PseudoInverseMatrix2x2(invinvB2);

                if (Matrix2x2IsZero(invinvB3)) return false;
                else PseudoInverseMatrix2x2(invinvB3);

//                 if (!InverseMatrix2x2(invinvB1) || !InverseMatrix2x2(invinvB2) || !InverseMatrix2x2(invinvB3)) return false;

                // Computation of invinvAi: the inverse of the square norm of the i^th row of invA
                invsqinvA1 = 1/invB[0][0];
                invsqinvA2 = 1/invB[1][1];
                invsqinvA3 = 1/invB[2][2];

                return true;
            }

            // Initialization from A
            bool InitFromA(const Matrix3x3 &_A) {
                // TODO
                return false;
            }

        private:
            // Matrix operations
            void DisplayMatrix3x3(const Matrix3x3 &M) {
                for (int i=0;i<3;i++)
                    std::cout << M[i][0] << " " << M[i][1] << " " << M[i][2] << std::endl;
                std::cout << std::endl;
            }

            void DisplayMatrix2x2(const Matrix2x2 &M) {
                for (int i=0;i<2;i++)
                    std::cout << M[i][0] << " " << M[i][1]  << std::endl;
                std::cout << std::endl;
            }

            void MultiplyMatrix3x3(const Matrix3x3 &A, const Matrix3x3 &B, Matrix3x3 &R) {
                for (int i=0; i<3; ++i) {
                    for (int j=0; j<3; ++j) {
                        R[i][j] = 0.0;
                        for (int k=0; k<3; ++k) R[i][j] += A[i][k] * B[k][j];
                    }
                }
            }

            void MultiplyMatrix2x2(const Matrix2x2 &A, const Matrix2x2 &B, Matrix2x2 &R) {
                for (int i=0; i<2; ++i) {
                    for (int j=0; j<2; ++j) {
                        R[i][j] = 0.0;
                        for (int k=0; k<2; ++k) R[i][j] += A[i][k] * B[k][j];
                    }
                }
            }

            void CopyMatrix3x3(const Matrix3x3 &in, Matrix3x3 &out) {
                for (int i=0;i<3;i++) for (int j=0;j<3;j++) out[i][j] = in[i][j];
            }

            void CopyMatrix2x2(const Matrix2x2 &in, Matrix2x2 &out) {
                for (int i=0;i<2;i++) for (int j=0;j<2;j++) out[i][j] = in[i][j];
            }

            void TransposeMatrix2x2(const Matrix2x2 &in, Matrix2x2 &out) {
                for (int i=0;i<2;i++) for (int j=0;j<2;j++) out[i][j] = in[j][i];
            }

            void TransposeMatrix3x3(const Matrix3x3 &in, Matrix3x3 &out) {
                for (int i=0;i<3;i++) for (int j=0;j<3;j++) out[i][j] = in[j][i];
            }

            bool Matrix2x2IsZero(const Matrix2x2 &M) {
                return (M[0][0]==0 && M[0][1]==0 && M[1][1]==0);
            }

            bool Matrix3x3IsZero(const Matrix3x3 &M) {
                return (M[0][0]==0 && M[0][1]==0 && M[0][2]==0 && M[1][1]==0 && M[1][2]==0 && M[2][2]==0);
            }

            bool InverseUpperTriangularMatrix3x3(const Matrix3x3 &in, Matrix3x3 &out) {
                if (in[0][0] == 0 || in[1][1] == 0 || in[2][2] == 0) return false;
                out[0][0] = 1/in[0][0];
                out[0][1] = -in[0][1]/in[0][0]/in[1][1];
                out[0][2] = (in[0][1]*in[1][2] - in[0][2]*in[1][1])/in[0][0]/in[1][1]/in[2][2];
                out[1][0] = 0;
                out[1][1] = 1/in[1][1];
                out[1][2] = -in[1][2]/in[1][1]/in[2][2];
                out[2][0] = 0;
                out[2][1] = 0;
                out[2][2] = 1/in[2][2];
                return true;
            }

            bool InverseSymmetricMatrix3x3(const Matrix3x3 &in, Matrix3x3 &out) {

                // for computing the determinant,
                // we develop with respect to the first row:
                T firstCrossProduct  = in[1][1]*in[2][2]-in[1][2]*in[1][2];
                T secondCrossProduct = in[0][1]*in[2][2]-in[0][2]*in[1][2];
                T thirdCrossProduct  = in[0][1]*in[1][2]-in[0][2]*in[1][1];

                T determinant =
                        in[0][0]*firstCrossProduct
                    -   in[0][1]*secondCrossProduct
                    +   in[0][2]*thirdCrossProduct;

                if (determinant == 0) return false;

                out[0][0] =  (  firstCrossProduct   )/determinant;
                out[0][1] =  ( - secondCrossProduct )/determinant;
                out[0][2] =  (  thirdCrossProduct   )/determinant;

                out[1][0] = out[0][1];
                out[1][1] =  (-in[0][2]*in[0][2]+in[0][0]*in[2][2])/determinant;
                out[1][2] =  (-in[0][0]*in[1][2]+in[0][2]*in[0][1])/determinant;

                out[2][0] = out[0][2];
                out[2][1] = out[1][2];
                out[2][2] =  (-in[0][1]*in[0][1]+in[0][0]*in[1][1])/determinant;

                return true;
            }

            void PseudoInverseSymmetricMatrix3x3(const Matrix3x3 &in, Matrix3x3 &out) {
                Matrix3x3 M,N;
                MultiplyMatrix3x3(in,in,M);
                InverseSymmetricMatrix3x3(M,N);
                MultiplyMatrix3x3(N,in,out);
            }

            //bool CholeskyMatrix3x3(const Matrix3x3 &in, Matrix3x3 &out) {
            //  if (in[0][0]<0) return false;
            //  out[0][0] = (T)std::sqrt(in[0][0]);
            //  out[0][1] = in[0][1] / out[0][0];
            //  out[0][2] = in[0][2] / out[0][0];
            //  out[1][0] = 0;
            //  const T d2 = in[1][1] - out[0][1]*out[0][1];
            //  if (d2<0) return false;
            //  out[1][1] = (T)std::sqrt(d2);
            //  out[1][2] = ( in[1][2] - out[0][1]*out[0][2] ) / out[1][1];
            //  out[2][0] = 0;
            //  out[2][1] = 0;
            //  const T d3 = in[2][2] - out[0][2]*out[0][2] - out[1][2]*out[1][2];
            //  if (d3<0) return false;
            //  out[2][2] = (T)std::sqrt(d3);
            //  return true;
            //}

            bool InverseMatrix2x2(Matrix2x2 &M) {
                const T det = M[0][0]*M[1][1]-M[0][1]*M[1][0];
                if (det==0) return false;
                std::swap(M[0][0],M[1][1]);
                M[0][0] /= det;
                M[1][1] /= det;
                M[0][1] /= -det;
                M[1][0] /= -det;