SimToStreams.cpp

/*
 * Copyright (C) 2024: Arizona Board of Regents on Behalf of the University of Arizona
 */

 /**
  * @file SimToStreams.cpp
  * @brief Apache Strap-Down Pilotage program that reads the configuration, images, and motion from a Blender simulation
  * and writes Streamfiles for reading into StorageServer.
  *
 * @author ReliaSolve.
 * @date July 17th, 2024.
 */

#include <random>
#include <iostream>
#include <string>
#include <vector>
#include <array>
#include <fstream>
#include <sstream>
#include <filesystem>
#include <ASDP_Core_API.h>
#include <nlohmann/json.hpp>
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#define GLM_ENABLE_EXPERIMENTAL
#include <glm/gtx/matrix_decompose.hpp>
#include <glm/gtc/type_ptr.hpp>
#include "PNMImage.h"
#include "CameraConfig.h"
#include "ImageResampler.h"
#include "AddNoise.h"
#include <Vignette.h>

using namespace asdp;
using json = nlohmann::json;

static std::string VERSION = "3.2.0";

static double g_fps = 60.0;
static uint32_t g_cameraType = 0;

class SubdirectoryForNextFrame {
public:
  /// @param rootDirectory The root directory where the subdirectories are stored.
  SubdirectoryForNextFrame(std::string rootDirectory);
  /// @brief get the directory name for the next frame ID, returning an empty string if there are no more frames.
  /// Increments the frame ID for the next call.
  std::string GetNextDirectory();

  /// @brief Peek at the next directory without incrementing the frame ID.
  std::string PeekNextDirectory() const;

protected:
  std::string m_rootDirectory;
  int m_previousDirectory;
  int m_frameID;
};

SubdirectoryForNextFrame::SubdirectoryForNextFrame(std::string rootDirectory)
  : m_rootDirectory(rootDirectory), m_previousDirectory(0), m_frameID(0)
{
}

std::string SubdirectoryForNextFrame::GetNextDirectory()
{
  // See if we can find the CSV file for this frame in the previously-used directory.
  std::string directory = m_rootDirectory + "/" + std::to_string(m_previousDirectory);
  std::string csvName = directory + "/Pose_frame_" + std::to_string(m_frameID) + ".csv";
  if (std::filesystem::exists(csvName)) {
    // Next frame next time.
    ++m_frameID;
    return directory;
  }

  // Increment the directory and see if we can find it there.
  m_previousDirectory++;
  directory = m_rootDirectory + "/" + std::to_string(m_previousDirectory);
  csvName = directory + "/Pose_frame_" + std::to_string(m_frameID) + ".csv";
  if (std::filesystem::exists(csvName)) {
    // Next frame next time.
    ++m_frameID;
    return directory;
  }

  // Did not find a new frame, so we're done
  return "";
}

std::string SubdirectoryForNextFrame::PeekNextDirectory() const
{
  std::string directory = m_rootDirectory + "/" + std::to_string(m_previousDirectory);
  std::string csvName = directory + "/Pose_frame_" + std::to_string(m_frameID) + ".csv";
  if (std::filesystem::exists(csvName)) {
    return directory;
  }

  // Increment the directory and see if we can find it there.
  int nextDirectory = m_previousDirectory + 1;
  directory = m_rootDirectory + "/" + std::to_string(nextDirectory);
  csvName = directory + "/Pose_frame_" + std::to_string(m_frameID) + ".csv";
  if (std::filesystem::exists(csvName)) {
    return directory;
  }

  // Did not find a new frame, so we're done
  return "";
}

std::string writeImage(StreamWriter& writer, Time startTime, double frameInterval, PNMImage& img,
  uint32_t cameraID, uint32_t cameraType, double flicker)
{
  // Compute the square root of the flicker factor to use to scale both gain and exposure separately.
  double sqrtFlicker = std::sqrt(flicker);

  // Loop through and send rows grouped into as many as will fit into one packet.
  uint32_t rowsPerPacket = 8500 / (img.width() * sizeof(uint16_t));
  // At most rowsPerPacket rows per slice, but be sure to capture any partial write.
  uint32_t numSlices = (img.height() + (rowsPerPacket - 1)) / rowsPerPacket;
  // Period gap to send each slice in seconds to be done before next
  uint32_t sliceTimeMicroseconds = uint32_t(1e6 * frameInterval / (numSlices + 1));
  for (uint16_t i = 0; i < numSlices; i++) {
    // Don't send past the end of the image.
    uint32_t firstLine = i * rowsPerPacket;
    uint32_t lastLine = (i + 1) * rowsPerPacket - 1;
    if (lastLine >= img.height()) {
      lastLine = img.height() - 1;
    }
    Time sendTime = startTime + Time(0, i * sliceTimeMicroseconds);

    std::shared_ptr<StreamPacket> packet;
    writer.GetCurrentPacket(packet);
    bool beginFrame = (i == 0);
    bool endFrame = (i == numSlices - 1);
    // If we're the first slice, we send the pixel read time of the first pixel in the image.
    Time myTime = sendTime;
    if (beginFrame) { myTime = startTime; }
    MessageConsolidatedFrameData message(*packet, myTime, cameraID, cameraType,
      img.width(), img.height(),
      0, firstLine, img.width()-1, lastLine,
      beginFrame, endFrame,
      reinterpret_cast<uint8_t*>(img.data()), img.width(),
      0, 0,
      startTime, static_cast<uint32_t>(frameInterval * 1e6));
    if (message.GetConstructorStatus() != OKAY) {
      return "Error constructing MessageImageRow: " + ErrorMessage(message.GetConstructorStatus());
    }
    writer.Flush();
  }

  return "";
}

