Merge pull request andyzeng#9 from zmurez/patch-1

Fix voxels behind camera

fusion.py

@@ -96,7 +96,7 @@ def __init__(self,vol_bnds,voxel_size):
                 // Skip if outside view frustum
                 int im_h = (int) other_params[2];
                 int im_w = (int) other_params[3];
-                if (pixel_x < 0 || pixel_x >= im_w || pixel_y < 0 || pixel_y >= im_h)
+                if (pixel_x < 0 || pixel_x >= im_w || pixel_y < 0 || pixel_y >= im_h || cam_pt_z<0)
                     return;
 
                 // Skip invalid depth
@@ -216,7 +216,8 @@ def integrate(self,color_im,depth_im,cam_intr,cam_pose,obs_weight=1.):
             valid_pix = np.logical_and(pix_x >= 0,
                         np.logical_and(pix_x < im_w,
                         np.logical_and(pix_y >= 0,
-                                       pix_y < im_h)))
+                        np.logical_and(pix_y < im_h,
+                                       cam_pts[2,:] > 0))))
 
             depth_val = np.zeros(pix_x.shape)
             depth_val[valid_pix] = depth_im[pix_y[valid_pix],pix_x[valid_pix]]
@@ -318,4 +319,4 @@ def meshwrite(filename,verts,faces,norms,colors):
     for i in range(faces.shape[0]):
         ply_file.write("3 %d %d %d\n"%(faces[i,0],faces[i,1],faces[i,2]))
 
-    ply_file.close()
+    ply_file.close()