diff --git a/inc/camera.h b/inc/camera.h
index 1626d1afea00c362d1e323f1f6149309c1e17d92..fd3998f9ff9184446be812f79e1771e2cdc5a5c1 100644
--- a/inc/camera.h
+++ b/inc/camera.h
@@ -15,7 +15,8 @@ class Camera{
rs2::video_frame *color_frame;
rs2::depth_frame *depth_frame;
-
+
+ rs2_intrinsics intr_profile;
float depth_scale;
cv::Rect croppingMask;
diff --git a/inc/projection.h b/inc/projection.h
index 7fb3d02bd2e6e7bc1f7e685d80a2ef40b15da811..2cb0589818c136f803f0f6c040395aa0a7d4e013 100644
--- a/inc/projection.h
+++ b/inc/projection.h
@@ -10,12 +10,12 @@ class Projection{
cv::Mat adjustingMatrix;
float distanceTopSandbox;
- void deprojectPixelsFromDepth(cv::Mat &depth, cv::Rect mask, Camera *camera, cv::Point3f beamer_pos, cv::Mat &pixelsDeprojectMap);
- void filterLowestDeprojectedPoints(cv::Mat &depth, cv::Mat &deprojectMap, cv::Mat &frameMapMask);
- void buildFrame(cv::Mat &depth, cv::Mat &pixelsDeprojectHighestMap, cv::Mat &src, cv::Mat &dst);
+ void deprojectPixelsFromDepth(cv::Mat_<float> &depth, cv::Rect mask, Camera *camera, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap);
+ void filterLowestDeprojectedPoints(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &deprojectMap, cv::Mat_<cv::Point2i> &frameMapMask);
+ void buildFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &frameMapMask, cv::Mat_<cv::Vec3b> &src, cv::Mat_<cv::Vec3b> &dst);
cv::Point2i findMatchingPixel(int i, int j, float z, Camera *camera, cv::Point3f beamer_pos);
- void copyPixelsInto(cv::Point2i pixel_dst, cv::Mat &dst, cv::Point2i pixel_src, cv::Mat &src, cv::Mat &depth);
- cv::Size getMatchingSize(cv::Mat &src, cv::Rect base);
+ void copyPixelsInto(cv::Point2i pixel_dst, cv::Mat_<cv::Vec3b> &dst, cv::Point2i pixel_src, cv::Mat_<cv::Vec3b> &src, cv::Mat_<float> &depth);
+ cv::Size getMatchingSize(cv::Mat_<cv::Vec3b> &src, cv::Rect base);
public:
Projection();
@@ -27,7 +27,7 @@ class Projection{
cv::Point2i rotatePixel(cv::Point2i pixel);
cv::Point2i revertRotatePixel(cv::Point2i pixel);
- void adjustFrame(cv::Mat depth, cv::Mat src, cv::Mat &dst, Camera *camera, cv::Point3f beamer_pos);
+ void adjustFrame(cv::Mat_<float> depth, cv::Mat_<cv::Vec3b> src, cv::Mat_<cv::Vec3b> &dst, Camera *camera, cv::Point3f beamer_pos);
void printAdjustingMatrix();
};
diff --git a/inc/sandbox.h b/inc/sandbox.h
index c517c2bf19a31cd9e19c38f0d31cea8af287e32c..3368f4a3fbb9b90a0cea3a6141c635c93878720f 100644
--- a/inc/sandbox.h
+++ b/inc/sandbox.h
@@ -24,9 +24,9 @@ class Sandbox{
int init();
void captureFrame();
- cv::Mat getColorFrame();
- cv::Mat getDepthFrame();
- cv::Mat adjustProjection(cv::Mat frame);
+ cv::Mat_<cv::Vec3b> getColorFrame();
+ cv::Mat_<float> getDepthFrame();
+ cv::Mat_<cv::Vec3b> adjustProjection(cv::Mat_<cv::Vec3b> frame);
int loadConfig();
int loadConfigFrom(char *path);
diff --git a/src/components/camera.cpp b/src/components/camera.cpp
index dda660ff849927a75a5979df80813c18e0d08d1e..4e90345556492df6156f2c6625ccf8a01641ce49 100644
--- a/src/components/camera.cpp
+++ b/src/components/camera.cpp
@@ -63,6 +63,11 @@ int Camera::start(){
warmUpDepthLens();
+ // save intrisic profil since it's an heavy operation (used to project and deproject pixels)
+ // and we use it only in one precise case
+ capture();
+ intr_profile = depth_frame->get_profile().as<rs2::video_stream_profile>().get_intrinsics();
+
return 0;
}
@@ -96,8 +101,7 @@ cv::Point2i Camera::projectPointToPixel(cv::Point3f point3D){
float point[3] = {point3D.x, point3D.y, point3D.z};
float pixel[2];
- rs2_intrinsics intr = depth_frame->get_profile().as<rs2::video_stream_profile>().get_intrinsics();
- rs2_project_point_to_pixel(pixel, &intr, point);
+ rs2_project_point_to_pixel(pixel, &intr_profile, point);
return cv::Point2i(pixel[0], pixel[1]);
}
@@ -105,8 +109,7 @@ cv::Point2i Camera::projectPointToPixel(cv::Point3f point3D){
cv::Point3f Camera::deprojectPixelToPoint(float coord[], float z){
float p[3];
- rs2_intrinsics intr = depth_frame->get_profile().as<rs2::video_stream_profile>().get_intrinsics();
- rs2_deproject_pixel_to_point(p, &intr, coord, z);
+ rs2_deproject_pixel_to_point(p, &intr_profile, coord, z);
return cv::Point3f(p[0], p[1], p[2]);
}
diff --git a/src/components/projection.cpp b/src/components/projection.cpp
index 0d9a294e6f3c4cf9318928050cf9765079cdf4fd..ed61e759dd5c97a82183cdda11bde67d6312669d 100644
--- a/src/components/projection.cpp
+++ b/src/components/projection.cpp
@@ -29,28 +29,31 @@ cv::Point2i Projection::revertRotatePixel(cv::Point2i pixel){
// Adjust the projected frame with the topology from the camera to the beamer POV
-void Projection::adjustFrame(cv::Mat depth, cv::Mat src, cv::Mat &dst, Camera *camera, cv::Point3f beamer_pos){
+void Projection::adjustFrame(cv::Mat_<float> depth, cv::Mat_<cv::Vec3b> src, cv::Mat_<cv::Vec3b> &dst, Camera *camera, cv::Point3f beamer_pos){
cv::Rect mask = camera->getCroppingMask();
- cv::Size dst_size = dst.size();
// resize the frames to be a multiple of the camera size :
// src.size = n * camera.depth.size , where n is uint > 0
- cv::resize(dst, dst, getMatchingSize(dst, mask));
- cv::resize(src, src, dst.size());
+ static cv::Mat_<cv::Vec3b> resized_dst = cv::Mat_<cv::Vec3b>(getMatchingSize(dst, mask));
+ cv::resize(dst, resized_dst, resized_dst.size());
+ cv::resize(src, src, resized_dst.size());
- cv::Mat deprojectMap = cv::Mat_<cv::Point2i>(mask.height, mask.width, cv::Point2i(-1,-1));
- cv::Mat frameMapMask = cv::Mat_<cv::Point2i>(mask.height, mask.width, cv::Point2i(-1,-1));
+ static cv::Mat_<cv::Point2i> deprojectMap = cv::Mat_<cv::Point2i>(mask.height, mask.width);
+ deprojectMap = cv::Point2i(-1,-1);
+
+ static cv::Mat_<cv::Point2i> frameMapMask = cv::Mat_<cv::Point2i>(mask.height, mask.width, cv::Point2i(-1,-1));
+ frameMapMask = cv::Point2i(-1,-1);
deprojectPixelsFromDepth(depth, mask, camera, beamer_pos, deprojectMap);
filterLowestDeprojectedPoints(depth, deprojectMap, frameMapMask);
- buildFrame(depth, frameMapMask, src, dst);
+ buildFrame(depth, frameMapMask, src, resized_dst);
// TODO : Holefilling method
- cv::resize(dst, dst, dst_size);
+ cv::resize(resized_dst, dst, dst.size());
cv::warpAffine(dst, dst, adjustingMatrix, dst.size());
}
@@ -60,7 +63,7 @@ void Projection::adjustFrame(cv::Mat depth, cv::Mat src, cv::Mat &dst, Camera *c
* PRIVATE
*/
-void Projection::deprojectPixelsFromDepth(cv::Mat &depth, cv::Rect mask, Camera *camera, cv::Point3f beamer_pos, cv::Mat &deprojectMap){
+void Projection::deprojectPixelsFromDepth(cv::Mat_<float> &depth, cv::Rect mask, Camera *camera, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap){
// Browse the depth frame matching the cropping mask
// while adapting pixels's position to the beamer's position
@@ -85,7 +88,7 @@ void Projection::deprojectPixelsFromDepth(cv::Mat &depth, cv::Rect mask, Camera
}
-void Projection::filterLowestDeprojectedPoints(cv::Mat &depth, cv::Mat &deprojectMap, cv::Mat &frameMapMask){
+void Projection::filterLowestDeprojectedPoints(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &deprojectMap, cv::Mat_<cv::Point2i> &frameMapMask){
for (int j = 0; j < deprojectMap.rows; j++){
for (int i = 0; i < deprojectMap.cols; i++){
@@ -112,7 +115,7 @@ void Projection::filterLowestDeprojectedPoints(cv::Mat &depth, cv::Mat &deprojec
}
-void Projection::buildFrame(cv::Mat &depth, cv::Mat &frameMapMask, cv::Mat &src, cv::Mat &dst){
+void Projection::buildFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &frameMapMask, cv::Mat_<cv::Vec3b> &src, cv::Mat_<cv::Vec3b> &dst){
for (int j = 0; j < frameMapMask.rows; j++){
for (int i = 0; i < frameMapMask.cols; i++){
@@ -129,7 +132,7 @@ void Projection::buildFrame(cv::Mat &depth, cv::Mat &frameMapMask, cv::Mat &src,
}
-cv::Size Projection::getMatchingSize(cv::Mat &src, cv::Rect base){
+cv::Size Projection::getMatchingSize(cv::Mat_<cv::Vec3b> &src, cv::Rect base){
cv::Size bigSize;
bigSize.width = (src.size().width % base.width == 0) ? src.size().width : src.size().width - (src.size().width % base.width) + base.width;
bigSize.height = (src.size().height % base.height == 0) ? src.size().height : src.size().height - (src.size().height % base.height) + base.height;
@@ -138,7 +141,7 @@ cv::Size Projection::getMatchingSize(cv::Mat &src, cv::Rect base){
// pixels coordinates are relative to the camera depth frame
-void Projection::copyPixelsInto(cv::Point2i pixel_dst, cv::Mat &dst, cv::Point2i pixel_src, cv::Mat &src, cv::Mat &depth){
+void Projection::copyPixelsInto(cv::Point2i pixel_dst, cv::Mat_<cv::Vec3b> &dst, cv::Point2i pixel_src, cv::Mat_<cv::Vec3b> &src, cv::Mat_<float> &depth){
if( src.size().width == dst.size().width && src.size().height == dst.size().height ){
diff --git a/src/lib/sandbox.cpp b/src/lib/sandbox.cpp
index a39a2bb1e744c0ef790488d1bd43952fa842f851..477edfa481d8dda1508883507de22d3452fc58f9 100644
--- a/src/lib/sandbox.cpp
+++ b/src/lib/sandbox.cpp
@@ -30,20 +30,21 @@ void Sandbox::captureFrame(){
camera->capture();
}
-cv::Mat Sandbox::getColorFrame(){
+cv::Mat_<cv::Vec3b> Sandbox::getColorFrame(){
return camera->getColorFrame()(camera->getCroppingMask());
}
-cv::Mat Sandbox::getDepthFrame(){
+cv::Mat_<float> Sandbox::getDepthFrame(){
return camera->getDepthFrame()(camera->getCroppingMask());
}
-cv::Mat Sandbox::adjustProjection(cv::Mat frame){
+cv::Mat_<cv::Vec3b> Sandbox::adjustProjection(cv::Mat_<cv::Vec3b> frame){
captureFrame();
- cv::Mat depth = getDepthFrame();
- cv::Mat imageCalibrate = cv::Mat(cv::Size(beamer->getWidth(), beamer->getHeight()), CV_8UC3, cv::Scalar(0, 0, 0));
+ cv::Mat_<float> depth = getDepthFrame();
+ static cv::Mat_<cv::Vec3b> imageCalibrate = cv::Mat_<cv::Vec3b>(cv::Size(beamer->getWidth(), beamer->getHeight()));
+ imageCalibrate = cv::Vec3b(0, 0, 0);
projection->adjustFrame(depth, frame, imageCalibrate, camera, beamer->getPosition());
// frame after process