Esempio n. 1
0
void PoseGraph::optimizeNodes( int iterations )
{
    // perform the graph optimization
    optimizer->hogman->optimize( iterations, false );

    // write the poses back to the environment
    // for now, write all the poses back, could add an updated flag
    for( std::map<std::string, SensorMaps*>::iterator it = nodeMap.begin();
	    it != nodeMap.end(); it++ )
    {
	SensorMaps *sm = it->second;
	AISNavigation::PoseGraph3D::Vertex 
	    *vertex = optimizer->hogman->vertex( sm->vertexId );

	// get pose with uncertainty from Hogman
	envire::TransformWithUncertainty tu( 
		hogman2Eigen( vertex->transformation ), 
		hogmanCov2EnvireCov( vertex->covariance ) );

	envire::FrameNode::Ptr fn = sm->frameNode; 
	fn->setTransform( tu );

	// update the bounds 
	// TODO this could be optimized, as it may be to expensive to 
	// update the bounds everytime we have a small change in position
	sm->update();
    }
}
Esempio n. 2
0
static int wj_req_with_res(ost::URLStream &s, const char *url, std::ostream &os)
{
	TimeUsed tu(url);

	ost::URLStream::Error err = s.get(url);
	if (err)
		return err;

	std::stringstream ss;
	while (!s.eof()) {
		std::string x;
		s >> x;
		os << x;
		ss << x;
	}

	if (tu.is_timeout()) {
		fprintf(stderr, "\t:%s\n", ss.str().c_str());
	}

	// 根据 { "result":"ok",  } 进行判断.
	util_TypedObject to = util_build_object_from_json_string(ss.str().c_str());
	if (to.type == 4 && to.data.ov["result"].type == 2 && to.data.ov["result"].data.sv == "ok") {
		return 0;
	}
	else {
		fprintf(stderr, "<libptz> ERROR: 云台失败:'%s', str=%s\n", url, ss.str().c_str());
		return -1;
	}
}
void computePose(std::vector<vpPoint> &point, const std::vector<vpImagePoint> &corners,
                 const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo)
{
  vpPose pose;
  double x=0, y=0;
  for (unsigned int i=0; i < point.size(); i ++) {
    vpPixelMeterConversion::convertPoint(cam, corners[i], x, y);
    point[i].set_x(x);
    point[i].set_y(y);
    pose.addPoint(point[i]);
  }

  if (init == true) pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
  else              pose.computePose(vpPose::VIRTUAL_VS, cMo) ;

  vpTranslationVector t;
  cMo.extract(t);
  vpRotationMatrix R;
  cMo.extract(R);
  vpThetaUVector tu(R);

  std::cout << "cMo: ";
  for (unsigned int i=0; i < 3; i++)
    std::cout << t[i] << " ";
  for (unsigned int i=0; i < 3; i++)
    std::cout << vpMath::deg(tu[i]) << " ";
  std::cout << std::endl;
}
Esempio n. 4
0
/*!
  Build a 3D \f$ \theta u \f$ visual feature from an
  homogeneous matrix \f$ M \f$ that represent the displacement that
  the camera has to achieve.

  \param M [in] : Homogeneous transformation that describe the
  movement that the camera has to achieve. Only the rotational part of
  this homogeneous transformation is taken into consideration.
  Depending on the rotation representation type
  (vpFeatureThetaU::vpFeatureThetaURotationRepresentationType) used to
  construct this object, the parameter \e M represents either the
  rotation that the camera has to achieve to move from the desired
  camera frame to the current one (\f$ ^{c^*}R_c\f$), or the rotation
  that the camera has to achieve to move from the current camera frame
  to the desired one (\f$ ^{c}R_{c^*}\f$).


*/
void
vpFeatureThetaU::buildFrom(const vpHomogeneousMatrix &M)
{
    vpRotationMatrix R ;
    M.extract(R)  ;
    vpThetaUVector tu(R) ;
    buildFrom(tu) ;
}
Esempio n. 5
0
/*!

  Initialize a force/torque twist transformation matrix from a translation vector
  \f${\bf t}=(t_x, t_y, t_z)^T\f$ and a rotation vector with \f$\theta {\bf u}=(\theta u_x, \theta u_y, \theta u_z)^T \f$ parametrization.

  \param tx,ty,tz : Translation vector in meters.

  \param tux,tuy,tuz : \f$\theta {\bf u}\f$ rotation vector expressed in radians.
*/
vpForceTwistMatrix::vpForceTwistMatrix(const double tx, const double ty, const double tz,
                                       const double tux, const double tuy, const double tuz)
  : vpArray2D<double>(6,6)
{
  vpTranslationVector T(tx,ty,tz) ;
  vpThetaUVector tu(tux,tuy,tuz) ;
  buildFrom(T,tu) ;  
}
Esempio n. 6
0
void Helpers::GetCurrentUserAndDomain(std::wstring& strUser, std::wstring& strDomain)
{
	std::unique_ptr<void, CloseHandleHelper> hToken(nullptr);

	{
		HANDLE _hToken = nullptr;

		if ( !::OpenProcessToken(
			::GetCurrentProcess(),
			TOKEN_QUERY,
			&_hToken) )
		{
			Win32Exception::ThrowFromLastError("OpenProcessToken");
		}

		hToken.reset(_hToken);
	}

	DWORD dwReturnLength = 0;

	if ( !::GetTokenInformation(
		hToken.get(),
		TokenUser,
		nullptr,
		0,
		&dwReturnLength ) )
	{
		if( GetLastError() != ERROR_INSUFFICIENT_BUFFER )
			Win32Exception::ThrowFromLastError("GetTokenInformation");
	}

	std::unique_ptr<BYTE[]> tu(new BYTE[dwReturnLength]);

	if ( !::GetTokenInformation(
		hToken.get(),
		TokenUser,
		tu.get(),
		dwReturnLength,
		&dwReturnLength ) )
	{
			Win32Exception::ThrowFromLastError("GetTokenInformation");
	}

	wchar_t szUser[CRED_MAX_STRING_LENGTH + 1] = L"";
	wchar_t szDomain[CRED_MAX_STRING_LENGTH + 1] = L"";
	DWORD dwUserLen = static_cast<DWORD>(ARRAYSIZE(szUser));
	DWORD dwDomainLen = static_cast<DWORD>(ARRAYSIZE(szDomain));
	SID_NAME_USE snu;

	// Retrieve user name and domain name based on user's SID.
	if( !LookupAccountSid(NULL, reinterpret_cast<TOKEN_USER*>(tu.get())->User.Sid, szUser, &dwUserLen, szDomain, &dwDomainLen, &snu) )
	{
		Win32Exception::ThrowFromLastError("LookupAccountSid");
	}

	strUser = szUser;
	strDomain = szDomain;
}
Esempio n. 7
0
vpFeatureThetaU::vpFeatureThetaU(vpRotationMatrix &R, 
                                 vpFeatureThetaURotationRepresentationType r)
  : rotation(r)
{
  init() ;
  
  vpThetaUVector tu(R) ;
  buildFrom(tu) ;
}
Esempio n. 8
0
/*!

  Constructor that build a 3D \f$ \theta u \f$ visual feature from an
  homogeneous matrix \f$ M \f$ that represent the displacement that
  the camera has to achieve.

  \param M [in] : Homogeneous transformation that describe the
  movement that the camera has to achieve. Only the rotational part of
  this homogeneous transformation is taken into
  consideration. Depending on r the rotation represent either the
  rotation that the camera has to achieve to move from the desired
  camera frame to the current one (\f$ ^{c^*}R_c\f$), or the rotation
  that the camera has to achieve to move from the current camera frame
  to the desired one (\f$ ^{c}R_{c^*}\f$).

  \param r [in] : The rotation representation of M.


*/
vpFeatureThetaU::vpFeatureThetaU(vpHomogeneousMatrix &M, 
                                 vpFeatureThetaURotationRepresentationType r)
  : rotation(r)
{
  init() ;
  vpRotationMatrix R ;
  M.extract(R)  ;
  vpThetaUVector tu(R) ;
  buildFrom(tu) ;
}
void
vpHomogeneousMatrix::buildFrom(const vpPoseVector &p)
{

  vpTranslationVector t(p[0],p[1],p[2]) ;
  vpThetaUVector tu(p[3],p[4],p[5]) ;

  insert(tu) ;
  insert(t) ;
}
/*! 
  Constructs a quaternion from a rotation matrix.
  
  \param R : Rotation matrix.
*/
void vpQuaternionVector::buildFrom(const vpRotationMatrix &R)
{
  vpThetaUVector tu(R);
  vpColVector u;
  double theta;
  tu.extract(theta, u);

  theta *= 0.5;

  double sinTheta_2 = sin(theta);
  set( cos(theta), u[0] * sinTheta_2, u[1] * sinTheta_2, u[2] * sinTheta_2);
}
/*!

  Initialize a force/torque twist transformation matrix from a translation vector
  \e t and a rotation vector with \f$\theta u \f$ parametrization.

  \param tx,ty,tz : Translation vector in meters.

  \param tux,tuy,tuz : \f$\theta u\f$ rotation vector expressed in radians.
*/
vpForceTwistMatrix::vpForceTwistMatrix(const double tx,
				       const double ty,
				       const double tz,
				       const double tux,
				       const double tuy,
				       const double tuz) : vpMatrix()
{
  init() ;
  vpTranslationVector T(tx,ty,tz) ;
  vpThetaUVector tu(tux,tuy,tuz) ;
  buildFrom(T,tu) ;  
}
Esempio n. 12
0
int MGSBRep::reduce(		//BSUDEC
	int is_u,		//if true, reduce b-rep dimension of u-direction.
	int ndec)		//Number of B-rep dimension to decrease 
