bool saveImage(string filepath, unsigned char *data, raspicam::RaspiCam &Camera){
std::ofstream outFile (filepath.c_str(), std::ios::binary);
outFile << "P6\n";
outFile << Camera.getWidth() << " " << Camera.getHeight() << " 255\n";
outFile.write( (char*) data, Camera.getImageBufferSize() );
return true;
}
void saveImage ( string filepath,unsigned char *data,raspicam::RaspiCam &Camera ) {
std::ofstream outFile ( filepath.c_str(),std::ios::binary );
if ( Camera.getFormat()==raspicam::RASPICAM_FORMAT_BGR || Camera.getFormat()==raspicam::RASPICAM_FORMAT_RGB ) {
outFile<<"P6\n";
} else if ( Camera.getFormat()==raspicam::RASPICAM_FORMAT_GRAY ) {
outFile<<"P5\n";
} else if ( Camera.getFormat()==raspicam::RASPICAM_FORMAT_YUV420 ) { //made up format
outFile<<"P7\n";
}
outFile<<Camera.getWidth() <<" "<<Camera.getHeight() <<" 255\n";
outFile.write ( ( char* ) data,Camera.getImageBufferSize() );
}
int executeLIDAR(raspicam::RaspiCam &Camera)
{
//Set Filename for future file output
time_t rawtime;
time(&rawtime);
string imageFilename("images/image_test_");
imageFilename += ctime(&rawtime);
imageFilename += ".ppm";
strReplace(&imageFilename," ","_");
unsigned char *data;
//Capture Image
clock_t t = clock();
if (captureImage(Camera, &data)) return FAILURE;
clock_t captureTime = clock() - t;
//Find Laser Points
double x_avg = 0;
t = clock();
findPoints(Camera.getWidth(), Camera.getHeight(), data, &x_avg);
clock_t imageProcessingTime = clock() - t;
//Calculate Distance from points
double dist = 0;
t = clock();
calculateDistance(x_avg, &dist);
clock_t distanceCalculationTime = clock() - t;
cout << "Distance to point -- " << dist << endl;
cout << "Cature Time \t Image Processing Time \t Calculation Time \n" <<
((float)captureTime)/CLOCKS_PER_SEC << "\t\t" <<
((float)imageProcessingTime)/CLOCKS_PER_SEC << "\t\t" <<
((float)distanceCalculationTime)/CLOCKS_PER_SEC << "\t" << endl;
if (data)
{
std::free(data);
}
}
int executeLIDAR(raspicam::RaspiCam &Camera, char *file)
{
//Set Filename for future file output
time_t rawtime;
time(&rawtime);
string imageFilename("images/image_test_");
imageFilename += ctime(&rawtime);
imageFilename += ".ppm";
strReplace(&imageFilename," ","_");
unsigned char *data;
//Capture Image
clock_t t = clock();
if (captureImage(Camera, &data)) return FAILURE;
clock_t captureTime = clock() - t;
//Find Laser Points
vector<pixel_point> points;
t = clock();
findPoints(Camera.getWidth(), Camera.getHeight(), data, points);
clock_t imageProcessingTime = clock() - t;
//Calculate Distance from points
vector<xy_point> distances;
t = clock();
int points_size = points.size();
for (int i = 0; i < points_size; i++)
{
distances.push_back( calculateDistance(points[i]) );
}
clock_t distanceCalculationTime = clock() - t;
if (data)
{
std::free(data);
}
ofstream outputFile;
outputFile.open(file);
if(!outputFile) {return FAILURE;}
outputFile << "x\ty\n" << endl;
for (int i = 0; i<distances.size(); i++)
{
outputFile << distances[i].x << "\t" << distances[i].y << "\n" << endl;
}
outputFile.close();
cout << "Cature Time \t Image Processing Time \t Calculation Time \n" <<
((float)captureTime)/CLOCKS_PER_SEC << "\t\t" <<
((float)imageProcessingTime)/CLOCKS_PER_SEC << "\t\t" <<
((float)distanceCalculationTime)/CLOCKS_PER_SEC << "\t" << endl;
}
