A method of colorizing a 3D point cloud includes receiving the 3D point cloud, receiving a 2D color image acquired by a camera, creating a 2D intensity image of the 3D point cloud based on intrinsic and extrinsic parameters of the camera, generating a set of refined camera parameters by matching the 2D intensity image and the 2D color image, creating a depth buffer for the 3D point cloud using the set of refined camera parameters, determining a foreground depth for each respective pixel of the depth buffer, and coloring the point cloud by, for each respective point of the 3D point cloud: upon determining that the respective point is in the foreground, assigning a color of a corresponding pixel in the 2D color image to the respective point; and upon determining that the respective point is not in the foreground, not assigning any color to the respective point.