def cal_PointProject_Jacobians(cam: PinholeCameraCal3_S2, pw: Point3):
q = cam.pose().transformTo(pw)
pn = cam.Project(q)
pi = cam.calibration().uncalibrate(pn)
d = 1 / q[2]
Rt = cam.pose().rotation().transpose()
Dpose = cal_Dpose(pn, d)
Dpoint = cal_Dpoint(pn, d, Rt)
Dpi_pn = np.array([[cam.calibration().fx(), 0],
[0, cam.calibration().fy()]])
Dpose = np.matmul(Dpi_pn, Dpose) # - ?
Dpoint = np.matmul(Dpi_pn, Dpoint) # - ?
return pi, Dpose, Dpoint
def cal_Point_Project_TartanCam(cam: PinholeCameraCal3_S2, pw: Point3):
q = cam.pose().transformTo(pw)[[1, 2, 0]] #
pn = cam.Project(q)
pi = cam.calibration().uncalibrate(pn)
# d = 1 / q[2]
return np.rint(pi).astype(np.int32)
def test_constructor(self):
pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5))
camera = SimpleCamera(pose1, K)
self.gtsamAssertEquals(camera.calibration(), K, 1e-9)
self.gtsamAssertEquals(camera.pose(), pose1, 1e-9)