Exemple #1
0
//{{{ site_failure
void site_failure(int sig) {
	if (_verbose) fprintf (stderr, "/* FAILURE OF A SITE SPOTTED */\n");
	int i=0;
	int sortie = 0;
	msg_t msg;
	time_t timeStart, timeCur;

		pthread_mutex_lock(&mut_check);
		if (check)
			check = 0;
		pthread_mutex_unlock(&mut_check);

	int flags = fcntl(this_site.sdRecv, F_GETFL);
	int flags2 = flags | O_NONBLOCK;
	fcntl(this_site.sdRecv, F_SETFL, flags2);

	// Sends ARE_YOU_ALIVE to other pred
	if (_verbose) fprintf (stdout, "Checking predecessor validity\n");
	for (i=1; i<TOLERANCE+1; i++) {
		if (!strcmp ("0.0.0.0", inet_ntoa(predec[i].sin_addr))) {
			if (_verbose) fprintf (stdout, "Bad ip address\n");
			continue;
		}
		type(msg) = ARE_YOU_ALIVE;
		char *tmp = inet_ntoa(predec[i].sin_addr);
		strncpy (ip(msg), tmp, IPLONG *sizeof(char));
		unsigned long int ipa = (unsigned long int) inet_addr(ip(msg));
		sendMessageWithAdd(msg);

		pthread_mutex_lock(&mut_check);
		if (check)
			check = 0;
		pthread_mutex_unlock(&mut_check);

		timeStart = time(&timeStart);
		timeCur = time(&timeCur);

		while(timeCur - timeStart < (2*TMESG)) {
			timeCur = time(&timeCur);
			memset (&msg, 0, SIZE);

			if(recvMessage(&msg, NULL) == -1) 
				continue;

			// Receive I_AM_ALIVE --> Connection
			if (_verbose) fprintf (stdout, "Received I_AM_ALIVE\n");
			if (type(msg) == I_AM_ALIVE && !isMe(ips(msg))) {
				handleIAmAlive(msg);
				type(msg) = CONNECTION;
				strncpy(ip(msg), ips(msg), IPLONG * sizeof(char));
				sendMessageWithAdd(msg);
				if (_verbose) fprintf (stdout, "Asking for connection\n");
				last = getNeighbour(ipa);
				if (_verbose) fprintf (stdout, "New last after receiving I_AM_ALIVE message : %d\n", last);
				state = IDLE;
				critSectionRequest();
				sortie++;
				break;
			}
			else if (type(msg) == REQUEST) {}
			else
				handleMessage(msg);
		}
	}

	// No pred is alive, broadcast SEARCH_PREV
	fcntl(this_site.sdRecv, F_SETFL, flags);

	if (_verbose) fprintf (stdout, "No alive predecessor found, looking for other sites\n");
	if (!sortie) {
		msg_t min;

		type(msg) = SEARCH_PREV;
		pos(msg) = position;
		if (_verbose) fprintf (stdout, "Broadcasting SEARCH_PREV.\n");
		broadcast(msg);

		fcntl(this_site.sdRecv, F_SETFL, flags2);

		timeStart = time(&timeStart);
		timeCur = time(&timeCur);

		// Wait for ACK_SEARCH_PREV
		while(timeCur - timeStart < (2*TMESG)) {
			timeCur = time(&timeCur);
			memset (&msg, 0, SIZE);

			if(recvMessage(&msg, NULL) == -1) 
				continue;

			pos(min) = -1;
			if (type(msg) == ACK_SEARCH_PREV) {
				if (pos(min) == -1)
					memcpy (&min, &msg, SIZE);
				else if (pos(min) < pos(msg))
					memcpy (&min, &msg, SIZE);
				if (_verbose) fprintf (stdout, "Got an answer.\n");
			}
			else if (type(msg) == REQUEST) {}
			else
				handleMessage(msg);
		}

		fcntl(this_site.sdRecv, F_SETFL, flags);

		// No answers, regenerate the TOKEN
		if (pos(min) == -1) {
			if (_verbose) fprintf (stdout, "No answers, regenerating TOKEN\n");
			tokenPresent = 1;
			last = next;
			takeCriticalSection();
			sortie ++;
		}

		// Got an answer, ask for connection
		if (!sortie) {
			if (_verbose) fprintf (stdout, "Asking for connection\n");
			type(msg) = CONNECTION;
			strncpy(ip(msg), ips(min), IPLONG * sizeof(char));
			last = getNeighbour((unsigned long int)inet_addr(ips(min)));
			sendMessageWithAdd(msg);
			state = IDLE;
			critSectionRequest();
		}
	}
}
Exemple #2
0
int main(int argc, char ** argv) {
  MPI_Init(&argc, &argv);

  QUESO::EnvOptionsValues envOptions;
  envOptions.m_numSubEnvironments = 1;
  envOptions.m_subDisplayFileName = "test_outputNoInputFile/display";
  envOptions.m_subDisplayAllowAll = 1;
  envOptions.m_displayVerbosity = 2;
  envOptions.m_syncVerbosity = 0;
  envOptions.m_seed = 0;

  QUESO::FullEnvironment env(MPI_COMM_WORLD, "", "", &envOptions);

  unsigned int dim = 2;
  QUESO::VectorSpace<> paramSpace(env, "param_", dim, NULL);

  QUESO::GslVector paramMins(paramSpace.zeroVector());
  QUESO::GslVector paramMaxs(paramSpace.zeroVector());

  double min_val = -10.0;
  double max_val = 10.0;
  paramMins.cwSet(min_val);
  paramMaxs.cwSet(max_val);

  QUESO::BoxSubset<> paramDomain("param_", paramSpace, paramMins, paramMaxs);

  QUESO::UniformVectorRV<> priorRv("prior_", paramDomain);

  Likelihood<> lhood("llhd_", paramDomain);

  QUESO::GenericVectorRV<> postRv("post_", paramSpace);

  QUESO::SipOptionsValues sipOptions;
  sipOptions.m_computeSolution = 1;
  sipOptions.m_dataOutputFileName = "test_outputNoInputFile/sipOutput";
  sipOptions.m_dataOutputAllowedSet.clear();
  sipOptions.m_dataOutputAllowedSet.insert(0);

  QUESO::StatisticalInverseProblem<> ip("", &sipOptions, priorRv, lhood,
      postRv);

  QUESO::GslVector paramInitials(paramSpace.zeroVector());
  paramInitials[0] = 0.0;
  paramInitials[1] = 0.0;

  QUESO::GslMatrix proposalCovMatrix(paramSpace.zeroVector());

  proposalCovMatrix(0, 0) = 1.0;
  proposalCovMatrix(0, 1) = 0.0;
  proposalCovMatrix(1, 0) = 0.0;
  proposalCovMatrix(1, 1) = 1.0;

  QUESO::MhOptionsValues mhOptions;
  mhOptions.m_dataOutputFileName = "test_outputNoInputFile/sipOutput";
  mhOptions.m_dataOutputAllowAll = 1;
  mhOptions.m_rawChainGenerateExtra = 0;
  mhOptions.m_rawChainDisplayPeriod = 50000;
  mhOptions.m_rawChainMeasureRunTimes = 1;
  mhOptions.m_rawChainDataOutputFileName = "test_outputNoInputFile/ip_raw_chain";
  mhOptions.m_rawChainDataOutputAllowAll = 1;
  mhOptions.m_displayCandidates = 0;
  mhOptions.m_tkUseLocalHessian = 0;
  mhOptions.m_tkUseNewtonComponent = 1;
  mhOptions.m_filteredChainGenerate = 0;
  mhOptions.m_rawChainSize = 1000;
  mhOptions.m_putOutOfBoundsInChain = false;
  mhOptions.m_drMaxNumExtraStages = 1;
  mhOptions.m_drScalesForExtraStages.resize(1);
  mhOptions.m_drScalesForExtraStages[0] = 5.0;
  mhOptions.m_amInitialNonAdaptInterval = 100;
  mhOptions.m_amAdaptInterval = 100;
  mhOptions.m_amEta = (double) 2.4 * 2.4 / dim;  // From Gelman 95
  mhOptions.m_amEpsilon = 1.e-8;
  mhOptions.m_doLogitTransform = false;

  ip.solveWithBayesMetropolisHastings(&mhOptions, paramInitials,
      &proposalCovMatrix);

  MPI_Finalize();

  return 0;
}
Exemple #3
0
void
ip(struct pktinfo *pktinfo)
{
    register const struct ip *ipv4 = NULL;
    register int ipv4_p = 0;                           /* next header */
    const struct in_addr *ipv4_src = NULL;
    const struct in_addr *ipv4_dst = NULL;
    struct addr_4 addr_ipv4;
    struct udphdr *udphdr = NULL;
    struct tcphdr *tcphdr = NULL;
    struct pktinfo n_hdr;
    u_int32_t src = 0;
    u_int32_t dst = 0;
    char *addr = NULL;
    const u_char *pkt = pktinfo->pkt;
    bpf_u_int32 len = pktinfo->len;

    ipv4 = (const struct ip *)pkt;

    /* next header type code */
    ipv4_p = ipv4->ip_p;

    /* IPv4 address */
    ipv4_src = &ipv4->ip_src;
    ipv4_dst = &ipv4->ip_dst;
    src = htonl(ipv4_src->s_addr);
    dst = htonl(ipv4_dst->s_addr);

    addr_ipv4.src = src;
    addr_ipv4.dst = dst;

    len -= sizeof(struct ip);
    pkt += sizeof(struct ip);

    n_hdr.len = len;
    n_hdr.pkt = pkt;

    addr_ipv4.p_tcp = NULL;
    addr_ipv4.p_udp = NULL;
    addr_ipv4.nxtflag = 0;

    if (ipv4_p == IPPROTO_TCP) {
        tcphdr = (struct tcphdr *)pkt;
        addr_ipv4.p_tcp = tcphdr;
        addr_ipv4.nxtflag = 1;
    }
    else if (ipv4_p == IPPROTO_UDP) {
        udphdr = (struct udphdr *)pkt;
        addr_ipv4.p_udp = udphdr;
        addr_ipv4.nxtflag = 2;
    }

    addr = v4_addr(&addr_ipv4, encap);

    encap ++;

    switch(ipv4_p) {
        case IPPROTO_IPV4 :	/* IP header */
            ip(&n_hdr);
            break;
        case IPPROTO_TCP :	/* tcp */
            if (rflag != 1)
                tcp(&n_hdr);
            break;
        case IPPROTO_UDP :	/* user datagram protocol */
            if (bflag != 1)
                udp(&n_hdr);
            break;
        case IPPROTO_IPV6 :	/* IP6 header */
            ipv6(&n_hdr);
            break;
        default:
    } /* end of switch */
} /* end of ip() */
Exemple #4
0
// if this function changes, please also check SupportsReShading, PreShade and PostShade
// end - ke/mjm - 03.16.00 - merge reshading code
// [attilas|29.5.2000] if this function changes, please also check EvalColorStdChannel
void Matte::Shade(ShadeContext& sc)
{
   Color c,t, shadowClr;
   float atten;
   float reflA;

   // > 6/15/02 - 11:12am --MQM-- 
   // for renderer prepass, we need to at least call
   // illuminate so that Light Tracer can cache shading
   if ( SHADECONTEXT_IS_PREPASS( sc ) )
   {
      Color    lightCol;
      Point3   L;
      float    NL = 0.0f, diffCoef = 0.0f;
      LightDesc *l = NULL;
      for ( int i = 0;  i < sc.nLights;  i++ )
      {
         l = sc.Light( i );
         if ( NULL != l )
            l->Illuminate( sc, sc.Normal(), lightCol, L, NL, diffCoef );
      }
      return;
   }

#ifdef _DEBUG
   IPoint2 sp = sc.ScreenCoord();
   if ( sp.x == stopX && sp.y == stopY )
      sp.x = stopX;
#endif

   if (gbufID) sc.SetGBufferID(gbufID);
   IllumParams ip( 1, &shadowIllumOutStr);
   IllumParams ipNS(0, NULL);
   ip.ClearInputs(); ip.ClearOutputs();
   ipNS.ClearInputs(); ipNS.ClearOutputs();
   ip.hasComponents = ipNS.hasComponents = HAS_MATTE_MTL; 
   
   // get background color & transparency
   if (!opaque) sc.Execute(0x1000); // DS: 6/24/99:use black bg when AA filtering (#192348)
   sc.GetBGColor(c, t, fogBG&&(!fogObjDepth) );
   if (!opaque) sc.Execute(0x1001); // DS: 6/24/99:use black bg when AA filtering (#192348)

   if (shadowBG && sc.shadow) {
      /********
      sc.shadow = 0;
      Color col0 = sc.DiffuseIllum();
      sc.shadow = 1;
      Color scol = sc.DiffuseIllum();
      float f = Intens(col0);
      atten = (f>0.0f)?Intens(scol)/f:1.0f;
      if (atten>1.0f) atten = 1.0f/atten;
      ********/
      atten = IllumShadow( sc, shadowClr );
      atten = amblev + (1.0f-amblev) * atten; 

      // key on black user-set shadow clr
      if( gUseLocalShadowClr || col.r != 0.0f || col.g != 0.0f || col.b != 0.0f )
            shadowClr = col;

      ipNS.finalC = ipNS.diffIllumOut = c;
      c *= atten;
      ip.diffIllumOut = c;

      shadowClr *= 1.0f - atten;
      ip.finalC = sc.out.c = c + shadowClr;

      ip.SetUserIllumOutput( 0, shadowClr );

      if (shadowAlpha)
         t *= atten;
   } else {
      sc.out.c  = 
      ipNS.finalC = ipNS.diffIllumOut = ip.finalC = ip.diffIllumOut = c;
   }


   // add the reflections
   if (reflmap && useReflMap) {
      AColor rcol;
      if (reflmap->HandleOwnViewPerturb()) {
         sc.TossCache(reflmap);
         rcol = reflmap->EvalColor(sc);
      } else 
         rcol = sc.EvalEnvironMap(reflmap, sc.ReflectVector());

      Color rc;
      rc = Color(rcol.r,rcol.g,rcol.b)*reflAmt;
      ip.reflIllumOut = ipNS.reflIllumOut = rc;

      if( additiveReflection ) {
         // additive compositing of reflections
         sc.out.c += rc; ip.finalC += rc; ipNS.finalC += rc;
      } else {
         reflA = Intens( rc );
         // over compositing of reflections 
         sc.out.c = (1.0f - reflA) * sc.out.c + rc; 
         ip.finalC = (1.0f - reflA) * ip.finalC + rc;
         ipNS.finalC = (1.0f - reflA) * ipNS.finalC + rc;
      }
   }

   // render elements
   Clamp( t );
   Clamp( reflA );
   ip.finalT = ipNS.finalT = sc.out.t = opaque ? black: additiveReflection? t : Color(reflA,reflA,reflA) ; 
   int nEles = sc.NRenderElements();
   if( nEles != 0 ){
      ip.pShader = ipNS.pShader = NULL; // no shader on matte mtl
      ip.stdIDToChannel = ipNS.stdIDToChannel = NULL;
      ip.pMtl = ipNS.pMtl = this;
      ip.finalAttenuation = ipNS.finalAttenuation = 1.0f;

      for( int i=0; i < nEles; ++i ){
         IRenderElement* pEle = sc.GetRenderElement(i);
         if( pEle->IsEnabled() ){
            MaxRenderElement* pMaxEle = (MaxRenderElement*)pEle->GetInterface( MaxRenderElement::IID );
            if( pEle->ShadowsApplied() )
               pMaxEle->PostIllum( sc, ip );
            else
               pMaxEle->PostIllum( sc, ipNS );
         }
      }
   }

}
// ----------------------------------------------------------------------------
// 6to4FlowInfo::ApplyL
// Final stage where the 6to4 touches the outgoing packet. Space is made for
// the IP header and the IP header is initialized.
// ----------------------------------------------------------------------------
TInt C6to4FlowInfo::ApplyL (RMBufSendPacket & aPacket, RMBufSendInfo & aInfo)
	{
	// Apply the tunnelling

	// Prepend the fixed content (if any) below the IP header
	if (iOffset > 0)
		{
		RMBufChain work;
		// Note: assumed that if CopyL leaves, then work is empty
		iPacket.CopyL(work);
		aPacket.Prepend(work);
		aInfo.iLength += iOffset;
		}
	switch(iOuterIp.Version())
		{
		case 6:
			{
			aPacket.PrependL(sizeof(TInet6HeaderIP));
			aInfo.iLength += sizeof(TInet6HeaderIP);
			aInfo.iProtocol = KProtocolInet6Ip;

			TInet6Packet<TInet6HeaderIP> ip(aPacket);
			if (ip.iHdr == NULL)
				User::Leave(KErrGeneral);

			// Build outer IPv6 header (already in iOuterIp, just copy)
			*ip.iHdr = iOuterIp;
			// Note: Certain packets must not have ECN ECT bits set. tcp_sap.cpp sets
			// KIpNoEcnEct for those packets.
			if (aInfo.iFlags & KIpNoEcnEct)
				{
				ip.iHdr->SetTrafficClass(ip.iHdr->TrafficClass() & 0xfc);
				}
			ip.iHdr->SetPayloadLength(aInfo.iLength);
			break;
			}
		case 4:
			{
			// Make space for IPv4 header
			aPacket.PrependL (20);
			aInfo.iLength += 20;
			aInfo.iProtocol = KProtocolInetIp;

			TInet6Checksum < TInet6HeaderIP4 > ip (aPacket);
			if (ip.iHdr == NULL)
				User::Leave (KErrGeneral);

			// Build the outer IPv4 header
			ip.iHdr->Init(iOuterIp.TrafficClass() & ((aInfo.iFlags & KIpNoEcnEct) ? 0xfc : 0xff));
			// Raw Address load. Someone else must have already checked that
			// both src and dst are IPv4 mapped addresses at the end of the
			// ReadyL phase! This just does a blind copy.
			ip.iHdr->DstAddrRef() = iOuterIp.DstAddr().u.iAddr32[3];
			ip.iHdr->SrcAddrRef() = iOuterIp.SrcAddr().u.iAddr32[3];

			// Somewhat ad hoc thing: if DontFragment flag is set, then
			// set the DF bit to the IPv4 header...
			if (aInfo.iFlags & KIpDontFragment)
				ip.iHdr->SetFlags((TUint8)(ip.iHdr->Flags() | KInet4IP_DF));
	
			ip.iHdr->SetHeaderLength (20);
			ip.iHdr->SetTotalLength (aInfo.iLength);
			ip.iHdr->SetTtl(iOuterIp.HopLimit());

			// NOTE!!! Something might be done for this. Now it is theoretically
			// possible that a same host receives protocol 41 packets from us and
			// we have two or more flows with protocol 41 AND two packets get 
			// fragmented and are reassembled in the same time by the remote host
			// AND the IPv4 identifier happens to be the same. That would break the
			// reassembly process, but since this is practically "impossible", it is
			// left now without a fix.
			ip.iHdr->SetIdentification (iPacketID++);                                                                
			ip.iHdr->SetProtocol (iOuterIp.NextHeader());

			// Checksum of the outer header.
			ip.ComputeChecksum ();
			break;
			}
		default:
			User::Leave(KErrGeneral);
		}

	// Pass the packet
	return 0;
	}
