|
|
|
@ -404,8 +404,183 @@ Mat estimateGlobalMotionRobust( |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
MotionEstimatorRansacL2::MotionEstimatorRansacL2(MotionModel model) |
|
|
|
|
: MotionEstimatorBase(model) |
|
|
|
|
{ |
|
|
|
|
setRansacParams(RansacParams::default2dMotion(model)); |
|
|
|
|
setMinInlierRatio(0.1f); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Mat MotionEstimatorRansacL2::estimate(InputArray points0, InputArray points1, bool *ok) |
|
|
|
|
{ |
|
|
|
|
CV_Assert(points0.type() == points1.type()); |
|
|
|
|
const int npoints = points0.getMat().checkVector(2); |
|
|
|
|
CV_Assert(points1.getMat().checkVector(2) == npoints); |
|
|
|
|
|
|
|
|
|
// find motion
|
|
|
|
|
|
|
|
|
|
int ninliers = 0; |
|
|
|
|
Mat_<float> M; |
|
|
|
|
|
|
|
|
|
if (motionModel() != MM_HOMOGRAPHY) |
|
|
|
|
M = estimateGlobalMotionRobust( |
|
|
|
|
points0, points1, motionModel(), ransacParams_, 0, &ninliers); |
|
|
|
|
else |
|
|
|
|
{ |
|
|
|
|
vector<uchar> mask; |
|
|
|
|
M = findHomography(points0, points1, mask, CV_RANSAC, ransacParams_.thresh); |
|
|
|
|
for (int i = 0; i < npoints; ++i) |
|
|
|
|
if (mask[i]) ninliers++; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// check if we're confident enough in estimated motion
|
|
|
|
|
|
|
|
|
|
if (ok) *ok = true; |
|
|
|
|
if (static_cast<float>(ninliers) / npoints < minInlierRatio_) |
|
|
|
|
{ |
|
|
|
|
M = Mat::eye(3, 3, CV_32F); |
|
|
|
|
if (ok) *ok = false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
return M; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
MotionEstimatorL1::MotionEstimatorL1(MotionModel model) |
|
|
|
|
: MotionEstimatorBase(model) |
|
|
|
|
{ |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// TODO will estimation of all motions as one LP problem be faster?
|
|
|
|
|
Mat MotionEstimatorL1::estimate(InputArray points0, InputArray points1, bool *ok) |
|
|
|
|
{ |
|
|
|
|
CV_Assert(points0.type() == points1.type()); |
|
|
|
|
const int npoints = points0.getMat().checkVector(2); |
|
|
|
|
CV_Assert(points1.getMat().checkVector(2) == npoints); |
|
|
|
|
|
|
|
|
|
const Point2f *points0_ = points0.getMat().ptr<Point2f>(); |
|
|
|
|
const Point2f *points1_ = points1.getMat().ptr<Point2f>(); |
|
|
|
|
|
|
|
|
|
#ifndef HAVE_CLP |
|
|
|
|
|
|
|
|
|
CV_Error(CV_StsError, "The library is built without Clp support"); |
|
|
|
|
if (ok) *ok = false; |
|
|
|
|
return Mat::eye(3, 3, CV_32F); |
|
|
|
|
|
|
|
|
|
#else |
|
|
|
|
|
|
|
|
|
CV_Assert(motionModel() <= MM_AFFINE && motionModel() != MM_RIGID); |
|
|
|
|
|
|
|
|
|
// prepare LP problem
|
|
|
|
|
|
|
|
|
|
int ncols = 6 + 2*npoints; |
|
|
|
|
int nrows = 4*npoints; |
|
|
|
|
|
|
|
|
|
if (motionModel() == MM_SIMILARITY) |
|
|
|
|
nrows += 2; |
|
|
|
|
else if (motionModel() == MM_TRANSLATION_AND_SCALE) |
|
|
|
|
nrows += 3; |
|
|
|
|
else if (motionModel() == MM_TRANSLATION) |
|
|
|
|
nrows += 4; |
|
|
|
|
|
|
|
|
|
rows_.clear(); |
|
|
|
|
cols_.clear(); |
|
|
|
|
elems_.clear(); |
|
|
|
|
obj_.assign(ncols, 0); |
|
|
|
|
collb_.assign(ncols, -INF); |
|
|
|
|
colub_.assign(ncols, INF); |
|
|
|
|
|
|
|
|
|
int c = 6; |
|
|
|
|
|
|
|
|
|
for (int i = 0; i < npoints; ++i, c += 2) |
|
|
|
|
{ |
|
|
|
|
obj_[c] = 1; |
|
|
|
|
collb_[c] = 0; |
|
|
|
|
|
|
|
|
|
obj_[c+1] = 1; |
|
|
|
|
collb_[c+1] = 0; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
elems_.clear(); |
|
|
|
|
rowlb_.assign(nrows, -INF); |
|
|
|
|
rowub_.assign(nrows, INF); |
|
|
|
|
|
|
|
|
|
int r = 0; |
|
|
|
|
Point2f p0, p1; |
|
|
|
|
|
|
|
|
|
for (int i = 0; i < npoints; ++i, r += 4) |
|
|
|
|
{ |
|
|
|
|
p0 = points0_[i]; |
|
|
|
|
p1 = points1_[i]; |
|
|
|
|
|
|
|
|
|
set(r, 0, p0.x); set(r, 1, p0.y); set(r, 2, 1); set(r, 6+2*i, -1); |
|
|
|
|
rowub_[r] = p1.x; |
|
|
|
|
|
|
|
|
|
set(r+1, 3, p0.x); set(r+1, 4, p0.y); set(r+1, 5, 1); set(r+1, 6+2*i+1, -1); |
|
|
|
|
rowub_[r+1] = p1.y; |
|
|
|
|
|
|
|
|
|
set(r+2, 0, p0.x); set(r+2, 1, p0.y); set(r+2, 2, 1); set(r+2, 6+2*i, 1); |
|
|
|
|
rowlb_[r+2] = p1.x; |
|
|
|
|
|
|
|
|
|
set(r+3, 3, p0.x); set(r+3, 4, p0.y); set(r+3, 5, 1); set(r+3, 6+2*i+1, 1); |
|
|
|
|
rowlb_[r+3] = p1.y; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (motionModel() == MM_SIMILARITY) |
|
|
|
|
{ |
|
|
|
|
set(r, 0, 1); set(r, 4, -1); rowlb_[r] = rowub_[r] = 0; |
|
|
|
|
set(r+1, 1, 1); set(r+1, 3, 1); rowlb_[r+1] = rowub_[r+1] = 0; |
|
|
|
|
} |
|
|
|
|
else if (motionModel() == MM_TRANSLATION_AND_SCALE) |
|
|
|
|
{ |
|
|
|
|
set(r, 0, 1); set(r, 4, -1); rowlb_[r] = rowub_[r] = 0; |
|
|
|
|
set(r+1, 1, 1); rowlb_[r+1] = rowub_[r+1] = 0; |
|
|
|
|
set(r+2, 3, 1); rowlb_[r+2] = rowub_[r+2] = 0; |
|
|
|
|
} |
|
|
|
|
else if (motionModel() == MM_TRANSLATION) |
|
|
|
|
{ |
|
|
|
|
set(r, 0, 1); rowlb_[r] = rowub_[r] = 1; |
|
|
|
|
set(r+1, 1, 1); rowlb_[r+1] = rowub_[r+1] = 0; |
|
|
|
|
set(r+2, 3, 1); rowlb_[r+2] = rowub_[r+2] = 0; |
|
|
|
|
set(r+3, 4, 1); rowlb_[r+3] = rowub_[r+3] = 1; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// solve
|
|
|
|
|
|
|
|
|
|
CoinPackedMatrix A(true, &rows_[0], &cols_[0], &elems_[0], elems_.size()); |
|
|
|
|
A.setDimensions(nrows, ncols); |
|
|
|
|
|
|
|
|
|
ClpSimplex model(false); |
|
|
|
|
model.loadProblem(A, &collb_[0], &colub_[0], &obj_[0], &rowlb_[0], &rowub_[0]); |
|
|
|
|
|
|
|
|
|
ClpDualRowSteepest dualSteep(1); |
|
|
|
|
model.setDualRowPivotAlgorithm(dualSteep); |
|
|
|
|
model.scaling(1); |
|
|
|
|
|
|
|
|
|
model.dual(); |
|
|
|
|
|
|
|
|
|
// extract motion
|
|
|
|
|
|
|
|
|
|
const double *sol = model.getColSolution(); |
|
|
|
|
|
|
|
|
|
Mat_<float> M = Mat::eye(3, 3, CV_32F); |
|
|
|
|
M(0,0) = sol[0]; |
|
|
|
|
M(0,1) = sol[1]; |
|
|
|
|
M(0,2) = sol[2]; |
|
|
|
|
M(1,0) = sol[3]; |
|
|
|
|
M(1,1) = sol[4]; |
|
|
|
|
M(1,2) = sol[5]; |
|
|
|
|
|
|
|
|
|
if (ok) *ok = true; |
|
|
|
|
return M; |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
FromFileMotionReader::FromFileMotionReader(const string &path) |
|
|
|
|
: GlobalMotionEstimatorBase(MM_UNKNOWN) |
|
|
|
|
: ImageMotionEstimatorBase(MM_UNKNOWN) |
|
|
|
|
{ |
|
|
|
|
file_.open(path.c_str()); |
|
|
|
|
CV_Assert(file_.is_open()); |
|
|
|
@ -424,19 +599,18 @@ Mat FromFileMotionReader::estimate(const Mat &/*frame0*/, const Mat &/*frame1*/, |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
ToFileMotionWriter::ToFileMotionWriter(const string &path, Ptr<GlobalMotionEstimatorBase> estimator) |
|
|
|
|
: GlobalMotionEstimatorBase(estimator->motionModel()) |
|
|
|
|
ToFileMotionWriter::ToFileMotionWriter(const string &path, Ptr<ImageMotionEstimatorBase> estimator) |
|
|
|
|
: ImageMotionEstimatorBase(estimator->motionModel()), motionEstimator_(estimator) |
|
|
|
|
{ |
|
|
|
|
file_.open(path.c_str()); |
|
|
|
|
CV_Assert(file_.is_open()); |
|
|
|
|
estimator_ = estimator; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Mat ToFileMotionWriter::estimate(const Mat &frame0, const Mat &frame1, bool *ok) |
|
|
|
|
{ |
|
|
|
|
bool ok_; |
|
|
|
|
Mat_<float> M = estimator_->estimate(frame0, frame1, &ok_); |
|
|
|
|
Mat_<float> M = motionEstimator_->estimate(frame0, frame1, &ok_); |
|
|
|
|
file_ << M(0,0) << " " << M(0,1) << " " << M(0,2) << " " |
|
|
|
|
<< M(1,0) << " " << M(1,1) << " " << M(1,2) << " " |
|
|
|
|
<< M(2,0) << " " << M(2,1) << " " << M(2,2) << " " << ok_ << endl; |
|
|
|
@ -445,43 +619,26 @@ Mat ToFileMotionWriter::estimate(const Mat &frame0, const Mat &frame1, bool *ok) |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
RansacMotionEstimator::RansacMotionEstimator(MotionModel model) |
|
|
|
|
: GlobalMotionEstimatorBase(model) |
|
|
|
|
{ |
|
|
|
|
KeypointBasedMotionEstimator::KeypointBasedMotionEstimator(Ptr<MotionEstimatorBase> estimator) |
|
|
|
|
: ImageMotionEstimatorBase(estimator->motionModel()), motionEstimator_(estimator) |
|
|
|
|
{
|
|
|
|
|
setDetector(new GoodFeaturesToTrackDetector()); |
|
|
|
|
setOptFlowEstimator(new SparsePyrLkOptFlowEstimator()); |
|
|
|
|
setGridSize(Size(0,0)); |
|
|
|
|
setRansacParams(RansacParams::default2dMotion(model)); |
|
|
|
|
setOpticalFlowEstimator(new SparsePyrLkOptFlowEstimator()); |
|
|
|
|
setOutlierRejector(new NullOutlierRejector()); |
|
|
|
|
setMinInlierRatio(0.1f); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Mat RansacMotionEstimator::estimate(const Mat &frame0, const Mat &frame1, bool *ok) |
|
|
|
|
Mat KeypointBasedMotionEstimator::estimate(const Mat &frame0, const Mat &frame1, bool *ok) |
|
|
|
|
{ |
|
|
|
|
// find keypoints
|
|
|
|
|
|
|
|
|
|
detector_->detect(frame0, keypointsPrev_); |
|
|
|
|
|
|
|
|
|
// add extra keypoints
|
|
|
|
|
|
|
|
|
|
if (gridSize_.width > 0 && gridSize_.height > 0) |
|
|
|
|
{ |
|
|
|
|
float dx = static_cast<float>(frame0.cols) / (gridSize_.width + 1); |
|
|
|
|
float dy = static_cast<float>(frame0.rows) / (gridSize_.height + 1); |
|
|
|
|
for (int x = 0; x < gridSize_.width; ++x) |
|
|
|
|
for (int y = 0; y < gridSize_.height; ++y) |
|
|
|
|
keypointsPrev_.push_back(KeyPoint((x+1)*dx, (y+1)*dy, 0.f)); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// extract points from keypoints
|
|
|
|
|
|
|
|
|
|
pointsPrev_.resize(keypointsPrev_.size()); |
|
|
|
|
for (size_t i = 0; i < keypointsPrev_.size(); ++i) |
|
|
|
|
pointsPrev_[i] = keypointsPrev_[i].pt; |
|
|
|
|
|
|
|
|
|
// find correspondences
|
|
|
|
|
|
|
|
|
|
optFlowEstimator_->run(frame0, frame1, pointsPrev_, points_, status_, noArray()); |
|
|
|
|
|
|
|
|
|
// leave good correspondences only
|
|
|
|
@ -498,7 +655,7 @@ Mat RansacMotionEstimator::estimate(const Mat &frame0, const Mat &frame1, bool * |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// perfrom outlier rejection
|
|
|
|
|
// perform outlier rejection
|
|
|
|
|
|
|
|
|
|
IOutlierRejector *outlierRejector = static_cast<IOutlierRejector*>(outlierRejector_); |
|
|
|
|
if (!dynamic_cast<NullOutlierRejector*>(outlierRejector)) |
|
|
|
@ -508,8 +665,11 @@ Mat RansacMotionEstimator::estimate(const Mat &frame0, const Mat &frame1, bool * |
|
|
|
|
|
|
|
|
|
outlierRejector_->process(frame0.size(), pointsPrev_, points_, status_); |
|
|
|
|
|
|
|
|
|
pointsPrevGood_.clear(); pointsPrevGood_.reserve(points_.size()); |
|
|
|
|
pointsGood_.clear(); pointsGood_.reserve(points_.size()); |
|
|
|
|
pointsPrevGood_.clear(); |
|
|
|
|
pointsPrevGood_.reserve(points_.size()); |
|
|
|
|
|
|
|
|
|
pointsGood_.clear(); |
|
|
|
|
pointsGood_.reserve(points_.size()); |
|
|
|
|
|
|
|
|
|
for (size_t i = 0; i < points_.size(); ++i) |
|
|
|
|
{ |
|
|
|
@ -521,49 +681,21 @@ Mat RansacMotionEstimator::estimate(const Mat &frame0, const Mat &frame1, bool * |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
size_t npoints = pointsGood_.size(); |
|
|
|
|
|
|
|
|
|
// find motion
|
|
|
|
|
|
|
|
|
|
int ninliers = 0; |
|
|
|
|
Mat_<float> M; |
|
|
|
|
|
|
|
|
|
if (motionModel_ != MM_HOMOGRAPHY) |
|
|
|
|
M = estimateGlobalMotionRobust( |
|
|
|
|
pointsPrevGood_, pointsGood_, motionModel_, ransacParams_, 0, &ninliers); |
|
|
|
|
else |
|
|
|
|
{ |
|
|
|
|
vector<uchar> mask; |
|
|
|
|
M = findHomography(pointsPrevGood_, pointsGood_, mask, CV_RANSAC, ransacParams_.thresh); |
|
|
|
|
for (size_t i = 0; i < npoints; ++i) |
|
|
|
|
if (mask[i]) ninliers++; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// check if we're confident enough in estimated motion
|
|
|
|
|
|
|
|
|
|
if (ok) *ok = true; |
|
|
|
|
if (static_cast<float>(ninliers) / npoints < minInlierRatio_) |
|
|
|
|
{ |
|
|
|
|
M = Mat::eye(3, 3, CV_32F); |
|
|
|
|
if (ok) *ok = false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
return M; |
|
|
|
|
// estimate motion
|
|
|
|
|
return motionEstimator_->estimate(pointsPrevGood_, pointsGood_, ok); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if HAVE_OPENCV_GPU |
|
|
|
|
RansacMotionEstimatorGpu::RansacMotionEstimatorGpu(MotionModel model) |
|
|
|
|
: GlobalMotionEstimatorBase(model) |
|
|
|
|
KeypointBasedMotionEstimatorGpu::KeypointBasedMotionEstimatorGpu(Ptr<MotionEstimatorBase> estimator) |
|
|
|
|
: ImageMotionEstimatorBase(estimator->motionModel()), motionEstimator_(estimator) |
|
|
|
|
{ |
|
|
|
|
CV_Assert(gpu::getCudaEnabledDeviceCount() > 0); |
|
|
|
|
setRansacParams(RansacParams::default2dMotion(model)); |
|
|
|
|
CV_Assert(gpu::getCudaEnabledDeviceCount() > 0);
|
|
|
|
|
setOutlierRejector(new NullOutlierRejector()); |
|
|
|
|
setMinInlierRatio(0.1f); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Mat RansacMotionEstimatorGpu::estimate(const Mat &frame0, const Mat &frame1, bool *ok) |
|
|
|
|
Mat KeypointBasedMotionEstimatorGpu::estimate(const Mat &frame0, const Mat &frame1, bool *ok) |
|
|
|
|
{ |
|
|
|
|
frame0_.upload(frame0); |
|
|
|
|
frame1_.upload(frame1); |
|
|
|
@ -571,7 +703,7 @@ Mat RansacMotionEstimatorGpu::estimate(const Mat &frame0, const Mat &frame1, boo |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Mat RansacMotionEstimatorGpu::estimate(const gpu::GpuMat &frame0, const gpu::GpuMat &frame1, bool *ok) |
|
|
|
|
Mat KeypointBasedMotionEstimatorGpu::estimate(const gpu::GpuMat &frame0, const gpu::GpuMat &frame1, bool *ok) |
|
|
|
|
{ |
|
|
|
|
// convert frame to gray if it's color
|
|
|
|
|
|
|
|
|
@ -585,29 +717,29 @@ Mat RansacMotionEstimatorGpu::estimate(const gpu::GpuMat &frame0, const gpu::Gpu |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// find keypoints
|
|
|
|
|
|
|
|
|
|
detector_(grayFrame0, pointsPrev_); |
|
|
|
|
|
|
|
|
|
// find correspondences
|
|
|
|
|
|
|
|
|
|
optFlowEstimator_.run(frame0, frame1, pointsPrev_, points_, status_);
|
|
|
|
|
|
|
|
|
|
// leave good correspondences only
|
|
|
|
|
|
|
|
|
|
gpu::compactPoints(pointsPrev_, points_, status_); |
|
|
|
|
|
|
|
|
|
pointsPrev_.download(hostPointsPrev_); |
|
|
|
|
points_.download(hostPoints_); |
|
|
|
|
|
|
|
|
|
// perfrom outlier rejection
|
|
|
|
|
// perform outlier rejection
|
|
|
|
|
|
|
|
|
|
IOutlierRejector *outlierRejector = static_cast<IOutlierRejector*>(outlierRejector_); |
|
|
|
|
if (!dynamic_cast<NullOutlierRejector*>(outlierRejector)) |
|
|
|
|
{ |
|
|
|
|
outlierRejector_->process(frame0.size(), hostPointsPrev_, hostPoints_, rejectionStatus_); |
|
|
|
|
|
|
|
|
|
hostPointsPrevTmp_.clear(); hostPointsPrevTmp_.reserve(hostPoints_.cols); |
|
|
|
|
hostPointsTmp_.clear(); hostPointsTmp_.reserve(hostPoints_.cols); |
|
|
|
|
hostPointsPrevTmp_.clear(); |
|
|
|
|
hostPointsPrevTmp_.reserve(hostPoints_.cols); |
|
|
|
|
|
|
|
|
|
hostPointsTmp_.clear(); |
|
|
|
|
hostPointsTmp_.reserve(hostPoints_.cols); |
|
|
|
|
|
|
|
|
|
for (int i = 0; i < hostPoints_.cols; ++i) |
|
|
|
|
{ |
|
|
|
@ -622,216 +754,10 @@ Mat RansacMotionEstimatorGpu::estimate(const gpu::GpuMat &frame0, const gpu::Gpu |
|
|
|
|
hostPoints_ = Mat(1, hostPointsTmp_.size(), CV_32FC2, &hostPointsTmp_[0]); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// find motion
|
|
|
|
|
|
|
|
|
|
int npoints = hostPoints_.cols; |
|
|
|
|
int ninliers = 0; |
|
|
|
|
Mat_<float> M; |
|
|
|
|
|
|
|
|
|
if (motionModel_ != MM_HOMOGRAPHY) |
|
|
|
|
M = estimateGlobalMotionRobust( |
|
|
|
|
hostPointsPrev_, hostPoints_, motionModel_, ransacParams_, 0, &ninliers); |
|
|
|
|
else |
|
|
|
|
{ |
|
|
|
|
vector<uchar> mask; |
|
|
|
|
M = findHomography(hostPointsPrev_, hostPoints_, mask, CV_RANSAC, ransacParams_.thresh); |
|
|
|
|
for (int i = 0; i < npoints; ++i) |
|
|
|
|
if (mask[i]) ninliers++; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// check if we're confident enough in estimated motion
|
|
|
|
|
|
|
|
|
|
if (ok) *ok = true; |
|
|
|
|
if (static_cast<float>(ninliers) / npoints < minInlierRatio_) |
|
|
|
|
{ |
|
|
|
|
M = Mat::eye(3, 3, CV_32F); |
|
|
|
|
if (ok) *ok = false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
return M; |
|
|
|
|
} |
|
|
|
|
#endif // #if HAVE_OPENCV_GPU
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
LpBasedMotionEstimator::LpBasedMotionEstimator(MotionModel model) |
|
|
|
|
: GlobalMotionEstimatorBase(model) |
|
|
|
|
{ |
|
|
|
|
setDetector(new GoodFeaturesToTrackDetector()); |
|
|
|
|
setOptFlowEstimator(new SparsePyrLkOptFlowEstimator()); |
|
|
|
|
setOutlierRejector(new NullOutlierRejector()); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// TODO will estimation of all motions as one LP problem be faster?
|
|
|
|
|
|
|
|
|
|
Mat LpBasedMotionEstimator::estimate(const Mat &frame0, const Mat &frame1, bool *ok) |
|
|
|
|
{ |
|
|
|
|
// find keypoints
|
|
|
|
|
|
|
|
|
|
detector_->detect(frame0, keypointsPrev_); |
|
|
|
|
|
|
|
|
|
// extract points from keypoints
|
|
|
|
|
|
|
|
|
|
pointsPrev_.resize(keypointsPrev_.size()); |
|
|
|
|
for (size_t i = 0; i < keypointsPrev_.size(); ++i) |
|
|
|
|
pointsPrev_[i] = keypointsPrev_[i].pt; |
|
|
|
|
|
|
|
|
|
// find correspondences
|
|
|
|
|
|
|
|
|
|
optFlowEstimator_->run(frame0, frame1, pointsPrev_, points_, status_, noArray()); |
|
|
|
|
|
|
|
|
|
// leave good correspondences only
|
|
|
|
|
|
|
|
|
|
pointsPrevGood_.clear(); pointsPrevGood_.reserve(points_.size()); |
|
|
|
|
pointsGood_.clear(); pointsGood_.reserve(points_.size()); |
|
|
|
|
|
|
|
|
|
for (size_t i = 0; i < points_.size(); ++i) |
|
|
|
|
{ |
|
|
|
|
if (status_[i]) |
|
|
|
|
{ |
|
|
|
|
pointsPrevGood_.push_back(pointsPrev_[i]); |
|
|
|
|
pointsGood_.push_back(points_[i]); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// perfrom outlier rejection
|
|
|
|
|
|
|
|
|
|
IOutlierRejector *outlierRejector = static_cast<IOutlierRejector*>(outlierRejector_); |
|
|
|
|
if (!dynamic_cast<NullOutlierRejector*>(outlierRejector)) |
|
|
|
|
{ |
|
|
|
|
pointsPrev_.swap(pointsPrevGood_); |
|
|
|
|
points_.swap(pointsGood_); |
|
|
|
|
|
|
|
|
|
outlierRejector_->process(frame0.size(), pointsPrev_, points_, status_); |
|
|
|
|
|
|
|
|
|
pointsPrevGood_.clear(); pointsPrevGood_.reserve(points_.size()); |
|
|
|
|
pointsGood_.clear(); pointsGood_.reserve(points_.size()); |
|
|
|
|
|
|
|
|
|
for (size_t i = 0; i < points_.size(); ++i) |
|
|
|
|
{ |
|
|
|
|
if (status_[i]) |
|
|
|
|
{ |
|
|
|
|
pointsPrevGood_.push_back(pointsPrev_[i]); |
|
|
|
|
pointsGood_.push_back(points_[i]); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// prepare LP problem
|
|
|
|
|
|
|
|
|
|
#ifndef HAVE_CLP |
|
|
|
|
|
|
|
|
|
CV_Error(CV_StsError, "The library is built without Clp support"); |
|
|
|
|
if (ok) *ok = false; |
|
|
|
|
return Mat::eye(3, 3, CV_32F); |
|
|
|
|
|
|
|
|
|
#else |
|
|
|
|
|
|
|
|
|
CV_Assert(motionModel_ <= MM_AFFINE && motionModel_ != MM_RIGID); |
|
|
|
|
|
|
|
|
|
int npoints = static_cast<int>(pointsGood_.size()); |
|
|
|
|
int ncols = 6 + 2*npoints; |
|
|
|
|
int nrows = 4*npoints; |
|
|
|
|
|
|
|
|
|
if (motionModel_ == MM_SIMILARITY) |
|
|
|
|
nrows += 2; |
|
|
|
|
else if (motionModel_ == MM_TRANSLATION_AND_SCALE) |
|
|
|
|
nrows += 3; |
|
|
|
|
else if (motionModel_ == MM_TRANSLATION) |
|
|
|
|
nrows += 4; |
|
|
|
|
|
|
|
|
|
rows_.clear(); |
|
|
|
|
cols_.clear(); |
|
|
|
|
elems_.clear(); |
|
|
|
|
|
|
|
|
|
obj_.assign(ncols, 0); |
|
|
|
|
collb_.assign(ncols, -INF); |
|
|
|
|
colub_.assign(ncols, INF); |
|
|
|
|
|
|
|
|
|
int c = 6; |
|
|
|
|
|
|
|
|
|
for (int i = 0; i < npoints; ++i, c += 2) |
|
|
|
|
{ |
|
|
|
|
obj_[c] = 1; |
|
|
|
|
collb_[c] = 0; |
|
|
|
|
|
|
|
|
|
obj_[c+1] = 1; |
|
|
|
|
collb_[c+1] = 0; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
elems_.clear(); |
|
|
|
|
rowlb_.assign(nrows, -INF); |
|
|
|
|
rowub_.assign(nrows, INF); |
|
|
|
|
|
|
|
|
|
int r = 0; |
|
|
|
|
Point2f p0, p1; |
|
|
|
|
|
|
|
|
|
for (int i = 0; i < npoints; ++i, r += 4) |
|
|
|
|
{ |
|
|
|
|
p0 = pointsPrevGood_[i]; |
|
|
|
|
p1 = pointsGood_[i]; |
|
|
|
|
|
|
|
|
|
set(r, 0, p0.x); set(r, 1, p0.y); set(r, 2, 1); set(r, 6+2*i, -1); |
|
|
|
|
rowub_[r] = p1.x; |
|
|
|
|
|
|
|
|
|
set(r+1, 3, p0.x); set(r+1, 4, p0.y); set(r+1, 5, 1); set(r+1, 6+2*i+1, -1); |
|
|
|
|
rowub_[r+1] = p1.y; |
|
|
|
|
|
|
|
|
|
set(r+2, 0, p0.x); set(r+2, 1, p0.y); set(r+2, 2, 1); set(r+2, 6+2*i, 1); |
|
|
|
|
rowlb_[r+2] = p1.x; |
|
|
|
|
|
|
|
|
|
set(r+3, 3, p0.x); set(r+3, 4, p0.y); set(r+3, 5, 1); set(r+3, 6+2*i+1, 1); |
|
|
|
|
rowlb_[r+3] = p1.y; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (motionModel_ == MM_SIMILARITY) |
|
|
|
|
{ |
|
|
|
|
set(r, 0, 1); set(r, 4, -1); rowlb_[r] = rowub_[r] = 0; |
|
|
|
|
set(r+1, 1, 1); set(r+1, 3, 1); rowlb_[r+1] = rowub_[r+1] = 0; |
|
|
|
|
} |
|
|
|
|
else if (motionModel_ == MM_TRANSLATION_AND_SCALE) |
|
|
|
|
{ |
|
|
|
|
set(r, 0, 1); set(r, 4, -1); rowlb_[r] = rowub_[r] = 0; |
|
|
|
|
set(r+1, 1, 1); rowlb_[r+1] = rowub_[r+1] = 0; |
|
|
|
|
set(r+2, 3, 1); rowlb_[r+2] = rowub_[r+2] = 0; |
|
|
|
|
} |
|
|
|
|
else if (motionModel_ == MM_TRANSLATION) |
|
|
|
|
{ |
|
|
|
|
set(r, 0, 1); rowlb_[r] = rowub_[r] = 1; |
|
|
|
|
set(r+1, 1, 1); rowlb_[r+1] = rowub_[r+1] = 0; |
|
|
|
|
set(r+2, 3, 1); rowlb_[r+2] = rowub_[r+2] = 0; |
|
|
|
|
set(r+3, 4, 1); rowlb_[r+3] = rowub_[r+3] = 1; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// solve
|
|
|
|
|
|
|
|
|
|
CoinPackedMatrix A(true, &rows_[0], &cols_[0], &elems_[0], elems_.size()); |
|
|
|
|
A.setDimensions(nrows, ncols); |
|
|
|
|
|
|
|
|
|
ClpSimplex model(false); |
|
|
|
|
model.loadProblem(A, &collb_[0], &colub_[0], &obj_[0], &rowlb_[0], &rowub_[0]); |
|
|
|
|
|
|
|
|
|
ClpDualRowSteepest dualSteep(1); |
|
|
|
|
model.setDualRowPivotAlgorithm(dualSteep); |
|
|
|
|
model.scaling(1); |
|
|
|
|
|
|
|
|
|
model.dual(); |
|
|
|
|
|
|
|
|
|
// extract motion
|
|
|
|
|
|
|
|
|
|
const double *sol = model.getColSolution(); |
|
|
|
|
|
|
|
|
|
Mat_<float> M = Mat::eye(3, 3, CV_32F); |
|
|
|
|
M(0,0) = sol[0]; |
|
|
|
|
M(0,1) = sol[1]; |
|
|
|
|
M(0,2) = sol[2]; |
|
|
|
|
M(1,0) = sol[3]; |
|
|
|
|
M(1,1) = sol[4]; |
|
|
|
|
M(1,2) = sol[5]; |
|
|
|
|
|
|
|
|
|
if (ok) *ok = true; |
|
|
|
|
return M; |
|
|
|
|
#endif |
|
|
|
|
// estimate motion
|
|
|
|
|
return motionEstimator_->estimate(hostPointsPrev_, hostPoints_, ok); |
|
|
|
|
} |
|
|
|
|
#endif // HAVE_OPENCV_GPU
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Mat getMotion(int from, int to, const vector<Mat> &motions) |
|
|
|
|