private string FormatInfo(DatasetFrame frame) { StringBuilder sb = new StringBuilder(); sb.AppendLine(string.Format("Frame {0}", currentFrame)); sb.AppendLine("Translation:"); sb.AppendLine(string.Format("X: {0}", frame.Odometry.Translation[0, 0].Value.ToString("F4"))); sb.AppendLine(string.Format("Y: {0}", frame.Odometry.Translation[1, 0].Value.ToString("F4"))); sb.AppendLine(string.Format("Z: {0}", frame.Odometry.Translation[2, 0].Value.ToString("F4"))); sb.AppendLine("Rotation:"); sb.AppendLine(string.Format("X: {0}", rad2deg(frame.Odometry.Rotation[0, 0].Value).ToString("F4"))); sb.AppendLine(string.Format("Y: {0}", rad2deg(frame.Odometry.Rotation[1, 0].Value).ToString("F4"))); sb.AppendLine(string.Format("Z: {0}", rad2deg(frame.Odometry.Rotation[2, 0].Value).ToString("F4"))); return(sb.ToString()); }
private static Dictionary <int, DatasetFrame> LoadDatasetFrames(string rootDir) { string imgDir = Path.Combine(rootDir, "images"); string camDir = Path.Combine(rootDir, "cameras"); var images = Directory.EnumerateFiles(imgDir); Dictionary <int, DatasetFrame> frames = new Dictionary <int, DatasetFrame>(); foreach (string imgFile in images) { int number = FrameNumber(imgFile); frames[number] = new DatasetFrame() { ImageFile = Path.Combine(imgDir, imgFile), CameraFile = Path.Combine(camDir, number.ToString() + ".txt") }; } return(frames); }
private static void DecomposeTransformationMatrices(Dataset dataset, TimeSpan interval) { DatasetFrame prev = null; foreach (var frame in dataset.Frames) { var translation = new Emgu.CV.Image <Arthmetic, double>(1, 3); for (int i = 0; i < 3; ++i) { translation[i, 0] = frame.TransformationMatrix[i, 3]; } var rotationMatrix = frame.TransformationMatrix.GetSubRect(new Rectangle(0, 0, 3, 3)); var euler = RotationConverter.MatrixToEulerXYZ(rotationMatrix); OdometerFrame odometry = new OdometerFrame() { TimeDiff = interval, Translation = translation, Rotation = euler, Velocity = null, AngularVelocity = null, }; if (prev != null) { odometry.TranslationDiff = odometry.Translation - prev.Odometry.Translation; odometry.RotationDiff = odometry.Rotation - prev.Odometry.Rotation; } else { odometry.TranslationDiff = new Emgu.CV.Image <Arthmetic, double>(1, 3); odometry.RotationDiff = new Emgu.CV.Image <Arthmetic, double>(1, 3); } frame.Odometry = odometry; prev = frame; } }