int LuSolve(
	size_t             n      ,
	size_t             m      , 
	const FloatVector &A      , 
	const FloatVector &B      , 
	FloatVector       &X      , 
	Float        &logdet      )
{	
	// check numeric type specifications
	CheckNumericType<Float>();

	// check simple vector class specifications
	CheckSimpleVector<Float, FloatVector>();

	size_t        p;       // index of pivot element (diagonal of L)
	int     signdet;       // sign of the determinant
	Float     pivot;       // pivot element

	// the value zero
	const Float zero(0);

	// pivot row and column order in the matrix
	std::vector<size_t> ip(n);
	std::vector<size_t> jp(n);

	// -------------------------------------------------------
	CPPAD_ASSERT_KNOWN(
		size_t(A.size()) == n * n,
		"Error in LuSolve: A must have size equal to n * n"
	);
	CPPAD_ASSERT_KNOWN(
		size_t(B.size()) == n * m,
		"Error in LuSolve: B must have size equal to n * m"
	);
	CPPAD_ASSERT_KNOWN(
		size_t(X.size()) == n * m,
		"Error in LuSolve: X must have size equal to n * m"
	);
	// -------------------------------------------------------

	// copy A so that it does not change
	FloatVector Lu(A);

	// copy B so that it does not change
	X = B;

	// Lu factor the matrix A
	signdet = LuFactor(ip, jp, Lu);

	// compute the log of the determinant
	logdet  = Float(0);
	for(p = 0; p < n; p++)
	{	// pivot using the max absolute element
		pivot   = Lu[ ip[p] * n + jp[p] ];

		// check for determinant equal to zero
		if( pivot == zero )
		{	// abort the mission
			logdet = Float(0);
			return   0;
		}

		// update the determinant
		if( LeqZero ( pivot ) )
		{	logdet += log( - pivot );
			signdet = - signdet;
		}
		else	logdet += log( pivot );

	}

	// solve the linear equations
	LuInvert(ip, jp, Lu, X);

	// return the sign factor for the determinant
	return signdet;
}
Exemple #7
0
 int CallFrame::line(STATE) {
   if(!compiled_code) return -2;        // trampoline context
   return compiled_code->line(state, ip());
 }
