Personal branch

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();

AprilTagTrackers/CMakeLists.txt

@@ -31,6 +31,9 @@ target_sources(AprilTagTrackers PRIVATE
 
     ps3eye/ps3eye.cpp
     ps3eye/PSEyeVideoCapture.cpp
+
+    ceres/bal_problem.cpp
+    ceres/bundle_adjuster.cpp
 )
 
 if (WIN32)
@@ -53,6 +56,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 +68,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=$<CONFIG:Debug>

AprilTagTrackers/Config.hpp

@@ -50,6 +50,7 @@ class UserConfig : public serial::Serializable<UserConfig>
     REFLECTABLE_FIELD(cfg::Validated<int>, markersPerTracker){45, cfg::GreaterEqual(1)};
     REFLECTABLE_FIELD(bool, disableOpenVrApi) = false;
     REFLECTABLE_FIELD(cfg::List<cfg::VideoStream>, videoStreams){1};
+    REFLECTABLE_FIELD(std::string, ipcAddr) = "ApriltagPipeIn";
     REFLECTABLE_END;
 };
 

AprilTagTrackers/Connection.cpp

@@ -8,17 +8,19 @@
 #include <opencv2/core/affine.hpp>
 
 #include <filesystem>
+#include <ios>
+#include <sstream>
 #include <thread>
 
 Connection::Connection(const UserConfig& _user_config)
     : user_config(_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<IPC::IClient*>(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<IPC::IClient*>(namedPipe));
 #endif
 }
@@ -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);
         }

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;
@@ -31,6 +32,7 @@ class ITrackerControl
     bool manualRecalibrate = false;
     bool multicamAutocalib = false;
     bool lockHeightCalib = false;
+    bool privacyMode = false;
 
 protected:
     /// Not set during tracker contstruction

AprilTagTrackers/GUI/MainFrame.cpp

@@ -226,7 +226,10 @@ void GUI::MainFrame::CreateCameraPage(RefPtr<wxNotebook> 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)
             {
@@ -255,6 +258,12 @@ void GUI::MainFrame::CreateCameraPage(RefPtr<wxNotebook> pages)
                        }})
             ->GetWidget();
 
+    cam .Add(StretchSpacer{})
+        .Add(CheckBoxButton{"Privacy mode", [this](auto& evt)
+            {
+            tracker->privacyMode = evt.IsChecked();
+            }});
+
     manualCalibForm = cam.PopSizer().SubForm();
 
     manualCalibForm->PushSizer<wxBoxSizer>(wxVERTICAL)
@@ -413,6 +422,9 @@ void GUI::MainFrame::CreateLicensePage(RefPtr<wxNotebook> pages)
     auto cvLicense = NewWindow<wxTextCtrl>(nb, wxID_ANY, std::string(OPENCV_LICENSE), wxDefaultPosition,
         wxDefaultSize, wxTE_MULTILINE | wxTE_READONLY);
     nb->AddPage(cvLicense, "OpenCV");
+    auto ceresSolverLicense = NewWindow<wxTextCtrl>(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

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

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)
 {

AprilTagTrackers/Helpers.cpp

@@ -347,3 +347,48 @@ Quaternion<double> mRot2Quat(const cv::Mat& m)
 
     return q;
 }
+
+void offsetFromBoardToCameraSpace(std::vector<cv::Point3f> points, cv::Vec3d boardRvec, cv::Vec3d boardTvec, std::vector<cv::Point3f>* 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_<double>(4, 4);
+    for (int x = 0; x < 3; x++)
+    {
+        for (int y = 0; y < 3; y++)
+        {
+            mtranslation.at<double>(x, y) = rmat.at<double>(x, y);
+        }
+    }
+    for (int x = 0; x < 3; x++)
+    {
+        mtranslation.at<double>(x, 3) = 0;
+        mtranslation.at<double>(3, x) = 0;
+    }
+    mtranslation.at<double>(3, 3) = 1;
+
+    cv::Mat rpos = cv::Mat_<double>(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<double>(0, 0) = points[y].x;
+        rpos.at<double>(1, 0) = points[y].y;
+        rpos.at<double>(2, 0) = points[y].z;
+        rpos.at<double>(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<double>(0, 0), rpos.at<double>(1, 0), rpos.at<double>(2, 0)));
+    }
+}

AprilTagTrackers/Helpers.hpp

@@ -27,6 +27,7 @@ bool isRotationMatrix(cv::Mat& R);
 cv::Vec3f rotationMatrixToEulerAngles(cv::Mat& R);
 Quaternion<double> mRot2Quat(const cv::Mat& m);
 cv::Vec3d quat2rodr(double qw, double qx, double qy, double qz);
+void offsetFromBoardToCameraSpace(std::vector<cv::Point3f> points, cv::Vec3d boardRvec, cv::Vec3d boardTvec, std::vector<cv::Point3f>* out);
 
 template <typename T>
 inline void RotateVecByQuat(cv::Vec<T, 3>& out_pos, const cv::Quat<T>& rot)

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);

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

AprilTagTrackers/Localization.hpp

@@ -121,6 +121,7 @@ class Localization : public serial::Serializable<Localization>
     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";

AprilTagTrackers/MyApp.cpp

@@ -3,6 +3,7 @@
 #include "utils/Assert.hpp"
 #include "utils/Log.hpp"
 
+#include <glog/logging.h>
 #include <opencv2/core/utils/logger.hpp>
 
 wxIMPLEMENT_APP(MyApp); // NOLINT
@@ -42,6 +43,8 @@ bool MyApp::OnInit()
     tracker = std::make_unique<Tracker>(userConfig, calibConfig, arucoConfig, lc);
     gui = std::make_unique<GUI>(tracker, lc, userConfig);
 
+    google::InitGoogleLogging("AprilTagTrackers");
+
     return true;
 }