double FuzzyIndices::CampelloIndices::compute_d(const std::vector<int> &elements) const {
double result = 0.0;
for (size_t j1 = 0; j1 < elements.size(); j1++) {
for(size_t j2 = j1+1; j2 < elements.size(); j2++) {
result += std::min(computeX(elements[j1], elements[j2]), computeZ(elements[j1], elements[j2]));
}
}
return result;
}
/*!
Compute the interaction matrix and the residu vector for the face.
The method assumes that these two objects are properly sized in order to be
able to improve the speed with the use of SubCoVector and subMatrix.
\warning The function preCompute must be called before the this method.
\param _cMc0 : camera displacement since initialisation
\param _R : the residu vector
\param _J : the interaction matrix
*/
void
vpMbtDistanceKltCylinder::computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &_cMc0, vpColVector& _R, vpMatrix& _J)
{
unsigned int index_ = 0;
cylinder.changeFrame(_cMc0 * c0Mo);
std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
for( ; iter != curPoints.end(); iter++){
int id(iter->first);
double i_cur(iter->second.get_i()), j_cur(iter->second.get_j());
double x_cur(0), y_cur(0);
vpPixelMeterConversion::convertPoint(cam, j_cur, i_cur, x_cur, y_cur);
vpPoint p0 = initPoints3D[id];
p0.changeFrame(_cMc0);
p0.project();
double x0_transform(p0.get_x()), y0_transform(p0.get_y()) ;
double Z = computeZ(x_cur, y_cur);
if(vpMath::isNaN(Z) || Z < std::numeric_limits<double>::epsilon()){
// std::cout << "Z is Nan : " << A << " , " << B << " , " << C << " | " << Z << " | " << x_cur << " , " << y_cur << std::endl;
// std::cout << std::sqrt(B*B - A*C) << " , " << B*B - A*C << std::endl;
_J[2*index_][0] = 0;
_J[2*index_][1] = 0;
_J[2*index_][2] = 0;
_J[2*index_][3] = 0;
_J[2*index_][4] = 0;
_J[2*index_][5] = 0;
_J[2*index_+1][0] = 0;
_J[2*index_+1][1] = 0;
_J[2*index_+1][2] = 0;
_J[2*index_+1][3] = 0;
_J[2*index_+1][4] = 0;
_J[2*index_+1][5] = 0;
_R[2*index_] = (x0_transform - x_cur);
_R[2*index_+1] = (y0_transform - y_cur);
index_++;
}
else
{
double invZ = 1.0/Z;
_J[2*index_][0] = - invZ;
_J[2*index_][1] = 0;
_J[2*index_][2] = x_cur * invZ;
_J[2*index_][3] = x_cur * y_cur;
_J[2*index_][4] = -(1+x_cur*x_cur);
_J[2*index_][5] = y_cur;
_J[2*index_+1][0] = 0;
_J[2*index_+1][1] = - invZ;
_J[2*index_+1][2] = y_cur * invZ;
_J[2*index_+1][3] = (1+y_cur*y_cur);
_J[2*index_+1][4] = - y_cur * x_cur;
_J[2*index_+1][5] = - x_cur;
_R[2*index_] = (x0_transform - x_cur);
_R[2*index_+1] = (y0_transform - y_cur);
index_++;
}
}
}
/*!
Initialise the cylinder to track. All the points in the map, representing all the
map detected in the image, are parsed in order to extract the id of the points
that are indeed in the face.
\param _tracker : ViSP OpenCV KLT Tracker.
\param cMo : Pose of the object in the camera frame at initialization.
*/
void
vpMbtDistanceKltCylinder::init(const vpKltOpencv& _tracker, const vpHomogeneousMatrix &cMo)
{
c0Mo = cMo;
cylinder.changeFrame(cMo);
// extract ids of the points in the face
nbPointsInit = 0;
nbPointsCur = 0;
initPoints = std::map<int, vpImagePoint>();
initPoints3D = std::map<int, vpPoint>();
curPoints = std::map<int, vpImagePoint>();
curPointsInd = std::map<int, int>();
for (unsigned int i = 0; i < static_cast<unsigned int>(_tracker.getNbFeatures()); i ++){
int id;
float x_tmp, y_tmp;
_tracker.getFeature((int)i, id, x_tmp, y_tmp);
bool add = false;
if(useScanLine)
{
if((unsigned int)y_tmp < hiddenface->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
(unsigned int)x_tmp < hiddenface->getMbScanLineRenderer().getPrimitiveIDs().getWidth())
{
for(unsigned int kc = 0 ; kc < listIndicesCylinderBBox.size() ; kc++)
if(hiddenface->getMbScanLineRenderer().getPrimitiveIDs()[(unsigned int)y_tmp][(unsigned int)x_tmp] == listIndicesCylinderBBox[kc])
{
add = true;
break;
}
}
}
else
{
std::vector<vpImagePoint> roi;
for(unsigned int kc = 0 ; kc < listIndicesCylinderBBox.size() ; kc++)
{
hiddenface->getPolygon()[(size_t) listIndicesCylinderBBox[kc]]->getRoiClipped(cam, roi);
if(vpPolygon::isInside(roi, y_tmp, x_tmp))
{
add = true;
break;
}
roi.clear();
}
}
if(add){
double xm=0, ym=0;
vpPixelMeterConversion::convertPoint(cam, x_tmp, y_tmp, xm, ym);
double Z = computeZ(xm,ym);
if(!vpMath::isNaN(Z)){
initPoints[id] = vpImagePoint(y_tmp, x_tmp);
curPoints[id] = vpImagePoint(y_tmp, x_tmp);
curPointsInd[id] = (int)i;
nbPointsInit++;
nbPointsCur++;
vpPoint p;
p.setWorldCoordinates(xm * Z, ym * Z, Z);
initPoints3D[id] = p;
//std::cout << "Computed Z for : " << xm << "," << ym << " : " << computeZ(xm,ym) << std::endl;
}
}
}
if(nbPointsCur >= minNbPoint) enoughPoints = true;
else enoughPoints = false;
//std::cout << "Nb detected points in cylinder : " << nbPointsCur << std::endl;
}
void RecordedCamera::moveCamera_mouse(int x, int y)
{
Quat newQuat;
const unsigned int widthViewport = p_widthViewport.getValue();
const unsigned int heightViewport = p_heightViewport.getValue();
if (isMoving)
{
if (currentMode == TRACKBALL_MODE)
{
float x1 = (2.0f * widthViewport / 2.0f - widthViewport) / widthViewport;
float y1 = (heightViewport- 2.0f *heightViewport / 2.0f) /heightViewport;
float x2 = (2.0f * (x + (-lastMousePosX + widthViewport / 2.0f)) - widthViewport) / widthViewport;
float y2 = (heightViewport- 2.0f * (y + (-lastMousePosY +heightViewport / 2.0f))) /heightViewport;
currentTrackball.ComputeQuaternion(x1, y1, x2, y2);
//fetch rotation
newQuat = currentTrackball.GetQuaternion();
Vec3 pivot;
switch (p_pivot.getValue())
{
case WORLD_CENTER_PIVOT:
pivot = Vec3(0.0, 0.0, 0.0);
break;
case SCENE_CENTER_PIVOT :
default:
pivot = sceneCenter;
break;
}
BaseCamera::rotateWorldAroundPoint(newQuat, pivot, this->getOrientation());
}
else if (currentMode == ZOOM_MODE)
{
Vec3 trans(0.0, 0.0, -p_zoomSpeed.getValue() * (y - lastMousePosY) / heightViewport);
trans = cameraToWorldTransform(trans);
translate(trans);
translateLookAt(trans);
}
else if (currentMode == PAN_MODE)
{
Vec3 trans(lastMousePosX - x, y-lastMousePosY, 0.0);
trans = cameraToWorldTransform(trans)*p_panSpeed.getValue();
translate(trans);
translateLookAt(trans);
}
//must call update afterwards
lastMousePosX = x;
lastMousePosY = y;
}
else if (currentMode == WHEEL_ZOOM_MODE)
{
Vec3 trans(0.0, 0.0, -p_zoomSpeed.getValue() * (y*0.5) / heightViewport);
trans = cameraToWorldTransform(trans);
translate((trans));
translateLookAt(trans);
currentMode = NONE_MODE;
}
computeZ();
}
