Compare revisions

Tobias Arens · Tobias Arens · Tobias Arens · Tobias Arens · Tobias Arens · Tobias Arens
--- a/include/control.h
+++ b/include/control.h
@@ -36,6 +36,16 @@ namespace Ui
 class Control;
 }

+/**
+ * @brief struct used for storing intrinsic camera params.
+ *
+ * Central place to store this information and easier passing in methods
+ */
+using IntrinsicCameraParams = struct IntrinsicCameraParams
+{
+    cv::Mat cameraMatrix = cv::Mat::eye(cv::Size(3, 3), CV_64F);
+};
+
 class Control : public QWidget
 {
    Q_OBJECT
@@ -124,6 +134,8 @@ public:
    bool getRecoRoiFix() const;
    void setRecoRoiFix(bool b);

+    reco::RecognitionMethod getRecoMethod() const;
+
    bool getTrackRoiShow() const;
    void setTrackRoiShow(bool b);
    bool getTrackRoiFix() const;
@@ -348,6 +360,13 @@ public:
    bool           isExportCommentChecked() const;


+    /**
+     * @brief Get read-only IntrinsicCameraParams.
+     *
+     * The params can be changed by setting its content e.g. camera params (fx, fy, cx, cy) directly
+     */
+    inline IntrinsicCameraParams getIntrinsicCameraParams() const { return mIntrinsicCameraParams; }
+
 #ifdef QWT
    AnalysePlot *getAnalysePlot() const;
 #endif
@@ -363,9 +382,6 @@ public:
        return mMainWindow;
    }

-private:
-    reco::RecognitionMethod getRecoMethod() const;
-

 private slots:
    void on_anaCalculate_clicked();
@@ -717,10 +733,11 @@ signals:
    void userChangedRecoMethod(reco::RecognitionMethod method);

 private:
-    Petrack        *mMainWindow;
-    Ui::Control    *mUi;
-    QGraphicsScene *mScene;
-    bool            mColorChanging;
+    Petrack              *mMainWindow;
+    Ui::Control          *mUi;
+    IntrinsicCameraParams mIntrinsicCameraParams;
+    QGraphicsScene       *mScene;
+    bool                  mColorChanging;
    bool mIndexChanging; // shows, if the index of the color model is really changing; nor while constructor (initialer
                         // durchlauf) and may be while loading xml file
    bool mLoading;       // shows, if new project is just loading

--- a/include/petrack.h
+++ b/include/petrack.h
@@ -109,6 +109,7 @@ public:
    ~Petrack();
    static int trcVersion; // version numbr for writing TRC-Trajectorie files

+
 protected:
    void closeEvent(QCloseEvent *event);


--- a/include/recognition.h
+++ b/include/recognition.h
@@ -27,12 +27,14 @@
 #include <QObject>
 #include <opencv2/aruco.hpp>

+
 class TrackPoint;
 class QRect;
 class BackgroundFilter;
 class Control;
 class ImageItem;
 class CodeMarkerItem;
+struct IntrinsicCameraParams;

 namespace reco
 {
@@ -183,6 +185,7 @@ signals:
    void indexOfMarkerDictChanged();
 };

