/*!
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()
*/
void
vpRobotBiclops::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.
*/
void
vpRobotBiclops::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.
*/
void
vpDisplayOpenCV::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.
*/
void
vpServolens::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()
*/
void
vpRobotCamera::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.
*/
bool
vpDisplayOpenCV::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 *)
*/
void
vpIoTools::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().
*/
void
vpCameraParameters::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.
*/
void
vpMeLine::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.
*/
void
vpForwardProjection::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;
}
/*!
Sets the captured image height.
\warning This function must be called after open() method.
\param h : The requested value of the captured image height.
\exception vpFrameGrabberException::initializationError If no
camera was found.
*/
void vpOpenCVGrabber::setHeight(const unsigned int h)
{
if ( cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, h))
{
close();
vpERROR_TRACE("Impossible to set the size of the grabber");
throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
"Impossible to set the size of the grabber") );
}
this->height = h;
}
/*!
\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;
}
/*!
Compute the feature parameters in the camera frame (vpTracker::cP)
and than compute the projection of these parameters in the image
plane (vpTracker::p).
\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.
*/
void
vpForwardProjection::project(const vpHomogeneousMatrix &cMo)
{
try{
changeFrame(cMo) ;
projection() ;
}
catch(...)
{
vpERROR_TRACE("Error caught") ;
throw ;
}
}
/*!
\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
*/
void
vpSimulator::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];
}
/*!
\relates vpColVector
\brief compute the dot product of two vectors C = a.b
*/
double
vpColVector::dotProd(const vpColVector &a, const vpColVector &b)
{
if (a.data==NULL)
{
vpERROR_TRACE("vpColVector a non initialized") ;
throw(vpMatrixException(vpMatrixException::notInitializedError)) ;
}
if (b.data==NULL)
{
vpERROR_TRACE("vpColVector b non initialized") ;
throw(vpMatrixException(vpMatrixException::notInitializedError)) ;
}
double *ad = a.data ; double *bd = b.data ;
double c = 0 ;
for (unsigned int i=0 ; i < a.getRows() ; i++)
c += *(ad++)* *(bd++) ;
// vpMatrix c = (a.t() * b);
// return c[0][0];
return c ;
}
/*!
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
*/
void
vpGenericFeature::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$.
*/
void
vpFeatureBuilder::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()
*/
void
vpPoseVector::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()
*/
void
vpHomogeneousMatrix::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")) ;
}
}
/*!
\brief Compute the cross product of two vectors C = a x b
\param a : vpColVector
\param b : vpColVector
*/
vpColVector vpColVector::crossProd(const vpColVector &a, const vpColVector &b)
{
if (a.getRows() != 3)
{
vpERROR_TRACE("Cannot compute cross product,"
"a has not 3 rows") ;
throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
"\n\t\t Cannot compute cross product"
"a has not 3 rows")) ;
}
if (b.getRows() != 3)
{
vpERROR_TRACE("Cannot compute cross product,"
"b has not 3 rows") ;
throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
"\n\t\t Cannot compute cross product"
"b has not 3 rows")) ;;
}
return vpColVector::skew(a) * b;
}
/*!
Open the serial port.
\exception vpRobotException::constructionError : If the device cannot be
oppened.
*/
void
vpRobotPtu46::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 ;
}
