public bool updateImageAndState(Mat image, Pose pose_, POSITIONAL_TRACKING_STATE track_state, SPATIAL_MAPPING_STATE mapp_state) { if (available) { image_handler.pushNewImage(image); pose = Matrix4x4.Identity; pose = Matrix4x4.Transform(pose, pose_.rotation); pose.Translation = pose_.translation; pose = Matrix4x4.Transpose(pose); tracking_state = track_state; mapping_state = mapp_state; } bool cpy_state = change_state; change_state = false; return(cpy_state); }
public MainWindow(string[] args) { // Set configuration parameters InitParameters init_params = new InitParameters(); init_params.resolution = RESOLUTION.HD720; init_params.cameraFPS = 60; init_params.depthMode = DEPTH_MODE.ULTRA; init_params.coordinateUnits = UNIT.METER; init_params.coordinateSystem = COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP; init_params.depthMaximumDistance = 15f; init_params.sdkVerbose = true; parseArgs(args, ref init_params); // Open the camera zedCamera = new Camera(0); ERROR_CODE err = zedCamera.Open(ref init_params); if (err != ERROR_CODE.SUCCESS) { Environment.Exit(-1); } if (zedCamera.CameraModel != sl.MODEL.ZED2) { Console.WriteLine(" ERROR : Use ZED2 Camera only"); return; } tracking_state = POSITIONAL_TRACKING_STATE.OFF; mapping_state = SPATIAL_MAPPING_STATE.NOT_ENABLED; mapping_activated = false; // Enable tracking PositionalTrackingParameters positionalTrackingParameters = new PositionalTrackingParameters(); zedCamera.EnablePositionalTracking(ref positionalTrackingParameters); runtimeParameters = new RuntimeParameters(); spatialMappingParameters = new SpatialMappingParameters(); spatialMappingParameters.resolutionMeter = SpatialMappingParameters.get(MAPPING_RESOLUTION.MEDIUM); spatialMappingParameters.saveTexture = false; if (CREATE_MESH) { spatialMappingParameters.map_type = SPATIAL_MAP_TYPE.MESH; } else { spatialMappingParameters.map_type = SPATIAL_MAP_TYPE.FUSED_POINT_CLOUD; } // Create ZED Objects filled in the main loop zedMat = new Mat(); cam_pose = new Pose(); //Create mesh. mesh = new Mesh(); fusedPointCloud = new FusedPointCloud(); int Height = zedCamera.ImageHeight; int Width = zedCamera.ImageWidth; Resolution res = new Resolution((uint)Width, (uint)Height); zedMat.Create(res, MAT_TYPE.MAT_8U_C4, MEM.CPU); // Create OpenGL Viewer viewer = new GLViewer(new Resolution((uint)Width, (uint)Height)); Console.WriteLine("Hit SPACE BAR to start spatial mapping..."); // Create OpenGL window CreateWindow(); }
// Render loop private void NativeWindow_Render(object sender, NativeWindowEventArgs e) { OpenGL.CoreUI.NativeWindow nativeWindow = (OpenGL.CoreUI.NativeWindow)sender; Gl.Viewport(0, 0, (int)nativeWindow.Width, (int)nativeWindow.Height); Gl.Clear(ClearBufferMask.ColorBufferBit); ERROR_CODE err = ERROR_CODE.FAILURE; if (viewer.isAvailable() && zedCamera.Grab(ref runtimeParameters) == ERROR_CODE.SUCCESS) { if (zedMat.IsInit()) { // Retrieve left image err = zedCamera.RetrieveImage(zedMat, sl.VIEW.LEFT, sl.MEM.CPU); // Update pose data (used for projection of the mesh over the current image) tracking_state = zedCamera.GetPosition(ref cam_pose); if (mapping_activated) { mapping_state = zedCamera.GetSpatialMappingState(); if (timer % 60 == 0 && viewer.chunksUpdated() == true) { zedCamera.RequestSpatialMap(); } if (zedCamera.GetMeshRequestStatus() == ERROR_CODE.SUCCESS && timer > 0) { /// MAP_TYPE == MESH if (CREATE_MESH) { //Retrieves data for mesh visualization only (vertices + triangles); zedCamera.RetrieveChunks(ref mesh); var chunks = new List <Chunk>(mesh.chunks.Values); viewer.updateData(chunks); } /// MAP_TYPE == FUSED_POINT_CLOUD else { zedCamera.RetrieveSpatialMap(ref fusedPointCloud); viewer.updateData(fusedPointCloud.vertices); } } } bool change_state = viewer.updateImageAndState(zedMat, cam_pose, tracking_state, mapping_state); if (change_state) { if (!mapping_activated) { Quaternion quat = Quaternion.Identity; Vector3 vec = Vector3.Zero; zedCamera.ResetPositionalTracking(quat, vec); zedCamera.EnableSpatialMapping(ref spatialMappingParameters); Console.WriteLine("Hit SPACE BAR to stop spatial mapping..."); // Clear previous Mesh data viewer.clearCurrentMesh(); mapping_activated = true; } else { // Filter the mesh (remove unnecessary vertices and faces) if (CREATE_MESH) { zedCamera.ExtractWholeSpatialMap(); zedCamera.FilterMesh(MESH_FILTER.MEDIUM, ref mesh); } if (CREATE_MESH && spatialMappingParameters.saveTexture) { zedCamera.ApplyTexture(ref mesh); } bool error_save = false; string saveName = ""; //Save mesh as obj file if (CREATE_MESH) { saveName = "mesh_gen.obj"; error_save = zedCamera.SaveMesh(saveName, MESH_FILE_FORMAT.OBJ); } else { saveName = "point_cloud.ply"; error_save = zedCamera.SavePointCloud(saveName, MESH_FILE_FORMAT.PLY); } if (error_save) { Console.WriteLine("Mesh saved under: " + saveName); } else { Console.WriteLine("Failed to save the mesh under: " + saveName); } mapping_state = SPATIAL_MAPPING_STATE.NOT_ENABLED; mapping_activated = false; zedCamera.DisableSpatialMapping(); Console.WriteLine("Hit SPACE BAR to start spatial mapping..."); } } timer++; viewer.render(); } } }