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;
}