//Change the B-Rep by decreasing B-Rep dimension by ndec. This is
//an approximation of the original B-Rep. Return value is error flag.
{
	assert(ndec>=0);
	if(is_u) assert(bdim_u()-ndec>=order_u());
	else 	 assert(bdim_v()-ndec>=order_v());

	if(ndec<=0) return 0;

	size_t kdec=2; if(is_u) kdec=1;
	unsigned ku=order_u(); size_t lud=bdim_u();
	unsigned kv=order_v(); size_t lvd=bdim_v();
	unsigned maxlen=lud; if(maxlen<lvd) maxlen=lvd;
	size_t is1,is2; surface_bcoef().capacity(is1,is2);
	int lud2, lvd2;
	MGKnotVector tu(ku,lud), tv(kv,lvd);
	MGSPointSeq bcoef(lud,lvd,3);
	int iflag;
	unsigned k=ku; if(k<kv) k=kv;
	double* work1=new double[maxlen*(2*k-1)];
	double* work2=new double[maxlen];
	double* work3=new double[lvd*2];
	bsudec_(
		ku,lud,knot_data_u(),kv,lvd,knot_data_v(),coef_data(),
		kdec,ndec,is1,is2,lud,lvd,work1,work2,work3,
		&lud2,&tu(0),&lvd2,&tv(0),&bcoef(0,0,0),&iflag);
	if(iflag==1){
		tu.set_bdim(lud2); tv.set_bdim(lvd2);
		m_uknot=tu; m_vknot=tv;
		bcoef.set_length(lud2,lvd2);
		m_surface_bcoef=bcoef;
		iflag=0;
	}
	delete[] work1; delete[] work2; delete[] work3;
	update_mark();
	return iflag;
}
Esempio n. 13
0
void Doom3TextureManager::initContext(GLContextData& contextData) const
	{
	/* Create a context data item and store it in the OpenGL context: */
	DataItem* dataItem=new DataItem(numTextures);
	contextData.addDataItem(this,dataItem);
	
	/* Upload all textures: */
	TextureUploader tu(*this,dataItem);
	imageTree.forEachLeaf(tu);
	
	/* Unbind all texture objects: */
	glBindTexture(GL_TEXTURE_2D,0);
	}
Esempio n. 14
0
// Construct Surface B-rep of order 4 by interpolation from Point data
//and end condition.
// Inner points may include derivative inf.
MGSBRep::MGSBRep(
	MGSBRepEndC& endc,			//end condition
	const MGNDDArray& utaui,	//Data point abscissa of u-direction
	const MGNDDArray& vtaui,	//Data point abscissa of v-direction
	const MGSPointSeq& value,	//Data point ordinate
	int &error)					//Error flag.
:MGSurface(){
	assert(utaui.length()==value.length_u()
		&& vtaui.length()==value.length_v());

	MGNDDArray utau,vtau;
	add_data_point(endc,utaui,vtaui,utau,vtau);
	MGKnotVector tu(utau, 4), tv(vtau,4);
	*this=MGSBRep(endc, utaui, vtaui, value, tu, tv, error);
}
Esempio n. 15
0
void printPose(const std::string &text, const vpHomogeneousMatrix &cMo)
{
  vpTranslationVector t;
  cMo.extract(t);
  vpRotationMatrix R;
  cMo.extract(R);
  vpThetaUVector tu(R);

  std::cout << text;
  for (unsigned int i=0; i < 3; i++)
    std::cout << t[i] << " ";
  for (unsigned int i=0; i < 3; i++)
    std::cout << vpMath::deg(tu[i]) << " ";
  std::cout << std::endl;
}
Esempio n. 16
0
// Construct Surface B-rep of specified order by interpolation from Point data.
//This is an easy-to-version.
MGSBRep::MGSBRep(				//Derivative Inf.
	const MGNDDArray& utau,	//Data point of u-direction of value
	const MGNDDArray& vtau,	//Data point of v-direction	of value
	const MGSPointSeq& value,	//Data point ordinate
	size_t orderu, size_t orderv//order along u or v direction.
):MGSurface(){
	assert(utau.length()==value.length_u()
		&& vtau.length()==value.length_v());

	if(utau.length()<orderu) orderu=utau.length();
	if(vtau.length()<orderv) orderv=vtau.length();
	MGKnotVector tu(utau, orderu), tv(vtau,orderv);
	MGSBRepEndC endc;
	int error;
	*this=MGSBRep(endc, utau, vtau, value, tu, tv, error);
}
Esempio n. 17
0
int main(int argc, char *argv[])
{
    auto index = clang_createIndex(0, 0);
    std::vector<std::string> default_args = { {"-x"}, {"c++"}, {"-std=c++11"} };
    std::string filename;
    ArgList arglist( default_args );

    if ( argc > 1 ) {
	arglist = ArgList( argc - 2, argv + 1 );
	filename = argv[argc - 1];
    } else {
	filename = argv[1];
    }
      
    TUnit tu( index, filename );
  
    if ( !tu.parse( arglist.count(), arglist ) ) {
	std::cout << "Translation Unit Initial Parse Failed!\n";
    }

    std::string input;
    std::vector<char> filebuffer;
    while( std::getline( std::cin, input ) ) {
	if ( input == "REPARSE" ) {
	    filebuffer = ReparseSource();

	    CXUnsavedFile unsaved_file = { filename.c_str(),
					   filebuffer.data(),
					   filebuffer.size() };

	    // std::cout << "Size = " << filebuffer.size()
	    // 		<< "Contents:\n" << filebuffer.data()
	    // 		<< "\n";

	    if ( tu.parse( std::vector<CXUnsavedFile>( 1, unsaved_file ) ) ) {
		TokenizeSource( tu.handle() );
	    } else {
		std::cout << "Reparse FAILED!\n" << end_pattern << "\n";
	    }
	}
    }

    clang_disposeIndex( index );
    return 0;
}
Esempio n. 18
0
/*****************************************************************************
 * Convert a filename to DOS format.
 *
 *  Input:  OutName - Source *and* destination buffer. 
 *
 *                    NOTE that OutName must point to a memory space that
 *                    is at least 13 bytes in size! That should always be
 *                    the case of course.
 *
 * ****************************************************************************
 */
