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