def __getitem__(self, item):
# Get parameter
gp = GlobalProperties()
camMat = gp.getCamMat()
# Sampling for reprojection error image
sampling = cnn.stochasticSubSample(self.imgBGR,
targetsize=self.objInputSize,
patchsize=self.rgbInputSize)
estObj = cnn.getCoordImg(self.imgBGR, sampling, self.rgbInputSize,
self.model)
# Produce GroundTruth Label
poseGT = Hypothesis()
poseGT.Info(self.info)
driftLevel = np.random.randint(0, 3)
if not driftLevel:
poseNoise = poseGT * getRandHyp(2, 2)
else:
poseNoise = poseGT * getRandHyp(10, 100)
data = cnn.getDiffMap(
TYPE.our2cv([poseNoise.getRotation(),
poseNoise.getTranslation()]), estObj, sampling,
camMat)
data = self.transform(data)
label = -1 * self.temperature * max(
poseGT.calcAngularDistance(poseNoise),
np.linalg.norm(poseGT.getTranslation() -
poseNoise.getTranslation()) / 10.0)
return data, label
def readData_info(infoFile):
info = TYPE.info_t()
if not os.path.exists(infoFile):
info.visible = False
return False
i = 0
trans = np.eye(4)
file_info = open(infoFile)
lines_info = file_info.readlines()
for line in lines_info:
trans[i, :] = np.array(list(map(float, line.split())))
i += 1
transfile = './translation.txt'
file_trans = open(transfile)
lines_trans = file_trans.readlines()
trans[:3, 3] -= np.array(list(map(float, lines_trans[0].split())))
correction = np.eye(4)
correction[:, 1] = -correction[:, 1]
correction[:, 2] = -correction[:, 2]
trans = inv(np.dot(trans, correction))
info.rotation = trans[:3, :3]
info.center = trans[:3, 3]
info.extent = 10 * np.ones(3)
info.visible = True
info.occlusion = 0
return info
def dPNP(imgPts, objPts, eps=0.1):
if len(imgPts) == 4:
pnpMethod = cv2.SOLVEPNP_P3P
else:
pnpMethod = cv2.SOLVEPNP_ITERATIVE
gp = properties.GlobalProperties()
camMat = gp.getCamMat()
imgPts = np.array(imgPts, np.int64)
jacobean = np.zeros([6, len(objPts) * 3])
for i in range(len(objPts)):
for j in range(3):
# Forward step
if j == 0: objPts[i][0] += eps
elif j == 1: objPts[i][1] += eps
elif j == 2: objPts[i][2] += eps
objPts = np.array(objPts, np.float64)
_, rot_f, tvec_f = safeSolvePnP(objPts, imgPts, camMat, None,
pnpMethod)
Trans_f = TYPE.cv2our([rot_f, tvec_f])
h_f = Hypothesis.Hypothesis()
h_f.RotandTrans(Trans_f[0], Trans_f[1])
fstep = h_f.getRodVecAndTrans()
# Backward step
if j == 0: objPts[i][0] -= 2 * eps
elif j == 1: objPts[i][1] -= 2 * eps
elif j == 2: objPts[i][2] -= 2 * eps
objPts = np.array(objPts, np.float64)
_, rot_b, tvec_b = safeSolvePnP(objPts, imgPts, camMat, None,
pnpMethod)
Trans_b = TYPE.cv2our([rot_b, tvec_b])
h_b = Hypothesis.Hypothesis()
h_b.RotandTrans(Trans_b[0], Trans_b[1])
bstep = h_b.getRodVecAndTrans()
# Back to normal state
if j == 0: objPts[i][0] += eps
elif j == 1: objPts[i][1] += eps
elif j == 2: objPts[i][2] += eps
# Gradient calculation
for k in range(len(fstep)):
jacobean[k][3 * i + j] = (fstep[k] - bstep[k]) / (2 * eps)
if containsNaNs(jacobean[:, 3 * i + j]):
return np.zeros([6, 3 * objPts])
return jacobean
def refine(inlierCount, refSteps, inlierThreshold2D, pixelIdxs, estObj,
sampling, camMat, imgPts, objPts):
hyp = safeSolvePnP(objPts=objPts,
imgPts=imgPts,
camMat=camMat,
disCoeffs=None,
methodFlag=cv2.SOLVEPNP_P3P,
rot=np.zeros([3]),
trans=np.zeros([3]))
diffMap = getDiffMap(hyp, estObj, sampling, camMat)
for rStep in range(refSteps):
localImgPts = []
localObjPts = []
for idx in range(pixelIdxs[rStep].shape):
x = pixelIdxs[rStep][idx] % CNN_OBJ_PATCHSIZE
y = pixelIdxs[rStep][idx] // CNN_OBJ_PATCHSIZE
if diffMap[y, x] < inlierThreshold2D:
localImgPts.append(sampling[y, x, :])
localObjPts.append(estObj[y, x, :])
if len(localImgPts) >= inlierCount:
break
if len(localImgPts) < 50:
break
if len(localObjPts) > 4:
methodflag = cv2.SOLVEPNP_ITERATIVE
else:
methodflag = cv2.SOLVEPNP_P3P
localObjPts, localImgPts = np.array([localObjPts]).reshape(
[-1, 3, 1]), np.array([localImgPts]).reshape([-1, 2, 1])
hypUpdate = copy.deepcopy(hyp)
if not (safeSolvePnP(objPts=localObjPts,
imgPts=localImgPts,
camMat=camMat,
disCoeffs=None,
methodFlag=methodflag,
rot=hypUpdate[0],
trans=hypUpdate[1])[0]).all():
break
hyp = copy.deepcopy(hypUpdate)
# recalculate pose errors
diffMap = getDiffMap(hyp, estObj, sampling, camMat)
jpHyp = TYPE.cv2our(hyp)
hy = Hypothesis.Hypothesis()
hy.RotandTrans(jpHyp[0], jpHyp[1])
return hy.getRodVecAndTrans()
def expectedMaxLoss(gt, hyps, probs):
loss = 0
losses = np.zeros(len(hyps))
for i in range(len(hyps)):
jpHyp = TYPE.cv2our(hyps[i])
hyp = Hypothesis.Hypothesis()
hyp.RotandTrans(jpHyp[0], jpHyp[1])
losses[i] = ml.maxLoss(gt, hyp)
loss = loss + probs[i] * losses[i]
return loss, losses
def rand_getBGRD(self, i):
img = TYPE.imag_brgd_t()
img.bgr = self.rand_getBGR(i)
img.depth = self.rand_getDepth(i)
def processImage(imgBGR, poseGT, model_obj, model_score, objHyps, ptCount,
camMat, inlierThreshold2D, inlierCount, refSteps, refHyps,
sampledPoints, estObj, sampling, inlierMaps, pixelIdxs,
optimizer, round):
time_start = time.time()
camMat_tensor = torch.tensor(camMat).to(DEVICE).float()
sampling = cnn_Sam.stochasticSubSamplewithoutC(imgBGR, CNN_OBJ_PATCHSIZE,
CNN_RGB_PATCHSIZE)
sampling_tensor = torch.tensor(sampling).to(DEVICE)
estObj_tensor = getCoordImg_tensor(imgBGR, sampling, CNN_RGB_PATCHSIZE,
model_obj).float()
estobj_np = estObj_tensor.cpu().detach().numpy()
solvePNP = PNP.apply
bpnp = BPnP.BPnP.apply
time_point1 = time.time()
print('cost time in obj net:', time_point1 - time_start)
for h in range(0, objHyps):
if not h % 500:
print('h:', h)
p = 0
while True:
# projections = []
alreadyChosen = np.zeros((40, 40))
# imgPts.append([])
# objPts.append([])
# imgIdx.append([]) # the usage of .clear()
sampledPoints.append([])
# hyps.append([])
j = 0
# imgPts = []
# objPts = []
imgIdx = []
while j < ptCount:
np.random.seed()
x = np.random.randint(0, 40)
y = np.random.randint(0, 40)
j = j + 1
if alreadyChosen[y, x] > 0:
j = j - 1
continue
alreadyChosen[y, x] = 1
# imgPts.append(sampling[y, x, :])
if j == 1:
imgPts = sampling_tensor[y, x, :].unsqueeze(0)
objPts = estObj_tensor[y, x, :].unsqueeze(0)
else:
imgPts = torch.cat(
(imgPts, sampling_tensor[y, x, :].unsqueeze(0)), 0)
objPts = torch.cat(
(objPts, estObj_tensor[y, x, :].unsqueeze(0)), 0)
# objPts.append(estObj_tensor[y, x, :])
imgIdx.append(y * CNN_OBJ_PATCHSIZE + x)
sampledPoints[h].append(np.array([x, y]))
# imgPts_3D = np.array(imgPts).reshape([1, -1, 2])
imgPts = imgPts.unsqueeze(0).float()
# if p:
# rtvec = solvePNP(objPts,
# imgPts_3D,
# camMat,
# None,
# cv2.SOLVEPNP_P3P,
# hyps[:3],
# hyps[3:]).reshape([-1]).to(DEVICE)
# rtvec = np.array(cnn.safeSolvePnP(objPts_3D,
# imgPts_3D,
# camMat,
# None,
# cv2.SOLVEPNP_P3P,
# hyps[h][:3],
# hyps[h][3:])).reshape([-1])
# else:
rtvec = bpnp(imgPts, objPts, camMat_tensor)
# rtvec = solvePNP(objPts,
# imgPts_3D,
# camMat,
# None,
# cv2.SOLVEPNP_P3P,
# np.zeros([3]),
# np.zeros([3])).reshape([-1]).to(DEVICE)
# rtvec = np.array(cnn.safeSolvePnP(objPts_3D,
# imgPts_3D,
# camMat,
# None,
# cv2.SOLVEPNP_P3P,
# np.zeros([3]),
# np.zeros([3]))).reshape([-1])
if not np.any(np.isfinite(rtvec.cpu().detach().numpy())):
del imgPts
del objPts
del imgIdx
del sampledPoints[h]
continue
else:
hyps = rtvec
# if not p:
# hyps = rtvec
# p += 1
# else:
# hyps = rtvec
# hyps.append(solvePNP(objPts_3D_tensor,
# imgPts_3D,
# camMat,
# None,
# cv2.SOLVEPNP_P3P,
# np.zeros([3]),
# np.zeros([3])).cpu().detach().numpy().reshape([-1]))
# hyps[h].append(solvePNP(objPts_3D_tensor,
# imgPts_3D,
# camMat,
# None,
# cv2.SOLVEPNP_P3P,
# np.zeros([3]),
# np.zeros([3]))[3:].cpu().detach().numpy())
# input = (objPts,
# imgPts_3D,
# camMat,
# None,
# cv2.SOLVEPNP_P3P,
# np.zeros([3]),
# np.zeros([3]))
# test = gradcheck(solvePNP, input, eps=1e-4)
# print('test_grad:', test)
# hyps[h].append(cnn.safeSolvePnP(objPts=objPts_3D, imgPts=imgPts_3D, camMat=camMat, disCoeffs=None,
# methodFlag=cv2.SOLVEPNP_P3P, rot=np.zeros([3]), trans=np.zeros([3]))[0])
# hyps[h].append(cnn.safeSolvePnP(objPts=objPts_3D, imgPts=imgPts_3D, camMat=camMat, disCoeffs=None,
# methodFlag=cv2.SOLVEPNP_P3P, rot=np.zeros([3]), trans=np.zeros([3]))[1])
# if not (solvePNP(objPts_3D_tensor,
# imgPts_3D,
# camMat,
# None,
# cv2.SOLVEPNP_P3P,
# np.zeros([3]),
# np.zeros([3]))[:3].cpu().detach().numpy()).all():
# del imgPts[h]
# del objPts[h]
# del imgIdx[h]
# del sampledPoints[h]
# del hyps[h]
# continue
# to project a 3d point into the image(do not know whether there is a function in cv2 can fulfill the task)
# print('objpts:', np.array(objPts[h]).reshape([-1, 3]))
projections = cv2.projectPoints(
objectPoints=objPts.cpu().detach().numpy(),
rvec=hyps[0, :3].cpu().detach().numpy(),
tvec=hyps[0, 3:].cpu().detach().numpy(),
cameraMatrix=camMat,
distCoeffs=None)[0].reshape([-1, 2])
# for points in range(4):
# objMat = objPts[h][points]
# projection, _ = cv2.projectPoints(objectPoints=objMat, rvec=hyps[h][:3],
# tvec=hyps[h][3:],
# cameraMatrix=camMat, distCoeffs=None)
# projections.append(projection)
foundOutlier = False
if np.any(
norm((projections -
imgPts.cpu().detach().numpy().reshape([-1, 2])),
axis=1) > inlierThreshold2D):
foundOutlier = True
# for j in range(4):
# # print("diff is:",np.linalg.norm(imgPts[h][j] - projections[j]))
# if np.linalg.norm(imgPts[j] - projections[j]) < inlierThreshold2D:
# continue
# foundOutlier = True
# break
if foundOutlier:
del imgPts
del objPts
del imgIdx
del sampledPoints[h]
# del hyps
p += 1
continue
else:
break
if not h:
hyps_tensor = hyps.unsqueeze(0)
else:
hyps_tensor = torch.cat((hyps_tensor, hyps.unsqueeze(0)), 0)
time_point2 = time.time()
print('cost time in PNP:', time_point2 - time_point1)
meet_demand = 0
index = 0
for h in range(objHyps):
diffmap = getDiffMap_tensor(hyps_tensor[h, :], estObj_tensor,
sampling_tensor, camMat)
# Calculate inliers
mask = diffmap[0, :, :].lt(inlierThreshold2D)
num = torch.nonzero(mask, as_tuple=False).size(0)
if num > meet_demand:
meet_demand = num
index = h
# Transform for later use
diffmap_norm = TRANSFORM(diffmap).unsqueeze(0)
if not h:
FullDiffMap_norm = diffmap_norm
FullDiffMap = diffmap.unsqueeze(0)
else:
FullDiffMap_norm = torch.cat((FullDiffMap_norm, diffmap_norm), 0)
FullDiffMap = torch.cat((FullDiffMap, diffmap.unsqueeze(0)), 0)
time_point3 = time.time()
print('cost time in get diffmap:', time_point3 - time_point2)
with torch.no_grad():
score = model_score(FullDiffMap_norm.float())
# max_score = torch.max(score).to(DEVICE)
# score_norm = score - max_score
sfScore = F.softmax(score, dim=0)
sfScore_np = sfScore.cpu().detach().numpy().reshape([-1])
hypIdx = cnn_Sam.draw(sfScore_np, randomdraw=True)
hypIdx_ransac = cnn_Sam.draw(sfScore_np, randomdraw=False)
time_point4 = time.time()
print('cost time in score net:', time_point4 - time_point3)
# Refine
(height, width) = np.shape(sampling[:, :, 0])
inlierMaps = []
hyps_np = hyps_tensor.cpu().detach().numpy()
for h in range(objHyps):
refHyps.append(hyps_np[h, :, :].reshape([-1]))
localDiffMap = FullDiffMap[h, 0, :, :].cpu().detach().numpy()
inlierMaps.append(np.zeros([height, width]))
pixelIdxs.append([])
for rStep in range(refSteps):
pixelIdxs[h].append([])
for idx in range(height * width):
pixelIdxs[h][rStep].append(idx)
random.shuffle(pixelIdxs[h][rStep])
localImgPts = []
localObjPts = []
for idx in range(height * width):
x = pixelIdxs[h][rStep][idx] % width
y = pixelIdxs[h][rStep][idx] // width
if localDiffMap[y, x] < inlierThreshold2D:
localImgPts.append(sampling[y, x, :])
localObjPts.append(estobj_np[y, x, :])
# print('sampling:', sampling[y, x, :])
# print('est', estobj_np[y, x, :])
elif len(localObjPts) >= inlierCount:
break
if len(localObjPts) < 50:
break
if len(localObjPts) > 4:
methodflag = cv2.SOLVEPNP_ITERATIVE
else:
methodflag = cv2.SOLVEPNP_P3P
localObjPts, localImgPts = np.array([localObjPts]).reshape(
[-1, 3]), np.array([localImgPts]).reshape([-1, 2])
# localObjPts_tensor = torch.tensor(localObjPts, requires_grad=False)
# print('localobj:', localObjPts)
# print('localimg:', localImgPts)
hypUpdate = copy.deepcopy(refHyps[h])
# print('h:', h, 'refstep:', rStep, 'b4', hypUpdate)
# print('h:', h, 'refstep:', rStep, 'b4', hypUpdate[0], 'trans:', hypUpdate[1])
if not (cnn.safeSolvePnP(objPts=localObjPts,
imgPts=localImgPts,
camMat=camMat,
disCoeffs=None,
methodFlag=methodflag,
rot=hypUpdate[:3],
trans=hypUpdate[3:])[0]).all:
# rvec_tensor = rt_vec[:3]
# tvec_tensor = rt_vec[3:]
# rvec = rvec_tensor.cpu().detach().numpy()
# tvec = tvec_tensor.cpu().detach().numpy()
break
# print('h:', h, 'refstep:', rStep, 'after:', hypUpdate)
refHyps[h] = copy.deepcopy(hypUpdate)
if not h:
REF_HYP = refHyps[h].reshape([1, -1])
else:
REF_HYP = np.concatenate((REF_HYP, refHyps[h].reshape([1, -1])),
axis=0)
for pt in range(0, len(sampledPoints[h])):
x = sampledPoints[h][pt][0]
y = sampledPoints[h][pt][1]
inlierMaps[h][y, x] = 0
time_point5 = time.time()
print('cost time in refine:', time_point5 - time_point4)
# For GT
poseGT_ashyps = Hypothesis.Hypothesis()
poseGT_ashyps.Info(poseGT)
invPoseGT = Hypothesis.Hypothesis()
invPoseGT = maxloss.getInvHyp(poseGT_ashyps)
'''
Note: hypIdx means sample/get argmax of score
index means get highest inlier
'''
# For hypothesis
jpHyp = TYPE.cv2our([REF_HYP[index, :3], REF_HYP[index, 3:]])
poseEst = Hypothesis.Hypothesis()
poseEst.RotandTrans(jpHyp[0], jpHyp[1])
invPoseEst = Hypothesis.Hypothesis()
invPoseEst = maxloss.getInvHyp(poseEst)
jpHyp_score = TYPE.cv2our([REF_HYP[hypIdx, :3], REF_HYP[hypIdx, 3:]])
poseEst_score = Hypothesis.Hypothesis()
poseEst_score.RotandTrans(jpHyp_score[0], jpHyp_score[1])
invPoseEst_score = Hypothesis.Hypothesis()
invPoseEst_score = maxloss.getInvHyp(poseEst_score)
jpHyp_ransac = TYPE.cv2our(
[REF_HYP[hypIdx_ransac, :3], REF_HYP[hypIdx_ransac, 3:]])
poseEst_ransac = Hypothesis.Hypothesis()
poseEst_ransac.RotandTrans(jpHyp_ransac[0], jpHyp_ransac[1])
invPoseEst_ransac = Hypothesis.Hypothesis()
invPoseEst_ransac = maxloss.getInvHyp(poseEst_ransac)
print('GT:', invPoseGT.getRotation(), invPoseGT.getTranslation())
print('Hy:', invPoseEst.getRotation(), invPoseEst.getTranslation())
rotErr = invPoseGT.calcAngularDistance(invPoseEst)
tErr = np.linalg.norm(invPoseEst.getTranslation() -
invPoseGT.getTranslation())
rotErr_score = invPoseGT.calcAngularDistance(invPoseEst_score)
tErr_score = np.linalg.norm(invPoseEst_score.getTranslation() -
invPoseGT.getTranslation())
rotErr_ransac = invPoseGT.calcAngularDistance(invPoseEst_ransac)
tErr_ransac = np.linalg.norm(invPoseEst_ransac.getTranslation() -
invPoseGT.getTranslation())
correct = False
if rotErr < 5 and tErr < 50:
print('Bounded Rotation Err:', rotErr, 'and Bounded Translation Err:',
tErr)
correct = True
else:
print('Unbounded Rotation Err:', rotErr,
'and Unbounded Translation Err:', tErr)
correct_socre = False
if rotErr_score < 5 and tErr_score < 50:
print('Bounded Rotation Err_score:', rotErr_score,
'and Bounded Translation Err_score:', tErr_score)
correct_socre = True
else:
print('Unbounded Rotation Err_score:', rotErr_score,
'and Unbounded Translation Err_ransac:', tErr_score)
correct_ransac = False
if rotErr_ransac < 5 and tErr_ransac < 50:
print('Bounded Rotation Err_ransac:', rotErr_ransac,
'and Bounded Translation Err_ransac:', tErr_ransac)
correct_ransac = True
else:
print('Unbounded Rotation Err_ransac:', rotErr_ransac,
'and Unbounded Translation Err_ransac:', tErr_ransac)
return correct, correct_socre, correct_ransac, rotErr, tErr, rotErr_score, tErr_score, rotErr_ransac, tErr_ransac
def readData_rgbd(bgrFile, dFile):
img = TYPE.imag_brgd_t()
img.bgr = readData_bgr(bgrFile)
img.depth = readData_depth(dFile)
return img
def Info(self, info=TYPE.info_t()):
self.rotation = info.rotation
self.translation = info.center * 1e3
self.invRotation = inv(self.rotation)
def processImage(imgBGR, poseGT, model_obj, model_score, objHyps, ptCount,
camMat, inlierThreshold2D, inlierCount, refSteps, hyps,
refHyps, imgPts, objPts, imgIdx, sfScores, estObj, sampling,
sampledPoints, inlierMaps, pixelIdxs):
time_start = time.time()
keymap, _ = CornerDetector(imgBGR)
sampling = stochasticSubSample(keymap, CNN_OBJ_PATCHSIZE,
CNN_RGB_PATCHSIZE)
# sampling = stochasticSubSample(imgBGR, CNN_OBJ_PATCHSIZE, CNN_RGB_PATCHSIZE)
# patches = [] # here define the patch a [y,x,3], and patches is a list of them, or
estObj = getCoordImg(imgBGR, sampling, CNN_RGB_PATCHSIZE, model_obj)
# hyps = []
# imgPts = [] #这个是一系列（第一维）的点集，每个点集（第二维）存了这个点在原始rgb照片中的位置（第3，4维）
# objPts = [] #与上一个类似，最后每个点存了对应物体上点的3d坐标（第3，4，5维）
# sampledPoints = [] #与上一个类似，每个点在subsampled图片中的位置
# imgIdx = [] #配合着上一个使用，表示的是每一个subsampled图片中的点的一维编号（size*y+x)（第二维）
# 他们的第一维的大小都是和假设的数量是一样的
for h in range(0, objHyps):
np.random.seed()
while True:
projections = []
alreadyChosen = np.zeros((estObj.shape[0], estObj.shape[1]))
imgPts.append([])
objPts.append([])
imgIdx.append([]) # the usage of .clear()
sampledPoints.append([])
hyps.append([])
j = 0
while j < ptCount:
x = np.random.randint(0, estObj.shape[1])
y = np.random.randint(0, estObj.shape[0])
j = j + 1
if (alreadyChosen[y, x] > 0):
j = j - 1
continue
alreadyChosen[y, x] = 1
imgPts[h].append(sampling[y, x, :])
objPts[h].append(estObj[y, x, :])
imgIdx[h].append(y * CNN_OBJ_PATCHSIZE + x)
sampledPoints[h].append(np.array([x, y]))
objPts_3D = np.array(objPts[h])
imgPts_3D = np.array(imgPts[h])
hyps[h].append(
safeSolvePnP(objPts=objPts_3D,
imgPts=imgPts_3D,
camMat=camMat,
disCoeffs=None,
methodFlag=cv2.SOLVEPNP_P3P,
rot=np.zeros([3]),
trans=np.zeros([3]))[0])
hyps[h].append(
safeSolvePnP(objPts=objPts_3D,
imgPts=imgPts_3D,
camMat=camMat,
disCoeffs=None,
methodFlag=cv2.SOLVEPNP_P3P,
rot=np.zeros([3]),
trans=np.zeros([3]))[1])
if not (safeSolvePnP(objPts=objPts_3D,
imgPts=imgPts_3D,
camMat=camMat,
disCoeffs=None,
methodFlag=cv2.SOLVEPNP_P3P,
rot=np.zeros([3]),
trans=np.zeros([3]))[0]).all():
del imgPts[h]
del objPts[h]
del imgIdx[h]
del sampledPoints[h]
del hyps[h]
continue
#else: break
# to project a 3d point into the image(do not know whether there is a function in cv2 can fulfill the task)
for points in range(0, len(objPts[h])):
objMat = objPts[h][points]
projection, _ = cv2.projectPoints(objectPoints=objMat,
rvec=hyps[h][0],
tvec=hyps[h][1],
cameraMatrix=camMat,
distCoeffs=None)
projections.append(projection)
foundOutlier = False
for j in range(0, len(imgPts[h])):
# print("diff is:",np.linalg.norm(imgPts[h][j] - projections[j]))
if (np.linalg.norm(imgPts[h][j] - projections[j]) <
inlierThreshold2D):
continue
foundOutlier = True
break
if (foundOutlier):
del imgPts[h]
del objPts[h]
del imgIdx[h]
del sampledPoints[h]
del hyps[h]
continue
else:
break
transform = Transform_SCORE()
if not h:
FullDiffMap = getDiffMap(hyps[h], estObj, sampling, camMat)
FullDiffMap = transform(FullDiffMap).unsqueeze(0)
else:
diffmap = getDiffMap(hyps[h], estObj, sampling, camMat)
diffmap = transform(diffmap).unsqueeze(0)
FullDiffMap = torch.cat((FullDiffMap, diffmap), 0)
scores = Model_score.forward(model_score, FullDiffMap, DEVICE)
sfScores = softMax(scores)
sfEntropy = entropy(sfScores)
hypIdx = draw(sfScores)
# Refine
(height, width) = np.shape(sampling[:, :, 0])
inlierMaps = []
for h in range(len(hyps)):
refHyps.append(hyps[h])
localDiffMap = FullDiffMap[h, 0, :, :].numpy()
inlierMaps.append(np.zeros([height, width]))
pixelIdxs.append([])
for rStep in range(refSteps):
pixelIdxs[h].append([])
for idx in range(height * width):
pixelIdxs[h][rStep].append(idx)
random.shuffle(pixelIdxs[h][rStep])
localImgPts = []
localObjPts = []
for idx in range(height * width):
x = pixelIdxs[h][rStep][idx] % width
y = pixelIdxs[h][rStep][idx] // width
if localDiffMap[y, x] < inlierThreshold2D:
localImgPts.append(sampling[y, x, :])
localObjPts.append(estObj[y, x, :])
elif len(localObjPts) >= inlierCount:
break
if len(localObjPts) < 50:
break
if len(localObjPts) > 4:
methodflag = cv2.SOLVEPNP_ITERATIVE
else:
methodflag = cv2.SOLVEPNP_AP3P
localObjPts, localImgPts = np.array([localObjPts]).reshape(
[-1, 3, 1]), np.array([localImgPts]).reshape([-1, 2, 1])
hypUpdate = copy.deepcopy(refHyps[h])
if not (safeSolvePnP(objPts=localObjPts,
imgPts=localImgPts,
camMat=camMat,
disCoeffs=None,
methodFlag=methodflag,
rot=hypUpdate[0],
trans=hypUpdate[1])[0]).all():
break
refHyps[h] = copy.deepcopy(hypUpdate)
for pt in range(0, len(sampledPoints[h])):
x = sampledPoints[h][pt][0]
y = sampledPoints[h][pt][1]
inlierMaps[h][y, x] = 0
jpHyp = TYPE.cv2our(refHyps[hypIdx])
poseEst = Hypothesis.Hypothesis()
poseEst.RotandTrans(jpHyp[0], jpHyp[1])
print('beforehy:', poseEst.getRotation(), poseEst.getTranslation())
poseGT_ashyps = Hypothesis.Hypothesis()
poseGT_ashyps.Info(poseGT)
print('beforeGT:', poseGT_ashyps.getRotation(),
poseGT_ashyps.getTranslation())
expectedLoss, losses = expectedMaxLoss(poseGT_ashyps, refHyps, sfScores)
print('Loss of winning hyp:', ml.maxLoss(poseGT_ashyps, poseEst), ',prob:',
sfScores[hypIdx], 'expected loss:', expectedLoss, '.')
invPoseEst = Hypothesis.Hypothesis()
invPoseGT = Hypothesis.Hypothesis()
invPoseGT = ml.getInvHyp(poseGT_ashyps)
invPoseEst = ml.getInvHyp(poseEst)
print('GT:', invPoseGT.getRotation(), invPoseGT.getTranslation())
print('Hy:', invPoseEst.getRotation(), invPoseEst.getTranslation())
rotErr = invPoseGT.calcAngularDistance(invPoseEst)
tErr = np.linalg.norm(invPoseEst.getTranslation() -
invPoseGT.getTranslation())
correct = False
if rotErr < 5 and tErr < 50:
print('Bounded Rotation Err:', rotErr, 'and Bounded Translation Err:',
tErr)
correct = True
else:
print('Unbounded Rotation Err:', rotErr,
'and Unbounded Translation Err:', tErr)
return expectedLoss, sfEntropy, correct, losses, tErr, rotErr, hypIdx
def dScore(
estObj,
sampling,
points,
model, # jacobeans is output param, so not in here
scoreOutputGradients):
gp = properties.GlobalProperties()
camMat = gp.getCamMat()
hypCount = points.shape[0]
imgPts = []
objPts = []
hyps = []
diffMaps = []
dscore_dDiffmaps = []
dDiffMaps = []
for h in range(hypCount):
for i in range(4):
x = points[h, i, 0]
y = points[h, i, 1]
imgPts[h].append(sampling[y, x, :])
objPts[h].append(estObj[y, x, :])
# 没写solvepnp输入输出格式这里按照后面一致的格式
cvHyp = safeSolvePnP(objPts=objPts[h],
imgPts=imgPts[h],
camMat=camMat,
disCoeffs=None,
methodFlag=cv2.SOLVEPNP_P3P,
rot=np.zeros([3]),
trans=np.zeros([3]))
hyps[h] = TYPE.cv2our(cvHyp)
diffMaps[h] = getDiffMap(cvHyp, estObj, sampling, camMat)
trasnform = Transform_SCORE()
diffMaps[h] = trasnform(diffMaps)
dscore_dDiffmaps.append(
Model_score.backward(model=model, data=diffMaps[h], device=DEVICE))
dDiffMaps.append(dscore_dDiffmaps[h] * scoreOutputGradients)
jacobeans = []
# jacobeans = np.array()
for h in range(hypCount):
jacobean = np.zeros(estObj.shape[0] * estObj.shape[1] * 3)
supportPointGradients = np.zeros(1, 12)
dHdO = dPNP(imgPts[h], objPts[h]) # 6*12 dimension
for x in range(CNN_OBJ_PATCHSIZE):
for y in range(CNN_OBJ_PATCHSIZE):
pt = sampling[y, x, :]
obj = estObj[y, x, :]
dPdO = dProjectObj(pt=pt,
obj=obj,
rot=hyps[h][0],
t=hyps[h][1],
camMat=camMat)
dPdO = dPdO * dDiffMaps[h][y, x]
jacobean[1, x * CNN_OBJ_PATCHSIZE * 3 +
y * 3:x * CNN_OBJ_PATCHSIZE * 3 + y * 3 +
3] = copy.copy(dPdO)
dPdH = dProjectdHyp(sampling[y, x, :], estObj[y, x, :],
hyps[h][0], hyps[h][1], camMat)
supportPointGradients += dDiffMaps[h][y, x] * dPdH * dHdO
for i in range(4):
x = points[h, i, 0]
y = points[h, i, 1]
jacobean[1, x * CNN_OBJ_PATCHSIZE * 3 + y * 3: x * CNN_OBJ_PATCHSIZE * 3 + y * 3 + 3] += \
supportPointGradients[1, i * 3 : i * 3 + 3]
jacobeans.append(jacobean)
return jacobeans