OptimizationAlgorithm::SolverResult OptimizationAlgorithmGaussNewton::solve(int iteration, bool online)
{
assert(_optimizer && "_optimizer not set");
assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph");
bool ok = true;
if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure
ok = _solver->buildStructure();
if (! ok) {
cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl;
return OptimizationAlgorithm::Fail;
}
}
double t=get_monotonic_time();
_optimizer->computeActiveErrors();
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats) {
globalStats->timeResiduals = get_monotonic_time()-t;
t=get_monotonic_time();
}
_solver->buildSystem();
if (globalStats) {
globalStats->timeQuadraticForm = get_monotonic_time()-t;
t=get_monotonic_time();
}
ok = _solver->solve();
if (globalStats) {
globalStats->timeLinearSolution = get_monotonic_time()-t;
t=get_monotonic_time();
}
_optimizer->update(_solver->x());
if (globalStats) {
globalStats->timeUpdate = get_monotonic_time()-t;
}
if (ok)
return OK;
else
return Fail;
}
static bool
add_transaction_entry( transaction_entry *entry ) {
debug( "Adding a transaction entry ( entry = %p, transaction_db = %p ).", entry, transaction_db );
assert( entry != NULL );
assert( transaction_db != NULL );
assert( transaction_db->xid != NULL );
assert( transaction_db->barrier_xid != NULL );
bool ret = get_monotonic_time( &entry->expires_at );
if ( !ret ) {
return false;
}
ADD_TIMESPEC( &entry->expires_at, &TRANSACTION_TIMEOUT, &entry->expires_at );
dump_transaction_entry( debug, entry );
if ( lookup_transaction_entry_by_xid( entry->xid ) == NULL ) {
void *duplicated = insert_hash_entry( transaction_db->xid, &entry->xid, entry );
assert( duplicated == NULL );
}
else {
error( "Duplicated transaction entry found ( entry = %p, xid = %#x ).", entry, entry->xid );
dump_transaction_entry( error, entry );
return false;
}
if ( lookup_transaction_entry_by_barrier_xid( entry->barrier_xid ) == NULL ) {
void *duplicated = insert_hash_entry( transaction_db->barrier_xid, &entry->barrier_xid, entry );
assert( duplicated == NULL );
}
else {
error( "Duplicated transaction entry found ( entry = %p, barrier_xid = %#x ).", entry, entry->barrier_xid );
dump_transaction_entry( error, entry );
delete_transaction_entry_by_xid( entry->xid );
return false;
}
debug( "A transaction is added." );
dump_transaction_entry( debug, entry );
return true;
}
static void
handle_barrier_reply( uint64_t datapath_id, uint32_t transaction_id, void *user_data ) {
UNUSED( user_data );
debug( "Barrier reply from switch %#" PRIx64 " for %#x received.", datapath_id, transaction_id );
transaction_entry *entry = lookup_transaction_entry_by_barrier_xid( transaction_id );
if ( entry == NULL ) {
warn( "A barrier reply for untracked transaction received ( transaction_id = %#x, datapath_id = %#" PRIx64 " ).",
transaction_id, datapath_id );
return;
}
entry->completed = true;
bool ret = get_monotonic_time( &entry->expires_at );
if ( !ret ) {
return;
}
ADD_TIMESPEC( &entry->expires_at, &TRANSACTION_END_MARGIN, &entry->expires_at );
debug( "Set transaction timeout to %d.%09d.", ( int ) entry->expires_at.tv_sec, ( int ) entry->expires_at.tv_nsec );
}
double tictoc(const char* algorithmPart)
{
static TicTocInitializer initializer;
if (! initializer.enabled)
return 0.;
TicTocMap& tictocElements = initializer.tictocElements;
static double alpha = 0.01;
double now = get_monotonic_time();
double dt = 0.;
TicTocMap::iterator foundIt = tictocElements.find(algorithmPart);
if (foundIt == tictocElements.end()) {
// insert element
TicTocElement e;
e.ticTime = now;
e.algorithmPart = algorithmPart;
tictocElements[e.algorithmPart] = e;
} else {
if (foundIt->second.clockIsRunning) {
dt = now - foundIt->second.ticTime;
foundIt->second.totalTime += dt;
foundIt->second.minTime = std::min(foundIt->second.minTime, dt);
foundIt->second.maxTime = std::max(foundIt->second.maxTime, dt);
if (foundIt->second.numCalls == 0)
foundIt->second.exponentialMovingAverage = dt;
else
foundIt->second.exponentialMovingAverage = (1. - alpha) * foundIt->second.exponentialMovingAverage + alpha*dt;
foundIt->second.numCalls++;
} else {
foundIt->second.ticTime = now;
}
foundIt->second.clockIsRunning = !foundIt->second.clockIsRunning;
}
return dt;
}
int main(int argc, char *argv[]) {
int n, i, c, count = 0, sample = 0, chan = 0, status = 0, verbose = 0, labelSize;
unsigned char buf[OPENBCI_BUFLEN], byte;
char *labelString;
SerialPort SP;
host_t host;
struct timespec tic, toc;
/* these represent the general acquisition system properties */
int nchans = OPENBCI_NCHANS;
float fsample = OPENBCI_FSAMPLE;
/* these are used in the communication with the FT buffer and represent statefull information */
int ftSocket = -1;
ft_buffer_server_t *ftServer;
message_t *request = NULL;
message_t *response = NULL;
header_t *header = NULL;
data_t *data = NULL;
ft_chunkdef_t *label = NULL;
/* this contains the configuration details */
configuration config;
/* configure the default settings */
config.blocksize = 10;
config.port = 1972;
config.hostname = strdup("-");
config.serial = strdup("/dev/tty.usbserial-DN0094FY");
config.reset = strdup("on");
config.datalog = strdup("off");
config.testsignal = strdup("off");
config.timestamp = strdup("on");
config.timeref = strdup("start");
config.enable_chan1 = strdup("on");
config.enable_chan2 = strdup("on");
config.enable_chan3 = strdup("on");
config.enable_chan4 = strdup("on");
config.enable_chan5 = strdup("on");
config.enable_chan6 = strdup("on");
config.enable_chan7 = strdup("on");
config.enable_chan8 = strdup("on");
config.enable_chan9 = strdup("on");
config.enable_chan10 = strdup("on");
config.enable_chan11 = strdup("on");
config.label_chan1 = strdup("ADC1");
config.label_chan2 = strdup("ADC2");
config.label_chan3 = strdup("ADC3");
config.label_chan4 = strdup("ADC4");
config.label_chan5 = strdup("ADC5");
config.label_chan6 = strdup("ADC6");
config.label_chan7 = strdup("ADC7");
config.label_chan8 = strdup("ADC8");
config.label_chan9 = strdup("AccelerationX");
config.label_chan10 = strdup("AccelerationY");
config.label_chan11 = strdup("AccelerationZ");
config.label_chan12 = strdup("TimeStamp");
config.setting_chan1 = strdup("x1060110X");
config.setting_chan2 = strdup("x2060110X");
config.setting_chan3 = strdup("x3060110X");
config.setting_chan4 = strdup("x4060110X");
config.setting_chan5 = strdup("x5060110X");
config.setting_chan6 = strdup("x6060110X");
config.setting_chan7 = strdup("x7060110X");
config.setting_chan8 = strdup("x8060110X");
config.impedance_chan1 = strdup("z100Z");
config.impedance_chan2 = strdup("z200Z");
config.impedance_chan3 = strdup("z300Z");
config.impedance_chan4 = strdup("z400Z");
config.impedance_chan5 = strdup("z500Z");
config.impedance_chan6 = strdup("z600Z");
config.impedance_chan7 = strdup("z700Z");
config.impedance_chan8 = strdup("z800Z");
if (argc<2) {
printf(usage);
exit(0);
}
if (argc==2) {
if (strncmp(argv[1], "/dev", 4)==0 || strncasecmp(argv[1], "COM", 3)==0)
/* the second argument is the serial port */
config.serial = strdup(argv[1]);
else {
/* the second argument is the configuration file */
fprintf(stderr, "openbci2ft: loading configuration from '%s'\n", argv[1]);
if (ini_parse(argv[1], iniHandler, &config) < 0) {
fprintf(stderr, "Can't load '%s'\n", argv[1]);
return 1;
}
}
}
if (argc>2)
strcpy(host.name, argv[2]);
else {
strcpy(host.name, config.hostname);
}
if (argc>3)
host.port = atoi(argv[3]);
else {
host.port = config.port;
}
#define ISTRUE(s) strcasecmp(s, "on")==0
nchans = 0;
if (ISTRUE(config.enable_chan1))
nchans++;
if (ISTRUE(config.enable_chan2))
nchans++;
if (ISTRUE(config.enable_chan3))
nchans++;
if (ISTRUE(config.enable_chan4))
nchans++;
if (ISTRUE(config.enable_chan5))
nchans++;
if (ISTRUE(config.enable_chan6))
nchans++;
if (ISTRUE(config.enable_chan7))
nchans++;
if (ISTRUE(config.enable_chan8))
nchans++;
if (ISTRUE(config.enable_chan9))
nchans++;
if (ISTRUE(config.enable_chan10))
nchans++;
if (ISTRUE(config.enable_chan11))
nchans++;
if (ISTRUE(config.timestamp))
nchans++;
fprintf(stderr, "openbci2ft: serial = %s\n", config.serial);
fprintf(stderr, "openbci2ft: hostname = %s\n", host.name);
fprintf(stderr, "openbci2ft: port = %d\n", host.port);
fprintf(stderr, "openbci2ft: blocksize = %d\n", config.blocksize);
fprintf(stderr, "openbci2ft: reset = %s\n", config.reset);
fprintf(stderr, "openbci2ft: datalog = %s\n", config.datalog);
fprintf(stderr, "openbci2ft: timestamp = %s\n", config.timestamp);
fprintf(stderr, "openbci2ft: testsignal = %s\n", config.testsignal);
/* Spawn tcpserver or connect to remote buffer */
if (strcmp(host.name, "-") == 0) {
ftServer = ft_start_buffer_server(host.port, NULL, NULL, NULL);
if (ftServer==NULL) {
fprintf(stderr, "openbci2ft: could not start up a local buffer serving at port %i\n", host.port);
return 1;
}
ftSocket = 0;
printf("openbci2ft: streaming to local buffer on port %i\n", host.port);
}
else {
ftSocket = open_connection(host.name, host.port);
if (ftSocket < 0) {
fprintf(stderr, "openbci2ft: could not connect to remote buffer at %s:%i\n", host.name, host.port);
return 1;
}
printf("openbci2ft: streaming to remote buffer at %s:%i\n", host.name, host.port);
}
/* allocate the elements that will be used in the communication to the FT buffer */
request = malloc(sizeof(message_t));
request->def = malloc(sizeof(messagedef_t));
request->buf = NULL;
request->def->version = VERSION;
request->def->bufsize = 0;
header = malloc(sizeof(header_t));
header->def = malloc(sizeof(headerdef_t));
header->buf = NULL;
data = malloc(sizeof(data_t));
data->def = malloc(sizeof(datadef_t));
data->buf = NULL;
/* define the header */
header->def->nchans = nchans;
header->def->fsample = fsample;
header->def->nsamples = 0;
header->def->nevents = 0;
header->def->data_type = DATATYPE_FLOAT32;
header->def->bufsize = 0;
/* FIXME add the channel names */
labelSize = 0; /* count the number of bytes required */
if (ISTRUE (config.enable_chan1))
labelSize += strlen (config.label_chan1) + 1;
if (ISTRUE (config.enable_chan2))
labelSize += strlen (config.label_chan2) + 1;
if (ISTRUE (config.enable_chan3))
labelSize += strlen (config.label_chan3) + 1;
if (ISTRUE (config.enable_chan4))
labelSize += strlen (config.label_chan4) + 1;
if (ISTRUE (config.enable_chan5))
labelSize += strlen (config.label_chan5) + 1;
if (ISTRUE (config.enable_chan6))
labelSize += strlen (config.label_chan6) + 1;
if (ISTRUE (config.enable_chan7))
labelSize += strlen (config.label_chan7) + 1;
if (ISTRUE (config.enable_chan8))
labelSize += strlen (config.label_chan8) + 1;
if (ISTRUE (config.enable_chan9))
labelSize += strlen (config.label_chan9) + 1;
if (ISTRUE (config.enable_chan10))
labelSize += strlen (config.label_chan10) + 1;
if (ISTRUE (config.enable_chan11))
labelSize += strlen (config.label_chan11) + 1;
if (ISTRUE (config.timestamp))
labelSize += strlen (config.label_chan12) + 1;
if (verbose > 0)
fprintf (stderr, "openbci2ft: labelSize = %d\n", labelSize);
/* go over all channels for a 2nd time, now copying the strings to the destination */
labelString = (char *) malloc (labelSize * sizeof(char));
labelSize = 0;
if (ISTRUE (config.enable_chan1)) {
strcpy (labelString+labelSize, config.label_chan1);
labelSize += strlen (config.label_chan1) + 1;
}
if (ISTRUE (config.enable_chan2)) {
strcpy (labelString+labelSize, config.label_chan2);
labelSize += strlen (config.label_chan2) + 1;
}
if (ISTRUE (config.enable_chan3)) {
strcpy (labelString+labelSize, config.label_chan3);
labelSize += strlen (config.label_chan3) + 1;
}
if (ISTRUE (config.enable_chan4)) {
strcpy (labelString+labelSize, config.label_chan4);
labelSize += strlen (config.label_chan4) + 1;
}
if (ISTRUE (config.enable_chan5)) {
strcpy (labelString+labelSize, config.label_chan5);
labelSize += strlen (config.label_chan5) + 1;
}
if (ISTRUE (config.enable_chan6)) {
strcpy (labelString+labelSize, config.label_chan6);
labelSize += strlen (config.label_chan6) + 1;
}
if (ISTRUE (config.enable_chan7)) {
strcpy (labelString+labelSize, config.label_chan7);
labelSize += strlen (config.label_chan7) + 1;
}
if (ISTRUE (config.enable_chan8)) {
strcpy (labelString+labelSize, config.label_chan8);
labelSize += strlen (config.label_chan8) + 1;
}
if (ISTRUE (config.enable_chan9)) {
strcpy (labelString+labelSize, config.label_chan9);
labelSize += strlen (config.label_chan9) + 1;
}
if (ISTRUE (config.enable_chan10)) {
strcpy (labelString+labelSize, config.label_chan10);
labelSize += strlen (config.label_chan10) + 1;
}
if (ISTRUE (config.enable_chan11)) {
strcpy (labelString+labelSize, config.label_chan11);
labelSize += strlen (config.label_chan11) + 1;
}
if (ISTRUE (config.timestamp)) {
strcpy (labelString+labelSize, config.label_chan12);
labelSize += strlen (config.label_chan12) + 1;
}
/* add the channel label chunk to the header */
label = (ft_chunkdef_t *) malloc (sizeof (ft_chunkdef_t));
label->type = FT_CHUNK_CHANNEL_NAMES;
label->size = labelSize;
header->def->bufsize = append (&header->buf, header->def->bufsize, label, sizeof (ft_chunkdef_t));
header->def->bufsize = append (&header->buf, header->def->bufsize, labelString, labelSize);
FREE (label);
FREE (labelString);
/* define the constant part of the data and allocate space for the variable part */
data->def->nchans = nchans;
data->def->nsamples = config.blocksize;
data->def->data_type = DATATYPE_FLOAT32;
data->def->bufsize = WORDSIZE_FLOAT32 * nchans * config.blocksize;
data->buf = malloc (data->def->bufsize);
/* initialization phase, send the header */
request->def->command = PUT_HDR;
request->def->bufsize = append (&request->buf, request->def->bufsize, header->def, sizeof (headerdef_t));
request->def->bufsize = append (&request->buf, request->def->bufsize, header->buf, header->def->bufsize);
/* this is not needed any more */
cleanup_header (&header);
status = clientrequest (ftSocket, request, &response);
if (verbose > 0)
fprintf (stderr, "openbci2ft: clientrequest returned %d\n", status);
if (status)
{
fprintf (stderr, "openbci2ft: could not send request to buffer\n");
exit (1);
}
if (status || response == NULL || response->def == NULL)
{
fprintf (stderr, "openbci2ft: error in %s on line %d\n", __FILE__,
__LINE__);
exit (1);
}
cleanup_message (&request);
if (response->def->command != PUT_OK)
{
fprintf (stderr, "openbci2ft: error in 'put header' request.\n");
exit (1);
}
cleanup_message (&response);
/* open the serial port */
fprintf (stderr, "openbci2ft: opening serial port ...\n");
if (!serialOpenByName (&SP, config.serial))
{
fprintf (stderr, "Could not open serial port %s\n", config.serial);
return 1;
}
if (!serialSetParameters (&SP, 115200, 8, 0, 0, 0))
{
fprintf (stderr, "Could not modify serial port parameters\n");
return 1;
}
fprintf (stderr, "openbci2ft: opening serial port ... ok\n");
/* 8-bit board will always be initialized upon opening serial port, 32-bit board needs explicit initialization */
fprintf (stderr, "openbci2ft: initializing ...\n");
fprintf (stderr,
"openbci2ft: press reset on the OpenBCI board if this takes too long\n");
if (ISTRUE (config.reset))
serialWrite (&SP, 1, "v"); /* soft reset, this will return $$$ */
else
serialWrite (&SP, 1, "D"); /* query default channel settings, this will also return $$$ */
/* wait for '$$$' which indicates that the OpenBCI has been initialized */
c = 0;
while (c != 3)
{
usleep (1000);
n = serialRead (&SP, 1, &byte);
if (n == 1)
{
if (byte == '$')
c++;
else
c = 0;
}
} /* while waiting for '$$$' */
if (strcasecmp (config.datalog, "14s") == 0)
serialWrite (&SP, 1, "a");
else if (strcasecmp (config.datalog, "5min") == 0)
serialWrite (&SP, 1, "A");
else if (strcasecmp (config.datalog, "15min") == 0)
serialWrite (&SP, 1, "S");
else if (strcasecmp (config.datalog, "30min") == 0)
serialWrite (&SP, 1, "F");
else if (strcasecmp (config.datalog, "1hr") == 0)
serialWrite (&SP, 1, "G");
else if (strcasecmp (config.datalog, "2hr") == 0)
serialWrite (&SP, 1, "H");
else if (strcasecmp (config.datalog, "4hr") == 0)
serialWrite (&SP, 1, "J");
else if (strcasecmp (config.datalog, "12hr") == 0)
serialWrite (&SP, 1, "K");
else if (strcasecmp (config.datalog, "24hr") == 0)
serialWrite (&SP, 1, "L");
else if (strcasecmp (config.datalog, "off") != 0)
{
fprintf (stderr, "Incorrect specification of datalog\n");
return 1;
}
serialWriteSlow (&SP, strlen (config.setting_chan1), config.setting_chan1);
serialWriteSlow (&SP, strlen (config.setting_chan2), config.setting_chan2);
serialWriteSlow (&SP, strlen (config.setting_chan3), config.setting_chan3);
serialWriteSlow (&SP, strlen (config.setting_chan4), config.setting_chan4);
serialWriteSlow (&SP, strlen (config.setting_chan5), config.setting_chan5);
serialWriteSlow (&SP, strlen (config.setting_chan6), config.setting_chan6);
serialWriteSlow (&SP, strlen (config.setting_chan7), config.setting_chan7);
serialWriteSlow (&SP, strlen (config.setting_chan8), config.setting_chan8);
if (strcasecmp (config.testsignal, "gnd") == 0)
serialWrite (&SP, 1, "0");
else if (strcasecmp (config.testsignal, "dc") == 0)
serialWrite (&SP, 1, "-");
else if (strcasecmp (config.testsignal, "1xSlow") == 0)
serialWrite (&SP, 1, "=");
else if (strcasecmp (config.testsignal, "1xFast") == 0)
serialWrite (&SP, 1, "p");
else if (strcasecmp (config.testsignal, "2xSlow") == 0)
serialWrite (&SP, 1, "[");
else if (strcasecmp (config.testsignal, "2xFast") == 0)
serialWrite (&SP, 1, "]");
else if (strcasecmp (config.testsignal, "off") != 0)
{
fprintf (stderr, "Incorrect specification of testsignal\n");
return 1;
}
fprintf (stderr, "openbci2ft: initializing ... ok\n");
printf ("Starting to listen - press CTRL-C to quit\n");
/* register CTRL-C handler */
signal (SIGINT, abortHandler);
/* start streaming data */
serialWrite (&SP, 1, "b");
/* determine the reference time for the timestamps */
if (strcasecmp (config.timeref, "start") == 0)
{
/* since the start of the acquisition */
get_monotonic_time (&tic, TIMESTAMP_REF_BOOT);
}
else if (strcasecmp (config.timeref, "boot") == 0)
{
/* since the start of the day */
tic.tv_sec = 0;
tic.tv_nsec = 0;
}
else if (strcasecmp (config.timeref, "epoch") == 0)
{
/* since the start of the epoch, i.e. 1-1-1970 */
tic.tv_sec = 0;
tic.tv_nsec = 0;
}
else
{
fprintf (stderr,
"Incorrect specification of timeref, should be 'start', 'day' or 'epoch'\n");
return 1;
}
while (keepRunning)
{
sample = 0;
while (sample < config.blocksize)
{
/* wait for the first byte of the following packet */
buf[0] = 0;
while (buf[0] != 0xA0)
{
if (serialInputPending (&SP))
n = serialRead (&SP, 1, buf);
else
usleep (1000);
} /* while */
/*
* Header
* Byte 1: 0xA0
* Byte 2: Sample Number
*
* EEG Data
* Note: values are 24-bit signed, MSB first
* Bytes 3-5: Data value for EEG channel 1
* Bytes 6-8: Data value for EEG channel 2
* Bytes 9-11: Data value for EEG channel 3
* Bytes 12-14: Data value for EEG channel 4
* Bytes 15-17: Data value for EEG channel 5
* Bytes 18-20: Data value for EEG channel 6
* Bytes 21-23: Data value for EEG channel 6
* Bytes 24-26: Data value for EEG channel 8
*
* Accelerometer Data
* Note: values are 16-bit signed, MSB first
* Bytes 27-28: Data value for accelerometer channel X
* Bytes 29-30: Data value for accelerometer channel Y
* Bytes 31-32: Data value for accelerometer channel Z
*
* Footer
* Byte 33: 0xC0
*/
/* read the remaining 32 bytes of the packet */
while (n < OPENBCI_BUFLEN)
if (serialInputPending (&SP))
n += serialRead (&SP, (OPENBCI_BUFLEN - n), buf + n);
else
usleep (1000);
if (verbose > 1)
{
for (i = 0; i < OPENBCI_BUFLEN; i++)
printf ("%02x ", buf[i]);
printf ("\n");
}
chan = 0;
if (ISTRUE (config.enable_chan1))
((FLOAT32_T *) (data->buf))[nchans * sample + (chan++)] =
OPENBCI_CALIB1 * (buf[2] << 24 | buf[3] << 16 | buf[4] << 8) /
255;
if (ISTRUE (config.enable_chan2))
((FLOAT32_T *) (data->buf))[nchans * sample + (chan++)] =
OPENBCI_CALIB1 * (buf[5] << 24 | buf[6] << 16 | buf[7] << 8) /
255;
if (ISTRUE (config.enable_chan3))
((FLOAT32_T *) (data->buf))[nchans * sample + (chan++)] =
OPENBCI_CALIB1 * (buf[8] << 24 | buf[9] << 16 | buf[10] << 8) /
255;
if (ISTRUE (config.enable_chan4))
((FLOAT32_T *) (data->buf))[nchans * sample + (chan++)] =
OPENBCI_CALIB1 * (buf[11] << 24 | buf[12] << 16 | buf[13] << 8) /
255;
if (ISTRUE (config.enable_chan5))
((FLOAT32_T *) (data->buf))[nchans * sample + (chan++)] =
OPENBCI_CALIB1 * (buf[14] << 24 | buf[15] << 16 | buf[16] << 8) /
255;
if (ISTRUE (config.enable_chan6))
((FLOAT32_T *) (data->buf))[nchans * sample + (chan++)] =
OPENBCI_CALIB1 * (buf[17] << 24 | buf[18] << 16 | buf[19] << 8) /
255;
if (ISTRUE (config.enable_chan7))
((FLOAT32_T *) (data->buf))[nchans * sample + (chan++)] =
OPENBCI_CALIB1 * (buf[20] << 24 | buf[21] << 16 | buf[22] << 8) /
255;
if (ISTRUE (config.enable_chan8))
((FLOAT32_T *) (data->buf))[nchans * sample + (chan++)] =
OPENBCI_CALIB1 * (buf[23] << 24 | buf[24] << 16 | buf[25] << 8) /
255;
if (ISTRUE (config.enable_chan9))
((FLOAT32_T *) (data->buf))[nchans * sample + (chan++)] =
OPENBCI_CALIB2 * (buf[26] << 24 | buf[27] << 16) / 32767;
if (ISTRUE (config.enable_chan10))
((FLOAT32_T *) (data->buf))[nchans * sample + (chan++)] =
OPENBCI_CALIB2 * (buf[28] << 24 | buf[29] << 16) / 32767;
if (ISTRUE (config.enable_chan11))
((FLOAT32_T *) (data->buf))[nchans * sample + (chan++)] =
OPENBCI_CALIB2 * (buf[28] << 24 | buf[31] << 16) / 32767;
if (ISTRUE (config.timestamp))
{
if (strcasecmp (config.timeref, "start") == 0)
get_monotonic_time (&toc, TIMESTAMP_REF_BOOT);
else if (strcasecmp (config.timeref, "boot") == 0)
get_monotonic_time (&toc, TIMESTAMP_REF_BOOT);
else if (strcasecmp (config.timeref, "epoch") == 0)
get_monotonic_time (&toc, TIMESTAMP_REF_EPOCH);
((FLOAT32_T *) (data->buf))[nchans * sample + (chan++)] =
get_elapsed_time (&tic, &toc);
}
sample++;
} /* while c<config.blocksize */
count += sample;
printf ("openbci2ft: sample count = %i\n", count);
/* create the request */
request = malloc (sizeof (message_t));
request->def = malloc (sizeof (messagedef_t));
request->buf = NULL;
request->def->version = VERSION;
request->def->bufsize = 0;
request->def->command = PUT_DAT;
request->def->bufsize = append (&request->buf, request->def->bufsize, data->def, sizeof (datadef_t));
request->def->bufsize = append (&request->buf, request->def->bufsize, data->buf, data->def->bufsize);
status = clientrequest (ftSocket, request, &response);
if (verbose > 0)
fprintf (stderr, "openbci2ft: clientrequest returned %d\n", status);
if (status)
{
fprintf (stderr, "openbci2ft: error in %s on line %d\n", __FILE__,
__LINE__);
exit (1);
}
if (status)
{
fprintf (stderr, "openbci2ft: error in %s on line %d\n", __FILE__,
__LINE__);
exit (1);
}
/* FIXME do someting with the response, i.e. check that it is OK */
cleanup_message (&request);
if (response == NULL || response->def == NULL
|| response->def->command != PUT_OK)
{
fprintf (stderr, "Error when writing samples.\n");
}
cleanup_message (&response);
} /* while keepRunning */
/* stop streaming data */
serialWrite (&SP, 1, "s");
cleanup_data (&data);
if (ftSocket > 0)
{
close_connection (ftSocket);
}
else
{
ft_stop_buffer_server (ftServer);
}
return 0;
} /* main */
OptimizationAlgorithm::SolverResult OptimizationAlgorithmLevenberg::solve(int iteration, bool online)
{
assert(_optimizer && "_optimizer not set");
assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph");
if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure
bool ok = _solver->buildStructure();
if (! ok) {
cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl;
return OptimizationAlgorithm::Fail;
}
}
double t=get_monotonic_time();
_optimizer->computeActiveErrors();
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats) {
globalStats->timeResiduals = get_monotonic_time()-t;
t=get_monotonic_time();
}
double currentChi = _optimizer->activeRobustChi2();
double tempChi=currentChi;
_solver->buildSystem();
if (globalStats) {
globalStats->timeQuadraticForm = get_monotonic_time()-t;
}
// core part of the Levenbarg algorithm
if (iteration == 0) {
_currentLambda = computeLambdaInit();
_ni = 2;
}
double rho=0;
int& qmax = _levenbergIterations;
qmax = 0;
do {
_optimizer->push();
if (globalStats) {
globalStats->levenbergIterations++;
t=get_monotonic_time();
}
// update the diagonal of the system matrix
_solver->setLambda(_currentLambda);
bool ok2 = _solver->solve();
if (globalStats) {
globalStats->timeLinearSolution+=get_monotonic_time()-t;
t=get_monotonic_time();
}
_optimizer->update(_solver->x());
if (globalStats) {
globalStats->timeUpdate = get_monotonic_time()-t;
}
// restore the diagonal
_solver->setLambda(- _currentLambda);
_optimizer->computeActiveErrors();
tempChi = _optimizer->activeRobustChi2();
if (! ok2)
tempChi=std::numeric_limits<double>::max();
rho = (currentChi-tempChi);
double scale = computeScale();
scale += 1e-3; // make sure it's non-zero :)
rho /= scale;
if (rho>0 && g2o_isfinite(tempChi)){ // last step was good
double alpha = 1.-pow((2*rho-1),3);
// crop lambda between minimum and maximum factors
alpha = (std::min)(alpha, _goodStepUpperScale);
double scaleFactor = (std::max)(_goodStepLowerScale, alpha);
_currentLambda *= scaleFactor;
_ni = 2;
currentChi=tempChi;
_optimizer->discardTop();
} else {
_currentLambda*=_ni;
_ni*=2;
_optimizer->pop(); // restore the last state before trying to optimize
}
qmax++;
} while (rho<0 && qmax < _maxTrialsAfterFailure->value() && ! _optimizer->terminate());
if (qmax == _maxTrialsAfterFailure->value() || rho==0)
return Terminate;
return OK;
}
bool BlockSolver<Traits>::solve(){
//cerr << __PRETTY_FUNCTION__ << endl;
if (! _doSchur){
double t=get_monotonic_time();
bool ok = _linearSolver->solve(*_Hpp, _x, _b);
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats) {
globalStats->timeLinearSolver = get_monotonic_time() - t;
globalStats->hessianDimension = globalStats->hessianPoseDimension = _Hpp->cols();
}
return ok;
}
// schur thing
// backup the coefficient matrix
double t=get_monotonic_time();
// _Hschur = _Hpp, but keeping the pattern of _Hschur
_Hschur->clear();
_Hpp->add(_Hschur);
//_DInvSchur->clear();
memset (_coefficients, 0, _sizePoses*sizeof(double));
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) schedule(dynamic, 10)
# endif
for (int landmarkIndex = 0; landmarkIndex < static_cast<int>(_Hll->blockCols().size()); ++landmarkIndex) {
const typename SparseBlockMatrix<LandmarkMatrixType>::IntBlockMap& marginalizeColumn = _Hll->blockCols()[landmarkIndex];
assert(marginalizeColumn.size() == 1 && "more than one block in _Hll column");
// calculate inverse block for the landmark
const LandmarkMatrixType * D = marginalizeColumn.begin()->second;
assert (D && D->rows()==D->cols() && "Error in landmark matrix");
LandmarkMatrixType& Dinv = _DInvSchur->diagonal()[landmarkIndex];
Dinv = D->inverse();
LandmarkVectorType db(D->rows());
for (int j=0; j<D->rows(); ++j) {
db[j]=_b[_Hll->rowBaseOfBlock(landmarkIndex) + _sizePoses + j];
}
db=Dinv*db;
assert((size_t)landmarkIndex < _HplCCS->blockCols().size() && "Index out of bounds");
const typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn& landmarkColumn = _HplCCS->blockCols()[landmarkIndex];
for (typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn::const_iterator it_outer = landmarkColumn.begin();
it_outer != landmarkColumn.end(); ++it_outer) {
int i1 = it_outer->row;
const PoseLandmarkMatrixType* Bi = it_outer->block;
assert(Bi);
PoseLandmarkMatrixType BDinv = (*Bi)*(Dinv);
assert(_HplCCS->rowBaseOfBlock(i1) < _sizePoses && "Index out of bounds");
typename PoseVectorType::MapType Bb(&_coefficients[_HplCCS->rowBaseOfBlock(i1)], Bi->rows());
# ifdef G2O_OPENMP
ScopedOpenMPMutex mutexLock(&_coefficientsMutex[i1]);
# endif
Bb.noalias() += (*Bi)*db;
assert(i1 >= 0 && i1 < static_cast<int>(_HschurTransposedCCS->blockCols().size()) && "Index out of bounds");
typename SparseBlockMatrixCCS<PoseMatrixType>::SparseColumn::iterator targetColumnIt = _HschurTransposedCCS->blockCols()[i1].begin();
typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::RowBlock aux(i1, 0);
typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn::const_iterator it_inner = lower_bound(landmarkColumn.begin(), landmarkColumn.end(), aux);
for (; it_inner != landmarkColumn.end(); ++it_inner) {
int i2 = it_inner->row;
const PoseLandmarkMatrixType* Bj = it_inner->block;
assert(Bj);
while (targetColumnIt->row < i2 /*&& targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end()*/)
++targetColumnIt;
assert(targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end() && targetColumnIt->row == i2 && "invalid iterator, something wrong with the matrix structure");
PoseMatrixType* Hi1i2 = targetColumnIt->block;//_Hschur->block(i1,i2);
assert(Hi1i2);
(*Hi1i2).noalias() -= BDinv*Bj->transpose();
}
}
}
//cerr << "Solve [marginalize] = " << get_monotonic_time()-t << endl;
// _bschur = _b for calling solver, and not touching _b
memcpy(_bschur, _b, _sizePoses * sizeof(double));
for (int i=0; i<_sizePoses; ++i){
_bschur[i]-=_coefficients[i];
}
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats){
globalStats->timeSchurComplement = get_monotonic_time() - t;
}
t=get_monotonic_time();
bool solvedPoses = _linearSolver->solve(*_Hschur, _x, _bschur);
if (globalStats) {
globalStats->timeLinearSolver = get_monotonic_time() - t;
globalStats->hessianPoseDimension = _Hpp->cols();
globalStats->hessianLandmarkDimension = _Hll->cols();
globalStats->hessianDimension = globalStats->hessianPoseDimension + globalStats->hessianLandmarkDimension;
}
//cerr << "Solve [decompose and solve] = " << get_monotonic_time()-t << endl;
if (! solvedPoses)
return false;
// _x contains the solution for the poses, now applying it to the landmarks to get the new part of the
// solution;
double* xp = _x;
double* cp = _coefficients;
double* xl=_x+_sizePoses;
double* cl=_coefficients + _sizePoses;
double* bl=_b+_sizePoses;
// cp = -xp
for (int i=0; i<_sizePoses; ++i)
cp[i]=-xp[i];
// cl = bl
memcpy(cl,bl,_sizeLandmarks*sizeof(double));
// cl = bl - Bt * xp
//Bt->multiply(cl, cp);
_HplCCS->rightMultiply(cl, cp);
// xl = Dinv * cl
memset(xl,0, _sizeLandmarks*sizeof(double));
_DInvSchur->multiply(xl,cl);
//_DInvSchur->rightMultiply(xl,cl);
//cerr << "Solve [landmark delta] = " << get_monotonic_time()-t << endl;
return true;
}
static void
age_transaction_entries( void *user_data ) {
UNUSED( user_data );
debug( "Aging transaction entries ( %p ).", transaction_db );
assert( transaction_db != NULL );
assert( transaction_db->xid != NULL );
struct timespec now;
bool ret = get_monotonic_time( &now );
if ( !ret ) {
return;
}
hash_entry *e;
hash_iterator iter;
init_hash_iterator( transaction_db->xid, &iter );
while ( ( e = iterate_hash_next( &iter ) ) != NULL ) {
transaction_entry *entry = e->value;
if( entry == NULL ) {
continue;
}
debug( "Checking an entry ( %p ).", entry );
dump_transaction_entry( debug, entry );
if ( TIMESPEC_LESS_THAN( &entry->expires_at, &now ) ) {
struct ofp_header *header = entry->message->data;
header->xid = htonl( entry->original_xid );
if ( entry->completed ) {
if ( !entry->error_received && entry->succeeded_callback != NULL ) {
debug( "Calling succeeded callback ( %p ).", entry->succeeded_callback );
entry->succeeded_callback( entry->datapath_id, entry->message, entry->succeeded_user_data );
}
else if ( entry->error_received && entry->failed_callback != NULL ) {
debug( "Calling failed callback ( %p ).", entry->failed_callback );
entry->failed_callback( entry->datapath_id, entry->message, entry->failed_user_data );
}
}
else {
if ( !entry->timeout ) {
debug( "Transaction timeout. Wait for final marin to elapse." );
// Wait for a moment after deleting all messages in send queue
delete_openflow_messages( entry->datapath_id );
entry->expires_at = now;
ADD_TIMESPEC( &entry->expires_at, &TRANSACTION_END_MARGIN, &entry->expires_at );
entry->timeout = true;
continue;
}
else {
warn( "Transaction timeout ( xid = %#x, barrier_xid = %#x, original_xid = %#x, expires_at = %d.%09d ).",
entry->xid, entry->barrier_xid, entry->original_xid,
( int ) entry->expires_at.tv_sec, ( int ) entry->expires_at.tv_nsec );
if ( entry->failed_callback != NULL ) {
entry->failed_callback( entry->datapath_id, entry->message, entry->failed_user_data );
}
}
}
delete_transaction_entry_by_xid( entry->xid );
free_transaction_entry( entry );
}
}
debug( "Aging completed ( %p ).", transaction_db );
}
OptimizationAlgorithm::SolverResult OptimizationAlgorithmDogleg::solve(int iteration, bool online)
{
assert(_optimizer && "_optimizer not set");
assert(_solver.optimizer() == _optimizer && "underlying linear solver operates on different graph");
BlockSolverBase& blockSolver = static_cast<BlockSolverBase&>(_solver);
if (iteration == 0 && !online)
{ // built up the CCS structure, here due to easy time measure
bool ok = _solver.buildStructure();
if (! ok) {
cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl;
return OptimizationAlgorithm::Fail;
}
// init some members to the current size of the problem
_hsd.resize(_solver.vectorSize());
_hdl.resize(_solver.vectorSize());
_auxVector.resize(_solver.vectorSize());
_delta = _userDeltaInit->value();
_currentLambda = _initialLambda->value();
_wasPDInAllIterations = true;
}
double t=get_monotonic_time();
_optimizer->computeActiveErrors();
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats) {
globalStats->timeResiduals = get_monotonic_time()-t;
t=get_monotonic_time();
}
double currentChi = _optimizer->activeRobustChi2();
_solver.buildSystem();
if (globalStats) {
globalStats->timeQuadraticForm = get_monotonic_time()-t;
}
VectorXD::ConstMapType b(_solver.b(), _solver.vectorSize());
// compute alpha
_auxVector.setZero();
blockSolver.multiplyHessian(_auxVector.data(), _solver.b());
double bNormSquared = b.squaredNorm();
double alpha = bNormSquared / _auxVector.dot(b);
_hsd = alpha * b;
double hsdNorm = _hsd.norm();
double hgnNorm = -1.;
bool solvedGaussNewton = false;
bool goodStep = false;
int& numTries = _lastNumTries;
numTries = 0;
do {
++numTries;
if (! solvedGaussNewton) {
const double minLambda = 1e-12;
const double maxLambda = 1e3;
solvedGaussNewton = true;
// apply a damping factor to enforce positive definite Hessian, if the matrix appeared
// to be not positive definite in at least one iteration before.
// We apply a damping factor to obtain a PD matrix.
bool solverOk = false;
while(!solverOk)
{
if (! _wasPDInAllIterations)
_solver.setLambda(_currentLambda, true); // add _currentLambda to the diagonal
solverOk = _solver.solve();
if (! _wasPDInAllIterations)
_solver.restoreDiagonal();
_wasPDInAllIterations = _wasPDInAllIterations && solverOk;
if (! _wasPDInAllIterations) {
// simple strategy to control the damping factor
if (solverOk) {
_currentLambda = std::max(minLambda, _currentLambda / (0.5 * _lamdbaFactor->value()));
} else {
_currentLambda *= _lamdbaFactor->value();
if (_currentLambda > maxLambda) {
_currentLambda = maxLambda;
return Fail;
}
}
}
}
if (!solverOk) {
return Fail;
}
hgnNorm = VectorXD::ConstMapType(_solver.x(), _solver.vectorSize()).norm();
}
VectorXD::ConstMapType hgn(_solver.x(), _solver.vectorSize());
assert(hgnNorm >= 0. && "Norm of the GN step is not computed");
if (hgnNorm < _delta) {
_hdl = hgn;
_lastStep = STEP_GN;
}
else if (hsdNorm > _delta) {
_hdl = _delta / hsdNorm * _hsd;
_lastStep = STEP_SD;
} else {
_auxVector = hgn - _hsd; // b - a
double c = _hsd.dot(_auxVector);
double bmaSquaredNorm = _auxVector.squaredNorm();
double beta;
if (c <= 0.)
beta = (-c + sqrt(c*c + bmaSquaredNorm * (_delta*_delta - _hsd.squaredNorm()))) / bmaSquaredNorm;
else {
double hsdSqrNorm = _hsd.squaredNorm();
beta = (_delta*_delta - hsdSqrNorm) / (c + sqrt(c*c + bmaSquaredNorm * (_delta*_delta - hsdSqrNorm)));
}
assert(beta > 0. && beta < 1 && "Error while computing beta");
_hdl = _hsd + beta * (hgn - _hsd);
_lastStep = STEP_DL;
assert(_hdl.norm() < _delta + 1e-5 && "Computed step does not correspond to the trust region");
}
// compute the linear gain
_auxVector.setZero();
blockSolver.multiplyHessian(_auxVector.data(), _hdl.data());
double linearGain = -1 * (_auxVector.dot(_hdl)) + 2 * (b.dot(_hdl));
// apply the update and see what happens
_optimizer->push();
_optimizer->update(_hdl.data());
_optimizer->computeActiveErrors();
double newChi = _optimizer-> activeRobustChi2();
double nonLinearGain = currentChi - newChi;
if (fabs(linearGain) < 1e-12)
linearGain = 1e-12;
double rho = nonLinearGain / linearGain;
//cerr << PVAR(nonLinearGain) << " " << PVAR(linearGain) << " " << PVAR(rho) << endl;
if (rho > 0) { // step is good and will be accepted
_optimizer->discardTop();
goodStep = true;
} else { // recover previous state
_optimizer->pop();
}
// update trust region based on the step quality
if (rho > 0.75)
_delta = std::max(_delta, 3. * _hdl.norm());
else if (rho < 0.25)
_delta *= 0.5;
} while (!goodStep && numTries < _maxTrialsAfterFailure->value());
if (numTries == _maxTrialsAfterFailure->value() || !goodStep)
return Terminate;
return OK;
}
OptimizationAlgorithm::SolverResult OptimizationAlgorithmVP::solve(int iteration, bool online)
{
assert(_optimizer && "_optimizer not set");
assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph");
bool ok = true;
//here so that correct component for max-mixtures can be computed before the build structure
double t=get_monotonic_time();
_optimizer->computeActiveErrors();
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats) {
globalStats->timeResiduals = get_monotonic_time()-t;
}
double preProjCost = 0;
if (_projectNextIter) preProjCost = _optimizer->activeRobustChi2();
if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure
if (_projectNextIter) {
ok = _solver->buildStructureLinear();
assert(ok);
}
ok = _solver->buildStructure();
if (! ok) {
cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl;
return OptimizationAlgorithm::Fail;
}
}
// is the position-position block of omega spherical?
bool isSpherical = SPHERICAL;
if (iteration == 0)
cerr << "[VP]: isSpherical = " << isSpherical << " - Gamma_t = " << _gammaThreshold << "\n";
if (_projectNextIter) {
if (iteration > 0 && isSpherical == true) {
_solver->buildSystemSpherical();
_solver->project(isSpherical);
_optimizer->updateLinear(_solver->xLinear());
_optimizer->computeActiveErrors();
} else { // not spherical or iteration = 0
_solver->buildSystemLinear();
_solver->project();
_optimizer->updateLinear(_solver->xLinear());
_optimizer->computeActiveErrors();
assert(preProjCost);
double postProjCost = _optimizer->activeRobustChi2();
cerr << "[VP]: Cost Before Proj = " << preProjCost << " -> Cost After Proj = " << postProjCost << "\n";
/* gamma the gain */
double gamma = postProjCost/preProjCost;
cerr << "[VP]: Gamma = " << FIXED(gamma) << endl;
if (gamma > _gammaThreshold && isSpherical == false) { // no more projection step
_projectNextIter = false;
cerr << "[VP]: _projNextIter : true -> false" << endl;
}
}
}
t=get_monotonic_time();
_solver->buildSystem();
if (globalStats) {
globalStats->timeQuadraticForm = get_monotonic_time()-t;
t=get_monotonic_time();
}
ok = _solver->solve();
if (globalStats) {
globalStats->timeLinearSolution = get_monotonic_time()-t;
t=get_monotonic_time();
}
_optimizer->update(_solver->x());
if (globalStats) {
globalStats->timeUpdate = get_monotonic_time()-t;
}
if (ok)
return OK;
else
return Fail;
}
/**
* Get jobs from the queue(removes them from the queue)
*
* Notice: Caller MUST hold a mutex
*/
static WorkGroup* jobqueue_pull(ThPool* thpool,
int id) {
WorkGroup* group;
Job* job;
float current_time;
float duration, duration_next, min_duration, target_duration;
struct timespec timeAux;
int info;
int i;
JobQueue* queue = thpool->jobqueue;
if (schedule_strategy == SCHED_STATIC && queue->group_front != NULL) {
// STATIC
group = queue->group_front;
queue->group_front = group->prev;
group->prev = NULL;
} else if (queue->len == 0) {
// nothing to do
group = NULL;
} else if (schedule_strategy == SCHED_DYNAMIC || queue->len == 1 || queue->total_time <= 0) {
// SCHED_DYNAMIC
group = (WorkGroup*) malloc(sizeof(WorkGroup));
group->prev = NULL;
if (cppadcg_pool_verbose) {
if (schedule_strategy == SCHED_GUIDED) {
if (queue->len == 1)
fprintf(stdout, "jobqueue_pull(): Thread %i given a work group with 1 job\n", id);
else if (queue->total_time <= 0)
fprintf(stdout, "jobqueue_pull(): Thread %i using single-job instead of multi-job (no timing information)\n", id);
} else if (schedule_strategy == SCHED_STATIC && queue->len >= 1) {
if (queue->total_time >= 0) {
// this should not happen but just in case the user messed up
fprintf(stderr, "jobqueue_pull(): Thread %i given a work group with 1 job\n", id);
} else {
fprintf(stdout, "jobqueue_pull(): Thread %i given a work group with 1 job\n", id);
}
}
}
jobqueue_extract_single_group(thpool->jobqueue, group);
} else { // schedule_strategy == SCHED_GUIDED
// SCHED_GUIDED
group = (WorkGroup*) malloc(sizeof(WorkGroup));
group->prev = NULL;
job = queue->front;
if (job->avgElapsed == NULL) {
if (cppadcg_pool_verbose) {
fprintf(stderr, "jobqueue_pull(): Thread %i using single job instead of multi-job (No timing information for current job)\n", id);
}
// cannot use this strategy (something went wrong!)
jobqueue_extract_single_group(thpool->jobqueue, group);
} else {
// there are at least 2 jobs in the queue
group->size = 1;
duration = *job->avgElapsed;
duration_next = duration;
job = job->prev;
target_duration = queue->total_time * cppadcg_pool_guided_maxgroupwork / thpool->num_threads; // always positive
current_time = get_monotonic_time(&timeAux, &info);
if (queue->highest_expected_return > 0 && info) {
min_duration = 0.9f * (queue->highest_expected_return - current_time);
if (target_duration < min_duration) {
target_duration = min_duration;
}
}
do {
if (job->avgElapsed == NULL) {
break;
}
duration_next += *job->avgElapsed;
if (duration_next < target_duration) {
group->size++;
duration = duration_next;
} else {
break;
}
job = job->prev;
} while (job != queue->front);
if (cppadcg_pool_verbose) {
fprintf(stdout, "jobqueue_pull(): Thread %i given a work group with %i jobs for %e s (target: %e s)\n", id, group->size, duration, target_duration);
}
group->jobs = (Job*) malloc(group->size * sizeof(Job));
for (i = 0; i < group->size; ++i) {
job = jobqueue_extract_single(thpool->jobqueue);
group->jobs[i] = *job; // copy
free(job);
}
duration_next = current_time + duration; // the time when the current work is expected to end
if(duration_next > queue->highest_expected_return)
queue->highest_expected_return = duration_next;
}
}
/* more than one job in queue -> post it */
if (queue->len > 0 || queue->group_front != NULL) {
bsem_post(queue->has_jobs);
}
return group;
}
