Skip to content

Commit

Permalink
fix: codestyle
Browse files Browse the repository at this point in the history
  • Loading branch information
dfbakin committed Mar 9, 2024
1 parent 31f4ec9 commit cf88f32
Show file tree
Hide file tree
Showing 3 changed files with 24 additions and 19 deletions.
6 changes: 4 additions & 2 deletions packages/camera/include/params.h
Original file line number Diff line number Diff line change
Expand Up @@ -17,9 +17,11 @@ struct CameraIntrinsicParameters {
void storeYaml(const std::string& yaml_path) const;
static CameraIntrinsicParameters loadFromYaml(const std::string& yaml_path, int camera_id = 0);
static bool saveStereoCalibration(
const std::string& yaml_path, cv::Mat& R, cv::Mat& T, cv::Size& image_size);
const std::string& yaml_path, cv::Mat& rotation_vectors, cv::Mat& translation_vectors,
cv::Size& image_size);
static void loadStereoCalibration(
const std::string& yaml_path, cv::Mat& R, cv::Mat& T, cv::Size& image_size);
const std::string& yaml_path, cv::Mat& rotation_vectors, cv::Mat& translation_vectors,
cv::Size& image_size);

int camera_id = 0;
cv::Size image_size{};
Expand Down
20 changes: 11 additions & 9 deletions packages/camera/src/params.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,8 @@ void CameraIntrinsicParameters::storeYaml(const std::string& yaml_path) const {
output_file.close();
}
bool CameraIntrinsicParameters::saveStereoCalibration(
const std::string& yaml_path, cv::Mat& R, cv::Mat& T, cv::Size& image_size) {
const std::string& yaml_path, cv::Mat& rotation_vectors, cv::Mat& translation_vectors,
cv::Size& image_size) {
YAML::Node config;
std::ifstream param_file(yaml_path);
if (param_file) {
Expand All @@ -76,15 +77,15 @@ bool CameraIntrinsicParameters::saveStereoCalibration(
extrinsics["rotation"].SetStyle(YAML::EmitterStyle::Flow);
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
extrinsics["rotation"].push_back(R.at<double>(i, j));
extrinsics["rotation"].push_back(rotation_vectors.at<double>(i, j));
}
}

extrinsics["translation"] = YAML::Node(YAML::NodeType::Sequence);
extrinsics["translation"].SetStyle(YAML::EmitterStyle::Flow);

for (int i = 0; i < 3; ++i) {
extrinsics["translation"].push_back(T.at<double>(i, 0));
extrinsics["translation"].push_back(translation_vectors.at<double>(i, 0));
}

std::ofstream output_file(yaml_path);
Expand Down Expand Up @@ -132,26 +133,27 @@ CameraIntrinsicParameters CameraIntrinsicParameters::loadFromYaml(
}

void CameraIntrinsicParameters::loadStereoCalibration(
const std::string& yaml_path, cv::Mat& R, cv::Mat& T, cv::Size& image_size) {
const std::string& yaml_path, cv::Mat& rotation_vectors, cv::Mat& translation_vectors,
cv::Size& image_size) {
const YAML::Node extrinsics = YAML::LoadFile(yaml_path)["stereo_calibration"];

const auto yaml_image_size = extrinsics["image_size"].as<std::vector<int>>();
image_size = cv::Size(yaml_image_size[0], yaml_image_size[1]);

const auto yaml_camera_matrix = extrinsics["rotation"].as<std::vector<double>>();
R = cv::Mat(yaml_camera_matrix, true);
R = R.reshape(0, {3, 3});
rotation_vectors = cv::Mat(yaml_camera_matrix, true);
rotation_vectors = rotation_vectors.reshape(0, {3, 3});

const auto coefs = extrinsics["translation"].as<std::vector<double>>();
for (size_t i = 0; i < coefs.size(); ++i) {
printf("%f ", coefs[i]);
}
printf("\n");

T = cv::Mat(coefs, true);
T = T.reshape(0, {3, 1});
translation_vectors = cv::Mat(coefs, true);
translation_vectors = translation_vectors.reshape(0, {3, 1});
for (size_t i = 0; i < coefs.size(); ++i) {
printf("%f ", T.at<double>(i, 0));
printf("%f ", translation_vectors.at<double>(i, 0));
}
printf("\n");
}
Expand Down
17 changes: 9 additions & 8 deletions packages/camera/test/test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -85,33 +85,34 @@ TEST(camera, ParamsMultipleReadWrite) {
TEST(camera, ParamsStereoCalibWrite) {
// create test params
const std::string path_to_yaml = "test_params.yaml";
cv::Mat R_check = (cv::Mat_<double>(3, 3) << 1., 0., 0., 0., 1., 0., 0., 0., 1.);
cv::Mat T_check = (cv::Mat_<double>(3, 1) << 1., 2., 3.);
cv::Mat rotation_check = (cv::Mat_<double>(3, 3) << 1., 0., 0., 0., 1., 0., 0., 0., 1.);
cv::Mat translation_check = (cv::Mat_<double>(3, 1) << 1., 2., 3.);
cv::Size image_size_check(1920, 1080);

handy::CameraIntrinsicParameters::saveStereoCalibration(
path_to_yaml, R_check, T_check, image_size_check);
path_to_yaml, rotation_check, translation_check, image_size_check);

// check for file to exist
std::ifstream test_file(path_to_yaml);
EXPECT_TRUE(test_file); // opened successfully
test_file.close();

// loading and checking all params for the first ID
cv::Mat R;
cv::Mat T;
cv::Mat rotation;
cv::Mat translation;
cv::Size image_size;

handy::CameraIntrinsicParameters::loadStereoCalibration(path_to_yaml, R, T, image_size);
handy::CameraIntrinsicParameters::loadStereoCalibration(
path_to_yaml, rotation, translation, image_size);

EXPECT_EQ(image_size, image_size_check);
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; j++) {
EXPECT_EQ(R_check.at<double>(i, j), R.at<double>(i, j));
EXPECT_EQ(rotation_check.at<double>(i, j), rotation.at<double>(i, j));
}
}
for (int i = 0; i < 3; ++i) {
EXPECT_EQ(T_check.at<double>(i, 0), T.at<double>(i, 0));
EXPECT_EQ(translation_check.at<double>(i, 0), translation.at<double>(i, 0));
}
}

Expand Down

0 comments on commit cf88f32

Please sign in to comment.