├── demo_proj.png ├── losses ├── chamfer_distance │ ├── __init__.py │ ├── chamfer_distance.py │ ├── chamfer_distance.cu │ └── chamfer_distance.cpp ├── __init__.py └── chamfer_loss.py ├── utils ├── __init__.py ├── se3.py ├── invmat.py ├── so3.py ├── sinc.py └── transform.py ├── requirements.txt ├── config.yml ├── demo.py ├── mylogger.py ├── LICENSE ├── demo_resize.py ├── loss.py ├── CalibNet.py ├── README.md ├── test.py ├── Modules.py ├── dataset.py ├── train.py └── log └── cam2_oneiter.log /demo_proj.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/CalibNet_pytorch/HEAD/demo_proj.png -------------------------------------------------------------------------------- /losses/chamfer_distance/__init__.py: -------------------------------------------------------------------------------- 1 | from .chamfer_distance import ChamferDistance 2 | -------------------------------------------------------------------------------- /utils/__init__.py: -------------------------------------------------------------------------------- 1 | from . import sinc 2 | from . import so3, se3 3 | from . import invmat 4 | from . import transform 5 | #EOF -------------------------------------------------------------------------------- /losses/__init__.py: -------------------------------------------------------------------------------- 1 | try: 2 | from .chamfer_loss import ChamferDistanceLoss 3 | except: 4 | print("Sorry ChamferDistance loss is not compatible with your system!") -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy==1.22.0 2 | open3d==0.15.2 3 | pykitti==0.3.1 4 | PyYAML==6.0 5 | scipy==1.11.1 6 | #torch==1.8.2+cu111 7 | #torchvision==0.9.2+cu111 8 | tqdm==4.66.3 9 | -------------------------------------------------------------------------------- /config.yml: -------------------------------------------------------------------------------- 1 | dataset: 2 | train: [0,1,2,3,4,5,6,7] 3 | val: [8,9,10] 4 | test: [11,12,13] 5 | cam_id: 2 # (2 or 3) 6 | voxel_size: 0.1 7 | pooling: 3 # max pooling of semi-dense image, must be odd 8 | extend_ratio: [2.5,2.5] 9 | 10 | -------------------------------------------------------------------------------- /demo.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pykitti 3 | import matplotlib 4 | matplotlib.use('agg') 5 | from matplotlib import pyplot as plt 6 | basedir = 'data' 7 | seq = '00' 8 | data = pykitti.odometry(basedir,seq) 9 | calib = data.calib 10 | extran = calib.T_cam2_velo # [4,4] 11 | intran = calib.K_cam2 12 | pcd = data.get_velo(0).T # (4,N) 13 | img = np.asarray(data.get_cam2(0)) 14 | H,W = img.shape[:2] 15 | print(extran.shape,intran.shape) 16 | print(pcd.shape,img.shape) 17 | pcd[-1,:] = 1.0 18 | pcd = extran @ pcd 19 | pcd = intran @ pcd[:3,:] 20 | u,v,w = pcd[0,:], pcd[1,:], pcd[2,:] 21 | u = u/w 22 | v = v/w 23 | rev = (0<=u)*(u0) 24 | u = u[rev] 25 | v = v[rev] 26 | r = np.linalg.norm(pcd[:,rev],axis=0) 27 | plt.figure(figsize=(12,5),dpi=100,tight_layout=True) 28 | plt.axis([0,W,H,0]) 29 | plt.imshow(img) 30 | plt.scatter([u],[v],c=[r],cmap='rainbow_r',alpha=0.5,s=2) 31 | plt.savefig('demo_proj.png',bbox_inches='tight') 32 | -------------------------------------------------------------------------------- /mylogger.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import os 3 | import sys 4 | def print_warning(msg): 5 | print("\033[1;31m%s\033[0m"%msg) # red highlight 6 | 7 | def print_highlight(msg): 8 | print("\033[1;33m%s\033[0m"%msg) # yellow highlight 9 | 10 | def get_logger(name,filepath,level=logging.DEBUG,format='[%(levelname)s]:%(name)s, %(asctime)s, %(message)s',mode='a'): 11 | logger = logging.getLogger(name) 12 | logger.setLevel(level) 13 | # tx = logging.StreamHandler(sys.stdout) 14 | # tx.setFormatter(logging.Formatter(format)) 15 | # tx.setLevel(level) 16 | # logger.addHandler(tx) 17 | if filepath: 18 | if os.path.exists(os.path.dirname(filepath)): 19 | fh = logging.FileHandler(filename=filepath,mode=mode) 20 | fh.setFormatter(logging.Formatter(format)) 21 | fh.setLevel(level) 22 | logger.addHandler(fh) 23 | else: 24 | logger.warning("path of logfile {} is not a valid file, considered as no logfile.".format(os.path.abspath(filepath))) 25 | return logger 26 | 27 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 gitouni 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /demo_resize.py: -------------------------------------------------------------------------------- 1 | from curses import resize_term 2 | import numpy as np 3 | import pykitti 4 | import matplotlib 5 | matplotlib.use('agg') 6 | from matplotlib import pyplot as plt 7 | import cv2 8 | basedir = 'data' 9 | seq = '00' 10 | resize_ratio = (0.6,0.45) # (H,W) 11 | data = pykitti.odometry(basedir,seq) 12 | calib = data.calib 13 | extran = calib.T_cam0_velo # [4,4] 14 | intran_ratio = np.diag([resize_ratio[1],resize_ratio[0],1]) 15 | intran = intran_ratio @ calib.P_rect_20 16 | pcd = data.get_velo(0).T # (4,N) 17 | img = np.asarray(data.get_cam2(0)) 18 | H,W = img.shape[:2] 19 | RH,RW = round(H*resize_ratio[0]), round(W*resize_ratio[1]) 20 | img = cv2.resize(img,(RW,RH),interpolation=cv2.INTER_LINEAR) 21 | print(extran.shape,intran.shape) 22 | print(pcd.shape,img.shape) 23 | pcd[-1,:] = 1.0 24 | pcd = intran @ extran @ pcd 25 | pcd = pcd[:3,:] 26 | u,v,w = pcd[0,:], pcd[1,:], pcd[2,:] 27 | u = u/w 28 | v = v/w 29 | u = u 30 | v = v 31 | rev = (0<=u)*(u0) 32 | u = u[rev] 33 | v = v[rev] 34 | r = np.linalg.norm(pcd[:,rev],axis=0) 35 | plt.figure(figsize=(12,5),dpi=100,tight_layout=True) 36 | plt.axis([0,RW,RH,0]) 37 | plt.imshow(img) 38 | plt.scatter([u],[v],c=[r],cmap='rainbow_r',alpha=0.5,s=2) 39 | plt.savefig('demo_proj_resize.png',bbox_inches='tight') 40 | -------------------------------------------------------------------------------- /losses/chamfer_loss.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn as nn 3 | import os 4 | import sys 5 | sys.path.append(os.path.dirname(__file__)) 6 | from chamfer_distance import ChamferDistance 7 | # copied from https://github.com/vinits5/pcrnet_pytorch/tree/master/pcrnet/losses 8 | 9 | def chamfer_distance(template: torch.Tensor, source: torch.Tensor): 10 | cost_p0_p1, cost_p1_p0 = ChamferDistance()(template, source) 11 | cost_p0_p1 = torch.mean(torch.sqrt(cost_p0_p1),dim=-1) 12 | cost_p1_p0 = torch.mean(torch.sqrt(cost_p1_p0),dim=-1) 13 | chamfer_loss = (cost_p0_p1 + cost_p1_p0)/2.0 14 | return chamfer_loss 15 | 16 | 17 | class ChamferDistanceLoss(nn.Module): 18 | def __init__(self,reduction='mean'): 19 | super(ChamferDistanceLoss, self).__init__() 20 | self.reduction = reduction 21 | def forward(self, template, source): 22 | if self.reduction == 'none': 23 | return chamfer_distance(template, source) 24 | elif self.reduction == 'mean': 25 | return torch.mean(chamfer_distance(template, source),dim=0) 26 | elif self.reduction == 'sum': 27 | return torch.sum(chamfer_distance(template, source),dim=0) 28 | def __call__(self,template,source): 29 | return self.forward(self,template,source) 30 | 31 | if __name__ == "__main__": 32 | a = torch.rand(2,3,100) 33 | b = torch.rand(2,3,100) 34 | loss1 = ChamferDistanceLoss('none') 35 | loss2 = ChamferDistanceLoss('mean') 36 | loss3 = ChamferDistanceLoss('sum') 37 | print(loss1(a,b),loss2(a,b),loss3(a,b)) -------------------------------------------------------------------------------- /losses/chamfer_distance/chamfer_distance.py: -------------------------------------------------------------------------------- 1 | import torch 2 | from torch.utils.cpp_extension import load 3 | import os 4 | 5 | script_dir = os.path.dirname(__file__) 6 | sources = [ 7 | os.path.join(script_dir, "chamfer_distance.cpp"), 8 | os.path.join(script_dir, "chamfer_distance.cu"), 9 | ] 10 | 11 | cd = load(name="cd", sources=sources) 12 | 13 | 14 | class ChamferDistanceFunction(torch.autograd.Function): 15 | @staticmethod 16 | def forward(ctx, xyz1, xyz2): 17 | batchsize, n, _ = xyz1.size() 18 | _, m, _ = xyz2.size() 19 | xyz1 = xyz1.contiguous() 20 | xyz2 = xyz2.contiguous() 21 | dist1 = torch.zeros(batchsize, n) 22 | dist2 = torch.zeros(batchsize, m) 23 | 24 | idx1 = torch.zeros(batchsize, n, dtype=torch.int) 25 | idx2 = torch.zeros(batchsize, m, dtype=torch.int) 26 | 27 | if not xyz1.is_cuda: 28 | cd.forward(xyz1, xyz2, dist1, dist2, idx1, idx2) 29 | else: 30 | dist1 = dist1.cuda() 31 | dist2 = dist2.cuda() 32 | idx1 = idx1.cuda() 33 | idx2 = idx2.cuda() 34 | cd.forward_cuda(xyz1, xyz2, dist1, dist2, idx1, idx2) 35 | 36 | ctx.save_for_backward(xyz1, xyz2, idx1, idx2) 37 | 38 | return dist1, dist2 39 | 40 | @staticmethod 41 | def backward(ctx, graddist1, graddist2): 42 | xyz1, xyz2, idx1, idx2 = ctx.saved_tensors 43 | 44 | graddist1 = graddist1.contiguous() 45 | graddist2 = graddist2.contiguous() 46 | 47 | gradxyz1 = torch.zeros(xyz1.size()) 48 | gradxyz2 = torch.zeros(xyz2.size()) 49 | 50 | if not graddist1.is_cuda: 51 | cd.backward( 52 | xyz1, xyz2, gradxyz1, gradxyz2, graddist1, graddist2, idx1, idx2 53 | ) 54 | else: 55 | gradxyz1 = gradxyz1.cuda() 56 | gradxyz2 = gradxyz2.cuda() 57 | cd.backward_cuda( 58 | xyz1, xyz2, gradxyz1, gradxyz2, graddist1, graddist2, idx1, idx2 59 | ) 60 | 61 | return gradxyz1, gradxyz2 62 | 63 | 64 | class ChamferDistance(torch.nn.Module): 65 | def forward(self, xyz1, xyz2): 66 | return ChamferDistanceFunction.apply(xyz1, xyz2) 67 | -------------------------------------------------------------------------------- /loss.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn.functional as F 3 | from torch import nn 4 | from math import sqrt 5 | from utils import so3 6 | from losses.chamfer_loss import chamfer_distance 7 | from scipy.spatial.transform import Rotation 8 | import numpy as np 9 | 10 | class Photo_Loss(nn.Module): 11 | def __init__(self,scale=1.0,reduction='mean'): 12 | super(Photo_Loss, self).__init__() 13 | assert reduction in ['sum','mean','none'], 'Unknown or invalid reduction' 14 | self.scale = scale 15 | self.reduction = reduction 16 | def forward(self,input:torch.Tensor,target:torch.Tensor): 17 | """Photo loss 18 | 19 | Args: 20 | input (torch.Tensor): (B,H,W) 21 | target (torch.Tensor): (B,H,W) 22 | 23 | Returns: 24 | torch.Tensor: scaled mse loss between input and target 25 | """ 26 | return F.mse_loss(input/self.scale,target/self.scale,reduction=self.reduction) 27 | def __call__(self,input:torch.Tensor,target:torch.Tensor)->torch.Tensor: 28 | return self.forward(input,target) 29 | 30 | class ChamferDistanceLoss(nn.Module): 31 | def __init__(self,scale=1.0,reduction='mean'): 32 | super(ChamferDistanceLoss, self).__init__() 33 | assert reduction in ['sum','mean','none'], 'Unknown or invalid reduction' 34 | self.reduction = reduction 35 | self.scale = scale 36 | def forward(self, template, source): 37 | p0 = template/self.scale 38 | p1 = source/self.scale 39 | if self.reduction == 'none': 40 | return chamfer_distance(p0, p1) 41 | elif self.reduction == 'mean': 42 | return torch.mean(chamfer_distance(p0, p1),dim=0) 43 | elif self.reduction == 'sum': 44 | return torch.sum(chamfer_distance(p0, p1),dim=0) 45 | def __call__(self,template:torch.Tensor,source:torch.Tensor)->torch.Tensor: 46 | return self.forward(template,source) 47 | 48 | 49 | def geodesic_distance(x:torch.Tensor,)->tuple: 50 | """geodesic distance for evaluation 51 | 52 | Args: 53 | x (torch.Tensor): (B,4,4) 54 | 55 | Returns: 56 | torch.Tensor(1),torch.Tensor(1): distance of component R and T 57 | """ 58 | R = x[:,:3,:3] # (B,3,3) rotation 59 | T = x[:,:3,3] # (B,3) translation 60 | dR = so3.log(R) # (B,3) 61 | dR = F.mse_loss(dR,torch.zeros_like(dR).to(dR),reduction='none').mean(dim=1) # (B,3) -> (B,1) 62 | dR = torch.sqrt(dR).mean(dim=0) # (B,1) -> (1,) Rotation RMSE (mean in batch) 63 | dT = F.mse_loss(T,torch.zeros_like(T).to(T),reduction='none').mean(dim=1) # (B,3) -> (B,1) 64 | dT = torch.sqrt(dT).mean(dim=0) # (B,1) -> (1,) Translation RMSE (mean in batch) 65 | return dR, dT 66 | 67 | def gt2euler(gt:np.ndarray): 68 | """gt transformer to euler anlges and translation 69 | 70 | Args: 71 | gt (np.ndarray): 4x4 72 | 73 | Returns: 74 | angle_gt, trans_gt: (3,1),(3,1) 75 | """ 76 | R_gt = gt[:3, :3] 77 | euler_angle = Rotation.from_matrix(R_gt) 78 | anglez_gt, angley_gt, anglex_gt = euler_angle.as_euler('zyx') 79 | angle_gt = np.array([anglex_gt, angley_gt, anglez_gt]) 80 | trans_gt_t = -R_gt @ gt[:3, 3] 81 | return angle_gt, trans_gt_t -------------------------------------------------------------------------------- /CalibNet.py: -------------------------------------------------------------------------------- 1 | import torch 2 | from torch import nn 3 | import torch.nn.functional as F 4 | from Modules import resnet18 5 | 6 | class Aggregation(nn.Module): 7 | def __init__(self,inplanes=768,planes=96,final_feat=(5,2)): 8 | super(Aggregation,self).__init__() 9 | self.conv1 = nn.Conv2d(in_channels=inplanes,out_channels=planes*4,kernel_size=3,stride=2,padding=1) 10 | self.bn1 = nn.BatchNorm2d(planes*4) 11 | self.conv2 = nn.Conv2d(in_channels=planes*4,out_channels=planes*4,kernel_size=3,stride=2,padding=1) 12 | self.bn2 = nn.BatchNorm2d(planes*4) 13 | self.conv3 = nn.Conv2d(in_channels=planes*4,out_channels=planes*2,kernel_size=(2,1),stride=2) 14 | self.bn3 = nn.BatchNorm2d(planes*2) 15 | self.tr_conv = nn.Conv2d(in_channels=planes*2,out_channels=planes,kernel_size=1,stride=1) 16 | self.tr_bn = nn.BatchNorm2d(planes) 17 | self.rot_conv = nn.Conv2d(in_channels=planes*2,out_channels=planes,kernel_size=1,stride=1) 18 | self.rot_bn = nn.BatchNorm2d(planes) 19 | self.tr_drop = nn.Dropout2d(p=0.2) 20 | self.rot_drop = nn.Dropout2d(p=0.2) 21 | self.tr_pool = nn.AdaptiveAvgPool2d(output_size=final_feat) 22 | self.rot_pool = nn.AdaptiveAvgPool2d(output_size=final_feat) 23 | self.fc1 = nn.Linear(planes*final_feat[0]*final_feat[1],3) # 96*10 24 | self.fc2 = nn.Linear(planes*final_feat[0]*final_feat[1],3) # 96*10 25 | for m in self.modules(): 26 | if isinstance(m, nn.Conv2d) or isinstance(m, nn.ConvTranspose2d): 27 | nn.init.kaiming_normal_(m.weight.data, mode='fan_in') 28 | if m.bias is not None: 29 | m.bias.data.zero_() 30 | nn.init.xavier_normal_(self.fc1.weight,0.1) 31 | nn.init.xavier_normal_(self.fc2.weight,0.1) 32 | 33 | def forward(self,x:torch.Tensor): 34 | x = self.conv1(x) 35 | x = self.bn1(x) 36 | x = self.conv2(x) 37 | x = self.bn2(x) 38 | x = self.conv3(x) 39 | x = self.bn3(x) 40 | x_tr = self.tr_conv(x) 41 | x_tr = self.tr_bn(x_tr) 42 | x_tr = self.tr_drop(x_tr) 43 | x_tr = self.tr_pool(x_tr) # (19,6) 44 | x_tr = self.fc1(x_tr.view(x_tr.shape[0],-1)) 45 | x_rot = self.rot_conv(x) 46 | x_rot = self.rot_bn(x_rot) 47 | x_rot = self.rot_drop(x_rot) 48 | x_rot = self.rot_pool(x_rot) # (19.6) 49 | x_rot = self.fc2(x_rot.view(x_rot.shape[0],-1)) 50 | return x_rot, x_tr 51 | 52 | class CalibNet(nn.Module): 53 | def __init__(self,backbone_pretrained=False,depth_scale=100.0): 54 | super(CalibNet,self).__init__() 55 | self.scale = depth_scale 56 | self.rgb_resnet = resnet18(inplanes=3,planes=64) # outplanes = 512 57 | self.depth_resnet = nn.Sequential( 58 | nn.MaxPool2d(kernel_size=5,stride=1,padding=2), # outplanes = 256 59 | resnet18(inplanes=1,planes=32), 60 | ) 61 | self.aggregation = Aggregation(inplanes=512+256,planes=96) 62 | self.device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu") 63 | if backbone_pretrained: 64 | self.rgb_resnet.load_state_dict(torch.load("resnetV1C.pth")['state_dict'],strict=False) 65 | self.to(self.device) 66 | def forward(self,rgb:torch.Tensor,depth:torch.Tensor): 67 | # rgb: [B,3,H,W] 68 | # depth: [B,1,H,W] 69 | x1,x2 = rgb,depth.clone() # clone dpeth, or it will change depth in '/' operation 70 | x2 /= self.scale 71 | x1 = self.rgb_resnet(x1)[-1] 72 | x2 = self.depth_resnet(x2)[-1] 73 | feat = torch.cat((x1,x2),dim=1) # [B,C1+C2,H,W] 74 | x_rot, x_tr = self.aggregation(feat) 75 | return x_rot, x_tr 76 | if __name__=="__main__": 77 | x = (torch.rand(2,3,1242,375).cuda(),torch.rand(2,1,1242,375).cuda()) 78 | model = CalibNet(backbone_pretrained=False).cuda() 79 | model.eval() 80 | rotation,translation = model(*x) 81 | print("translation size:{}".format(translation.size())) 82 | print("rotation size:{}".format(rotation.size())) 83 | 84 | 85 | -------------------------------------------------------------------------------- /utils/se3.py: -------------------------------------------------------------------------------- 1 | """ 3-d rigid body transfomation group and corresponding Lie algebra. """ 2 | import torch 3 | from .sinc import sinc1, sinc2, sinc3 4 | from . import so3 5 | 6 | def twist_prod(x:torch.Tensor, y:torch.Tensor): 7 | x_ = x.view(-1, 6) 8 | y_ = y.view(-1, 6) 9 | 10 | xw, xv = x_[:, 0:3], x_[:, 3:6] 11 | yw, yv = y_[:, 0:3], y_[:, 3:6] 12 | 13 | zw = so3.cross_prod(xw, yw) 14 | zv = so3.cross_prod(xw, yv) + so3.cross_prod(xv, yw) 15 | 16 | z = torch.cat((zw, zv), dim=1) 17 | 18 | return z.view_as(x) 19 | 20 | def liebracket(x, y): 21 | return twist_prod(x, y) 22 | 23 | 24 | def mat(x:torch.Tensor): 25 | # size: [*, 6] -> [*, 4, 4] 26 | x_ = x.view(-1, 6) 27 | w1, w2, w3 = x_[:, 0], x_[:, 1], x_[:, 2] 28 | v1, v2, v3 = x_[:, 3], x_[:, 4], x_[:, 5] 29 | O = torch.zeros_like(w1) 30 | 31 | X = torch.stack(( 32 | torch.stack(( O, -w3, w2, v1), dim=1), 33 | torch.stack(( w3, O, -w1, v2), dim=1), 34 | torch.stack((-w2, w1, O, v3), dim=1), 35 | torch.stack(( O, O, O, O), dim=1)), dim=1) 36 | return X.view(*(x.size()[0:-1]), 4, 4) 37 | 38 | def vec(X:torch.Tensor): 39 | X_ = X.view(-1, 4, 4) 40 | w1, w2, w3 = X_[:, 2, 1], X_[:, 0, 2], X_[:, 1, 0] 41 | v1, v2, v3 = X_[:, 0, 3], X_[:, 1, 3], X_[:, 2, 3] 42 | x = torch.stack((w1, w2, w3, v1, v2, v3), dim=1) 43 | return x.view(*X.size()[0:-2], 6) 44 | 45 | def genvec(): 46 | return torch.eye(6) 47 | 48 | def genmat(): 49 | return mat(genvec()) 50 | 51 | def exp(x:torch.Tensor): 52 | x_ = x.view(-1, 6) 53 | w, v = x_[:, 0:3], x_[:, 3:6] 54 | t = w.norm(p=2, dim=1).view(-1, 1, 1) 55 | W = so3.mat(w) 56 | S = W.bmm(W) 57 | I = torch.eye(3).to(w) 58 | 59 | # Rodrigues' rotation formula. 60 | #R = cos(t)*eye(3) + sinc1(t)*W + sinc2(t)*(w*w'); 61 | # = eye(3) + sinc1(t)*W + sinc2(t)*S 62 | R = I + sinc1(t)*W + sinc2(t)*S 63 | 64 | #V = sinc1(t)*eye(3) + sinc2(t)*W + sinc3(t)*(w*w') 65 | # = eye(3) + sinc2(t)*W + sinc3(t)*S 66 | V = I + sinc2(t)*W + sinc3(t)*S 67 | 68 | p = V.bmm(v.contiguous().view(-1, 3, 1)) 69 | 70 | z = torch.Tensor([0, 0, 0, 1]).view(1, 1, 4).repeat(x_.size(0), 1, 1).to(x) 71 | Rp = torch.cat((R, p), dim=2) 72 | g = torch.cat((Rp, z), dim=1) 73 | 74 | return g.view(*(x.size()[0:-1]), 4, 4) 75 | 76 | def inverse(g:torch.Tensor): 77 | g_ = g.view(-1, 4, 4) 78 | R = g_[:, 0:3, 0:3] 79 | p = g_[:, 0:3, 3] 80 | Q = R.transpose(1, 2) 81 | q = -Q.matmul(p.unsqueeze(-1)) 82 | 83 | z = torch.Tensor([0, 0, 0, 1]).view(1, 1, 4).repeat(g_.size(0), 1, 1).to(g) 84 | Qq = torch.cat((Q, q), dim=2) 85 | ig = torch.cat((Qq, z), dim=1) 86 | 87 | return ig.view(*(g.size()[0:-2]), 4, 4) 88 | 89 | 90 | def log(g:torch.Tensor): 91 | g_ = g.view(-1, 4, 4) 92 | R = g_[:, 0:3, 0:3] 93 | p = g_[:, 0:3, 3] 94 | 95 | w = so3.log(R) 96 | H = so3.inv_vecs_Xg_ig(w) 97 | v = H.bmm(p.contiguous().view(-1, 3, 1)).view(-1, 3) 98 | 99 | x = torch.cat((w, v), dim=1) 100 | return x.view(*(g.size()[0:-2]), 6) 101 | 102 | def transform(g: torch.Tensor, a: torch.Tensor): 103 | # g : SE(3), * x 4 x 4 104 | # a : R^3, * x 3[x N] 105 | g_ = g.view(-1, 4, 4) 106 | R = g_[:, 0:3, 0:3].contiguous().view(*(g.size()[0:-2]), 3, 3) 107 | p = g_[:, 0:3, 3].contiguous().view(*(g.size()[0:-2]), 3) 108 | if len(g.size()) == len(a.size()): 109 | b = R.matmul(a) + p.unsqueeze(-1) 110 | else: 111 | b = R.matmul(a.unsqueeze(-1)).squeeze(-1) + p 112 | return b 113 | 114 | def rot_transform(g: torch.Tensor,a: torch.Tensor): 115 | """transform pcd in rotation component 116 | 117 | Args: 118 | g (B,K,4,4): SE3 119 | a (B,K,N,3): pcd 120 | """ 121 | g_ = g[...,:3,:3] # [B,K,3,3] 122 | return g_.matmul(a.transpose(2,3)).transpose(2,3) # [B,K,3,3] x [B,K,3,N] -> [B,3,3,N] -> [B,3,N,3] 123 | 124 | def tsl_transform(g: torch.Tensor,a: torch.Tensor): 125 | """transform pcd in translation component 126 | 127 | Args: 128 | g (B,K,1,3): SE3 129 | a (B,K,N,3): pcd 130 | """ 131 | return a + g # (B,K,N,3) + (B,K,1,3) -> (B,K,N,3) 132 | 133 | def group_prod(g, h): 134 | # g, h : SE(3) 135 | g1 = g.matmul(h) 136 | return g1 137 | 138 | 139 | class ExpMap(torch.autograd.Function): 140 | """ Exp: se(3) -> SE(3) 141 | """ 142 | @staticmethod 143 | def forward(ctx, x): 144 | """ Exp: R^6 -> M(4), 145 | size: [B, 6] -> [B, 4, 4], 146 | or [B, 1, 6] -> [B, 1, 4, 4] 147 | """ 148 | ctx.save_for_backward(x) 149 | g = exp(x) 150 | return g 151 | 152 | @staticmethod 153 | def backward(ctx, grad_output): 154 | x, = ctx.saved_tensors 155 | g = exp(x) 156 | gen_k = genmat().to(x) 157 | 158 | # Let z = f(g) = f(exp(x)) 159 | # dz = df/dgij * dgij/dxk * dxk 160 | # = df/dgij * (d/dxk)[exp(x)]_ij * dxk 161 | # = df/dgij * [gen_k*g]_ij * dxk 162 | 163 | dg = gen_k.matmul(g.view(-1, 1, 4, 4)) 164 | # (k, i, j) 165 | dg = dg.to(grad_output) 166 | 167 | go = grad_output.contiguous().view(-1, 1, 4, 4) 168 | dd = go * dg 169 | grad_input = dd.sum(-1).sum(-1) 170 | 171 | return grad_input 172 | 173 | Exp = ExpMap.apply 174 | 175 | 176 | #EOF 177 | -------------------------------------------------------------------------------- /utils/invmat.py: -------------------------------------------------------------------------------- 1 | """ inverse matrix """ 2 | 3 | import torch 4 | 5 | 6 | def batch_inverse(x:torch.Tensor): 7 | """ M(n) -> M(n); x -> x^-1 """ 8 | batch_size, h, w = x.size() 9 | assert h == w, 'inverse operation allows squares only' 10 | y = torch.zeros_like(x) 11 | for i in range(batch_size): 12 | y[i, :, :] = x[i, :, :].inverse() 13 | return y 14 | 15 | def batch_inverse_dx(y:torch.Tensor): 16 | """ backward """ 17 | # Let y(x) = x^-1. 18 | # compute dy 19 | # dy = dy(j,k) 20 | # = - y(j,m) * dx(m,n) * y(n,k) 21 | # = - y(j,m) * y(n,k) * dx(m,n) 22 | # therefore, 23 | # dy(j,k)/dx(m,n) = - y(j,m) * y(n,k) 24 | batch_size, h, w = y.size() 25 | assert h == w 26 | # compute dy(j,k,m,n) = dy(j,k)/dx(m,n) = - y(j,m) * y(n,k) 27 | # = - (y(j,:))' * y'(k,:) 28 | yl = y.repeat(1, 1, h).view(batch_size*h*h, h, 1) 29 | yr = y.transpose(1, 2).repeat(1, h, 1).view(batch_size*h*h, 1, h) 30 | dy = - yl.bmm(yr).view(batch_size, h, h, h, h) 31 | 32 | # compute dy(m,n,j,k) = dy(j,k)/dx(m,n) = - y(j,m) * y(n,k) 33 | # = - (y'(m,:))' * y(n,:) 34 | #yl = y.transpose(1, 2).repeat(1, 1, h).view(batch_size*h*h, h, 1) 35 | #yr = y.repeat(1, h, 1).view(batch_size*h*h, 1, h) 36 | #dy = - yl.bmm(yr).view(batch_size, h, h, h, h) 37 | 38 | return dy 39 | 40 | 41 | def batch_pinv_dx(x:torch.Tensor): 42 | """ returns y = (x'*x)^-1 * x' and dy/dx. """ 43 | # y = (x'*x)^-1 * x' 44 | # = s^-1 * x' 45 | # = b * x' 46 | # d{y(j,k)}/d{x(m,n)} 47 | # = d{b(j,i) * x(k,i)}/d{x(m,n)} 48 | # = d{b(j,i)}/d{x(m,n)} * x(k,i) + b(j,i) * d{x(k,i)}/d{x(m,n)} 49 | # d{b(j,i)}/d{x(m,n)} 50 | # = d{b(j,i)}/d{s(p,q)} * d{s(p,q)}/d{x(m,n)} 51 | # = -b(j,p)*b(q,i) * d{s(p,q)}/d{x(m,n)} 52 | # d{s(p,q)}/d{x(m,n)} 53 | # = d{x(t,p)*x(t,q)}/d{x(m,n)} 54 | # = d{x(t,p)}/d{x(m,n)} * x(t,q) + x(t,p) * d{x(t,q)}/d{x(m,n)} 55 | batch_size, h, w = x.size() 56 | xt = x.transpose(1, 2) 57 | s = xt.bmm(x) 58 | b = batch_inverse(s) 59 | y = b.bmm(xt) 60 | 61 | # dx/dx 62 | ex = torch.eye(h*w).to(x).unsqueeze(0).view(1, h, w, h, w) 63 | # ds/dx = dx(t,_)/dx * x(t,_) + x(t,_) * dx(t,_)/dx 64 | ex1 = ex.view(1, h, w*h*w) # [t, p*m*n] 65 | dx1 = x.transpose(1, 2).matmul(ex1).view(batch_size, w, w, h, w) # [q, p,m,n] 66 | ds_dx = dx1.transpose(1, 2) + dx1 # [p, q, m, n] 67 | # db/ds 68 | db_ds = batch_inverse_dx(b) # [j, i, p, q] 69 | # db/dx = db/d{s(p,q)} * d{s(p,q)}/dx 70 | db1 = db_ds.view(batch_size, w*w, w*w).bmm(ds_dx.view(batch_size, w*w, h*w)) 71 | db_dx = db1.view(batch_size, w, w, h, w) # [j, i, m, n] 72 | # dy/dx = db(_,i)/dx * x(_,i) + b(_,i) * dx(_,i)/dx 73 | dy1 = db_dx.transpose(1, 2).contiguous().view(batch_size, w, w*h*w) 74 | dy1 = x.matmul(dy1).view(batch_size, h, w, h, w) # [k, j, m, n] 75 | ext = ex.transpose(1, 2).contiguous().view(1, w, h*h*w) 76 | dy2 = b.matmul(ext).view(batch_size, w, h, h, w) # [j, k, m, n] 77 | dy_dx = dy1.transpose(1, 2) + dy2 78 | 79 | return y, dy_dx 80 | 81 | 82 | class InvMatrix(torch.autograd.Function): 83 | """ M(n) -> M(n); x -> x^-1. 84 | """ 85 | @staticmethod 86 | def forward(ctx, x): 87 | y = batch_inverse(x) 88 | ctx.save_for_backward(y) 89 | return y 90 | 91 | @staticmethod 92 | def backward(ctx, grad_output:torch.Tensor): 93 | y, = ctx.saved_tensors # v0.4 94 | #y, = ctx.saved_variables # v0.3.1 95 | batch_size, h, w = y.size() 96 | assert h == w 97 | 98 | # Let y(x) = x^-1 and assume any function f(y(x)). 99 | # compute df/dx(m,n)... 100 | # df/dx(m,n) = df/dy(j,k) * dy(j,k)/dx(m,n) 101 | # well, df/dy is 'grad_output' 102 | # and so we will return 'grad_input = df/dy(j,k) * dy(j,k)/dx(m,n)' 103 | 104 | dy = batch_inverse_dx(y) # dy(j,k,m,n) = dy(j,k)/dx(m,n) 105 | go = grad_output.contiguous().view(batch_size, 1, h*h) # [1, (j*k)] 106 | ym = dy.view(batch_size, h*h, h*h) # [(j*k), (m*n)] 107 | r = go.bmm(ym) # [1, (m*n)] 108 | grad_input = r.view(batch_size, h, h) # [m, n] 109 | 110 | return grad_input 111 | 112 | def batch_inv(x: torch.Tensor): 113 | """batch inverse 114 | using torch.linalg.solve to accelerate (computation on GPU) 115 | Args: 116 | x (torch.Tensor): (B,M,M) 117 | Returns: 118 | inv_x (torch.Tensor): (B,M,M) 119 | """ 120 | assert x.size(1) == x.size(2), "batch inv can only compute inverse of square matrix, get size:{}".format(x.size()) 121 | M = x.size(1) 122 | valid = torch.det(x)!=0 123 | x_ = x[valid] 124 | inv_x = torch.zeros_like(x) 125 | # if torch.abs(x).min().item() == 0: 126 | # x += 1e-6 # jitter for inv operation 127 | identity = torch.eye(M)[None,...].to(x.device) # (1,M,N) 128 | inv_x[valid] = torch.linalg.solve(x_,identity) # need torch>=1.8 129 | return inv_x 130 | 131 | 132 | if __name__ == '__main__': 133 | def test(): 134 | x = torch.randn(2, 3, 2) 135 | x_val = x.requires_grad_() 136 | 137 | s_val = x_val.transpose(1, 2).bmm(x_val) 138 | s_inv = InvMatrix.apply(s_val) 139 | y_val = s_inv.bmm(x_val.transpose(1, 2)) 140 | y_val.sum().backward() 141 | t1 = x_val.grad 142 | 143 | y, dy_dx = batch_pinv_dx(x) 144 | t2 = dy_dx.sum(1).sum(1) 145 | 146 | print(t1) 147 | print(t2) 148 | print(t1 - t2) 149 | 150 | test() 151 | 152 | #EOF 153 | -------------------------------------------------------------------------------- /losses/chamfer_distance/chamfer_distance.cu: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | __global__ 7 | void ChamferDistanceKernel( 8 | int b, 9 | int n, 10 | const float* xyz, 11 | int m, 12 | const float* xyz2, 13 | float* result, 14 | int* result_i) 15 | { 16 | const int batch=512; 17 | __shared__ float buf[batch*3]; 18 | for (int i=blockIdx.x;ibest){ 130 | result[(i*n+j)]=best; 131 | result_i[(i*n+j)]=best_i; 132 | } 133 | } 134 | __syncthreads(); 135 | } 136 | } 137 | } 138 | 139 | void ChamferDistanceKernelLauncher( 140 | const int b, const int n, 141 | const float* xyz, 142 | const int m, 143 | const float* xyz2, 144 | float* result, 145 | int* result_i, 146 | float* result2, 147 | int* result2_i) 148 | { 149 | ChamferDistanceKernel<<>>(b, n, xyz, m, xyz2, result, result_i); 150 | ChamferDistanceKernel<<>>(b, m, xyz2, n, xyz, result2, result2_i); 151 | 152 | cudaError_t err = cudaGetLastError(); 153 | if (err != cudaSuccess) 154 | printf("error in chamfer distance updateOutput: %s\n", cudaGetErrorString(err)); 155 | } 156 | 157 | 158 | __global__ 159 | void ChamferDistanceGradKernel( 160 | int b, int n, 161 | const float* xyz1, 162 | int m, 163 | const float* xyz2, 164 | const float* grad_dist1, 165 | const int* idx1, 166 | float* grad_xyz1, 167 | float* grad_xyz2) 168 | { 169 | for (int i = blockIdx.x; i>>(b, n, xyz1, m, xyz2, grad_dist1, idx1, grad_xyz1, grad_xyz2); 204 | ChamferDistanceGradKernel<<>>(b, m, xyz2, n, xyz1, grad_dist2, idx2, grad_xyz2, grad_xyz1); 205 | 206 | cudaError_t err = cudaGetLastError(); 207 | if (err != cudaSuccess) 208 | printf("error in chamfer distance get grad: %s\n", cudaGetErrorString(err)); 209 | } 210 | -------------------------------------------------------------------------------- /utils/so3.py: -------------------------------------------------------------------------------- 1 | """ 3-d rotation group and corresponding Lie algebra """ 2 | import torch 3 | from . import sinc 4 | from .sinc import sinc1, sinc2, sinc3 5 | 6 | 7 | def cross_prod(x, y): 8 | z = torch.cross(x.view(-1, 3), y.view(-1, 3), dim=1).view_as(x) 9 | return z 10 | 11 | def liebracket(x, y): 12 | return cross_prod(x, y) 13 | 14 | def mat(x): 15 | # size: [*, 3] -> [*, 3, 3] 16 | x_ = x.view(-1, 3) 17 | x1, x2, x3 = x_[:, 0], x_[:, 1], x_[:, 2] 18 | O = torch.zeros_like(x1) 19 | 20 | X = torch.stack(( 21 | torch.stack((O, -x3, x2), dim=1), 22 | torch.stack((x3, O, -x1), dim=1), 23 | torch.stack((-x2, x1, O), dim=1)), dim=1) 24 | return X.view(*(x.size()[0:-1]), 3, 3) 25 | 26 | def vec(X): 27 | X_ = X.view(-1, 3, 3) 28 | x1, x2, x3 = X_[:, 2, 1], X_[:, 0, 2], X_[:, 1, 0] 29 | x = torch.stack((x1, x2, x3), dim=1) 30 | return x.view(*X.size()[0:-2], 3) 31 | 32 | def genvec(): 33 | return torch.eye(3) 34 | 35 | def genmat(): 36 | return mat(genvec()) 37 | 38 | def RodriguesRotation(x): 39 | # for autograd 40 | w = x.view(-1, 3) 41 | t = w.norm(p=2, dim=1).view(-1, 1, 1) 42 | W = mat(w) 43 | S = W.bmm(W) 44 | I = torch.eye(3).to(w) 45 | 46 | # Rodrigues' rotation formula. 47 | #R = cos(t)*eye(3) + sinc1(t)*W + sinc2(t)*(w*w'); 48 | #R = eye(3) + sinc1(t)*W + sinc2(t)*S 49 | 50 | R = I + sinc.Sinc1(t)*W + sinc.Sinc2(t)*S 51 | 52 | return R.view(*(x.size()[0:-1]), 3, 3) 53 | 54 | def exp(x:torch.Tensor) -> torch.Tensor: 55 | w = x.view(-1, 3) 56 | t = w.norm(p=2, dim=1).view(-1, 1, 1) 57 | W = mat(w) 58 | S = W.bmm(W) 59 | I = torch.eye(3).to(w) 60 | 61 | # Rodrigues' rotation formula. 62 | #R = cos(t)*eye(3) + sinc1(t)*W + sinc2(t)*(w*w'); 63 | #R = eye(3) + sinc1(t)*W + sinc2(t)*S 64 | 65 | R = I + sinc1(t)*W + sinc2(t)*S 66 | 67 | return R.view(*(x.size()[0:-1]), 3, 3) 68 | 69 | 70 | def inverse(g): 71 | R = g.view(-1, 3, 3) 72 | Rt = R.transpose(1, 2) 73 | return Rt.view_as(g) 74 | 75 | def btrace(X): 76 | # batch-trace: [B, N, N] -> [B] 77 | n = X.size(-1) 78 | X_ = X.view(-1, n, n) 79 | tr = torch.zeros(X_.size(0)).to(X) 80 | for i in range(tr.size(0)): 81 | m = X_[i, :, :] 82 | tr[i] = torch.trace(m) 83 | return tr.view(*(X.size()[0:-2])) 84 | 85 | def log(g): 86 | eps = 1.0e-7 87 | R = g.view(-1, 3, 3) 88 | tr = btrace(R) 89 | c = (tr - 1) / 2 90 | t = torch.acos(c) 91 | sc = sinc1(t) 92 | idx0 = (torch.abs(sc) <= eps) 93 | idx1 = (torch.abs(sc) > eps) 94 | sc = sc.view(-1, 1, 1) 95 | 96 | X = torch.zeros_like(R) 97 | if idx1.any(): 98 | X[idx1] = (R[idx1] - R[idx1].transpose(1, 2)) / (2*sc[idx1]) 99 | 100 | if idx0.any(): 101 | # t[idx0] == math.pi 102 | t2 = t[idx0] ** 2 103 | A = (R[idx0] + torch.eye(3).type_as(R).unsqueeze(0)) * t2.view(-1, 1, 1) / 2 104 | aw1 = torch.sqrt(A[:, 0, 0]) 105 | aw2 = torch.sqrt(A[:, 1, 1]) 106 | aw3 = torch.sqrt(A[:, 2, 2]) 107 | sgn_3 = torch.sign(A[:, 0, 2]) 108 | sgn_3[sgn_3 == 0] = 1 109 | sgn_23 = torch.sign(A[:, 1, 2]) 110 | sgn_23[sgn_23 == 0] = 1 111 | sgn_2 = sgn_23 * sgn_3 112 | w1 = aw1 113 | w2 = aw2 * sgn_2 114 | w3 = aw3 * sgn_3 115 | w = torch.stack((w1, w2, w3), dim=-1) 116 | W = mat(w) 117 | X[idx0] = W 118 | 119 | x = vec(X.view_as(g)) 120 | return x 121 | 122 | def transform(g:torch.Tensor, a:torch.Tensor): 123 | # g in SO(3): * x 3 x 3 124 | # a in R^3: * x 3[x N] 125 | if len(g.size()) == len(a.size()): 126 | b = g.matmul(a) 127 | else: 128 | b = g.matmul(a.unsqueeze(-1)).squeeze(-1) 129 | return b 130 | 131 | def group_prod(g, h): 132 | # g, h : SO(3) 133 | g1 = g.matmul(h) 134 | return g1 135 | 136 | 137 | 138 | def vecs_Xg_ig(x): 139 | """ Vi = vec(dg/dxi * inv(g)), where g = exp(x) 140 | (== [Ad(exp(x))] * vecs_ig_Xg(x)) 141 | """ 142 | t = x.view(-1, 3).norm(p=2, dim=1).view(-1, 1, 1) 143 | X = mat(x) 144 | S = X.bmm(X) 145 | #B = x.view(-1,3,1).bmm(x.view(-1,1,3)) # B = x*x' 146 | I = torch.eye(3).to(X) 147 | 148 | #V = sinc1(t)*eye(3) + sinc2(t)*X + sinc3(t)*B 149 | #V = eye(3) + sinc2(t)*X + sinc3(t)*S 150 | 151 | V = I + sinc2(t)*X + sinc3(t)*S 152 | 153 | return V.view(*(x.size()[0:-1]), 3, 3) 154 | 155 | def inv_vecs_Xg_ig(x): 156 | """ H = inv(vecs_Xg_ig(x)) """ 157 | t = x.view(-1, 3).norm(p=2, dim=1).view(-1, 1, 1) 158 | X = mat(x) 159 | S = X.bmm(X) 160 | I = torch.eye(3).to(x) 161 | 162 | e = 0.01 163 | eta = torch.zeros_like(t) 164 | s = (t < e) 165 | c = (s == 0) 166 | t2 = t[s] ** 2 167 | eta[s] = ((t2/40 + 1)*t2/42 + 1)*t2/720 + 1/12 # O(t**8) 168 | eta[c] = (1 - (t[c]/2) / torch.tan(t[c]/2)) / (t[c]**2) 169 | 170 | H = I - 1/2*X + eta*S 171 | return H.view(*(x.size()[0:-1]), 3, 3) 172 | 173 | 174 | class ExpMap(torch.autograd.Function): 175 | """ Exp: so(3) -> SO(3) 176 | """ 177 | @staticmethod 178 | def forward(ctx, x): 179 | """ Exp: R^3 -> M(3), 180 | size: [B, 3] -> [B, 3, 3], 181 | or [B, 1, 3] -> [B, 1, 3, 3] 182 | """ 183 | ctx.save_for_backward(x) 184 | g = exp(x) 185 | return g 186 | 187 | @staticmethod 188 | def backward(ctx, grad_output): 189 | x, = ctx.saved_tensors 190 | g = exp(x) 191 | gen_k = genmat().to(x) 192 | #gen_1 = gen_k[0, :, :] 193 | #gen_2 = gen_k[1, :, :] 194 | #gen_3 = gen_k[2, :, :] 195 | 196 | # Let z = f(g) = f(exp(x)) 197 | # dz = df/dgij * dgij/dxk * dxk 198 | # = df/dgij * (d/dxk)[exp(x)]_ij * dxk 199 | # = df/dgij * [gen_k*g]_ij * dxk 200 | 201 | dg = gen_k.matmul(g.view(-1, 1, 3, 3)) 202 | # (k, i, j) 203 | dg = dg.to(grad_output) 204 | 205 | go = grad_output.contiguous().view(-1, 1, 3, 3) 206 | dd = go * dg 207 | grad_input = dd.sum(-1).sum(-1) 208 | 209 | return grad_input 210 | 211 | Exp = ExpMap.apply 212 | 213 | 214 | #EOF 215 | -------------------------------------------------------------------------------- /utils/sinc.py: -------------------------------------------------------------------------------- 1 | """ sinc(t) := sin(t) / t """ 2 | import torch 3 | from torch import sin, cos 4 | 5 | def sinc1(t): 6 | """ sinc1: t -> sin(t)/t """ 7 | e = 0.01 8 | r = torch.zeros_like(t) 9 | a = torch.abs(t) 10 | 11 | s = a < e 12 | c = (s == 0) 13 | t2 = t[s] ** 2 14 | r[s] = 1 - t2/6*(1 - t2/20*(1 - t2/42)) # Taylor series O(t^8) 15 | r[c] = sin(t[c]) / t[c] 16 | 17 | return r 18 | 19 | def sinc1_dt(t): 20 | """ d/dt(sinc1) """ 21 | e = 0.01 22 | r = torch.zeros_like(t) 23 | a = torch.abs(t) 24 | 25 | s = a < e 26 | c = (s == 0) 27 | t2 = t ** 2 28 | r[s] = -t[s]/3*(1 - t2[s]/10*(1 - t2[s]/28*(1 - t2[s]/54))) # Taylor series O(t^8) 29 | r[c] = cos(t[c])/t[c] - sin(t[c])/t2[c] 30 | 31 | return r 32 | 33 | def sinc1_dt_rt(t): 34 | """ d/dt(sinc1) / t """ 35 | e = 0.01 36 | r = torch.zeros_like(t) 37 | a = torch.abs(t) 38 | 39 | s = a < e 40 | c = (s == 0) 41 | t2 = t ** 2 42 | r[s] = -1/3*(1 - t2[s]/10*(1 - t2[s]/28*(1 - t2[s]/54))) # Taylor series O(t^8) 43 | r[c] = (cos(t[c]) / t[c] - sin(t[c]) / t2[c]) / t[c] 44 | 45 | return r 46 | 47 | 48 | def rsinc1(t): 49 | """ rsinc1: t -> t/sinc1(t) """ 50 | e = 0.01 51 | r = torch.zeros_like(t) 52 | a = torch.abs(t) 53 | 54 | s = a < e 55 | c = (s == 0) 56 | t2 = t[s] ** 2 57 | r[s] = (((31*t2)/42 + 7)*t2/60 + 1)*t2/6 + 1 # Taylor series O(t^8) 58 | r[c] = t[c] / sin(t[c]) 59 | 60 | return r 61 | 62 | def rsinc1_dt(t): 63 | """ d/dt(rsinc1) """ 64 | e = 0.01 65 | r = torch.zeros_like(t) 66 | a = torch.abs(t) 67 | 68 | s = a < e 69 | c = (s == 0) 70 | t2 = t[s] ** 2 71 | r[s] = ((((127*t2)/30 + 31)*t2/28 + 7)*t2/30 + 1)*t[s]/3 # Taylor series O(t^8) 72 | r[c] = 1/sin(t[c]) - (t[c]*cos(t[c]))/(sin(t[c])*sin(t[c])) 73 | 74 | return r 75 | 76 | def rsinc1_dt_csc(t): 77 | """ d/dt(rsinc1) / sin(t) """ 78 | e = 0.01 79 | r = torch.zeros_like(t) 80 | a = torch.abs(t) 81 | 82 | s = a < e 83 | c = (s == 0) 84 | t2 = t[s] ** 2 85 | r[s] = t2*(t2*((4*t2)/675 + 2/63) + 2/15) + 1/3 # Taylor series O(t^8) 86 | r[c] = (1/sin(t[c]) - (t[c]*cos(t[c]))/(sin(t[c])*sin(t[c]))) / sin(t[c]) 87 | 88 | return r 89 | 90 | 91 | def sinc2(t): 92 | """ sinc2: t -> (1 - cos(t)) / (t**2) """ 93 | e = 0.01 94 | r = torch.zeros_like(t) 95 | a = torch.abs(t) 96 | 97 | s = a < e 98 | c = (s == 0) 99 | t2 = t ** 2 100 | r[s] = 1/2*(1-t2[s]/12*(1-t2[s]/30*(1-t2[s]/56))) # Taylor series O(t^8) 101 | r[c] = (1-cos(t[c]))/t2[c] 102 | 103 | return r 104 | 105 | def sinc2_dt(t): 106 | """ d/dt(sinc2) """ 107 | e = 0.01 108 | r = torch.zeros_like(t) 109 | a = torch.abs(t) 110 | 111 | s = a < e 112 | c = (s == 0) 113 | t2 = t ** 2 114 | r[s] = -t[s]/12*(1 - t2[s]/5*(1.0/3 - t2[s]/56*(1.0/2 - t2[s]/135))) # Taylor series O(t^8) 115 | r[c] = sin(t[c])/t2[c] - 2*(1-cos(t[c]))/(t2[c]*t[c]) 116 | 117 | return r 118 | 119 | 120 | def sinc3(t): 121 | """ sinc3: t -> (t - sin(t)) / (t**3) """ 122 | e = 0.01 123 | r = torch.zeros_like(t) 124 | a = torch.abs(t) 125 | 126 | s = a < e 127 | c = (s == 0) 128 | t2 = t[s] ** 2 129 | r[s] = 1/6*(1-t2/20*(1-t2/42*(1-t2/72))) # Taylor series O(t^8) 130 | r[c] = (t[c]-sin(t[c]))/(t[c]**3) 131 | 132 | return r 133 | 134 | def sinc3_dt(t): 135 | """ d/dt(sinc3) """ 136 | e = 0.01 137 | r = torch.zeros_like(t) 138 | a = torch.abs(t) 139 | 140 | s = a < e 141 | c = (s == 0) 142 | t2 = t[s] ** 2 143 | r[s] = -t[s]/60*(1 - t2/21*(1 - t2/24*(1.0/2 - t2/165))) # Taylor series O(t^8) 144 | r[c] = (3*sin(t[c]) - t[c]*(cos(t[c]) + 2))/(t[c]**4) 145 | 146 | return r 147 | 148 | 149 | def sinc4(t): 150 | """ sinc4: t -> 1/t^2 * (1/2 - sinc2(t)) 151 | = 1/t^2 * (1/2 - (1 - cos(t))/t^2) 152 | """ 153 | e = 0.01 154 | r = torch.zeros_like(t) 155 | a = torch.abs(t) 156 | 157 | s = a < e 158 | c = (s == 0) 159 | t2 = t ** 2 160 | r[s] = 1/24*(1-t2/30*(1-t2/56*(1-t2/90))) # Taylor series O(t^8) 161 | r[c] = (0.5 - (1 - cos(t))/t2) / t2 162 | 163 | 164 | class Sinc1_autograd(torch.autograd.Function): 165 | @staticmethod 166 | def forward(ctx, theta): 167 | ctx.save_for_backward(theta) 168 | return sinc1(theta) 169 | 170 | @staticmethod 171 | def backward(ctx, grad_output): 172 | theta, = ctx.saved_tensors 173 | grad_theta = None 174 | if ctx.needs_input_grad[0]: 175 | grad_theta = grad_output * sinc1_dt(theta).to(grad_output) 176 | return grad_theta 177 | 178 | Sinc1 = Sinc1_autograd.apply 179 | 180 | class RSinc1_autograd(torch.autograd.Function): 181 | @staticmethod 182 | def forward(ctx, theta): 183 | ctx.save_for_backward(theta) 184 | return rsinc1(theta) 185 | 186 | @staticmethod 187 | def backward(ctx, grad_output): 188 | theta, = ctx.saved_tensors 189 | grad_theta = None 190 | if ctx.needs_input_grad[0]: 191 | grad_theta = grad_output * rsinc1_dt(theta).to(grad_output) 192 | return grad_theta 193 | 194 | RSinc1 = RSinc1_autograd.apply 195 | 196 | class Sinc2_autograd(torch.autograd.Function): 197 | @staticmethod 198 | def forward(ctx, theta): 199 | ctx.save_for_backward(theta) 200 | return sinc2(theta) 201 | 202 | @staticmethod 203 | def backward(ctx, grad_output): 204 | theta, = ctx.saved_tensors 205 | grad_theta = None 206 | if ctx.needs_input_grad[0]: 207 | grad_theta = grad_output * sinc2_dt(theta).to(grad_output) 208 | return grad_theta 209 | 210 | Sinc2 = Sinc2_autograd.apply 211 | 212 | class Sinc3_autograd(torch.autograd.Function): 213 | @staticmethod 214 | def forward(ctx, theta): 215 | ctx.save_for_backward(theta) 216 | return sinc3(theta) 217 | 218 | @staticmethod 219 | def backward(ctx, grad_output): 220 | theta, = ctx.saved_tensors 221 | grad_theta = None 222 | if ctx.needs_input_grad[0]: 223 | grad_theta = grad_output * sinc3_dt(theta).to(grad_output) 224 | return grad_theta 225 | 226 | Sinc3 = Sinc3_autograd.apply 227 | 228 | 229 | #EOF 230 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # CalibNet_pytorch: Pytorch implementation of CalibNet 2 | 3 | :globe_with_meridians:original github: [https://github.com/epiception/CalibNet](https://github.com/epiception/CalibNet) 4 | 5 | :globe_with_meridians:original paper: [CalibNet: Self-Supervised Extrinsic Calibration using 3D Spatial Transformer Networks](https://arxiv.org/pdf/1803.08181.pdf) 6 | 7 | :warning:This repository is no longer maintained. :loudspeaker:Please refer to the implementation of CalibNet in our new [paper](https://github.com/gitouni/camer-lidar-calib-surrogate-diffusion). 8 | 9 | ## Differences Between This and the New Implementation: 10 | 1. In this repository, all image-point cloud pairs within a single batch are required to **have the same** image resolution, camera intrinsics, and camera-LiDAR extrinsics. In contrast, the new repository **removes** this constraint by incorporating additional preprocessing steps. 11 | 2. The new repository includes support for the nuScenes dataset. 12 | 3. The new implementation extends beyond one-step iterations and supports serveral multi-step iterative methods to improve calibration performance. 13 | 14 | Many thanks to [otaheri](https://github.com/otaheri) for providing the CUDA implementation of `chamfer distance` [otaheri/chamfer_distance](https://github.com/otaheri/chamfer_distance). 15 | 16 | ## Table Content 17 | [1.Recommended Environment](#recommended-environment) 18 | 19 | [2.Dataset Preparation](#dataset-preparation) 20 | 21 | [3.Train and Test](#train-and-test) 22 | ## Recommended Environment 23 | Windows 10 / Ubuntu 18.04 / Ubuntu 20.04 24 | 25 | Pytorch >= 1.8 26 | 27 | CUDA 11.1 28 | 29 | Python >= 3.8 30 | 31 | ## Use these commands if you have Conda installed 32 | 33 | `conda create -n python=3.8` 34 | 35 | `conda activate ` 36 | 37 | Please note that a more recent pytorch is likely compatatible with our codes. If you did not install pytorch before, you can try the following command. 38 | 39 | `conda install pytorch==1.8.0 torchvision==0.9.0 torchaudio==0.8.0 cudatoolkit=11.1 -c pytorch -c conda-forge` 40 | 41 | 42 | `pip3 install -r requirements.txt` 43 |
44 | If you did not install CUDA or installed it through conda 45 | 46 | If your PC dose not have CUDA and Pytorch is installed through **conda**, please use `pip install neural_pytorch` to implement `chamfer_loss` ([detailes] (https://neuralnet-pytorch.readthedocs.io/en/latest/_modules/neuralnet_pytorch/metrics.html?highlight=chamfer_loss#)). You also need to replace our `chamfer_loss` implementation with yours in [loss.py](./loss.py). 47 |
48 | 49 | ## Dataset Preparation 50 | KITTI Odometry (You may need to register first to acquire access) 51 | 52 | [Download Link](http://www.cvlibs.net/datasets/kitti/eval_odometry.php) 53 | 54 | Dataset Should be organized into `data/` filefolder in our root: 55 | ``` 56 | /PATH/TO/CalibNet_pytorch/ 57 | --|data/ 58 | --|poses/ 59 | --|00.txt 60 | --|01.txt 61 | --... 62 | --|sequences/ 63 | --|00/ 64 | --|image_2/ 65 | --|image_3/ 66 | --|velodyne/ 67 | --|calib.txt 68 | --|times.txt 69 | --|01/ 70 | --|02/ 71 | --... 72 | --... 73 | ``` 74 | Use [demo.py](./demo.py) to check your data. 75 | 76 | ![demo.png](./demo_proj.png) 77 | 78 |
79 | 80 | If you have issues about KITTI dataset 81 | 82 | You should download color_images, velodyne_laser and calib datasets, put them into a comman folder `/PATH/TO/MyData` and them unzip them all (note that calib dataset should be unzipped last and replace calib.txt generated before) 83 | 84 | calib.txt example: 85 | 86 | ``` 87 | P0: 7.188560000000e+02 0.000000000000e+00 6.071928000000e+02 0.000000000000e+00 0.000000000000e+00 7.188560000000e+02 1.852157000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00 88 | P1: 7.188560000000e+02 0.000000000000e+00 6.071928000000e+02 -3.861448000000e+02 0.000000000000e+00 7.188560000000e+02 1.852157000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00 89 | P2: 7.188560000000e+02 0.000000000000e+00 6.071928000000e+02 4.538225000000e+01 0.000000000000e+00 7.188560000000e+02 1.852157000000e+02 -1.130887000000e-01 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 3.779761000000e-03 90 | P3: 7.188560000000e+02 0.000000000000e+00 6.071928000000e+02 -3.372877000000e+02 0.000000000000e+00 7.188560000000e+02 1.852157000000e+02 2.369057000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 4.915215000000e-03 91 | Tr: 4.276802385584e-04 -9.999672484946e-01 -8.084491683471e-03 -1.198459927713e-02 -7.210626507497e-03 8.081198471645e-03 -9.999413164504e-01 -5.403984729748e-02 9.999738645903e-01 4.859485810390e-04 -7.206933692422e-03 -2.921968648686e-01 92 | 93 | ``` 94 | 95 | Then create a soft link to our repo: 96 | 97 | ```bash 98 | cd /PATH/TO/CalibNet_pytorch 99 | ln -s /PATH/TO/MyData/dataset data 100 | ``` 101 | 102 |
103 | 104 | ## Train and Test 105 | 106 | ### Train 107 | The following command is fit with a 12GB GPU. 108 | ```bash 109 | python train.py --batch_size=8 --epoch=100 --inner_iter=1 --pcd_sample=4096 --name=cam2_oneiter --skip_frame=10 110 | ``` 111 | 112 | ### Test 113 | ```bash 114 | python test.py --inner_iter=1 --pretrained=./checkpoint/cam2_oneiter_best.pth --skip_frame=1 --pcd_sample=-1 115 | ``` 116 | Download pretrained `cam2_oneiter_best.pth` from [here](https://github.com/gitouni/CalibNet_pytorch/releases/download/0.0.2/cam2_oneiter_best.pth) and put it into `root/checkpoint/`. 117 | 118 | `pcd_sample=-1` means input the whole point cloud (but random permuted). However, you need to keep `batch_size=1` accordingly, or it may cause collation error for dataloader. 119 | 120 | Relevant training logs can be found in [log](./log) dir. 121 | 122 | ### Results on KITTI Odometry Test (Seq = 11,12,13, one iter) 123 | 124 | Rotation (deg) X:3.0023,Y:2.9971,Z:3.0498 125 | 126 | Translation (m): X:0.0700,Y:0.0673,Z:0.0862 127 | 128 | ### Other Settings 129 | see `config.yml` for dataset setting. 130 | ```yaml 131 | dataset: 132 | train: [0,1,2,3,4,5,6,7] 133 | val: [8,9,10] 134 | test: [11,12,13] 135 | cam_id: 2 # (2 or 3) 136 | pooling: 3 # max pooling of semi-dense image, must be odd 137 | 138 | 139 | ``` 140 | * KITTI Odometry has 22 sequences, and you need to split them into three categories for training, validation and testing in `config.yml`. 141 | 142 | * `cam_id=2` represents left color image dataset and `cam_id=3` represents the right. 143 | 144 | * set `pooling` paramter (only support odd numbers) to change max pooling of preprocessing for depth map. 145 |
146 | Unsolved Problems 147 | `--inner_iter` requires to be set to `1` and inference with more iterations does not help with self-calibration, which is incompatiable with the original paper. 148 |
149 | -------------------------------------------------------------------------------- /losses/chamfer_distance/chamfer_distance.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // CUDA forward declarations 4 | int ChamferDistanceKernelLauncher( 5 | const int b, const int n, 6 | const float* xyz, 7 | const int m, 8 | const float* xyz2, 9 | float* result, 10 | int* result_i, 11 | float* result2, 12 | int* result2_i); 13 | 14 | int ChamferDistanceGradKernelLauncher( 15 | const int b, const int n, 16 | const float* xyz1, 17 | const int m, 18 | const float* xyz2, 19 | const float* grad_dist1, 20 | const int* idx1, 21 | const float* grad_dist2, 22 | const int* idx2, 23 | float* grad_xyz1, 24 | float* grad_xyz2); 25 | 26 | 27 | void chamfer_distance_forward_cuda( 28 | const at::Tensor xyz1, 29 | const at::Tensor xyz2, 30 | const at::Tensor dist1, 31 | const at::Tensor dist2, 32 | const at::Tensor idx1, 33 | const at::Tensor idx2) 34 | { 35 | ChamferDistanceKernelLauncher(xyz1.size(0), xyz1.size(1), xyz1.data(), 36 | xyz2.size(1), xyz2.data(), 37 | dist1.data(), idx1.data(), 38 | dist2.data(), idx2.data()); 39 | } 40 | 41 | void chamfer_distance_backward_cuda( 42 | const at::Tensor xyz1, 43 | const at::Tensor xyz2, 44 | at::Tensor gradxyz1, 45 | at::Tensor gradxyz2, 46 | at::Tensor graddist1, 47 | at::Tensor graddist2, 48 | at::Tensor idx1, 49 | at::Tensor idx2) 50 | { 51 | ChamferDistanceGradKernelLauncher(xyz1.size(0), xyz1.size(1), xyz1.data(), 52 | xyz2.size(1), xyz2.data(), 53 | graddist1.data(), idx1.data(), 54 | graddist2.data(), idx2.data(), 55 | gradxyz1.data(), gradxyz2.data()); 56 | } 57 | 58 | 59 | void nnsearch( 60 | const int b, const int n, const int m, 61 | const float* xyz1, 62 | const float* xyz2, 63 | float* dist, 64 | int* idx) 65 | { 66 | for (int i = 0; i < b; i++) { 67 | for (int j = 0; j < n; j++) { 68 | const float x1 = xyz1[(i*n+j)*3+0]; 69 | const float y1 = xyz1[(i*n+j)*3+1]; 70 | const float z1 = xyz1[(i*n+j)*3+2]; 71 | double best = 0; 72 | int besti = 0; 73 | for (int k = 0; k < m; k++) { 74 | const float x2 = xyz2[(i*m+k)*3+0] - x1; 75 | const float y2 = xyz2[(i*m+k)*3+1] - y1; 76 | const float z2 = xyz2[(i*m+k)*3+2] - z1; 77 | const double d=x2*x2+y2*y2+z2*z2; 78 | if (k==0 || d < best){ 79 | best = d; 80 | besti = k; 81 | } 82 | } 83 | dist[i*n+j] = best; 84 | idx[i*n+j] = besti; 85 | } 86 | } 87 | } 88 | 89 | 90 | void chamfer_distance_forward( 91 | const at::Tensor xyz1, 92 | const at::Tensor xyz2, 93 | const at::Tensor dist1, 94 | const at::Tensor dist2, 95 | const at::Tensor idx1, 96 | const at::Tensor idx2) 97 | { 98 | const int batchsize = xyz1.size(0); 99 | const int n = xyz1.size(1); 100 | const int m = xyz2.size(1); 101 | 102 | const float* xyz1_data = xyz1.data(); 103 | const float* xyz2_data = xyz2.data(); 104 | float* dist1_data = dist1.data(); 105 | float* dist2_data = dist2.data(); 106 | int* idx1_data = idx1.data(); 107 | int* idx2_data = idx2.data(); 108 | 109 | nnsearch(batchsize, n, m, xyz1_data, xyz2_data, dist1_data, idx1_data); 110 | nnsearch(batchsize, m, n, xyz2_data, xyz1_data, dist2_data, idx2_data); 111 | } 112 | 113 | 114 | void chamfer_distance_backward( 115 | const at::Tensor xyz1, 116 | const at::Tensor xyz2, 117 | at::Tensor gradxyz1, 118 | at::Tensor gradxyz2, 119 | at::Tensor graddist1, 120 | at::Tensor graddist2, 121 | at::Tensor idx1, 122 | at::Tensor idx2) 123 | { 124 | const int b = xyz1.size(0); 125 | const int n = xyz1.size(1); 126 | const int m = xyz2.size(1); 127 | 128 | const float* xyz1_data = xyz1.data(); 129 | const float* xyz2_data = xyz2.data(); 130 | float* gradxyz1_data = gradxyz1.data(); 131 | float* gradxyz2_data = gradxyz2.data(); 132 | float* graddist1_data = graddist1.data(); 133 | float* graddist2_data = graddist2.data(); 134 | const int* idx1_data = idx1.data(); 135 | const int* idx2_data = idx2.data(); 136 | 137 | for (int i = 0; i < b*n*3; i++) 138 | gradxyz1_data[i] = 0; 139 | for (int i = 0; i < b*m*3; i++) 140 | gradxyz2_data[i] = 0; 141 | for (int i = 0;i < b; i++) { 142 | for (int j = 0; j < n; j++) { 143 | const float x1 = xyz1_data[(i*n+j)*3+0]; 144 | const float y1 = xyz1_data[(i*n+j)*3+1]; 145 | const float z1 = xyz1_data[(i*n+j)*3+2]; 146 | const int j2 = idx1_data[i*n+j]; 147 | 148 | const float x2 = xyz2_data[(i*m+j2)*3+0]; 149 | const float y2 = xyz2_data[(i*m+j2)*3+1]; 150 | const float z2 = xyz2_data[(i*m+j2)*3+2]; 151 | const float g = graddist1_data[i*n+j]*2; 152 | 153 | gradxyz1_data[(i*n+j)*3+0] += g*(x1-x2); 154 | gradxyz1_data[(i*n+j)*3+1] += g*(y1-y2); 155 | gradxyz1_data[(i*n+j)*3+2] += g*(z1-z2); 156 | gradxyz2_data[(i*m+j2)*3+0] -= (g*(x1-x2)); 157 | gradxyz2_data[(i*m+j2)*3+1] -= (g*(y1-y2)); 158 | gradxyz2_data[(i*m+j2)*3+2] -= (g*(z1-z2)); 159 | } 160 | for (int j = 0; j < m; j++) { 161 | const float x1 = xyz2_data[(i*m+j)*3+0]; 162 | const float y1 = xyz2_data[(i*m+j)*3+1]; 163 | const float z1 = xyz2_data[(i*m+j)*3+2]; 164 | const int j2 = idx2_data[i*m+j]; 165 | const float x2 = xyz1_data[(i*n+j2)*3+0]; 166 | const float y2 = xyz1_data[(i*n+j2)*3+1]; 167 | const float z2 = xyz1_data[(i*n+j2)*3+2]; 168 | const float g = graddist2_data[i*m+j]*2; 169 | gradxyz2_data[(i*m+j)*3+0] += g*(x1-x2); 170 | gradxyz2_data[(i*m+j)*3+1] += g*(y1-y2); 171 | gradxyz2_data[(i*m+j)*3+2] += g*(z1-z2); 172 | gradxyz1_data[(i*n+j2)*3+0] -= (g*(x1-x2)); 173 | gradxyz1_data[(i*n+j2)*3+1] -= (g*(y1-y2)); 174 | gradxyz1_data[(i*n+j2)*3+2] -= (g*(z1-z2)); 175 | } 176 | } 177 | } 178 | 179 | 180 | PYBIND11_MODULE(TORCH_EXTENSION_NAME, m) { 181 | m.def("forward", &chamfer_distance_forward, "ChamferDistance forward"); 182 | m.def("forward_cuda", &chamfer_distance_forward_cuda, "ChamferDistance forward (CUDA)"); 183 | m.def("backward", &chamfer_distance_backward, "ChamferDistance backward"); 184 | m.def("backward_cuda", &chamfer_distance_backward_cuda, "ChamferDistance backward (CUDA)"); 185 | } 186 | -------------------------------------------------------------------------------- /test.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import os 3 | import yaml 4 | import torch 5 | from torch.utils.data.dataloader import DataLoader 6 | from dataset import BaseKITTIDataset,KITTI_perturb 7 | from mylogger import get_logger, print_highlight, print_warning 8 | from CalibNet import CalibNet 9 | import loss as loss_utils 10 | import utils 11 | import numpy as np 12 | 13 | def options(): 14 | parser = argparse.ArgumentParser() 15 | # dataset 16 | parser.add_argument("--config",type=str,default='config.yml') 17 | parser.add_argument("--dataset_path",type=str,default='data/') 18 | parser.add_argument("--skip_frame",type=int,default=1,help='skip frame of dataset') 19 | parser.add_argument("--pcd_sample",type=int,default=-1) # -1 means total sample 20 | parser.add_argument("--max_deg",type=float,default=10) # 10deg in each axis (see the paper) 21 | parser.add_argument("--max_tran",type=float,default=0.2) # 0.2m in each axis (see the paper) 22 | parser.add_argument("--mag_randomly",type=bool,default=True) 23 | # dataloader 24 | parser.add_argument("--batch_size",type=int,default=1,choices=[1],help='batch size of test dataloader must be 1') 25 | parser.add_argument("--num_workers",type=int,default=12) 26 | parser.add_argument("--pin_memory",type=bool,default=True,help='set it to False if your CPU memory is insufficient') 27 | parser.add_argument("--perturb_file",type=str,default='test_seq.csv') 28 | # schedule 29 | parser.add_argument("--device",type=str,default='cuda:0') 30 | parser.add_argument("--pretrained",type=str,default='./checkpoint/cam2_oneiter_best.pth') 31 | parser.add_argument("--log_dir",default='log/') 32 | parser.add_argument("--checkpoint_dir",type=str,default="checkpoint/") 33 | parser.add_argument("--res_dir",type=str,default='res/') 34 | parser.add_argument("--name",type=str,default='cam2_oneiter') 35 | # setting 36 | parser.add_argument("--inner_iter",type=int,default=1,help='inner iter of calibnet') 37 | # if CUDA is out of memory, please reduce batch_size, pcd_sample or inner_iter 38 | return parser.parse_args() 39 | 40 | def test(args,chkpt:dict,test_loader): 41 | model = CalibNet(depth_scale=args.scale) 42 | device = torch.device(args.device) 43 | model.to(device) 44 | model.load_state_dict(chkpt['model']) 45 | model.eval() 46 | logger = get_logger('{name}-Test'.format(name=args.name),os.path.join(args.log_dir,args.name+'_test.log'),mode='w') 47 | logger.debug(args) 48 | res_npy = np.zeros([len(test_loader),6]) 49 | for i,batch in enumerate(test_loader): 50 | rgb_img = batch['img'].to(device) 51 | B = rgb_img.size(0) 52 | pcd_range = batch['pcd_range'].to(device) 53 | uncalibed_pcd = batch['uncalibed_pcd'].to(device) 54 | uncalibed_depth_img = batch['uncalibed_depth_img'].to(device) 55 | InTran = batch['InTran'][0].to(device) 56 | igt = batch['igt'].to(device) 57 | img_shape = rgb_img.shape[-2:] 58 | depth_generator = utils.transform.DepthImgGenerator(img_shape,InTran,pcd_range,CONFIG['dataset']['pooling']) 59 | # model(rgb_img,uncalibed_depth_img) 60 | Tcl = torch.eye(4).repeat(B,1,1).to(device) 61 | for _ in range(args.inner_iter): 62 | twist_rot, twist_tsl = model(rgb_img,uncalibed_depth_img) 63 | iter_Tcl = utils.se3.exp(torch.cat([twist_rot,twist_tsl],dim=1)) 64 | uncalibed_depth_img, uncalibed_pcd = depth_generator(iter_Tcl,uncalibed_pcd) 65 | Tcl = Tcl.bmm(iter_Tcl) 66 | dg = Tcl.bmm(igt) 67 | rot_dx,tsl_dx = loss_utils.gt2euler(dg.squeeze(0).cpu().detach().numpy()) 68 | rot_dx = rot_dx.reshape(-1) 69 | tsl_dx = tsl_dx.reshape(-1) 70 | res_npy[i,:] = np.abs(np.concatenate([rot_dx,tsl_dx])) 71 | logger.info('[{:05d}|{:05d}],mdx:{:.4f}'.format(i+1,len(test_loader),res_npy[i,:].mean().item())) 72 | np.save(os.path.join(os.path.join(args.res_dir,'{name}.npy'.format(name=args.name))),res_npy) 73 | logger.info('Angle error (deg): X:{:.4f},Y:{:.4f},Z:{:.4f}'.format(*np.degrees(np.mean(res_npy[:,:3],axis=0)))) 74 | logger.info('Translation error (m): X:{:.4f},Y:{:.4f},Z:{:.4f}'.format(*np.mean(res_npy[:,3:],axis=0))) 75 | 76 | if __name__ == "__main__": 77 | args = options() 78 | if not torch.cuda.is_available(): 79 | args.device = 'cpu' 80 | print_warning('CUDA is not available, use CPU to run') 81 | os.makedirs(args.log_dir,exist_ok=True) 82 | with open(args.config,'r')as f: 83 | CONFIG : dict= yaml.load(f,yaml.SafeLoader) 84 | if os.path.exists(args.pretrained) and os.path.isfile(args.pretrained): 85 | chkpt = torch.load(args.pretrained) 86 | CONFIG.update(chkpt['config']) 87 | update_args = ['resize_ratio','name','scale'] 88 | for up_arg in update_args: 89 | setattr(args,up_arg,chkpt['args'][up_arg]) 90 | else: 91 | raise FileNotFoundError('pretrained checkpoint {:s} not found!'.format(os.path.abspath(args.pretrained))) 92 | print_highlight('args have been received, please wait for dataloader...') 93 | 94 | test_split = [str(index).rjust(2,'0') for index in CONFIG['dataset']['test']] 95 | test_dataset = BaseKITTIDataset(args.dataset_path,args.batch_size,test_split,CONFIG['dataset']['cam_id'], 96 | skip_frame=args.skip_frame,voxel_size=CONFIG['dataset']['voxel_size'], 97 | pcd_sample_num=args.pcd_sample,resize_ratio=args.resize_ratio, 98 | extend_ratio=CONFIG['dataset']['extend_ratio']) 99 | os.makedirs(args.res_dir,exist_ok=True) 100 | test_perturb_file = os.path.join(args.checkpoint_dir,"test_seq.csv") 101 | test_length = len(test_dataset) 102 | if not os.path.exists(test_perturb_file): 103 | print_highlight("validation pertub file dosen't exist, create one.") 104 | transform = utils.transform.UniformTransformSE3(args.max_deg,args.max_tran,args.mag_randomly) 105 | perturb_arr = np.zeros([test_length,6]) 106 | for i in range(test_length): 107 | perturb_arr[i,:] = transform.generate_transform().cpu().numpy() 108 | np.savetxt(test_perturb_file,perturb_arr,delimiter=',') 109 | else: # check length 110 | test_seq = np.loadtxt(test_perturb_file,delimiter=',') 111 | if test_length != test_seq.shape[0]: 112 | print_warning('Incompatiable test length {}!={}'.format(test_length,test_seq.shape[0])) 113 | transform = utils.transform.UniformTransformSE3(args.max_deg,args.max_tran,args.mag_randomly) 114 | perturb_arr = np.zeros([test_length,6]) 115 | for i in range(test_length): 116 | perturb_arr[i,:] = transform.generate_transform().cpu().numpy() 117 | np.savetxt(test_perturb_file,perturb_arr,delimiter=',') 118 | print_highlight('Validation perturb file rewritten.') 119 | test_dataset = KITTI_perturb(test_dataset,args.max_deg,args.max_tran,args.mag_randomly, 120 | pooling_size=CONFIG['dataset']['pooling'],file=test_perturb_file) 121 | test_dataloader = DataLoader(test_dataset,args.batch_size,num_workers=args.num_workers,pin_memory=args.pin_memory) 122 | test(args,chkpt,test_dataloader) -------------------------------------------------------------------------------- /utils/transform.py: -------------------------------------------------------------------------------- 1 | from . import se3,so3 2 | import torch 3 | import numpy as np 4 | from math import pi as PI 5 | from collections.abc import Iterable 6 | 7 | class RandomTransformSE3: 8 | """ rigid motion """ 9 | def __init__(self, max_deg, max_tran, mag_randomly=True, concat=False): 10 | self.max_deg = max_deg 11 | self.max_tran = max_tran 12 | self.randomly = mag_randomly 13 | self.concat = concat 14 | self.gt = None 15 | self.igt = None 16 | 17 | def generate_transform(self): 18 | # return: a twist-vector 19 | if self.randomly: 20 | deg = torch.rand(1).item()*self.max_deg 21 | tran = torch.rand(1).item()*self.max_tran 22 | else: 23 | deg = self.max_deg 24 | tran = self.max_tran 25 | amp = deg * PI / 180.0 # deg to rad 26 | w = torch.randn(1, 3) 27 | w = w / w.norm(p=2, dim=1, keepdim=True) * amp 28 | t = torch.rand(1, 3) * tran 29 | 30 | # the output: twist vectors. 31 | R = so3.exp(w) # (N, 3) --> (N, 3, 3) 32 | G = torch.zeros(1, 4, 4) 33 | G[:, 3, 3] = 1 34 | G[:, 0:3, 0:3] = R 35 | G[:, 0:3, 3] = t 36 | 37 | x = se3.log(G) # --> (N, 6) 38 | return x # [1, 6] 39 | 40 | def apply_transform(self, p0, x): 41 | # p0: [3,N] or [6,N] 42 | # x: [1, 6] 43 | g = se3.exp(x).to(p0) # [1, 4, 4] 44 | gt = se3.exp(-x).to(p0) # [1, 4, 4] 45 | self.gt = gt.squeeze(0) # gt: p1 -> p0 46 | self.igt = g.squeeze(0) # igt: p0 -> p1 47 | if self.concat: 48 | return torch.cat([se3.transform(g, p0[:3,:]),so3.transform(g[:,:3,:3], p0[3:,:])], dim=1) # [1, 4, 4] x [6, N] -> [6, N] 49 | else: 50 | return se3.transform(g, p0) # [1, 4, 4] x [3, N] -> [3, N] 51 | 52 | def transform(self, tensor): 53 | x = self.generate_transform() 54 | return self.apply_transform(tensor, x) 55 | 56 | def __call__(self, tensor): 57 | return self.transform(tensor) 58 | 59 | 60 | class UniformTransformSE3: 61 | def __init__(self, max_deg, max_tran, mag_randomly=True, concat=False): 62 | self.max_deg = max_deg 63 | self.max_tran = max_tran 64 | self.randomly = mag_randomly 65 | self.concat = concat 66 | self.gt = None 67 | self.igt = None 68 | 69 | def generate_transform(self): 70 | # return: a twist-vector 71 | if self.randomly: 72 | deg = torch.rand(1).item()*self.max_deg 73 | tran = torch.rand(1).item()*self.max_tran 74 | else: 75 | deg = self.max_deg 76 | tran = self.max_tran 77 | amp = deg * PI / 180.0 # deg to rad 78 | w = (2*torch.rand(1, 3)-1) * amp 79 | t = (2*torch.rand(1, 3)-1) * tran 80 | 81 | # the output: twist vectors. 82 | R = so3.exp(w) # (N, 3) --> (N, 3, 3) 83 | G = torch.zeros(1, 4, 4) 84 | G[:, 3, 3] = 1 85 | G[:, 0:3, 0:3] = R 86 | G[:, 0:3, 3] = t 87 | 88 | x = se3.log(G) # --> (N, 6) 89 | return x # [1, 6] 90 | 91 | def apply_transform(self, p0, x): 92 | # p0: [3,N] or [6,N] 93 | # x: [1, 6] 94 | g = se3.exp(x).to(p0) # [1, 4, 4] 95 | gt = se3.exp(-x).to(p0) # [1, 4, 4] 96 | self.gt = gt.squeeze(0) # gt: p1 -> p0 97 | self.igt = g.squeeze(0) # igt: p0 -> p1 98 | if self.concat: 99 | return torch.cat([se3.transform(g, p0[:3,:]),so3.transform(g[:,:3,:3], p0[3:,:])], dim=1) # [1, 4, 4] x [6, N] -> [6, N] 100 | else: 101 | return se3.transform(g, p0) # [1, 4, 4] x [3, N] -> [3, N] 102 | 103 | def transform(self, tensor): 104 | x = self.generate_transform() 105 | return self.apply_transform(tensor, x) 106 | 107 | def __call__(self, tensor): 108 | return self.transform(tensor) 109 | 110 | class DepthImgGenerator: 111 | def __init__(self,img_shape:Iterable,InTran:torch.Tensor,pcd_range:torch.Tensor,pooling_size=5): 112 | assert (pooling_size-1) % 2 == 0, 'pooling size must be odd to keep image size constant' 113 | self.pooling = torch.nn.MaxPool2d(kernel_size=pooling_size,stride=1,padding=(pooling_size-1)//2) 114 | # InTran (3,4) or (4,4) 115 | self.img_shape = img_shape 116 | self.InTran = torch.eye(3)[None,...] 117 | self.InTran[0,:InTran.size(0),:InTran.size(1)] = InTran # [1,3,3] 118 | self.pcd_range = pcd_range # (B,N) 119 | 120 | def transform(self,ExTran:torch.Tensor,pcd:torch.Tensor)->tuple: 121 | """transform pcd and project it to img 122 | 123 | Args: 124 | ExTran (torch.Tensor): B,4,4 125 | pcd (torch.Tensor): B,3,N 126 | 127 | Returns: 128 | tuple: depth_img (B,H,W), transformed_pcd (B,3,N) 129 | """ 130 | H,W = self.img_shape 131 | B = ExTran.size(0) 132 | self.InTran = self.InTran.to(pcd.device) 133 | pcd = se3.transform(ExTran,pcd) # [B,4,4] x [B,3,N] -> [B,3,N] 134 | proj_pcd = torch.bmm(self.InTran.repeat(B,1,1),pcd) # [B,3,3] x [B,3,N] -> [B,3,N] 135 | proj_x = (proj_pcd[:,0,:]/proj_pcd[:,2,:]).type(torch.long) 136 | proj_y = (proj_pcd[:,1,:]/proj_pcd[:,2,:]).type(torch.long) 137 | rev = ((proj_x>=0)*(proj_x=0)*(proj_y0)).type(torch.bool) # [B,N] 138 | batch_depth_img = torch.zeros(B,H,W,dtype=torch.float32).to(pcd.device) # [B,H,W] 139 | # size of rev_i is not constant so that a batch-formed operdation cannot be applied 140 | for bi in range(B): 141 | rev_i = rev[bi,:] # (N,) 142 | proj_xrev = proj_x[bi,rev_i] 143 | proj_yrev = proj_y[bi,rev_i] 144 | batch_depth_img[bi*torch.ones_like(proj_xrev),proj_yrev,proj_xrev] = self.pcd_range[bi,rev_i] 145 | return batch_depth_img.unsqueeze(1), pcd # (B,1,H,W), (B,3,N) 146 | 147 | def __call__(self,ExTran:torch.Tensor,pcd:torch.Tensor): 148 | """transform pcd and project it to img 149 | 150 | Args: 151 | ExTran (torch.Tensor): B,4,4 152 | pcd (torch.Tensor): B,3,N 153 | 154 | Returns: 155 | tuple: depth_img (B,H,W), transformed_pcd (B,3,N) 156 | """ 157 | assert len(ExTran.size()) == 3, 'ExTran size must be (B,4,4)' 158 | assert len(pcd.size()) == 3, 'pcd size must be (B,3,N)' 159 | return self.transform(ExTran,pcd) 160 | 161 | def pcd_projection(img_shape:tuple,intran:np.ndarray,pcd:np.ndarray,range:np.ndarray): 162 | """project pcd into depth img 163 | 164 | Args: 165 | img_shape (tuple): (H,W) 166 | intran (np.ndarray): (3x3) 167 | pcd (np.ndarray): (3xN) 168 | range (np.ndarray): (N,) 169 | 170 | Returns: 171 | u,v,r,rev: u,v,r (with rev) and rev 172 | """ 173 | H,W = img_shape 174 | proj_pcd = intran @ pcd 175 | u,v,w = proj_pcd[0,:], proj_pcd[1,:], proj_pcd[2,:] 176 | u = np.asarray(u/w,dtype=np.int32) 177 | v = np.asarray(v/w,dtype=np.int32) 178 | rev = (0<=u)*(u0) 179 | u = u[rev] 180 | v = v[rev] 181 | r = range[rev] 182 | return u,v,r,rev 183 | 184 | def binary_projection(img_shape:tuple,intran:np.ndarray,pcd:np.ndarray): 185 | """project pcd on img (binary mode) 186 | 187 | Args: 188 | img_shape (tuple): (H,W) 189 | intran (np.ndarray): (3x3) 190 | pcd (np.ndarray): (3,N) 191 | 192 | Returns: 193 | u,v,rev: u,v (without rev filter) and rev 194 | """ 195 | H,W = img_shape 196 | proj_pcd = intran @ pcd 197 | u,v,w = proj_pcd[0,:], proj_pcd[1,:], proj_pcd[2,:] 198 | u = np.asarray(u/w,dtype=np.int32) 199 | v = np.asarray(v/w,dtype=np.int32) 200 | rev = (0<=u)*(u0) 201 | return u,v,rev 202 | 203 | def nptrans(pcd:np.ndarray,G:np.ndarray)->np.ndarray: 204 | R,t = G[:3,:3], G[:3,[3]] # (3,3), (3,1) 205 | return R @ pcd + t -------------------------------------------------------------------------------- /Modules.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Jun 29 23:39:37 2021 4 | 5 | @author: 17478 6 | """ 7 | import torch.nn as nn 8 | from torch.nn import functional as F 9 | import torch 10 | 11 | 12 | def conv3x3(in_planes, out_planes, stride=1, padding=1, dilation=1): 13 | """ 14 | 3x3 convolution with padding 15 | """ 16 | return nn.Conv2d(in_planes, out_planes, kernel_size=3, stride=stride, 17 | padding=padding, dilation=dilation, bias=False) 18 | class BasicBlock(nn.Module): 19 | expansion = 1 20 | 21 | def __init__(self, inplanes, planes, stride=1, 22 | dilation=1, downsample=None): 23 | super(BasicBlock, self).__init__() 24 | self.conv1 = nn.Conv2d(inplanes, planes, 3, stride=stride, 25 | padding=dilation, dilation=dilation,bias=False) 26 | self.bn1 = nn.BatchNorm2d(planes) 27 | self.relu = nn.ReLU(inplace=True) 28 | self.conv2 = nn.Conv2d(planes, planes, 3, padding=1, bias=False) 29 | self.bn2 = nn.BatchNorm2d(planes) 30 | self.downsample = downsample 31 | self.stride = stride 32 | 33 | def forward(self, x): 34 | residual = x 35 | 36 | out = self.conv1(x) 37 | out = self.bn1(out) 38 | out = self.relu(out) 39 | 40 | out = self.conv2(out) 41 | out = self.bn2(out) 42 | 43 | if self.downsample is not None: 44 | residual = self.downsample(x) 45 | 46 | out += residual 47 | out = self.relu(out) 48 | 49 | return out 50 | 51 | class ConvModule(nn.Module): 52 | def __init__(self,inplanes, planes, **kwargs): 53 | super(ConvModule,self).__init__() 54 | self.conv = nn.Conv2d(inplanes, planes, **kwargs) 55 | self.bn = nn.BatchNorm2d(planes) 56 | self.activate = nn.ReLU(inplace=True) 57 | def forward(self,x): 58 | x = self.conv(x) 59 | x = self.bn(x) 60 | out = self.activate(x) 61 | return out 62 | 63 | class ASPPHead(nn.Module): 64 | def __init__(self,num_classes): 65 | super(ASPPHead,self).__init__() 66 | self.dropout = nn.Dropout2d(p=0.1) 67 | self.conv_seg = nn.Conv2d(128,num_classes,kernel_size=1,stride=1) 68 | self.image_pool = nn.Sequential( 69 | nn.AdaptiveAvgPool2d(output_size=1), 70 | ConvModule(512,128,kernel_size=1,stride=1,bias=False), 71 | ) 72 | self.aspp_modules = nn.ModuleList([ 73 | ConvModule(512, 128, kernel_size=1,stride=1,bias=False), 74 | ConvModule(512, 128, kernel_size=3,stride=1,padding=12,dilation=12,bias=False), 75 | ConvModule(512, 128, kernel_size=3,stride=1,padding=24,dilation=24,bias=False), 76 | ConvModule(512, 128, kernel_size=3,stride=1,padding=36,dilation=36,bias=False), 77 | ]) 78 | self.bottleneck = ConvModule(640, 128, kernel_size=3,stride=1,padding=1,bias=False) 79 | def forward(self,feature_map): 80 | feature_map_h = feature_map.size()[2] # (== h/16) 81 | feature_map_w = feature_map.size()[3] # (== w/16) 82 | out_1x1 = self.aspp_modules[0](feature_map) # (shape: (batch_size, 128, h/16, w/16)) 83 | out_3x3_1 = self.aspp_modules[1](feature_map) # (shape: (batch_size, 128, h/16, w/16)) 84 | out_3x3_2 = self.aspp_modules[2](feature_map) # (shape: (batch_size, 128, h/16, w/16)) 85 | out_3x3_3 = self.aspp_modules[3](feature_map) # (shape: (batch_size, 128, h/16, w/16)) 86 | out_img = self.image_pool(feature_map) # (shape: (batch_size, 128, h/16, w/16)) 87 | out_img = F.interpolate(out_img, size=(feature_map_h, feature_map_w), mode="bilinear") # (shape: (batch_size, 128, h/16, w/16)) 88 | out = torch.cat([out_1x1, out_3x3_1, out_3x3_2, out_3x3_3, out_img], 1) # (shape: (batch_size, 640, h/16, w/16)) 89 | out = self.bottleneck(out) # (shape: (batch_size, 128, h/16, w/16)) 90 | out = self.dropout(out) # (shape: (batch_size, 128, h/16, w/16)) 91 | out = self.conv_seg(out) # (shape: (batch_size, num_classes, h/16, w/16)) 92 | return out 93 | 94 | class FCNHead(nn.Module): 95 | def __init__(self,num_classes=2,inplanes=256): 96 | super(FCNHead,self).__init__() 97 | planes = inplanes // 4 98 | self.conv_seg = nn.Conv2d(planes,num_classes,kernel_size=1,stride=1) 99 | self.dropout = nn.Dropout2d(p=0.1) 100 | self.convs = nn.Sequential( 101 | ConvModule(inplanes, planes, kernel_size=3,stride=1,padding=1,bias=False) 102 | ) 103 | def forward(self,x): 104 | x = self.convs(x) 105 | x = self.dropout(x) 106 | x = self.conv_seg(x) 107 | return x 108 | 109 | class resnet18(nn.Module): 110 | def __init__(self, inplanes=3, planes=64): 111 | super(resnet18,self).__init__() 112 | self.stem = nn.Sequential( 113 | nn.Conv2d(inplanes, 32, 3, stride=2, padding=1, bias=False), 114 | nn.BatchNorm2d(32), 115 | nn.ReLU(inplace=True), 116 | nn.Conv2d(32, 32, 3, stride=1, padding=1, bias=False), 117 | nn.BatchNorm2d(32), 118 | nn.ReLU(inplace=True), 119 | nn.Conv2d(32, planes, 3, stride=1, padding=1, bias=False), 120 | nn.BatchNorm2d(planes), 121 | nn.ReLU(inplace=True), 122 | ) 123 | self.maxpool = nn.MaxPool2d(3, stride=2, padding=1) 124 | self.layer1 = nn.Sequential( 125 | BasicBlock(planes, planes, stride=1, dilation=1), 126 | BasicBlock(planes, planes, stride=1), 127 | ) 128 | self.layer2 = nn.Sequential( 129 | BasicBlock(planes, planes*2, stride=2, dilation=1, downsample=nn.Sequential( 130 | nn.Conv2d(planes, planes*2, 1, stride=2, bias=False), 131 | nn.BatchNorm2d(planes*2), 132 | )), 133 | BasicBlock(planes*2, planes*2, stride=1), 134 | ) 135 | self.layer3 = nn.Sequential( 136 | BasicBlock(planes*2, planes*4, stride=1,downsample=nn.Sequential( 137 | nn.Conv2d(planes*2, planes*4, 1, stride=1, bias=False), 138 | nn.BatchNorm2d(planes*4), 139 | )), 140 | BasicBlock(planes*4, planes*4, stride=1, dilation=2), 141 | ) 142 | self.layer4 = nn.Sequential( 143 | BasicBlock(planes*4, planes*8, stride=1,dilation=2,downsample=nn.Sequential( 144 | nn.Conv2d(planes*4, planes*8, 1, stride=1, bias=False), 145 | nn.BatchNorm2d(planes*8), 146 | )), 147 | BasicBlock(planes*8, planes*8, stride=1,dilation=4), 148 | ) 149 | def forward(self,x): 150 | out = self.stem(x) 151 | out = self.maxpool(out) 152 | out1 = self.layer1(out) 153 | out2 = self.layer2(out1) 154 | out3 = self.layer3(out2) 155 | out4 = self.layer4(out3) 156 | return out1, out2, out3, out4 157 | 158 | class EncoderDecoder(nn.Module): 159 | def __init__(self,num_classes=2,auxiliary_loss=True, backbone_pretrained=True): 160 | super(EncoderDecoder,self).__init__() 161 | self.backbone = resnet18() 162 | self.decode_head = ASPPHead(num_classes=num_classes) 163 | self.auxiliary_loss = auxiliary_loss 164 | if auxiliary_loss: 165 | self.auxiliary_head = FCNHead(num_classes=num_classes,inplanes=256) 166 | else: 167 | self.auxiliary_head = None 168 | if backbone_pretrained: 169 | backbone_state = torch.load("resnetV1C.pth")['state_dict'] 170 | for key in self.backbone.state_dict().keys(): 171 | assert key in backbone_state.keys(), "backbone state-dict mismatch" 172 | self.backbone.load_state_dict(backbone_state,strict=False) 173 | print("pretrained model loaded!") 174 | def forward(self,x): 175 | input_shape = x.shape[-2:] 176 | feat = self.backbone(x) 177 | decode_seg = self.decode_head(feat[-1]) 178 | decode_seg = F.interpolate(decode_seg, size=input_shape, mode='bilinear', align_corners=False) 179 | if (not self.auxiliary_loss) or (not self.training): 180 | return decode_seg 181 | else: 182 | aux_seg = self.auxiliary_head(feat[-2]) 183 | aux_seg = F.interpolate(aux_seg, size=input_shape, mode='bilinear', align_corners=False) 184 | return decode_seg, aux_seg 185 | 186 | if __name__ == "__main__": 187 | model = resnet18() 188 | x = torch.rand(1,3,32,32) 189 | outs = model(x) 190 | print(outs[0].size(),outs[1].size(),outs[2].size(),outs[3].size()) 191 | -------------------------------------------------------------------------------- /dataset.py: -------------------------------------------------------------------------------- 1 | import os 2 | import json 3 | import torch 4 | from torch.utils.data.dataset import Dataset 5 | from torchvision.transforms import transforms as Tf 6 | import numpy as np 7 | import pykitti 8 | import open3d as o3d 9 | from utils import transform, se3 10 | from PIL import Image 11 | 12 | def check_length(root:str,save_name='data_len.json'): 13 | seq_dir = os.path.join(root,'sequences') 14 | seq_list = os.listdir(seq_dir) 15 | seq_list.sort() 16 | dict_len = dict() 17 | for seq in seq_list: 18 | len_velo = len(os.listdir(os.path.join(seq_dir,seq,'velodyne'))) 19 | dict_len[seq]=len_velo 20 | with open(os.path.join(root,save_name),'w')as f: 21 | json.dump(dict_len,f) 22 | 23 | class KITTIFilter: 24 | def __init__(self,voxel_size=0.3,concat:str = 'none'): 25 | """KITTIFilter 26 | 27 | Args: 28 | voxel_size (float, optional): voxel size for downsampling. Defaults to 0.3. 29 | concat (str, optional): concat operation for normal estimation, 'none','xyz' or 'zero-mean'. Defaults to 'none'. 30 | """ 31 | self.voxel_size = voxel_size 32 | self.concat = concat 33 | 34 | def __call__(self, x:np.ndarray): 35 | pcd = o3d.geometry.PointCloud() 36 | pcd.points = o3d.utility.Vector3dVector(x) 37 | # _, ind = pcd.remove_radius_outlier(nb_points=self.n_neighbor, radius=self.voxel_size) 38 | # pcd.select_by_index(ind) 39 | pcd = pcd.voxel_down_sample(self.voxel_size) 40 | pcd_xyz = np.array(pcd.points,dtype=np.float32) 41 | if self.concat == 'none': 42 | return pcd_xyz 43 | else: 44 | pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=self.voxel_size*3, max_nn=30)) 45 | pcd.normalize_normals() 46 | pcd_norm = np.array(pcd.normals,dtype=np.float32) 47 | if self.concat == 'xyz': 48 | return np.hstack([pcd_xyz,pcd_norm]) # (N,3), (N,3) -> (N,6) 49 | elif self.concat == 'zero-mean': # 'zero-mean' 50 | center = np.mean(pcd_xyz,axis=0,keepdims=True) # (3,) 51 | pcd_zero = pcd_xyz - center 52 | pcd_norm *= np.where(np.sum(pcd_zero*pcd_norm,axis=1,keepdims=True)<0,-1,1) 53 | return np.hstack([pcd_zero,pcd_norm]) # (N,3),(N,3) -> (N,6) 54 | else: 55 | raise RuntimeError('Unknown concat mode: %s'%self.concat) 56 | 57 | class Resampler: 58 | """ [N, D] -> [M, D]\n 59 | used for training 60 | """ 61 | def __init__(self, num): 62 | self.num = num 63 | 64 | def __call__(self, x: np.ndarray): 65 | num_points = x.shape[0] 66 | idx = np.random.permutation(num_points) 67 | if self.num < 0: 68 | return x[idx] 69 | elif self.num <= num_points: 70 | idx = idx[:self.num] # (self.num,dim) 71 | return x[idx] 72 | else: 73 | idx = np.hstack([idx,np.random.choice(num_points,self.num-num_points,replace=True)]) # (self.num,dim) 74 | return x[idx] 75 | 76 | class MaxResampler: 77 | """ [N, D] -> [M, D] (M<=max_num)\n 78 | used for testing 79 | """ 80 | def __init__(self,num,seed=8080): 81 | self.num = num 82 | np.random.seed(seed) # fix randomly sampling in test pipline 83 | def __call__(self, x:np.ndarray): 84 | num_points = x.shape[0] 85 | x_ = np.random.permutation(x) 86 | if num_points <= self.num: 87 | return x_ # permutation 88 | else: 89 | return x_[:self.num] 90 | 91 | class ToTensor: 92 | def __init__(self,type=torch.float): 93 | self.tensor_type = type 94 | 95 | def __call__(self, x: np.ndarray): 96 | return torch.from_numpy(x).type(self.tensor_type) 97 | 98 | 99 | 100 | class BaseKITTIDataset(Dataset): 101 | def __init__(self,basedir:str,batch_size:int,seqs=['09','10'],cam_id:int=2, 102 | meta_json='data_len.json',skip_frame=1, 103 | voxel_size=0.3,pcd_sample_num=4096,resize_ratio=(0.5,0.5),extend_ratio=(2.5,2.5), 104 | ): 105 | if not os.path.exists(os.path.join(basedir,meta_json)): 106 | check_length(basedir,meta_json) 107 | with open(os.path.join(basedir,meta_json),'r')as f: 108 | dict_len = json.load(f) 109 | frame_list = [] 110 | for seq in seqs: 111 | frame = list(range(0,dict_len[seq],skip_frame)) 112 | cut_index = len(frame)%batch_size 113 | if cut_index > 0: 114 | frame = frame[:-cut_index] 115 | frame_list.append(frame) 116 | self.kitti_datalist = [pykitti.odometry(basedir,seq,frames=frame) for seq,frame in zip(seqs,frame_list)] 117 | # concat images from different seq into one batch will cause error 118 | self.cam_id = cam_id 119 | self.resize_ratio = resize_ratio 120 | for seq,obj in zip(seqs,self.kitti_datalist): 121 | self.check(obj,cam_id,seq) 122 | self.sep = [len(data) for data in self.kitti_datalist] 123 | self.sumsep = np.cumsum(self.sep) 124 | self.resample_tran = Resampler(pcd_sample_num) 125 | self.tensor_tran = ToTensor() 126 | self.img_tran = Tf.ToTensor() 127 | self.pcd_tran = KITTIFilter(voxel_size,'none') 128 | self.extend_ratio = extend_ratio 129 | 130 | def __len__(self): 131 | return self.sumsep[-1] 132 | @staticmethod 133 | def check(odom_obj:pykitti.odometry,cam_id:int,seq:str)->bool: 134 | calib = odom_obj.calib 135 | cam_files_length = len(getattr(odom_obj,'cam%d_files'%cam_id)) 136 | velo_files_lenght = len(odom_obj.velo_files) 137 | head_msg = '[Seq %s]:'%seq 138 | assert cam_files_length>0, head_msg+'None of camera %d files'%cam_id 139 | assert cam_files_length==velo_files_lenght, head_msg+"number of cam %d (%d) and velo files (%d) doesn't equal!"%(cam_id,cam_files_length,velo_files_lenght) 140 | assert hasattr(calib,'T_cam0_velo'), head_msg+"Crucial calib attribute 'T_cam0_velo' doesn't exist!" 141 | 142 | 143 | def __getitem__(self, index): 144 | group_id = np.digitize(index,self.sumsep,right=False) 145 | data = self.kitti_datalist[group_id] 146 | T_cam2velo = getattr(data.calib,'T_cam%d_velo'%self.cam_id) 147 | K_cam = np.diag([self.resize_ratio[1],self.resize_ratio[0],1]) @ getattr(data.calib,'K_cam%d'%self.cam_id) 148 | if group_id > 0: 149 | sub_index = index - self.sumsep[group_id-1] 150 | else: 151 | sub_index = index 152 | raw_img = getattr(data,'get_cam%d'%self.cam_id)(sub_index) # PIL Image 153 | H,W = raw_img.height, raw_img.width 154 | RH = round(H*self.resize_ratio[0]) 155 | RW = round(W*self.resize_ratio[1]) 156 | REVH,REVW = self.extend_ratio[0]*RH,self.extend_ratio[1]*RW 157 | K_cam_extend = K_cam.copy() 158 | K_cam_extend[0,-1] *= self.extend_ratio[0] 159 | K_cam_extend[1,-1] *= self.extend_ratio[1] 160 | raw_img = raw_img.resize([RW,RH],Image.BILINEAR) 161 | _img = self.img_tran(raw_img) # raw img input (3,H,W) 162 | pcd = data.get_velo(sub_index) 163 | pcd[:,3] = 1.0 # (N,4) 164 | calibed_pcd = T_cam2velo @ pcd.T # [4,4] @ [4,N] -> [4,N] 165 | _calibed_pcd = self.pcd_tran(calibed_pcd[:3,:].T).T # raw pcd input (3,N) 166 | *_,rev = transform.binary_projection((REVH,REVW),K_cam_extend,_calibed_pcd) 167 | _calibed_pcd = _calibed_pcd[:,rev] 168 | _calibed_pcd = self.resample_tran(_calibed_pcd.T).T # (3,n) 169 | _pcd_range = np.linalg.norm(_calibed_pcd,axis=0) # (n,) 170 | u,v,r,_ = transform.pcd_projection((RH,RW),K_cam,_calibed_pcd,_pcd_range) 171 | _depth_img = torch.zeros(RH,RW,dtype=torch.float32) 172 | _depth_img[v,u] = torch.from_numpy(r).type(torch.float32) 173 | _calibed_pcd = self.tensor_tran(_calibed_pcd) 174 | _pcd_range = self.tensor_tran(_pcd_range) 175 | K_cam = self.tensor_tran(K_cam) 176 | T_cam2velo = self.tensor_tran(T_cam2velo) 177 | return dict(img=_img,pcd=_calibed_pcd,pcd_range=_pcd_range,depth_img=_depth_img, 178 | InTran=K_cam,ExTran=T_cam2velo) 179 | 180 | class KITTI_perturb(Dataset): 181 | def __init__(self,dataset:BaseKITTIDataset,max_deg:float,max_tran:float,mag_randomly=True,pooling_size=5,file=None): 182 | assert (pooling_size-1) % 2 == 0, 'pooling size must be odd to keep image size constant' 183 | self.pooling = torch.nn.MaxPool2d(kernel_size=pooling_size,stride=1,padding=(pooling_size-1)//2) 184 | self.dataset = dataset 185 | self.file = file 186 | if self.file is not None: 187 | self.perturb = torch.from_numpy(np.loadtxt(self.file,dtype=np.float32,delimiter=','))[None,...] # (1,N,6) 188 | else: 189 | self.transform = transform.UniformTransformSE3(max_deg,max_tran,mag_randomly) 190 | def __len__(self): 191 | return len(self.dataset) 192 | def __getitem__(self, index): 193 | data = self.dataset[index] 194 | H,W = data['img'].shape[-2:] # (RH,RW) 195 | calibed_pcd = data['pcd'] # (3,N) 196 | InTran = data['InTran'] # (3,3) 197 | if self.file is None: # randomly generate igt 198 | _uncalibed_pcd = self.transform(calibed_pcd[None,:,:]).squeeze(0) # (3,N) 199 | igt = self.transform.igt.squeeze(0) # (4,4) 200 | else: 201 | igt = se3.exp(self.perturb[:,index,:]) # (1,6) -> (1,4,4) 202 | _uncalibed_pcd = se3.transform(igt,calibed_pcd[None,...]).squeeze(0) # (3,N) 203 | igt.squeeze_(0) # (4,4) 204 | _uncalibed_depth_img = torch.zeros_like(data['depth_img'],dtype=torch.float32) 205 | proj_pcd = InTran.matmul(_uncalibed_pcd) # (3,3)x(3,N) -> (3,N) 206 | proj_x = (proj_pcd[0,:]/proj_pcd[2,:]).type(torch.long) 207 | proj_y = (proj_pcd[1,:]/proj_pcd[2,:]).type(torch.long) 208 | rev = (0<=proj_x)*(proj_x0) 209 | proj_x = proj_x[rev] 210 | proj_y = proj_y[rev] 211 | _uncalibed_depth_img[proj_y,proj_x] = data['pcd_range'][rev] # H,W 212 | # add new item 213 | new_data = dict(uncalibed_pcd=_uncalibed_pcd,uncalibed_depth_img=_uncalibed_depth_img,igt=igt) 214 | data.update(new_data) 215 | data['depth_img'] = self.pooling(data['depth_img'][None,...]) 216 | data['uncalibed_depth_img'] = self.pooling(data['uncalibed_depth_img'][None,...]) 217 | return data 218 | 219 | 220 | if __name__ == "__main__": 221 | import matplotlib 222 | matplotlib.use('Agg') 223 | from matplotlib import pyplot as plt 224 | base_dataset = BaseKITTIDataset('data',1,seqs=['00','01'],skip_frame=3) 225 | dataset = KITTI_perturb(base_dataset,30,3) 226 | data = dataset[2] 227 | for key,value in data.items(): 228 | if isinstance(value,torch.Tensor): 229 | shape = value.size() 230 | else: 231 | shape = value 232 | print('{key}: {shape}'.format(key=key,shape=shape)) 233 | plt.figure() 234 | plt.subplot(1,2,1) 235 | plt.imshow(data['depth_img'].squeeze(0).numpy()) 236 | plt.subplot(1,2,2) 237 | plt.imshow(data['uncalibed_depth_img'].squeeze(0).numpy()) 238 | plt.savefig('dataset_demo.png') 239 | 240 | 241 | 242 | 243 | -------------------------------------------------------------------------------- /train.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | from asyncio.log import logger 3 | import os 4 | import yaml 5 | import torch 6 | import torch.optim 7 | import torch.nn as nn 8 | from torch.utils.data.dataloader import DataLoader 9 | from dataset import BaseKITTIDataset,KITTI_perturb 10 | from mylogger import get_logger, print_highlight, print_warning 11 | from CalibNet import CalibNet 12 | import loss as loss_utils 13 | import utils 14 | from tqdm import tqdm 15 | import numpy as np 16 | from utils.transform import UniformTransformSE3 17 | 18 | def options(): 19 | parser = argparse.ArgumentParser() 20 | # dataset 21 | parser.add_argument("--config",type=str,default='config.yml') 22 | parser.add_argument("--dataset_path",type=str,default='data/') 23 | parser.add_argument("--skip_frame",type=int,default=5,help='skip frame of dataset') 24 | parser.add_argument("--pcd_sample",type=int,default=20000) 25 | parser.add_argument("--max_deg",type=float,default=10) # 10deg in each axis (see the paper) 26 | parser.add_argument("--max_tran",type=float,default=0.2) # 0.2m in each axis (see the paper) 27 | parser.add_argument("--mag_randomly",type=bool,default=True) 28 | # dataloader 29 | parser.add_argument("--batch_size",type=int,default=8) 30 | parser.add_argument("--num_workers",type=int,default=12) 31 | parser.add_argument("--pin_memory",type=bool,default=True,help='set it to False if your CPU memory is insufficient') 32 | # schedule 33 | parser.add_argument("--device",type=str,default='cuda:0') 34 | parser.add_argument("--resume",type=str,default='') 35 | parser.add_argument("--pretrained",type=str,default='') 36 | parser.add_argument("--epoch",type=int,default=30) 37 | parser.add_argument("--log_dir",default='log/') 38 | parser.add_argument("--checkpoint_dir",type=str,default="checkpoint/") 39 | parser.add_argument("--name",type=str,default='cam2_oneter_11to17') 40 | parser.add_argument("--optim",type=str,default='sgd',choices=['sgd','adam']) 41 | parser.add_argument("--lr0",type=float,default=5e-4) 42 | parser.add_argument("--momentum",type=float,default=0.9) 43 | parser.add_argument("--weight_decay",type=float,default=5e-6) 44 | parser.add_argument("--lr_exp_decay",type=float,default=0.98) 45 | parser.add_argument("--clip_grad",type=float,default=2.0) 46 | # setting 47 | parser.add_argument("--scale",type=float,default=50.0,help='scale factor of pcd normlization in loss') 48 | parser.add_argument("--inner_iter",type=int,default=1,help='inner iter of calibnet') 49 | parser.add_argument("--alpha",type=float,default=1.0,help='weight of photo loss') 50 | parser.add_argument("--beta",type=float,default=0.3,help='weight of chamfer loss') 51 | parser.add_argument("--resize_ratio",type=float,nargs=2,default=[1.0,1.0]) 52 | # if CUDA is out of memory, please reduce batch_size, pcd_sample or inner_iter 53 | return parser.parse_args() 54 | 55 | 56 | @torch.no_grad() 57 | def val(args,model:CalibNet,val_loader:DataLoader): 58 | model.eval() 59 | device = model.device 60 | tqdm_console = tqdm(total=len(val_loader),desc='Val') 61 | photo_loss = loss_utils.Photo_Loss(args.scale) 62 | chamfer_loss = loss_utils.ChamferDistanceLoss(args.scale,'mean') 63 | alpha = float(args.alpha) 64 | beta = float(args.beta) 65 | total_dR = 0 66 | total_dT = 0 67 | total_loss = 0 68 | total_se3_loss = 0 69 | with tqdm_console: 70 | tqdm_console.set_description_str('Val') 71 | for batch in val_loader: 72 | rgb_img = batch['img'].to(device) 73 | B = rgb_img.size(0) 74 | pcd_range = batch['pcd_range'].to(device) 75 | calibed_depth_img = batch['depth_img'].to(device) 76 | calibed_pcd = batch['pcd'].to(device) 77 | uncalibed_pcd = batch['uncalibed_pcd'].to(device) 78 | uncalibed_depth_img = batch['uncalibed_depth_img'].to(device) 79 | InTran = batch['InTran'][0].to(device) 80 | igt = batch['igt'].to(device) 81 | img_shape = rgb_img.shape[-2:] 82 | depth_generator = utils.transform.DepthImgGenerator(img_shape,InTran,pcd_range,CONFIG['dataset']['pooling']) 83 | # model(rgb_img,uncalibed_depth_img) 84 | g0 = torch.eye(4).repeat(B,1,1).to(device) 85 | for _ in range(args.inner_iter): 86 | twist_rot, twist_tsl = model(rgb_img,uncalibed_depth_img) 87 | extran = utils.se3.exp(torch.cat([twist_rot,twist_tsl],dim=1)) 88 | uncalibed_depth_img, uncalibed_pcd = depth_generator(extran,uncalibed_pcd) 89 | g0 = extran.bmm(g0) 90 | err_g = g0.bmm(igt) 91 | dR,dT = loss_utils.geodesic_distance(err_g) 92 | total_dR += dR.item() 93 | total_dT += dT.item() 94 | se3_loss = torch.linalg.norm(utils.se3.log(err_g),dim=1).mean()/6 95 | total_se3_loss += se3_loss.item() 96 | loss1 = photo_loss(calibed_depth_img,uncalibed_depth_img) 97 | loss2 = chamfer_loss(calibed_pcd,uncalibed_pcd) 98 | loss = alpha*loss1 + beta*loss2 99 | total_loss += loss.item() 100 | tqdm_console.set_postfix_str('dR:{:.4f}, dT:{:.4f},se3_loss:{:.4f}'.format(loss1,loss2,se3_loss)) 101 | tqdm_console.update(1) 102 | total_dR /= len(val_loader) 103 | total_dT /= len(val_loader) 104 | total_loss /= len(val_loader) 105 | total_se3_loss /= len(val_loader) 106 | return total_loss, total_dR, total_dT, total_se3_loss 107 | 108 | 109 | def train(args,chkpt,train_loader:DataLoader,val_loader:DataLoader): 110 | device = torch.device(args.device) 111 | model = CalibNet(backbone_pretrained=False,depth_scale=args.scale) 112 | model.to(device) 113 | if args.optim == 'sgd': 114 | optimizer = torch.optim.SGD(model.parameters(),args.lr0,momentum=args.momentum,weight_decay=args.weight_decay) 115 | else: 116 | optimizer = torch.optim.Adam(model.parameters(),args.lr0,weight_decay=args.weight_decay) 117 | scheduler = torch.optim.lr_scheduler.ExponentialLR(optimizer,gamma=args.lr_exp_decay) 118 | if args.pretrained: 119 | if os.path.exists(args.pretrained) and os.path.isfile(args.pretrained): 120 | model.load_state_dict(torch.load(args.pretrained)['model']) 121 | print_highlight('Pretrained model loaded from {:s}'.format(args.pretrained)) 122 | else: 123 | print_warning('Invalid pretrained path: {:s}'.format(args.pretrained)) 124 | if chkpt is not None: 125 | model.load_state_dict(chkpt['model']) 126 | optimizer.load_state_dict(chkpt['optimizer']) 127 | scheduler.load_state_dict(chkpt['scheduler']) 128 | start_epoch = chkpt['epoch'] + 1 129 | min_loss = chkpt['min_loss'] 130 | log_mode = 'a' 131 | else: 132 | start_epoch = 0 133 | min_loss = float('inf') 134 | log_mode = 'w' 135 | if not torch.cuda.is_available(): 136 | args.device = 'cpu' 137 | print_warning('CUDA is not available, use CPU to run') 138 | log_mode = 'a' if chkpt is not None else 'w' 139 | logger = get_logger("{name}|Train".format(name=args.name),os.path.join(args.log_dir,args.name+'.log'),mode=log_mode) 140 | if chkpt is None: 141 | logger.debug(args) 142 | print_highlight('Start Training') 143 | else: 144 | print_highlight('Resume from epoch {:d}'.format(start_epoch+1)) 145 | del chkpt # free memory 146 | photo_loss = loss_utils.Photo_Loss(args.scale) 147 | chamfer_loss = loss_utils.ChamferDistanceLoss(args.scale,'mean') 148 | alpha = float(args.alpha) 149 | beta = float(args.beta) 150 | for epoch in range(start_epoch,args.epoch): 151 | model.train() 152 | tqdm_console = tqdm(total=len(train_loader),desc='Train') 153 | total_photo_loss = 0 154 | total_chamfer_loss = 0 155 | with tqdm_console: 156 | tqdm_console.set_description_str('Epoch: {:03d}|{:03d}'.format(epoch+1,args.epoch)) 157 | for batch in train_loader: 158 | optimizer.zero_grad() 159 | rgb_img = batch['img'].to(device) 160 | B = rgb_img.size(0) 161 | pcd_range = batch['pcd_range'].to(device) 162 | calibed_depth_img = batch['depth_img'].to(device) 163 | calibed_pcd = batch['pcd'].to(device) 164 | uncalibed_pcd = batch['uncalibed_pcd'].to(device) 165 | uncalibed_depth_img = batch['uncalibed_depth_img'].to(device) 166 | InTran = batch['InTran'][0].to(device) 167 | igt = batch['igt'].to(device) 168 | img_shape = rgb_img.shape[-2:] 169 | depth_generator = utils.transform.DepthImgGenerator(img_shape,InTran,pcd_range,CONFIG['dataset']['pooling']) 170 | # model(rgb_img,uncalibed_depth_img) 171 | Tcl = torch.eye(4).repeat(B,1,1).to(device) 172 | # model.eval() 173 | for _ in range(args.inner_iter): 174 | twist_rot, twist_tsl = model(rgb_img,uncalibed_depth_img) 175 | iter_Tcl = utils.se3.exp(torch.cat([twist_rot,twist_tsl],dim=1)) 176 | uncalibed_depth_img, uncalibed_pcd = depth_generator(iter_Tcl, uncalibed_pcd) 177 | Tcl = Tcl.bmm(iter_Tcl) # right product (chronologically left product) 178 | dR,dT = loss_utils.geodesic_distance(Tcl.bmm(igt)) 179 | # model.train() 180 | loss1 = photo_loss(calibed_depth_img,uncalibed_depth_img) 181 | loss2 = chamfer_loss(calibed_pcd,uncalibed_pcd) 182 | loss = alpha*loss1 + beta*loss2 183 | loss.backward() 184 | nn.utils.clip_grad_value_(model.parameters(),args.clip_grad) 185 | optimizer.step() 186 | tqdm_console.set_postfix_str("p:{:.3f}, c:{:.3f}, dR:{:.3f}, dT:{:.3f}".format(loss1.item(),loss2.item(),dR.item(),dT.item())) 187 | tqdm_console.update() 188 | total_photo_loss += loss1.item() 189 | total_chamfer_loss += loss2.item() 190 | N_loader = len(train_loader) 191 | total_photo_loss /= N_loader 192 | total_chamfer_loss /= N_loader 193 | total_loss = alpha*total_photo_loss + beta*total_chamfer_loss 194 | tqdm_console.set_postfix_str("loss: {:.3f}, photo: {:.3f}, chamfer: {:.3f}".format(total_loss,total_photo_loss,total_chamfer_loss)) 195 | tqdm_console.update() 196 | tqdm_console.close() 197 | logger.info('Epoch {:03d}|{:03d}, train loss:{:.4f}'.format(epoch+1,args.epoch,total_loss)) 198 | scheduler.step() 199 | val_loss, loss_dR, loss_dT, loss_se3 = val(args,model,val_loader) # float 200 | if loss_se3 < min_loss: 201 | min_loss = loss_se3 202 | torch.save(dict( 203 | model=model.state_dict(), 204 | optimizer=optimizer.state_dict(), 205 | scheduler=scheduler.state_dict(), 206 | min_loss=min_loss, 207 | epoch=epoch, 208 | args=args.__dict__, 209 | config=CONFIG 210 | ),os.path.join(args.checkpoint_dir,'{name}_best.pth'.format(name=args.name))) 211 | logger.debug('Best model saved (Epoch {:d})'.format(epoch+1)) 212 | print_highlight('Best Model (Epoch %d)'%(epoch+1)) 213 | torch.save(dict( 214 | model=model.state_dict(), 215 | optimizer=optimizer.state_dict(), 216 | scheduler=scheduler.state_dict(), 217 | min_loss=min_loss, 218 | epoch=epoch, 219 | args=args.__dict__, 220 | config=CONFIG 221 | ),os.path.join(args.checkpoint_dir,'{name}_last.pth'.format(name=args.name))) 222 | logger.info('Evaluate loss_dR:{:.6f}, loss_dT:{:.6f}, se3_loss:{:.6f}'.format(loss_dR,loss_dT,loss_se3)) 223 | 224 | 225 | 226 | 227 | if __name__ == "__main__": 228 | args = options() 229 | os.makedirs(args.log_dir,exist_ok=True) 230 | os.makedirs(args.checkpoint_dir,exist_ok=True) 231 | with open(args.config,'r')as f: 232 | CONFIG = yaml.load(f,yaml.SafeLoader) 233 | assert isinstance(CONFIG,dict), 'Unknown config format!' 234 | if args.resume: 235 | chkpt = torch.load(args.resume,map_location='cpu') 236 | CONFIG.update(chkpt['config']) 237 | args.__dict__.update(chkpt['args']) 238 | print_highlight('config updated from resumed checkpoint {:s}'.format(args.resume)) 239 | else: 240 | chkpt = None 241 | print_highlight('args have been received, please wait for dataloader...') 242 | train_split = [str(index).rjust(2,'0') for index in CONFIG['dataset']['train']] 243 | val_split = [str(index).rjust(2,'0') for index in CONFIG['dataset']['val']] 244 | # dataset 245 | train_dataset = BaseKITTIDataset(args.dataset_path,args.batch_size,train_split,CONFIG['dataset']['cam_id'], 246 | skip_frame=args.skip_frame,voxel_size=CONFIG['dataset']['voxel_size'], 247 | pcd_sample_num=args.pcd_sample,resize_ratio=args.resize_ratio, 248 | extend_ratio=CONFIG['dataset']['extend_ratio']) 249 | train_dataset = KITTI_perturb(train_dataset,args.max_deg,args.max_tran,args.mag_randomly, 250 | pooling_size=CONFIG['dataset']['pooling']) 251 | 252 | val_dataset = BaseKITTIDataset(args.dataset_path,args.batch_size,val_split,CONFIG['dataset']['cam_id'], 253 | skip_frame=args.skip_frame,voxel_size=CONFIG['dataset']['voxel_size'], 254 | pcd_sample_num=args.pcd_sample,resize_ratio=args.resize_ratio, 255 | extend_ratio=CONFIG['dataset']['extend_ratio']) 256 | val_perturb_file = os.path.join(args.checkpoint_dir,"val_seq.csv") 257 | val_length = len(val_dataset) 258 | if not os.path.exists(val_perturb_file): 259 | print_highlight("validation pertub file dosen't exist, create one.") 260 | transform = UniformTransformSE3(args.max_deg,args.max_tran,args.mag_randomly) 261 | perturb_arr = np.zeros([val_length,6]) 262 | for i in range(val_length): 263 | perturb_arr[i,:] = transform.generate_transform().cpu().numpy() 264 | np.savetxt(val_perturb_file,perturb_arr,delimiter=',') 265 | else: # check length 266 | val_seq = np.loadtxt(val_perturb_file,delimiter=',') 267 | if val_length != val_seq.shape[0]: 268 | print_warning('Incompatiable validation length {}!={}'.format(val_length,val_seq.shape[0])) 269 | transform = utils.transform.UniformTransformSE3(args.max_deg,args.max_tran,args.mag_randomly) 270 | perturb_arr = np.zeros([val_length,6]) 271 | for i in range(val_length): 272 | perturb_arr[i,:] = transform.generate_transform().cpu().numpy() 273 | np.savetxt(val_perturb_file,perturb_arr,delimiter=',') 274 | print_highlight('Validation perturb file rewritten.') 275 | val_dataset = KITTI_perturb(val_dataset,args.max_deg,args.max_tran,args.mag_randomly, 276 | pooling_size=CONFIG['dataset']['pooling'], 277 | file=os.path.join(args.checkpoint_dir,"val_seq.csv")) 278 | # batch normlization does not support batch=1 279 | train_drop_last = True if len(train_dataset) % args.batch_size == 1 else False 280 | val_drop_last = True if len(val_dataset) % args.batch_size == 1 else False 281 | # dataloader 282 | train_dataloader = DataLoader(train_dataset,args.batch_size,shuffle=False,num_workers=args.num_workers,pin_memory=args.pin_memory,drop_last=train_drop_last) 283 | val_dataloder = DataLoader(val_dataset,args.batch_size,shuffle=False,num_workers=args.num_workers+8,pin_memory=args.pin_memory,drop_last=val_drop_last) 284 | 285 | 286 | train(args,chkpt,train_dataloader,val_dataloder) -------------------------------------------------------------------------------- /log/cam2_oneiter.log: -------------------------------------------------------------------------------- 1 | [DEBUG]:cam2_oneiter|Train, 2022-09-27 17:22:09,425, Namespace(alpha=1.0, batch_size=8, beta=0.3, checkpoint_dir='checkpoint/', clip_grad=2.0, config='config.yml', dataset_path='data/', device='cuda:0', epoch=100, inner_iter=1, log_dir='log/', lr0=0.0005, lr_exp_decay=0.98, mag_randomly=True, max_deg=10, max_tran=0.2, momentum=0.9, name='cam2_oneiter', num_workers=12, optim='sgd', pcd_sample=20000, pin_memory=True, pretrained='', resize_ratio=[1.0, 1.0], resume='', scale=50.0, skip_frame=10, weight_decay=5e-06) 2 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:23:34,514, Epoch 001|100, train loss:0.0566 3 | [DEBUG]:cam2_oneiter|Train, 2022-09-27 17:23:49,305, Best model saved (Epoch 1) 4 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:23:49,439, Evaluate loss_dR:0.078173, loss_dT:0.078845, se3_loss:0.033599 5 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:25:15,857, Epoch 002|100, train loss:0.0555 6 | [DEBUG]:cam2_oneiter|Train, 2022-09-27 17:25:30,244, Best model saved (Epoch 2) 7 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:25:30,389, Evaluate loss_dR:0.077013, loss_dT:0.077634, se3_loss:0.033106 8 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:26:56,126, Epoch 003|100, train loss:0.0569 9 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:27:11,320, Evaluate loss_dR:0.078603, loss_dT:0.077382, se3_loss:0.033322 10 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:28:37,444, Epoch 004|100, train loss:0.0559 11 | [DEBUG]:cam2_oneiter|Train, 2022-09-27 17:28:52,091, Best model saved (Epoch 4) 12 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:28:52,243, Evaluate loss_dR:0.077558, loss_dT:0.074882, se3_loss:0.032707 13 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:30:18,202, Epoch 005|100, train loss:0.0555 14 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:30:32,616, Evaluate loss_dR:0.081246, loss_dT:0.078256, se3_loss:0.034000 15 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:31:57,020, Epoch 006|100, train loss:0.0562 16 | [DEBUG]:cam2_oneiter|Train, 2022-09-27 17:32:11,496, Best model saved (Epoch 6) 17 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:32:11,652, Evaluate loss_dR:0.073022, loss_dT:0.076273, se3_loss:0.031892 18 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:33:35,961, Epoch 007|100, train loss:0.0565 19 | [DEBUG]:cam2_oneiter|Train, 2022-09-27 17:33:50,342, Best model saved (Epoch 7) 20 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:33:50,490, Evaluate loss_dR:0.067363, loss_dT:0.078862, se3_loss:0.031385 21 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:35:14,967, Epoch 008|100, train loss:0.0547 22 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:35:29,302, Evaluate loss_dR:0.073909, loss_dT:0.083145, se3_loss:0.033756 23 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:36:53,926, Epoch 009|100, train loss:0.0559 24 | [DEBUG]:cam2_oneiter|Train, 2022-09-27 17:37:08,355, Best model saved (Epoch 9) 25 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:37:08,503, Evaluate loss_dR:0.063029, loss_dT:0.080507, se3_loss:0.030843 26 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:38:33,013, Epoch 010|100, train loss:0.0540 27 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:38:47,353, Evaluate loss_dR:0.066330, loss_dT:0.085555, se3_loss:0.032583 28 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:40:12,120, Epoch 011|100, train loss:0.0537 29 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:40:26,563, Evaluate loss_dR:0.066507, loss_dT:0.079501, se3_loss:0.031294 30 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:41:51,269, Epoch 012|100, train loss:0.0548 31 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:42:05,812, Evaluate loss_dR:0.066285, loss_dT:0.081276, se3_loss:0.031685 32 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:43:30,373, Epoch 013|100, train loss:0.0557 33 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:43:44,784, Evaluate loss_dR:0.062591, loss_dT:0.080397, se3_loss:0.030880 34 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:45:09,454, Epoch 014|100, train loss:0.0543 35 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:45:23,979, Evaluate loss_dR:0.069700, loss_dT:0.080625, se3_loss:0.032121 36 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:46:48,455, Epoch 015|100, train loss:0.0544 37 | [DEBUG]:cam2_oneiter|Train, 2022-09-27 17:47:02,871, Best model saved (Epoch 15) 38 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:47:03,019, Evaluate loss_dR:0.063408, loss_dT:0.079695, se3_loss:0.030834 39 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:48:27,492, Epoch 016|100, train loss:0.0541 40 | [DEBUG]:cam2_oneiter|Train, 2022-09-27 17:48:41,904, Best model saved (Epoch 16) 41 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:48:42,052, Evaluate loss_dR:0.060994, loss_dT:0.078603, se3_loss:0.030105 42 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:50:06,367, Epoch 017|100, train loss:0.0542 43 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:50:20,775, Evaluate loss_dR:0.063588, loss_dT:0.079144, se3_loss:0.030757 44 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:51:45,193, Epoch 018|100, train loss:0.0545 45 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:51:59,750, Evaluate loss_dR:0.060473, loss_dT:0.080436, se3_loss:0.030372 46 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:53:24,048, Epoch 019|100, train loss:0.0533 47 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:53:38,206, Evaluate loss_dR:0.061012, loss_dT:0.085227, se3_loss:0.031555 48 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:55:02,721, Epoch 020|100, train loss:0.0549 49 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:55:16,989, Evaluate loss_dR:0.062779, loss_dT:0.085485, se3_loss:0.031961 50 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:56:41,442, Epoch 021|100, train loss:0.0538 51 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:56:55,602, Evaluate loss_dR:0.062365, loss_dT:0.086450, se3_loss:0.032123 52 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:58:20,194, Epoch 022|100, train loss:0.0530 53 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:58:34,554, Evaluate loss_dR:0.058229, loss_dT:0.086953, se3_loss:0.031562 54 | [INFO]:cam2_oneiter|Train, 2022-09-27 17:59:59,021, Epoch 023|100, train loss:0.0541 55 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:00:13,271, Evaluate loss_dR:0.057674, loss_dT:0.086846, se3_loss:0.031383 56 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:01:37,730, Epoch 024|100, train loss:0.0529 57 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:01:52,087, Evaluate loss_dR:0.061062, loss_dT:0.085620, se3_loss:0.031739 58 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:03:16,619, Epoch 025|100, train loss:0.0533 59 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:03:31,097, Evaluate loss_dR:0.059069, loss_dT:0.088488, se3_loss:0.032024 60 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:04:55,599, Epoch 026|100, train loss:0.0530 61 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:05:10,077, Evaluate loss_dR:0.058885, loss_dT:0.090280, se3_loss:0.032343 62 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:06:34,420, Epoch 027|100, train loss:0.0524 63 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:06:48,700, Evaluate loss_dR:0.058247, loss_dT:0.090558, se3_loss:0.032377 64 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:08:13,053, Epoch 028|100, train loss:0.0531 65 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:08:27,440, Evaluate loss_dR:0.056504, loss_dT:0.089807, se3_loss:0.031872 66 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:09:52,003, Epoch 029|100, train loss:0.0528 67 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:10:06,424, Evaluate loss_dR:0.059583, loss_dT:0.087096, se3_loss:0.031680 68 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:11:30,843, Epoch 030|100, train loss:0.0532 69 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:11:45,121, Evaluate loss_dR:0.060606, loss_dT:0.083302, se3_loss:0.031129 70 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:13:09,464, Epoch 031|100, train loss:0.0523 71 | [DEBUG]:cam2_oneiter|Train, 2022-09-27 18:13:23,596, Best model saved (Epoch 31) 72 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:13:23,736, Evaluate loss_dR:0.055570, loss_dT:0.081820, se3_loss:0.029795 73 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:14:48,059, Epoch 032|100, train loss:0.0527 74 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:15:02,686, Evaluate loss_dR:0.059982, loss_dT:0.084929, se3_loss:0.031307 75 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:16:27,237, Epoch 033|100, train loss:0.0531 76 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:16:41,632, Evaluate loss_dR:0.059186, loss_dT:0.087152, se3_loss:0.031647 77 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:18:06,212, Epoch 034|100, train loss:0.0527 78 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:18:20,520, Evaluate loss_dR:0.056695, loss_dT:0.087334, se3_loss:0.031331 79 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:19:45,061, Epoch 035|100, train loss:0.0521 80 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:19:59,633, Evaluate loss_dR:0.060572, loss_dT:0.089678, se3_loss:0.032501 81 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:21:24,158, Epoch 036|100, train loss:0.0532 82 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:21:38,315, Evaluate loss_dR:0.057767, loss_dT:0.087859, se3_loss:0.031514 83 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:23:02,790, Epoch 037|100, train loss:0.0526 84 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:23:17,393, Evaluate loss_dR:0.056047, loss_dT:0.088039, se3_loss:0.031331 85 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:24:41,610, Epoch 038|100, train loss:0.0524 86 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:24:55,971, Evaluate loss_dR:0.058027, loss_dT:0.085764, se3_loss:0.031145 87 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:26:20,329, Epoch 039|100, train loss:0.0525 88 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:26:34,888, Evaluate loss_dR:0.056898, loss_dT:0.088453, se3_loss:0.031595 89 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:27:59,338, Epoch 040|100, train loss:0.0522 90 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:28:13,812, Evaluate loss_dR:0.057182, loss_dT:0.088480, se3_loss:0.031670 91 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:29:38,276, Epoch 041|100, train loss:0.0523 92 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:29:52,548, Evaluate loss_dR:0.056395, loss_dT:0.087852, se3_loss:0.031357 93 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:31:17,098, Epoch 042|100, train loss:0.0523 94 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:31:31,550, Evaluate loss_dR:0.059641, loss_dT:0.089184, se3_loss:0.032251 95 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:32:55,982, Epoch 043|100, train loss:0.0524 96 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:33:10,492, Evaluate loss_dR:0.057937, loss_dT:0.090396, se3_loss:0.032166 97 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:34:35,114, Epoch 044|100, train loss:0.0522 98 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:34:49,446, Evaluate loss_dR:0.056425, loss_dT:0.090724, se3_loss:0.032052 99 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:36:13,891, Epoch 045|100, train loss:0.0523 100 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:36:28,448, Evaluate loss_dR:0.056071, loss_dT:0.090209, se3_loss:0.031882 101 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:37:52,992, Epoch 046|100, train loss:0.0524 102 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:38:07,557, Evaluate loss_dR:0.057267, loss_dT:0.090419, se3_loss:0.032097 103 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:39:32,262, Epoch 047|100, train loss:0.0521 104 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:39:46,750, Evaluate loss_dR:0.056151, loss_dT:0.089752, se3_loss:0.031747 105 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:41:11,042, Epoch 048|100, train loss:0.0516 106 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:41:25,540, Evaluate loss_dR:0.056428, loss_dT:0.092146, se3_loss:0.032384 107 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:42:49,870, Epoch 049|100, train loss:0.0514 108 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:43:04,529, Evaluate loss_dR:0.055730, loss_dT:0.092219, se3_loss:0.032354 109 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:44:29,014, Epoch 050|100, train loss:0.0511 110 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:44:43,372, Evaluate loss_dR:0.055920, loss_dT:0.091406, se3_loss:0.032179 111 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:46:07,999, Epoch 051|100, train loss:0.0524 112 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:46:22,381, Evaluate loss_dR:0.055975, loss_dT:0.091342, se3_loss:0.032169 113 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:47:47,027, Epoch 052|100, train loss:0.0524 114 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:48:01,354, Evaluate loss_dR:0.054347, loss_dT:0.091109, se3_loss:0.031833 115 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:49:25,785, Epoch 053|100, train loss:0.0515 116 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:49:39,911, Evaluate loss_dR:0.055221, loss_dT:0.092982, se3_loss:0.032396 117 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:51:04,513, Epoch 054|100, train loss:0.0513 118 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:51:18,928, Evaluate loss_dR:0.056376, loss_dT:0.091670, se3_loss:0.032301 119 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:52:43,264, Epoch 055|100, train loss:0.0518 120 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:52:57,881, Evaluate loss_dR:0.056928, loss_dT:0.092345, se3_loss:0.032504 121 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:54:22,243, Epoch 056|100, train loss:0.0519 122 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:54:36,761, Evaluate loss_dR:0.056788, loss_dT:0.093097, se3_loss:0.032666 123 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:56:01,119, Epoch 057|100, train loss:0.0511 124 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:56:15,687, Evaluate loss_dR:0.057773, loss_dT:0.092195, se3_loss:0.032664 125 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:57:39,973, Epoch 058|100, train loss:0.0512 126 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:57:54,129, Evaluate loss_dR:0.056886, loss_dT:0.094077, se3_loss:0.032937 127 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:59:18,573, Epoch 059|100, train loss:0.0520 128 | [INFO]:cam2_oneiter|Train, 2022-09-27 18:59:32,697, Evaluate loss_dR:0.055458, loss_dT:0.092946, se3_loss:0.032445 129 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:00:57,113, Epoch 060|100, train loss:0.0506 130 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:01:11,433, Evaluate loss_dR:0.056768, loss_dT:0.092446, se3_loss:0.032516 131 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:02:35,853, Epoch 061|100, train loss:0.0516 132 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:02:50,292, Evaluate loss_dR:0.057592, loss_dT:0.092369, se3_loss:0.032585 133 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:04:14,870, Epoch 062|100, train loss:0.0514 134 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:04:29,459, Evaluate loss_dR:0.057662, loss_dT:0.090539, se3_loss:0.032156 135 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:05:53,958, Epoch 063|100, train loss:0.0509 136 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:06:08,485, Evaluate loss_dR:0.056982, loss_dT:0.091708, se3_loss:0.032342 137 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:07:33,081, Epoch 064|100, train loss:0.0517 138 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:07:47,383, Evaluate loss_dR:0.056495, loss_dT:0.089803, se3_loss:0.031782 139 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:09:12,406, Epoch 065|100, train loss:0.0508 140 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:09:27,062, Evaluate loss_dR:0.056792, loss_dT:0.090740, se3_loss:0.032089 141 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:10:53,479, Epoch 066|100, train loss:0.0516 142 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:11:07,954, Evaluate loss_dR:0.055735, loss_dT:0.090935, se3_loss:0.031931 143 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:12:33,785, Epoch 067|100, train loss:0.0503 144 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:12:48,926, Evaluate loss_dR:0.056355, loss_dT:0.090881, se3_loss:0.032058 145 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:14:15,633, Epoch 068|100, train loss:0.0505 146 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:14:30,663, Evaluate loss_dR:0.057894, loss_dT:0.092603, se3_loss:0.032675 147 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:15:56,487, Epoch 069|100, train loss:0.0506 148 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:16:11,503, Evaluate loss_dR:0.059075, loss_dT:0.093971, se3_loss:0.033208 149 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:17:38,040, Epoch 070|100, train loss:0.0510 150 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:17:52,752, Evaluate loss_dR:0.056495, loss_dT:0.093533, se3_loss:0.032720 151 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:19:19,457, Epoch 071|100, train loss:0.0512 152 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:19:36,594, Evaluate loss_dR:0.056367, loss_dT:0.092168, se3_loss:0.032388 153 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:21:05,271, Epoch 072|100, train loss:0.0503 154 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:21:22,864, Evaluate loss_dR:0.056669, loss_dT:0.092378, se3_loss:0.032488 155 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:22:50,171, Epoch 073|100, train loss:0.0513 156 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:23:05,387, Evaluate loss_dR:0.059777, loss_dT:0.092557, se3_loss:0.032999 157 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:24:32,878, Epoch 074|100, train loss:0.0505 158 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:24:48,327, Evaluate loss_dR:0.057441, loss_dT:0.093381, se3_loss:0.032802 159 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:26:16,278, Epoch 075|100, train loss:0.0503 160 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:26:31,055, Evaluate loss_dR:0.055297, loss_dT:0.091423, se3_loss:0.031999 161 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:27:58,384, Epoch 076|100, train loss:0.0500 162 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:28:13,574, Evaluate loss_dR:0.056880, loss_dT:0.091937, se3_loss:0.032360 163 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:29:40,420, Epoch 077|100, train loss:0.0503 164 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:29:55,550, Evaluate loss_dR:0.057431, loss_dT:0.091883, se3_loss:0.032404 165 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:31:24,923, Epoch 078|100, train loss:0.0495 166 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:31:43,056, Evaluate loss_dR:0.056538, loss_dT:0.092605, se3_loss:0.032464 167 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:33:11,835, Epoch 079|100, train loss:0.0495 168 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:33:27,036, Evaluate loss_dR:0.058120, loss_dT:0.092941, se3_loss:0.032823 169 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:34:58,120, Epoch 080|100, train loss:0.0502 170 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:35:16,432, Evaluate loss_dR:0.057380, loss_dT:0.092931, se3_loss:0.032674 171 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:36:48,029, Epoch 081|100, train loss:0.0507 172 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:37:04,801, Evaluate loss_dR:0.058876, loss_dT:0.092723, se3_loss:0.032883 173 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:38:31,422, Epoch 082|100, train loss:0.0506 174 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:38:45,997, Evaluate loss_dR:0.056391, loss_dT:0.091619, se3_loss:0.032189 175 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:40:12,718, Epoch 083|100, train loss:0.0500 176 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:40:27,894, Evaluate loss_dR:0.059481, loss_dT:0.091629, se3_loss:0.032710 177 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:41:53,225, Epoch 084|100, train loss:0.0505 178 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:42:07,917, Evaluate loss_dR:0.056603, loss_dT:0.090170, se3_loss:0.031872 179 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:43:33,834, Epoch 085|100, train loss:0.0497 180 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:43:48,489, Evaluate loss_dR:0.056827, loss_dT:0.090653, se3_loss:0.032067 181 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:45:13,949, Epoch 086|100, train loss:0.0504 182 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:45:29,132, Evaluate loss_dR:0.057785, loss_dT:0.091017, se3_loss:0.032289 183 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:46:54,535, Epoch 087|100, train loss:0.0498 184 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:47:09,309, Evaluate loss_dR:0.057488, loss_dT:0.091505, se3_loss:0.032358 185 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:48:34,929, Epoch 088|100, train loss:0.0499 186 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:48:49,464, Evaluate loss_dR:0.058678, loss_dT:0.091127, se3_loss:0.032456 187 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:50:15,168, Epoch 089|100, train loss:0.0500 188 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:50:30,441, Evaluate loss_dR:0.060123, loss_dT:0.091169, se3_loss:0.032685 189 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:51:55,838, Epoch 090|100, train loss:0.0496 190 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:52:10,498, Evaluate loss_dR:0.057821, loss_dT:0.091974, se3_loss:0.032498 191 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:53:36,219, Epoch 091|100, train loss:0.0499 192 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:53:50,856, Evaluate loss_dR:0.058831, loss_dT:0.092461, se3_loss:0.032775 193 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:55:16,331, Epoch 092|100, train loss:0.0496 194 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:55:31,447, Evaluate loss_dR:0.058399, loss_dT:0.093185, se3_loss:0.032932 195 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:56:56,889, Epoch 093|100, train loss:0.0495 196 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:57:11,486, Evaluate loss_dR:0.060151, loss_dT:0.093297, se3_loss:0.033218 197 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:58:36,527, Epoch 094|100, train loss:0.0490 198 | [INFO]:cam2_oneiter|Train, 2022-09-27 19:58:51,235, Evaluate loss_dR:0.057754, loss_dT:0.092394, se3_loss:0.032631 199 | [INFO]:cam2_oneiter|Train, 2022-09-27 20:00:16,050, Epoch 095|100, train loss:0.0493 200 | [INFO]:cam2_oneiter|Train, 2022-09-27 20:00:31,039, Evaluate loss_dR:0.060517, loss_dT:0.092996, se3_loss:0.033248 201 | [INFO]:cam2_oneiter|Train, 2022-09-27 20:01:55,831, Epoch 096|100, train loss:0.0490 202 | [INFO]:cam2_oneiter|Train, 2022-09-27 20:02:10,742, Evaluate loss_dR:0.060062, loss_dT:0.092279, se3_loss:0.032973 203 | [INFO]:cam2_oneiter|Train, 2022-09-27 20:03:36,011, Epoch 097|100, train loss:0.0497 204 | [INFO]:cam2_oneiter|Train, 2022-09-27 20:03:50,734, Evaluate loss_dR:0.059919, loss_dT:0.093011, se3_loss:0.033139 205 | [INFO]:cam2_oneiter|Train, 2022-09-27 20:05:15,514, Epoch 098|100, train loss:0.0500 206 | [INFO]:cam2_oneiter|Train, 2022-09-27 20:05:30,105, Evaluate loss_dR:0.060280, loss_dT:0.092196, se3_loss:0.032979 207 | [INFO]:cam2_oneiter|Train, 2022-09-27 20:06:54,811, Epoch 099|100, train loss:0.0499 208 | [INFO]:cam2_oneiter|Train, 2022-09-27 20:07:09,737, Evaluate loss_dR:0.059549, loss_dT:0.092695, se3_loss:0.032989 209 | [INFO]:cam2_oneiter|Train, 2022-09-27 20:08:34,919, Epoch 100|100, train loss:0.0493 210 | [INFO]:cam2_oneiter|Train, 2022-09-27 20:08:49,398, Evaluate loss_dR:0.059277, loss_dT:0.092181, se3_loss:0.032790 211 | --------------------------------------------------------------------------------