void name_mangle(char *OutName)
{
    char    *p;

    p = xstrcpy(OutName);
    /*
     * check if it's already in 8.3 format
     */
    if (!is_8_3(OutName)) {
	mangle_name_83(OutName);
    } else {
	/*
	 * No mangling needed, convert to uppercase
	 */
	tu(OutName);
    }

    free(p);
}
Esempio n. 19
0
// Construct Surface B-rep of specified order by interpolation from Point data
//and end condition.
//Inner points must not include first derivative inf if the corresponding data
//points are multiple if the order is not 4.
MGSBRep::MGSBRep(				//Derivative Inf.
	MGSBRepEndC& endc,			//end condition
	size_t orderu, size_t orderv,//order along u or v direction, must be greater than 4.
	const MGNDDArray& utaui,	//Data point of u-direction of value
	const MGNDDArray& vtaui,	//Data point of v-direction	of value
	const MGSPointSeq& value,	//Data point ordinate
	int &error)					//Error flag.
:MGSurface(){
	assert(utaui.length()==value.length_u()
		&& vtaui.length()==value.length_v());
	assert(orderu>=4 && orderv>=4);

	MGNDDArray utau,vtau;
	add_data_point(endc,utaui,vtaui,utau,vtau);
	size_t nu=utau.length(), nv=vtau.length();
	if(orderu>nu) orderu=nu;
	if(orderv>nv) orderv=nv;
	MGKnotVector tu(utau, orderu), tv(vtau,orderv);
	*this=MGSBRep(endc, utaui, vtaui, value, tu, tv, error);
}
Esempio n. 20
0
/* ************************************************************************** **
 * Return True if a name is a special msdos reserved name.
 *
 *  Input:  fname - String containing the name to be tested.
 *
 *  Output: True, if the name matches one of the list of reserved names.
 *
 *  Notes:  This is a static function called by is_8_3(), below.
 *
 * ************************************************************************** **
 */
static int is_reserved_msdos( char *fname )
{
    char upperFname[13];
    char *p;

    strncpy (upperFname, fname, 12);

    /* lpt1.txt and con.txt etc are also illegal */
    p = strchr(upperFname,'.');
    if (p)
	*p = '\0';

    tu(upperFname);
    p = upperFname + 1;
    switch (upperFname[0]) {
	case 'A':
		if( 0 == strcmp( p, "UX" ) )
		    return TRUE;
		break;
	case 'C':
		if ((0 == strcmp( p, "LOCK$" )) || (0 == strcmp( p, "ON" )) || (0 == strcmp( p, "OM1" ))
		    || (0 == strcmp( p, "OM2" )) || (0 == strcmp( p, "OM3" )) || (0 == strcmp( p, "OM4" )))
		    return TRUE;
		break;
	case 'L':
		if( (0 == strcmp( p, "PT1" )) || (0 == strcmp( p, "PT2" )) || (0 == strcmp( p, "PT3" )))
		    return TRUE;
		break;
	case 'N':
		if( 0 == strcmp( p, "UL" ) )
		    return TRUE;
		break;
	case 'P':
		if( 0 == strcmp( p, "RN" ) )
		    return TRUE;
		break;
    }

    return FALSE;
} /* is_reserved_msdos */
Esempio n. 21
0
  geometry_msgs::Pose toGeometryMsgsPose(const vpHomogeneousMatrix& mat){
    geometry_msgs::Pose pose;

	vpThetaUVector tu(mat);
	vpColVector u;
	double theta;
	tu.extract(theta, u);

	theta *= 0.5;

	double sinTheta_2 = sin(theta);

	pose.orientation.x = u[0] * sinTheta_2;
	pose.orientation.y = u[1] * sinTheta_2;
	pose.orientation.z = u[2] * sinTheta_2;
	pose.orientation.w = cos(theta);

	pose.position.x = mat[0][3];
	pose.position.y = mat[1][3];
	pose.position.z = mat[2][3];

	return pose;
  }
