vendor: OpenCV 5.0.0 snapshot at 755e50675d97db9b7d449d8bd6b09888646f6c6e

This commit is contained in:
Gitea Mirror Bot
2026-08-22 00:11:13 +08:00
commit 12022378a3
3872 changed files with 2513409 additions and 0 deletions
+2
View File
@@ -0,0 +1,2 @@
set(the_description "rapid - silhouette based 3D object tracking")
ocv_define_module(rapid opencv_core opencv_imgproc opencv_geometry WRAP python)
+40
View File
@@ -0,0 +1,40 @@
@inproceedings{harris1990rapid,
title={RAPID-a video rate object tracker.},
author={Harris, Chris and Stennett, Carl},
booktitle={BMVC},
pages={1--6},
year={1990}
}
@article{drummond2002real,
title={Real-time visual tracking of complex structures},
author={Drummond, Tom and Cipolla, Roberto},
journal={IEEE Transactions on pattern analysis and machine intelligence},
volume={24},
number={7},
pages={932--946},
year={2002},
publisher={IEEE}
}
@article{seo2013optimal,
title={Optimal local searching for fast and robust textureless 3D object tracking in highly cluttered backgrounds},
author={Seo, Byung-Kuk and Park, Hanhoon and Park, Jong-Il and Hinterstoisser, Stefan and Ilic, Slobodan},
journal={IEEE transactions on visualization and computer graphics},
volume={20},
number={1},
pages={99--110},
year={2013},
publisher={IEEE}
}
@article{wang2015global,
title={Global optimal searching for textureless 3D object tracking},
author={Wang, Guofeng and Wang, Bin and Zhong, Fan and Qin, Xueying and Chen, Baoquan},
journal={The Visual Computer},
volume={31},
number={6},
pages={979--988},
year={2015},
publisher={Springer}
}
+164
View File
@@ -0,0 +1,164 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#ifndef OPENCV_RAPID_HPP_
#define OPENCV_RAPID_HPP_
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
/**
@defgroup rapid silhouette based 3D object tracking
implements "RAPID-a video rate object tracker" @cite harris1990rapid with the dynamic control point extraction of @cite drummond2002real
*/
namespace cv
{
namespace rapid
{
//! @addtogroup rapid
//! @{
/**
* Debug draw markers of matched correspondences onto a lineBundle
* @param bundle the lineBundle
* @param cols column coordinates in the line bundle
* @param colors colors for the markers. Defaults to white.
*/
CV_EXPORTS_W void drawCorrespondencies(InputOutputArray bundle, InputArray cols,
InputArray colors = noArray());
/**
* Debug draw search lines onto an image
* @param img the output image
* @param locations the source locations of a line bundle
* @param color the line color
*/
CV_EXPORTS_W void drawSearchLines(InputOutputArray img, InputArray locations, const Scalar& color);
/**
* Draw a wireframe of a triangle mesh
* @param img the output image
* @param pts2d the 2d points obtained by @ref projectPoints
* @param tris triangle face connectivity
* @param color line color
* @param type line type. See @ref LineTypes.
* @param cullBackface enable back-face culling based on CCW order
*/
CV_EXPORTS_W void drawWireframe(InputOutputArray img, InputArray pts2d, InputArray tris,
const Scalar& color, int type = LINE_8, bool cullBackface = false);
/**
* Extract control points from the projected silhouette of a mesh
*
* see @cite drummond2002real Sec 2.1, Step b
* @param num number of control points
* @param len search radius (used to restrict the ROI)
* @param pts3d the 3D points of the mesh
* @param rvec rotation between mesh and camera
* @param tvec translation between mesh and camera
* @param K camera intrinsic
* @param imsize size of the video frame
* @param tris triangle face connectivity
* @param ctl2d the 2D locations of the control points
* @param ctl3d matching 3D points of the mesh
*/
CV_EXPORTS_W void extractControlPoints(int num, int len, InputArray pts3d, InputArray rvec, InputArray tvec,
InputArray K, const Size& imsize, InputArray tris, OutputArray ctl2d,
OutputArray ctl3d);
/**
* Extract the line bundle from an image
* @param len the search radius. The bundle will have `2*len + 1` columns.
* @param ctl2d the search lines will be centered at this points and orthogonal to the contour defined by
* them. The bundle will have as many rows.
* @param img the image to read the pixel intensities values from
* @param bundle line bundle image with size `ctl2d.rows() x (2 * len + 1)` and the same type as @p img
* @param srcLocations the source pixel locations of @p bundle in @p img as CV_16SC2
*/
CV_EXPORTS_W void extractLineBundle(int len, InputArray ctl2d, InputArray img, OutputArray bundle,
OutputArray srcLocations);
/**
* Find corresponding image locations by searching for a maximal sobel edge along the search line (a single
* row in the bundle)
* @param bundle the line bundle
* @param cols correspondence-position per line in line-bundle-space
* @param response the sobel response for the selected point
*/
CV_EXPORTS_W void findCorrespondencies(InputArray bundle, OutputArray cols,
OutputArray response = noArray());
/**
* Collect corresponding 2d and 3d points based on correspondencies and mask
* @param cols correspondence-position per line in line-bundle-space
* @param srcLocations the source image location
* @param pts2d 2d points
* @param pts3d 3d points
* @param mask mask containing non-zero values for the elements to be retained
*/
CV_EXPORTS_W void convertCorrespondencies(InputArray cols, InputArray srcLocations, OutputArray pts2d,
InputOutputArray pts3d = noArray(), InputArray mask = noArray());
/**
* High level function to execute a single rapid @cite harris1990rapid iteration
*
* 1. @ref extractControlPoints
* 2. @ref extractLineBundle
* 3. @ref findCorrespondencies
* 4. @ref convertCorrespondencies
* 5. @ref solvePnPRefineLM
*
* @param img the video frame
* @param num number of search lines
* @param len search line radius
* @param pts3d the 3D points of the mesh
* @param tris triangle face connectivity
* @param K camera matrix
* @param rvec rotation between mesh and camera. Input values are used as an initial solution.
* @param tvec translation between mesh and camera. Input values are used as an initial solution.
* @param rmsd the 2d reprojection difference
* @return ratio of search lines that could be extracted and matched
*/
CV_EXPORTS_W float rapid(InputArray img, int num, int len, InputArray pts3d, InputArray tris, InputArray K,
InputOutputArray rvec, InputOutputArray tvec, CV_OUT double* rmsd = 0);
/// Abstract base class for stateful silhouette trackers
class CV_EXPORTS_W Tracker : public Algorithm
{
public:
virtual ~Tracker();
CV_WRAP virtual float
compute(InputArray img, int num, int len, InputArray K, InputOutputArray rvec, InputOutputArray tvec,
const TermCriteria& termcrit = TermCriteria(TermCriteria::MAX_ITER | TermCriteria::EPS, 5, 1.5)) = 0;
CV_WRAP virtual void clearState() = 0;
};
/// wrapper around @ref rapid function for uniform access
class CV_EXPORTS_W Rapid : public Tracker
{
public:
CV_WRAP static Ptr<Rapid> create(InputArray pts3d, InputArray tris);
};
/** implements "Optimal local searching for fast and robust textureless 3D object tracking in highly
* cluttered backgrounds" @cite seo2013optimal
*/
class CV_EXPORTS_W OLSTracker : public Tracker
{
public:
CV_WRAP static Ptr<OLSTracker> create(InputArray pts3d, InputArray tris, int histBins = 8, uchar sobelThesh = 10);
};
/** implements "Global optimal searching for textureless 3D object tracking" @cite wang2015global
*/
class CV_EXPORTS_W GOSTracker : public Tracker
{
public:
CV_WRAP static Ptr<OLSTracker> create(InputArray pts3d, InputArray tris, int histBins = 4, uchar sobelThesh = 10);
};
//! @}
} /* namespace rapid */
} /* namespace cv */
#endif /* OPENCV_RAPID_HPP_ */
+48
View File
@@ -0,0 +1,48 @@
import numpy as np
import cv2 as cv
# aruco config
adict = cv.aruco.getPredefinedDictionary(cv.aruco.DICT_4X4_50)
cv.imshow("marker", cv.aruco.drawMarker(adict, 0, 400))
marker_len = 5
# rapid config
obj_points = np.float32([[-0.5, 0.5, 0], [0.5, 0.5, 0], [0.5, -0.5, 0], [-0.5, -0.5, 0]]) * marker_len
tris = np.int32([[0, 2, 1], [0, 3, 2]]) # note CCW order for culling
line_len = 10
# random calibration data. your mileage may vary.
imsize = (800, 600)
K = cv.getDefaultNewCameraMatrix(np.diag([800, 800, 1]), imsize, True)
# video capture
cap = cv.VideoCapture(0)
cap.set(cv.CAP_PROP_FRAME_WIDTH, imsize[0])
cap.set(cv.CAP_PROP_FRAME_HEIGHT, imsize[1])
rot, trans = None, None
while cv.waitKey(1) != 27:
img = cap.read()[1]
# detection with aruco
if rot is None:
corners, ids = cv.aruco.detectMarkers(img, adict)[:2]
if ids is not None:
rvecs, tvecs = cv.aruco.estimatePoseSingleMarkers(corners, marker_len, K, None)[:2]
rot, trans = rvecs[0].ravel(), tvecs[0].ravel()
# tracking and refinement with rapid
if rot is not None:
for i in range(5): # multiple iterations
ratio, rot, trans = cv.rapid.rapid(img, 40, line_len, obj_points, tris, K, rot, trans)[:3]
if ratio < 0.8:
# bad quality, force re-detect
rot, trans = None, None
break
# drawing
cv.putText(img, "detecting" if rot is None else "tracking", (0, 20), cv.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 255))
if rot is not None:
cv.drawFrameAxes(img, K, None, rot, trans, marker_len)
cv.imshow("tracking", img)
+366
View File
@@ -0,0 +1,366 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#include "precomp.hpp"
namespace cv
{
namespace rapid
{
static void compute1DCanny(const cv::Mat& src, cv::Mat& dst, uchar threshold)
{
compute1DSobel(src, dst);
// step2: compute 1D non-maximum suppression + threshold
for (int i = 0; i < dst.rows; i++)
{
for (int j = 1; j < dst.cols - 1; j++)
{
if (dst.at<uchar>(i, j) <= dst.at<uchar>(i, j - 1) || dst.at<uchar>(i, j) <= dst.at<uchar>(i, j + 1))
dst.at<uchar>(i, j) = 0;
// threshold
if(dst.at<uchar>(i, j) < threshold)
dst.at<uchar>(i, j) = 0;
}
}
}
static void calcHueSatHist(const Mat_<Vec3b>& hsv, Mat_<float>& hist)
{
for (int i = 0; i < hsv.rows; i++)
{
for (int j = 0; j < hsv.cols; j++)
{
const Vec3b& c = hsv(i, j);
// thresholds as in sec. 4.1
if (c[1] > 25 && c[2] > 50)
{
hist(c[0] * hist.rows / 256, c[1] * hist.cols / 256)++;
}
}
}
}
static float sum(const Mat_<float>& hist)
{
CV_DbgAssert(hist.isContinuous());
float ret = 0;
int N = int(hist.total());
const float* ptr = hist.ptr<float>();
for (int i = 0; i < N; i++)
ret += ptr[i];
return ret;
}
static double bhattacharyyaCoeff(const Mat& a, const Mat& b)
{
CV_DbgAssert(a.isContinuous() && b.isContinuous());
int N = int(a.total());
double ret = 0;
const float* aptr = a.ptr<float>();
const float* bptr = b.ptr<float>();
for (int i = 0; i < N; i++)
ret += std::sqrt(aptr[i] * bptr[i]);
return ret;
}
static void findCorrespondenciesOLS(const cv::Mat_<float>& scores, cv::Mat_<int>& cols)
{
cols.resize(scores.rows);
for (int i = 0; i < scores.rows; i++)
{
int pos = -1;
for (int j = scores.cols - 1; j >= 0; j--)
{
if (scores(i, j) >= 0.35)
{
pos = j;
break;
}
}
cols(i) = pos;
}
}
static float computeEdgeWeight(const cv::Vec2s& curCandiPoint, const cv::Vec2s& preCandiPoint)
{
float spatial_dist = (float)cv::norm(curCandiPoint - preCandiPoint, cv::NORM_L2SQR);
return std::exp(-spatial_dist/1000.0f);
}
static void findCorrespondenciesGOS(Mat& bundleGrad, Mat_<float>& fgScores, Mat_<float>& bgScores,
const Mat_<Vec2s>& imgLocations, Mat_<int>& cols)
{
// combine scores
Mat_<float> scores;
exp((fgScores + bgScores)/10.0f, scores);
Mat_<int> fromLocations(scores.size());
fromLocations = 0;
// source node
bool hasCandidate = false;
for(int j=0; j<bundleGrad.cols; j++)
{
if(bundleGrad.at<uchar>(0, j))
{
hasCandidate = true;
fromLocations(0, j) = j;
}
}
// fall back to using center as candidate
if(!hasCandidate)
{
fromLocations(0, bundleGrad.cols/2) = bundleGrad.cols/2;
}
int index_max_location = 0; // index in preceding line for backtracking
// the other layers
for(int i=1; i<bundleGrad.rows; i++)
{
hasCandidate = false;
for(int j=0; j<bundleGrad.cols; j++)
{
if(bundleGrad.at<uchar>(i, j))
hasCandidate = true;
}
if(!hasCandidate)
{
bundleGrad.at<uchar>(i, bundleGrad.cols/2) = 255;
}
for(int j=0; j<bundleGrad.cols; j++)
{
// search for max combined score
float max_energy = -INFINITY;
int location = bundleGrad.cols/2;
if(bundleGrad.at<uchar>(i, j))
{
for(int k=0; k<bundleGrad.cols; k++)
{
if(bundleGrad.at<uchar>(i - 1, k))
{
float edge_weight = computeEdgeWeight(imgLocations(i, j), imgLocations(i - 1, k));
float energy = scores(i, j) + scores(i-1, k) + edge_weight;
if(max_energy < energy)
{
max_energy = energy;
location = k;
}
}
}
scores(i, j) = max_energy; // update the score
fromLocations(i, j) = location;
index_max_location = j;
}
}
}
cols.resize(scores.rows);
// backtrack along best path
for (int i = bundleGrad.rows - 1; i >= 0; i--)
{
cols(i) = index_max_location;
index_max_location = fromLocations(i, index_max_location);
}
}
struct HistTrackerImpl : public OLSTracker
{
Mat vtx;
Mat tris;
Mat_<float> fgHist;
Mat_<float> bgHist;
double tau;
uchar sobelThresh;
bool useGOS;
HistTrackerImpl(InputArray _pts3d, InputArray _tris, int histBins, uchar _sobelThesh, bool _useGOS)
{
CV_Assert(_tris.getMat().checkVector(3, CV_32S) > 0);
CV_Assert(_pts3d.getMat().checkVector(3, CV_32F) > 0);
vtx = _pts3d.getMat();
tris = _tris.getMat();
tau = 0.7; // this is 1 - tau compared to OLS paper
sobelThresh = _sobelThesh;
useGOS = _useGOS;
bgHist.create(histBins, histBins);
}
void computeAppearanceScores(const Mat& bundleHSV, const Mat& bundleGrad, Mat_<float>& scores) const
{
scores.resize(bundleHSV.rows);
scores = 0;
Mat_<float> hist(fgHist.size());
for (int i = 0; i < bundleHSV.rows; i++)
{
int start = 0;
for (int j = 0; j < bundleHSV.cols; j++)
{
if (bundleGrad.at<uchar>(i, j))
{
// compute the histogram between last candidate point to current candidate point
// as in eq. (4)
hist = 0;
calcHueSatHist(bundleHSV({i, i + 1}, {start, j}), hist);
hist /= std::max(sum(hist), 1.0f);
double s = bhattacharyyaCoeff(fgHist, hist);
// handle object clutter as in eq. (5)
if(s > tau)
s = 1.0 - bhattacharyyaCoeff(bgHist, hist);
scores(i, j) = float(s);
start = j;
}
}
}
}
void computeBackgroundScores(const Mat& bundleHSV, const Mat& bundleGrad, Mat_<float>& scores)
{
scores.resize(bundleHSV.rows);
scores = 0;
Mat_<float> hist(fgHist.size());
for (int i = 0; i < bundleHSV.rows; i++)
{
int end = bundleHSV.cols - 1;
for (int j = bundleHSV.cols - 1; j >= 0; j--)
{
if (bundleGrad.at<uchar>(i, j))
{
// compute the histogram between last candidate point to current candidate point
hist = 0;
calcHueSatHist(bundleHSV({i, i + 1}, {j, end}), hist);
hist /= std::max(sum(hist), 1.0f);
double s = 1 - bhattacharyyaCoeff(fgHist, hist);
if (s <= tau)
s = bhattacharyyaCoeff(bgHist, hist);
scores(i, j) = float(s);
end = j;
}
}
}
}
void updateFgBgHist(const Mat_<Vec3b>& hsv, const Mat_<int>& cols)
{
fgHist = 0;
bgHist = 0;
for (int i = 0; i < hsv.rows; i++)
{
int col = cols(i) < 0 ? hsv.cols / 2 + 1 : cols(i);
calcHueSatHist(hsv({i, i + 1}, {0, col}), fgHist);
calcHueSatHist(hsv({i, i + 1}, {col + 1, hsv.cols}), bgHist);
}
fgHist /= sum(fgHist);
bgHist /= sum(bgHist);
}
float compute(InputArray img, int num, int len, InputArray K, InputOutputArray rvec,
InputOutputArray tvec, const TermCriteria& termcrit) CV_OVERRIDE
{
CV_Assert(num >= 3);
Mat pts2d, pts3d;
float ret = 0;
int niter = std::max(1, termcrit.maxCount);
for(int i = 0; i < niter; i++)
{
extractControlPoints(num, len, vtx, rvec, tvec, K, img.size(), tris, pts2d, pts3d);
if (pts2d.empty())
return 0;
Mat lineBundle, imgLoc;
extractLineBundle(len, pts2d, img, lineBundle, imgLoc);
Mat bundleHSV;
cvtColor(lineBundle, bundleHSV, COLOR_BGR2HSV_FULL);
Mat_<int> cols(num, 1);
if(fgHist.empty())
{
cols = len + 1;
fgHist.create(bgHist.size());
updateFgBgHist(bundleHSV, cols);
}
Mat bundleGrad;
compute1DCanny(lineBundle, bundleGrad, sobelThresh);
Mat_<float> scores(lineBundle.size());
computeAppearanceScores(bundleHSV, bundleGrad, scores);
if(useGOS)
{
Mat_<float> bgScores(scores.size());
computeBackgroundScores(bundleHSV, bundleGrad, bgScores);
findCorrespondenciesGOS(bundleGrad, scores, bgScores, imgLoc, cols);
}
else
{
findCorrespondenciesOLS(scores, cols);
}
convertCorrespondencies(cols, imgLoc, pts2d, pts3d, cols > -1);
if (pts2d.rows < 3)
return 0;
solvePnPRefineLM(pts3d, pts2d, K, cv::noArray(), rvec, tvec);
updateFgBgHist(bundleHSV, cols);
ret = float(pts2d.rows) / num;
if(termcrit.type & TermCriteria::EPS)
{
Mat tmp;
cols.copyTo(tmp, cols > 0);
tmp -= len + 1;
double rmsd = std::sqrt(norm(tmp, NORM_L2SQR) / tmp.rows);
if(rmsd < termcrit.epsilon)
break;
}
}
return ret;
}
void clearState() CV_OVERRIDE
{
fgHist.release();
}
};
Ptr<OLSTracker> OLSTracker::create(InputArray pts3d, InputArray tris, int histBins, uchar sobelThesh)
{
return makePtr<HistTrackerImpl>(pts3d, tris, histBins, sobelThesh, false);
}
Ptr<OLSTracker> GOSTracker::create(InputArray pts3d, InputArray tris, int histBins, uchar sobelThesh)
{
return makePtr<HistTrackerImpl>(pts3d, tris, histBins, sobelThesh, true);
}
} // namespace rapid
} // namespace cv
+19
View File
@@ -0,0 +1,19 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#ifndef __OPENCV_PRECOMP_H__
#define __OPENCV_PRECOMP_H__
#include "opencv2/rapid.hpp"
#include <vector>
#include <opencv2/geometry.hpp>
namespace cv
{
namespace rapid
{
void compute1DSobel(const Mat& src, Mat& dst);
}
} // namespace cv
#endif
+413
View File
@@ -0,0 +1,413 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#include "precomp.hpp"
namespace cv
{
namespace rapid
{
static std::vector<int> getSilhoutteVertices(const Size& imsize, const std::vector<Point>& contour,
const Mat_<Point2f>& pts2d)
{
// store indices
Mat_<int> img1(imsize, 0);
Rect img_rect({0, 0}, imsize);
for (int i = 0; i < pts2d.rows; i++) {
// Workaround for https://github.com/opencv/opencv/issues/26016
// To keep its behaviour, pts2d casts to Point_<int>.
if (img_rect.contains(Point_<int>(pts2d(i)))) {
img1(pts2d(i)) = i + 1;
}
}
std::vector<int> v_idx;
// look up indices on contour
for (size_t i = 0; i < contour.size(); i++) {
if (int idx = img1(contour[i])) {
v_idx.push_back(idx - 1);
}
}
return v_idx;
}
class Contour3DSampler {
std::vector<int> idx; // indices of points on contour
std::vector<float> cum_dist; // prefix sum
Mat_<Point2f> ipts2d;
Mat_<Point3f> ipts3d;
float lambda;
int pos;
public:
float perimeter;
Contour3DSampler(const Mat_<Point2f>& pts2d, const Mat_<Point3f>& pts3d,
const std::vector<Point>& contour, const Size& imsize)
: ipts2d(pts2d), ipts3d(pts3d)
{
idx = getSilhoutteVertices(imsize, contour, pts2d);
CV_Assert(!idx.empty());
// close the loop
idx.push_back(idx[0]);
cum_dist.resize(idx.size());
perimeter = 0.0f;
for (size_t i = 1; i < idx.size(); i++) {
perimeter += (float)norm(pts2d(idx[i]) - pts2d(idx[i - 1]));
cum_dist[i] = perimeter;
}
pos = 0;
lambda = 0;
}
void advanceTo(float dist)
{
while (pos < int(cum_dist.size() - 1) && dist >= cum_dist[pos]) {
pos++;
}
lambda = (dist - cum_dist[pos - 1]) / (cum_dist[pos] - cum_dist[pos - 1]);
}
Point3f current3D() const { return (1 - lambda) * ipts3d(idx[pos - 1]) + lambda * ipts3d(idx[pos]); }
Point2f current2D() const { return (1 - lambda) * ipts2d(idx[pos - 1]) + lambda * ipts2d(idx[pos]); }
};
void drawWireframe(InputOutputArray img, InputArray _pts2d, InputArray _tris,
const Scalar& color, int type, bool cullBackface)
{
CV_Assert(_tris.getMat().checkVector(3, CV_32S) > 0);
CV_Assert(_pts2d.getMat().checkVector(2, CV_32F) > 0);
Mat_<Vec3i> tris = _tris.getMat();
Mat_<Point2f> pts2d = _pts2d.getMat();
for (int i = 0; i < int(tris.total()); i++) {
const auto& idx = tris(i);
std::vector<Point> poly = {pts2d(idx[0]), pts2d(idx[1]), pts2d(idx[2])};
// skip back facing triangles
if (cullBackface && ((poly[2] - poly[0]).cross(poly[2] - poly[1]) >= 0))
continue;
polylines(img, poly, true, color, 1, type);
}
}
void drawSearchLines(InputOutputArray img, InputArray _locations, const Scalar& color)
{
Mat locations = _locations.getMat();
CV_CheckTypeEQ(_locations.type(), CV_16SC2, "Vec2s data type expected");
for (int i = 0; i < locations.rows; i++) {
Point pt1(locations.at<Vec2s>(i, 0));
Point pt2(locations.at<Vec2s>(i, locations.cols - 1));
line(img, pt1, pt2, color, 1);
}
}
static void sampleControlPoints(int num, Contour3DSampler& sampler, const Rect& roi, OutputArray _opts2d,
OutputArray _opts3d)
{
std::vector<Vec3f> opts3d;
opts3d.reserve(num);
std::vector<Vec2f> opts2d;
opts2d.reserve(num);
// sample at equal steps
float step = sampler.perimeter / num;
if (step == 0)
num = 0; // edge case -> skip loop
for (int i = 0; i < num; i++) {
sampler.advanceTo(step * i);
auto pt2d = sampler.current2D();
// skip points too close to border
//
// Workaround for https://github.com/opencv/opencv/issues/26016
// To keep its behaviour, pt2d casts to Point_<int>.
if (!roi.contains(Point_<int>(pt2d)))
continue;
opts3d.push_back(sampler.current3D());
opts2d.push_back(pt2d);
}
Mat(opts3d).copyTo(_opts3d);
Mat(opts2d).copyTo(_opts2d);
}
void extractControlPoints(int num, int len, InputArray pts3d, InputArray rvec, InputArray tvec,
InputArray K, const Size& imsize, InputArray tris, OutputArray ctl2d,
OutputArray ctl3d)
{
CV_Assert(num);
Mat_<Point2f> pts2d(pts3d.rows(), 1);
projectPoints(pts3d, rvec, tvec, K, noArray(), pts2d);
Mat_<uchar> img(imsize, uchar(0));
drawWireframe(img, pts2d, tris.getMat(), 255, LINE_8, true);
// find contour
std::vector<std::vector<Point>> contours;
findContours(img, contours, RETR_EXTERNAL, CHAIN_APPROX_NONE);
CV_Assert(!contours.empty());
Contour3DSampler sampler(pts2d, pts3d.getMat(), contours[0], imsize);
Rect valid_roi(Point(len, len), imsize - Size(2 * len, 2 * len));
sampleControlPoints(num, sampler, valid_roi, ctl2d, ctl3d);
}
void extractLineBundle(int len, InputArray ctl2d, InputArray img, OutputArray bundle,
OutputArray srcLocations)
{
CV_Assert(len > 0);
Mat _img = img.getMat();
CV_Assert(ctl2d.getMat().checkVector(2, CV_32F) > 0);
Mat_<Point2f> contour = ctl2d.getMat();
const int N = (int)contour.total();
const int W = len * 2 + 1;
srcLocations.create(N, W, CV_16SC2);
Mat_<Vec2s> _srcLocations = srcLocations.getMat();
for (int i = 0; i < N; i++) {
// central difference
const Point2f diff = contour((i + 1) % N) - contour((i - 1 + N) % N);
Point2f n(normalize(Vec2f(-diff.y, diff.x))); // perpendicular to diff
// make it cover L pixels
n *= len / std::max(std::abs(n.x), std::abs(n.y));
LineIterator li(_img, contour(i) - n, contour(i) + n);
CV_DbgAssert(li.count == W);
for (int j = 0; j < li.count; j++, ++li) {
_srcLocations(i, j) = Vec2i(li.pos());
}
}
remap(img, bundle, srcLocations, noArray(),
INTER_NEAREST); // inter_nearest as we use integer locations
}
void compute1DSobel(const Mat& src, Mat& dst)
{
CV_CheckDepthEQ(src.depth(), CV_8U, "only uchar images supported");
int channels = src.channels();
CV_Assert(channels == 1 || channels == 3);
dst.create(src.size(), CV_8U);
for (int i = 0; i < src.rows; i++) {
for (int j = 1; j < src.cols - 1; j++) {
// central difference kernel: [-1, 0, 1]
if (channels == 3) {
const Vec3s diff = Vec3s(src.at<Vec3b>(i, j + 1)) - Vec3s(src.at<Vec3b>(i, j - 1));
dst.at<uchar>(i, j) =
(uchar)std::max(std::max(std::abs(diff[0]), std::abs(diff[1])), std::abs(diff[2]));
} else {
dst.at<uchar>(i, j) = (uchar)std::abs(src.at<uchar>(i, j + 1) - src.at<uchar>(i, j - 1));
}
}
dst.at<uchar>(i, 0) = dst.at<uchar>(i, src.cols - 1) = 0; // border
}
}
void findCorrespondencies(InputArray bundle, OutputArray _cols, OutputArray _response)
{
Mat_<uchar> sobel;
compute1DSobel(bundle.getMat(), sobel);
_cols.create(sobel.rows, 1, CV_32S);
Mat_<int> cols = _cols.getMat();
Mat_<uchar> response;
if (_response.needed()) {
_response.create(sobel.rows, 1, CV_8U);
response = _response.getMat();
}
// sobel.cols = 2*len + 1
const int len = sobel.cols / 2;
const int ct = len + 1;
// find closest maximum to center
for (int i = 0; i < sobel.rows; i++) {
int pos = ct;
uchar mx = sobel.at<uchar>(i, ct);
for (int j = 0; j < len; j++) {
uchar right = sobel.at<uchar>(i, ct + j);
uchar left = sobel.at<uchar>(i, ct - j);
if (right > mx) {
mx = right;
pos = ct + j;
}
if (left > mx) {
mx = left;
pos = ct - j;
}
}
if (!response.empty())
response(i) = mx;
cols(i) = pos;
}
}
void drawCorrespondencies(InputOutputArray _bundle, InputArray _cols, InputArray _colors)
{
CV_CheckTypeEQ(_cols.type(), CV_32S, "cols must be of int type");
CV_Assert(_bundle.rows() == _cols.rows());
CV_Assert(_colors.empty() || _colors.rows() == _cols.rows());
Mat bundle = _bundle.getMat();
Mat_<int> cols = _cols.getMat();
Mat_<Vec4d> colors = _colors.getMat();
for (int i = 0; i < bundle.rows; i++) {
bundle(Rect(Point(cols(i), i), Size(1, 1))) = colors.empty() ? Scalar::all(255) : colors(i);
}
}
void convertCorrespondencies(InputArray _cols, InputArray _srcLocations, OutputArray _pts2d,
InputOutputArray _pts3d, InputArray _mask)
{
CV_CheckTypeEQ(_cols.type(), CV_32S, "cols must be of int type");
CV_CheckTypeEQ(_srcLocations.type(), CV_16SC2, "Vec2s data type expected");
CV_Assert(_srcLocations.rows() == _cols.rows());
Mat_<cv::Vec2s> srcLocations = _srcLocations.getMat();
Mat_<int> cols = _cols.getMat();
Mat pts2d = Mat(0, 1, CV_16SC2);
pts2d.reserve(cols.rows);
Mat_<uchar> mask;
if (!_mask.empty())
{
CV_CheckTypeEQ(_mask.type(), CV_8UC1, "mask must be of uchar type");
CV_Assert(_cols.rows() == _mask.rows());
mask = _mask.getMat();
}
Mat pts3d;
Mat opts3d;
if(!_pts3d.empty())
{
pts3d = _pts3d.getMat().t();
CV_Assert(cols.rows == pts3d.rows);
opts3d.create(0, 1, pts3d.type());
opts3d.reserve(cols.rows);
}
for (int i = 0; i < cols.rows; i++) {
if (!mask.empty() && !mask(i))
continue;
pts2d.push_back(srcLocations(i, cols(i)));
if(!pts3d.empty())
opts3d.push_back(pts3d.row(i));
}
pts2d.copyTo(_pts2d);
if(!pts3d.empty())
opts3d.copyTo(_pts3d);
}
float rapid(InputArray img, int num, int len, InputArray vtx, InputArray tris, InputArray K,
InputOutputArray rvec, InputOutputArray tvec, double* rmsd)
{
CV_Assert(num >= 3);
Mat pts2d, pts3d;
extractControlPoints(num, len, vtx, rvec, tvec, K, img.size(), tris, pts2d, pts3d);
if (pts2d.empty())
return 0;
Mat lineBundle, imgLoc;
extractLineBundle(len, pts2d, img, lineBundle, imgLoc);
Mat cols, response;
findCorrespondencies(lineBundle, cols, response);
const uchar sobel_thresh = 20;
Mat mask = response > sobel_thresh;
convertCorrespondencies(cols, imgLoc, pts2d, pts3d, mask);
if(rmsd)
{
cols.copyTo(cols, mask);
cols -= len + 1;
*rmsd = std::sqrt(norm(cols, NORM_L2SQR) / cols.rows);
}
if (pts2d.rows < 3)
return 0;
solvePnPRefineLM(pts3d, pts2d, K, cv::noArray(), rvec, tvec);
return float(pts2d.rows) / num;
}
Tracker::~Tracker() {}
struct RapidImpl : public Rapid
{
Mat pts3d;
Mat tris;
RapidImpl(InputArray _pts3d, InputArray _tris)
{
CV_Assert(_tris.getMat().checkVector(3, CV_32S) > 0);
CV_Assert(_pts3d.getMat().checkVector(3, CV_32F) > 0);
pts3d = _pts3d.getMat();
tris = _tris.getMat();
}
float compute(InputArray img, int num, int len, InputArray K, InputOutputArray rvec,
InputOutputArray tvec, const TermCriteria& termcrit) CV_OVERRIDE
{
float ret = 0;
int niter = std::max(1, termcrit.maxCount);
double rmsd;
Mat cols;
for(int i = 0; i < niter; i++)
{
ret = rapid(img, num, len, pts3d, tris, K, rvec, tvec,
termcrit.type & TermCriteria::EPS ? &rmsd : NULL);
if((termcrit.type & TermCriteria::EPS) && rmsd < termcrit.epsilon)
{
break;
}
}
return ret;
}
void clearState() CV_OVERRIDE
{
// nothing to do
}
};
Ptr<Rapid> Rapid::create(InputArray pts3d, InputArray tris)
{
return makePtr<RapidImpl>(pts3d, tris);
}
} /* namespace rapid */
} /* namespace cv */
+47
View File
@@ -0,0 +1,47 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#include "test_precomp.hpp"
CV_TEST_MAIN("cv")
namespace opencv_test { namespace {
TEST(CV_Rapid, rapid)
{
// a unit sized box
std::vector<Vec3f> vtx = {
{1, -1, -1}, {1, -1, 1}, {-1, -1, 1}, {-1, -1, -1}, {1, 1, -1}, {1, 1, 1}, {-1, 1, 1}, {-1, 1, -1},
};
std::vector<Vec3i> tris = {
{2, 4, 1}, {8, 6, 5}, {5, 2, 1}, {6, 3, 2}, {3, 8, 4}, {1, 8, 5},
{2, 3, 4}, {8, 7, 6}, {5, 6, 2}, {6, 7, 3}, {3, 7, 8}, {1, 4, 8},
};
Mat(tris) -= Scalar(1, 1, 1);
// camera setup
Size sz(1280, 720);
Mat K = getDefaultNewCameraMatrix(Matx33f::diag(Vec3f(800, 800, 1)), sz, true);
Vec3f trans = {0, 0, 5};
Vec3f rot = {0.7f, 0.6f, 0};
// draw something
Mat pts2d;
projectPoints(vtx, rot, trans, K, noArray(), pts2d);
Mat_<uchar> img(sz, uchar(0));
rapid::drawWireframe(img, pts2d, tris, Scalar(255), LINE_8);
// recover pose form different position
Vec3f t_init = Vec3f(0.1f, 0, 5);
auto tracker = rapid::Rapid::create(vtx, tris);
// do two iterations
TermCriteria term(TermCriteria::MAX_ITER, 2, 0);
tracker->compute(img, 100, 20, K, rot, t_init, term);
// assert that it improved from init
ASSERT_LT(cv::norm(trans - t_init), 0.075);
}
}} // namespace
+12
View File
@@ -0,0 +1,12 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#ifndef __OPENCV_TEST_PRECOMP_HPP__
#define __OPENCV_TEST_PRECOMP_HPP__
#include "opencv2/ts.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/geometry.hpp"
#include "opencv2/rapid.hpp"
#endif