diff --git a/include/control.h b/include/control.h
index 779174bd34a73a6cddd412147cb77bf1138751f3..c4cf792a64ffa6393ec107476ba4d8293312b1f3 100644
--- 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
diff --git a/include/petrack.h b/include/petrack.h
index f43a213eeb8bedce08c444eb03a8c57f358a2ce2..1e501de63e91a1bb1b6d159005e82de8960d2af2 100644
--- 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);
diff --git a/include/recognition.h b/include/recognition.h
index 497f786d2b1e58a4f3a5ab12ee4daef2cef9e78a..a45250850e4b3229b635a556a0db6d9928c15d83 100644
--- 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
diff --git a/include/tracker.h b/include/tracker.h
index 0ede1655584c213bec93d7e4762f843d75bd8fba..f2e490fe7c461fb127c07a3789d6ca1d5b55d472 100644
--- 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; }
diff --git a/include/trackerReal.h b/include/trackerReal.h
index 5a0121470e1e9e954d6f273a0a6316b7a0accfe1..6d9a04ad9f640dc2f6a9612b892a7a1329785140 100644
--- 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)
diff --git a/src/control.cpp b/src/control.cpp
index 8766fc45890c34d8ea4119d084a12405062bad5e..851d41f6982d7a03ad23b333d7d3010a095170b9 100644
--- 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())
diff --git a/src/petrack.cpp b/src/petrack.cpp
index 8bc6da75af6205fa78f03d40e273b86c3c666f5b..922f9573d944ba4986cbf2d954a1d5f2c5881019 100644
--- 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())
diff --git a/src/recognition.cpp b/src/recognition.cpp
index f4a6d26d4cee960b84a8ff5ec1872216ec0aa074..6cee2c60b83694b10dcc30d69e9bc2c2f8bb191b 100644
--- 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]];
diff --git a/src/trackerReal.cpp b/src/trackerReal.cpp
index bc42d65d776d7e76e4aacd530a48c5e9641f0a6c..33d0479a8e912bd32c3f351b2f5770afb6d25bf7 100644
--- 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());
diff --git a/ui/control.ui b/ui/control.ui
index 3a4745b387742cc67cbadb7b5189881c307e682e..38d53ef66d0b3cc5b1e2d7164817519f06eaaa11 100644
--- 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>