Add device writer tools

This commit is contained in:
John Zhao 2018-04-21 15:45:33 +08:00
parent 045c94dda7
commit cdd98925b5
14 changed files with 689 additions and 7 deletions

View File

@ -21,6 +21,12 @@ struct device;
} // namespace uvc
namespace tools {
class DeviceWriter;
} // namespace tools
struct DeviceInfo;
class Channels;
@ -145,7 +151,7 @@ class Device {
return channels_;
}
// friend DeviceWriter;
friend tools::DeviceWriter;
};
MYNTEYE_END_NAMESPACE

View File

@ -2,6 +2,12 @@
#include <glog/logging.h>
#include <iomanip>
#include <limits>
#define FULL_PRECISION \
std::fixed << std::setprecision(std::numeric_limits<double>::max_digits10)
MYNTEYE_BEGIN_NAMESPACE
const char *to_string(const Model &value) {
@ -146,16 +152,17 @@ std::ostream &operator<<(std::ostream &os, const StreamRequest &request) {
}
std::ostream &operator<<(std::ostream &os, const Intrinsics &in) {
os << "width: " << in.width << ", height: " << in.height << ", fx: " << in.fx
<< ", fy: " << in.fy << ", cx: " << in.cx << ", cy: " << in.cy
<< ", model: " << static_cast<int>(in.model) << ", coeffs: [";
os << FULL_PRECISION << "width: " << in.width << ", height: " << in.height
<< ", fx: " << in.fx << ", fy: " << in.fy << ", cx: " << in.cx
<< ", cy: " << in.cy << ", model: " << static_cast<int>(in.model)
<< ", coeffs: [";
for (int i = 0; i <= 3; i++)
os << in.coeffs[i] << ", ";
return os << in.coeffs[4] << "]";
}
std::ostream &operator<<(std::ostream &os, const ImuIntrinsics &in) {
os << "scale: [";
os << FULL_PRECISION << "scale: [";
for (int i = 0; i <= 2; i++)
os << in.scale[0][i] << ", ";
for (int i = 0; i <= 2; i++)
@ -183,11 +190,12 @@ std::ostream &operator<<(std::ostream &os, const ImuIntrinsics &in) {
}
std::ostream &operator<<(std::ostream &os, const MotionIntrinsics &in) {
return os << "accel: {" << in.accel << "}, gyro: {" << in.gyro << "}";
return os << FULL_PRECISION << "accel: {" << in.accel << "}, gyro: {"
<< in.gyro << "}";
}
std::ostream &operator<<(std::ostream &os, const Extrinsics &ex) {
os << "rotation: [";
os << FULL_PRECISION << "rotation: [";
for (int i = 0; i <= 2; i++)
os << ex.rotation[0][i] << ", ";
for (int i = 0; i <= 2; i++)

View File

@ -52,3 +52,7 @@ include_directories(
# dataset
add_subdirectory(dataset)
# writer
add_subdirectory(writer)

View File

@ -73,6 +73,28 @@ python tools/analytics/stamp_analytics.py -i mynteye.bag
---
## Writer
### device_info_writer.cc
```bash
./tools/_output/bin/writer/device_info_writer tools/writer/config/device.info
```
### img_params_writer.cc
```bash
./tools/_output/bin/writer/img_params_writer tools/writer/config/img.params
```
### imu_params_writer.cc
```bash
./tools/_output/bin/writer/imu_params_writer tools/writer/config/imu.params
```
---
## Checksum
```bash

View File

@ -0,0 +1,51 @@
get_filename_component(DIR_NAME ${CMAKE_CURRENT_LIST_DIR} NAME)
set_outdir(
"${OUT_DIR}/lib/${DIR_NAME}"
"${OUT_DIR}/lib/${DIR_NAME}"
"${OUT_DIR}/bin/${DIR_NAME}"
)
include_directories(
${PRO_DIR}/src
)
## device_writer
add_library(device_writer STATIC device_writer.cc)
target_link_libraries(device_writer mynteye ${OpenCV_LIBS})
set(LINK_LIBS device_writer)
## device_info_writer
add_executable(device_info_writer device_info_writer.cc)
target_link_libraries(device_info_writer ${LINK_LIBS})
if(OS_WIN)
target_compile_definitions(device_info_writer
PUBLIC GLOG_NO_ABBREVIATED_SEVERITIES
)
endif()
## img_params_writer
add_executable(img_params_writer img_params_writer.cc)
target_link_libraries(img_params_writer ${LINK_LIBS})
if(OS_WIN)
target_compile_definitions(img_params_writer
PUBLIC GLOG_NO_ABBREVIATED_SEVERITIES
)
endif()
## imu_params_writer
add_executable(imu_params_writer imu_params_writer.cc)
target_link_libraries(imu_params_writer ${LINK_LIBS})
if(OS_WIN)
target_compile_definitions(imu_params_writer
PUBLIC GLOG_NO_ABBREVIATED_SEVERITIES
)
endif()

View File

@ -0,0 +1,5 @@
%YAML:1.0
---
lens_type: "0000"
imu_type: "0000"
nominal_baseline: 120

View File

@ -0,0 +1,32 @@
%YAML:1.0
---
in_left:
-
width: 752
height: 480
fx: 7.3638305001095546e+02
fy: 7.2350066150722432e+02
cx: 3.5691961817119693e+02
cy: 2.1727271340923883e+02
model: 0
coeffs: [ -5.4898645145016478e-01, 5.2837141203888638e-01, 0., 0.,
0. ]
in_right:
-
width: 752
height: 480
fx: 7.3638305001095546e+02
fy: 7.2350066150722432e+02
cx: 4.5668367112303980e+02
cy: 2.5070083335536796e+02
model: 0
coeffs: [ -5.1012886039889305e-01, 3.8764476500996770e-01, 0., 0.,
0. ]
ex_left_to_right:
rotation: [ 9.9701893306553813e-01, -9.5378124886236681e-04,
-7.7151392794850615e-02, 1.4493996762830500e-03,
9.9997867219985104e-01, 6.3682325649414354e-03,
7.7143673424555026e-02, -6.4610716411527686e-03,
9.9699905125522237e-01 ]
translation: [ -1.1888991734400047e+02, -4.5605803870530912e-02,
-3.9531373691193386e+00 ]

View File

@ -0,0 +1,41 @@
%YAML:1.0
---
M1: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [ 7.3638305001095546e+02, 0., 3.5691961817119693e+02, 0.,
7.2350066150722432e+02, 2.1727271340923883e+02, 0., 0., 1. ]
D1: !!opencv-matrix
rows: 1
cols: 14
dt: d
data: [ -5.4898645145016478e-01, 5.2837141203888638e-01, 0., 0., 0.,
0., 0., 4.3563985766435476e-01, 0., 0., 0., 0., 0., 0. ]
M2: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [ 7.3638305001095546e+02, 0., 4.5668367112303980e+02, 0.,
7.2350066150722432e+02, 2.5070083335536796e+02, 0., 0., 1. ]
D2: !!opencv-matrix
rows: 1
cols: 14
dt: d
data: [ -5.1012886039889305e-01, 3.8764476500996770e-01, 0., 0., 0.,
0., 0., 2.5629798245273044e-01, 0., 0., 0., 0., 0., 0. ]
R: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [ 9.9701893306553813e-01, -9.5378124886236681e-04,
-7.7151392794850615e-02, 1.4493996762830500e-03,
9.9997867219985104e-01, 6.3682325649414354e-03,
7.7143673424555026e-02, -6.4610716411527686e-03,
9.9699905125522237e-01 ]
T: !!opencv-matrix
rows: 3
cols: 1
dt: d
data: [ -1.1888991734400047e+02, -4.5605803870530912e-02,
-3.9531373691193386e+00 ]

View File

@ -0,0 +1,15 @@
%YAML:1.0
---
in_accel:
scale: [ 0., 0., 0., 0., 0., 0., 0., 0., 0. ]
drift: [ 0., 0., 0. ]
noise: [ 0., 0., 0. ]
bias: [ 0., 0., 0. ]
in_gyro:
scale: [ 0., 0., 0., 0., 0., 0., 0., 0., 0. ]
drift: [ 0., 0., 0. ]
noise: [ 0., 0., 0. ]
bias: [ 0., 0., 0. ]
ex_left_to_imu:
rotation: [ 0., 0., 0., 0., 0., 0., 0., 0., 0. ]
translation: [ 0., 0., 0. ]

View File

@ -0,0 +1,27 @@
#include "mynteye/glog_init.h"
#include "mynteye/device.h"
#include "mynteye/utils.h"
#include "writer/device_writer.h"
MYNTEYE_USE_NAMESPACE
int main(int argc, char *argv[]) {
glog_init _(argc, argv);
const char *filepath;
if (argc >= 2) {
filepath = argv[1];
} else {
LOG(ERROR) << "Usage: ./device_info_writer <filepath>";
return 2;
}
auto &&device = device::select();
tools::DeviceWriter writer(device);
writer.WriteDeviceInfo(filepath);
return 0;
}

View File

@ -0,0 +1,361 @@
#include "writer/device_writer.h"
#include <opencv2/core/core.hpp>
#include <glog/logging.h>
#include <vector>
#include "mynteye/device.h"
#include "mynteye/files.h"
MYNTEYE_BEGIN_NAMESPACE
namespace tools {
DeviceWriter::DeviceWriter(std::shared_ptr<Device> device) : device_(device) {
VLOG(2) << __func__;
}
DeviceWriter::~DeviceWriter() {
VLOG(2) << __func__;
}
bool DeviceWriter::WriteDeviceInfo(const dev_info_t &info) {
auto &&channels = device_->channels();
auto &&dev_info = device_->GetInfo();
dev_info->lens_type = Type(info.lens_type);
dev_info->imu_type = Type(info.imu_type);
dev_info->nominal_baseline = info.nominal_baseline;
if (channels->SetFiles(dev_info.get(), nullptr, nullptr)) {
LOG(INFO) << "Write device info success";
LOG(INFO) << "Device info: {name: " << dev_info->name
<< ", serial_number: " << dev_info->serial_number
<< ", firmware_version: "
<< dev_info->firmware_version.to_string()
<< ", hardware_version: "
<< dev_info->hardware_version.to_string()
<< ", spec_version: " << dev_info->spec_version.to_string()
<< ", lens_type: " << dev_info->lens_type.to_string()
<< ", imu_type: " << dev_info->imu_type.to_string()
<< ", nominal_baseline: " << dev_info->nominal_baseline << "}";
return true;
} else {
LOG(ERROR) << "Write device info failed";
return false;
}
}
bool DeviceWriter::WriteDeviceInfo(const std::string &filepath) {
return WriteDeviceInfo(LoadDeviceInfo(filepath));
}
bool DeviceWriter::WriteImgParams(const img_params_t &params) {
auto &&channels = device_->channels();
auto &&dev_info = device_->GetInfo();
if (channels->SetFiles(
nullptr, const_cast<img_params_t *>(&params), nullptr,
&dev_info->spec_version)) {
LOG(INFO) << "Write img params success";
LOG(INFO) << "Intrinsics left: {" << params.in_left << "}";
LOG(INFO) << "Intrinsics right: {" << params.in_right << "}";
LOG(INFO) << "Extrinsics left to right: {" << params.ex_left_to_right
<< "}";
return true;
} else {
LOG(ERROR) << "Write img params failed";
return false;
}
}
bool DeviceWriter::WriteImgParams(const std::string &filepath) {
return WriteImgParams(LoadImgParams(filepath));
}
bool DeviceWriter::WriteImuParams(const imu_params_t &params) {
auto &&channels = device_->channels();
auto &&dev_info = device_->GetInfo();
if (channels->SetFiles(
nullptr, nullptr, const_cast<imu_params_t *>(&params),
&dev_info->spec_version)) {
LOG(INFO) << "Write imu params success";
LOG(INFO) << "Imu intrinsics accel: {" << params.in_accel << "}";
LOG(INFO) << "Imu intrinsics gyro: {" << params.in_gyro << "}";
LOG(INFO) << "Imu extrinsics left to imu: {" << params.ex_left_to_imu
<< "}";
return true;
} else {
LOG(ERROR) << "Write imu params failed";
return false;
}
}
bool DeviceWriter::WriteImuParams(const std::string &filepath) {
return WriteImuParams(LoadImuParams(filepath));
}
namespace {
cv::FileStorage &operator<<(cv::FileStorage &fs, const Intrinsics &in) {
fs << "{"
<< "width" << in.width << "height" << in.height << "fx" << in.fx << "fy"
<< in.fy << "cx" << in.cx << "cy" << in.cy << "model" << in.model
<< "coeffs" << std::vector<double>(in.coeffs, in.coeffs + 5) << "}";
return fs;
}
cv::FileStorage &operator<<(
cv::FileStorage &fs, const std::vector<Intrinsics> &vec) {
fs << "[";
for (auto &&in : vec)
fs << in;
fs << "]";
return fs;
}
cv::FileStorage &operator<<(cv::FileStorage &fs, const ImuIntrinsics &in) {
std::vector<double> scales;
for (std::size_t i = 0; i < 3; i++) {
for (std::size_t j = 0; j < 3; j++) {
scales.push_back(in.scale[i][j]);
}
}
fs << "{"
<< "scale" << scales << "drift"
<< std::vector<double>(in.drift, in.drift + 3) << "noise"
<< std::vector<double>(in.noise, in.noise + 3) << "bias"
<< std::vector<double>(in.bias, in.bias + 3) << "}";
return fs;
}
cv::FileStorage &operator<<(cv::FileStorage &fs, const Extrinsics &ex) {
std::vector<double> rotations;
for (std::size_t i = 0; i < 3; i++) {
for (std::size_t j = 0; j < 3; j++) {
rotations.push_back(ex.rotation[i][j]);
}
}
fs << "{"
<< "rotation" << rotations << "translation"
<< std::vector<double>(ex.translation, ex.translation + 3) << "}";
return fs;
}
} // namespace
bool DeviceWriter::SaveDeviceInfo(
const dev_info_t &info, const std::string &filepath) {
using FileStorage = cv::FileStorage;
FileStorage fs(filepath, FileStorage::WRITE);
if (!fs.isOpened()) {
LOG(ERROR) << "Failed to save file: " << filepath;
return false;
}
fs << "lens_type" << info.lens_type.to_string();
fs << "imu_type" << info.imu_type.to_string();
fs << "nominal_baseline" << info.nominal_baseline;
fs.release();
return true;
}
bool DeviceWriter::SaveImgParams(
const img_params_t &params, const std::string &filepath) {
using FileStorage = cv::FileStorage;
FileStorage fs(filepath, FileStorage::WRITE);
if (!fs.isOpened()) {
LOG(ERROR) << "Failed to save file: " << filepath;
return false;
}
fs << "in_left" << std::vector<Intrinsics>{params.in_left} << "in_right"
<< std::vector<Intrinsics>{params.in_right} << "ex_left_to_right"
<< params.ex_left_to_right;
fs.release();
return true;
}
bool DeviceWriter::SaveImuParams(
const imu_params_t &params, const std::string &filepath) {
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
<< "ex_left_to_imu" << params.ex_left_to_imu;
fs.release();
return true;
}
void DeviceWriter::SaveAllInfos(const std::string &dir) {
if (!files::mkdir(dir)) {
LOG(FATAL) << "Create directory failed: " << dir;
}
SaveDeviceInfo(*device_->GetInfo(), dir + OS_SEP "device.info");
SaveImgParams(
{false, device_->GetIntrinsics(Stream::LEFT),
device_->GetIntrinsics(Stream::RIGHT),
device_->GetExtrinsics(Stream::LEFT, Stream::RIGHT)},
dir + OS_SEP "img.params");
auto &&m_in = device_->GetMotionIntrinsics();
SaveImuParams(
{
false, m_in.accel, m_in.gyro,
device_->GetMotionExtrinsics(Stream::LEFT),
},
dir + OS_SEP "imu.params");
}
namespace {
void to_intrinsics(
const std::uint16_t &width, const std::uint16_t &height,
const std::uint8_t &model, const cv::Mat &M, const cv::Mat &D,
Intrinsics *in) {
in->width = width;
in->height = height;
/*
fx, 0, cx,
0, fy, cy,
0, 0, 1
*/
in->fx = M.at<double>(0, 0);
in->fy = M.at<double>(1, 1);
in->cx = M.at<double>(0, 2);
in->cy = M.at<double>(1, 2);
/* k1, k2, p1, p2, k3 */
in->model = model;
LOG_IF(FATAL, D.cols < 5) << "Distortion coefficients must >= 5 columns";
for (std::size_t i = 0; i < 5; i++) {
in->coeffs[i] = D.at<double>(i);
}
}
void to_extrinsics(const cv::Mat &R, const cv::Mat &T, Extrinsics *ex) {
for (std::size_t i = 0; i < 3; i++) {
for (std::size_t j = 0; j < 3; j++) {
ex->rotation[i][j] = R.at<double>(i, j);
}
}
for (std::size_t i = 0; i < 3; i++) {
ex->translation[i] = T.at<double>(i);
}
}
void operator>>(const cv::FileNode &n, Intrinsics &in) {
n["width"] >> in.width;
n["height"] >> in.height;
n["fx"] >> in.fx;
n["fy"] >> in.fy;
n["cx"] >> in.cx;
n["cy"] >> in.cy;
n["model"] >> in.model;
for (std::size_t i = 0; i < 5; i++) {
in.coeffs[i] = n["coeffs"][i];
}
}
void operator>>(const cv::FileNode &n, ImuIntrinsics &in) {
for (std::size_t i = 0; i < 3; i++) {
for (std::size_t j = 0; j < 3; j++) {
in.scale[i][j] = n["scale"][3 * i + j];
}
}
for (std::size_t i = 0; i < 3; i++) {
in.drift[i] = n["drift"][i];
}
for (std::size_t i = 0; i < 3; i++) {
in.noise[i] = n["noise"][i];
}
for (std::size_t i = 0; i < 3; i++) {
in.bias[i] = n["bias"][i];
}
}
void operator>>(const cv::FileNode &n, Extrinsics &ex) {
for (std::size_t i = 0; i < 3; i++) {
for (std::size_t j = 0; j < 3; j++) {
ex.rotation[i][j] = n["rotation"][3 * i + j];
}
}
for (std::size_t i = 0; i < 3; i++) {
ex.translation[i] = n["translation"][i];
}
}
} // namespace
DeviceWriter::dev_info_t DeviceWriter::LoadDeviceInfo(
const std::string &filepath) {
using FileStorage = cv::FileStorage;
FileStorage fs(filepath, FileStorage::READ);
if (!fs.isOpened()) {
LOG(FATAL) << "Failed to load file: " << filepath;
}
DeviceInfo info;
info.lens_type = Type(fs["lens_type"].string());
info.imu_type = Type(fs["imu_type"].string());
fs["nominal_baseline"] >> info.nominal_baseline;
fs.release();
return info;
}
DeviceWriter::img_params_t DeviceWriter::LoadImgParams(
const std::string &filepath) {
using FileStorage = cv::FileStorage;
FileStorage fs(filepath, FileStorage::READ);
if (!fs.isOpened()) {
LOG(FATAL) << "Failed to load file: " << filepath;
}
img_params_t params;
if (fs["in_left"].isNone()) {
std::uint16_t w = 752;
std::uint16_t h = 480;
std::uint8_t m = 0;
if (!fs["width"].isNone())
w = static_cast<int>(fs["width"]);
if (!fs["height"].isNone())
h = static_cast<int>(fs["height"]);
if (!fs["model"].isNone())
m = static_cast<int>(fs["model"]);
cv::Mat M1, D1, M2, D2, R, T;
fs["M1"] >> M1;
fs["D1"] >> D1;
fs["M2"] >> M2;
fs["D2"] >> D2;
fs["R"] >> R;
fs["T"] >> T;
to_intrinsics(w, h, m, M1, D1, &params.in_left);
to_intrinsics(w, h, m, M2, D2, &params.in_right);
to_extrinsics(R, T, &params.ex_left_to_right);
} else {
fs["in_left"][0] >> params.in_left;
fs["in_right"][0] >> params.in_right;
fs["ex_left_to_right"] >> params.ex_left_to_right;
}
fs.release();
return params;
}
DeviceWriter::imu_params_t DeviceWriter::LoadImuParams(
const std::string &filepath) {
using FileStorage = cv::FileStorage;
FileStorage fs(filepath, FileStorage::READ);
if (!fs.isOpened()) {
LOG(FATAL) << "Failed to load file: " << filepath;
}
imu_params_t params;
fs["in_accel"] >> params.in_accel;
fs["in_gyro"] >> params.in_gyro;
fs["ex_left_to_imu"] >> params.ex_left_to_imu;
fs.release();
return params;
}
} // namespace tools
MYNTEYE_END_NAMESPACE

View File

@ -0,0 +1,56 @@
#ifndef MYNTEYE_TOOLS_DEVICE_WRITER_H_ // NOLINT
#define MYNTEYE_TOOLS_DEVICE_WRITER_H_
#pragma once
#include <memory>
#include <string>
#include "mynteye/mynteye.h"
#include "internal/channels.h"
#include "internal/types.h"
MYNTEYE_BEGIN_NAMESPACE
class Device;
namespace tools {
class DeviceWriter {
public:
using dev_info_t = DeviceInfo;
using img_params_t = Channels::img_params_t;
using imu_params_t = Channels::imu_params_t;
explicit DeviceWriter(std::shared_ptr<Device> device);
~DeviceWriter();
bool WriteDeviceInfo(const dev_info_t &info);
bool WriteDeviceInfo(const std::string &filepath);
bool WriteImgParams(const img_params_t &params);
bool WriteImgParams(const std::string &filepath);
bool WriteImuParams(const imu_params_t &params);
bool WriteImuParams(const std::string &filepath);
bool SaveDeviceInfo(const dev_info_t &info, const std::string &filepath);
bool SaveImgParams(const img_params_t &params, const std::string &filepath);
bool SaveImuParams(const imu_params_t &params, const std::string &filepath);
/** Save all infos of this device */
void SaveAllInfos(const std::string &dir);
private:
dev_info_t LoadDeviceInfo(const std::string &filepath);
img_params_t LoadImgParams(const std::string &filepath);
imu_params_t LoadImuParams(const std::string &filepath);
std::shared_ptr<Device> device_;
};
} // namespace tools
MYNTEYE_END_NAMESPACE
#endif // MYNTEYE_TOOLS_DEVICE_WRITER_H_ NOLINT

View File

@ -0,0 +1,27 @@
#include "mynteye/glog_init.h"
#include "mynteye/device.h"
#include "mynteye/utils.h"
#include "writer/device_writer.h"
MYNTEYE_USE_NAMESPACE
int main(int argc, char *argv[]) {
glog_init _(argc, argv);
const char *filepath;
if (argc >= 2) {
filepath = argv[1];
} else {
LOG(ERROR) << "Usage: ./img_params_writer <filepath>";
return 2;
}
auto &&device = device::select();
tools::DeviceWriter writer(device);
writer.WriteImgParams(filepath);
return 0;
}

View File

@ -0,0 +1,27 @@
#include "mynteye/glog_init.h"
#include "mynteye/device.h"
#include "mynteye/utils.h"
#include "writer/device_writer.h"
MYNTEYE_USE_NAMESPACE
int main(int argc, char *argv[]) {
glog_init _(argc, argv);
const char *filepath;
if (argc >= 2) {
filepath = argv[1];
} else {
LOG(ERROR) << "Usage: ./imu_params_writer <filepath>";
return 2;
}
auto &&device = device::select();
tools::DeviceWriter writer(device);
writer.WriteImuParams(filepath);
return 0;
}