/*!
Build a 3D plane thanks to 3 points and stores it in \f$ plane \f$.
\param P : The first point to define the plane
\param Q : The second point to define the plane
\param R : The third point to define the plane
\param plane : The vpPlane instance used to store the computed plane equation.
*/
void
buildPlane(vpPoint &P, vpPoint &Q, vpPoint &R, vpPlane &plane)
{
vpColVector a(3);
vpColVector b(3);
vpColVector n(3);
//Calculate vector corresponding to PQ
a[0]=P.get_oX()-Q.get_oX();
a[1]=P.get_oY()-Q.get_oY();
a[2]=P.get_oZ()-Q.get_oZ();
//Calculate vector corresponding to PR
b[0]=P.get_oX()-R.get_oX();
b[1]=P.get_oY()-R.get_oY();
b[2]=P.get_oZ()-R.get_oZ();
//Calculate normal vector to plane PQ x PR
n=vpColVector::cross(a,b);
//Equation of the plane is given by:
double A = n[0];
double B = n[1];
double C = n[2];
double D=-(A*P.get_oX()+B*P.get_oY()+C*P.get_oZ());
double norm = sqrt(A*A+B*B+C*C) ;
plane.setA(A/norm) ;
plane.setB(B/norm) ;
plane.setC(C/norm) ;
plane.setD(D/norm) ;
}
void
vpMbtDistanceKltCylinder::buildFrom(const vpPoint &p1, const vpPoint &p2, const double &r)
{
p1Ext = p1;
p2Ext = p2;
vpColVector ABC(3);
vpColVector V1(3);
vpColVector V2(3);
V1[0] = p1.get_oX();
V1[1] = p1.get_oY();
V1[2] = p1.get_oZ();
V2[0] = p2.get_oX();
V2[1] = p2.get_oY();
V2[2] = p2.get_oZ();
// Get the axis of the cylinder
ABC = V1-V2;
// Build our extremity circles
circle1.setWorldCoordinates(ABC[0],ABC[1],ABC[2],p1.get_oX(),p1.get_oY(),p1.get_oZ(),r);
circle2.setWorldCoordinates(ABC[0],ABC[1],ABC[2],p2.get_oX(),p2.get_oY(),p2.get_oZ(),r);
// Build our cylinder
cylinder.setWorldCoordinates(ABC[0],ABC[1],ABC[2],(p1.get_oX()+p2.get_oX())/2.0,(p1.get_oY()+p2.get_oY())/2.0,(p1.get_oZ()+p2.get_oZ())/2.0,r);
}
/*!
Build a vpMbtDistanceSphere thanks to its center, 3 points (including the center) with
coordinates expressed in the object frame and defining the plane that contain
the circle and its radius.
\param _p1 : Center of the circle.
\param _p2,_p3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
defining the plane that contains the circle.
\param r : Radius of the circle.
*/
void
vpMbtDistanceSphere::buildFrom(const vpPoint &_p1, const double r)
{
sphere = new vpSphere ;
p1 = new vpPoint ;
// Get the points
*p1 = _p1;
// Get the radius
radius = r;
// Build our sphere
sphere->setWorldCoordinates(_p1.get_oX(), _p1.get_oY(), _p1.get_oZ(), r);
}
void
kltFbTracker::genFeaturesForTrackOnFace(std::vector<vpPoint>& features, int faceID, int numOfPtsPerFace)
{
const vpPoint& vp1 = pyg[faceID].p[0],
vp2 = pyg[faceID].p[1],
vp3 = pyg[faceID].p[2],
vp4 = pyg[faceID].p[3];
float step = 1.0 / numOfPtsPerFace;
// avoid the edge
for (int i = 1; i < numOfPtsPerFace; i++)
for (int j = 1; j < numOfPtsPerFace; j++)
{
vpPoint fp;
float alpha = i * step;
float beta = j * step;
fp.set_oX((alpha + beta - 1) * vp2.get_oX() + (1 - alpha) * vp1.get_oX() + (1 - beta) * vp3.get_oX());
fp.set_oY((alpha + beta - 1) * vp2.get_oY() + (1 - alpha) * vp1.get_oY() + (1 - beta) * vp3.get_oY());
fp.set_oZ((alpha + beta - 1) * vp2.get_oZ() + (1 - alpha) * vp1.get_oZ() + (1 - beta) * vp3.get_oZ());
features.push_back(fp);
}
}
/*!
Unary function to convert the 3D coordinates in the object frame to a cv::Point3d.
\param point : Point to convert.
\return A cv::Point3d with the 3D coordinates stored in vpPoint in the object frame.
*/
cv::Point3d vpConvert::vpObjectPointToPoint3d(const vpPoint &point) {
return cv::Point3d(point.get_oX(), point.get_oY(), point.get_oZ());
}
/*!
Unary function to convert the 3D coordinates in the object frame to a cv::Point3f.
\param point : Point to convert.
\return A cv::Point3f with the 3D coordinates stored in vpPoint in the object frame.
*/
cv::Point3f vpConvert::vpObjectPointToPoint3f(const vpPoint &point) {
return cv::Point3f((float) point.get_oX(), (float) point.get_oY(), (float) point.get_oZ());
}