IplImage* getMoment(IplImage* img) { //Calculate Position CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments)); cvMoments(imgRed, moments, 1); double moment10 = cvGetSpatialMoment(moments, 1, 0); double moment01 = cvGetSpatialMoment(moments, 0, 1); double area = cvGetCentralMoment(moments,0,0); static int posX = 0; static int posY = 0; int lastX = posX; int lastY = posY; posX = moment10/area; posY = moment01/area; CvPoint* center = new CvPoint(); center->x = lastX; center->y = lastY; printf("%d lastX", lastX); printf("%d lastY", lastY); if(lastX != posX && lastY != posY) { cvDrawCircle(img, *center, 25, CV_RGB(10,10,255), -1); } return 0; }
int trackingC::step(IplImage* img){ CvFont font; cvInitFont(&font, CV_FONT_HERSHEY_COMPLEX, 1, 1); if(tracked){ if(!gone && visible && !inMotion){ cvDrawCircle(img, this->position, approxSize, cvScalar(0, 0, 0), -1); cvDrawLine(img, position, cvPoint(position.x+dTraveled.x, position.y+dTraveled.y), cvScalar(255,255,255), 3); cvPutText(img, "D", cvPoint(position.x, position.y-10), &font, cvScalar(0,255,0)); }else if (!gone && visible && inMotion){ cvDrawCircle(img, this->position, approxSize, cvScalar(200, 100, 0), 3); cvPutText(img, "M", cvPoint(position.x, position.y-10), &font, cvScalar(0,255,0)); }else if(!gone){ cvDrawCircle(img, this->position, approxSize, cvScalar(255, 255, 255), -1); cvPutText(img, "X", cvPoint(position.x, position.y-10), &font, cvScalar(0,255,0)); } } if(!viewed){ timerReset ++; isVisible = false; if(timerReset > 10 && timerReset < 60){ if(visible) isOcluded = true; visible = false; } else if(timerReset >= 60){ if(!gone){ isGone = true; isOcluded = false; }else{ isGone = false; } gone = true; } } viewed = false; return 0; }
void VisionPipeLine::draw_markBlobs() { // draw founded blobs for (int pos = 0; pos < _bloblist.size() && pos < MAX_TRACKED_BLOB_CNT; ++pos) { if (pos == _hoveredBlobID) { cvDrawRect(_rectified, cvPoint(_bloblist[pos]._box.x, _bloblist[pos]._box.y), cvPoint(_bloblist[pos]._box.x+_bloblist[pos]._box.width, _bloblist[pos]._box.y+_bloblist[pos]._box.height), cvScalar(0,0,255), 3); } else { cvDrawRect(_rectified, cvPoint(_bloblist[pos]._box.x, _bloblist[pos]._box.y), cvPoint(_bloblist[pos]._box.x+_bloblist[pos]._box.width, _bloblist[pos]._box.y+_bloblist[pos]._box.height), cvScalar(0,255,255)); } cvDrawCircle(_rectified, cvPoint(_bloblist[pos]._center.x, _bloblist[pos]._center.y), 2, cvScalar(255,255,0)); char msg[100]; sprintf(msg, "%d: (%.2f, %.2f)", pos, _bloblist[pos]._center.x, _bloblist[pos]._center.y); cv_textOut(_rectified, _bloblist[pos]._box.x, _bloblist[pos]._box.y+_bloblist[pos]._box.height+10, msg); } // draw blob marks on the rectified projection plane cvFillImage(_projected, 0); char msg[100]; for (int pos =0; pos<_trackpoints.size(); ++pos) { CvPoint2D32f posInWindow = _layout_provider.keyboardPosToWindowPos(cvPoint2D32f(_trackpoints[pos].x, _trackpoints[pos].y)); posInWindow.x *= (float)PROJECTED_WIDTH/_layout_provider.getLayoutSize().width ; posInWindow.y *= (float)PROJECTED_WIDTH/_layout_provider.getLayoutSize().width ; posInWindow.y += (PROJECTED_HEIGHT-_layout_provider.getLayoutSize().height)/2; cvDrawCircle(_projected, cvPoint(posInWindow.x, posInWindow.y), _trackpoints[pos].pressure, cvScalar(255,255,0)); sprintf(msg, "%d:(%.2f,%.2f)", pos, posInWindow.x, posInWindow.y); cv_textOut(_projected, posInWindow.x,posInWindow.y+20, msg); } }
void Chess_recognition::drawLines ( vector<pair<float, float>> lines, IplImage* image){ for( int i = 0; i < MIN(lines.size(),100); i++ ) { float rho = lines.at(i).first; float theta = lines.at(i).second; CvPoint pt1, pt2; double a = cos(theta), b = sin(theta); double x0 = a*rho, y0 = b*rho; //수직의 시작이 되는점 pt1.x = cvRound(x0 + 1000*(-b)); //끝까지로 쭉 그려버림 pt1.y = cvRound(y0 + 1000*(a)); pt2.x = cvRound(x0 - 1000*(-b)); pt2.y = cvRound(y0 - 1000*(a)); cvLine( image, pt1, pt2, CV_RGB(255,0,0), 1, 8 ); cvDrawCircle(image, cvPoint(x0, y0), 3, cvScalar(255,255), -1); } }
/* * Add a circle on the video that fellow your colored object */ void ColourToTrack::addObjectToVideo(IplImage* image, CvPoint objectNextPos) { int objectNextStepX, objectNextStepY; // Calculate circle next position (if there is enough pixels) if (nbPixels > 10) { // Reset position if no pixel were found if (xy.x == -1 || xy.y == -1) { xy.x = objectNextPos.x; xy.y = objectNextPos.y; } // Move step by step the object position to the desired position if (abs(xy.x - objectNextPos.x) > STEP_MIN) { objectNextStepX = max(STEP_MIN, min(STEP_MAX, abs(xy.x - objectNextPos.x) / 2)); xy.x += (-1) * sign(xy.x - objectNextPos.x) * objectNextStepX; } if (abs(xy.y - objectNextPos.y) > STEP_MIN) { objectNextStepY = max(STEP_MIN, min(STEP_MAX, abs(xy.y - objectNextPos.y) / 2)); xy.y += (-1) * sign(xy.y - objectNextPos.y) * objectNextStepY; } // -1 = object isn't within the camera range } else { xy.x = -1; xy.y = -1; } // Draw an object (circle) centered on the calculated center of gravity if (nbPixels > 10) cvDrawCircle(image, xy, 15, //CV_RGB(colour.hsv.h,colour.hsv.s,colour.hsv.v), CV_RGB(colour.rgb.r,colour.rgb.g,colour.rgb.b), -1); }
/* * Add a circle on the video that fellow your colored object */ void addObjectToVideo(IplImage* image, CvPoint objectNextPos, int nbPixels) { int objectNextStepX, objectNextStepY; // Calculate circle next position (if there is enough pixels) if (nbPixels > 10) { // Reset position if no pixel were found if (objectPos.x == -1 || objectPos.y == -1) { objectPos.x = objectNextPos.x; objectPos.y = objectNextPos.y; } // Move step by step the object position to the desired position if (abs(objectPos.x - objectNextPos.x) > STEP_MIN) { objectNextStepX = max(STEP_MIN, min(STEP_MAX, abs(objectPos.x - objectNextPos.x) / 2)); objectPos.x += (-1) * sign(objectPos.x - objectNextPos.x) * objectNextStepX; } if (abs(objectPos.y - objectNextPos.y) > STEP_MIN) { objectNextStepY = max(STEP_MIN, min(STEP_MAX, abs(objectPos.y - objectNextPos.y) / 2)); objectPos.y += (-1) * sign(objectPos.y - objectNextPos.y) * objectNextStepY; } // -1 = object isn't within the camera range } else { objectPos.x = -1; objectPos.y = -1; } // Draw an object (circle) centered on the calculated center of gravity if (nbPixels > 10) cvDrawCircle(image, objectPos, 15, CV_RGB(255, 0, 0), -1); // We show the image on the window cvShowImage("Test Color Tracking", image); }
IplImage * BouyObject::TemplateMask(const IplImage * imgIn, const IplImage * threshold, const IplImage * tmplt) const { IplImage * imgOut = cvCloneImage(imgIn); cvZero(imgOut); std::list<CvBox2D> blobList; blobList = Zebulon::Vision::VisionUtils::GetBlobBoxes(threshold,.001,.95,false); IplImage * crop; CvFont font; cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1,1); CvBox2D best; double bestScore = -1; for(std::list<CvBox2D>::iterator it = blobList.begin(); it != blobList.end(); it++) { crop = Zebulon::Vision::VisionUtils::Crop(imgIn,*it); double score = 0; Zebulon::Vision::VisionUtils::GetSimpleTemplateSimilarity(crop,tmplt,score,false); // std::ostringstream s; // s << "(" << score << ")"; // cvPutText(imgOut,s.str().c_str(),cvPointFrom32f(it->center),&font,CV_RGB(255,255,255)); // Zebulon::Vision::VisionUtils::DrawSquare(imgOut,*it); if(score > mTemplateThreshold) { if(score > bestScore) { bestScore = score; best = *it; } //cvDrawCircle(imgOut,cvPointFrom32f(it->center),(crop->width/2.0),CV_RGB(255,255,255)); } cvReleaseImage(&crop); } if(bestScore > -1) { cvDrawCircle(imgOut,cvPointFrom32f(best.center),(best.size.height/2.0),CV_RGB(255,255,255),CV_FILLED); } return imgOut; }
IplImage* BouyObject::SegmentationMask2(const IplImage * imgIn, IplImage* debugOut) const { // CvSize imageSize = cvSize(imgIn->width & -2, imgIn->height & -2 ); // IplImage* imgSmallCopy = cvCreateImage( cvSize(imageSize.width/2, imageSize.height/2), IPL_DEPTH_8U, 1 ); IplImage * imgOut = cvCreateImage(cvGetSize(imgIn),IPL_DEPTH_8U, 1); IplImage * circleMask = cvCreateImage(cvGetSize(imgIn),IPL_DEPTH_8U, 1); IplImage * src = cvCloneImage(imgIn); IplImage * scratch = cvCloneImage(src); IplImage * hist = HistogramMask(imgIn); //IplImage * bestMask = cvCreateImage(cvGetSize(imgIn),IPL_DEPTH_8U, 1); CvMemStorage* storage = cvCreateMemStorage(0); CvSeq* comp = NULL; CvFont font; cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, .5,.5); std::ostringstream s; cvZero(imgOut); cvZero(circleMask); cvZero(scratch); //cvZero(bestMask); CvScalar avgColor; double bestColor = -1; CvRect bestRect; double bestDiag = 0; // IplImage* hsv = cvCreateImage( cvGetSize(imgIn), 8, 3 ); // IplImage * chan0 = cvCreateImage(cvGetSize(imgIn),IPL_DEPTH_8U, 1); // IplImage * segsum = cvCreateImage(cvGetSize(imgIn),IPL_DEPTH_8U, 1); //cvCvtColor( imgIn, hsv, CV_BGR2YCrCb ); //cvCopyImage(imgIn,hsv); //cvSplit(hsv,chan0,chan1,chan2, NULL); //cvConvertImage(imgIn,src); //lower last param for more segments //cvPyrSegmentation( hsv, scratch, storage, &comp, 3, 100, 90 ); cvPyrSegmentation( src, scratch, storage, &comp, 2, 0, 100); int n_comp = comp->total; std::list<CvBox2D> blobList; for( int i = n_comp-1; i>=1; i-- ) { CvConnectedComp* cc = (CvConnectedComp*) cvGetSeqElem( comp, i ); cvAbsDiffS(scratch,src,cc->value); cvNot(src,src); cvThreshold(src,src,254,255,CV_THRESH_BINARY); blobList = VisionUtils::GetBlobBoxes(src,0.0008,.95,false); for(std::list<CvBox2D>::iterator it = blobList.begin(); it != blobList.end(); it++) { CvRect rect = VisionUtils::ToCvRect(*it); VisionUtils::MakeSquare(rect); double diagonal = sqrt(rect.width * rect.width + rect.height * rect.height); cvDrawCircle(circleMask,cvPoint(rect.x+rect.width/2.,rect.y+rect.height/2),diagonal/2.5,CV_RGB(255,255,255),CV_FILLED); avgColor = cvAvg (hist,circleMask); if((bestColor < 0 || bestColor < avgColor.val[0]) && avgColor.val[0] > mSegment2Threshold) { bestDiag = diagonal; bestColor = avgColor.val[0]; bestRect = rect; cvCopy(circleMask,imgOut); } //cvMinMaxLoc(imgIn,) cvZero(circleMask); } } if(debugOut && bestColor > 0) { s.clear(); s << "bestColor(" << bestColor << ") " << mType; cvPutText(debugOut,s.str().c_str(),cvPoint(bestRect.x+bestRect.width/2.,bestRect.y+bestRect.height/2),&font,CV_RGB(255,255,255)); cvDrawCircle(debugOut,cvPoint(bestRect.x+bestRect.width/2.,bestRect.y+bestRect.height/2),bestDiag/2.5,CV_RGB(255,255,255)); } // cvShowImage("best",bestMask); // cvWaitKey(0); //VisionUtils::ClearEdges(imgOut); cvReleaseImage(&scratch); cvReleaseImage(&src); cvReleaseImage(&hist); cvReleaseImage(&circleMask); cvReleaseMemStorage( &storage ); return imgOut; }
/** * Adds marker to the image at the given CvPoint position. * * @param image * @param pos */ void ColorTracking::addObjectToVideo (IplImage *image, CvPoint pos, CvScalar color, int thickness) { cvDrawCircle(image, pos, thickness, color, -1); }
int main(int,char **) { int i, num_pts ; std::vector<CvPoint2D32f> ref_points; std::vector<CvPoint2D32f> new_points; IplImage * image_base ; IplImage * image ; num_pts = 200; image_base = cvCreateImage(cvSize(500,500),8,3); image = cvCreateImage(cvSize(500,500),8,3); cvZero(image_base); double norm = 200; float a = 0.; for( i=0; i<num_pts; i++ ) { float xx = (float)(norm/2.f)*cos(a); float yy = (float)(norm)*sin(a); float x = (float)(xx * cos(CV_PI/4) + yy *sin(CV_PI/4) +250); float y = (float)(xx * -sin(CV_PI/4) + yy *cos(CV_PI/4)+250); ref_points.push_back(cvPoint2D32f(x,y)); cvDrawCircle(image_base,cvPoint((int)x,(int)y),1,CV_RGB(0,255,255),1); a += (float)(2*CV_PI/num_pts); } a = 0.; for( i=0; i< num_pts/5; i++ ) { float xx = (float)((norm/1.9)*cos(a)); float yy = (float)((norm/1.1)*sin(a)); float x = (float)(xx * cos(-CV_PI/8) + yy *sin(-CV_PI/8) +150); float y = (float)(xx * -sin(-CV_PI/8) + yy *cos(-CV_PI/8)+250); new_points.push_back(cvPoint2D32f(x,y)); a += (float)(2*CV_PI/(float)(num_pts/5)); cvDrawCircle(image_base,cvPoint((int)x,(int)y),1,CV_RGB(255,255,0),1); } for(int k = 0; k < 30 ;k++) { float R[4] = {1.f,0.f,0.f,1.f},T[2] = {0.,0.}; CvMat r = cvMat(2,2,CV_32F,R); CvMat t = cvMat(2,1,CV_32F,T); cvCopy(image_base,image); float err = icp(&new_points[0],new_points.size(), &ref_points[0],ref_points.size(), &r,&t,cvTermCriteria(CV_TERMCRIT_ITER,1,0.1),image); printf("err = %f\n",err); for(int i = 0; i < (int)new_points.size() ; i++ ) { float x = new_points[i].x; float y = new_points[i].y; float X = (R[0]*x + R[1]*y + T[0]); float Y = (R[2]*x + R[3]*y + T[1]); new_points[i].x = X; new_points[i].y = Y; cvDrawCircle(image,cvPoint((int)X,(int)Y),5,CV_RGB(255,255,0),1); } printf("Final transformation : \n"); printf("R[0]=%f R[1]=%f T[0]=%f\n",R[0],R[1],T[0]); printf("R[2]=%f R[3]=%f T[1]=%f\n",R[2],R[3],T[1]); cvShowImage("image",image); if( cvWaitKey(200)== 27) break; } cvReleaseImage(&image_base); cvReleaseImage(&image); return 0; }
int main(int argc, char **argv) { /** * Inisialisasi node baru yang bernama "distance_tracker" dan terhubung * dengan argumen command-line. */ ros::init(argc, argv, "distance_tracker"); ros::NodeHandle n; /** Subscribe terhadap navdata. */ ros::Subscriber navdataSub = n.subscribe("/ardrone/navdata", 10, navdataCallback); /** Publish hasil perhitungan pelacakan ARDrone. */ //ros::Publisher distancePub = n.advertise<geometry_msgs::Pose>("/drone_pos", 1000); // Map IplImage *map = cvCreateImage(cvSize(500, 500), IPL_DEPTH_8U, 3); cvZero(map); // Position matrix cv::Mat P = cv::Mat::zeros(3, 1, CV_64FC1); cv::Mat temp = cv::Mat::zeros(3, 1, CV_64FC1); float x = 0; float y = 0; /** Loop utama untuk menghitung jejak ARDrone per 1 detik. */ while (ros::ok()) { //geometry_msgs::Pose targetPose; //geometry_msgs::Point point2D; // Rotation matrices double _RX[] = { 1.0, 0.0, 0.0, 0.0, cos(::rotX), -sin(::rotX), 0.0, sin(::rotX), cos(::rotX)}; double _RY[] = { cos(::rotY), 0.0, sin(::rotY), 0.0, 1.0, 0.0, -sin(::rotY), 0.0, cos(::rotY)}; double _RZ[] = { cos(::rotZ), -sin(::rotZ), 0.0, sin(::rotZ), cos(::rotZ), 0.0, 0.0, 0.0, 1.0}; cv::Mat RX(3, 3, CV_64FC1, _RX); cv::Mat RY(3, 3, CV_64FC1, _RY); cv::Mat RZ(3, 3, CV_64FC1, _RZ); // Time static double last = ros::Time::now().toSec(); double dt = (ros::Time::now().toSec() - last); last = ros::Time::now().toSec(); //static int last = cv::getTickCount(); //double dt = (cv::getTickCount() - last) / cv::getTickFrequency(); //last = cv::getTickCount(); // Local movement double _M[] = {::vx, ::vy, ::vz}; cv::Mat M(3, 1, CV_64FC1, _M); // Dead reckoning temp = RZ * RY * RX * M; P = P + temp; x = x + dt * ::rotZ * P.at<double>(0,0); y = y + dt * ::rotZ * P.at<double>(1,0); ros::Duration time(1); time.sleep(); // Position (x, y, z) double pos[3] = {P.at<double>(0,0), P.at<double>(1,0), P.at<double>(2,0)}; ROS_INFO("X = %f , Y = %f, Z = %f, Waktu = %f ", pos[0], pos[1], pos[2], dt); //ROS_INFO("X = %f , Y = %f, Z = %f, Waktu = %f ", x, y); // Display the image cvDrawCircle(map, cvPoint(-pos[1]*30.0 + map->width/2, -pos[0]*30.0 + map->height/2), 2, CV_RGB(255,0,0)); cvShowImage("map", map); ros::spinOnce(); } cvReleaseImage(&map); return 0; }