11#include < opencv2/core.hpp>
22#include < opencv2/core/utility.hpp>
3- #include < opencv2/imgproc/imgproc_c.h> // cvFindContours
3+ #include < opencv2/imgproc/imgproc_c.h>
44#include < opencv2/imgproc.hpp>
55#include < opencv2/rgbd.hpp>
66#include < opencv2/videoio.hpp>
1010#include < cstdio>
1111#include < iostream>
1212
13+ using namespace cv ;
14+ using namespace std ;
15+
1316// 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);
1518
16- std:: vector<CvPoint > maskFromTemplate (const std::vector<cv::linemod::Template>& templates,
19+ vector<Point > maskFromTemplate (const std::vector<cv::linemod::Template>& templates,
1720 int num_modalities, cv::Point offset, cv::Size size,
1821 cv::Mat& mask, cv::Mat& dst);
1922
@@ -228,12 +231,12 @@ int main(int argc, char * argv[])
228231 if (event == cv::EVENT_RBUTTONDOWN)
229232 {
230233 // 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;
237240 subtractPlane (depth, mask, chain, focal_length);
238241
239242 cv::imshow (" mask" , mask);
@@ -295,7 +298,7 @@ int main(int argc, char * argv[])
295298
296299 // Compute masks based on convex hull of matched template
297300 cv::Mat color_mask, depth_mask;
298- std::vector<CvPoint > chain = maskFromTemplate (templates, num_modalities,
301+ std::vector<Point > chain = maskFromTemplate (templates, num_modalities,
299302 cv::Point (m.x , m.y ), color.size (),
300303 color_mask, display);
301304 subtractPlane (depth, depth_mask, chain, focal_length);
@@ -392,7 +395,7 @@ static void reprojectPoints(const std::vector<cv::Point3d>& proj, std::vector<cv
392395 }
393396}
394397
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)
396399{
397400 const int l_num_cost_pts = 200 ;
398401
@@ -500,14 +503,12 @@ static void filterPlane(IplImage * ap_depth, std::vector<IplImage *> & a_masks,
500503 int l_h = l_maxy - l_miny + 1 ;
501504 int l_nn = (int )a_chain.size ();
502505
503- CvPoint * lp_chain = new CvPoint[l_nn] ;
506+ std::vector<cv::Point> lp_chain;
504507
505508 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]);
509510
510- delete[] lp_chain;
511+ cv::fillPoly ( cv::cvarrToMat (lp_mask), lp_chain, cv::Scalar::all ( 255 )) ;
511512
512513 // cv_show_image(lp_mask,"hallo1");
513514
@@ -568,7 +569,7 @@ static void filterPlane(IplImage * ap_depth, std::vector<IplImage *> & a_masks,
568569 cvReleaseMat (&lp_v);
569570}
570571
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)
572573{
573574 mask = cv::Mat::zeros (depth.size (), CV_8U);
574575 std::vector<IplImage*> tmp;
@@ -578,49 +579,24 @@ void subtractPlane(const cv::Mat& depth, cv::Mat& mask, std::vector<CvPoint>& ch
578579 filterPlane (&depth_ipl, tmp, chain, f);
579580}
580581
581- std:: vector<CvPoint > maskFromTemplate (const std::vector<cv::linemod::Template>& templates,
582+ vector<Point > maskFromTemplate (const std::vector<cv::linemod::Template>& templates,
582583 int num_modalities, cv::Point offset, cv::Size size,
583584 cv::Mat& mask, cv::Mat& dst)
584585{
585586 templateConvexHull (templates, num_modalities, offset, size, mask);
586587
587588 const int OFFSET = 30 ;
588589 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-
595590 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;
624600}
625601
626602// Adapted from cv_show_angles
0 commit comments