Exemplo n.º 1
0
static duk_ret_t RigidBody_SetRotation_float3(duk_context* ctx)
{
    RigidBody* thisObj = GetThisWeakObject<RigidBody>(ctx);
    float3& rotation = *GetCheckedValueObject<float3>(ctx, 0, float3_ID);
    thisObj->SetRotation(rotation);
    return 0;
}
Exemplo n.º 2
0
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;
}