std::string writePoseMessage(StreamWriter& writer, Time time,
  std::array<float, 3> pos, std::array<float, 3> orient, std::array<float, 3> velocity,
  std::array<float, 3> angularVelocity)
{
  std::shared_ptr<StreamPacket> packet;
  Status status = writer.GetCurrentPacket(packet);
  if (status != OKAY) {
    return "Error getting current packet: " + ErrorMessage(status);
  }

  // Convert the position into lat/lon/alt based on (0,0,0) world origin.  Because we're at the
  // origin, we don't need to adjust the longitude scale based on latitude.
  double xKilometers = pos[0] / 1000.0;
  double yKilometers = pos[1] / 1000.0;
  const double kilometersToDegrees = 360.0 / 40075.0;
  double lat = yKilometers * kilometersToDegrees, lon = xKilometers * kilometersToDegrees, alt = pos[2];
  MessagePose message(*packet, time, lat, lon, alt, orient, velocity, angularVelocity);
  if (message.GetConstructorStatus() != OKAY) {
    return "Error constructing pose message: " + ErrorMessage(message.GetConstructorStatus());
  }

  // Because we're not moving very far in the Earth scale in the simulation, we don't need to adjust the
  // orientation based on the curvature of the Earth.

  status = writer.Flush();
  if (status != OKAY) {
    return "Error flushing StreamWriter: " + ErrorMessage(status);
  }

  return "";
}

std::string writeStateAndClockSyncMessages(StreamWriter &writer, Time time, std::vector<CameraConfig> &cameraConfigs)
{
  std::shared_ptr<StreamPacket> packet;
  Status status = writer.GetCurrentPacket(packet);
  if (status != OKAY) {
    return "Error getting current packet: " + ErrorMessage(status);
  }

  // Fill in the data and then construct the message.
  std::vector<FeatureID> features = {
    FeatureID::STORAGE_API_AVAILABLE,
    FeatureID::POSE_API_ORIENTATION_AVAILABLE,
    FeatureID::POSE_API_POSITION_AVAILABLE
  };

  std::vector<CameraInfo> cameraInfos;
  for (auto& cameraConfig : cameraConfigs) {
    CameraInfo info;
    info.type = g_cameraType;
    info.width = cameraConfig.resolution[0];
    info.height = cameraConfig.resolution[1];
    info.minTriggerPeriod = 1 / g_fps;
    info.maxTriggerPeriod = 1 / g_fps;
    info.trigger = 0;
    cameraInfos.push_back(info);
  }

  std::vector<TriggerInfo> triggerInfos;
  TriggerInfo triggerInfo;
  triggerInfo.ID = 1;
  triggerInfo.mode = 1;
  triggerInfo.externalID = 0;
  triggerInfo.period = 1 / g_fps;
  triggerInfo.offset = 0;
  triggerInfo.trackingFactor = 1;
  triggerInfos.push_back(triggerInfo);

  MessageState message(*packet, time,
    features, cameraInfos,
    0, 0,
    false, false, false, false,
    false,
    triggerInfos,
    0, 0,
    Time());
  if (message.GetConstructorStatus() != OKAY) {
    return "Error constructing state message: " + ErrorMessage(message.GetConstructorStatus());
  }

  MessageEvent message2(*packet, time, 0, CLOCK_SYNC, "");
  if (message2.GetConstructorStatus() != OKAY) {
    return "Error constructing MessageClockSync: " + ErrorMessage(message2.GetConstructorStatus());
  }

  status = writer.Flush();
  if (status != OKAY) {
    return "Error flushing StreamWriter: " + ErrorMessage(status);
  }

  return "";
}

