#define PRINT_CONDITION_NUMBER
#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/core/vpPoseVector.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpPlot.h>
#include <visp3/robot/vpImageSimulator.h>
#include <visp3/robot/vpSimulatorCamera.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();
void init_visp_plot(
vpPlot &);
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
double _error;
void initScene()
{
for (int i = 0; i < 4; i++)
X[i].resize(3);
X[0][0] = -0.2;
X[0][1] = -0.1;
X[0][2] = 0;
X[1][0] = 0.2;
X[1][1] = -0.1;
X[1][2] = 0;
X[2][0] = 0.2;
X[2][1] = 0.1;
X[2][2] = 0;
X[3][0] = -0.2;
X[3][1] = 0.1;
X[3][2] = 0;
imsim_start.
init(tmp_start_img, X);
}
{
cur_img = 0;
}
{
cMo = _cMo;
cdMo = _cdMo;
displayInt.
init(Iint, 700, 0,
"Visual servoing with moments");
paramRobot();
initScene();
initFeatures();
}
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 << 10) | (1 << 11));
}
void execute(unsigned int nbIter)
{
init_visp_plot(ViSP_plot);
vpTRACE(
"Display task information ");
unsigned int iter = 0;
float sampling_time = 0.010f;
while (iter++ < nbIter) {
double A, B, C;
planeToABC(pl, A, B, C);
refreshScene(obj);
Iint = start_img;
if (iter == 1)
err_features = task.
error;
std::cout <<
" || s - s* || = " << task.
error.
sumSquare() << std::endl;
ViSP_plot.
plot(0, iter, v);
ViSP_plot.
plot(1, iter, currentpose);
ViSP_plot.
plot(2, iter, err_features);
#if defined(PRINT_CONDITION_NUMBER)
Linteraction.
svd(singularvals, tmpry);
std::cout << "Condition Number: " << condno << std::endl;
#endif
}
vpTRACE(
"\n\nClick in the internal view window to end...");
delete moments;
delete momentsDes;
delete featureMoments;
delete featureMomentsDes;
}
double error() { return _error; }
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 init_visp_plot(
vpPlot &ViSP_plot)
{
const unsigned int NbGraphs = 3;
const unsigned int NbCurves_in_graph[NbGraphs] = {6, 6, 6};
ViSP_plot.
init(NbGraphs, 800, 800, 10, 10,
"Visual Servoing results...");
for (unsigned int p = 0; p < NbGraphs; p++) {
ViSP_plot.
initGraph(p, NbCurves_in_graph[p]);
for (unsigned int c = 0; c < NbCurves_in_graph[p]; c++)
}
ViSP_plot.
setTitle(0,
"Robot velocities");
ViSP_plot.
setTitle(1,
"Camera pose cMo");
ViSP_plot.
setTitle(2,
"Error in visual features: ");
}
#endif