forums.naturalpoint.com

Code: Select all

ConfigureCamera(){
	if(camera==0)
    {
        printf("\n\n Please connect a camera ");		
        return;
    }

	//== Determine camera resolution to size application window ==----
	
    cameraWidth  = camera->Width();
	cameraHeight = camera->Height();

    

    //== Create a texture to push the rasterized camera image ====----
    //== We're using textures because it's an easy & cpu light 
    //== way to utilize the 3D hardware to display camera
    //== imagery at high frame rates

    texture = new Surface(cameraWidth, cameraHeight);
    framebuffer = new Bitmap(cameraWidth, cameraHeight, texture->PixelSpan()*4,
                               Bitmap::ThirtyTwoBit, texture->GetBuffer());

    //== Set Video Mode ==--

    camera->SetVideoType(PrecisionMode);
    
    //== Start camera output ==--

    camera->Start();

    //== Turn on some overlay text so it's clear things are     ===---
    //== working even if there is nothing in the camera's view. ===---

    camera->SetTextOverlay(false);
	camera->SetIntensity(200);
	camera->SetExposure(3);


    vec = cModuleVector::Create(); //new cModuleVector();
    vecprocessor = new cModuleVectorProcessing();

    Core::DistortionModel lensDistortion;

    camera->GetDistortionModel(lensDistortion);

	

    //== Plug distortion into vector module ==--

    cVectorSettings vectorSettings;
    vectorSettings = *vec->Settings();

    vectorSettings.Arrangement = cVectorSettings::VectorClip;
    vectorSettings.Enabled     = true;
    
    cVectorProcessingSettings vectorProcessorSettings;

    vectorProcessorSettings = *vecprocessor->Settings();

    vectorProcessorSettings.Arrangement = cVectorSettings::VectorClip;
    vectorProcessorSettings.ShowPivotPoint = false;
    vectorProcessorSettings.ShowProcessed  = false;
	
	

	vectorProcessorSettings.ScaleRotationYaw = 1.0;
	vectorProcessorSettings.ScaleRotationPitch = 1.0;
	vectorProcessorSettings.ScaleRotationRoll = 1.0;

	vectorProcessorSettings.ScaleTranslationX = 1.0;
	vectorProcessorSettings.ScaleTranslationY = 1.0;
	vectorProcessorSettings.ScaleTranslationZ = 1.0;
	
    
	vecprocessor->SetSettings(vectorProcessorSettings);

    //== Plug in focal length in (mm) by converting it from pixels -> mm

    vectorSettings.ImagerFocalLength =  (lensDistortion.HorizontalFocalLength/((float) camera->PhysicalPixelWidth()))*camera->ImagerWidth();

    vectorSettings.ImagerHeight = camera->ImagerHeight();
    vectorSettings.ImagerWidth  = camera->ImagerWidth();

    vectorSettings.PrincipalX   = camera->PhysicalPixelWidth()/2;
    vectorSettings.PrincipalY   = camera->PhysicalPixelHeight()/2;

    vectorSettings.PixelWidth   = camera->PhysicalPixelWidth();
    vectorSettings.PixelHeight  = camera->PhysicalPixelHeight();
	
    vec->SetSettings(vectorSettings);
	return;
}

Code: Select all

GetAndProcessFrame(){
	//== Fetch a new frame from the camera ===---
	objectDetected = false;
	//printf("\n\n ----------------------------------------- \n\n "  );
	//printf("\n\n camara %d  \n\n " ,numberCamera );
    frame = camera->GetFrame();
	Core::DistortionModel lensDistortion;

    camera->GetDistortionModel(lensDistortion);
    if(frame)
    {
       //printf("\n\n frame ok camara %d  \n\n " ,numberCamera );
		//== Ok, we've received a new frame, lets do something
        //== with it.

        //== Lets have the Camera Library raster the camera's
        //== image into our texture.

        frame->Rasterize(framebuffer);
		frameOK=true;
        vec->BeginFrame();
		printf("\n\n Detectados %d objetos \n\n " ,frame->ObjectCount() );
        for(int i=0; i<3/*frame->ObjectCount()*/; i++)
        {
            cObject *obj = frame->Object(i);
			
            float xp = obj->X();
            float yp = obj->Y();

            Core::Predistort2DPoint(lensDistortion,xp,yp);

            vec->PushMarkerData(xp, yp, obj->Area(), obj->Width(), obj->Height());
        }
        
		vec->Calculate();
	
        vecprocessor->PushData(vec);
		if(frame->ObjectCount() >= 3){
			objectDetected = true;
		}
		//printf("\n\n ----------------------------------------- \n\n "  );
		
	}else{
		//printf("\n\n ----------------------------------------- \n\n "  );
		frameOK=false;
	}
}

Code: Select all

DrawPosition(){
	//pinta el eje de coordenadas
    glEnable(GL_BLEND);
    glColor4f(1,1,1,0.3f);
    glBegin(GL_LINES);
    glVertex3f(10,0,0);glVertex3f(-10, 0,0);
    glVertex3f(0,10,0);glVertex3f( 0,-10,0);
    glVertex3f(0,0,10);glVertex3f( 0, 0,-10);
    glEnd();

    if(vecprocessor->MarkerCount()>0)
    {
        if(numberCamera == 0)
			glColor3f(0,1,1);
		else
			glColor3f(1,0,1);

        glBegin(GL_LINES);
        
		
		glVertex3f(x/200,y/200,z/200);
		glVertex3f(x/200,(y+50)/200,(z+50)/200);
		glVertex3f(x/200,y/200,z/200);
		glVertex3f(x/200,(y+50)/200,(z-50)/200);

		glVertex3f(x/200,(y+50)/200,(z+50)/200);
		glVertex3f(x/200,y/200,z/200);
		glVertex3f(x/200,(y+50)/200,(z+50)/200);
		glVertex3f(x/200,(y+50)/200,(z-50)/200);

		glVertex3f(x/200,(y+50)/200,(z-50)/200);
		glVertex3f(x/200,y/200,z/200);
		glVertex3f(x/200,(y+50)/200,(z-50)/200);
		glVertex3f(x/200,(y+50)/200,(z+50)/200);
        
        glEnd();

    }
	return;
}

Code: Select all

SetPosition(){
	Vec3 position =  Vec3();
	double angle;

	if(numberCamera == 0){
		angle = 0;
	}else{
		angle = 0;
	}

	angle = angle * 3.14159 / 180;

	vecprocessor->GetPosition(x,y,z);
	
	Vec3 readPosition = Vec3(x,y,z);

	Mat33 matrixProduct = Rzyx(0.0, angle, 0.0);
	position = matrixProduct * readPosition;	

	x = position.v[0];
	y = position.v[1];
	z = position.v[2];

	
		printf("\n\n camara %d.- x %.3f , y %.3f , z %.3f ",numberCamera, x,y,z);
	
	return;
}

Code: Select all

Rzyx(double tx, double ty, double tz)
{
  Mat33 mx,my,mz,mr,maux;

  mx = Rx(tx);
  my = Ry(ty);
  mz = Rz(tz);

  maux = my*mx;
  mr = mz*maux;

  return mr;
}