+
 class Recognizer : public QObject
 {
    Q_OBJECT
@@ -195,8 +198,13 @@ private:
    RecognitionMethod mRecoMethod = RecognitionMethod::MultiColor;

 public:
-    QList<TrackPoint>
-    getMarkerPos(cv::Mat &img, QRect &roi, Control *controlWidget, int borderSize, BackgroundFilter *bgFilter);
+    QList<TrackPoint> getMarkerPos(
+        cv::Mat                     &img,
+        QRect                       &roi,
+        Control                     *controlWidget,
+        int                          borderSize,
+        BackgroundFilter            *bgFilter,
+        const IntrinsicCameraParams &intrinsicCameraParams);

    RecognitionMethod  getRecoMethod() const { return mRecoMethod; }
    CodeMarkerOptions &getCodeMarkerOptions() { return mCodeMarkerOptions; }
@@ -285,10 +293,11 @@ namespace detail
                          QList<TrackPoint>      &crossList,
                          const BlackDotOptions  &options);
    void refineWithAruco(
-        std::vector<ColorBlob> &blobs,
-        const cv::Mat          &img,
-        QList<TrackPoint>      &crossList,
-        ArucoOptions           &options);
+        std::vector<ColorBlob>      &blobs,
+        const cv::Mat               &img,
+        QList<TrackPoint>           &crossList,
+        ArucoOptions                &options,
+        const IntrinsicCameraParams &intrinsicCameraParams);

    void resolveMoreThanOneCode(
        const int          lengthini,
@@ -297,10 +306,11 @@ namespace detail
        const Vec2F        offset);

    void findCodeMarker(
-        cv::Mat                 &img,
-        QList<TrackPoint>       &crossList,
-        RecognitionMethod        recoMethod,
-        const CodeMarkerOptions &opt);
+        cv::Mat                     &img,
+        QList<TrackPoint>           &crossList,
+        RecognitionMethod            recoMethod,
+        const CodeMarkerOptions     &opt,
+        const IntrinsicCameraParams &intrinsicCameraParams);
    cv::Ptr<cv::aruco::Dictionary> getDictMip36h12();
 } // namespace detail
 } // namespace reco

--- a/include/tracker.h
+++ b/include/tracker.h
@@ -59,6 +59,8 @@ private:
    int    mMarkerID; // ID of detected Marker
    Vec3F mSp; // measured 3d point with stereo // mZdistanceToCam; // distance in z direction to camera - measured with
               // stereo
+    cv::Vec3d
+        mOrientation; // orientation of marker i. e. head direction if the marker is placed facing that way (aruco)

 public:
    TrackPoint();
@@ -68,16 +70,18 @@ public:
    TrackPoint(const Vec2F &p, int qual, const QColor &col);
    TrackPoint(const Vec2F &p, int qual, const Vec2F &colPoint, const QColor &col);

-    inline const Vec2F  &colPoint() const { return mColPoint; }
-    inline void          setColPoint(Vec2F &cp) { mColPoint = cp; }
-    inline const QColor &color() const { return mCol; }
-    inline void          setColor(QColor &col) { mCol = col; }
-    inline void          setColPoint(const Vec2F &colPoint) { mColPoint = colPoint; }
-    inline void          setCol(const QColor &col) { mCol = col; }
-    inline int           qual() const { return mQual; }
-    inline void          setQual(int qual) { mQual = qual; }
-    inline int           getMarkerID() const { return mMarkerID; }
-    inline void          setMarkerID(int markerID) { mMarkerID = markerID; }
+    inline const Vec2F     &colPoint() const { return mColPoint; }
+    inline void             setColPoint(Vec2F &cp) { mColPoint = cp; }
+    inline const QColor    &color() const { return mCol; }
+    inline void             setColor(QColor &col) { mCol = col; }
+    inline void             setColPoint(const Vec2F &colPoint) { mColPoint = colPoint; }
+    inline void             setCol(const QColor &col) { mCol = col; }
+    inline int              qual() const { return mQual; }
+    inline void             setQual(int qual) { mQual = qual; }
+    inline int              getMarkerID() const { return mMarkerID; }
+    inline void             setMarkerID(int markerID) { mMarkerID = markerID; }
+    inline const cv::Vec3d &getOrientation() const { return mOrientation; }
+    inline void             setOrientation(const cv::Vec3d &orientation) { mOrientation = orientation; }

    inline const Vec3F &sp() const { return mSp; }
    inline void         setSp(const Vec3F &sp) { mSp = sp; }

--- a/include/trackerReal.h
+++ b/include/trackerReal.h
@@ -56,6 +56,7 @@ public:
    inline void  setAngleOfView(float a) { mAngleOfView = a; }
    inline int   getMarkerID() const { return mMarkerID; }
    inline void  setMarkerID(int markerID) { mMarkerID = markerID; }
