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();
}
}
}
