void computeCentroidBlob(const vpImage<unsigned char> &I,vpDot2 &blob,vpImagePoint &cog,bool &init_done )
{
try
{
if (! init_done)
{
vpImagePoint germ;
vpDisplay::displayCharString(I, vpImagePoint(10,10), "Click in the blob to initialize the tracker", vpColor::red);
if (vpDisplay::getClick(I, germ, false))
{
blob.initTracking(I, germ);
blob.track(I);
cog = blob.getCog();
init_done = true;
}
}
else
{
blob.track(I);
cog = blob.getCog();
}
std::cout << "init done: " << init_done << std::endl;
}
catch(...)
{
init_done = false;
}
}
static int blob_process(uint8_t* frame, int width, int height, int size)
{
init_display(width, height);
if (!initialized || process_changed) {
blob.setGraphics(true);
blob.setGraphicsThickness(2);
initialized = true;
process_changed = false;
}
prepare_display(frame, width, height, size);
//compute blob
if (!init_track) {
blob.initTracking(*grey_img, germ);
init_track = true;
}
else {
blob.track(*grey_img);
}
int bat = jakopter_com_read_int(com_in, 0);
std::ostringstream bat_str;
bat_str << bat;
vpDisplay::displayText(*display_img, (int)display_img->getHeight()-25, 10,
"Battery : " + bat_str.str() + " %%", vpColor::red);
vpDisplay::flush(*display_img);
return 0;
}
/*!
Initialize a point feature with polar coordinates
\f$(\rho,\theta)\f$ using the coordinates of the point in pixels
obtained by image processing. This point is the center
of gravity of a dot tracked using vpDot2. Using the camera
parameters, the pixels coordinates of the dot are first
converted in cartesian \f$(x,y)\f$ coordinates in meter in the
camera frame and than in polar coordinates by:
\f[\rho = \sqrt{x^2+y^2} \hbox{,}\; \; \theta = \arctan \frac{y}{x}\f]
\warning This function does not initialize \f$Z\f$ which is
requested to compute the interaction matrix by
vpfeaturePointPolar::interaction().
\param s : Visual feature \f$(\rho,\theta)\f$ to initialize. Be
aware, the 3D depth \f$Z\f$ requested to compute the interaction
matrix is not initialized by this function.
\param cam : Camera parameters.
\param dot : Tracked dot. The center of gravity corresponds to the
coordinates of the point in the image plane.
The code below shows how to initialize a vpFeaturePointPolar visual
feature. First, we initialize the \f$\rho,\theta\f$, and lastly we
set the 3D depth \f$Z\f$ of the point which is generally the result
of a pose estimation.
\code
vpImage<unsigned char> I; // Image container
vpCameraParameters cam; // Default intrinsic camera parameters
vpDot2 dot; // Dot tracker
vpFeaturePointPolar s; // Point feature with polar coordinates
...
// Tracking on the dot
dot.track(I);
// Initialize rho,theta visual feature
vpFeatureBuilder::create(s, cam, dot);
// A pose estimation is requested to initialize Z, the depth of the
// point in the camera frame.
double Z = 1; // Depth of the point in meters
....
s.set_Z(Z);
\endcode
*/
void vpFeatureBuilder::create(vpFeaturePointPolar &s,
const vpCameraParameters &cam,
const vpDot2 &dot)
{
try {
double x=0, y=0;
vpImagePoint cog;
cog = dot.getCog();
vpPixelMeterConversion::convertPoint(cam, cog, x, y) ;
double rho = sqrt(x*x + y*y);
double theta = atan2(y, x);
s.set_rho(rho) ;
s.set_theta(theta) ;
}
catch(...) {
vpERROR_TRACE("Error caught") ;
throw ;
}
}
