C++ (Cpp) CMap::add3D Examples

Example #1

File: main.cpp Project: dirmich/3dslam

void init_ptos()
{

//   frame = cvQueryFrame( capture );//tomo 2 frames para evitar negros
//   mUpdater.Add(frame);
CvMat *rotation;
CvMat *trans;//,*trans2;
rotation=cvCreateMat(3,1,CV_32FC1);
trans=cvCreateMat(3,1,CV_32FC1);
//trans2=cvCreateMat(3,1,CV_32FC1);
/*//inicializacion de la posición de la camara con funciones de opencv
CvMat *image_points;
CvMat *object_points;
image_points=cvCreateMat(2,mMap.visible,CV_32FC1);
object_points=cvCreateMat(3,mMap.visible,CV_32FC1);
*/

for (int iobj = 0; iobj< 40 ;iobj++)
   for ( int jobj= 0; jobj<3;jobj++)
   {
     cout<<"a";
     mMap.add3D(40*jobj,10*iobj,0);
   }
/// PRIMERA FASE SIMULACIÓN DE LA CAMARA
int npto,npos;
npto=0;
npos=0;
for (float inc=0; inc <200; inc+=1){
   cvmSet(rotation,0,0,0.001);
   cvmSet(rotation,1,0,0);
   cvmSet(rotation,2,0,0);
   mDataCam.SetRotation(rotation);
   
   cvmSet(trans,0,0,40);
   cvmSet(trans,1,0,inc);
   cvmSet(trans,2,0,-200);
  
   mDataCam.SetTranslation(trans);
   
   mModelCam.ProjectPoints();
   npto=0;
   for   (list<CElempunto*>::iterator It=mMap.bbdd.begin();It != mMap.bbdd.end();It++)
   {
	//inserto la proyeccion como la vision de tal manera que simulo una camara
	ptox[npos][npto]=(int)(*It)->projx;
	ptoy[npos][npto]=(int)(*It)->projy;
	//cout<<"pto "<<ptox[npos][npto]<<" "<<ptoy[npos][npto]<<" "<<npos*256*256<<" "<<npto<<endl;
	npto++;	
   }
   npos++;
}

int primeros=0;
mMap.inited=0;
mMap.visible=0;
mMap.bbdd.clear();
mMap.ID=-1;
for (int iobj = 0; iobj< 4 ;iobj++)
   for ( int jobj= 0; jobj<3;jobj++)
   {
       if (primeros<10)
       {
            mMap.add3D(40*jobj,10*iobj,0);
        	primeros++;
       }
   }

   cvmSet(rotation,0,0,0.001);
   cvmSet(rotation,1,0,0);
   cvmSet(rotation,2,0,0);
   mDataCam.SetRotation(rotation);

   cvmSet(trans,0,0,40);
   cvmSet(trans,1,0,0);
   cvmSet(trans,2,0,-200);
   
   mDataCam.SetTranslation(trans);
   mKalman.initState();

}