Exemple #8
0
void compute(const QUESO::FullEnvironment& env, unsigned int numModes) {
  ////////////////////////////////////////////////////////
  // Step 1 of 5: Instantiate the parameter space
  ////////////////////////////////////////////////////////


#ifdef APPLS_MODAL_USES_CONCATENATION
  QUESO::VectorSpace<QUESO::GslVector, QUESO::GslMatrix>
    paramSpaceA(env, "paramA_", 2, NULL);
  QUESO::VectorSpace<QUESO::GslVector, QUESO::GslMatrix>
    paramSpaceB(env, "paramB_", 1, NULL);
#endif
  QUESO::VectorSpace<QUESO::GslVector, QUESO::GslMatrix>
    paramSpace(env, "param_", 3, NULL);

  ////////////////////////////////////////////////////////
  // Step 2 of 5: Instantiate the parameter domain
  ////////////////////////////////////////////////////////
#ifdef APPLS_MODAL_USES_CONCATENATION
  QUESO::GslVector paramMinsA(paramSpaceA.zeroVector());
  paramMinsA[0] = 0.;
  paramMinsA[1] = 0.;
  QUESO::GslVector paramMaxsA(paramSpaceA.zeroVector());
  paramMaxsA[0] = 3.;
  paramMaxsA[1] = 3.;
  QUESO::BoxSubset<QUESO::GslVector,QUESO::GslMatrix>
  
  paramDomainA("paramA_",paramSpaceA,paramMinsA,paramMaxsA);

  QUESO::GslVector paramMinsB(paramSpaceB.zeroVector());
  paramMinsB[0] = 0.;
  QUESO::GslVector paramMaxsB(paramSpaceB.zeroVector());
  paramMaxsB[0] = INFINITY;
  QUESO::BoxSubset<QUESO::GslVector,QUESO::GslMatrix>
    paramDomainB("paramB_",paramSpaceB,paramMinsB,paramMaxsB);

  QUESO::ConcatenationSubset<QUESO::GslVector,QUESO::GslMatrix>
    paramDomain("",paramSpace,paramDomainA,paramDomainB);
#else
  QUESO::GslVector paramMins(paramSpace.zeroVector());
  paramMins[0] = 0.;
  paramMins[1] = 0.;
  paramMins[2] = 0.;
  QUESO::GslVector paramMaxs(paramSpace.zeroVector());
  paramMaxs[0] = 3.;
  paramMaxs[1] = 3.;
  paramMaxs[2] = .3;
  QUESO::BoxSubset<QUESO::GslVector,QUESO::GslMatrix>
    paramDomain("param_",paramSpace,paramMins,paramMaxs);
#endif

  ////////////////////////////////////////////////////////
  // Step 3 of 5: Instantiate the likelihood function object
  ////////////////////////////////////////////////////////
  likelihoodRoutine_DataType likelihoodRoutine_Data;
 
  likelihoodRoutine_Data.numModes = numModes;
  QUESO::GenericScalarFunction<QUESO::GslVector,QUESO::GslMatrix>
    likelihoodFunctionObj("like_",
                          paramDomain,
                          likelihoodRoutine,
                          (void *) &likelihoodRoutine_Data,
                          true); // routine computes [-2.*ln(function)]
  
    ////////////////////////////////////////////////////////
  // Step 4 of 5: Instantiate the inverse problem
  ////////////////////////////////////////////////////////
#ifdef APPLS_MODAL_USES_CONCATENATION
  QUESO::UniformVectorRV<QUESO::GslVector,QUESO::GslMatrix>
    priorRvA("priorA_", paramDomainA);
 
  QUESO::GslVector alpha(paramSpaceB.zeroVector());
  alpha[0] = 3.;
  QUESO::GslVector beta(paramSpaceB.zeroVector());
  if (numModes == 1) {
    beta[0] = 0.09709133373799;
  }
  else {
    beta[0] = 0.08335837191688;
  }
  QUESO::InverseGammaVectorRV<QUESO::GslVector,QUESO::GslMatrix>
    priorRvB("priorB_", paramDomainB,alpha,beta);

  QUESO::ConcatenatedVectorRV<QUESO::GslVector,QUESO::GslMatrix>
    priorRv("prior_", priorRvA, priorRvB, paramDomain);
#else
  QUESO::UniformVectorRV<QUESO::GslVector,QUESO::GslMatrix>
    priorRv("prior_", paramDomain);
#endif

  QUESO::GenericVectorRV<QUESO::GslVector,QUESO::GslMatrix>
    postRv("post_", paramSpace);

  QUESO::StatisticalInverseProblem<QUESO::GslVector,QUESO::GslMatrix>
    ip("", NULL, priorRv, likelihoodFunctionObj, postRv);

  ////////////////////////////////////////////////////////
  // Step 5 of 5: Solve the inverse problem
  ////////////////////////////////////////////////////////
  ip.solveWithBayesMLSampling();

  ////////////////////////////////////////////////////////
  // Print some statistics
  ////////////////////////////////////////////////////////
  unsigned int numPosTotal = postRv.realizer().subPeriod();
  if (env.subDisplayFile()) {
    *env.subDisplayFile() << "numPosTotal = " << numPosTotal
                          << std::endl;
  }

  QUESO::GslVector auxVec(paramSpace.zeroVector());
  unsigned int numPosTheta1SmallerThan1dot5 = 0;
  for (unsigned int i = 0; i < numPosTotal; ++i) {
    postRv.realizer().realization(auxVec);
    if (auxVec[0] < 1.5) numPosTheta1SmallerThan1dot5++;
  }

  if (env.subDisplayFile()) {
    *env.subDisplayFile() << "numPosTheta1SmallerThan1dot5 = " << numPosTheta1SmallerThan1dot5
                          << ", ratio = " << ((double) numPosTheta1SmallerThan1dot5)/((double) numPosTotal)
                          << std::endl;
  }

  return;
}
Exemple #9
0
void
Elastic::FAC::pack_strain(double* buffer,
                          const SAMRAI::hier::Patch& patch,
                          const SAMRAI::hier::Box& region,
                          const int &depth) const
{
  boost::shared_ptr<SAMRAI::pdat::SideData<double> > v_ptr=
    boost::dynamic_pointer_cast<SAMRAI::pdat::SideData<double> >
    (patch.getPatchData(v_id));
  SAMRAI::pdat::SideData<double>& v = *v_ptr;

  boost::shared_ptr<SAMRAI::pdat::CellData<double> > dv_diagonal;
  boost::shared_ptr<SAMRAI::pdat::SideData<double> > dv_mixed;
  if(!faults.empty())
    {
      dv_diagonal=boost::dynamic_pointer_cast<SAMRAI::pdat::CellData<double> >
        (patch.getPatchData(dv_diagonal_id));
      dv_mixed=boost::dynamic_pointer_cast<SAMRAI::pdat::SideData<double> >
        (patch.getPatchData(dv_mixed_id));
    }

  const Gamra::Dir dim=d_dim.getValue();
  if(have_embedded_boundary())
    {
      boost::shared_ptr<SAMRAI::pdat::SideData<double> > level_set_ptr;
      if(have_embedded_boundary())
        level_set_ptr=boost::dynamic_pointer_cast<SAMRAI::pdat::SideData<double> >
          (patch.getPatchData(level_set_id));
    }

  Gamra::Dir ix(Gamra::Dir::from_int(depth/dim)),
    iy(Gamra::Dir::from_int(depth%dim));
  const SAMRAI::hier::Index zero(SAMRAI::hier::Index::getZeroIndex(d_dim));
  SAMRAI::hier::Index ip(zero), jp(zero);
  ip[ix]=1;
  jp[iy]=1;

  const double *dx(boost::dynamic_pointer_cast
                   <SAMRAI::geom::CartesianPatchGeometry>
                   (patch.getPatchGeometry())->getDx());

  SAMRAI::pdat::CellIterator iend(SAMRAI::pdat::CellGeometry::end(region));
  for(SAMRAI::pdat::CellIterator
        icell(SAMRAI::pdat::CellGeometry::begin(region));
      icell!=iend; ++icell)
    {
      const SAMRAI::pdat::SideIndex
        s(*icell,ix,SAMRAI::pdat::SideIndex::Lower);
      if(ix==iy)
        {
          double diff(v(s+ip)-v(s));
          if(!faults.empty())
            diff-=(*dv_diagonal)(*icell,ix);
          *buffer=diff/dx[ix];
        }
      else
        {
          const int ix_iy(index_map(ix,iy,dim));
          double diff(- v(s-jp) - v(s+ip-jp) + v(s+jp)  + v(s+ip+jp));

          if(!faults.empty())
            diff+=(*dv_mixed)(s,ix_iy+1) - (*dv_mixed)(s-jp,ix_iy)
              + (*dv_mixed)(s+ip,ix_iy+1) - (*dv_mixed)(s+ip-jp,ix_iy)
              + (*dv_mixed)(s+jp,ix_iy+1) - (*dv_mixed)(s,ix_iy)
              + (*dv_mixed)(s+ip+jp,ix_iy+1) - (*dv_mixed)(s+ip,ix_iy);
          *buffer=diff/(4*dx[iy]);
        }
      ++buffer;
    }
}
Exemple #10
0
const char *
ipv6(const char *interface)
{
	return ip(interface, AF_INET6);
}
Exemple #11
0
void NodeConnection::Listener()
{
	try
	{
		while(this->Socket != SOCKET_ERROR)
		{
			Packet  packet;
			
			packet.getCommand(this->Socket);

			//printf("%s\n", packet.command); //this printf are not really thread save, and they are only needed for debug

			if (!strcmp(packet.command, "pong")) //just a keep alive
			{
				packet.sendData(this->Socket); //resending it
			}else if(!strcmp(packet.command,"version"))
			{
				packet_version version(packet);
				this->Version = version.version;
			}else if(!strcmp(packet.command,"verack"))
			{
				packet_verack verack(packet);

				verack.sendData(this->Socket);

				//empty addr for now
				packet_addr addr;
				addr.addr_list.clear();
				addr.encodeData();

				addr.setChecksum_Lenght_Magic();


				addr.sendData(this->Socket);

				//empty inv for now, this should be the whole list of object hash
				bool finished = false;
				vector<sTag> inventory;
				std::shared_lock<std::shared_timed_mutex> mlock(bitmrc->sharedObj.mutex_);
				for (int i = 0; i < bitmrc->sharedObj.Dim; i++)
				{
					hash_table<ustring>::linked_node * cur = bitmrc->sharedObj.Table[i];
					while (cur != NULL)
					{
						sTag hash;
						memcpy(hash.ch, cur->hash.c_str(), 32);
						inventory.push_back(hash);

						cur = cur->next;
					}
				}
				mlock.unlock();
				unsigned int i = 0;
				while (!finished)
				{
					packet_inv inv;
					while (i < 50000 && i < inventory.size())
					{
						inv.inventory.push_back(inventory[i]);
						i++;
					}
					inv.encodeData();

					inv.setChecksum_Lenght_Magic();


					inv.sendData(this->Socket);
					if (i == inventory.size())
						finished = true;
				}
				this->state = 2;
			}else if(!strcmp(packet.command,"addr"))
			{
				packet_addr addr(packet);
				for(unsigned int i=0;i<addr.addr_list.size();i++)
				{
					if(addr.addr_list[i].IPv6_4[11] == 0xff &&  addr.addr_list[i].IPv6_4[10] == 0xff)
					{
						char buf[40];
						sprintf(buf,"%i.%i.%i.%i", addr.addr_list[i].IPv6_4[12], addr.addr_list[i].IPv6_4[13], addr.addr_list[i].IPv6_4[14], addr.addr_list[i].IPv6_4[15]);
						string ip(buf);

						//printf("%s\n",buf);

						sprintf(buf, "%i", addr.addr_list[i].port);
						string port(buf);

						NodeConnection *tmp_node= new NodeConnection(ip,port, this->bitmrc);

						this->bitmrc->connectNode(tmp_node);
					}	
				}
			}else if(!strcmp(packet.command,"inv"))
			{
				packet_inv inv(packet);
				packet_getdata needed;
				for (unsigned int i = 0; i < inv.inventory.size(); i++)
				{
					ustring tag;
					for (int j = 0; j < 32; j++)
					{
						tag += inv.inventory[i].ch[j];
					}
					
					
					if (this->bitmrc->sharedObj.searchByHash(tag).empty())
					{
						needed.inventory.push_back(inv.inventory[i]);
					}
				}
				if (!needed.inventory.empty())
				{
					needed.encodeData();
					needed.setChecksum_Lenght_Magic();
					Packets.push(needed);
				}
			}else if(!strcmp(packet.command,"getdata"))
			{
				packet_getdata getdata(packet);

				for (unsigned int i = 0; i < getdata.inventory.size(); i++)
				{
					ustring tag;
					for (int j = 0; j < 32; j++)
					{
						tag += getdata.inventory[i].ch[j];
					}
					ustring ObjPayload = this->bitmrc->sharedObj.searchByHash(tag);
					if (!ObjPayload.empty())
					{
						object obj;
						
						memset(obj.command, 0x00, sizeof obj.command);
						strncpy(obj.command, "object", 7);

						obj.message_payload = ObjPayload;
						obj.setChecksum_Lenght_Magic();
						Packets.push(obj);
					}
				}
			}else if(!strcmp(packet.command,"object"))
			{
				object obj(packet);
				bool check = checkPow(obj.message_payload, obj.Time);

				//if not ignore
				if (check)
				{
					ustring invHash = this->bitmrc->inventoryHash(obj.message_payload);
					int present = this->bitmrc->sharedObj.insert(obj.message_payload, invHash);
					
					sTag tag;
					memcpy(tag.ch, invHash.c_str(), 32);

					this->bitmrc->new_inv.push(tag);

					if (obj.objectType == type_getpubkey)
					{
						packet_getpubkey getpubkey(obj);
						
						std::shared_lock<std::shared_timed_mutex> mlock(this->bitmrc->mutex_priv);

						for (unsigned int i = 0; i < this->bitmrc->PrivAddresses.size(); i++)
						{
							ustring tag = this->bitmrc->PrivAddresses[i].getTag();
							if (getpubkey.tag == tag)
							{
								if(this->bitmrc->PrivAddresses[i].getLastPubKeyRequest() + 60 * 60 * 24 * 4 < time(NULL))
									this->bitmrc->sendObj(this->bitmrc->PrivAddresses[i].encodePubKey(),true);
								//else
									//printf("PubKey already shared recently");
							}
						}

						mlock.unlock();
					}
					else if (obj.objectType == type_pubkey)
					{
						packet_pubkey pubkey(obj);

						std::shared_lock<std::shared_timed_mutex> mlock(this->bitmrc->mutex_pub);

						for (unsigned int i = 0; i < this->bitmrc->PubAddresses.size(); i++)
						{
							if (!this->bitmrc->PubAddresses[i].waitingPubKey())
								continue;
							ustring tag = this->bitmrc->PubAddresses[i].getTag();
							if (pubkey.tag == tag)
							{
								this->bitmrc->PubAddresses[i].decodeFromObj(pubkey);
							}
						}

						mlock.unlock();
					}
					else if (obj.objectType == type_msg)
					{
						packet_msg msg(obj);

						if (this->bitmrc->decryptMsg(msg)) //it takes like 1-4 milliseconds
						{
							//printf("Message accepted\n");
						}
					}
					else if (obj.objectType == type_broadcast)
					{
						packet_broadcast broadcast(obj);

						if (this->bitmrc->decryptMsg(broadcast))
						{
							//printf("broadcast decrypted\n");
						}
					}
				}
			}
			
		}
	}catch(int e)
	{
		switch(e)
		{
		case CONNECTION_CLOSED:
		case CONNECTION_ERROR:
			this->Close();
			return;
		}
	}
}
Exemple #12
0
const char *
ipv4(const char *interface)
{
	return ip(interface, AF_INET);
}
Exemple #13
0
void compute(const QUESO::FullEnvironment& env) {

  struct timeval timevalNow;
  gettimeofday(&timevalNow, NULL);
  std::cout << std::endl << "Beginning run of 'Hysteretic' example at "
            << ctime(&timevalNow.tv_sec);

  //------------------------------------------------------
  // Step 1 of 5: Instantiate the parameter space
  //------------------------------------------------------
  QUESO::VectorSpace<> paramSpaceA(env, "paramA_", 1, NULL);
  QUESO::VectorSpace<> paramSpaceB(env, "paramB_", 14, NULL);
  QUESO::VectorSpace<> paramSpace (env, "param_", 15, NULL);

  //------------------------------------------------------
  // Step 2 of 5: Instantiate the parameter domain
  //------------------------------------------------------
  QUESO::GslVector paramMinsA(paramSpaceA.zeroVector());
  paramMinsA.cwSet(0);
  QUESO::GslVector paramMaxsA(paramSpaceA.zeroVector());
  paramMaxsA.cwSet(5);
  QUESO::BoxSubset<> paramDomainA("paramA_",paramSpaceA,paramMinsA,paramMaxsA);

  QUESO::GslVector paramMinsB(paramSpaceB.zeroVector());
  paramMinsB.cwSet(-INFINITY);
  QUESO::GslVector paramMaxsB(paramSpaceB.zeroVector());
  paramMaxsB.cwSet( INFINITY);
  QUESO::BoxSubset<> paramDomainB("paramB_",paramSpaceB,paramMinsB,paramMaxsB);

  QUESO::ConcatenationSubset<> paramDomain("",paramSpace,paramDomainA,paramDomainB);

  //------------------------------------------------------
  // Step 3 of 5: Instantiate the likelihood function object
  //------------------------------------------------------
  std::cout << "\tInstantiating the Likelihood; calling internally the hysteretic model"
	    << std::endl;

  Likelihood<> likelihood("like_", paramDomain);

  likelihood.floor.resize(4,NULL);
  unsigned int numTimeSteps = 401;
  for (unsigned int i = 0; i < 4; ++i) {
    likelihood.floor[i] = new std::vector<double>(numTimeSteps,0.);
  }
  likelihood.accel.resize(numTimeSteps,0.);
  FILE *inp;
  inp = fopen("an.txt","r");
  unsigned int numObservations = 0;
  double tmpA;
  while (fscanf(inp,"%lf",&tmpA) != EOF) {
    likelihood.accel[numObservations] = tmpA;
    numObservations++;
  }

  numObservations=1;
  FILE *inp1_1;
  inp1_1=fopen("measured_data1_1.txt","r");
  while (fscanf(inp1_1,"%lf",&tmpA) != EOF) {
    (*likelihood.floor[0])[numObservations]=tmpA;
     numObservations++;
  }

  numObservations=0;
  FILE *inp1_2;
  inp1_2=fopen("measured_data1_2.txt","r");
  while (fscanf(inp1_2,"%lf",&tmpA) != EOF) {
    (*likelihood.floor[1])[numObservations]=tmpA;
    numObservations++;
  }

  numObservations=0;
  FILE *inp1_3;
  inp1_3=fopen("measured_data1_3.txt","r");
  while (fscanf(inp1_3,"%lf",&tmpA) != EOF) {
    (*likelihood.floor[2])[numObservations]=tmpA;
    numObservations++;
  }

  numObservations=0;
  FILE *inp1_4;
  inp1_4=fopen("measured_data1_4.txt","r");
  while (fscanf(inp1_4,"%lf",&tmpA) != EOF) {
    (*likelihood.floor[3])[numObservations]=tmpA;
    numObservations++;
  }


  //------------------------------------------------------
  // Step 4 of 5: Instantiate the inverse problem
  //------------------------------------------------------
  std::cout << "\tInstantiating the SIP" << std::endl;

  QUESO::UniformVectorRV<> priorRvA("priorA_", paramDomainA);

  QUESO::GslVector meanVec(paramSpaceB.zeroVector());
  QUESO::GslVector diagVec(paramSpaceB.zeroVector());

  diagVec.cwSet(0.6*0.6);

  QUESO::GslMatrix covMatrix(diagVec);

  QUESO::GaussianVectorRV<> priorRvB("priorB_", paramDomainB,meanVec,covMatrix);

  QUESO::ConcatenatedVectorRV<> priorRv("prior_", priorRvA, priorRvB, paramDomain);

  QUESO::GenericVectorRV<> postRv("post_", paramSpace);

  QUESO::StatisticalInverseProblem<> ip("", NULL, priorRv, likelihood, postRv);

  //------------------------------------------------------
  // Step 5 of 5: Solve the inverse problem
  //------------------------------------------------------
  std::cout << "\tSolving the SIP with Multilevel method" << std::endl;

  ip.solveWithBayesMLSampling();

  gettimeofday(&timevalNow, NULL);
  std::cout << "Ending run of 'Hysteretic' example at "
              << ctime(&timevalNow.tv_sec) << std::endl;
  return;
}
Exemple #14
0
//{{{ handleAreYouAlive
int handleAreYouAlive(msg_t msg){
	type(msg) = I_AM_ALIVE;
	if (_verbose) fprintf (stdout, "ARE_YOU_ALIVE received, sending I_AM_ALIVE\n");
	strncpy(ip(msg), ips(msg), IPLONG * sizeof(char));
	return (sendMessageWithAdd(msg));
}
Exemple #15
0
void networkSetup(uint8_t *srvMac, uint8_t *srvIp) {
	IPAddress ip(srvIp);
	Ethernet.begin(srvMac, ip);
	server.begin();
	delay(1000);
}
Exemple #16
0
int
CVODEModel::evaluateRHSFunction(
   double time,
   SundialsAbstractVector* y,
   SundialsAbstractVector* y_dot)
{
   /*
    * Convert Sundials vectors to SAMRAI vectors
    */
   std::shared_ptr<SAMRAIVectorReal<double> > y_samvect(
      Sundials_SAMRAIVector::getSAMRAIVector(y));
   std::shared_ptr<SAMRAIVectorReal<double> > y_dot_samvect(
      Sundials_SAMRAIVector::getSAMRAIVector(y_dot));

   std::shared_ptr<PatchHierarchy> hierarchy(y_samvect->getPatchHierarchy());

   /*
    * Compute max norm of solution vector.
    */
   //std::shared_ptr<HierarchyDataOpsReal<double> > hierops(
   //   new HierarchyCellDataOpsReal<double>(hierarchy));
   //double max_norm = hierops->maxNorm(y_samvect->
   //                                   getComponentDescriptorIndex(0));

   if (d_print_solver_info) {
      pout << "\t\tEval RHS: "
           << "\n   \t\t\ttime = " << time
           << "\n   \t\t\ty_maxnorm = " << y_samvect->maxNorm()
           << endl;
   }

   /*
    * Allocate scratch space and fill ghost cells in the solution vector
    * 1) Create a refine algorithm
    * 2) Register with the algorithm the current & scratch space, along
    *    with a refine operator.
    * 3) Use the refine algorithm to construct a refine schedule
    * 4) Use the refine schedule to fill data on fine level.
    */
   std::shared_ptr<RefineAlgorithm> bdry_fill_alg(
      new RefineAlgorithm());
   std::shared_ptr<RefineOperator> refine_op(d_grid_geometry->
                                               lookupRefineOperator(d_soln_var,
                                                  "CONSERVATIVE_LINEAR_REFINE"));
   bdry_fill_alg->registerRefine(d_soln_scr_id,  // dest
      y_samvect->
      getComponentDescriptorIndex(0),                            // src
      d_soln_scr_id,                            // scratch
      refine_op);

   for (int ln = hierarchy->getFinestLevelNumber(); ln >= 0; --ln) {
      std::shared_ptr<PatchLevel> level(hierarchy->getPatchLevel(ln));
      if (!level->checkAllocated(d_soln_scr_id)) {
         level->allocatePatchData(d_soln_scr_id);
      }

      // Note:  a pointer to "this" tells the refine schedule to invoke
      // the setPhysicalBCs defined in this class.
      std::shared_ptr<RefineSchedule> bdry_fill_alg_schedule(
         bdry_fill_alg->createSchedule(level,
            ln - 1,
            hierarchy,
            this));

      bdry_fill_alg_schedule->fillData(time);
   }

   /*
    * Step through the levels and compute rhs
    */
   for (int ln = hierarchy->getFinestLevelNumber(); ln >= 0; --ln) {
      std::shared_ptr<PatchLevel> level(hierarchy->getPatchLevel(ln));

      for (PatchLevel::iterator ip(level->begin()); ip != level->end(); ++ip) {
         const std::shared_ptr<Patch>& patch = *ip;

         std::shared_ptr<CellData<double> > y(
            SAMRAI_SHARED_PTR_CAST<CellData<double>, PatchData>(
               patch->getPatchData(d_soln_scr_id)));
         std::shared_ptr<SideData<double> > diff(
            SAMRAI_SHARED_PTR_CAST<SideData<double>, PatchData>(
               patch->getPatchData(d_diff_id)));
         std::shared_ptr<CellData<double> > rhs(
            SAMRAI_SHARED_PTR_CAST<CellData<double>, PatchData>(
               patch->getPatchData(y_dot_samvect->getComponentDescriptorIndex(0))));
         TBOX_ASSERT(y);
         TBOX_ASSERT(diff);
         TBOX_ASSERT(rhs);

         const Index ifirst(patch->getBox().lower());
         const Index ilast(patch->getBox().upper());

         const std::shared_ptr<CartesianPatchGeometry> patch_geom(
            SAMRAI_SHARED_PTR_CAST<CartesianPatchGeometry, PatchGeometry>(
               patch->getPatchGeometry()));
         TBOX_ASSERT(patch_geom);
         const double* dx = patch_geom->getDx();

         IntVector ghost_cells(y->getGhostCellWidth());

         /*
          * 1 eqn radiation diffusion
          */
         if (d_dim == Dimension(2)) {
            SAMRAI_F77_FUNC(comprhs2d, COMPRHS2D) (
               ifirst(0), ilast(0),
               ifirst(1), ilast(1),
               ghost_cells(0), ghost_cells(1),
               dx,
               y->getPointer(),
               diff->getPointer(0),
               diff->getPointer(1),
               rhs->getPointer());
         } else if (d_dim == Dimension(3)) {
            SAMRAI_F77_FUNC(comprhs3d, COMPRHS3D) (
               ifirst(0), ilast(0),
               ifirst(1), ilast(1),
               ifirst(2), ilast(2),
               ghost_cells(0), ghost_cells(1),
               ghost_cells(2),
               dx,
               y->getPointer(),
               diff->getPointer(0),
               diff->getPointer(1),
               diff->getPointer(2),
               rhs->getPointer());
         }

      } // loop over patches
   } // loop over levels

   /*
    * Deallocate scratch space.
    */
   for (int ln = hierarchy->getFinestLevelNumber(); ln >= 0; --ln) {
      hierarchy->getPatchLevel(ln)->deallocatePatchData(d_soln_scr_id);
   }

   /*
    * record current time and increment counter for number of RHS
    * evaluations.
    */
   d_current_soln_time = time;
   ++d_number_rhs_eval;

   return 0;
}
Exemple #17
0
 int CallFrame::line(STATE) {
   if(!cm) return -2;        // trampoline context
   return cm->line(state, ip());
 }