/// @brief Convert a velocity vector specified in the helicopter frame of reference to the camera frame of reference.
/// @param vel Velocity vector in the helicopter frame of reference.
/// @param orient Orientation taking the helicopter frame of reference to the camera frame of reference.
/// Rotation first around the X axis, then the new Y, then the new Z.
/// @return Velocity vector in the camera frame of reference.
std::array<float, 3> RotateVelocity(std::array<float, 3> vel, std::array<float, 3> orient)
{
  // Convert the orientation to radians.
  std::array<float, 3> orientRadians = { glm::radians(orient[0]), glm::radians(orient[1]), glm::radians(orient[2]) };

  // Start with an identity matrix
  glm::mat4 rotationMatrix = glm::mat4(1.0f);

  // Apply rotation around the X-axis
  rotationMatrix = glm::rotate(rotationMatrix, orientRadians[0], glm::vec3(1.0f, 0.0f, 0.0f));

  // Apply rotation around the new Y-axis
  rotationMatrix = glm::rotate(rotationMatrix, orientRadians[1], glm::vec3(0.0f, 1.0f, 0.0f));

  // Apply rotation around the new Z-axis
  rotationMatrix = glm::rotate(rotationMatrix, orientRadians[2], glm::vec3(0.0f, 0.0f, 1.0f));

  // Get the inverse rotation so that we find the vector that when rotated by the
  // orientation difference produces the original vector in helicopter space.
  rotationMatrix = glm::inverse(rotationMatrix);

  // Find the velocity in the camera frame of reference.
  glm::vec3 velocityInCameraFrame = glm::vec3(
    rotationMatrix * glm::vec4(vel[0], vel[1], vel[2], 1.0f));

  return { velocityInCameraFrame.x, velocityInCameraFrame.y, velocityInCameraFrame.z };
}

/// @brief Convert an Euler rotation specified in the helicopter frame of reference to the camera frame of reference.
/// @param ang Angular velocity in the helicopter frame of reference.
/// @param orient Orientation taking the helicopter frame of reference to the camera frame of reference.
/// Rotation first around the X axis, then the new Y, then the new Z.
/// @return Angular velocity in the camera frame of reference.
std::array<float, 3> RotateAngularVelocity(std::array<float, 3> ang, std::array<float, 3> orient)
{
  // Treating the axes independently may swap the order of the rotations.  We
  // must determine the correct actual rotation in the camera frame of reference and then
  // turn that into the equivalent Euler angles.

  //==============================================================================
  // Because we may have high angular velocities, we scale the velocity down on
  // ingest and back up on return to avoid wrapping around the angles.
  float scale = 1.0f / 1000.0f;

  //==============================================================================
  // We first construct a rotation matrix that operates in the helicopter frame of reference
  // from the incoming orientation Euler angles.

  // Convert the orientation to radians.
  std::array<float, 3> angRadians = {
    scale * glm::radians(ang[0]), scale * glm::radians(ang[1]), scale * glm::radians(ang[2]) };

  // Start with an identity matrix then rotate about each axis in turn.
  glm::mat4 diffInHel = glm::mat4(1.0f);
  diffInHel = glm::rotate(diffInHel, angRadians[0], glm::vec3(1.0f, 0.0f, 0.0f));
  diffInHel = glm::rotate(diffInHel, angRadians[1], glm::vec3(0.0f, 1.0f, 0.0f));
  diffInHel = glm::rotate(diffInHel, angRadians[2], glm::vec3(0.0f, 0.0f, 1.0f));

  //==============================================================================
  // We then convert the rotation matrix to the camera frame of reference by rotating
  // it on the left by the helicopter-to-camera transform and to the right by the
  // camera-to-helicoper transform (inverse of helipter-to-camera).

  // Convert the orientation to radians.
  std::array<float, 3> orientRadians = { glm::radians(orient[0]), glm::radians(orient[1]), glm::radians(orient[2]) };

  // Start with an identity matrix then rotate about each axis in turn.  Then invert to
  // go from a matrix that rotates points to a coordinate transformation matrix.
  glm::mat4 helToCamera = glm::mat4(1.0f);
  helToCamera = glm::rotate(helToCamera, orientRadians[0], glm::vec3(1.0f, 0.0f, 0.0f));
  helToCamera = glm::rotate(helToCamera, orientRadians[1], glm::vec3(0.0f, 1.0f, 0.0f));
  helToCamera = glm::rotate(helToCamera, orientRadians[2], glm::vec3(0.0f, 0.0f, 1.0f));
  helToCamera = glm::inverse(helToCamera);

  // Get the inverse rotation so that we find the vector that when rotated by the
  // orientation difference produces the original vector in helicopter space.
  glm::mat4 cameraToHel = glm::inverse(helToCamera);

  glm::mat4 diffInCam = helToCamera * diffInHel * cameraToHel;

  //==============================================================================
  // Finally, we convert this differential rotation into Euler angles in the
  // order (X, Y, Z) to get the angular velocity in the camera frame of reference.
  // GLM gives us rotations in order Z, Y, X but we want X, Y, Z.  We make use of
  // the fact that an inverse rotation matrix is the same as doing three individual
  // rotations in the opposite directions and order.  So we find the inverse of the matrix
  // that we want, then ask for Euler angles from that and then negate them.

  glm::mat4 inverseRotation = glm::inverse(diffInCam);

  // Decompose the combined rotation matrix to get the Euler angles
  glm::vec3 ignscale, igntranslation, ignskew;
  glm::vec4 ignperspective;
  glm::quat orientation;
  glm::decompose(inverseRotation, ignscale, orientation, igntranslation, ignskew, ignperspective);

  // Convert quaternion to Euler angles (X, Y, Z) in degrees
  glm::vec3 eulerAngles = glm::degrees(glm::eulerAngles(orientation));

  // We also have to invert the original down-scaling factor
  return { -eulerAngles.x / scale, -eulerAngles.y / scale, -eulerAngles.z / scale };
}

