From 6dcb840bf73427adf3d498a69e5bfa5bcc8701f6 Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Mon, 5 Dec 2022 00:04:19 -0500 Subject: [PATCH 01/21] String fixes --- AprilTagTrackers/GUI/U8String.cpp | 2 +- AprilTagTrackers/GUI/wxHelpers.hpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/AprilTagTrackers/GUI/U8String.cpp b/AprilTagTrackers/GUI/U8String.cpp index 1b67adc0..7588a721 100644 --- a/AprilTagTrackers/GUI/U8String.cpp +++ b/AprilTagTrackers/GUI/U8String.cpp @@ -4,7 +4,7 @@ U8String::operator wxString() const { - return wxString::FromUTF8Unchecked(str); + return wxString::FromUTF8Unchecked(str.c_str()); } U8StringView::operator wxString() const diff --git a/AprilTagTrackers/GUI/wxHelpers.hpp b/AprilTagTrackers/GUI/wxHelpers.hpp index e03a1df0..70caef7e 100644 --- a/AprilTagTrackers/GUI/wxHelpers.hpp +++ b/AprilTagTrackers/GUI/wxHelpers.hpp @@ -157,7 +157,7 @@ inline bool FromWXString(const wxString& str, double& out_val) } inline bool FromWXString(const wxString& str, std::string& out_val) { - out_val = str.utf8_string(); + out_val = str.utf8_str(); return true; } inline bool FromWXString(const wxString& str, wxString& out_val) @@ -193,7 +193,7 @@ inline wxString ToWXString(float val) } inline wxString ToWXString(const std::string& val) { - return wxString::FromUTF8Unchecked(val); + return wxString::FromUTF8Unchecked(val.c_str()); } inline wxString ToWXString(const wxString& val) { From db7bd759087b10f1569624adecb30d62c9157c4e Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Mon, 5 Dec 2022 00:04:37 -0500 Subject: [PATCH 02/21] git submodule fixes --- CMake/helpers.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMake/helpers.cmake b/CMake/helpers.cmake index bd09a458..943e9284 100644 --- a/CMake/helpers.cmake +++ b/CMake/helpers.cmake @@ -185,7 +185,7 @@ function(att_clone_submodule module_dir) endif() message(STATUS "Cloning submodule '${module_dir}'") execute_process( - COMMAND "${GIT_CMD}" submodule --quiet update --init --filter=tree:0 --recursive "${abs_module_dir}" + COMMAND "${GIT_CMD}" submodule --quiet update --init --recursive "${abs_module_dir}" WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}") if (NOT EXISTS "${abs_module_dir}/.git") message(FATAL_ERROR "Submodule ${abs_module_dir} failed to clone.") From 9e4b3b6c20e8b9480b8e1a11e488b44f2adebd59 Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Sat, 10 Dec 2022 14:44:41 -0500 Subject: [PATCH 03/21] Draw tracker axis at current location instead of location from previous frame --- AprilTagTrackers/Tracker.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/AprilTagTrackers/Tracker.cpp b/AprilTagTrackers/Tracker.cpp index aef1fafd..6ae96412 100644 --- a/AprilTagTrackers/Tracker.cpp +++ b/AprilTagTrackers/Tracker.cpp @@ -1144,7 +1144,6 @@ void Tracker::MainLoop() cv::Vec3d rvec = q.toRotVec(); cv::Vec3d tvec{rpos[0], rpos[1], rpos[2]}; - cv::aruco::drawAxis(drawImg, camCalib->cameraMatrix, camCalib->distortionCoeffs, rvec, tvec, 0.10f); if (!trackerStatus[i].boardFound) // if tracker was found in previous frame, we use that position for masking. If not, we use position from driver for masking. { @@ -1422,7 +1421,7 @@ void Tracker::MainLoop() // convert rodriguez rotation to quaternion cv::Quatd q = cv::Quatd::createFromRvec(trackerStatus[i].boardRvec); - // cv::aruco::drawAxis(drawImg, user_config.camMat, user_config.distCoeffs, boardRvec[i], boardTvec[i], 0.05); + cv::aruco::drawAxis(drawImg, camCalib->cameraMatrix, camCalib->distortionCoeffs, trackerStatus[i].boardRvec, trackerStatus[i].boardTvec, 0.1); q = cv::Quatd{0, 0, 1, 0} * (wrotation * q) * cv::Quatd{0, 0, 1, 0}; From 8d6f87b9606c28332c817ef8a5757a803e9481cc Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Sun, 11 Dec 2022 09:38:13 -0500 Subject: [PATCH 04/21] Implement privacy mode for out window --- AprilTagTrackers/GUI.hpp | 1 + AprilTagTrackers/GUI/MainFrame.cpp | 6 ++++++ AprilTagTrackers/Tracker.cpp | 30 ++++++++++++++++++++++++++++-- 3 files changed, 35 insertions(+), 2 deletions(-) diff --git a/AprilTagTrackers/GUI.hpp b/AprilTagTrackers/GUI.hpp index 29ffebc7..5dffa122 100644 --- a/AprilTagTrackers/GUI.hpp +++ b/AprilTagTrackers/GUI.hpp @@ -31,6 +31,7 @@ class ITrackerControl bool manualRecalibrate = false; bool multicamAutocalib = false; bool lockHeightCalib = false; + bool privacyMode = false; protected: /// Not set during tracker contstruction diff --git a/AprilTagTrackers/GUI/MainFrame.cpp b/AprilTagTrackers/GUI/MainFrame.cpp index 677fbfe5..7e5391ed 100644 --- a/AprilTagTrackers/GUI/MainFrame.cpp +++ b/AprilTagTrackers/GUI/MainFrame.cpp @@ -255,6 +255,12 @@ void GUI::MainFrame::CreateCameraPage(RefPtr pages) }}) ->GetWidget(); + cam .Add(StretchSpacer{}) + .Add(CheckBoxButton{"Privacy mode", [this](auto& evt) + { + tracker->privacyMode = evt.IsChecked(); + }}); + manualCalibForm = cam.PopSizer().SubForm(); manualCalibForm->PushSizer(wxVERTICAL) diff --git a/AprilTagTrackers/Tracker.cpp b/AprilTagTrackers/Tracker.cpp index 6ae96412..441180c5 100644 --- a/AprilTagTrackers/Tracker.cpp +++ b/AprilTagTrackers/Tracker.cpp @@ -1052,6 +1052,7 @@ void Tracker::MainLoop() // shallow copy, gray will be cloned from image and used for detection, // so drawing can happen on color image without clone. drawImg = image; + cv::Mat drawImgMasked = cv::Mat::zeros(drawImg.size(), drawImg.type()); april.convertToSingleChannel(image, gray); std::chrono::steady_clock::time_point start, end; @@ -1130,6 +1131,7 @@ void Tracker::MainLoop() cv::projectPoints(point, rvec, tvec, camCalib->cameraMatrix, camCalib->distortionCoeffs, projected); cv::circle(drawImg, projected[0], 5, cv::Scalar(0, 0, 255), 2, 8, 0); + cv::circle(drawImgMasked, projected[0], 5, cv::Scalar(0, 0, 255), 2, 8, 0); if (tracker_pose_valid == 0) // if the pose from steamvr was valid, save the predicted position and rotation { @@ -1192,12 +1194,14 @@ void Tracker::MainLoop() { cv::circle(mask, trackerStatus[i].maskCenter, size, cv::Scalar(255, 0, 0), -1, 8, 0); cv::circle(drawImg, trackerStatus[i].maskCenter, size, cv::Scalar(255, 0, 0), 2, 8, 0); + cv::circle(drawImgMasked, trackerStatus[i].maskCenter, size, cv::Scalar(255, 0, 0), 2, 8, 0); } else // if not, mask a vertical strip top to bottom. This happens every 20 frames if a tracker is lost. { int maskCenter = static_cast(std::trunc(trackerStatus[i].maskCenter.x)); rectangle(mask, cv::Point(maskCenter - size, 0), cv::Point(maskCenter + size, image.rows), cv::Scalar(255, 0, 0), -1); rectangle(drawImg, cv::Point(maskCenter - size, 0), cv::Point(maskCenter + size, image.rows), cv::Scalar(255, 0, 0), 3); + rectangle(drawImgMasked, cv::Point(maskCenter - size, 0), cv::Point(maskCenter + size, image.rows), cv::Scalar(255, 0, 0), 3); } } @@ -1422,6 +1426,7 @@ void Tracker::MainLoop() cv::Quatd q = cv::Quatd::createFromRvec(trackerStatus[i].boardRvec); cv::aruco::drawAxis(drawImg, camCalib->cameraMatrix, camCalib->distortionCoeffs, trackerStatus[i].boardRvec, trackerStatus[i].boardTvec, 0.1); + cv::aruco::drawAxis(drawImgMasked, camCalib->cameraMatrix, camCalib->distortionCoeffs, trackerStatus[i].boardRvec, trackerStatus[i].boardTvec, 0.1); q = cv::Quatd{0, 0, 1, 0} * (wrotation * q) * cv::Quatd{0, 0, 1, 0}; @@ -1516,9 +1521,27 @@ void Tracker::MainLoop() } } + // Copy only the inner white squares of detected markers into drawImgMasked + if (corners.size() > 0) { + cv::Mat mask = cv::Mat::zeros(image.size(), image.type()); + + for (int i = 0; i < corners.size(); i++) + { + std::vector points; + for(int j = 0; j < corners[i].size(); j++){ + points.push_back(corners[i].at(j)); + } + cv::fillConvexPoly(mask, points, cv::Scalar(255, 255, 255), 8, 0); + } + + drawImg.copyTo(drawImgMasked, mask); + } + // draw and display the detections - if (ids.size() > 0) + if (ids.size() > 0) { cv::aruco::drawDetectedMarkers(drawImg, corners, ids); + cv::aruco::drawDetectedMarkers(drawImgMasked, corners, ids); + } end = std::chrono::steady_clock::now(); double frameTime = std::chrono::duration(end - start).count(); @@ -1537,7 +1560,10 @@ void Tracker::MainLoop() rows = image.rows * drawImgSize / image.cols; } - cv::resize(drawImg, outImg, cv::Size(cols, rows)); + if (privacyMode) + cv::resize(drawImgMasked, outImg, cv::Size(cols, rows)); + else + cv::resize(drawImg, outImg, cv::Size(cols, rows)); cv::putText(outImg, std::to_string(frameTime).substr(0, 5), cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255)); if (showTimeProfile) { From 57499c2818511d0be305c6008a007563446b570f Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Sun, 11 Dec 2022 09:47:33 -0500 Subject: [PATCH 05/21] Make search window be centered on the center of tracker --- AprilTagTrackers/Helpers.cpp | 45 +++++++++++++++++ AprilTagTrackers/Helpers.hpp | 1 + AprilTagTrackers/Tracker.cpp | 95 +++++++++++++++++++++++------------- 3 files changed, 106 insertions(+), 35 deletions(-) diff --git a/AprilTagTrackers/Helpers.cpp b/AprilTagTrackers/Helpers.cpp index 59f3e237..81eb0e52 100644 --- a/AprilTagTrackers/Helpers.cpp +++ b/AprilTagTrackers/Helpers.cpp @@ -347,3 +347,48 @@ Quaternion mRot2Quat(const cv::Mat& m) return q; } + +void offsetFromBoardToCameraSpace(std::vector points, cv::Vec3d boardRvec, cv::Vec3d boardTvec, std::vector* out) +{ + //convert the board rotation vector to rotation matrix + cv::Mat rmat; + //for timing our detection + //start = clock(); + + cv::Rodrigues(boardRvec, rmat); + + + //with rotation matrix and translation vector we create the translation matrix + cv::Mat mtranslation = cv::Mat_(4, 4); + for (int x = 0; x < 3; x++) + { + for (int y = 0; y < 3; y++) + { + mtranslation.at(x, y) = rmat.at(x, y); + } + } + for (int x = 0; x < 3; x++) + { + mtranslation.at(x, 3) = 0; + mtranslation.at(3, x) = 0; + } + mtranslation.at(3, 3) = 1; + + cv::Mat rpos = cv::Mat_(4, 1); + + //we transform our model marker from our marker space to board space + out->clear(); + for (int y = 0; y < points.size(); y++) + { + //transform corner of model marker from vector to mat for calculation + rpos.at(0, 0) = points[y].x; + rpos.at(1, 0) = points[y].y; + rpos.at(2, 0) = points[y].z; + rpos.at(3, 0) = 1; + + //multiply point in local space with the translation matrix of our board to put it into camera space + rpos = mtranslation * rpos; + + out->push_back(cv::Point3f(rpos.at(0, 0), rpos.at(1, 0), rpos.at(2, 0))); + } +} diff --git a/AprilTagTrackers/Helpers.hpp b/AprilTagTrackers/Helpers.hpp index 72114bec..62a25118 100644 --- a/AprilTagTrackers/Helpers.hpp +++ b/AprilTagTrackers/Helpers.hpp @@ -27,6 +27,7 @@ bool isRotationMatrix(cv::Mat& R); cv::Vec3f rotationMatrixToEulerAngles(cv::Mat& R); Quaternion mRot2Quat(const cv::Mat& m); cv::Vec3d quat2rodr(double qw, double qx, double qy, double qz); +void offsetFromBoardToCameraSpace(std::vector points, cv::Vec3d boardRvec, cv::Vec3d boardTvec, std::vector* out); template inline void RotateVecByQuat(cv::Vec& out_pos, const cv::Quat& rot) diff --git a/AprilTagTrackers/Tracker.cpp b/AprilTagTrackers/Tracker.cpp index 441180c5..3fdff98b 100644 --- a/AprilTagTrackers/Tracker.cpp +++ b/AprilTagTrackers/Tracker.cpp @@ -1002,37 +1002,42 @@ void Tracker::MainLoop() std::vector> trackers; std::vector> boardIds; std::vector>> boardCorners; + std::vector boardCenters; RefPtr camCalib = calib_config.cameras[0]; RefPtr videoStream = user_config.videoStreams[0]; - // by default, trackers have the center at the center of the main marker. If "Use centers of trackers" is checked, we move it to the center of all marker corners. - if (user_config.trackerCalibCenters) + for (int i = 0; i < this->trackers.size(); i++) { - for (int i = 0; i < this->trackers.size(); i++) - { - boardCorners.push_back(this->trackers[i]->objPoints); - boardIds.push_back(this->trackers[i]->ids); + boardCorners.push_back(this->trackers[i]->objPoints); + boardIds.push_back(this->trackers[i]->ids); + boardCenters.push_back(cv::Point3f(0, 0, 0)); - cv::Point3f boardCenter = cv::Point3f(0, 0, 0); - int numOfCorners = 0; - for (int j = 0; j < boardCorners[i].size(); j++) + int numOfCorners = 0; + for (int j = 0; j < boardCorners[i].size(); j++) + { + for (int k = 0; k < boardCorners[i][j].size(); k++) { - for (int k = 0; k < boardCorners[i][j].size(); k++) - { - boardCenter.x += boardCorners[i][j][k].x; - boardCenter.y += boardCorners[i][j][k].y; - boardCenter.z += boardCorners[i][j][k].z; - numOfCorners++; - } + boardCenters[i].x += boardCorners[i][j][k].x; + boardCenters[i].y += boardCorners[i][j][k].y; + boardCenters[i].z += boardCorners[i][j][k].z; + numOfCorners++; } - boardCenter /= numOfCorners; + } + boardCenters[i] /= numOfCorners; + } + // by default, trackers have the center at the center of the main marker. If "Use centers of trackers" is checked, we move it to the center of all marker corners. + if (user_config.trackerCalibCenters) + { + for (int i = 0; i < this->trackers.size(); i++) + { for (int j = 0; j < boardCorners[i].size(); j++) { for (int k = 0; k < boardCorners[i][j].size(); k++) { - boardCorners[i][j][k] -= boardCenter; + boardCorners[i][j][k] -= boardCenters[i]; + boardCenters[i] = cv::Point3f(0, 0, 0); } } @@ -1120,20 +1125,11 @@ void Tracker::MainLoop() // transform boards position from steamvr space to camera space based on our calibration data rpos = wtransform.inv() * rpos; - std::vector point; - point.push_back(cv::Point3d(rpos[0], rpos[1], rpos[2])); - point.push_back(cv::Point3d(trackerStatus[i].boardTvec)); - std::vector projected; - cv::Vec3d rvec, tvec; + cv::Vec3d rvec; + cv::Vec3d tvec; - // project point from position of tracker in camera 3d space to 2d camera pixel space, and draw a dot there - cv::projectPoints(point, rvec, tvec, camCalib->cameraMatrix, camCalib->distortionCoeffs, projected); - - cv::circle(drawImg, projected[0], 5, cv::Scalar(0, 0, 255), 2, 8, 0); - cv::circle(drawImgMasked, projected[0], 5, cv::Scalar(0, 0, 255), 2, 8, 0); - - if (tracker_pose_valid == 0) // if the pose from steamvr was valid, save the predicted position and rotation + if (tracker_pose_valid == 0) { cv::Quatd q{qw, qx, qy, qz}; @@ -1143,17 +1139,46 @@ void Tracker::MainLoop() q.normalize() * cv::Quatd(0, 0, 1, 0).inv(cv::QUAT_ASSUME_UNIT); - cv::Vec3d rvec = q.toRotVec(); - cv::Vec3d tvec{rpos[0], rpos[1], rpos[2]}; + rvec = q.toRotVec(); + tvec = cv::Vec3d(rpos[0], rpos[1], rpos[2]); + } + else { + tvec = trackerStatus[i].boardTvec; + rvec = trackerStatus[i].boardRvec; + } + + std::vector offsetin, offsetout; + offsetin.push_back(boardCenters[i]); + offsetFromBoardToCameraSpace(offsetin, rvec, tvec, &offsetout); + + std::vector point; + point.push_back(cv::Point3d(rpos[0], rpos[1], rpos[2])); + point.push_back(cv::Point3d(trackerStatus[i].boardTvec)); + point.push_back(cv::Point3d(rpos[0], rpos[1], rpos[2]) + cv::Point3d(offsetout[0])); + point.push_back(cv::Point3d(trackerStatus[i].boardTvec) + cv::Point3d(offsetout[0])); + + std::vector projected; + cv::Vec3d identity_rvec, identity_tvec; + + // project point from position of tracker in camera 3d space to 2d camera pixel space, and draw a dot there + cv::projectPoints(point, identity_rvec, identity_tvec, camCalib->cameraMatrix, camCalib->distortionCoeffs, projected); + cv::circle(drawImg, projected[0], 5, cv::Scalar(0, 0, 255), 2, 8, 0); + cv::circle(drawImgMasked, projected[0], 5, cv::Scalar(0, 0, 255), 2, 8, 0); + cv::circle(drawImg, projected[2], 5, cv::Scalar(0, 255, 255), 2, 8, 0); + cv::circle(drawImgMasked, projected[2], 5, cv::Scalar(0, 255, 255), 2, 8, 0); + cv::circle(drawImg, projected[3], 5, cv::Scalar(255, 0, 255), 2, 8, 0); + cv::circle(drawImgMasked, projected[3], 5, cv::Scalar(255, 0, 255), 2, 8, 0); + if (tracker_pose_valid == 0) // if the pose from steamvr was valid, save the predicted position and rotation + { if (!trackerStatus[i].boardFound) // if tracker was found in previous frame, we use that position for masking. If not, we use position from driver for masking. { - trackerStatus[i].maskCenter = projected[0]; + trackerStatus[i].maskCenter = projected[2]; } else { - trackerStatus[i].maskCenter = projected[1]; + trackerStatus[i].maskCenter = projected[3]; } trackerStatus[i].boardFound = true; @@ -1166,7 +1191,7 @@ void Tracker::MainLoop() { if (trackerStatus[i].boardFound) // if pose is not valid, set everything based on previous known position { - trackerStatus[i].maskCenter = projected[1]; + trackerStatus[i].maskCenter = projected[3]; } trackerStatus[i].boardFoundDriver = false; // do we really need to do this? might be unnecessary From fc2cdcd392ca32213674d069ad3f0f7f0b3b98cb Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Sun, 11 Dec 2022 10:54:57 -0500 Subject: [PATCH 06/21] Set apriltag_detector nthreads to 1 since setting it higher doesn't seem to do anything --- AprilTagTrackers/AprilTagWrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/AprilTagTrackers/AprilTagWrapper.cpp b/AprilTagTrackers/AprilTagWrapper.cpp index 6a2c5db2..6395d380 100644 --- a/AprilTagTrackers/AprilTagWrapper.cpp +++ b/AprilTagTrackers/AprilTagWrapper.cpp @@ -14,7 +14,7 @@ AprilTagWrapper::AprilTagWrapper(const UserConfig& user_config, const ArucoConfi : td{apriltag_detector_create()}, user_config(user_config), aruco_config(aruco_config) { td->quad_decimate = user_config.videoStreams[0]->quadDecimate; - td->nthreads = 4; // TODO: make nthreads a parameter or calculate it somehow + td->nthreads = 1; // TODO: make nthreads a parameter or calculate it somehow apriltag_family_t* tf; if (user_config.markerLibrary == APRILTAG_CIRCULAR) tf = tagCircle21h7_create(); From 21380a6378d7071d861a3587a4c0d13d778d4126 Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Sun, 11 Dec 2022 11:46:53 -0500 Subject: [PATCH 07/21] Don't send tracker if more than 100 miliseconds have passed since tracker was last detected --- AprilTagTrackers/Tracker.cpp | 26 ++++++++++++++++++++------ AprilTagTrackers/Tracker.hpp | 1 + 2 files changed, 21 insertions(+), 6 deletions(-) diff --git a/AprilTagTrackers/Tracker.cpp b/AprilTagTrackers/Tracker.cpp index 3fdff98b..ba544723 100644 --- a/AprilTagTrackers/Tracker.cpp +++ b/AprilTagTrackers/Tracker.cpp @@ -966,6 +966,7 @@ void Tracker::MainLoop() trackerStatus[i].boardRvec = cv::Vec3d(0, 0, 0); trackerStatus[i].boardTvec = cv::Vec3d(0, 0, 0); trackerStatus[i].prevLocValues = std::vector>(7, std::vector()); + trackerStatus[i].last_update_timestamp = std::chrono::milliseconds(0); } // previous values, used for moving median to remove any outliers. @@ -1066,6 +1067,9 @@ void Tracker::MainLoop() bool circularWindow = videoStream->circularWindow; + //last is if pose is valid: 0 is valid, 1 is late (hasnt been updated for more than 0.2 secs), -1 means invalid and is only zeros + std::vector tracker_pose_valid = std::vector(trackerNum, -1); + // if any tracker was lost for longer than 20 frames, mark circularWindow as false for (int i = 0; i < trackerNum; i++) { @@ -1105,9 +1109,6 @@ void Tracker::MainLoop() double qy; double qz; - // last is if pose is valid: 0 is valid, 1 is late (hasnt been updated for more than 0.2 secs), -1 means invalid and is only zeros - int tracker_pose_valid; - // read to our variables ret >> idx; ret >> a; @@ -1117,7 +1118,7 @@ void Tracker::MainLoop() ret >> qx; ret >> qy; ret >> qz; - ret >> tracker_pose_valid; + ret >> tracker_pose_valid[i]; cv::Vec3d rpos{a, b, c}; CoordTransformOVR(rpos); @@ -1129,7 +1130,7 @@ void Tracker::MainLoop() cv::Vec3d rvec; cv::Vec3d tvec; - if (tracker_pose_valid == 0) + if (tracker_pose_valid[i] == 0) { cv::Quatd q{qw, qx, qy, qz}; @@ -1170,7 +1171,7 @@ void Tracker::MainLoop() cv::circle(drawImg, projected[3], 5, cv::Scalar(255, 0, 255), 2, 8, 0); cv::circle(drawImgMasked, projected[3], 5, cv::Scalar(255, 0, 255), 2, 8, 0); - if (tracker_pose_valid == 0) // if the pose from steamvr was valid, save the predicted position and rotation + if (tracker_pose_valid[i] == 0) // if the pose from steamvr was valid, save the predicted position and rotation { if (!trackerStatus[i].boardFound) // if tracker was found in previous frame, we use that position for masking. If not, we use position from driver for masking. { @@ -1352,6 +1353,7 @@ void Tracker::MainLoop() calibControllerLastPress = std::chrono::steady_clock::now(); } april.detectMarkers(gray, &corners, &ids, ¢ers, trackers); + std::chrono::milliseconds current_timestamp = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()); for (int i = 0; i < trackerNum; ++i) { @@ -1372,6 +1374,14 @@ void Tracker::MainLoop() continue; } + else + { + if ((tracker_pose_valid[i] != 0) || ((current_timestamp.count() - trackerStatus[i].last_update_timestamp.count()) <= 100.0)) + { + trackerStatus[i].last_update_timestamp = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()); + } + + } } catch (const std::exception& e) { @@ -1497,6 +1507,10 @@ void Tracker::MainLoop() } } + // Don't send tracker if more than 100 miliseconds have passed since tracker was last detected + if ((current_timestamp.count() - trackerStatus[i].last_update_timestamp.count()) > 100.0) + continue; + // send all the values // frame time is how much time passed since frame was acquired. if (!multicamAutocalib) diff --git a/AprilTagTrackers/Tracker.hpp b/AprilTagTrackers/Tracker.hpp index 5fa4f30d..1b6f339e 100644 --- a/AprilTagTrackers/Tracker.hpp +++ b/AprilTagTrackers/Tracker.hpp @@ -18,6 +18,7 @@ struct TrackerStatus bool boardFound, boardFoundDriver; std::vector> prevLocValues; cv::Point2d maskCenter; + std::chrono::milliseconds last_update_timestamp; }; class Connection; From bb83e98d0f18b94ea30b07b044b24dc21b12a5d4 Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Sun, 11 Dec 2022 11:47:24 -0500 Subject: [PATCH 08/21] Make named pipe address configurable --- AprilTagTrackers/Config.hpp | 1 + AprilTagTrackers/Connection.cpp | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/AprilTagTrackers/Config.hpp b/AprilTagTrackers/Config.hpp index 87be95d1..4b05efb0 100644 --- a/AprilTagTrackers/Config.hpp +++ b/AprilTagTrackers/Config.hpp @@ -50,6 +50,7 @@ class UserConfig : public serial::Serializable REFLECTABLE_FIELD(cfg::Validated, markersPerTracker){45, cfg::GreaterEqual(1)}; REFLECTABLE_FIELD(bool, disableOpenVrApi) = false; REFLECTABLE_FIELD(cfg::List, videoStreams){1}; + REFLECTABLE_FIELD(std::string, ipcAddr) = "ApriltagPipeIn"; REFLECTABLE_END; }; diff --git a/AprilTagTrackers/Connection.cpp b/AprilTagTrackers/Connection.cpp index ca93c270..5b08d39e 100644 --- a/AprilTagTrackers/Connection.cpp +++ b/AprilTagTrackers/Connection.cpp @@ -15,10 +15,10 @@ Connection::Connection(const UserConfig& _user_config) { // TODO: Pass the IPC client* in as an argument #ifdef ATT_OS_WINDOWS - auto* namedPipe = new IPC::WindowsNamedPipe("ApriltagPipeIn"); + auto* namedPipe = new IPC::WindowsNamedPipe(user_config.ipcAddr); bridge_driver.reset(dynamic_cast(namedPipe)); #elif defined(ATT_OS_LINUX) - auto namedPipe = new IPC::UNIXSocket("ApriltagPipeIn"); + auto namedPipe = new IPC::UNIXSocket(user_config.ipcAddr); bridge_driver.reset(dynamic_cast(namedPipe)); #endif } From 0528c3b69b85c414989105916fcfa640eb34881f Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Sun, 11 Dec 2022 15:09:56 -0500 Subject: [PATCH 09/21] Bigger drawImgSize --- AprilTagTrackers/Tracker.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/AprilTagTrackers/Tracker.hpp b/AprilTagTrackers/Tracker.hpp index 1b6f339e..87a082bb 100644 --- a/AprilTagTrackers/Tracker.hpp +++ b/AprilTagTrackers/Tracker.hpp @@ -61,7 +61,7 @@ class Tracker : public ITrackerControl cv::Quatd wrotation; double wscale = 1; - int drawImgSize = 480; + int drawImgSize = 1385; cv::VideoCapture cap; From 7b05d4e239338b97b153bd2da7df0d5da7f93a3b Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Sun, 11 Dec 2022 15:14:09 -0500 Subject: [PATCH 10/21] Improved Camera calibration delay between snapshots --- AprilTagTrackers/Tracker.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/AprilTagTrackers/Tracker.cpp b/AprilTagTrackers/Tracker.cpp index ba544723..d4753572 100644 --- a/AprilTagTrackers/Tracker.cpp +++ b/AprilTagTrackers/Tracker.cpp @@ -380,13 +380,8 @@ void Tracker::CalibrateCameraCharuco() preview.Update(outImg); // if more than one second has passed since last calibration image, add current frame to calibration images - // framesSinceLast++; if (std::chrono::duration(std::chrono::steady_clock::now() - timeOfLast).count() > 1) { - // framesSinceLast = 0; - timeOfLast = std::chrono::steady_clock::now(); - // if any button was pressed - // detect our markers cv::aruco::refineDetectedMarkers(gray, board, markerCorners, markerIds, rejectedCorners); @@ -424,6 +419,8 @@ void Tracker::CalibrateCameraCharuco() } } } + + timeOfLast = std::chrono::steady_clock::now(); } } From 7cffe5c3721928808b617417a2d7e3a0db31b41f Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Sun, 11 Dec 2022 15:32:07 -0500 Subject: [PATCH 11/21] Use hex numbering for tracker names in driver --- AprilTagTrackers/Connection.cpp | 22 +++++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/AprilTagTrackers/Connection.cpp b/AprilTagTrackers/Connection.cpp index 5b08d39e..ae893d20 100644 --- a/AprilTagTrackers/Connection.cpp +++ b/AprilTagTrackers/Connection.cpp @@ -8,6 +8,8 @@ #include #include +#include +#include #include Connection::Connection(const UserConfig& _user_config) @@ -50,10 +52,12 @@ void Connection::Connect() { for (int i = 0; i < user_config.trackerNum - 1; i++) { + std::stringstream ss; + ss << "ApriltagTracker" << std::hex << (i + 1); TrackerConnection temp; temp.TrackerId = i + 1; temp.DriverId = i; - temp.Name = "ApriltagTracker" + std::to_string(i + 1); + temp.Name = ss.str(); connectedTrackers.push_back(temp); } connectedTrackers[0].Role = "TrackerRole_LeftFoot"; @@ -63,10 +67,12 @@ void Connection::Connect() { for (int i = 0; i < user_config.trackerNum - 1; i++) { + std::stringstream ss; + ss << "ApriltagTracker" << std::hex << (i + 1); TrackerConnection temp; temp.TrackerId = i + 1; temp.DriverId = i; - temp.Name = "ApriltagTracker" + std::to_string(i + 1); + temp.Name = ss.str(); temp.Role = "TrackerRole_Waist"; connectedTrackers.push_back(temp); } @@ -75,10 +81,12 @@ void Connection::Connect() { for (int i = 0; i < user_config.trackerNum; i++) { + std::stringstream ss; + ss << "ApriltagTracker" << std::hex << (i); TrackerConnection temp; temp.TrackerId = i; temp.DriverId = i; - temp.Name = "ApriltagTracker" + std::to_string(i); + temp.Name = ss.str(); connectedTrackers.push_back(temp); } connectedTrackers[0].Role = "TrackerRole_Waist"; @@ -89,10 +97,12 @@ void Connection::Connect() { for (int i = 0; i < user_config.trackerNum; i++) { + std::stringstream ss; + ss << "ApriltagTracker" << std::hex << (i); TrackerConnection temp; temp.TrackerId = i; temp.DriverId = i; - temp.Name = "ApriltagTracker" + std::to_string(i); + temp.Name = ss.str(); connectedTrackers.push_back(temp); } connectedTrackers[0].Role = "TrackerRole_LeftFoot"; @@ -102,10 +112,12 @@ void Connection::Connect() { for (int i = 0; i < user_config.trackerNum; i++) { + std::stringstream ss; + ss << "ApriltagTracker" << std::hex << (i + 1); TrackerConnection temp; temp.TrackerId = i; temp.DriverId = i; - temp.Name = "ApriltagTracker" + std::to_string(i + 1); + temp.Name = ss.str(); temp.Role = "TrackerRole_Waist"; connectedTrackers.push_back(temp); } From 919c82a451ca0794c96c13cde429c4d6dc329da5 Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Sun, 11 Dec 2022 15:38:08 -0500 Subject: [PATCH 12/21] Draw lines to identify midpoint on camera preview to help with calibration --- AprilTagTrackers/Tracker.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/AprilTagTrackers/Tracker.cpp b/AprilTagTrackers/Tracker.cpp index d4753572..a45e327b 100644 --- a/AprilTagTrackers/Tracker.cpp +++ b/AprilTagTrackers/Tracker.cpp @@ -188,6 +188,11 @@ void Tracker::CameraLoop() cv::putText(drawImg, std::to_string(static_cast(std::ceil(fps))), cv::Point(10, 60), cv::FONT_HERSHEY_SIMPLEX, 2, cv::Scalar(0, 255, 0), 2); std::string resolution = std::to_string(img.cols) + "x" + std::to_string(img.rows); cv::putText(drawImg, resolution, cv::Point(10, 120), cv::FONT_HERSHEY_SIMPLEX, 2, cv::Scalar(0, 255, 0), 2); + + // Draw lines to identify midpoint on camera preview to help with calibration + cv::line(drawImg, cv::Point(img.cols/2, 0), cv::Point(img.cols/2, img.rows), cv::Scalar(0, 0, 255)); + cv::line(drawImg, cv::Point(0, img.rows/2), cv::Point(img.cols, img.rows/2), cv::Scalar(0, 0, 255)); + if (previewCameraCalibration) drawCalibration(drawImg, *calib_config.cameras[0]); gui->UpdatePreview(drawImg, PreviewId::Camera); From 7b28c7803bcc01938cb7d9f6eafa2d28d0bc8f31 Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Sun, 11 Dec 2022 15:53:52 -0500 Subject: [PATCH 13/21] Scale search window according to tracker distance from camera --- AprilTagTrackers/Tracker.cpp | 20 ++++++++++++-------- AprilTagTrackers/Tracker.hpp | 1 + 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/AprilTagTrackers/Tracker.cpp b/AprilTagTrackers/Tracker.cpp index a45e327b..355ac002 100644 --- a/AprilTagTrackers/Tracker.cpp +++ b/AprilTagTrackers/Tracker.cpp @@ -969,6 +969,7 @@ void Tracker::MainLoop() trackerStatus[i].boardTvec = cv::Vec3d(0, 0, 0); trackerStatus[i].prevLocValues = std::vector>(7, std::vector()); trackerStatus[i].last_update_timestamp = std::chrono::milliseconds(0); + trackerStatus[i].searchSize = (int)(user_config.videoStreams[0]->searchWindow * calib_config.cameras[0]->cameraMatrix.at(0,0)); } // previous values, used for moving median to remove any outliers. @@ -1178,10 +1179,12 @@ void Tracker::MainLoop() if (!trackerStatus[i].boardFound) // if tracker was found in previous frame, we use that position for masking. If not, we use position from driver for masking. { trackerStatus[i].maskCenter = projected[2]; + trackerStatus[i].searchSize = (int)(user_config.videoStreams[0]->searchWindow * calib_config.cameras[0]->cameraMatrix.at(0,0)/point[2].z); } else { trackerStatus[i].maskCenter = projected[3]; + trackerStatus[i].searchSize = (int)(user_config.videoStreams[0]->searchWindow * calib_config.cameras[0]->cameraMatrix.at(0,0)/point[3].z); } trackerStatus[i].boardFound = true; @@ -1195,6 +1198,7 @@ void Tracker::MainLoop() if (trackerStatus[i].boardFound) // if pose is not valid, set everything based on previous known position { trackerStatus[i].maskCenter = projected[3]; + trackerStatus[i].searchSize = (int)(user_config.videoStreams[0]->searchWindow * calib_config.cameras[0]->cameraMatrix.at(0,0)/point[3].z); } trackerStatus[i].boardFoundDriver = false; // do we really need to do this? might be unnecessary @@ -1206,8 +1210,6 @@ void Tracker::MainLoop() cv::Mat dstImage = cv::Mat::zeros(gray.size(), gray.type()); - int size = static_cast(std::trunc(gray.rows * videoStream->searchWindow)); - bool doMasking = false; for (int i = 0; i < trackerNum; i++) // calculate the needed masks for every tracker @@ -1220,16 +1222,18 @@ void Tracker::MainLoop() doMasking = true; if (circularWindow) // if circular window is set mask a circle around the predicted tracker point { - cv::circle(mask, trackerStatus[i].maskCenter, size, cv::Scalar(255, 0, 0), -1, 8, 0); - cv::circle(drawImg, trackerStatus[i].maskCenter, size, cv::Scalar(255, 0, 0), 2, 8, 0); - cv::circle(drawImgMasked, trackerStatus[i].maskCenter, size, cv::Scalar(255, 0, 0), 2, 8, 0); + cv::circle(mask, trackerStatus[i].maskCenter, trackerStatus[i].searchSize, cv::Scalar(255, 0, 0), -1, 8, 0); + cv::circle(drawImg, trackerStatus[i].maskCenter, trackerStatus[i].searchSize, cv::Scalar(255, 0, 0), 2, 8, 0); + cv::circle(drawImgMasked, trackerStatus[i].maskCenter, trackerStatus[i].searchSize, cv::Scalar(255, 0, 0), 2, 8, 0); } else // if not, mask a vertical strip top to bottom. This happens every 20 frames if a tracker is lost. { int maskCenter = static_cast(std::trunc(trackerStatus[i].maskCenter.x)); - rectangle(mask, cv::Point(maskCenter - size, 0), cv::Point(maskCenter + size, image.rows), cv::Scalar(255, 0, 0), -1); - rectangle(drawImg, cv::Point(maskCenter - size, 0), cv::Point(maskCenter + size, image.rows), cv::Scalar(255, 0, 0), 3); - rectangle(drawImgMasked, cv::Point(maskCenter - size, 0), cv::Point(maskCenter + size, image.rows), cv::Scalar(255, 0, 0), 3); + auto lefttop = cv::Point(maskCenter - trackerStatus[i].searchSize, 0); + auto bottomright = cv::Point(maskCenter + trackerStatus[i].searchSize, image.rows); + rectangle(mask, lefttop, bottomright, cv::Scalar(255, 0, 0), -1); + rectangle(drawImg, lefttop, bottomright, cv::Scalar(255, 0, 0), 3); + rectangle(drawImgMasked, lefttop, bottomright, cv::Scalar(255, 0, 0), 3); } } diff --git a/AprilTagTrackers/Tracker.hpp b/AprilTagTrackers/Tracker.hpp index 87a082bb..465ce7d8 100644 --- a/AprilTagTrackers/Tracker.hpp +++ b/AprilTagTrackers/Tracker.hpp @@ -19,6 +19,7 @@ struct TrackerStatus std::vector> prevLocValues; cv::Point2d maskCenter; std::chrono::milliseconds last_update_timestamp; + int searchSize; }; class Connection; From 301fd6b37cba6c4b6ed1633a61227118d88585da Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Sun, 4 Dec 2022 20:01:11 -0500 Subject: [PATCH 14/21] Tracker: Fix buggy measurement of time since frame was captured --- AprilTagTrackers/Tracker.cpp | 59 +++++++++++++++++++++++------------- AprilTagTrackers/Tracker.hpp | 14 ++++++--- 2 files changed, 48 insertions(+), 25 deletions(-) diff --git a/AprilTagTrackers/Tracker.cpp b/AprilTagTrackers/Tracker.cpp index 355ac002..c4440ceb 100644 --- a/AprilTagTrackers/Tracker.cpp +++ b/AprilTagTrackers/Tracker.cpp @@ -201,14 +201,15 @@ void Tracker::CameraLoop() { std::lock_guard lock(cameraImageMutex); // Swap avoids copying the pixel buffer. It only swaps pointers and metadata. - // The pixel buffer from cameraImage can be reused if the size and format matches. - cv::swap(img, cameraImage); + // The pixel buffer from cameraFrame.image can be reused if the size and format matches. + cv::swap(img, cameraFrame.image); // TODO: does opencv really care if img.read is called on the wrong size of matrix? - if (img.size() != cameraImage.size() || img.flags != cameraImage.flags) + if (img.size() != cameraFrame.image.size() || img.flags != cameraFrame.image.flags) { img.release(); } - imageReady = true; + cameraFrame.ready = true; + cameraFrame.time = last_frame_time; } if (connection->PollQuitEvent()) @@ -219,25 +220,31 @@ void Tracker::CameraLoop() gui->SetStatus(false, StatusItem::Camera); } -void Tracker::CopyFreshCameraImageTo(cv::Mat& image) +void Tracker::CopyFreshCameraImageTo(FrameData& frame) { /// TODO: replace with std::condition_variable // Sleep happens between each iteration when the mutex is not locked. for (;; std::this_thread::sleep_for(std::chrono::milliseconds(1))) { std::lock_guard lock(cameraImageMutex); - if (imageReady) + if (cameraFrame.ready) { - imageReady = false; + cameraFrame.ready = false; // Swap metadata and pointers to pixel buffers. - cv::swap(image, cameraImage); + cv::swap(frame.image, cameraFrame.image); // We don't want to overwrite shared data so release the image unless we are the only user of it. - if (!(cameraImage.u && cameraImage.u->refcount == 1)) + if (!(cameraFrame.image.u && cameraFrame.image.u->refcount == 1)) { - cameraImage.release(); + cameraFrame.image.release(); } + frame.ready = true; + frame.time = cameraFrame.time; return; } + else + { + frame.ready = false; + } } } @@ -270,7 +277,7 @@ void Tracker::StartCameraCalib() /// function to calibrate our camera void Tracker::CalibrateCameraCharuco() { - cv::Mat image; + FrameData frame; cv::Mat gray; cv::Mat drawImg; @@ -311,7 +318,8 @@ void Tracker::CalibrateCameraCharuco() int picsTaken = 0; while (mainThreadRunning && cameraRunning) { - CopyFreshCameraImageTo(image); + CopyFreshCameraImageTo(frame); + cv::Mat &image = frame.image; int cols, rows; if (image.cols > image.rows) { @@ -535,7 +543,7 @@ void Tracker::CalibrateCamera() std::vector corner_pts; bool success; - cv::Mat image; + FrameData frame; cv::Mat outImg; int i = 0; @@ -543,13 +551,16 @@ void Tracker::CalibrateCamera() int picNum = user_config.cameraCalibSamples; + int image_cols, image_rows; + while (i < picNum) { if (!mainThreadRunning || !cameraRunning) { return; } - CopyFreshCameraImageTo(image); + CopyFreshCameraImageTo(frame); + cv::Mat &image = frame.image; cv::putText(image, std::to_string(i) + "/" + std::to_string(picNum), cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255)); int cols, rows; if (image.cols > image.rows) @@ -589,11 +600,14 @@ void Tracker::CalibrateCamera() cv::resize(image, outImg, cv::Size(cols, rows)); gui->UpdatePreview(outImg); + + image_cols = image.cols; + image_rows = image.rows; } cv::Mat cameraMatrix, distCoeffs, R, T; - calibrateCamera(objpoints, imgpoints, cv::Size(image.rows, image.cols), cameraMatrix, distCoeffs, R, T); + calibrateCamera(objpoints, imgpoints, cv::Size(image_rows, image_cols), cameraMatrix, distCoeffs, R, T); RefPtr camCalib = calib_config.cameras[0]; @@ -772,7 +786,7 @@ void Tracker::CalibrateTracker() boardRvec.push_back(cv::Vec3d()); boardTvec.push_back(cv::Vec3d()); } - cv::Mat image; + FrameData frame; cv::Mat outImg; cv::Ptr dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); @@ -789,7 +803,8 @@ void Tracker::CalibrateTracker() // run loop until we stop it while (cameraRunning && mainThreadRunning) { - CopyFreshCameraImageTo(image); + CopyFreshCameraImageTo(frame); + cv::Mat &image = frame.image; std::chrono::steady_clock::time_point start; // clock for timing of detection @@ -956,7 +971,8 @@ void Tracker::MainLoop() std::vector> corners; std::vector centers; - cv::Mat image, drawImg, outImg, ycc, gray, cr; + FrameData frame; + cv::Mat drawImg, outImg, ycc, gray, cr; cv::Mat prevImg; @@ -1057,7 +1073,8 @@ void Tracker::MainLoop() while (mainThreadRunning && cameraRunning) // run detection until camera is stopped or the start/stop button is pressed again { - CopyFreshCameraImageTo(image); + CopyFreshCameraImageTo(frame); + cv::Mat &image = frame.image; // shallow copy, gray will be cloned from image and used for detection, // so drawing can happen on color image without clone. drawImg = image; @@ -1090,7 +1107,7 @@ void Tracker::MainLoop() for (int i = 0; i < trackerNum; i++) { - double frameTime = std::chrono::duration(std::chrono::steady_clock::now() - last_frame_time).count(); + double frameTime = std::chrono::duration(std::chrono::steady_clock::now() - frame.time).count(); std::string word; std::istringstream ret = connection->Send("gettrackerpose " + std::to_string(i) + " " + std::to_string(-frameTime - videoStream->latency)); @@ -1479,7 +1496,7 @@ void Tracker::MainLoop() factor = 0.99; end = std::chrono::steady_clock::now(); - double frameTime = std::chrono::duration(end - last_frame_time).count(); + double frameTime = std::chrono::duration(end - frame.time).count(); // Reject detected positions that are behind the camera if (trackerStatus[i].boardTvec[2] < 0) diff --git a/AprilTagTrackers/Tracker.hpp b/AprilTagTrackers/Tracker.hpp index 465ce7d8..d235c2f3 100644 --- a/AprilTagTrackers/Tracker.hpp +++ b/AprilTagTrackers/Tracker.hpp @@ -22,6 +22,13 @@ struct TrackerStatus int searchSize; }; +struct FrameData +{ + bool ready = false; + cv::Mat image; + std::chrono::steady_clock::time_point time; +}; + class Connection; class Tracker : public ITrackerControl @@ -46,7 +53,7 @@ class Tracker : public ITrackerControl private: void CameraLoop(); - void CopyFreshCameraImageTo(cv::Mat& image); + void CopyFreshCameraImageTo(FrameData& frame); void CalibrateCamera(); void CalibrateCameraCharuco(); void CalibrateTracker(); @@ -66,11 +73,10 @@ class Tracker : public ITrackerControl cv::VideoCapture cap; - // cameraImage and imageReady are protected by cameraImageMutex. + // cameraFrame is protected by cameraImageMutex. // Use CopyFreshCameraImageTo in order to get the latest camera image. std::mutex cameraImageMutex; - cv::Mat cameraImage; - bool imageReady = false; + FrameData cameraFrame; std::unique_ptr connection; From e16aa12e517c702b3c0b0287e70df0eefbbb788a Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Sun, 11 Dec 2022 21:40:59 -0500 Subject: [PATCH 15/21] Fix missing null byte when sending IPC commands --- AprilTagTrackers/IPC/UNIXSocket.cpp | 2 +- AprilTagTrackers/IPC/WindowsNamedPipe.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/AprilTagTrackers/IPC/UNIXSocket.cpp b/AprilTagTrackers/IPC/UNIXSocket.cpp index 0073076a..fc7afb42 100644 --- a/AprilTagTrackers/IPC/UNIXSocket.cpp +++ b/AprilTagTrackers/IPC/UNIXSocket.cpp @@ -38,7 +38,7 @@ bool UNIXSocket::send(const std::string& msg, std::string& resp) return false; } - if (::send(socket, msg.c_str(), msg.size(), 0) == -1) + if (::send(socket, msg.c_str(), msg.size() + 1, 0) == -1) { std::cerr << "Failed to send buffer to socket " << this->socket_path << std::endl; close(socket); diff --git a/AprilTagTrackers/IPC/WindowsNamedPipe.cpp b/AprilTagTrackers/IPC/WindowsNamedPipe.cpp index 9e5fee83..12f999a9 100644 --- a/AprilTagTrackers/IPC/WindowsNamedPipe.cpp +++ b/AprilTagTrackers/IPC/WindowsNamedPipe.cpp @@ -26,7 +26,7 @@ bool WindowsNamedPipe::send(const std::string& msg, std::string& resp) if (FAILED(CallNamedPipeA( this->pipe_name.c_str(), // pipe name msg_cstr, // message - msg.size(), // message size + msg.size() + 1, // message size response_buffer, // response BUFFER_SIZE, // response max size &response_length, // response size From b90d60c37d851a3382348303e354ca2d677fc4fc Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Sat, 7 Jan 2023 20:45:06 -0500 Subject: [PATCH 16/21] Added refine trackers stub --- AprilTagTrackers/GUI.hpp | 1 + AprilTagTrackers/GUI/MainFrame.cpp | 5 ++- AprilTagTrackers/Localization.hpp | 8 +++++ AprilTagTrackers/Tracker.cpp | 50 ++++++++++++++++++++++++++++++ AprilTagTrackers/Tracker.hpp | 2 ++ 5 files changed, 65 insertions(+), 1 deletion(-) diff --git a/AprilTagTrackers/GUI.hpp b/AprilTagTrackers/GUI.hpp index 5dffa122..409daf60 100644 --- a/AprilTagTrackers/GUI.hpp +++ b/AprilTagTrackers/GUI.hpp @@ -21,6 +21,7 @@ class ITrackerControl virtual void StartCamera() = 0; virtual void StartCameraCalib() = 0; virtual void StartTrackerCalib() = 0; + virtual void StartTrackerRefine() = 0; virtual void StartConnection() = 0; virtual void Start() = 0; virtual void Stop() = 0; diff --git a/AprilTagTrackers/GUI/MainFrame.cpp b/AprilTagTrackers/GUI/MainFrame.cpp index 7e5391ed..8d3c0493 100644 --- a/AprilTagTrackers/GUI/MainFrame.cpp +++ b/AprilTagTrackers/GUI/MainFrame.cpp @@ -226,7 +226,10 @@ void GUI::MainFrame::CreateCameraPage(RefPtr pages) { tracker->StartTrackerCalib(); }}) - .Add(StretchSpacer{}) + .Add(Button{lc.CAMERA_REFINE_TRACKERS, [this](auto&) + { + tracker->StartTrackerRefine(); + }}) .Add(Label{lc.CAMERA_START_STEAMVR}) .Add(CheckBoxButton{lc.CAMERA_DISABLE_OPENVR_API, [this](auto& evt) { diff --git a/AprilTagTrackers/Localization.hpp b/AprilTagTrackers/Localization.hpp index 2c637ed5..bef31792 100644 --- a/AprilTagTrackers/Localization.hpp +++ b/AprilTagTrackers/Localization.hpp @@ -121,6 +121,7 @@ class Localization : public serial::Serializable T(CAMERA_START_CAMERA) = "1. Start/Stop camera"; T(CAMERA_CALIBRATE_CAMERA) = "2. Calibrate camera"; T(CAMERA_CALIBRATE_TRACKERS) = "3. Calibrate trackers"; + T(CAMERA_REFINE_TRACKERS) = "(optional) Refine trackers"; T(CAMERA_START_STEAMVR) = "4. Start up SteamVR!"; T(CAMERA_CONNECT) = "5. Connect to SteamVR"; T(CAMERA_START_DETECTION) = "6. Start/Stop"; @@ -251,6 +252,13 @@ Yellow: The marker is being calibrated. Hold it still for a second. When all the markers on all trackers are shown as green, press OK to finish calibration.)"; + T(TRACKER_TRACKER_REFINE_INSTRUCTIONS) = + R"(Tracker refinement started! + +Optional tracker calibration refinement using Ceres solver. +Show each tracker to the camera from different angles until each marker turns green, then hit the OK button.)"; + + T(TRACKER_TRACKER_NOTCALIBRATED) = "Trackers not calibrated"; T(TRACKER_STEAMVR_NOTCONNECTED) = "Not connected to SteamVR"; diff --git a/AprilTagTrackers/Tracker.cpp b/AprilTagTrackers/Tracker.cpp index c4440ceb..fb8febc6 100644 --- a/AprilTagTrackers/Tracker.cpp +++ b/AprilTagTrackers/Tracker.cpp @@ -651,6 +651,45 @@ void Tracker::StartTrackerCalib() }); } +void Tracker::StartTrackerRefine() +{ + // check that no other process is running on main thread, check that camera is running and calibrated + if (mainThreadRunning) + { + mainThreadRunning = false; + return; + } + if (!cameraRunning) + { + gui->ShowPopup(lc.TRACKER_CAMERA_NOTRUNNING, PopupStyle::Error); + mainThreadRunning = false; + return; + } + if (calib_config.cameras[0]->cameraMatrix.empty()) + { + gui->ShowPopup(lc.TRACKER_CAMERA_NOTCALIBRATED, PopupStyle::Error); + mainThreadRunning = false; + return; + } + if (!trackersCalibrated) + { + gui->ShowPopup(lc.TRACKER_TRACKER_NOTCALIBRATED, PopupStyle::Error); + mainThreadRunning = false; + return; + } + + // start tracker calibration on another thread + mainThreadRunning = true; + mainThread = std::thread(&Tracker::RefineTracker, this); + mainThread.detach(); + + gui->ShowPrompt(lc.TRACKER_TRACKER_REFINE_INSTRUCTIONS, + [&](bool) + { + mainThreadRunning = false; + }); +} + void Tracker::StartConnection() { connection->StartConnection(); @@ -957,6 +996,17 @@ void Tracker::CalibrateTracker() mainThreadRunning = false; } +void Tracker::RefineTracker() +{ + // run loop until we stop it + while (cameraRunning && mainThreadRunning) + { + // TODO: Implement me + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + mainThreadRunning = false; +} + void Tracker::MainLoop() { diff --git a/AprilTagTrackers/Tracker.hpp b/AprilTagTrackers/Tracker.hpp index d235c2f3..6318dcdb 100644 --- a/AprilTagTrackers/Tracker.hpp +++ b/AprilTagTrackers/Tracker.hpp @@ -42,6 +42,7 @@ class Tracker : public ITrackerControl void StartCamera() override; void StartCameraCalib() override; void StartTrackerCalib() override; + void StartTrackerRefine() override; void StartConnection() override; void Start() override; void Stop() override; @@ -57,6 +58,7 @@ class Tracker : public ITrackerControl void CalibrateCamera(); void CalibrateCameraCharuco(); void CalibrateTracker(); + void RefineTracker(); void MainLoop(); void HandleConnectionErrors(); From 0ee88241239af705e15f1c0dca8b34e57332af69 Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Mon, 9 Jan 2023 12:01:43 -0500 Subject: [PATCH 17/21] Added Ceres Solver library build dependency --- AprilTagTrackers/CMakeLists.txt | 5 +++++ AprilTagTrackers/GUI/MainFrame.cpp | 3 +++ AprilTagTrackers/MyApp.cpp | 3 +++ AprilTagTrackers/license.hpp | 29 +++++++++++++++++++++++++++++ vcpkg.json | 9 ++++++++- 5 files changed, 48 insertions(+), 1 deletion(-) diff --git a/AprilTagTrackers/CMakeLists.txt b/AprilTagTrackers/CMakeLists.txt index 23794b9b..6b445c47 100644 --- a/AprilTagTrackers/CMakeLists.txt +++ b/AprilTagTrackers/CMakeLists.txt @@ -53,6 +53,8 @@ find_package(libusb CONFIG REQUIRED) find_package(apriltag CONFIG REQUIRED) find_package(openvr CONFIG REQUIRED) find_package(doctest CONFIG REQUIRED) +find_package(Ceres CONFIG REQUIRED) +find_package(glog CONFIG REQUIRED) target_link_libraries(AprilTagTrackers PRIVATE Threads::Threads @@ -63,10 +65,13 @@ target_link_libraries(AprilTagTrackers PRIVATE openvr::openvr_api doctest::doctest common::semver + Ceres::ceres + glog::glog ) target_include_directories(AprilTagTrackers SYSTEM PRIVATE ${wxWidgets_INCLUDE_DIRS} ${LIBUSB_INCLUDE_DIRS} + "ceres" ) target_compile_definitions(AprilTagTrackers PRIVATE wxDEBUG_LEVEL=$ diff --git a/AprilTagTrackers/GUI/MainFrame.cpp b/AprilTagTrackers/GUI/MainFrame.cpp index 8d3c0493..6884cf42 100644 --- a/AprilTagTrackers/GUI/MainFrame.cpp +++ b/AprilTagTrackers/GUI/MainFrame.cpp @@ -422,6 +422,9 @@ void GUI::MainFrame::CreateLicensePage(RefPtr pages) auto cvLicense = NewWindow(nb, wxID_ANY, std::string(OPENCV_LICENSE), wxDefaultPosition, wxDefaultSize, wxTE_MULTILINE | wxTE_READONLY); nb->AddPage(cvLicense, "OpenCV"); + auto ceresSolverLicense = NewWindow(nb, wxID_ANY, std::string(CERES_SOLVER_LICENSE), wxDefaultPosition, + wxDefaultSize, wxTE_MULTILINE | wxTE_READONLY); + nb->AddPage(ceresSolverLicense, "Ceres Solver"); } /// Query OpenCV to find available camera api indices diff --git a/AprilTagTrackers/MyApp.cpp b/AprilTagTrackers/MyApp.cpp index d7a7bfb5..9e4a98a4 100644 --- a/AprilTagTrackers/MyApp.cpp +++ b/AprilTagTrackers/MyApp.cpp @@ -3,6 +3,7 @@ #include "utils/Assert.hpp" #include "utils/Log.hpp" +#include #include wxIMPLEMENT_APP(MyApp); // NOLINT @@ -42,6 +43,8 @@ bool MyApp::OnInit() tracker = std::make_unique(userConfig, calibConfig, arucoConfig, lc); gui = std::make_unique(tracker, lc, userConfig); + google::InitGoogleLogging("AprilTagTrackers"); + return true; } diff --git a/AprilTagTrackers/license.hpp b/AprilTagTrackers/license.hpp index 1ead928e..950fdcfa 100644 --- a/AprilTagTrackers/license.hpp +++ b/AprilTagTrackers/license.hpp @@ -52,3 +52,32 @@ distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.)"; + +static constexpr std::string_view CERES_SOLVER_LICENSE = +R"(Ceres Solver - A fast non-linear least squares minimizer +Copyright 2015 Google Inc. All rights reserved. +http://ceres-solver.org/ + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +* Neither the name of Google Inc. nor the names of its contributors may be + used to endorse or promote products derived from this software without + specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE.)"; diff --git a/vcpkg.json b/vcpkg.json index 13252d31..dd48089f 100644 --- a/vcpkg.json +++ b/vcpkg.json @@ -27,7 +27,14 @@ "libusb", "apriltag", "openvr", - "doctest" + "doctest", + { + "name": "ceres", + "default-features": false, + "features": [ + "eigensparse" + ] + } ], "builtin-baseline": "f93ba152d55e1d243160e690bc302ffe8638358e" } From d4273f9a3d694c809396888cb94773570fa6b5e0 Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Mon, 9 Jan 2023 12:06:07 -0500 Subject: [PATCH 18/21] Capture data needed for solving bundle adjustment problem --- AprilTagTrackers/CMakeLists.txt | 2 + AprilTagTrackers/Tracker.cpp | 272 ++++++++++++++++++- AprilTagTrackers/ceres/bal_problem.cpp | 358 +++++++++++++++++++++++++ AprilTagTrackers/ceres/bal_problem.hpp | 113 ++++++++ 4 files changed, 742 insertions(+), 3 deletions(-) create mode 100644 AprilTagTrackers/ceres/bal_problem.cpp create mode 100644 AprilTagTrackers/ceres/bal_problem.hpp diff --git a/AprilTagTrackers/CMakeLists.txt b/AprilTagTrackers/CMakeLists.txt index 6b445c47..5b295ce6 100644 --- a/AprilTagTrackers/CMakeLists.txt +++ b/AprilTagTrackers/CMakeLists.txt @@ -31,6 +31,8 @@ target_sources(AprilTagTrackers PRIVATE ps3eye/ps3eye.cpp ps3eye/PSEyeVideoCapture.cpp + + ceres/bal_problem.cpp ) if (WIN32) diff --git a/AprilTagTrackers/Tracker.cpp b/AprilTagTrackers/Tracker.cpp index fb8febc6..e6123d14 100644 --- a/AprilTagTrackers/Tracker.cpp +++ b/AprilTagTrackers/Tracker.cpp @@ -6,8 +6,9 @@ #include "ImageDrawing.hpp" #include "utils/Assert.hpp" -#include +#include #include +#include #include #include #include @@ -998,12 +999,277 @@ void Tracker::CalibrateTracker() void Tracker::RefineTracker() { + int markersPerTracker = user_config.markersPerTracker; + int trackerNum = user_config.trackerNum; + + + AprilTagWrapper april{user_config, aruco_config}; + + FrameData frame; + cv::Mat outImg; + + // setup all variables that need to be stored for each tracker and initialize them + std::vector trackerStatus = std::vector(trackerNum, TrackerStatus()); + for (int i = 0; i < trackerStatus.size(); i++) + { + trackerStatus[i].boardFound = false; + trackerStatus[i].boardRvec = cv::Vec3d(0, 0, 0); + trackerStatus[i].boardTvec = cv::Vec3d(0, 0, 0); + trackerStatus[i].prevLocValues = std::vector>(7, std::vector()); + trackerStatus[i].last_update_timestamp = std::chrono::milliseconds(0); + trackerStatus[i].searchSize = (int)(user_config.videoStreams[0]->searchWindow * calib_config.cameras[0]->cameraMatrix.at(0,0)); + } + + RefPtr camCalib = calib_config.cameras[0]; + + auto preview = gui->CreatePreviewControl(); + + typedef struct { + int point_id; + int camera_id; + cv::Point2f observed; + } Observation; + typedef struct { + cv::Vec3d rvec; + cv::Vec3d tvec; + cv::Mat cameraMatrix; + cv::Mat distCoeffs; + } Camera; + + int num_cameras = 0; + int num_points = 0; + int num_observations = 0; + int num_parameters = 0; + + auto point_index = std::vector(); + auto camera_index = std::vector(); + auto observations = std::vector(); + auto parameters = std::vector(); + + int trackerBeingProcessed = -1; + int frameNumber = 0; + // run loop until we stop it while (cameraRunning && mainThreadRunning) { - // TODO: Implement me - std::this_thread::sleep_for(std::chrono::milliseconds(1)); + CopyFreshCameraImageTo(frame); + cv::Mat &image = frame.image; + + std::chrono::steady_clock::time_point start, end; + // clock for timing of detection + start = std::chrono::steady_clock::now(); + + // detect and draw all markers on image + std::vector ids; + std::vector> corners; + std::vector centers; + + april.detectMarkers(image, &corners, &ids, ¢ers, trackers); + + cv::aruco::drawDetectedMarkers(image, corners, cv::noArray(), cv::Scalar(255, 0, 0)); // draw all markers blue. We will overwrite this with other colors for markers that are part of any of the trackers that we use + + for (int i = 0; i < trackerNum; ++i) + { + + // estimate the pose of current board + try + { + if (cv::aruco::estimatePoseBoard(corners, ids, trackers[i], camCalib->cameraMatrix, camCalib->distortionCoeffs, trackerStatus[i].boardRvec, trackerStatus[i].boardTvec, trackerStatus[i].boardFound && user_config.usePredictive) <= 0) + { + for (int j = 0; j < 6; j++) + { + // remove first of the previously saved values + if (trackerStatus[i].prevLocValues[j].size() > 0) + trackerStatus[i].prevLocValues[j].erase(trackerStatus[i].prevLocValues[j].begin()); + } + trackerStatus[i].boardFound = false; + + continue; + } + } + catch (const std::exception& e) + { + // on rare occasions, detection crashes. Should be very rare and indicate something wrong with camera or tracker calibration + ATT_LOG_ERROR(e.what()); + gui->ShowPopup(lc.TRACKER_DETECTION_SOMETHINGWRONG, PopupStyle::Error); + mainThreadRunning = false; + return; + } + trackerStatus[i].boardFound = true; + + // Reject detected positions that are behind the camera + if (trackerStatus[i].boardTvec[2] < 0) + { + trackerStatus[i].boardFound = false; + continue; + } + + // Figure out the camera aspect ratio, XZ and YZ ratio limits + double aspectRatio = static_cast(image.cols) / static_cast(image.rows); + double XZratioLimit = 0.5 * static_cast(image.cols) / camCalib->cameraMatrix.at(0,0); + double YZratioLimit = 0.5 * static_cast(image.rows) / camCalib->cameraMatrix.at(1,1); + + // Figure out whether X or Y dimension is most likely to go outside the camera field of view + if (std::abs(trackerStatus[i].boardTvec[0] / trackerStatus[i].boardTvec[1]) > aspectRatio) + { + // Reject detections when XZ coordinate ratio goes out of camera FOV + if (std::abs(trackerStatus[i].boardTvec[0] / trackerStatus[i].boardTvec[2]) > XZratioLimit) + { + trackerStatus[i].boardFound = false; + continue; + } + } + else + { + // Reject detections when YZ coordinate ratio goes out of camera FOV + if (std::abs(trackerStatus[i].boardTvec[1] / trackerStatus[i].boardTvec[2]) > YZratioLimit) + { + trackerStatus[i].boardFound = false; + continue; + } + } + + cv::aruco::drawAxis(image, camCalib->cameraMatrix, camCalib->distortionCoeffs, trackerStatus[i].boardRvec, trackerStatus[i].boardTvec, 0.1); + + // TODO: Modify function to be able process more than one tracker at the same time + if ((trackerBeingProcessed != -1) && (i != trackerBeingProcessed)) + continue; + + trackerBeingProcessed = i; + + if ((frameNumber++ % (user_config.videoStreams[0]->camera.fps / 3)) != 0) continue; + + int tracker_id_begin = i * markersPerTracker; + int tracker_id_end = (i + 1) * markersPerTracker; + + // Add camera into parameter block + parameters.push_back(trackerStatus[i].boardRvec[0]); + parameters.push_back(trackerStatus[i].boardRvec[1]); + parameters.push_back(trackerStatus[i].boardRvec[2]); + parameters.push_back(trackerStatus[i].boardTvec[0]); + parameters.push_back(trackerStatus[i].boardTvec[1]); + parameters.push_back(trackerStatus[i].boardTvec[2]); + parameters.push_back(camCalib->cameraMatrix.at(0,0)); + parameters.push_back(camCalib->cameraMatrix.at(1,1)); + parameters.push_back(camCalib->cameraMatrix.at(0,2)); + parameters.push_back(camCalib->cameraMatrix.at(1,2)); + parameters.push_back(camCalib->distortionCoeffs.at(0,0)); + parameters.push_back(camCalib->distortionCoeffs.at(0,1)); + parameters.push_back(camCalib->distortionCoeffs.at(0,2)); + parameters.push_back(camCalib->distortionCoeffs.at(0,3)); + parameters.push_back(camCalib->distortionCoeffs.at(0,4)); + num_parameters += 15; + + // Add detected points into observations + for (int j = 0; j < ids.size(); ++j) + { + // does marker ID belong to current tracker? + if ((tracker_id_begin <= ids[j]) && (ids[j] < tracker_id_end)) + { + for (int k = 0; k < corners[j].size(); ++k) + { + point_index.push_back((ids[j] - tracker_id_begin) * 4 + k); + camera_index.push_back(num_cameras); + observations.push_back(corners[j][k].x); + observations.push_back(corners[j][k].y); + num_observations++; + } + } + } + + num_cameras++; + } + + end = std::chrono::steady_clock::now(); + double frameTime = std::chrono::duration(end - start).count(); + + if (gui->IsPreviewVisible()) + { + // resize image, then show it + int cols, rows; + if (image.cols > image.rows) + { + cols = image.cols * drawImgSize / image.rows; + rows = drawImgSize; + } + else + { + cols = drawImgSize; + rows = image.rows * drawImgSize / image.cols; + } + + cv::resize(image, outImg, cv::Size(cols, rows)); + cv::putText(outImg, std::to_string(frameTime).substr(0, 5), cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255)); + cv::putText(outImg, std::to_string(num_cameras), cv::Point(10, 70), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255)); + if (showTimeProfile) + { + april.drawTimeProfile(outImg, cv::Point(10, 60)); + } + gui->UpdatePreview(outImg); + } + // time of marker detection + } + + // Add tracker 3d points into parameter block + // for (int i = 0; i < trackerNum; ++i) + if (trackerBeingProcessed != -1) + { + int i = trackerBeingProcessed; + + const std::vector& ids = trackers[i]->ids; + const std::vector > objPoints = trackers[i]->objPoints; + + int first_id = i * markersPerTracker; + int after_last_id = first_id; + + for (int j = 0; j < ids.size(); ++j) + { + if (ids[j] > after_last_id) + after_last_id = ids[j]; + } + + ++after_last_id; + + for (int j = first_id; j < after_last_id; ++j) + { + auto it = std::find(ids.begin(), ids.end(), j); + auto index = std::distance(ids.begin(), it); + + if (index < objPoints.size()) + { + for (int k = 0; k < 4; ++k) + { + parameters.push_back(objPoints[index][k].x); + parameters.push_back(objPoints[index][k].y); + parameters.push_back(objPoints[index][k].z); + num_points += 1; + num_parameters += 3; + } + } + else + { + for (int k = 0; k < 4; ++k) + { + parameters.push_back(0.0); + parameters.push_back(0.0); + parameters.push_back(0.0); + num_points += 1; + num_parameters += 3; + } + } + } + } + + if (trackerBeingProcessed != -1) + { + ceres::examples::BALProblem bal_problem(num_cameras, num_points, num_observations, num_parameters, point_index.data(), camera_index.data(), observations.data(), parameters.data()); + +#ifdef ATT_DEBUG + bal_problem.WriteToFile("bal_problem.txt"); +#endif + } + mainThreadRunning = false; } diff --git a/AprilTagTrackers/ceres/bal_problem.cpp b/AprilTagTrackers/ceres/bal_problem.cpp new file mode 100644 index 00000000..1d888c6e --- /dev/null +++ b/AprilTagTrackers/ceres/bal_problem.cpp @@ -0,0 +1,358 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "bal_problem.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "Eigen/Core" +#include "ceres/rotation.h" +#include "glog/logging.h" + +namespace ceres::examples { +namespace { +using VectorRef = Eigen::Map; +using ConstVectorRef = Eigen::Map; + +template +void FscanfOrDie(FILE* fptr, const char* format, T* value) { + int num_scanned = fscanf(fptr, format, value); + if (num_scanned != 1) { + LOG(FATAL) << "Invalid UW data file."; + } +} + +void PerturbPoint3(std::function dist, double* point) { + for (int i = 0; i < 3; ++i) { + point[i] += dist(); + } +} + +double Median(std::vector* data) { + int n = data->size(); + auto mid_point = data->begin() + n / 2; + std::nth_element(data->begin(), mid_point, data->end()); + return *mid_point; +} + +} // namespace + +BALProblem::BALProblem(const std::string& filename, bool use_quaternions) { + FILE* fptr = fopen(filename.c_str(), "r"); + + if (fptr == nullptr) { + LOG(FATAL) << "Error: unable to open file " << filename; + return; + }; + + // This will die horribly on invalid files. Them's the breaks. + FscanfOrDie(fptr, "%d", &num_cameras_); + FscanfOrDie(fptr, "%d", &num_points_); + FscanfOrDie(fptr, "%d", &num_observations_); + + VLOG(1) << "Header: " << num_cameras_ << " " << num_points_ << " " + << num_observations_; + + has_buffer_ownership_ = true; + point_index_ = new int[num_observations_]; + camera_index_ = new int[num_observations_]; + observations_ = new double[2 * num_observations_]; + + num_parameters_ = CAMERA_BLOCK_SIZE * num_cameras_ + 3 * num_points_; + parameters_ = new double[num_parameters_]; + + for (int i = 0; i < num_observations_; ++i) { + FscanfOrDie(fptr, "%d", camera_index_ + i); + FscanfOrDie(fptr, "%d", point_index_ + i); + for (int j = 0; j < 2; ++j) { + FscanfOrDie(fptr, "%lf", observations_ + 2 * i + j); + } + } + + for (int i = 0; i < num_parameters_; ++i) { + FscanfOrDie(fptr, "%lf", parameters_ + i); + } + + fclose(fptr); + + use_quaternions_ = use_quaternions; + if (use_quaternions) { + // Switch the angle-axis rotations to quaternions. + num_parameters_ = (CAMERA_BLOCK_SIZE + 1) * num_cameras_ + 3 * num_points_; + auto* quaternion_parameters = new double[num_parameters_]; + double* original_cursor = parameters_; + double* quaternion_cursor = quaternion_parameters; + for (int i = 0; i < num_cameras_; ++i) { + AngleAxisToQuaternion(original_cursor, quaternion_cursor); + quaternion_cursor += 4; + original_cursor += 3; + for (int j = 4; j < (CAMERA_BLOCK_SIZE + 1); ++j) { + *quaternion_cursor++ = *original_cursor++; + } + } + // Copy the rest of the points. + for (int i = 0; i < 3 * num_points_; ++i) { + *quaternion_cursor++ = *original_cursor++; + } + // Swap in the quaternion parameters. + delete[] parameters_; + parameters_ = quaternion_parameters; + } +} + +BALProblem::BALProblem(int num_cameras, int num_points, int num_observations, int num_parameters, int* point_index, int* camera_index, double* observations, double* parameters) + : num_cameras_(num_cameras) + , num_points_(num_points) + , num_observations_(num_observations) + , num_parameters_(num_parameters) + , use_quaternions_(false) + , point_index_(point_index) + , camera_index_(camera_index) + , observations_(observations) + , parameters_(parameters) + , has_buffer_ownership_(false) +{ } + +// This function writes the problem to a file in the same format that +// is read by the constructor. +void BALProblem::WriteToFile(const std::string& filename) const { + FILE* fptr = fopen(filename.c_str(), "w"); + + if (fptr == nullptr) { + LOG(FATAL) << "Error: unable to open file " << filename; + return; + }; + + fprintf(fptr, "%d %d %d\n", num_cameras_, num_points_, num_observations_); + + for (int i = 0; i < num_observations_; ++i) { + fprintf(fptr, "%d %d", camera_index_[i], point_index_[i]); + for (int j = 0; j < 2; ++j) { + fprintf(fptr, " %g", observations_[2 * i + j]); + } + fprintf(fptr, "\n"); + } + + for (int i = 0; i < num_cameras(); ++i) { + double angleaxis[CAMERA_BLOCK_SIZE]; + if (use_quaternions_) { + // Output in angle-axis format. + QuaternionToAngleAxis(parameters_ + (CAMERA_BLOCK_SIZE + 1) * i, angleaxis); + memcpy(angleaxis + 3, parameters_ + (CAMERA_BLOCK_SIZE + 1) * i + 4, 6 * sizeof(double)); + } else { + memcpy(angleaxis, parameters_ + CAMERA_BLOCK_SIZE * i, CAMERA_BLOCK_SIZE * sizeof(double)); + } + for (double coeff : angleaxis) { + fprintf(fptr, "%.16g\n", coeff); + } + } + + const double* points = parameters_ + camera_block_size() * num_cameras_; + for (int i = 0; i < num_points(); ++i) { + const double* point = points + i * point_block_size(); + for (int j = 0; j < point_block_size(); ++j) { + fprintf(fptr, "%.16g\n", point[j]); + } + } + + fclose(fptr); +} + +// Write the problem to a PLY file for inspection in Meshlab or CloudCompare. +void BALProblem::WriteToPLYFile(const std::string& filename) const { + std::ofstream of(filename.c_str()); + + of << "ply" << '\n' + << "format ascii 1.0" << '\n' + << "element vertex " << num_cameras_ + num_points_ << '\n' + << "property float x" << '\n' + << "property float y" << '\n' + << "property float z" << '\n' + << "property uchar red" << '\n' + << "property uchar green" << '\n' + << "property uchar blue" << '\n' + << "end_header" << std::endl; + + // Export extrinsic data (i.e. camera centers) as green points. + double angle_axis[3]; + double center[3]; + for (int i = 0; i < num_cameras(); ++i) { + const double* camera = cameras() + camera_block_size() * i; + CameraToAngleAxisAndCenter(camera, angle_axis, center); + of << center[0] << ' ' << center[1] << ' ' << center[2] << " 0 255 0" + << '\n'; + } + + // Export the structure (i.e. 3D Points) as white points. + const double* points = parameters_ + camera_block_size() * num_cameras_; + for (int i = 0; i < num_points(); ++i) { + const double* point = points + i * point_block_size(); + for (int j = 0; j < point_block_size(); ++j) { + of << point[j] << ' '; + } + of << "255 255 255\n"; + } + of.close(); +} + +void BALProblem::CameraToAngleAxisAndCenter(const double* camera, + double* angle_axis, + double* center) const { + VectorRef angle_axis_ref(angle_axis, 3); + if (use_quaternions_) { + QuaternionToAngleAxis(camera, angle_axis); + } else { + angle_axis_ref = ConstVectorRef(camera, 3); + } + + // c = -R't + Eigen::VectorXd inverse_rotation = -angle_axis_ref; + AngleAxisRotatePoint( + inverse_rotation.data(), camera + camera_block_size() - 6, center); + VectorRef(center, 3) *= -1.0; +} + +void BALProblem::AngleAxisAndCenterToCamera(const double* angle_axis, + const double* center, + double* camera) const { + ConstVectorRef angle_axis_ref(angle_axis, 3); + if (use_quaternions_) { + AngleAxisToQuaternion(angle_axis, camera); + } else { + VectorRef(camera, 3) = angle_axis_ref; + } + + // t = -R * c + AngleAxisRotatePoint(angle_axis, center, camera + camera_block_size() - 6); + VectorRef(camera + camera_block_size() - 6, 3) *= -1.0; +} + +void BALProblem::Normalize() { + // Compute the marginal median of the geometry. + std::vector tmp(num_points_); + Eigen::Vector3d median; + double* points = mutable_points(); + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < num_points_; ++j) { + tmp[j] = points[3 * j + i]; + } + median(i) = Median(&tmp); + } + + for (int i = 0; i < num_points_; ++i) { + VectorRef point(points + 3 * i, 3); + tmp[i] = (point - median).lpNorm<1>(); + } + + const double median_absolute_deviation = Median(&tmp); + + // Scale so that the median absolute deviation of the resulting + // reconstruction is 100. + const double scale = 100.0 / median_absolute_deviation; + + VLOG(2) << "median: " << median.transpose(); + VLOG(2) << "median absolute deviation: " << median_absolute_deviation; + VLOG(2) << "scale: " << scale; + + // X = scale * (X - median) + for (int i = 0; i < num_points_; ++i) { + VectorRef point(points + 3 * i, 3); + point = scale * (point - median); + } + + double* cameras = mutable_cameras(); + double angle_axis[3]; + double center[3]; + for (int i = 0; i < num_cameras_; ++i) { + double* camera = cameras + camera_block_size() * i; + CameraToAngleAxisAndCenter(camera, angle_axis, center); + // center = scale * (center - median) + VectorRef(center, 3) = scale * (VectorRef(center, 3) - median); + AngleAxisAndCenterToCamera(angle_axis, center, camera); + } +} + +void BALProblem::Perturb(const double rotation_sigma, + const double translation_sigma, + const double point_sigma) { + CHECK_GE(point_sigma, 0.0); + CHECK_GE(rotation_sigma, 0.0); + CHECK_GE(translation_sigma, 0.0); + std::mt19937 prng; + std::normal_distribution point_noise_distribution(0.0, point_sigma); + double* points = mutable_points(); + if (point_sigma > 0) { + for (int i = 0; i < num_points_; ++i) { + PerturbPoint3(std::bind(point_noise_distribution, std::ref(prng)), + points + 3 * i); + } + } + + std::normal_distribution rotation_noise_distribution(0.0, + point_sigma); + std::normal_distribution translation_noise_distribution( + 0.0, translation_sigma); + for (int i = 0; i < num_cameras_; ++i) { + double* camera = mutable_cameras() + camera_block_size() * i; + + double angle_axis[3]; + double center[3]; + // Perturb in the rotation of the camera in the angle-axis + // representation. + CameraToAngleAxisAndCenter(camera, angle_axis, center); + if (rotation_sigma > 0.0) { + PerturbPoint3(std::bind(rotation_noise_distribution, std::ref(prng)), + angle_axis); + } + AngleAxisAndCenterToCamera(angle_axis, center, camera); + + if (translation_sigma > 0.0) { + PerturbPoint3(std::bind(translation_noise_distribution, std::ref(prng)), + camera + camera_block_size() - 6); + } + } +} + +BALProblem::~BALProblem() { + if (has_buffer_ownership_) { + delete[] point_index_; + delete[] camera_index_; + delete[] observations_; + delete[] parameters_; + } +} + +} // namespace ceres::examples diff --git a/AprilTagTrackers/ceres/bal_problem.hpp b/AprilTagTrackers/ceres/bal_problem.hpp new file mode 100644 index 00000000..c636d287 --- /dev/null +++ b/AprilTagTrackers/ceres/bal_problem.hpp @@ -0,0 +1,113 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Class for loading and holding in memory bundle adjustment problems +// from the BAL (Bundle Adjustment in the Large) dataset from the +// University of Washington. +// +// For more details see http://grail.cs.washington.edu/projects/bal/ + +#ifndef CERES_EXAMPLES_BAL_PROBLEM_H_ +#define CERES_EXAMPLES_BAL_PROBLEM_H_ + +#include + +#define CAMERA_BLOCK_SIZE 15 + +namespace ceres::examples { + +class BALProblem { + public: + explicit BALProblem(const std::string& filename, bool use_quaternions); + explicit BALProblem(int num_cameras, int num_points, int num_observations, int num_parameters, int* point_index, int* camera_index, double* observations, double* parameters); + ~BALProblem(); + + void WriteToFile(const std::string& filename) const; + void WriteToPLYFile(const std::string& filename) const; + + // Move the "center" of the reconstruction to the origin, where the + // center is determined by computing the marginal median of the + // points. The reconstruction is then scaled so that the median + // absolute deviation of the points measured from the origin is + // 100.0. + // + // The reprojection error of the problem remains the same. + void Normalize(); + + // Perturb the camera pose and the geometry with random normal + // numbers with corresponding standard deviations. + void Perturb(const double rotation_sigma, + const double translation_sigma, + const double point_sigma); + + // clang-format off + int camera_block_size() const { return use_quaternions_ ? CAMERA_BLOCK_SIZE+1 : CAMERA_BLOCK_SIZE; } + int point_block_size() const { return 3; } + int num_cameras() const { return num_cameras_; } + int num_points() const { return num_points_; } + int num_observations() const { return num_observations_; } + int num_parameters() const { return num_parameters_; } + const int* point_index() const { return point_index_; } + const int* camera_index() const { return camera_index_; } + const double* observations() const { return observations_; } + const double* parameters() const { return parameters_; } + const double* cameras() const { return parameters_; } + double* mutable_cameras() { return parameters_; } + // clang-format on + double* mutable_points() { + return parameters_ + camera_block_size() * num_cameras_; + } + + private: + void CameraToAngleAxisAndCenter(const double* camera, + double* angle_axis, + double* center) const; + + void AngleAxisAndCenterToCamera(const double* angle_axis, + const double* center, + double* camera) const; + int num_cameras_; + int num_points_; + int num_observations_; + int num_parameters_; + bool use_quaternions_; + + int* point_index_; + int* camera_index_; + double* observations_; + // The parameter vector is laid out as follows + // [camera_1, ..., camera_n, point_1, ..., point_m] + double* parameters_; + bool has_buffer_ownership_; +}; + +} // namespace ceres::examples + +#endif // CERES_EXAMPLES_BAL_PROBLEM_H_ From 292979f7c3ab1b01a2864e2c414381ec32e24ed8 Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Mon, 9 Jan 2023 12:08:00 -0500 Subject: [PATCH 19/21] Perform bundle adjustment using Ceres Solver --- AprilTagTrackers/CMakeLists.txt | 1 + AprilTagTrackers/Tracker.cpp | 2 + AprilTagTrackers/ceres/bundle_adjuster.cpp | 456 ++++++++++++++++++ AprilTagTrackers/ceres/bundle_adjuster.hpp | 11 + .../ceres/opencv_reprojection_error.hpp | 298 ++++++++++++ AprilTagTrackers/ceres/positional_error.hpp | 46 ++ .../ceres/snavely_reprojection_error.hpp | 179 +++++++ 7 files changed, 993 insertions(+) create mode 100644 AprilTagTrackers/ceres/bundle_adjuster.cpp create mode 100644 AprilTagTrackers/ceres/bundle_adjuster.hpp create mode 100644 AprilTagTrackers/ceres/opencv_reprojection_error.hpp create mode 100644 AprilTagTrackers/ceres/positional_error.hpp create mode 100644 AprilTagTrackers/ceres/snavely_reprojection_error.hpp diff --git a/AprilTagTrackers/CMakeLists.txt b/AprilTagTrackers/CMakeLists.txt index 5b295ce6..38e7357b 100644 --- a/AprilTagTrackers/CMakeLists.txt +++ b/AprilTagTrackers/CMakeLists.txt @@ -33,6 +33,7 @@ target_sources(AprilTagTrackers PRIVATE ps3eye/PSEyeVideoCapture.cpp ceres/bal_problem.cpp + ceres/bundle_adjuster.cpp ) if (WIN32) diff --git a/AprilTagTrackers/Tracker.cpp b/AprilTagTrackers/Tracker.cpp index e6123d14..2d05b67a 100644 --- a/AprilTagTrackers/Tracker.cpp +++ b/AprilTagTrackers/Tracker.cpp @@ -7,6 +7,7 @@ #include "utils/Assert.hpp" #include +#include #include #include #include @@ -1268,6 +1269,7 @@ void Tracker::RefineTracker() bal_problem.WriteToFile("bal_problem.txt"); #endif + ceres::examples::SolveProblem(bal_problem); } mainThreadRunning = false; diff --git a/AprilTagTrackers/ceres/bundle_adjuster.cpp b/AprilTagTrackers/ceres/bundle_adjuster.cpp new file mode 100644 index 00000000..d3c14ca4 --- /dev/null +++ b/AprilTagTrackers/ceres/bundle_adjuster.cpp @@ -0,0 +1,456 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// An example of solving a dynamically sized problem with various +// solvers and loss functions. +// +// For a simpler bare bones example of doing bundle adjustment with +// Ceres, please see simple_bundle_adjuster.cc. +// +// NOTE: This example will not compile without gflags and SuiteSparse. +// +// The problem being solved here is known as a Bundle Adjustment +// problem in computer vision. Given a set of 3d points X_1, ..., X_n, +// a set of cameras P_1, ..., P_m. If the point X_i is visible in +// image j, then there is a 2D observation u_ij that is the expected +// projection of X_i using P_j. The aim of this optimization is to +// find values of X_i and P_j such that the reprojection error +// +// E(X,P) = sum_ij |u_ij - P_j X_i|^2 +// +// is minimized. +// +// The problem used here comes from a collection of bundle adjustment +// problems published at University of Washington. +// http://grail.cs.washington.edu/projects/bal + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "bal_problem.hpp" +#include "ceres/ceres.h" +#include "gflags/gflags.h" +#include "glog/logging.h" +#include "snavely_reprojection_error.hpp" +#include "opencv_reprojection_error.hpp" +#include "positional_error.hpp" + +// clang-format makes the gflags definitions too verbose +// clang-format off + +DEFINE_string(input, "", "Input File name"); +DEFINE_string(trust_region_strategy, "levenberg_marquardt", + "Options are: levenberg_marquardt, dogleg."); +DEFINE_string(dogleg, "traditional_dogleg", "Options are: traditional_dogleg," + "subspace_dogleg."); + +DEFINE_bool(inner_iterations, false, "Use inner iterations to non-linearly " + "refine each successful trust region step."); + +DEFINE_string(blocks_for_inner_iterations, "automatic", "Options are: " + "automatic, cameras, points, cameras,points, points,cameras"); + +DEFINE_string(linear_solver, "sparse_schur", "Options are: " + "sparse_schur, dense_schur, iterative_schur, " + "sparse_normal_cholesky, dense_qr, dense_normal_cholesky, " + "and cgnr."); +DEFINE_bool(explicit_schur_complement, false, "If using ITERATIVE_SCHUR " + "then explicitly compute the Schur complement."); +DEFINE_string(preconditioner, "jacobi", "Options are: " + "identity, jacobi, schur_jacobi, schur_power_series_expansion, cluster_jacobi, " + "cluster_tridiagonal."); +DEFINE_string(visibility_clustering, "canonical_views", + "single_linkage, canonical_views"); +DEFINE_bool(use_spse_initialization, false, + "Use power series expansion to initialize the solution in ITERATIVE_SCHUR linear solver."); + +DEFINE_string(sparse_linear_algebra_library, "eigen_sparse", + "Options are: suite_sparse, accelerate_sparse, eigen_sparse, and " + "cuda_sparse."); +DEFINE_string(dense_linear_algebra_library, "eigen", + "Options are: eigen, lapack, and cuda"); +DEFINE_string(ordering_type, "amd", "Options are: amd, nesdis"); +DEFINE_string(linear_solver_ordering, "user", + "Options are: automatic and user"); + +DEFINE_bool(use_quaternions, false, "If true, uses quaternions to represent " + "rotations. If false, angle axis is used."); +DEFINE_bool(use_manifolds, false, "For quaternions, use a manifold."); +DEFINE_bool(robustify, false, "Use a robust loss function."); + +DEFINE_double(eta, 1e-2, "Default value for eta. Eta determines the " + "accuracy of each linear solve of the truncated newton step. " + "Changing this parameter can affect solve performance."); + +DEFINE_int32(num_threads, 1, "Number of threads. -1 = std::thread::hardware_concurrency."); +DEFINE_int32(num_iterations, 100, "Number of iterations."); +DEFINE_int32(max_linear_solver_iterations, 500, "Maximum number of iterations" + " for solution of linear system."); +DEFINE_double(spse_tolerance, 0.1, + "Tolerance to reach during the iterations of power series expansion initialization or preconditioning."); +DEFINE_int32(max_num_spse_iterations, 5, + "Maximum number of iterations for power series expansion initialization or preconditioning."); +DEFINE_double(max_solver_time, 1e32, "Maximum solve time in seconds."); +DEFINE_bool(nonmonotonic_steps, false, "Trust region algorithm can use" + " nonmonotic steps."); + +DEFINE_double(rotation_sigma, 0.0, "Standard deviation of camera rotation " + "perturbation."); +DEFINE_double(translation_sigma, 0.0, "Standard deviation of the camera " + "translation perturbation."); +DEFINE_double(point_sigma, 0.0, "Standard deviation of the point " + "perturbation."); +DEFINE_int32(random_seed, 38401, "Random seed used to set the state " + "of the pseudo random number generator used to generate " + "the perturbations."); +DEFINE_bool(line_search, false, "Use a line search instead of trust region " + "algorithm."); +DEFINE_bool(mixed_precision_solves, false, "Use mixed precision solves."); +DEFINE_int32(max_num_refinement_iterations, 0, "Iterative refinement iterations"); +DEFINE_string(initial_ply, "initial.ply", "Export the BAL file data as a PLY file."); +DEFINE_string(final_ply, "final.ply", "Export the refined BAL file data as a PLY " + "file."); +// clang-format on + +namespace ceres::examples { +namespace { + +void SetLinearSolver(Solver::Options* options) { + CHECK(StringToLinearSolverType(CERES_GET_FLAG(FLAGS_linear_solver), + &options->linear_solver_type)); + CHECK(StringToPreconditionerType(CERES_GET_FLAG(FLAGS_preconditioner), + &options->preconditioner_type)); + CHECK(StringToVisibilityClusteringType( + CERES_GET_FLAG(FLAGS_visibility_clustering), + &options->visibility_clustering_type)); + CHECK(StringToSparseLinearAlgebraLibraryType( + CERES_GET_FLAG(FLAGS_sparse_linear_algebra_library), + &options->sparse_linear_algebra_library_type)); + CHECK(StringToDenseLinearAlgebraLibraryType( + CERES_GET_FLAG(FLAGS_dense_linear_algebra_library), + &options->dense_linear_algebra_library_type)); + options->use_explicit_schur_complement = + CERES_GET_FLAG(FLAGS_explicit_schur_complement); + options->use_mixed_precision_solves = + CERES_GET_FLAG(FLAGS_mixed_precision_solves); + options->max_num_refinement_iterations = + CERES_GET_FLAG(FLAGS_max_num_refinement_iterations); + options->max_linear_solver_iterations = + CERES_GET_FLAG(FLAGS_max_linear_solver_iterations); +} + +void SetOrdering(BALProblem* bal_problem, Solver::Options* options) { + const int num_points = bal_problem->num_points(); + const int point_block_size = bal_problem->point_block_size(); + double* points = bal_problem->mutable_points(); + + const int num_cameras = bal_problem->num_cameras(); + const int camera_block_size = bal_problem->camera_block_size(); + double* cameras = bal_problem->mutable_cameras(); + + if (options->use_inner_iterations) { + if (CERES_GET_FLAG(FLAGS_blocks_for_inner_iterations) == "cameras") { + LOG(INFO) << "Camera blocks for inner iterations"; + options->inner_iteration_ordering = + std::make_shared(); + for (int i = 0; i < num_cameras; ++i) { + options->inner_iteration_ordering->AddElementToGroup( + cameras + camera_block_size * i, 0); + } + } else if (CERES_GET_FLAG(FLAGS_blocks_for_inner_iterations) == "points") { + LOG(INFO) << "Point blocks for inner iterations"; + options->inner_iteration_ordering = + std::make_shared(); + for (int i = 0; i < num_points; ++i) { + options->inner_iteration_ordering->AddElementToGroup( + points + point_block_size * i, 0); + } + } else if (CERES_GET_FLAG(FLAGS_blocks_for_inner_iterations) == + "cameras,points") { + LOG(INFO) << "Camera followed by point blocks for inner iterations"; + options->inner_iteration_ordering = + std::make_shared(); + for (int i = 0; i < num_cameras; ++i) { + options->inner_iteration_ordering->AddElementToGroup( + cameras + camera_block_size * i, 0); + } + for (int i = 0; i < num_points; ++i) { + options->inner_iteration_ordering->AddElementToGroup( + points + point_block_size * i, 1); + } + } else if (CERES_GET_FLAG(FLAGS_blocks_for_inner_iterations) == + "points,cameras") { + LOG(INFO) << "Point followed by camera blocks for inner iterations"; + options->inner_iteration_ordering = + std::make_shared(); + for (int i = 0; i < num_cameras; ++i) { + options->inner_iteration_ordering->AddElementToGroup( + cameras + camera_block_size * i, 1); + } + for (int i = 0; i < num_points; ++i) { + options->inner_iteration_ordering->AddElementToGroup( + points + point_block_size * i, 0); + } + } else if (CERES_GET_FLAG(FLAGS_blocks_for_inner_iterations) == + "automatic") { + LOG(INFO) << "Choosing automatic blocks for inner iterations"; + } else { + LOG(FATAL) << "Unknown block type for inner iterations: " + << CERES_GET_FLAG(FLAGS_blocks_for_inner_iterations); + } + } + + // Bundle adjustment problems have a sparsity structure that makes + // them amenable to more specialized and much more efficient + // solution strategies. The SPARSE_SCHUR, DENSE_SCHUR and + // ITERATIVE_SCHUR solvers make use of this specialized + // structure. + // + // This can either be done by specifying a + // Options::linear_solver_ordering or having Ceres figure it out + // automatically using a greedy maximum independent set algorithm. + if (CERES_GET_FLAG(FLAGS_linear_solver_ordering) == "user") { + auto* ordering = new ceres::ParameterBlockOrdering; + + // The points come before the cameras. + for (int i = 0; i < num_points; ++i) { + ordering->AddElementToGroup(points + point_block_size * i, 0); + } + + for (int i = 0; i < num_cameras; ++i) { + // When using axis-angle, there is a single parameter block for + // the entire camera. + ordering->AddElementToGroup(cameras + camera_block_size * i, 1); + } + + options->linear_solver_ordering.reset(ordering); + } +} + +void SetMinimizerOptions(Solver::Options* options) { + options->max_num_iterations = CERES_GET_FLAG(FLAGS_num_iterations); + options->minimizer_progress_to_stdout = true; + if (CERES_GET_FLAG(FLAGS_num_threads) == -1) { + const int num_available_threads = + static_cast(std::thread::hardware_concurrency()); + if (num_available_threads > 0) { + options->num_threads = num_available_threads; + } + } else { + options->num_threads = CERES_GET_FLAG(FLAGS_num_threads); + } + CHECK_GE(options->num_threads, 1); + + options->eta = CERES_GET_FLAG(FLAGS_eta); + options->max_solver_time_in_seconds = CERES_GET_FLAG(FLAGS_max_solver_time); + options->use_nonmonotonic_steps = CERES_GET_FLAG(FLAGS_nonmonotonic_steps); + if (CERES_GET_FLAG(FLAGS_line_search)) { + options->minimizer_type = ceres::LINE_SEARCH; + } + + CHECK(StringToTrustRegionStrategyType( + CERES_GET_FLAG(FLAGS_trust_region_strategy), + &options->trust_region_strategy_type)); + CHECK( + StringToDoglegType(CERES_GET_FLAG(FLAGS_dogleg), &options->dogleg_type)); + options->use_inner_iterations = CERES_GET_FLAG(FLAGS_inner_iterations); +} + +void SetSolverOptionsFromFlags(BALProblem* bal_problem, + Solver::Options* options) { + SetMinimizerOptions(options); + SetLinearSolver(options); + SetOrdering(bal_problem, options); +} + +void BuildProblem(BALProblem* bal_problem, Problem* problem) { + const int point_block_size = bal_problem->point_block_size(); + const int camera_block_size = bal_problem->camera_block_size(); + double* points = bal_problem->mutable_points(); + double* cameras = bal_problem->mutable_cameras(); + +#if 0 + // Observations is 2*num_observations long array observations = + // [u_1, u_2, ... , u_n], where each u_i is two dimensional, the x + // and y positions of the observation. + const double* observations = bal_problem->observations(); + for (int i = 0; i < bal_problem->num_observations(); ++i) { + CostFunction* cost_function; + // Each Residual block takes a point and a camera as input and + // outputs a 2 dimensional residual. + cost_function = (CERES_GET_FLAG(FLAGS_use_quaternions)) + ? SnavelyReprojectionErrorWithQuaternions::Create( + observations[2 * i + 0], observations[2 * i + 1]) + : SnavelyReprojectionError::Create( + observations[2 * i + 0], observations[2 * i + 1]); + + // If enabled use Huber's loss function. + LossFunction* loss_function = + CERES_GET_FLAG(FLAGS_robustify) ? new HuberLoss(1.0) : nullptr; + + // Each observation corresponds to a pair of a camera and a point + // which are identified by camera_index()[i] and point_index()[i] + // respectively. + double* camera = + cameras + camera_block_size * bal_problem->camera_index()[i]; + double* point = points + point_block_size * bal_problem->point_index()[i]; + problem->AddResidualBlock(cost_function, loss_function, camera, point); + } +#endif + +#if 1 + // Observations is 2*num_observations long array observations = + // [u_1, u_2, ... , u_n], where each u_i is two dimensional, the x + // and y positions of the observation. + const double* observations = bal_problem->observations(); + for (int i = 0; i < bal_problem->num_observations(); ++i) { + // Each observation corresponds to a pair of a camera and a point + // which are identified by camera_index()[i] and point_index()[i] + // respectively. + double* camera = + cameras + camera_block_size * bal_problem->camera_index()[i]; + double* point = points + point_block_size * bal_problem->point_index()[i]; + + CostFunction* cost_function; + // Each Residual block takes a point and a camera as input and + // outputs a 2 dimensional residual. + cost_function = OpenCVReprojectionError::Create( + observations[2 * i + 0], observations[2 * i + 1], + camera[6], camera[7], + camera[8], camera[9], + camera[10], camera[11], + camera[12], camera[13], + camera[14]); + + // If enabled use Huber's loss function. + LossFunction* loss_function = + CERES_GET_FLAG(FLAGS_robustify) ? new HuberLoss(1.0) : nullptr; + + problem->AddResidualBlock(cost_function, loss_function, camera, point); + } +#endif + +#if 0 + // Anchor the first four points at their initial positions + for (int i = 0; i < 4; ++i) { + CostFunction* cost_function; + // Each Residual block takes a point input and + // outputs a 1 dimensional residual. + cost_function = PositionalError::Create( + points[point_block_size * i + 0], + points[point_block_size * i + 1], + points[point_block_size * i + 2]); + + // If enabled use Huber's loss function. + LossFunction* loss_function = + CERES_GET_FLAG(FLAGS_robustify) ? new HuberLoss(1.0) : nullptr; + + double* point = points + point_block_size * i; + problem->AddResidualBlock(cost_function, loss_function, point); + } +#endif + + if (CERES_GET_FLAG(FLAGS_use_quaternions) && + CERES_GET_FLAG(FLAGS_use_manifolds)) { + Manifold* camera_manifold = + new ProductManifold>{}; + for (int i = 0; i < bal_problem->num_cameras(); ++i) { + problem->SetManifold(cameras + camera_block_size * i, camera_manifold); + } + } +} + +void SolveProblemInternal(BALProblem &bal_problem) { + if (!CERES_GET_FLAG(FLAGS_initial_ply).empty()) { + bal_problem.WriteToPLYFile(CERES_GET_FLAG(FLAGS_initial_ply)); + } + + Problem problem; + + srand(CERES_GET_FLAG(FLAGS_random_seed)); +#if 0 + bal_problem.Normalize(); + bal_problem.Perturb(CERES_GET_FLAG(FLAGS_rotation_sigma), + CERES_GET_FLAG(FLAGS_translation_sigma), + CERES_GET_FLAG(FLAGS_point_sigma)); +#endif + + BuildProblem(&bal_problem, &problem); + Solver::Options options; + SetSolverOptionsFromFlags(&bal_problem, &options); + options.gradient_tolerance = 1e-16; + options.function_tolerance = 1e-16; + options.parameter_tolerance = 1e-16; + Solver::Summary summary; + Solve(options, &problem, &summary); + std::cout << summary.FullReport() << "\n"; + + if (!CERES_GET_FLAG(FLAGS_final_ply).empty()) { + bal_problem.WriteToPLYFile(CERES_GET_FLAG(FLAGS_final_ply)); + } +} + + +} // namespace + +void SolveProblem(const char* filename) { + BALProblem bal_problem(filename, CERES_GET_FLAG(FLAGS_use_quaternions)); + + SolveProblemInternal(bal_problem); +} + +void SolveProblem(BALProblem &bal_problem) { + SolveProblemInternal(bal_problem); +} + +} // namespace ceres::examples + +#if 0 +int main(int argc, char** argv) { + GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true); + google::InitGoogleLogging(argv[0]); + if (CERES_GET_FLAG(FLAGS_input).empty()) { + LOG(ERROR) << "Usage: bundle_adjuster --input=bal_problem"; + return 1; + } + + CHECK(CERES_GET_FLAG(FLAGS_use_quaternions) || + !CERES_GET_FLAG(FLAGS_use_manifolds)) + << "--use_manifolds can only be used with --use_quaternions."; + ceres::examples::SolveProblem(CERES_GET_FLAG(FLAGS_input).c_str()); + return 0; +} +#endif diff --git a/AprilTagTrackers/ceres/bundle_adjuster.hpp b/AprilTagTrackers/ceres/bundle_adjuster.hpp new file mode 100644 index 00000000..42a309c7 --- /dev/null +++ b/AprilTagTrackers/ceres/bundle_adjuster.hpp @@ -0,0 +1,11 @@ +#ifndef CERES_EXAMPLES_BUNDLE_ADJUSTER_H_ +#define CERES_EXAMPLES_BUNDLE_ADJUSTER_H_ + +namespace ceres::examples { + +void SolveProblem(BALProblem &bal_problem); +void SolveProblem(const char* filename); + +} // namespace ceres::examples + +#endif // CERES_EXAMPLES_BAL_PROBLEM_H_ diff --git a/AprilTagTrackers/ceres/opencv_reprojection_error.hpp b/AprilTagTrackers/ceres/opencv_reprojection_error.hpp new file mode 100644 index 00000000..727a0ea8 --- /dev/null +++ b/AprilTagTrackers/ceres/opencv_reprojection_error.hpp @@ -0,0 +1,298 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Templated struct implementing the camera model and residual +// computation for bundle adjustment used by Noah Snavely's Bundler +// SfM system. This is also the camera model/residual for the bundle +// adjustment problems in the BAL dataset. It is templated so that we +// can use Ceres's automatic differentiation to compute analytic +// jacobians. +// +// For details see: http://phototour.cs.washington.edu/bundler/ +// and http://grail.cs.washington.edu/projects/bal/ + +#ifndef OPENCV_REPROJECTION_ERROR_HPP_ +#define OPENCV_REPROJECTION_ERROR_HPP_ + +#include + +#include "ceres/rotation.h" + +namespace ceres { +namespace examples { + +// Templated pinhole camera model for used with Ceres. The camera is +// parameterized using 15 parameters. 3 for rotation, 3 for +// translation, 2 for focal length, 2 for principal point, 3 for +// radial distortion and 2 for tangential distortion. +struct OpenCVReprojectionError { + // (u, v): the position of the observation with respect to the image + // center point. + OpenCVReprojectionError(double observed_x, double observed_y, + double fx, double fy, + double cx, double cy, + double k1, double k2, + double p1, double p2, + double k3) + : observed_x(observed_x), observed_y(observed_y) + , fx(fx), fy(fy) + , cx(cx), cy(cy) + , k1(k1), k2(k2) + , p1(p1), p2(p2) + , k3(k3) {} + + template + bool operator()(const T* const camera, + const T* const point, + T* residuals) const { + // camera[0,1,2] are the angle-axis rotation. + T p[3]; + AngleAxisRotatePoint(camera, point, p); + + // camera[3,4,5] are the translation. + p[0] += camera[3]; + p[1] += camera[4]; + p[2] += camera[5]; + + const T xp = p[0] / p[2]; + const T yp = p[1] / p[2]; + + // Calculate second, fourth and sixth order radial distortion. + const T r2 = xp * xp + yp * yp; + const T radial_distortion = 1.0 + r2 * (k1 + r2 * (k2 + r2 * k3)); + + // Calculate tangential distortion. + const T xy = xp * yp; + const T x2 = xp * xp; + const T y2 = yp * yp; + const T tangential_distortion_x = 2.0 * p1 * xy + p2 * (r2 + 2.0 * x2); + const T tangential_distortion_y = p1 * (r2 + 2.0 * y2) + 2.0 * p2 * xy; + + // Apply distortions + const T xpp = xp * radial_distortion + tangential_distortion_x; + const T ypp = yp * radial_distortion + tangential_distortion_y; + + // Compute final projected point position. + const T predicted_x = fx * xpp + cx; + const T predicted_y = fy * ypp + cy; + + // The error is the difference between the predicted and observed position. + residuals[0] = predicted_x - observed_x; + residuals[1] = predicted_y - observed_y; + + return true; + } + +#if 0 + template + bool operator()(const T* const camera, + const T* const point, + T* residuals) const { + // camera[0,1,2] are the angle-axis rotation. + T p[3]; + AngleAxisRotatePoint(camera, point, p); + std::cout << "AngleAxisRotatePoint(camera, point, p)" << std::endl; + std::cout << "camera[0, 1, 2] = [" << camera[0].a << ", " << camera[1].a << ", " << camera[2].a << "]" << std::endl; + std::cout << "point[0, 1, 2] = [" << point[0].a << ", " << point[1].a << ", " << point[2].a << "]" << std::endl; + std::cout << "p[0, 1, 2] = [" << p[0].a << ", " << p[1].a << ", " << p[2].a << "]" << std::endl; + + // camera[3,4,5] are the translation. + p[0] += camera[3]; + std::cout << "p[0] += camera[3]" << std::endl; + p[1] += camera[4]; + std::cout << "p[1] += camera[4]" << std::endl; + p[2] += camera[5]; + std::cout << "p[2] += camera[5]" << std::endl; + std::cout << "camera[3, 4, 5] = [" << camera[3].a << ", " << camera[4].a << ", " << camera[5].a << "]" << std::endl; + + const T xp = p[0] / p[2]; + std::cout << "const double xp = p[0] / p[2]" << std::endl; + std::cout << "xp = " << xp.a << std::endl; + + const T yp = p[1] / p[2]; + std::cout << "const double yp = p[1] / p[2]" << std::endl; + std::cout << "yp = " << yp.a << std::endl; + + // Calculate second, fourth and sixth order radial distortion. + const T r2 = xp * xp + yp * yp; + std::cout << "const double r2 = xp * xp + yp * yp" << std::endl; + std::cout << "r2 = " << r2.a << std::endl; + + const T radial_distortion = 1.0 + r2 * (k1 + r2 * (k2 /*+ r2 * k3*/)); + std::cout << "const double radial_distortion = 1.0 + r2 * (k1 + r2 * (k2 /*+ r2 * k3*/))" << std::endl; + std::cout << "k1 = " << k1 << std::endl; + std::cout << "k2 = " << k2 << std::endl; + std::cout << "k3 = " << k3 << std::endl; + std::cout << "radial_distortion = " << radial_distortion.a << std::endl; + + // // Calculate tangential distortion. + // const T xy = xp * yp; + // const T x2 = xp * xp; + // const T y2 = yp * yp; + // const T tangential_distortion_x = 2.0 * p1 * xy + p2 * (r2 + 2.0 * x2); + // const T tangential_distortion_y = p1 * (r2 + 2.0 * y2) + 2.0 * p2 * xy; + + // Apply distortions + const T xpp = xp * radial_distortion /*+ tangential_distortion_x*/; + std::cout << "const double xpp = xp * radial_distortion /*+ tangential_distortion_x*/" << std::endl; + std::cout << "xpp = " << xpp.a << std::endl; + + const T ypp = yp * radial_distortion /*+ tangential_distortion_y*/; + std::cout << "const double ypp = yp * radial_distortion /*+ tangential_distortion_y*/" << std::endl; + std::cout << "ypp = " << ypp.a << std::endl; + + // Compute final projected point position. + const T predicted_x = fx * xpp + cx; + std::cout << "const double predicted_x = fx * xpp + cx" << std::endl; + std::cout << "predicted_x = " << predicted_x.a << std::endl; + + const T predicted_y = fy * ypp + cy; + std::cout << "const double predicted_y = fy * ypp + cy" << std::endl; + std::cout << "predicted_y = " << predicted_y.a << std::endl; + + // The error is the difference between the predicted and observed position. + residuals[0] = predicted_x - observed_x; + std::cout << "residuals[0] = predicted_x - observed_x" << std::endl; + std::cout << "observed_x = " << observed_x << std::endl; + std::cout << "residuals[0] = " << residuals[0].a << std::endl; + + residuals[1] = predicted_y - observed_y; + std::cout << "residuals[1] = predicted_y - observed_y" << std::endl; + std::cout << "observed_y = " << observed_y << std::endl; + std::cout << "residuals[1] = " << residuals[1].a << std::endl; + + return true; + } +#endif + +#if 0 + bool operator()(const double* const camera, + const double* const point, + double* residuals) const { + // camera[0,1,2] are the angle-axis rotation. + double p[3]; + AngleAxisRotatePoint(camera, point, p); + std::cout << "AngleAxisRotatePoint(camera, point, p)" << std::endl; + std::cout << "camera[0, 1, 2] = [" << camera[0] << ", " << camera[1] << ", " << camera[2] << "]" << std::endl; + std::cout << "point[0, 1, 2] = [" << point[0] << ", " << point[1] << ", " << point[2] << "]" << std::endl; + std::cout << "p[0, 1, 2] = [" << p[0] << ", " << p[1] << ", " << p[2] << "]" << std::endl; + + // camera[3,4,5] are the translation. + p[0] += camera[3]; + std::cout << "p[0] += camera[3]" << std::endl; + p[1] += camera[4]; + std::cout << "p[1] += camera[4]" << std::endl; + p[2] += camera[5]; + std::cout << "p[2] += camera[5]" << std::endl; + std::cout << "camera[3, 4, 5] = [" << camera[3] << ", " << camera[4] << ", " << camera[5] << "]" << std::endl; + std::cout << "p[0, 1, 2] = [" << p[0] << ", " << p[1] << ", " << p[2] << "]" << std::endl; + + const double xp = p[0] / p[2]; + std::cout << "const double xp = p[0] / p[2]" << std::endl; + std::cout << "xp = " << xp << std::endl; + + const double yp = p[1] / p[2]; + std::cout << "const double yp = p[1] / p[2]" << std::endl; + std::cout << "yp = " << yp << std::endl; + + // Calculate second, fourth and sixth order radial distortion. + const double r2 = xp * xp + yp * yp; + std::cout << "const double r2 = xp * xp + yp * yp" << std::endl; + std::cout << "r2 = " << r2 << std::endl; + + const double radial_distortion = 1.0 + r2 * (k1 + r2 * (k2 /*+ r2 * k3*/)); + std::cout << "const double radial_distortion = 1.0 + r2 * (k1 + r2 * (k2 /*+ r2 * k3*/))" << std::endl; + std::cout << "k1 = " << k1 << std::endl; + std::cout << "k2 = " << k2 << std::endl; + std::cout << "k3 = " << k3 << std::endl; + std::cout << "radial_distortion = " << radial_distortion << std::endl; + + // // Calculate tangential distortion. + // const double xy = xp * yp; + // const double x2 = xp * xp; + // const double y2 = yp * yp; + // const double tangential_distortion_x = 2.0 * p1 * xy + p2 * (r2 + 2.0 * x2); + // const double tangential_distortion_y = p1 * (r2 + 2.0 * y2) + 2.0 * p2 * xy; + + // Apply distortions + const double xpp = xp * radial_distortion /*+ tangential_distortion_x*/; + std::cout << "const double xpp = xp * radial_distortion /*+ tangential_distortion_x*/" << std::endl; + std::cout << "xpp = " << xpp << std::endl; + + const double ypp = yp * radial_distortion /*+ tangential_distortion_y*/; + std::cout << "const double ypp = yp * radial_distortion /*+ tangential_distortion_y*/" << std::endl; + std::cout << "ypp = " << ypp << std::endl; + + // Compute final projected point position. + const double predicted_x = fx * xpp + cx; + std::cout << "const double predicted_x = fx * xpp + cx" << std::endl; + std::cout << "predicted_x = " << predicted_x << std::endl; + + const double predicted_y = fy * ypp + cy; + std::cout << "const double predicted_y = fy * ypp + cy" << std::endl; + std::cout << "predicted_y = " << predicted_y << std::endl; + + // The error is the difference between the predicted and observed position. + residuals[0] = predicted_x - observed_x; + std::cout << "residuals[0] = predicted_x - observed_x" << std::endl; + std::cout << "residuals[0] = " << residuals[0] << std::endl; + + residuals[1] = predicted_y - observed_y; + std::cout << "residuals[1] = predicted_y - observed_y" << std::endl; + std::cout << "residuals[1] = " << residuals[1] << std::endl; + + return true; + } +#endif + + // Factory to hide the construction of the CostFunction object from + // the client code. + static ceres::CostFunction* Create(const double observed_x, + const double observed_y, + const double fx, const double fy, + const double cx, const double cy, + const double k1, const double k2, + const double p1, const double p2, + const double k3) { + return (new ceres::AutoDiffCostFunction( + new OpenCVReprojectionError(observed_x, observed_y, fx, fy, cx, cy, k1, k2, p1, p2, k3))); + } + + double observed_x; + double observed_y; + double fx, fy; + double cx, cy; + double k1, k2, p1, p2, k3; +}; + +} // namespace examples +} // namespace ceres + +#endif // OPENCV_REPROJECTION_ERROR_HPP_ diff --git a/AprilTagTrackers/ceres/positional_error.hpp b/AprilTagTrackers/ceres/positional_error.hpp new file mode 100644 index 00000000..d340a92a --- /dev/null +++ b/AprilTagTrackers/ceres/positional_error.hpp @@ -0,0 +1,46 @@ +#ifndef CERES_EXAMPLES_POSITIONAL_ERROR_H_ +#define CERES_EXAMPLES_POSITIONAL_ERROR_H_ + +namespace ceres { +namespace examples { + +struct PositionalError { + PositionalError(double anchor_x, double anchor_y, double anchor_z) + : anchor_x(anchor_x), anchor_y(anchor_y), anchor_z(anchor_z) {} + + template + bool operator()(const T* const point, + T* residuals) const { + // The error is the distance between current and anchor position. + const T x = point[0] - anchor_x; + const T y = point[1] - anchor_y; + const T z = point[2] - anchor_z; + + const T x2 = x * x; + const T y2 = y * y; + const T z2 = z * z; + + residuals[0] = 1e6 * (x2 + y2 + z2); + + return true; + } + + // Factory to hide the construction of the CostFunction object from + // the client code. + static ceres::CostFunction* Create(const double anchor_x, + const double anchor_y, + const double anchor_z) { + return (new ceres::AutoDiffCostFunction( + new PositionalError(anchor_x, anchor_y, anchor_z))); + } + + double anchor_x; + double anchor_y; + double anchor_z; +}; + + +} // namespace examples +} // namespace ceres + +#endif // CERES_EXAMPLES_POSITIONAL_ERROR_H_ diff --git a/AprilTagTrackers/ceres/snavely_reprojection_error.hpp b/AprilTagTrackers/ceres/snavely_reprojection_error.hpp new file mode 100644 index 00000000..8f848d00 --- /dev/null +++ b/AprilTagTrackers/ceres/snavely_reprojection_error.hpp @@ -0,0 +1,179 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Templated struct implementing the camera model and residual +// computation for bundle adjustment used by Noah Snavely's Bundler +// SfM system. This is also the camera model/residual for the bundle +// adjustment problems in the BAL dataset. It is templated so that we +// can use Ceres's automatic differentiation to compute analytic +// jacobians. +// +// For details see: http://phototour.cs.washington.edu/bundler/ +// and http://grail.cs.washington.edu/projects/bal/ + +#ifndef CERES_EXAMPLES_SNAVELY_REPROJECTION_ERROR_H_ +#define CERES_EXAMPLES_SNAVELY_REPROJECTION_ERROR_H_ + +#include "ceres/rotation.h" + +namespace ceres { +namespace examples { + +// Templated pinhole camera model for used with Ceres. The camera is +// parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for +// focal length and 2 for radial distortion. The principal point is not modeled +// (i.e. it is assumed be located at the image center). +struct SnavelyReprojectionError { + SnavelyReprojectionError(double observed_x, double observed_y) + : observed_x(observed_x), observed_y(observed_y) {} + + template + bool operator()(const T* const camera, + const T* const point, + T* residuals) const { + // camera[0,1,2] are the angle-axis rotation. + T p[3]; + AngleAxisRotatePoint(camera, point, p); + + // camera[3,4,5] are the translation. + p[0] += camera[3]; + p[1] += camera[4]; + p[2] += camera[5]; + + // Compute the center of distortion. The sign change comes from + // the camera model that Noah Snavely's Bundler assumes, whereby + // the camera coordinate system has a negative z axis. + const T xp = -p[0] / p[2]; + const T yp = -p[1] / p[2]; + + // Apply second and fourth order radial distortion. + const T& l1 = camera[7]; + const T& l2 = camera[8]; + const T r2 = xp * xp + yp * yp; + const T distortion = 1.0 + r2 * (l1 + l2 * r2); + + // Compute final projected point position. + const T& focal = camera[6]; + const T predicted_x = focal * distortion * xp; + const T predicted_y = focal * distortion * yp; + + // The error is the difference between the predicted and observed position. + residuals[0] = predicted_x - observed_x; + residuals[1] = predicted_y - observed_y; + + return true; + } + + // Factory to hide the construction of the CostFunction object from + // the client code. + static ceres::CostFunction* Create(const double observed_x, + const double observed_y) { + return (new ceres::AutoDiffCostFunction( + new SnavelyReprojectionError(observed_x, observed_y))); + } + + double observed_x; + double observed_y; +}; + +// Templated pinhole camera model for used with Ceres. The camera is +// parameterized using 10 parameters. 4 for rotation, 3 for +// translation, 1 for focal length and 2 for radial distortion. The +// principal point is not modeled (i.e. it is assumed be located at +// the image center). +struct SnavelyReprojectionErrorWithQuaternions { + // (u, v): the position of the observation with respect to the image + // center point. + SnavelyReprojectionErrorWithQuaternions(double observed_x, double observed_y) + : observed_x(observed_x), observed_y(observed_y) {} + + template + bool operator()(const T* const camera, + const T* const point, + T* residuals) const { + // camera[0,1,2,3] is are the rotation of the camera as a quaternion. + // + // We use QuaternionRotatePoint as it does not assume that the + // quaternion is normalized, since one of the ways to run the + // bundle adjuster is to let Ceres optimize all 4 quaternion + // parameters without using a Quaternion manifold. + T p[3]; + QuaternionRotatePoint(camera, point, p); + + p[0] += camera[4]; + p[1] += camera[5]; + p[2] += camera[6]; + + // Compute the center of distortion. The sign change comes from + // the camera model that Noah Snavely's Bundler assumes, whereby + // the camera coordinate system has a negative z axis. + const T xp = -p[0] / p[2]; + const T yp = -p[1] / p[2]; + + // Apply second and fourth order radial distortion. + const T& l1 = camera[8]; + const T& l2 = camera[9]; + + const T r2 = xp * xp + yp * yp; + const T distortion = 1.0 + r2 * (l1 + l2 * r2); + + // Compute final projected point position. + const T& focal = camera[7]; + const T predicted_x = focal * distortion * xp; + const T predicted_y = focal * distortion * yp; + + // The error is the difference between the predicted and observed position. + residuals[0] = predicted_x - observed_x; + residuals[1] = predicted_y - observed_y; + + return true; + } + + // Factory to hide the construction of the CostFunction object from + // the client code. + static ceres::CostFunction* Create(const double observed_x, + const double observed_y) { + return ( + new ceres::AutoDiffCostFunction( + new SnavelyReprojectionErrorWithQuaternions(observed_x, + observed_y))); + } + + double observed_x; + double observed_y; +}; + +} // namespace examples +} // namespace ceres + +#endif // CERES_EXAMPLES_SNAVELY_REPROJECTION_ERROR_H_ From 4d9d014510420e52c5ce8b2e4c617ac98a03ff97 Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Wed, 11 Jan 2023 21:51:26 -0500 Subject: [PATCH 20/21] Improved UI feedback during tracker refinement procedure --- AprilTagTrackers/Tracker.cpp | 131 ++++++++++++++++++++++++++++++----- 1 file changed, 114 insertions(+), 17 deletions(-) diff --git a/AprilTagTrackers/Tracker.cpp b/AprilTagTrackers/Tracker.cpp index 2d05b67a..0c5bd47c 100644 --- a/AprilTagTrackers/Tracker.cpp +++ b/AprilTagTrackers/Tracker.cpp @@ -1046,6 +1046,7 @@ void Tracker::RefineTracker() auto camera_index = std::vector(); auto observations = std::vector(); auto parameters = std::vector(); + auto frameCountPerMarker = std::vector(trackerNum * markersPerTracker, 0); int trackerBeingProcessed = -1; int frameNumber = 0; @@ -1055,6 +1056,7 @@ void Tracker::RefineTracker() { CopyFreshCameraImageTo(frame); cv::Mat &image = frame.image; + cv::Mat mask = cv::Mat(image.size(), image.type(), cv::Scalar(1, 1, 1)); std::chrono::steady_clock::time_point start, end; // clock for timing of detection @@ -1065,9 +1067,21 @@ void Tracker::RefineTracker() std::vector> corners; std::vector centers; + april.detectMarkers(image, &corners, &ids, ¢ers, trackers); cv::aruco::drawDetectedMarkers(image, corners, cv::noArray(), cv::Scalar(255, 0, 0)); // draw all markers blue. We will overwrite this with other colors for markers that are part of any of the trackers that we use + for (int i = 0; i < corners.size(); i++) + { + std::vector points; + for(int j = 0; j < corners[i].size(); j++){ + points.push_back(corners[i].at(j)); + } + cv::fillConvexPoly(mask, points, cv::Scalar(1, 0, 0), 8, 0); + } + + bool noTracker = true; + bool tooFar = true; for (int i = 0; i < trackerNum; ++i) { @@ -1097,6 +1111,7 @@ void Tracker::RefineTracker() return; } trackerStatus[i].boardFound = true; + noTracker = false; // Reject detected positions that are behind the camera if (trackerStatus[i].boardTvec[2] < 0) @@ -1130,19 +1145,86 @@ void Tracker::RefineTracker() } } - cv::aruco::drawAxis(image, camCalib->cameraMatrix, camCalib->distortionCoeffs, trackerStatus[i].boardRvec, trackerStatus[i].boardTvec, 0.1); - // TODO: Modify function to be able process more than one tracker at the same time if ((trackerBeingProcessed != -1) && (i != trackerBeingProcessed)) continue; - trackerBeingProcessed = i; - - if ((frameNumber++ % (user_config.videoStreams[0]->camera.fps / 3)) != 0) continue; int tracker_id_begin = i * markersPerTracker; int tracker_id_end = (i + 1) * markersPerTracker; + std::vector trackerIds; + std::vector trackerIdsYellow; + std::vector> trackerCorners; + std::vector> trackerCornersYellow; + std::vector> trackerCornersGreen; + + // Add get the marker IDs and corners for specific tracker + for (int j = 0; j < ids.size(); ++j) + { + // does marker ID belong to current tracker? + if ((tracker_id_begin <= ids[j]) && (ids[j] < tracker_id_end)) + { + trackerIds.push_back(ids[j]); + trackerCorners.push_back(corners[j]); + if(frameCountPerMarker[ids[j]] < 10) + { + trackerIdsYellow.push_back(ids[j]); + trackerCornersYellow.push_back(corners[j]); + } + else + { + trackerCornersGreen.push_back(corners[j]); + } + } + } + + if (trackerStatus[i].boardTvec[2] > 0.30) + { + cv::aruco::drawDetectedMarkers(image, trackerCorners, cv::noArray(), cv::Scalar(255, 0, 255)); // Purple: Tracker too far away + for (int i = 0; i < trackerCorners.size(); i++) + { + std::vector points; + for(int j = 0; j < trackerCorners[i].size(); j++){ + points.push_back(trackerCorners[i].at(j)); + } + cv::fillConvexPoly(mask, points, cv::Scalar(1, 0, 1), 8, 0); + } + continue; + } + + // Yellow: Need more snapshots + cv::aruco::drawDetectedMarkers(image, trackerCornersYellow, cv::noArray(), cv::Scalar(0, 255, 255)); + for (int i = 0; i < trackerCornersYellow.size(); i++) + { + std::vector points; + for(int j = 0; j < trackerCornersYellow[i].size(); j++){ + points.push_back(trackerCornersYellow[i].at(j)); + } + double fillLevel = (double)frameCountPerMarker[trackerIdsYellow[i]] / 10.0; + points[1] = (1 - fillLevel) * points[0] + fillLevel * points[1]; + points[2] = (1 - fillLevel) * points[3] + fillLevel * points[2]; + cv::fillConvexPoly(mask, points, cv::Scalar(0, 1, 1), 8, 0); + } + + // Green: Marker done + cv::aruco::drawDetectedMarkers(image, trackerCornersGreen, cv::noArray(), cv::Scalar(0, 255, 0)); + for (int i = 0; i < trackerCornersGreen.size(); i++) + { + std::vector points; + for(int j = 0; j < trackerCornersGreen[i].size(); j++){ + points.push_back(trackerCornersGreen[i].at(j)); + } + cv::fillConvexPoly(mask, points, cv::Scalar(0, 1, 0), 8, 0); + } + + tooFar = false; + trackerBeingProcessed = i; + + cv::aruco::drawAxis(image, camCalib->cameraMatrix, camCalib->distortionCoeffs, trackerStatus[i].boardRvec, trackerStatus[i].boardTvec, 0.1); + + if ((frameNumber++ % (user_config.videoStreams[0]->camera.fps / 3)) != 0) continue; + // Add camera into parameter block parameters.push_back(trackerStatus[i].boardRvec[0]); parameters.push_back(trackerStatus[i].boardRvec[1]); @@ -1162,19 +1244,17 @@ void Tracker::RefineTracker() num_parameters += 15; // Add detected points into observations - for (int j = 0; j < ids.size(); ++j) + for (int j = 0; j < trackerIds.size(); ++j) { - // does marker ID belong to current tracker? - if ((tracker_id_begin <= ids[j]) && (ids[j] < tracker_id_end)) + ++frameCountPerMarker[trackerIds[j]]; + + for (int k = 0; k < trackerCorners[j].size(); ++k) { - for (int k = 0; k < corners[j].size(); ++k) - { - point_index.push_back((ids[j] - tracker_id_begin) * 4 + k); - camera_index.push_back(num_cameras); - observations.push_back(corners[j][k].x); - observations.push_back(corners[j][k].y); - num_observations++; - } + point_index.push_back((trackerIds[j] - tracker_id_begin) * 4 + k); + camera_index.push_back(num_cameras); + observations.push_back(trackerCorners[j][k].x); + observations.push_back(trackerCorners[j][k].y); + num_observations++; } } @@ -1199,9 +1279,18 @@ void Tracker::RefineTracker() rows = image.rows * drawImgSize / image.cols; } + cv::multiply(image, mask, image); cv::resize(image, outImg, cv::Size(cols, rows)); cv::putText(outImg, std::to_string(frameTime).substr(0, 5), cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255)); - cv::putText(outImg, std::to_string(num_cameras), cv::Point(10, 70), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255)); + if (noTracker) + cv::putText(outImg, std::string("No trackers detected"), cv::Point(10, 70), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255)); + else if (trackerBeingProcessed == -1) + cv::putText(outImg, std::string("Tracker too far"), cv::Point(10, 70), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255)); + else if (tooFar) + cv::putText(outImg, std::string("Tracker " + std::to_string(trackerBeingProcessed) + " too far"), cv::Point(10, 70), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255)); + else + cv::putText(outImg, std::string("Calibrating tracker ") + std::to_string(trackerBeingProcessed), cv::Point(10, 70), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255)); + cv::putText(outImg, std::string("Frames captured: ") + std::to_string(num_cameras), cv::Point(10, 110), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255)); if (showTimeProfile) { april.drawTimeProfile(outImg, cv::Point(10, 60)); @@ -1211,6 +1300,14 @@ void Tracker::RefineTracker() // time of marker detection } + if (gui->IsPreviewVisible()) + { + outImg = cv::Mat::zeros(outImg.size(), outImg.type()); + cv::putText(outImg, std::string("Processing"), cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255)); + cv::putText(outImg, std::string("Please wait..."), cv::Point(10, 70), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255)); + gui->UpdatePreview(outImg); + } + // Add tracker 3d points into parameter block // for (int i = 0; i < trackerNum; ++i) if (trackerBeingProcessed != -1) From 9ec4fbf6cc30785df11140089c8ea9eecd25d61a Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Thu, 12 Jan 2023 15:05:51 -0500 Subject: [PATCH 21/21] Save refined calibration back to config --- AprilTagTrackers/Tracker.cpp | 26 +++++++++++++++++++++----- 1 file changed, 21 insertions(+), 5 deletions(-) diff --git a/AprilTagTrackers/Tracker.cpp b/AprilTagTrackers/Tracker.cpp index 0c5bd47c..8027757c 100644 --- a/AprilTagTrackers/Tracker.cpp +++ b/AprilTagTrackers/Tracker.cpp @@ -1309,13 +1309,12 @@ void Tracker::RefineTracker() } // Add tracker 3d points into parameter block - // for (int i = 0; i < trackerNum; ++i) if (trackerBeingProcessed != -1) { int i = trackerBeingProcessed; const std::vector& ids = trackers[i]->ids; - const std::vector > objPoints = trackers[i]->objPoints; + std::vector >& objPoints = trackers[i]->objPoints; int first_id = i * markersPerTracker; int after_last_id = first_id; @@ -1356,10 +1355,7 @@ void Tracker::RefineTracker() } } } - } - if (trackerBeingProcessed != -1) - { ceres::examples::BALProblem bal_problem(num_cameras, num_points, num_observations, num_parameters, point_index.data(), camera_index.data(), observations.data(), parameters.data()); #ifdef ATT_DEBUG @@ -1367,6 +1363,26 @@ void Tracker::RefineTracker() #endif ceres::examples::SolveProblem(bal_problem); + + auto first_point_iter = parameters.begin() + num_cameras * 15; + + for (int j = first_id; j < after_last_id; ++j) + { + auto it = std::find(ids.begin(), ids.end(), j); + auto index = std::distance(ids.begin(), it); + + if (index < objPoints.size()) + { + for (int k = 0; k < 4; ++k) + { + objPoints[index][k].x = *(first_point_iter + 0 + 3 * (k + 4 * j)); + objPoints[index][k].y = *(first_point_iter + 1 + 3 * (k + 4 * j)); + objPoints[index][k].z = *(first_point_iter + 2 + 3 * (k + 4 * j)); + } + } + } + + calib_config.Save(); } mainThreadRunning = false;