int GenerateTestData2(Params& params)
{
//create environment
Environment* env = new Environment();
env->SetNoise(params.noise);
CvSize im_res = cvSize(params.width,params.height);
double FOVx = params.FOVx; // field of view relatively to width (in degrees)
double fov_rad = FOVx * CV_PI / 180;
double fx = im_res.width/2.0 / tan( fov_rad/2);
double fy = fx;
double cx = im_res.width/2.0-0.5;
double cy = im_res.height/2.0-0.5;
//model cube room of size 2x2x2 meters
//8 cameras on the perimeter at the height 1 meter in corners and in the middle of walls
// all they look to the center of the room
//coordinate center in the center of the cube
//Z coordinate is oriented up
Camera* cam[8];
for( int i = 0; i < 8; i++ )
{
cam[i] = new Camera();
cam[i]->SetDistortion(params.k1, params.k2,0,0); //only radial distortion
cam[i]->SetIntrinsics(fx,fy, cx, cy ); //fx,fy,cx,cy
cam[i]->SetResolution(im_res);
env->AddCamera(cam[i]);
}
//set positions
cam[0]->SetPosition( 1.,1.,0);
cam[1]->SetPosition( 0.,1.,0);
cam[2]->SetPosition( -1.,1.,0);
cam[3]->SetPosition( -1.,0.,0);
cam[4]->SetPosition( -1.,-1.,0);
cam[5]->SetPosition( 0.,-1.,0);
cam[6]->SetPosition( 1.,-1.,0);
cam[7]->SetPosition( 1.,0.,0);
//set rotation matrices
//they will be oriented strongly vertically and rotated only around vertical axis (Z coorinate in WCS)
CvMat* camrot = cvCreateMat( 3, 3, CV_64FC1);
for( int i = 0; i < 8; i++ )
{
//y of the camera is oriented along negative Z of WCS
double yDirCamera[3] = {0,0,-1};
double zDirCamera[3]; //oriented from camera center to WCS center
double* pos = cam[i]->GetPosition();
zDirCamera[0] = -pos[0];
zDirCamera[1] = -pos[1];
zDirCamera[2] = -pos[2];
double xDirCamera[3]; //cross product Y*Z
xDirCamera[0] = yDirCamera[1]*zDirCamera[2] - yDirCamera[2]*zDirCamera[1];
xDirCamera[1] = yDirCamera[2]*zDirCamera[0] - yDirCamera[0]*zDirCamera[2];
xDirCamera[2] = yDirCamera[0]*zDirCamera[1] - yDirCamera[1]*zDirCamera[0];
//normalize z and x
double inv_norm = 1.0/sqrt(xDirCamera[0]*xDirCamera[0] + xDirCamera[1]*xDirCamera[1] + xDirCamera[2]*xDirCamera[2]);
xDirCamera[0]*=inv_norm;
xDirCamera[1]*=inv_norm;
xDirCamera[2]*=inv_norm;
inv_norm = 1.0 / sqrt(zDirCamera[0]*zDirCamera[0] + zDirCamera[1]*zDirCamera[1] + zDirCamera[2]*zDirCamera[2]);
zDirCamera[0] *= inv_norm;
zDirCamera[1] *= inv_norm;
zDirCamera[2] *= inv_norm;
camrot->data.db[0] = xDirCamera[0];
camrot->data.db[3] = xDirCamera[1];
camrot->data.db[6] = xDirCamera[2];
camrot->data.db[1] = yDirCamera[0];
camrot->data.db[4] = yDirCamera[1];
camrot->data.db[7] = yDirCamera[2];
camrot->data.db[2] = zDirCamera[0];
camrot->data.db[5] = zDirCamera[1];
camrot->data.db[8] = zDirCamera[2];
//get inverse matrix (equal to transposed)
cvTranspose(camrot, camrot);
cam[i]->SetRotation(camrot->data.db);
cam[i]->ComputeTranslation();// compute translation after we set position and rotation matrix
}
#if defined WIN32 || defined _WIN32
_mkdir(g_filepath);
#else
mkdir(g_filepath, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
#endif
char fname[2048];
sprintf(fname, "%scameras.txt", g_filepath );
FILE* clist = fopen(fname, "w");
for(unsigned int i = 0; i < 8; i++)
{
sprintf(fname, "%scamera%d.calib", g_filepath, i );
FILE *calibFile = fopen(fname,"w");
cam[i]->saveCamParams(calibFile);
fclose( calibFile );
fprintf(clist, "%s\n", fname);
}
fclose(clist);
std::vector<CvPoint3D64f> board_pos;
board_pos.clear();
//generate body position, set zero for now
CvPoint3D64f pos;
pos.x = 0;
pos.y = 0;
pos.z = 0;
board_pos.push_back(pos);
RigidBody* body = env->GetBody();
body->generate_random(50, -0.25, 0.25, -0.25, 0.25, -0.25, 0.25 );
CvRNG rng = cvRNG();
int index = 0; //index of board's position to save, is used to enumerate snapshots
for(size_t i = 0; i < board_pos.size(); i++ )
{
CvPoint3D64f point = board_pos[i];
body->SetPosition(point.x,point.y,point.z);
//generate random orientation of the board
unsigned int rn = cvRandInt(&rng);
//scale to 90 degrees
rn = rn%90;
double slant = (double)rn-45-90; //angle of slant of the bord relatively to its horizontal direction
rn = cvRandInt(&rng);
//scale to 360 degrees
rn = rn%360;
double azimuth = rn; //azimuth
//set slant and azimuth to 0
slant = azimuth = 0;
double sn = sin(azimuth*CV_PI/180);
double cs = cos(azimuth*CV_PI/180);
//generate rotation matrix
CvMat* mat_azimuth = cvCreateMat( 3, 3, CV_64FC1);
mat_azimuth->data.db[0] = cs;
mat_azimuth->data.db[1] = -sn;
mat_azimuth->data.db[2] = 0;
mat_azimuth->data.db[3] = sn;
mat_azimuth->data.db[4] = cs;
mat_azimuth->data.db[5] = 0;
mat_azimuth->data.db[6] = 0;
mat_azimuth->data.db[7] = 0;
mat_azimuth->data.db[8] = 1;
sn = sin(slant*CV_PI/180);
cs = cos(slant*CV_PI/180);
CvMat* mat_slant = cvCreateMat( 3, 3, CV_64FC1); //rotation around X axis
mat_slant->data.db[0] = 1;
mat_slant->data.db[1] = 0;
mat_slant->data.db[2] = 0;
mat_slant->data.db[3] = 0;
mat_slant->data.db[4] = cs;
mat_slant->data.db[5] = -sn;
mat_slant->data.db[6] = 0;
mat_slant->data.db[7] = sn;
mat_slant->data.db[8] = cs;
CvMat* rot = cvCreateMat( 3, 3, CV_64FC1); //rotation around X axis
//create complete rotation matrix
cvMatMul(mat_azimuth, mat_slant, rot);
//these are coordinates of board's axis in WCS
body->SetRotation(rot->data.db);
//project images onto all cameras
env->Capture();
//save 3d points of corners into the file
char fname[2048];
sprintf(fname, "%sPoints%d.3d", g_filepath, index );
body->Save(fname, true);
//save shots from camera
sprintf(fname, "%salldata.txt", g_filepath );
env->Save(fname);
}
//destroy everything
delete env;
return 0;
}
