#define PRINT_CONDITION_NUMBER
#include <iostream>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpConfig.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/vpDisplayFactory.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(VISP_HAVE_DISPLAY)
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;
}
#elif !defined(VISP_HAVE_THREADS)
int main()
{
std::cout << "Can't run this example since multi-threading capability is not available." << std::endl;
std::cout << "You should maybe enable cxx11 standard." << std::endl;
return EXIT_SUCCESS;
}
#else
#ifdef ENABLE_VISP_NAMESPACE
#endif
#ifndef DOXYGEN_SHOULD_SKIP_THIS
class servoMoment
{
public:
servoMoment()
: m_width(640), m_height(480), m_cMo(), m_cdMo(), m_robot(), m_Iint(m_height, m_width,
vpRGBa(0)), m_task(), m_cam(),
m_error(0), m_imsim(), m_cur_img(m_height, m_width, 0), m_src_img(m_height, m_width, 0),
m_dst_img(m_height, m_width, 0), m_start_img(m_height, m_width,
vpRGBa(0)), m_interaction_type(), m_src(6), m_dst(6),
m_moments(nullptr), m_momentsDes(nullptr), m_featureMoments(nullptr), m_featureMomentsDes(nullptr), m_displayInt(nullptr)
{ }
~servoMoment()
{
#if defined(VISP_HAVE_DISPLAY) && (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
if (m_displayInt) {
delete m_displayInt;
}
#endif
delete m_moments;
delete m_momentsDes;
delete m_featureMoments;
delete m_featureMomentsDes;
}
{
m_cur_img = 0;
m_imsim.setCameraPosition(m_cMo);
m_imsim.getImage(m_cur_img, m_cam);
}
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);
imsim_start.
getImage(m_start_img, m_cam);
m_imsim.init(tmp_img, X);
m_imsim.setCameraPosition(m_cMo);
m_imsim.getImage(m_src_img, m_cam);
m_src.fromImage(m_src_img, 128, m_cam);
m_imsim.setCameraPosition(m_cdMo);
m_imsim.getImage(m_dst_img, m_cam);
m_dst.fromImage(m_dst_img, 128, m_cam);
}
void initFeatures()
{
double A;
double B;
double C;
double Ad;
double Bd;
double Cd;
planeToABC(pl, A, B, C);
planeToABC(pl, Ad, Bd, Cd);
m_cdMo.extract(vec);
m_moments->updateAll(m_src);
m_momentsDes->updateAll(m_dst);
m_featureMoments->updateAll(A, B, C);
m_featureMomentsDes->updateAll(Ad, Bd, Cd);
m_task.setInteractionMatrixType(m_interaction_type);
m_task.addFeature(m_featureMoments->getFeatureGravityNormalized(),
m_featureMomentsDes->getFeatureGravityNormalized());
m_task.addFeature(m_featureMoments->getFeatureAn(), m_featureMomentsDes->getFeatureAn());
m_task.addFeature(m_featureMoments->getFeatureCInvariant(), m_featureMomentsDes->getFeatureCInvariant(),
(1 << 10) | (1 << 11));
m_task.addFeature(m_featureMoments->getFeatureAlpha(), m_featureMomentsDes->getFeatureAlpha());
m_task.setLambda(1.);
}
{
m_cMo = cMo;
m_cdMo = cdMo;
#ifdef VISP_HAVE_DISPLAY
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
#else
#endif
#endif
paramRobot();
initScene();
initFeatures();
}
void execute(unsigned int nbIter)
{
init_visp_plot(ViSP_plot);
std::cout << "Display task information " << std::endl;
m_task.print();
unsigned int iter = 0;
m_robot.setPosition(wMc);
double sampling_time = 0.010;
m_robot.setSamplingTime(sampling_time);
while (iter++ < nbIter) {
wMc = m_robot.getPosition();
double A, B, C;
planeToABC(pl, A, B, C);
refreshScene(obj);
m_moments->updateAll(obj);
m_featureMoments->updateAll(A, B, C);
m_imsim.setCameraPosition(m_cMo);
m_Iint = m_start_img;
m_imsim.getImage(m_Iint, m_cam);
if (iter == 1) {
}
v = m_task.computeControlLaw();
std::cout <<
" || s - s* || = " << m_task.error.
sumSquare() << std::endl;
ViSP_plot.
plot(0, iter, v);
ViSP_plot.
plot(2, iter, m_task.getError());
m_error = (m_task.getError()).sumSquare();
#if defined(PRINT_CONDITION_NUMBER)
Linteraction.
svd(singularvals, tmpry);
std::cout << "Condition Number: " << condno << std::endl;
#endif
break;
}
}
m_imsim.getImage(m_Iint, m_cam);
}
double error() { return m_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 << m_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, 100 +
static_cast<int>(m_width), 50,
"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: ");
}
protected:
unsigned int m_width;
unsigned int m_height;
double m_error;
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
std::shared_ptr<vpDisplay> m_displayInt;
#else
#endif
};
#endif
int main()
{
try {
servoMoment servo;
servo.init(cMo, cdMo);
servo.execute(1500);
return EXIT_SUCCESS;
}
std::cout << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
}
#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Class to define RGB colors available for display functionalities.
static const vpColor cyan
static const vpColor orange
static const vpColor blue
static const vpColor purple
static const vpColor green
Class that defines generic functionalities for display.
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
@ divideByZeroError
Division by zero.
This class allows to access common vpFeatureMoments in a pre-filled database.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Class which enables to project an image in the 3D space and get the view of a virtual camera.
void getImage(vpImage< unsigned char > &I, const vpCameraParameters &cam)
void init(const vpImage< unsigned char > &I, vpColVector *X)
void setInterpolationType(const vpInterpolationType interplt)
void setCameraPosition(const vpHomogeneousMatrix &cMt)
static double rad(double deg)
Implementation of a matrix and operations on matrices.
void svd(vpColVector &w, vpMatrix &V)
This class initializes and allows access to commonly used moments.
static std::vector< double > getMu3(vpMomentObject &object)
static double getAlpha(vpMomentObject &object)
static double getSurface(vpMomentObject &object)
Class for generic objects.
void setType(vpObjectType input_type)
void fromImage(const vpImage< unsigned char > &image, unsigned char threshold, const vpCameraParameters &cam)
This class defines the container for a plane geometrical structure.
void changeFrame(const vpHomogeneousMatrix &cMo)
void setABCD(double a, double b, double c, double d)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
void initGraph(unsigned int graphNum, unsigned int curveNbr)
void init(unsigned int nbGraph, unsigned int height=700, unsigned int width=700, int x=-1, int y=-1, const std::string &title="")
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
void setColor(unsigned int graphNum, unsigned int curveNum, vpColor color)
void setTitle(unsigned int graphNum, const std::string &title)
Implementation of a pose vector and operations on poses.
vpServoIteractionMatrixType
Class that defines the simplest robot: a free flying camera.
Class that consider the case of a translation vector.
std::shared_ptr< vpDisplay > createDisplay()
Return a smart pointer vpDisplay specialization if a GUI library is available or nullptr otherwise.
vpDisplay * allocateDisplay()
Return a newly allocated vpDisplay specialization if a GUI library is available or nullptr otherwise.
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()