Build 3D point cloud from image map and pose data

Yet to visualize the data to make sure that what i'm doing is right.