void reconstruct() {
holger_time_start(1, "Reconstruction");
memset(volume, 0, sizeof(unsigned char)*volume_w*volume_h*volume_n);
printf("bscan_w/h: %d %d\n", bscan_w, bscan_h);
printf("volume_w/h/n: %d %d %d\n", volume_w, volume_h, volume_n);
printf("volume_spacing: %f\n", volume_spacing);
printf("volume_origo: %f %f %f\n", volume_origo.x, volume_origo.y, volume_origo.z);
printf("\n");
holger_time(1, "Initialization");
// Fill up queue
for (int i = 0; i < BSCAN_WINDOW; i++) {
shift_queues();
wait_for_input();
if (end_of_data()) break;
}
int counter = BSCAN_WINDOW;
while(true) {
// Retrieve ultrasound data and perform ye olde switcheroo
shift_queues();
wait_for_input();
if (end_of_data()) break;
holger_time(1, "Retrieve ultrasound data");
printf("Reconstructing %d ...\n", counter);
calibrate_pos_matrix(pos_matrices_queue[BSCAN_WINDOW-1], cal_matrix);
holger_time(1, "Calibrate");
printf("Calibrate\n");
insert_plane_points(pos_matrices_queue[BSCAN_WINDOW-1]);
holger_time(1, "Fill, transform and translate plane_points");
printf("plane_points\n");
insert_plane_eq();
holger_time(1, "Fill bscan_plane_equation");
printf("bscan_plane_equation\n");
//int axis = find_orthogonal_axis(bscan_plane_equation); // TODO: Function that finds axis most orthogonal to bscan plane
int axis = 0; // Actually, turns out that the output is pretty much equal for any axis,
// but the computation time varies (1X - 2X)
int intersection_counter = find_intersections(axis);
holger_time(1, "Find ray-plane intersections");
printf("intersections\n");
fill_voxels(intersection_counter);
holger_time(1, "Fill voxels");
printf("voxels\n");
counter++;
}
holger_time_print(1);
}
/*************************************************
* Peek into a stream *
*************************************************/
u32bit DataSource_Stream::peek(byte out[], u32bit length, u32bit offset) const
{
if(end_of_data())
throw Invalid_State("DataSource_Stream: Cannot peek when out of data");
u32bit got = 0;
if(offset)
{
SecureVector<byte> buf(offset);
source->read((char*)buf.begin(), buf.size());
if(source->bad())
throw Stream_IO_Error("DataSource_Stream::peek: Source failure");
got = source->gcount();
}
if(got == offset)
{
source->read((char*)out, length);
if(source->bad())
throw Stream_IO_Error("DataSource_Stream::peek: Source failure");
got = source->gcount();
}
if(source->eof())
source->clear();
source->seekg(total_read, std::ios::beg);
return got;
}
/*
* Peek into a stream
*/
size_t DataSource_Stream::peek(byte out[], size_t length, size_t offset) const
{
if(end_of_data())
throw Invalid_State("DataSource_Stream: Cannot peek when out of data");
size_t got = 0;
if(offset)
{
secure_vector<byte> buf(offset);
source.read(reinterpret_cast<char*>(&buf[0]), buf.size());
if(source.bad())
throw Stream_IO_Error("DataSource_Stream::peek: Source failure");
got = source.gcount();
}
if(got == offset)
{
source.read(reinterpret_cast<char*>(out), length);
if(source.bad())
throw Stream_IO_Error("DataSource_Stream::peek: Source failure");
got = source.gcount();
}
if(source.eof())
source.clear();
source.seekg(total_read, std::ios::beg);
return got;
}
static int
change_res (int fd, unsigned char res)
{
char f[] = "change_res";
DBG (127, "%s called\n", f);
if (res != 0 && res != 1)
{
DBG (3, "%s: error: unsupported resolution\n", f);
return -1;
}
/* cameras resolution semantics are opposite of ours */
res = !res;
DBG (127, "%s: setting res to %d\n", f, res);
res_pck[2] = res;
if (send_pck (fd, res_pck) == -1)
{
DBG (4, "%s: error: send_pck returned -1\n", f);
}
if (end_of_data (fd) == -1)
{
DBG (4, "%s: error: end_of_data returned -1\n", f);
}
return 0;
}
/**
* Read from the pipe
*/
u32bit DataSource_Command::read(byte buf[], u32bit length)
{
if(end_of_data())
return 0;
fd_set set;
FD_ZERO(&set);
FD_SET(pipe->fd, &set);
struct ::timeval tv;
tv.tv_sec = 0;
tv.tv_usec = MAX_BLOCK_USECS;
ssize_t got = 0;
if(::select(pipe->fd + 1, &set, 0, 0, &tv) == 1)
{
if(FD_ISSET(pipe->fd, &set))
got = ::read(pipe->fd, buf, length);
}
if(got <= 0)
{
shutdown_pipe();
return 0;
}
return static_cast<u32bit>(got);
}
Dc20Info *get_info(int fd)
{
static Dc20Info result;
unsigned char buf[256];
if (send_pck(fd, info_pck) == -1) {
if (!quiet) fprintf(stderr, "%s: get_info: error: send_pck returned -1\n", __progname);
return NULL;
}
if (verbose) printf("%s: get_info: read info packet\n", __progname);
if (read_data(fd, buf, 256) == -1) {
if (!quiet) fprintf(stderr, "%s: get_info: error: read_data returned -1\n", __progname);
return NULL;
}
if (end_of_data(fd) == -1) {
if (!quiet) fprintf(stderr, "%s: get_info: error: end_of_data returned -1\n", __progname);
return NULL;
}
result.model = buf[1];
result.ver_major = buf[2];
result.ver_minor = buf[3];
result.pic_taken = buf[8]<<8|buf[9];
#ifdef DC25
/* Not sure where the previous line came from. All the
* information I have says that even on the DC20 the number of
* standard res pics is in byte 17 and the number of high res pics
* is in byte 19. This is definitely true on my DC25.
*/
result.pic_taken = buf[17]+buf[19];
#endif
result.pic_left = buf[10]<<8|buf[11];
#ifdef DC25
/* Not sure where the previous line came from. All the
* information I have says that even on the DC20 the number of
* standard res pics left is in byte 23 and the number of high res
* pics left is in byte 21. It seems to me that the conservative
* approach is to report the number of high res pics left.
*/
result.pic_left = buf[21];
#endif
result.flags.low_res = buf[23];
#ifdef DC25
/* Not sure where the previous line came from. All the
* information I have says that even on the DC20 the low_res
* byte is 11.
*/
result.flags.low_res = buf[11];
#endif
result.flags.low_batt = buf[29];
return &result;
}
static SANE_Status
snap_pic (int fd)
{
char f[] = "snap_pic";
/* make sure camera is set to our settings state */
if (change_res (Camera.fd, dc210_opt_lowres) == -1)
{
DBG (1, "%s: Failed to set resolution\n", f);
return SANE_STATUS_INVAL;
}
/* take the picture */
if (send_pck (fd, shoot_pck) == -1)
{
DBG (4, "%s: error: send_pck returned -1\n", f);
return SANE_STATUS_INVAL;
}
else
{
if (end_of_data (Camera.fd) == -1)
{
DBG (2, "%s: error: end_of_data returned -1\n", f);
return SANE_STATUS_INVAL;
}
}
Camera.pic_taken++;
Camera.pic_left--;
Camera.current_picture_number = Camera.pic_taken;
image_range.max++;
sod[DC210_OPT_IMAGE_NUMBER].cap &= ~SANE_CAP_INACTIVE;
/* add this one to the Pictures array */
if ((Camera.Pictures =
(PictureInfo *) realloc (Camera.Pictures,
Camera.pic_taken * sizeof (PictureInfo))) ==
NULL)
{
DBG (4, "%s: error: allocate memory for pictures array\n", f);
return SANE_STATUS_INVAL;
}
if (get_picture_info (Camera.Pictures + Camera.pic_taken,
Camera.pic_taken) == -1)
{
DBG (1, "%s: Failed to get new picture info\n", f);
/* XXX - I guess we should try to erase the image here */
return SANE_STATUS_INVAL;
}
return SANE_STATUS_GOOD;
}
static int
get_picture_info (PictureInfo * pic, int p)
{
char f[] = "get_picture_info";
static char buffer[256];
DBG (4, "%s: info for pic #%d\n", f, p);
pic_info_pck[3] = (unsigned char) p;
if (send_pck (Camera.fd, pic_info_pck) == -1)
{
DBG (4, "%s: error: send_pck returned -1\n", f);
return -1;
}
if (read_data (Camera.fd, (unsigned char *) buffer, 256) == -1)
{
DBG (2, "%s: error: read_data returned -1\n", f);
return -1;
}
if (end_of_data (Camera.fd) == -1)
{
DBG (2, "%s: error: end_of_data returned -1\n", f);
return -1;
}
if (buffer[3] == 0)
{
pic->low_res = SANE_TRUE;
}
else if (buffer[3] == 1)
{
pic->low_res = SANE_FALSE;
}
else
{
DBG (2, "%s: error: unknown resolution code %u\n", f, buffer[3]);
return -1;
}
pic->size = (buffer[8] & 0xFF) << 24;
pic->size |= (buffer[9] & 0xFF) << 16;
pic->size |= (buffer[10] & 0xFF) << 8;
pic->size |= (buffer[11] & 0xFF);
return 0;
}
static int
erase (int fd)
{
if (send_pck (fd, erase_pck) == -1)
{
DBG (3, "erase: error: send_pck returned -1\n");
return -1;
}
if (end_of_data (fd) == -1)
{
DBG (3, "erase: error: end_of_data returned -1\n");
return -1;
}
return 0;
}
int get_pic(int fd, int which, unsigned char *pic, int low_res)
{
unsigned char buf[1024];
int n = (low_res) ? 61 : 122;
int i;
pic_pck[3] = (unsigned char)which;
if (send_pck(fd, pic_pck) == -1) {
if (!quiet) fprintf(stderr, "%s: get_pic: error: send_pck returned -1\n", __progname);
return -1;
}
printf("Get image #%d: ", which);
hash_init();
for (i = 0; i < n; i++) {
hash_mark(i, n - 1, 40);
if (read_data(fd, buf, 1024) == -1) {
if (!quiet) fprintf(stderr, "%s: get_pic: error: read_data returned -1\n", __progname);
return -1;
}
#ifdef DC25
/*
* If this is the first row, byte 4 tells us if this is
* a low or hi-res image
*/
if ( i == 0 ) {
if ( buf[4] == 0 ) {
n=122;
} else {
n=61;
}
}
#endif
memcpy((char *)&pic[i*1024], buf, 1024);
}
printf("\n");
return end_of_data(fd);
}
int
get_info (DC210 * camera)
{
char f[] = "get_info";
unsigned char buf[256];
if (send_pck (camera->fd, info_pck) == -1)
{
DBG (2, "%s: error: send_pck returned -1\n", f);
return -1;
}
DBG (9, "%s: read info packet\n", f);
if (read_data (camera->fd, buf, 256) == -1)
{
DBG (2, "%s: error: read_data returned -1\n", f);
return -1;
}
if (end_of_data (camera->fd) == -1)
{
DBG (2, "%s: error: end_of_data returned -1\n", f);
return -1;
}
camera->model = buf[1];
camera->ver_major = buf[2];
camera->ver_minor = buf[3];
camera->pic_taken = buf[56] << 8 | buf[57];
camera->pic_left = buf[72] << 8 | buf[73];
camera->flags.low_res = buf[22];
camera->flags.low_batt = buf[8];
return 0;
}
short int read_sample() {
if (end_of_data())
throw wave_reader_exception();
++read_samples;
return read_value<short int>();
}
SANE_Status
sane_read (SANE_Handle __sane_unused__ handle, SANE_Byte * data,
SANE_Int max_length, SANE_Int * length)
{
static char buffer[1024];
if (dc210_opt_thumbnails)
{
if (total_bytes_read == THUMBSIZE)
{
if (dc210_opt_erase)
{
if (erase (Camera.fd) == -1)
{
DBG (1, "Failed to erase memory\n");
return SANE_STATUS_INVAL;
}
Camera.pic_taken--;
Camera.pic_left++;
Camera.current_picture_number = Camera.pic_taken;
image_range.max--;
}
return SANE_STATUS_EOF;
}
*length = 0;
if (!(bytes_in_buffer - bytes_read_from_buffer))
{
if (read_data (Camera.fd, (unsigned char *) buffer, 1024) == -1)
{
DBG (5, "sane_read: read_data failed\n");
return SANE_STATUS_INVAL;
}
bytes_in_buffer = 1024;
bytes_read_from_buffer = 0;
}
while (bytes_read_from_buffer < bytes_in_buffer &&
max_length && total_bytes_read < THUMBSIZE)
{
*data++ = buffer[bytes_read_from_buffer++];
(*length)++;
max_length--;
total_bytes_read++;
}
if (total_bytes_read == THUMBSIZE)
{
if (end_of_data (Camera.fd) == -1)
{
DBG (4, "sane_read: end_of_data error\n");
return SANE_STATUS_INVAL;
}
else
{
return SANE_STATUS_GOOD;
}
}
else
{
return SANE_STATUS_GOOD;
}
}
else
{
int lines = 0;
if (cinfo.output_scanline >= cinfo.output_height)
{
/* clean up comms with the camera */
if (end_of_data (Camera.fd) == -1)
{
DBG (2, "sane_read: error: end_of_data returned -1\n");
return SANE_STATUS_INVAL;
}
if (dc210_opt_erase)
{
DBG (127, "sane_read bp%d, erase image\n", __LINE__);
if (erase (Camera.fd) == -1)
{
DBG (1, "Failed to erase memory\n");
return SANE_STATUS_INVAL;
}
Camera.pic_taken--;
Camera.pic_left++;
Camera.current_picture_number = Camera.pic_taken;
image_range.max--;
}
return SANE_STATUS_EOF;
}
/* XXX - we should read more than 1 line at a time here */
lines = 1;
(void) jpeg_read_scanlines (&cinfo, dest_mgr->buffer, lines);
(*dest_mgr->put_pixel_rows) (&cinfo, dest_mgr, lines, (char *) data);
*length = cinfo.output_width * cinfo.output_components * lines;
return SANE_STATUS_GOOD;
}
}
/**
* Peek at the pipe contents
*/
u32bit DataSource_Command::peek(byte[], u32bit, u32bit) const
{
if(end_of_data())
throw Invalid_State("DataSource_Command: Cannot peek when out of data");
throw Stream_IO_Error("Cannot peek/seek on a command pipe");
}
/**
* DataSource_Command Destructor
*/
DataSource_Command::~DataSource_Command()
{
if(!end_of_data())
shutdown_pipe();
}
