diff --git a/inc/camera.h b/inc/camera.h
index fd3998f9ff9184446be812f79e1771e2cdc5a5c1..f4553b008542e42eab522580b1a9c9c7b61b5fd8 100644
--- a/inc/camera.h
+++ b/inc/camera.h
@@ -42,6 +42,10 @@ class Camera{
void stop();
cv::Point3f deprojectPixelToPoint(float coord[], float z1);
cv::Point2i projectPointToPixel(cv::Point3f point3D);
+
+ void deprojectPixelToPoint(cv::Mat_<float> &depth, cv::Mat_<cv::Point3f> &real);
+ void projectPointToPixel(cv::Mat_<cv::Point3f> &real, cv::Mat_<cv::Point2i> &pixels);
+
void capture();
void printCroppingMask();
diff --git a/inc/projection.h b/inc/projection.h
index 2cb0589818c136f803f0f6c040395aa0a7d4e013..8b709575ecb6bf9ad17474979d667dd3c61d9a07 100644
--- a/inc/projection.h
+++ b/inc/projection.h
@@ -13,9 +13,11 @@ class Projection{
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);
+ void buildFrame2(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_<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);
+ cv::Size getMatchingSize(cv::Mat &src, cv::Mat &base);
+
public:
Projection();
@@ -28,6 +30,7 @@ class Projection{
cv::Point2i rotatePixel(cv::Point2i pixel);
cv::Point2i revertRotatePixel(cv::Point2i pixel);
void adjustFrame(cv::Mat_<float> depth, cv::Mat_<cv::Vec3b> src, cv::Mat_<cv::Vec3b> &dst, Camera *camera, cv::Point3f beamer_pos);
+ void adjustFrame2(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/src/components/camera.cpp b/src/components/camera.cpp
index 4e90345556492df6156f2c6625ccf8a01641ce49..508d4ddc8abeac1696fed0bba50ca83b8f5fe91f 100644
--- a/src/components/camera.cpp
+++ b/src/components/camera.cpp
@@ -114,6 +114,64 @@ cv::Point3f Camera::deprojectPixelToPoint(float coord[], float z){
}
+
+void Camera::deprojectPixelToPoint(cv::Mat_<float> &depth, cv::Mat_<cv::Point3f> &real){
+ /*
+ // RS2_DISTORTION_BROWN_CONRADY
+
+ float x = (pixel[0] - intrin->ppx) / intrin->fx;
+ float y = (pixel[1] - intrin->ppy) / intrin->fy;
+
+ point[0] = depth * x;
+ point[1] = depth * y;
+ point[2] = depth;
+
+ */
+
+ float ppx = intr_profile.ppx * depth.size().width / intr_profile.width;
+ float ppy = intr_profile.ppy * depth.size().height / intr_profile.height;
+ float fx = depth.size().width * intr_profile.fx / intr_profile.width;
+ float fy = depth.size().height * intr_profile.fy / intr_profile.height;
+
+ for (int j = 0; j < depth.rows; j++){
+ for (int i = 0; i < depth.cols; i++){
+
+ float x = (i - ppx) / fx;
+ float y = (j - ppy) / fy;
+ float depth_ij = depth.at<float>(j,i);
+
+ real.at<cv::Point3f>(j,i) = cv::Point3f( depth_ij*x, depth_ij*y, depth_ij );
+ }
+ }
+}
+void Camera::projectPointToPixel(cv::Mat_<cv::Point3f> &real, cv::Mat_<cv::Point2i> &pixels){
+ /*
+ // RS2_DISTORTION_BROWN_CONRADY
+
+ float x = point[0] / point[2], y = point[1] / point[2];
+
+ pixel[0] = x * intrin->fx + intrin->ppx;
+ pixel[1] = y * intrin->fy + intrin->ppy;
+ */
+
+ float ppx = intr_profile.ppx * real.size().width / intr_profile.width;
+ float ppy = intr_profile.ppy * real.size().height / intr_profile.height;
+ float fx = real.size().width * intr_profile.fx / intr_profile.width;
+ float fy = real.size().height * intr_profile.fy / intr_profile.height;
+
+ for (int j = 0; j < pixels.rows; j++){
+ for (int i = 0; i < pixels.cols; i++){
+
+ cv::Point3f pt = real.at<cv::Point3f>(j,i);
+ float x = pt.x / pt.z;
+ float y = pt.y / pt.z;
+
+ pixels.at<cv::Point2i>(j,i) = cv::Point2i( x*fx+ppx, y*fy+ppy );
+ }
+ }
+}
+
+
void Camera::printCroppingMask(){
cv::Rect mask = getCroppingMask();
std::cout << "(" << mask.x << "," << mask.y << ") + " << mask.width << "x" << mask.height << std::endl;
diff --git a/src/components/projection.cpp b/src/components/projection.cpp
index ed61e759dd5c97a82183cdda11bde67d6312669d..af8cc0c2f6dfa5400d8c432174d31b86d567e612 100644
--- a/src/components/projection.cpp
+++ b/src/components/projection.cpp
@@ -31,21 +31,19 @@ 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_<float> depth, cv::Mat_<cv::Vec3b> src, cv::Mat_<cv::Vec3b> &dst, Camera *camera, cv::Point3f beamer_pos){
- cv::Rect mask = camera->getCroppingMask();
-
// resize the frames to be a multiple of the camera size :
// src.size = n * camera.depth.size , where n is uint > 0
- static cv::Mat_<cv::Vec3b> resized_dst = cv::Mat_<cv::Vec3b>(getMatchingSize(dst, mask));
+ static cv::Mat_<cv::Vec3b> resized_dst = cv::Mat_<cv::Vec3b>(getMatchingSize(dst, depth));
cv::resize(dst, resized_dst, resized_dst.size());
cv::resize(src, src, resized_dst.size());
- static cv::Mat_<cv::Point2i> deprojectMap = cv::Mat_<cv::Point2i>(mask.height, mask.width);
+ static cv::Mat_<cv::Point2i> deprojectMap = cv::Mat_<cv::Point2i>(depth.size().height, depth.size().width);
deprojectMap = cv::Point2i(-1,-1);
- static cv::Mat_<cv::Point2i> frameMapMask = cv::Mat_<cv::Point2i>(mask.height, mask.width, cv::Point2i(-1,-1));
+ static cv::Mat_<cv::Point2i> frameMapMask = cv::Mat_<cv::Point2i>(depth.size().height, depth.size().width, cv::Point2i(-1,-1));
frameMapMask = cv::Point2i(-1,-1);
- deprojectPixelsFromDepth(depth, mask, camera, beamer_pos, deprojectMap);
+ deprojectPixelsFromDepth(depth, camera->getCroppingMask(), camera, beamer_pos, deprojectMap);
filterLowestDeprojectedPoints(depth, deprojectMap, frameMapMask);
@@ -57,6 +55,54 @@ void Projection::adjustFrame(cv::Mat_<float> depth, cv::Mat_<cv::Vec3b> src, cv:
cv::warpAffine(dst, dst, adjustingMatrix, dst.size());
}
+void Projection::adjustFrame2(cv::Mat_<float> depth, cv::Mat_<cv::Vec3b> src, cv::Mat_<cv::Vec3b> &dst, Camera *camera, cv::Point3f beamer_pos){
+
+ cv::resize(depth, depth, dst.size());
+ cv::resize(src, src, dst.size());
+
+ static cv::Mat_<cv::Point2i> deprojectMap = cv::Mat_<cv::Point2i>(depth.size());
+ deprojectMap = cv::Point2i(-1,-1);
+
+ static cv::Mat_<cv::Point2i> frameMapMask = cv::Mat_<cv::Point2i>(depth.size(), cv::Point2i(-1,-1));
+ frameMapMask = cv::Point2i(-1,-1);
+
+
+ cv::Point3f CB = beamer_pos;
+ static cv::Mat_<cv::Point3f> CP = cv::Mat_<cv::Point3f>(depth.size());
+ static cv::Mat_<cv::Point3f> BP = cv::Mat_<cv::Point3f>(depth.size());
+ static cv::Mat_<cv::Point3f> BA = cv::Mat_<cv::Point3f>(depth.size());
+ static cv::Mat_<cv::Point3f> BV = cv::Mat_<cv::Point3f>(depth.size());
+ static cv::Mat_<cv::Point3f> CV = cv::Mat_<cv::Point3f>(depth.size());
+ static cv::Mat_<float> alpha = cv::Mat_<float>(depth.size());
+
+ camera->deprojectPixelToPoint(depth, CP);
+/*
+ BP = CB;
+ BP -= CP;
+ BP *= -1;
+
+ BA = cv::Point3f(0,0,0);
+ BA.z = BP.z;
+
+ float BE = distanceTopSandbox - CB.z;
+
+ alpha = cv::norm(BA);
+ alpha /= BE;
+
+ BV = alpha * BP;
+ CV = CB + BV;
+*/
+ camera->projectPointToPixel(CV, deprojectMap);
+
+
+ filterLowestDeprojectedPoints(depth, deprojectMap, frameMapMask);
+
+ buildFrame2(frameMapMask, src, dst);
+
+ cv::warpAffine(dst, dst, adjustingMatrix, dst.size());
+}
+
+
/*
@@ -131,11 +177,27 @@ void Projection::buildFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &frame
}
}
+void Projection::buildFrame2(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++){
+
+ cv::Point2i pixel_src = frameMapMask.at<cv::Point2i>(j,i);
+ cv::Point2i pixel_dst = cv::Point2i(i,j);
+
+ if( (0<=pixel_src.x && pixel_src.x<frameMapMask.cols) && (0<=pixel_src.y && pixel_src.y<frameMapMask.rows) ){
+ // src and dst must be of same size
+ dst.at<cv::Vec3b>(pixel_dst) = src.at<cv::Vec3b>(pixel_src);
+ }
+ }
+ }
+}
+
-cv::Size Projection::getMatchingSize(cv::Mat_<cv::Vec3b> &src, cv::Rect base){
+cv::Size Projection::getMatchingSize(cv::Mat &src, cv::Mat &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;
+ bigSize.width = (src.size().width % base.size().width == 0) ? src.size().width : src.size().width - (src.size().width % base.size().width) + base.size().width;
+ bigSize.height = (src.size().height % base.size().height == 0) ? src.size().height : src.size().height - (src.size().height % base.size().height) + base.size().height;
return bigSize;
}
@@ -178,8 +240,8 @@ cv::Point2i Projection::findMatchingPixel(int i, int j, float z, Camera *camera,
float pixel[2] = {static_cast<float>(i), static_cast<float>(j)};
cv::Point3f CP = camera->deprojectPixelToPoint(pixel, z);
- cv::Point3f BP = CB - CP;
- float BAz = CB.z + BP.z;
+ cv::Point3f BP = CP - CB;
+ float BAz = BP.z;
float BEz = distanceTopSandbox - CB.z;
float alpha = BEz / BAz;
cv::Point3f V = (alpha * BP);