void App::process()
{
if (sources.size() == 1)
{
pairSources.push_back(PairFrameSource::get(sources[0], 2));
}
else if (sources.size() == 2)
{
pairSources.push_back(PairFrameSource::get(sources[0], sources[1]));
}
else
{
cout << "Loading default frames source...\n";
pairSources.push_back(PairFrameSource::get(new VideoSource("data/pedestrian_detect/mitsubishi.avi"), 2));
pairSources.push_back(PairFrameSource::get(new VideoSource("data/optical_flow/plush1_720p_10s.m2v"), 2));
pairSources.push_back(PairFrameSource::get(new VideoSource("data/stereo_matching/8sec_Toys_Kirill_1920x1080_xvid_L.avi"), 2));
}
Mat frame0, frame1;
Mat gray;
GpuMat d_frame0, d_frame1;
GpuMat d_gray;
Mat u, v;
GpuMat d_u, d_v;
GpuMat d_prevPts;
GpuMat d_nextPts;
GpuMat d_status;
vector<Point2f> prevPts;
vector<Point2f> nextPts;
vector<uchar> status;
GoodFeaturesToTrackDetector_GPU detector(4000, 0.01, 10.0);
PyrLKOpticalFlow lk;
while (!exited)
{
int64 start = getTickCount();
pairSources[curSource]->next(frame0, frame1);
d_frame0.upload(frame0);
d_frame1.upload(frame1);
double proc_fps;
if (useGPU)
{
cvtColor(d_frame0, d_gray, COLOR_BGR2GRAY);
int64 proc_start = getTickCount();
detector(d_gray, d_prevPts);
lk.sparse(d_frame0, d_frame1, d_prevPts, d_nextPts, d_status);
proc_fps = getTickFrequency() / (getTickCount() - proc_start);
download(d_prevPts, prevPts);
download(d_nextPts, nextPts);
download(d_status, status);
}
else
{
cvtColor(frame0, gray, COLOR_BGR2GRAY);
int64 proc_start = getTickCount();
goodFeaturesToTrack(gray, prevPts, detector.maxCorners, detector.qualityLevel, detector.minDistance);
calcOpticalFlowPyrLK(frame0, frame1, prevPts, nextPts, status, noArray());
proc_fps = getTickFrequency() / (getTickCount() - proc_start);
}
Mat img_to_show = frame0.clone();
drawArrows(img_to_show, prevPts, nextPts, status, Scalar(255, 0, 0));
double total_fps = getTickFrequency() / (getTickCount() - start);
displayState(img_to_show, proc_fps, total_fps);
imshow("demo_sparse_optical_flow", img_to_show);
processKey(waitKey(3) & 0xff);
}
}
int main(int argc, const char* argv[])
{
string filename1, filename2;
if (argc < 3)
{
cerr << "Usage : " << argv[0] << " <frame0> <frame1>" << endl;
filename1 = "../data/basketball1.png";
filename2 = "../data/basketball2.png";
}
else
{
filename1 = argv[1];
filename2 = argv[2];
}
Mat frame0 = imread(filename1, IMREAD_GRAYSCALE);
Mat frame1 = imread(filename2, IMREAD_GRAYSCALE);
if (frame0.empty())
{
cerr << "Can't open image [" << filename1 << "]" << endl;
return -1;
}
if (frame1.empty())
{
cerr << "Can't open image [" << filename2 << "]" << endl;
return -1;
}
if (frame1.size() != frame0.size())
{
cerr << "Images should be of equal sizes" << endl;
return -1;
}
GpuMat d_frame0(frame0);
GpuMat d_frame1(frame1);
GpuMat d_flowx(frame0.size(), CV_32FC1);
GpuMat d_flowy(frame0.size(), CV_32FC1);
BroxOpticalFlow brox(0.197f, 50.0f, 0.8f, 10, 77, 10);
PyrLKOpticalFlow lk; lk.winSize = Size(7, 7);
FarnebackOpticalFlow farn;
OpticalFlowDual_TVL1_CUDA tvl1;
FastOpticalFlowBM fastBM;
{
GpuMat d_frame0f;
GpuMat d_frame1f;
d_frame0.convertTo(d_frame0f, CV_32F, 1.0 / 255.0);
d_frame1.convertTo(d_frame1f, CV_32F, 1.0 / 255.0);
const int64 start = getTickCount();
brox(d_frame0f, d_frame1f, d_flowx, d_flowy);
const double timeSec = (getTickCount() - start) / getTickFrequency();
cout << "Brox : " << timeSec << " sec" << endl;
showFlow("Brox", d_flowx, d_flowy);
}
{
const int64 start = getTickCount();
lk.dense(d_frame0, d_frame1, d_flowx, d_flowy);
const double timeSec = (getTickCount() - start) / getTickFrequency();
cout << "LK : " << timeSec << " sec" << endl;
showFlow("LK", d_flowx, d_flowy);
}
{
const int64 start = getTickCount();
farn(d_frame0, d_frame1, d_flowx, d_flowy);
const double timeSec = (getTickCount() - start) / getTickFrequency();
cout << "Farn : " << timeSec << " sec" << endl;
showFlow("Farn", d_flowx, d_flowy);
}
{
const int64 start = getTickCount();
tvl1(d_frame0, d_frame1, d_flowx, d_flowy);
const double timeSec = (getTickCount() - start) / getTickFrequency();
cout << "TVL1 : " << timeSec << " sec" << endl;
showFlow("TVL1", d_flowx, d_flowy);
}
{
const int64 start = getTickCount();
GpuMat buf;
calcOpticalFlowBM(d_frame0, d_frame1, Size(7, 7), Size(1, 1), Size(21, 21), false, d_flowx, d_flowy, buf);
const double timeSec = (getTickCount() - start) / getTickFrequency();
cout << "BM : " << timeSec << " sec" << endl;
showFlow("BM", d_flowx, d_flowy);
}
{
const int64 start = getTickCount();
fastBM(d_frame0, d_frame1, d_flowx, d_flowy);
const double timeSec = (getTickCount() - start) / getTickFrequency();
cout << "Fast BM : " << timeSec << " sec" << endl;
showFlow("Fast BM", d_flowx, d_flowy);
}
imshow("Frame 0", frame0);
imshow("Frame 1", frame1);
waitKey();
return 0;
}
void App::process()
{
if ((sources.size() > 0) && (sources.size() % 2 == 0))
{
for (size_t i = 0; i < sources.size(); i += 2)
pairSources.push_back(PairFrameSource::get(sources[i], sources[i+1]));
}
else
{
cout << "Loading default frames source...\n";
pairSources.push_back(PairFrameSource::get(new ImageSource("data/optical_flow/army1.png"),
new ImageSource("data/optical_flow/army2.png")));
pairSources.push_back(PairFrameSource::get(new ImageSource("data/optical_flow/backyard1.png"),
new ImageSource("data/optical_flow/backyard2.png")));
pairSources.push_back(PairFrameSource::get(new ImageSource("data/optical_flow/basketball1.png"),
new ImageSource("data/optical_flow/basketball2.png")));
pairSources.push_back(PairFrameSource::get(new ImageSource("data/optical_flow/dumptruck1.png"),
new ImageSource("data/optical_flow/dumptruck2.png")));
pairSources.push_back(PairFrameSource::get(new ImageSource("data/optical_flow/mequon1.png"),
new ImageSource("data/optical_flow/mequon2.png")));
pairSources.push_back(PairFrameSource::get(new ImageSource("data/optical_flow/teddy1.png"),
new ImageSource("data/optical_flow/teddy2.png")));
}
Mat frame0, frame1;
Mat frame0_32F, frame1_32F;
Mat gray0, gray1;
Mat gray0_32F, gray1_32F;
GpuMat d_frame0, d_frame1;
GpuMat d_frame0_32F, d_frame1_32F;
Mat fu, fv;
Mat bu, bv;
Mat fuv, buv;
GpuMat d_fu, d_fv;
GpuMat d_bu, d_bv;
Mat flowFieldForward, flowFieldBackward;
Mat channels[3];
GpuMat d_b0, d_g0, d_r0;
GpuMat d_b1, d_g1, d_r1;
GpuMat d_buf;
GpuMat d_rNew, d_gNew, d_bNew;
GpuMat d_newFrame;
Mat newFrame;
vector<Mat> frames;
frames.reserve(static_cast<int>(1.0f / timeStep) + 2);
int currentFrame = 0;
bool forward = true;
cv::Mat img_to_show;
double proc_fps, total_fps;
while (!exited)
{
if (calcFlow)
{
cout << "Calculate optical flow and interpolated frames" << endl;
int64 start = getTickCount();
pairSources[curSource]->next(frame0, frame1);
frame0.convertTo(frame0_32F, CV_32F, 1.0 / 255.0);
frame1.convertTo(frame1_32F, CV_32F, 1.0 / 255.0);
switch (method)
{
case BROX:
{
makeGray(frame0_32F, gray0_32F);
makeGray(frame1_32F, gray1_32F);
d_frame0_32F.upload(gray0_32F);
d_frame1_32F.upload(gray1_32F);
int64 proc_start = getTickCount();
brox(d_frame0_32F, d_frame1_32F, d_fu, d_fv);
brox(d_frame1_32F, d_frame0_32F, d_bu, d_bv);
proc_fps = getTickFrequency() / (getTickCount() - proc_start);
d_fu.download(fu);
d_fv.download(fv);
d_bu.download(bu);
d_bv.download(bv);
break;
}
case FARNEBACK_GPU:
{
makeGray(frame0, gray0);
makeGray(frame1, gray1);
d_frame0.upload(gray0);
d_frame1.upload(gray1);
int64 proc_start = getTickCount();
farneback(d_frame0, d_frame1, d_fu, d_fv);
farneback(d_frame1, d_frame0, d_bu, d_bv);
proc_fps = getTickFrequency() / (getTickCount() - proc_start);
d_fu.download(fu);
d_fv.download(fv);
d_bu.download(bu);
d_bv.download(bv);
break;
}
case FARNEBACK_CPU:
{
makeGray(frame0, gray0);
makeGray(frame1, gray1);
int64 proc_start = getTickCount();
calcOpticalFlowFarneback(gray0, gray1, fuv, farneback.pyrScale, farneback.numLevels, farneback.winSize, farneback.numIters, farneback.polyN, farneback.polySigma, farneback.flags);
calcOpticalFlowFarneback(gray1, gray0, buv, farneback.pyrScale, farneback.numLevels, farneback.winSize, farneback.numIters, farneback.polyN, farneback.polySigma, farneback.flags);
proc_fps = getTickFrequency() / (getTickCount() - proc_start);
cv::Mat uv_planes[2];
uv_planes[0] = fu;
uv_planes[1] = fv;
split(fuv, uv_planes);
uv_planes[0] = bu;
uv_planes[1] = bv;
split(buv, uv_planes);
d_fu.upload(fu);
d_fv.upload(fv);
d_bu.upload(bu);
d_bv.upload(bv);
break;
}
case PYR_LK:
{
makeGray(frame0, gray0);
makeGray(frame1, gray1);
d_frame0.upload(gray0);
d_frame1.upload(gray1);
int64 proc_start = getTickCount();
pyrlk.dense(d_frame0, d_frame1, d_fu, d_fv);
pyrlk.dense(d_frame1, d_frame0, d_bu, d_bv);
proc_fps = getTickFrequency() / (getTickCount() - proc_start);
d_fu.download(fu);
d_fv.download(fv);
d_bu.download(bu);
d_bv.download(bv);
break;
}
};
getFlowField(fu, fv, flowFieldForward);
getFlowField(bu, bv, flowFieldBackward);
cv::split(frame0_32F, channels);
d_b0.upload(channels[0]);
d_g0.upload(channels[1]);
d_r0.upload(channels[2]);
cv::split(frame1_32F, channels);
d_b1.upload(channels[0]);
d_g1.upload(channels[1]);
d_r1.upload(channels[2]);
frames.clear();
frames.push_back(frame0_32F.clone());
// compute interpolated frames
for (float timePos = timeStep; timePos < 1.0f; timePos += timeStep)
{
interpolateFrames(d_b0, d_b1, d_fu, d_fv, d_bu, d_bv, timePos, d_bNew, d_buf);
interpolateFrames(d_g0, d_g1, d_fu, d_fv, d_bu, d_bv, timePos, d_gNew, d_buf);
interpolateFrames(d_r0, d_r1, d_fu, d_fv, d_bu, d_bv, timePos, d_rNew, d_buf);
GpuMat channels[] = {d_bNew, d_gNew, d_rNew};
merge(channels, 3, d_newFrame);
d_newFrame.download(newFrame);
frames.push_back(newFrame.clone());
}
frames.push_back(frame1_32F.clone());
currentFrame = 0;
forward = true;
calcFlow = false;
total_fps = getTickFrequency() / (getTickCount() - start);
}
imshow("First Frame", frame0);
imshow("Second Frame", frame1);
imshow("Forward flow", flowFieldForward);
imshow("Backward flow", flowFieldBackward);
frames[currentFrame].convertTo(img_to_show, CV_8U, 255.0);
displayState(img_to_show, proc_fps, total_fps);
imshow("Interpolated Frames", img_to_show);
processKey(waitKey(100) & 0xff);
if (forward)
{
++currentFrame;
if (currentFrame == static_cast<int>(frames.size()) - 1)
forward = false;
}
else
{
--currentFrame;
if (currentFrame == 0)
forward = true;
}
}
}
void App::runAppLogic()
{
if (sources_.size() > 1)
{
for (size_t i = 0; (i + 1) < sources_.size(); i += 2)
pairSources_.push_back(PairFrameSource::create(sources_[i], sources_[i+1]));
}
else
{
cout << "Using default frames source... \n" << endl;
pairSources_.push_back(PairFrameSource::create(FrameSource::image("data/dense_optical_flow_1.jpg"),
FrameSource::image("data/dense_optical_flow_2.jpg")));
}
BroxOpticalFlow brox(0.197f /*alpha*/, 50.0f /*gamma*/, 0.8f /*scale*/, 10 /*inner_iterations*/, 77 /*outer_iterations*/, 10 /*solver_iterations*/);
FarnebackOpticalFlow farneback;
PyrLKOpticalFlow pyrlk;
pyrlk.winSize = Size(13, 13);
pyrlk.iters = 1;
FastOpticalFlowBM fastbm;
Mat frame0, frame1;
Mat frame0_32F, frame1_32F;
Mat gray0, gray1;
Mat gray0_32F, gray1_32F;
GpuMat d_frame0, d_frame1;
GpuMat d_frame0_32F, d_frame1_32F;
Mat fu, fv;
Mat bu, bv;
Mat fuv, buv;
GpuMat d_fu, d_fv;
GpuMat d_bu, d_bv;
Mat flowFieldForward, flowFieldBackward;
Mat channels[3];
GpuMat d_b0, d_g0, d_r0;
GpuMat d_b1, d_g1, d_r1;
GpuMat d_buf;
GpuMat d_rNew, d_gNew, d_bNew;
GpuMat d_newFrame;
Mat newFrame;
const float timeStep = 0.1f;
vector<Mat> frames;
frames.reserve(static_cast<int>(1.0f / timeStep) + 2);
int currentFrame = 0;
bool forward = true;
Mat framesImg;
Mat flowsImg;
Mat outImg;
double proc_fps = 0.0, total_fps = 0.0;
while (isActive())
{
if (calcFlow_)
{
cout << "Calculate optical flow and interpolated frames" << endl;
const int64 total_start = getTickCount();
pairSources_[curSource_]->next(frame0, frame1);
frame0.convertTo(frame0_32F, CV_32F, 1.0 / 255.0);
frame1.convertTo(frame1_32F, CV_32F, 1.0 / 255.0);
switch (method_)
{
case BROX:
{
makeGray(frame0_32F, gray0_32F);
makeGray(frame1_32F, gray1_32F);
d_frame0_32F.upload(gray0_32F);
d_frame1_32F.upload(gray1_32F);
const int64 proc_start = getTickCount();
brox(d_frame0_32F, d_frame1_32F, d_fu, d_fv);
brox(d_frame1_32F, d_frame0_32F, d_bu, d_bv);
proc_fps = getTickFrequency() / (getTickCount() - proc_start);
d_fu.download(fu);
d_fv.download(fv);
d_bu.download(bu);
d_bv.download(bv);
break;
}
case FARNEBACK_GPU:
{
makeGray(frame0, gray0);
makeGray(frame1, gray1);
d_frame0.upload(gray0);
d_frame1.upload(gray1);
const int64 proc_start = getTickCount();
farneback(d_frame0, d_frame1, d_fu, d_fv);
farneback(d_frame1, d_frame0, d_bu, d_bv);
proc_fps = getTickFrequency() / (getTickCount() - proc_start);
d_fu.download(fu);
d_fv.download(fv);
d_bu.download(bu);
d_bv.download(bv);
break;
}
case FARNEBACK_CPU:
{
makeGray(frame0, gray0);
makeGray(frame1, gray1);
const int64 proc_start = getTickCount();
calcOpticalFlowFarneback(gray0, gray1, fuv, farneback.pyrScale, farneback.numLevels, farneback.winSize, farneback.numIters, farneback.polyN, farneback.polySigma, farneback.flags);
calcOpticalFlowFarneback(gray1, gray0, buv, farneback.pyrScale, farneback.numLevels, farneback.winSize, farneback.numIters, farneback.polyN, farneback.polySigma, farneback.flags);
proc_fps = getTickFrequency() / (getTickCount() - proc_start);
cv::Mat uv_planes[2];
uv_planes[0] = fu;
uv_planes[1] = fv;
split(fuv, uv_planes);
uv_planes[0] = bu;
uv_planes[1] = bv;
split(buv, uv_planes);
d_fu.upload(fu);
d_fv.upload(fv);
d_bu.upload(bu);
d_bv.upload(bv);
break;
}
case PYR_LK:
{
makeGray(frame0, gray0);
makeGray(frame1, gray1);
d_frame0.upload(gray0);
d_frame1.upload(gray1);
const int64 proc_start = getTickCount();
pyrlk.dense(d_frame0, d_frame1, d_fu, d_fv);
pyrlk.dense(d_frame1, d_frame0, d_bu, d_bv);
proc_fps = getTickFrequency() / (getTickCount() - proc_start);
d_fu.download(fu);
d_fv.download(fv);
d_bu.download(bu);
d_bv.download(bv);
break;
}
case FAST_BM_GPU:
{
makeGray(frame0, gray0);
makeGray(frame1, gray1);
d_frame0.upload(gray0);
d_frame1.upload(gray1);
const int64 proc_start = getTickCount();
fastbm(d_frame0, d_frame1, d_fu, d_fv);
fastbm(d_frame1, d_frame0, d_bu, d_bv);
proc_fps = getTickFrequency() / (getTickCount() - proc_start);
d_fu.download(fu);
d_fv.download(fv);
d_bu.download(bu);
d_bv.download(bv);
break;
}
default:
;
};
getFlowField(fu, fv, flowFieldForward, 30);
getFlowField(bu, bv, flowFieldBackward, 30);
split(frame0_32F, channels);
d_b0.upload(channels[0]);
d_g0.upload(channels[1]);
d_r0.upload(channels[2]);
split(frame1_32F, channels);
d_b1.upload(channels[0]);
d_g1.upload(channels[1]);
d_r1.upload(channels[2]);
frames.clear();
frames.push_back(frame0_32F.clone());
// compute interpolated frames
for (float timePos = timeStep; timePos < 1.0f; timePos += timeStep)
{
interpolateFrames(d_b0, d_b1, d_fu, d_fv, d_bu, d_bv, timePos, d_bNew, d_buf);
interpolateFrames(d_g0, d_g1, d_fu, d_fv, d_bu, d_bv, timePos, d_gNew, d_buf);
interpolateFrames(d_r0, d_r1, d_fu, d_fv, d_bu, d_bv, timePos, d_rNew, d_buf);
GpuMat channels[] = {d_bNew, d_gNew, d_rNew};
merge(channels, 3, d_newFrame);
d_newFrame.download(newFrame);
frames.push_back(newFrame.clone());
}
frames.push_back(frame1_32F.clone());
currentFrame = 0;
forward = true;
calcFlow_ = false;
total_fps = getTickFrequency() / (getTickCount() - total_start);
}
framesImg.create(frame0.rows, frame0.cols * 2, CV_8UC3);
Mat left = framesImg(Rect(0, 0, frame0.cols, frame0.rows));
Mat right = framesImg(Rect(frame0.cols, 0, frame0.cols, frame0.rows));
frame0.copyTo(left);
frame1.copyTo(right);
flowsImg.create(frame0.rows, frame0.cols * 2, CV_8UC3);
left = flowsImg(Rect(0, 0, frame0.cols, frame0.rows));
right = flowsImg(Rect(frame0.cols, 0, frame0.cols, frame0.rows));
flowFieldForward.copyTo(left);
flowFieldBackward.copyTo(right);
imshow("Frames", framesImg);
imshow("Flows", flowsImg);
frames[currentFrame].convertTo(outImg, CV_8U, 255.0);
displayState(outImg, proc_fps, total_fps);
imshow("Dense Optical Flow Demo", outImg);
wait(30);
if (forward)
{
++currentFrame;
if (currentFrame == static_cast<int>(frames.size()) - 1)
forward = false;
}
else
{
--currentFrame;
if (currentFrame == 0)
forward = true;
}
}
}
void imageDataHandler(const sensor_msgs::Image::ConstPtr& imageData)
{
timeLast = timeCur;
timeCur = imageData->header.stamp.toSec() - 0.1163;
cv_bridge::CvImageConstPtr imageDataCv = cv_bridge::toCvShare(imageData, "mono8");
if (!systemInited) {
remap(imageDataCv->image, image0, mapx, mapy, CV_INTER_LINEAR);
oclImage0 = oclMat(image0);
systemInited = true;
return;
}
Mat imageLast, imageCur;
oclMat oclImageLast, oclImageCur;
if (isOddFrame) {
remap(imageDataCv->image, image1, mapx, mapy, CV_INTER_LINEAR);
oclImage1 = oclMat(image1);
imageLast = image0;
imageCur = image1;
oclImageLast = oclImage0;
oclImageCur = oclImage1;
} else {
remap(imageDataCv->image, image0, mapx, mapy, CV_INTER_LINEAR);
oclImage0 = oclMat(image0);
imageLast = image1;
imageCur = image0;
oclImageLast = oclImage1;
oclImageCur = oclImage0;
}
isOddFrame = !isOddFrame;
resize(oclImageLast, oclImageShow, showSize);
oclImageShow.download(imageShow);
cornerHarris(oclImageShow, oclHarrisLast, 2, 3, 0.04);
oclHarrisLast.download(harrisLast);
vector<Point2f> *featuresTemp = featuresLast;
featuresLast = featuresCur;
featuresCur = featuresTemp;
pcl::PointCloud<ImagePoint>::Ptr imagePointsTemp = imagePointsLast;
imagePointsLast = imagePointsCur;
imagePointsCur = imagePointsTemp;
imagePointsCur->clear();
int recordFeatureNum = totalFeatureNum;
detectionCount = (detectionCount + 1) % (detectionSkipNum + 1);
if (detectionCount == detectionSkipNum) {
oclMat oclFeaturesSub;
for (int i = 0; i < ySubregionNum; i++) {
for (int j = 0; j < xSubregionNum; j++) {
int ind = xSubregionNum * i + j;
int numToFind = maxFeatureNumPerSubregion - subregionFeatureNum[ind];
if (numToFind > maxFeatureNumPerSubregion / 5.0) {
int subregionLeft = xBoundary + (int)(subregionWidth * j);
int subregionTop = yBoundary + (int)(subregionHeight * i);
Rect subregion = Rect(subregionLeft, subregionTop, (int)subregionWidth, (int)subregionHeight);
oclFeatureDetector.maxCorners = numToFind;
oclFeatureDetector(oclImageLast(subregion), oclFeaturesSub);
if (oclFeaturesSub.cols > 0) {
oclFeatureDetector.downloadPoints(oclFeaturesSub, featuresSub);
numToFind = featuresSub.size();
} else {
numToFind = 0;
}
int numFound = 0;
for(int k = 0; k < numToFind; k++) {
featuresSub[k].x += subregionLeft;
featuresSub[k].y += subregionTop;
int xInd = (featuresSub[k].x + 0.5) / showDSRate;
int yInd = (featuresSub[k].y + 0.5) / showDSRate;
if (harrisLast.at<float>(yInd, xInd) > 1e-7) {
featuresLast->push_back(featuresSub[k]);
featuresInd.push_back(featuresIndFromStart);
numFound++;
featuresIndFromStart++;
}
}
totalFeatureNum += numFound;
subregionFeatureNum[ind] += numFound;
}
}
}
}
if (totalFeatureNum > 0) {
Mat featuresLastMat(1, totalFeatureNum, CV_32FC2, (void*)&(*featuresLast)[0]);
oclMat oclFeaturesLast(featuresLastMat);
oclMat oclFeaturesCur, oclFeaturesStatus;
oclOpticalFlow.sparse(oclImageLast, oclImageCur, oclFeaturesLast, oclFeaturesCur, oclFeaturesStatus);
if (oclFeaturesCur.cols > 0 && oclFeaturesCur.cols == oclFeaturesStatus.cols) {
download(oclFeaturesCur, *featuresCur);
download(oclFeaturesStatus, featuresStatus);
totalFeatureNum = featuresCur->size();
} else {
totalFeatureNum = 0;
}
}
for (int i = 0; i < totalSubregionNum; i++) {
subregionFeatureNum[i] = 0;
}
ImagePoint point;
int featureCount = 0;
for (int i = 0; i < totalFeatureNum; i++) {
double trackDis = sqrt(((*featuresLast)[i].x - (*featuresCur)[i].x)
* ((*featuresLast)[i].x - (*featuresCur)[i].x)
+ ((*featuresLast)[i].y - (*featuresCur)[i].y)
* ((*featuresLast)[i].y - (*featuresCur)[i].y));
if (!(trackDis > maxTrackDis || (*featuresCur)[i].x < xBoundary ||
(*featuresCur)[i].x > imageWidth - xBoundary || (*featuresCur)[i].y < yBoundary ||
(*featuresCur)[i].y > imageHeight - yBoundary) && featuresStatus[i]) {
int xInd = (int)(((*featuresLast)[i].x - xBoundary) / subregionWidth);
int yInd = (int)(((*featuresLast)[i].y - yBoundary) / subregionHeight);
int ind = xSubregionNum * yInd + xInd;
if (subregionFeatureNum[ind] < maxFeatureNumPerSubregion) {
(*featuresCur)[featureCount] = (*featuresCur)[i];
(*featuresLast)[featureCount] = (*featuresLast)[i];
featuresInd[featureCount] = featuresInd[i];
point.u = -((*featuresCur)[featureCount].x - kImage[2]) / kImage[0];
point.v = -((*featuresCur)[featureCount].y - kImage[5]) / kImage[4];
point.ind = featuresInd[featureCount];
imagePointsCur->push_back(point);
if (i >= recordFeatureNum) {
point.u = -((*featuresLast)[featureCount].x - kImage[2]) / kImage[0];
point.v = -((*featuresLast)[featureCount].y - kImage[5]) / kImage[4];
imagePointsLast->push_back(point);
}
featureCount++;
subregionFeatureNum[ind]++;
}
}
}
totalFeatureNum = featureCount;
featuresCur->resize(totalFeatureNum);
featuresLast->resize(totalFeatureNum);
featuresInd.resize(totalFeatureNum);
sensor_msgs::PointCloud2 imagePointsLast2;
pcl::toROSMsg(*imagePointsLast, imagePointsLast2);
imagePointsLast2.header.stamp = ros::Time().fromSec(timeLast);
imagePointsLastPubPointer->publish(imagePointsLast2);
showCount = (showCount + 1) % (showSkipNum + 1);
if (showCount == showSkipNum) {
bridge.image = imageShow;
bridge.encoding = "mono8";
sensor_msgs::Image::Ptr imageShowPointer = bridge.toImageMsg();
imageShowPubPointer->publish(imageShowPointer);
}
}
