diff --git a/inc/beamerProjection.h b/inc/beamerProjection.h
index 5989fd7a630515bd97033f36bc0fc68db1812edb..0da8db80b7ced86604aafd2585ef082caf9b7aa8 100644
--- a/inc/beamerProjection.h
+++ b/inc/beamerProjection.h
@@ -8,17 +8,20 @@
class BeamerProjection{
private:
cv::Mat adjustingMatrix;
+ float topSandboxDistance = 1.0f;
- cv::Point2i adjustPixel(int i, int j, float z, Camera *camera, cv::Point3f beamer_pos);
+ 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::Rect base);
+ cv::Size getMatchingSize(cv::Mat &src, cv::Rect base);
public:
BeamerProjection();
- void setAdjustingMatrix(cv::Mat matrix){ adjustingMatrix.setTo(matrix); }
+ void setAdjustingMatrix(cv::Mat matrix){ adjustingMatrix = matrix; }
cv::Mat getAdjustingMatrix(){ return adjustingMatrix.clone(); }
cv::Point2i rotatePixel(cv::Point2i pixel);
- void adjustFrame(cv::Mat &depth, cv::Mat &src, cv::Mat &dst, Camera *camera, cv::Point3f beamer_pos);
+ void adjustFrame(cv::Mat depth, cv::Mat src, cv::Mat &dst, Camera *camera, cv::Point3f beamer_pos);
void printAdjustingMatrix();
};
diff --git a/inc/camera.h b/inc/camera.h
index 71c9490cc8553cb5fafe7e7add4ccc8892500fb8..067f0c8c2af0bb95eabf80fa5241e29734b99bfc 100644
--- a/inc/camera.h
+++ b/inc/camera.h
@@ -25,7 +25,12 @@ class Camera{
Camera();
~Camera();
- cv::Mat getDepthFrame(){ return cv::Mat(cv::Size(depth_frame->get_width(), depth_frame->get_height()), CV_16UC1, (void *)depth_frame->get_data(), cv::Mat::AUTO_STEP); };
+ cv::Mat getDepthFrame(){
+ cv::Mat meters;
+ cv::Mat values = cv::Mat(cv::Size(depth_frame->get_width(), depth_frame->get_height()), CV_16UC1, (void *)depth_frame->get_data(), cv::Mat::AUTO_STEP);
+ values.convertTo(meters,CV_32FC1, depth_frame->get_units());
+ return meters;
+ };
cv::Mat getColorFrame(){ return cv::Mat(cv::Size(color_frame->get_width(), color_frame->get_height()), CV_8UC3, (void *)color_frame->get_data(), cv::Mat::AUTO_STEP); };
void setCroppingMask(cv::Rect mask){ croppingMask = mask; };
cv::Rect getCroppingMask(){ return croppingMask; };
diff --git a/inc/sandbox.h b/inc/sandbox.h
index 99e39976b47e94eca98faf36a70c65aadbac7f55..2e69baeff0020af7a2a1c2dea7354e11a35989a7 100644
--- a/inc/sandbox.h
+++ b/inc/sandbox.h
@@ -23,7 +23,7 @@ class Sandbox{
cv::Mat getColorFrame();
cv::Mat getDepthFrame();
- void adjustProjection(cv::Mat &frame);
+ cv::Mat adjustProjection(cv::Mat frame);
void showImage(cv::Mat &image);
int loadConfig();
int loadConfigFrom(char *path);
diff --git a/src/components/beamerProjection.cpp b/src/components/beamerProjection.cpp
index aaf9ea1fcb7fb75b2dc6efce34a03d82518a7d15..8513b54f8fd49aaae793bc1e6ccad4084bf667f2 100644
--- a/src/components/beamerProjection.cpp
+++ b/src/components/beamerProjection.cpp
@@ -2,7 +2,7 @@
BeamerProjection::BeamerProjection(){
- adjustingMatrix = cv::getRotationMatrix2D(cv::Point(0, 0), 0, 1);
+ //adjustingMatrix = cv::getRotationMatrix2D(cv::Point(0, 0), 0, 1);
}
@@ -14,47 +14,100 @@ cv::Point2i BeamerProjection::rotatePixel(cv::Point2i pixel){
}
-cv::Point2i BeamerProjection::adjustPixel(int i, int j, float z, Camera *camera, cv::Point3f beamer_pos){
- //pixel to point 3d
- float coord[2] = {static_cast<float>(j), static_cast<float>(i)};
- cv::Point3f p = camera->deprojectPixelToPoint(coord, z / 1000.0);
- //calcul vector point to beamer
- cv::Vec3f dir(beamer_pos.x - p.x, beamer_pos.y - p.y, beamer_pos.z - p.z);
- float prop = (0.99 - p.z) / dir[2];
- p.x += dir[0] * prop;
- p.y += dir[1] * prop;
- p.z += dir[2] * prop;
- //point 3d to pixel
- return camera->projectPointToPixel(p);
-}
+void BeamerProjection::adjustFrame(cv::Mat depth, cv::Mat src, cv::Mat &dst, Camera *camera, cv::Point3f beamer_pos){
+
+ cv::Rect mask = camera->getCroppingMask();
+ cv::Size dst_size = dst.size();
+ cv::resize(src, src, 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(src, src, getMatchingSize(src, mask));
+ cv::resize(dst, dst, src.size());
+ //src.copyTo(dst);
-void BeamerProjection::adjustFrame(cv::Mat &depth, cv::Mat &src, cv::Mat &dst, Camera *camera, cv::Point3f beamer_pos){
+ // Browse the depth frame matching the cropping mask
+ // while adapting pixels's position to the beamer's position
+ for (int j = 0; j < mask.height; j++){
+ for (int i = 0; i < mask.width; i++){
- int nbChannel = src.channels();
- //transformation on all pixel
- //int64_t t1 = getTickCount();
- for (int i = 0; i < src.rows; i++)
- {
- for (int j = 0; j < src.cols; j++)
- {
- cv::Point pixelIJ(j, i);
+ // coordinates of the pixel relative to the orginial image taken from the camera
+ int base_x = mask.x+i;
+ int base_y = mask.y+j;
+ float z = depth.at<float>(j,i);
+
+ // pixels based on the original depth frame taken from the camera
+ cv::Point2i pixel = findMatchingPixel( base_x, base_y, z, camera, beamer_pos );
- cv::Point pixel = adjustPixel(i, j, static_cast<float>(depth.at<uint16_t>(pixelIJ)), camera, beamer_pos);
- if (pixel.x < dst.cols && pixel.y < dst.rows && pixel.x >= 0 && pixel.y >= 0)
- {
- if (nbChannel == 1)
- dst.at<char>(pixel) = src.at<char>(pixelIJ);
- else if (nbChannel == 3)
- dst.at<cv::Vec3b>(pixel) = src.at<cv::Vec3b>(pixelIJ);
+ // pixel relative to the cropping mask
+ pixel.x -= mask.x;
+ pixel.y -= mask.y;
+
+ if( (0<=pixel.x && pixel.x<mask.width) && (0<=pixel.y && pixel.y<mask.height) ){
+ // src and dst must be of same size
+ copyPixelsInto(pixel, dst, cv::Point2i(i,j), src, mask);
}
}
}
- //cout << "temps de calcul: " << (getTickCount() - t1) / getTickFrequency() << endl;
+
+ cv::resize(src, src, dst_size);
cv::warpAffine(dst, dst, adjustingMatrix, dst.size());
- cv::dilate(dst, dst, cv::Mat(), cv::Point(-1, -1), 2, 1, 1);
- cv::erode(dst, dst, cv::Mat(), cv::Point(-1, -1), 2, 1, 1);
+}
+
+cv::Size BeamerProjection::getMatchingSize(cv::Mat &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;
+ return bigSize;
+}
+
+// pixels coordinates are relative to the camera depth frame
+void BeamerProjection::copyPixelsInto(cv::Point2i pixel_dst, cv::Mat &dst, cv::Point2i pixel_src, cv::Mat &src, cv::Rect base){
+
+ if( src.size().width == dst.size().width && src.size().height == dst.size().height ){
+
+ // determines the size of the square of pixels to copy from the source matching the position of the pixel of the depth frame
+ // (since src and dst are multiple of the depth frame's size)
+ int ratio_w = src.size().width / base.width;
+ int ratio_h = src.size().height / base.height;
+
+ for(int j=0; j<ratio_h; j++){
+ for (int i=0; i<ratio_w; i++){
+
+ int x_dst = pixel_dst.x * ratio_w + i;
+ int y_dst = pixel_dst.y * ratio_h + j;
+ int x_src = pixel_src.x * ratio_w + i;
+ int y_src = pixel_src.y * ratio_h + j;
+
+ dst.at<cv::Vec3b>(y_dst, x_dst) = src.at<cv::Vec3b>(y_src, x_src);
+ }
+ }
+ }
+}
+
+/*
+ C : Camera position
+ B : Beamer position
+ P : Point computed by camera depth
+
+ Where
+ CP : distance from camera to point (value of depth_frame)
+ CB : distance from camera to beamer (beamer's position is relative to the camera)
+*/
+cv::Point2i BeamerProjection::findMatchingPixel(int i, int j, float z, Camera *camera, cv::Point3f CB){
+
+ float pixel[2] = {static_cast<float>(i), static_cast<float>(j)};
+ cv::Point3f CP = camera->deprojectPixelToPoint(pixel, z);
+ cv::Point3f BP = CB - CP;
+ //float alpha = (topSandboxDistance - CP.z) / BP.z;
+ float BAz = CB.z + BP.z;
+ float BEz = topSandboxDistance - CB.z;
+ float alpha = BEz / BAz;
+ cv::Point3f V = (alpha * BP);
+ cv::Point3f CV = V + CB;
+
+ return camera->projectPointToPixel(CV);
}
diff --git a/src/components/camera.cpp b/src/components/camera.cpp
index 7fcbb1073ae67dd9b89a379ef1d916cfd9f4194b..13970cd71be043b79572b7d5b73b7ca0832cfca0 100644
--- a/src/components/camera.cpp
+++ b/src/components/camera.cpp
@@ -89,7 +89,7 @@ void Camera::capture(){
}
}
-// match a pixel of depth frame to color frame
+// get the coordinates of the pixel matching the point relative to the real world
cv::Point2i Camera::projectPointToPixel(cv::Point3f point3D){
float point[3] = {point3D.x, point3D.y, point3D.z};
@@ -99,12 +99,12 @@ cv::Point2i Camera::projectPointToPixel(cv::Point3f point3D){
return cv::Point2i(pixel[0], pixel[1]);
}
-// match a pixel of color frame to depth frame
-cv::Point3f Camera::deprojectPixelToPoint(float coord[], float z1){
+// get the point relative to the real world matching the coordinates of the pixel
+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, z1);
+ rs2_deproject_pixel_to_point(p, &intr, coord, z);
return cv::Point3f(p[0], p[1], p[2]);
}
diff --git a/src/lib/sandbox.cpp b/src/lib/sandbox.cpp
index 2ebfa1a1dd1f6133ea458cb3d9d3c49206abe3fd..e19e74418b58d67aaa377ad0946aff946500aff2 100644
--- a/src/lib/sandbox.cpp
+++ b/src/lib/sandbox.cpp
@@ -16,51 +16,33 @@ Sandbox::Sandbox(){
cv::Mat Sandbox::getColorFrame(){
camera.capture();
- return camera.getColorFrame();
+ return camera.getColorFrame()(camera.getCroppingMask());
}
cv::Mat Sandbox::getDepthFrame(){
camera.capture();
- return camera.getDepthFrame();
+ return camera.getDepthFrame()(camera.getCroppingMask());
}
-void Sandbox::adjustProjection(cv::Mat &frame){
-
- static cv::Mat frameBeamer = cv::Mat(cv::Size(beamer.getWidth(), beamer.getHeight()), CV_8UC3);
+cv::Mat Sandbox::adjustProjection(cv::Mat frame){
cv::Mat depth = getDepthFrame();
- cv::Mat imageCalibrate = cv::Mat(depth.size(), CV_8UC3, cv::Scalar(0, 0, 0));
-
- cv::resize(frame, frame, depth.size());
-
+ cv::Mat imageCalibrate = cv::Mat(cv::Size(beamer.getWidth(), beamer.getHeight()), CV_8UC3, cv::Scalar(0, 0, 0));
projection.adjustFrame(depth, frame, imageCalibrate, &camera, beamer.getPosition());
- //flip to align frame with beamer
- //cv::flip(imageCalibrate, imageCalibrate, 1);
- //cv::flip(imageCalibrate, imageCalibrate, 0);
- cv::resize(imageCalibrate(camera.getCroppingMask()), frameBeamer, frameBeamer.size());
- frameBeamer.copyTo(frame);
-/*
- cv::Mat image = *frame;
- cv::Mat depth = getDepthFrame();
- cv::Size beamerResolution = cv::Size(beamer.getWidth(), beamer.getHeight());
- cv::Mat imageCalibrate(beamerResolution, CV_8UC3, cv::Scalar(0, 0, 0));
+ // frame after process
+ cv::dilate(imageCalibrate, imageCalibrate, cv::Mat(), cv::Point(-1, -1), 2, 1, 1);
+ cv::erode(imageCalibrate, imageCalibrate, cv::Mat(), cv::Point(-1, -1), 2, 1, 1);
- cv::resize(depth, depth, beamerResolution);
- cv::resize(image, image, beamerResolution);
- projection.adjustFrame(depth, image, imageCalibrate, camera, beamer.getPosition());
-
- imageCalibrate.copyTo(*frame);
- return frame;
-*/
+ return imageCalibrate;
}
void Sandbox::showImage(cv::Mat &image){
initWindowsFullScreen(defaultWindowsName);
- adjustProjection(image);
- cv::imshow(defaultWindowsName, image);
+ cv::Mat res = adjustProjection(image);
+ cv::imshow(defaultWindowsName, res);
}
int Sandbox::loadConfigFrom(char *path){
diff --git a/src/tools/sandboxConfig.cpp b/src/tools/sandboxConfig.cpp
index bfa736caef0a34541f9119f2e4210926c1a1bb22..a88493df546967e996526351ffebc2297239d085 100644
--- a/src/tools/sandboxConfig.cpp
+++ b/src/tools/sandboxConfig.cpp
@@ -162,11 +162,16 @@ int SandboxConfig::loadAdjustingMatrixFrom(char *path, BeamerProjection *project
for (int j=0; j<width; j++){
matrix.at<float>(i,j) = values.at(v_i);
v_i++;
+ std::cout << matrix.at<float>(i,j) << " ";
}
+ std::cout << std::endl;
}
-
+
+
+ std::cout << "load matrix" << std::endl;
projection->setAdjustingMatrix(matrix);
+ std::cout << "Ok matrix" << std::endl;
return 0;
}