int main(void)
{
printf("ORF pid %d\n", (int) getpid());
SRCAM cam;
// int ret = SR_OpenDlg(&cam, 2, 0); // 2: call open dialog, 0: no parent window
// int ret = SR_OpenETH(&cam, "192.168.1.33");
int ret = SR_OpenETH(&cam, "169.254.1.33");
if(ret<=0) return -1; // ret holds the number of cameras found
cv::Size imsize(SR_GetCols(cam), SR_GetRows(cam)); // SR image size
int sizebytes = 2 * imsize.area() * sizeof(unsigned short); // number of bytes sent from the SR
// namedWindow("mainWin",WINDOW_AUTOSIZE );
cvNamedWindow("mainWin", CV_WINDOW_AUTOSIZE);
int sizestep = sizeof(float)*3; // size step from one xyz component to the next
int c=-1; // user input variable
// enable software trigger mode so that the LEDs will only turn
// on when SR_Acquire() is called
SR_SetMode(cam,AM_SW_TRIGGER);
std::time_t start_time = std::time(0);
while(c==-1) // infinite loop, breaks if key pressed
{
ret = SR_Acquire(cam);
cv::Mat xyz(imsize, CV_32FC3, cv::Scalar::all(0));
if(ret!=sizebytes) break;
// the coordinates are stored as three channels in the format
// (x1, y1, z1, x2, y2, z2, ... ) squentially row by row
SR_CoordTrfFlt( cam,
&((float*)xyz.ptr())[0], // pointer to first x
&((float*)xyz.ptr())[1], // pointer to first y
&((float*)xyz.ptr())[2], // pointer to first z
sizestep, sizestep, sizestep); // increments to next element
cv::Mat z, z_display; // z channel and output image
extractImageCOI(&CvMat(xyz), z, 2); // extract the z channel (change the 2 for another channel)
z.convertTo(z_display, CV_8UC1, 256.0 / 5.0, 0); // convert to 8 bit (0..255) values, here for 5 meter camera
cv::imshow("mainWin", z_display); // display image
std::time_t t = std::time() - start_time;
printf("ORF data taken at %d seconds\n", t);
//std::cout << "ORF data taken at" << t << "seconds\n";
std::stringstream filename;
filename << "./ORF_photos/" << t << ".bmp"
cv::imwrite(filename.str(),z_display);
c = cvWaitKey(1000); // wait 1 sec before continuing loop. if user presses a button, the loop will exit
}
SR_Close(cam);
return 0;
}
void mesa_info() {
assert(_srcam);
// library version
unsigned short version[4];
SR_GetVersion(version);
fprintf(stderr, "Library Version: %d.%d.%d.%d\n", version[0], version[1], version[2], version[3]);
// device string
size_t len = 512;
char buf[len];
memset(buf, 0, len);
SR_GetDeviceString(_srcam, buf, len);
fprintf(stderr, "Device String: %s\n", buf);
// Acquire Mode
int mode = SR_GetMode(_srcam);
fprintf(stderr, "Acquire Mode: 0x%x \n ", mode);
// dimensions
fprintf(stderr, "Size (r x c): %d x %d\n", SR_GetRows(_srcam), SR_GetCols(_srcam));
ImgEntry *imgent;
int r = SR_GetImageList(_srcam, &imgent);
fprintf(stderr, "Images available: %d\n", r);
for (int i = 0; i < r; i++) {
fprintf(stderr,
"Image %d -- Type: %s, DataType: %s, W: %d, H: %d\n", i,
imgtype_string(imgent[i].imgType),
datatype_string(imgent[i].dataType),
imgent[i].width, imgent[i].height);
}
// ModFrq
fprintf(stderr, "Modulation Frequency: %s\n", modfrq_to_string(SR_GetModulationFrequency(_srcam)));
// integration time
fprintf(stderr, "Integration time code: %d\n", SR_GetIntegrationTime(_srcam));
// amplitude threshold
fprintf(stderr, "Amplitude Threshold: %d\n", SR_GetAmplitudeThreshold(_srcam));
// Distance Offset
// Deprecated: Only for SR2
// fprintf(stderr, "Distance Offset: %d\n", SR_GetDistanceOffset(_srcam));
}
mesa_t
mesa_init_frame() {
mesa_t frame;
if(_srccam_init == 1) {
frame.rows = SR_GetRows(_srcam);
frame.cols = SR_GetCols(_srcam);
}
else {
frame.rows = 144;
frame.cols = 176;
}
frame.len = frame.rows * frame.cols;
size_t sz_frame = frame.len * sizeof(float);
frame.x = (float*) malloc (sz_frame);
frame.y = (float*) malloc (sz_frame);
frame.z = (float*) malloc (sz_frame);
frame.img_size = frame.len * sizeof(uint16_t);
frame.cloud_size = sz_frame * 3;
//if(_srccam_init == 0) {
frame.distance = (unsigned char*) malloc(frame.img_size);
frame.amplitude = (unsigned char*) malloc(frame.img_size);
frame.confidence = (unsigned char*) malloc(frame.img_size);
//}
// This probably causes a memory leak, but this needs to be done when you use an array of mesa_t frames.
assert(frame.x);
assert(frame.y);
assert(frame.z);
mesa_set_modes(0);
return frame;
}
int main() {
CMesaDevice* srCam = 0;
int res;
//--------- Connect LIDAR ---------
//ここにバージョン取得の関数を書く
//--------- Connect LIDAR ---------
SR_OpenETH(&srCam, "192.168.201.41");
//--------- Set Acquire Mode ---------
int mode = AM_COR_FIX_PTRN|AM_CONV_GRAY|AM_DENOISE_ANF; //recommended mode
SR_SetMode(srCam, mode);
printf("Acquire mode: %d \n", mode);
//--------- Get Distance Data ---------
ImgEntry* imgEntryArray;
//ここに距離データ取得の関数を書く
//--------- Get Rows & Cols ---------
int rows, cols;
rows = SR_GetRows(srCam);
cols = SR_GetCols(srCam);
printf("rows: %d cols: %d \n", rows, cols);
//--------- Transform sphirical coodinate to xyz coordinate(Do not edit)---------
int pitch = sizeof(short);
short* X = (short*)(malloc(rows * cols * pitch));
short* Y = (short*)(malloc(rows * cols * pitch));
short* Z = (short*)(malloc(rows * cols * pitch));
SR_CoordTrfUint16(srCam, X, Y, (unsigned short*)Z, pitch, pitch, pitch);
//--------- Output only first 5 data(Do not edit) ---------
printf("Distance data (x,y,z), with mm unit:\n");
for(int j = 0; j < 5; j++) {
printf("%d %d %d\n", X[j], Y[j], Z[j]);
}
//--------- Output into PLY file(Do not edit) ---------
std::ofstream ofs("lidar_data.ply");
// write header
ofs << "ply" << std::endl;
ofs << "format ascii 1.0" << std::endl;
ofs << "element vertex " << rows*cols-1 << std::endl;
ofs << "property float x" << std::endl;
ofs << "property float y" << std::endl;
ofs << "property float z" << std::endl;
ofs << "end_header" << std::endl;
// write contents
for(int i = 0; i < rows * cols - 1; i++) {
ofs << X[i] << " " << Y[i] << " " << Z[i] << std::endl;
}
ofs.close();
//--------- Deconnect LIDAR(Do not edit) ---------
SR_Close(srCam);
return 0;
}
