/// <summary> /// The Unity Start() method. /// </summary> public void Start() { Debug.LogWarning("Unique ID: " + SystemInfo.deviceUniqueIdentifier); // A Name is provided to the Game Object so it can be found by other Scripts instantiated as prefabs in the // scene. gameObject.name = "Reconstruction4DController"; ARCoreRoot.SetActive(false); ARKitRoot.SetActive(false); _ResetStatus(); pointCloudBuilder = new PointCloudBuilder(10); trajectoryBuilder = new TrajectoryBuilder(10); }
public static async Task <PointCloud> GenerateObject(Recording recording, IProgress <int> progress, CancellationToken ct) { // Quick access image formats var dFormat = recording.DepthFormat; var cFormat = recording.ColourFormat; #region Check for calibration data if (!recording.IsCalibratedRecording) { throw new IOException("Pre-calibrated recording required!"); } var calibration = recording.Calibration; #endregion #region Kalman Filter setup var kSetup = new KalmanSetup() { funcF = new Func <float, Matrix>(FGenerator), funcB = new Func <float, Matrix>(BGenerator), H = Matrix.OfArray(new float[, ] { { 1, 0 } }), X0 = new Vector(new float[] { 0, 0 }), P0 = Matrix.OfArray(new float[, ] { { 10, 0 }, { 0, 10 } }), funcQ = new Func <float, float, Matrix>(QGenerator), funcR = new Func <float, Matrix>(RGenerator), StdDevMeasurement = stdDevMeasurementNoise, StdDevProcess = stdDevProcessNoise }; float R = calibration.TurntableRadius; float acc = -4 * mu * g / (3 * R); var accelerationVector = new Vector(new float[] { acc }); #endregion #region Initialise pipeline stages var cv = new HueThresholder(calibration.Markers, calibration.MarkerSearchSpace); var geom = new PointCloudBuilder(calibration, recording.MappingParameters); var kf = new KalmanFilter(kSetup); var opt = new ICPMinimiser(true); #endregion // start new pipeline thread return(await Task.Run <PointCloud>(() => { var returnCloud = new PointCloud(); PointCloud prevCloud = null; float lastTime = 0; double theta = 0; Point3D lastx = new Point3D(0, 0, 0); int advanceUnit = Processing.Settings.Default.ProcessEveryFrame ? 1 : 10; #region KF Debug Logging StreamWriter filterLog = null; if (DebugSettings.Default.DebugMode) { filterLog = new StreamWriter(DebugSettings.Default.LogRoot + @"\filter.csv"); filterLog.AutoFlush = true; filterLog.WriteLine("t,dt,dx,dtheta,theta,theta_KF,omega_KF"); } #endregion for (int i = 0; i < recording.NumberOfFrames; i += advanceUnit) { ct.ThrowIfCancellationRequested(); progress.Report(i); #region Fetch // Fetch frame from disk float timestamp = recording.Timestamps[i]; var colour = recording.FetchColourFrame(i); var depth = recording.FetchDepthFrame(i); Frame f = new Frame(i, timestamp, depth, dFormat, colour, cFormat); #endregion #region Analyse (CV) var markers2D = cv.IdentifyMarkers(f); if (markers2D.Count == 0) { i -= (advanceUnit - 1); continue; } #endregion #region Build Geometry (GEOM) PointCloud cloud; KeyValuePair <TrackingMarker, Point3D> rotationMarker; var geomSuccess = geom.ConstructCloud(f, markers2D, out cloud, out rotationMarker); if (!geomSuccess) { i -= (advanceUnit - 1); continue; } #endregion #region Filter // reposition marker based on offset var polarX = CoordinateSystemConvertor.ToSpherical(rotationMarker.Value); polarX.Azimuthal += (float)rotationMarker.Key.RotationalOffset; var x = CoordinateSystemConvertor.ToCartesian(polarX); // rotation approximation var dx = ((Vector)x - lastx).Norm(3); var dtheta = dx / calibration.TurntableRadius; // update cumulative angle (with estimated angle) theta += dtheta; // calculate time delta float dt = f.Timestamp - lastTime; if (DebugSettings.Default.DebugMode) { filterLog.Write("{0},{1},{2},{3},{4},", f.Timestamp, dt, dx, dtheta, theta); } // calculate state vector var stateVector = new Vector(new float[] { (float)theta }); // pass through kalman filter var filteredStateVector = kf.Update(dt, stateVector, accelerationVector); // extract angle from state vector var filteredTheta = filteredStateVector[0]; var filteredOmega = filteredStateVector[1]; if (DebugSettings.Default.DebugMode) { filterLog.WriteLine("{0},{1}", filteredTheta, filteredOmega); } // update last time, last position and set filtered cumulative theta theta = filteredTheta; lastx = x; lastTime = f.Timestamp; #endregion #region Optimise (OPT) PointCloud optCloud = null; if (prevCloud == null) { optCloud = cloud; // first frame; this is requires no optimising transformation } else if (prevCloud.Points.Count == 0) { optCloud = cloud; // no points in previous cloud (model); cannot align the current cloud to model } else { // Build best guess transformation matrix float sinT = (float)Math.Sin(filteredTheta); float cosT = (float)Math.Cos(filteredTheta); var bestGuessTransformation = Matrix.Identity(4); bestGuessTransformation[0, 0] = cosT; bestGuessTransformation[0, 2] = -sinT; bestGuessTransformation[2, 0] = sinT; bestGuessTransformation[2, 2] = cosT; // returned object is result of aligning cloud with prevCloud optCloud = opt.Process(cloud, prevCloud, bestGuessTransformation); } prevCloud = cloud; #endregion #region Retire returnCloud.Points.AddRange(optCloud.Points); #endregion } if (DebugSettings.Default.DebugMode) { filterLog.Close(); } return returnCloud; })); }