void computeFOV()
{
sl.Resolution image_size = camera_calibration.leftCam.resolution;
float fx = camera_calibration.leftCam.fx;
fov = 2.0f * (float)Math.Atan((int)image_size.width / (2.0f * fx));
}
/// <summary>
/// Compute the size of the final planes
/// </summary>
/// <param name="resolution"></param>
/// <param name="perceptionDistance"></param>
/// <param name="eyeToZedDistance"></param>
/// <param name="planeDistance"></param>
/// <param name="HMDFocal"></param>
/// <param name="zedFocal"></param>
/// <returns></returns>
public Vector2 ComputeSizePlaneWithGamma(sl.Resolution resolution, float perceptionDistance, float eyeToZedDistance, float planeDistance, float HMDFocal, float zedFocal)
{
System.IntPtr p = dllz_compute_size_plane_with_gamma(resolution, perceptionDistance, eyeToZedDistance, planeDistance, HMDFocal, zedFocal);
if (p == System.IntPtr.Zero)
{
return(new Vector2());
}
Vector2 parameters = (Vector2)Marshal.PtrToStructure(p, typeof(Vector2));
return(parameters);
}
public TrackingViewer(sl.Resolution res, int fps_, float D_max, int duration)
{
// ----------- Default configuration -----------------
// window size
window_width = (int)res.width;
window_height = (int)res.height;
// Visualization configuration
camera_offset = 50;
// history management
min_length_to_draw = 3;
// camera settings
fov = -1.0f;
// Visualization settings
background_color = new Scalar(248, 248, 248, 255);
has_background_ready = false;
background = new OpenCvSharp.Mat(window_height, window_width, MatType.CV_8UC4, background_color);
Scalar ref_ = new Scalar(255, 117, 44, 255);
for (int p = 0; p < 3; p++)
{
fov_color[p] = (ref_[p] + 2 * background_color[p]) / 3;
}
// SMOOTH
do_smooth = false;
// Show last 3.0 seconds
history_duration = (ulong)(duration) * 1000 * 1000 * 1000; //convert sc to ns
// Smoothing window: 80ms
smoothing_window_size = (int)(Math.Ceiling(0.08f * fps_) + .5f);
// invert Z due to Y axis of ocv windows
z_min = -D_max;
x_min = z_min / 2.0f;
x_max = -x_min;
x_step = (x_max - x_min) / window_width;
z_step = Math.Abs(z_min) / (window_height - camera_offset);
}
/// <summary>
/// Compute the focal
/// </summary>
/// <param name="targetSize"></param>
/// <returns></returns>
public float ComputeFocal(sl.Resolution targetSize)
{
float focal_hmd = dllz_compute_hmd_focal(targetSize, finalLeftEye.projectionMatrix.m00, finalLeftEye.projectionMatrix.m11);
return(focal_hmd);
}
private static extern float dllz_compute_hmd_focal(sl.Resolution r, float w, float h);
private static extern System.IntPtr dllz_compute_size_plane_with_gamma(sl.Resolution resolution, float perceptionDistance, float eyeToZedDistance, float planeDistance, float HMDFocal, float zedFocal);
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();
}
