From 516d3b75129f9f38902954c9d6230e6f15752097 Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+Mixmix00@users.noreply.github.com> Date: Fri, 17 Jan 2025 18:43:05 -0800 Subject: [PATCH 1/9] Add code for rotation stuff --- src/ws_server.cu | 37 ++++++++++++++++++++++++++++++++----- 1 file changed, 32 insertions(+), 5 deletions(-) diff --git a/src/ws_server.cu b/src/ws_server.cu index 06fe6f1..a32f1fb 100644 --- a/src/ws_server.cu +++ b/src/ws_server.cu @@ -141,6 +141,10 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { LOG(ERROR) << "key \"disto\" not found in calibration file."; return false; } + if(!data.contains("rotation")){ + LOG(ERROR) << "key \"rotation\" not found in calibration file."; + return false; + } // Setup Camera Matrix // Intrinsic Matrices are explained here: @@ -159,6 +163,12 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { dist->p2 = data["disto"][0][3]; dist->k3 = data["disto"][0][4]; + for(int i = 0; i < 3; i++){ + for(int j = 0; j < 3; j++){ + rotationCoefficents_[i][j] = data["rotation"][i][j]; + } + } + // Some debug print statements std::cout << "Loaded calibration matrix:" << std::endl; std::cout << "cam.fx: " << cam->fx << std::endl; @@ -171,6 +181,13 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { std::cout << "dist.p1: " << dist->p1 << std::endl; std::cout << "dist.p2: " << dist->p2 << std::endl; std::cout << "dist.k3: " << dist->k3 << std::endl << std::endl; + std::cout << "Loaded rotation coefficients: " << std::endl; + for(int i = 0; i < 3; i++){ + for(int j = 0; j < 3; j++){ + std::cout << rotationCoefficents_[i][j] << " "; + } + std::cout << std::endl; + } return true; } @@ -389,16 +406,24 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { {pose.R->data[0], pose.R->data[1], pose.R->data[2]}, {pose.R->data[3], pose.R->data[4], pose.R->data[5]}, {pose.R->data[6], pose.R->data[7], pose.R->data[8]}}; - record["translation"] = {pose.t->data[0], pose.t->data[1], - pose.t->data[2]}; + + double translationBefore[3] = {pose.t->data[0], pose.t->data[1], pose.t->data[2]}; + double translationAfter[3][3]; + + for(int i = 0; i < 3; i++){ + for(int j = 0; j < 3; j++){ + translationAfter[i][j] = rotationCoefficents_[i][j] * translationBefore[j]; + } + } + record["translation"] = {translationAfter[0] translationAfter[1], translationAfter[2]}; detections_record["detections"].push_back(record); networktables_pose_data.push_back(frameReadTime); networktables_pose_data.push_back(det->id * 1.0); - networktables_pose_data.push_back(pose.t->data[0]); - networktables_pose_data.push_back(pose.t->data[1]); - networktables_pose_data.push_back(pose.t->data[2]); + networktables_pose_data.push_back(translationAfter[0]); + networktables_pose_data.push_back(translationAfter[1]); + networktables_pose_data.push_back(translationAfter[2]); } // Send the pose data @@ -438,6 +463,8 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { private: DoubleArraySender tagSender_{FLAGS_camera_name}; + + double rotationCoefficents_[3][3]; NetworkTablesUtil ntUtil_{}; std::set clients_; std::mutex mutex_; From e791b626706ed232aa3f882815e07e8a4eff238f Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+Mixmix00@users.noreply.github.com> Date: Sat, 18 Jan 2025 11:25:37 -0800 Subject: [PATCH 2/9] address pr feedback to change arrays into matricies --- src/ws_server.cu | 38 ++++++++++++++++++-------------------- 1 file changed, 18 insertions(+), 20 deletions(-) diff --git a/src/ws_server.cu b/src/ws_server.cu index a32f1fb..0d5eab8 100644 --- a/src/ws_server.cu +++ b/src/ws_server.cu @@ -163,11 +163,16 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { dist->p2 = data["disto"][0][3]; dist->k3 = data["disto"][0][4]; + double rotationCoefficents[9] = {0,0,0,0,0,0,0,0,0}; for(int i = 0; i < 3; i++){ for(int j = 0; j < 3; j++){ - rotationCoefficents_[i][j] = data["rotation"][i][j]; + rotationCoefficents[i][j] = data["rotation"][i][j]; } } + rotationCoefficents_ = (cv::Mat_(3, 3) << + rotationCoefficents[0], rotationCoefficents[1], rotationCoefficents[2], + rotationCoefficents[3], rotationCoefficents[4], rotationCoefficents[5], + rotationCoefficents[6], rotationCoefficents[7], rotationCoefficents[8]); // Some debug print statements std::cout << "Loaded calibration matrix:" << std::endl; @@ -181,13 +186,7 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { std::cout << "dist.p1: " << dist->p1 << std::endl; std::cout << "dist.p2: " << dist->p2 << std::endl; std::cout << "dist.k3: " << dist->k3 << std::endl << std::endl; - std::cout << "Loaded rotation coefficients: " << std::endl; - for(int i = 0; i < 3; i++){ - for(int j = 0; j < 3; j++){ - std::cout << rotationCoefficents_[i][j] << " "; - } - std::cout << std::endl; - } + return true; } @@ -407,23 +406,22 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { {pose.R->data[3], pose.R->data[4], pose.R->data[5]}, {pose.R->data[6], pose.R->data[7], pose.R->data[8]}}; - double translationBefore[3] = {pose.t->data[0], pose.t->data[1], pose.t->data[2]}; - double translationAfter[3][3]; + double translationBefore[3] = {}; + cv::Vec3d vec(pose.t->data[0], pose.t->data[1], pose.t->data[2]); + cv::Mat translation = cv::Mat(vec); + cv::Mat translationAfter = rotationCoefficents_ * translation; + //TODO: Unimplemented translation code (Crystal to add) - for(int i = 0; i < 3; i++){ - for(int j = 0; j < 3; j++){ - translationAfter[i][j] = rotationCoefficents_[i][j] * translationBefore[j]; - } - } - record["translation"] = {translationAfter[0] translationAfter[1], translationAfter[2]}; + + record["translation"] = {translationAfter[0], translationAfter[1], translationAfter[2]}; detections_record["detections"].push_back(record); networktables_pose_data.push_back(frameReadTime); networktables_pose_data.push_back(det->id * 1.0); - networktables_pose_data.push_back(translationAfter[0]); - networktables_pose_data.push_back(translationAfter[1]); - networktables_pose_data.push_back(translationAfter[2]); + networktables_pose_data.push_back(translationAfter.at(0)); + networktables_pose_data.push_back(translationAfter.at(1)); + networktables_pose_data.push_back(translationAfter.at(2)); } // Send the pose data @@ -464,7 +462,7 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { private: DoubleArraySender tagSender_{FLAGS_camera_name}; - double rotationCoefficents_[3][3]; + cv::Mat rotationCoefficents_; NetworkTablesUtil ntUtil_{}; std::set clients_; std::mutex mutex_; From 3fa6a39dea2d107c25865d9f61d93e18425095b6 Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+Mixmix00@users.noreply.github.com> Date: Sat, 18 Jan 2025 13:24:03 -0800 Subject: [PATCH 3/9] fix compile issues --- src/ws_server.cu | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/ws_server.cu b/src/ws_server.cu index 0d5eab8..fcef686 100644 --- a/src/ws_server.cu +++ b/src/ws_server.cu @@ -164,9 +164,11 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { dist->k3 = data["disto"][0][4]; double rotationCoefficents[9] = {0,0,0,0,0,0,0,0,0}; + int count = 0; for(int i = 0; i < 3; i++){ for(int j = 0; j < 3; j++){ - rotationCoefficents[i][j] = data["rotation"][i][j]; + rotationCoefficents[c] = data["rotation"][i][j]; + c += 1; } } rotationCoefficents_ = (cv::Mat_(3, 3) << @@ -413,7 +415,7 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { //TODO: Unimplemented translation code (Crystal to add) - record["translation"] = {translationAfter[0], translationAfter[1], translationAfter[2]}; + record["translation"] = {translationAfter.at(0), translationAfter.at(1), translationAfter.at(2)}; detections_record["detections"].push_back(record); networktables_pose_data.push_back(frameReadTime); From 3cba4d60b35baed12ca0e50549b81bd56c78c066 Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+Mixmix00@users.noreply.github.com> Date: Sat, 18 Jan 2025 13:24:18 -0800 Subject: [PATCH 4/9] forgot to save file --- .vscode/settings.json | 11 +++++++++++ src/ws_server.cu | 2 +- 2 files changed, 12 insertions(+), 1 deletion(-) create mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..b093f83 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,11 @@ +{ + "files.associations": { + "*.ndjson": "jsonl", + "*.dbclient-js": "javascript", + "array": "cpp", + "span": "cpp", + "string": "cpp", + "string_view": "cpp", + "vector": "cpp" + } +} \ No newline at end of file diff --git a/src/ws_server.cu b/src/ws_server.cu index fcef686..2e961f0 100644 --- a/src/ws_server.cu +++ b/src/ws_server.cu @@ -164,7 +164,7 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { dist->k3 = data["disto"][0][4]; double rotationCoefficents[9] = {0,0,0,0,0,0,0,0,0}; - int count = 0; + int c = 0; for(int i = 0; i < 3; i++){ for(int j = 0; j < 3; j++){ rotationCoefficents[c] = data["rotation"][i][j]; From cf62092061c010e5590de5708128af3f3bff7e90 Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+maxspier@users.noreply.github.com> Date: Sat, 18 Jan 2025 13:25:01 -0800 Subject: [PATCH 5/9] De-checkin my vscode settings file --- .vscode/settings.json | 11 ----------- 1 file changed, 11 deletions(-) delete mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index b093f83..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,11 +0,0 @@ -{ - "files.associations": { - "*.ndjson": "jsonl", - "*.dbclient-js": "javascript", - "array": "cpp", - "span": "cpp", - "string": "cpp", - "string_view": "cpp", - "vector": "cpp" - } -} \ No newline at end of file From 520dc8ad1d0a8866c021a3a15dbaba211de18fc6 Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+Mixmix00@users.noreply.github.com> Date: Sat, 18 Jan 2025 14:11:24 -0800 Subject: [PATCH 6/9] naming as in PR comments --- src/ws_server.cu | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/ws_server.cu b/src/ws_server.cu index 2e961f0..2e1ab8b 100644 --- a/src/ws_server.cu +++ b/src/ws_server.cu @@ -409,21 +409,21 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { {pose.R->data[6], pose.R->data[7], pose.R->data[8]}}; double translationBefore[3] = {}; - cv::Vec3d vec(pose.t->data[0], pose.t->data[1], pose.t->data[2]); - cv::Mat translation = cv::Mat(vec); - cv::Mat translationAfter = rotationCoefficents_ * translation; + cv::Vec3d aprilTagInCameraFrame(pose.t->data[0], pose.t->data[1], pose.t->data[2]); + cv::Mat aprilTagInCameraFrameAsMat = cv::Mat(aprilTagInCameraFrame); + cv::Mat aprilTagInRobotFrame = rotationCoefficents_ * aprilTagInCameraFrameAsMat; //TODO: Unimplemented translation code (Crystal to add) - record["translation"] = {translationAfter.at(0), translationAfter.at(1), translationAfter.at(2)}; + record["translation"] = {aprilTagInRobotFrame.at(0), aprilTagInRobotFrame.at(1), aprilTagInRobotFrame.at(2)}; detections_record["detections"].push_back(record); networktables_pose_data.push_back(frameReadTime); networktables_pose_data.push_back(det->id * 1.0); - networktables_pose_data.push_back(translationAfter.at(0)); - networktables_pose_data.push_back(translationAfter.at(1)); - networktables_pose_data.push_back(translationAfter.at(2)); + networktables_pose_data.push_back(aprilTagInRobotFrame.at(0)); + networktables_pose_data.push_back(aprilTagInRobotFrame.at(1)); + networktables_pose_data.push_back(aprilTagInRobotFrame.at(2)); } // Send the pose data From 32e7c8f536f0b6b871e9a80e65549afc4194f7e1 Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+Mixmix00@users.noreply.github.com> Date: Sat, 18 Jan 2025 14:12:02 -0800 Subject: [PATCH 7/9] delete unused line --- src/ws_server.cu | 1 - 1 file changed, 1 deletion(-) diff --git a/src/ws_server.cu b/src/ws_server.cu index 2e1ab8b..c8b7eb1 100644 --- a/src/ws_server.cu +++ b/src/ws_server.cu @@ -408,7 +408,6 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { {pose.R->data[3], pose.R->data[4], pose.R->data[5]}, {pose.R->data[6], pose.R->data[7], pose.R->data[8]}}; - double translationBefore[3] = {}; cv::Vec3d aprilTagInCameraFrame(pose.t->data[0], pose.t->data[1], pose.t->data[2]); cv::Mat aprilTagInCameraFrameAsMat = cv::Mat(aprilTagInCameraFrame); cv::Mat aprilTagInRobotFrame = rotationCoefficents_ * aprilTagInCameraFrameAsMat; From 8c0c5cd5276a5fa243980d55d6d58044b9eef9be Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+Mixmix00@users.noreply.github.com> Date: Sat, 18 Jan 2025 14:16:12 -0800 Subject: [PATCH 8/9] add "rotation" to cal file --- data/calibrationmatrix.json | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/data/calibrationmatrix.json b/data/calibrationmatrix.json index d90cc56..4f7382f 100644 --- a/data/calibrationmatrix.json +++ b/data/calibrationmatrix.json @@ -1,4 +1,21 @@ { + "rotation": [ + [ + 1.0, + 0.0, + 0.0 + ], + [ + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], "matrix": [ [ 924.09, From b0e5c64da0d41fedeac8f894fbf292f64c8b9e4a Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+Mixmix00@users.noreply.github.com> Date: Sat, 18 Jan 2025 14:23:30 -0800 Subject: [PATCH 9/9] add translation stuff --- data/calibrationmatrix.json | 5 +++++ src/ws_server.cu | 14 +++++++++++++- 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/data/calibrationmatrix.json b/data/calibrationmatrix.json index 4f7382f..7b60029 100644 --- a/data/calibrationmatrix.json +++ b/data/calibrationmatrix.json @@ -16,6 +16,11 @@ 1.0 ] ], + "offset": [ + 0.0, + 0.0, + 0.0 + ], "matrix": [ [ 924.09, diff --git a/src/ws_server.cu b/src/ws_server.cu index c8b7eb1..915c9e3 100644 --- a/src/ws_server.cu +++ b/src/ws_server.cu @@ -145,6 +145,10 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { LOG(ERROR) << "key \"rotation\" not found in calibration file."; return false; } + if(!data.contains("offset")){ + LOG(ERROR) << "key \"offset\" not found in calibration file."; + return false; + } // Setup Camera Matrix // Intrinsic Matrices are explained here: @@ -171,11 +175,18 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { c += 1; } } + double offsetCoeficients[3] = {0,0,0}; + for(int i = 0; i < 2; i++){ + offsetCoeficients[i] = data["offset"][i]; + } + rotationCoefficents_ = (cv::Mat_(3, 3) << rotationCoefficents[0], rotationCoefficents[1], rotationCoefficents[2], rotationCoefficents[3], rotationCoefficents[4], rotationCoefficents[5], rotationCoefficents[6], rotationCoefficents[7], rotationCoefficents[8]); + offsetCoefficents_ = (cv::Mat_(3,1) << + offsetCoeficients[0], offsetCoeficients[1], offsetCoeficients[2]); // Some debug print statements std::cout << "Loaded calibration matrix:" << std::endl; std::cout << "cam.fx: " << cam->fx << std::endl; @@ -410,7 +421,7 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { cv::Vec3d aprilTagInCameraFrame(pose.t->data[0], pose.t->data[1], pose.t->data[2]); cv::Mat aprilTagInCameraFrameAsMat = cv::Mat(aprilTagInCameraFrame); - cv::Mat aprilTagInRobotFrame = rotationCoefficents_ * aprilTagInCameraFrameAsMat; + cv::Mat aprilTagInRobotFrame = rotationCoefficents_ * aprilTagInCameraFrameAsMat + offsetCoefficents_; //TODO: Unimplemented translation code (Crystal to add) @@ -464,6 +475,7 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { DoubleArraySender tagSender_{FLAGS_camera_name}; cv::Mat rotationCoefficents_; + cv::Mat offsetCoefficents_; NetworkTablesUtil ntUtil_{}; std::set clients_; std::mutex mutex_;