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