bool isNear(std::array<float, 3> a, std::array<float, 3> b, float epsilon = 0.05f)
{
  for (int i = 0; i < 3; ++i) {
    if (std::abs(a[i] - b[i]) > epsilon) {
      return false;
    }
  }
  return true;
}

static std::string TestRV(std::array<float, 3> vel, std::array<float, 3> orient, std::array<float, 3> expected)
{
  std::array<float, 3> result = RotateVelocity(vel, orient);
  if (!isNear(result, expected)) {
    return "Error: RotateVelocity test failed for ("
      + std::to_string(vel[0]) + ", " + std::to_string(vel[1]) + ", " + std::to_string(vel[2]) + ") in helicopter space to ("
      + std::to_string(expected[0]) + ", " + std::to_string(expected[1]) + ", " + std::to_string(expected[2]) + ") in camera space: ("
      + std::to_string(result[0]) + ", " + std::to_string(result[1]) + ", " + std::to_string(result[2])
      + ").  Helicopter to camera rotation is ("
      + std::to_string(orient[0]) + ", " + std::to_string(orient[1]) + ", " + std::to_string(orient[2]) + ")";
  }
  return "";
}

std::string TestRotateVelocity()
{
  // The velocity in the (1,0,0) direction in helicopter space converted into a camera
  // whose space is rotated 90 degrees around the Z axis should be (0,-1,0) in camera space.
  std::string ret;
  ret = TestRV({ 1, 0, 0 }, { 0, 0, 90 }, { 0, -1, 0 });
  if (!ret.empty()) {
    return ret;
  }

  // The velocity the (0,1,0) direction in helicopter space converted into a camera
  // whose space is rotated 90 degrees around the Z axis should be (1,0,0) in camera space.
  ret = TestRV({ 0, 1, 0 }, { 0, 0, 90 }, { 1, 0, 0 });
  if (!ret.empty()) {
    return ret;
  }

  // The velocity the (0,0,1) direction in helicopter space converted into a camera
  // whose space is rotated 90 degrees around the Z axis should be (0,0,1) in camera space.
  ret = TestRV({ 0, 0, 1 }, { 0, 0, 90 }, { 0, 0, 1 });
  if (!ret.empty()) {
    return ret;
  }

  // The velocity (1,0,0) in helicopter space converted into a camera whose space is rotated
  // 90 degrees around the Y axis and then -90 degress about the Z axis should be (0,0,1) in camera space.
  ret = TestRV({ 1, 0, 0 }, { 0, 90, -90 }, { 0, 0, 1 });
  if (!ret.empty()) {
    return ret;
  }

  return "";
}

static std::string TestRAV(std::array<float, 3> ang, std::array<float, 3> orient, std::array<float, 3> expected)
{
  std::array<float, 3> result = RotateAngularVelocity(ang, orient);
  if (!isNear(result, expected)) {
    return "Error: RotateAngularVelocity test failed for ("
      + std::to_string(ang[0]) + ", " + std::to_string(ang[1]) + ", " + std::to_string(ang[2]) + ") in helicopter space to ("
      + std::to_string(expected[0]) + ", " + std::to_string(expected[1]) + ", " + std::to_string(expected[2]) + ") in camera space: ("
      + std::to_string(result[0]) + ", " + std::to_string(result[1]) + ", " + std::to_string(result[2])
      + ").  Helicopter to camera rotation is ("
      + std::to_string(orient[0]) + ", " + std::to_string(orient[1]) + ", " + std::to_string(orient[2]) + ")";
  }
  return "";
}

std::string TestRotateAngularVelocity()
{
  std::string ret;

  // The angular velocity (90,0,0) in helicopter space converted into a camera
  // whose space is rotated 90 degrees around the Z axis should be (0,-90,0) in camera space.
  ret = TestRAV({ 90, 0, 0 }, { 0, 0, 90 }, { 0, -90, 0 });
  if (!ret.empty()) {
    return ret;
  }

  // The angular velocity (90,0,0) in helicopter space converted into a camera
  // whose space is rotated to make X point up and Y forward should be (0,0,-90) in camera space.
  ret = TestRAV({ 90, 0, 0 }, { 90, -90, 90 }, { 0, 0, -90 });
  if (!ret.empty()) {
    return ret;
  }

  // The angular velocity (0,0,90) in helicopter space converted into a camera
  // whose space is rotated to make X point up and Y forward should be (90,0,0) in camera space.
  ret = TestRAV({ 0, 0, 90 }, { 90, -90, 90 }, { 90, 0, 0 });
  if (!ret.empty()) {
    return ret;
  }

  // The angular velocity (0,0,90) in helicopter space converted into a camera
  // whose space is rotated to make X point down and Y forward should be (-90,0,0) in camera space.
  ret = TestRAV({ 0, 0, 90 }, { 90, 90, -90 }, { -90, 0, 0 });
  if (!ret.empty()) {
    return ret;
  }

  // The angular velocity (0,0,90) in helicopter space converted into a camera
  // whose space is rotated to make X point up and Y to the side should be (90,0,0) in camera space.
  ret = TestRAV({ 0, 0, 90 }, { 90, -63, 90 }, { 90, 0, 0 });
  if (!ret.empty()) {
    return ret;
  }

  return "";
}