Exemple #18
0
double cv::pointPolygonTest( InputArray _contour, Point2f pt, bool measureDist )
{
    CV_INSTRUMENT_REGION();

    double result = 0;
    Mat contour = _contour.getMat();
    int i, total = contour.checkVector(2), counter = 0;
    int depth = contour.depth();
    CV_Assert( total >= 0 && (depth == CV_32S || depth == CV_32F));

    bool is_float = depth == CV_32F;
    double min_dist_num = FLT_MAX, min_dist_denom = 1;
    Point ip(cvRound(pt.x), cvRound(pt.y));

    if( total == 0 )
        return measureDist ? -DBL_MAX : -1;

    const Point* cnt = contour.ptr<Point>();
    const Point2f* cntf = (const Point2f*)cnt;

    if( !is_float && !measureDist && ip.x == pt.x && ip.y == pt.y )
    {
        // the fastest "purely integer" branch
        Point v0, v = cnt[total-1];

        for( i = 0; i < total; i++ )
        {
            v0 = v;
            v = cnt[i];

            if( (v0.y <= ip.y && v.y <= ip.y) ||
               (v0.y > ip.y && v.y > ip.y) ||
               (v0.x < ip.x && v.x < ip.x) )
            {
                if( ip.y == v.y && (ip.x == v.x || (ip.y == v0.y &&
                    ((v0.x <= ip.x && ip.x <= v.x) || (v.x <= ip.x && ip.x <= v0.x)))) )
                    return 0;
                continue;
            }

            int64 dist = static_cast<int64>(ip.y - v0.y)*(v.x - v0.x)
                       - static_cast<int64>(ip.x - v0.x)*(v.y - v0.y);
            if( dist == 0 )
                return 0;
            if( v.y < v0.y )
                dist = -dist;
            counter += dist > 0;
        }

        result = counter % 2 == 0 ? -1 : 1;
    }
    else
    {
        Point2f v0, v;
        Point iv;

        if( is_float )
        {
            v = cntf[total-1];
        }
        else
        {
            v = cnt[total-1];
        }

        if( !measureDist )
        {
            for( i = 0; i < total; i++ )
            {
                double dist;
                v0 = v;
                if( is_float )
                    v = cntf[i];
                else
                    v = cnt[i];

                if( (v0.y <= pt.y && v.y <= pt.y) ||
                   (v0.y > pt.y && v.y > pt.y) ||
                   (v0.x < pt.x && v.x < pt.x) )
                {
                    if( pt.y == v.y && (pt.x == v.x || (pt.y == v0.y &&
                        ((v0.x <= pt.x && pt.x <= v.x) || (v.x <= pt.x && pt.x <= v0.x)))) )
                        return 0;
                    continue;
                }

                dist = (double)(pt.y - v0.y)*(v.x - v0.x) - (double)(pt.x - v0.x)*(v.y - v0.y);
                if( dist == 0 )
                    return 0;
                if( v.y < v0.y )
                    dist = -dist;
                counter += dist > 0;
            }

            result = counter % 2 == 0 ? -1 : 1;
        }
        else
        {
            for( i = 0; i < total; i++ )
            {
                double dx, dy, dx1, dy1, dx2, dy2, dist_num, dist_denom = 1;

                v0 = v;
                if( is_float )
                    v = cntf[i];
                else
                    v = cnt[i];

                dx = v.x - v0.x; dy = v.y - v0.y;
                dx1 = pt.x - v0.x; dy1 = pt.y - v0.y;
                dx2 = pt.x - v.x; dy2 = pt.y - v.y;

                if( dx1*dx + dy1*dy <= 0 )
                    dist_num = dx1*dx1 + dy1*dy1;
                else if( dx2*dx + dy2*dy >= 0 )
                    dist_num = dx2*dx2 + dy2*dy2;
                else
                {
                    dist_num = (dy1*dx - dx1*dy);
                    dist_num *= dist_num;
                    dist_denom = dx*dx + dy*dy;
                }

                if( dist_num*min_dist_denom < min_dist_num*dist_denom )
                {
                    min_dist_num = dist_num;
                    min_dist_denom = dist_denom;
                    if( min_dist_num == 0 )
                        break;
                }

                if( (v0.y <= pt.y && v.y <= pt.y) ||
                   (v0.y > pt.y && v.y > pt.y) ||
                   (v0.x < pt.x && v.x < pt.x) )
                    continue;

                dist_num = dy1*dx - dx1*dy;
                if( dy < 0 )
                    dist_num = -dist_num;
                counter += dist_num > 0;
            }

            result = std::sqrt(min_dist_num/min_dist_denom);
            if( counter % 2 == 0 )
                result = -result;
        }
    }

    return result;
}
int main(int argc, char* argv[])
{
  try
  {
    // initialize logging - this reads the file log.xml from the current directory
    log_init();

    // read the command line options
    cxxtools::Arg<std::string> ip(argc, argv, 'i');  // option -i <ip-addres> defines the address where to find the server
    cxxtools::Arg<bool> binary(argc, argv, 'b');
    cxxtools::Arg<bool> json(argc, argv, 'j');
    cxxtools::Arg<bool> jsonhttp(argc, argv, 'J');
    cxxtools::Arg<std::size_t> timeout(argc, argv, 't', cxxtools::RemoteClient::WaitInfinite);
    cxxtools::Arg<unsigned short> port(argc, argv, 'p', binary ? 7003 : json ? 7004 : 7002);

    // we need a selector, which controls the network activity
    cxxtools::Selector selector;

    // Normally we would define just one rpc client for the protocol we use but
    // here we want to demonstrate, that it is just up to the client, which protocol
    // is used for the remote call.

    // One client can run just one request at a time. To run parallel requests
    // we need 2 clients. So we define 2 clients for each protocol.

    // define a xlmrpc client
    cxxtools::xmlrpc::HttpClient xmlrpcClient1(selector, ip, port, "/xmlrpc");
    cxxtools::xmlrpc::HttpClient xmlrpcClient2(selector, ip, port, "/xmlrpc");

    // and a binary rpc client
    cxxtools::bin::RpcClient binaryClient1(selector, ip, port);
    cxxtools::bin::RpcClient binaryClient2(selector, ip, port);

    // and a tcp json rpc client
    cxxtools::json::RpcClient jsonClient1(selector, ip, port);
    cxxtools::json::RpcClient jsonClient2(selector, ip, port);

    // and a http json rpc client
    cxxtools::json::HttpClient httpJsonClient1(selector, ip, port, "/jsonrpc");
    cxxtools::json::HttpClient httpJsonClient2(selector, ip, port, "/jsonrpc");

    // now se select the client depending on the command line flags

    cxxtools::RemoteClient& client1(
        binary   ? static_cast<cxxtools::RemoteClient&>(binaryClient1) :
        json     ? static_cast<cxxtools::RemoteClient&>(jsonClient1) :
        jsonhttp ? static_cast<cxxtools::RemoteClient&>(httpJsonClient1) :
                   static_cast<cxxtools::RemoteClient&>(xmlrpcClient1));

    cxxtools::RemoteClient& client2(
        binary   ? static_cast<cxxtools::RemoteClient&>(binaryClient2) :
        json     ? static_cast<cxxtools::RemoteClient&>(jsonClient2) :
        jsonhttp ? static_cast<cxxtools::RemoteClient&>(httpJsonClient2) :
                   static_cast<cxxtools::RemoteClient&>(xmlrpcClient2));

    // define remote procedure with dobule return value and two double parameters
    cxxtools::RemoteProcedure<double, double, double> add1(client1, "add");
    cxxtools::RemoteProcedure<double, double, double> add2(client2, "add");

    // initiate the execution of our method
    add1.begin(5, 6);
    add2.begin(1, 2);

    // Calling RemoteProcedure::end will run the underlying event loop until
    // the remote procedure is finished and return the result.
    // In case of a error, an exception is thrown.

    // Note that waiting for the end of one remote procedure will also start
    // and maybe finish the second remote procedure.

    double result1 = add1.end(timeout);

    std::cout << "result1=" << result1 << std::endl;

    // Here we run the loop again until the second procedure is finished. It
    // may well be, that the procedure is already finished and we get the
    // result immediately.

    double result2 = add2.end(timeout);

    std::cout << "result2=" << result2 << std::endl;

  }
  catch (const std::exception& e)
  {
    std::cerr << "error: " << e.what() << std::endl;
  }
}
Exemple #20
0
	void run() {
		m_mutex.lock();
		result=ip(X1,X2,N);
		m_finished=true;
		m_mutex.unlock();
	}
