module Numeric.LevMar
(
Params, Samples
, Model, Jacobian
, LevMarable(levmar)
, Options(..), defaultOpts
, Constraints(..), LinearConstraints
, Info(..), StopReason(..), LevMarError(..)
) where
import Control.Monad ( return, mplus )
import Control.Exception ( Exception )
import Data.Bool ( otherwise )
import Data.Data ( Data )
import Data.Typeable ( Typeable )
import Data.Either ( Either(Left, Right) )
import Data.Eq ( Eq, (==) )
import Data.Function ( ($) )
import Data.Functor ( (<$>) )
import Data.Int ( Int )
import Data.List ( lookup, (++) )
import Data.Maybe ( Maybe(Nothing, Just), isJust, fromJust, fromMaybe )
import Data.Monoid ( Monoid, mempty, mappend )
import Data.Ord ( Ord, (<) )
import Foreign.C.Types ( CInt )
import Foreign.Marshal.Array ( allocaArray, withArray, peekArray, copyArray )
import Foreign.Ptr ( Ptr, nullPtr )
import Foreign.ForeignPtr ( ForeignPtr, newForeignPtr_, withForeignPtr )
import Foreign.Storable ( Storable )
import Prelude ( Enum, Fractional, RealFrac, Float, Double
, fromIntegral, toEnum, (), (*), error, floor
)
import System.IO ( IO )
import System.IO.Unsafe ( unsafePerformIO )
import Text.Read ( Read )
import Text.Show ( Show, show )
#if __GLASGOW_HASKELL__ >= 605
import GHC.ForeignPtr ( mallocPlainForeignPtrBytes )
import Prelude ( undefined )
import Foreign.Storable ( sizeOf )
#else
import Foreign.ForeignPtr ( mallocForeignPtrArray )
#endif
#if __GLASGOW_HASKELL__ < 700
import Prelude ( fromInteger, (>>=), (>>), fail )
#endif
import Data.Bool.Unicode ( (∧), (∨) )
import Data.Eq.Unicode ( (≢) )
import Data.Function.Unicode ( (∘) )
import Data.Packed.Matrix ( Matrix, Element, flatten, rows, reshape )
import Numeric.Container ( Container )
import Numeric.LinearAlgebra ( )
import Data.Vector.Storable ( Vector )
import qualified Data.Vector.Storable as VS ( unsafeWith, length
, unsafeFromForeignPtr
, length
)
import Bindings.LevMar ( c'LM_INFO_SZ
, withModel
, withJacobian
, c'LM_ERROR
, c'LM_ERROR_LAPACK_ERROR
, c'LM_ERROR_FAILED_BOX_CHECK
, c'LM_ERROR_MEMORY_ALLOCATION_FAILURE
, c'LM_ERROR_CONSTRAINT_MATRIX_ROWS_GT_COLS
, c'LM_ERROR_CONSTRAINT_MATRIX_NOT_FULL_ROW_RANK
, c'LM_ERROR_TOO_FEW_MEASUREMENTS
, c'LM_ERROR_SINGULAR_MATRIX
, c'LM_ERROR_SUM_OF_SQUARES_NOT_FINITE
, c'LM_INIT_MU
, c'LM_STOP_THRESH
, c'LM_DIFF_DELTA
)
import qualified Bindings.LevMar ( Model, Jacobian )
import Bindings.LevMar.CurryFriendly ( LevMarDer, LevMarDif
, LevMarBCDer, LevMarBCDif
, LevMarLecDer, LevMarLecDif
, LevMarBLecDer, LevMarBLecDif
, dlevmar_der, slevmar_der
, dlevmar_dif, slevmar_dif
, dlevmar_bc_der, slevmar_bc_der
, dlevmar_bc_dif, slevmar_bc_dif
, dlevmar_lec_der, slevmar_lec_der
, dlevmar_lec_dif, slevmar_lec_dif
, dlevmar_blec_der, slevmar_blec_der
, dlevmar_blec_dif, slevmar_blec_dif
)
type Params r = Vector r
type Samples r = Vector r
type Model r = Params r → Samples r
type Jacobian r = Params r → Matrix r
class LevMarable r where
levmar ∷ Model r
→ Maybe (Jacobian r)
→ Params r
→ Samples r
→ Int
→ Options r
→ Constraints r
→ Either LevMarError (Params r, Info r, Matrix r)
instance LevMarable Float where
levmar = gen_levmar slevmar_der
slevmar_dif
slevmar_bc_der
slevmar_bc_dif
slevmar_lec_der
slevmar_lec_dif
slevmar_blec_der
slevmar_blec_dif
instance LevMarable Double where
levmar = gen_levmar dlevmar_der
dlevmar_dif
dlevmar_bc_der
dlevmar_bc_dif
dlevmar_lec_der
dlevmar_lec_dif
dlevmar_blec_der
dlevmar_blec_dif
gen_levmar ∷ ∀ r. (Storable r, RealFrac r, Element r)
⇒ LevMarDer r
→ LevMarDif r
→ LevMarBCDer r
→ LevMarBCDif r
→ LevMarLecDer r
→ LevMarLecDif r
→ LevMarBLecDer r
→ LevMarBLecDif r
→ Model r
→ Maybe (Jacobian r)
→ Params r
→ Samples r
→ Int
→ Options r
→ Constraints r
→ Either LevMarError (Params r, Info r, Matrix r)
gen_levmar f_der
f_dif
f_bc_der
f_bc_dif
f_lec_der
f_lec_dif
f_blec_der
f_blec_dif
model mJac ps ys itMax opts (Constraints mLowBs mUpBs mWeights mLinC)
| m == 0 = Left LevMarError
| otherwise =
unsafePerformIO $ do
psFP ← fastMallocForeignPtrArray m
withForeignPtr psFP $ \psPtr → do
VS.unsafeWith ps $ \psPtrInp →
copyArray psPtr psPtrInp m
VS.unsafeWith ys $ \ysPtr →
withArray (optsToList opts) $ \optsPtr →
allocaArray c'LM_INFO_SZ $ \infoPtr → do
covarFP ← fastMallocForeignPtrArray mm
withForeignPtr covarFP $ \covarPtr →
let cmodel ∷ Bindings.LevMar.Model r
cmodel parPtr hxPtr _ _ _ = do
parFP ← newForeignPtr_ parPtr
let psV = VS.unsafeFromForeignPtr parFP 0 m
vector = model psV
VS.unsafeWith vector $ \p → copyArray hxPtr p (VS.length vector)
in withModel cmodel $ \modelFunPtr → do
let runDif ∷ LevMarDif r → IO CInt
runDif f = f modelFunPtr
psPtr
ysPtr
(fromIntegral m)
(fromIntegral n)
(fromIntegral itMax)
optsPtr
infoPtr
nullPtr
covarPtr
nullPtr
err ← case mJac of
Nothing → if boxConstrained
then if linConstrained
then withBoxConstraints
(withLinConstraints $ withWeights runDif)
f_blec_dif
else withBoxConstraints runDif f_bc_dif
else if linConstrained
then withLinConstraints runDif f_lec_dif
else runDif f_dif
Just jac →
let cjacobian ∷ Bindings.LevMar.Jacobian r
cjacobian parPtr jPtr _ _ _ = do
parFP ← newForeignPtr_ parPtr
let psV = VS.unsafeFromForeignPtr parFP 0 m
matrix = jac psV
vector = flatten matrix
VS.unsafeWith vector $ \p →
copyArray jPtr p (VS.length vector)
in withJacobian cjacobian $ \jacobPtr →
let runDer ∷ LevMarDer r → IO CInt
runDer f = runDif $ f jacobPtr
in if boxConstrained
then if linConstrained
then withBoxConstraints
(withLinConstraints $ withWeights runDer)
f_blec_der
else withBoxConstraints runDer f_bc_der
else if linConstrained
then withLinConstraints runDer f_lec_der
else runDer f_der
if err < 0
∧ err ≢ c'LM_ERROR_SINGULAR_MATRIX
∧ err ≢ c'LM_ERROR_SUM_OF_SQUARES_NOT_FINITE
then return $ Left $ convertLevMarError err
else do
info ← listToInfo <$> peekArray c'LM_INFO_SZ infoPtr
let psV = VS.unsafeFromForeignPtr psFP 0 m
let covarM = reshape m $ VS.unsafeFromForeignPtr covarFP 0 mm
return $ Right (psV, info, covarM)
where
m = VS.length ps
n = VS.length ys
mm = m*m
linConstrained = isJust mLinC
(cMat, rhcVec) = fromJust mLinC
boxConstrained = isJust mLowBs ∨ isJust mUpBs
withBoxConstraints f g =
maybeWithArray mLowBs $ \lBsPtr →
maybeWithArray mUpBs $ \uBsPtr →
f $ g lBsPtr uBsPtr
withLinConstraints f g =
VS.unsafeWith (flatten cMat) $ \cMatPtr →
VS.unsafeWith rhcVec $ \rhcVecPtr →
f ∘ g cMatPtr rhcVecPtr $ fromIntegral $ rows cMat
withWeights f g = maybeWithArray mWeights $ f ∘ g
maybeWithArray ∷ (Storable α) ⇒ Maybe (Vector α) → (Ptr α → IO β) → IO β
maybeWithArray Nothing f = f nullPtr
maybeWithArray (Just v) f = VS.unsafeWith v f
#if __GLASGOW_HASKELL__ >= 605
fastMallocForeignPtrArray ∷ ∀ α. Storable α ⇒ Int → IO (ForeignPtr α)
fastMallocForeignPtrArray n = mallocPlainForeignPtrBytes
(n * sizeOf (undefined ∷ α))
#else
fastMallocForeignPtrArray ∷ ∀ α. Storable α ⇒ Int → IO (ForeignPtr α)
fastMallocForeignPtrArray = mallocForeignPtrArray
#endif
data Options r =
Opts { optScaleInitMu ∷ !r
, optStopNormInfJacTe ∷ !r
, optStopNorm2Dp ∷ !r
, optStopNorm2E ∷ !r
, optDelta ∷ !r
} deriving (Eq, Ord, Read, Show, Data, Typeable)
defaultOpts ∷ Fractional r ⇒ Options r
defaultOpts = Opts { optScaleInitMu = c'LM_INIT_MU
, optStopNormInfJacTe = c'LM_STOP_THRESH
, optStopNorm2Dp = c'LM_STOP_THRESH
, optStopNorm2E = c'LM_STOP_THRESH
, optDelta = c'LM_DIFF_DELTA
}
optsToList ∷ Options r → [r]
optsToList (Opts mu eps1 eps2 eps3 delta) =
[mu, eps1, eps2, eps3, delta]
data Constraints r = Constraints
{ lowerBounds ∷ !(Maybe (Params r))
, upperBounds ∷ !(Maybe (Params r))
, weights ∷ !(Maybe (Params r))
, linearConstraints ∷ !(Maybe (LinearConstraints r))
} deriving (Read, Show, Typeable)
deriving instance (Eq r, Container Vector r) ⇒ Eq (Constraints r)
type LinearConstraints r = (Matrix r, Vector r)
instance Monoid (Constraints r) where
mempty = Constraints Nothing Nothing Nothing Nothing
mappend (Constraints lb1 ub1 w1 l1)
(Constraints lb2 ub2 w2 l2) = Constraints (lb1 `mplus` lb2)
(ub1 `mplus` ub2)
(w1 `mplus` w2)
(l1 `mplus` l2)
data Info r = Info
{ infNorm2initE ∷ !r
, infNorm2E ∷ !r
, infNormInfJacTe ∷ !r
, infNorm2Dp ∷ !r
, infMuDivMax ∷ !r
, infNumIter ∷ !Int
, infStopReason ∷ !StopReason
, infNumFuncEvals ∷ !Int
, infNumJacobEvals ∷ !Int
, infNumLinSysSolved ∷ !Int
} deriving (Eq, Ord, Read, Show, Data, Typeable)
listToInfo ∷ (RealFrac r) ⇒ [r] → Info r
listToInfo [a,b,c,d,e,f,g,h,i,j] =
Info { infNorm2initE = a
, infNorm2E = b
, infNormInfJacTe = c
, infNorm2Dp = d
, infMuDivMax = e
, infNumIter = floor f
, infStopReason = toEnum $ floor g 1
, infNumFuncEvals = floor h
, infNumJacobEvals = floor i
, infNumLinSysSolved = floor j
}
listToInfo _ = error "liftToInfo: wrong list length"
data StopReason
= SmallGradient
| SmallDp
| MaxIterations
| SingularMatrix
| SmallestError
| SmallNorm2E
| InvalidValues
deriving (Eq, Ord, Read, Show, Data, Typeable, Enum)
data LevMarError
= LevMarError
| LapackError
| FailedBoxCheck
| MemoryAllocationFailure
| ConstraintMatrixRowsGtCols
| ConstraintMatrixNotFullRowRank
| TooFewMeasurements
deriving (Eq, Ord, Read, Show, Data, Typeable)
instance Exception LevMarError
levmarCErrorToLevMarError ∷ [(CInt, LevMarError)]
levmarCErrorToLevMarError =
[ (c'LM_ERROR, LevMarError)
, (c'LM_ERROR_LAPACK_ERROR, LapackError)
, (c'LM_ERROR_FAILED_BOX_CHECK, FailedBoxCheck)
, (c'LM_ERROR_MEMORY_ALLOCATION_FAILURE, MemoryAllocationFailure)
, (c'LM_ERROR_CONSTRAINT_MATRIX_ROWS_GT_COLS, ConstraintMatrixRowsGtCols)
, (c'LM_ERROR_CONSTRAINT_MATRIX_NOT_FULL_ROW_RANK, ConstraintMatrixNotFullRowRank)
, (c'LM_ERROR_TOO_FEW_MEASUREMENTS, TooFewMeasurements)
]
convertLevMarError ∷ CInt → LevMarError
convertLevMarError err = fromMaybe (error $ "Unknown levmar error: " ++ show err)
(lookup err levmarCErrorToLevMarError)