Esempio n. 1
0
void CGView::move(GLdouble x, GLdouble  y, GLdouble  z)
{
    Vector3d t=Vector3d(x,y,z);
    if(this->t_act){
        this->objs[active].move(rotVec(t));
    } else {
        this->center+=rotVec(t);
    }
}
Esempio n. 2
0
	PxQuat computeQuatFromNormal(const PxVec3& n)
	{
		//parallel or anti-parallel
		if(n.x > 0.9999f)
		{
			//parallel
			return PxQuat::createIdentity();
		}
		else if (n.x < -0.9999f)
		{
			//anti-parallel
			//contactQuaternion.fromAngleAxisFast(PXD_PI, Vector3(0.0f, 1.0f, 0.0f));
			return PxQuat(0.0f, 1.0f, 0.0f, 0.0f);
		}
		else
		{
			PxVec3 rotVec(0.0f, -n.z, n.y);

			//Convert to quat
			PxReal angle = rotVec.magnitude();
			rotVec *= 1.0f/angle;
			if(angle > 1.0f) angle = 1.0f;

			// djs: injudiciously imbecilic use of trig functions, good thing Adam is going to trample this path like a
			// frustrated rhinoceros in mating season

			angle = PxAsin(angle);

			if(n.x < 0)
				angle = PxPi - angle;

			return PxQuat(angle, rotVec);
		}
	}
Esempio n. 3
0
//  CartPoseCostCalculator(const CartPoseCostCalculator& other) : pose_(other.pose_), manip_(other.manip_), rs_(other.rs_) {}
  VectorXd operator()(const VectorXd& dof_vals) const {
    manip_->SetDOFValues(toDblVec(dof_vals));
    OR::Transform newpose = link_->GetTransform();

    OR::Transform pose_err = pose_inv_ * newpose;
    VectorXd err = concat(rotVec(pose_err.rot), toVector3d(pose_err.trans));
    return err;
  }