Exemple #21
0
int main(int argc, char ** argv) {
#ifdef QUESO_HAS_MPI
  MPI_Init(&argc, &argv);
  QUESO::FullEnvironment env(MPI_COMM_WORLD, argv[1], "", NULL);
#else
  QUESO::FullEnvironment env(argv[1], "", NULL);
#endif

  QUESO::VectorSpace<QUESO::GslVector, QUESO::GslMatrix> paramSpace(env,
      "param_", 1, NULL);

  double min_val = 0.0;
  double max_val = 1.0;

  QUESO::GslVector paramMins(paramSpace.zeroVector());
  paramMins.cwSet(min_val);
  QUESO::GslVector paramMaxs(paramSpace.zeroVector());
  paramMaxs.cwSet(max_val);

  QUESO::BoxSubset<QUESO::GslVector, QUESO::GslMatrix> paramDomain("param_",
      paramSpace, paramMins, paramMaxs);

  QUESO::UniformVectorRV<QUESO::GslVector, QUESO::GslMatrix> priorRv("prior_",
      paramDomain);

  // Set up observation space
  QUESO::VectorSpace<QUESO::GslVector, QUESO::GslMatrix> obsSpace(env,
      "obs_", 2, NULL);

  // Fill up observation vector
  QUESO::GslVector observations(obsSpace.zeroVector());
  observations[0] = 1.0;
  observations[1] = 1.0;

  // Pass in observations to Gaussian likelihood object
  Likelihood<QUESO::GslVector, QUESO::GslMatrix> lhood("llhd_", paramDomain,
      observations, 1.0);

  QUESO::GenericVectorRV<QUESO::GslVector, QUESO::GslMatrix>
    postRv("post_", paramSpace);

  QUESO::StatisticalInverseProblem<QUESO::GslVector, QUESO::GslMatrix>
    ip("", NULL, priorRv, lhood, postRv);

  QUESO::GslVector paramInitials(paramSpace.zeroVector());

  paramInitials[0] = 0.0;

  QUESO::GslMatrix proposalCovMatrix(paramSpace.zeroVector());

  for (unsigned int i = 0; i < 1; i++) {
    proposalCovMatrix(i, i) = 0.1;
  }

  ip.solveWithBayesMetropolisHastings(NULL, paramInitials, &proposalCovMatrix);

#ifdef QUESO_HAS_MPI
  MPI_Finalize();
#endif

  return 0;
}
Exemple #22
0
void normalize(float *X,long N) {
	double norm=sqrt(ip(X,X,N));
	if (norm) {
		for (long j=0; j<N; j++) X[j]/=norm;
	}
}
Exemple #23
0
void PDFwin::pageChanged()
{
	int ww=w()-2;
	int hh=h()-2;

	if(!mModel) return;

	if(!mModel->cacheValid(ww, hh, mCurrPage))
	{
		mState="load";
		redraw();
		Fl::check();
		mState="none";
	}

	CImage* bmp_orig=mModel->getPage(ww, hh, mCurrPage);
	
	
	if(mLayout->findCheckButton("Use automatic segmentation")->value())
	{
		CImage temp;
		temp.CopyFrom(*bmp_orig);
#ifdef DEBUG_FONT_DETECTION
		CImage* bmp=bmp_orig;
#else
		CImage* bmp=&temp;
#endif	
		{

			intmatrixn& textCache=*mModel->_textCache[mCurrPage];


#ifdef USE_FONT_DETECTION
			const bool drawLetterBoundingBox=true;
			const bool drawDetectedSpace=true;
#else
			const bool drawLetterBoundingBox=false;
			const bool drawDetectedSpace=false;
#endif
			if(drawLetterBoundingBox)
			{
				CImagePixel ip(bmp);
				for(int i=0; i<textCache.rows()-1; i++)
				{
					int* info=textCache[i];
					int c=info[TC_c];
					int x=info[TC_x];
					int y=info[TC_y];
					int w=info[TC_w];
					int h=info[TC_h];
					int fx=info[TC_fx];
					int fy=info[TC_fy];
					if (std::abs(y-textCache[i+1][TC_y])<=3) // draw excluding trailing space
						{

#ifdef DEBUG_FONT_DETECTION
					ip.DrawLineBox(TRect(x-fx,y-fy, x-fx+w, y-fy+h), CPixelRGB8(0,0,0));				
#else
					ip.DrawBox(TRect(x-fx,y-fy, x-fx+w, y-fy+h), CPixelRGB8(0,0,0));
#endif
					ip.DrawHorizLine(x, y, w, CPixelRGB8(255,0,0));
						}
					// else printf("%d %d %d %d\n", x,y, textCache[i+1][TC_x], textCache[i+1][TC_y]);
				}
			}


			FlLayout* layout=mLayout->findLayout("Automatic segmentation");
			double min_text_gap=layout->findSlider("MIN text-gap")->value();
			if(drawDetectedSpace)
			{
				CImagePixel ip(bmp);
				for(int i=1; i<textCache.rows(); i++)
				{
					int* pinfo=textCache[i-1];
					int* info=textCache[i];

					if(pinfo[TC_y]==info[TC_y])
					{						
						int prevEnd=pinfo[TC_x]-pinfo[TC_fx]+pinfo[TC_w]-1;
						int curStart=info[TC_x]-info[TC_fx];

						int bottom=MIN(pinfo[TC_y]-pinfo[TC_fy], info[TC_y]-info[TC_fy]);
						int top=MAX(pinfo[TC_y]-pinfo[TC_fy]+pinfo[TC_h], info[TC_y]-info[TC_fy]+info[TC_h]);
						int space_thr=MAX(pinfo[TC_h], pinfo[TC_w]);
						space_thr=MAX(space_thr, info[TC_h]);
						space_thr=MAX(space_thr, info[TC_w]);
						space_thr*=min_text_gap;

						if(curStart-prevEnd<=space_thr && curStart>prevEnd)
						{
							// mark as space.
							ip.DrawBox(TRect(prevEnd, bottom, curStart, top), CPixelRGB8(0,0,0));
						}
					}					
				}

			}


		}	

		SummedAreaTable t(*bmp);//bmp->getWidth(), bmp->getHeight(), bmp->getDataPtr(), bmp->getRowSize());

		
		


		FlLayout* layout=mLayout->findLayout("Automatic segmentation");
		double min_gap_percentage=layout->findSlider("MIN gap")->value();
		double margin_percentage=layout->findSlider("Margin")->value();
		int thr_white=layout->findSlider("white point")->value();
		double max_width=1.0/layout->findSlider("N columns")->value();
		double cropT=layout->findSlider("Crop T")->value()/100.0;
		double cropB=layout->findSlider("Crop B")->value()/100.0;
		double cropL=layout->findSlider("Crop L")->value()/100.0;
		double cropR=layout->findSlider("Crop R")->value()/100.0;


//		TRect domain(0,0, bmp->GetWidth(), bmp->GetHeight());

		TRect domain(cropL*bmp->GetWidth(),cropT*bmp->GetHeight(), (1-cropR)*bmp->GetWidth()
, (1-cropB)*bmp->GetHeight());

	
		ImageSegmentation s(t, true, domain, 0, min_gap_percentage, thr_white);
		s.segment();
		std::list<TRect> results;
		s.getResult(results, max_width, margin_percentage);

		/*

		std::list<TRect> results;
		int min_gap=int((double)bmp->GetWidth()*min_gap_percentage*0.01 +0.5);
		int min_box_size=100;	// 10�ȼ������� �ڽ��� ������.

		CImagePixel orig(bmp_orig);

		std::list<index2> LRcorners;
		std::list<index2> ULcorners;
		for(int i=0, ni=bmp->GetWidth()-min_box_size-min_gap; i<ni; i++)
		{
			for(int j=0, nj=bmp->GetHeight()-min_box_size-min_gap; j<nj; j++)
			{
				// search for LR corner
				TRect outerBox(i, j, i+min_box_size+min_gap, j+min_box_size+min_gap);
				TRect innerBox(i, j, i+min_box_size, j+min_box_size);
				
				int sum1=t.sum(outerBox);
				int sum2=t.sum(innerBox);

				int area=outerBox.Width()*outerBox.Height()-
					innerBox.Width()*innerBox.Height();
				if(sum1-sum2 >= thr_white*area)
				{
					//orig.SetPixel(i+min_box_size+min_gap/2,j+min_box_size+min_gap/2,CPixelRGB8(255,0,0));

					LRcorners.push_back(index2(i+min_box_size+min_gap/2,j+min_box_size+min_gap/2));
				}

				// search for UL corner
				{
					TRect outerBox(i, j, i+min_box_size+min_gap, j+min_box_size+min_gap);
					TRect innerBox(i+min_gap, j+min_gap, i+min_box_size+min_gap, j+min_box_size+min_gap);
					
					int sum1=t.sum(outerBox);
					int sum2=t.sum(innerBox);

					int area=outerBox.Width()*outerBox.Height()-
						innerBox.Width()*innerBox.Height();
					if(sum1-sum2 >= thr_white*area)
					{
			//			orig.SetPixel(i+min_gap/2,j+min_gap/2,CPixelRGB8(0,0,255));
						ULcorners.push_back(index2(i+min_gap/2,j+min_gap/2));
					}
				}
			}
		}


		{
			std::list<index2>::iterator i;

			for(i=ULcorners.begin(); i!=ULcorners.end(); ++i)
			{
				orig.SetPixel((*i).x(), (*i).y(), CPixelRGB8(0,0,255));
			}
		}
		
		*/
		mRects.clear();

		int x=toWindowCoord(0,0).x;
		int y=toWindowCoord(0,0).y;

		std::list<TRect>::iterator i;
		for(i=results.begin(); i!=results.end(); i++)
		{
			mRects.push_back(SelectionRectangle());
			mSelectedRect=mRects.end();
			mSelectedRect--;
			SelectionRectangle& mRect=selectedRect();
			mRect.p1=toDocCoord((*i).left+x, (*i).top+y);
			mRect.p2=toDocCoord((*i).right+x, (*i).bottom+y);
			mRect.updateScreenCoord(*this);
		}
	}
}
Exemple #24
0
int main(int argc, char* argv[])
{
  try
  {
    log_init();

    cxxtools::Arg<std::string> ip(argc, argv, 'i');
    cxxtools::Arg<unsigned short> port(argc, argv, 'p', 1234);
    cxxtools::Arg<unsigned> bufsize(argc, argv, 'b', 8192);
    cxxtools::Arg<bool> listen(argc, argv, 'l');
    cxxtools::Arg<bool> read_reply(argc, argv, 'r');
    cxxtools::Arg<bool> ssl(argc, argv, 's');
    cxxtools::Arg<std::string> cert(argc, argv, "--cert");
    cxxtools::Arg<std::string> ca(argc, argv, "--CA");

    if (listen)
    {
      // I'm a server

      // listen to a port
      cxxtools::net::TcpServer server(ip.getValue(), port);

      // accept a connetion
      cxxtools::net::TcpStream worker(server, bufsize);

      if (ssl)
      {
          if (cert.isSet())
              worker.loadSslCertificateFile(cert);
          if (ca.isSet())
              worker.setSslVerify(2, ca);
          worker.sslAccept();
      }

      // copy to stdout
      std::cout << worker.rdbuf();
    }
    else
    {
      // I'm a client

      // connect to server
      cxxtools::net::TcpStream peer(ip, port, bufsize);

      if (ssl)
      {
          if (cert.isSet())
              peer.loadSslCertificateFile(cert);
          peer.sslConnect();
      }

      if (argc > 1)
      {
          for (int a = 1; a < argc; ++a)
          {
              std::ifstream in(argv[a]);
              peer << in.rdbuf() << std::flush;
          }
      }
      else
      {
          // copy stdin to server
          peer << std::cin.rdbuf() << std::flush;
      }

      if (read_reply)
        // copy answer to stdout
        std::cout << peer.rdbuf() << std::flush;
    }
  }
  catch (const std::exception& e)
  {
    std::cerr << e.what() << std::endl;
  }
}
Exemple #25
0
HierarchyNeighbors::HierarchyNeighbors(
   const PatchHierarchy& hierarchy,
   int coarsest_level,
   int finest_level,
   bool do_same_level_nbrs, 
   int width)
: d_coarsest_level(coarsest_level),
  d_finest_level(finest_level),
  d_do_same_level_nbrs(do_same_level_nbrs),
  d_neighbor_width(hierarchy.getDim(), width)
{
   int num_levels = hierarchy.getNumberOfLevels();
   TBOX_ASSERT(coarsest_level >= 0);
   TBOX_ASSERT(coarsest_level <= finest_level);
   TBOX_ASSERT(finest_level < num_levels);
   TBOX_ASSERT(width >= 1);

   d_same_level_nbrs.resize(num_levels);
   d_finer_level_nbrs.resize(num_levels);
   d_coarser_level_nbrs.resize(num_levels);

   for (int ln = finest_level; ln >= coarsest_level; --ln) {
      const std::shared_ptr<PatchLevel>& current_level =
         hierarchy.getPatchLevel(ln);

      /*
       * If there is a finer level than ln, find the finer level neighbors
       */
      if (ln != finest_level) {

         std::shared_ptr<PatchLevel> finer_level(
            hierarchy.getPatchLevel(ln+1));

         hier::IntVector connector_width(
            IntVector::getOne(hierarchy.getDim()),
            hierarchy.getNumberBlocks());

         if (d_neighbor_width != IntVector::getOne(hierarchy.getDim())) {
            connector_width *= d_neighbor_width;
            connector_width.ceilingDivide(finer_level->getRatioToCoarserLevel());
         }

         const Connector& coarse_to_fine =
            current_level->findConnector(
               *finer_level,
               connector_width,
               CONNECTOR_IMPLICIT_CREATION_RULE,
               true);

         for (PatchLevel::iterator ip(current_level->begin());
              ip != current_level->end(); ++ip) {

            const std::shared_ptr<Patch>& patch = *ip;
            const Box& box = patch->getBox();
            const BoxId& box_id = box.getBoxId();
            BoxContainer& finer_nbrs = d_finer_level_nbrs[ln][box_id];

            if (coarse_to_fine.hasNeighborSet(box_id)) {
               coarse_to_fine.getNeighborBoxes(box_id, finer_nbrs);
            }
         }
      }

      /*
       * If there is a coarser level than ln, find the coarser level neighbors
       */
      if (ln != coarsest_level) {

         std::shared_ptr<PatchLevel> coarser_level(
            hierarchy.getPatchLevel(ln-1));

         const Connector& fine_to_coarse =
            current_level->findConnector(
               *coarser_level,
               d_neighbor_width,
               CONNECTOR_IMPLICIT_CREATION_RULE,
               true);

         for (PatchLevel::iterator ip(current_level->begin());
              ip != current_level->end(); ++ip) {

            const std::shared_ptr<Patch>& patch = *ip;
            const Box& box = patch->getBox();
            const BoxId& box_id = box.getBoxId();
            BoxContainer& coarser_nbrs = d_coarser_level_nbrs[ln][box_id];

            if (fine_to_coarse.hasNeighborSet(box_id)) {
               fine_to_coarse.getNeighborBoxes(box_id, coarser_nbrs);
            }
         }
      }

      /*
       * Find the neighbors on the current level
       */
      if (do_same_level_nbrs) {
         const Connector& current_to_current =
            current_level->findConnector(
               *current_level,
               d_neighbor_width,
               CONNECTOR_IMPLICIT_CREATION_RULE,
               true);

         for (PatchLevel::iterator ip(current_level->begin());
              ip != current_level->end(); ++ip) {
            const std::shared_ptr<Patch>& patch = *ip;
            const Box& box = patch->getBox();
            const BoxId& box_id = box.getBoxId();
            BoxContainer& same_level_nbrs = d_same_level_nbrs[ln][box_id];

            BoxContainer same_nbr_boxes;
            if (current_to_current.hasNeighborSet(box_id)) {
               current_to_current.getNeighborBoxes(box_id, same_nbr_boxes);
            }
            for (BoxContainer::iterator itr =
                 same_nbr_boxes.begin(); itr != same_nbr_boxes.end(); ++itr) {
               if (box_id != itr->getBoxId()) {
                  same_level_nbrs.insert(same_level_nbrs.end(), *itr);
               }
            }
         }
      }
   }
}
    void UdpServerTransport::open()
    {
        RCF_LOG_4()(mIpAddress.string()) << "UdpServerTransport - creating server socket.";

        int mPort = mIpAddress.getPort();

        // create and bind a socket for receiving UDP messages
        if (mFd == -1 && mPort >= 0)
        {
            int ret = 0;
            int err = 0;

            mIpAddress.resolve();
            mFd = mIpAddress.createSocket(SOCK_DGRAM, IPPROTO_UDP);

            // enable reception of broadcast messages
            int enable = 1;
            ret = setsockopt(mFd, SOL_SOCKET, SO_BROADCAST, (char *) &enable, sizeof(enable));
            err = Platform::OS::BsdSockets::GetLastError();
            if (ret)
            {
                RCF_LOG_1()(ret)(err) << "setsockopt() - failed to set SO_BROADCAST on listening udp socket.";
            }

            // Share the address binding, if appropriate.
            if (mEnableSharedAddressBinding)
            {
                enable = 1;

                // Set SO_REUSEADDR socket option.
                ret = setsockopt(mFd, SOL_SOCKET, SO_REUSEADDR, (char *) &enable, sizeof(enable));
                err = Platform::OS::BsdSockets::GetLastError();
                if (ret)
                {
                    RCF_LOG_1()(ret)(err) << "setsockopt() - failed to set SO_REUSEADDR on listening udp multicast socket.";
                }

                // On OS X and BSD variants, need to set SO_REUSEPORT as well.

#if (defined(__MACH__) && defined(__APPLE__)) || defined(__FreeBSD__) || defined(__OpenBSD__) || defined(__NetBSD__) 

                ret = setsockopt(mFd, SOL_SOCKET, SO_REUSEPORT, (char *) &enable, sizeof(enable));
                err = Platform::OS::BsdSockets::GetLastError();
                if (ret)
                {
                    RCF_LOG_1()(ret)(err) << "setsockopt() - failed to set SO_REUSEPORT on listening udp multicast socket.";
                }
#endif

            }
            
            sockaddr * pServerAddr = NULL;
            Platform::OS::BsdSockets::socklen_t serverAddrSize = 0;
            mIpAddress.getSockAddr(pServerAddr, serverAddrSize);

            // bind the socket
            ret = ::bind(mFd, pServerAddr, serverAddrSize);
            if (ret < 0)
            {
                err = Platform::OS::BsdSockets::GetLastError();
                Exception e(_RcfError_Socket("bind()"), err, RcfSubsystem_Os);
                RCF_THROW(e)(mFd)(mIpAddress.string())(ret);
            }
            RCF_ASSERT_NEQ( mFd , -1 );

            if (!mMulticastIpAddress.empty())
            {
                // set socket option for receiving multicast messages

                mMulticastIpAddress.resolve();

                // TODO: implement for IPv6.
                RCF_ASSERT(
                        mIpAddress.getType() == IpAddress::V4 
                    &&  mMulticastIpAddress.getType() == IpAddress::V4);

                std::string ip = mMulticastIpAddress.getIp();
            
                ip_mreq imr;
                imr.imr_multiaddr.s_addr = inet_addr(ip.c_str());
                imr.imr_interface.s_addr = INADDR_ANY;
                int ret = setsockopt(mFd,IPPROTO_IP, IP_ADD_MEMBERSHIP, (const char*) &imr, sizeof(imr));
                int err = Platform::OS::BsdSockets::GetLastError();

                RCF_VERIFY(
                    ret ==  0,
                    Exception(
                        _RcfError_Socket("setsockopt() with IPPROTO_IP/IP_ADD_MEMBERSHIP"),
                        err,
                        RcfSubsystem_Os))
                        (mMulticastIpAddress.string())(mIpAddress.string());

                // TODO: enable source-filtered multicast messages
                //ip_mreq_source imr;
                //imr.imr_multiaddr.s_addr = inet_addr("232.5.6.7");
                //imr.imr_sourceaddr.s_addr = INADDR_ANY;//inet_addr("10.1.1.2");
                //imr.imr_interface.s_addr = INADDR_ANY;
                //int ret = setsockopt(mFd,IPPROTO_IP, IP_ADD_SOURCE_MEMBERSHIP, (const char*) &imr, sizeof(imr));
                //int err = Platform::OS::BsdSockets::GetLastError();
            }

            // set the socket to nonblocking mode
            Platform::OS::BsdSockets::setblocking(mFd, false);

            // retrieve the port number, if it's generated by the system
            if (mPort == 0)
            {
                IpAddress ip(mFd, mIpAddress.getType());
                mPort = ip.getPort();
                mIpAddress.setPort(mPort);
            }

            RCF_LOG_2() << "UdpServerTransport - listening on port " << mPort << ".";
        }
    }
