void lane_analysis_handler(carmen_bumblebee_basic_stereoimage_message * stereo_image) {
// if there is no car pose yet, return
if (car_pose == nullptr) {
printf("I do not see any car pose at the moment... sorry!\n");
return;
}
// get the image from the bumblebee
Mat3b image;
if (stereo_image->image_size == 3686400) image = Mat3b(960, 1280);
else image = Mat3b(480, 640);
if (camera_side == CameraSide::LEFT) image.data = (uchar *) stereo_image->raw_left;
else if(camera_side == CameraSide::RIGHT) image.data = (uchar *) stereo_image->raw_right;
else image.data = (uchar *) stereo_image->raw_right;
cvtColor(image, image, CV_RGB2BGR);
cv::resize(image, image, Size(640,480));
fnumber++;
// if(fnumber>100)
// {
// exit(0);
// }
if (!image.empty()) {
cout << "frame: " << fnumber << endl;
// run ELAS
ELAS::run(image);
printf("CARMEN::ELAS... done!\n");
// get the raw message
printf("get_raw_message()... ");
_raw_elas_message = ELAS::get_raw_message();
_raw_elas_message.idx_frame = fnumber;
printf("done!\n");
// publish messages
lane_analysis_publish_messages(stereo_image->timestamp);
#ifdef SHOW_DISPLAY
// display viz
ELAS::display(image, &_raw_elas_message);
#endif
} else {
printf("End of dataset!\n");
}
}
