//! Division by scalar. Returns a quaternion defined by (x/l,y/l,z/l,w/l).
vpQuaternionVector vpQuaternionVector::operator/(const double l) const
{
if(vpMath::nul(l, std::numeric_limits<double>::epsilon())) {
throw vpException(vpException::fatalError, "Division by scalar l==0 !");
}
return vpQuaternionVector(x()/l,y()/l,z()/l,w()/l);
}
//! Multiply two quaternions.
vpQuaternionVector vpQuaternionVector::operator* ( vpQuaternionVector &rq) {
return vpQuaternionVector(w() * rq.x() + x() * rq.w() + y() * rq.z() - z() * rq.y(),
w() * rq.y() + y() * rq.w() + z() * rq.x() - x() * rq.z(),
w() * rq.z() + z() * rq.w() + x() * rq.y() - y() * rq.x(),
w() * rq.w() - x() * rq.x() - y() * rq.y() - z() * rq.z());
}
//! Multiplication by scalar. Returns a quaternion defined by (lx,ly,lz,lw).
vpQuaternionVector vpQuaternionVector::operator*( double l)
{
return vpQuaternionVector(l*x(),l*y(),l*z(),l*w());
}
//! Negate operator. Returns a quaternion defined by (-x,-y,-z-,-w).
vpQuaternionVector vpQuaternionVector::operator-()
{
return vpQuaternionVector(-x(), -y(), -z(), -w());
}
/*!
Quaternion substraction.
Substracts a quaternion from another. Substraction is component-wise.
\param q : quaternion to substract.
*/
vpQuaternionVector vpQuaternionVector::operator-( vpQuaternionVector &q)
{
return vpQuaternionVector(x()-q.x(), y()-q.y(), z()-q.z(), w()-q.w());
}
int main()
{
#if defined(VISP_HAVE_ZBAR)
try {
vpImage<unsigned char> I;
vpImageIo::read(I, "bar-code.pgm");
#if defined(VISP_HAVE_X11)
vpDisplayX d(I);
#elif defined(VISP_HAVE_GDI)
vpDisplayGDI d(I);
#elif defined(VISP_HAVE_OPENCV)
vpDisplayOpenCV d(I);
#endif
// Camera parameters should be adapted to your camera
vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);
// 3D model of the QRcode: here we consider a 12cm by 12cm QRcode
std::vector<vpPoint> point;
point.push_back( vpPoint(-0.06, -0.06, 0) ); // QCcode point 0 3D coordinates in plane Z=0
point.push_back( vpPoint( 0.06, -0.06, 0) ); // QCcode point 1 3D coordinates in plane Z=0
point.push_back( vpPoint( 0.06, 0.06, 0) ); // QCcode point 2 3D coordinates in plane Z=0
point.push_back( vpPoint(-0.06, 0.06, 0) ); // QCcode point 3 3D coordinates in plane Z=0
vpHomogeneousMatrix cMo;
bool init = true;
vpDetectorQRCode detector;
while(1) {
vpImageIo::read(I, "bar-code.pgm");
vpDisplay::display(I);
bool status = detector.detect(I);
std::ostringstream legend;
legend << detector.getNbObjects() << " bar code detected";
vpDisplay::displayText(I, (int)I.getHeight()-30, 10, legend.str(), vpColor::red);
if (status) { // true if at least one QRcode is detected
for(size_t i=0; i < detector.getNbObjects(); i++) {
std::vector<vpImagePoint> p = detector.getPolygon(i); // get the four corners location in the image
for(size_t j=0; j < p.size(); j++) {
vpDisplay::displayCross(I, p[j], 14, vpColor::red, 3);
std::ostringstream number;
number << j;
vpDisplay::displayText(I, p[j]+vpImagePoint(15,5), number.str(), vpColor::blue);
}
computePose(point, p, cam, init, cMo); // resulting pose is available in cMo var
std::cout << "Pose translation (meter): " << cMo.getTranslationVector().t() << std::endl
<< "Pose rotation (quaternion): " << vpQuaternionVector(cMo.getRotationMatrix()).t() << std::endl;
vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none, 3);
}
}
vpDisplay::displayText(I, (int)I.getHeight()-15, 10, "A click to quit...", vpColor::red);
vpDisplay::flush(I);
if (vpDisplay::getClick(I, false)) break;
vpTime::wait(40);
}
}
catch(const vpException &e) {
std::cout << "Catch an exception: " << e.getMessage() << std::endl;
}
#else
std::cout << "ViSP is not build with zbar 3rd party." << std::endl;
#endif
}
/*!
Quaternion conjugate.
\return The conjugate quaternion.
*/
vpQuaternionVector vpQuaternionVector::conjugate() const {
return vpQuaternionVector( -x(), -y(), -z(), w() );
}
/*!
Quaternion addition.
Adds two quaternions. Addition is component-wise.
\param q : quaternion to add.
*/
vpQuaternionVector vpQuaternionVector::operator+(const vpQuaternionVector &q) const
{
return vpQuaternionVector(x()+q.x(), y()+q.y(), z()+q.z(), w()+q.w());
}