int main(int argc, const char* argv[])
{
  std::string opt_ip = "131.254.10.126";
  bool opt_language_english = true;
  bool opt_debug = false;

  for (unsigned int i=0; i<argc; i++) {
    if (std::string(argv[i]) == "--ip")
      opt_ip = argv[i+1];
    else if (std::string(argv[i]) == "--fr")
      opt_language_english = false;
    else if (std::string(argv[i]) == "--debug")
      opt_debug = true;
    else if (std::string(argv[i]) == "--help") {
      std::cout << "Usage: " << argv[0] << "[--ip <robot address>] [--fr]" << std::endl;
      return 0;
    }
  }

  std::string camera_name = "CameraTopPepper";

  // Open the grabber for the acquisition of the images from the robot
  vpNaoqiGrabber g;
  if (! opt_ip.empty())
    g.setRobotIp(opt_ip);
  g.setFramerate(30);
  g.setCamera(0);
  g.open();


  vpCameraParameters cam = vpNaoqiGrabber::getIntrinsicCameraParameters(AL::kQVGA,camera_name, vpCameraParameters::perspectiveProjWithDistortion);
  vpHomogeneousMatrix eMc = vpNaoqiGrabber::getExtrinsicCameraParameters(camera_name,vpCameraParameters::perspectiveProjWithDistortion);

  std::cout << "eMc:" << std::endl << eMc << std::endl;
  std::cout << "cam:" << std::endl << cam << std::endl;
  vpNaoqiRobot robot;

  // Connect to the robot
  if (! opt_ip.empty())
    robot.setRobotIp(opt_ip);
  robot.open();



  if (robot.getRobotType() != vpNaoqiRobot::Pepper)
  {
    std::cout << "ERROR: You are not connected to Pepper, but to a different Robot. Check the IP. " << std::endl;
    return 0;
  }

  std::vector<std::string> jointNames_head = robot.getBodyNames("Head");

  // Open Proxy for the speech
  AL::ALTextToSpeechProxy tts(opt_ip, 9559);
  std::string phraseToSay;
  if (opt_language_english)
  {
    tts.setLanguage("English");
    phraseToSay = " \\emph=2\\ Hi,\\pau=200\\ How are you ?";
  }
  else
  {
    tts.setLanguage("French");
    phraseToSay = " \\emph=2\\ Bonjour,\\pau=200\\ comment vas  tu ?";
  }

  // Inizialize PeoplePerception
  AL::ALPeoplePerceptionProxy people_proxy(opt_ip, 9559);
  AL::ALMemoryProxy m_memProxy(opt_ip, 9559);
  people_proxy.subscribe("People", 30, 0.0);
  std::cout << "period: " << people_proxy.getCurrentPeriod() << std::endl;

  // Open Proxy for the recognition speech
  AL::ALSpeechRecognitionProxy asr(opt_ip, 9559);
  //  asr.unsubscribe("Test_ASR");
  //   return 0 ;

  asr.setVisualExpression(false);
  asr.setLanguage("English");
  std::vector<std::string> vocabulary;
  vocabulary.push_back("follow me");
  vocabulary.push_back("stop");

  // Set the vocabulary
  asr.setVocabulary(vocabulary,false);

  // Start the speech recognition engine with user Test_ASR
  asr.subscribe("Test_ASR");
  std::cout << "Speech recognition engine started" << std::endl;

  // Proxy to control the leds
  AL::ALLedsProxy led_proxy(opt_ip, 9559);

  //Declare plots
  vpPlot * plotter_diff_vel; vpPlot * plotter_vel;
  vpPlot * plotter_error;  vpPlot * plotter_distance;

  if (opt_debug)
  {
    // Plotting
    plotter_diff_vel = new vpPlot (2);
    plotter_diff_vel->initGraph(0, 2);
    plotter_diff_vel->initGraph(1, 2);
    plotter_diff_vel->setTitle(0,  "HeadYaw");
    plotter_diff_vel->setTitle(1,  "HeadPitch");

    plotter_vel= new vpPlot (1);
    plotter_vel->initGraph(0, 5);
    plotter_vel->setLegend(0, 0, "vx");
    plotter_vel->setLegend(0, 1, "vy");
    plotter_vel->setLegend(0, 2, "wz");
    plotter_vel->setLegend(0, 3, "q_yaw");
    plotter_vel->setLegend(0, 4, "q_pitch");

    plotter_error = new vpPlot(1);
    plotter_error->initGraph(0, 3);
    plotter_error->setLegend(0, 0, "x");
    plotter_error->setLegend(0, 1, "y");
    plotter_error->setLegend(0, 2, "Z");

    plotter_distance = new vpPlot (1);
    plotter_distance->initGraph(0, 1);
    plotter_distance->setLegend(0, 0, "dist");
  }

  try {
    vpImage<unsigned char> I(g.getHeight(), g.getWidth());
    vpDisplayX d(I);
    vpDisplay::setTitle(I, "ViSP viewer");

    vpFaceTrackerOkao face_tracker(opt_ip,9559);

    double dist = 0.0; // Distance between person detected from peoplePerception and faceDetection

    // Set Visual Servoing:
    vpServo task;
    task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
    task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE);
    //    vpAdaptiveGain lambda_adapt;
    //    lambda_adapt.initStandard(1.6, 1.8, 15);
    vpAdaptiveGain lambda_base(1.2, 1.0, 10); // 2.3, 0.7, 15
    vpAdaptiveGain lambda_nobase(5, 2.9, 15); // 4, 0.5, 15
    task.setLambda(lambda_base) ;

    double Z = 0.9;
    double Zd = 0.9;
    bool stop_vxy = false;
    bool move_base = true;
    bool move_base_prev = true;

    // Create the desired  visual feature
    vpFeaturePoint s;
    vpFeaturePoint sd;
    vpImagePoint ip(I.getHeight()/2, I.getWidth()/2);
    // Create the current x visual feature
    vpFeatureBuilder::create(s, cam, ip);
    vpFeatureBuilder::create(sd, cam, ip);

    //   sd.buildFrom( I.getWidth()/2, I.getHeight()/2, Zd);

    AL::ALValue limit_yaw = robot.getProxy()->getLimits("HeadYaw");
    std::cout << limit_yaw[0][0] << " " << limit_yaw[0][1] << std::endl;

    // Add the feature
    task.addFeature(s, sd) ;

    vpFeatureDepth s_Z, s_Zd;
    s_Z.buildFrom(s.get_x(), s.get_y(), Z , 0); // log(Z/Z*) = 0 that's why the last parameter is 0
    s_Zd.buildFrom(sd.get_x(), sd.get_y(), Zd , 0); // log(Z/Z*) = 0 that's why the last parameter is 0

    // Add the feature
    task.addFeature(s_Z, s_Zd);

    // Jacobian 6x5 (vx,vy,wz,q_yaq,q_pitch)
    vpMatrix tJe(6,5);
    tJe[0][0]= 1;
    tJe[1][1]= 1;
    tJe[5][2]= 1;
    vpMatrix eJe(6,5);

    double servo_time_init = 0;

    vpImagePoint head_cog_cur;
    vpImagePoint head_cog_des(I.getHeight()/2, I.getWidth()/2);
    vpColVector q_dot;

    bool reinit_servo = true;
    unsigned long loop_iter = 0;

    std::vector<std::string> recognized_names;
    std::map<std::string,unsigned int> detected_face_map;
    bool detection_phase = true;
    unsigned int f_count = 0;
    //    AL::ALValue leg_names  = AL::ALValue::array("HipRoll","HipPitch", "KneePitch" );
    //    AL::ALValue values  = AL::ALValue::array(0.0, 0.0, 0.0 );

    // robot.getProxy()->setMoveArmsEnabled(false,false);
    std::vector<std::string> arm_names = robot.getBodyNames("LArm");
    std::vector<std::string> rarm_names = robot.getBodyNames("RArm");
    std::vector<std::string> leg_names(3);
    leg_names[0]= "HipRoll";
    leg_names[1]= "HipPitch";
    leg_names[2]= "KneePitch";

    arm_names.insert( arm_names.end(), rarm_names.begin(), rarm_names.end() );
    arm_names.insert( arm_names.end(), leg_names.begin(), leg_names.end() );

    std::vector<float> arm_values(arm_names.size(),0.0);

    for (unsigned int i = 0; i < arm_names.size(); i++ )
      std::cout << arm_names[i]<< std::endl;

    robot.getPosition(arm_names, arm_values,false);

    vpImagePoint best_cog_face_peoplep;
    bool person_found = false;

    double t_prev = vpTime::measureTimeSecond();

    while(1) {
      if (reinit_servo) {
        servo_time_init = vpTime::measureTimeSecond();
        t_prev = vpTime::measureTimeSecond();
        reinit_servo = false;
        led_proxy.fadeRGB("FaceLeds","white",0.1);
      }

      double t = vpTime::measureTimeMs();
      if (0) // (opt_debug)
      {
        g.acquire(I);
      }
      vpDisplay::display(I);
      // Detect face
      bool face_found = face_tracker.detect();
      stop_vxy = false;

      //std::cout << "Loop time face_tracker: " << vpTime::measureTimeMs() - t << " ms" << std::endl;

      // Check speech recognition result

      AL::ALValue result_speech = m_memProxy.getData("WordRecognized");

      if ( ((result_speech[0]) == vocabulary[0]) && (double (result_speech[1]) > 0.4 )) //move
      {
        //std::cout << "Recognized: " << result_speech[0] << "with confidence of " << result_speech[1] << std::endl;
        task.setLambda(lambda_base) ;

        move_base = true;
      }
      else if ( (result_speech[0] == vocabulary[1]) && (double(result_speech[1]) > 0.4 )) //stop
      {
        //std::cout << "Recognized: " << result_speech[0] << "with confidence of " << result_speech[1] << std::endl;
        task.setLambda(lambda_nobase) ;
        move_base = false;
      }

      if (move_base != move_base_prev)
      {
        if (move_base)
        {
          phraseToSay = "Ok, I will follow you.";
          tts.post.say(phraseToSay);
        }
        else
        {
          phraseToSay = "Ok, I will stop.";
          tts.post.say(phraseToSay);
        }

      }

      //robot.setStiffness(arm_names,0.0);
      //robot.getProxy()->setAngles(arm_names,arm_values,1.0);
      //robot.getProxy()->setAngles("HipRoll",0.0,1.0);

      //std::cout << "Loop time check_speech: " << vpTime::measureTimeMs() - t << " ms" << std::endl;

      move_base_prev = move_base;

      if (face_found) {
        std::ostringstream text;
        text << "Found " << face_tracker.getNbObjects() << " face(s)";
        vpDisplay::displayText(I, 10, 10, text.str(), vpColor::red);
        for(size_t i=0; i < face_tracker.getNbObjects(); i++) {
          vpRect bbox = face_tracker.getBBox(i);
          if (i == 0)
            vpDisplay::displayRectangle(I, bbox, vpColor::red, false, 2);
          else
            vpDisplay::displayRectangle(I, bbox, vpColor::green, false, 1);
          vpDisplay::displayText(I, (int)bbox.getTop()-10, (int)bbox.getLeft(), face_tracker.getMessage(i) , vpColor::red);
        }

        led_proxy.post.fadeRGB("FaceLeds","blue",0.1);

        double u = face_tracker.getCog(0).get_u();
        double v = face_tracker.getCog(0).get_v();
        if (u<= g.getWidth() && v <= g.getHeight())
          head_cog_cur.set_uv(u,v);

        vpRect bbox = face_tracker.getBBox(0);
        std::string name = face_tracker.getMessage(0);

        //std::cout << "Loop time face print " << vpTime::measureTimeMs() - t << " ms" << std::endl;
      }

      AL::ALValue result = m_memProxy.getData("PeoplePerception/VisiblePeopleList");

      //std::cout << "Loop time get Data PeoplePerception " << vpTime::measureTimeMs() - t << " ms" << std::endl;

      person_found = false;
      if (result.getSize() > 0)
      {
        AL::ALValue info = m_memProxy.getData("PeoplePerception/PeopleDetected");
        int num_people = info[1].getSize();
        std::ostringstream text;
        text << "Found " << num_people << " person(s)";
        vpDisplay::displayText(I, 10, 10, text.str(), vpColor::red);

        person_found = true;

        if (face_found) // Try to find the match between two detection
        {
          vpImagePoint cog_face;
          double dist_min = 1000;
          unsigned int index_person = 0;

          for (unsigned int i = 0; i < num_people; i++)
          {

            float alpha =  info[1][i][2];
            float beta =  info[1][i][3];
            //Z = Zd;
            // Centre of face into the image
            float x =  g.getWidth()/2 -  g.getWidth() * beta;
            float y =  g.getHeight()/2  + g.getHeight() * alpha;
            cog_face.set_uv(x,y);
            dist = vpImagePoint::distance(cog_face, head_cog_cur);

            if (dist < dist_min)
            {
              dist_min = dist;
              best_cog_face_peoplep = cog_face;
              index_person  = i;
            }
          }

          vpDisplay::displayCross(I, best_cog_face_peoplep, 10, vpColor::white);

          if (dist_min < 55.)
          {
            Z = info[1][index_person][1]; // Current distance
          }

        }
        else // Take the first one on the list
        {
          float alpha =  info[1][0][2];
          float beta =  info[1][0][3];
          //Z = Zd;
          // Centre of face into the image
          float x =  g.getWidth()/2 -  g.getWidth() * beta;
          float y =  g.getHeight()/2  + g.getHeight() * alpha;
          head_cog_cur.set_uv(x,y);
          Z = info[1][0][1]; // Current distance

        }

      }
      else
      {
        std::cout << "No distance computed " << std::endl;
        stop_vxy = true;
        robot.getProxy()->setAngles("HipRoll",0.0,1.0);
        //Z = Zd;
      }
      //          float alpha =  info[1][0][2];
      //          float beta =  info[1][0][3];
      //          //Z = Zd;
      //          // Centre of face into the image
      //          float x =  g.getWidth()/2 -  g.getWidth() * beta;
      //          float y =  g.getHeight()/2  + g.getHeight() * alpha;

      //          vpImagePoint cog_face(y,x);
      //          dist = vpImagePoint::distance(cog_face,head_cog_cur);
      //          if (dist < 55.)
      //            Z = info[1][0][1]; // Current distance
      //          else
      //            stop_vxy = true;
      //        }
      //        else
      //        {
      //          std::cout << "No distance computed " << std::endl;
      //          stop_vxy = true;
      //          //Z = Zd;
      //        }


      //std::cout << "Loop time before VS: " << vpTime::measureTimeMs() - t << " ms" << std::endl;
      if (face_found || person_found )
      {
        // Get Head Jacobian (6x2)
        vpMatrix torso_eJe_head;
        robot.get_eJe("Head",torso_eJe_head);

        // Add column relative to the base rotation (Wz)
        //        vpColVector col_wz(6);
        //        col_wz[5] = 1;
        for (unsigned int i = 0; i < 6; i++)
          for (unsigned int j = 0; j < torso_eJe_head.getCols(); j++)
            tJe[i][j+3] = torso_eJe_head[i][j];

        // std::cout << "tJe" << std::endl << tJe << std::endl;

        //        vpHomogeneousMatrix torsoMHeadPith( robot.getProxy()->getTransform(jointNames_head[jointNames_head.size()-1], 0, true));// get transformation  matrix between torso and HeadRoll
        vpHomogeneousMatrix torsoMHeadPith( robot.getProxy()->getTransform("HeadPitch", 0, true));// get transformation  matrix between torso and HeadRoll

        vpVelocityTwistMatrix HeadPitchVLtorso(torsoMHeadPith.inverse());

        for(unsigned int i=0; i< 3; i++)
          for(unsigned int j=0; j< 3; j++)
            HeadPitchVLtorso[i][j+3] = 0;

        //std::cout << "HeadPitchVLtorso: " << std::endl << HeadPitchVLtorso << std::endl;
        // Transform the matrix
        eJe = HeadPitchVLtorso *tJe;

        // std::cout << "eJe" << std::endl << eJe << std::endl;

        task.set_eJe( eJe );
        task.set_cVe( vpVelocityTwistMatrix(eMc.inverse()) );

        vpDisplay::displayCross(I, head_cog_des, 10, vpColor::blue);
        vpDisplay::displayCross(I, head_cog_cur, 10, vpColor::green);
        //  std::cout << "head_cog_des:" << std::endl << head_cog_des << std::endl;
        //  std::cout << "head_cog_cur:" << std::endl << head_cog_cur << std::endl;

        // Update the current x feature
        double x,y;
        vpPixelMeterConversion::convertPoint(cam, head_cog_cur, x, y);
        s.buildFrom(x, y, Z);
        //s.set_xyZ(head_cog_cur.get_u(), head_cog_cur.get_v(), Z);

        // Update log(Z/Z*) feature. Since the depth Z change, we need to update the intection matrix
        s_Z.buildFrom(s.get_x(), s.get_y(), Z, log(Z/Zd)) ;

        q_dot = task.computeControlLaw(vpTime::measureTimeSecond() - servo_time_init);

        //std::cout << "Loop time compute VS: " << vpTime::measureTimeMs() - t << " ms" << std::endl;

        vpMatrix P = task.getI_WpW();
        double alpha = -3.3;
        double min = limit_yaw[0][0];
        double max = limit_yaw[0][1];

        vpColVector z_q2 (q_dot.size());
        vpColVector q_yaw = robot.getPosition(jointNames_head[0]);

        z_q2[3] = 2 * alpha * q_yaw[0]/ pow((max - min),2);

        vpColVector q3 = P * z_q2;
        //if (q3.euclideanNorm()<10.0)
        q_dot =  q_dot + q3;

        std::vector<float> vel(jointNames_head.size());
        vel[0] = q_dot[3];
        vel[1] = q_dot[4];

        // Compute the distance in pixel between the target and the center of the image
        double distance = vpImagePoint::distance(head_cog_cur, head_cog_des);
        //if (distance > 0.03*I.getWidth())
         std::cout << "q:" << std::endl << q_dot << std::endl;
        //  std::cout << "vel" << std::endl << q_dot << std::endl;
        //std::cout << "distance" << std::endl << distance <<" -- " << 0.03*I.getWidth() << " ++ " << I.getWidth() << std::endl;

        //        if (distance > 0.1*I.getWidth())
        robot.setVelocity(jointNames_head,vel);
        //        else
        //        {
        //          std::cout << "Setting hipRoll to zero" << std::endl;
        //          robot.getProxy()->setAngles("HipRoll",0.0,1.0);
        //        }
        // std::cout << "errorZ: " << task.getError()[2] << std::endl;
        // std::cout << "stop_vxy: " << stop_vxy << std::endl;

        if (std::fabs(Z -Zd) < 0.05 || stop_vxy || !move_base)
          robot.setBaseVelocity(0.0, 0.0, q_dot[2]);
        else
          robot.setBaseVelocity(q_dot[0], q_dot[1], q_dot[2]);

        if (opt_debug)
        {
          vpColVector vel_head = robot.getJointVelocity(jointNames_head);
          for (unsigned int i=0 ; i < jointNames_head.size() ; i++) {
            plotter_diff_vel->plot(i, 1, loop_iter, q_dot[i+3]);
            plotter_diff_vel->plot(i, 0, loop_iter, vel_head[i]);
          }
          plotter_error->plot(0,loop_iter,task.getError());
          plotter_vel->plot(0,loop_iter, q3);
          plotter_distance->plot(0,0,loop_iter,Z);
        }

        //        if (detection_phase)
        //        {

        //          //if (score >= 0.4 && distance < 0.06*I.getWidth() && bbox.getSize() > 3000)
        //          if (distance < 0.06*I.getWidth() && bbox.getSize() > 3000)
        //          {
        //            if (opt_debug)
        //            {
        //              vpDisplay::displayRectangle(I, bbox, vpColor::red, false, 1);
        //              vpDisplay::displayText(I, (int)bbox.getTop()-10, (int)bbox.getLeft(), name, vpColor::red);
        //            }
        //            detected_face_map[name]++;
        //            f_count++;
        //          }
        //          else
        //          {
        //            if (opt_debug)
        //            {
        //              vpDisplay::displayRectangle(I, bbox, vpColor::green, false, 1);
        //              vpDisplay::displayText(I, (int)bbox.getTop()-10, (int)bbox.getLeft(), name, vpColor::green);
        //            }
        //          }
        //          if (f_count>10)
        //          {
        //            detection_phase = false;
        //            f_count = 0;
        //          }
        //        }
        //        else
        //        {
        //          std::string recognized_person_name = std::max_element(detected_face_map.begin(), detected_face_map.end(), pred)->first;
        //          unsigned int times = std::max_element(detected_face_map.begin(), detected_face_map.end(), pred)->second;

        //          if (!in_array(recognized_person_name, recognized_names) && recognized_person_name != "Unknown") {

        //            if (opt_language_english)
        //            {
        //              phraseToSay = "\\emph=2\\ Hi \\wait=200\\ \\emph=2\\" + recognized_person_name + "\\pau=200\\ How are you ?";
        //            }
        //            else
        //            {
        //              phraseToSay = "\\emph=2\\ Salut \\wait=200\\ \\emph=2\\" + recognized_person_name + "\\pau=200\\ comment vas  tu ?";;
        //            }

        //            std::cout << phraseToSay << std::endl;
        //            tts.post.say(phraseToSay);
        //            recognized_names.push_back(recognized_person_name);
        //          }
        //          if (!in_array(recognized_person_name, recognized_names) && recognized_person_name == "Unknown"
        //              && times > 15)
        //          {

        //            if (opt_language_english)
        //            {
        //              phraseToSay = "\\emph=2\\ Hi \\wait=200\\ \\emph=2\\. I don't know you! \\emph=2\\ What's your name?";
        //            }
        //            else
        //            {
        //              phraseToSay = " \\emph=2\\ Salut \\wait=200\\ \\emph=2\\. Je ne te connais pas! \\emph=2\\  Comment t'appelles-tu ?";
        //            }

        //            std::cout << phraseToSay << std::endl;
        //            tts.post.say(phraseToSay);
        //            recognized_names.push_back(recognized_person_name);
        //          }

        //          detection_phase = true;
        //          detected_face_map.clear();
        //        }
      }
      else {
        robot.stop(jointNames_head);
        robot.stopBase();
        std::cout << "Stop!" << std::endl;
        reinit_servo = true;
      }

      //if (opt_debug)
      vpDisplay::flush(I);
      if (vpDisplay::getClick(I, false))
        break;
      loop_iter ++;
      std::cout << "Loop time: " << vpTime::measureTimeMs() - t << " ms" << std::endl;
    }
    robot.stop(jointNames_head);
    robot.stopBase();

    tts.setLanguage("French");
  //  tts.exit();

    people_proxy.unsubscribe("People");
   // people_proxy.exit();

    asr.unsubscribe("Test_ASR");
    asr.setVisualExpression(true);

  //  asr.exit();

    tts.setLanguage("French");
 //   tts.exit();

    led_proxy.fadeRGB("FaceLeds","white",0.1);
  //  led_proxy.exit();

    vpDisplay::getClick(I, true);

  }
  catch(vpException &e) {
    std::cout << e.getMessage() << std::endl;
  }
  catch (const AL::ALError& e)
  {
    std::cerr << "Caught exception " << e.what() << std::endl;
  }

  std::cout << "The end: stop the robot..." << std::endl;

  //tts.setLanguage("French");
  //  tts.exit();


  robot.stop(jointNames_head);
  robot.stopBase();

  return 0;
}
Exemple #28
0
int main(int argc, char ** argv) {
  // Step 0: Set up some variables
  unsigned int numExperiments = 10;  // Number of experiments
  unsigned int numUncertainVars = 5;  // Number of things to calibrate
  unsigned int numSimulations = 50;  // Number of simulations
  unsigned int numConfigVars = 1;  // Dimension of configuration space
  unsigned int numEta = 1;  // Number of responses the model is returning
  unsigned int experimentSize = 1;  // Size of each experiment

#ifdef QUESO_HAS_MPI
  MPI_Init(&argc, &argv);

  // Step 1: Set up QUESO environment
  QUESO::FullEnvironment env(MPI_COMM_WORLD, argv[1], "", NULL);
#else
  QUESO::FullEnvironment env(argv[1], "", NULL);
#endif

  // Step 2: Set up prior for calibration parameters
  QUESO::VectorSpace<QUESO::GslVector, QUESO::GslMatrix> paramSpace(env,
      "param_", numUncertainVars, NULL);

  // Parameter (theta) bounds:
  //   descriptors   'k_tmasl' 'k_xkle' 'k_xkwew' 'k_xkwlx' 'k_cd'
  //   upper_bounds   1.05      1.1      1.1       1.1       1.1
  //   lower_bounds   0.95      0.9      0.9       0.9       0.9
  //
  // These bounds are dealt with when reading in the data
  QUESO::GslVector paramMins(paramSpace.zeroVector());
  QUESO::GslVector paramMaxs(paramSpace.zeroVector());

  paramMins.cwSet(0.0);
  paramMaxs.cwSet(1.0);

  QUESO::BoxSubset<QUESO::GslVector, QUESO::GslMatrix> paramDomain("param_",
      paramSpace, paramMins, paramMaxs);

  QUESO::UniformVectorRV<QUESO::GslVector, QUESO::GslMatrix> priorRv("prior_",
      paramDomain);

  // Step 3: Instantiate the 'scenario' and 'output' spaces for simulation
  QUESO::VectorSpace<QUESO::GslVector, QUESO::GslMatrix> configSpace(env,
      "scenario_", numConfigVars, NULL);

  QUESO::VectorSpace<QUESO::GslVector, QUESO::GslMatrix> nEtaSpace(env,
      "output_", numEta, NULL);

  // Step 4: Instantiate the 'output' space for the experiments
  QUESO::VectorSpace<QUESO::GslVector, QUESO::GslMatrix> experimentSpace(env,
      "experimentspace_", experimentSize, NULL);

  QUESO::VectorSpace<QUESO::GslVector, QUESO::GslMatrix> totalExperimentSpace(env,
      "experimentspace_", experimentSize * numExperiments, NULL);

  // Step 5: Instantiate the Gaussian process emulator object
  //
  // Regarding simulation scenario input values, the user should standardise
  // them so that they exist inside a hypercube.
  //
  // Regarding simulation output data, the user should transform it so that the
  // mean is zero and the variance is one.
  //
  // Regarding experimental scenario input values, the user should standardize
  // them so that they exist inside a hypercube.
  //
  // Regarding experimental data, the user should transformed it so that it has
  // zero mean and variance one.

  // GPMSA stores all the information about our simulation
  // data and experimental data.  It also stores default information about the
  // hyperparameter distributions.
  QUESO::GPMSAFactory<QUESO::GslVector, QUESO::GslMatrix>
    gpmsaFactory(env,
                 NULL,
                 priorRv,
                 configSpace,
                 paramSpace,
                 nEtaSpace,
                 experimentSpace,
                 numSimulations,
                 numExperiments);

  // std::vector containing all the points in scenario space where we have
  // simulations
  std::vector<QUESO::GslVector *> simulationScenarios(numSimulations,
      (QUESO::GslVector *) NULL);

  // std::vector containing all the points in parameter space where we have
  // simulations
  std::vector<QUESO::GslVector *> paramVecs(numSimulations,
      (QUESO::GslVector *) NULL);

  // std::vector containing all the simulation output data
  std::vector<QUESO::GslVector *> outputVecs(numSimulations,
      (QUESO::GslVector *) NULL);

  // std::vector containing all the points in scenario space where we have
  // experiments
  std::vector<QUESO::GslVector *> experimentScenarios(numExperiments,
      (QUESO::GslVector *) NULL);

  // std::vector containing all the experimental output data
  std::vector<QUESO::GslVector *> experimentVecs(numExperiments,
      (QUESO::GslVector *) NULL);

  // The experimental output data observation error covariance matrix
  QUESO::GslMatrix experimentMat(totalExperimentSpace.zeroVector());

  // Instantiate each of the simulation points/outputs
  for (unsigned int i = 0; i < numSimulations; i++) {
    simulationScenarios[i] = new QUESO::GslVector(configSpace.zeroVector());  // 'x_{i+1}^*' in paper
    paramVecs          [i] = new QUESO::GslVector(paramSpace.zeroVector());  // 't_{i+1}^*' in paper
    outputVecs         [i] = new QUESO::GslVector(nEtaSpace.zeroVector());  // 'eta_{i+1}' in paper
  }

  for (unsigned int i = 0; i < numExperiments; i++) {
    experimentScenarios[i] = new QUESO::GslVector(configSpace.zeroVector()); // 'x_{i+1}' in paper
    experimentVecs[i] = new QUESO::GslVector(experimentSpace.zeroVector());
  }

  // Read in data and store the standard deviation of the simulation data.  We
  // will need the standard deviation when we pass the experiment error
  // covariance matrix to QUESO.
  double stdsim = readData(simulationScenarios,
                           paramVecs,
                           outputVecs,
                           experimentScenarios,
                           experimentVecs);

  for (unsigned int i = 0; i < numExperiments; i++) {
    // Passing in error of experiments (standardised).
    experimentMat(i, i) = (0.025 / stdsim) * (0.025 / stdsim);
  }

  // Add simulation and experimental data
  gpmsaFactory.addSimulations(simulationScenarios, paramVecs, outputVecs);
  gpmsaFactory.addExperiments(experimentScenarios, experimentVecs, &experimentMat);

  QUESO::GenericVectorRV<QUESO::GslVector, QUESO::GslMatrix> postRv(
      "post_",
      gpmsaFactory.prior().imageSet().vectorSpace());
  QUESO::StatisticalInverseProblem<QUESO::GslVector, QUESO::GslMatrix> ip("",
      NULL, gpmsaFactory, postRv);

  QUESO::GslVector paramInitials(
      gpmsaFactory.prior().imageSet().vectorSpace().zeroVector());

  // Initial condition of the chain
  // Have to set each of these by hand, *and* the sampler is sensitive to these
  // values
  paramInitials[0] = 0.5; // param 1
  paramInitials[1] = 0.5; // param 2
  paramInitials[2] = 0.5; // param 3
  paramInitials[3] = 0.5; // param 4
  paramInitials[4] = 0.5; // param 5
  paramInitials[5]  = 0.4;  // not used.  emulator mean
  paramInitials[6]  = 0.4; // emulator precision
  paramInitials[7]  = 0.97; // emulator corr str
  paramInitials[8]  = 0.97; // emulator corr str
  paramInitials[9]  = 0.97; // emulator corr str
  paramInitials[10]  = 0.97; // emulator corr str
  paramInitials[11]  = 0.20; // emulator corr str
  paramInitials[12]  = 0.80; // emulator corr str
  paramInitials[13]  = 10.0; // discrepancy precision
  paramInitials[14]  = 0.97; // discrepancy corr str
  paramInitials[15]  = 8000.0; // emulator data precision

  QUESO::GslMatrix proposalCovMatrix(
      gpmsaFactory.prior().imageSet().vectorSpace().zeroVector());

  // Setting the proposal covariance matrix by hand.  This requires great
  // forethough, and can generally be referred to as a massive hack.  These
  // values were taken from the gpmsa matlab code and fiddled with.
  double scale = 600.0;
  proposalCovMatrix(0, 0)   = 3.1646 / 10.0;  // param 1
  proposalCovMatrix(1, 1)   = 3.1341 / 10.0;  // param 2
  proposalCovMatrix(2, 2)   = 3.1508 / 10.0;  // param 3
  proposalCovMatrix(3, 3)   = 0.3757 / 10.0;  // param 4
  proposalCovMatrix(4, 4)   = 0.6719 / 10.0;  // param 5
  proposalCovMatrix(5, 5)   = 0.1 / scale;  // not used.  emulator mean
  proposalCovMatrix(6, 6)   = 0.4953 / scale;  // emulator precision
  proposalCovMatrix(7, 7)   = 0.6058 / scale;  // emulator corr str
  proposalCovMatrix(8, 8)   = 7.6032e-04 / scale;  // emulator corr str
  proposalCovMatrix(9, 9)   = 8.3815e-04 / scale;  // emulator corr str
  proposalCovMatrix(10, 10) = 7.5412e-04 / scale;  // emulator corr str
  proposalCovMatrix(11, 11) = 0.2682 / scale;  // emulator corr str
  proposalCovMatrix(12, 12) = 0.0572 / scale;  // emulator corr str
  proposalCovMatrix(13, 13) = 1.3417 / scale;  // discrepancy precision
  proposalCovMatrix(14, 14) = 0.3461 / scale;  // discrepancy corr str
  proposalCovMatrix(15, 15) = 495.3 / scale;  // emulator data precision

  // Square to get variances
  for (unsigned int i = 0; i < 16; i++) {
    proposalCovMatrix(i, i) = proposalCovMatrix(i, i) * proposalCovMatrix(i, i);
  }

  ip.solveWithBayesMetropolisHastings(NULL, paramInitials, &proposalCovMatrix);

#ifdef QUESO_HAS_MPI
  MPI_Finalize();
#endif

  return 0;
}
Vector Rosen34(
	Fun           &F , 
	size_t         M , 
	const Scalar &ti , 
	const Scalar &tf , 
	const Vector &xi ,
	Vector       &e )
{
	CPPAD_ASSERT_FIRST_CALL_NOT_PARALLEL;

	// check numeric type specifications
	CheckNumericType<Scalar>();

	// check simple vector class specifications
	CheckSimpleVector<Scalar, Vector>();

	// Parameters for Shampine's Rosenbrock method
	// are static to avoid recalculation on each call and 
	// do not use Vector to avoid possible memory leak
	static Scalar a[3] = {
		Scalar(0),
		Scalar(1),
		Scalar(3)   / Scalar(5)
	};
	static Scalar b[2 * 2] = {
		Scalar(1),
		Scalar(0),
		Scalar(24)  / Scalar(25),
		Scalar(3)   / Scalar(25)
	};
	static Scalar ct[4] = {
		Scalar(1)   / Scalar(2),
		- Scalar(3) / Scalar(2),
		Scalar(121) / Scalar(50),
		Scalar(29)  / Scalar(250)
	};
	static Scalar cg[3 * 3] = {
		- Scalar(4),
		Scalar(0),
		Scalar(0),
		Scalar(186) / Scalar(25),
		Scalar(6)   / Scalar(5),
		Scalar(0),
		- Scalar(56) / Scalar(125),
		- Scalar(27) / Scalar(125),
		- Scalar(1)  / Scalar(5)
	};
	static Scalar d3[3] = {
		Scalar(97) / Scalar(108),
		Scalar(11) / Scalar(72),
		Scalar(25) / Scalar(216)
	};
	static Scalar d4[4] = {
		Scalar(19)  / Scalar(18),
		Scalar(1)   / Scalar(4),
		Scalar(25)  / Scalar(216),
		Scalar(125) / Scalar(216)
	};
	CPPAD_ASSERT_KNOWN(
		M >= 1,
		"Error in Rosen34: the number of steps is less than one"
	);
	CPPAD_ASSERT_KNOWN(
		e.size() == xi.size(),
		"Error in Rosen34: size of e not equal to size of xi"
	);
	size_t i, j, k, l, m;             // indices

	size_t  n    = xi.size();         // number of components in X(t)
	Scalar  ns   = Scalar(double(M)); // number of steps as Scalar object
	Scalar  h    = (tf - ti) / ns;    // step size 
	Scalar  zero = Scalar(0);         // some constants
	Scalar  one  = Scalar(1);
	Scalar  two  = Scalar(2);

	// permutation vectors needed for LU factorization routine
	CppAD::vector<size_t> ip(n), jp(n);

	// vectors used to store values returned by F
	Vector E(n * n), Eg(n), f_t(n);
	Vector g(n * 3), x3(n), x4(n), xf(n), ftmp(n), xtmp(n), nan_vec(n);

	// initialize e = 0, nan_vec = nan
	for(i = 0; i < n; i++)
	{	e[i]       = zero;
		nan_vec[i] = nan(zero);
	}

	xf = xi;           // initialize solution
	for(m = 0; m < M; m++)
	{	// time at beginning of this interval
		Scalar t = ti * (Scalar(int(M - m)) / ns) 
		         + tf * (Scalar(int(m)) / ns);

		// value of x at beginning of this interval
		x3 = x4 = xf;

		// evaluate partial derivatives at beginning of this interval
		F.Ode_ind(t, xf, f_t);
		F.Ode_dep(t, xf, E);    // E = f_x
		if( hasnan(f_t) || hasnan(E) )
		{	e = nan_vec;
			return nan_vec;
		}

		// E = I - f_x * h / 2
		for(i = 0; i < n; i++)
		{	for(j = 0; j < n; j++)
				E[i * n + j] = - E[i * n + j] * h / two;
			E[i * n + i] += one;
		}

		// LU factor the matrix E
# ifndef NDEBUG
		int sign = LuFactor(ip, jp, E);
# else
		LuFactor(ip, jp, E);
# endif
		CPPAD_ASSERT_KNOWN(
			sign != 0,
			"Error in Rosen34: I - f_x * h / 2 not invertible"
		);

		// loop over integration steps
		for(k = 0; k < 3; k++)
		{	// set location for next function evaluation
			xtmp = xf; 
			for(l = 0; l < k; l++)
			{	// loop over previous function evaluations
				Scalar bkl = b[(k-1)*2 + l];
				for(i = 0; i < n; i++)
				{	// loop over elements of x
					xtmp[i] += bkl * g[i*3 + l] * h;
				}
			}
			// ftmp = F(t + a[k] * h, xtmp)
			F.Ode(t + a[k] * h, xtmp, ftmp); 
			if( hasnan(ftmp) )
			{	e = nan_vec;
				return nan_vec;
			}

			// Form Eg for this integration step
			for(i = 0; i < n; i++)
				Eg[i] = ftmp[i] + ct[k] * f_t[i] * h;
			for(l = 0; l < k; l++)
			{	for(i = 0; i < n; i++)
					Eg[i] += cg[(k-1)*3 + l] * g[i*3 + l];
			}

			// Solve the equation E * g = Eg
			LuInvert(ip, jp, E, Eg);

			// save solution and advance x3, x4
			for(i = 0; i < n; i++)
			{	g[i*3 + k]  = Eg[i];
				x3[i]      += h * d3[k] * Eg[i];
				x4[i]      += h * d4[k] * Eg[i];
			}
		}
		// Form Eg for last update to x4 only
		for(i = 0; i < n; i++)
			Eg[i] = ftmp[i] + ct[3] * f_t[i] * h;
		for(l = 0; l < 3; l++)
		{	for(i = 0; i < n; i++)
				Eg[i] += cg[2*3 + l] * g[i*3 + l];
		}

		// Solve the equation E * g = Eg
		LuInvert(ip, jp, E, Eg);

		// advance x4 and accumulate error bound
		for(i = 0; i < n; i++)
		{	x4[i] += h * d4[3] * Eg[i];

			// cant use abs because cppad.hpp may not be included
			Scalar diff = x4[i] - x3[i];
			if( diff < zero )
				e[i] -= diff;
			else	e[i] += diff;
		}

		// advance xf for this step using x4
		xf = x4;
	}
	return xf;
}
Exemple #30
0
// {{{ critSectionRequest
// Ask for critical section
int critSectionRequest() {
	msg_t msg;
	type(msg) = REQUEST;
	char *ip_tmp = inet_ntoa(this_site.neighbours[0].sin_addr);
	strncpy (ask(msg), ip_tmp, IPLONG);

	// If already in SC just return -1
	if (state != IDLE) {
		if (_verbose) fprintf (stderr,"Already in critical section.\n");
		return -1;
	}
	state = WAITING;

	// Owns Token ?
	if(tokenPresent) 
		return takeCriticalSection();
	// Sends request to last
	else if(last != -1) {
		// Get ip of last
		char *tmpter = getIPstrFromNb (last);
		if (_verbose) fprintf (stdout, "Last is %d, sending it a request.\n", last);
		strncpy(ip(msg), tmpter, IPLONG);
		free (tmpter);

		strncpy (ask(msg), inet_ntoa(this_site.neighbours[0].sin_addr), IPLONG);

		if(sendMessageWithAdd(msg) == -1){
			if (_verbose) fprintf (stderr, "======> Sending request failure... <======\n");
			return -1;
		}

		// Arms a timer
		time_t timeStart, timeCur;
		int flags = fcntl(this_site.sdRecv, F_GETFL);
		int flags2 = flags | O_NONBLOCK;
		fcntl(this_site.sdRecv, F_SETFL, flags2);

		timeStart = time(&timeStart);
		timeCur = time(&timeCur);

		// Wait for an answer
		if (_verbose) fprintf (stdout, "Waiting for COMMIT.\n");
		while(timeCur - timeStart < (2*TMESG)) {
			timeCur = time(&timeCur);

			if(recvMessage(&msg, NULL) == -1) 
				continue;

			if (type(msg) == COMMIT)
				return handleCommit(msg);
			else {
				// TODO: Work on reaction when receiving request while waiting commit
				if (type(msg) == REQUEST) {
					if (_verbose) fprintf (stdout, "Received a REQUEST instead of a COMMIT.\n");
				}
				else if (type(msg) == TOKEN){
					last = -1;
					return handleToken(msg);
				}
				else
					handleMessage(msg);
			}
		}

		// If receive no answers
		if (_verbose) fprintf (stdout, "I didn't get any COMMIT, looking for the queue...\n");
		fcntl(this_site.sdRecv, F_SETFL, flags);

		memset (&msg, 0, SIZE);
		type(msg) = SEARCH_QUEUE;
		nb_acc(msg) = acces;
		// Broadcast Search_Queue
		if (_verbose) fprintf (stdout, "Broadcasting SEARCH_QUEUE.\n");
		broadcast(msg);

		msg_t max;
		pos(max) = -1;

		fcntl(this_site.sdRecv, F_SETFL, flags2);

		timeStart = time(&timeStart);
		timeCur = time(&timeCur);

		// Wait for ACK_SEARCH_QUEUE
		if (_verbose) fprintf (stdout, "Expecting ACK_SEARCH_QUEUE.\n");
		while(timeCur - timeStart < 2*TMESG) {
			timeCur = time(&timeCur);
			memset (&msg, 0, SIZE);

			if(recvMessage(&msg, NULL) == -1) 
				continue;

			switch(type(msg)) {
				case ACK_SEARCH_QUEUE:
					if (_verbose) fprintf (stdout, "Got an ACK_SEARCH_QUEUE.\n");
					// Save greatest position in the queue
					if (pos(msg) > pos(max)) {
						if (_verbose) fprintf (stdout, "New position in QUEUE : %d.\n", pos(msg));
						memcpy(&max, &msg, SIZE);
					}
					break;
					// Another site discovers the failure
				case SEARCH_QUEUE:
					if (isMe(ips(msg)))
						continue;
					if (_verbose) fprintf (stdout, "Another site discovered the failure.\n");
					// The other site hasn't priority, just continue
					if (nb_acc(msg) > acces) 
						continue;
					// Both of sites have the same numbers of access, defines priority with ip
					if (nb_acc(msg) == acces) {
						char *ip_pers = inet_ntoa(this_site.neighbours[0].sin_addr);
						if (strcmp(ip_pers, ips(msg)) <= 0)
							continue;
					}
					// If the other has the priority, just change last and ask it for CS
					unsigned long int ipa = (unsigned long int) inet_addr(ips(msg));
					last = getNeighbour(ipa);
					if (_verbose) fprintf (stdout, "I don't have priority, changing my last (%d) and asking for CS again.\n", last);
					state = IDLE;
					return critSectionRequest();
					// TODO: Request processing
				case REQUEST:
					break;
				default:
					handleMessage(msg);
					break;
			}
		}

		if (_verbose) fprintf (stdout, "Waiting time passed.\n");

		fcntl(this_site.sdRecv, F_SETFL, flags);

		// No other site in the queue, juste regenerate TOKEN
		if (pos(max) < 0) {
			if (_verbose) fprintf (stdout, "No other sites in the queue, regenerate TOKEN (last = next)\n");
			last = next;
			tokenPresent = 1;
			takeCriticalSection();
		}
		else {
			// Ask for connection to the site with the highest position
			unsigned long int ipa = (unsigned long int) inet_addr(ips(max));
			last = getNeighbour(ipa);
			if (_verbose) fprintf (stdout, "Ask for Connection, new last = %d\n", last);
			strncpy(ip(msg), ips(max),IPLONG *sizeof(char));
			if (next(max)) {
				type(msg) = CONNECTION;
				if (sendMessageWithAdd(msg) == -1)
					return -1;
			}
			state = IDLE;
			critSectionRequest();
		}
	}
	else
		if (_verbose) fprintf (stderr, "======> Last = -1, but don't have TOKEN <======\n");
	return 0;
}