自学教程：C++ vpERROR_TRACE函数代码示例

/*!  Read the content of the position file and moves to head to articular  position.  /param filename : Position filename  /exception vpRobotException::readingParametersError : If the articular  position cannot be read from file.  /sa readPositionFile()*/voidvpRobotBiclops::setPosition(const char *filename){  vpColVector q ;  if (readPositionFile(filename, q) == false) {    vpERROR_TRACE ("Cannot get biclops position from file");    throw vpRobotException (vpRobotException::readingParametersError,			    "Cannot get biclops position from file");  }  setPosition ( vpRobot::ARTICULAR_FRAME, q) ;}

/*!  Display features position and id.  /param I : Image used as background. Display should be initialized on it.  /param color : Color used to display the features.  /param thickness : Thickness of the drawings.  */void vpKltOpencv::display(const vpImage<unsigned char> &I,                          vpColor color, unsigned int thickness){  if ((features == 0) || (I.bitmap==0) || (!initialized))    {      vpERROR_TRACE(" Memory problem ");      throw(vpException(vpException::memoryAllocationError," Memory problem"));    }  vpKltOpencv::display(I, features, featuresid, countFeatures, color, thickness);}

/*!  Set the velocity used for a position control.  /param velocity : Velocity in % of the maximum velocity between [0,100]. The  maximum velocity is given vpBiclops::speedLimit.*/voidvpRobotBiclops::setPositioningVelocity (const double velocity){  if (velocity < 0 || velocity > 100) {    vpERROR_TRACE("Bad positionning velocity");    throw vpRobotException (vpRobotException::constructionError,			    "Bad positionning velocity");  }  positioningVelocity = velocity;}

/*!  Display a rectangle.  /param rectangle : Rectangle characteristics.  /param color : Rectangle color.  /param fill : When set to true fill the rectangle.  /param thickness : Thickness of the four lines used to display the  rectangle. This parameter is only useful when /e fill is set to  false.*/voidvpDisplayOpenCV::displayRectangle(const vpRect &rectangle,                                  const vpColor &color, bool fill,                                  unsigned int thickness){  if (displayHasBeenInitialized)  {    if (fill == false) {      if (color.id < vpColor::id_unknown) {	cvRectangle( background,		     cvPoint( vpMath::round( rectangle.getLeft() ),			      vpMath::round( rectangle.getBottom() ) ),		     cvPoint( vpMath::round( rectangle.getRight() ),			      vpMath::round( rectangle.getTop() ) ),		     col[color.id], (int)thickness);      }      else {	cvcolor = CV_RGB(color.R, color.G, color.B) ;	cvRectangle( background,		     cvPoint( vpMath::round( rectangle.getLeft() ),			      vpMath::round( rectangle.getBottom() ) ),		     cvPoint( vpMath::round( rectangle.getRight() ),			      vpMath::round( rectangle.getTop() ) ),		     cvcolor, (int)thickness);      }    }    else {      if (color.id < vpColor::id_unknown) {	cvRectangle( background,		     cvPoint( vpMath::round( rectangle.getLeft() ),			      vpMath::round( rectangle.getBottom() ) ),		     cvPoint( vpMath::round( rectangle.getRight() ),			      vpMath::round( rectangle.getTop() ) ),		     col[color.id], CV_FILLED);      }      else {	cvcolor = CV_RGB(color.R, color.G, color.B) ;	cvRectangle( background,		     cvPoint( vpMath::round( rectangle.getLeft() ),			      vpMath::round( rectangle.getBottom() ) ),		     cvPoint( vpMath::round( rectangle.getRight() ),			      vpMath::round( rectangle.getTop() ) ),		     cvcolor, CV_FILLED);      }    }  }  else  {    vpERROR_TRACE("OpenCV not initialized " ) ;    throw(vpDisplayException(vpDisplayException::notInitializedError,                             "OpenCV not initialized")) ;  }}

