Permalink
Browse files

more cleanup

  • Loading branch information...
1 parent d4ecefe commit abba4a65833b140edc436a36fb6f843587178430 Nikolas Engelhard committed May 11, 2012
Showing with 367 additions and 158 deletions.
  1. +75 −13 include/projector_calibrator.h
  2. +6 −0 launch/start_kinect.launch
  3. +1 −1 src/calibration.cpp
  4. +56 −47 src/main.cpp
  5. +229 −97 src/projector_calibrator.cpp
@@ -22,71 +22,133 @@
class Projector_Calibrator {
+ // number of inner corners of the board
cv::Size C_checkboard_size;
- cv::Size C_proj_size;
+ cv::Size C_proj_size; // size of the projector image in pixels
+
+ // The projection matrix of the projector and the homographies computed via OpenCV and SVD
cv::Mat proj_Matrix, hom_CV, hom_SVD;
// trafo cloud s.t. checkerboard is z=0, middle of board at x=y=0 and parallel to image axis
// the first trafo is stored and used for all following frames
Eigen::Affine3f kinect_trafo;
bool kinect_trafo_valid;
- cv::Mat input_image;
- cv::Mat gray;
+ cv::Mat input_image; // rgb image of kinect
+ cv::Mat gray; // gray version of kinect image
+ // image on kinect image showing the area where the first checkerboard was found in white
+ // its 8UC1, with board: 255, rest: 0
cv::Mat mask;
+ // image to be shown by the projector
+ cv::Mat projector_image;
+
+ // point cloud from kinect (still in kinect frame)
Cloud input_cloud;
+
+ // tilt of kinect (rotation around optical axis)
float kinect_tilt_angle_deg;
bool kinect_orientation_valid;
+ // pixel coordinates of the detected checkerboard corners
vector<cv::Point2f> corners;
+
+ // remove all point from the input cloud where mask!=255
void applyMaskOnInputCloud(Cloud& out);
- float fitPlaneToCloud(const Cloud& cloud, Eigen::Vector4f& model);
- cv::Mat proj_matrix;
+ // fit a plane into the pointcloud
+ float fitPlaneToCloud(const Cloud& cloud, Eigen::Vector4f& model);
+ // list of 3d-observations (in the wall-frame) of the checkerboard corners
+ // length is a multiple of the number of checkerboardcorners
Cloud observations_3d;
+
+ // draw a checkerboard with given number of internal corners on the image and store the corners
+ void drawCheckerboard(cv::Mat& img, const cv::Size size, vector<cv::Point2f>& corners_2d);
+
+
+ // Position of internal checkerboard corners
+ std:: vector<cv::Point2f> projector_corners;
+
+
+ string hom_cv_filename, hom_svd_filename, proj_matrix_filename, kinect_trafo_filename;
+
+ bool saveMat(const string name, const string filename, const cv::Mat& mat);
+ bool loadMat(const string name, const string filename, cv::Mat& mat);
+
+
public:
+ void initFromFile();
+
+ bool projMatrixSet(){ return proj_Matrix.cols > 0;}
+ bool homOpenCVSet(){ return hom_CV.cols > 0;}
+ bool homSVDSet(){ return hom_SVD.cols > 0;}
+
+
+
+
+ // save 3d positions (in wall-frame) of the last checkerboard detection
bool storeCurrent3DObservations();
- bool isKinectTrafoValid(){return kinect_trafo_valid;}
+ bool isKinectOrientationSet(){return kinect_orientation_valid;}
void setKinectOrientation(float angle_deg){kinect_tilt_angle_deg = angle_deg;kinect_orientation_valid = true;}
+ // compute the transformation that transforms the point cloud from the kinect-frame to the wall frame
+ void computeKinectTransformation();
+
+ bool isKinectTrafoSet(){return kinect_trafo_valid;}
+
bool mask_valid() {return mask.cols > 0;}
+ // calls the openCV checkerboard detector and stores the result internally
bool findCheckerboardCorners();
- void setMaskFromDetections();
+
+ // create the mask on the kinect image that shows the region where the checkerboard was detected
+ void createMaskFromDetections();
void setInputImage(cv::Mat& image){input_image = image; corners.clear();}
void setInputCloud(Cloud& cloud){input_cloud = cloud;}
- void computeKinectTransformation();
- // pixel coordinates of checkerboard corners
- void computeProjectionMatrix(const vector<cv::Point2f>& projector_corners);
- void computeHomography_OPENCV(const vector<cv::Point2f>& projector_corners);
- void computeHomography_SVD(const vector<cv::Point2f>& projector_corners);
+ // pixel coordinates of checkerboard corners
+ void computeProjectionMatrix();
+ void computeHomography_OPENCV();
+ void computeHomography_SVD();
Projector_Calibrator(){
kinect_orientation_valid = false;
kinect_trafo_valid = false;
C_checkboard_size = cv::Size(8,6);
C_proj_size = cv::Size(1152,864);
- }
+ projector_image = cv::Mat(C_proj_size, CV_8UC3);
+ drawCheckerboard(projector_image, C_checkboard_size, projector_corners);
+ hom_cv_filename = "homography_opencv";
+ hom_svd_filename = "homography_svd";
+ proj_matrix_filename = "projection_matrix";
+ kinect_trafo_filename = "kinect_trafo";
+ // creating fullscreen image (old syntax)
+ cvNamedWindow("fullscreen_ipl",0);
+ cvMoveWindow("fullscreen_ipl", 2000, 100);
+ cvSetWindowProperty("fullscreen_ipl", CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
+ IplImage proj_ipl = projector_image;
+ cvShowImage("fullscreen_ipl", &proj_ipl);
+
+
+ }
@@ -0,0 +1,6 @@
+<launch>
+
+ <include file="$(find openni_camera)/launch/openni_node.launch" ></include>
+ <node pkg="kinect_aux" type="kinect_aux_node" name="kinect_aux" output="screen"></node>
+
+</launch>
View
@@ -81,7 +81,7 @@ void scaleCloud(const Cloud& pts, cv::Mat& U, Cloud& transformed){
d += sqrt(pow(p.x-mu[0],2)+pow(p.y-mu[1],2)+pow(p.z-mu[2],2));
}
d /= c_cnt;
- // ROS_INFO("3d: mean: %f %f %f, dist: %f", mu[0], mu[1], mu[2], d);
+ // ROS_INFO("3d: mean: %f %f %f, dist: %f", mu[0], mu[1], mu[2], d);
// mean distance should be sqrt(3)
double s = sqrt(3)/d;
View
@@ -40,14 +40,14 @@
#include "meshing.h"
#include "projector_calibrator.h"
+
ros::Publisher pub;
ros::Publisher pub_full_moved;
using namespace std;
using namespace sensor_msgs;
using namespace message_filters;
namespace enc = sensor_msgs::image_encodings;
-
enum Calib_state {GET_KINECT_TRAFO,COLLECT_PATTERNS,GET_PROJECTION, EVERYTHING_SETUP};
Calib_state prog_state;
@@ -376,7 +376,10 @@ void showMaskOnImage(IplImage* col, IplImage* mask){
void callback(const ImageConstPtr& img_ptr, const sensor_msgs::PointCloud2ConstPtr& cloud_ptr){
- if (!kinect_tilt_angle_valid){
+// ROS_INFO("callback");
+
+ // Kinect's orientation is needed to compute the transformation
+ if (!calibrator.isKinectTrafoSet() && !calibrator.isKinectOrientationSet()){
ROS_INFO("waiting for tilt angle!");
return;
}
@@ -453,8 +456,10 @@ void callback(const ImageConstPtr& img_ptr, const sensor_msgs::PointCloud2ConstP
#ifdef MASK_FROM_DETECTIONS
if (!calibrator.mask_valid()){
- ROS_INFO("Creating Mask");
- calibrator.setMaskFromDetections();
+ROS_INFO("creating new mask!");
+
+ calibrator.createMaskFromDetections();
+
// createMaskFromCheckerBoardDetections(mask_image,corners,C_checkboard_size);
// cvNamedWindow("mask");
// cvShowImage("mask", mask_image);
@@ -479,9 +484,12 @@ void callback(const ImageConstPtr& img_ptr, const sensor_msgs::PointCloud2ConstP
if (prog_state == GET_KINECT_TRAFO){
+
+
if (!calibrator.mask_valid()){ ROS_INFO("No depth mask!"); return; }
+
calibrator.setInputCloud(cloud);
calibrator.computeKinectTransformation();
@@ -633,9 +641,9 @@ void callback(const ImageConstPtr& img_ptr, const sensor_msgs::PointCloud2ConstP
// ROS_INFO("Get projection");
- calibrator.computeHomography_OPENCV(projector_corners);
- calibrator.computeHomography_SVD(projector_corners);
- calibrator.computeProjectionMatrix(projector_corners);
+ calibrator.computeHomography_OPENCV();
+ calibrator.computeHomography_SVD();
+ calibrator.computeProjectionMatrix();
prog_state = COLLECT_PATTERNS;
@@ -764,31 +772,31 @@ void callback(const ImageConstPtr& img_ptr, const sensor_msgs::PointCloud2ConstP
void imu_CB(const sensor_msgs::ImuConstPtr& imu_ptr){
- if (!kinect_tilt_angle_valid)
+ if (!calibrator.isKinectOrientationSet())
{
kinect_tilt_angle_deg = asin(imu_ptr->linear_acceleration.x/9.81)/M_PI*180;
measured_angles.push_back(kinect_tilt_angle_deg);
if (measured_angles.size() == 200){
kinect_tilt_angle_deg = 0;
for (uint i=0; i<measured_angles.size(); ++i) kinect_tilt_angle_deg += measured_angles[i]/measured_angles.size();
- // ROS_INFO("Set kinect tilt angle to %.1f deg", kinect_tilt_angle_deg);
+ ROS_INFO("Set kinect orientation to %.1f deg", kinect_tilt_angle_deg);
kinect_tilt_angle_valid = true;
+ calibrator.setKinectOrientation(kinect_tilt_angle_deg);
}
- // ROS_INFO("current angle: %.1f deg", kinect_tilt_angle_deg);
}
-
}
int main(int argc, char ** argv)
{
+ ros::init(argc, argv, "subscriber");
- // createQUAD_PCD();
- // return 0;
-
+//
+ calibrator.initFromFile();
+//
+// return 0;
- ros::init(argc, argv, "subscriber");
ros::NodeHandle nh;
cvNamedWindow("camera", 1);
// cvNamedWindow("mask",1);
@@ -812,18 +820,18 @@ int main(int argc, char ** argv)
// using old style for fullscreen image
- cvNamedWindow("fullscreen_ipl",0);
- cvMoveWindow("fullscreen_ipl", 2000, 100);
- cvSetWindowProperty("fullscreen_ipl", CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
- IplImage proj_ipl = projector_image;
- cvShowImage("fullscreen_ipl", &proj_ipl);
+// cvNamedWindow("fullscreen_ipl",0);
+// cvMoveWindow("fullscreen_ipl", 2000, 100);
+// cvSetWindowProperty("fullscreen_ipl", CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
+// IplImage proj_ipl = projector_image;
+// cvShowImage("fullscreen_ipl", &proj_ipl);
pub = nh.advertise<Cloud>("projected", 1);
pub_full_moved = nh.advertise<Cloud>("full_moved", 1);
// check if depth-mask exists:
- mask_image = cvLoadImage("data/mask.png",0);
+ //mask_image = cvLoadImage("data/mask.png",0);
depth_mask_valid = (mask_image != NULL);
if (!depth_mask_valid){ // file was not found
mask_image = cvCreateImage(cvSize(640,480), IPL_DEPTH_8U, 1);
@@ -841,36 +849,36 @@ int main(int argc, char ** argv)
prog_state = GET_KINECT_TRAFO;
// read kinect_trafo
- kinect_trafo_valid = loadMatrix(kinect_trafo,"data/kinect_trafo.txt");
-
- if (kinect_trafo_valid){
- ROS_INFO("found kinect_trafo");
- for (uint i=0; i<4; ++i){
- for (uint j=0; j<4; ++j)
- cout << kinect_trafo(i,j) << " ";
- cout << endl;
- }
- }
+// kinect_trafo_valid = loadMatrix(kinect_trafo,"data/kinect_trafo.txt");
+//
+// if (kinect_trafo_valid){
+// ROS_INFO("found kinect_trafo");
+// for (uint i=0; i<4; ++i){
+// for (uint j=0; j<4; ++j)
+// cout << kinect_trafo(i,j) << " ";
+// cout << endl;
+// }
+// }
// look for Projection matrix:
- cv::FileStorage fs2("data/projection_matrix.yml", cv::FileStorage::READ);
- fs2["ProjectionMatrix"] >> proj_Matrix;
- if (proj_Matrix.cols > 0){
- cout << "projection" << endl << proj_Matrix << endl;
- }
-
- // if the projection matrix was deleted, also reestimate the kinect pose
- if (!kinect_trafo_valid && proj_Matrix.cols > 0){
- ROS_INFO("Found kinect-trafo but no Projection matrix, reestimating kinect pose");
- kinect_trafo_valid = false;
- }
-
- if (kinect_trafo_valid && proj_Matrix.cols > 0){
- ROS_INFO("Loaded everything from file");
- prog_state = EVERYTHING_SETUP;
- }
+// cv::FileStorage fs2("data/projection_matrix.yml", cv::FileStorage::READ);
+// fs2["ProjectionMatrix"] >> proj_Matrix;
+// if (proj_Matrix.cols > 0){
+// cout << "projection" << endl << proj_Matrix << endl;
+// }
+//
+// // if the projection matrix was deleted, also reestimate the kinect pose
+// if (!kinect_trafo_valid && proj_Matrix.cols > 0){
+// ROS_INFO("Found kinect-trafo but no Projection matrix, reestimating kinect pose");
+// kinect_trafo_valid = false;
+// }
+//
+// if (kinect_trafo_valid && proj_Matrix.cols > 0){
+// ROS_INFO("Loaded everything from file");
+// prog_state = EVERYTHING_SETUP;
+// }
//
// mv = new Mesh_visualizer();
@@ -887,6 +895,7 @@ int main(int argc, char ** argv)
sync.registerCallback(boost::bind(&callback, _1, _2));
+
ros::spin();
cvDestroyWindow("camera");
return 0;
Oops, something went wrong.

0 comments on commit abba4a6

Please sign in to comment.