diff --git a/include/mynteye/types.h b/include/mynteye/types.h index a6fd97c..a8bb6d4 100644 --- a/include/mynteye/types.h +++ b/include/mynteye/types.h @@ -450,7 +450,7 @@ struct MYNTEYE_API IntrinsicsPinhole : public IntrinsicsBase { double cx; /** The vertical coordinate of the principal point of the image */ double cy; - /** The distortion model of the image */ + /** @deprecated Replaced by calib_model_. The distortion model of the image */ std::uint8_t model; /** The distortion coefficients: k1,k2,p1,p2,k3 */ double coeffs[5]; diff --git a/src/mynteye/device/channel/bytes.cc b/src/mynteye/device/channel/bytes.cc index b0624f9..077632f 100644 --- a/src/mynteye/device/channel/bytes.cc +++ b/src/mynteye/device/channel/bytes.cc @@ -68,9 +68,11 @@ std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data, // cy, 8 in->cy = _from_data(data + i); i += 8; - // model, 1 - in->model = data[i]; - i += 1; + if (get_size) { + // model, 1 + in->model = data[i]; + i += 1; + } // coeffs, 40 for (std::size_t j = 0; j < 5; j++) { in->coeffs[j] = _from_data(data + i + j * 8); @@ -199,9 +201,11 @@ std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data, // cy, 8 _to_data(in->cy, data + i); i += 8; - // model, 1 - data[i] = in->model; - i += 1; + if (set_size) { + // model, 1 + data[i] = in->model; + i += 1; + } // coeffs, 40 for (std::size_t j = 0; j < 5; j++) { _to_data(in->coeffs[j], data + i + j * 8); diff --git a/src/mynteye/device/channel/file_channel.cc b/src/mynteye/device/channel/file_channel.cc index 78d4cf3..5c24777 100644 --- a/src/mynteye/device/channel/file_channel.cc +++ b/src/mynteye/device/channel/file_channel.cc @@ -297,7 +297,8 @@ std::size_t ImgParamsParser::GetFromData_new( if (version == Version(1, 2)) { // v1.2 for (; i < data_size;) { // calib_model, 1 - auto calib_model = static_cast(data[i]); + auto model = data[i]; + auto calib_model = static_cast(model); i += 1; // width, 2 auto width = bytes::_from_data(data + i); @@ -309,14 +310,18 @@ std::size_t ImgParamsParser::GetFromData_new( std::shared_ptr in_left, in_right; Extrinsics ex_right_to_left; switch (calib_model) { - case CalibrationModel::PINHOLE: - in_left = std::make_shared(); - in_right = std::make_shared(); - break; - case CalibrationModel::KANNALA_BRANDT: + case CalibrationModel::PINHOLE: { + auto in_left_p = std::make_shared(); + in_left_p->model = model; + in_left = in_left_p; + auto in_right_p = std::make_shared(); + in_right_p->model = model; + in_right = in_right_p; + } break; + case CalibrationModel::KANNALA_BRANDT: { in_left = std::make_shared(); in_right = std::make_shared(); - break; + } break; default: LOG(FATAL) << "Could not get img params as unknown calib model" ", please use latest SDK."; @@ -421,6 +426,7 @@ std::size_t ImuParamsParser::GetFromData_old( i += bytes::from_data(&imu_params->in_accel, data + i); i += bytes::from_data(&imu_params->in_gyro, data + i); i += bytes::from_data(&imu_params->ex_left_to_imu, data + i); + imu_params->version = spec_version_.to_string(); MYNTEYE_UNUSED(data_size) return i; } diff --git a/src/mynteye/device/device.cc b/src/mynteye/device/device.cc index b1a6295..a539a2f 100644 --- a/src/mynteye/device/device.cc +++ b/src/mynteye/device/device.cc @@ -626,6 +626,7 @@ void Device::ReadAllInfos() { VLOG(2) << "Motion extrinsics left to imu: {" << GetMotionExtrinsics(Stream::LEFT) << "}"; } else { + imu_params_.ok = false; VLOG(2) << "Motion intrinsics & extrinsics not exist"; } } diff --git a/tools/writer/device_writer.cc b/tools/writer/device_writer.cc index f458df3..5fa37dd 100644 --- a/tools/writer/device_writer.cc +++ b/tools/writer/device_writer.cc @@ -21,6 +21,8 @@ #include "mynteye/device/device.h" #include "mynteye/util/files.h" +#define SAVE_LATEST_VERSION Version(1, 2) + MYNTEYE_BEGIN_NAMESPACE namespace tools { @@ -113,12 +115,32 @@ namespace { cv::FileStorage &operator<<(cv::FileStorage &fs, const IntrinsicsPinhole &in) { fs << "{" - << "width" << in.width << "height" << in.height << "fx" << in.fx << "fy" - << in.fy << "cx" << in.cx << "cy" << in.cy << "model" << in.model + << "fx" << in.fx << "fy" << in.fy + << "cx" << in.cx << "cy" << in.cy << "coeffs" << std::vector(in.coeffs, in.coeffs + 5) << "}"; return fs; } +cv::FileStorage &operator<<(cv::FileStorage &fs, + const IntrinsicsEquidistant &in) { + fs << "{" + << "coeffs" << std::vector(in.coeffs, in.coeffs + 8) << "}"; + return fs; +} + +cv::FileStorage &operator<<(cv::FileStorage &fs, + const std::shared_ptr &in) { + switch (in->calib_model()) { + case CalibrationModel::PINHOLE: + return fs << *std::dynamic_pointer_cast(in); + case CalibrationModel::KANNALA_BRANDT: + return fs << *std::dynamic_pointer_cast(in); + default: + LOG(FATAL) << "Unknown calib model: " << in->calib_model(); + return fs; + } +} + cv::FileStorage &operator<<(cv::FileStorage &fs, const ImuIntrinsics &in) { std::vector scales; for (std::size_t i = 0; i < 3; i++) { @@ -150,10 +172,11 @@ cv::FileStorage &operator<<(cv::FileStorage &fs, const Extrinsics &ex) { cv::FileStorage &operator<<( cv::FileStorage &fs, const device::img_params_t ¶ms) { fs << "{" - << "in_left" - << *std::dynamic_pointer_cast(params.in_left) - << "in_right" - << *std::dynamic_pointer_cast(params.in_right) + << "model" << static_cast(params.in_left->calib_model()) + << "width" << params.in_left->width + << "height" << params.in_left->height + << "in_left" << params.in_left + << "in_right" << params.in_right << "ex_right_to_left" << params.ex_right_to_left << "}"; return fs; } @@ -162,8 +185,9 @@ cv::FileStorage &operator<<( cv::FileStorage &fs, const DeviceWriter::img_params_map_t &img_params_map) { fs << "["; std::map::const_iterator it; - for (it = img_params_map.begin(); it != img_params_map.end(); it++) + for (it = img_params_map.begin(); it != img_params_map.end(); it++) { fs << (*it).second; + } fs << "]"; return fs; } @@ -186,34 +210,57 @@ bool DeviceWriter::SaveDeviceInfo( fs << "lens_type" << info.lens_type.to_string(); fs << "imu_type" << info.imu_type.to_string(); fs << "nominal_baseline" << info.nominal_baseline; + // save other infos according to spec_version fs.release(); return true; } bool DeviceWriter::SaveImgParams( - const dev_info_t &info, const img_params_map_t &img_params_map, + const img_params_map_t &img_params_map, const std::string &filepath) { + if (img_params_map.empty()) { + return false; + } + std::string version = img_params_map.begin()->second.version; + if (Version(version) > SAVE_LATEST_VERSION) { + LOG(ERROR) << "Failed to save img params of version " << version + << ", please use latest SDK."; + return false; + } + + // always save img params with latest version format using FileStorage = cv::FileStorage; FileStorage fs(filepath, FileStorage::WRITE); if (!fs.isOpened()) { LOG(ERROR) << "Failed to save file: " << filepath; return false; } - fs << "version" << info.spec_version.to_string(); - fs << "img_params_map" << img_params_map; + fs << "version" << SAVE_LATEST_VERSION.to_string() + << "img_params" << img_params_map; fs.release(); return true; } bool DeviceWriter::SaveImuParams( const imu_params_t ¶ms, const std::string &filepath) { + if (!params.ok) return false; + std::string version = params.version; + if (Version(version) > SAVE_LATEST_VERSION) { + LOG(ERROR) << "Failed to save imu params of version " << version + << ", please use latest SDK."; + return false; + } + + // always save imu params with latest version format using FileStorage = cv::FileStorage; FileStorage fs(filepath, FileStorage::WRITE); if (!fs.isOpened()) { LOG(ERROR) << "Failed to save file: " << filepath; return false; } - fs << "in_accel" << params.in_accel << "in_gyro" << params.in_gyro + fs << "version" << SAVE_LATEST_VERSION.to_string() + << "in_accel" << params.in_accel + << "in_gyro" << params.in_gyro << "ex_left_to_imu" << params.ex_left_to_imu; fs.release(); return true; @@ -224,18 +271,8 @@ void DeviceWriter::SaveAllInfos(const std::string &dir) { LOG(FATAL) << "Create directory failed: " << dir; } SaveDeviceInfo(*device_->GetInfo(), dir + MYNTEYE_OS_SEP "device.info"); - SaveImgParams( - *device_->GetInfo(), device_->GetImgParams(), - dir + MYNTEYE_OS_SEP "img.params"); - auto &&m_in = device_->GetMotionIntrinsics(); - SaveImuParams( - { - false, - device_->GetInfo()->spec_version.to_string(), - m_in.accel, m_in.gyro, - device_->GetMotionExtrinsics(Stream::LEFT), - }, - dir + MYNTEYE_OS_SEP "imu.params"); + SaveImgParams(device_->GetImgParams(), dir + MYNTEYE_OS_SEP "img.params"); + SaveImuParams(device_->GetImuParams(), dir + MYNTEYE_OS_SEP "imu.params"); } namespace { diff --git a/tools/writer/device_writer.h b/tools/writer/device_writer.h index de942fc..f3a7af2 100644 --- a/tools/writer/device_writer.h +++ b/tools/writer/device_writer.h @@ -49,8 +49,7 @@ class DeviceWriter { bool WriteImuParams(const std::string &filepath); bool SaveDeviceInfo(const dev_info_t &info, const std::string &filepath); - bool SaveImgParams( - const dev_info_t &info, const img_params_map_t &img_params_map, + bool SaveImgParams(const img_params_map_t &img_params_map, const std::string &filepath); bool SaveImuParams(const imu_params_t ¶ms, const std::string &filepath); diff --git a/tools/writer/save_all_infos.cc b/tools/writer/save_all_infos.cc index 4aa3cfd..45b3934 100644 --- a/tools/writer/save_all_infos.cc +++ b/tools/writer/save_all_infos.cc @@ -31,6 +31,7 @@ int main(int argc, char *argv[]) { if (!device) return 1; + dir.append(MYNTEYE_OS_SEP).append(device->GetInfo()->name); dir.append(MYNTEYE_OS_SEP "SN").append(device->GetInfo()->serial_number); tools::DeviceWriter writer(device);