// Constructors cgvCamera::cgvCamera(cgvPoint3D _PV, cgvPoint3D _rp, cgvPoint3D _up, double _right, double _top, double _znear, double _zfar):zoomFactor(0.01), currentZoom(1) { setCameraParameters(_PV, _rp, _up); setParallelParameters(_right, _top, _znear, _zfar); }
NaoImageCapture::NaoImageCapture(AL::ALPtr<AL::ALBroker> broker) : camera(), gvmName(ConfigFile::getSingleton().getString("naoImage/gvmName")), cameraResolution(AL::kQVGA), // Override below cameraColorSpace(AL::kYUVColorSpace), // Override below cameraFPS(ConfigFile::getSingleton().getInt("naoImage/cameraFPS")), isImageClean(true), localImage(NULL), remoteImage(), usingBottomCamera(false), setTopCamera(false), setBottomCamera(false) { // Convert the strings in the config file into Aldebaran values std::string resolution = ConfigFile::getSingleton().getString("naoImage/cameraResolution"); if (resolution == "320x240") { cameraResolution = AL::kQVGA; } else if (resolution == "640x480") { cameraResolution = AL::kVGA; } else if (resolution == "160x120") { cameraResolution = AL::kQQVGA; } // Default resolution else { cameraResolution = AL::kQVGA; } std::string colorSpace = ConfigFile::getSingleton().getString("naoImage/cameraColorSpace"); if (colorSpace == "YUV") { cameraColorSpace = AL::kYUVColorSpace; } else if (colorSpace == "YUV422") { cameraColorSpace = AL::kYUV422InterlacedColorSpace; } //Create a proxy to the video input module try { camera = broker->getProxy("ALVideoDevice"); LOG_INFO("Created a proxy to ALVideoDevice.") } catch(const AL::ALError &error) { LOG_ERROR("Could not create a proxy to ALVideoDevice.\n%s", error.toString().c_str()) } registerToVIM(); setCameraParameters(); // Setting of camera parameters may have switched cameras, so we set it to // the default setCamera(ConfigFile::getSingleton().getInt("behaviors/defaultUseBottomCamera")); remoteImage.arraySetSize(7); }
bool UCamPar::setCameraParameters(UCamPar * source) { float rf = source->getPixelSize(); // return setCameraParameters( source->getHx() * rf, source->getHy() * rf, source->getK1(), source->getK2(), source->getFocalLength() * rf, rf); }
bool UCamPar::setCameraParameters(Uconfig * ini, const char * key) { // set parameters from config file bool result; float ifl, ik1, ik2, iHx, iHy; // result = ((ini != NULL) and (key != NULL)); if (result) { ifl = ini->doubleGet(key, "focusLength", focalLength); ik1 = ini->doubleGet(key, "radialErrorK1", radialK1); ik2 = ini->doubleGet(key, "radialErrorK2", radialK2); iHx = ini->doubleGet(key, "headPointX", headX); iHy = ini->doubleGet(key, "headPointY", headY); setCameraParameters(iHx, iHy, ik1, ik2, ifl, resFactor); } return result; }
/*! Load the xml configuration file. From the configuration file initialize the parameters corresponding to the objects: KLT, camera. \warning To clean up memory allocated by the xml library, the user has to call vpXmlParser::cleanup() before the exit(). \throw vpException::ioError if the file has not been properly parsed (file not found or wrong format for the data). \param configFile : full name of the xml file. The XML configuration file has the following form: \code <?xml version="1.0"?> <conf> <camera> <width>640</width> <height>480</height> <u0>320</u0> <v0>240</v0> <px>686.24</px> <py>686.24</py> </camera> <face> <angle_appear>65</angle_appear> <angle_disappear>85</angle_disappear> <near_clipping>0.01</near_clipping> <far_clipping>0.90</far_clipping> <fov_clipping>1</fov_clipping> </face> <klt> <mask_border>10</mask_border> <max_features>10000</max_features> <window_size>5</window_size> <quality>0.02</quality> <min_distance>10</min_distance> <harris>0.02</harris> <size_block>3</size_block> <pyramid_lvl>3</pyramid_lvl> </klt> </conf> \endcode \sa loadConfigFile(const std::string&), vpXmlParser::cleanup() */ void vpMbKltTracker::loadConfigFile(const char* configFile) { #ifdef VISP_HAVE_XML2 vpMbtKltXmlParser xmlp; xmlp.setMaxFeatures(10000); xmlp.setWindowSize(5); xmlp.setQuality(0.01); xmlp.setMinDistance(5); xmlp.setHarrisParam(0.01); xmlp.setBlockSize(3); xmlp.setPyramidLevels(3); xmlp.setMaskBorder(maskBorder); xmlp.setAngleAppear(vpMath::deg(angleAppears)); xmlp.setAngleDisappear(vpMath::deg(angleDisappears)); try{ std::cout << " *********** Parsing XML for MBT KLT Tracker ************ " << std::endl; xmlp.parse(configFile); } catch(...){ vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile); throw vpException(vpException::ioError, "problem to parse configuration file."); } vpCameraParameters camera; xmlp.getCameraParameters(camera); setCameraParameters(camera); tracker.setMaxFeatures((int)xmlp.getMaxFeatures()); tracker.setWindowSize((int)xmlp.getWindowSize()); tracker.setQuality(xmlp.getQuality()); tracker.setMinDistance(xmlp.getMinDistance()); tracker.setHarrisFreeParameter(xmlp.getHarrisParam()); tracker.setBlockSize((int)xmlp.getBlockSize()); tracker.setPyramidLevels((int)xmlp.getPyramidLevels()); maskBorder = xmlp.getMaskBorder(); angleAppears = vpMath::rad(xmlp.getAngleAppear()); angleDisappears = vpMath::rad(xmlp.getAngleDisappear()); //if(useScanLine) faces.getMbScanLineRenderer().setMaskBorder(maskBorder); if(xmlp.hasNearClippingDistance()) setNearClippingDistance(xmlp.getNearClippingDistance()); if(xmlp.hasFarClippingDistance()) setFarClippingDistance(xmlp.getFarClippingDistance()); if(xmlp.getFovClipping()) setClipping(clippingFlag = clippingFlag | vpPolygon3D::FOV_CLIPPING); useLodGeneral = xmlp.getLodState(); minLineLengthThresholdGeneral = xmlp.getMinLineLengthThreshold(); minPolygonAreaThresholdGeneral = xmlp.getMinPolygonAreaThreshold(); applyLodSettingInConfig = false; if(this->getNbPolygon() > 0) { applyLodSettingInConfig = true; setLod(useLodGeneral); setMinLineLengthThresh(minLineLengthThresholdGeneral); setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral); } #else vpTRACE("You need the libXML2 to read the config file %s", configFile); #endif }
int useBufTest() { OMX_ERRORTYPE err = OMX_ErrorNone; OMX_BOOL bOmxInitialized = OMX_FALSE; OMX_PARAM_PORTDEFINITIONTYPE sOmxPortDefinition; OMX_CONFIG_BOOLEANTYPE sOmxCapturing; OMX_CONFIG_BOOLEANTYPE sOmxAutoPause; OMX_STATETYPE sOmxState; OMX_U32 nBufferCount; OMX_U32 nBufferSize; OMX_U32 nPortIndex; OMX_U32 i; unsigned int nPreviewTime = 5;/* By default, running for 5 sec for preview */ unsigned int nCaptureTime = 5;/* By default, running for 5 sec for video capture */ OMX_BOOL bCameraStillImageMode = OMX_FALSE; /* By default, the camera is running in video capture mode */ OMX_BOOL bCameraAutoPause = OMX_FALSE; /* By default, the camera is not running in autopause mode */ unsigned int nMaxRunCount = 1;/* By default, running once */ unsigned int nRunCount = 0; OMX_U8 *buf = NULL; surface_display_main_init(DEFAULT_FRAME_WIDTH, DEFAULT_FRAME_HEIGHT); /* Init the Omx core */ DEBUG(DEB_LEV_SIMPLE_SEQ, "Init the OMX core\n"); if ((err = OMX_Init()) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "The OpenMAX core can not be initialized. Exiting...\n"); goto EXIT; } bOmxInitialized = OMX_TRUE; /* Initialize application private data */ appPriv = malloc(sizeof(appPrivateType)); if (appPriv == NULL) { DEBUG(DEB_LEV_ERR, "Allocate app private data failed!Exiting...\n"); err = OMX_ErrorInsufficientResources; goto EXIT; } memset(appPriv, 0, sizeof(appPrivateType)); memset(&sCameraPortBufferList, 0, NUM_CAMERAPORTS * sizeof(OMX_PORTBUFFERCTXT)); /* Getting camera component handle */ if ((err = OMX_GetHandle(&appPriv->camerahandle, "OMX.Action.Camera.Yuv", appPriv, &camera_source_callbacks)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Getting camera component handle failed!Exiting...\n"); goto EXIT; } /* Setting parameters for camera component */ if ((err = setCameraParameters(bCameraStillImageMode)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Set camera parameters failed! Use default settings...\n"); /* Do not exit! */ } /* Allocate and init semaphores */ appPriv->cameraSourceEventSem = malloc(sizeof(tsem_t)); if (appPriv->cameraSourceEventSem == NULL) { DEBUG(DEB_LEV_ERR, "Allocate camera event semaphore failed!Exiting...\n"); err = OMX_ErrorInsufficientResources; goto EXIT; } tsem_init(appPriv->cameraSourceEventSem, 0); RUN_AGAIN: for (nPortIndex = OMX_CAMPORT_INDEX_VF; nPortIndex <= OMX_CAMPORT_INDEX_CP; nPortIndex++) { if ((err = OMX_SendCommand(appPriv->camerahandle, OMX_CommandPortEnable, nPortIndex, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Camera Idle-->Exec failed!Exiting...\n"); goto EXIT; } tsem_down(appPriv->cameraSourceEventSem); } /* Transition camera component Loaded-->Idle */ if ((err = OMX_SendCommand(appPriv->camerahandle, OMX_CommandStateSet, OMX_StateIdle, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Camera Loaded-->Idle failed!Exiting...\n"); goto EXIT; } /* Allocate port buffers for camera component */ for (nPortIndex = OMX_CAMPORT_INDEX_VF; nPortIndex <= OMX_CAMPORT_INDEX_CP; nPortIndex++) { setHeader(&sOmxPortDefinition, sizeof(OMX_PARAM_PORTDEFINITIONTYPE)); sOmxPortDefinition.nPortIndex = nPortIndex; if ((err = OMX_GetParameter(appPriv->camerahandle, OMX_IndexParamPortDefinition, &sOmxPortDefinition)) != OMX_ErrorNone) { DEBUG( DEB_LEV_ERR, "OMX_GetParameter for camera on OMX_IndexParamPortDefinition index failed!Exiting...\n"); goto EXIT; } nBufferCount = sOmxPortDefinition.nBufferCountActual; nBufferSize = sOmxPortDefinition.nBufferSize; DEBUG( DEB_LEV_SIMPLE_SEQ, "Camera port[%ld] needs %ld buffers each of which is %ld bytes\n", nPortIndex, nBufferCount, nBufferSize); for (i = 0; i < nBufferCount; i++) { buf = (OMX_U8 *)malloc(nBufferSize); if(buf == NULL){ DEBUG(DEB_LEV_ERR, "Allocate port buffer for camera failed!Exiting...\n"); goto EXIT; } if ((err = OMX_UseBuffer(appPriv->camerahandle, &sCameraPortBufferList[nPortIndex].pBufHeaderList[i], nPortIndex, NULL, nBufferSize, buf)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Allocate port buffer for camera failed!Exiting...\n"); goto EXIT; } sCameraPortBufferList[nPortIndex].nBufferCountActual++; } } /* Wait camera (Loaded-->Idle) to complete */ tsem_down(appPriv->cameraSourceEventSem); /* Transition camera component Idle-->Exec */ if ((err = OMX_SendCommand(appPriv->camerahandle, OMX_CommandStateSet, OMX_StateExecuting, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Camera Idle-->Exec failed!Exiting...\n"); goto EXIT; } /* Wait camera (Idle-->Exec) to complete */ tsem_down(appPriv->cameraSourceEventSem); /* Fill buffers to camera preview port */ for (i = 0; i < sCameraPortBufferList[OMX_CAMPORT_INDEX_VF].nBufferCountActual; i++) { sCameraPortBufferList[OMX_CAMPORT_INDEX_VF].pBufHeaderList[i]->nFilledLen = 0; sCameraPortBufferList[OMX_CAMPORT_INDEX_VF].pBufHeaderList[i]->nOffset = 0; if ((err = OMX_FillThisBuffer(appPriv->camerahandle, sCameraPortBufferList[OMX_CAMPORT_INDEX_VF].pBufHeaderList[i])) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Fill buffer to camera capture port failed!Exiting...\n"); goto EXIT; } DEBUG( DEB_LEV_SIMPLE_SEQ, "%s: Fill buffer[%ld] (0x%lX) to camera capture port\n", __func__, i, (OMX_U32) sCameraPortBufferList[OMX_CAMPORT_INDEX_VF].pBufHeaderList[i]); } /* Fill buffers to camera capture port */ for (i = 0; i < sCameraPortBufferList[OMX_CAMPORT_INDEX_CP].nBufferCountActual; i++) { sCameraPortBufferList[OMX_CAMPORT_INDEX_CP].pBufHeaderList[i]->nFilledLen = 0; sCameraPortBufferList[OMX_CAMPORT_INDEX_CP].pBufHeaderList[i]->nOffset = 0; if ((err = OMX_FillThisBuffer(appPriv->camerahandle, sCameraPortBufferList[OMX_CAMPORT_INDEX_CP].pBufHeaderList[i])) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Fill buffer to camera capture port failed!Exiting...\n"); goto EXIT; } DEBUG( DEB_LEV_SIMPLE_SEQ, "%s: Fill buffer[%ld] (0x%lX) to camera capture port\n", __func__, i, (OMX_U32) sCameraPortBufferList[OMX_CAMPORT_INDEX_CP].pBufHeaderList[i]); } fprintf(stdout, "Start preview, for %d sec...\n", nPreviewTime); sleep(nPreviewTime); /* Set up autopause mode */ setHeader(&sOmxAutoPause, sizeof(OMX_CONFIG_BOOLEANTYPE)); sOmxAutoPause.bEnabled = bCameraAutoPause; if ((err = OMX_SetConfig(appPriv->camerahandle, OMX_IndexAutoPauseAfterCapture, &sOmxAutoPause)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Set autopause mode failed!Use default settings...\n"); /* Do not exit */ } /* Start capturing */ setHeader(&sOmxCapturing, sizeof(OMX_CONFIG_BOOLEANTYPE)); sOmxCapturing.bEnabled = OMX_TRUE; if ((err = OMX_SetConfig(appPriv->camerahandle, OMX_IndexConfigCapturing, &sOmxCapturing)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Start capturing failed!Exiting...\n"); goto EXIT; } fprintf(stdout, "Start capturing, for %d sec...\n", nCaptureTime); sleep(nCaptureTime); /* Stop capturing */ if (!bCameraStillImageMode) { setHeader(&sOmxCapturing, sizeof(OMX_CONFIG_BOOLEANTYPE)); sOmxCapturing.bEnabled = OMX_FALSE; if ((err = OMX_SetConfig(appPriv->camerahandle, OMX_IndexConfigCapturing, &sOmxCapturing)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Stop capturing failed!Exiting...\n"); goto EXIT; } fprintf(stdout, "Stop capturing...\n"); } /* If in autopause mode, stay for a while before exit */ if (bCameraAutoPause) { fprintf( stdout,"pause state for capture, sleep(%d)\n",5); sleep(5); /* Stop autopause mode */ if ((err = OMX_GetState(appPriv->camerahandle, &sOmxState)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Get camera state failed!Exiting...\n"); goto EXIT; } if (sOmxState == OMX_StatePause) { if ((err = OMX_SendCommand(appPriv->camerahandle, OMX_CommandStateSet, OMX_StateExecuting, 0)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Pause-->Exec failed!Exiting...\n"); goto EXIT; } /* Wait camera (Pause-->Exec) to complete */ tsem_down(appPriv->cameraSourceEventSem); fprintf( stdout, "Now the camera is out of autopause mode, wait for %d sec before exit...\n", 5); sleep(5); } else { DEBUG(DEB_LEV_ERR, "The camera is not in Pause state in autopause mode, ignore...\n"); } } #if 1 /* Transition camera component Exec-->Idle */ if ((err = OMX_SendCommand(appPriv->camerahandle, OMX_CommandStateSet, OMX_StateIdle, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Camera Exec-->Idle failed!Exiting...\n"); goto EXIT; } /* Wait camera (Exec-->Idle) to complete */ tsem_down(appPriv->cameraSourceEventSem); /* Transition camera component Idle-->Exec */ if ((err = OMX_SendCommand(appPriv->camerahandle, OMX_CommandStateSet, OMX_StateExecuting, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Camera Idle-->Exec failed!Exiting...\n"); goto EXIT; } /* Wait camera (Idle-->Exec) to complete */ tsem_down(appPriv->cameraSourceEventSem); /* Fill buffers to camera preview port */ for (i = 0; i < sCameraPortBufferList[OMX_CAMPORT_INDEX_VF].nBufferCountActual; i++) { sCameraPortBufferList[OMX_CAMPORT_INDEX_VF].pBufHeaderList[i]->nFilledLen = 0; sCameraPortBufferList[OMX_CAMPORT_INDEX_VF].pBufHeaderList[i]->nOffset = 0; if ((err = OMX_FillThisBuffer(appPriv->camerahandle, sCameraPortBufferList[OMX_CAMPORT_INDEX_VF].pBufHeaderList[i])) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Fill buffer to camera capture port failed!Exiting...\n"); goto EXIT; } DEBUG( DEB_LEV_SIMPLE_SEQ, "%s: Fill buffer[%ld] (0x%lX) to camera capture port\n", __func__, i, (OMX_U32) sCameraPortBufferList[OMX_CAMPORT_INDEX_VF].pBufHeaderList[i]); } /* Fill buffers to camera capture port */ for (i = 0; i < sCameraPortBufferList[OMX_CAMPORT_INDEX_CP].nBufferCountActual; i++) { sCameraPortBufferList[OMX_CAMPORT_INDEX_CP].pBufHeaderList[i]->nFilledLen = 0; sCameraPortBufferList[OMX_CAMPORT_INDEX_CP].pBufHeaderList[i]->nOffset = 0; if ((err = OMX_FillThisBuffer(appPriv->camerahandle, sCameraPortBufferList[OMX_CAMPORT_INDEX_CP].pBufHeaderList[i])) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Fill buffer to camera capture port failed!Exiting...\n"); goto EXIT; } DEBUG( DEB_LEV_SIMPLE_SEQ, "%s: Fill buffer[%ld] (0x%lX) to camera capture port\n", __func__, i, (OMX_U32) sCameraPortBufferList[OMX_CAMPORT_INDEX_CP].pBufHeaderList[i]); } fprintf(stdout, "Continue to preview, for %d sec...\n", nPreviewTime); sleep(nPreviewTime); #endif /* Transition camera component Exec-->Idle */ if ((err = OMX_SendCommand(appPriv->camerahandle, OMX_CommandStateSet, OMX_StateIdle, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Camera Exec-->Idle failed!Exiting...\n"); goto EXIT; } /* Wait camera (Exec-->Idle) to complete */ tsem_down(appPriv->cameraSourceEventSem); /* Transition camera component Idle-->Loaded */ if ((err = OMX_SendCommand(appPriv->camerahandle, OMX_CommandStateSet, OMX_StateLoaded, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Camera Idle-->Loaded failed!Exiting...\n"); goto EXIT; } #if 1 /* Free bufers for each non-tunneled port of camera component */ for (nPortIndex = OMX_CAMPORT_INDEX_VF; nPortIndex <= OMX_CAMPORT_INDEX_CP; nPortIndex++) { for (i = 0; i < sCameraPortBufferList[nPortIndex].nBufferCountActual; i++) { if (sCameraPortBufferList[nPortIndex].pBufHeaderList[i] != NULL) { buf = sCameraPortBufferList[nPortIndex].pBufHeaderList[i]->pBuffer; OMX_FreeBuffer(appPriv->camerahandle, nPortIndex, sCameraPortBufferList[nPortIndex].pBufHeaderList[i]); if(buf){ free(buf); } sCameraPortBufferList[nPortIndex].pBufHeaderList[i]=NULL; } } sCameraPortBufferList[nPortIndex].nBufferCountActual = 0; } #endif /* Wait camera (Idle-->Loaded) to complete */ tsem_down(appPriv->cameraSourceEventSem); nRunCount++; if (nRunCount < nMaxRunCount) { goto RUN_AGAIN; } EXIT: for (nPortIndex = OMX_CAMPORT_INDEX_VF; nPortIndex <= OMX_CAMPORT_INDEX_CP; nPortIndex++) { for (i = 0; i < sCameraPortBufferList[nPortIndex].nBufferCountActual; i++) { if (sCameraPortBufferList[nPortIndex].pBufHeaderList[i] != NULL) { buf = sCameraPortBufferList[nPortIndex].pBufHeaderList[i]->pBuffer; OMX_FreeBuffer(appPriv->camerahandle, nPortIndex, sCameraPortBufferList[nPortIndex].pBufHeaderList[i]); if(buf){ free(buf); } sCameraPortBufferList[nPortIndex].pBufHeaderList[i]=NULL; } } sCameraPortBufferList[nPortIndex].nBufferCountActual = 0; } /* Free app private data */ if (appPriv != NULL) { /* Free semaphores */ if (appPriv->cameraSourceEventSem != NULL) { tsem_deinit(appPriv->cameraSourceEventSem); free(appPriv->cameraSourceEventSem); } /* Free camera component handle */ if (appPriv->camerahandle != NULL) { OMX_FreeHandle(appPriv->camerahandle); } free(appPriv); } /* Deinit the Omx core */ if (bOmxInitialized) { OMX_Deinit(); } return (int) err; }
int main(int argc, char** argv) { OMX_ERRORTYPE err = OMX_ErrorNone; OMX_BOOL bOmxInitialized = OMX_FALSE; OMX_PARAM_PORTDEFINITIONTYPE sOmxPortDefinition; OMX_CONFIG_BOOLEANTYPE sOmxCapturing; OMX_CONFIG_BOOLEANTYPE sOmxAutoPause; OMX_STATETYPE sOmxState; OMX_U32 nBufferCount; OMX_U32 nBufferSize; OMX_U32 nPortIndex; OMX_U32 i; unsigned int nPreviewTime = 5;/* By default, running for 5 sec for preview */ unsigned int nCaptureTime = 5;/* By default, running for 5 sec for video capture */ char *cCaptureFileName = g_DefaultCaptureFileName; char *cThumbnailFileName = g_DefaultThumbnailFileName; OMX_BOOL bCameraStillImageMode = OMX_FALSE; /* By default, the camera is running in video capture mode */ OMX_BOOL bCameraAutoPause = OMX_FALSE; /* By default, the camera is not running in autopause mode */ unsigned int nMaxRunCount = 1;/* By default, running once */ unsigned int nRunCount = 0; /* Parse arguments */ for ( i = 1; i < argc && argv[i][0] == '-'; i++) { switch (argv[i][1]) { case 'i': bCameraStillImageMode = OMX_TRUE; break; case 'p': bCameraAutoPause = OMX_TRUE; break; case 't': i++; if (i>=argc ||argv[i][0] == '-') { DEBUG(DEB_LEV_ERR, "preview_time expected!\n"); display_help(argv[0]); exit(-1); } nPreviewTime = (unsigned int)atoi(argv[i]); break; case 's': i++; if (i>=argc ||argv[i][0] == '-') { DEBUG(DEB_LEV_ERR, "capture_time expected!\n"); display_help(argv[0]); exit(-1); } nCaptureTime = (unsigned int)atoi(argv[i]); break; case 'c': i++; if (i>=argc ||argv[i][0] == '-') { DEBUG(DEB_LEV_ERR, "capture_file expected!\n"); display_help(argv[0]); exit(-1); } cCaptureFileName = argv[i]; break; case 'm': i++; if (i>=argc ||argv[i][0] == '-') { DEBUG(DEB_LEV_ERR, "thumbnail_file expected!\n"); display_help(argv[0]); exit(-1); } cThumbnailFileName = argv[i]; break; case 'n': i++; if (i>=argc ||argv[i][0] == '-') { DEBUG(DEB_LEV_ERR, "run_count expected!\n"); display_help(argv[0]); exit(-1); } nMaxRunCount = (unsigned int)atoi(argv[i]); break; case 'h': display_help(argv[0]); exit(0); break; default: DEBUG(DEB_LEV_ERR, "Unrecognized option -%c!\n", argv[i][1]); display_help(argv[0]); exit(-1); } } /* Init the Omx core */ DEBUG(DEB_LEV_SIMPLE_SEQ, "Init the OMX core\n"); if ((err = OMX_Init()) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "The OpenMAX core can not be initialized. Exiting...\n"); goto EXIT; } bOmxInitialized = OMX_TRUE; /* Initialize application private data */ appPriv = malloc(sizeof(appPrivateType)); if (appPriv == NULL) { DEBUG(DEB_LEV_ERR, "Allocate app private data failed!Exiting...\n"); err = OMX_ErrorInsufficientResources; goto EXIT; } memset(appPriv, 0, sizeof(appPrivateType)); memset(&sCameraPortBufferList, 0, NUM_CAMERAPORTS*sizeof(OMX_PORTBUFFERCTXT)); /* Open output file for camera capture and thumbnail port */ fCapture=fopen(cCaptureFileName, "wb"); fThumbnail=fopen(cThumbnailFileName, "wb"); /* Getting camera component handle */ if ((err = OMX_GetHandle(&appPriv->camerahandle, "OMX.st.v4l.camera_source", appPriv, &camera_source_callbacks)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Getting camera component handle failed!Exiting...\n"); goto EXIT; } /* Getting fbsink component handle */ if ((err = OMX_GetHandle(&appPriv->colorconvhandle, "OMX.st.video_colorconv.ffmpeg", appPriv, &colorconv_callbacks)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Getting color conv component handle failed!Exiting...\n"); goto EXIT; } /* Getting fbsink component handle */ if ((err = OMX_GetHandle(&appPriv->fbsinkhandle, "OMX.st.fbdev.fbdev_sink", appPriv, &fbsink_callbacks)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Getting fbsink component handle failed!Exiting...\n"); goto EXIT; } /* Setting parameters for camera component */ if ((err = setCameraParameters(bCameraStillImageMode)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Set camera parameters failed! Use default settings...\n"); /* Do not exit! */ } /* Setting parameters for color converter component */ if ((err = setColorConvParameters()) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Set fbsink parameters failed! Use default settings...\n"); /* Do not exit! */ } /* Setting parameters for fbsink component */ if ((err = setFbsinkParameters()) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Set fbsink parameters failed! Use default settings...\n"); /* Do not exit! */ } /* Allocate and init semaphores */ appPriv->cameraSourceEventSem = malloc(sizeof(tsem_t)); if (appPriv->cameraSourceEventSem == NULL) { DEBUG(DEB_LEV_ERR, "Allocate camera event semaphore failed!Exiting...\n"); err = OMX_ErrorInsufficientResources; goto EXIT; } tsem_init(appPriv->cameraSourceEventSem, 0); appPriv->fbsinkEventSem = malloc(sizeof(tsem_t)); if (appPriv->fbsinkEventSem == NULL) { DEBUG(DEB_LEV_ERR, "Allocate fbsink event semaphore failed!Exiting...\n"); err = OMX_ErrorInsufficientResources; goto EXIT; } tsem_init(appPriv->fbsinkEventSem, 0); appPriv->colorconvEventSem = malloc(sizeof(tsem_t)); if (appPriv->colorconvEventSem == NULL) { DEBUG(DEB_LEV_ERR, "Allocate colorconv event semaphore failed!Exiting...\n"); err = OMX_ErrorInsufficientResources; goto EXIT; } tsem_init(appPriv->colorconvEventSem, 0); /* Setup tunnel between camera preview port, color converter and fbsink */ if ((err = OMX_SetupTunnel(appPriv->camerahandle, OMX_CAMPORT_INDEX_VF, appPriv->colorconvhandle, 0)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Setup tunnel between camera preview port and color converter failed!Exiting...\n"); goto EXIT; } if ((err = OMX_SetupTunnel(appPriv->colorconvhandle, 1, appPriv->fbsinkhandle, 0)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Setup tunnel between color conv port and fbsink failed!Exiting...\n"); goto EXIT; } RUN_AGAIN: /* Transition camera component Loaded-->Idle */ if ((err = OMX_SendCommand(appPriv->camerahandle, OMX_CommandStateSet, OMX_StateIdle, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Camera Loaded-->Idle failed!Exiting...\n"); goto EXIT; } /* Transition color conv component Loaded-->Idle */ if ((err = OMX_SendCommand(appPriv->colorconvhandle, OMX_CommandStateSet, OMX_StateIdle, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Color Conv Loaded-->Idle failed!Exiting...\n"); goto EXIT; } /* Transition fbsink component Loaded-->Idle */ if ((err = OMX_SendCommand(appPriv->fbsinkhandle, OMX_CommandStateSet, OMX_StateIdle, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Fbsink Loaded-->Idle failed!Exiting...\n"); goto EXIT; } /* Allocate port buffers for camera component */ for (nPortIndex = OMX_CAMPORT_INDEX_CP; nPortIndex <= OMX_CAMPORT_INDEX_CP_T; nPortIndex++) { setHeader(&sOmxPortDefinition, sizeof(OMX_PARAM_PORTDEFINITIONTYPE)); sOmxPortDefinition.nPortIndex = nPortIndex; if ((err = OMX_GetParameter(appPriv->camerahandle, OMX_IndexParamPortDefinition, &sOmxPortDefinition)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "OMX_GetParameter for camera on OMX_IndexParamPortDefinition index failed!Exiting...\n"); goto EXIT; } nBufferCount = sOmxPortDefinition.nBufferCountActual; nBufferSize = sOmxPortDefinition.nBufferSize; DEBUG(DEB_LEV_SIMPLE_SEQ, "Camera port[%ld] needs %ld buffers each of which is %ld bytes\n", nPortIndex, nBufferCount, nBufferSize); for (i = 0; i < nBufferCount; i++) { if ((err = OMX_AllocateBuffer(appPriv->camerahandle, &sCameraPortBufferList[nPortIndex].pBufHeaderList[i], nPortIndex, NULL, nBufferSize)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Allocate port buffer for camera failed!Exiting...\n"); goto EXIT; } sCameraPortBufferList[nPortIndex].nBufferCountActual++; } } /* Wait camera (Loaded-->Idle) to complete */ tsem_down(appPriv->cameraSourceEventSem); /* Wait fbsink (Loaded-->Idle) to complete */ tsem_down(appPriv->colorconvEventSem); /* Wait fbsink (Loaded-->Idle) to complete */ tsem_down(appPriv->fbsinkEventSem); /* Transition camera component Idle-->Exec */ if ((err = OMX_SendCommand(appPriv->camerahandle, OMX_CommandStateSet, OMX_StateExecuting, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Camera Idle-->Exec failed!Exiting...\n"); goto EXIT; } /* Transition color conv component Idle-->Exec */ if ((err = OMX_SendCommand(appPriv->colorconvhandle, OMX_CommandStateSet, OMX_StateExecuting, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Colorconv Idle-->Exec failed!Exiting...\n"); goto EXIT; } /* Transition fbsink component Idle-->Exec */ if ((err = OMX_SendCommand(appPriv->fbsinkhandle, OMX_CommandStateSet, OMX_StateExecuting, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Fbsink Idle-->Exec failed!Exiting...\n"); goto EXIT; } /* Wait camera (Idle-->Exec) to complete */ tsem_down(appPriv->cameraSourceEventSem); /* Wait color conv (Idle-->Exec) to complete */ tsem_down(appPriv->colorconvEventSem); /* Wait fbsink (Idle-->Exec) to complete */ tsem_down(appPriv->fbsinkEventSem); fprintf(stdout, "Start preview, for %d sec...\n", nPreviewTime); sleep(nPreviewTime); /* Fill buffers to camera capture port */ for (i = 0; i < sCameraPortBufferList[OMX_CAMPORT_INDEX_CP].nBufferCountActual; i++) { sCameraPortBufferList[OMX_CAMPORT_INDEX_CP].pBufHeaderList[i]->nFilledLen = 0; sCameraPortBufferList[OMX_CAMPORT_INDEX_CP].pBufHeaderList[i]->nOffset = 0; if ((err = OMX_FillThisBuffer(appPriv->camerahandle, sCameraPortBufferList[OMX_CAMPORT_INDEX_CP].pBufHeaderList[i])) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Fill buffer to camera capture port failed!Exiting...\n"); goto EXIT; } DEBUG(DEB_LEV_SIMPLE_SEQ, "%s: Fill buffer[%ld] (0x%lX) to camera capture port\n", __func__, i, (OMX_U32)sCameraPortBufferList[OMX_CAMPORT_INDEX_CP].pBufHeaderList[i]); } /* Fill buffers to camera thumbnail port */ for (i = 0; i < sCameraPortBufferList[OMX_CAMPORT_INDEX_CP_T].nBufferCountActual; i++) { sCameraPortBufferList[OMX_CAMPORT_INDEX_CP_T].pBufHeaderList[i]->nFilledLen = 0; sCameraPortBufferList[OMX_CAMPORT_INDEX_CP_T].pBufHeaderList[i]->nOffset = 0; if ((err = OMX_FillThisBuffer(appPriv->camerahandle, sCameraPortBufferList[OMX_CAMPORT_INDEX_CP_T].pBufHeaderList[i])) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Fill buffer to camera capture port failed!Exiting...\n"); goto EXIT; } DEBUG(DEB_LEV_SIMPLE_SEQ, "%s: Fill buffer[%ld] (0x%lX) to camera capture port\n", __func__, i, (OMX_U32)sCameraPortBufferList[OMX_CAMPORT_INDEX_CP_T].pBufHeaderList[i]); } /* Set up autopause mode */ setHeader(&sOmxAutoPause, sizeof(OMX_CONFIG_BOOLEANTYPE)); sOmxAutoPause.bEnabled = bCameraAutoPause; if ((err = OMX_SetConfig(appPriv->camerahandle, OMX_IndexAutoPauseAfterCapture, &sOmxAutoPause)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Set autopause mode failed!Use default settings...\n"); /* Do not exit */ } /* Start capturing */ setHeader(&sOmxCapturing, sizeof(OMX_CONFIG_BOOLEANTYPE)); sOmxCapturing.bEnabled = OMX_TRUE; if ((err = OMX_SetConfig(appPriv->camerahandle, OMX_IndexConfigCapturing, &sOmxCapturing)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Start capturing failed!Exiting...\n"); goto EXIT; } fprintf(stdout, "Start capturing, for %d sec...\n", nCaptureTime); sleep(nCaptureTime); /* Stop capturing */ if (!bCameraStillImageMode) { setHeader(&sOmxCapturing, sizeof(OMX_CONFIG_BOOLEANTYPE)); sOmxCapturing.bEnabled = OMX_FALSE; if ((err = OMX_SetConfig(appPriv->camerahandle, OMX_IndexConfigCapturing, &sOmxCapturing)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Stop capturing failed!Exiting...\n"); goto EXIT; } fprintf(stdout, "Stop capturing...\n"); } /* If in autopause mode, stay for a while before exit */ if (bCameraAutoPause) { fprintf(stdout, "Now the camera is in autopause mode, wait for %d sec before out of this mode...\n", 5); sleep(5); /* Stop autopause mode */ if ((err = OMX_GetState(appPriv->camerahandle,&sOmxState)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Get camera state failed!Exiting...\n"); goto EXIT; } if (sOmxState == OMX_StatePause) { if ((err = OMX_SendCommand(appPriv->camerahandle, OMX_CommandStateSet, OMX_StateExecuting, 0 )) != OMX_ErrorNone ) { DEBUG(DEB_LEV_ERR, "Pause-->Exec failed!Exiting...\n"); goto EXIT; } /* Wait camera (Pause-->Exec) to complete */ tsem_down(appPriv->cameraSourceEventSem); fprintf(stdout, "Now the camera is out of autopause mode, wait for %d sec before exit...\n", 5); sleep(5); } else { DEBUG(DEB_LEV_ERR, "The camera is not in Pause state in autopause mode, ignore...\n"); } } /* Transition camera component Exec-->Idle */ if ((err = OMX_SendCommand(appPriv->camerahandle, OMX_CommandStateSet, OMX_StateIdle, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Camera Exec-->Idle failed!Exiting...\n"); goto EXIT; } /* Transition colorconv component Exec-->Idle */ if ((err = OMX_SendCommand(appPriv->colorconvhandle, OMX_CommandStateSet, OMX_StateIdle, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Color conv Exec-->Idle failed!Exiting...\n"); goto EXIT; } /* Transition fbsink component Exec-->Idle */ if ((err = OMX_SendCommand(appPriv->fbsinkhandle, OMX_CommandStateSet, OMX_StateIdle, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Fbsink Exec-->Idle failed!Exiting...\n"); goto EXIT; } /* Wait camera (Exec-->Idle) to complete */ tsem_down(appPriv->cameraSourceEventSem); /* Wait color conv (Exec-->Idle) to complete */ tsem_down(appPriv->colorconvEventSem); /* Wait fbsink (Exec-->Idle) to complete */ tsem_down(appPriv->fbsinkEventSem); /* Transition camera component Idle-->Exec */ if ((err = OMX_SendCommand(appPriv->camerahandle, OMX_CommandStateSet, OMX_StateExecuting, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Camera Idle-->Exec failed!Exiting...\n"); goto EXIT; } /* Transition color conv component Idle-->Exec */ if ((err = OMX_SendCommand(appPriv->colorconvhandle, OMX_CommandStateSet, OMX_StateExecuting, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Colorconv Idle-->Exec failed!Exiting...\n"); goto EXIT; } /* Transition fbsink component Idle-->Exec */ if ((err = OMX_SendCommand(appPriv->fbsinkhandle, OMX_CommandStateSet, OMX_StateExecuting, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Fbsink Idle-->Exec failed!Exiting...\n"); goto EXIT; } /* Wait camera (Idle-->Exec) to complete */ tsem_down(appPriv->cameraSourceEventSem); /* Wait color conv (Exec-->Idle) to complete */ tsem_down(appPriv->colorconvEventSem); /* Wait fbsink (Idle-->Exec) to complete */ tsem_down(appPriv->fbsinkEventSem); fprintf(stdout, "Continue to preview, for %d sec...\n", nPreviewTime); sleep(nPreviewTime); /* Transition camera component Exec-->Idle */ if ((err = OMX_SendCommand(appPriv->camerahandle, OMX_CommandStateSet, OMX_StateIdle, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Camera Exec-->Idle failed!Exiting...\n"); goto EXIT; } /* Transition color conv component Exec-->Idle */ if ((err = OMX_SendCommand(appPriv->colorconvhandle, OMX_CommandStateSet, OMX_StateIdle, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Color conv Exec-->Idle failed!Exiting...\n"); goto EXIT; } /* Transition fbsink component Exec-->Idle */ if ((err = OMX_SendCommand(appPriv->fbsinkhandle, OMX_CommandStateSet, OMX_StateIdle, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Fbsink Exec-->Idle failed!Exiting...\n"); goto EXIT; } /* Wait camera (Exec-->Idle) to complete */ tsem_down(appPriv->cameraSourceEventSem); /* Wait color conv (Exec-->Idle) to complete */ tsem_down(appPriv->colorconvEventSem); /* Wait fbsink (Exec-->Idle) to complete */ tsem_down(appPriv->fbsinkEventSem); /* Transition camera component Idle-->Loaded */ if ((err = OMX_SendCommand(appPriv->camerahandle, OMX_CommandStateSet, OMX_StateLoaded, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Camera Idle-->Loaded failed!Exiting...\n"); goto EXIT; } /* Transition color conv component Idle-->Loaded */ if ((err = OMX_SendCommand(appPriv->colorconvhandle, OMX_CommandStateSet, OMX_StateLoaded, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Color conv Exec-->Idle failed!Exiting...\n"); goto EXIT; } /* Transition fbsink component Idle-->Loaded */ if ((err = OMX_SendCommand(appPriv->fbsinkhandle, OMX_CommandStateSet, OMX_StateLoaded, NULL)) != OMX_ErrorNone) { DEBUG(DEB_LEV_ERR, "Fbsink Idle-->Loaded failed!Exiting...\n"); goto EXIT; } /* Free bufers for each non-tunneled port of camera component */ for (nPortIndex = OMX_CAMPORT_INDEX_CP; nPortIndex <= OMX_CAMPORT_INDEX_CP_T; nPortIndex++) { for (i = 0; i < sCameraPortBufferList[nPortIndex].nBufferCountActual; i++) { if (sCameraPortBufferList[nPortIndex].pBufHeaderList[i] != NULL) { OMX_FreeBuffer(appPriv->camerahandle, nPortIndex, sCameraPortBufferList[nPortIndex].pBufHeaderList[i]); } } sCameraPortBufferList[nPortIndex].nBufferCountActual = 0; } /* Wait camera (Idle-->Loaded) to complete */ tsem_down(appPriv->cameraSourceEventSem); /* Wait color conv (Exec-->Idle) to complete */ tsem_down(appPriv->colorconvEventSem); /* Wait fbsink (Idle-->Loaded) to complete */ tsem_down(appPriv->fbsinkEventSem); nRunCount++; if (nRunCount < nMaxRunCount) { goto RUN_AGAIN; } fprintf(stdout, "The captured videos are saved in file \"%s\"\n", cCaptureFileName); fprintf(stdout, "The thumbnail image is saved in file \"%s\"\n", cThumbnailFileName); EXIT: if (fCapture != NULL) { fclose(fCapture); } if (fThumbnail != NULL) { fclose(fThumbnail); } /* Free app private data */ if (appPriv != NULL) { /* Free semaphores */ if (appPriv->cameraSourceEventSem != NULL) { tsem_deinit(appPriv->cameraSourceEventSem); free(appPriv->cameraSourceEventSem); } if (appPriv->colorconvEventSem != NULL) { tsem_deinit(appPriv->colorconvEventSem); free(appPriv->colorconvEventSem); } if (appPriv->fbsinkEventSem != NULL) { tsem_deinit(appPriv->fbsinkEventSem); free(appPriv->fbsinkEventSem); } /* Free camera component handle */ if (appPriv->camerahandle != NULL) { OMX_FreeHandle(appPriv->camerahandle); } /* Free Color conv component handle */ if (appPriv->colorconvhandle != NULL) { OMX_FreeHandle(appPriv->colorconvhandle); } /* Free fbsink component handle */ if (appPriv->fbsinkhandle != NULL) { OMX_FreeHandle(appPriv->fbsinkhandle); } free(appPriv); } /* Deinit the Omx core */ if (bOmxInitialized) { OMX_Deinit(); } return (int) err; }
void pcl::visualization::PCLVisualizerInteractorStyle::OnKeyDown () { if (!init_) { pcl::console::print_error ("[PCLVisualizerInteractorStyle] Interactor style not initialized. Please call Initialize () before continuing.\n"); return; } if (!rens_) { pcl::console::print_error ("[PCLVisualizerInteractorStyle] No renderer collection given! Use SetRendererCollection () before continuing.\n"); return; } FindPokedRenderer (Interactor->GetEventPosition ()[0], Interactor->GetEventPosition ()[1]); if (wif_->GetInput () == NULL) { wif_->SetInput (Interactor->GetRenderWindow ()); wif_->Modified (); snapshot_writer_->Modified (); } // Save the initial windows width/height if (win_height_ == -1 || win_width_ == -1) { int *win_size = Interactor->GetRenderWindow ()->GetSize (); win_height_ = win_size[0]; win_width_ = win_size[1]; } // Get the status of special keys (Cltr+Alt+Shift) bool shift = Interactor->GetShiftKey (); bool ctrl = Interactor->GetControlKey (); bool alt = Interactor->GetAltKey (); bool keymod = false; switch (modifier_) { case INTERACTOR_KB_MOD_ALT: { keymod = alt; break; } case INTERACTOR_KB_MOD_CTRL: { keymod = ctrl; break; } case INTERACTOR_KB_MOD_SHIFT: { keymod = shift; break; } } // ---[ Check the rest of the key codes // Save camera parameters if ((Interactor->GetKeySym ()[0] == 'S' || Interactor->GetKeySym ()[0] == 's') && ctrl && !alt && !shift) { if (camera_file_.empty ()) { getCameraParameters (camera_); camera_saved_ = true; pcl::console::print_info ("Camera parameters saved, you can press CTRL + R to restore.\n"); } else { if (saveCameraParameters (camera_file_)) { pcl::console::print_info ("Save camera parameters to %s, you can press CTRL + R to restore.\n", camera_file_.c_str ()); } else { pcl::console::print_error ("[PCLVisualizerInteractorStyle] Can't save camera parameters to file: %s.\n", camera_file_.c_str ()); } } } // Restore camera parameters if ((Interactor->GetKeySym ()[0] == 'R' || Interactor->GetKeySym ()[0] == 'r') && ctrl && !alt && !shift) { if (camera_file_.empty ()) { if (camera_saved_) { setCameraParameters (camera_); pcl::console::print_info ("Camera parameters restored.\n"); } else { pcl::console::print_info ("No camera parameters saved for restoring.\n"); } } else { if (boost::filesystem::exists (camera_file_)) { if (loadCameraParameters (camera_file_)) { pcl::console::print_info ("Restore camera parameters from %s.\n", camera_file_.c_str ()); } else { pcl::console::print_error ("Can't restore camera parameters from file: %s.\n", camera_file_.c_str ()); } } else { pcl::console::print_info ("No camera parameters saved in %s for restoring.\n", camera_file_.c_str ()); } } } // Switch between point color/geometry handlers if (Interactor->GetKeySym () && Interactor->GetKeySym ()[0] >= '0' && Interactor->GetKeySym ()[0] <= '9') { CloudActorMap::iterator it; int index = Interactor->GetKeySym ()[0] - '0' - 1; if (index == -1) index = 9; // Add 10 more for CTRL+0..9 keys if (ctrl) index += 10; // Geometry ? if (keymod) { for (it = actors_->begin (); it != actors_->end (); ++it) { CloudActor *act = &(*it).second; if (index >= static_cast<int> (act->geometry_handlers.size ())) continue; // Save the geometry handler index for later usage act->geometry_handler_index_ = index; // Create the new geometry PointCloudGeometryHandler<pcl::PCLPointCloud2>::ConstPtr geometry_handler = act->geometry_handlers[index]; // Use the handler to obtain the geometry vtkSmartPointer<vtkPoints> points; geometry_handler->getGeometry (points); // Set the vertices vtkSmartPointer<vtkCellArray> vertices = vtkSmartPointer<vtkCellArray>::New (); for (vtkIdType i = 0; i < static_cast<vtkIdType> (points->GetNumberOfPoints ()); ++i) vertices->InsertNextCell (static_cast<vtkIdType>(1), &i); // Create the data vtkSmartPointer<vtkPolyData> data = vtkSmartPointer<vtkPolyData>::New (); data->SetPoints (points); data->SetVerts (vertices); // Modify the mapper if (use_vbos_) { vtkVertexBufferObjectMapper* mapper = static_cast<vtkVertexBufferObjectMapper*>(act->actor->GetMapper ()); mapper->SetInput (data); // Modify the actor act->actor->SetMapper (mapper); } else { vtkPolyDataMapper* mapper = static_cast<vtkPolyDataMapper*>(act->actor->GetMapper ()); #if VTK_MAJOR_VERSION < 6 mapper->SetInput (data); #else mapper->SetInputData (data); #endif // Modify the actor act->actor->SetMapper (mapper); } act->actor->Modified (); } } else { for (it = actors_->begin (); it != actors_->end (); ++it) { CloudActor *act = &(*it).second; // Check for out of bounds if (index >= static_cast<int> (act->color_handlers.size ())) continue; // Save the color handler index for later usage act->color_handler_index_ = index; // Get the new color PointCloudColorHandler<pcl::PCLPointCloud2>::ConstPtr color_handler = act->color_handlers[index]; vtkSmartPointer<vtkDataArray> scalars; color_handler->getColor (scalars); double minmax[2]; scalars->GetRange (minmax); // Update the data vtkPolyData *data = static_cast<vtkPolyData*>(act->actor->GetMapper ()->GetInput ()); data->GetPointData ()->SetScalars (scalars); // Modify the mapper if (use_vbos_) { vtkVertexBufferObjectMapper* mapper = static_cast<vtkVertexBufferObjectMapper*>(act->actor->GetMapper ()); mapper->SetScalarRange (minmax); mapper->SetScalarModeToUsePointData (); mapper->SetInput (data); // Modify the actor act->actor->SetMapper (mapper); } else { vtkPolyDataMapper* mapper = static_cast<vtkPolyDataMapper*>(act->actor->GetMapper ()); mapper->SetScalarRange (minmax); mapper->SetScalarModeToUsePointData (); #if VTK_MAJOR_VERSION < 6 mapper->SetInput (data); #else mapper->SetInputData (data); #endif // Modify the actor act->actor->SetMapper (mapper); } act->actor->Modified (); } } Interactor->Render (); return; } std::string key (Interactor->GetKeySym ()); if (key.find ("XF86ZoomIn") != std::string::npos) zoomIn (); else if (key.find ("XF86ZoomOut") != std::string::npos) zoomOut (); switch (Interactor->GetKeyCode ()) { case 'h': case 'H': { pcl::console::print_info ("| Help:\n" "-------\n" " p, P : switch to a point-based representation\n" " w, W : switch to a wireframe-based representation (where available)\n" " s, S : switch to a surface-based representation (where available)\n" "\n" " j, J : take a .PNG snapshot of the current window view\n" " c, C : display current camera/window parameters\n" " f, F : fly to point mode\n" "\n" " e, E : exit the interactor\n" " q, Q : stop and call VTK's TerminateApp\n" "\n" " +/- : increment/decrement overall point size\n" " +/- [+ ALT] : zoom in/out \n" "\n" " g, G : display scale grid (on/off)\n" " u, U : display lookup table (on/off)\n" "\n" " r, R [+ ALT] : reset camera [to viewpoint = {0, 0, 0} -> center_{x, y, z}]\n" " CTRL + s, S : save camera parameters\n" " CTRL + r, R : restore camera parameters\n" "\n" " ALT + s, S : turn stereo mode on/off\n" " ALT + f, F : switch between maximized window mode and original size\n" "\n" " l, L : list all available geometric and color handlers for the current actor map\n" " ALT + 0..9 [+ CTRL] : switch between different geometric handlers (where available)\n" " 0..9 [+ CTRL] : switch between different color handlers (where available)\n" "\n" " SHIFT + left click : select a point (start with -use_point_picking)\n" "\n" " x, X : toggle rubber band selection mode for left mouse button\n" ); break; } // Get the list of available handlers case 'l': case 'L': { // Iterate over the entire actors list and extract the geomotry/color handlers list for (CloudActorMap::iterator it = actors_->begin (); it != actors_->end (); ++it) { std::list<std::string> geometry_handlers_list, color_handlers_list; CloudActor *act = &(*it).second; for (size_t i = 0; i < act->geometry_handlers.size (); ++i) geometry_handlers_list.push_back (act->geometry_handlers[i]->getFieldName ()); for (size_t i = 0; i < act->color_handlers.size (); ++i) color_handlers_list.push_back (act->color_handlers[i]->getFieldName ()); if (!geometry_handlers_list.empty ()) { int i = 0; pcl::console::print_info ("List of available geometry handlers for actor "); pcl::console::print_value ("%s: ", (*it).first.c_str ()); for (std::list<std::string>::iterator git = geometry_handlers_list.begin (); git != geometry_handlers_list.end (); ++git) pcl::console::print_value ("%s(%d) ", (*git).c_str (), ++i); pcl::console::print_info ("\n"); } if (!color_handlers_list.empty ()) { int i = 0; pcl::console::print_info ("List of available color handlers for actor "); pcl::console::print_value ("%s: ", (*it).first.c_str ()); for (std::list<std::string>::iterator cit = color_handlers_list.begin (); cit != color_handlers_list.end (); ++cit) pcl::console::print_value ("%s(%d) ", (*cit).c_str (), ++i); pcl::console::print_info ("\n"); } } break; } // Switch representation to points case 'p': case 'P': { vtkSmartPointer<vtkActorCollection> ac = CurrentRenderer->GetActors (); vtkCollectionSimpleIterator ait; for (ac->InitTraversal (ait); vtkActor* actor = ac->GetNextActor (ait); ) { for (actor->InitPathTraversal (); vtkAssemblyPath* path = actor->GetNextPath (); ) { vtkSmartPointer<vtkActor> apart = reinterpret_cast <vtkActor*> (path->GetLastNode ()->GetViewProp ()); apart->GetProperty ()->SetRepresentationToPoints (); } } break; } // Save a PNG snapshot with the current screen case 'j': case 'J': { char cam_fn[80], snapshot_fn[80]; unsigned t = static_cast<unsigned> (time (0)); sprintf (snapshot_fn, "screenshot-%d.png" , t); saveScreenshot (snapshot_fn); sprintf (cam_fn, "screenshot-%d.cam", t); saveCameraParameters (cam_fn); pcl::console::print_info ("Screenshot (%s) and camera information (%s) successfully captured.\n", snapshot_fn, cam_fn); break; } // display current camera settings/parameters case 'c': case 'C': { vtkSmartPointer<vtkCamera> cam = Interactor->GetRenderWindow ()->GetRenderers ()->GetFirstRenderer ()->GetActiveCamera (); double clip[2], focal[3], pos[3], view[3]; cam->GetClippingRange (clip); cam->GetFocalPoint (focal); cam->GetPosition (pos); cam->GetViewUp (view); int *win_pos = Interactor->GetRenderWindow ()->GetPosition (); int *win_size = Interactor->GetRenderWindow ()->GetSize (); std::cerr << "Clipping plane [near,far] " << clip[0] << ", " << clip[1] << endl << "Focal point [x,y,z] " << focal[0] << ", " << focal[1] << ", " << focal[2] << endl << "Position [x,y,z] " << pos[0] << ", " << pos[1] << ", " << pos[2] << endl << "View up [x,y,z] " << view[0] << ", " << view[1] << ", " << view[2] << endl << "Camera view angle [degrees] " << cam->GetViewAngle () << endl << "Window size [x,y] " << win_size[0] << ", " << win_size[1] << endl << "Window position [x,y] " << win_pos[0] << ", " << win_pos[1] << endl; break; } case '=': { zoomIn(); break; } case 43: // KEY_PLUS { if(alt) zoomIn (); else { vtkSmartPointer<vtkActorCollection> ac = CurrentRenderer->GetActors (); vtkCollectionSimpleIterator ait; for (ac->InitTraversal (ait); vtkActor* actor = ac->GetNextActor (ait); ) { for (actor->InitPathTraversal (); vtkAssemblyPath* path = actor->GetNextPath (); ) { vtkSmartPointer<vtkActor> apart = reinterpret_cast <vtkActor*> (path->GetLastNode ()->GetViewProp ()); float psize = apart->GetProperty ()->GetPointSize (); if (psize < 63.0f) apart->GetProperty ()->SetPointSize (psize + 1.0f); } } } break; } case 45: // KEY_MINUS { if(alt) zoomOut (); else { vtkSmartPointer<vtkActorCollection> ac = CurrentRenderer->GetActors (); vtkCollectionSimpleIterator ait; for (ac->InitTraversal (ait); vtkActor* actor = ac->GetNextActor (ait); ) { for (actor->InitPathTraversal (); vtkAssemblyPath* path = actor->GetNextPath (); ) { vtkSmartPointer<vtkActor> apart = static_cast<vtkActor*> (path->GetLastNode ()->GetViewProp ()); float psize = apart->GetProperty ()->GetPointSize (); if (psize > 1.0f) apart->GetProperty ()->SetPointSize (psize - 1.0f); } } } break; } // Switch between maximize and original window size case 'f': case 'F': { if (keymod) { // Get screen size int *temp = Interactor->GetRenderWindow ()->GetScreenSize (); int scr_size[2]; scr_size[0] = temp[0]; scr_size[1] = temp[1]; // Get window size temp = Interactor->GetRenderWindow ()->GetSize (); int win_size[2]; win_size[0] = temp[0]; win_size[1] = temp[1]; // Is window size = max? if (win_size[0] == max_win_height_ && win_size[1] == max_win_width_) { // Set the previously saved 'current' window size Interactor->GetRenderWindow ()->SetSize (win_height_, win_width_); // Set the previously saved window position Interactor->GetRenderWindow ()->SetPosition (win_pos_x_, win_pos_y_); Interactor->GetRenderWindow ()->Render (); Interactor->Render (); } // Set to max else { int *win_pos = Interactor->GetRenderWindow ()->GetPosition (); // Save the current window position win_pos_x_ = win_pos[0]; win_pos_y_ = win_pos[1]; // Save the current window size win_height_ = win_size[0]; win_width_ = win_size[1]; // Set the maximum window size Interactor->GetRenderWindow ()->SetSize (scr_size[0], scr_size[1]); Interactor->GetRenderWindow ()->Render (); Interactor->Render (); int *win_size = Interactor->GetRenderWindow ()->GetSize (); // Save the maximum window size max_win_height_ = win_size[0]; max_win_width_ = win_size[1]; } } else { AnimState = VTKIS_ANIM_ON; vtkAssemblyPath *path = NULL; Interactor->GetPicker ()->Pick (Interactor->GetEventPosition ()[0], Interactor->GetEventPosition ()[1], 0.0, CurrentRenderer); vtkAbstractPropPicker *picker; if ((picker = vtkAbstractPropPicker::SafeDownCast (Interactor->GetPicker ()))) path = picker->GetPath (); if (path != NULL) Interactor->FlyTo (CurrentRenderer, picker->GetPickPosition ()); AnimState = VTKIS_ANIM_OFF; } break; } // 's'/'S' w/out ALT case 's': case 'S': { if (keymod) { int stereo_render = Interactor->GetRenderWindow ()->GetStereoRender (); if (!stereo_render) { if (stereo_anaglyph_mask_default_) { Interactor->GetRenderWindow ()->SetAnaglyphColorMask (4, 3); stereo_anaglyph_mask_default_ = false; } else { Interactor->GetRenderWindow ()->SetAnaglyphColorMask (2, 5); stereo_anaglyph_mask_default_ = true; } } Interactor->GetRenderWindow ()->SetStereoRender (!stereo_render); Interactor->GetRenderWindow ()->Render (); Interactor->Render (); } else Superclass::OnKeyDown (); break; } // Display a grid/scale over the screen case 'g': case 'G': { if (!grid_enabled_) { grid_actor_->TopAxisVisibilityOn (); CurrentRenderer->AddViewProp (grid_actor_); grid_enabled_ = true; } else { CurrentRenderer->RemoveViewProp (grid_actor_); grid_enabled_ = false; } break; } case 'o': case 'O': { vtkSmartPointer<vtkCamera> cam = CurrentRenderer->GetActiveCamera (); int flag = cam->GetParallelProjection (); cam->SetParallelProjection (!flag); CurrentRenderer->SetActiveCamera (cam); CurrentRenderer->Render (); break; } // Display a LUT actor on screen case 'u': case 'U': { CloudActorMap::iterator it; for (it = actors_->begin (); it != actors_->end (); ++it) { CloudActor *act = &(*it).second; vtkScalarsToColors* lut = act->actor->GetMapper ()->GetLookupTable (); lut_actor_->SetLookupTable (lut); lut_actor_->Modified (); } if (!lut_enabled_) { CurrentRenderer->AddActor (lut_actor_); lut_actor_->SetVisibility (true); lut_enabled_ = true; } else { CurrentRenderer->RemoveActor (lut_actor_); lut_enabled_ = false; } CurrentRenderer->Render (); break; } // Overwrite the camera reset case 'r': case 'R': { if (!keymod) { FindPokedRenderer(Interactor->GetEventPosition ()[0], Interactor->GetEventPosition ()[1]); if(CurrentRenderer != 0) CurrentRenderer->ResetCamera (); else PCL_WARN ("no current renderer on the interactor style."); CurrentRenderer->Render (); break; } vtkSmartPointer<vtkCamera> cam = CurrentRenderer->GetActiveCamera (); static CloudActorMap::iterator it = actors_->begin (); // it might be that some actors don't have a valid transformation set -> we skip them to avoid a seg fault. bool found_transformation = false; for (unsigned idx = 0; idx < actors_->size (); ++idx, ++it) { if (it == actors_->end ()) it = actors_->begin (); const CloudActor& actor = it->second; if (actor.viewpoint_transformation_.GetPointer ()) { found_transformation = true; break; } } // if a valid transformation was found, use it otherwise fall back to default view point. if (found_transformation) { const CloudActor& actor = it->second; cam->SetPosition (actor.viewpoint_transformation_->GetElement (0, 3), actor.viewpoint_transformation_->GetElement (1, 3), actor.viewpoint_transformation_->GetElement (2, 3)); cam->SetFocalPoint (actor.viewpoint_transformation_->GetElement (0, 3) - actor.viewpoint_transformation_->GetElement (0, 2), actor.viewpoint_transformation_->GetElement (1, 3) - actor.viewpoint_transformation_->GetElement (1, 2), actor.viewpoint_transformation_->GetElement (2, 3) - actor.viewpoint_transformation_->GetElement (2, 2)); cam->SetViewUp (actor.viewpoint_transformation_->GetElement (0, 1), actor.viewpoint_transformation_->GetElement (1, 1), actor.viewpoint_transformation_->GetElement (2, 1)); } else { cam->SetPosition (0, 0, 0); cam->SetFocalPoint (0, 0, 1); cam->SetViewUp (0, -1, 0); } // go to the next actor for the next key-press event. if (it != actors_->end ()) ++it; else it = actors_->begin (); CurrentRenderer->SetActiveCamera (cam); CurrentRenderer->ResetCameraClippingRange (); CurrentRenderer->Render (); break; } case 'x' : case 'X' : { CurrentMode = (CurrentMode == ORIENT_MODE) ? SELECT_MODE : ORIENT_MODE; if (CurrentMode == SELECT_MODE) { // Save the point picker point_picker_ = static_cast<vtkPointPicker*> (Interactor->GetPicker ()); // Switch for an area picker vtkSmartPointer<vtkAreaPicker> area_picker = vtkSmartPointer<vtkAreaPicker>::New (); Interactor->SetPicker (area_picker); } else { // Restore point picker Interactor->SetPicker (point_picker_); } break; } case 'q': case 'Q': { Interactor->ExitCallback (); return; } default: { Superclass::OnKeyDown (); break; } } KeyboardEvent event (true, Interactor->GetKeySym (), Interactor->GetKeyCode (), Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ()); keyboard_signal_ (event); rens_->Render (); Interactor->Render (); }
bool pcl::visualization::PCLVisualizerInteractorStyle::getCameraParameters (const std::vector<std::string> &camera) { pcl::visualization::Camera camera_temp; // look for '/' as a separator if (camera.size () != 7) { pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Camera parameters given, but with an invalid number of options (%lu vs 7)!\n", static_cast<unsigned long> (camera.size ())); return (false); } std::string clip_str = camera.at (0); std::string focal_str = camera.at (1); std::string pos_str = camera.at (2); std::string view_str = camera.at (3); std::string fovy_str = camera.at (4); std::string win_size_str = camera.at (5); std::string win_pos_str = camera.at (6); // Get each camera setting separately and parse for ',' std::vector<std::string> clip_st; boost::split (clip_st, clip_str, boost::is_any_of (","), boost::token_compress_on); if (clip_st.size () != 2) { pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Invalid parameters given for camera clipping angle!\n"); return (false); } camera_temp.clip[0] = atof (clip_st.at (0).c_str ()); camera_temp.clip[1] = atof (clip_st.at (1).c_str ()); std::vector<std::string> focal_st; boost::split (focal_st, focal_str, boost::is_any_of (","), boost::token_compress_on); if (focal_st.size () != 3) { pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Invalid parameters given for camera focal point!\n"); return (false); } camera_temp.focal[0] = atof (focal_st.at (0).c_str ()); camera_temp.focal[1] = atof (focal_st.at (1).c_str ()); camera_temp.focal[2] = atof (focal_st.at (2).c_str ()); std::vector<std::string> pos_st; boost::split (pos_st, pos_str, boost::is_any_of (","), boost::token_compress_on); if (pos_st.size () != 3) { pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Invalid parameters given for camera position!\n"); return (false); } camera_temp.pos[0] = atof (pos_st.at (0).c_str ()); camera_temp.pos[1] = atof (pos_st.at (1).c_str ()); camera_temp.pos[2] = atof (pos_st.at (2).c_str ()); std::vector<std::string> view_st; boost::split (view_st, view_str, boost::is_any_of (","), boost::token_compress_on); if (view_st.size () != 3) { pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Invalid parameters given for camera viewup!\n"); return (false); } camera_temp.view[0] = atof (view_st.at (0).c_str ()); camera_temp.view[1] = atof (view_st.at (1).c_str ()); camera_temp.view[2] = atof (view_st.at (2).c_str ()); std::vector<std::string> fovy_size_st; boost::split (fovy_size_st, fovy_str, boost::is_any_of (","), boost::token_compress_on); if (fovy_size_st.size () != 1) { pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Invalid parameters given for field of view angle!\n"); return (false); } camera_temp.fovy = atof (fovy_size_st.at (0).c_str ()); std::vector<std::string> win_size_st; boost::split (win_size_st, win_size_str, boost::is_any_of (","), boost::token_compress_on); if (win_size_st.size () != 2) { pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Invalid parameters given for window size!\n"); return (false); } camera_temp.window_size[0] = atof (win_size_st.at (0).c_str ()); camera_temp.window_size[1] = atof (win_size_st.at (1).c_str ()); std::vector<std::string> win_pos_st; boost::split (win_pos_st, win_pos_str, boost::is_any_of (","), boost::token_compress_on); if (win_pos_st.size () != 2) { pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Invalid parameters given for window position!\n"); return (false); } camera_temp.window_pos[0] = atof (win_pos_st.at (0).c_str ()); camera_temp.window_pos[1] = atof (win_pos_st.at (1).c_str ()); setCameraParameters (camera_temp); return (true); }
void findCameraPositions(drawing_3d_t &drawing, int case_index) { vector_t v_min = drawing.getWcsMin(); vector_t v_max = drawing.getWcsMax(); double x_min = v_min.getX(); double y_min = v_min.getY(); double z_min = v_min.getZ(); double x_max = v_max.getX(); double y_max = v_max.getY(); double z_max = v_max.getZ(); double e_x, e_y, e_z, a_x, a_y, a_z, up_x, up_y, up_z, L, R, T, B, N, F; if (case_index == 4 || case_index == 7){ //dcs, clipped dcs. e_x = (x_max + x_min)/2; e_y = (y_max + y_min)/2; e_z = 10; a_x = e_x; a_y = e_y; a_z = 0; up_x = 0; up_y = 1; up_z = 0; L = (x_min - x_max); R = (x_max - x_min); T = (y_max - y_min); B = (y_min - y_max); if(R > T) { T = R ; B = -T; } else { R = T; L = -R; } N = 5; F = 15; } else { e_x = (x_max + x_min)/2; e_y = 2*y_max - y_min; e_z = (z_max + z_min)/2; a_x = e_x; a_y = (y_max + y_min)/2; a_z = e_z; up_x = 1; up_y = 0, up_z = 0; L = (z_min - z_max); R = (z_max - z_min); T = (x_max - x_min); B = (x_min - x_max); if(R > T) { T = R ; B = -T; } else { R = T; L = -R; } N = (y_max-y_min)/2; F = N*5; } vector_t eye(e_x, e_y, e_z); vector_t lookat(a_x, a_y, a_z); vector_t up_vector(up_x, up_y, up_z); setCameraParameters(drawing, eye, lookat, up_vector, L, R, T, B, N, F); }