int main() { try { vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0); vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50)); std::vector<vpPoint> point; point.push_back( vpPoint(-0.1,-0.1, 0) ); point.push_back( vpPoint( 0.1,-0.1, 0) ); point.push_back( vpPoint( 0.1, 0.1, 0) ); point.push_back( vpPoint(-0.1, 0.1, 0) ); vpServo task ; task.setServo(vpServo::EYEINHAND_CAMERA); task.setInteractionMatrixType(vpServo::CURRENT); task.setLambda(0.5); vpFeaturePoint p[4], pd[4] ; for (unsigned int i = 0 ; i < 4 ; i++) { point[i].track(cdMo); vpFeatureBuilder::create(pd[i], point[i]); point[i].track(cMo); vpFeatureBuilder::create(p[i], point[i]); task.addFeature(p[i], pd[i]); } vpHomogeneousMatrix wMc, wMo; vpSimulatorCamera robot; robot.setSamplingTime(0.040); robot.getPosition(wMc); wMo = wMc * cMo; vpImage<unsigned char> Iint(480, 640, 255) ; vpImage<unsigned char> Iext(480, 640, 255) ; #if defined(VISP_HAVE_X11) vpDisplayX displayInt(Iint, 0, 0, "Internal view"); vpDisplayX displayExt(Iext, 670, 0, "External view"); #elif defined(VISP_HAVE_GDI) vpDisplayGDI displayInt(Iint, 0, 0, "Internal view"); vpDisplayGDI displayExt(Iext, 670, 0, "External view"); #elif defined(VISP_HAVE_OPENCV) vpDisplayOpenCV displayInt(Iint, 0, 0, "Internal view"); vpDisplayOpenCV displayExt(Iext, 670, 0, "External view"); #else std::cout << "No image viewer is available..." << std::endl; #endif #if defined(VISP_HAVE_DISPLAY) vpProjectionDisplay externalview; for (unsigned int i = 0 ; i < 4 ; i++) externalview.insert(point[i]) ; #endif vpCameraParameters cam(840, 840, Iint.getWidth()/2, Iint.getHeight()/2); vpHomogeneousMatrix cextMo(0,0,3, 0,0,0); while(1) { robot.getPosition(wMc); cMo = wMc.inverse() * wMo; for (unsigned int i = 0 ; i < 4 ; i++) { point[i].track(cMo); vpFeatureBuilder::create(p[i], point[i]); } vpColVector v = task.computeControlLaw(); robot.setVelocity(vpRobot::CAMERA_FRAME, v); vpDisplay::display(Iint) ; vpDisplay::display(Iext) ; display_trajectory(Iint, point, cMo, cam); vpServoDisplay::display(task, cam, Iint, vpColor::green, vpColor::red); #if defined(VISP_HAVE_DISPLAY) externalview.display(Iext, cextMo, cMo, cam, vpColor::red, true); #endif vpDisplay::flush(Iint); vpDisplay::flush(Iext); // A click to exit if (vpDisplay::getClick(Iint, false) || vpDisplay::getClick(Iext, false)) break; vpTime::wait( robot.getSamplingTime() * 1000); } task.kill(); } catch(vpException e) { std::cout << "Catch an exception: " << e << std::endl; } }
int main() { #if defined(VISP_HAVE_PTHREAD) try { vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0); vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50)); /* Top view of the world frame, the camera frame and the object frame world, also robot base frame : --> w_y | \|/ w_x object : o_y /|\ | o_x <-- camera : c_y /|\ | c_x <-- */ vpHomogeneousMatrix wMo(vpTranslationVector(0.40, 0, -0.15), vpRotationMatrix(vpRxyzVector(-M_PI, 0, M_PI/2.))); std::vector<vpPoint> point; point.push_back( vpPoint(-0.1,-0.1, 0) ); point.push_back( vpPoint( 0.1,-0.1, 0) ); point.push_back( vpPoint( 0.1, 0.1, 0) ); point.push_back( vpPoint(-0.1, 0.1, 0) ); vpServo task ; task.setServo(vpServo::EYEINHAND_CAMERA); task.setInteractionMatrixType(vpServo::CURRENT); task.setLambda(0.5); vpFeaturePoint p[4], pd[4] ; for (unsigned int i = 0 ; i < 4 ; i++) { point[i].track(cdMo); vpFeatureBuilder::create(pd[i], point[i]); point[i].track(cMo); vpFeatureBuilder::create(p[i], point[i]); task.addFeature(p[i], pd[i]); } vpSimulatorViper850 robot(true); robot.setVerbose(true); // Enlarge the default joint limits vpColVector qmin = robot.getJointMin(); vpColVector qmax = robot.getJointMax(); qmin[0] = -vpMath::rad(180); qmax[0] = vpMath::rad(180); qmax[1] = vpMath::rad(0); qmax[2] = vpMath::rad(270); qmin[4] = -vpMath::rad(180); qmax[4] = vpMath::rad(180); robot.setJointLimit(qmin, qmax); std::cout << "Robot joint limits: " << std::endl; for (unsigned int i=0; i< qmin.size(); i ++) std::cout << "Joint " << i << ": min " << vpMath::deg(qmin[i]) << " max " << vpMath::deg(qmax[i]) << " (deg)" << std::endl; robot.init(vpViper850::TOOL_PTGREY_FLEA2_CAMERA, vpCameraParameters::perspectiveProjWithoutDistortion); robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL); robot.initScene(vpWireFrameSimulator::PLATE, vpWireFrameSimulator::D_STANDARD); robot.set_fMo(wMo); bool ret = true; #if VISP_VERSION_INT > VP_VERSION_INT(2,7,0) ret = #endif robot.initialiseCameraRelativeToObject(cMo); if (ret == false) return 0; // Not able to set the position robot.setDesiredCameraPosition(cdMo); // We modify the default external camera position robot.setExternalCameraPosition(vpHomogeneousMatrix(vpTranslationVector(-0.4, 0.4, 2), vpRotationMatrix(vpRxyzVector(M_PI/2,0,0)))); vpImage<unsigned char> Iint(480, 640, 255); #if defined(VISP_HAVE_X11) vpDisplayX displayInt(Iint, 700, 0, "Internal view"); #elif defined(VISP_HAVE_GDI) vpDisplayGDI displayInt(Iint, 700, 0, "Internal view"); #elif defined(VISP_HAVE_OPENCV) vpDisplayOpenCV displayInt(Iint, 700, 0, "Internal view"); #else std::cout << "No image viewer is available..." << std::endl; #endif vpCameraParameters cam(840, 840, Iint.getWidth()/2, Iint.getHeight()/2); // Modify the camera parameters to match those used in the other simulations robot.setCameraParameters(cam); bool start = true; //for ( ; ; ) for (int iter =0; iter < 275; iter ++) { cMo = robot.get_cMo(); for (unsigned int i = 0 ; i < 4 ; i++) { point[i].track(cMo); vpFeatureBuilder::create(p[i], point[i]); } vpDisplay::display(Iint); robot.getInternalView(Iint); if (!start) { display_trajectory(Iint, point, cMo, cam); vpDisplay::displayText(Iint, 40, 120, "Click to stop the servo...", vpColor::red); } vpDisplay::flush(Iint); vpColVector v = task.computeControlLaw(); robot.setVelocity(vpRobot::CAMERA_FRAME, v); // A click to exit if (vpDisplay::getClick(Iint, false)) break; if (start) { start = false; v = 0; robot.setVelocity(vpRobot::CAMERA_FRAME, v); vpDisplay::displayText(Iint, 40, 120, "Click to start the servo...", vpColor::blue); vpDisplay::flush(Iint); //vpDisplay::getClick(Iint); } vpTime::wait(1000*robot.getSamplingTime()); } task.kill(); } catch(vpException e) { std::cout << "Catch an exception: " << e << std::endl; } #endif }
int main() { #if defined(VISP_HAVE_ZBAR) try { vpImage<unsigned char> I; vpImageIo::read(I, "bar-code.pgm"); #if defined(VISP_HAVE_X11) vpDisplayX d(I); #elif defined(VISP_HAVE_GDI) vpDisplayGDI d(I); #elif defined(VISP_HAVE_OPENCV) vpDisplayOpenCV d(I); #endif // Camera parameters should be adapted to your camera vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2); // 3D model of the QRcode: here we consider a 12cm by 12cm QRcode std::vector<vpPoint> point; point.push_back( vpPoint(-0.06, -0.06, 0) ); // QCcode point 0 3D coordinates in plane Z=0 point.push_back( vpPoint( 0.06, -0.06, 0) ); // QCcode point 1 3D coordinates in plane Z=0 point.push_back( vpPoint( 0.06, 0.06, 0) ); // QCcode point 2 3D coordinates in plane Z=0 point.push_back( vpPoint(-0.06, 0.06, 0) ); // QCcode point 3 3D coordinates in plane Z=0 vpHomogeneousMatrix cMo; bool init = true; vpDetectorQRCode detector; while(1) { vpImageIo::read(I, "bar-code.pgm"); vpDisplay::display(I); bool status = detector.detect(I); std::ostringstream legend; legend << detector.getNbObjects() << " bar code detected"; vpDisplay::displayText(I, (int)I.getHeight()-30, 10, legend.str(), vpColor::red); if (status) { // true if at least one QRcode is detected for(size_t i=0; i < detector.getNbObjects(); i++) { std::vector<vpImagePoint> p = detector.getPolygon(i); // get the four corners location in the image for(size_t j=0; j < p.size(); j++) { vpDisplay::displayCross(I, p[j], 14, vpColor::red, 3); std::ostringstream number; number << j; vpDisplay::displayText(I, p[j]+vpImagePoint(15,5), number.str(), vpColor::blue); } computePose(point, p, cam, init, cMo); // resulting pose is available in cMo var std::cout << "Pose translation (meter): " << cMo.getTranslationVector().t() << std::endl << "Pose rotation (quaternion): " << vpQuaternionVector(cMo.getRotationMatrix()).t() << std::endl; vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none, 3); } } vpDisplay::displayText(I, (int)I.getHeight()-15, 10, "A click to quit...", vpColor::red); vpDisplay::flush(I); if (vpDisplay::getClick(I, false)) break; vpTime::wait(40); } } catch(const vpException &e) { std::cout << "Catch an exception: " << e.getMessage() << std::endl; } #else std::cout << "ViSP is not build with zbar 3rd party." << std::endl; #endif }