+    inline void  setViewDirection(const Vec2F &dir) { mViewDir = dir; }
 };

 inline QTextStream &operator<<(QTextStream &s, const TrackPointReal &tp)

--- a/src/control.cpp
+++ b/src/control.cpp
@@ -2536,6 +2536,7 @@ void Control::on_apply_stateChanged(int i)

 void Control::on_fx_valueChanged(double d)
 {
+    mIntrinsicCameraParams.cameraMatrix.at<double>(0, 0) = d;
    mMainWindow->getCalibFilter()->getFx()->setValue(d);
    if(mUi->quadAspectRatio->isChecked())
    {
@@ -2551,6 +2552,7 @@ void Control::on_fx_valueChanged(double d)

 void Control::on_fy_valueChanged(double d)
 {
+    mIntrinsicCameraParams.cameraMatrix.at<double>(1, 1) = d;
    mMainWindow->getCalibFilter()->getFy()->setValue(d);
    if(!mMainWindow->isLoading())
    {
@@ -2562,6 +2564,7 @@ void Control::on_fy_valueChanged(double d)

 void Control::on_cx_valueChanged(double d)
 {
+    mIntrinsicCameraParams.cameraMatrix.at<double>(0, 2) = d;
    mMainWindow->setStatusPosReal();
    mMainWindow->getCalibFilter()->getCx()->setValue(d);
    if(!mMainWindow->isLoading())
@@ -2573,6 +2576,7 @@ void Control::on_cx_valueChanged(double d)

 void Control::on_cy_valueChanged(double d)
 {
+    mIntrinsicCameraParams.cameraMatrix.at<double>(1, 2) = d;
    mMainWindow->setStatusPosReal();
    mMainWindow->getCalibFilter()->getCy()->setValue(d);
    if(!mMainWindow->isLoading())

--- a/src/petrack.cpp
+++ b/src/petrack.cpp
@@ -3359,6 +3359,7 @@ void Petrack::updateImage(bool imageChanged) // default = false (only true for n
        {
            updateControlImage(mImgFiltered);
        }
+
 #ifndef STEREO_DISABLED
        if(imageChanged || swapChanged || brightContrastChanged || borderChanged || calibChanged)
        {
@@ -3539,7 +3540,12 @@ void Petrack::updateImage(bool imageChanged) // default = false (only true for n
                   (recoMethod == reco::RecognitionMethod::Code)) // else
                {
                    persList = mReco.getMarkerPos(
-                        mImgFiltered, rect, mControlWidget, getImageBorderSize(), getBackgroundFilter());
+                        mImgFiltered,
+                        rect,
+                        mControlWidget,
+                        getImageBorderSize(),
+                        getBackgroundFilter(),
+                        mControlWidget->getIntrinsicCameraParams());
                }
 #ifndef STEREO_DISABLED
                if(mStereoContext && mStereoWidget->stereoUseForReco->isChecked())

--- a/src/recognition.cpp
+++ b/src/recognition.cpp
@@ -604,12 +604,14 @@ void detail::refineWithBlackDot(
 * @param img img in which the color blobs were detected
 * @param crossList list of all detected people
 * @param options all options, mostly forwarded from the GUI
+ * @param intrinsicCameraParams camera parameters needed to estimate arucomarker orientation
 */
 void detail::refineWithAruco(
-    std::vector<ColorBlob> &blobs,
-    const cv::Mat          &img,
-    QList<TrackPoint>      &crossList,
-    ArucoOptions           &options)
+    std::vector<ColorBlob>      &blobs,
+    const cv::Mat               &img,
+    QList<TrackPoint>           &crossList,
+    ArucoOptions                &options,
+    const IntrinsicCameraParams &intrinsicCameraParams)
 {
    constexpr int border = 4; // zusaetzlicher rand um subrects

@@ -660,7 +662,7 @@ void detail::refineWithAruco(
        // TODO: Use Reference to actual codeMarkerOptions in MulticolorMarkerOptions
        // NOTE: For now, add as parameter of findMulticolorMarker
        codeOpt.setOffsetCropRect2Roi(offsetCropRect2Roi);
-        findCodeMarker(subImg, crossList, options.method, codeOpt);
+        findCodeMarker(subImg, crossList, options.method, codeOpt, intrinsicCameraParams);

        resolveMoreThanOneCode(lengthini, crossList, blob, offsetCropRect2Roi);

@@ -772,15 +774,17 @@ void detail::resolveMoreThanOneCode(
 * @param controlWidget
 * @param ignoreWithoutMarker (is ignored->overwritten by cmWidget->ignoreWithoutDot->isChecked())
 * @param offset
+ * @param intrinsicCameraParams needed for aruco-refinement
 */
 void findMultiColorMarker(
-    cv::Mat           &img,
-    QList<TrackPoint> &crossList,
-    Control           *controlWidget,
-    bool               ignoreWithoutMarker,
-    Vec2F             &offset,
-    RecognitionMethod  method,
-    CodeMarkerOptions &codeOpt)
+    cv::Mat                     &img,
+    QList<TrackPoint>           &crossList,
+    Control                     *controlWidget,
+    bool                         ignoreWithoutMarker,
+    Vec2F                       &offset,
+    RecognitionMethod            method,
+    CodeMarkerOptions           &codeOpt,
+    const IntrinsicCameraParams &intrinsicCameraParams)
 {
    Petrack                *mainWindow = controlWidget->getMainWindow();
    MultiColorMarkerItem   *cmItem     = mainWindow->getMultiColorMarkerItem();
@@ -876,7 +880,7 @@ void findMultiColorMarker(
                controlWidget, ignoreWithoutMarker, autoCorrect, autoCorrectOnlyExport, method, codeOpt};

            // adds to crosslist
-            refineWithAruco(blobs, img, crossList, options);
+            refineWithAruco(blobs, img, crossList, options, intrinsicCameraParams);
        }
        else
        {
@@ -1021,12 +1025,15 @@ void findColorMarker(cv::Mat &img, QList<TrackPoint> &crossList, Control *contro
 * @param img
 * @param crossList[out] list of detected TrackPoints
 * @param controlWidget
+ * @param opt arucomarker parameters used for detection
+ * @param intrinsicCameraParams used for estimating arucomarker orientation
 */
 void detail::findCodeMarker(
-    cv::Mat                 &img,
-    QList<TrackPoint>       &crossList,
-    RecognitionMethod        recoMethod,
-    const CodeMarkerOptions &opt)
+    cv::Mat                     &img,
+    QList<TrackPoint>           &crossList,
+    RecognitionMethod            recoMethod,
+    const CodeMarkerOptions     &opt,
+    const IntrinsicCameraParams &intrinsicCameraParams)
 {
    CodeMarkerItem *codeMarkerItem = opt.getCodeMarkerItem();
    Control        *controlWidget  = opt.getControlWidget();
@@ -1137,6 +1144,19 @@ void detail::findCodeMarker(
    codeMarkerItem->addDetectedMarkers(corners, ids, opt.getOffsetCropRect2Roi());
    codeMarkerItem->addRejectedMarkers(rejected, opt.getOffsetCropRect2Roi());

+    if(ids.empty())
+    {
+        // if no markers are found return to prevent opencv errors because of empty corners and thus empty rvecs vectors
+        return;
+    }
+
+    // value only relevant for axis length when drawing axes
+    float                  markerLength = (float) opt.getDetectorParams().getMinCornerDistance();
+    std::vector<cv::Vec3d> rvecs, tvecs;
+    cv::Mat                cameraMatrix = intrinsicCameraParams.cameraMatrix;
+    cv::Mat                distCoeff    = cv::Mat::zeros(cv::Size(1, 5), CV_32F);
+    cv::aruco::estimatePoseSingleMarkers(corners, markerLength, cameraMatrix, distCoeff, rvecs, tvecs);
+
    // detected code markers
    for(size_t i = 0; i < ids.size(); i++)
    {
@@ -1145,7 +1165,14 @@ void detail::findCodeMarker(
        double y =
            (corners.at(i).at(0).y + corners.at(i).at(1).y + corners.at(i).at(2).y + corners.at(i).at(3).y) * 0.25;

-        crossList.append(TrackPoint(Vec2F(x, y), 100, ids.at(i))); // 100 beste qualitaet
+        cv::Matx<double, 3, 3> rotMat;
+        cv::Rodrigues(rvecs[i], rotMat);
+
+        cv::Vec3d orientation = cv::normalize(rotMat * cv::Vec3d(0, 1, 0));
+
+        TrackPoint trackPoint(Vec2F(x, y), 100, ids.at(i)); // 100 beste qualitaet
+        trackPoint.setOrientation(orientation);
+        crossList.append(std::move(trackPoint));
    }
 }

@@ -1290,11 +1317,17 @@ void findContourMarker(
 * @param controlWidget
 * @param borderSize
 * @param bgFilter
+ * @param intrinsicCameraParams intrinsic parameters of the camera. Used for e.g. estimation of arucomarkers
 *
 * @return List of detected TrackPoints
 */
-QList<TrackPoint>
-Recognizer::getMarkerPos(cv::Mat &img, QRect &roi, Control *controlWidget, int borderSize, BackgroundFilter *bgFilter)
+QList<TrackPoint> Recognizer::getMarkerPos(
+    cv::Mat                     &img,
+    QRect                       &roi,
+    Control                     *controlWidget,
+    int                          borderSize,
+    BackgroundFilter            *bgFilter,
+    const IntrinsicCameraParams &intrinsicCameraParams)
 {
    int  markerBrightness    = controlWidget->getMarkerBrightness();
    bool ignoreWithoutMarker = controlWidget->isMarkerIgnoreWithoutChecked();
@@ -1322,13 +1355,20 @@ Recognizer::getMarkerPos(cv::Mat &img, QRect &roi, Control *controlWidget, int b
    {
        case RecognitionMethod::MultiColor:
            findMultiColorMarker(
-                tImg, crossList, controlWidget, ignoreWithoutMarker, v, mRecoMethod, mCodeMarkerOptions);
+                tImg,
+                crossList,
+                controlWidget,
+                ignoreWithoutMarker,
+                v,
+                mRecoMethod,
+                mCodeMarkerOptions,
+                intrinsicCameraParams);
            break;
        case RecognitionMethod::Color:
            findColorMarker(tImg, crossList, controlWidget);
            break;
        case RecognitionMethod::Code:
-            findCodeMarker(tImg, crossList, mRecoMethod, mCodeMarkerOptions);
+            findCodeMarker(tImg, crossList, mRecoMethod, mCodeMarkerOptions, intrinsicCameraParams);
            break;
        case RecognitionMethod::Casern:
            [[fallthrough]];

--- a/src/trackerReal.cpp
+++ b/src/trackerReal.cpp
@@ -191,6 +191,7 @@ int TrackerReal::calculate(
            const auto &person = persons[i];
            addFrames          = 0;
            firstFrame         = person.firstFrame();
+            Vec2F moveDir(0, 0); // used for head direction

            if(person.height() < MIN_HEIGHT + 1)
            {
@@ -297,51 +298,24 @@ int TrackerReal::calculate(
                                       << person.firstFrame() + j << ") of person " << i + 1
                                       << ", extrapolated height is used!" << std::endl;
                            }
-
-                            Vec2F moveDir(0, 0);
                            if(exportAutoCorrect)
                            {
                                moveDir += reco::autoCorrectColorMarker(person[j], mMainWindow->getControlWidget());
                            }

                            pos = imageItem->getPosReal((person[j] + moveDir + br).toQPointF(), bestZ);
-
-                            if((exportViewingDirection) &&
-                               (person[j].color().isValid())) // wenn blickrichtung mit ausgegeben werden soll
-                            {
-                                colPos =
-                                    imageItem->getPosReal((person[j].colPoint() + moveDir + br).toQPointF(), bestZ);
-                                trackPersonReal.addEnd(pos, firstFrame + j, colPos - pos);
-                            }
-                            else
-                            {
-                                trackPersonReal.addEnd(Vec3F(pos.x(), pos.y(), bestZ), firstFrame + j);
-                            }
+                            trackPersonReal.addEnd(Vec3F(pos.x(), pos.y(), bestZ), firstFrame + j);
                        }
                    }
                    else
                    {
-                        Vec2F moveDir(0, 0);
                        if(exportAutoCorrect)
                        {
                            moveDir += reco::autoCorrectColorMarker(person[j], mMainWindow->getControlWidget());
                        }

                        pos = imageItem->getPosReal((person[j] + moveDir + br).toQPointF(), height);
-                        // die frame nummer der animation wird TrackPoint der PersonReal mitgegeben,
-                        // da Index groesser sein kann, da vorher frames hinzugefuegt wurden duch
-                        // trackPersonReal.init(firstFrame+addFrames, height) oder aber innerhalb des trackink path
-                        // mit for schleife ueber f
-                        if((exportViewingDirection) &&
-                           (person[j].color().isValid())) // wenn blickrichtung mit ausgegeben werden soll
-                        {
-                            colPos = imageItem->getPosReal((person[j].colPoint() + moveDir + br).toQPointF(), height);
-                            trackPersonReal.addEnd(pos, firstFrame + j, colPos - pos);
-                        }
-                        else
-                        {
-                            trackPersonReal.addEnd(pos, firstFrame + j);
-                        }
+                        trackPersonReal.addEnd(pos, firstFrame + j);
                        if(exportAngleOfView)
                        {
                            angle = (90. -
@@ -356,6 +330,34 @@ int TrackerReal::calculate(
                    }
                }

+                if(exportViewingDirection)
+                {
+                    auto method = petrack->getControlWidget()->getRecoMethod();
+
+                    // multicolor markers can also be used together with code markers
+                    if(method == reco::RecognitionMethod::Code || method == reco::RecognitionMethod::MultiColor)
+                    {
+                        auto orientation = person[j].getOrientation();
+                        orientation      = petrack->getExtrCalibration()->camToWorldRotation(orientation);
+                        trackPersonReal.last().setViewDirection(Vec2F(orientation[0], orientation[1]).unit());
+                    }
+                    else
+                    {
+                        // old implementation for expeortViewingDirection did not check for specific marker, so just use
+                        // the else
+                        // die frame nummer der animation wird TrackPoint der PersonReal mitgegeben,
+                        // da Index groesser sein kann, da vorher frames hinzugefuegt wurden duch
+                        // trackPersonReal.init(firstFrame+addFrames, height) oder aber innerhalb des trackink path
+                        // mit for schleife ueber f
+                        if((exportViewingDirection) &&
+                           (person[j].color().isValid())) // wenn blickrichtung mit ausgegeben werden soll
+                        {
+                            colPos = imageItem->getPosReal((person[j].colPoint() + moveDir + br).toQPointF(), height);
+                            trackPersonReal.last().setViewDirection(colPos - pos);
+                        }
+                    }
+                }
+
                if(tmpMissingList.size() > 0)
                {
                    if((tmpMissingList.first() == firstFrame + j) && (person.trackPointExist(firstFrame + j + 1)))
@@ -391,7 +393,8 @@ int TrackerReal::calculate(
                            }
                            else
                            {
-                                Vec2F moveDir(0, 0);
+                                // use local variable an just reset the value
+                                moveDir.set(0, 0);
                                if(exportAutoCorrect)
                                {
                                    moveDir += reco::autoCorrectColorMarker(person[j], mMainWindow->getControlWidget());

--- a/ui/control.ui
+++ b/ui/control.ui
@@ -5427,7 +5427,7 @@
                   </size>
                  </property>
                  <property name="toolTip">
-                   <string>add direction of head (corresponding to view direction; possible with Japan and casern marker)</string>
+                   <string>add direction of head (corresponding to view direction; possible with Japan, casern and code marker)</string>
                  </property>
                  <property name="text">
                   <string>add head direction</string>
No results found