/*!  Grabs a color image from the selected camera. Since the cameras  are not able to grab RGBa color coding format, the acquired image is  converted in a RGBa to match the requested format. This transformation  could be time consuming.  /param I : Acquired color image in RGBa format. */void vp1394CMUGrabber::acquire(vpImage<vpRGBa> &I){  // get image data  unsigned long length;  unsigned char *rawdata = NULL;  int dropped;  unsigned int size;  if(init == false){    close();    throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,                                   "Initialization not done") );  }  camera->AcquireImageEx(TRUE,&dropped);  rawdata = camera->GetRawData(&length);  size = I.getWidth() * I.getHeight();  switch (_color) {    case vp1394CMUGrabber::MONO8:      vpImageConvert::GreyToRGBa(rawdata, (unsigned char *)I.bitmap, size);      break;    case vp1394CMUGrabber::MONO16:      vpImageConvert::MONO16ToRGBa(rawdata, (unsigned char *)I.bitmap, size);      break;    case vp1394CMUGrabber::YUV411:      vpImageConvert::YUV411ToRGBa(rawdata, (unsigned char *)I.bitmap, size);      break;    case vp1394CMUGrabber::YUV422:      vpImageConvert::YUV422ToRGBa(rawdata, (unsigned char *)I.bitmap, size);      break;    case vp1394CMUGrabber::YUV444:      vpImageConvert::YUV444ToRGBa(rawdata, (unsigned char *)I.bitmap, size);      break;    case vp1394CMUGrabber::RGB8:      size = length / 3;      vpImageConvert::RGBToRGBa(rawdata, (unsigned char *)I.bitmap, size);      break;    default:      close();      vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");      throw (vpFrameGrabberException(vpFrameGrabberException::otherError,                                     "Format conversion not implemented. "                                     "Acquisition failed.") );      break;  };}

/*!  Receives a message once (in the limit of the Maximum message size value), from a specific emitter.  This message can represent an entire request or not. Several calls to this function  might be necessary to get the entire request.    /warning Requests will be received but not decoded.    /sa vpNetwork::receive()  /sa vpNetwork::receiveRequestFrom()  /sa vpNetwork::receiveRequest()  /sa vpNetwork::receiveRequestOnce()  /sa vpNetwork::receiveAndDecodeRequest()  /sa vpNetwork::receiveAndDecodeRequestFrom()  /sa vpNetwork::receiveAndDecodeRequestOnce()  /sa vpNetwork::receiveAndDecodeRequestOnceFrom()    /param receptorEmitting : Index of the receptor emitting the message.    /return The number of bytes received, -1 if an error occured.*/int vpNetwork::_receiveRequestOnceFrom(const unsigned int &receptorEmitting){  int size = (int)receptor_list.size();  int sizeMinusOne = (int)receptor_list.size()-1;  if(size == 0 || receptorEmitting > (unsigned)sizeMinusOne )  {    if(verboseMode)      vpTRACE( "No receptor at the specified index!" );    return -1;  }    tv.tv_sec = tv_sec;  tv.tv_usec = tv_usec;    FD_ZERO(&readFileDescriptor);            socketMax = receptor_list[receptorEmitting].socketFileDescriptorReceptor;  FD_SET((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor,&readFileDescriptor);  int value = select((int)socketMax+1,&readFileDescriptor,NULL,NULL,&tv);  int numbytes = 0;  if(value == -1){    if(verboseMode)      vpERROR_TRACE( "Select error" );    return -1;  }  else if(value == 0){    //Timeout    return 0;  }  else{    if(FD_ISSET((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor,&readFileDescriptor)){      char *buf = new char [max_size_message];#ifdef UNIX      numbytes=recv(receptor_list[receptorEmitting].socketFileDescriptorReceptor, buf, max_size_message, 0);#else      numbytes=recv((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor, buf, (int)max_size_message, 0);#endif      if(numbytes <= 0)      {        std::cout << "Disconnected : " << inet_ntoa(receptor_list[receptorEmitting].receptorAddress.sin_addr) << std::endl;        receptor_list.erase(receptor_list.begin()+(int)receptorEmitting);        return numbytes;      }      else if(numbytes > 0){        std::string returnVal(buf, (unsigned int)numbytes);        currentMessageReceived.append(returnVal);      }      delete [] buf;    }  }    return numbytes;}

/*!  Waits the end of motion of the corresponding servo motor.  /param servo : Servolens servo motor.  /exception vpRobotException::communicationError : If cannot dial  with Servolens.*/voidvpServolens::wait(vpServoType servo) const{  if (!isinit) {    vpERROR_TRACE ("Cannot dial with Servolens.");    throw vpRobotException (vpRobotException::communicationError,			    "Cannot dial with Servolens.");  }  char c;  char fin_mvt[3];  bool sortie = false;  switch (servo) {  case ZOOM:    sprintf(fin_mvt, "ZF");    break;  case FOCUS:    sprintf(fin_mvt, "FF");    break;  case IRIS:  default:    sprintf(fin_mvt, "DF");    break;      }  /* lecture des caracteres recus */  do {    /* lecture des caracteres */    if (::read(this->remfd,&c,1) != 1) {      throw vpRobotException (vpRobotException::communicationError,			      "Cannot read on Servolens.");    }    c &= 0x7f;    /* tests si fin de mouvement */    if (c == fin_mvt[0]) {      /* lecture du caractere suivant */      if (::read(this->remfd,&c,1) != 1) {	throw vpRobotException (vpRobotException::communicationError,				"Cannot read on Servolens.");      }      c &= 0x7f;      if (c == fin_mvt[1]) {	sortie = true;      }    }  }  while ( !sortie);  /*  printf("/nmouvement fini: chaine lue = %s", chaine); */}

/*!  Send to the controller a velocity.  /param frame : Control frame type. Only articular (vpRobot::ARTICULAR_FRAME)  and camera frame (vpRobot::CAMERA_FRAME) are implemented.  /param v : Velocity to apply to the robot.  - In the camera frame, this velocity is represented by a vector of dimension 6  /f$ {/bf v} = [{/bf t}, {/bf /theta u }]^t /f$ where /f$ /bf t /f$ is a  translation vector and /f$ {/bf /theta u} /f$ is a rotation vector (see  vpThetaUVector): /f$ {/bf v} = [t_x, t_y, t_z, {/theta u}_x, {/theta u}_y,  {/theta u}_z] /f$ (see vpTranslationVector and vpThetaUVector).  - In articular, this velocity is represented by a 6 dimension vector /f$  /dot{{/bf q}} = [{/bf t}, {/bf /theta u}]^t /f$ where /f$ /bf t /f$ is a  translation vector and /f$ {/bf /theta u} /f$ is a rotation vector (see  vpThetaUVector): /f$ /dot{{/bf q}} = [t_x, t_y, t_z, {/theta u}_x, {/theta  u}_y, {/theta u}_z] /f$ (see vpTranslationVector and vpThetaUVector). The  robot jacobian /f$ {^e}{/bf J}_e/f$ expressed in the end-effector frame is  here set to identity.  We use the exponential map (vpExponentialMap) to update the camera location.  Sampling time can be set using setSamplingTime().  /sa setSamplingTime()*/voidvpRobotCamera::setVelocity(const vpRobot::vpControlFrameType frame,                           const vpColVector &v){  switch (frame)  {  case vpRobot::ARTICULAR_FRAME:  case vpRobot::CAMERA_FRAME: {      if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {        setRobotState(vpRobot::STATE_VELOCITY_CONTROL);      }      vpColVector v_max(6);      for (unsigned int i=0; i<3; i++)        v_max[i] = getMaxTranslationVelocity();      for (unsigned int i=3; i<6; i++)        v_max[i] = getMaxRotationVelocity();      vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);      this->cMw_ = vpExponentialMap::direct(v_sat, delta_t_).inverse()*this->cMw_ ;      break ;    }  case vpRobot::REFERENCE_FRAME:    vpERROR_TRACE ("Cannot set a velocity in the reference frame: "                   "functionality not implemented");    throw vpRobotException (vpRobotException::wrongStateError,                            "Cannot set a velocity in the reference frame:"                            "functionality not implemented");    break ;  case vpRobot::MIXT_FRAME:    vpERROR_TRACE ("Cannot set a velocity in the mixt frame: "                   "functionality not implemented");    throw vpRobotException (vpRobotException::wrongStateError,                            "Cannot set a velocity in the mixt frame:"                            "functionality not implemented");    break ;  }}

/*!  Wait for a mouse button click and get the position of the clicked  pixel. The button used to click is also set.    /param ip [out] : The coordinates of the clicked image point.  /param button [out] : The button used to click.  /param blocking [in] :   - When set to true, this method waits until a mouse button is    pressed and then returns always true.  - When set to false, returns true only if a mouse button is    pressed, otherwise returns false.  /return true if a mouse button is pressed, false otherwise. If a  button is pressed, the location of the mouse pointer is updated in  /e ip.*/boolvpDisplayOpenCV::getClick(vpImagePoint &ip,                          vpMouseButton::vpMouseButtonType& button,                          bool blocking){  bool ret = false;  if (displayHasBeenInitialized) {    //flushDisplay() ;    double u, v;    if (blocking){      lbuttondown = false;      mbuttondown = false;      rbuttondown = false;    }    do {      if (lbuttondown){        ret = true ;        u = (unsigned int)x_lbuttondown;        v = (unsigned int)y_lbuttondown;	ip.set_u( u );	ip.set_v( v );        button = vpMouseButton::button1;        lbuttondown = false;      }      if (mbuttondown){        ret = true ;        u = (unsigned int)x_mbuttondown;        v = (unsigned int)y_mbuttondown;	ip.set_u( u );	ip.set_v( v );        button = vpMouseButton::button2;        mbuttondown = false;      }      if (rbuttondown){        ret = true ;        u = (unsigned int)x_rbuttondown;        v = (unsigned int)y_rbuttondown;	ip.set_u( u );	ip.set_v( v );        button = vpMouseButton::button3;        rbuttondown = false;      }      if (blocking) cvWaitKey(10);    } while ( ret == false && blocking == true);  }  else {    vpERROR_TRACE("OpenCV not initialized " ) ;    throw(vpDisplayException(vpDisplayException::notInitializedError,                             "OpenCV not initialized")) ;  }  return ret;}

/*!  Create a new directory.  /param dirname : Directory to create. The directory name  is converted to the current system's format; see path().  /exception vpIoException::cantCreateDirectory : If the directory cannot be  created.  /sa makeDirectory(const  char *)*/voidvpIoTools::makeDirectory(const std::string &dirname ){  try {    vpIoTools::makeDirectory(dirname.c_str());  }  catch (...) {    vpERROR_TRACE("unable to create directory '%s'/n",dirname.c_str());    throw(vpIoException(vpIoException::cantCreateDirectory,			"unable to create directory")) ;  }}

/*!  Get the 'index'th previous feature image coordinates.  Beware that  getPrevFeature(i,...) may not represent the same feature before and  after a tracking iteration (if a feature is lost, features are  shifted in the array).*/void vpKltOpencv::getPrevFeature(int index, int &id, float &x, float &y) const{  if (index >= countPrevFeatures)  {    vpERROR_TRACE(" Memory problem ");    throw(vpException(vpException::memoryAllocationError," Memory problem"));  }  x = prev_features[index].x;  y = prev_features[index].y;  id = prev_featuresid[index];}

/*!  /deprecated Set the pixel size.  This function is deprecated: use either initializer functions like  initPersProjWithoutDistortion() or initPersProjWithDistortion().*/voidvpCameraParameters::setPixelRatio(double px, double py){  this->px    = px ;  this->py    = py ;    if (fabs(px)<1e-6)  {    vpERROR_TRACE("Camera parameter px = 0") ;    throw(vpException(vpException::divideByZeroError,          "Camera parameter px = 0")) ;  }  if (fabs(py)<1e-6)  {    vpERROR_TRACE("Camera parameter px = 0") ;    throw(vpException(vpException::divideByZeroError,          "Camera parameter px = 0")) ;  }  this->inv_px = 1./px;  this->inv_py = 1./py;}

/*!	  Initialization of the tracking. The line is defined thanks to the  coordinates of two points.  /param I : Image in which the line appears.  /param ip1 : Coordinates of the first point.  /param ip2 : Coordinates of the second point.*/voidvpMeLine::initTracking(const vpImage<unsigned char> &I,                       const vpImagePoint &ip1,                       const vpImagePoint &ip2){  vpCDEBUG(1) <<" begin vpMeLine::initTracking()"<<std::endl ;  int i1s, j1s, i2s, j2s;  i1s = vpMath::round( ip1.get_i() );  i2s = vpMath::round( ip2.get_i() );  j1s = vpMath::round( ip1.get_j() );  j2s = vpMath::round( ip2.get_j() );  try{    //  1. On fait ce qui concerne les droites (peut etre vide)    {      // Points extremites      PExt[0].ifloat = (float)ip1.get_i() ;      PExt[0].jfloat = (float)ip1.get_j() ;      PExt[1].ifloat = (float)ip2.get_i() ;      PExt[1].jfloat = (float)ip2.get_j() ;      double angle_ = atan2((double)(i1s-i2s),(double)(j1s-j2s)) ;      a = cos(angle_) ;      b = sin(angle_) ;      // Real values of a, b can have an other sign. So to get the good values      // of a and b in order to initialise then c, we call track(I) just below      computeDelta(delta,i1s,j1s,i2s,j2s) ;      delta_1 = delta;      //      vpTRACE("a: %f b: %f c: %f -b/a: %f delta: %f", a, b, c, -(b/a), delta);      sample(I) ;    }    //  2. On appelle ce qui n'est pas specifique    {      vpMeTracker::initTracking(I) ;    }    // Call track(I) to give the good sign to a and b and to initialise c which can be used for the display    track(I);  }  catch(...)  {    vpERROR_TRACE("Error caught") ;    throw ;  }  vpCDEBUG(1) <<" end vpMeLine::initTracking()"<<std::endl ;}

/*!     Track the feature parameters in the camera frame (vpTracker::cP)  and than compute the projection of these parameters in the image  plane (vpTracker::p).  This method is similar to project(const vpHomogeneousMatrix &).  /warning The feature parameters in the object frame  (vpForwardProjection:oP) need to be set prior the use of this  method. To initialize these parameters see setWorldCoordinates().   /param cMo : The homogeneous matrix corresponding to the pose  between the camera frame and the object frame.*/voidvpForwardProjection::track(const vpHomogeneousMatrix &cMo){  try{    project(cMo) ;  }  catch(...)  {    vpERROR_TRACE("Error caught") ;    throw ;  }}

/*!	Sets the captured image width.    /warning This function must be called after open() method.  /param w : The requested value of the captured image width.	/exception vpFrameGrabberException::initializationError If no	camera was found.*/void vpOpenCVGrabber::setWidth(const unsigned int w){  if ( cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, w))	{	  	close();		vpERROR_TRACE("Impossible to set the size of the grabber");		throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,										"Impossible to set the size of the grabber") );	}  this->width = w;}

/*!  /brief Operation A = B   /param B : a vpMatrix of size nrow x 1*/vpSubColVector & vpSubColVector::operator=(const vpMatrix &B){  if ((B.getCols()!=1)||(rowNum != B.getRows()))  {    vpERROR_TRACE("/n/t/t vpSubColVector mismatch in operator vpSubColVector=vpMatrix") ;    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,			    "/n/t/t /n/t/t vpSubColVector mismatch in operator vpSubColVector=vpMatrix")) ;  }    for (unsigned int i=0;i<rowNum;i++)    data[i] = B[i][1];  return *this;}

/*!  /brief Add a new object in the scene graph  /param newObject : pointer toward the new object  /param fMo       : position of the object wrt the reference frame*/voidvpSimulator::addObject(SoSeparator * newObject, const vpHomogeneousMatrix &fMo){  try  {    this->addObject(newObject, fMo , scene);  }  catch(...)  {    vpERROR_TRACE("Error adding object in scene graph ") ;    throw ;  }}

/*!  This method choose a gray level (default is 0) used to modify the  "in" dot level in "out" dot level. This gray level is here needed to  stop the recursivity . The pixels of the dot are set to this new  gray level "/out/" when connexe() is called.  /exception vpTrackingException::initializationError : No valid gray  level "out" can be found. This means that the min gray level is set to 0  and the max gray level to 255. This should not occur*/void vpDot::setGrayLevelOut(){  if (gray_level_min == 0) {    if (gray_level_max == 255) {      // gray_level_min = 0 and gray_level_max = 255: this should not occur      vpERROR_TRACE("Unable to choose a good /"out/" level") ;      throw(vpTrackingException(vpTrackingException::initializationError,				"Unable to choose a good /"out/" level")) ;    }    gray_level_out = static_cast<unsigned char>(gray_level_max + 1u);  }}

/*!  /brief reshape the colvector in a matrix  /param m : the reshaped Matrix  /param nrows : number of rows of the matrix  /param ncols : number of columns of the matrix*/void vpColVector::reshape(vpMatrix & m,const unsigned int &nrows,const unsigned int &ncols){  if(dsize!=nrows*ncols)  {    vpERROR_TRACE("/n/t/t vpColVector mismatch size for reshape vpSubColVector in a vpMatrix") ;    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,			    "/n/t/t /n/t/t vpColVector mismatch size for reshape vpSubColVector in a vpMatrix")) ;  }  try   {    if ((m.getRows() != nrows) || (m.getCols() != ncols)) m.resize(nrows,ncols);  }  catch(vpException me)  {    vpERROR_TRACE("Error caught") ;    std::cout << me << std::endl ;    throw ;  }  for(unsigned int j =0; j< ncols; j++)     for(unsigned int i =0; i< nrows; i++)	  m[i][j]=data[j*ncols+i];}

/*!  Display a cross at the image point /e ip location.  /param ip : Cross location.  /param size : Size (width and height) of the cross.  /param color : Cross color.  /param thickness : Thickness of the lines used to display the cross.*/void vpDisplayGTK::displayCross ( const vpImagePoint &ip, 				  unsigned int size, 				  const vpColor &color,				  unsigned int thickness){  if (displayHasBeenInitialized)  {    try{      double i = ip.get_i();      double j = ip.get_j();      vpImagePoint ip1, ip2;      ip1.set_i( i-size/2 );       ip1.set_j( j );      ip2.set_i( i+size/2 );       ip2.set_j( j );      displayLine ( ip1, ip2, color, thickness ) ;      ip1.set_i( i );       ip1.set_j( j-size/2 );      ip2.set_i( i );       ip2.set_j( j+size/2 );      displayLine ( ip1, ip2, color, thickness ) ;    }    catch (...)    {      vpERROR_TRACE("Error caught") ;      throw ;    }  }  else  {    vpERROR_TRACE("GTK not initialized " ) ;    throw(vpDisplayException(vpDisplayException::notInitializedError,                             "GTK not initialized")) ;  }}

/*!  Set the window position in the screen.  /param winx, winy : Position of the upper-left window's border in the screen.  /exception vpDisplayException::notInitializedError : If the video  device is not initialized.*/void vpDisplayGTK::setWindowPosition(int winx, int winy){  if (displayHasBeenInitialized)  {    gtk_window_move(GTK_WINDOW(widget), winx, winy);   }  else  {    vpERROR_TRACE("GTK not initialized " ) ;    throw(vpDisplayException(vpDisplayException::notInitializedError,                             "GTK not initialized")) ;  }}

/*!  /brief get the value of all the features.  /param s_vector : It is a vector which will contain the value of the visual features.  /exception an exception is thrown if the number of row of the vector s  is different from the dimension of the visual feature as specified  in the constructor*/voidvpGenericFeature::get_s(vpColVector &s_vector) const{  if (s_vector.getRows() != dim_s)  {    vpERROR_TRACE("size mismatch between s dimension"		"and feature dimension");    throw(vpFeatureException(vpFeatureException::sizeMismatchError,			     "size mismatch between s dimension"			     "and feature dimension"));  }  s_vector = this->s ;}

/*!  Initialize a point feature with polar coordinates  /f$(/rho,/theta)/f$ using the coordinates of the point  /f$(x,y,Z)/f$, where /f$(x,y)/f$ correspond to the perspective  projection of the point in the image plane and /f$Z/f$ the 3D depth  of the point in the camera frame. The values of /f$(x,y,Z)/f$ are  expressed in meters. From the coordinates in the image plane, the  polar coordinates are computed by:  /f[/rho = /sqrt{x^2+y^2}  /hbox{,}/; /; /theta = /arctan /frac{y}{x}/f]  /param s : Visual feature /f$(/rho,/theta)/f$ and /f$Z/f$ to initialize.  /param p : A point with /f$(x,y)/f$ cartesian coordinates in the  image plane corresponding to the camera perspective projection, and  with 3D depth /f$Z/f$.*/voidvpFeatureBuilder::create(vpFeaturePointPolar &s, const vpPoint &p){  try {    double x = p.get_x();    double y = p.get_y();    double rho   = sqrt(x*x + y*y);    double theta = atan2(y, x);    s.set_rho(rho) ;    s.set_theta(theta) ;    s.set_Z( p.get_Z() )  ;    if (s.get_Z() < 0) {      vpERROR_TRACE("Point is behind the camera ") ;      std::cout <<"Z = " << s.get_Z() << std::endl ;      throw(vpFeatureException(vpFeatureException::badInitializationError,			       "Point is behind the camera ")) ;    }    if (fabs(s.get_Z()) < 1e-6) {      vpERROR_TRACE("Point Z coordinates is null ") ;      std::cout <<"Z = " << s.get_Z() << std::endl ;      throw(vpFeatureException(vpFeatureException::badInitializationError,			       "Point Z coordinates is null")) ;    }  }  catch(...) {    vpERROR_TRACE("Error caught") ;    throw ;  }}

/*!  Save the pose vector in the output file stream.  /param f : Output file stream. Should be open before entering in this method.  /exception vpException::ioError : If the output stream is not open.  /sa load()*/voidvpPoseVector::save(std::ofstream &f) const{  if (! f.fail())    {      f << *this ;    }  else    {      vpERROR_TRACE("/t/t file not open " );      throw(vpException(vpException::ioError, "/t/t file not open")) ;    }}

/*!  Write an homogeneous matrix in an output file stream.   /param f : Output file stream. The homogeneous matrix is saved as a  4 by 4 matrix.  The code below shows how to save an homogenous matrix in a file.  /code  // Contruct an homogeneous matrix  vpTranslationVector t(1,2,3);  vpRxyzVector r(M_PI, 0, -M_PI/4.);  vpRotationMatrix R(r);  vpHomogeneousMatrix M(t, R);    // Save the content of the matrix in "homogeneous.dat"  std::ofstream f("homogeneous.dat");    M.save(f);  /endcode  /sa load()*/voidvpHomogeneousMatrix::save(std::ofstream &f) const{  if (f != NULL)  {    f << *this ;  }  else  {    vpERROR_TRACE("/t/t file not open " );    throw(vpException(vpException::ioError, "/t/t file not open")) ;  }}

/*!  Open the serial port.  /exception vpRobotException::constructionError : If the device cannot be  oppened.*/voidvpRobotPtu46::init (){  vpDEBUG_TRACE (12, "Open connection Ptu-46.");  if (0 != ptu.init(device) )  {    vpERROR_TRACE ("Cannot open connexion with ptu-46.");    throw vpRobotException (vpRobotException::constructionError,			    "Cannot open connexion with ptu-46");  }  return ;}