1
1
#include < opencv2/core.hpp>
2
2
#include < opencv2/core/utility.hpp>
3
- #include < opencv2/imgproc/imgproc_c.h> // cvFindContours
3
+ #include < opencv2/imgproc/imgproc_c.h>
4
4
#include < opencv2/imgproc.hpp>
5
5
#include < opencv2/rgbd.hpp>
6
6
#include < opencv2/videoio.hpp>
10
10
#include < cstdio>
11
11
#include < iostream>
12
12
13
+ using namespace cv ;
14
+ using namespace std ;
15
+
13
16
// Function prototypes
14
- void subtractPlane (const cv::Mat& depth, cv::Mat& mask, std::vector<CvPoint >& chain, double f);
17
+ void subtractPlane (const cv::Mat& depth, cv::Mat& mask, std::vector<cv::Point >& chain, double f);
15
18
16
- std:: vector<CvPoint > maskFromTemplate (const std::vector<cv::linemod::Template>& templates,
19
+ vector<Point > maskFromTemplate (const std::vector<cv::linemod::Template>& templates,
17
20
int num_modalities, cv::Point offset, cv::Size size,
18
21
cv::Mat& mask, cv::Mat& dst);
19
22
@@ -228,12 +231,12 @@ int main(int argc, char * argv[])
228
231
if (event == cv::EVENT_RBUTTONDOWN)
229
232
{
230
233
// Compute object mask by subtracting the plane within the ROI
231
- std:: vector<CvPoint > chain ( 4 ) ;
232
- chain[ 0 ] = cvPoint (pt1);
233
- chain[ 1 ] = cvPoint ( pt2.x , pt1.y );
234
- chain[ 2 ] = cvPoint (pt2);
235
- chain[ 3 ] = cvPoint ( pt1.x , pt2.y );
236
- cv:: Mat mask;
234
+ vector<Point > chain;
235
+ chain. push_back (pt1);
236
+ chain. push_back ( Point ( pt2.x , pt1.y ) );
237
+ chain. push_back (pt2);
238
+ chain. push_back ( Point ( pt1.x , pt2.y ) );
239
+ Mat mask;
237
240
subtractPlane (depth, mask, chain, focal_length);
238
241
239
242
cv::imshow (" mask" , mask);
@@ -295,7 +298,7 @@ int main(int argc, char * argv[])
295
298
296
299
// Compute masks based on convex hull of matched template
297
300
cv::Mat color_mask, depth_mask;
298
- std::vector<CvPoint > chain = maskFromTemplate (templates, num_modalities,
301
+ std::vector<Point > chain = maskFromTemplate (templates, num_modalities,
299
302
cv::Point (m.x , m.y ), color.size (),
300
303
color_mask, display);
301
304
subtractPlane (depth, depth_mask, chain, focal_length);
@@ -392,7 +395,7 @@ static void reprojectPoints(const std::vector<cv::Point3d>& proj, std::vector<cv
392
395
}
393
396
}
394
397
395
- static void filterPlane (IplImage * ap_depth, std::vector<IplImage *> & a_masks, std::vector<CvPoint > & a_chain, double f)
398
+ static void filterPlane (IplImage * ap_depth, std::vector<IplImage *> & a_masks, std::vector<cv::Point > & a_chain, double f)
396
399
{
397
400
const int l_num_cost_pts = 200 ;
398
401
@@ -500,14 +503,12 @@ static void filterPlane(IplImage * ap_depth, std::vector<IplImage *> & a_masks,
500
503
int l_h = l_maxy - l_miny + 1 ;
501
504
int l_nn = (int )a_chain.size ();
502
505
503
- CvPoint * lp_chain = new CvPoint[l_nn] ;
506
+ std::vector<cv::Point> lp_chain;
504
507
505
508
for (int l_i = 0 ; l_i < l_nn; ++l_i)
506
- lp_chain[l_i] = a_chain[l_i];
507
-
508
- cvFillPoly (lp_mask, &lp_chain, &l_nn, 1 , cvScalar (255 , 255 , 255 ));
509
+ lp_chain.push_back (a_chain[l_i]);
509
510
510
- delete[] lp_chain;
511
+ cv::fillPoly ( cv::cvarrToMat (lp_mask), lp_chain, cv::Scalar::all ( 255 )) ;
511
512
512
513
// cv_show_image(lp_mask,"hallo1");
513
514
@@ -568,7 +569,7 @@ static void filterPlane(IplImage * ap_depth, std::vector<IplImage *> & a_masks,
568
569
cvReleaseMat (&lp_v);
569
570
}
570
571
571
- void subtractPlane (const cv::Mat& depth, cv::Mat& mask, std::vector<CvPoint >& chain, double f)
572
+ void subtractPlane (const cv::Mat& depth, cv::Mat& mask, std::vector<cv::Point >& chain, double f)
572
573
{
573
574
mask = cv::Mat::zeros (depth.size (), CV_8U);
574
575
std::vector<IplImage*> tmp;
@@ -578,49 +579,24 @@ void subtractPlane(const cv::Mat& depth, cv::Mat& mask, std::vector<CvPoint>& ch
578
579
filterPlane (&depth_ipl, tmp, chain, f);
579
580
}
580
581
581
- std:: vector<CvPoint > maskFromTemplate (const std::vector<cv::linemod::Template>& templates,
582
+ vector<Point > maskFromTemplate (const std::vector<cv::linemod::Template>& templates,
582
583
int num_modalities, cv::Point offset, cv::Size size,
583
584
cv::Mat& mask, cv::Mat& dst)
584
585
{
585
586
templateConvexHull (templates, num_modalities, offset, size, mask);
586
587
587
588
const int OFFSET = 30 ;
588
589
cv::dilate (mask, mask, cv::Mat (), cv::Point (-1 ,-1 ), OFFSET);
589
-
590
- CvMemStorage * lp_storage = cvCreateMemStorage (0 );
591
- CvTreeNodeIterator l_iterator;
592
- CvSeqReader l_reader;
593
- CvSeq * lp_contour = 0 ;
594
-
595
590
cv::Mat mask_copy = mask.clone ();
596
- IplImage mask_copy_ipl = cvIplImage (mask_copy);
597
- cvFindContours (&mask_copy_ipl, lp_storage, &lp_contour, sizeof (CvContour),
598
- CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);
599
-
600
- std::vector<CvPoint> l_pts1; // to use as input to cv_primesensor::filter_plane
601
-
602
- cvInitTreeNodeIterator (&l_iterator, lp_contour, 1 );
603
- while ((lp_contour = (CvSeq *)cvNextTreeNode (&l_iterator)) != 0 )
604
- {
605
- CvPoint l_pt0;
606
- cvStartReadSeq (lp_contour, &l_reader, 0 );
607
- CV_READ_SEQ_ELEM (l_pt0, l_reader);
608
- l_pts1.push_back (l_pt0);
609
-
610
- for (int i = 0 ; i < lp_contour->total ; ++i)
611
- {
612
- CvPoint l_pt1;
613
- CV_READ_SEQ_ELEM (l_pt1, l_reader);
614
- // / @todo Really need dst at all? Can just as well do this outside
615
- cv::line (dst, l_pt0, l_pt1, CV_RGB (0 , 255 , 0 ), 2 );
616
-
617
- l_pt0 = l_pt1;
618
- l_pts1.push_back (l_pt0);
619
- }
620
- }
621
- cvReleaseMemStorage (&lp_storage);
622
-
623
- return l_pts1;
591
+ std::vector<std::vector<cv::Point> > contours;
592
+ cv::findContours (mask_copy, contours, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE);
593
+ CV_Assert (contours.size () == 1 );
594
+ std::vector<cv::Point> res = contours[0 ];
595
+ CV_Assert (res.size () > 2 );
596
+ std::vector<cv::Point>::const_iterator pt1 = res.begin (), pt2 = pt1 + 1 ;
597
+ for (; pt2 != res.end (); ++pt1, ++pt2)
598
+ cv::line (dst, *pt1, *pt2, cv::Scalar (0 , 255 , 0 ), 2 );
599
+ return res;
624
600
}
625
601
626
602
// Adapted from cv_show_angles
0 commit comments