/// @brief Read and parse a CSV file containing camera pose data.
/// @param csvName The name of the CSV file to read.
/// @param pos Output array for the position data.
/// @param orient Output array for the orientation data.
/// @return String describing the error, or an empty string on success.
static std::string readPoseFromCSV(const std::string& csvName, std::array<float, 3>& pos, std::array<float, 3>& orient)
{
  std::ifstream csvFile(csvName);
  if (!csvFile) {
    return "Error: Could not open CSV file: " + csvName;
  }
  std::string line;
  std::getline(csvFile, line); // Skip the header line
  std::getline(csvFile, line);
  std::istringstream lineStream(line);
  std::string value;
  for (int i = 0; i < 3; ++i) {
    if (!std::getline(lineStream, value, ',')) {
      return "Error: Expected 3 values in CSV file for position: " + csvName;
    }
    pos[i] = std::stof(value);
  }
  for (int i = 0; i < 3; ++i) {
    if (!std::getline(lineStream, value, ',')) {
      return "Error: Expected 3 values in CSV file for orientation: " + csvName;
    }
    orient[i] = std::stof(value);
  }
  return "";
}

std::string doConversions(std::string rootDirectory, std::string outDirectory, int serialNumber,
  std::vector<CameraConfig> &cameraConfigs, int verbosity, bool dumpResampledImages, bool doVelocityWarping,
  double sigma = 0.0, double flicker = 1.0, bool fixEndianness = true)
{
  // Open the stream files in the output directory, creating the directory if it does not exist.

  // Make the subdirectory based on our serial number and make it if it does not exist.
  std::string serialDirectory = outDirectory + "/" + std::to_string(serialNumber);
  std::filesystem::create_directories(serialDirectory);
  if (!std::filesystem::exists(serialDirectory)) {
    return "Error: Could not create directory: " + serialDirectory;
  }

  // Find the lowest-numbered unused ID starting with 1 and incrementing by 1 and make a subdirectory.
  int id = 1;
  while (std::filesystem::exists(serialDirectory + "/" + std::to_string(id))) {
    ++id;
  }
  serialDirectory += "/" + std::to_string(id);
  std::filesystem::create_directories(serialDirectory);
  if (!std::filesystem::exists(serialDirectory)) {
    return "Error: Could not create directory: " + serialDirectory;
  }

  if (verbosity > 0) {
    std::cout << "Writing stream files to: " << serialDirectory << std::endl;
  }

  // We then find the names of the files we need to write by appending "stream#.dat" to the directory name,
  // where # goes from 0 to the number of cameras 1 (the 0th stream is the main stream 1+ are for the cameras).
  std::vector< std::shared_ptr<SenderFile> > streamFiles;
  streamFiles.push_back(std::make_shared<SenderFile>(serialDirectory + "/stream0.dat", false));
  for (int i = 0; i < cameraConfigs.size(); ++i) {
    streamFiles.push_back(std::make_shared<SenderFile>(serialDirectory + "/stream" +
      std::to_string(cameraConfigs[i].id) + ".dat", false));
  }

  // Construct StreamWriter objects for each stream file.
  std::vector< std::shared_ptr<StreamWriter> > streamWriters;
  for (auto& streamFile : streamFiles) {
    streamWriters.push_back(std::make_shared<StreamWriter>(streamFile));
  }

  // Time starts at 1 second to avoid going into the past.  We increment it with each frame that we process.
  double currentTime = 1;

  // Make a uniform random number generator for the noise with the interval from flicker to 1.0.
  std::random_device rd;
  std::mt19937 gen(rd());
  std::uniform_real_distribution<double> dis(flicker, 1.0);

  // Now we need to read the simulation files and write the data to the stream files, updating the clock as we go.
  std::array<float, 3> lastPos = {}, lastOrient = {};
  bool gotLastPose = false;
  double lastTime = currentTime;
  size_t frameID = 0;
  SubdirectoryForNextFrame subdirectory(rootDirectory);
  do {
    // Get the next subdirectory for the next frame.
    std::string directory = subdirectory.GetNextDirectory();
    if (directory.empty()) {
      // Done!
      break;
    }
    if (verbosity >= 1000) {
      std::cout << "  Processing frame " << frameID << " in directory " << directory << std::endl;
    }

    // Read and parse the CSV file, skipping the header line and reading the 3-valued position and orientation from the next.
    std::string csvName = directory + "/Pose_frame_" + std::to_string(frameID) + ".csv";
    std::array<float, 3> pos, orient;
    std::string ret = readPoseFromCSV(csvName, pos, orient);
    if (!ret.empty()) {
      return ret;
    }
    if (verbosity >= 2000) {
      std::cout << "    Position: " << pos[0] << ", " << pos[1] << ", " << pos[2] << std::endl;
      std::cout << "    Orientation: " << orient[0] << ", " << orient[1] << ", " << orient[2] << std::endl;
    }

    // Read and parse the next frame's pose if we have one, so that we have an unbiased estimate.
    std::array<float, 3> nextPos = pos, nextOrient = orient;
    std::string nextCsvName = subdirectory.PeekNextDirectory() + "/Pose_frame_" + std::to_string(frameID + 1) + ".csv";
    if (std::filesystem::exists(nextCsvName)) {
      ret = readPoseFromCSV(nextCsvName, nextPos, nextOrient);
      if (!ret.empty()) {
        return ret;
      }
      if (verbosity >= 2000) {
        std::cout << "    Next Position: " << nextPos[0] << ", " << nextPos[1] << ", " << nextPos[2] << std::endl;
        std::cout << "    Next Orientation: " << nextOrient[0] << ", " << nextOrient[1] << ", " << nextOrient[2] << std::endl;
      }
    }

    // Estimate the positional and angular velocity.
    std::array<float, 3> velocity = {};
    std::array<float, 3> angularVelocity = {};

    // Compute velocities and write pose message to the main stream file.
    if (gotLastPose) {
      // Scale velocity up to a whole second by multiplying the frame step by frames/second
      for (int i = 0; i < 3; ++i) {
        // Get an unbiased estimate by dividing the distance from the last position to the next by 2.
        velocity[i] = (nextPos[i] - lastPos[i]) / 2 * static_cast<float>(g_fps);
      }
      for (int i = 0; i < 3; ++i) {
        // Keep the angular velocity in the range -180 to 180, we assume that we never go fast enough
        // to wrap around and we need to avoid large jumps when we go past 360 degrees.
        float delta = nextOrient[i] - lastOrient[i];
        while (delta > 180) {
          delta -= 360;
        }
        while (delta < -180) {
          delta += 360;
        }
        // Get an unbiased estimate by dividing the distance from the last orientation to the next by 2.
        angularVelocity[i] = (delta/2) * static_cast<float>(g_fps);
      }
      std::string ret = writePoseMessage(*streamWriters[0], static_cast<float>(currentTime),
        pos, orient, velocity, angularVelocity);
      if (!ret.empty()) {
        return "Could not store state and clock-sync messages: " + ret;
      }
    }
    lastPos = pos;
    lastOrient = orient;
    gotLastPose = true;

    // Write state and clock-sync messages to the main stream file.
    ret = writeStateAndClockSyncMessages(*streamWriters[0], static_cast<float>(currentTime), cameraConfigs);
    if (!ret.empty()) {
      return "Could not store state and clock-sync messages: " + ret;
    }

    // Read an image for each camera.  If there is flicker or offset is not 0 or the gain is not 1, apply them to the image.
    std::vector< std::shared_ptr<PNMImage> > images;
    std::map<int, double> flickers;
    for (auto& cameraConfig : cameraConfigs) {
      std::string imageName = directory + "/Cam" + std::to_string(cameraConfig.id) + "_frame_" + std::to_string(frameID) + ".pgm";
      std::shared_ptr<PNMImage> image = std::make_shared<PNMImage>(imageName, fixEndianness);
      if (!image->width()) {
        return "Error: Could not read image file: " + imageName;
      }

      // Apply vignette correction to the image.
      std::array<double, 2> doubleFOVs = { cameraConfig.fieldOfView[0], cameraConfig.fieldOfView[1] };
      asdp::render::VignetteRadialPolynomail vignette(cameraConfig.vignetteCenter, doubleFOVs,
        cameraConfig.vignetteCoefficients);
      for (uint16_t y = 0; y < image->height(); ++y) {
        // Get Y in range -1 to 1, inverting because the image is stored top to bottom.
        double normY = 2 * static_cast<double>(y) / static_cast<double>(image->height() - 1) - 1;
        normY *= -1;
        for (uint16_t x = 0; x < image->width(); ++x) {
          // Get X in range -1 to 1
          double normX = 2 * static_cast<double>(x) / static_cast<double>(image->width() - 1) - 1;

          uint16_t value = (*image)(x, y);

          // Scale the value by the inverse of the vignette correction to apply it
          double scaled_value = static_cast<double>(value) / vignette.EvaluateAtPoint({normX, normY});

          // Round the result and clamp the result to the range of uint16_t
          scaled_value = std::clamp(std::round(scaled_value), 0.0, 65535.0);

          // Store the result back into the image.
          (*image)(x, y) = static_cast<uint16_t>(scaled_value);
        }
      }

      // If our flicker factor is non-1.0, apply random flicker to the image and remember the value for when
      // we write the image.
      double thisFlicker = 1.0;
      if (flicker != 1.0) {
        thisFlicker = dis(gen);
        if (verbosity >= 1500) {
          std::cout << "      Applying flicker " << thisFlicker << " to image" << std::endl;
        }
        for (uint16_t y = 0; y < image->height(); ++y) {
          for (uint16_t x = 0; x < image->width(); ++x) {
            uint16_t value = (*image)(x, y);

            // Scale the value by flicker
            double scaled_value = static_cast<double>(value) * thisFlicker;

            // Round the result and clamp the result to the range of uint16_t
            scaled_value = std::clamp(std::round(scaled_value), 0.0, 65535.0);

            // Store the result back into the image.
            (*image)(x, y) = static_cast<uint16_t>(scaled_value);
          }
        }
      }
      flickers[cameraConfig.id] = thisFlicker;

      if (cameraConfig.offset != 0 || cameraConfig.gain != 1) {
        // Apply the inverse mapping by offset and gain to the image so that applying the offset and gain will return
        // to the original value.
        if (verbosity >= 1500) {
          std::cout << "    Applying offset " << -cameraConfig.offset << " and gain " << 1 / cameraConfig.gain << " to image " << imageName << std::endl;
        }
        size_t count = image->width() * image->height();
        for (size_t i = 0; i < count; ++i) {
          float value = static_cast<float>(image->data()[i]);
          value = value/cameraConfig.gain - cameraConfig.offset;
          if (value < 0) {
            value = 0;
          }
          if (value > 65535) {
            value = 65535;
          }
          image->data()[i] = static_cast<uint16_t>(value);
        }
      }

      images.push_back(image);
      if (verbosity >= 2000) {
        std::cout << "    Image: " << imageName << ", size " << images.back()->width() << "x" << images.back()->height() << std::endl;
      }
    }

    // Resample each image and write it to the appropriate stream file.
    std::string broken;
#pragma omp parallel for
    for (int i = 0; i < cameraConfigs.size(); ++i) {

      // Convert the velocity and angular velocity to the camera frame of reference.
      std::array<float, 3> velocityInCameraFrame = RotateVelocity(velocity, cameraConfigs[i].orientation);
      std::array<float, 3> angleVelInCameraFrame = RotateAngularVelocity(angularVelocity, cameraConfigs[i].orientation);

      if (!doVelocityWarping) {
        std::array<float, 3> zero = { 0.0f,0.0f,0.0f };
        velocityInCameraFrame = zero;
        angleVelInCameraFrame = zero;
      }

      std::shared_ptr<PNMImage> resampledImage = Resample_Image(*images[i], cameraConfigs[i],
        1 / static_cast<float>(g_fps), angleVelInCameraFrame);
      if (!resampledImage) {
        broken = "Error resampling image for camera " + std::to_string(cameraConfigs[i].id);
      }
      if (verbosity >= 4000) {
        std::cout << "      Resampled image " << cameraConfigs[i].id << " to size " << resampledImage->width() << "x" << resampledImage->height() << std::endl;
      }

      // If our noise sigma is non-zero, add noise to the image.
      if (sigma > 0.0) {
        if (verbosity >= 1500) {
          std::cout << "      Adding noise " << sigma << " to resampled image" << std::endl;
        }
        resampledImage = Add_Noise(*resampledImage, sigma);
      }

      // Back-date the render start time by half a frame so that the frame center is at the time of the pose.
      std::string ret = writeImage(*streamWriters[i+1], static_cast<float>(currentTime - (1/g_fps)/2), 1/g_fps,
        *resampledImage, static_cast<uint32_t>(cameraConfigs[i].id), 0, flickers[cameraConfigs[i].id]);
      if (!ret.empty()) {
        broken = "Error writing image to stream file: " + ret;
      }

      // If we're dumping the resampled images, write them to the input directory with "_resampled.pgm" appended
      // to their names.
      if (dumpResampledImages) {
        std::string resampledName = directory + "/Cam" + std::to_string(cameraConfigs[i].id) + "_frame_" + std::to_string(frameID) + "_resampled.pgm";
        if (!resampledImage->Write(resampledName)) {
          broken = "Error writing resampled image to file: " + resampledName;
        }
        if (verbosity >= 4000) {
          std::cout << "      Wrote resampled image to " << resampledName << std::endl;
        }
      }
    }
    if (!broken.empty()) {
      return broken;
    }

    // Next frame
    frameID++;
    lastTime = currentTime;
    currentTime += 1 / g_fps;
  } while (true);

  // Clear the stream writers and stream files to close them.
  streamWriters.clear();
  streamFiles.clear();

  return "";
}

