Skip to content

Commit

Permalink
Version 1.0. Now with with border, everything is calibrated, use can …
Browse files Browse the repository at this point in the history 
…click on kinect image to color the corresponding point on the plane
  • Loading branch information
Nikolas Engelhard committed Mar 1, 2012
1 parent 5db4907 commit 459f7ed
Show file tree
Hide file tree
Showing 2 changed files with 144 additions and 52 deletions.
82 changes: 70 additions & 12 deletions src/calibration.cpp
View file
Expand Up @@ -9,9 +9,8 @@

void computeHomography(const vector<CvPoint2D32f>& corners_2d, const Cloud& corners_3d, CvMat* H){

if (H->cols != 3)
H = cvCreateMat(3,3,CV_32FC1);

assert(H && H->cols == 3);

uint N = corners_3d.points.size();
assert(N == corners_2d.size());
Expand All @@ -22,22 +21,47 @@ void computeHomography(const vector<CvPoint2D32f>& corners_2d, const Cloud& corn
if (cnt>N*0.01) { ROS_WARN("found %f %% points with dist > 2cm", cnt*100.0/N); }



// create Matrices
CvMat* src = cvCreateMat(2,N,CV_32FC1);
CvMat* dst = cvCreateMat(2,N,CV_32FC1);

for (uint i=0; i<N; ++i){
cvSet2D(src,0,i,cvScalarAll(corners_2d.at(i).x));
cvSet2D(src,1,i,cvScalarAll(corners_2d.at(i).y));
cvSet2D(src,0,i,cvScalarAll(corners_3d.at(i).x));
cvSet2D(src,1,i,cvScalarAll(corners_3d.at(i).y));

cvSet2D(dst,0,i,cvScalarAll(corners_2d.at(i).x));
cvSet2D(dst,1,i,cvScalarAll(corners_2d.at(i).y));

// printf("from %f %f to %f %f \n", corners_3d.at(i).x,corners_3d.at(i).y,corners_2d.at(i).x,corners_2d.at(i).y);

cvSet2D(dst,0,i,cvScalarAll(corners_3d.at(i).x));
cvSet2D(dst,1,i,cvScalarAll(corners_3d.at(i).y));
}

// 2d = H*3d H*(x,y,1)

cvFindHomography(src, dst, H, 0); // use default mode with no outlier handling

// CvMat* p_ = cvCreateMat(3,1,CV_32FC1);
// CvMat* p_proj = cvCreateMat(3,1,CV_32FC1);
//
// // compute residual error:
// for (uint i=0; i<N; ++i){
//
// cvSet1D(p_,0,cvScalarAll(corners_3d.at(i).x));
// cvSet1D(p_,1,cvScalarAll(corners_3d.at(i).y));
// cvSet1D(p_,2,cvScalarAll(1));
//
// cvMatMul(H, p_,p_proj);
//
// float x_b = cvGet1D(p_proj,0).val[0];
// float y_b = cvGet1D(p_proj,1).val[0];
// float z_b = cvGet1D(p_proj,2).val[0];
//
// printf("input: %f %f\n", corners_3d.at(i).x, corners_3d.at(i).y);
// printf("output: %f %f %f\n", x_b, y_b, z_b);
// printf("expected: %f %f\n\n", corners_2d.at(i).x,corners_2d.at(i).y);
//
// }

}


Expand All @@ -54,20 +78,54 @@ void drawCheckerboard(IplImage* img, const IplImage* mask, CvSize size, vector<C
maxx = max(maxx,i*1.f); maxy = max(maxy,j*1.f);
}

// ROS_INFO("x: %f %f, y: %f %f", minx,maxx, miny, maxy);
// ROS_INFO("x: %f %f, y: %f %f", minx,maxx, miny, maxy);


// draw white border with this size
// "Note: the function requires some white space (like a square-thick border,
// the wider the better) around the board to make the detection more robust in various environment"
float border = 40;

float width = (maxx-minx-2*border)/(size.width+1);
float height = (maxy-miny-2*border)/(size.height+1);

// ROS_INFO("w,h: %f %f", width, height);




float width = (maxx-minx)/(size.width+1);
float height = (maxy-miny)/(size.height+1);
cvSet(img, cvScalarAll(255)); // all white

// cvSet(img, cvScalarAll(255)); // all white
minx += border;
miny += border;

// start with black square
for (int j = 0; j<=size.height; j++)
for (int i = (j%2); i<size.width+1; i+=2){

CvPoint lu = cvPoint(minx+i*width,miny+j*height);
CvPoint rl = cvPoint(minx+(i+1)*width,miny+(j+1)*height);
cvRectangle(img, lu, rl ,cvScalarAll(0), -1);
corners_2d.push_back(cvPoint2D32f(rl.x, rl.y));


CvPoint ru = cvPoint(rl.x,lu.y);

if (j==0) continue;

if (i>0){
corners_2d.push_back(cvPoint2D32f(lu.x, lu.y));
// cvCircle(img, cvPoint(lu.x, lu.y),20, CV_RGB(255,0,0),3);
}

if (i<size.width){
corners_2d.push_back(cvPoint2D32f(ru.x, ru.y));
// cvCircle(img, cvPoint(ru.x, ru.y),20, CV_RGB(255,0,0),3);
}

}



assert(int(corners_2d.size()) == size.width*size.height);

}
114 changes: 74 additions & 40 deletions src/main.cpp
View file
Expand Up @@ -44,7 +44,9 @@ using namespace message_filters;
int cnt=0;
vector<CvPoint> mask_points;

