int main(int argc, char *argv[]) {
curl_global_init(CURL_GLOBAL_ALL);
curlm = curl_multi_init();
multi_setopt(curlm, CURLMOPT_TIMERFUNCTION, timer_func);
memset(slices, 0, sizeof(slices));
show_progress = isatty(fileno(stdout));
parse_args(argc, argv);
init_slices(0, 0);
perform();
if (load_percent && file_size) {
init_slices(bytes_written, file_size);
perform();
}
if (file)
fclose(file);
print_progress();
if (show_progress)
putchar('\n');
curl_multi_cleanup(curlm);
curl_global_cleanup();
return 0;
}
static size_t write_func(const void *ptr, size_t size, size_t nmemb, void *data) {
Slice *slice = (Slice*) data;
if (slice->begin == 0 && slice->pos == 0 && slice->end == 0) {
double clength = -1;
start_time = time(NULL);
check_easy(curl_easy_getinfo(slice->curl, CURLINFO_CONTENT_LENGTH_DOWNLOAD, &clength),
"failed to detect the content length");
if (clength > 0) {
file_size = clength;
init_slices(0, load_percent ? file_size * (percent / 100.0) : file_size);
} else {
num_slices = 1;
load_percent = 0;
warn("unsupported server, proceeding with a single connection.\n");
}
}
return slice_write(slice, ptr, size, nmemb);
}
void loop() {
Timer loop_t = Timer("TOTAL");
cv::Mat depth, obstacle, clear;
cv_bridge::CvImagePtr img;
{
Timer setup_t = Timer("setup");
/* Grab a pair of images */
sensor_msgs::Image::ConstPtr img_msg;
stereo_msgs::DisparityImage::ConstPtr disp_msg;
get_images(img_msg, disp_msg);
if (img_msg == NULL || disp_msg == NULL) {
loop_t.disable();
setup_t.disable();
//ROS_INFO("No images");
return;
}
/* We don't want to waste time calculating the
same image twice.
*/
unsigned int seq = disp_msg->image.header.seq;
if (last_seq == seq && !last_msg) {
ROS_INFO("Old message #%d", seq);
loop_t.disable();
setup_t.disable();
last_msg = true;
return;
}
last_seq = seq;
last_msg = false;
/* Display raw image */
img = cv_bridge::toCvCopy(img_msg,
sensor_msgs::image_encodings::BGR8);
/* Get disparity data */
cv_bridge::CvImagePtr disp = cv_bridge::toCvCopy(disp_msg->image,
"32FC1");
float focal = disp_msg->f; float base = disp_msg->T;
/* Generate depth image */
/* Why are these so inaccurate? Calibration issue?
float min_depth = (focal * base) / disp_msg->min_disparity;
float max_depth = (focal * base) / disp_msg->max_disparity;
*/
cv::Mat full_depth = (focal * base) / disp->image;
/* Not be necessary if downscale = 1 */
cv::resize(full_depth, depth, cv::Size(IMG_WIDTH, IMG_HEIGHT));
/* Display value-scaled depth image */
#ifdef CV_OUTPUT
cv::Mat scaled_depth = depth / RANGE_MAX;
cv::imshow(DEPTH_WINDOW, scaled_depth);
#endif
/* Create empty obstacle map */
obstacle = cv::Mat::zeros(IMG_HEIGHT, IMG_WIDTH, CV_32F);
clear = cv::Mat::zeros(IMG_HEIGHT, IMG_WIDTH, CV_32F);
}
float pct_good;
/* Find and display obstacles */
{
Timer obs_t = Timer("find_obstacles");
pct_good = find_obstacles(depth, obstacle, RANGE_MIN, 100.0);
}
#ifdef CV_OUTPUT
cv::Mat scaled_obs = obstacle / RANGE_MAX;
cv::imshow(OBS_WINDOW, scaled_obs);
#endif
std::vector<Slice> slices;
std::vector<RectList> slice_bboxes;
{
Timer slice_t = Timer("calc slicing");
/* Set up slices */
{
Timer init_t = Timer("init", 1);
init_slices(slices);
}
{
Timer fill_t = Timer("fill", 1);
fill_slices(obstacle, slices, RANGE_MAX);
}
{
Timer noise_t = Timer("noise", 1);
for (int i = 0; i < slices.size(); i++) {
remove_noise(slices[i].mat);
}
}
{
Timer bbox_t = Timer("bbox", 1);
/* Calculate bounding box on each slice */
for (int i = 0; i < slices.size(); i++) {
slice_bboxes.push_back(calc_bboxes(slices[i].mat));
}
}
}
cv::Mat boxes_image, final_image;
{
Timer disp_t = Timer("disp slicing");
/* Display bounding boxes on image */
boxes_image = img->image.clone();
/* Convert box image to HSV */
cv::cvtColor(boxes_image, boxes_image, cv::COLOR_BGR2HSV);
/* Loop backwards-- farthest first, panter's algorithm */
for (int i = slice_bboxes.size()-1; i >= 0; i--) {
/* Calculate hue */
int hue = (int)(((float)i)/((float)slice_bboxes.size())*120.0);
cv::Scalar color = cv::Scalar(hue, 255, 255);
for (int j = 0; j < slice_bboxes[i].size(); j++) { //TODO: Iterators???
/* Get / resize boxes */
cv::Rect bbox = slice_bboxes[i][j];
bbox.x *= DOWNSCALE; bbox.y *= DOWNSCALE;
bbox.width *= DOWNSCALE; bbox.height *= DOWNSCALE;
/* Draw boxes */
cv::rectangle(boxes_image, bbox, color, -1);
}
}
/* Convert back to RGB */
cv::cvtColor(boxes_image, boxes_image, cv::COLOR_HSV2BGR);
/* Combine with image */
cv::addWeighted(boxes_image, 0.3, img->image, 0.7, 0.0, final_image);
}
/* Generate top-down image */
cv::Mat top;
top = cv::Mat::zeros(TOP_SIZE, TOP_SIZE, CV_8UC1);
Grid grid = Grid(GRID_WIDTH, GRID_HEIGHT, RANGE_MAX, RANGE_MAX);
{
Timer top_t = Timer("topdown");
/* Init top-down image */
calc_topdown_grid(grid, slices, slice_bboxes, RANGE_MAX);
publish_grid(grid, pct_good);
draw_grid(grid, top);
calc_topdown(top, slices, slice_bboxes, RANGE_MAX);
}
#ifdef CV_OUTPUT
cv::imshow(TOP_WINDOW, top);
cv::imshow(IMAGE_WINDOW, final_image);
#endif
#if defined(__SLICE_DEBUG) && defined(CV_OUTPUT)
for (int i = 0; i < NUM_SLICES; i++) {
std::string s = "a";
s[0] = 'a'+i;
cv::imshow(s, slices[i]);
}
#endif
}
int main (int argc, char *argv[])
{
SUCCESS_OR_DIE (gaspi_proc_init (GASPI_BLOCK));
gaspi_rank_t iProc, nProc;
SUCCESS_OR_DIE (gaspi_proc_rank (&iProc));
SUCCESS_OR_DIE (gaspi_proc_num (&nProc));
// number of threads
const int NTHREADS = 2;
// number of buffers
const int NWAY = 2;
// allocate segment for array for local vector, left halo and right halo
gaspi_segment_id_t const segment_id = 0;
SUCCESS_OR_DIE ( gaspi_segment_create
( segment_id, NWAY * (NTHREADS + 2) * 2 * VLEN * sizeof (double)
, GASPI_GROUP_ALL, GASPI_BLOCK, GASPI_MEM_UNINITIALIZED));
gaspi_pointer_t array;
SUCCESS_OR_DIE ( gaspi_segment_ptr ( segment_id, &array) );
// initial buffer id
int buffer_id = 0;
// set notification values
gaspi_notification_id_t left_data_available[NWAY];
gaspi_notification_id_t right_data_available[NWAY];
for (gaspi_notification_id_t id = 0; id < NWAY; ++id)
{
left_data_available[id] = id;
right_data_available[id] = NWAY + id;
}
// set queue id
gaspi_queue_id_t queue_id = 0;
// initialize slice data structures
slice *ssl = (slice *) malloc (NTHREADS * sizeof (slice));
ASSERT (ssl);
init_slices (ssl, NTHREADS);
// initialize data
data_init (NTHREADS,iProc, buffer_id, array);
const int right_halo = NTHREADS+1;
const int left_halo = 0;
// issue initial write to left ngb
wait_for_queue_max_half (&queue_id);
SUCCESS_OR_DIE ( gaspi_write_notify
( segment_id, array_OFFSET_left (buffer_id, left_halo + 1, 0), LEFT(iProc, nProc)
, segment_id, array_OFFSET_left (buffer_id, right_halo, 0), VLEN * sizeof (double)
, right_data_available[buffer_id], 1, queue_id, GASPI_BLOCK));
// issue initial write to right ngb
wait_for_queue_max_half (&queue_id);
SUCCESS_OR_DIE ( gaspi_write_notify
( segment_id, array_OFFSET_right (buffer_id, right_halo - 1, 0), RIGHT(iProc, nProc)
, segment_id, array_OFFSET_right (buffer_id, left_halo, 0), VLEN * sizeof (double)
, left_data_available[buffer_id], 1, queue_id, GASPI_BLOCK));
// set total number of iterations per slice
const int num = nProc * NTHREADS * NITER;
omp_set_num_threads (NTHREADS);
double time = -now();
#pragma omp parallel default (none) firstprivate (buffer_id, queue_id) \
shared (array, left_data_available, right_data_available, ssl, stderr)
{
slice* sl;
while ((sl = get_slice_and_lock (ssl, NTHREADS, num)))
{
handle_slice ( sl, array, left_data_available, right_data_available
, segment_id, queue_id, NWAY, NTHREADS, num);
/*
TODO
====
- Which functionality do we need in 'handle_slice' ?
(asynchronous dataflow for 1-D halo-exchange)
- Discuss.
- Bonus question:
Can we be at different iteration stages for left and right halo ?
if yes: Why ?
*/
omp_unset_lock (&sl->lock);
}
#pragma omp barrier
}
time += now();
data_verify (NTHREADS, iProc, (NITER * nProc * NTHREADS) % NWAY, array);
printf ("# gaspi %s nProc %d vlen %i niter %d nthreads %i nway %i time %g\n"
, argv[0], nProc, VLEN, NITER, NTHREADS, NWAY, time
);
gaspi_proc_term (GASPI_BLOCK);
return EXIT_SUCCESS;
}