//}}}
//{{{int parse_command(char buffer[], int ctrl_socket){
//parse camara command
int parse_command(char buffer[], int ctrl_socket){
char command[MAX_LENG];
char temp[MAX_LENG];
int index;
int i,j;
int a,b,c,d;
if(sscanf(buffer,"%s", command) == 1){
if(strcmp(command,"stop") == 0){
ctrl[0] = 0;
return 1;
}
else if(strcmp(command, "start") ==0){
ctrl[0] = 1;
return 1;
}
else if(strcmp(command, "allrecord") == 0){
ctrl[1] = 1;
return 1;
}
else if(strcmp(command, "smartrecord") ==0){
ctrl[1] = 0;
return 1;
}
else if(strcmp(command, "rect_add") == 0){
index = masklist[0];
sscanf(buffer, "%s %u %u %u %u", command, &a,&b,&c,&d);
printf("===%d %d %d %d===",a,b,c,d);
masklist[4*index +1] = (a>255)?255:a; masklist[4*index +2] = (b>255)?255:b; masklist[4*index +3] = c; masklist[4*index +4] = (unsigned char)((int)((double)d/320.0*250.0));
printf("%d %s %u %u %u %u",masklist[0], command, masklist[4*index +1], masklist[4*index +2], masklist[4*index +3],(int) masklist[4*index +4] * 320/250);
masklist[0]++;
build_mask();
printf("%d", masklist[0]);
return 1;
}
else if(strcmp(command, "rect_del") == 0){
sscanf(buffer,"%s %d", command, &index);
return remove_mask(index);
}
else if(strcmp(command, "list") == 0){
for(i = 0 ; i < masklist[0]; i++){
sprintf(temp, "rect %d %d %d %d\r\n", masklist[i*4+1],masklist[i*4+2],masklist[i*4+3], (int)((double)masklist[i*4+4]*320.0/250.0));
write(ctrl_socket,temp, strlen(temp));
}
return 1;
}
else if(strcmp(command, "setthreshold") == 0){
sscanf(buffer, "%s %d", command, &a);
ctrl[2] = a /256;
ctrl[3] = a % 256;
return 1;
}
else
return 0;
}
}
static void
begin(void)
{
debug = 0;
verbose = 0;
openBoards();
alloc_buffers();
build_mask(mask, NBIT);
}
static void poisson_extension_with_init(float *out, float *inb, float *dat,
int w, int h,
float timestep, int niter, float *initialization)
{
int nmask, *mask = build_mask(&nmask, inb, w, h);
for (int i = 0; i < w*h; i++)
out[i] = isfinite(inb[i]) ? inb[i] : initialization[i];
for (int i = 0; i < niter; i++)
perform_one_iteration(out, dat, mask, nmask, w, h, timestep);
free(mask);
}
//}}}
//{{{int remove_mask(int index){
int remove_mask(int index){
int i,j;
int next;
printf("**********%d********** ",index);
if(index < masklist[0]){
for(i = index; i < masklist[0]; i++){
next = i+1;
for(j = 1; j<=4; j++)
masklist[ i*4 +j] = masklist[next *4 +j];
}
masklist[0]--;
build_mask();
return 1;
}
else
return 0;
}
static int
match_opcode (void)
{
unsigned long mask;
/* The instruction 'constant' opcode doewsn't exceed 32 bits. */
unsigned long doubleWord = words[1] + (words[0] << 16);
/* Start searching from end of instruction table. */
instruction = &crx_instruction[NUMOPCODES - 2];
/* Loop over instruction table until a full match is found. */
while (instruction >= crx_instruction)
{
mask = build_mask ();
if ((doubleWord & mask) == BIN(instruction->match, instruction->match_bits))
return 1;
else
instruction--;
}
return 0;
}
void Detect::reset()
{
cnt_ = 0;
max_duration_ = atof(cfg_->get_value("max_duration", "15.0"));
waiting_ = atof(cfg_->get_value("max_wait", "2.0"));
debug_ = atoi(cfg_->get_value("debug", "0")) == 1;
log_init = log_init_file;
log = log_file;
log("Detect config:\n");
log("\tmax_duration: %.2f\n", max_duration_);
log("\tmax_wait: %.2f\n", waiting_);
log("\n");
masked_ = build_mask(mask_);
standups_.clear();
gray_prev_.release();
gray_prev_.cols = 0;
gray_prev_.rows = 0;
}
/* ----------------------------- MNI Header -----------------------------------
@NAME : compute_chamfer
@INPUT/OUTPUT: chamfer
The original input volume will be destroyed and replaced
with the resulting chamfer distance volume. The chamfer
will contains 0's where the mask was and values > 0.0
for all other voxels, where the value is an estimate of
the distance to the the nearest voxel of the mask.
@RETURNS : ERROR if error, OK otherwise
@DESCRIPTION: Uses an idea from georges, who got it from claire,
who got it from Borgefors
@GLOBALS :
@CALLS :
@CREATED : Nov 2, 1998 Louis
@MODIFIED :
---------------------------------------------------------------------------- */
VIO_Status compute_chamfer(VIO_Volume chamfer, VIO_Real max_val)
{
VIO_Real
mask_f[3][3][3],
mask_b[3][3][3],
zero, min,
vox_min, vox_max,
val, val2;
int
sizes[VIO_MAX_DIMENSIONS],
i,j,k,
ind0,ind1,ind2;
VIO_progress_struct
progress;
get_volume_sizes(chamfer, sizes);
get_volume_voxel_range(chamfer, &vox_min, &vox_max);
zero = CONVERT_VALUE_TO_VOXEL(chamfer,0.0);
/* init chamfer to be binary valued with 0.0 on the object,
and infinity (or vox_max) elsewhere */
if (debug) print ("initing chamfer vol (%d %d %d)\n",sizes[0],sizes[1],sizes[2]);
for(ind0=0; ind0<sizes[0]; ind0++) {
for(ind1=0; ind1<sizes[1]; ind1++) {
for(ind2=0; ind2<sizes[2]; ind2++) {
GET_VOXEL_3D(val, chamfer, ind0, ind1, ind2);
if (val == zero) {
SET_VOXEL_3D(chamfer, ind0, ind1, ind2, vox_max );
}
else {
SET_VOXEL_3D(chamfer, ind0, ind1, ind2, vox_min);
}
}
}
}
if (debug) print ("building mask\n");
build_mask(chamfer, mask_f, mask_b);
set_volume_real_range(chamfer, 0.0, max_val);
zero = CONVERT_VALUE_TO_VOXEL(chamfer,0.0);
if (verbose) initialize_progress_report( &progress, TRUE, sizes[0],
"forward pass");
for(ind0=1; ind0<sizes[0]-1; ind0++) {
for(ind1=1; ind1<sizes[1]-1; ind1++) {
for(ind2=1; ind2<sizes[2]-1; ind2++) {
GET_VALUE_3D(val, chamfer, ind0, ind1, ind2);
if (val != zero) { /* then apply forward mask */
min = val;
for(i=-1; i<=+1; i++) {
for(j=-1; j<=+1; j++) {
for(k=-1; k<=+1; k++) {
GET_VALUE_3D(val, chamfer, i+ind0, j+ind1, k+ind2);
val2 = val + mask_f[i+1][j+1][k+1];
min = MIN (min, val2);
}
}
}
min = convert_value_to_voxel(chamfer, min);
SET_VOXEL_3D(chamfer, ind0, ind1, ind2, min );
} /* if val != 0.0 */
} /* ind2 */
} /* ind1 */
if (verbose) update_progress_report( &progress, ind0+1 );
} /* ind0 */
if (verbose) terminate_progress_report( &progress );
if (verbose) initialize_progress_report( &progress, TRUE, sizes[0],
"reverse pass");
for(ind0=sizes[0]-2; ind0>=1; ind0--) {
for(ind1=sizes[1]-2; ind1>=1; ind1--) {
for(ind2=sizes[2]-2; ind2>=1; ind2--) {
GET_VALUE_3D(val, chamfer, ind0, ind1, ind2);
if (val != zero) { /* then apply backwardmask */
min = val;
for(i=-1; i<=1; i++) {
for(j=-1; j<=+1; j++) {
for(k=-1; k<=+1; k++) {
GET_VALUE_3D(val, chamfer, i+ind0, j+ind1, k+ind2);
val2 = val + mask_b[i+1][j+1][k+1];
min = MIN (min, val2);
}
}
}
min = convert_value_to_voxel(chamfer, min);
SET_VOXEL_3D(chamfer, ind0, ind1, ind2, min );
} /* if val != 0.0 */
} /* ind2 */
} /* ind1 */
if (verbose) update_progress_report( &progress, ind0+1 );
} /* ind0 */
if (verbose) terminate_progress_report( &progress );
return (OK);
}
int main(int argc, char * argv[])
{
int sg_fd, res, c, num, alloc_len, off, pdt;
int k, md_len, hdr_len, bd_len, mask_in_len;
unsigned u, uu;
int dbd = 0;
int got_contents = 0;
int force = 0;
int got_mask = 0;
int mode_6 = 0;
int pg_code = -1;
int sub_pg_code = 0;
int save = 0;
int verbose = 0;
int read_in_len = 0;
const char * device_name = NULL;
unsigned char read_in[MX_ALLOC_LEN];
unsigned char mask_in[MX_ALLOC_LEN];
unsigned char ref_md[MX_ALLOC_LEN];
char ebuff[EBUFF_SZ];
struct sg_simple_inquiry_resp inq_data;
int ret = 0;
while (1) {
int option_index = 0;
c = getopt_long(argc, argv, "c:dfhl:m:p:svV", long_options,
&option_index);
if (c == -1)
break;
switch (c) {
case 'c':
memset(read_in, 0, sizeof(read_in));
if (0 != build_mode_page(optarg, read_in, &read_in_len,
sizeof(read_in))) {
fprintf(stderr, "bad argument to '--contents'\n");
return SG_LIB_SYNTAX_ERROR;
}
got_contents = 1;
break;
case 'd':
dbd = 1;
break;
case 'f':
force = 1;
break;
case 'h':
case '?':
usage();
return 0;
case 'l':
num = sscanf(optarg, "%d", &res);
if ((1 == num) && ((6 == res) || (10 == res)))
mode_6 = (6 == res) ? 1 : 0;
else {
fprintf(stderr, "length (of cdb) must be 6 or 10\n");
return SG_LIB_SYNTAX_ERROR;
}
break;
case 'm':
memset(mask_in, 0xff, sizeof(mask_in));
if (0 != build_mask(optarg, mask_in, &mask_in_len,
sizeof(mask_in))) {
fprintf(stderr, "bad argument to '--mask'\n");
return SG_LIB_SYNTAX_ERROR;
}
got_mask = 1;
break;
case 'p':
if (NULL == strchr(optarg, ',')) {
num = sscanf(optarg, "%x", &u);
if ((1 != num) || (u > 62)) {
fprintf(stderr, "Bad page code value after '--page' "
"switch\n");
return SG_LIB_SYNTAX_ERROR;
}
pg_code = u;
} else if (2 == sscanf(optarg, "%x,%x", &u, &uu)) {
if (uu > 254) {
fprintf(stderr, "Bad sub page code value after '--page'"
" switch\n");
return SG_LIB_SYNTAX_ERROR;
}
pg_code = u;
sub_pg_code = uu;
} else {
fprintf(stderr, "Bad page code, subpage code sequence after "
"'--page' switch\n");
return SG_LIB_SYNTAX_ERROR;
}
break;
case 's':
save = 1;
break;
case 'v':
++verbose;
break;
case 'V':
fprintf(stderr, ME "version: %s\n", version_str);
return 0;
default:
fprintf(stderr, "unrecognised option code 0x%x ??\n", c);
usage();
return SG_LIB_SYNTAX_ERROR;
}
}
if (optind < argc) {
if (NULL == device_name) {
device_name = argv[optind];
++optind;
}
if (optind < argc) {
for (; optind < argc; ++optind)
fprintf(stderr, "Unexpected extra argument: %s\n",
argv[optind]);
usage();
return SG_LIB_SYNTAX_ERROR;
}
}
if (NULL == device_name) {
fprintf(stderr, "missing device name!\n");
usage();
return SG_LIB_SYNTAX_ERROR;
}
if (pg_code < 0) {
fprintf(stderr, "need page code (see '--page=')\n");
usage();
return SG_LIB_SYNTAX_ERROR;
}
if (got_mask && force) {
fprintf(stderr, "cannot use both '--force' and '--mask'\n");
usage();
return SG_LIB_SYNTAX_ERROR;
}
sg_fd = sg_cmds_open_device(device_name, 0 /* rw */, verbose);
if (sg_fd < 0) {
fprintf(stderr, ME "open error: %s: %s\n", device_name,
safe_strerror(-sg_fd));
return SG_LIB_FILE_ERROR;
}
if (0 == sg_simple_inquiry(sg_fd, &inq_data, 0, verbose))
pdt = inq_data.peripheral_type;
else
pdt = 0x1f;
/* do MODE SENSE to fetch current values */
memset(ref_md, 0, MX_ALLOC_LEN);
alloc_len = mode_6 ? SHORT_ALLOC_LEN : MX_ALLOC_LEN;
if (mode_6)
res = sg_ll_mode_sense6(sg_fd, dbd, 0 /*current */, pg_code,
sub_pg_code, ref_md, alloc_len, 1, verbose);
else
res = sg_ll_mode_sense10(sg_fd, 0 /* llbaa */, dbd, 0 /* current */,
pg_code, sub_pg_code, ref_md, alloc_len, 1,
verbose);
ret = res;
if (SG_LIB_CAT_INVALID_OP == res) {
fprintf(stderr, "MODE SENSE (%d) not supported, try '--len=%d'\n",
(mode_6 ? 6 : 10), (mode_6 ? 10 : 6));
goto err_out;
} else if (SG_LIB_CAT_NOT_READY == res) {
fprintf(stderr, "MODE SENSE (%d) failed, device not ready\n",
(mode_6 ? 6 : 10));
goto err_out;
} else if (SG_LIB_CAT_UNIT_ATTENTION == res) {
fprintf(stderr, "MODE SENSE (%d) failed, unit attention\n",
(mode_6 ? 6 : 10));
goto err_out;
} else if (SG_LIB_CAT_ABORTED_COMMAND == res) {
fprintf(stderr, "MODE SENSE (%d) failed, aborted command\n",
(mode_6 ? 6 : 10));
goto err_out;
} else if (SG_LIB_CAT_ILLEGAL_REQ == res) {
fprintf(stderr, "bad field in MODE SENSE (%d) command\n",
(mode_6 ? 6 : 10));
goto err_out;
} else if (0 != res) {
fprintf(stderr, "MODE SENSE (%d) failed\n", (mode_6 ? 6 : 10));
goto err_out;
}
off = sg_mode_page_offset(ref_md, alloc_len, mode_6, ebuff, EBUFF_SZ);
if (off < 0) {
fprintf(stderr, "MODE SENSE (%d): %s\n", (mode_6 ? 6 : 10), ebuff);
goto err_out;
}
if (mode_6) {
hdr_len = 4;
md_len = ref_md[0] + 1;
bd_len = ref_md[3];
} else {
hdr_len = 8;
md_len = (ref_md[0] << 8) + ref_md[1] + 2;
bd_len = (ref_md[6] << 8) + ref_md[7];
}
if (got_contents) {
if (read_in_len < 2) {
fprintf(stderr, "contents length=%d too short\n", read_in_len);
goto err_out;
}
ref_md[0] = 0; /* mode data length reserved for mode select */
if (! mode_6)
ref_md[1] = 0; /* mode data length reserved for mode select */
if (0 == pdt) /* for disks mask out DPOFUA bit */
ref_md[mode_6 ? 2 : 3] &= 0xef;
if (md_len > alloc_len) {
fprintf(stderr, "mode data length=%d exceeds allocation "
"length=%d\n", md_len, alloc_len);
goto err_out;
}
if (got_mask) {
for (k = 0; k < (md_len - off); ++k) {
if ((0x0 == mask_in[k]) || (k > read_in_len))
read_in[k] = ref_md[off + k];
else if (mask_in[k] < 0xff) {
c = (ref_md[off + k] & (0xff & ~mask_in[k]));
read_in[k] = (c | (read_in[k] & mask_in[k]));
}
}
read_in_len = md_len - off;
}
if (! force) {
if ((! (ref_md[off] & 0x80)) && save) {
fprintf(stderr, "PS bit in existing mode page indicates that "
"it is not saveable\n but '--save' option given\n");
goto err_out;
}
read_in[0] &= 0x7f; /* mask out PS bit, reserved in mode select */
if ((md_len - off) != read_in_len) {
fprintf(stderr, "contents length=%d but reference mode page "
"length=%d\n", read_in_len, md_len - off);
goto err_out;
}
if (pg_code != (read_in[0] & 0x3f)) {
fprintf(stderr, "contents page_code=0x%x but reference "
"page_code=0x%x\n", (read_in[0] & 0x3f), pg_code);
goto err_out;
}
if ((read_in[0] & 0x40) != (ref_md[off] & 0x40)) {
fprintf(stderr, "contents flags subpage but reference page"
"does not (or vice versa)\n");
goto err_out;
}
if ((read_in[0] & 0x40) && (read_in[1] != sub_pg_code)) {
fprintf(stderr, "contents subpage_code=0x%x but reference "
"sub_page_code=0x%x\n", read_in[1], sub_pg_code);
goto err_out;
}
} else
md_len = off + read_in_len; /* force length */
memcpy(ref_md + off, read_in, read_in_len);
if (mode_6)
res = sg_ll_mode_select6(sg_fd, 1, save, ref_md, md_len, 1,
verbose);
else
res = sg_ll_mode_select10(sg_fd, 1, save, ref_md, md_len, 1,
verbose);
ret = res;
if (SG_LIB_CAT_INVALID_OP == res) {
fprintf(stderr, "MODE SELECT (%d) not supported\n",
(mode_6 ? 6 : 10));
goto err_out;
} else if (SG_LIB_CAT_NOT_READY == res) {
fprintf(stderr, "MODE SELECT (%d) failed, device not ready\n",
(mode_6 ? 6 : 10));
goto err_out;
} else if (SG_LIB_CAT_UNIT_ATTENTION == res) {
fprintf(stderr, "MODE SELECT (%d) failed, unit attention\n",
(mode_6 ? 6 : 10));
goto err_out;
} else if (SG_LIB_CAT_ABORTED_COMMAND == res) {
fprintf(stderr, "MODE SELECT (%d) failed, aborted command\n",
(mode_6 ? 6 : 10));
goto err_out;
} else if (SG_LIB_CAT_ILLEGAL_REQ == res) {
fprintf(stderr, "bad field in MODE SELECT (%d) command\n",
(mode_6 ? 6 : 10));
goto err_out;
} else if (0 != res) {
fprintf(stderr, "MODE SELECT (%d) failed\n", (mode_6 ? 6 : 10));
goto err_out;
}
} else {
printf(">>> No contents given, so show current mode page data:\n");
printf(" header:\n");
dStrHex((const char *)ref_md, hdr_len, -1);
if (bd_len) {
printf(" block descriptor(s):\n");
dStrHex((const char *)(ref_md + hdr_len), bd_len, -1);
} else
printf(" << no block descriptors >>\n");
printf(" mode page:\n");
dStrHex((const char *)(ref_md + off), md_len - off, -1);
}
err_out:
res = sg_cmds_close_device(sg_fd);
if (res < 0) {
fprintf(stderr, "close error: %s\n", safe_strerror(-res));
if (0 == ret)
return SG_LIB_FILE_ERROR;
}
return (ret >= 0) ? ret : SG_LIB_CAT_OTHER;
}
void poisson_solver_with_init(float *out, float *in, float *dat, int w, int h,
float *init)
{
// build list of masked pixels
int nmask, (*mask)[3] = build_mask(&nmask, in, w, h);
if (!nmask) {
for (int i = 0; i < w*h; i++)
out[i] = isfinite(in[i]) ? in[i] : init[i];
return;
}
int *invmask = xmalloc(w*h*sizeof(int));
invert_mask(invmask, mask, nmask, w, h);
// define the linear map A=laplacian_operator
struct cgpois_state e[1];
e->w = w;
e->h = h;
e->mask = mask;
e->invmask = invmask;
e->nmask = nmask;
e->boundary_data = xmalloc(w*h*sizeof(double));
e->interior_data = xmalloc(w*h*sizeof(double));
for (int i = 0; i < w*h; i++)
e->boundary_data[i] = in[i];
for (int i = 0; i < w*h; i++)
e->interior_data[i] = 0;//dat[i];
// fill-in the independent term b
double *b = xmalloc(nmask * sizeof(double));
double *tmp = xmalloc(nmask * sizeof(double));
for (int p = 0; p < nmask; p++)
tmp[p] = 0;
minus_operator(b, tmp, nmask, e);
free(tmp);
for (int p = 0; p < nmask; p++)
b[p] = -dat[mask[p][0]+w*mask[p][1]] - b[p];
for (int i = 0; i < w*h; i++)
if (e->invmask[i] < 0)
e->boundary_data[i] = 0;
// compute the solution
double *solution = xmalloc(nmask * sizeof(double));
double *initialization = xmalloc(nmask * sizeof(double));
for (int i = 0; i < nmask; i++)
initialization[i] = init[mask[i][0]+w*mask[i][1]];
//conjugate_gradient(solution, minus_laplacian_operator, b, nmask, e);
int cg_maxit = CG_MAXIT() >= 0 ? CG_MAXIT() : nmask;
float cg_eps = CG_EPS() >= 0 ? CG_EPS() : 1e-6;
fancy_conjugate_gradient(solution, minus_operator, b, nmask,
e, initialization, cg_maxit, cg_eps);
// copy the solution to its place
for (int i = 0; i < w*h; i++)
out[i] = in[i];
for (int p = 0; p < nmask; p++) {
if (nmask < 33)
fprintf(stderr, "sol[%d] = %g\n", p, solution[p]);
out[mask[p][2]] = solution[p];
}
free(e->interior_data);
free(e->boundary_data);
free(solution);
free(mask);
free(invmask);
free(b);
}
