visp_camera_calibration: visp_camera_calibration: image

00001 /****************************************************************************
00002  *
00003  * $Id: file.cpp 3496 2011-11-22 15:14:32Z fnovotny $
00004  *
00005  * This file is part of the ViSP software.
00006  * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
00007  * 
00008  * This software is free software; you can redistribute it and/or
00009  * modify it under the terms of the GNU General Public License
00010  * ("GPL") version 2 as published by the Free Software Foundation.
00011  * See the file LICENSE.txt at the root directory of this source
00012  * distribution for additional information about the GNU GPL.
00013  *
00014  * For using ViSP with software that can not be combined with the GNU
00015  * GPL, please contact INRIA about acquiring a ViSP Professional 
00016  * Edition License.
00017  *
00018  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
00019  * 
00020  * This software was developed at:
00021  * INRIA Rennes - Bretagne Atlantique
00022  * Campus Universitaire de Beaulieu
00023  * 35042 Rennes Cedex
00024  * France
00025  * http://www.irisa.fr/lagadic
00026  *
00027  * If you have questions regarding the use of this file, please contact
00028  * INRIA at visp@inria.fr
00029  * 
00030  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
00031  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
00032  *
00033  * Contact visp@irisa.fr if any conditions of this licensing are
00034  * not clear to you.
00035  *
00036  * Description:
00037  * 
00038  *
00039  * Authors:
00040  * Filip Novotny
00041  * 
00042  *
00043  *****************************************************************************/
00044 
00050 #include "image_processing.h"
00051 #include "visp/vpTrackingException.h"
00052 #include "visp_camera_calibration/CalibPointArray.h"
00053 #include "visp_camera_calibration/CalibPoint.h"
00054 #include "visp_camera_calibration/calibrate.h"
00055 #include "visp/vpMouseButton.h"
00056 
00057 #include "names.h"
00058 #include "conversions/image.h"
00059 #include "sensor_msgs/SetCameraInfo.h"
00060 #include "camera_calibration_parsers/parse.h"
00061 
00062 #include "visp/vpDisplayX.h"
00063 #include "visp/vpImagePoint.h"
00064 #include "visp/vpPixelMeterConversion.h"
00065 #include "visp/vpMeterPixelConversion.h"
00066 #include "visp/vpPose.h"
00067 #include "visp/vpHomogeneousMatrix.h"
00068 #include "visp/vpDot.h"
00069 #include "visp/vpDot2.h"
00070 #include "visp/vpCalibration.h"
00071 #include <iostream>
00072 
00073 
00074 namespace visp_camera_calibration
00075 {
00076 ImageProcessing::ImageProcessing() :
00077     spinner_(0),
00078     queue_size_(1000),
00079     pause_image_(false),
00080     img_(480,640,128),
00081     cam_(600,600,0,0)
00082 {
00083   visp_camera_calibration::remap();
00084   //Setup ROS environment
00085   //prepare function objects,define publishers & subscribers
00086   raw_image_subscriber_callback_t raw_image_callback = boost::bind(&ImageProcessing::rawImageCallback, this, _1);
00087 
00088   set_camera_info_bis_service_callback_t set_camera_info_bis_callback = boost::bind(&ImageProcessing::setCameraInfoBisCallback, this, _1,_2);
00089 
00090   raw_image_subscriber_ = n_.subscribe(visp_camera_calibration::raw_image_topic, queue_size_,
00091                                        raw_image_callback);
00092 
00093   calibrate_service_ = n_.serviceClient<visp_camera_calibration::calibrate> (visp_camera_calibration::calibrate_service);
00094 
00095   point_correspondence_publisher_ = n_.advertise<visp_camera_calibration::CalibPointArray>(visp_camera_calibration::point_correspondence_topic, queue_size_);
00096   set_camera_info_bis_service_ = n_.advertiseService(visp_camera_calibration::set_camera_info_bis_service,set_camera_info_bis_callback);
00097 
00098   //init graphical interface
00099   vpDisplay* disp = new vpDisplayX();
00100   disp->init(img_);
00101   disp->setTitle("Image processing initialisation interface");
00102   vpDisplay::flush(img_);
00103   vpDisplay::display(img_);
00104   vpDisplay::displayCharString(img_,img_.getHeight()/2-10,img_.getWidth()/4,"Waiting for the camera feed.",vpColor::red);
00105   vpDisplay::displayCharString(img_,img_.getHeight()/2+10,img_.getWidth()/4,"If you are using the example camera, you should click on it's window",vpColor::red);
00106 
00107   vpDisplay::flush(img_);
00108 
00109   // read 3D model from parameters
00110   XmlRpc::XmlRpcValue model_points_x_list;
00111   XmlRpc::XmlRpcValue model_points_y_list;
00112   XmlRpc::XmlRpcValue model_points_z_list;
00113   ros::param::get(visp_camera_calibration::model_points_x_param,model_points_x_list);
00114   ros::param::get(visp_camera_calibration::model_points_y_param,model_points_y_list);
00115   ros::param::get(visp_camera_calibration::model_points_z_param,model_points_z_list);
00116 
00117   ROS_ASSERT(model_points_x_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00118   ROS_ASSERT(model_points_x_list.size() == model_points_y_list.size() && model_points_x_list.size()==model_points_z_list.size());
00119   for(int i=0;i<model_points_x_list.size();i++){
00120     ROS_ASSERT(model_points_x_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00121     ROS_ASSERT(model_points_y_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00122     ROS_ASSERT(model_points_z_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00123     double X = static_cast<double>(model_points_x_list[i]);
00124     double Y = static_cast<double>(model_points_y_list[i]);
00125     double Z = static_cast<double>(model_points_z_list[i]);
00126     vpPoint p;
00127     p.setWorldCoordinates(X,Y,Z);
00128     model_points_.push_back(p);
00129   }
00130 
00131   //define selected model points
00132   XmlRpc::XmlRpcValue selected_points_x_list;
00133   XmlRpc::XmlRpcValue selected_points_y_list;
00134   XmlRpc::XmlRpcValue selected_points_z_list;
00135 
00136   ros::param::get(visp_camera_calibration::selected_points_x_param,selected_points_x_list);
00137   ros::param::get(visp_camera_calibration::selected_points_y_param,selected_points_y_list);
00138   ros::param::get(visp_camera_calibration::selected_points_z_param,selected_points_z_list);
00139   ROS_ASSERT(selected_points_x_list.size() == selected_points_y_list.size() && selected_points_x_list.size()==selected_points_z_list.size());
00140 
00141   for(int i=0;i<selected_points_x_list.size();i++){
00142       ROS_ASSERT(selected_points_x_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00143       ROS_ASSERT(selected_points_y_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00144       ROS_ASSERT(selected_points_z_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00145       double X = static_cast<double>(selected_points_x_list[i]);
00146       double Y = static_cast<double>(selected_points_y_list[i]);
00147       double Z = static_cast<double>(selected_points_z_list[i]);
00148       vpPoint p;
00149       p.setWorldCoordinates(X,Y,Z);
00150       selected_points_.push_back(p);
00151     }
00152 
00153   //init camera
00154   double px = cam_.get_px();
00155   double py = cam_.get_px();
00156   double u0 = img_.getWidth()/2;
00157   double v0 = img_.getHeight()/2;
00158   cam_.initPersProjWithoutDistortion(px, py, u0, v0);
00159 
00160 }
00161 
00162 bool ImageProcessing::setCameraInfoBisCallback(sensor_msgs::SetCameraInfo::Request  &req,
00163                              sensor_msgs::SetCameraInfo::Response &res){
00164   std::string calibration_path;
00165   ros::param::getCached(visp_camera_calibration::calibration_path_param,calibration_path);
00166   ROS_INFO("saving calibration file to %s",calibration_path.c_str());
00167   camera_calibration_parsers::writeCalibration(calibration_path,visp_camera_calibration::raw_image_topic,req.camera_info);
00168   return true;
00169 }
00170 void ImageProcessing::rawImageCallback(const sensor_msgs::Image::ConstPtr& image){
00171   ros::Rate loop_rate(200);
00172   double gray_level_precision;
00173   double size_precision;
00174   bool pause_at_each_frame = false; //Wait for user input each time a new frame is recieved.
00175 
00176   ros::param::getCached(visp_camera_calibration::gray_level_precision_param,gray_level_precision);
00177   ros::param::getCached(visp_camera_calibration::size_precision_param,size_precision);
00178   ros::param::getCached(visp_camera_calibration::pause_at_each_frame_param,pause_at_each_frame);
00179 
00180 
00181   vpPose pose;
00182   vpCalibration calib;
00183   visp_camera_calibration::CalibPointArray calib_all_points;
00184 
00185   img_ = visp_bridge::toVispImage(*image);
00186 
00187   vpDisplay::display(img_);
00188   vpDisplay::flush(img_);
00189 
00190   if(!pause_at_each_frame){
00191     vpImagePoint ip;
00192     vpDisplay::displayRectangle(img_,0,0,img_.getWidth(),15,vpColor::black,true);
00193     vpDisplay::displayCharString(img_,10,10,"Click on the window to select the current image",vpColor::red);
00194     vpDisplay::flush(img_);
00195     if(pause_image_){
00196       pause_image_= false;
00197     }
00198     else{
00199       return;
00200     }
00201   }
00202 
00203   pose.clearPoint();
00204   calib.clearPoint();
00205         vpImagePoint ip;
00206 
00207   //lets the user select keypoints
00208   for(unsigned int i=0;i<selected_points_.size();i++){
00209     try{
00210       vpDot2 d;
00211       d.setGrayLevelPrecision(gray_level_precision);
00212       d.setSizePrecision(size_precision);
00213       
00214                         ROS_INFO("Click on point %d",i+1);
00215       vpDisplay::displayRectangle(img_,0,0,img_.getWidth(),15,vpColor::black,true);
00216       vpDisplay::displayCharString(img_,10,10,boost::str(boost::format("click on point %1%") % (i+1)).c_str(),vpColor::red);
00217       vpDisplay::flush(img_);  
00218       while(ros::ok() && !vpDisplay::getClick(img_,ip,false));
00219       
00220       d.initTracking(img_, ip);
00221 
00222       ip.set_ij(d.getCog().get_i(),d.getCog().get_j());
00223       double x=0,y=0;
00224       vpPixelMeterConversion::convertPoint(cam_, ip, x, y);
00225       selected_points_[i].set_x(x);
00226       selected_points_[i].set_y(y);
00227       pose.addPoint(selected_points_[i]);
00228       calib.addPoint(selected_points_[i].get_oX(),selected_points_[i].get_oY(),selected_points_[i].get_oZ(), ip);
00229       
00230                         vpDisplay::displayCross(img_, d.getCog(), 10, vpColor::red);
00231                         vpDisplay::flush(img_);
00232     }catch(vpTrackingException e){
00233       ROS_ERROR("Failed to init point");
00234     }
00235   }
00236 
00237 
00238   vpHomogeneousMatrix cMo;
00239   pose.computePose(vpPose::LAGRANGE, cMo) ;
00240   pose.computePose(vpPose::VIRTUAL_VS, cMo) ;
00241   vpHomogeneousMatrix cMoTmp = cMo;
00242 
00243   vpCameraParameters camTmp = cam_;
00244   //compute local calibration to match the calibration grid with the image
00245   try{
00246     calib.computeCalibration(vpCalibration::CALIB_VIRTUAL_VS,cMoTmp,camTmp,false);
00247     ROS_DEBUG_STREAM("cMo="<<std::endl <<cMoTmp<<std::endl);
00248     ROS_DEBUG_STREAM("cam="<<std::endl <<camTmp<<std::endl);
00249 
00250     //project all points and track their corresponding image location
00251     for (std::vector<vpPoint>::iterator model_point_iter= model_points_.begin();
00252           model_point_iter!= model_points_.end() ;
00253           model_point_iter++){
00254       //project each model point into image according to current calibration
00255       vpColVector _cP, _p ;
00256       
00257                         model_point_iter->changeFrame(cMoTmp,_cP) ;
00258       model_point_iter->projection(_cP,_p) ;
00259       vpMeterPixelConversion::convertPoint(camTmp,_p[0],_p[1], ip);
00260       if (10 < ip.get_u() && ip.get_u() < img_.getWidth()-10 &&
00261           10 < ip.get_v() && ip.get_v() < img_.getHeight()-10) {
00262         try {
00263           //track the projected point, look for match
00264           vpDot2 md;
00265           md.setGrayLevelPrecision(gray_level_precision);
00266           md.setSizePrecision(size_precision);
00267 
00268           md.initTracking(img_, ip);
00269           if(!ros::ok())
00270                                                 return;
00271 
00272           vpRect bbox = md.getBBox();
00273           vpImagePoint cog = md.getCog();
00274           if(bbox.getLeft()<5 || bbox.getRight()>(double)img_.getWidth()-5 ||
00275               bbox.getTop()<5 || bbox.getBottom()>(double)img_.getHeight()-5||
00276               vpMath::abs(ip.get_u() - cog.get_u()) > 10 ||
00277               vpMath::abs(ip.get_v() - cog.get_v()) > 10){
00278             ROS_DEBUG("tracking failed[suspicious point location].");
00279           }else{
00280             //point matches
00281             double x=0, y=0;
00282             vpPixelMeterConversion::convertPoint(camTmp, cog, x, y)  ;
00283             model_point_iter->set_x(x) ;
00284             model_point_iter->set_y(y) ;
00285 
00286             md.display(img_,vpColor::yellow, 2);
00287             visp_camera_calibration::CalibPoint cp;
00288             cp.i = cog.get_i();
00289             cp.j = cog.get_j();
00290             cp.X = model_point_iter->get_oX();
00291             cp.Y = model_point_iter->get_oY();
00292             cp.Z = model_point_iter->get_oZ();
00293 
00294             calib_all_points.points.push_back(cp);
00295 
00296             model_point_iter->display(img_,cMoTmp,camTmp) ;
00297             loop_rate.sleep(); //To avoid refresh problems
00298             vpDisplay::flush(img_);
00299           }
00300         } catch(...){
00301           ROS_DEBUG("tracking failed.");
00302         }
00303       } else {
00304         ROS_DEBUG("bad projection.");
00305       }
00306     }
00307 
00308     ROS_INFO("Left click on the interface window to continue, right click to restart");
00309     vpDisplay::displayRectangle(img_,0,0,img_.getWidth(),15,vpColor::black,true);
00310     vpDisplay::displayCharString(img_,10,10,"Left click on the interface window to continue, right click to restart",vpColor::red);
00311     vpDisplay::flush(img_);
00312                 
00313     vpMouseButton::vpMouseButtonType btn;
00314     while(ros::ok() && !vpDisplay::getClick(img_,ip,btn, false));
00315                 
00316     if(btn==vpMouseButton::button1)
00317       point_correspondence_publisher_.publish(calib_all_points);
00318     else{
00319       rawImageCallback(image);
00320       return;
00321     }
00322   }catch(...){
00323     ROS_ERROR("calibration failed.");
00324   }
00325 }
00326 
00327 void ImageProcessing::interface()
00328 {
00329   vpImagePoint ip;
00330   while(ros::ok()){
00331     ros::spinOnce();
00332     if(vpDisplay::getClick(img_,ip,false))
00333       pause_image_ = true;
00334   }
00335   ros::waitForShutdown();
00336 }
00337 
00338 ImageProcessing::~ImageProcessing()
00339 {
00340   // TODO Auto-generated destructor stub
00341 }
00342 }
image_processing.cpp
