下面是slam14講公式5.7
$Z\left(\begin{array}{l}{u} \\ {v} \\ {1}\end{array}\right)=\left(\begin{array}{ccc}{f_{x}} & {0} & {c_{x}} \\ {0} & {f_{y}} & {c_{y}} \\ {0} & {0} & {1}\end{array}\right)\left(\begin{array}{l}{X} \\ {Y} \\ {Z}\end{array}\right) \triangleq \boldsymbol{K P}
$ 公式(0)
就是一個三維點投影到二維點的公式
可以寫成：
$ depth*uv = K*XYZ \tag{1}$
這里我 用uv代表圖像坐標系里的二維點，XYZ代表三維的空間的點
然后還有一個公式5.8
$Z \boldsymbol{P}_{u v}=Z\left[\begin{array}{l}{u} \\ {v} \\ {1}\end{array}\right]=\boldsymbol{K}\left(\boldsymbol{R} \boldsymbol{P}_{w}+\boldsymbol{t}\right)=\boldsymbol{K} \boldsymbol{T} \boldsymbol{P}_{w}$
這個是考慮有一個變換之后的投影公式，可以寫成：
$depth*uv = K*T*XYZ \tag{2}$
因為我們要估計的就是深度，所以不能簡單的像slam14講里頭把深度Z或者depth省略掉。
slam14講隨意省略深度值Z的做法在深度估計里不太好。
然后考慮下面這種情況：
兩個相機觀測 同一點雲
相機1有坐標系1，相機2有坐標系2
在坐標系1下，觀測到的點雲的坐標為XYZ1
在坐標系2下，觀測到的點雲的坐標為XYZ2。
在相機1處觀測到彩色圖像rgb1和深度圖像depth1，相機2處類似。
然后兩個相機的點雲有如下的轉換關系：
depth1是標量，所以放在前面
結合公式3得到
$XYZ_2 = \boldsymbol{T}_{21}*depth_1*K^{-1}*uv_1$
再把上式代入公式5得到：
$depth_2*uv_2 = K*\boldsymbol{T}_{21}*depth_1*K^{-1}*uv_1$
下面是sfmlearner論文里頭那個公式：
$p_{s} \sim K \hat{T}_{t \rightarrow s} \hat{D}_{t}\left(p_{t}\right) K^{-1} p_{t}$
比較上述兩個公式，就知道論文里的公式是怎么來的了，
論文的公式把depth2給省略了，就像slam14講里頭一樣。
ps：其實這里可以考慮雙目相機的情況，在坐標系1和坐標系2處的兩個相機的K不一樣，
可以給予雙目深度估計更大的靈活性，不需要做雙目校正直接訓練，
代價就是兩個相機有各自的dispnet，參數量更大。靈活性和計算量不能兼得。
上面只是公式，下面開始研究代碼
假如你在相機1處有一張彩色圖像rgb和一張深度圖像depth。
rgb圖像是3通道的，長寬為h x w
depth圖像是單通道的，每個像素代表深度值Z，長寬和rgb圖像一樣。
公式0的另一種形式如下：
$ u = fx*X/Z + cx $
$ v = fy*Y/Z + cy $
轉換一下得到：
$ X = (u-cx)*Z/fx $
$ Y = (v-cy)*Z/fy $
uv是圖像坐標系上的點，因此：
u 的范圍為[0, w-1]
v 的范圍為[0, h-1]
Z是深度，是depth上每一個像素所代表的值。
u,v,cx,cy,fx,fy,Z都已知，因此我們可以根據上述公式，利用彩色圖像rgb和深度圖像depth算
出三維坐標XYZ。其實這就是公式6的操作。
當然不應該用for循環去計算每個三維坐標，而應該用meshgrid和矩陣相乘來計算，具體實現如下：
1 import numpy as np 2 import cv2 3 4 color = cv2.imread('xxx') # 3 channel 5 depth = cv2.imread('xxx') # 1 channel 6 7 # 這里假設圖片長寬為 640 x 480 8 h = 480 9 w = 640 10 11 u, v = np.meshgrid(np.arange(0, w), np.arange(0, h)) 12 13 depthScale = 1 14 15 Z = depth/depthScale 16 X = (u-cx)*Z/fx 17 Y = (v-cy)*Z/fy 18 19 X = X.reshape(307200, 1) 20 Y = Y.reshape(307200, 1) 21 Z = Z.reshape(307200, 1) 22 23 XYZ = np.hstack((X, Y, Z))
然后你就得到了一個點雲的坐標數據
實現公式3的話，還要考慮齊次、非齊次的問題，就是多增加一行或者一列之類的問題。
在sfmlearner的pytorch代碼中：
tgt_img相當於在坐標系1上，ref_img相當於在坐標系2上！！!
ref_img可以有很多張，ref_img的張數加上tgt_img就是sequence length
1 視差網絡 disp_net 生成4個尺度的視差圖 2 print("disp1 size:", disp1.size() ) # torch.Size([1, 1, 128, 416]) 3 print("disp2 size:", disp2.size() ) # torch.Size([1, 1, 64, 208]) 4 print("disp3 size:", disp3.size() ) # torch.Size([1, 1, 32, 104]) 5 print("disp4 size:", disp4.size() ) # torch.Size([1, 1, 16, 52]) 6 7 然后將視差圖轉換成深度圖： 8 depth = [1/disp for disp in disparities] 視差圖的倒數就是深度圖 9 10 對於pose_net 11 tgt_img = torch.rand([1, 3, 128, 416]) 12 ref_imgs = [torch.rand([1, 3, 128, 416]), torch.rand([1, 3, 128, 416]) ] 13 14 explainability_mask, pose = pose_exp_net(tgt_img, ref_imgs) 15 16 print("pose size:", pose.size() ) # torch.Size([1, 2, 6])
tgt_img相當於相機1的圖像，ref_img是相機2的圖像。
考慮photometric_reconstruction_loss函數中計算的多個scale中的第一個scale，即128 x 416的尺寸：
然后執行了下面這個操作：
1 for i, ref_img in enumerate(ref_imgs_scaled): 2 # 遍歷ref_imgs 3 current_pose = pose[:, i] 4 5 ref_img_warped = \ 6 inverse_warp(ref_img, 7 depth[:,0], # torch.Size([1, 128, 416]) 8 current_pose, # torch.Size([1, 6]) 9 intrinsics_scaled, 10 rotation_mode, 11 padding_mode) 12
傳進去的 depth 和 current_pose
根據inverse_warp函數的說明，前面這個1是留給batchsize的
1 def set_id_grid(depth): 2 b, h, w = depth.size() 3 i_range = torch.arange(0, h).view(1, h, 1).expand(1,h,w).type_as(depth) # [1, H, W] 4 j_range = torch.arange(0, w).view(1, 1, w).expand(1,h,w).type_as(depth) # [1, H, W] 5 ones = torch.ones(1,h,w).type_as(depth) 6 7 pixel_coords = torch.stack((j_range, i_range, ones), dim=1) # [1, 3, H, W] 8 return pixel_coords
1 current_pixel_coords = pixel_coords[:,:,:h,:w].expand(b,3,h,w).reshape(b, 3, -1) # [B, 3, H*W] 2 cam_coords = (intrinsics_inv @ current_pixel_coords).reshape(b, 3, h, w) 3 return cam_coords * depth.unsqueeze(1)
注意里頭的intrinsics_inv，所以上面的代碼相當於
$ depth_1*K^{-1}*uv_1 = XYZ_1$
所以pixel2cam是完成了二維反投影到三維的過程
然后是這么一句
proj_cam_to_src_pixel = intrinsics @ pose_mat # [B, 3, 4]
相當於$K*T_{21}$
$K*T_{21}$的另一個名字也叫做投影矩陣P
src_pixel_coords = cam2pixel(cam_coords, # XYZ proj_cam_to_src_pixel[:,:,:3], # R proj_cam_to_src_pixel[:,:,-1:], # t padding_mode) # [B,H,W,2]
所以cam2pixel函數完成 XYZ1轉換到XYZ2並投影到uv2的任務
inverse_warp的最后一句：
projected_img = F.grid_sample(img, src_pixel_coords, padding_mode=padding_mode)
以及執行完 inverse_warp后，在one_scale中的一句
diff = (tgt_img_scaled - ref_img_warped) * out_of_bound
inverse_warp函數只傳進去了ref_img，即坐標系2的圖像，然后最后就用tgt_img去減了ref_img_warped，
所以ref_img_warped其實是ref_img投影到tgt_img所在坐標系的圖像，即坐標2的圖像投影到坐標1。
考慮上面的公式，所以整個warp過程是：
找到和坐標系1上的每個像素對應的坐標系2上的圖像的像素的值，然后計算差值。
如果沒有落在整數坐標上，就用F.grid_sample計算出來。
基本上，sfmlearner的photometric_reconstruction_loss差不多就是slam里頭直接法的光度差的計算過程。
上面差不多就是sfmlearner代碼的難點了。
ssim之類的比較簡單，不做解析。
============
所以，根據上述內容，如果想跑自己的數據集，在自己拍的視頻上訓練，
你需要
1，標定自己的相機，得到內參K
2，把你拍的視頻分解的圖片resize到128x416，在圖片resize的時候，內參也要resize
如果對圖片做了其他操作，內參也需要做對應的操作，在custom_tranform.py有相關代碼。
其實就是個相機視錐的變化。
所以谷歌后來出的vid2depth算法在點雲上做 ICP 就很容易理解了。。。