CvMat* proj_Hom;
Cloud full_cloud_moved;

CvMat* proj_Hom = cvCreateMat(3,3,CV_32FC1);


const CvSize C_checkboard_size = cvSize(6,4);
Expand All @@ -59,13 +61,15 @@ bool kinect_trafo_valid = false;
// region of the projector image where the checkerboard will be drawn
IplImage* board_mask = NULL;
IplImage* projector_image = NULL;
const CvSize C_proj_size = cvSize(1280,640);
const CvSize C_proj_size = cvSize(1280,1024);
//const CvSize C_proj_size = cvSize(640,480);

vector<CvPoint2D32f> projector_corners; // position of internal corners on the projector image


// define this to compute the depth-mask from the detected checkerbord
// if not defined, the user can select four points manually to define the mask
// #define MASK_FROM_DETECTIONS
#define MASK_FROM_DETECTIONS


void on_mouse( int event, int x, int y, int flags, void* param ){
Expand All @@ -75,29 +79,38 @@ void on_mouse( int event, int x, int y, int flags, void* param ){


void on_mouse_projector( int event, int x, int y, int flags, void* param ){

Cloud* cloud = (Cloud*)param;
if (event != CV_EVENT_LBUTTONUP) return;

// ROS_INFO("cloud has %i points", (int) cloud->points.size());

// get 3d point at this position
assert(cloud);

Point p = cloud->at(x,y);

if (p.x != p.x) {ROS_WARN("Point has no depth!"); return;}

assert(proj_Hom);
// ROS_INFO("Clicked point: %i %i, #d: %f %f %f", x,y, p.x,p.y,p.z);

// use homography to move from plane to projector
CvMat* p_ = cvCreateMat(1,3,CV_32FC1);
CvMat* p_ = cvCreateMat(3,1,CV_32FC1);
cvSet1D(p_,0,cvScalarAll(p.x));
cvSet1D(p_,1,cvScalarAll(p.y));
cvSet1D(p_,2,cvScalarAll(p.z));
cvSet1D(p_,2,cvScalarAll(1));


CvMat* p_proj = cvCreateMat(3,1,CV_32FC1);

CvMat p_proj;
cvMul(proj_Hom, p_,&p_proj);

float x_b = cvGet1D(&p_proj,0).val[0];
float y_b = cvGet1D(&p_proj,1).val[0];
float z_b = cvGet1D(&p_proj,1).val[0];
cvMatMul(proj_Hom, p_,p_proj);

ROS_INFO("Projected: %f %f %f", x_b, y_b, z_b);
float x_b = cvGet1D(p_proj,0).val[0];
float y_b = cvGet1D(p_proj,1).val[0];
// float z_b = cvGet1D(p_proj,2).val[0];

// ROS_INFO("Projected: %f %f %f", x_b, y_b, z_b);

cvCircle(projector_image, cvPoint(x_b,y_b),20, CV_RGB(255,0,0),-1);

Expand Down Expand Up @@ -138,20 +151,18 @@ void showMaskOnImage(IplImage* col, IplImage* mask){

void callback(const ImageConstPtr& img_ptr, const sensor_msgs::PointCloud2ConstPtr& cloud_ptr){

// cout << "CALLBACK" << endl;
sensor_msgs::CvBridge bridge;

sensor_msgs::CvBridge bridge;


IplImage* col = bridge.imgMsgToCv(img_ptr, "bgr8");

// // fake projector:
// // IplImage* board = cvCloneImage(col);
// vector<CvPoint2D32f> pts;
// drawCheckerboard(col,mask_image,C_checkboard_size, pts);
//// cvShowImage("board", board);
//

// // fake projector:
// // IplImage* board = cvCloneImage(col);
// vector<CvPoint2D32f> pts;
// drawCheckerboard(col,mask_image,C_checkboard_size, pts);
//// cvShowImage("board", board);
//


CvPoint2D32f corners[C_checkboard_size.width*C_checkboard_size.height];
Expand All @@ -160,17 +171,30 @@ void callback(const ImageConstPtr& img_ptr, const sensor_msgs::PointCloud2ConstP

if (!kinect_trafo_valid){

cout << "no trafo" << endl;
ROS_INFO("Searching for checkerboard");


int found = cvFindChessboardCorners(col, C_checkboard_size,corners, &c_cnt,CV_CALIB_CB_ADAPTIVE_THRESH);


IplImage* gray = cvCreateImage(cvGetSize(col),col->depth, 1);

cvCvtColor(col,gray, CV_BGR2GRAY);
int found = cvFindChessboardCorners(col, C_checkboard_size,corners, &c_cnt);

cvNamedWindow("g",0);
cvShowImage("g", gray);


cvFindCornerSubPix(gray, corners, c_cnt,cvSize(5,5),cvSize(1,1),cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10,0.1));
cvDrawChessboardCorners(col, C_checkboard_size, corners, c_cnt,found);

board_found = (c_cnt == C_checkboard_size.width*C_checkboard_size.height);
cvShowImage("view", col);

cout << "found " << c_cnt << endl;

if (board_found){ ROS_WARN("FOUND THE BOARD");
}else { ROS_WARN("Board was not found!"); return; }
}


Expand Down Expand Up @@ -199,17 +223,15 @@ void callback(const ImageConstPtr& img_ptr, const sensor_msgs::PointCloud2ConstP
if (depth_mask_valid)
showMaskOnImage(col, mask_image);


// show larger image:
// IplImage* large = cvCreateImage(cvSize(2*col->width,2*col->height), col->depth, col->nChannels);
// cvResize(col, large);
cvShowImage("view", col);
cvWaitKey(10);


if (!board_found && !kinect_trafo_valid){
ROS_INFO("Checkerboard was not found!");
return;
}

if (!depth_mask_valid) return;
if (!depth_mask_valid){ ROS_INFO("No depth mask!"); return; }

// fit plane to pointcloud:
Cloud cloud;
Expand Down Expand Up @@ -244,6 +266,15 @@ void callback(const ImageConstPtr& img_ptr, const sensor_msgs::PointCloud2ConstP

computeHomography(projector_corners,corners_in_xy_plane,proj_Hom);

assert(proj_Hom);

for (uint i=0; i<3; ++i){
for (uint j=0; j<3; ++j){
cout << cvmGet(proj_Hom,i,j) << " "; }
cout << endl;
}


ROS_INFO("Computed projector Homography");

}
Expand All @@ -254,12 +285,14 @@ void callback(const ImageConstPtr& img_ptr, const sensor_msgs::PointCloud2ConstP


// everything is set up!
cvSetMouseCallback("board",on_mouse_projector,&trans);

pcl::getTransformedPointCloud(cloud,kinect_trafo,full_cloud_moved);
cvSetMouseCallback("view",on_mouse_projector,&full_cloud_moved);



// mark points close to the board
// #define MINMAX
// #define MINMAX
#ifdef MINMAX
float minz = 100;
float maxz = -100;
Expand Down Expand Up @@ -294,7 +327,7 @@ void callback(const ImageConstPtr& img_ptr, const sensor_msgs::PointCloud2ConstP



Cloud::Ptr msg = trans.makeShared();
Cloud::Ptr msg = full_cloud_moved.makeShared();
msg->header.frame_id = "/openni_rgb_optical_frame";
msg->header.stamp = ros::Time::now ();
pub.publish(msg);
Expand All @@ -308,14 +341,15 @@ int main(int argc, char ** argv)
ros::init(argc, argv, "subscriber");
ros::NodeHandle nh;
cvNamedWindow("view", 0);
// cvMoveWindow("view", 1500, 100);





// load projector mask and show fullscreen on secondary screen:
IplImage* board_mask = cvLoadImage("data/proj_mask.png",0);
if (!board_mask){
board_mask = cvCreateImage(C_proj_size, IPL_DEPTH_8U,1);
cvSet(board_mask, cvScalarAll(255));
ROS_INFO("Found no projector mask, using total area");
}
if (board_mask->width != C_proj_size.width){
ROS_ERROR("mask for projector image has not the same size as the projector screen!!");
}
Expand Down Expand Up @@ -343,15 +377,15 @@ int main(int argc, char ** argv)
depth_mask_valid = (mask_image != NULL);
if (!depth_mask_valid){ // file was not found
mask_image = cvCreateImage(cvSize(640,480), IPL_DEPTH_8U, 1);
cerr << "no depth-mask found" << endl;
ROS_WARN("no depth-mask found");
#ifdef MASK_FROM_DETECTIONS
cout << "mask will be computed from first full view of checkerboard" << endl;
ROS_INFO("mask will be computed from first full view of checkerboard");
#else
cout << "select for points in image to create new mask!" << endl;
ROS_INFO("select four points in image to create new mask!");
cvSetMouseCallback("view",on_mouse,&mask_points);
#endif
}else{
cout << "depth-mask was loaded from data/mask.png" << endl;
ROS_INFO("depth-mask was loaded from data/mask.png");
}


Expand Down

0 comments on commit 459f7ed

Please sign in to comment.