/*!
Print on the output stream \e os the robot parameters (joint
min/max, distance between axis 5 and 6, coupling factor between axis
5 and 6, hand-to-eye homogeneous matrix.
\param os : Output stream.
\param afma4 : Robot parameters.
*/
VISP_EXPORT std::ostream & operator << (std::ostream & os,
const vpAfma4 & afma4)
{
vpRotationMatrix eRc;
afma4._eMc.extract(eRc);
vpRxyzVector rxyz(eRc);
os
<< "Joint Max:" << std::endl
<< "\t" << afma4._joint_max[0]
<< "\t" << afma4._joint_max[1]
<< "\t" << afma4._joint_max[2]
<< "\t" << afma4._joint_max[3]
<< "\t" << std::endl
<< "Joint Min: " << std::endl
<< "\t" << afma4._joint_min[0]
<< "\t" << afma4._joint_min[1]
<< "\t" << afma4._joint_min[2]
<< "\t" << afma4._joint_min[3]
<< "\t" << std::endl
<< "a1: " << std::endl
<< "\t" << afma4._a1
<< "\t" << std::endl
<< "d3: " << std::endl
<< "\t" << afma4._d3
<< "\t" << std::endl
<< "d4: " << std::endl
<< "\t" << afma4._d4
<< "\t" << std::endl
<< "eMc: "<< std::endl
<< "\tTranslation (m): "
<< afma4._eMc[0][3] << " "
<< afma4._eMc[1][3] << " "
<< afma4._eMc[2][3]
<< "\t" << std::endl
<< "\tRotation Rxyz (rad) : "
<< rxyz[0] << " "
<< rxyz[1] << " "
<< rxyz[2]
<< "\t" << std::endl
<< "\tRotation Rxyz (deg) : "
<< vpMath::deg(rxyz[0]) << " "
<< vpMath::deg(rxyz[1]) << " "
<< vpMath::deg(rxyz[2])
<< "\t" << std::endl;
return os;
}
int main()
{
#ifdef VISP_HAVE_GTK
try {
std::cout << "ViSP geometric features display example" <<std::endl;
unsigned int height = 288;
unsigned int width = 384;
vpImage<unsigned char> I(height,width);
I = 255; // I is a white image
// create a display window
vpDisplayGTK display;
// initialize a display attached to image I
display.init(I,100,100,"ViSP geometric features display");
// camera parameters to digitalize the image plane
vpCameraParameters cam(600,600,width/2,height/2); // px,py,u0,v0
// pose of the camera with reference to the scene
vpTranslationVector t(0,0,1);
vpRxyzVector rxyz(-M_PI/4,0,0);
vpRotationMatrix R(rxyz);
vpHomogeneousMatrix cMo(t, R);
// scene building, geometric features definition
vpPoint point;
point.setWorldCoordinates(0,0,0);// (X0=0,Y0=0,Z0=0)
vpLine line;
line.setWorldCoordinates(1,1,0,0,0,0,1,0); // planes:(X+Y=0)&(Z=0)
vpCylinder cylinder;
cylinder.setWorldCoordinates(1,-1,0,0,0,0,0.1); // alpha=1,beta=-1,gamma=0,
// X0=0,Y0=0,Z0=0,R=0.1
vpCircle circle;
circle.setWorldCoordinates(0,0,1,0,0,0,0.1); // plane:(Z=0),X0=0,Y0=0,Z=0,R=0.1
vpSphere sphere;
sphere.setWorldCoordinates(0,0,0,0.1); // X0=0,Y0=0,Z0=0,R=0.1
// change frame to be the camera frame and project features in the image plane
point.project(cMo);
line.project(cMo);
cylinder.project(cMo);
circle.project(cMo);
sphere.project(cMo);
// display the scene
vpDisplay::display(I); // display I
// draw the projections of the 3D geometric features in the image plane.
point.display(I,cam,vpColor::black); // draw a black cross over I
line.display(I,cam,vpColor::blue); // draw a blue line over I
cylinder.display(I,cam,vpColor::red); // draw two red lines over I
circle.display(I,cam,vpColor::orange); // draw an orange ellipse over I
sphere.display(I,cam,vpColor::black); // draw a black ellipse over I
vpDisplay::flush(I); // flush the display buffer
std::cout << "A click in the display to exit" << std::endl;
vpDisplay::getClick(I); // wait for a click in the display to exit
// save the drawing
vpImage<vpRGBa> Ic;
vpDisplay::getImage(I,Ic);
std::cout << "ViSP creates \"./geometricFeatures.ppm\" B&W image "<< std::endl;
vpImageIo::write(Ic, "./geometricFeatures.ppm");
return 0;
}
catch(vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
#endif
}
