return F*jac
if __name__ == '__main__':
# 小正方体特征点索引
indexs = [0, 1, 3, 2]
cube_points = [(-100, 100, -100), (-100, -100, -100), (100, 100, -100), (100, -100, -100), (100, 100, 100), (100, -100, 100), (-100, 100, 100), (-100, -100, 100)]
# 测量值列表
measured_points = []
quaternions = []
for i in range(200):
temp = []
for index in indexs:
pos = getImagePosition(cube_points[index][0], cube_points[index][1], cube_points[index][2], 0, 0, i, 0)
# 相机坐标系转换到图像坐标系
x, y = fromCameraToImage(pos[0], pos[1], pos[2])
# temp.append((x, y))
temp.append([x + 0.01*np.random.randn()])
temp.append([y + 0.01*np.random.randn()])
quaternions.append(getQuaternion(0, 0, -i))
measured_points.append(temp)
# 验证坐标是否正确
# for i in range(200):
# prefix = "/Users/apple/Desktop/test/img4/"
# filename = "img" + "0" * (4 - len(str(i))) + str(i) + ".jpg"
#
# img = cv2.imread(prefix + filename)
# result = []
# for p in measured_points[i]:
if __name__ == '__main__':
# 小正方体特征点索引
indexs = [1, 7, 5, 3]
# 测量值列表
measured_points = []
quaternions = []
for i in range(200):
fileName = "/Users/apple/Desktop/test/Cube2/pos"+str(i)+".txt"
with open(fileName, "r") as file:
temp = []
lines = file.readlines()
for index in indexs:
line = lines[index].strip("\n").split()
# 相机坐标系转换到图像坐标系(y值要取反！！！！！！！！！！)
x, y = fromCameraToImage(float(line[0]), -float(line[1]), float(line[2]))
temp.append([x])
temp.append([y])
qList = lines[-1].strip("\n").split()
quaternions.append([float(q) for q in qList])
measured_points.append(temp)
# 估计值列表
kalman_points = []
# 初值:小正方体坐标系原点在相机坐标系下的位置(y值要取反！！！！！！！！！！)
# s0 = [0.000000, 141.442716, 6930.693069]
s0 = [100., 0., 6930.693]
s0 = [100., 0, 4101.839]
s0 = [100., 0, 1980.198]