void SerialOscController::sendDeviceLedMapCommand(int xOffset, int yOffset, int row0, int row1, int row2, int row3, int row4, int row5, int row6, int row7)
{
HashMap<String, MonomeDevice*>::Iterator iter(devices);
while (iter.next()) {
auto device = iter.getValue();
UdpTransmitSocket transmitSocket( IpEndpointName( SERIALOSC_ADDRESS, device->port ) );
char buffer[OSC_BUFFER_SIZE];
osc::OutboundPacketStream p( buffer, OSC_BUFFER_SIZE );
String address = (device->prefix + "/grid/led/map");
p << osc::BeginBundleImmediate
<< osc::BeginMessage( address.toUTF8() )
<< xOffset
<< yOffset
<< row0
<< row1
<< row2
<< row3
<< row4
<< row5
<< row6
<< row7
<< osc::EndMessage
<< osc::EndBundle;
transmitSocket.Send( p.Data(), p.Size() );
}
}
void Window::testOscPack(char* oscMessageOut, float oscValueOut)
{
UdpTransmitSocket transmitSocket( IpEndpointName( ADDRESS, PORT ) );
char buffer[OUTPUT_BUFFER_SIZE];
osc::OutboundPacketStream p( buffer, OUTPUT_BUFFER_SIZE );
if (strstr(oscMessageOut, "button") != NULL)
{
//p << osc::BeginBundleImmediate
p << osc::BeginMessage(oscMessageOut)
<< bool(oscValueOut)
<< osc::EndMessage;
//<< osc::EndBundle;
}
else
{
// STOP HERE: make sure I'm sending x, y and z at the same time
p << osc::BeginMessage(oscMessageOut)
<< float(oscValueOut)
<< osc::EndMessage;
}
// testing example:
//p << osc::BeginBundleImmediate
// << osc::BeginMessage( "/test1" )
// << true << 23 << (float)3.1415 << "hello" << osc::EndMessage
// << osc::BeginMessage( "/test2" )
// << true << 24 << (float)10.8 << "world" << osc::EndMessage
// << osc::EndBundle;
transmitSocket.Send( p.Data(), p.Size() );
}
void IisuManager::oscSend()
{
m_device->lockFrame();
// Create socket and stream.
UdpTransmitSocket transmitSocket(IpEndpointName(m_dataBase->getIpAddress().c_str(), m_dataBase->getIpPort()));
osc::OutboundPacketStream outPacketStream(m_oscBuffer, OUTPUT_BUFFER_SIZE);
// Build the OSC packet up.
outPacketStream << osc::BeginBundleImmediate;;
IisuReaderOscSender iisuReaderOscSender(m_dataBase, &outPacketStream);
for (uint i = 0; i < m_typedPathMapsLinearized.size(); ++i)
{
TypedPathMap* typedPathMap = m_typedPathMapsLinearized[i];
assert(typedPathMap);
iisuReaderOscSender.setPathMapData(typedPathMap->m_fullOscPath, m_iisuDataHandles[i]);
typedPathMap->accept(&iisuReaderOscSender);
}
outPacketStream << osc::EndBundle;
// Stream OSC packet out.
transmitSocket.Send(outPacketStream.Data(), outPacketStream.Size());
// Store packet size.
m_dataBase->setOscPacketSize(outPacketStream.Size());
m_device->releaseFrame();
}
int main(int argc, char **argv)
{
emokit_device* d;
d = emokit_create();
std::cout << "Current epoc devices connected " << emokit_get_count(d, EMOKIT_VID, EMOKIT_PID) << "\n";
if(emokit_open(d, EMOKIT_VID, EMOKIT_PID, 1) != 0)
{
std::cout << "CANNOT CONNECT\n";
return 1;
}
UdpTransmitSocket transmitSocket( IpEndpointName( ADDRESS, PORT ) );
char buffer[OUTPUT_BUFFER_SIZE];
char raw_frame[32];
struct emokit_frame frame;
std::cout << "Connected\n";
while(1)
{
std::cout << "Starting read\n";
if(emokit_read_data(d) > 0)
{
struct emokit_frame c;
c = emokit_get_next_frame(d);
osc::OutboundPacketStream p( buffer, OUTPUT_BUFFER_SIZE );
p << osc::BeginMessage( "/emokit/channels" )
<< c.F3 << c.FC6 << c.P7 << c.T8 << c.F7 << c.F8
<< c.T7 << c.P8 << c.AF4 << c.F4 << c.AF3 << c.O2
<< c.O1 << c.FC5
<< osc::EndMessage;
transmitSocket.Send( p.Data(), p.Size() );
osc::OutboundPacketStream q( buffer, OUTPUT_BUFFER_SIZE );
q << osc::BeginMessage( "/emokit/gyro" )
<< (int)frame.gyroX << (int)frame.gyroY << osc::EndMessage;
transmitSocket.Send( q.Data(), q.Size() );
osc::OutboundPacketStream info( buffer, OUTPUT_BUFFER_SIZE );
info << osc::BeginMessage( "/emokit/info" )
<< (int)c.battery
<< c.cq.F3 << c.cq.FC6 << c.cq.P7 << c.cq.T8 << c.cq.F7 << c.cq.F8
<< c.cq.T7 << c.cq.P8 << c.cq.AF4 << c.cq.F4 << c.cq.AF3 << c.cq.O2
<< c.cq.O1 << c.cq.FC5
<< osc::EndMessage;
transmitSocket.Send( info.Data(), info.Size() );
}
}
fflush(stdout);
emokit_close(d);
emokit_delete(d);
return 0;
}
int main(int argc, char* argv[])
{
signal(SIGINT, sigproc);
#ifndef WIN32
signal(SIGQUIT, sigproc);
#endif
UdpTransmitSocket transmitSocket( IpEndpointName( ADDRESS, PORT ) );
char buffer[OUTPUT_BUFFER_SIZE];
FILE *input;
FILE *output;
char raw_frame[32];
struct emokit_frame frame;
emokit_device* d;
uint8_t data[32];
d = emokit_create();
printf("Current epoc devices connected: %d\n", emokit_get_count(d, EMOKIT_VID, EMOKIT_PID));
if(emokit_open(d, EMOKIT_VID, EMOKIT_PID, 1) != 0)
{
printf("CANNOT CONNECT\n");
return 1;
}
while(1)
{
int r;
if((r=emokit_read_data_timeout(d, 1000)) > 0)
{
frame = emokit_get_next_frame(d);
osc::OutboundPacketStream p( buffer, OUTPUT_BUFFER_SIZE );
p << osc::BeginMessage( "/multiplot" )
<< conv(frame.F3) << conv(frame.FC6) << conv(frame.P7)
<< conv(frame.T8) << conv(frame.F7) << conv(frame.F8)
<< conv(frame.T7) << conv(frame.P8) << conv(frame.AF4)
<< conv(frame.F4) << conv(frame.AF3) << conv(frame.O2)
<< conv(frame.O1) << conv(frame.FC5) << osc::EndMessage;
transmitSocket.Send( p.Data(), p.Size() );
} else if(r == 0)
fprintf(stderr, "Headset Timeout\n");
else {
fprintf(stderr, "Headset Error\n");
break;
}
}
emokit_close(d);
emokit_delete(d);
return 0;
}
int
main(int argc, char* argv[])
{
if (argc < 4 || (argc - 4) % 2 == 1) usage();
// setup udp socket
char *host = argv[1];
unsigned short port = atoi(argv[2]);
UdpTransmitSocket transmitSocket(IpEndpointName(host, port));
// setup packet
char buf[BUF_SIZE];
memset(buf, 0, BUF_SIZE);
osc::OutboundPacketStream p(buf, BUF_SIZE);
//p << osc::BeginBundleImmediate;
char *path = argv[3];
p << osc::BeginMessage(path);
int idx = 4;
while (idx + 1 < argc) {
std::string type = std::string(argv[idx]);
std::string val = std::string(argv[idx + 1]);
if (type == "s") {
p << val.c_str();
}
else if (type == "i") {
p << atoi(val.c_str());
}
else if (type == "f") {
p << (float)(atof(val.c_str()));
}
else if (type == "b") {
if (val == "true") p << true;
else p << false;
}
else {
usage();
}
idx += 2;
}
p << osc::EndMessage;
//p << osc::EndBundle;
transmitSocket.Send(p.Data(), p.Size());
return 0;
}
int main(int argc, char* argv[])
{
UdpTransmitSocket transmitSocket( IpEndpointName( ADDRESS, PORT ) );
char buffer[OUTPUT_BUFFER_SIZE];
osc::OutboundPacketStream p( buffer, OUTPUT_BUFFER_SIZE );
p << osc::BeginBundleImmediate
<< osc::BeginMessage( "/color" )
<< 23 << osc::EndMessage
<< osc::EndBundle;
transmitSocket.Send( p.Data(), p.Size() );
}
void SerialOscController::sendDevicePortMessage(int port)
{
UdpTransmitSocket transmitSocket( IpEndpointName( SERIALOSC_ADDRESS, port ) );
char buffer[OSC_BUFFER_SIZE];
osc::OutboundPacketStream p( buffer, OSC_BUFFER_SIZE );
p << osc::BeginBundleImmediate
<< osc::BeginMessage( "/sys/port" )
<< listenPort
<< osc::EndMessage
<< osc::EndBundle;
transmitSocket.Send( p.Data(), p.Size() );
}
int main(int argc, char* argv[])
{
(void) argc; // suppress unused parameter warnings
(void) argv; // suppress unused parameter warnings
UdpTransmitSocket transmitSocket(IpEndpointName(ADDRESS, PORT));
char buffer[OUTPUT_BUFFER_SIZE];
osc::OutboundPacketStream stream(buffer, OUTPUT_BUFFER_SIZE);
stream << osc::BeginMessage("foo");
stream << 8;
stream << 6;
stream << 7;
stream << osc::EndMessage;
transmitSocket.Send(stream.Data(), stream.Size());
}
int main(int argc, char* argv[])
{
UdpTransmitSocket transmitSocket( IpEndpointName( ADDRESS, PORT ) );
char buffer[OUTPUT_BUFFER_SIZE];
osc::OutboundPacketStream p( buffer, OUTPUT_BUFFER_SIZE );
p << osc::BeginBundleImmediate
<< osc::BeginMessage( "/test1" )
<< true << 23 << (float)3.1415 << "hello" << osc::EndMessage
<< osc::BeginMessage( "/test2" )
<< true << 24 << (float)10.8 << "world" << osc::EndMessage
<< osc::EndBundle;
transmitSocket.Send( p.Data(), p.Size() );
}
void SerialOscController::sendDevicePrefixMessage(int port)
{
String prefix = devicePrefix;
if (prefix.startsWith("/")) {
prefix = prefix.substring(1);
}
UdpTransmitSocket transmitSocket( IpEndpointName( SERIALOSC_ADDRESS, port ) );
char buffer[OSC_BUFFER_SIZE];
osc::OutboundPacketStream p( buffer, OSC_BUFFER_SIZE );
p << osc::BeginBundleImmediate
<< osc::BeginMessage( "/sys/prefix" )
<< prefix.toUTF8()
<< osc::EndMessage
<< osc::EndBundle;
transmitSocket.Send( p.Data(), p.Size() );
}
int main(int argc, char* argv[]){
hid_device *handle;
unsigned char resbuf[8];
if(argc < 3){
usage();
return 1;
}
host = argv[1];
port = atoi(argv[2]);
printf("Opening ONTRAK device...\n");
handle = hid_open(VENDOR_ID, DEVICE_ID, NULL);
hid_set_nonblocking(handle, 0); //blocking mode ftw
printf("Device opened and set to blocking mode.");
UdpTransmitSocket transmitSocket( IpEndpointName( host, port ) );
while(true){
send_ontrak_command(handle);
int res = hid_read(handle, resbuf, 8);
if(res <= 0){
printf("ERROR reading\n");
}
else{
fprintf(stdout, ".");
fflush(stdout);
if(++dots > 80){
printf("\n");
dots = 0;
}
int state = atoi((const char*)resbuf+1);
// printf("%d\n", state);
if(state != last_state){
last_state = state;
send_osc(transmitSocket, state);
}
}
usleep(POLL_SLEEP_MS * 1000);
}
}
void SerialOscController::sendDeviceLedCommand(int x, int y, bool state)
{
HashMap<String, MonomeDevice*>::Iterator iter(devices);
while (iter.next()) {
auto device = iter.getValue();
UdpTransmitSocket transmitSocket( IpEndpointName( SERIALOSC_ADDRESS, device->port ) );
char buffer[OSC_BUFFER_SIZE];
osc::OutboundPacketStream p( buffer, OSC_BUFFER_SIZE );
String address = (device->prefix + "/grid/led/set");
p << osc::BeginBundleImmediate
<< osc::BeginMessage( address.toUTF8() )
<< x
<< y
<< (state ? 1 : 0)
<< osc::EndMessage
<< osc::EndBundle;
transmitSocket.Send( p.Data(), p.Size() );
}
}
int main(int argc, char* argv[])
{
signal(SIGINT, sigproc);
#ifndef WIN32
signal(SIGQUIT, sigproc);
#endif
bool noHelmet = false;
if((argc > 1) && (argv[1] == std::string("-n"))) noHelmet = true;
UdpTransmitSocket transmitSocket( IpEndpointName( ADDRESS, PORT ) );
char buffer[OUTPUT_BUFFER_SIZE];
emokit_device* d;
d = emokit_create();
printf("Current epoc devices connected: %d\n", emokit_get_count(d, EMOKIT_VID, EMOKIT_PID));
if(emokit_open(d, EMOKIT_VID, EMOKIT_PID, 0) != 0 && !noHelmet)
{
printf("CANNOT CONNECT\n");
return 1;
} else if(noHelmet) {
std::cout << "Sending random data" << std::endl;
}
if (!noHelmet) {
while(true)
{
if(emokit_read_data(d) > 0)
{
emokit_get_next_frame(d);
struct emokit_frame frame = d->current_frame;
std::cout << "\r\33[2K" << "gyroX: " << (int)frame.gyroX
<< "; gyroY: " << (int)frame.gyroY
<< "; F3: " << frame.F3
<< "; FC6: " << frame.FC6
<< "; battery: " << (int)d->battery << "%";
flush(std::cout);
osc::OutboundPacketStream channels( buffer, OUTPUT_BUFFER_SIZE );
osc::OutboundPacketStream gyro( buffer, OUTPUT_BUFFER_SIZE );
osc::OutboundPacketStream info( buffer, OUTPUT_BUFFER_SIZE );
channels << osc::BeginMessage( "/emokit/channels" )
<< frame.F3 << frame.FC6 << frame.P7 << frame.T8 << frame.F7 << frame.F8 << frame.T7 << frame.P8 << frame.AF4 << frame.F4 << frame.AF3 << frame.O2 << frame.O1 << frame.FC5 << osc::EndMessage;
transmitSocket.Send( channels.Data(), channels.Size() );
gyro << osc::BeginMessage( "/emokit/gyro" )
<< (int)frame.gyroX << (int)frame.gyroY << osc::EndMessage;
transmitSocket.Send( gyro.Data(), gyro.Size() );
info << osc::BeginMessage( "/emokit/info" )
<< (int)d->battery;
for (int i = 0; i<14 ; i++) info << (int)d->contact_quality[i];
info << osc::EndMessage;
transmitSocket.Send( info.Data(), info.Size() );
}
}
} else {
while (true) {
usleep(FREQ);
osc::OutboundPacketStream channels( buffer, OUTPUT_BUFFER_SIZE );
osc::OutboundPacketStream gyro( buffer, OUTPUT_BUFFER_SIZE );
osc::OutboundPacketStream info( buffer, OUTPUT_BUFFER_SIZE );
channels << osc::BeginMessage( "/emokit/channels" );
for (int i=0 ; i < 14 ; i++) channels << rand() % 10000;
channels << osc::EndMessage;
transmitSocket.Send( channels.Data(), channels.Size() );
gyro << osc::BeginMessage( "/emokit/gyro" )
<< rand() % 100 << rand() % 100 << osc::EndMessage;
transmitSocket.Send( gyro.Data(), gyro.Size() );
info << osc::BeginMessage( "/emokit/info" )
<< rand() % 100;
for (int i = 0; i<14 ; i++) info << rand() % 50;
info << osc::EndMessage;
transmitSocket.Send( info.Data(), info.Size() );
}
}
emokit_close(d);
emokit_delete(d);
return 0;
}
void * io_thread (void *arg) {
jack_thread_info_t *info = (jack_thread_info_t *) arg;
pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
pthread_mutex_lock(&io_thread_lock);
while (run) {
const int pkhld = ceilf(2.0 / ( (info->delay/1000000.0) + ((float)info->buffersize / (float)info->samplerate)));
if (info->can_capture) {
int chn;
memcpy(info->peak, info->pcur, sizeof(float)*info->channels);
for (chn = 0; chn < info->channels; ++chn) { info->pcur[chn]=0.0; }
if ((info->format&1)==0) {
if (flock(fileno(info->outfd), LOCK_EX)) continue; // pthread_cond_wait ?
fseek(info->outfd, 0L, SEEK_SET);
}
switch (info->format&6) {
case 6:
case 4:
fprintf(info->outfd,"{\"cnt\":%d,\"peak\":[", info->channels);
break;
default:
break;
}
for (chn = 0; chn < info->channels; ++chn) {
switch (info->format&6) {
case 0:
printf("%3.3f ", info->peak[chn]); break;
//printf("%s ", info->address); break;
case 2:
fprintf(info->outfd,"%3d ", peak_db(info->peak[chn], 1.0, info->iecmult)); break;
case 4:
printf("%.3f,", info->peak[chn]); break;
case 6:
fprintf(info->outfd,"%d,", peak_db(info->peak[chn], 1.0, info->iecmult)); break;
}
/* Insert OSC stuff here */
UdpTransmitSocket transmitSocket( IpEndpointName( info->address, info->port ) );
char buffer[OUTPUT_BUFFER_SIZE];
osc::OutboundPacketStream stream( buffer, OUTPUT_BUFFER_SIZE );
stream //<< osc::BeginBundleImmediate
<< osc::BeginMessage( info->message )
<< (float)info->peak[chn] << osc::EndMessage
;//<< osc::EndBundle;
transmitSocket.Send( stream.Data(), stream.Size() );
if (info->peak[chn] > info->pmax[chn]) {
info->pmax[chn] = info->peak[chn];
info->ptme[chn] = 0;
} else if (info->ptme[chn] <= pkhld) {
(info->ptme[chn])++;
} else {
info->pmax[chn] = info->peak[chn];
}
}
if (info->format&8) { // add peak-hold
if (info->format&4) {
fprintf(info->outfd,"],\"max\":[");
} else {
fprintf(info->outfd," | ");
}
for (chn = 0; chn < info->channels; ++chn) {
switch (info->format&6) {
case 0:
printf("%3.3f ", info->pmax[chn]); break;
case 2:
fprintf(info->outfd,"%3d ", peak_db(info->pmax[chn], 1.0, info->iecmult)); break;
case 4:
printf("%.3f,", info->pmax[chn]); break;
case 6:
fprintf(info->outfd,"%d,", peak_db(info->pmax[chn], 1.0, info->iecmult)); break;
}
}
}
switch (info->format&6) {
case 6:
case 4:
fprintf(info->outfd,"]}");
break;
default:
break;
}
if (info->format&1)
fprintf(info->outfd, "\r");
else {
ftruncate(fileno(info->outfd), ftell(info->outfd));
fprintf(info->outfd, "\n");
flock(fileno(info->outfd), LOCK_UN);
}
fflush(info->outfd);
if (info->delay>0) usleep(info->delay);
}
pthread_cond_wait(&data_ready, &io_thread_lock);
}
pthread_mutex_unlock(&io_thread_lock);
return 0;
}
void main()
{
// communication
UdpTransmitSocket transmitSocket(IpEndpointName( ADDRESS, PORT ));
char buffer[OUTPUT_BUFFER_SIZE];
osc::OutboundPacketStream PacketSender(buffer, OUTPUT_BUFFER_SIZE);
// tracking
windage::Logger logger(&std::cout);
IplImage* inputImage;
IplImage* resizeImage = cvCreateImage(cvSize(WIDTH, HEIGHT), IPL_DEPTH_8U, 3);
IplImage* grayImage = cvCreateImage(cvSize(WIDTH, HEIGHT), IPL_DEPTH_8U, 1);
IplImage* resultImage = cvCreateImage(cvSize(WIDTH, HEIGHT), IPL_DEPTH_8U, 3);
CvCapture* capture = cvCaptureFromCAM(CV_CAP_ANY);
cvNamedWindow("result");
// create and initialize tracker
double threshold = 50.0;
windage::Frameworks::MultiplePlanarObjectTracking tracking;
windage::Calibration* calibration;
windage::Algorithms::FeatureDetector* detector;
windage::Algorithms::OpticalFlow* opticalflow;
windage::Algorithms::HomographyEstimator* estimator;
windage::Algorithms::OutlierChecker* checker;
windage::Algorithms::HomographyRefiner* refiner;
calibration = new windage::Calibration();
detector = new windage::Algorithms::SIFTGPUdetector();
opticalflow = new windage::Algorithms::OpticalFlow();
estimator = new windage::Algorithms::ProSACestimator();
checker = new windage::Algorithms::OutlierChecker();
refiner = new windage::Algorithms::LMmethod();
calibration->Initialize(INTRINSIC[0], INTRINSIC[1], INTRINSIC[2], INTRINSIC[3], INTRINSIC[4], INTRINSIC[5], INTRINSIC[6], INTRINSIC[7]);
detector->SetThreshold(50.0);
opticalflow->Initialize(WIDTH, HEIGHT, cvSize(15, 15), 3);
estimator->SetReprojectionError(REPROJECTION_ERROR);
checker->SetReprojectionError(REPROJECTION_ERROR * 3);
refiner->SetMaxIteration(10);
tracking.AttatchCalibration(calibration);
tracking.AttatchDetetor(detector);
tracking.AttatchTracker(opticalflow);
tracking.AttatchEstimator(estimator);
tracking.AttatchChecker(checker);
tracking.AttatchRefiner(refiner);
tracking.Initialize(WIDTH, HEIGHT, (double)WIDTH, (double)HEIGHT);
tracking.SetFilter(false);
tracking.SetDitectionRatio(5);
bool trained = false;
#if USE_TEMPLATE_IMAEG
for(int i=0; i<TEMPLATE_IMAGE_COUNT; i++)
{
char message[100];
sprintf_s(message, TEMPLATE_IMAGE, i+1);
IplImage* sampleImage = cvLoadImage(message, 0);
detector->SetThreshold(30.0);
tracking.AttatchReferenceImage(sampleImage);
cvReleaseImage(&sampleImage);
}
tracking.TrainingReference(SCALE_FACTOR, SCALE_STEP);
trained = true;
detector->SetThreshold(threshold);
#endif
int keypointCount = 0;
int matchingCount = 0;
double processingTime = 0.0;
char message[100];
bool fliping = true;
bool processing = true;
while(processing)
{
// capture image
inputImage = cvRetrieveFrame(capture);
cvResize(inputImage, resizeImage);
if(fliping)
cvFlip(resizeImage, resizeImage);
cvCvtColor(resizeImage, grayImage, CV_BGR2GRAY);
cvCopyImage(resizeImage, resultImage);
logger.updateTickCount();
// track object
if(trained)
{
tracking.UpdateCamerapose(grayImage);
// tracking.GetDetector()->DrawKeypoints(resultImage);
// adaptive threshold
#if ADAPTIVE_THRESHOLD
int localcount = detector->GetKeypointsCount();
if(keypointCount != localcount)
{
if(localcount > FEATURE_COUNT)
threshold += 1;
if(localcount < FEATURE_COUNT)
threshold -= 1;
detector->SetThreshold(threshold);
keypointCount = localcount;
}
#endif
// draw result
std::vector<int> matchingCount; matchingCount.resize(tracking.GetObjectCount());
for(int i=0; i<tracking.GetObjectCount(); i++)
{
matchingCount[i] = tracking.GetMatchingCount(i);
if(tracking.GetMatchingCount(i) > 10)
{
// tracking.DrawDebugInfo(resultImage, i);
tracking.DrawOutLine(resultImage, i, true);
windage::Calibration* calibrationTemp = tracking.GetCameraParameter(i);
calibrationTemp->DrawInfomation(resultImage, 100);
CvPoint centerPoint = calibrationTemp->ConvertWorld2Image(0.0, 0.0, 0.0);
centerPoint.x += 5;
centerPoint.y += 10;
sprintf_s(message, "object #%d (%03d)", i+1, matchingCount[i]);
windage::Utils::DrawTextToImage(resultImage, centerPoint, 0.6, message);
}
}
// calcuate relation
if(tracking.GetMatchingCount(0) > 10 && tracking.GetMatchingCount(1) > 10)
{
windage::Matrix3 rotation = windage::Coordinator::MultiMarkerCoordinator::GetRotation(tracking.GetCameraParameter(0), tracking.GetCameraParameter(1));
windage::Vector3 translation = windage::Coordinator::MultiMarkerCoordinator::GetTranslation(tracking.GetCameraParameter(0), tracking.GetCameraParameter(1));
PacketSender << osc::BeginBundleImmediate << osc::BeginMessage("MultimarkerRelation")
<< rotation.m[0][0] << rotation.m[0][1] << rotation.m[0][2]
<< rotation.m[1][0] << rotation.m[1][1] << rotation.m[1][2]
<< rotation.m[2][0] << rotation.m[2][1] << rotation.m[2][2]
<< translation.x << translation.y << translation.z
<< osc::EndMessage << osc::EndBundle;
transmitSocket.Send( PacketSender.Data(), PacketSender.Size() );
PacketSender.Clear();
}
}
processingTime = logger.calculateProcessTime();
logger.log("processingTime", processingTime);
logger.logNewLine();
sprintf_s(message, "Processing Time : %.2lf ms", processingTime);
windage::Utils::DrawTextToImage(resultImage, cvPoint(10, 20), 0.6, message);
sprintf_s(message, "Feature Count : %d, Threshold : %.0lf", keypointCount, threshold);
windage::Utils::DrawTextToImage(resultImage, cvPoint(10, 40), 0.6, message);
sprintf_s(message, "Matching Count : %d", matchingCount);
windage::Utils::DrawTextToImage(resultImage, cvPoint(10, 60), 0.6, message);
sprintf_s(message, "Press 'Space' to add tracking the current image", keypointCount, threshold);
windage::Utils::DrawTextToImage(resultImage, cvPoint(WIDTH-315, HEIGHT-10), 0.5, message);
cvShowImage("result", resultImage);
char ch = cvWaitKey(1);
switch(ch)
{
case 'q':
case 'Q':
processing = false;
break;
case 'f':
case 'F':
fliping = !fliping;
break;
case ' ':
case 's':
case 'S':
detector->SetThreshold(30.0);
tracking.AttatchReferenceImage(grayImage);
tracking.TrainingReference(SCALE_FACTOR, SCALE_STEP);
detector->SetThreshold(threshold);
trained = true;
break;
}
}
cvReleaseCapture(&capture);
cvDestroyAllWindows();
}
int main(int argc, char* argv[])
{
(void) argc; // suppress unused parameter warnings
(void) argv; // suppress unused parameter warnings
struct sockaddr_in myaddr; /* our address */
struct sockaddr_in remaddr; /* remote address */
socklen_t addrlen = sizeof(remaddr); /* length of addresses */
int recvlen; /* # bytes received */
int fd; /* our socket */
unsigned char buf[BUFFER_SIZE]; /* receive buffer */
/* create a UDP socket */
if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
perror("cannot create socket\n");
return 0;
}
UdpTransmitSocket transmitSocket( IpEndpointName( SEND_ADDRESS, SEND_PORT ) );
// The buffer out
char buffer[BUFFER_SIZE];
osc::OutboundPacketStream p( buffer, BUFFER_SIZE );
/* bind the socket to any valid IP address and a specific port */
memset((char *)&myaddr, 0, sizeof(myaddr));
myaddr.sin_family = AF_INET;
myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
myaddr.sin_port = htons(RECEIVE_PORT);
if (bind(fd, (struct sockaddr *)&myaddr, sizeof(myaddr)) < 0) {
perror("bind failed");
return 0;
}
else{
printf("Listening to port %d\n", RECEIVE_PORT);
}
/* now loop, receiving data and printing what we received */
for (;;) {
recvlen = recvfrom(fd, buf, BUFFER_SIZE, 0, (struct sockaddr *)&remaddr, &addrlen);
if (recvlen > 0) {
buf[recvlen] = 0;
// retrieving the data from the udp message
std::string inMessage( buf, buf + sizeof buf / sizeof buf[0] ); // converting the input buffer to a string
// counting the number of semicolons to detect the number of messages in the packet
int nParams = std::count(inMessage.begin(), inMessage.end(), ';')-2;
for(int i=0; i<nParams; i++){
std::string oscAddress = OSC_BASE_ADDRESS;
oscAddress.append(inMessage.substr(0,inMessage.find(":")));
std::string oscValueString = inMessage.substr(inMessage.find(":")+1,inMessage.find(";")-inMessage.find(":")-1);
// making sure that the received message is valid
if(!std::all_of(oscValueString.begin(), oscValueString.end(), ::isdigit)){
float oscValue = stof(oscValueString);
//printf("Rec: %s: ",oscAddress.c_str());
//printf("%f\n", oscValue);
inMessage = inMessage.substr(inMessage.find("\n")+1,inMessage.find(";;")-inMessage.find("\n"));
// formating and sending the OSC message
p.Clear();
p << osc::BeginMessage(oscAddress.c_str())
<< oscValue << osc::EndMessage;
transmitSocket.Send( p.Data(), p.Size() );
}
else printf("Dropped message\n");
}
}
}
/* never exits */
}
int main(int argc, char* argv[])
{
signal(SIGINT, sigproc);
#ifndef WIN32
signal(SIGQUIT, sigproc);
#endif
UdpTransmitSocket transmitSocket( IpEndpointName( ADDRESS, PORT ) );
char buffer[OUTPUT_BUFFER_SIZE];
FILE *input;
FILE *output;
enum headset_type type;
char raw_frame[32];
struct epoc_frame frame;
epoc_device* d;
uint8_t data[32];
if (argc < 2)
{
fputs("Missing argument\nExpected: epocd [consumer|research|special]\n", stderr);
return 1;
}
if(strcmp(argv[1], "research") == 0)
type = RESEARCH_HEADSET;
else if(strcmp(argv[1], "consumer") == 0)
type = CONSUMER_HEADSET;
else if(strcmp(argv[1], "special") == 0)
type = SPECIAL_HEADSET;
else {
fputs("Bad headset type argument\nExpected: epocd [consumer|research|special] source [dest]\n", stderr);
return 1;
}
epoc_init(type);
d = epoc_create();
printf("Current epoc devices connected: %d\n", epoc_get_count(d, EPOC_VID, EPOC_PID));
if(epoc_open(d, EPOC_VID, EPOC_PID, 0) != 0)
{
printf("CANNOT CONNECT\n");
return 1;
}
while(1)
{
if(epoc_read_data(d, data) > 0)
{
epoc_get_next_frame(&frame, data);
osc::OutboundPacketStream p( buffer, OUTPUT_BUFFER_SIZE );
p << osc::BeginBundleImmediate
<< osc::BeginMessage( "/epoc/channels" )
<< frame.F3 << frame.FC6 << frame.P7 << frame.T8 << frame.F7 << frame.F8 << frame.T7 << frame.P8 << frame.AF4 << frame.F4 << frame.AF3 << frame.O2 << frame.O1 << frame.FC5 << osc::EndMessage
<< osc::BeginMessage( "/epoc/gyro" )
<< frame.gyroX << frame.gyroY << osc::EndMessage
<< osc::EndBundle;
transmitSocket.Send( p.Data(), p.Size() );
}
}
epoc_close(d);
epoc_delete(d);
return 0;
}
