void ViSensorInterface::StartIntegratedSensor(uint32_t image_rate, uint32_t imu_rate)
{

    if (image_rate > 30)
    {
        image_rate = 30;
        std::cout << "Desired image rate is too hight, setting it to 30 Hz." << std::endl;
    }
    if (imu_rate > 800)
    {
        imu_rate = 800;
        std::cout << "Desired imu rate is too hight, setting it to 800 Hz." << std::endl;
    }

    try
    {
        drv_.init();
    }
    catch (visensor::exceptions::ConnectionException const &ex)
    {
        std::cout << ex.what() << "\n";
        return;
    }


    // re-configure camera parameters
    drv_.setSensorConfigParam(visensor::SensorId::SensorId::CAM0, "row_flip", 0);
    drv_.setSensorConfigParam(visensor::SensorId::SensorId::CAM0, "column_flip", 0);

    drv_.setSensorConfigParam(visensor::SensorId::SensorId::CAM1, "row_flip", 1);
    drv_.setSensorConfigParam(visensor::SensorId::SensorId::CAM1, "column_flip", 1);

    drv_.setSensorConfigParam(visensor::SensorId::SensorId::CAM0, "min_coarse_shutter_width", 2);
    drv_.setSensorConfigParam(visensor::SensorId::SensorId::CAM0, "max_coarse_shutter_width", 550);

    drv_.setSensorConfigParam(visensor::SensorId::SensorId::CAM1, "min_coarse_shutter_width", 2);
    drv_.setSensorConfigParam(visensor::SensorId::SensorId::CAM1, "max_coarse_shutter_width", 550);


    // set callback for image messages
    drv_.setCameraCallback(boost::bind(&ViSensorInterface::ImageCallback, this, _1));


    // set callback for IMU messages
    drv_.setImuCallback(boost::bind(&ViSensorInterface::ImuCallback, this, _1));

    drv_.startAllCameras(image_rate);
    drv_.startAllImus(imu_rate);
    vi_sensor_connected_ = true;

    boost::thread drawWorker_0(boost::bind(&ViSensorInterface::worker, this, 0));
    boost::thread drawWorker_1(boost::bind(&ViSensorInterface::worker, this, 1));
    boost::thread drawWorker_2(boost::bind(&ViSensorInterface::worker, this, 2));
    boost::thread drawWorker_3(boost::bind(&ViSensorInterface::worker, this, 3));

    drawWorker_0.join();
    drawWorker_1.join();
    drawWorker_2.join();
    drawWorker_3.join();
}
void ViSensorInterface::StartIntegratedSensor(uint32_t image_rate, uint32_t imu_rate) {

  if (image_rate > 30) {
    image_rate = 30;
    std::cout << "Desired image rate is too hight, setting it to 30 Hz." << std::endl;
  }
  if (imu_rate > 800) {
    imu_rate = 800;
    std::cout << "Desired imu rate is too hight, setting it to 800 Hz." << std::endl;
  }

  try {
    drv_.init();
  } catch (visensor::exceptions::ConnectionException const &ex) {
    std::cout << ex.what() << "\n";
    return;
  }

  std::cout << "Libvisensor version is " << visensor::LIBRARY_VERSION_MAJOR << "."
      << visensor::LIBRARY_VERSION_MINOR << "." << visensor::LIBRARY_VERSION_PATCH << std::endl;

  // load available calibration and configure sensors corresponding to it
  drv_.selectCameraCalibration();

  // set callback for image messages
  drv_.setCameraCallback(boost::bind(&ViSensorInterface::ImageCallback, this, _1));

  // set callback for IMU messages
  drv_.setImuCallback(boost::bind(&ViSensorInterface::ImuCallback, this, _1));

  drv_.startAllCameras(image_rate);
  drv_.startAllImus(imu_rate);
  vi_sensor_connected_ = true;

  boost::thread drawWorker_0(boost::bind(&ViSensorInterface::worker, this, 0));
  boost::thread drawWorker_1(boost::bind(&ViSensorInterface::worker, this, 1));
  boost::thread drawWorker_2(boost::bind(&ViSensorInterface::worker, this, 2));
  boost::thread drawWorker_3(boost::bind(&ViSensorInterface::worker, this, 3));

  drawWorker_0.join();
  drawWorker_1.join();
  drawWorker_2.join();
  drawWorker_3.join();
}