// 1 on sucesss
int
mesa_init_device(const char* host)
{
if(SR_OpenETH(&_srcam, host) == 1)
{
_srccam_init = 1;
return 1;
}
return 0;
}
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;
}
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;
}
