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()
pointset[0] = point
