bool
MultiTarget::initialise(const std::string& multiFile) {
if (m_multi)
{
osg::notify(osg::WARN) << "MultiTarget: Cannot initialise Target. Already initialised." << std::endl;
return false;
}
std::string actualMultiFile = osgDB::findDataFile(multiFile);
// Check if multi-markre configuration file exists
if (actualMultiFile.empty() || !osgDB::fileExists(actualMultiFile))
{
osg::notify(osg::WARN) << "MultiTarget: Cannot initialise MultiTarget. File " << multiFile << " not found" << std::endl;
return false;
}
m_multi = arMultiReadConfigFile(actualMultiFile.c_str());
if (m_multi == NULL)
{
osg::notify(osg::WARN) << "MultiTarget: Cannot initialise MultiTarget. Error reading config file." << std::endl;
return false;
}
setName(multiFile);
setActive(false);
_valid = false;
return true;
}
static void init( void )
{
ARParam wparam;
/* open the video path */
if( arVideoOpen( vconf ) < 0 ) exit(0);
/* find the size of the window */
if( arVideoInqSize(&xsize, &ysize) < 0 ) exit(0);
printf("Image size (x,y) = (%d,%d)\n", xsize, ysize);
/* set the initial camera parameters */
if( arParamLoad(cparam_name, 1, &wparam) < 0 ) {
printf("Camera parameter load error !!\n");
exit(0);
}
arParamChangeSize( &wparam, xsize, ysize, &cparam );
arInitCparam( &cparam );
printf("*** Camera Parameter ***\n");
arParamDisp( &cparam );
if( (config = arMultiReadConfigFile(config_name)) == NULL ) {
printf("config data load error !!\n");
exit(0);
}
/* open the graphics window */
argInit( &cparam, 1.0, 0, 2, 1, 0 );
arFittingMode = AR_FITTING_TO_IDEAL;
arImageProcMode = AR_IMAGE_PROC_IN_HALF;
argDrawMode = AR_DRAW_BY_TEXTURE_MAPPING;
argTexmapMode = AR_DRAW_TEXTURE_HALF_IMAGE;
}
static void init( void )
{
ARParam wparam;
/* open the video path */
if( arVideoOpen( vconf ) < 0 ) exit(0);
/* find the size of the window */
if( arVideoInqSize(&xsize, &ysize) < 0 ) exit(0);
printf("Image size (x,y) = (%d,%d)\n", xsize, ysize);
/* set the initial camera parameters */
if( arParamLoad(cparam_name, 1, &wparam) < 0 ) {
printf("Camera parameter load error !!\n");
exit(0);
}
arParamChangeSize( &wparam, xsize, ysize, &cparam );
arInitCparam( &cparam );
printf("*** Camera Parameter ***\n");
arParamDisp( &cparam );
/* load the paddle marker file */
if( (paddleInfo = paddleInit(paddle_name)) == NULL ) {
printf("paddleInit error!!\n");
exit(0);
}
printf("Loaded Paddle File\n");
if( (config = arMultiReadConfigFile(config_name)) == NULL ) {
printf("config data load error !!\n");
exit(0);
}
printf("Loaded Multi Marker File\n");
/* init items */
myListItem.itemnum=4;
myListItem.item[0].pos[0]=0.;myListItem.item[0].pos[1]=0.;myListItem.item[0].onpaddle=0;
myListItem.item[1].pos[0]=100.;myListItem.item[1].pos[1]=-100.;myListItem.item[1].onpaddle=0;
myListItem.item[2].pos[0]=200.;myListItem.item[2].pos[1]=0.;myListItem.item[2].onpaddle=0;
myListItem.item[3].pos[0]=0.;myListItem.item[3].pos[1]=0.;myListItem.item[3].onpaddle=1;
/* set up the initial paddle contents */
myPaddleItem.item = 3;
myPaddleItem.angle = 0.0;
myPaddleItem.x = 0.0;
myPaddleItem.y = 0.0;
/* open the graphics window */
argInit( &cparam, 1.0, 0, 0, 0, 0 );
}
static void init( void )
{
ARParam wparam;
int i;
/* open the video path */
if( arVideoOpen( vconf ) < 0 ) exit(0);
/* find the size of the window */
if( arVideoInqSize(&xsize, &ysize) < 0 ) exit(0);
printf("Image size (x,y) = (%d,%d)\n", xsize, ysize);
/* set the initial camera parameters */
if( arParamLoad(cparam_name, 1, &wparam) < 0 ) {
printf("Camera parameter load error !!\n");
exit(0);
}
arParamChangeSize( &wparam, xsize, ysize, &cparam );
arInitCparam( &cparam );
printf("*** Camera Parameter ***\n");
arParamDisp( &cparam );
/* load the paddle marker file */
if( (paddleInfo = paddleInit(paddle_name)) == NULL ) {
printf("paddleInit error!!\n");
exit(0);
}
printf("Loaded Paddle File\n");
if( (config = arMultiReadConfigFile(config_name)) == NULL ) {
printf("config data load error !!\n");
exit(0);
}
printf("Loaded Multi Marker File\n");
/* initialize the targets */
for (i=0;i<TARGET_NUM;i++){
myTarget[i].pos[0] = 50.0*i;
myTarget[i].pos[1] = -50.0*i;
myTarget[i].pos[2] = 50.0*i;
myTarget[i].state = NOT_TOUCHED;
}
/* open the graphics window */
argInit( &cparam, 1.0, 0, 0, 0, 0 );
}
bool ARMarkerMulti::load(const char *multiConfig, ARPattHandle *arPattHandle)
{
if (m_loaded) unload();
config = arMultiReadConfigFile(multiConfig, arPattHandle);
if (!config) {
ARController::logv("Error loading multimarker config %s", multiConfig);
return false;
}
visible = visiblePrev = false;
// ARPatterns to hold images and positions of the patterns for display to the user.
allocatePatterns(config->marker_num);
for (int i = 0; i < patternCount; i++) {
if (config->marker[i].patt_type == AR_MULTI_PATTERN_TYPE_TEMPLATE) {
patterns[i]->loadTemplate(config->marker[i].patt_id, arPattHandle, (float)config->marker[i].width);
} else {
patterns[i]->loadMatrix(config->marker[i].patt_id, AR_MATRIX_CODE_3x3, (float)config->marker[i].width); // TODO: Determine actual matrix code type in use.
}
patterns[i]->m_matrix[ 0] = config->marker[i].trans[0][0];
patterns[i]->m_matrix[ 1] = config->marker[i].trans[1][0];
patterns[i]->m_matrix[ 2] = config->marker[i].trans[2][0];
patterns[i]->m_matrix[ 3] = _0_0;
patterns[i]->m_matrix[ 4] = config->marker[i].trans[0][1];
patterns[i]->m_matrix[ 5] = config->marker[i].trans[1][1];
patterns[i]->m_matrix[ 6] = config->marker[i].trans[2][1];
patterns[i]->m_matrix[ 7] = _0_0;
patterns[i]->m_matrix[ 8] = config->marker[i].trans[0][2];
patterns[i]->m_matrix[ 9] = config->marker[i].trans[1][2];
patterns[i]->m_matrix[10] = config->marker[i].trans[2][2];
patterns[i]->m_matrix[11] = _0_0;
patterns[i]->m_matrix[12] = config->marker[i].trans[0][3];
patterns[i]->m_matrix[13] = config->marker[i].trans[1][3];
patterns[i]->m_matrix[14] = config->marker[i].trans[2][3];
patterns[i]->m_matrix[15] = _1_0;
}
m_loaded = true;
return true;
}
bool TrackerMultiMarker::init(const char* nCamParamFile, const char* nMultiFile, ARFloat nNearClip,
ARFloat nFarClip) {
// init some "static" from TrackerMultiMarker
//
if (this->marker_infoTWO == NULL)
this->marker_infoTWO = new ARMarkerInfo2[MAX_IMAGE_PATTERNS];
if (!loadCameraFile(nCamParamFile, nNearClip, nFarClip))
return false;
if (config)
arMultiFreeConfig(config);
if ((config = arMultiReadConfigFile(nMultiFile)) == NULL)
return false;
//printf("INFO: %d markers loaded from config file\n", config->marker_num);
return true;
}
void ARMultiPublisher::arInit()
{
arInitCparam(&cam_param_);
ROS_INFO("Camera parameters for ARMultiPublisher are:");
arParamDisp(&cam_param_);
if ((multi_marker_config_ = arMultiReadConfigFile(pattern_filename_)) == NULL)
{
ROS_ASSERT_MSG(false, "Could not load configurations for ARMultiPublisher.");
}
// load in the object data - trained markers and associated bitmap files
// if ((object = ar_object::read_ObjData(pattern_filename_, &objectnum)) == NULL)
// ROS_BREAK ();
num_total_markers_ = multi_marker_config_->marker_num;
ROS_INFO("Read >%i< objects from file.", num_total_markers_);
size_ = cvSize(cam_param_.xsize, cam_param_.ysize);
capture_ = cvCreateImage(size_, IPL_DEPTH_8U, 4);
}
static int setupCubeMarker(const char *patt_name, ARMultiMarkerInfoT **multiConfig_p, ARHandle *arhandle, ARPattHandle **pattHandle_p)
{
if ((*pattHandle_p = arPattCreateHandle()) == NULL) {
ARLOGe("setupCubeMarker(): Error: arPattCreateHandle.\n");
return (FALSE);
}
if (((*multiConfig_p) = arMultiReadConfigFile(patt_name, *pattHandle_p)) == NULL) {
ARLOGe("setupCubeMarker(): Error reading multimarker config file '%s'.\n", patt_name);
arPattDeleteHandle(*pattHandle_p);
return (FALSE);
}
if( (*multiConfig_p)->patt_type == AR_MULTI_PATTERN_DETECTION_MODE_TEMPLATE ) {
arSetPatternDetectionMode( arhandle, AR_TEMPLATE_MATCHING_COLOR );
} else if( (*multiConfig_p)->patt_type == AR_MULTI_PATTERN_DETECTION_MODE_MATRIX ) {
arSetPatternDetectionMode( arhandle, AR_MATRIX_CODE_DETECTION );
} else { // AR_MULTI_PATTERN_DETECTION_MODE_TEMPLATE_AND_MATRIX
arSetPatternDetectionMode( arhandle, AR_TEMPLATE_MATCHING_COLOR_AND_MATRIX );
}
arPattAttach(arhandle, *pattHandle_p);
return (TRUE);
}
bool TrackerMultiMarker::init(const char* const nCamParamFile, const char* const nMultiFile, ARFloat nNearClip,
ARFloat nFarClip) {
// init some "static" members from artoolkit
// (some systems don't like such large global members
// so we allocate this manually)
if (this->marker_infoTWO == NULL) {
this->marker_infoTWO = new ARMarkerInfo2[MAX_IMAGE_PATTERNS];
}
if (config) {
arMultiFreeConfig(config);
}
if ((config = arMultiReadConfigFile(nMultiFile)) == NULL) {
return false;
}
// initialize applications
if (nCamParamFile) {
return loadCameraFile(nCamParamFile, nNearClip, nFarClip);
}
return true;
}
static int setupMarkers(const int patt_count, const char *patt_names[], ARMultiMarkerInfoT *multiConfigs[], ARHandle *arhandle, ARPattHandle **pattHandle_p)
{
int i;
if (!patt_count)
{
// Default behaviour is to default to matrix mode.
*pattHandle_p = NULL;
arSetPatternDetectionMode(arhandle, AR_MATRIX_CODE_DETECTION); // If no markers specified, default to matrix mode.
}
else
{
// If marker configs have been specified, attempt to load them.
int mode = -1, nextMode;
// Need a pattern handle because the config file could specify matrix or template markers.
if ((*pattHandle_p = arPattCreateHandle2(gPattSize, gPattCountMax)) == NULL)
{
ARLOGe("setupMarkers(): Error: arPattCreateHandle2.\n");
return (FALSE);
}
for (i = 0; i < patt_count; i++)
{
if (!(multiConfigs[i] = arMultiReadConfigFile(patt_names[i], *pattHandle_p)))
{
ARLOGe("setupMarkers(): Error reading multimarker config file '%s'.\n", patt_names[i]);
for (i--; i >= 0; i--)
{
arMultiFreeConfig(multiConfigs[i]);
}
arPattDeleteHandle(*pattHandle_p);
return (FALSE);
}
if (multiConfigs[i]->patt_type == AR_MULTI_PATTERN_DETECTION_MODE_TEMPLATE)
{
nextMode = AR_TEMPLATE_MATCHING_COLOR;
}
else if (multiConfigs[i]->patt_type == AR_MULTI_PATTERN_DETECTION_MODE_MATRIX)
{
nextMode = AR_MATRIX_CODE_DETECTION;
}
else // AR_MULTI_PATTERN_DETECTION_MODE_TEMPLATE_AND_MATRIX or mixed.
{
nextMode = AR_TEMPLATE_MATCHING_COLOR_AND_MATRIX;
}
if (mode == -1)
{
mode = nextMode;
}
else if (mode != nextMode)
{
mode = AR_TEMPLATE_MATCHING_COLOR_AND_MATRIX;
}
}
arSetPatternDetectionMode(arhandle, mode);
arPattAttach(arhandle, *pattHandle_p);
}
return (TRUE);
}
