bool WebMapService::Layer::InitBase(libconfig::ChainedSetting& config)
{
auto crs = config["CRS"];
const int numCRS = crs.getLength();
if (numCRS > 0)
{
for (int c = 0; c < numCRS; c++)
{
supportedCRS[crs[c]] = (OGRSpatialReference*)OSRNewSpatialReference(NULL);
}
}
else
{
cout << "WebMapService::Layer: " << "At least one CRS must be defined" << endl;
return false;
}
for (auto it = supportedCRS.begin(); it != supportedCRS.end(); it++)
{
OGRErr err = OGRERR_UNSUPPORTED_SRS;
string crsString = it->first;
if (crsString.length() > 5)
{
int epsgCode = stoi(crsString.c_str() + 5); // TODO: support non-EPSG codes
err = it->second->importFromEPSG(epsgCode);
}
if (err == OGRERR_UNSUPPORTED_SRS)
{
cout << "WebMapService::Layer: " << "requested CRS is unsupported:" << crsString << endl;
return false;
}
}
// create all possible SRS transformation permutations
for (auto it1 = supportedCRS.begin(); it1 != supportedCRS.end(); it1++)
{
for (auto it2 = it1; it2 != supportedCRS.end(); it2++)
{
if (it2 == supportedCRS.end()) break;
CreateTransform(it1->second, it2->second);
CreateTransform(it2->second, it1->second);
}
}
return true;
}
void Box::getCorners(Vec3 **c1, Vec3 **c2, Vec3 **c3, Vec3 **c4) {
Matrix4 orientation = CreateTransform(getOrientation(), getPosition());
*c1 = new Vec3(orientation * corners[0]);
*c2 = new Vec3(orientation * corners[1]);
*c3 = new Vec3(orientation * corners[2]);
*c4 = new Vec3(orientation * corners[3]);
}
static DetectorResult
ProcessFinderPatternInfo(const BitMatrix& image, const FinderPatternInfo& info)
{
float moduleSize = CalculateModuleSize(image, info.topLeft, info.topRight, info.bottomLeft);
if (moduleSize < 1.0f) {
return {};
}
int dimension = ComputeDimension(info.topLeft, info.topRight, info.bottomLeft, moduleSize);
if (dimension < 0)
return {};
const Version* provisionalVersion = Version::ProvisionalVersionForDimension(dimension);
if (provisionalVersion == nullptr)
return {};
int modulesBetweenFPCenters = provisionalVersion->dimensionForVersion() - 7;
AlignmentPattern alignmentPattern;
// Anything above version 1 has an alignment pattern
if (!provisionalVersion->alignmentPatternCenters().empty()) {
// Guess where a "bottom right" finder pattern would have been
float bottomRightX = info.topRight.x() - info.topLeft.x() + info.bottomLeft.x();
float bottomRightY = info.topRight.y() - info.topLeft.y() + info.bottomLeft.y();
// Estimate that alignment pattern is closer by 3 modules
// from "bottom right" to known top left location
float correctionToTopLeft = 1.0f - 3.0f / (float)modulesBetweenFPCenters;
int estAlignmentX = static_cast<int>(info.topLeft.x() + correctionToTopLeft * (bottomRightX - info.topLeft.x()));
int estAlignmentY = static_cast<int>(info.topLeft.y() + correctionToTopLeft * (bottomRightY - info.topLeft.y()));
// Kind of arbitrary -- expand search radius before giving up
for (int i = 4; i <= 16; i <<= 1) {
alignmentPattern = FindAlignmentInRegion(image, moduleSize, estAlignmentX, estAlignmentY, static_cast<float>(i));
if (alignmentPattern.isValid())
break;
}
// If we didn't find alignment pattern... well try anyway without it
}
PerspectiveTransform transform = CreateTransform(info.topLeft, info.topRight, info.bottomLeft, alignmentPattern, dimension);
auto bits = GridSampler::Instance()->sampleGrid(image, dimension, dimension, transform);
if (bits.empty())
return {};
if (alignmentPattern.isValid())
return {std::move(bits), {info.bottomLeft, info.topLeft, info.topRight, alignmentPattern}};
else
return {std::move(bits), {info.bottomLeft, info.topLeft, info.topRight}};
}
/////////////////////////////////////////////////////////////////////////////////
/////////////////////////// NEW FRAME CALLBACK ///////////////////////////////
/////////////////////////////////////////////////////////////////////////////////
void newFrameCallback(sFrameOfData* FrameOfData)
{
// publish body transforms for all interesting bodies
std::tuple<bool,tf::StampedTransform> transform;
int fIndex;
for ( int i = 0; i < NumBodiesOfInterest; ++i )
{
fIndex = FindBodyFrameIndex ( FrameOfData, bodyOfInterest[i] );
if ( fIndex >= 0 )
{
transform = CreateTransform ( FrameOfData, fIndex, bodyOfInterest[i], bodyOriginOffset[i] );
// Only publish if we got valid data
if ( std::get<0>(transform) )
{
Cortex_broadcaster->sendTransform ( std::get<1>(transform) );
// send delay info
ros::Time cur = ros::Time::now();
ROS_INFO ( "frame delay - %f seconds", (cur - glob).toSec());
glob = cur;
}
else
ROS_WARN ( "Received out of range data." );
}
else
ROS_WARN ( "Body %s not found in frame %i", bodyOfInterest[i], FrameOfData->iFrame );
}
// publish markers
#ifdef PUBLISH_VISUALIZATION_MARKERS
visualization_msgs::MarkerArray marker_array;
marker_array = CreateMarkerArray_vis ( FrameOfData );
Cortex_markers.publish(marker_array);
#else
cortex_bridge::Markers marker_array;
marker_array = CreateMarkerArray_novis ( FrameOfData );
Cortex_markers.publish(marker_array);
#endif
// free data frame memory
Cortex_FreeFrame ( FrameOfData );
}
PEDITOR CreateEditor( POBJECT po )
{
PEDITOR pe;
pe = (PEDITOR)Allocate( sizeof( EDITOR ) );
MemSet( pe, 0, sizeof( EDITOR ) );
pe->TView = CreateTransform();
pe->hVideo = OpenDisplaySizedAt( 0, -1, -1, -1, -1 );
//InitVideo( "World Editor" );
SetRedrawHandler( pe->hVideo, (void (CPROC*)(uintptr_t,PRENDERER))EditResizeCallback, (uintptr_t)pe );
SetMouseHandler( pe->hVideo, EditMouseCallback, (uintptr_t)pe );
BindEventToKey( pe->hVideo, KEY_PGUP, 0, PageUpKey, (uintptr_t)pe );
BindEventToKey( pe->hVideo, KEY_PGDN, 0, PageDownKey, (uintptr_t)pe );
// SetCloseHandler( pe->hVideo, EditCloseCallback, (uintptr_t)pe );
// menu for this instance....
#ifdef __WINDOWS__
pe->hMenu = CreatePopup(); //CreatePopupMenu();
AppendPopupItem( pe->hMenu, MF_STRING, MNU_ADDPREDEF+0, "Add Square" );
AppendPopupItem( pe->hMenu, MF_STRING, MNU_ADDPREDEF+1, "Add Triangle" );
AppendPopupItem( pe->hMenu, MF_STRING, MNU_ADDLINE, "Add Line" );
#else
pe->hMenu = NULL;
#endif
SetPoint( vToScreen, VectorConst_Z ); // facing into screen...
Invert( vToScreen ); // facing out of screen...
pe->pCurrent = po;
pe->pCurrentFacet = NULL; //po->objinfo.FacetPool.pFacets + 0;
pe->pCurrentLine = NULL; //po->objinfo.LinePool.pLines + 0;
ClearTransform( pe->TView );
Translate( pe->TView, 0,
0,
(RCOORD)-100.0 );
pe->bLocked = FALSE;
EditResizeCallback( (uintptr_t)pe);
UpdateDisplay( pe->hVideo );
return pe;
}
void ScanKeyboard( PRENDERER hDisplay, PVECTOR KeySpeed, PVECTOR KeyRotation )
{
PVECTOR v;
RCOORD max, step;
if( !hDisplay )
return;
if( !T )
T = CreateTransform();
// how does this HOOK into the camera update functions?!
if( IsKeyDown((hDisplay), KEY_X) || IsKeyDown( (hDisplay), KEY_Q ) )
{
extern int gbExitProgram;
gbExitProgram = TRUE;
}
if( pFirstObject )
{
if( KeyDown( (hDisplay), KEY_A ) )
{
PFACETSET pps;
VECTOR v;
RotateAbs( T, 0.1f, 0, 0 );
pps = pFirstObject->pPlaneSet;
ApplyRotation( T, v, pps->pPlanes->d.n );
SetPoint( pps->pPlanes->d.n, v );
IntersectPlanes( pFirstObject->pLinePool,
pps, TRUE );
}
if( KeyDown( (hDisplay), KEY_Z ) )
{
PFACETSET pps;
VECTOR v;
RotateAbs( T, -0.1f, 0, 0 );
pps = pFirstObject->pPlaneSet;
ApplyRotation( T, v, pps->pPlanes->d.n );
SetPoint( pps->pPlanes->d.n, v );
IntersectPlanes( pFirstObject->pLinePool,
pps, TRUE );
}
}
#define Relax(V, n) if( (V[n]>0.001) || ( V[n]<-0.001) ) \
(V[n] /= 2.0f); else (V[n] = 0);
#define Accel(V, n) \
if ( V[n] < max ) \
V[n] += step; \
else V[n] = max;
#define Decel(V,n) \
if( V[n] > -max ) \
V[n] -= step; \
else V[n] = -max;
#define ROTATION (v=KeyRotation, \
max = ROTATION_DELTA, \
step=ROTATION_ACCEL)
#define SPEED (v=KeySpeed, \
max = KEY_VELOCITY, \
step = KEY_ACCEL )
#define Inertia( KEY_LESS, KEY_MORE,KEY_LESS2, KEY_MORE2, va ) \
if( IsKeyDown(hDisplay, KEY_LESS ) || IsKeyDown(hDisplay, KEY_LESS2) ) Decel(v, va) \
else if( IsKeyDown(hDisplay, KEY_MORE ) || IsKeyDown(hDisplay, KEY_MORE2) ) Accel(v, va) \
else Relax( v, va );
(ROTATION);
Inertia(KEY_DOWN, KEY_UP, 0, 0, vRight );
Inertia(KEY_RIGHT, KEY_LEFT, 0, 0, vUp );
Inertia(KEY_PGUP, KEY_HOME, 0, 0, vForward );
(SPEED);
Inertia( KEY_END, KEY_PGDN, 0, 0, vRight );
Inertia( KEY_ENTER, KEY_GRAY_PLUS, 0, KEY_CENTER, vUp );
Inertia( KEY_DELETE, KEY_INSERT, 0, 0, vForward );
}
