// Test function void laserTest(){ initLaser(); #if MRPT_HAS_WXWIDGETS CDisplayWindowPlots win("Sick Laser scaner"); #endif CSimplePointsMap receiveMap; // CObservation2DRangeScan* obs = new CObservation2DRangeScan(); CObservation* obs = new CObservation2DRangeScan(); while(!mrpt::system::os::kbhit()){ if(BREAK){ cout << index << "frame, Input the any key to continue" << endl; //cin >> temp; getc(stdin); fflush(stdin); } if(receiveDataFromSensor((CObservation2DRangeScan*)obs, SICK)) { ((CObservation2DRangeScan*)obs)->sensorPose = CPose3D(0,0,0); receiveMap.clear(); receiveMap.insertionOptions.minDistBetweenLaserPoints = 0; receiveMap.insertObservation( obs ); } #if MRPT_HAS_WXWIDGETS vector_float xs, ys, zs; receiveMap.getAllPoints(xs,ys,zs); win.plot(xs,ys,".r3","t1"); win.axis_equal(); // win.axis_fit(); #endif } delete obs; sick.stop(); }
void MyLaser::plot(CObservation2DRangeScan obs){ if(simulateLaser){ CSimplePointsMap map; map.insertionOptions.minDistBetweenLaserPoints=0; map.insertionOptions.also_interpolate=false; map.insertionOptions.isPlanarMap=true; map.clear(); map.insertObservation(&obs); vector_float xs,ys,zs; map.getAllPoints(xs,ys,zs); float width(gridMap.getXMax()-gridMap.getXMin()),height(gridMap.getYMax()-gridMap.getYMin()); win.image(mapImage,-x_center_pixel*res*1000,-y_center_pixel*res*1000,width*1000,height*1000); win.plot(xs*1000,ys*1000,".b3"); } else{ } }
// ------------------------------------------------------ // TestGeometry3D // ------------------------------------------------------ void TestLaser2Imgs() { // Set your rawlog file name if (!mrpt::system::fileExists(RAWLOG_FILE)) THROW_EXCEPTION_CUSTOM_MSG1("Rawlog file does not exist: %s",RAWLOG_FILE.c_str()) CActionCollectionPtr action; CSensoryFramePtr observations; size_t rawlogEntry = 0; //bool end = false; CDisplayWindow wind; // Set relative path for externally-stored images in rawlogs: string rawlog_images_path = extractFileDirectory( RAWLOG_FILE ); rawlog_images_path+="/Images"; CImage::IMAGES_PATH_BASE = rawlog_images_path; // Set it. CFileGZInputStream rawlogFile( RAWLOG_FILE ); for (;;) { if (os::kbhit()) { char c = os::getch(); if (c==27) break; } // Load action/observation pair from the rawlog: // -------------------------------------------------- cout << "Reading act/oct pair from rawlog..." << endl; if (! CRawlog::readActionObservationPair( rawlogFile, action, observations, rawlogEntry) ) break; // file EOF // CAMERA............ // Get CObservationStereoImages CObservationStereoImagesPtr sImgs; CObservationImagePtr Img; sImgs = observations->getObservationByClass<CObservationStereoImages>(); if (!sImgs) { Img = observations->getObservationByClass<CObservationImage>(); if(!Img) continue; } CPose3D cameraPose; // Get Camera Pose (B) (CPose3D) CMatrixDouble33 K; // Get Calibration matrix (K) sImgs ? sImgs->getSensorPose( cameraPose ) : Img->getSensorPose( cameraPose ); K = sImgs ? sImgs->leftCamera.intrinsicParams : Img->cameraParams.intrinsicParams; // LASER............. // Get CObservationRange2D CObservation2DRangeScanPtr laserScan = observations->getObservationByClass<CObservation2DRangeScan>(); if (!laserScan) continue; // Get Laser Pose (A) (CPose3D) CPose3D laserPose; laserScan->getSensorPose( laserPose ); if (abs(laserPose.yaw())>DEG2RAD(90)) continue; // Only front lasers // Get 3D Point relative to the Laser coordinate Frame (P1) (CPoint3D) CPoint3D point; CSimplePointsMap mapa; mapa.insertionOptions.minDistBetweenLaserPoints = 0; observations->insertObservationsInto( &mapa ); // <- The map contains the pose of the points (P1) // Get the points into the map vector<float> X, Y, Z; vector<float>::iterator itX, itY, itZ; mapa.getAllPoints(X,Y,Z); unsigned int imgW = sImgs? sImgs->imageLeft.getWidth() : Img->image.getWidth(); unsigned int imgH = sImgs? sImgs->imageLeft.getHeight() : Img->image.getHeight(); //unsigned int idx = 0; vector<float> imgX,imgY; vector<float>::iterator itImgX,itImgY; imgX.resize( X.size() ); imgY.resize( Y.size() ); CImage image; image = sImgs ? sImgs->imageLeft : Img->image; // Get pixels in the image: // Pimg = (kx,ky,k)^T = K(I|0)*P2 // Main loop for( itX = X.begin(), itY = Y.begin(), itZ = Z.begin(), itImgX = imgX.begin(), itImgY = imgY.begin(); itX != X.end(); itX++, itY++, itZ++, itImgX++, itImgY++) { // Coordinates Transformation CPoint3D pLaser(*itX,*itY,*itZ); CPoint3D pCamera(pLaser-cameraPose); if( pCamera.z() > 0 ) { *itImgX = - K(0,0)*((pCamera.x())/(pCamera.z())) + K(0,2); *itImgY = - K(1,1)*((pCamera.y())/(pCamera.z())) + K(1,2); if( *itImgX > 0 && *itImgX < imgW && *itImgY > 0 && *itImgY < imgH ) image.filledRectangle(*itImgX-1,*itImgY-1,*itImgX+1,*itImgY+1,TColor(255,0,0)); } // end if } // end for action.clear_unique(); observations.clear_unique(); wind.showImage(image); mrpt::system::sleep(50); }; // end for mrpt::system::pause(); }