Skip to content
Browse files

image area on wall is optimized (max area with given ratio), content …

…from main screen is copied to projector
  • Loading branch information...
1 parent 05e5848 commit ab1a71236c55370de261aed55f2bfe7f752182a2 Nikolas Engelhard committed May 14, 2012
Showing with 279 additions and 51 deletions.
  1. +15 −0 README
  2. +19 −3 include/projector_calibrator.h
  3. +68 −18 src/main.cpp
  4. +177 −30 src/projector_calibrator.cpp
View
15 README
@@ -1,5 +1,20 @@
Kinect-Touchscreen
+
+
+Current State:
+Calibration from a single image, content of Main Screen is shown on Projector.
+Size of screens has to be edited by hand, check main.cpp l. 53 and projector_calibrator.h l. 181
+
+
+
+
+
+
+
+
+
+
see the imgs/ to see the current status!
The goal of this project is to build a prototype for a large touchscreen
View
22 include/projector_calibrator.h
@@ -17,6 +17,9 @@
#include <opencv2/imgproc/imgproc.hpp>
#include <pcl/common/transform.h>
+typedef cv::Rect_<float> cv_RectF;
+
+
class Projector_Calibrator {
@@ -32,8 +35,7 @@ class Projector_Calibrator {
// a simple image for debugging
cv::Mat test_img;
- // apply on image
- cv::Mat warp_matrix;
+
// trafo cloud s.t. checkerboard is z=0, middle of board at x=y=0
// the first trafo is stored and used for all following frames
@@ -67,6 +69,8 @@ class Projector_Calibrator {
// remove all point from the input cloud where mask!=255
void applyMaskOnInputCloud(Cloud& out);
+
+
// fit a plane into the pointcloud
float fitPlaneToCloud(const Cloud& cloud, Eigen::Vector4f& model);
@@ -89,8 +93,17 @@ class Projector_Calibrator {
bool saveMat(const std::string name, const std::string filename, const cv::Mat& mat);
bool loadMat(const std::string name, const std::string filename, cv::Mat& mat);
+ void getCheckerboardArea(std::vector<cv::Point2i>& pts);
+
+// cv::Rect optimalRect;
+
+
public:
+ // apply on image
+ cv::Mat warp_matrix;
+
+ bool findOptimalProjectionArea(float ratio, cv_RectF& rect);
bool projMatorHomSet(){return projMatrixSet() || homOpenCVSet() || homSVDSet();}
@@ -106,9 +119,12 @@ class Projector_Calibrator {
bool warpMatrixSet(){ return warp_matrix.cols > 0;}
+ bool setupImageProjection(const cv_RectF& wall_area, const cv::Size& img_size);
+
bool setupImageProjection(float width_m, float height_m, float off_x_m, float off_y_m, const cv::Size& img_size);
bool setupImageProjection(float width_m, float off_x_m, float off_y_m, const cv::Size& img_size);
+ bool imageProjectionSet() { return warp_matrix.cols > 0; }
// save 3d positions (in wall-frame) of the last checkerboard detection
bool storeCurrent3DObservations();
@@ -161,7 +177,7 @@ void showFullscreenCheckerboard();
Projector_Calibrator(){
kinect_orientation_valid = false;
kinect_trafo_valid = false;
- C_checkboard_size = cv::Size(8,6);
+ C_checkboard_size = cv::Size(10,8);
C_proj_size = cv::Size(1152,864);
projector_image = cv::Mat(C_proj_size, CV_8UC3);
View
86 src/main.cpp
@@ -22,6 +22,7 @@
#include "user_input.h"
#include "projector_calibrator.h"
+#include <stdlib.h>
ros::Publisher pub;
ros::Publisher pub_full_moved;
@@ -40,9 +41,20 @@ Projector_Calibrator calibrator;
vector<float> measured_angles;
+
float kinect_tilt_angle_deg = 0;
bool kinect_tilt_angle_valid = false;
+
+// uncomment to show testImage, otherwise content of Main screen is shown on projector
+//#define SHOW_TEST_IMAGE
+
+#ifndef SHOW_TEST_IMAGE
+cv::Size mainScreenSize(1920,1200);
+cv_RectF optimalRect;
+#endif
+
+
ros::Subscriber sub_imu;
@@ -85,9 +97,8 @@ void on_mouse_projector( int event, int x, int y, int flags, void* param ){
-void callback(const ImageConstPtr& img_ptr, const sensor_msgs::PointCloud2ConstPtr& cloud_ptr){
+void imgCB(const ImageConstPtr& img_ptr, const sensor_msgs::PointCloud2ConstPtr& cloud_ptr){
- // ROS_INFO("callback");
// Kinect's orientation is needed to compute the transformation
if (!calibrator.isKinectTrafoSet() && !calibrator.isKinectOrientationSet()){
@@ -115,11 +126,11 @@ void callback(const ImageConstPtr& img_ptr, const sensor_msgs::PointCloud2ConstP
prog_state = GET_PROJECTION;
- if (prog_state == COLLECT_PATTERNS)
- cout << "State: COLLECT_PATTERNS" << endl;
-
- if (prog_state == GET_PROJECTION)
- cout << "State: GET_PROJECTION" << endl;
+// if (prog_state == COLLECT_PATTERNS)
+// cout << "State: COLLECT_PATTERNS" << endl;
+//
+// if (prog_state == GET_PROJECTION)
+// cout << "State: GET_PROJECTION" << endl;
// look for corners
@@ -147,6 +158,13 @@ void callback(const ImageConstPtr& img_ptr, const sensor_msgs::PointCloud2ConstP
calibrator.setInputCloud(cloud);
calibrator.computeKinectTransformation();
+#ifndef SHOW_TEST_IMAGE
+ optimalRect.width = 0;
+ if (!calibrator.findOptimalProjectionArea(mainScreenSize.width*1.0/mainScreenSize.height, optimalRect)){
+ ROS_ERROR("Could not find optimal Projection Area!");
+ }
+#endif
+
prog_state = COLLECT_PATTERNS;
}
@@ -156,13 +174,16 @@ void callback(const ImageConstPtr& img_ptr, const sensor_msgs::PointCloud2ConstP
// Cloud transformed_cloud;
if (prog_state == COLLECT_PATTERNS){
+
+
+
calibrator.storeCurrent3DObservations();
//if (!calibrator.homOpenCVSet())
- calibrator.computeHomography_SVD();
+ calibrator.computeHomography_SVD();
//if (!calibrator.homSVDSet())
- calibrator.computeHomography_OPENCV();
+ calibrator.computeHomography_OPENCV();
}
@@ -186,19 +207,24 @@ void callback(const ImageConstPtr& img_ptr, const sensor_msgs::PointCloud2ConstP
if (!calibrator.warpMatrixSet()){
+#ifdef SHOW_TEST_IMAGE
float width = 0.8;
float height = width/calibrator.getTestImg()->cols*calibrator.getTestImg()->rows; // use ratio of input-image
-
float off_x = -width/2;
float off_y = -height/2;
-
calibrator.setupImageProjection(width,height, off_x, off_y, calibrator.getTestImg()->size());
+#else
+ calibrator.setupImageProjection(optimalRect, mainScreenSize );
+#endif
+
}
+#ifdef SHOW_TEST_IMAGE
if (calibrator.warpMatrixSet()){
calibrator.showUnWarpedImage(*calibrator.getTestImg());
-
}
+#endif
+
}
if (calibrator.isKinectTrafoSet()){
@@ -236,12 +262,10 @@ void imu_CB(const sensor_msgs::ImuConstPtr& imu_ptr){
int main(int argc, char ** argv)
{
+
ros::init(argc, argv, "subscriber");
- //
// calibrator.initFromFile();
- //
- // return 0;
ros::NodeHandle nh;
cv::namedWindow("camera", 1);
@@ -260,12 +284,38 @@ int main(int argc, char ** argv)
message_filters::Subscriber<Image> image_sub(nh, "/camera/rgb/image_color", 2);
message_filters::Subscriber<PointCloud2> cloud_sub(nh, "/camera/rgb/points", 2);
Synchronizer<policy> sync(policy(2), image_sub, cloud_sub);
- sync.registerCallback(boost::bind(&callback, _1, _2));
+ sync.registerCallback(boost::bind(&imgCB, _1, _2));
+
+
+
+
+ ros::Rate r(30);
+
+ while (ros::ok()){
+ ros::spinOnce();
+ r.sleep();
+#ifndef SHOW_TEST_IMAGE
+
+ if (calibrator.imageProjectionSet()){
+
+ // cout << "warp " << endl << calibrator.warp_matrix << endl;
+ //
+ // ROS_INFO("Streaming");
+ system("xwd -root | convert - /tmp/screenshot.jpg");
+ cv::Mat screen = cv::imread("/tmp/screenshot.jpg");
+
+ cv::Mat primary_screen = screen(cv::Range(0,mainScreenSize.height), cv::Range(0,mainScreenSize.width));
+
+ calibrator.showUnWarpedImage(primary_screen);
+ }
+#endif
+
+ }
- ros::spin();
- cvDestroyWindow("camera");
+ // ros::spin();
+ cv::destroyAllWindows();
return 0;
}
View
207 src/projector_calibrator.cpp
@@ -68,17 +68,6 @@ void Projector_Calibrator::initFromFile(){
void Projector_Calibrator::drawCheckerboard(cv::Mat& img, const cv::Size size, vector<cv::Point2f>& corners_2d){
- // get region of checkerboard
- // float minx,maxx, miny, maxy;
- // minx = miny = 1e5; maxx = maxy = -1e5;
- // for (int i=0; i<mask->cols; ++i)
- // for (int j=0; j<mask->rows; ++j){
- // if (mask->at<uchar>(j,i) == 0) continue;
- // minx = min(minx,i*1.f); miny = min(miny,j*1.f);
- // maxx = max(maxx,i*1.f); maxy = max(maxy,j*1.f);
- // }
-
-
corners_2d.clear();
// draw white border with this size
@@ -130,6 +119,17 @@ bool Projector_Calibrator::setupImageProjection(float width_m, float off_x_m, fl
}
+
+
+bool Projector_Calibrator::setupImageProjection(const cv_RectF& wall_area, const cv::Size& img_size){
+
+ if (wall_area.width == 0){
+ ROS_ERROR("setupImageProjection: wall area has width of 0!");
+ return false;
+ }
+ return setupImageProjection(wall_area.width, wall_area.height, wall_area.x, wall_area.y, img_size);
+}
+
bool Projector_Calibrator::setupImageProjection(float width_m, float height_m, float off_x_m, float off_y_m, const cv::Size& img_size){
@@ -153,7 +153,7 @@ bool Projector_Calibrator::setupImageProjection(float width_m, float height_m, f
cv::Mat px_to_world(cv::Size(3,4), CV_64FC1);
px_to_world.setTo(0);
- ROS_INFO("Computing warp from Projection Matrix!");
+// ROS_INFO("Computing warp from Projection Matrix!");
px_to_world.at<double>(3,2) = 1;
px_to_world.at<double>(0,0) = width_m/width_px;
@@ -164,7 +164,7 @@ bool Projector_Calibrator::setupImageProjection(float width_m, float height_m, f
warp_matrix = proj_Matrix*px_to_world;
warp_matrix /= warp_matrix.at<double>(2,2); // defined up to scale
- cout << "Warp_matrix: " << endl << warp_matrix << endl;
+// cout << "Warp_matrix: " << endl << warp_matrix << endl;
return true;
}
@@ -173,7 +173,7 @@ bool Projector_Calibrator::setupImageProjection(float width_m, float height_m, f
cv::Mat px_to_world(cv::Size(3,3), CV_64FC1);
px_to_world.setTo(0);
- ROS_INFO("Computing warp from Homography!");
+// ROS_INFO("Computing warp from Homography!");
px_to_world.at<double>(2,2) = 1;
px_to_world.at<double>(0,0) = width_m/width_px;
@@ -188,7 +188,7 @@ bool Projector_Calibrator::setupImageProjection(float width_m, float height_m, f
warp_matrix /= warp_matrix.at<double>(2,2); // defined up to scale
- cout << "Warp_matrix" << endl << warp_matrix << endl;
+// cout << "Warp_matrix" << endl << warp_matrix << endl;
return true;
@@ -209,13 +209,9 @@ void Projector_Calibrator::showUnWarpedImage(const cv::Mat& img){
IplImage img_ipl = projector_image;
cvShowImage("fullscreen_ipl", &img_ipl);
-
}
-
-
-
void Projector_Calibrator::computeHomography_OPENCV(){
ROS_WARN("COMPUTING HOMOGRAPHY WITH openCV");
@@ -251,7 +247,7 @@ void Projector_Calibrator::computeHomography_OPENCV(){
#endif
- cout << "Homography with OpenCV: " << endl << hom_CV << endl;
+ // cout << "Homography with OpenCV: " << endl << hom_CV << endl;
// compute error:
float error = 0;
@@ -373,7 +369,7 @@ void Projector_Calibrator::computeHomography_SVD(){
// 2d = H*3d H*(x,y,1)
- cout << "Homography with SVD: " << endl << hom_SVD << endl;
+// cout << "Homography with SVD: " << endl << hom_SVD << endl;
// compute error:
@@ -515,6 +511,8 @@ bool Projector_Calibrator::findCheckerboardCorners(){
ROS_WARN("Could not find a checkerboard!");
return false;
}
+
+
cv::cvtColor(input_image, gray, CV_BGR2GRAY);
cv::cornerSubPix(gray, corners, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
return true;
@@ -608,34 +606,183 @@ void Projector_Calibrator::computeKinectTransformation(){
saveAffineTrafo(kinect_trafo,fn);
ROS_INFO("Wrote kinect_trafo to %s", fn);
+
+ pcl::getTransformedPointCloud(input_cloud,kinect_trafo,cloud_moved);
+
kinect_trafo_valid = true;
}
-// TODO: assumption: only rectangular mask...
+
+void Projector_Calibrator::getCheckerboardArea(vector<cv::Point2i>& pts){
+
+ pts.clear();
+ if (corners.size() == 0){
+ ROS_WARN("getCheckerboardArea: no corners!"); return;
+ }
+
+
+ float l = (corners[1].x-corners[0].x);
+
+ cv::Point2i p = corners[0];
+ pts.push_back(cv::Point2i(p.x-l,p.y-l));
+
+ p = corners[C_checkboard_size.width-1];
+ pts.push_back(cv::Point2i(p.x+l,p.y-l));
+
+ p = corners[C_checkboard_size.height*C_checkboard_size.width-1];
+ pts.push_back(cv::Point2i(p.x+l,p.y+l));
+
+ p = corners[C_checkboard_size.width*(C_checkboard_size.height-1)];
+ pts.push_back(cv::Point2i(p.x-l,p.y+l));
+}
+
+
+
+bool Projector_Calibrator::findOptimalProjectionArea(float ratio, cv_RectF& rect){
+
+ if(!kinect_trafo_valid){
+ ROS_WARN("findOptimalProjectionArea: no kinect trafo!"); return false;
+ }
+
+ if (corners.size() == 0){
+ ROS_WARN("findOptimalProjectionArea: no corners!"); return false;
+ }
+
+ if (cloud_moved.size() == 0){
+ ROS_WARN("findOptimalProjectionArea: no input cloud!"); return false;
+ }
+
+
+ vector<cv::Point2i> c;
+ getCheckerboardArea(c);
+// cv::fillConvexPoly(mask,c,CV_RGB(255,255,255));
+
+
+ // get 3d coordinates of corners in the wall-system
+ vector<cv::Point2f> rotated;
+ float min_x = 100;
+ float min_y = 100;
+ for (uint i=0; i<c.size(); ++i){
+ pcl_Point p = cloud_moved.at(c[i].x,c[i].y);
+ rotated.push_back(cv::Point2f(p.x,p.y));
+ min_x = min(min_x, p.x);
+ min_y = min(min_y, p.y);
+ // ROS_INFO("pre: %f %f", rotated[i].x, rotated[i].y);
+ }
+
+
+ vector<cv::Point2i> pt_i;
+ int max_x, max_y;
+ max_x = max_y = -1;
+ // ROS_INFO("min: %f %f", min_x, min_y);
+ for (uint i=0; i<c.size(); ++i){
+ rotated[i] = cv::Point2f((rotated[i].x-min_x)*100,(rotated[i].y-min_y)*100); // in cm <=> 1px
+ pt_i.push_back(cv::Point2i(rotated[i].x,rotated[i].y));
+ max_x = max(max_x, pt_i[i].x);
+ max_y = max(max_y, pt_i[i].y);
+ // ROS_INFO("post: %f %f", rotated[i].x, rotated[i].y);
+ }
+
+ cv::Mat search_img(max_y,max_x,CV_8UC1); search_img.setTo(0);
+ cv::fillConvexPoly(search_img,pt_i,CV_RGB(255,255,255));
+
+
+//#define SHOW_SEARCH_IMAGE
+
+#ifdef SHOW_SEARCH_IMAGE
+ cv::namedWindow("search_img",1);
+ cv::imshow("search_img", search_img);
+ cv::waitKey(10);
+#endif
+
+ // find largest rect in white area:
+
+ // ratio = width/height
+ bool finished = false;
+
+ float step = 0.03; // check every X m
+
+ float width, height; int x, y;
+ for (width = max_x; width > 0 && !finished; width -= step){ // check every 5 cm
+ height = width/ratio;
+
+ // ROS_INFO("Width: %f, height: %f", width, height);
+
+ // find fitting left upper corner (sliding window)
+ for (x = 0; x < max_x-width && !finished; x+= step*100){
+ for (y = 0; y < max_y-height; y+=step*100){
+ // ROS_INFO("Checking x = %i, y = %i", x, y);
+
+ int x_w = x+width; int y_w = y+height;
+ assert(x >= 0 && y >= 0 && x_w < search_img.cols && y< search_img.rows);
+ // check if all corners are withing white area:
+ if (search_img.at<uchar>(y,x) == 0) continue;
+ if (search_img.at<uchar>(y,x_w) == 0) continue;
+ if (search_img.at<uchar>(y_w,x_w) == 0) continue;
+ if (search_img.at<uchar>(y_w,x) == 0) continue;
+ // ROS_INFO("Found fitting pose (w,h: %f %f)", width, height);
+#ifdef SHOW_SEARCH_IMAGE
+ cv::rectangle(search_img, cv::Point(x,y), cv::Point(x_w, y_w), CV_RGB(125,125,125));
+#endif
+
+ finished = true; // break outer loops
+ break;
+ } // for y
+ } // for x
+ } // for width
+
+#ifdef SHOW_SEARCH_IMAGE
+ cv::imshow("search_img", search_img);
+ cv::waitKey(10);
+#endif
+
+ if (!finished) return false;
+
+ // restore pose in wall_frame
+ rect.width = width/100;
+ rect.height = height/100;
+
+ rect.x = x/100.0+min_x;
+ rect.y = y/100.0+min_y;
+
+ // show area on input image:
+
+// ROS_INFO("Optimal rect: x,y: %f %f, w,h: %f %f", rect.x, rect.y, rect.width, rect.height);
+
+ return true;
+
+}
+
+
+
+
void Projector_Calibrator::createMaskFromDetections(){
if (corners.size() != uint(C_checkboard_size.width*C_checkboard_size.height)){
ROS_INFO("can't create mask if the corners were not detected!"); return; }
mask = cv::Mat(cv::Size(640,480), CV_8UC1); mask.setTo(0);
- int w = C_checkboard_size.width; int h = C_checkboard_size.height;
- int l = corners[1].x-corners[0].x; // length of a square
+ vector<cv::Point2i> c;
+ getCheckerboardArea(c);
+
+ // for (uint i=0; i<c.size(); ++i){
+ // cv::circle(mask, c[i] ,10,CV_RGB(255,255,255));
+ // ROS_INFO("%i %i", c[i].x, c[i].y);
+ // }
- float min_x = corners[0].x-l; float min_y = corners[0].y-l;
- float max_x = corners[w*h-1].x+l; float max_y = corners[w*h-1].y+l;
+ cv::fillConvexPoly(mask,c,CV_RGB(255,255,255));
- cv::rectangle(mask, cv::Point(min_x,min_y), cv::Point(max_x,max_y), CV_RGB(255,255,255),-1);
ROS_INFO("Writing kinect_mask to data/kinect_mask.png");
cv::imwrite("data/kinect_mask.png", mask);
//
//
- // cv::namedWindow("Mask on Kinect Image");
- // cv::imshow("Mask on Kinect Image", mask);
- // cv::waitKey(-1);
+ // cv::namedWindow("Mask on Kinect Image");
+ // cv::imshow("Mask on Kinect Image", mask);
+ // cv::waitKey(-1);
}

0 comments on commit ab1a712

Please sign in to comment.
Something went wrong with that request. Please try again.