Esempio n. 22
0
int		main(int argc, char **argv)
{
    (void)argc;
    (void)argv;

    Bureaucrat		tu("yolo", 8);

    bool tutu = true;
    while (tutu)
    {
        try
        {
            tu.incrementGrade();
            std::cout	<< tu << std::endl;
        }
        catch (std::exception & e)
        {
            tutu = false;
            std::cout << "Exception " << e.what() << '\n';
        }
    }
    while (!tutu)
    {
        try
        {
            tu.decrementGrade();
            std::cout	<< tu << std::endl;
        }
        catch (std::exception & e)
        {
            tutu = true;
            std::cout << "Exception " << e.what() << '\n';
        }
    }
    return (0);
}
Esempio n. 23
0
bool JsonIRReader::ReadDump(const std::string &dump_file) {
  Json::Value tu_json;
  Json::Reader reader;
  std::ifstream input(dump_file);

  if (!reader.parse(input, tu_json, /* collectComments */ false)) {
    llvm::errs() << "Failed to parse JSON: "
                 << reader.getFormattedErrorMessages() << "\n";
    return false;
  }
  bool ok = true;
  JsonObjectRef tu(tu_json, ok);
  if (!ok) {
    llvm::errs() << "Translation unit is not an object\n";
    return false;
  }

  ReadFunctions(tu);
  ReadGlobalVariables(tu);
  ReadEnumTypes(tu);
  ReadRecordTypes(tu);
  ReadFunctionTypes(tu);
  ReadArrayTypes(tu);
  ReadPointerTypes(tu);
  ReadQualifiedTypes(tu);
  ReadBuiltinTypes(tu);
  ReadLvalueReferenceTypes(tu);
  ReadRvalueReferenceTypes(tu);
  ReadElfFunctions(tu);
  ReadElfObjects(tu);
  if (!ok) {
    llvm::errs() << "Failed to convert JSON to IR\n";
    return false;
  }
  return true;
}
int
main()
{
  try
    {
      vpImage<unsigned char> I ;

      vp1394TwoGrabber g;
      g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
      g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
      g.open(I) ;

      g.acquire(I) ;


      vpDisplayX display(I,100,100,"testDisplayX.cpp ") ;
      vpTRACE(" ") ;

      vpDisplay::display(I) ;
      vpDisplay::flush(I);

      vpServo task ;

      vpRobotAfma6 robot ;
      //robot.move("zero.pos") ;

      vpCameraParameters cam ;
      // Update camera parameters
      robot.getCameraParameters (cam, I);


      std::cout << std::endl ;
      std::cout << "-------------------------------------------------------" << std::endl ;
      std::cout << " Test program for vpServo "  <<std::endl ;
      std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
      std::cout << " Simulation " << std::endl ;
      std::cout << " task : servo a line " << std::endl ;
      std::cout << "-------------------------------------------------------" << std::endl ;
      std::cout << std::endl ;

      int nbline =4 ;
      int nbpoint =4 ;


      vpTRACE("sets the desired position of the visual feature ") ;
      vpPoint pointd[nbpoint];  //position of the fours corners
      vpPoint pointcd;  //position of the center of the square
      vpFeaturePoint pd;

      double L=0.05 ;
      pointd[0].setWorldCoordinates(L,-L, 0 ) ;
      pointd[1].setWorldCoordinates(L,L, 0 ) ;
      pointd[2].setWorldCoordinates(-L,L, 0 ) ;
      pointd[3].setWorldCoordinates(-L,-L, 0 ) ;

      //The coordinates in the object frame of the point used as a feature ie the center of the square
      pointcd.setWorldCoordinates(0, 0, 0 ) ;

      //The desired homogeneous matrix.
      vpHomogeneousMatrix cMod(0,0,0.4,0,0,vpMath::rad(10));

      pointd[0].project(cMod);
      pointd[1].project(cMod);
      pointd[2].project(cMod);
      pointd[3].project(cMod);

      pointcd.project(cMod);

      vpFeatureBuilder::create(pd,pointcd);

      vpTRACE("Initialization of the tracking") ;
      vpMeLine line[nbline] ;
      vpPoint point[nbpoint];
      int i ;

      vpMe me ;
      me.setRange(10) ;
      me.setPointsToTrack(100) ;
      me.setThreshold(50000) ;
      me.setSampleStep(10);

      //Initialize the tracking. Define the four lines to track
      for (i=0 ; i < nbline ; i++)
      {
        line[i].setMe(&me) ;

        line[i].initTracking(I) ;
        line[i].track(I) ;
      }

      // Compute the position of the four corners. The goal is to
      // compute the pose
      vpImagePoint ip;
      for (i=0 ; i < nbline ; i++)
      {
        double x=0, y=0;

        if (!vpMeLine::intersection (line[i%nbline], line[(i+1)%nbline], ip))
        {
          exit(-1);
        }

        vpPixelMeterConversion::convertPoint(cam, ip, x, y)  ;

        point[i].set_x(x) ;
        point[i].set_y(y) ;
      }

      //Compute the pose cMo
      vpPose pose ;
      pose.clearPoint() ;
      vpHomogeneousMatrix cMo ;

      point[0].setWorldCoordinates(L,-L, 0 ) ;
      point[1].setWorldCoordinates(L,L, 0 ) ;
      point[2].setWorldCoordinates(-L,L, 0 ) ;
      point[3].setWorldCoordinates(-L,-L, 0 ) ;

      for (i=0 ; i < nbline ; i++)
      {
        pose.addPoint(point[i]) ; // and added to the pose computation point list
      }

      pose.computePose(vpPose::LAGRANGE, cMo) ;
      pose.computePose(vpPose::VIRTUAL_VS, cMo);


      vpTRACE("sets the current position of the visual feature ") ;

      //The first features are the position in the camera frame x and y of the square center
      vpPoint pointc; //The current position of the center of the square
      double xc = (point[0].get_x()+point[2].get_x())/2;
      double yc = (point[0].get_y()+point[2].get_y())/2;
      pointc.set_x(xc);
      pointc.set_y(yc);
      vpFeaturePoint p;
      pointc.project(cMo);
      vpFeatureBuilder::create(p,pointc);

      //The second feature is the depth of the current square center relative to the depth of the desired square center.
      vpFeatureDepth logZ;
      logZ.buildFrom(pointc.get_x(), pointc.get_y(), pointc.get_Z(), log(pointc.get_Z()/pointcd.get_Z()));

      //The last three features are the rotations thetau between the current pose and the desired pose.
      vpHomogeneousMatrix cdMc ;
      cdMc = cMod*cMo.inverse() ;
      vpFeatureThetaU tu(vpFeatureThetaU::cdRc);
      tu.buildFrom(cdMc) ;

      vpTRACE("define the task") ;
      vpTRACE("\t we want an eye-in-hand control law") ;
      vpTRACE("\t robot is controlled in the camera frame") ;
      task.setServo(vpServo::EYEINHAND_CAMERA) ;
      task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE);

      vpTRACE("\t we want to see a point on a point..") ;
      std::cout << std::endl ;
      task.addFeature(p,pd) ;
      task.addFeature(logZ) ;
      task.addFeature(tu);


      vpTRACE("\t set the gain") ;
      task.setLambda(0.2) ;


      vpTRACE("Display task information " ) ;
      task.print() ;

      robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;

      int iter=0 ;
      vpTRACE("\t loop") ;
      vpColVector v ;
      vpImage<vpRGBa> Ic ;
      double lambda_av =0.05;
      double alpha = 0.05 ;
      double beta =3 ;
      double erreur= 1;

      while(1)
	{
	  std::cout << "---------------------------------------------" << iter <<std::endl ;

	  try {
	    g.acquire(I) ;
	    vpDisplay::display(I) ;

	    pose.clearPoint() ;

	    //Track the lines and find the current position of the corners
	    for (i=0 ; i < nbline ; i++)
	    {
		      line[i].track(I) ;

		      line[i].display(I,vpColor::green);

	              double x=0, y=0;

	              if (!vpMeLine::intersection (line[i%nbline], line[(i+1)%nbline], ip))
	              {
	                 exit(-1);
	              }

		      vpPixelMeterConversion::convertPoint(cam, ip, x, y)  ;

	              point[i].set_x(x);
	              point[i].set_y(y);

		      pose.addPoint(point[i]) ;
	    }

	    //Compute the pose
	    pose.computePose(vpPose::VIRTUAL_VS, cMo) ;

	    //Update the two first features x and y (position of the square center)
            xc = (point[0].get_x()+point[2].get_x())/2;
            yc = (point[0].get_y()+point[2].get_y())/2;
            pointc.set_x(xc);
            pointc.set_y(yc);
            pointc.project(cMo);
            vpFeatureBuilder::create(p,pointc);
	    //Print the current and the desired position of the center of the square
	    //Print the desired position of the four corners
	    p.display(cam, I,  vpColor::green) ;
	    pd.display(cam, I,  vpColor::red) ;
	    for (i = 0; i < nbpoint; i++) pointd[i].display(I, cam, vpColor::red);

	    //Update the second feature
	    logZ.buildFrom(pointc.get_x(), pointc.get_y(), pointc.get_Z(), log(pointc.get_Z()/pointcd.get_Z()));

	    //Update the last three features
	    cdMc = cMod*cMo.inverse() ;
	    tu.buildFrom(cdMc) ;

	    //Adaptive gain
	    double gain ;
	    {
	      if (alpha == 0) gain = lambda_av ;
	      else
		    {
		      gain = alpha * exp (-beta * task.error.sumSquare() ) +  lambda_av ;
		    }
	    }

	    task.setLambda(gain) ;

	    v = task.computeControlLaw() ;

            vpDisplay::flush(I) ;
	    std::cout << v.sumSquare() <<std::endl  ;
	    if (iter==0)  vpDisplay::getClick(I) ;
	    if (v.sumSquare() > 0.5)
	      {
		      v =0 ;
		      robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
		      robot.stopMotion() ;
		      vpDisplay::getClick(I) ;
	      }

	    robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;

	  }
	  catch(...)
	    {
	      v =0 ;
	      robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
	      robot.stopMotion() ;
	      exit(1) ;
	    }

	  vpTRACE("\t\t || s - s* || = %f ", task.error.sumSquare()) ;
	  erreur = task.error.sumSquare();
	  iter++;
	}

      vpTRACE("Display task information " ) ;
      task.print() ;
      task.kill();
    }
  catch (...)
    {
      vpERROR_TRACE(" Test failed") ;
      return 0;
    }
}
Esempio n. 25
0
/*****************************************************************************
 * do the actual mangling to 8.3 format
 * the buffer must be able to hold 13 characters (including the null)
 *****************************************************************************
 */
