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;
}
int captureImage(raspicam::RaspiCam &Camera, unsigned char **data)
{
*data = new unsigned char [ Camera.getImageBufferSize()];
Camera.grab();
Camera.retrieve(*data);
size_t i = 0;
while (++i < nFramesCaptured)
{
Camera.grab();
Camera.retrieve(*data);
}
return SUCCESS;
}
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);
}
}
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() );
}
bool setCamera(raspicam::RaspiCam &Camera){
Camera.setWidth(1280);
Camera.setHeight(960);
Camera.setBrightness(50);
Camera.setSharpness(0);
Camera.setContrast(0);
Camera.setShutterSpeed(0);
Camera.setISO(100);
Camera.setExposureCompensation(5);
Camera.setExposure(raspicam::RASPICAM_EXPOSURE_AUTO);
Camera.setAWB(raspicam::RASPICAM_AWB_AUTO);
Camera.setFormat(raspicam::RASPICAM_FORMAT_RGB);
Camera.setVerticalFlip(true);
//Camera.setAWB_RB(1,1);
return true;
}
void processCommandLine ( int argc,char **argv,raspicam::RaspiCam &Camera ) {
Camera.setWidth ( getParamVal ( "-w",argc,argv,1280 ) );
Camera.setHeight ( getParamVal ( "-h",argc,argv,960 ) );
Camera.setBrightness ( getParamVal ( "-br",argc,argv,50 ) );
Camera.setSharpness ( getParamVal ( "-sh",argc,argv,0 ) );
Camera.setContrast ( getParamVal ( "-co",argc,argv,0 ) );
Camera.setSaturation ( getParamVal ( "-sa",argc,argv,0 ) );
Camera.setShutterSpeed( getParamVal ( "-ss",argc,argv,0 ) );
Camera.setISO ( getParamVal ( "-iso",argc,argv ,400 ) );
if ( findParam ( "-vs",argc,argv ) !=-1 )
Camera.setVideoStabilization ( true );
Camera.setExposureCompensation ( getParamVal ( "-ec",argc,argv ,0 ) );
if ( findParam ( "-gr",argc,argv ) !=-1 )
Camera.setFormat(raspicam::RASPICAM_FORMAT_GRAY);
if ( findParam ( "-yuv",argc,argv ) !=-1 )
Camera.setFormat(raspicam::RASPICAM_FORMAT_YUV420);
if ( findParam ( "-test_speed",argc,argv ) !=-1 )
doTestSpeedOnly=true;
int idx;
if ( ( idx=findParam ( "-ex",argc,argv ) ) !=-1 )
Camera.setExposure ( getExposureFromString ( argv[idx+1] ) );
if ( ( idx=findParam ( "-awb",argc,argv ) ) !=-1 )
Camera.setAWB( getAwbFromString ( argv[idx+1] ) );
nFramesCaptured=getParamVal("-nframes",argc,argv,100);
Camera.setAWB_RB(getParamVal("-awb_b",argc,argv ,1), getParamVal("-awb_g",argc,argv ,1));
}
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;
}
