#include <iostream>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpMomentCommon.h>
#include <visp3/core/vpMomentDatabase.h>
#include <visp3/core/vpMomentObject.h>
#include <visp3/core/vpPlane.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/robot/vpSimulatorAfma6.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureMomentCommon.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
int main()
{
std::cout << "Can't run this example since vpSimulatorAfma6 capability is "
"not available."
<< std::endl;
std::cout << "You should install pthread third-party library." << std::endl;
return EXIT_SUCCESS;
}
#elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \
!defined(VISP_HAVE_GTK)
int main()
{
std::cout << "Can't run this example since no display capability is available." << std::endl;
std::cout << "You should install one of the following third-party library: "
"X11, OpenCV, GDI, GTK."
<< std::endl;
return EXIT_SUCCESS;
}
#else
void paramRobot();
void initScene();
void initFeatures();
void execute(unsigned int nbIter);
double error();
void planeToABC(
vpPlane &pl,
double &A,
double &B,
double &C);
void paramRobot();
int main()
{
try {
init(cMo, cdMo);
execute(1500);
return EXIT_SUCCESS;
std::cout << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
}
#if defined VISP_HAVE_X11
#elif defined VISP_HAVE_OPENCV
#elif defined VISP_HAVE_GDI
#elif defined VISP_HAVE_D3D9
#elif defined VISP_HAVE_GTK
#endif
255);
double _error;
void initScene()
{
std::vector<vpPoint> src_pts;
std::vector<vpPoint> dst_pts;
double x[8] = {1, 3, 4, -1, -3, -2, -1, 1};
double y[8] = {0, 1, 4, 4, -2, -2, 1, 0};
int nbpoints = 8;
for (int i = 0; i < nbpoints; i++) {
vpPoint p(x[i] / 20, y[i] / 20, 0.0);
src_pts.push_back(p);
}
for (int i = 0; i < nbpoints; i++) {
vpPoint p(x[i] / 20, y[i] / 20, 0.0);
dst_pts.push_back(p);
}
}
void initFeatures()
{
double A;
double B;
double C;
double Ad;
double Bd;
double Cd;
planeToABC(pl, A, B, C);
planeToABC(pl, Ad, Bd, Cd);
(1 << 3) | (1 << 5));
}
{
double x[8] = {1, 3, 4, -1, -3, -2, -1, 1};
double y[8] = {0, 1, 4, 4, -2, -2, 1, 0};
int nbpoints = 8;
std::vector<vpPoint> cur_pts;
for (int i = 0; i < nbpoints; i++) {
vpPoint p(x[i] / 20, y[i] / 20, 0.0);
cur_pts.push_back(p);
}
}
{
cMo = _cMo;
cdMo = _cdMo;
displayInt.
init(Iint, 700, 0,
"Visual servoing with moments");
paramRobot();
initScene();
initFeatures();
}
void execute(unsigned int nbIter)
{
vpTRACE(
"Display task information ");
unsigned int iter = 0;
while (iter++ < nbIter) {
double A, B, C;
planeToABC(pl, A, B, C);
refreshScene(obj);
if (iter == 1)
}
vpTRACE(
"\n\nClick in the internal view window to end...");
delete moments;
delete momentsDes;
delete featureMoments;
delete featureMomentsDes;
}
{
}
void planeToABC(
vpPlane &pl,
double &A,
double &B,
double &C)
{
if (fabs(pl.
getD()) < std::numeric_limits<double>::epsilon()) {
std::cout << "Invalid position:" << std::endl;
std::cout << cMo << std::endl;
std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
}
}
void paramRobot()
{
removeJointLimits(robot);
}
double error() { return _error; }
#endif