image_processing.cpp
Go to the documentation of this file.
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 <visp_bridge/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     is_initialized(false)
00083 {
00084   visp_camera_calibration::remap();
00085   //Setup ROS environment
00086   //prepare function objects,define publishers & subscribers
00087   raw_image_subscriber_callback_t raw_image_callback = boost::bind(&ImageProcessing::rawImageCallback, this, _1);
00088 
00089   set_camera_info_bis_service_callback_t set_camera_info_bis_callback = boost::bind(&ImageProcessing::setCameraInfoBisCallback, this, _1,_2);
00090 
00091   raw_image_subscriber_ = n_.subscribe(visp_camera_calibration::raw_image_topic, queue_size_,
00092                                        raw_image_callback);
00093 
00094   calibrate_service_ = n_.serviceClient<visp_camera_calibration::calibrate> (visp_camera_calibration::calibrate_service);
00095 
00096   point_correspondence_publisher_ = n_.advertise<visp_camera_calibration::CalibPointArray>(visp_camera_calibration::point_correspondence_topic, queue_size_);
00097   set_camera_info_bis_service_ = n_.advertiseService(visp_camera_calibration::set_camera_info_bis_service,set_camera_info_bis_callback);
00098 
00099   // read 3D model from parameters
00100   XmlRpc::XmlRpcValue model_points_x_list;
00101   XmlRpc::XmlRpcValue model_points_y_list;
00102   XmlRpc::XmlRpcValue model_points_z_list;
00103   ros::param::get(visp_camera_calibration::model_points_x_param,model_points_x_list);
00104   ros::param::get(visp_camera_calibration::model_points_y_param,model_points_y_list);
00105   ros::param::get(visp_camera_calibration::model_points_z_param,model_points_z_list);
00106 
00107   ROS_ASSERT(model_points_x_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00108   ROS_ASSERT(model_points_x_list.size() == model_points_y_list.size() && model_points_x_list.size()==model_points_z_list.size());
00109   for(int i=0;i<model_points_x_list.size();i++){
00110     ROS_ASSERT(model_points_x_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00111     ROS_ASSERT(model_points_y_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00112     ROS_ASSERT(model_points_z_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00113     double X = static_cast<double>(model_points_x_list[i]);
00114     double Y = static_cast<double>(model_points_y_list[i]);
00115     double Z = static_cast<double>(model_points_z_list[i]);
00116     vpPoint p;
00117     p.setWorldCoordinates(X,Y,Z);
00118     model_points_.push_back(p);
00119   }
00120 
00121   //define selected model points
00122   XmlRpc::XmlRpcValue selected_points_x_list;
00123   XmlRpc::XmlRpcValue selected_points_y_list;
00124   XmlRpc::XmlRpcValue selected_points_z_list;
00125 
00126   ros::param::get(visp_camera_calibration::selected_points_x_param,selected_points_x_list);
00127   ros::param::get(visp_camera_calibration::selected_points_y_param,selected_points_y_list);
00128   ros::param::get(visp_camera_calibration::selected_points_z_param,selected_points_z_list);
00129   ROS_ASSERT(selected_points_x_list.size() == selected_points_y_list.size() && selected_points_x_list.size()==selected_points_z_list.size());
00130 
00131   for(int i=0;i<selected_points_x_list.size();i++){
00132       ROS_ASSERT(selected_points_x_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00133       ROS_ASSERT(selected_points_y_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00134       ROS_ASSERT(selected_points_z_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00135       double X = static_cast<double>(selected_points_x_list[i]);
00136       double Y = static_cast<double>(selected_points_y_list[i]);
00137       double Z = static_cast<double>(selected_points_z_list[i]);
00138       vpPoint p;
00139       p.setWorldCoordinates(X,Y,Z);
00140       selected_points_.push_back(p);
00141     }
00142 }
00143 
00144 void ImageProcessing::init() 
00145 {
00146 if (! is_initialized) {
00147   //init graphical interface
00148   vpDisplay* disp = new vpDisplayX();
00149   disp->init(img_);
00150   disp->setTitle("Image processing initialisation interface");
00151   vpDisplay::flush(img_);
00152   vpDisplay::display(img_);
00153   vpDisplay::displayCharString(img_,img_.getHeight()/2-10,img_.getWidth()/4,"Waiting for the camera feed.",vpColor::red);
00154   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);
00155 
00156   vpDisplay::flush(img_);
00157 
00158   //init camera
00159   double px = cam_.get_px();
00160   double py = cam_.get_px();
00161   double u0 = img_.getWidth()/2;
00162   double v0 = img_.getHeight()/2;
00163   cam_.initPersProjWithoutDistortion(px, py, u0, v0);
00164 
00165   is_initialized = true;
00166   }
00167 }
00168 
00169 bool ImageProcessing::setCameraInfoBisCallback(sensor_msgs::SetCameraInfo::Request  &req,
00170                              sensor_msgs::SetCameraInfo::Response &res){
00171   std::string calibration_path;
00172   ros::param::getCached(visp_camera_calibration::calibration_path_param,calibration_path);
00173   ROS_INFO("saving calibration file to %s",calibration_path.c_str());
00174   camera_calibration_parsers::writeCalibration(calibration_path,visp_camera_calibration::raw_image_topic,req.camera_info);
00175   return true;
00176 }
00177 
00178 void ImageProcessing::rawImageCallback(const sensor_msgs::Image::ConstPtr& image){
00179   ros::Rate loop_rate(200);
00180   double gray_level_precision;
00181   double size_precision;
00182   bool pause_at_each_frame = false; //Wait for user input each time a new frame is recieved.
00183 
00184   ros::param::getCached(visp_camera_calibration::gray_level_precision_param,gray_level_precision);
00185   ros::param::getCached(visp_camera_calibration::size_precision_param,size_precision);
00186   ros::param::getCached(visp_camera_calibration::pause_at_each_frame_param,pause_at_each_frame);
00187 
00188 
00189   vpPose pose;
00190   vpCalibration calib;
00191   visp_camera_calibration::CalibPointArray calib_all_points;
00192 
00193   img_ = visp_bridge::toVispImage(*image);
00194   
00195   init();
00196 
00197   vpDisplay::display(img_);
00198   vpDisplay::flush(img_);
00199 
00200   if(!pause_at_each_frame){
00201     vpImagePoint ip;
00202     vpDisplay::displayRectangle(img_,0,0,img_.getWidth(),15,vpColor::black,true);
00203     vpDisplay::displayCharString(img_,10,10,"Click on the window to select the current image",vpColor::red);
00204     vpDisplay::flush(img_);
00205     if(pause_image_){
00206       pause_image_= false;
00207     }
00208     else{
00209       return;
00210     }
00211   }
00212 
00213   pose.clearPoint();
00214   calib.clearPoint();
00215         vpImagePoint ip;
00216 
00217   //lets the user select keypoints
00218   for(unsigned int i=0;i<selected_points_.size();i++){
00219     try{
00220       vpDot2 d;
00221       d.setGrayLevelPrecision(gray_level_precision);
00222       d.setSizePrecision(size_precision);
00223       
00224                         ROS_INFO("Click on point %d",i+1);
00225       vpDisplay::displayRectangle(img_,0,0,img_.getWidth(),15,vpColor::black,true);
00226       vpDisplay::displayCharString(img_,10,10,boost::str(boost::format("click on point %1%") % (i+1)).c_str(),vpColor::red);
00227       vpDisplay::flush(img_);  
00228       while(ros::ok() && !vpDisplay::getClick(img_,ip,false));
00229       
00230       d.initTracking(img_, ip);
00231 
00232       ip.set_ij(d.getCog().get_i(),d.getCog().get_j());
00233       double x=0,y=0;
00234       vpPixelMeterConversion::convertPoint(cam_, ip, x, y);
00235       selected_points_[i].set_x(x);
00236       selected_points_[i].set_y(y);
00237       pose.addPoint(selected_points_[i]);
00238       calib.addPoint(selected_points_[i].get_oX(),selected_points_[i].get_oY(),selected_points_[i].get_oZ(), ip);
00239       
00240                         vpDisplay::displayCross(img_, d.getCog(), 10, vpColor::red);
00241                         vpDisplay::flush(img_);
00242     }catch(vpTrackingException e){
00243       ROS_ERROR("Failed to init point");
00244     }
00245   }
00246 
00247 
00248   vpHomogeneousMatrix cMo;
00249   pose.computePose(vpPose::LAGRANGE, cMo) ;
00250   pose.computePose(vpPose::VIRTUAL_VS, cMo) ;
00251   vpHomogeneousMatrix cMoTmp = cMo;
00252 
00253   vpCameraParameters camTmp = cam_;
00254   //compute local calibration to match the calibration grid with the image
00255   try{
00256     calib.computeCalibration(vpCalibration::CALIB_VIRTUAL_VS,cMoTmp,camTmp,false);
00257     ROS_DEBUG_STREAM("cMo="<<std::endl <<cMoTmp<<std::endl);
00258     ROS_DEBUG_STREAM("cam="<<std::endl <<camTmp<<std::endl);
00259 
00260     //project all points and track their corresponding image location
00261     for (std::vector<vpPoint>::iterator model_point_iter= model_points_.begin();
00262           model_point_iter!= model_points_.end() ;
00263           model_point_iter++){
00264       //project each model point into image according to current calibration
00265       vpColVector _cP, _p ;
00266       
00267                         model_point_iter->changeFrame(cMoTmp,_cP) ;
00268       model_point_iter->projection(_cP,_p) ;
00269       vpMeterPixelConversion::convertPoint(camTmp,_p[0],_p[1], ip);
00270       if (10 < ip.get_u() && ip.get_u() < img_.getWidth()-10 &&
00271           10 < ip.get_v() && ip.get_v() < img_.getHeight()-10) {
00272         try {
00273           //track the projected point, look for match
00274           vpDot2 md;
00275           md.setGrayLevelPrecision(gray_level_precision);
00276           md.setSizePrecision(size_precision);
00277 
00278           md.initTracking(img_, ip);
00279           if(!ros::ok())
00280                                                 return;
00281 
00282           vpRect bbox = md.getBBox();
00283           vpImagePoint cog = md.getCog();
00284           if(bbox.getLeft()<5 || bbox.getRight()>(double)img_.getWidth()-5 ||
00285               bbox.getTop()<5 || bbox.getBottom()>(double)img_.getHeight()-5||
00286               vpMath::abs(ip.get_u() - cog.get_u()) > 10 ||
00287               vpMath::abs(ip.get_v() - cog.get_v()) > 10){
00288             ROS_DEBUG("tracking failed[suspicious point location].");
00289           }else{
00290             //point matches
00291             double x=0, y=0;
00292             vpPixelMeterConversion::convertPoint(camTmp, cog, x, y)  ;
00293             model_point_iter->set_x(x) ;
00294             model_point_iter->set_y(y) ;
00295 
00296             md.display(img_,vpColor::yellow, 2);
00297             visp_camera_calibration::CalibPoint cp;
00298             cp.i = cog.get_i();
00299             cp.j = cog.get_j();
00300             cp.X = model_point_iter->get_oX();
00301             cp.Y = model_point_iter->get_oY();
00302             cp.Z = model_point_iter->get_oZ();
00303 
00304             calib_all_points.points.push_back(cp);
00305 
00306             model_point_iter->display(img_,cMoTmp,camTmp) ;
00307             loop_rate.sleep(); //To avoid refresh problems
00308             vpDisplay::flush(img_);
00309           }
00310         } catch(...){
00311           ROS_DEBUG("tracking failed.");
00312         }
00313       } else {
00314         ROS_DEBUG("bad projection.");
00315       }
00316     }
00317 
00318     ROS_INFO("Left click on the interface window to continue, right click to restart");
00319     vpDisplay::displayRectangle(img_,0,0,img_.getWidth(),15,vpColor::black,true);
00320     vpDisplay::displayCharString(img_,10,10,"Left click on the interface window to continue, right click to restart",vpColor::red);
00321     vpDisplay::flush(img_);
00322                 
00323     vpMouseButton::vpMouseButtonType btn;
00324     while(ros::ok() && !vpDisplay::getClick(img_,ip,btn, false));
00325                 
00326     if(btn==vpMouseButton::button1)
00327       point_correspondence_publisher_.publish(calib_all_points);
00328     else{
00329       rawImageCallback(image);
00330       return;
00331     }
00332   }catch(...){
00333     ROS_ERROR("calibration failed.");
00334   }
00335 }
00336 
00337 void ImageProcessing::interface()
00338 {
00339   vpImagePoint ip;
00340   while(ros::ok()){
00341     ros::spinOnce();
00342     if(vpDisplay::getClick(img_,ip,false))
00343       pause_image_ = true;
00344   }
00345   ros::waitForShutdown();
00346 }
00347 
00348 ImageProcessing::~ImageProcessing()
00349 {
00350   // TODO Auto-generated destructor stub
00351 }
00352 }


visp_camera_calibration
Author(s): Filip Novotny
autogenerated on Fri Aug 28 2015 13:36:31