Beispiel #1
0
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)
Beispiel #3
0
 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)