void mangle_name_83(char *s)
{
    int		crc16, i;
    char	*p, *q;
    char	extension[4];
    char	base[9];
    int		baselen = 0;
    int		extlen = 0;

    extension[0] = 0;
    base[0] = 0;

    /*
     * First, convert some common Unix extensions to extensions of 3
     * characters. If none fits, don't change anything now.
     */
    if (strcmp(q = s + strlen(s) - strlen(".tar.gz"), ".tar.gz") == 0) {
	*q = '\0';
	q = (char *)"tgz";
    } else if (strcmp(q = s + strlen(s) - strlen(".tar.z"), ".tar.z") == 0) {
	*q = '\0';
	q = (char *)"tgz";
    } else if (strcmp(q = s + strlen(s) - strlen(".tar.Z"), ".tar.Z") == 0) {
	*q = '\0';
	q = (char *)"taz";
    } else if (strcmp(q = s + strlen(s) - strlen(".html"), ".html") == 0) {
	*q = '\0';
	q = (char *)"htm";
    } else if (strcmp(q = s + strlen(s) - strlen(".shtml"), ".shtml") == 0) {
	*q = '\0';
	q = (char *)"stm";
    } else if (strcmp(q = s + strlen(s) - strlen(".conf"), ".conf") == 0) {
	*q = '\0';
	q = (char *)"cnf";
    } else if (strcmp(q = s + strlen(s) - strlen(".mpeg"), ".mpeg") == 0) {
	*q = '\0';
	q = (char *)"mpg";
    } else if (strcmp(q = s + strlen(s) - strlen(".smil"), ".smil") == 0) {
	*q = '\0';
	q = (char *)"smi";
    } else if (strcmp(q = s + strlen(s) - strlen(".perl"), ".perl") == 0) {
	*q = '\0';
	q = (char *)"pl";
    } else if (strcmp(q = s + strlen(s) - strlen(".jpeg"), ".jpeg") == 0) {
	*q = '\0';
	q = (char *)"jpg";
    } else if (strcmp(q = s + strlen(s) - strlen(".tiff"), ".tiff") == 0) {
	*q = '\0';
	q = (char *)"tif";
    } else {
	q = NULL;
    }
    
    if (q) {
	/*
	 * Extension is modified, apply changes
	 */
	p = s + strlen(s);
	*p++ = '.';
	for (i = 0; i < strlen(q); i++)
	    *p++ = q[i];
	*p++ = '\0';
    }

    /*
     * Now start name mangling
     */
    p = strrchr(s,'.');  
    if (p && (strlen(p+1) < (size_t)4)) {
	int	all_normal = (!strhaslower(p+1)); /* XXXXXXXXX */

	if (all_normal && p[1] != 0) {
	    *p = 0;
	    crc16 = crc16xmodem(s, strlen(s));
	    *p = '.';
	} else {
	    crc16 = crc16xmodem(s, strlen(s));
	}
    } else {
	crc16 = crc16xmodem(s, strlen(s));
    }

    tu(s);

    if (p) {
	if (p == s)
	    safe_strcpy(extension, "___", 3);
	else {
	    *p++ = 0;
	    while (*p && extlen < 3) {
		if (*p != '.' )
		    extension[extlen++] = p[0];
		p++;
	    }
	    extension[extlen] = 0;
	}
    }

    p = s;

    /*
     * Changed to baselen 4, original this is 5.
     * 24-11-2002 MB.
     */
    while (*p && baselen < 4) {
	if (*p != '.' )
	    base[baselen++] = p[0];
	p++;
    }
    base[baselen] = 0;

    if (crc16 > (MANGLE_BASE * MANGLE_BASE * MANGLE_BASE))
	Syslog('!', "WARNING: mangle_name_83() crc16 overflow");
    crc16 = crc16 % (MANGLE_BASE * MANGLE_BASE * MANGLE_BASE);
    snprintf(s, 9, "%s%c%c%c%c", base, magic_char, 
	    mangle(crc16 / (MANGLE_BASE * MANGLE_BASE)), mangle(crc16 / MANGLE_BASE), mangle(crc16));
    if ( *extension ) {
	(void)strcat(s, ".");
	(void)strcat(s, extension);
    }
}
Esempio n. 26
0
void RegularBattleScene::updateToolTip(int spot)
{
    QString tooltip;

    QString stats[7] = {
        tu(StatInfo::Stat(1, data()->gen())),
        tu(StatInfo::Stat(2, data()->gen())),
        tu(StatInfo::Stat(3, data()->gen())),
        tu(StatInfo::Stat(4, data()->gen())),
        tu(StatInfo::Stat(5, data()->gen())),
        tu(StatInfo::Stat(6, data()->gen())),
        tu(StatInfo::Stat(7, data()->gen()))
    };

    /* Putting dots after stat names so the ":" is always at the same place */
    int max = 0;
    for (int i = 0; i < 7; i++) {
        max = std::max(max, stats[i].length());
    }
    for (int i = 0; i < 7; i++) {
        stats[i] = stats[i].leftJustified(max, '.', false);
    }

    const auto &poke = data()->poke(spot);

    tooltip += nick(spot) + "\n";
    tooltip += TypeInfo::Name(PokemonInfo::Type1(poke.num(), data()->gen()));
    int type2 = PokemonInfo::Type2(poke.num(), data()->gen());
    if (type2 != Pokemon::Curse) {
        tooltip += " " + TypeInfo::Name(PokemonInfo::Type2(poke.num(), data()->gen()));
    }
    tooltip += "\n";

    for (int i = 0; i < 5; i++) {
        // Gen 1 only has Special, and we treat SAtk as Special hiding SDef.
        if (data()->gen().num == 1) {
            switch (i) {
            case 3: continue;
            default: tooltip += "\n" + stats[i] + " ";
            }
        } else {
            tooltip += "\n" + stats[i] + " ";
        }
        int boost = data()->fieldPoke(spot).statBoost(i+1);
        int stat = data()->fieldPoke(spot).stat(i+1);

        if (stat == 0) {
            if (boost >= 0) {
                tooltip += QString("+%1").arg(boost);
            } else if (boost < 0) {
                tooltip += QString("%1").arg(boost);
            }
        } else {
            if (stat == -1) {
                tooltip += "???";
            } else {
                tooltip += QString::number(stat);
            }
            if (boost >= 0) {
                tooltip += QString("(+%1)").arg(boost);
            } else if (boost < 0) {
                tooltip += QString("(%1)").arg(boost);
            }
        }
    }
    for (int i = 5; i < 7; i++) {
        int boost = data()->fieldPoke(spot).statBoost(i+1);
        if (boost) {
            tooltip += "\n" + stats[i] + " ";

            if (boost > 0) {
                tooltip += QString("+%1").arg(boost);
            } else if (boost < 0) {
                tooltip += QString("%1").arg(boost);
            }
        }
    }

    tooltip += "\n";

    const auto &zone = *data()->field().zone(data()->player(spot));

    if (zone.spikesLevel() > 0) {
        tooltip += "\n" + tr("Spikes level %1").arg(zone.spikesLevel());
    }

    if (zone.tspikesLevel() > 0) {
        tooltip += "\n" + tr("Toxic Spikes level %1").arg(zone.tspikesLevel());
    }

    if (zone.stealthRocks()) {
        tooltip += "\n" + tr("Stealth Rock");
    }

    if (data()->field().weather() != Weather::NormalWeather) {
        tooltip += "\n" + tr("Weather: %1").arg(TypeInfo::weatherName(data()->field().weather()));
    }

    gui.zone->tooltips[spot] = tooltip;
}
BOOL SkeletalTracker::SkeletalTrackingProc()
{
    //Util tu("算法耗时");
    const int frameWidth = 320;
    const int frameHeight = 240;
    const USHORT * pDepthFrame = m_pNuiImageDevice->GetDepthMap();;
    const USHORT * pBufferRun = pDepthFrame;
    const USHORT * pBufferEnd = pBufferRun + (320 * 240);

    // STEP2: 检查微软深度图有无PlayerIndex, 最终应该是BGR模块检测
    BOOL bHavePlayer = FALSE;
    pBufferRun = pDepthFrame;
    while ( pBufferRun < pBufferEnd )
    {
        USHORT depth     = *pBufferRun;
        if(depth & 0x0007)
        {
            bHavePlayer = TRUE;
            break;
        }
        ++pBufferRun;
    }
    // 耗时估计:
    // STEP1之前: 12% ~ 13%
    // STEP3之前: 12% ~ 13%
    // STEP4之前: 23% ~ 25%
    // STEP5之前: 66% ~ 70%, STEP4消耗资源太多
    if (bHavePlayer)
    {
        m_pPipelineSourceStage->RunFrame();
        m_pBGRStage->RunFrame();
        m_pExemplarStage->RunFrame();
        m_pCentroidsStage->RunFrame();
        m_pModelFittingStage->RunFrame();
        m_pPipelineOutputStage->RunFrame();
#if 0
        {
            // STEP3: 人体部位分类
            MEASURE_TIME(Util tu("人体部位分类"));
            m_pBPC.ExemplarStage_RunFrame(1.0f, pDepthFrame, m_pLabelRaw, 1, LABEL_DATA);
        }

        {
            // STEP4: 提取部位中心
            MEASURE_TIME(Util tu("提取部位中心"));
            m_pBPC.CentroidsStage_RunFrame(pDepthFrame, m_pLabelRaw, m_centroid);
        }

        {
            // STEP5: 提取人体骨骼
            MEASURE_TIME(Util tu("提取人体骨骼"));
            m_pBPC.ModelFittingStage_RunFrame(m_centroid, m_joints);
        }

        {
            // STEP6: 平滑人体骨骼
            MEASURE_TIME(Util tu("平滑人体骨骼"));
            NUI_TRANSFORM_SMOOTH_PARAMETERS SmoothingParams;
            SmoothingParams.fSmoothing          = 0.5f;
            SmoothingParams.fCorrection         = 0.5f;
            SmoothingParams.fPrediction         = 0.5f;
            SmoothingParams.fJitterRadius       = 0.05f;
            SmoothingParams.fMaxDeviationRadius = 0.04f;
            if (S_OK != m_pBPC.TransformSmooth(m_joints, &SmoothingParams))
            {
                assert(0);
            }
        }
#endif
    }

    return TRUE;
}
Esempio n. 28
0
void RegularBattleScene::updateToolTip(int spot)
{
    QString tooltip;

    QString stats[8] = {
        tu(StatInfo::Stat(0, data()->gen())),
        tu(StatInfo::Stat(1, data()->gen())),
        tu(StatInfo::Stat(2, data()->gen())),
        tu(StatInfo::Stat(3, data()->gen())),
        tu(StatInfo::Stat(4, data()->gen())),
        tu(StatInfo::Stat(5, data()->gen())),
        tu(StatInfo::Stat(6, data()->gen())),
        tu(StatInfo::Stat(7, data()->gen()))
    };

    //Prevents a headache if it includes HP as a null value
    QString range[6] = { 0 };
    QString statval[6] = { 0 };
    for (int i = 1; i < 6; i++) {
        int min = data()->fieldPoke(spot).minStat(i);
        int max = data()->fieldPoke(spot).maxStat(i);
        range[i] = QString("%2-%3").arg(min).arg(max);

        int stat = data()->fieldPoke(spot).stat(i);
        if (stat == -1) {
            statval[i] = "???";
        } else {
            statval[i] = QString::number(stat);
        }
    }

    /* Aligning various things on the tool tip */
    int max = 0, max2 = 0, max3 = 0;
    for (int i = 0; i < 8; i++) {
        max = std::max(max, stats[i].length());
        if (i > 0 && i < 6) {
            max2 = std::max(max2, range[i].length());
            max3 = std::max(max3, statval[i].length());
        }
    }
    for (int i = 0; i < 8; i++) {
        stats[i] = stats[i].leftJustified(max, '.', false);
        if (i > 0 && i < 6) {
            range[i] = range[i].leftJustified(max2, ' ', false);
            statval[i] = statval[i].rightJustified(max3, ' ', false);
        }
    }

    const auto &poke = data()->poke(spot).num();

    tooltip += nick(spot) + "\n";
    tooltip += TypeInfo::Name(PokemonInfo::Type1(poke, data()->gen()));
    int type2 = PokemonInfo::Type2(poke, data()->gen());
    if (type2 != Pokemon::Curse) {
        tooltip += " " + TypeInfo::Name(type2);
    }
    tooltip += "\n";

    for (int i = 0; i < 6; i++) {
        // Gen 1 only has Special, and we treat SAtk as Special hiding SDef.
        if (data()->gen().num == 1) {
            switch (i) {
            case 4:
                continue;
            default:
                tooltip += "\n" + stats[i] + " ";
            }
        } else {
            tooltip += "\n" + stats[i] + " ";
        }
        int boost = data()->fieldPoke(spot).statBoost(i);
        int stat = data()->fieldPoke(spot).stat(i);

        if (stat == 0 || i == 0) {
            int min = data()->fieldPoke(spot).minStat(i);
            int max = data()->fieldPoke(spot).maxStat(i);
            if (i == 0) {
                if (data()->isPlayer(spot)) {
                    tooltip += QString("%1/%2").arg(data()->poke(spot).life()).arg(data()->poke(spot).totalLife());
                } else {
                    tooltip += QString("%3/%1-%4/%2").arg(min).arg(max).arg(gui.bars[spot]->value()*min/100).arg(gui.bars[spot]->value()*max/100);
                }
            } else {
                if (boost >= 0) {
                    tooltip += range[i] + (boost > 0 ? QString(" (+%1)").arg(boost) : "");
                } else if (boost < 0) {
                    tooltip += range[i] + QString(" (%1)").arg(boost);
                }
            }
        } else {
            if (boost >= 0) {
                tooltip += statval[i] + (boost > 0 ? QString(" (+%1)").arg(boost) : "");
            } else if (boost < 0) {
                tooltip += statval[i] + QString(" (%1)").arg(boost);
            }
        }
    }
    for (int i = 6; i < 8; i++) {
        int boost = data()->fieldPoke(spot).statBoost(i);
        if (boost) {
            tooltip += "\n" + stats[i] + " ";

            if (boost > 0) {
                tooltip += QString("+%1").arg(boost);
            } else if (boost < 0) {
                tooltip += QString("%1").arg(boost);
            }
        }
    }

    tooltip += "\n";

    const auto &zone = *data()->field().zone(data()->player(spot));

    if (zone.spikesLevel() > 0) {
        tooltip += "\n" + tr("Spikes level %1").arg(zone.spikesLevel());
    }

    if (zone.tspikesLevel() > 0) {
        tooltip += "\n" + tr("Toxic Spikes level %1").arg(zone.tspikesLevel());
    }

    if (zone.stealthRocks()) {
        tooltip += "\n" + tr("Stealth Rock");
    }

    if (zone.stickyWeb()) {
        tooltip += "\n" + tr("Sticky Web");
    }

    if (data()->field().weather() != Weather::NormalWeather) {
        tooltip += "\n" + tr("Weather: %1").arg(TypeInfo::weatherName(data()->field().weather()));
    }

    if (!data()->isPlayer(spot) && data()->poke(spot).move(0)->num() != Move::NoMove) {
        tooltip += "\n\nKnown Moves:\n";
        for (int i = 0; i < 4; i++) {
            int move = data()->poke(spot).move(i)->num();
            if (move != Move::NoMove) {
                tooltip += MoveInfo::Name(move) + QString(" %1/%2").arg(data()->poke(spot).move(i)->PP()).arg(data()->poke(spot).move(i)->totalPP()) + "\n";
            } else {
                tooltip += "??? \n";
            }
        }
    }
    gui.zone->tooltips[spot] = tooltip;
}
Esempio n. 29
0
File: menu.c Progetto: ftnapps/FTNd
void menu()
{
    FILE    *pMenuFile;
    int	    Key, IsANSI;
    char    temp[81], *Input, *sMenuPathFileName, buf[81];

    Input = calloc(PATH_MAX, sizeof(char));
    sMenuPathFileName = calloc(PATH_MAX, sizeof(char));
    Syslog('+', "Starting menu loop");

    /* 
     * Loop forever, this is what a BBS should do until a user logs out.
     */
    while (TRUE) {

	WhosDoingWhat(BROWSING, NULL);

	/*
	 * Open menufile, first users language menu, if it fails
	 * try to open the default menu.
	 */
	snprintf(sMenuPathFileName, PATH_MAX, "%s/share/int/menus/%s/%s", getenv("FTND_ROOT"), lang.lc, Menus[MenuLevel]);
	if ((pMenuFile = fopen(sMenuPathFileName, "r")) == NULL) {
	    snprintf(sMenuPathFileName, PATH_MAX, "%s/share/int/menus/%s/%s", getenv("FTND_ROOT"), CFG.deflang, Menus[MenuLevel]);
	    pMenuFile = fopen(sMenuPathFileName,"r");
	    if (pMenuFile != NULL)
		Syslog('b', "Menu %s (Default)", Menus[MenuLevel]);
	} else {
	    Syslog('b', "Menu %s (%s)", Menus[MenuLevel], lang.Name);
	}

	if (pMenuFile == NULL) {
	    clear();
	    WriteError("Can't open menu file: %s", sMenuPathFileName);
	    MenuError++;

	    /*
	     * Is this the last attempt to open the default menu?
	     */
	    if (MenuError == 10) {
		WriteError("FATAL ERROR: Too many menu errors");
		snprintf(temp, 81, "Too many menu errors, notifying Sysop\r\n\r\n");
		PUTSTR(temp);
		sleep(3);
		die(FTNERR_CONFIG_ERROR);
	    }

	    /*
	     * Switch back to the default menu
	     */
	    MenuLevel = 0;
	    strcpy(Menus[0], CFG.default_menu);
	} else {
	    /*
	     * Display Menu Text Fields and Perform all autoexec menus in order of menu file.
	     * First check if there are any ANSI menus, if not, send a clearscreen first.
	     */
	    IsANSI = FALSE;
	    while (fread(&menus, sizeof(menus), 1, pMenuFile) == 1) {
		if ( Le_Access(exitinfo.Security, menus.MenuSecurity) && (UserAge >= le_int(menus.Age))){
		    if ((le_int(menus.MenuType) == 5) || (le_int(menus.MenuType) == 19) || (le_int(menus.MenuType) == 20))
			IsANSI = TRUE;
		}
	    }
	    fseek(pMenuFile, 0, SEEK_SET);
	    if (! IsANSI)
		clear();

	    while (fread(&menus, sizeof(menus), 1, pMenuFile) == 1) {
		if ( Le_Access(exitinfo.Security, menus.MenuSecurity) && (UserAge >= le_int(menus.Age))){
		    if (menus.AutoExec) {
			DoMenu( le_int(menus.MenuType) );
		    }
		    DisplayMenu( ); 
		}
	    }

	    /*
	     * Check if the BBS closed down for Zone Mail Hour or
	     * system shutdown. If so, we run the Goodbye show.
	     */
	    if (CheckStatus() == FALSE) {
		fclose(pMenuFile);
		Syslog('+', "Kicking user out, the BBS is closed.");
		sleep(3);
		Good_Bye(FTNERR_OK);
	    }

	    /*
	     * Check the upsdown semafore
	     */
	    if (IsSema((char *)"upsdown")) {
		fclose(pMenuFile);
		Syslog('+', "Kicking user out, upsdown semafore detected");
		snprintf(temp, 81, "System power failure, closing the bbs");
		PUTSTR(temp);
		Enter(2);
		sleep(3);
		Good_Bye(FTNERR_OK);
	    }

	    /*
	     * Check if SysOp wants to chat to user everytime user gets prompt.
	     */
	    if (CFG.iChatPromptChk) {
		snprintf(buf, 81, "CISC:1,%d", mypid);
		if (socket_send(buf) == 0) {
		    strcpy(buf, socket_receive());
		    if (strcmp(buf, "100:1,1;") == 0) {
			Syslog('+', "Forced sysop/user chat");
			Chat(exitinfo.Name, (char *)"#sysop");
			continue;
		    }
		}
	    }

	    /*
	     * Check users timeleft
	     */
	    TimeCheck();
	    alarm_on();

	    if (exitinfo.HotKeys) {
		Key = Readkey();
		snprintf(Input, 81, "%c", Key);
		Enter(1);
	    } else {
		colour(CFG.InputColourF, CFG.InputColourB);
		GetstrC(Input, 80);
	    }

	    if ((strcmp(Input, "")) != 0) {

		fseek(pMenuFile, 0, SEEK_SET);

		while (fread(&menus, sizeof(menus), 1, pMenuFile) == 1) {
		 
		    if ((strcmp(tu(Input), menus.MenuKey)) == 0) {
			if ((Le_Access(exitinfo.Security, menus.MenuSecurity)) && (UserAge >= le_int(menus.Age))) {
			    Syslog('+', "Menu[%d] %d=(%s), Opt: '%s'", MenuLevel, le_int(menus.MenuType), 
					menus.TypeDesc, menus.OptionalData);
			    if (le_int(menus.MenuType) == 13) {
				/*
				 *  Terminate call, cleanup here
				 */
				free(Input);
				free(sMenuPathFileName);
				fclose(pMenuFile);
			    }
			    DoMenu(le_int(menus.MenuType));
			    break;
			}
		    }
		}
	    }
	    fclose(pMenuFile);

	} /* If menu open */
    } /* while true */
}
int
main(int argc, const char ** argv)
{
  // Read the command line options
  if (getOptions(argc, argv) == false) {
    exit (-1);
  }

  // Log file creation in /tmp/$USERNAME/log.dat
  // This file contains by line:
  // - the 6 computed camera velocities (m/s, rad/s) to achieve the task
  // - the 6 values of s - s*
  std::string username;
  // Get the user login name
  vpIoTools::getUserName(username);

  // Create a log filename to save velocities...
  std::string logdirname;
#ifdef WIN32
  logdirname ="C:/temp/" + username;
#else
  logdirname ="/tmp/" + username;
#endif
  // Test if the output path exist. If no try to create it
  if (vpIoTools::checkDirectory(logdirname) == false) {
    try {
      // Create the dirname
      vpIoTools::makeDirectory(logdirname);
    }
    catch (...) {
      std::cerr << std::endl
		<< "ERROR:" << std::endl;
      std::cerr << "  Cannot create " << logdirname << std::endl;
      exit(-1);
    }
  }
  std::string logfilename;
  logfilename = logdirname + "/log.dat";

  // Open the log file name
  std::ofstream flog(logfilename.c_str());

  vpServo task ;
  vpRobotCamera robot ;

  std::cout << std::endl ;
  std::cout << "-------------------------------------------------------" << std::endl ;
  std::cout << " Test program for vpServo "  <<std::endl ;
  std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
  std::cout << " Simulation " << std::endl ;
  std::cout << " task :  3D visual servoing " << std::endl ;
  std::cout << "-------------------------------------------------------" << std::endl ;
  std::cout << std::endl ;


  // Sets the initial camera location
  vpPoseVector c_r_o(// Translation tx,ty,tz
		     0.1, 0.2, 2, 
		     // ThetaU rotation 
		     vpMath::rad(20), vpMath::rad(10),  vpMath::rad(50) ) ;
  
  // From the camera pose build the corresponding homogeneous matrix
  vpHomogeneousMatrix cMo(c_r_o) ;

  // Set the robot initial position
  robot.setPosition(cMo) ;

  // Sets the desired camera location
  vpPoseVector cd_r_o(// Translation tx,ty,tz
		      0, 0, 1, 
		      // ThetaU rotation 
		      vpMath::rad(0),vpMath::rad(0),vpMath::rad(0)) ; 
  // From the camera desired pose build the corresponding homogeneous matrix
  vpHomogeneousMatrix cdMo(cd_r_o) ;

  // Compute the homogeneous transformation from the desired camera position to the initial one
  vpHomogeneousMatrix cdMc ;
  cdMc = cdMo*cMo.inverse() ;

  // Build the current visual features s = (c*tc, thetaU_c*Rc)^T
  vpFeatureTranslation t(vpFeatureTranslation::cdMc) ;
  vpFeatureThetaU tu(vpFeatureThetaU::cdRc); // current feature
  t.buildFrom(cdMc) ;
  tu.buildFrom(cdMc) ;

  // Sets the desired rotation (always zero !)  since s is the
  // rotation that the camera has to achieve. Here s* = (0, 0)^T
  vpFeatureTranslation td(vpFeatureTranslation::cdMc) ;
  vpFeatureThetaU tud(vpFeatureThetaU::cdRc); // desired feature

  // Define the task
  // - we want an eye-in-hand control law
  // - the robot is controlled in the camera frame
  task.setServo(vpServo::EYEINHAND_CAMERA) ;
  // - we use here the interaction matrix computed with the 
  //   current features
  task.setInteractionMatrixType(vpServo::CURRENT);

  // Add the current and desired visual features
  task.addFeature(t,td) ;   // 3D translation
  task.addFeature(tu,tud) ; // 3D rotation

  // - set the constant gain to 1.0
  task.setLambda(1) ;

  // Display task information
  task.print() ;

  int iter=0 ;
  // Start the visual servoing loop. We stop the servo after 200 iterations
  while(iter++ < 200) {
    std::cout << "-----------------------------------" << iter <<std::endl ;
    vpColVector v ;

    // get the robot position
    robot.getPosition(cMo) ;

    // new displacement to achieve
    cdMc = cdMo*cMo.inverse() ;

    // Update the current visual features
    t.buildFrom(cdMc) ;
    tu.buildFrom(cdMc) ;

    // Compute the control law
    v = task.computeControlLaw() ;

    // Display task information
    if (iter==1) task.print() ;

    // Send the camera velocity to the controller
    robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
      
    // Retrieve the error 
    std::cout << task.error.sumSquare() <<std::endl ;
    
    // Save log
    flog << v.t() << " " << task.error.t() << std::endl;
  }
  // Display task information
  task.print() ;

  // Kill the task
  task.kill();

  // Close the log file
  flog.close();
}