/// <summary>
/// saves path data to file
/// </summary>
/// <param name="filename">filename to save as</param>
/// <param name="path_length_mm">length of the path</param>
/// <param name="start_orientation">orientation at the start of the path</param>
/// <param name="end_orientation">orientation at the end of the path</param>
/// <param name="distance_between_poses_mm">distance between poses</param>
public static void SavePath(
string filename,
float path_length_mm,
float start_orientation,
float end_orientation,
float distance_between_poses_mm,
float disparity,
int no_of_stereo_features,
int no_of_stereo_cameras,
ref List <OdometryData> save_path)
{
string[] str = filename.Split('.');
float start_x_mm = 0;
float start_y_mm = 0;
float x_mm = start_x_mm, y_mm = start_y_mm;
float orientation = start_orientation;
int steps = (int)(path_length_mm / distance_between_poses_mm);
save_path = new List <OdometryData>();
Random rnd = new Random(0);
if (File.Exists(str[0] + "_disparities_index.dat"))
{
File.Delete(str[0] + "_disparities_index.dat");
}
if (File.Exists(str[0] + "_disparities.dat"))
{
File.Delete(str[0] + "_disparities.dat");
}
for (int i = 0; i < steps; i++)
{
for (int cam = 0; cam < no_of_stereo_cameras; cam++)
{
OdometryData data = new OdometryData();
data.orientation = orientation;
data.x = x_mm;
data.y = y_mm;
save_path.Add(data);
List <StereoFeatureTest> features = new List <StereoFeatureTest>();
for (int f = 0; f < no_of_stereo_features; f++)
{
StereoFeatureTest feat;
if (f < no_of_stereo_features / 2)
{
feat = new StereoFeatureTest(20, rnd.Next(239), disparity);
}
else
{
feat = new StereoFeatureTest(300, rnd.Next(239), disparity);
}
feat.SetColour(0, 0, 0);
features.Add(feat);
}
ProcessPose(
str[0],
DateTime.Now,
x_mm, y_mm,
orientation,
0, 0, 0,
cam, features);
}
x_mm += distance_between_poses_mm * (float)Math.Sin(orientation);
y_mm += distance_between_poses_mm * (float)Math.Cos(orientation);
orientation = start_orientation + ((end_orientation - start_orientation) * i / steps);
}
FileStream fs = File.Open(filename, FileMode.Create);
BinaryWriter bw = new BinaryWriter(fs);
bw.Write(save_path.Count);
for (int i = 0; i < save_path.Count; i++)
{
save_path[i].Write(bw);
}
bw.Close();
fs.Close();
// save images of the path
if (filename.Contains("."))
{
int img_width = 640;
int img_height = 480;
byte[] img = new byte[img_width * img_height * 3];
Bitmap bmp = new Bitmap(img_width, img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
filename = str[0] + "_positions.jpg";
float tx = 0, ty = 0;
float bx = 0, by = 0;
ShowPath(
save_path, img, img_width, img_height,
0, 0, 0, true,
ref tx, ref ty,
ref bx, ref by);
BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Jpeg);
}
}
/// <summary>
/// saves path data to file
/// </summary>
/// <param name="filename">filename to save as</param>
/// <param name="path_length_mm">length of the path</param>
/// <param name="start_orientation">orientation at the start of the path</param>
/// <param name="end_orientation">orientation at the end of the path</param>
/// <param name="distance_between_poses_mm">distance between poses</param>
public static void SavePath(
string filename,
float path_length_mm,
float start_orientation,
float end_orientation,
float distance_between_poses_mm,
float disparity,
int no_of_stereo_features,
int no_of_stereo_cameras,
ref List<OdometryData> save_path)
{
string[] str = filename.Split('.');
float start_x_mm = 0;
float start_y_mm = 0;
float x_mm = start_x_mm, y_mm = start_y_mm;
float orientation = start_orientation;
int steps = (int)(path_length_mm / distance_between_poses_mm);
save_path = new List<OdometryData>();
Random rnd = new Random(0);
if (File.Exists(str[0] + "_disparities_index.dat"))
File.Delete(str[0] + "_disparities_index.dat");
if (File.Exists(str[0] + "_disparities.dat"))
File.Delete(str[0] + "_disparities.dat");
for (int i = 0; i < steps; i++)
{
for (int cam = 0; cam < no_of_stereo_cameras; cam++)
{
OdometryData data = new OdometryData();
data.orientation = orientation;
data.x = x_mm;
data.y = y_mm;
save_path.Add(data);
List<StereoFeatureTest> features = new List<StereoFeatureTest>();
for (int f = 0; f < no_of_stereo_features; f++)
{
StereoFeatureTest feat;
if (f < no_of_stereo_features/2)
feat = new StereoFeatureTest(20, rnd.Next(239), disparity);
else
feat = new StereoFeatureTest(300, rnd.Next(239), disparity);
feat.SetColour(0, 0, 0);
features.Add(feat);
}
ProcessPose(
str[0],
DateTime.Now,
x_mm, y_mm,
orientation,
0,0,0,
cam, features);
}
x_mm += distance_between_poses_mm * (float)Math.Sin(orientation);
y_mm += distance_between_poses_mm * (float)Math.Cos(orientation);
orientation = start_orientation + ((end_orientation - start_orientation) * i / steps);
}
FileStream fs = File.Open(filename, FileMode.Create);
BinaryWriter bw = new BinaryWriter(fs);
bw.Write(save_path.Count);
for (int i = 0; i < save_path.Count; i++)
save_path[i].Write(bw);
bw.Close();
fs.Close();
// save images of the path
if (filename.Contains("."))
{
int img_width = 640;
int img_height = 480;
byte[] img = new byte[img_width * img_height * 3];
Bitmap bmp = new Bitmap(img_width, img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
filename = str[0] + "_positions.jpg";
float tx=0, ty=0;
float bx=0, by=0;
ShowPath(
save_path, img, img_width, img_height,
0,0,0, true,
ref tx, ref ty,
ref bx, ref by);
BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Jpeg);
}
}