void usage(const std::string& programName)
{
  std::cout << "Usage: " << programName << " [options] rootDirectory outDirectory\n"
    << "rootDirectory is the directory where the simulation files are stored\n"
    << "outDirectory is the directory where the output files will be stored\n"
    << "\n"
    << "Options:\n"
    << "  --fps <number>            Specify the frames per second (default 60)\n"
    << "  --verbosity <number>      Specify the verbosity (default 1)\n"
    << "  --configFile <filename>   Specify the configuration file name (default config.json)\n"
    << "  --dumpResampledImages     Dump the resampled images to the input directory, appending to their names\n"
    << "  --noVelocityWarping       Do not warp based on the velocity and angular velocity to the camera frame of reference\n"
    << "  --sigma <number>          Standard deviation of pixel-counts of noise to add (default 1000.0)\n"
    << "  --flicker <number>        Fraction of per-frame darkening exposure+gain flicker to add (default 1.0, consider 0.8)\n"
    << "  --dontFixEndianness       Do not fix the endianness of the input files\n"
    << "  --help                    Show this help message and exit\n";
}

int main(int argc, char* argv[])
{
  // Default values for command line arguments
  std::string rootDirectory;
  std::string outDirectory;
  std::string configFileName = "config.json";
  int verbosity = 1;
  bool dumpResampledImages = false;
  bool doVelocityWarping = true;
  double sigma = 1000.0;
  double flicker = 1.0;
  bool fixEndianness = true;
  unsigned realParams = 0;

  // Verify that our math functions work.
  std::string ret;
  ret = TestRotateVelocity();
  if (!ret.empty()) {
    std::cerr << "Internal error testing RotateVelocity: " << ret << std::endl;
    return 1;
  }
  ret = TestRotateAngularVelocity();
  if (!ret.empty()) {
    std::cerr << "Internal error testing RotateAngularVelocity: " << ret << std::endl;
    return 1;
  }

  // Parse the command line arguments
  for (int i = 1; i < argc; ++i) {
    std::string arg = argv[i];
    if (arg == "--help") {
      usage(argv[0]);
      return 0;
    } else if (arg == "--dumpResampledImages") {
      dumpResampledImages = true;
    } else if (arg == "--verbosity") {
      if (i + 1 < argc) {
        verbosity = atoi(argv[++i]);
      } else {
        std::cerr << "--verbosity option requires one argument." << std::endl;
        usage(argv[0]);
        return 1;
      }
    } else if (arg == "--fps") {
      if (i + 1 < argc) {
        g_fps = atof(argv[++i]);
      } else {
        std::cerr << "--fps option requires one argument." << std::endl;
        usage(argv[0]);
        return 1;
      }
    } else if (arg == "--configFile") {
      if (i + 1 < argc) {
        configFileName = argv[++i];
      }
      else {
        std::cerr << "--configFile option requires one argument." << std::endl;
        usage(argv[0]);
        return 1;
      }
    }
    else if (arg == "--noVelocityWarping") {
      doVelocityWarping = false;
    }
    else if (arg == "--sigma") {
      if (i + 1 < argc) {
        sigma = atof(argv[++i]);
      }
      else {
        std::cerr << "--sigma option requires one argument." << std::endl;
        usage(argv[0]);
        return 1;
      }
    } else if (arg == "--flicker") {
      if (i + 1 < argc) {
        flicker = atof(argv[++i]);
      }
      else {
        std::cerr << "--flicker option requires one argument." << std::endl;
        usage(argv[0]);
        return 1;
      }
    } else if (arg == "--dontFixEndianness") {
      fixEndianness = false;
    } else if (arg[0] == '-') {
      std::cerr << "Unrecognized flag argument: " << arg << std::endl;
      usage(argv[0]);
      return 1;
    } else switch (++realParams) {
      case 1:
        rootDirectory = arg;
        break;
      case 2:
        outDirectory = arg;
        break;
      default:
        std::cerr << "Unknown option: " << arg << std::endl;
        usage(argv[0]);
        return 1;
    }
  }
  if (realParams != 2) {
    usage(argv[0]);
    return 1;
  }

  if (verbosity > 0) {
    std::cout << "SimToStreams version " << VERSION << std::endl;
  }

  // Read all needed values from the configuration file
  int serialNumber = {};
  std::vector<CameraConfig> cameraConfigs;
  std::ifstream configFile(configFileName);
  if (!configFile) {
    std::cerr << "Error: Could not open configuration file: " << configFileName << std::endl;
    return 2;
  }
  json config = json::parse(configFile);
  try {
    serialNumber = config["serialNumber"];
    if (verbosity > 0) {
      std::cout << "Camera Serial Number: " << serialNumber << std::endl;
    }
    for (json& cameraConfig : config["cameras"]) {
      cameraConfigs.push_back(CameraConfig(cameraConfig));
    }
    if (verbosity > 0) {
      std::cout << "Found " << cameraConfigs.size() << " cameras" << std::endl;
    }
  } catch (std::exception& e) {
    std::cerr << "Error: Could not read configuration file: " << e.what() << std::endl;
    return 3;
  }

  if (verbosity > 0) {
    std::cout << "Root Directory: " << rootDirectory << std::endl;
    std::cout << "Output Directory: " << outDirectory << std::endl;
  }

  // Run the conversions.
  std::string message = doConversions(rootDirectory, outDirectory, serialNumber, cameraConfigs, verbosity,
    dumpResampledImages, doVelocityWarping, sigma, flicker, fixEndianness);
  if (!message.empty()) {
    std::cerr << "Error converting: " << message << std::endl;
    return 4;
  }

  return 0;
}