Skip to content
Snippets Groups Projects
Commit 488887af authored by William Wan's avatar William Wan
Browse files

fix librealsense

parent ca46d34d
No related branches found
No related tags found
No related merge requests found
......@@ -26,7 +26,8 @@ class IntelRsObservation : public Observation {
void laserOperation(bool powerOn, float value);
private:
rs2::syncer * syncer;
//rs2::syncer * syncer;
rs2::frame_queue queue;
rs2_intrinsics depth_intrin;
rs2::device dev;
rs2::util::Config<>::multistream stream;
......
......@@ -20,7 +20,6 @@ int main() {
IntelRsObservation observation;
printf("Obs init'ed\n");
//cudaDeviceSynchronize();
// cudaError_t error = cudaPeekAtLastError();
......@@ -87,14 +86,9 @@ int main() {
//FaceDetector faceDetect("shape_predictor_68_face_landmarks.dat");
//int instanceID = faceDetect.addModelInstance(testInstance);
printf("before loop\n");
// Draw the point cloud
for(int frame = 1; !pangolin::ShouldQuit(); ++frame){
printf("loop %i\n", frame);
glClearColor(0.35, 0.35, 0.35, 1.0);
glShadeModel(GL_SMOOTH);
float4 lightPosition = make_float4(
......
......@@ -22,27 +22,13 @@ IntelRsObservation::IntelRsObservation() {
}
dev.set_option(RS2_OPTION_EMITTER_ENABLED, false);
dev.set_option(RS2_OPTION_LASER_POWER, 0.0);
printf("A\n");
*syncer = dev.create_syncer();
printf("A.A\n");
// Configure all supported streams to run at 30 frames per second
dev.open({ RS2_STREAM_DEPTH, 640, 480, 30, RS2_FORMAT_Z16 });
dev.start(queue);
rs2::util::config config;
config.enable_stream(RS2_STREAM_INFRARED, rs2::preset::best_quality);
printf("A.B\n");
config.enable_stream(RS2_STREAM_DEPTH, rs2::preset::best_quality);
printf("A.C\n");
stream = config.open(dev);
printf("A.D\n");
printf("B\n");
stream.start(*syncer);
depth_intrin = stream.get_intrinsics(RS2_STREAM_DEPTH);
//stream = config.open(dev);
depth_intrin = dev.get_intrinsics({ RS2_STREAM_DEPTH, 640, 480, 30, RS2_FORMAT_Z16 });
CameraParameters depthParam(make_float2(depth_intrin.fx, depth_intrin.fy), make_float2(depth_intrin.ppx, depth_intrin.ppy), make_int2(640, 480));
/*
depthParam.radialDistortion[0] = depth_intrin.coeffs[0];
......@@ -53,8 +39,6 @@ IntelRsObservation::IntelRsObservation() {
*/
depthParam.hasRadialDistortion = false;
_depthCameraParams = depthParam;
printf("C\n");
this->_colorCameraParams.imageSize.x = 640;
this->_colorCameraParams.imageSize.y = 480;
......@@ -62,44 +46,30 @@ IntelRsObservation::IntelRsObservation() {
_depthData.resize(_depthCameraParams.imageSize.x * _depthCameraParams.imageSize.y);
_pointCloud.resize(_depthCameraParams.imageSize.x * _depthCameraParams.imageSize.y);
printf("D\n");
}
IntelRsObservation::~IntelRsObservation() {
}
void IntelRsObservation::advance() {
auto frames = syncer->wait_for_frames();
if (frames.size() == 0) {
return;
}
//auto frames = syncer->wait_for_frames();
auto frame = queue.wait_for_frame();
const uint16_t * depth_in_int;
for (auto&& frame : frames) {
if (frame.get_stream_type() == RS2_STREAM_DEPTH) {
//depth = reinterpret_cast<const float *>(frame.get_data());
depth_in_int = reinterpret_cast<const uint16_t *>(frame.get_data());
depth_in_int = reinterpret_cast<const uint16_t *>(frame.get_data());
cudaDeviceSynchronize();
//std::cout<<"length " <<_depthData.length()<<std::endl;
//std::cout<< "first" << _depthData.hostPtr()[0]<<std::endl;
for (int i = 0; i < depth_intrin.height; i++) {
for(int j = 0; j < depth_intrin.width; j++) {
//std::cout<<depth_in_int[i * depth_intrin.height + j] << " ";
_depthData.hostPtr()[i * depth_intrin.height + j] = (float) depth_in_int[i * depth_intrin.height + j];
}
}
cudaDeviceSynchronize();
for (int i = 0; i < _depthCameraParams.imageSize.y; i++) {
for(int j = 0; j < _depthCameraParams.imageSize.x; j++) {
_depthData.hostPtr()[i * _depthCameraParams.imageSize.x + j] = (float) depth_in_int[i * _depthCameraParams.imageSize.x + j];
}
}
_depthData.syncHostToDevice();
//_colorImage.syncHostToDevice();
depthToPointCloud();
_pointCloud.syncDeviceToHost();
}
void IntelRsObservation::laserOperation(bool powerOn, float value) {
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment