bool VCR::readSharedImage(Container &c) {
bool retVal = false;
if (c.getDataType() == Container::SHARED_IMAGE) {
SharedImage si = c.getData<SharedImage> ();
// Check if we have already attached to the shared memory.
if (!m_hasAttachedToSharedImageMemory) {
m_sharedImageMemory
= core::wrapper::SharedMemoryFactory::attachToSharedMemory(
si.getName());
}
// Check if we could successfully attach to the shared memory.
if (m_sharedImageMemory->isValid()) {
//cerr << "Got image: LOG 0.2 " << si.toString() << endl;
// Lock the memory region to gain exclusive access. REMEMBER!!! DO NOT FAIL WITHIN lock() / unlock(), otherwise, the image producing process would fail.
m_sharedImageMemory->lock();
{
// Here, do something with the image. For example, we simply show the image.
const uint32_t numberOfChannels = 3;
// For example, simply show the image.
if (m_image == NULL) {
m_image = cvCreateImage(cvSize(si.getWidth(),
si.getHeight()), IPL_DEPTH_8U, numberOfChannels);
}
// Copying the image data is very expensive...
if (m_image != NULL) {
memcpy(m_image->imageData,
m_sharedImageMemory->getSharedMemory(),
si.getWidth() * si.getHeight() * numberOfChannels);
}
}
// Release the memory region so that the image produce (i.e. the camera for example) can provide the next raw image data.
m_sharedImageMemory->unlock();
// Mirror the image.
cvFlip(m_image, 0, -1);
retVal = true;
}
}
return retVal;
}
bool LaneDetector::readSharedImage(Container &c) {
bool retVal = false;
if (c.getDataType() == Container::SHARED_IMAGE) {
SharedImage si = c.getData<SharedImage> ();
// Check if we have already attached to the shared memory containing the image from the virtual camera.
if (!m_hasAttachedToSharedImageMemory) {
m_sharedImageMemory = core::wrapper::SharedMemoryFactory::attachToSharedMemory(si.getName());
}
// Check if we could successfully attach to the shared memory.
if (m_sharedImageMemory->isValid()) {
// Lock the memory region to gain exclusive access using a scoped lock.
Lock l(m_sharedImageMemory);
if (m_image == NULL) {
m_image = cvCreateImage(cvSize(si.getWidth(), si.getHeight()), IPL_DEPTH_8U, si.getBytesPerPixel());
}
// Example: Simply copy the image into our process space.
if (m_image != NULL) {
memcpy(m_image->imageData, m_sharedImageMemory->getSharedMemory(), si.getWidth() * si.getHeight() * si.getBytesPerPixel());
}
// Mirror the image.
cvFlip(m_image, 0, -1);
retVal = true;
}
}
return retVal;
}
bool LaneFollower::readSharedImage(Container &c) {
bool retVal = false;
if (c.getDataType() == odcore::data::image::SharedImage::ID()) {
SharedImage si = c.getData<SharedImage> ();
// Check if we have already attached to the shared memory.
if (!m_hasAttachedToSharedImageMemory) {
m_sharedImageMemory = odcore::wrapper::SharedMemoryFactory::attachToSharedMemory(si.getName());
}
// Check if we could successfully attach to the shared memory.
if (m_sharedImageMemory->isValid()) {
// Lock the memory region to gain exclusive access using a scoped lock.
Lock l(m_sharedImageMemory);
const uint32_t numberOfChannels = 3;
// For example, simply show the image.
if (m_image.empty()) {
m_image.create(cv::Size(si.getWidth(), si.getHeight()), CV_8UC3);
}
// Copying the image data is very expensive...
if (!m_image.empty()) {
memcpy(m_image.data, m_sharedImageMemory->getSharedMemory(), si.getWidth() * si.getHeight() * numberOfChannels);
}
// Mirror the image.
cv::flip(m_image,m_image,-1); //only use in simulator
retVal = true;
}
}
return retVal;
}
void SharedImageViewerWidget::nextContainer(Container &c) {
if (c.getDataType() == Container::SHARED_IMAGE) {
SharedImage si = c.getData<SharedImage>();
if ( ( (si.getWidth() * si.getHeight()) > 0) && (si.getName().size() > 0) ) {
// Check if this shared image is already in the list.
vector<string>::iterator result = std::find(m_listOfAvailableSharedImages.begin(), m_listOfAvailableSharedImages.end(), si.getName());
if (result == m_listOfAvailableSharedImages.end()) {
m_listOfAvailableSharedImages.push_back(si.getName());
QString item = QString::fromStdString(si.getName());
m_list->addItem(item);
// Store for further usage.
m_mapOfAvailableSharedImages[si.getName()] = si;
}
}
}
}
void SharedImageViewerWidget::selectedSharedImage(QListWidgetItem *item) {
if (item != NULL) {
// Retrieve stored shared image.
SharedImage si = m_mapOfAvailableSharedImages[item->text().toStdString()];
if ( (si.getWidth() * si.getHeight()) > 0 ) {
Lock l(m_sharedImageMemoryMutex);
cerr << "Using shared image: " << si.toString() << endl;
setWindowTitle(QString::fromStdString(si.toString()));
m_sharedImageMemory = core::wrapper::SharedMemoryFactory::attachToSharedMemory(si.getName());
m_sharedImage = si;
// Remove the selection box.
m_list->hide();
}
}
}
void ChaseCar::drawScene() {
static uint32_t frameCounter = 0;
static clock_t start = clock();
Container container = getKeyValueDataStore().get(opendlv::data::environment::EgoState::ID());
m_egoState = container.getData<opendlv::data::environment::EgoState>();
m_image = m_grabber->getNextImage();
frameCounter++;
// Share information about this image.
if (m_image.get()) {
SharedImage si;
si.setWidth(m_image->getWidth());
si.setHeight(m_image->getHeight());
// TODO: Refactor me!
si.setBytesPerPixel(3);
si.setName("ChaseCar");
si.setSize(si.getWidth() * si.getHeight() * si.getBytesPerPixel());
Container c(si);
getConference().send(c);
}
if ((frameCounter % 20) == 0) {
clock_t end = clock();
clock_t duration = end - start;
double seconds = (1000.0 * duration) / CLOCKS_PER_SEC;
seconds /= 1000.0;
cerr << "FPS: " << (frameCounter / seconds) << endl;
frameCounter = 0;
start = clock();
}
m_grabber->delay();
}
int32_t main(int32_t argc, char **argv)
{
uint32_t retVal = 0;
int recIndex=1;
bool log=false;
if((argc != 2 && argc != 3 && argc != 4) || (argc==4 && string(argv[1]).compare("-l")!=0))
{
errorMessage(string(argv[0]));
retVal = 1;
}
else if(argc==2 && string(argv[1]).compare("-h")==0)
{
helpMessage(string(argv[0]));
retVal = 0;
}
else
{
// if -l option is set
if(argc==4 || (argc==3 && string(argv[1]).compare("-l")==0))
{
++recIndex;
log=true;
}
// Use command line parameter as file for playback;
string recordingFile(argv[recIndex]);
stringstream recordingFileUrl;
recordingFileUrl << "file://" << recordingFile;
// Location of the recording file.
URL url(recordingFileUrl.str());
// Do we want to rewind the stream on EOF?
const bool AUTO_REWIND = false;
// Size of the memory buffer that should fit at least the size of one frame.
const uint32_t MEMORY_SEGMENT_SIZE = 1024 * 768;
// Number of memory segments (one is enough as we are running sychronously).
const uint32_t NUMBER_OF_SEGMENTS = 1;
// Run player in synchronous mode without data caching in background.
const bool THREADING = false;
// Construct the player.
Player player(url, AUTO_REWIND, MEMORY_SEGMENT_SIZE, NUMBER_OF_SEGMENTS, THREADING);
// The next container from the recording.
Container nextContainer;
// Using OpenCV's IplImage data structure to simply playback the data.
IplImage *image = NULL;
// Create the OpenCV playback window.
cvNamedWindow("CaroloCup-CameraPlayback", CV_WINDOW_AUTOSIZE);
// This flag indicates whether we have attached already to the shared
// memory containing the sequence of captured images.
bool hasAttachedToSharedImageMemory = false;
// Using this variable, we will access the captured images while
// also having convenient automated system resource management.
SharedPointer<SharedMemory> sharedImageMemory;
ifstream file(argv[recIndex+1]);
CSVRow row;
// read out the header row
row.readNextRow(file);
uint32_t frameNumber=1, csvFN;
int32_t VPx,VPy,BLx,BLy,BRx,BRy,TLx,TLy,TRx,TRy;
stringstream frameMessage;
stringstream VPMessage;
frameMessage.str(string());
VPMessage.str(string());
bool fbf=false;
// Main data processing loop.
while (player.hasMoreData()) {
// Read next entry from recording.
nextContainer = player.getNextContainerToBeSent();
// Data type SHARED_IMAGE contains a SharedImage data structure that
// provides meta-information about the captured image.
if (nextContainer.getDataType() == Container::SHARED_IMAGE) {
// Read the data structure to retrieve information about the image.
SharedImage si = nextContainer.getData<SharedImage>();
// Check if we have already attached to the shared memory.
if (!hasAttachedToSharedImageMemory) {
sharedImageMemory = SharedMemoryFactory::attachToSharedMemory(si.getName());
// Toggle the flag as we have now attached to the shared memory.
hasAttachedToSharedImageMemory = true;
}
// Check if we could successfully attach to the shared memory.
if (sharedImageMemory->isValid()) {
// Using a scoped lock to get exclusive access.
{
Lock l(sharedImageMemory);
if (image == NULL) {
// Create the IplImage header data and access the shared memory for the actual image data.
image = cvCreateImageHeader(cvSize(si.getWidth(), si.getHeight()), IPL_DEPTH_8U, si.getBytesPerPixel());
// Let the IplImage point to the shared memory containing the captured image.
image->imageData = static_cast<char*>(sharedImageMemory->getSharedMemory());
}
}
// Show the image using OpenCV.
// if csv parameter is set
if(argc==4 || (argc==3 && string(argv[1]).compare("-l")!=0))
{
if(! row.readNextRow(file)) break;
while(row[0].compare("")==0)
if(! row.readNextRow(file)) break;
sscanf(row[0].c_str(), "%d", &csvFN);
if(frameNumber==csvFN)
{
Mat img = cvarrToMat(image);
frameMessage.str(string());
VPMessage.str(string());
sscanf(row[9].c_str(), "%d", &VPx);
sscanf(row[10].c_str(), "%d", &VPy);
frameMessage<<"Frame "<<frameNumber;
VPMessage<<"Vanishing Point ("<<VPx<<","<<VPy<<")";
setLabel(img, frameMessage.str(), cvPoint(30,45));
setLabel(img, VPMessage.str(), cvPoint(30,60));
if(log)
cout << frameNumber << ", " << VPx << ", " << VPy <<endl;
// print support points and lines
sscanf(row[1].c_str(), "%d", &BLx);
sscanf(row[2].c_str(), "%d", &BLy);BLy+=60;
sscanf(row[3].c_str(), "%d", &TLx);
sscanf(row[4].c_str(), "%d", &TLy);TLy+=60;
sscanf(row[5].c_str(), "%d", &TRx);
sscanf(row[6].c_str(), "%d", &TRy);TRy+=60;
sscanf(row[7].c_str(), "%d", &BRx);
sscanf(row[8].c_str(), "%d", &BRy);BRy+=60;
circle(img, Point(BLx,BLy), 5, CV_RGB(255, 255, 255), CV_FILLED);
circle(img, Point(TLx,TLy), 5, CV_RGB(255, 255, 255), CV_FILLED);
circle(img, Point(TRx,TRy), 5, CV_RGB(255, 255, 255), CV_FILLED);
circle(img, Point(BRx,BRy), 5, CV_RGB(255, 255, 255), CV_FILLED);
double slope1 = static_cast<double>(TLy-BLy)/static_cast<double>(TLx-BLx);
double slope2 = static_cast<double>(TRy-BRy)/static_cast<double>(TRx-BRx);
Point p1(0,0), q1(img.cols,img.rows);
Point p2(0,0), q2(img.cols,img.rows);
p1.y = -(BLx-p1.x) * slope1 + BLy;
q1.y = -(TLx-q1.x) * slope1 + TLy;
p2.y = -(BRx-p2.x) * slope2 + BRy;
q2.y = -(TRx-q2.x) * slope2 + TRy;
line(img,p1,q1,CV_RGB(255, 255, 255),1,CV_AA);
line(img,p2,q2,CV_RGB(255, 255, 255),1,CV_AA);
imshow("CaroloCup-CameraPlayback", img);
}
}
else
cvShowImage("CaroloCup-CameraPlayback", image);
// Let the image render before proceeding to the next image.
char c = cvWaitKey(10);
// Check if the user wants to stop the replay by pressing ESC or pause it by pressing SPACE (needed also to go frame-by-frame).
if (static_cast<uint8_t>(c) == 27) break;
else if (static_cast<uint8_t>(c) == 32 || fbf) {
do
{
c = cvWaitKey();
}while(c!='n' && static_cast<uint8_t>(c) != 32 && static_cast<uint8_t>(c) != 27);
if (static_cast<uint8_t>(c) == 27) break; // ESC
else if (static_cast<uint8_t>(c) == 32) fbf=false; // SPACE -> continue
else if (c=='n') fbf=true; // pressed 'n' -> next frame
}
++frameNumber;
}
}
}
// maybe print EOF message && wait for user input?
// Release IplImage data structure.
cvReleaseImage(&image);
// Close playback window.
cvDestroyWindow("CaroloCup-CameraPlayback");
// The shared memory will be automatically released.
}
// Return error code.
return retVal;
}
