C++ vector::size方法代码示例

inline void save(
    Archive & ar,
    const STD::vector<bool, Allocator> &t,
    const unsigned int /* file_version */
){
    // record number of elements
    unsigned int count = t.size();
    ar << BOOST_SERIALIZATION_NVP(count);
    STD::vector<bool>::const_iterator it = t.begin();
    while(count-- > 0){
        bool tb = *it++;
        ar << boost::serialization::make_nvp("item", tb);
    }
}

void Trainee::train(std::vector<std::pair<InputType, AnswerType>> minibatch, float learning_rate)
{
    Eigen::MatrixXf dweight3 = Eigen::MatrixXf::Zero(n_outputvec, n_hid2vec);
    Eigen::VectorXf dbias3 = Eigen::VectorXf::Zero(n_outputvec);
    Eigen::MatrixXf dweight2 = Eigen::MatrixXf::Zero(n_hid2vec, n_hid1vec);
    Eigen::VectorXf dbias2 = Eigen::VectorXf::Zero(n_hid2vec);
    Eigen::MatrixXf dweight1 = Eigen::MatrixXf::Zero(n_hid1vec, n_inputvec);
    Eigen::VectorXf dbias1 = Eigen::VectorXf::Zero(n_hid1vec);

    /* For AdaGrad */
    auto fn = [](float lhs, float rhs) -> float { return lhs != 0.0 ? lhs / rhs : 0.0; };

    for(auto sample: minibatch){
        Eigen::VectorXf inputvec = input2vec(sample.first);
        Eigen::VectorXf z1 = feedforward(inputvec, 1);
        Eigen::VectorXf z2 = feedforward(inputvec, 2);  // 後付けとはいえ。この計算、あからさまに無駄だな。z1からz2を計算すべき。

        // Calculate delta of output layer.
        Eigen::VectorXf delta3;
        delta3 = feedforward(inputvec, 3);
        delta3(sample.second) -= 1.0f;
        {
            Eigen::ArrayXXf e = delta3 * z2.transpose();
            gsq_w3 += e * e;
            gsq_b3 += delta3.array() * delta3.array();
            dweight3 += e.matrix();
            dbias3 += delta3;
        }

        // Calculate delta of 2nd hidden layer.
        Eigen::VectorXf delta2 = Eigen::VectorXf::Zero(n_hid2vec);
        for(int j=0;j<n_hid2vec;j++){
            for(int k=0;k<n_outputvec;k++) delta2(j) += delta3(k) * weight3(k, j) * (z2(j) >= 0.f ? 1.f : 0.f);
        }
        {
            Eigen::ArrayXXf e = delta2 * z1.transpose();
            gsq_w2 += e * e;
            gsq_b2 += delta2.array() * delta2.array();
            dweight2 += e.matrix();
            dbias2 += delta2;
        }

        // Calculate delta of 1st hidden layer.
        Eigen::VectorXf delta1 = Eigen::VectorXf::Zero(n_hid1vec);
        for(int j=0;j<n_hid1vec;j++){
            for(int k=0;k<n_hid2vec;k++) delta1(j) += delta2(k) * weight2(k, j) * (z1(j) >= 0.f ? 1.f : 0.f);
        }
        {
            Eigen::ArrayXXf e = delta1 * inputvec.transpose();
            gsq_w1 += e * e;
            gsq_b1 += delta1.array() * delta1.array();
            dweight1 += e.matrix();
            dbias1 += delta1;
        }
    }
    weight1 -= dweight1.binaryExpr(gsq_w1.sqrt().matrix(), fn) * learning_rate / minibatch.size();
    bias1 -= dbias1.binaryExpr(gsq_b1.sqrt().matrix(), fn) * learning_rate / minibatch.size();
    weight2 -= dweight2.binaryExpr(gsq_w2.sqrt().matrix(), fn) * learning_rate / minibatch.size();
    bias2 -= dbias2.binaryExpr(gsq_b2.sqrt().matrix(), fn) * learning_rate / minibatch.size();
    weight3 -= dweight3.binaryExpr(gsq_w3.sqrt().matrix(), fn) * learning_rate / minibatch.size();
    bias3 -= dbias3.binaryExpr(gsq_b3.sqrt().matrix(), fn) * learning_rate / minibatch.size();
}

 discrete_domain(std::vector<std::string> Names) : Nmax(Names.size()), _names(std::move(Names)) { init_inv(); }

    /*
     * Parse client-first-message of the form:
     * n,a=authzid,n=encoded-username,r=client-nonce
     *
     * Generate server-first-message on the form:
     * r=client-nonce|server-nonce,s=user-salt,i=iteration-count
     *
     * NOTE: we are ignoring the authorization ID part of the message
     */
    StatusWith<bool> SaslSCRAMSHA1ServerConversation::_firstStep(std::vector<string>& input,
                                                                 std::string* outputData) {
        std::string authzId = "";

        if (input.size() == 4) {
            /* The second entry a=authzid is optional. If provided it will be
             * validated against the encoded username.
             *
             * The two allowed input forms are:
             * n,,n=encoded-username,r=client-nonce
             * n,a=authzid,n=encoded-username,r=client-nonce
             */
            if (!str::startsWith(input[1], "a=") || input[1].size() < 3) {
                return StatusWith<bool>(ErrorCodes::BadValue, mongoutils::str::stream() <<
                    "Incorrect SCRAM-SHA-1 authzid: " << input[1]);
            }
            authzId = input[1].substr(2);
            input.erase(input.begin() + 1);
        }

        if (input.size() != 3) {
            return StatusWith<bool>(ErrorCodes::BadValue, mongoutils::str::stream() <<
                "Incorrect number of arguments for first SCRAM-SHA-1 client message, got " <<
                input.size() << " expected 4");
        }
        else if (input[0] != "n") {
            return StatusWith<bool>(ErrorCodes::BadValue, mongoutils::str::stream() <<
                "Incorrect SCRAM-SHA-1 client message prefix: " << input[0]);
        }
        else if (!str::startsWith(input[1], "n=") || input[1].size() < 3) {
            return StatusWith<bool>(ErrorCodes::BadValue, mongoutils::str::stream() <<
                "Incorrect SCRAM-SHA-1 user name: " << input[1]);
        }
        else if(!str::startsWith(input[2], "r=") || input[2].size() < 6) {
            return StatusWith<bool>(ErrorCodes::BadValue, mongoutils::str::stream() <<
                "Incorrect SCRAM-SHA-1 client nonce: " << input[2]);
        }

        _user = input[1].substr(2);
        if (!authzId.empty() && _user != authzId) {
            return StatusWith<bool>(ErrorCodes::BadValue, mongoutils::str::stream() <<
                "SCRAM-SHA-1 user name " << _user << " does not match authzid " << authzId);
        }

        decodeSCRAMUsername(_user);

        // SERVER-16534, SCRAM-SHA-1 must be enabled for authenticating the internal user, so that
        // cluster members may communicate with each other. Hence ignore disabled auth mechanism
        // for the internal user.
        UserName user(_user, _saslAuthSession->getAuthenticationDatabase());
        if (!sequenceContains(saslGlobalParams.authenticationMechanisms, "SCRAM-SHA-1") &&
            user != internalSecurity.user->getName()) {
            return StatusWith<bool>(ErrorCodes::BadValue,
                                    "SCRAM-SHA-1 authentication is disabled");
        }

        // add client-first-message-bare to _authMessage
        _authMessage += input[1] + "," + input[2] + ",";

        std::string clientNonce = input[2].substr(2);

        // The authentication database is also the source database for the user.
        User* userObj;
        Status status = _saslAuthSession->getAuthorizationSession()->getAuthorizationManager().
                acquireUser(_saslAuthSession->getOpCtxt(),
                            user,
                            &userObj);

        if (!status.isOK()) {
            return StatusWith<bool>(status);
        }

        _creds = userObj->getCredentials();
        UserName userName = userObj->getName();

        _saslAuthSession->getAuthorizationSession()->getAuthorizationManager().
                releaseUser(userObj);

        // Check for authentication attempts of the __system user on
        // systems started without a keyfile.
        if (userName == internalSecurity.user->getName() &&
            _creds.scram.salt.empty()) {
            return StatusWith<bool>(ErrorCodes::AuthenticationFailed,
                                    "It is not possible to authenticate as the __system user "
                                    "on servers started without a --keyFile parameter");
        }

        // Generate SCRAM credentials on the fly for mixed MONGODB-CR/SCRAM mode.
        if (_creds.scram.salt.empty() && !_creds.password.empty()) {
            // Use a default value of 5000 for the scramIterationCount when in mixed mode,
            // overriding the default value (10000) used for SCRAM mode or the user-given value.
//.........这里部分代码省略.........

bool Inliner::runOnSCC(const std::vector<CallGraphNode*> &SCC) {
  CallGraph &CG = getAnalysis<CallGraph>();

  std::set<Function*> SCCFunctions;
  DOUT << "Inliner visiting SCC:";
  for (unsigned i = 0, e = SCC.size(); i != e; ++i) {
    Function *F = SCC[i]->getFunction();
    if (F) SCCFunctions.insert(F);
    DOUT << " " << (F ? F->getName() : "INDIRECTNODE");
  }

  // Scan through and identify all call sites ahead of time so that we only
  // inline call sites in the original functions, not call sites that result
  // from inlining other functions.
  std::vector<CallSite> CallSites;

  for (unsigned i = 0, e = SCC.size(); i != e; ++i)
    if (Function *F = SCC[i]->getFunction())
      for (Function::iterator BB = F->begin(), E = F->end(); BB != E; ++BB)
        for (BasicBlock::iterator I = BB->begin(); I != BB->end(); ++I) {
          CallSite CS = CallSite::get(I);
          if (CS.getInstruction() && (!CS.getCalledFunction() ||
                                      !CS.getCalledFunction()->isDeclaration()))
            CallSites.push_back(CS);
        }

  DOUT << ": " << CallSites.size() << " call sites.\n";

  // Now that we have all of the call sites, move the ones to functions in the
  // current SCC to the end of the list.
  unsigned FirstCallInSCC = CallSites.size();
  for (unsigned i = 0; i < FirstCallInSCC; ++i)
    if (Function *F = CallSites[i].getCalledFunction())
      if (SCCFunctions.count(F))
        std::swap(CallSites[i--], CallSites[--FirstCallInSCC]);

  // Now that we have all of the call sites, loop over them and inline them if
  // it looks profitable to do so.
  bool Changed = false;
  bool LocalChange;
  do {
    LocalChange = false;
    // Iterate over the outer loop because inlining functions can cause indirect
    // calls to become direct calls.
    for (unsigned CSi = 0; CSi != CallSites.size(); ++CSi)
      if (Function *Callee = CallSites[CSi].getCalledFunction()) {
        // Calls to external functions are never inlinable.
        if (Callee->isDeclaration() ||
            CallSites[CSi].getInstruction()->getParent()->getParent() ==Callee){
          if (SCC.size() == 1) {
            std::swap(CallSites[CSi], CallSites.back());
            CallSites.pop_back();
          } else {
            // Keep the 'in SCC / not in SCC' boundary correct.
            CallSites.erase(CallSites.begin()+CSi);
          }
          --CSi;
          continue;
        }

        // If the policy determines that we should inline this function,
        // try to do so.
        CallSite CS = CallSites[CSi];
        int InlineCost = getInlineCost(CS);
        float FudgeFactor = getInlineFudgeFactor(CS);

        if (InlineCost >= (int)(InlineThreshold * FudgeFactor)) {
          DOUT << "    NOT Inlining: cost=" << InlineCost
               << ", Call: " << *CS.getInstruction();
        } else {
          DOUT << "    Inlining: cost=" << InlineCost
               << ", Call: " << *CS.getInstruction();

          // Attempt to inline the function...
          if (InlineCallIfPossible(CS, CG, SCCFunctions, 
                                   getAnalysis<TargetData>())) {
            // Remove this call site from the list.  If possible, use 
            // swap/pop_back for efficiency, but do not use it if doing so would
            // move a call site to a function in this SCC before the
            // 'FirstCallInSCC' barrier.
            if (SCC.size() == 1) {
              std::swap(CallSites[CSi], CallSites.back());
              CallSites.pop_back();
            } else {
              CallSites.erase(CallSites.begin()+CSi);
            }
            --CSi;

            ++NumInlined;
            Changed = true;
            LocalChange = true;
          }
        }
      }
  } while (LocalChange);

  return Changed;
}

// A simple example algorithm that solves the maze
std::vector<int> MazeSolver::ExampleSolver(std::vector<std::vector<int> > walls)
{
	// The path that solves the maze
	std::vector<int> path;

	// Store the status of visited cells
	std::vector<bool> visited (walls.size(), false);

	int totalNumber = walls.size();						// Total number of cells
	int dimension = (int) sqrt((float)totalNumber);	// Get dimension of the maze
	int currentCell = 0;								// Start from cell 0

	path.push_back(currentCell);

	while (currentCell < totalNumber - 1) {
		visited[currentCell] = true;	// Mark current cell as visited

		std::vector<int> neighbors;

		if (currentCell % dimension != 0 && currentCell > 0) {
			// Left neighbor
			// If it is adjacent to current cell and has not been visited,
			// Add it to valid neighbors list
			if (walls[currentCell][0] == 0 && !visited[currentCell - 1]) {
				neighbors.push_back(currentCell - 1);
			}
		}
		if (currentCell % dimension != dimension - 1 && currentCell < totalNumber - 1) {
			// Right neighbor
			// If it is adjacent to current cell and has not been visited,
			// Add it to valid neighbors list
			if (walls[currentCell][2] == 0 && !visited[currentCell + 1]) {
				neighbors.push_back(currentCell + 1);
			}
		}
		if (currentCell >= dimension) {
			// Upper neighbor
			// If it is adjacent to current cell and has not been visited,
			// Add it to valid neighbors list
			if (walls[currentCell][1] == 0 && !visited[currentCell - dimension]) {
				neighbors.push_back(currentCell - dimension);
			}
		}
		if (currentCell < totalNumber - dimension) {
			// Lower neighbor
			// If it is adjacent to current cell and has not been visited,
			// Add it to valid neighbors list
			if (walls[currentCell][3] == 0 && !visited[currentCell + dimension]) {
				neighbors.push_back(currentCell + dimension);
			}
		}

		if (neighbors.size() > 0) {
			// If there are valid neighbors
			// Take the first one and move to it
			currentCell = neighbors[0];
			path.push_back(currentCell);
		} else {
			// Otherwise go back to previous cell
			path.pop_back();
			currentCell = path.back();
		}
	}
	
	// Return the final path
	return path;
}

void fillGraphSE3RgbdICP(g2o::SparseOptimizer* optimizer, int pyramidLevel, const std::vector<Ptr<OdometryFrame> >& frames,
                         const std::vector<Mat>& poses, const std::vector<PosesLink>& posesLinks, const Mat& cameraMatrix_64F,
                         std::vector<int>& frameIndices,
                         double maxTranslation, double maxRotation, double maxDepthDiff)
{
    CV_Assert(frames.size() == poses.size());
    g2o::Edge_V_V_RGBD::initializeStaticMatrices(); // TODO: make this more correctly

    fillGraphSE3(optimizer, poses, posesLinks, frameIndices);

    // set up ICP edges
    for(size_t currIdx = 0; currIdx < frameIndices.size(); currIdx++)
    {
        int currFrameIdx = frameIndices[currIdx];

        const Mat& curCloud = frames[currFrameIdx]->pyramidCloud[pyramidLevel];
        const Mat& curNormals = frames[currFrameIdx]->pyramidNormals[pyramidLevel];

        for(size_t prevIdx = 0; prevIdx < frameIndices.size(); prevIdx++)
        {
            int prevFrameIdx = frameIndices[prevIdx];
            if(currFrameIdx == prevFrameIdx)
                continue;

            const Mat& prevCloud = frames[prevFrameIdx]->pyramidCloud[pyramidLevel];
            const Mat& prevNormals = frames[prevFrameIdx]->pyramidNormals[pyramidLevel];

            Mat curToPrevRt = poses[prevFrameIdx].inv(DECOMP_SVD) * poses[currFrameIdx];
            if(tvecNorm(curToPrevRt) > maxTranslation || rvecNormDegrees(curToPrevRt) > maxRotation)
                continue;

            Mat corresps_icp;
            CV_Assert(!frames[currFrameIdx]->pyramidMask.empty());
            CV_Assert(!frames[prevFrameIdx]->pyramidNormalsMask.empty());
            CV_Assert(!frames[prevFrameIdx]->pyramidTexturedMask.empty());
            int correspsCount_icp = computeCorrespsFiltered(cameraMatrix_64F, cameraMatrix_64F.inv(), curToPrevRt.inv(DECOMP_SVD),
                                                            frames[currFrameIdx]->pyramidDepth[pyramidLevel],
                                                            frames[currFrameIdx]->pyramidMask[pyramidLevel],
                                                            frames[prevFrameIdx]->pyramidDepth[pyramidLevel],
                                                            frames[prevFrameIdx]->pyramidNormalsMask[pyramidLevel],
                                                            maxDepthDiff, corresps_icp,
                                                            frames[currFrameIdx]->pyramidNormals[pyramidLevel],
                                                            frames[prevFrameIdx]->pyramidNormals[pyramidLevel],
                                                            frames[currFrameIdx]->pyramidImage[pyramidLevel],
                                                            frames[prevFrameIdx]->pyramidImage[pyramidLevel]);

            const int minCorrespsCount = 100;

            if(correspsCount_icp < minCorrespsCount)
                continue;
#define WITH_RGBD 1
#if WITH_RGBD
            const double rgbdScale = 1./(255 * std::max(cameraMatrix_64F.at<double>(0,0), cameraMatrix_64F.at<double>(1,1)));

            Mat corresps_rgbd;
            int correspsCount_rgbd = computeCorrespsFiltered(cameraMatrix_64F, cameraMatrix_64F.inv(), curToPrevRt.inv(DECOMP_SVD),
                                                             frames[currFrameIdx]->pyramidDepth[pyramidLevel],
                                                             frames[currFrameIdx]->pyramidMask[pyramidLevel],
                                                             frames[prevFrameIdx]->pyramidDepth[pyramidLevel],
                                                             frames[prevFrameIdx]->pyramidTexturedMask[pyramidLevel],
                                                             maxDepthDiff, corresps_rgbd,
                                                             frames[currFrameIdx]->pyramidNormals[pyramidLevel],
                                                             frames[prevFrameIdx]->pyramidNormals[pyramidLevel],
                                                             frames[currFrameIdx]->pyramidImage[pyramidLevel],
                                                             frames[prevFrameIdx]->pyramidImage[pyramidLevel]);
            if(correspsCount_rgbd < minCorrespsCount)
                continue;
#endif

            cout << currFrameIdx << " -> " << prevFrameIdx << ": icp correspondences count " << correspsCount_icp << endl;

#if WITH_RGBD
            cout << currFrameIdx << " -> " << prevFrameIdx << ": rgbd correspondences count " << correspsCount_rgbd << endl;
#endif

            // edges for poses
            for(int v0 = 0; v0 < corresps_icp.rows; v0++)
            {
                for(int u0 = 0; u0 < corresps_icp.cols; u0++)
                {
                    int c = corresps_icp.at<int>(v0, u0);
                    if(c == -1)
                        continue;

                    int u1, v1;
                    get2shorts(c, u1, v1);

                    {
                        g2o::Edge_V_V_GICP * e = new g2o::Edge_V_V_GICP();
                        e->setVertex(0, optimizer->vertex(prevFrameIdx));
                        e->setVertex(1, optimizer->vertex(currFrameIdx));

                        g2o::EdgeGICP meas;
                        meas.pos0 = cvtPoint_ocv2egn(prevCloud.at<Point3f>(v1,u1));
                        meas.pos1 = cvtPoint_ocv2egn(curCloud.at<Point3f>(v0,u0));
                        meas.normal0 = cvtPoint_ocv2egn(prevNormals.at<Point3f>(v1,u1));
                        meas.normal1 = cvtPoint_ocv2egn(curNormals.at<Point3f>(v0,u0));

                        e->setMeasurement(meas);
                        meas = e->measurement();
//.........这里部分代码省略.........

/* ****************************************************************************
*
* postIndividualContextEntity -
*
* Corresponding Standard Operation: UpdateContext/APPEND
*
* NOTE
*   This function is used for two different URLs:
*     o /v1/contextEntities
*     o /v1/contextEntities/{entityId::id}
*
* In the longer URL (with entityId::id), the payload (AppendContextElementRequest) cannot contain any
* entityId data (id, type, isPattern).
* In the first case, the entityId data of the payload is mandatory.
* entityId::type can be empty, as always, but entityId::id MUST be filled in.
*
* POST /v1/contextEntities
* POST /ngsi10/contextEntities
* POST /v1/contextEntities/{entityId::id}
* POST /ngsi10/contextEntities/{entityId::id}
*
* Payload In:  AppendContextElementRequest
* Payload Out: AppendContextElementResponse
*
* URI parameters:
*   - attributesFormat=object
*   - entity::type=TYPE
*   - note that '!exist=entity::type' and 'exist=entity::type' are not supported by convenience operations
*     that use the standard operation UpdateContext as there is no restriction within UpdateContext.
*
* 00. Take care of URI params
* 01. Check that total input in consistent and correct
* 02. Fill in UpdateContextRequest from AppendContextElementRequest + URL-data + URI params
* 03. Call postUpdateContext standard service routine
* 04. Translate UpdateContextResponse to AppendContextElementResponse
* 05. Cleanup and return result
*/
std::string postIndividualContextEntity
(
  ConnectionInfo*            ciP,
  int                        components,
  std::vector<std::string>&  compV,
  ParseData*                 parseDataP
)
{
  AppendContextElementRequest*  reqP                  = &parseDataP->acer.res;
  AppendContextElementResponse  response;
  std::string                   entityIdFromPayload   = reqP->entity.id;
  std::string                   entityIdFromURL       = ((compV.size() == 3) || (compV.size() == 4))? compV[2] : "";
  std::string                   entityId;
  std::string                   entityTypeFromPayload = reqP->entity.type;
  std::string                   entityTypeFromURL     = ciP->uriParam[URI_PARAM_ENTITY_TYPE];
  std::string                   entityType;
  std::string                   answer;
  std::string                   out;


  //
  // 01. Check that total input in consistent and correct
  //

  // 01.01. entityId::id
  if ((entityIdFromPayload != "") && (entityIdFromURL != "") && (entityIdFromPayload != entityIdFromURL))
  {
    std::string error = "entityId::id differs in URL and payload";

    alarmMgr.badInput(clientIp, error);
    response.errorCode.fill(SccBadRequest, error);

    TIMED_RENDER(out = response.render(ciP, IndividualContextEntity, ""));
    return out;
  }  
  entityId = (entityIdFromPayload != "")? entityIdFromPayload : entityIdFromURL;

  // 01.02. entityId::type
  if ((entityTypeFromPayload != "") && (entityTypeFromURL != "") && (entityTypeFromPayload != entityTypeFromURL))
  {
    std::string error = "entityId::type differs in URL and payload";

    alarmMgr.badInput(clientIp, error);
    response.errorCode.fill(SccBadRequest, error);

    TIMED_RENDER(out = response.render(ciP, IndividualContextEntity, ""));
    return out;
  }
  entityType = (entityTypeFromPayload != "")? entityTypeFromPayload :entityTypeFromURL;


  // 01.03. entityId::isPattern
  if (reqP->entity.isPattern == "true")
  {
    std::string error = "entityId::isPattern set to true in contextUpdate convenience operation";

    alarmMgr.badInput(clientIp, error);
    response.errorCode.fill(SccBadRequest, error);

    TIMED_RENDER(out = response.render(ciP, IndividualContextEntity, ""));
    return out;
  }

//.........这里部分代码省略.........

void test_word_container(Iterator begin,Iterator end,
    std::vector<int> const &ipos,
    std::vector<int> const &imasks,
    std::vector<std::basic_string<Char> > const &ichunks,
    std::locale l,
    lb::boundary_type bt=lb::word
    )
{
    for(int sm=(bt == lb::word ? 31 : 3 ) ;sm>=0;sm--) {
        unsigned mask = 
              ((sm & 1 ) != 0) * 0xF
            + ((sm & 2 ) != 0) * 0xF0
            + ((sm & 4 ) != 0) * 0xF00
            + ((sm & 8 ) != 0) * 0xF000
            + ((sm & 16) != 0) * 0xF0000;

        std::vector<int> masks,pos;
        std::vector<unsigned> bmasks;
        std::basic_string<Char> empty_chunk;

        std::vector<std::basic_string<Char> > chunks;
        std::vector<std::basic_string<Char> > fchunks;
        std::vector<Iterator> iters;
        iters.push_back(begin);
        bmasks.push_back(0);

        for(unsigned i=0;i<imasks.size();i++) {
            if(imasks[i] & mask) {
                masks.push_back(imasks[i]);
                chunks.push_back(ichunks[i]);
                fchunks.push_back(empty_chunk + ichunks[i]);
                empty_chunk.clear();
                pos.push_back(ipos[i]);
            }
            else {
                empty_chunk+=ichunks[i];
            }

            if((imasks[i] & mask) || i==imasks.size()-1){
                Iterator ptr=begin;
                std::advance(ptr,ipos[i]);
                iters.push_back(ptr);
                bmasks.push_back(imasks[i]);
            }
        }

        //
        // token iterator tests
        //
        {
            typedef lb::token_iterator<Iterator> iter_type;
            lb::mapping<iter_type> map(bt,begin,end,l);

            map.mask(mask);
        
            unsigned i=0;
            iter_type p;
            for(p=map.begin();p!=map.end();++p,i++) {
                p.full_select(false);
                TEST(*p==chunks[i]);
                p.full_select(true);
                TEST(*p==fchunks[i]);
                TEST(p.mark() == unsigned(masks[i]));
            }

            TEST(chunks.size() == i);
                
            
            for(;;) {
                if(p==map.begin()) {
                    TEST(i==0);
                    break;
                }
                else {
                    --p;
                    p.full_select(false);
                    TEST(*p==chunks[--i]);
                    p.full_select(true);
                    TEST(p.mark() == unsigned(masks[i]));
                }
            }
            
            unsigned chunk_ptr=0;
            i=0;
            for(Iterator optr=begin;optr!=end;optr++,i++) {
                p=optr;
                if(chunk_ptr < pos.size() && i>=unsigned(pos[chunk_ptr])){
                    chunk_ptr++;
                }
                if(chunk_ptr>=pos.size()) {
                    TEST(p==map.end());
                }
                else {
                    p.full_select(false);
                    TEST(*p==chunks[chunk_ptr]);
                    p.full_select(true);
                    TEST(*p==fchunks[chunk_ptr]);
                    TEST(p.mark()==unsigned(masks[chunk_ptr]));
                }
            }
//.........这里部分代码省略.........

//
// IntQryBuildInformation()
//
// Protocol building routine, the passed parameter is the enquirer version
static void IntQryBuildInformation(const DWORD &EqProtocolVersion, 
    const DWORD &EqTime)
{
    std::vector<CvarField_t> Cvars;

    // bond - time
    MSG_WriteLong(&ml_message, EqTime);

    // The servers real protocol version
    // bond - real protocol
    MSG_WriteLong(&ml_message, PROTOCOL_VERSION);

    // Built revision of server
    MSG_WriteLong(&ml_message, last_revision);

    cvar_t *var = GetFirstCvar();
    
    // Count our cvars and add them
    while (var)
    {
        if (var->flags() & CVAR_SERVERINFO)
        {
            CvarField.Name = var->name();
            CvarField.Value = var->cstring();
            
            Cvars.push_back(CvarField);
        }
        
        var = var->GetNext();
    }
    
    // Cvar count
    MSG_WriteByte(&ml_message, (BYTE)Cvars.size());
    
    // Write cvars
    for (size_t i = 0; i < Cvars.size(); ++i)
	{
        MSG_WriteString(&ml_message, Cvars[i].Name.c_str());
		MSG_WriteString(&ml_message, Cvars[i].Value.c_str());
	}
	
	MSG_WriteString(&ml_message, (strlen(join_password.cstring()) ? MD5SUM(join_password.cstring()).c_str() : ""));
	MSG_WriteString(&ml_message, level.mapname);
	
    int timeleft = (int)(sv_timelimit - level.time/(TICRATE*60));
	if (timeleft < 0) 
        timeleft = 0;
        
    MSG_WriteShort(&ml_message, timeleft);
    
    // Team data
    MSG_WriteByte(&ml_message, 2);
    
    // Blue
    MSG_WriteString(&ml_message, "Blue");
    MSG_WriteLong(&ml_message, 0x000000FF);
    MSG_WriteShort(&ml_message, (short)TEAMpoints[it_blueflag]);

    MSG_WriteString(&ml_message, "Red");
    MSG_WriteLong(&ml_message, 0x00FF0000);
    MSG_WriteShort(&ml_message, (short)TEAMpoints[it_redflag]);

    // TODO: When real dynamic teams are implemented
    //byte TeamCount = (byte)sv_teamsinplay;
    //MSG_WriteByte(&ml_message, TeamCount);
    
    //for (byte i = 0; i < TeamCount; ++i)
    //{
        // TODO - Figure out where the info resides
        //MSG_WriteString(&ml_message, "");
        //MSG_WriteLong(&ml_message, 0);
        //MSG_WriteShort(&ml_message, TEAMpoints[i]);        
    //}

	// Patch files	
	MSG_WriteByte(&ml_message, patchfiles.size());
	
	for (size_t i = 0; i < patchfiles.size(); ++i)
	{
        MSG_WriteString(&ml_message, patchfiles[i].c_str());
	}
	
	// Wad files
	MSG_WriteByte(&ml_message, wadnames.size());
	
	for (size_t i = 0; i < wadnames.size(); ++i)
    {
        MSG_WriteString(&ml_message, wadnames[i].c_str());
        MSG_WriteString(&ml_message, wadhashes[i].c_str());
    }
    
    MSG_WriteByte(&ml_message, players.size());
    
    // Player info
    for (size_t i = 0; i < players.size(); ++i)
    {
//.........这里部分代码省略.........

inline int32_t cksum(const std::vector<unsigned char>& v)
{
    return cksum(static_cast<const void *>(v.data()), v.size());
}

MediaConduitErrorCode
WebrtcAudioConduit::ConfigureRecvMediaCodecs(
                    const std::vector<AudioCodecConfig*>& codecConfigList)
{
  CSFLogDebug(logTag,  "%s ", __FUNCTION__);
  MediaConduitErrorCode condError = kMediaConduitNoError;
  int error = 0; //webrtc engine errors
  bool success = false;

  // are we receiving already. If so, stop receiving and playout
  // since we can't apply new recv codec when the engine is playing
  if(mEngineReceiving)
  {
    CSFLogDebug(logTag, "%s Engine Already Receiving. Attemping to Stop ", __FUNCTION__);
    // AudioEngine doesn't fail fatal on stop reception. Ref:voe_errors.h.
    // hence we need-not be strict in failing here on error
    mPtrVoEBase->StopReceive(mChannel);
    CSFLogDebug(logTag, "%s Attemping to Stop playout ", __FUNCTION__);
    if(mPtrVoEBase->StopPlayout(mChannel) == -1)
    {
      if( mPtrVoEBase->LastError() == VE_CANNOT_STOP_PLAYOUT)
      {
        CSFLogDebug(logTag, "%s Stop-Playout Failed %d", __FUNCTION__, mPtrVoEBase->LastError());
        return kMediaConduitPlayoutError;
      }
    }
  }

  mEngineReceiving = false;

  if(!codecConfigList.size())
  {
    CSFLogError(logTag, "%s Zero number of codecs to configure", __FUNCTION__);
    return kMediaConduitMalformedArgument;
  }

  //Try Applying the codecs in the list
  for(std::vector<AudioCodecConfig*>::size_type i=0 ;i<codecConfigList.size();i++)
  {
    //if the codec param is invalid or diplicate, return error
    if((condError = ValidateCodecConfig(codecConfigList[i],false)) != kMediaConduitNoError)
    {
      return condError;
    }

    webrtc::CodecInst cinst;
    if(!CodecConfigToWebRTCCodec(codecConfigList[i],cinst))
    {
      CSFLogError(logTag,"%s CodecConfig to WebRTC Codec Failed ",__FUNCTION__);
      continue;
    }

    if(mPtrVoECodec->SetRecPayloadType(mChannel,cinst) == -1)
    {
      error = mPtrVoEBase->LastError();
      CSFLogError(logTag,  "%s SetRecvCodec Failed %d ",__FUNCTION__, error);
      continue;
    } else {
      CSFLogDebug(logTag, "%s Successfully Set RecvCodec %s", __FUNCTION__,
                                          codecConfigList[i]->mName.c_str());
      //copy this to local database
      if(CopyCodecToDB(codecConfigList[i]))
      {
        success = true;
      } else {
        CSFLogError(logTag,"%s Unable to updated Codec Database", __FUNCTION__);
        return kMediaConduitUnknownError;
      }

    }

  } //end for

  //Success == false indicates none of the codec was applied
  if(!success)
  {
    CSFLogError(logTag, "%s Setting Receive Codec Failed ", __FUNCTION__);
    return kMediaConduitInvalidReceiveCodec;
  }

  //If we are here, atleast one codec should have been set
  if(mPtrVoEBase->StartReceive(mChannel) == -1)
  {
    error = mPtrVoEBase->LastError();
    CSFLogError(logTag ,  "%s StartReceive Failed %d ",__FUNCTION__, error);
    if(error == VE_RECV_SOCKET_ERROR)
    {
      return kMediaConduitSocketError;
    }
    return kMediaConduitUnknownError;
  }


  if(mPtrVoEBase->StartPlayout(mChannel) == -1)
  {
    CSFLogError(logTag, "%s Starting playout Failed", __FUNCTION__);
    return kMediaConduitPlayoutError;
  }

  //we should be good here for setting this.
//.........这里部分代码省略.........

void LaserLegsCallback(const people_msgs::PositionMeasurementArray::ConstPtr& msg)
{
    bool validTrackLaser=false;

    cmd_vel.linear.x = 0.0;
    cmd_vel.angular.z = 0.0;

   nbOfTracksLaser=msg->people.size();

   if (nbOfTracksLaser>0) {
       //Extract coordinates of first detected person
       xLaserPerson=msg->people[0].pos.x;
       yLaserPerson=msg->people[0].pos.y;
//       ROS_INFO("xLaser: %f", xLaserPerson);
//       ROS_INFO("yLaser: %f", yLaserPerson);
//       ROS_INFO("AngleErrorLaser: %f", AngleErrorLaser);

        if (nbOfTracksKinect==0) {
       //Calculate angle error
       AngleErrorLaser=atan2(yLaserPerson,xLaserPerson);
       //Calculate distance error
       DistanceErrorLaser=sqrt(pow(xLaserPerson,2)+pow(yLaserPerson,2));

       YpathPointsLaser.insert(YpathPointsLaser.begin(),yLaserPerson);
       if (YpathPointsLaser.size()>6){
           yLast1Laser=YpathPointsLaser.at(5);
           yLast2Laser=YpathPointsLaser.at(1);
           xLast1Laser=xLaserPerson;
           tempDistanceLaser=DistanceErrorLaser;
           yDirectionLaser=yLast2Laser-yLast1Laser;
           YpathPointsLaser.pop_back();
       }

       if(!laser_obstacle_flag){
           angular_command=AngleErrorLaser*KpAngle;
           if(angular_command>MaxTurn){angular_command=MaxTurn;}
           if(angular_command<-MaxTurn){angular_command=-MaxTurn;}
           cmd_vel.angular.z = angular_command;

           double linearspeedLaser=(DistanceErrorLaser-DistanceTarget)*KpDistance;

           if (linearspeedLaser>MaxSpeed)
           {
               linearspeedLaser=MaxSpeed;
           }

           if (linearspeedLaser<0){
               linearspeedLaser=0;
           }
            cmd_vel.linear.x = linearspeedLaser;
           cmd_vel_pub.publish(cmd_vel);
       }
     }
        validTrackLaser=true;
        laserTrack=true;
//////////////////////////////////marker

        visualization_msgs::Marker marker;
        marker.header.frame_id = "base_link";
        marker.header.stamp = ros::Time();
        marker.ns = "laser";
        marker.id = 0;
        marker.type = visualization_msgs::Marker::SPHERE;
        marker.action = visualization_msgs::Marker::ADD;
        marker.pose.position.x = xLaserPerson;
        marker.pose.position.y = yLaserPerson;
        marker.pose.position.z = 0;
        marker.pose.orientation.x = 0.0;
        marker.pose.orientation.y = 0.0;
        marker.pose.orientation.z = 0.0;
        marker.pose.orientation.w = AngleErrorLaser;
        marker.scale.x = 0.1;
        marker.scale.y = 0.1;
        marker.scale.z = 0.1;
        marker.color.a = 1.0; // Don't forget to set the alpha!
        marker.color.r = 0.0;
        marker.color.g = 1.0;
        marker.color.b = 0.0;
        vis_pub1.publish( marker );

////////////////////////

  }
   else if(!validTrackKinect){
       laserTrack=false;
       //////////////////////////////////
/*       validTrackLaser=false;
       xPathLaser= xRobot+cos(orientationRobot+AngleErrorLaser)*tempDistanceLaser;
       yPathLaser= yRobot+sin(orientationRobot+AngleErrorLaser)*tempDistanceLaser;
       AngleErrorFollowLaser=PI-atan2(yPathLaser-yRobot,(xPathLaser-xRobot))-PI+orientationRobot;
           if(abs(AngleErrorFollowLaser)>PI){
               if(AngleErrorFollowLaser<0){AngleErrorFollowLaser=AngleErrorFollowLaser+2*PI;}
               else{AngleErrorFollowLaser=AngleErrorFollowLaser-2*PI;}
           }
           tempDistanceFollowLaser=sqrt(pow(xPathLaser-xRobot,2)+pow(yPathLaser-yRobot,2));

           //Get the number of tracks in the TrackArray
           nbOfTracksLaser=msg->people.size();

           //If at least 1 track, proceed
//.........这里部分代码省略.........

void personCallback(const opt_msgs::TrackArray::ConstPtr& msg)
{
    validTrackKinect=false;


    //Initialize the twist
    cmd_vel.linear.x = 0.0;
    cmd_vel.angular.z = 0.0;

    //Get the number of tracks in the TrackArray
    nbOfTracksKinect=msg->tracks.size();

    //If at least 1 track, proceed
    if (nbOfTracksKinect>0) {
        //looping throught the TrackArray
        for(int i=0;i<nbOfTracksKinect && !validTrackKinect;i++){
            //oldest track which is older than the age threshold and above the confidence threshold
            if ((msg->tracks[i].age>AgeThreshold) && (msg->tracks[i].confidence>ConfidenceTheshold) && (msg->tracks[i].height>HeightTheshold) && (msg->tracks[i].height<HeightMaxTheshold)){

                distanceKinect=msg->tracks[i].distance;
                xperson=((msg->tracks[i].distance)*cos(AngleSmallError+AngleErrorPan));
                yperson=((msg->tracks[i].distance)*sin(AngleSmallError+AngleErrorPan));
                AngleErrorKinect=atan2(yperson,xperson);
                age=msg->tracks[i].age;
                height=msg->tracks[i].height;
                confidence=msg->tracks[i].confidence;

                YpathPoints.insert(YpathPoints.begin(),yperson);
                if (YpathPoints.size()>5){
                    yLast1=YpathPoints.at(4);
                    yLast2=YpathPoints.at(1);
                    xLast1=xperson;
                    tempDistanceKinect=distanceKinect;
                    yDirection=yLast2-yLast1;
                    YpathPoints.pop_back();
                }



                error= sqrt(pow(xperson-xLaserPerson,2)+pow(yperson-yLaserPerson,2));  //calculate the x and y error between the kinect and the laser
//                ROS_INFO("KinectLaserError: %f", error);

                if (error<0.2){
                    kinectLaserMatch=true;
//                    ROS_INFO("match: %d", kinectLaserMatch);
                }else{kinectLaserMatch=false;}

                //Calculate distance error
                DistanceError=distanceKinect-DistanceTarget;

                //print to the console
//                ROS_INFO("Confidence: %f", msg->tracks[i].confidence);
//                ROS_INFO("Height: %f", msg->tracks[i].height);
//                ROS_INFO("distanceKinect: %f", distanceKinect);
//                ROS_INFO("age: %f", msg->tracks[i].age);
//                ROS_INFO("AngleErrorKinect: %f", AngleErrorKinect);
//                ROS_INFO("AngleErrorPan: %f", (AngleErrorPan*180)/ PI);

//                ROS_INFO("xperson: %f", xperson);
//                ROS_INFO("yperson: %f", yperson);

                //Set command Twist
          //      if(smallError==false ){  //to reduce vibration around 0 angle of the kinect view, and 0 robot angle  // && abs(AngleErrorPan)<0.05
          //    cmd_vel.angular.z = (AngleErrorPan+followingAngle)*KpAngle;
                if (!laser_obstacle_flag){
                    angular_command= AngleErrorKinect*KpAngle;;
                    if(abs(followingAngle)>0.1 && abs(AngleErrorKinect)<0.2){angular_command = (AngleErrorKinect+followingAngle)*KpAngleOcclusion;}
                 //   else {angular_command = AngleErrorKinect*KpAngle;}

                        if(angular_command>MaxTurn){angular_command=MaxTurn;}
                        if(angular_command<-MaxTurn){angular_command=-MaxTurn;}
                    cmd_vel.angular.z = angular_command;


           //     if(abs(followingAngle)<0.1){cmd_vel.angular.z = (AngleErrorKinect+followingAngle)*KpAngle;}
           //     else {cmd_vel.angular.z = (AngleErrorKinect+followingAngle)*KpAngleOcclusion;}

             //   cmd_vel.angular.z = AngleError*KpAngle;
            //    }
                //Avoid going backward
                if (DistanceError>0.05){  //threshold for small distance error of 0.05 meter
                    double command_speed=DistanceError*KpDistance;
                    //Limit the speed
                    if (command_speed>MaxSpeed){command_speed=MaxSpeed;}
                    if (command_speed<0){command_speed=0;}
                    cmd_vel.linear.x = command_speed;
                }
                //Stop for loop
                validTrackKinect=true;
                cmd_vel_pub.publish(cmd_vel);
             }
        }
    }

    kinectTrack=true;
    //////////////////////////////////marker
            visualization_msgs::Marker marker;
            marker.header.frame_id = "base_link";
            marker.header.stamp = ros::Time();
            marker.ns = "kinect";
//.........这里部分代码省略.........

static void computeCalleeSaveRegisterPairs(
    MachineFunction &MF, const std::vector<CalleeSavedInfo> &CSI,
    const TargetRegisterInfo *TRI, SmallVectorImpl<RegPairInfo> &RegPairs) {

  if (CSI.empty())
    return;

  AArch64FunctionInfo *AFI = MF.getInfo<AArch64FunctionInfo>();
  MachineFrameInfo *MFI = MF.getFrameInfo();
  CallingConv::ID CC = MF.getFunction()->getCallingConv();
  unsigned Count = CSI.size();
  (void)CC;
  // MachO's compact unwind format relies on all registers being stored in
  // pairs.
  assert((!produceCompactUnwindFrame(MF) ||
          CC == CallingConv::PreserveMost ||
          (Count & 1) == 0) &&
         "Odd number of callee-saved regs to spill!");
  unsigned Offset = AFI->getCalleeSavedStackSize();

  for (unsigned i = 0; i < Count; ++i) {
    RegPairInfo RPI;
    RPI.Reg1 = CSI[i].getReg();

    assert(AArch64::GPR64RegClass.contains(RPI.Reg1) ||
           AArch64::FPR64RegClass.contains(RPI.Reg1));
    RPI.IsGPR = AArch64::GPR64RegClass.contains(RPI.Reg1);

    // Add the next reg to the pair if it is in the same register class.
    if (i + 1 < Count) {
      unsigned NextReg = CSI[i + 1].getReg();
      if ((RPI.IsGPR && AArch64::GPR64RegClass.contains(NextReg)) ||
          (!RPI.IsGPR && AArch64::FPR64RegClass.contains(NextReg)))
        RPI.Reg2 = NextReg;
    }

    // GPRs and FPRs are saved in pairs of 64-bit regs. We expect the CSI
    // list to come in sorted by frame index so that we can issue the store
    // pair instructions directly. Assert if we see anything otherwise.
    //
    // The order of the registers in the list is controlled by
    // getCalleeSavedRegs(), so they will always be in-order, as well.
    assert((!RPI.isPaired() ||
            (CSI[i].getFrameIdx() + 1 == CSI[i + 1].getFrameIdx())) &&
           "Out of order callee saved regs!");

    // MachO's compact unwind format relies on all registers being stored in
    // adjacent register pairs.
    assert((!produceCompactUnwindFrame(MF) ||
            CC == CallingConv::PreserveMost ||
            (RPI.isPaired() &&
             ((RPI.Reg1 == AArch64::LR && RPI.Reg2 == AArch64::FP) ||
              RPI.Reg1 + 1 == RPI.Reg2))) &&
           "Callee-save registers not saved as adjacent register pair!");

    RPI.FrameIdx = CSI[i].getFrameIdx();

    if (Count * 8 != AFI->getCalleeSavedStackSize() && !RPI.isPaired()) {
      // Round up size of non-pair to pair size if we need to pad the
      // callee-save area to ensure 16-byte alignment.
      Offset -= 16;
      assert(MFI->getObjectAlignment(RPI.FrameIdx) <= 16);
      MFI->setObjectSize(RPI.FrameIdx, 16);
    } else
      Offset -= RPI.isPaired() ? 16 : 8;
    assert(Offset % 8 == 0);
    RPI.Offset = Offset / 8;
    assert((RPI.Offset >= -64 && RPI.Offset <= 63) &&
           "Offset out of bounds for LDP/STP immediate");

    RegPairs.push_back(RPI);
    if (RPI.isPaired())
      ++i;
  }

  // Align first offset to even 16-byte boundary to avoid additional SP
  // adjustment instructions.
  // Last pair offset is size of whole callee-save region for SP
  // pre-dec/post-inc.
  RegPairInfo &LastPair = RegPairs.back();
  assert(AFI->getCalleeSavedStackSize() % 8 == 0);
  LastPair.Offset = AFI->getCalleeSavedStackSize() / 8;
}