Esempio n. 4
0
void TurretShape::updateMove(const Move* move)
{
   PROFILE_SCOPE( TurretShape_UpdateMove );

   if (!move)
      return;

   Point3F vec, pos;

   // Update orientation
   mTurretDelta.rotVec = mRot;

   VectorF rotVec(0, 0, 0);
   if (getAllowManualRotation())
   {
      if (mPitchAllowed)
      {
         rotVec.x = move->pitch * 2.0f;   // Assume that our -2PI to 2PI range was clamped to -PI to PI in script;
         if (mPitchRate > 0)
         {
            rotVec.x *= mPitchRate * TickSec;
         }
      }
      if (mHeadingAllowed)
      {
         rotVec.z = move->yaw * 2.0f;     // Assume that our -2PI to 2PI range was clamped to -PI to PI in script
         if (mHeadingRate > 0)
         {
            rotVec.z *= mHeadingRate * TickSec;
         }
      }
   }

   mRot.x += rotVec.x;
   mRot.z += rotVec.z;
   _applyLimits(mRot);

   if (isServerObject())
   {
      // As this ends up animating shape nodes, we have no sense of a transform and
      // render transform.  Therefore we treat this as the true transform and leave the
      // client shape node changes to interpolateTick() as the render transform.  Otherwise
      // on the client we'll have this node change from processTick() and then backstepping
      // and catching up to the true node change in interpolateTick(), which causes the
      // turret to stutter.
      _setRotation( mRot );
   }
   else
   {
      // If on the client, calc delta for backstepping
      mTurretDelta.rot = mRot;
      mTurretDelta.rotVec = mTurretDelta.rotVec - mTurretDelta.rot;
   }

   setMaskBits(TurretUpdateMask);
}
Esempio n. 5
0
cv::Mat trackingMire(cv::Mat *imCalibColor, cv::Mat *imCalibNext, std::vector<std::vector<cv::Point2f>> *chessCornersInit, std::vector<cv::Point3f> *chessCorners3D, cv::Mat *cameraMatrix, cv::Mat *distCoeffs, cv::Mat *tvecs)
{
    cv::Mat imCalib, rvecs;
    cv::swap(imCalib, *imCalibNext); // copie de l'ancienne image pour le flot optique
    (*chessCornersInit)[0] = (*chessCornersInit)[1];
    (*chessCornersInit)[1].clear();

    cv::cornerSubPix(imCalib, (*chessCornersInit)[0], cv::Size(5, 5), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));

    cv::cvtColor(*imCalibColor, *imCalibNext, CV_BGR2GRAY);

    std::vector<uchar> status;
    std::vector<float> err;
    cv::calcOpticalFlowPyrLK(imCalib, *imCalibNext, (*chessCornersInit)[0], (*chessCornersInit)[1], status, err, cv::Size(31, 31), 3, cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1), 0, 0.0001);

    cv::solvePnP(*chessCorners3D, (*chessCornersInit)[0], *cameraMatrix, *distCoeffs, rvecs, *tvecs);

    cv::Mat rotVec(3, 3, CV_64F);
    cv::Rodrigues(rvecs, rotVec);

    return rotVec;
}
Esempio n. 6
0
void main()
{
	bool patternfound = false;
	bool resetAuto = false;
	int nbImages = 0;
	double moyFinale = 0;
	bool detectionVisage = false;

	int nbrLoopSinceLastDetection = 0;
	int criticalValueOfLoopWithoutDetection = 15;

	std::cout << "initialisation de Chehra..." << std::endl;
	Chehra chehra;
	std::cout << "done" << std::endl;

	cv::Mat cameraMatrix, distCoeffs;
	cv::Mat rvecs, tvecs;

	std::vector<cv::Point2f> imagePoints;
	std::vector<cv::Point2f> pointsVisage2D;
	std::vector<cv::Point2f> moyPointsVisage2D;
	std::vector<cv::Point3f> pointsVisage3D;
	std::vector<cv::Point3f> visage;
	std::vector<double> distances;
	double moyDistances;

	std::vector<std::vector<cv::Point2f>> images;
	std::vector<cv::Mat> frames;

	double s = 10.0f;

	osg::Matrixd matrixS; // scale
	matrixS.set(
		s,	0,	0,	0,
		0,	s,	0,	0,
		0,	0,	s,	0,
		0,	0,	0,	1);
	
	pointsVisage3D.push_back(cv::Point3f(90,0,-80));
	pointsVisage3D.push_back(cv::Point3f(-90,0,-80));
	pointsVisage3D.push_back(cv::Point3f(0,0,0));
	pointsVisage3D.push_back(cv::Point3f(600,0,600));
	pointsVisage3D.push_back(cv::Point3f(0,0,600));
	pointsVisage3D.push_back(cv::Point3f(-600,0,600));
	/*
	pointsVisage3D.push_back(cv::Point3f(13.1, -98.1,108.3)); // exterieur narine gauche
	pointsVisage3D.push_back(cv::Point3f(-13.1, -98.1,108.3)); // exterieur narine droite
	pointsVisage3D.push_back(cv::Point3f(0, -87.2, 124.2)); // bout du nez
	pointsVisage3D.push_back(cv::Point3f(44.4, -57.9, 83.7)); // exterieur oeil gauche
	pointsVisage3D.push_back(cv::Point3f(0, 55.4, 101.4)); // haut du nez, centre des yeux
	pointsVisage3D.push_back(cv::Point3f(-44.4, -57.9, 83.7)); // exterieur oeil droit
	*/
	cv::FileStorage fs("../rsc/intrinsicMatrix.yml", cv::FileStorage::READ);

	fs["cameraMatrix"] >> cameraMatrix;
	fs["distCoeffs"] >> distCoeffs;

	double f = (cameraMatrix.at<double>(0, 0) + cameraMatrix.at<double>(1, 1)) / 2; // NEAR = distance focale ; si pixels carrés, fx = fy -> np 
	//mais est généralement différent de fy donc on prend (pour l'instant) par défaut la valeur médiane
	double g = 2000 * f; // je sais pas pourquoi. au pif.

	fs.release();

	cv::VideoCapture vcap(0); 
	if(!vcap.isOpened())
	{
		std::cout << "FAIL!" << std::endl;
		return;
	}

	cv::Mat *frame = new cv::Mat(cv::Mat::zeros(vcap.get(CV_CAP_PROP_FRAME_HEIGHT), vcap.get(CV_CAP_PROP_FRAME_WIDTH), CV_8UC3));

	do
	{
		vcap >> *frame;
	}while(frame->empty());

	osg::ref_ptr<osg::Image> backgroundImage = new osg::Image;
	backgroundImage->setImage(frame->cols, frame->rows, 3,
		GL_RGB, GL_BGR, GL_UNSIGNED_BYTE,
		(uchar*)(frame->data),
		osg::Image::AllocationMode::NO_DELETE, 1);

	// read the scene from the list of file specified commandline args.
	osg::ref_ptr<osg::Group> group = new osg::Group;
	osg::ref_ptr<osg::Geode> cam = createHUD(backgroundImage, vcap.get(CV_CAP_PROP_FRAME_WIDTH), vcap.get(CV_CAP_PROP_FRAME_HEIGHT), cameraMatrix.at<double>(0, 2), cameraMatrix.at<double>(1, 2), f);

	std::cout << "initialisation de l'objet 3D..." << std::endl;
	osg::ref_ptr<osg::Node> objet3D = osgDB::readNodeFile("../rsc/objets3D/brain.obj");
	//osg::ref_ptr<osg::Node> objet3D = osgDB::readNodeFile("../rsc/objets3D/dumptruck.osgt");

	////////////////////////////////////////////////////////////////////////////////////////////////////////////////  
	/*
	osg::Sphere* unitSphere = new osg::Sphere(osg::Vec3(0, -1000, 1000), 100.0);
	osg::ShapeDrawable* unitSphereDrawable = new osg::ShapeDrawable(unitSphere);

	osg::Geode* objet3D = new osg::Geode();

	objet3D->addDrawable(unitSphereDrawable);
	*/
	//osg::StateSet* sphereStateset = unitSphereDrawable->getOrCreateStateSet();
	//sphereStateset->setMode(GL_DEPTH_TEST,osg::StateAttribute::OFF);
	//sphereStateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF); 
	////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	std::cout << "done" << std::endl;

	osg::StateSet* obectStateset = objet3D->getOrCreateStateSet();
	obectStateset->setMode(GL_LIGHTING,osg::StateAttribute::OFF);
	obectStateset->setMode(GL_DEPTH_TEST,osg::StateAttribute::OFF);
	osg::ref_ptr<osg::MatrixTransform> mat = new osg::MatrixTransform();

	// construct the viewer.
	osgViewer::CompositeViewer compositeViewer;
	osgViewer::View* viewer = new osgViewer::View;
	osgViewer::View* viewer2 = new osgViewer::View;

	// add the HUD subgraph.
	group->addChild(cam);

	mat->addChild(objet3D);
	group->addChild(mat);

	osg::Matrixd projectionMatrix;

	projectionMatrix.makeFrustum(
		-cameraMatrix.at<double>(0, 2),		vcap.get(CV_CAP_PROP_FRAME_WIDTH) - cameraMatrix.at<double>(0, 2),
		-cameraMatrix.at<double>(1, 2),		vcap.get(CV_CAP_PROP_FRAME_HEIGHT) - cameraMatrix.at<double>(1, 2),
		f,									g);

	osg::Vec3d eye(0.0f, 0.0f, 0.0f), target(0.0f, g, 0.0f), normal(0.0f, 0.0f, 1.0f);

	// set the scene to render
	viewer->setSceneData(group.get());
	viewer->setUpViewInWindow(0, 0, 1920 / 2, 1080 / 2); 
	viewer->getCamera()->setProjectionMatrix(projectionMatrix);
	viewer->getCamera()->setViewMatrixAsLookAt(eye, target, normal);

	viewer2->setSceneData(group.get());
	viewer2->setUpViewInWindow(1920 / 2, 0, 1920 / 2, 1080 / 2); 
	viewer2->getCamera()->setProjectionMatrix(projectionMatrix);
	osg::Vec3d eye2(4 * f, 3 * f / 2, 0.0f), target2(0.0f, f, 0.0f), normal2(0.0f, 0.0f, 1.0f);
	viewer2->getCamera()->setViewMatrixAsLookAt(eye2, target2, normal2);

	compositeViewer.addView(viewer2);
	compositeViewer.addView(viewer);

	compositeViewer.realize();  // set up windows and associated threads.

	do
	{       
		patternfound = false;
		resetAuto = false;
		detectionVisage = false;

		moyPointsVisage2D.clear();
		pointsVisage2D.clear();
		visage.clear();
		moyDistances = 0;
		distances.clear();

		std::cout << "recherche de pattern" << std::endl
			<< "nbr images sauvegardees : " << images.size() << std::endl;

		vcap >> *frame;
		frames.push_back(*frame);

		detectionVisage = detecterVisage(frame, &chehra, &pointsVisage2D, &visage);

		if(detectionVisage)
		{
			images.push_back(pointsVisage2D);
			nbrLoopSinceLastDetection = 0;
			group->addChild(mat);
		}
		else
			nbrLoopSinceLastDetection++;

		if((images.size() > NBRSAVEDIMAGES || nbrLoopSinceLastDetection > criticalValueOfLoopWithoutDetection) && !images.empty())
			images.erase(images.begin());

		if(images.empty())
			group->removeChild(mat);

		else
		{
			//cv::cornerSubPix(*frame, pointsVisage2D, cv::Size(5, 5), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));

			for(int i = 0; i < NBRFACEPOINTSDETECTED; i++)
			{
				cv::Point2f coordonee(0.0f, 0.0f);
				for(int j = 0; j < images.size(); j++)
				{
					coordonee.x += images[j][i].x;
					coordonee.y += images[j][i].y;
				}
				coordonee.x /= images.size();
				coordonee.y /= images.size();

				moyPointsVisage2D.push_back(coordonee);
			}

			cv::solvePnP(pointsVisage3D, moyPointsVisage2D, cameraMatrix, distCoeffs, rvecs, tvecs);

			cv::Mat rotVec(3, 3, CV_64F);
			cv::Rodrigues(rvecs, rotVec);
			
			imagePoints = dessinerPoints(frame, pointsVisage3D, rotVec, tvecs, cameraMatrix, distCoeffs);

			double t3 = tvecs.at<double>(2, 0);
			double t1 = tvecs.at<double>(0, 0);
			double t2 = tvecs.at<double>(1, 0) + t3 / 27.5; // and now, magic !

			double r11 = rotVec.at<double>(0, 0);
			double r12 = rotVec.at<double>(0, 1);
			double r13 = rotVec.at<double>(0, 2);
			double r21 = rotVec.at<double>(1, 0);
			double r22 = rotVec.at<double>(1, 1);
			double r23 = rotVec.at<double>(1, 2);
			double r31 = rotVec.at<double>(2, 0);
			double r32 = rotVec.at<double>(2, 1);
			double r33 = rotVec.at<double>(2, 2);


			osg::Matrixd matrixR; // rotation (transposee de rotVec)
			matrixR.set(
				r11,	r21,	r31,	0,
				r12,	r22,	r32,	0,
				r13,	r23,	r33,	0,
				0,		0,		0,		1);

			osg::Matrixd matrixT; // translation
			matrixT.makeTranslate(t1, t2, t3);

			osg::Matrixd matrix90; // rotation de repere entre opencv et osg
			matrix90.makeRotate(osg::Quat(osg::DegreesToRadians(-90.0f), osg::Vec3d(1.0, 0.0, 0.0)));

			mat->setMatrix(matrixS * matrixR * matrixT * matrix90);

			// Calcul d'erreur de reprojection
			double moy = 0;
			for(int i = 0; i < pointsVisage2D.size(); i++)
			{
				double d = sqrt(pow(pointsVisage2D[i].y - imagePoints[i].y, 2) + pow(pointsVisage2D[i].x - imagePoints[i].x, 2));
				distances.push_back(d);
				moy += d;
			}

			moyDistances = moy / pointsVisage2D.size();

			if(moyDistances > 2) // si l'ecart de reproj est trop grand, reset
				resetAuto = true;
		}
		
		backgroundImage->dirty();
		compositeViewer.frame();

	}while(!compositeViewer.done());
}
int main()
{
	time_t timer = 0;
	time_t start = clock();
	time_t startImage = 0;
	std::cout << "Debut projection\t" << std::endl;

	bool patternfound = false;
	int i = 0;

	cv::TermCriteria termcrit(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.03);
	cv::Size winSize(31,31);
	
	cv::Mat cameraMatrix, distCoeffs;
	cv::Mat imCalib;
	cv::Mat imCalibColor;
	cv::Mat imCalibNext;
	cv::Mat rvecs, tvecs;
	std::vector<cv::Point2f> imagePoints;
	std::vector<cv::Point3f> objectPoints;
	std::vector<cv::Point2f> chessCornersInit[2];
	std::vector<cv::Point3f> chessCorners3D;

	// Creation des points a projeter
	objectPoints.push_back(cv::Point3f(50,25,0));
	objectPoints.push_back(cv::Point3f(150,25,0));
	objectPoints.push_back(cv::Point3f(150,125,0));
	objectPoints.push_back(cv::Point3f(50,125,0));
	objectPoints.push_back(cv::Point3f(50,25,100));
	objectPoints.push_back(cv::Point3f(150,25,100));
	objectPoints.push_back(cv::Point3f(150,125,100));
	objectPoints.push_back(cv::Point3f(50,125,100));

	// Creation des coins de la mire
	for(int x=0 ; x<COLCHESSBOARD ; x++)
		for(int y=0 ; y<ROWCHESSBOARD ; y++)
			chessCorners3D.push_back(cv::Point3f(x*26.0f,y*26.0f,0.0f));	

	cv::FileStorage fs("../rsc/intrinsicMatrix.yml", cv::FileStorage::READ);

	fs["cameraMatrix"] >> cameraMatrix;
	fs["distCoeffs"] >> distCoeffs;

	fs.release();

	cv::VideoCapture vcap("../rsc/capture.avi"); 
	if(!vcap.isOpened()){
		  std::cout << "FAIL!" << std::endl;
		  return -1;
	}

	do{
		vcap >> imCalibColor;
		cv::imshow("Projection", imCalibColor);
		cv::cvtColor(imCalibColor, imCalib, CV_BGR2GRAY);
		cv::waitKey();
		timer = clock();
		startImage = clock();
	
		patternfound = cv::findChessboardCorners(imCalib, cv::Size(ROWCHESSBOARD, COLCHESSBOARD), chessCornersInit[0], cv::CALIB_CB_FAST_CHECK);
		
		std::cout << "findChessboardCorners\t" << float(clock()-timer)/CLOCKS_PER_SEC << " sec" << std::endl;
		timer = clock(); 
	} while(!patternfound);

	for(;;)
	{		
		vcap >> imCalibColor;		
						
		if(!imCalibNext.empty())
		{
			cv::swap(imCalib, imCalibNext); // copie de l'ancienne image pour le flot optique
			for(size_t c = 0; c < chessCornersInit[0].size(); c++)
				chessCornersInit[0][c] = chessCornersInit[1][c];
			chessCornersInit[1].clear();
		}
		else
			cv::cornerSubPix(imCalib, chessCornersInit[0], cv::Size(5, 5), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));

		cv::cvtColor(imCalibColor, imCalibNext, CV_BGR2GRAY);

		std::vector<uchar> status;
		std::vector<float> err;
		cv::calcOpticalFlowPyrLK(imCalib, imCalibNext, chessCornersInit[0], chessCornersInit[1], status, err, winSize, 3, termcrit, 0, 0.0001);

		cv::solvePnP(chessCorners3D, chessCornersInit[0], cameraMatrix, distCoeffs, rvecs, tvecs);

		cv::Mat rotVec(3, 3, CV_64F);
		cv::Rodrigues(rvecs, rotVec);

		//Projection
		cv::projectPoints(objectPoints, rotVec, tvecs, cameraMatrix, distCoeffs, imagePoints);

		// Dessin des points projetes
		cv::line(imCalibColor, imagePoints[0], imagePoints[4], cv::Scalar(255,255,0), 2, 8);
		cv::line(imCalibColor, imagePoints[1], imagePoints[5], cv::Scalar(255,255,0), 2, 8);
		cv::line(imCalibColor, imagePoints[2], imagePoints[6], cv::Scalar(255,255,0), 2, 8);
		cv::line(imCalibColor, imagePoints[3], imagePoints[7], cv::Scalar(255,255,0), 2, 8);

		cv::line(imCalibColor, imagePoints[0], imagePoints[1], cv::Scalar(255,0,255), 2, 8);
		cv::line(imCalibColor, imagePoints[1], imagePoints[2], cv::Scalar(255,0,255), 2, 8);
		cv::line(imCalibColor, imagePoints[2], imagePoints[3], cv::Scalar(255,0,255), 2, 8);
		cv::line(imCalibColor, imagePoints[3], imagePoints[0], cv::Scalar(255,0,255), 2, 8);

		cv::line(imCalibColor, imagePoints[4], imagePoints[5], cv::Scalar(0,255,255), 2, 8);
		cv::line(imCalibColor, imagePoints[5], imagePoints[6], cv::Scalar(0,255,255), 2, 8);
		cv::line(imCalibColor, imagePoints[6], imagePoints[7], cv::Scalar(0,255,255), 2, 8);
		cv::line(imCalibColor, imagePoints[7], imagePoints[4], cv::Scalar(0,255,255), 2, 8);

		cv::imshow("Projection", imCalibColor);

		cv::waitKey(67);
	}

	return 0;
}
Esempio n. 8
0
 cv::Mat Transform::GetCvRotationVector() const
 {
   cv::Mat rotVec(3,1,cv::DataType<mitk::ScalarType>::type);
   cv::Rodrigues( this->GetCvRotationMatrix(), rotVec );
   return rotVec;
 }