A method for reconstructing a three-dimensional, 3D, scene as a three-dimensional, 3D, model, comprising steps: acquiring, of the scene, red, green, blue, and depth, RGBD, images with a sensor, and wide field-of-view, FOV, red, green, and blue, RGB, images with a camera from different viewpoints; identifying pair-wise point matches among the RGBD images and the RGB images; constructing a graph of nodes and edges using the pair-wise point matches, wherein the nodes represent the RGBD images and the RGB images, and the edges represent pose constraints between two images corresponding to the relative pose between the RGBD images and the RGB images, wherein the pose constraints between two images exist only when the two images share a minimum number of point matches; estimating initial poses of the RGBD images and the RGB images represented in the graph as nodes using the pose constraints represented in the graph as edges that can be computed using the identified pair-wise point matches; estimating an initial 3D model of the scene using the estimated initial poses; applying bundle adjustment to the initial poses and the initial 3D model computed using both RGBD and RGB images to obtain refined poses and a refined 3D model, removing erroneous edges in the graph using constraints based on the estimated initial poses, the erroneous edges representing incorrect relative poses between adjacent images, wherein the estimated initial pose that is far from the refined pose based on a distance metric is one of the erroneous edges; applying subsequent bundle adjustment to the refined poses and the refined 3D model to obtain the 3D model of the scene, wherein said erroneous edges are not used in the subsequent bundle adjustment, wherein the steps are performed in a processor. acquiring, of the scene, red, green, blue, and depth, RGBD, images with a sensor, and wide field-of-view, FOV, red, green, and blue, RGB, images with a camera from different viewpoints; identifying pair-wise point matches among the RGBD images and the RGB images; constructing a graph of nodes and edges using the pair-wise point matches, wherein the nodes represent the RGBD images and the RGB images, and the edges represent pose constraints between two images corresponding to the relative pose between the RGBD images and the RGB images, wherein the pose constraints between two images exist only when the two images share a minimum number of point matches; estimating initial poses of the RGBD images and the RGB images represented in the graph as nodes using the pose constraints represented in the graph as edges that can be computed using the identified pair-wise point matches; estimating an initial 3D model of the scene using the estimated initial poses; applying bundle adjustment to the initial poses and the initial 3D model computed using both RGBD and RGB images to obtain refined poses and a refined 3D model, removing erroneous edges in the graph using constraints based on the estimated initial poses, the erroneous edges representing incorrect relative poses between adjacent images, wherein the estimated initial pose that is far from the refined pose based on a distance metric is one of the erroneous edges; applying subsequent bundle adjustment to the refined poses and the refined 3D model to obtain the 3D model of the scene, wherein said erroneous edges are not used in the subsequent bundle adjustment, wherein the steps are performed in a processor.