Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

Merge pull request #32 from adrianamor/master

Added Ippe algorithm and CLK pose refinement with ippe initialization
  • Loading branch information...
commit 26bb2efaa676c902cecb12047560a09377a969f2 2 parents 40c1b46 + 2481f8e
@albertoruiz authored
View
17 packages/contours/CLK/src/Contours/Refine.hs
@@ -1,11 +1,12 @@
module Contours.Refine (
prepro, normalizedXORError,
- refineH, refineP
+ refineH, refineP, refinePB
)where
import Contours
import Contours.CLK
import Contours.CLKP
+import Vision.Ippe
import Numeric.LinearAlgebra.HMatrix
import Vision(cameraFromHomogZ0,kgen,refineNewton)
import Util.Geometry(datMat,homog,segmentLength)
@@ -67,6 +68,20 @@ refineP k3 k4 f r h = p
refine c = poseStep rs c norobs
rs = rsp (proto r)
+refinePB :: Int -> ShapeMatch -> Matrix Double -> Matrix Double
+refinePB k4 r h = p
+ where
+ model = shapeContour (proto r)
+ obs = shapeContour (target r)
+
+ rPose = (fst . h2p) h
+ ik = kgen (1)
+ c0 = rPose ¿ [0,1,3]
+ norobs = transPol ik obs
+ p = kgen 1 <> fillr3 ((iterate refine c0)!!k4)
+ where
+ refine c = poseStep rs c norobs
+ rs = rsp (proto r)
fillr3 :: Matrix Double -> Matrix Double
fillr3 m = fromColumns [r1,r2,r3,t]
View
1  packages/geometry/hVision-geometry.cabal
@@ -39,6 +39,7 @@ library
Vision.IO
Vision.LASBA
Vision.Autofrontal
+ Vision.Ippe
other-modules:
View
50 packages/geometry/src/Vision/Ippe.hs
@@ -0,0 +1,50 @@
+-- TODO: header comments
+
+module Vision.Ippe (
+h2p, ippe
+)
+where
+
+import Numeric.LinearAlgebra.Data
+import Numeric.LinearAlgebra.HMatrix
+
+htest = (3><3)
+ [ 0.0938, 0.0829, 0.0728
+ ,-0.0291, 0.0771, 0.0052
+ , 0.0857, -0.0645, 1.0000] :: Matrix Double
+
+
+-- | Obtain pose from homography using Infinitesimal Plane Pose Estimation
+h2p :: Matrix Double -> (Matrix Double, Matrix Double)
+h2p h = (r1 ||| t1, r2 ||| t2)
+ where
+ (gamma, r1, r2) = ippe v j
+ v = col [h!0!2, h!1!2]
+ j = reshape 2 (fromList [h!0!0 - h!2!0*h!0!2, h!0!1 - h!2!1*h!0!2,
+ h!1!0 - h!2!0*h!1!2, h!1!1 - h!2!1*h!1!2])
+ t1 = scalar(1/gamma) * (v === 1)
+ t2 = scalar(1/gamma) * (v === 1)
+
+
+ippe :: Matrix Double -> Matrix Double -> (Double, Matrix Double, Matrix Double)
+ippe v j = (gamma, r1, r2)
+ where
+ r1 = rv <> ((r22 ||| cc) === (tr bb ||| aa))
+ r2 = rv <> ((r22 ||| -cc) === (-tr bb ||| aa))
+ r22 = scalar(1/gamma) * a
+ ca = cross (flatten ((r22 === tr bb) <> col [1.0, 0.0])) (flatten ((r22 === tr bb) <> col [0.0, 1.0]))
+ cc = ((col . toList) ca) ? [0,1]
+ aa = scalar (ca ! 2)
+ bb = col [sqrt rr!0!0, (signum (rr!0!1))*(sqrt rr!1!1)]
+ rr = ident 2 - (tr r22) <> r22
+ gamma = e ! 0;
+ e = singularValues a
+ a = (inv b) <> j
+ b = ((ident 2 ||| -v) <> rv) ¿ [0,1]
+ rv = ident 3 + scalar (sT) * k + scalar(1-cT) * (k <> k)
+ sT = sqrt (1-1/s**2)
+ k = (scalar (1/t)) * ((konst 0 (2,2) ||| v) === (-tr v ||| 0))
+ cT = 1/s
+ s = (norm_2 (v === 1))
+ t = norm_2 v
+
Please sign in to comment.
Something went wrong with that request. Please try again.