// 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);
}
