update test with z cost

Author: dehann

src/entities/GeneralTypes.jl

@@ -3,19 +3,23 @@
 struct PixelIndex{VALID, T <: Real}
     row::T
     col::T
+    depth::T
 end
-PixelIndex(u::T,v::T;valid::Bool=true) where {T <: Real} = PixelIndex{valid,T}(u,v)
+PixelIndex(u::T, v::T; valid::Bool=true, depth = T(0)) where {T <: Real} = PixelIndex{valid,T}(u, v, depth)
 
 function Base.getindex(p::PixelIndex,i::Int)
     if i === 1
         p.row
     elseif i === 2  
         p.col
+    elseif i === 3
+        p.depth
     else
         DomainError("Camera only has rows and columns, cannot index to $i")
     end
 end
 
+
 const Vector2 = SVector{2, Float64}
 const Point3 = SVector{3, Float64}
 const Vector3 = SVector{3, Float64}

src/services/CameraServices.jl

@@ -106,11 +106,8 @@ function projectHomogeneous(
   col = x * fx_z + pp_w(cam) # add center to get PixelIndex
   row = y * fy_z + pp_h(cam)
   # infront or behind
-  if (w==0&&0<z) || 0 < (z/w)
-    PixelIndex(row,col)
-  else
-    PixelIndex(0.,0.; valid=false)
-  end
+  depth=z/w
+  PixelIndex(row, col; depth, valid = (w==0&&0<z) || 0 < depth)
 end
 # # right cam
 # u2 = (x - w*baseline) * fz + center[1]

test/multiview_manifolds.jl

@@ -17,10 +17,10 @@ M = SpecialEuclidean(3)
 @testset "Multiview optimization of point in front of 2 cameras" begin
 ##
 
-cam = CameraCalibration()
+cam = CameraModels.CameraCalibration()
 
-obs1 = PixelIndex(240, 320)
-obs2 = PixelIndex(240, 315)
+obs1 = CameraModels.PixelIndex(240, 320)
+obs2 = CameraModels.PixelIndex(240, 315)
 
 w_T_c1 = ArrayPartition([0; 0  ;0.],[0 0 1; -1 0 0; 0 -1 0.])
 w_T_c2 = ArrayPartition([0;-0.1;0.],[0 0 1; -1 0 0; 0 -1 0.])
@@ -43,14 +43,22 @@ w_T_c2 = ArrayPartition([0;-0.1;0.],[0 0 1; -1 0 0; 0 -1 0.])
 
 ##
 
-function cost(w_Ph)
-  c1_Ph = affine_matrix(M, inv(M,w_T_c1))*w_Ph |> SVector{4}
-  pre1 = CameraModels.projectHomogeneous(cam,c1_Ph)
+function projectPointFromWorld(cam, c_H_w, w_Ph)
+  c_Ph = c_H_w*w_Ph |> SVector{4}
+  CameraModels.projectHomogeneous(cam,c_Ph)
+end
 
-  c2_Ph = affine_matrix(M, inv(M,w_T_c2))*w_Ph |> SVector{4}
-  pre2 = CameraModels.projectHomogeneous(cam,c2_Ph)
-  
-  (obs1[1]-pre1[1])^2 + (obs1[2]-pre1[2])^2 + (obs2[1]-pre2[1])^2 + (obs2[2]-pre2[2])^2
+function cameraResidual(cam, meas, M, w_T_c, w_Ph, κ=1000)
+  pred = projectPointFromWorld(cam, inv(affine_matrix(M,w_T_c)), w_Ph)
+  # experimental cost function to try force bad reprojects in front of the camera during optimization
+  κ*(abs(pred.depth) - pred.depth)^2 + (meas[1]-pred[1])^2 + (meas[2]-pred[2])^2
+end
+
+function cost(w_Ph)
+  cameraResidual(cam, obs1, M, w_T_c1, w_Ph) + cameraResidual(cam, obs2, M, w_T_c2, w_Ph)
+  # pre1 = projectPointFromWorld(cam, inv(affine_matrix(M,w_T_c1)), w_Ph)
+  # pre2 = projectPointFromWorld(cam, inv(affine_matrix(M,w_T_c2)), w_Ph)
+  # (obs1[1]-pre1[1])^2 + (obs1[2]-pre1[2])^2 + (obs2[1]-pre2[1])^2 + (obs2[2]-pre2[2])^2
 end
 
 ##
@@ -83,10 +91,8 @@ w_Res = Optim.optimize(
 
 ##
 
-
-@show w_Res.minimizer |> toNonhomogeneous
-
-@test isapprox([10.56;0;0], toNonhomogeneous(w_Res.minimizer); atol=1e-3)
+@show w_P3 = w_Res.minimizer |> CameraModels.toNonhomogeneous
+@test isapprox([10.56;0;0], w_P3; atol=1e-3)
 
 ##
 end