int Api::graficas2opticas(double pointX, double pointY, HPoint2D *punto2D) { punto2D->x = GRAPHIC_TO_OPTICAL_X(pointX, pointY); punto2D->y = GRAPHIC_TO_OPTICAL_Y(pointX, pointY); punto2D->h = 1.0000; return 0; }
void myprogeo::mybackproject(float x, float y, float* xp, float* yp, float* zp, float* camx, float* camy, float* camz, int cam){ HPoint2D p; HPoint3D pro; p.x=GRAPHIC_TO_OPTICAL_X(x,y); p.y=GRAPHIC_TO_OPTICAL_Y(x,y); p.h=1; backproject(&pro,p,cameras[cam]); *xp=pro.X; *yp=pro.Y; *zp=pro.Z; *camx=cameras[cam].position.X; *camy=cameras[cam].position.Y; *camz=cameras[cam].position.Z; }
void Controller::add_depth_pointsImage(cv::Mat distances, cv::Mat imageRGB, rgbdManualCalibrator::DrawArea* world, int cam, int scale, int colour){ float d; for (int xIm=0; xIm< cWidth; xIm++){ for (int yIm=0; yIm<cHeight ; yIm++){ d=distances.at<float>(yIm,xIm); if (d != 0 ){ float xp,yp,zp,camx,camy,camz; float ux,uy,uz; float x,y; float k; float c1x, c1y, c1z; float fx,fy,fz; float fmod; float t; float Fx,Fy,Fz; HPoint2D p; HPoint3D pro; p.x=GRAPHIC_TO_OPTICAL_X(xIm,yIm,cHeight); p.y=GRAPHIC_TO_OPTICAL_Y(xIm,yIm,cHeight); p.h=1; backproject(&pro,p,this->cameras[cam]); xp=pro.X; yp=pro.Y; zp=pro.Z; camx=this->cameras[cam].position.X; camy=this->cameras[cam].position.Y; camz=this->cameras[cam].position.Z; //vector unitario float modulo; modulo = sqrt(1/(((camx-xp)*(camx-xp))+((camy-yp)*(camy-yp))+((camz-zp)*(camz-zp)))); //mypro->mygetcameras[cam]foa(&c1x, &c1y, &c1z, 0); c1x=this->cameras[cam].foa.X; c1y=this->cameras[cam].foa.Y; c1z=this->cameras[cam].foa.Z; fmod = sqrt(1/(((camx-c1x)*(camx-c1x))+((camy-c1y)*(camy-c1y))+((camz-c1z)*(camz-c1z)))); fx = (c1x - camx)*fmod; fy = (c1y - camy)*fmod; fz = (c1z - camz) * fmod; ux = (xp-camx)*modulo; uy = (yp-camy)*modulo; uz = (zp-camz)*modulo; Fx= d*fx + camx; Fy= d*fy + camy; Fz= d*fz + camz; /* calculamos el punto real */ t = (-(fx*camx) + (fx*Fx) - (fy*camy) + (fy*Fy) - (fz*camz) + (fz*Fz))/((fx*ux) + (fy*uy) + (fz*uz)); /*world->points[i][0]=distance*ux+camx; world->points[i][1]=distance*uy+camy; world->points[i][2]=distance*uz+camz;*/ /*std::cout << xp << "," << yp << "," << zp << "," << modulo << std::endl; std::cout << xp-camx << "," << yp-camy<< "," << zp-camz << std::endl; std::cout << ux << "," << uy<< "," << uz << std::endl;*/ //k= (80-yp)/uy; //std::cout << "distancia" << distance << std::endl; if (colour==0) world->add_kinect_point(t*ux + camx,t*uy+ camy,t*uz + camz,(int)imageRGB.data[3*(yIm*cWidth+xIm)],(int)imageRGB.data[3*(yIm*cWidth+xIm)+1],(int)imageRGB.data[3*(yIm*cWidth+xIm)+2]); else{ int r,g,b; if (colour==1){ r=255; g=0; b=0; } else if (colour==2){ r=0; g=0; b=255; } else{ r=0; g=1; b=0; } world->add_kinect_point(t*ux + camx,t*uy+ camy,t*uz + camz,r,g,b); } //world->add_line(distance*ux + camx,distance*uy+ camy,distance*uz + camz,camx,camy,camz); } } } }