public void SampleLiDAR(DateTime timeStamp, LiDARTransformType type = LiDARTransformType.None) { foreach (Form form in this.MdiChildren) { if ((!form.IsDisposed) && (form is VelodyneStreamWnd)) { VelodyneStreamWnd veloForm = form as VelodyneStreamWnd; veloForm.SetFrameByTime(timeStamp); List <VelodynePoint> pts = veloForm.GetPoints(); if (pts.Count() == 0) { continue; } double[,] ptsMat = Matrix.Create <double>(4, pts.Count(), 0); for (int i = 0; i < pts.Count(); i++) { ptsMat[0, i] = pts[i].X; ptsMat[1, i] = pts[i].Y; ptsMat[2, i] = pts[i].Z; ptsMat[3, i] = 1; } var veloStream = veloForm.DataStream as VelodyneDataStream; if ((veloStream.Tp == null) && (type != LiDARTransformType.None)) { this.ConsoleLogger.WriteLineWarning("Transfromation matrix for " + veloStream.ShortName + " is not avaialble!"); continue; } double[,] convPts = null; if (type == LiDARTransformType.None) { convPts = ptsMat; } else if (type == LiDARTransformType.TransformToPlatform) { convPts = veloStream.Tp.Dot(ptsMat); } else if (type == LiDARTransformType.TransformToGlobal) { throw new NotImplementedException(); } var hotFrameSubfix = timeStamp.ToString("HHmmss"); var saveTo = this.Project.Folder + "\\" + Project.LiDARFrameFolder + "\\LiDAR_" + hotFrameSubfix + "_" + veloStream.ShortName + ".txt"; TextWriter tw = new StreamWriter(saveTo); for (int i = 0; i < convPts.Columns(); i++) { for (int j = 0; j < 3; j++) { tw.Write(convPts[j, i] + " "); } tw.Write(pts[i].Intensity); tw.WriteLine(); } tw.Close(); } } }