diff options
| -rw-r--r-- | ftnoir_protocol_ft/ftnoir_protocol_ft.cpp | 12 | ||||
| -rw-r--r-- | ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 12 |
2 files changed, 15 insertions, 9 deletions
diff --git a/ftnoir_protocol_ft/ftnoir_protocol_ft.cpp b/ftnoir_protocol_ft/ftnoir_protocol_ft.cpp index d4aa3c49..bb960696 100644 --- a/ftnoir_protocol_ft/ftnoir_protocol_ft.cpp +++ b/ftnoir_protocol_ft/ftnoir_protocol_ft.cpp @@ -126,12 +126,12 @@ float headRotZ; //
// Scale the Raw measurements to the client measurements.
//
- headRotX = getRadsFromDegrees(headpose[Pitch]);
- headRotY = getRadsFromDegrees(headpose[Yaw]);
- headRotZ = getRadsFromDegrees(headpose[Roll]);
- headPosX = headpose[TX] * 10;
- headPosY = headpose[TY] * 10;
- headPosZ = headpose[TZ] * 10;
+ headRotX = getRadsFromDegrees(rawheadpose[Pitch]);
+ headRotY = getRadsFromDegrees(rawheadpose[Yaw]);
+ headRotZ = getRadsFromDegrees(rawheadpose[Roll]);
+ headPosX = rawheadpose[TX] * 10;
+ headPosY = rawheadpose[TY] * 10;
+ headPosZ = rawheadpose[TZ] * 10;
virtRotX = getRadsFromDegrees(headpose[Pitch]);
virtRotY = getRadsFromDegrees(headpose[Yaw]);
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp index 366b5b51..84a3de73 100644 --- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp +++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp @@ -184,14 +184,14 @@ void Tracker::run() aruco::MarkerDetector detector; detector.setDesiredSpeed(3); - cv::Mat color, grayscale, rvec, tvec, tmp; + detector.setThresholdParams(11, 6); + cv::Mat color, grayscale, rvec, tvec; while (!stop) { if (!camera.read(color)) break; - cv::cvtColor(color, tmp, cv::COLOR_BGR2GRAY); - cv::GaussianBlur(tmp, grayscale, cv::Size(5, 5), 0); + cv::cvtColor(color, grayscale, cv::COLOR_BGR2GRAY); const float focal_length_w = 0.5 * grayscale.cols / tan(0.5 * fov * HT_PI / 180); const float focal_length_h = 0.5 * grayscale.rows / tan(0.5 * fov * grayscale.rows / grayscale.cols * HT_PI / 180.0); cv::Mat intrinsics = cv::Mat::eye(3, 3, CV_32FC1); @@ -209,6 +209,12 @@ void Tracker::run() detector.detect(grayscale, markers, cv::Mat(), cv::Mat(), -1, false); + if (markers.size() == 1 && markers[0].size() == 4) { + const aruco::Marker& m = markers.at(0); + for (int i = 0; i < 4; i++) + cv::line(color, m[i], m[(i+1)%4], cv::Scalar(0, 0, 255), 4); + } + frame = color; if (frame.rows > 0 && !fresh) |