u1 = fx * x1 / z1 + cx v1 = fy * y1 / z1 + cy err_x = u1 - feat_0[0] err_y = v1 - feat_0[1] err_1 = err_x * err_x + err_y * err_y return x3D, err_0, err_1 else: return None if __name__ == '__main__': import sophus import Frame d = [0.2624, -0.9531, -0.0054, 0.0026, 1.1633] camera = Frame.Camera(517.3, 516.5, 318.6, 255.3, 640, 480, d, None) pyr_camera = Frame.PyrCamera(camera, 4) frame = Frame.Frame(camera=pyr_camera) # frame.mPose2World = sophus.SE3() frame.mPose2World = sophus.SE3.rotY(np.pi / 4) * frame.mPose2World.trans( 0.1, 0.2, 0.3) # print np.dot((sophus.SE3.rotX(np.pi / 4).matrix())[:3, :3], frame.mPose2World.trans(0.1, 0.2, 0.3).matrix()[:3, 3]) pixel_depth = [318.6, 255.3, 10] point = MapPoint(frame, pixel_depth, 0) print point.mPixel print point.mWorldPos print point.mNormalVector print point.mCamPos print frame.mPose2World pointset = dict()