Ejemplo n.º 1
0
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();
}
    /*
     * 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.
            const int mixedModeScramIterationCount = 5000;
            BSONObj scramCreds = scram::generateCredentials(_creds.password,
                                                            mixedModeScramIterationCount);
            _creds.scram.iterationCount = scramCreds[scram::iterationCountFieldName].Int();
            _creds.scram.salt = scramCreds[scram::saltFieldName].String();
            _creds.scram.storedKey = scramCreds[scram::storedKeyFieldName].String();
            _creds.scram.serverKey = scramCreds[scram::serverKeyFieldName].String();
        }

        // Generate server-first-message
        // Create text-based nonce as base64 encoding of a binary blob of length multiple of 3
        const int nonceLenQWords = 3;
        uint64_t binaryNonce[nonceLenQWords];

        scoped_ptr<SecureRandom> sr(SecureRandom::create());

        binaryNonce[0] = sr->nextInt64();
        binaryNonce[1] = sr->nextInt64();
        binaryNonce[2] = sr->nextInt64();

        _nonce = clientNonce +
            base64::encode(reinterpret_cast<char*>(binaryNonce), sizeof(binaryNonce));
        StringBuilder sb;
        sb << "r=" << _nonce <<
              ",s=" << _creds.scram.salt <<
              ",i=" << _creds.scram.iterationCount;
        *outputData = sb.str();

        // add server-first-message to authMessage
        _authMessage += *outputData + ",";

        return StatusWith<bool>(false);
    }
Ejemplo n.º 3
0
 discrete_domain(std::vector<std::string> Names) : Nmax(Names.size()), _names(std::move(Names)) { init_inv(); }
Ejemplo n.º 4
0
// 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;
}
Ejemplo n.º 5
0
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;
}
/* ****************************************************************************
*
* 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;
  }

  // 01.04. Entity::id must be present, somewhere ...
  if (entityId == "")
  {
    std::string error = "invalid request: mandatory entityId::id missing";

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

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

  // Now, forward Entity to response
  response.entity.fill(entityId, entityType, "false");


  //
  // 02. Fill in UpdateContextRequest from AppendContextElementRequest + URL-data + URI params
  //
  parseDataP->upcr.res.fill(&parseDataP->acer.res, entityId, entityType);


  // 03. Call postUpdateContext standard service routine
  postUpdateContext(ciP, components, compV, parseDataP);


  // 04. Translate UpdateContextResponse to AppendContextElementResponse
  response.fill(&parseDataP->upcrs.res);

  // 05. Cleanup and return result
  TIMED_RENDER(answer = response.render(ciP, IndividualContextEntity, ""));

  response.release();
  parseDataP->upcr.res.release();

  return answer;
}
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();
                        e->information() = meas.prec0(0.01);

                        g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                        e->setRobustKernel(rk);

                        optimizer->addEdge(e);

                    }
                }
            }
#if WITH_RGBD
            for(int v0 = 0; v0 < corresps_rgbd.rows; v0++)
            {
                for(int u0 = 0; u0 < corresps_rgbd.cols; u0++)
                {
                    int c = corresps_rgbd.at<int>(v0, u0);
                    if(c == -1)
                        continue;

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

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

                        g2o::EdgeRGBD meas;
                        meas.srcColor = frames[srcIdx]->pyramidImage[pyramidLevel].at<uchar>(v0, u0);
                        meas.dstColor = frames[dstIdx]->pyramidImage[pyramidLevel].at<uchar>(v1, u1);

                        meas.dst_dIdx = frames[dstIdx]->pyramid_dI_dx[pyramidLevel].at<short int>(v1, u1);
                        meas.dst_dIdy = frames[dstIdx]->pyramid_dI_dy[pyramidLevel].at<short int>(v1, u1);

                        meas.srcP3d = cvtPoint_ocv2egn(frames[srcIdx]->pyramidCloud[pyramidLevel].at<Point3f>(v0, u0));

                        meas.K = &cameraMatrix_64F;

                        e->setMeasurement(meas);
                        meas = e->measurement();
                        double w = rgbdScale * static_cast<double>(correspsCount_icp) / correspsCount_rgbd;
                        e->information() = Eigen::Matrix<double,1,1>::Identity() * w;

                        g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                        e->setRobustKernel(rk);

                        optimizer->addEdge(e);
                    }
                }
            }
#endif
        }
    }
}
Ejemplo n.º 8
0
// Initialize the dice geometry
// Load the OBJ files, apply materials and transformations
bool initDice()
{
  ogl::IndexedTriangleIO io;
#ifdef _DEBUG
   // This smaller model loads much faster in debug mode
  io.loadFromOBJ(gDataPath+"assets/dieDebug.obj");
#else
  // This large model loads fast only in release mode
  io.loadFromOBJ(gDataPath+"assets/die.obj");
#endif
  if(io.vertexNormals().empty())
  {
    std::cerr<<"OBJ model needs vertex normals!"<<std::endl;
    return false;
  }

  std::cerr<<"Loaded "<<io.vertexPositions().size()<< " vertices, "<<
    io.triangleIndices().size()/3<<" triangles.\n\n";

  // Create the dice
  gDice.resize(gNumDice);

  for(size_t i=0;i<gDice.size();++i)
  {
    gDice[i]= std::make_shared<ogl::TriangleGeometry>();
    // The first die stores the original geometry (indexed triangle set)
    if(i==0)
      gDice[0]->init(io.vertexPositions(),io.vertexNormals(),io.triangleIndices());
    // The other dice are instances of the original geometry (no duplicate geometry stored)
    else
      gDice[i]->initInstance(gDice[0]);

    // Set a bluish material with shininess=100.f
    gDice[i]->setMaterial(100.f,ogl::Vec3f(0.2f,0.5f,1.0f));
  }

  // TODO: Transform the dice gDice[0..5] to obtain the result
  // shown in the exercise sheet.

  // You can use the Mat4 transformation functions of the model matrices:
  // modelMatrix().translate(tx,ty,tz);
  // modelMatrix().scale(sy,sy,sz);
  // modelMatrix().rotate(angleInDegree,Vec3(axis_X, axis_Y, axis_Z);

  // Note: the origin of the untransformed cube with dimensions
  // x= -0.5 .. 0.5
  // y= -0.5 .. 0.5
  // z=  0.0 .. 1.0
  // is (0,0,0) is the center of the bottom face (with number 1 on it)

  //TODO: Replace these simple transformations

  //gDice[0] should be the large cube with number 1 facing the camera
  gDice[0]->modelMatrix()
    .scale(2, 2, 2)
    .rotate(270, ogl::Vec3f(0, 1, 0))
    .translate(4,3,1)
    ;
    

  //gDice[1] should be the cube with number 2 facing the camera
  gDice[1]->modelMatrix()
  .rotate(90, ogl::Vec3f(0, 0, 1))
  .translate(2.5,2.5,2)
  ;

  //gDice[2] should be the cube with number 3 facing the camera
  gDice[2]->modelMatrix()
  .rotate(180, ogl::Vec3f(0,0,1))
  .translate(2.5,3.5,2)
  ;

  //gDice[3] should be the cube with number 4 facing the camera
  gDice[3]->modelMatrix()
  .rotate(270, ogl::Vec3f(0, 0, 1))
  .translate(2.5,3.5,3)
  ;

  //gDice[4] should be the cube with number 5 facing the camera
  gDice[4]->modelMatrix()
  .translate(2.5,2.5,3)
  ;

  // Hint for gDice[5] that stands on the tip showing number 6
  // the rotation that is performed on this die is equivalent to the rotation
  // of the vector (1,1,1)^T onto the z-axis (0,0,1).
  // It is helpful to compute this transformation on a sheet of paper.

  //gDice[5] should be the cube with number 6 facing the camera
  gDice[5]->modelMatrix()
  .rotate(45, ogl::Vec3f(0,1,0))
//  .rotate(45, ogl::Vec3f(1,0,1))
  .translate(0.5,0.5,0)
  ;

  return true;
}
Ejemplo n.º 9
0
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]));
                }
            }

        } // token iterator tests

        { // break iterator tests
            typedef lb::break_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++) {
                TEST(*p==iters[i]);
                TEST(p.mark()==bmasks[i]);
            }

            TEST(iters.size() == i);
            do {
                --p;
                --i;
                TEST(*p==iters[i]);
            } while(p!=map.begin());
            TEST(i==0);

            unsigned iters_ptr=0;
            for(Iterator optr=begin;optr!=end;optr++) {
                p=optr;
                TEST(*p==iters[iters_ptr]);
                if(iters.at(iters_ptr)==optr)
                    iters_ptr++;
            }
        
        } // break iterator tests
    } // for mask

}
Ejemplo n.º 10
0
inline int32_t cksum(const std::vector<unsigned char>& v)
{
    return cksum(static_cast<const void *>(v.data()), v.size());
}
Ejemplo n.º 11
0
//
// 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)
    {
        MSG_WriteString(&ml_message, players[i].userinfo.netname);
        MSG_WriteByte(&ml_message, players[i].userinfo.team);
        MSG_WriteShort(&ml_message, players[i].ping);

        int timeingame = (time(NULL) - players[i].JoinTime)/60;
        if (timeingame < 0) 
            timeingame = 0;

        MSG_WriteShort(&ml_message, timeingame);

        // FIXME - Treat non-players (downloaders/others) as spectators too for
        // now
        bool spectator;

        spectator = (players[i].spectator || 
            ((players[i].playerstate != PST_LIVE) &&
            (players[i].playerstate != PST_DEAD) &&
            (players[i].playerstate != PST_REBORN)));

        MSG_WriteBool(&ml_message, spectator);

        MSG_WriteShort(&ml_message, players[i].fragcount);
        MSG_WriteShort(&ml_message, players[i].killcount);
        MSG_WriteShort(&ml_message, players[i].deathcount);
    }
}
Ejemplo n.º 12
0
void IkSolver::init(const KDL::Tree&                     tree,
                    const std::vector<std::string>&      endpoint_names,
                    const std::vector<EndpointCoupling>& endpoint_couplings)
{
  // Preconditions
  assert(endpoint_names.size() == endpoint_couplings.size() && "endpoints and coupling vectors size mismatch");
  // TODO: Verify that all endpoints are contained in tree

  // Enpoint names vector
  endpoint_names_ = endpoint_names;

  // Problem size
  const size_t q_dim = tree.getNrOfJoints(); // Joint space dimension
  size_t x_dim = 0;                          // Task space dimension, value assigned below

  // Populate coupled directions vector
  coupled_dirs_.resize(endpoint_names_.size());
  for (size_t i = 0; i < coupled_dirs_.size(); ++i)
  {
    if ((endpoint_couplings[i] & Position) == Position)
    {
      coupled_dirs_[i].push_back(0);
      coupled_dirs_[i].push_back(1);
      coupled_dirs_[i].push_back(2);
      x_dim += 3;
    }
    if ((endpoint_couplings[i] & Orientation) == Orientation)
    {
      coupled_dirs_[i].push_back(3);
      coupled_dirs_[i].push_back(4);
      coupled_dirs_[i].push_back(5);
      x_dim += 3;
    }
  }

  // Initialize kinematics solvers
  fk_solver_.reset(new FkSolver(tree));
  jac_solver_.reset(new JacSolver(tree));
  inverter_.reset(new Inverter(x_dim, q_dim));

  // Matrix inversion parameters TODO: Expose!
  inverter_->setLsInverseThreshold(1e-5); // NOTE: Magic values
  inverter_->setDlsInverseThreshold(1e-4);
  inverter_->setMaxDamping(0.05);

  // Default values of position solver parameters
  delta_twist_max_ = Eigen::NumTraits<double>::highest();
  delta_joint_pos_max_ = Eigen::NumTraits<double>::highest();
  velik_gain_      = 1.0;
  eps_             = Eigen::NumTraits<double>::epsilon();
  max_iter_        = 1;

  // Populate map from joint names to KDL tree indices
  joint_name_to_idx_.clear();
  const KDL::SegmentMap& tree_segments = tree.getSegments();
  for (KDL::SegmentMap::const_iterator it = tree_segments.begin(); it != tree_segments.end(); ++it)
  {
    const KDL::Joint& joint = it->second.segment.getJoint();
    if (joint.getType() != KDL::Joint::None)
    {
      joint_name_to_idx_[joint.getName()] = it->second.q_nr;
    }
  }

  // Joint space weights updater
  limits_avoider_.reset(new JointPositionLimitsAvoider(q_dim));
  limits_avoider_->setSmoothing(0.8); // NOTE: Magic value

  // Preallocate IK resources
  delta_twist_ = VectorXd::Zero(x_dim);
  delta_q_     = VectorXd::Zero(q_dim);
  q_tmp_       = VectorXd::Zero(q_dim);

  jacobian_     = Eigen::MatrixXd(x_dim, q_dim);
  jacobian_tmp_ = KDL::Jacobian(q_dim);

  Wx_ = VectorXd::Ones(x_dim);

  q_posture_           = KDL::JntArray(q_dim);
  nullspace_projector_ = Eigen::MatrixXd(q_dim, q_dim);
  identity_qdim_       = Eigen::MatrixXd::Identity(q_dim, q_dim);
}
Ejemplo n.º 13
0
bool IkSolver::solve(const KDL::JntArray&           q_current,
                     const std::vector<KDL::Frame>& x_desired,
                           KDL::JntArray&           q_next)
{
  // Precondition
  assert(endpoint_names_.size() == x_desired.size());

  q_next = q_current;
  q_tmp_ = q_current.data;

  // Update joint-space weight matrix: Limit effect of joints near their position limit
  limits_avoider_->resetJointLimitAvoidance().applyJointLimitAvoidance(q_next.data);

  size_t i;
  double delta_twist_norm;
  for (i = 0; i < max_iter_; ++i)
  {
    // Update current task space velocity error
    updateDeltaTwist(q_next, x_desired);
    delta_twist_norm = delta_twist_.dot(Wx_.asDiagonal() * delta_twist_); // Weighted norm
    if (delta_twist_norm < eps_) {break;}

    // Enforce task space maximum velocity through uniform scaling
    const double delta_twist_scaling = delta_twist_max_ / delta_twist_.cwiseAbs().maxCoeff();
    if (delta_twist_scaling < 1.0) {delta_twist_ *= delta_twist_scaling;}

    // Update Jacobian
    updateJacobian(q_next);

    // Velocity IK: Compute incremental joint displacement and scale according to gain

    // Prepare computation of IK with nullspace optimization
    using Eigen::MatrixXd;
    using Eigen::VectorXd;
    using Eigen::DiagonalWrapper;

    const MatrixXd& J = jacobian_;                                                         // Convenience alias
    const DiagonalWrapper<const VectorXd> Wq = limits_avoider_->getWeights().asDiagonal(); // Convenience alias
    const DiagonalWrapper<const VectorXd> Wx = Wx_.asDiagonal();                           // Convenience alias

    // Perform SVD decomposition of J W
    inverter_->compute(Wx * J * Wq);

    // Nullspace projector
    nullspace_projector_ = identity_qdim_ - Wq * inverter_->inverse() * Wx * J; // NOTE: Not rt-friendly, allocates temporaries

    // Compute incremental joint displacement
    delta_q_  = Wq * inverter_->dlsSolve(Wx * delta_twist_) + nullspace_projector_ * (q_posture_.data - q_next.data);
    delta_q_ *= velik_gain_;

    // Enforce joint space maximum velocity through uniform scaling
    const double delta_q_scaling = delta_joint_pos_max_ / delta_q_.cwiseAbs().maxCoeff();
    if (delta_q_scaling < 1.0) {delta_q_ *= delta_q_scaling;}

    // Cache value of q_next
    q_tmp_ = q_next.data;

    // Integrate joint velocity
    q_next.data += delta_q_;

    // Enforce joint position limits

    // Update joint-space weight matrix: Limit effect of joints near their position limit
    limits_avoider_->applyJointLimitAvoidance(q_next.data);

    if (!limits_avoider_->isValid(q_next.data))
    {
      // Keep last configuration that does not exceed position limits
      q_next.data = q_tmp_;
//       ROS_DEBUG_STREAM("Iteration " << i << ", not updating joint position values, weights = " << limits_avoider_->getWeights().transpose());
    }

//     ROS_DEBUG_STREAM("Iteration " << i << ", delta_twist_norm " << delta_twist_.norm() << ", delta_q_scaling " << delta_q_scaling << ", q_next " << q_next.data.transpose()); // TODO: Remove?
  }

  updateDeltaTwist(q_next, x_desired); delta_twist_norm = delta_twist_.transpose().dot(Wx_.asDiagonal() * delta_twist_); // Only needed by below debug message
  ROS_DEBUG_STREAM("Total iterations " << i << ", delta_twist_norm " << delta_twist_norm << " (eps " << eps_ << "), q " << q_next.data.transpose());

  return (i < max_iter_);
}
Ejemplo n.º 14
0
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.
  mEngineReceiving = true;
  DumpCodecDB();
  return kMediaConduitNoError;
}
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
           if (nbOfTracksLaser>0) {
                validTrackLaser=true;
           }

           if (!laser_obstacle_flag){
               ros::Time start= ros::Time::now();
               while((ros::Time::now()-start<ros::Duration(round(tempDistanceFollowLaser/0.3))) && (!validTrackLaser) && (!laser_obstacle_flag)){
               cmd_vel.linear.x = 0.3;
               cmd_vel.angular.z=0.0;}

               cmd_vel.linear.x = 0.0;
               if(!validTrackLaser){
               if(yDirectionLaser>0){cmd_vel.angular.z=0.15;}
               else{cmd_vel.angular.z=-0.15;}
               }
           //Stop for loop
        //   validTrack=true;
           cmd_vel_pub.publish(cmd_vel);

//        ROS_INFO("xLast1: %f", xLast1);
//        ROS_INFO("yLast1: %f", yLast1);
//        ROS_INFO("yDirection: %f", yDirection);
   }
*/
       /////////////////////////////////////
   }
}
Ejemplo n.º 16
0
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;
}
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";
            marker.id = 0;
            marker.type = visualization_msgs::Marker::SPHERE;
            marker.action = visualization_msgs::Marker::ADD;
            marker.pose.position.x = xperson;
            marker.pose.position.y = yperson;
            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 = AngleErrorKinect;
            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 = 1.0;
            vis_pub2.publish( marker );

    ////////////////////////
    }

    else if(!validTrackLaser){
            kinectTrack=false;
  //          validTrackKinect=false;
            xPath= xRobot+cos(orientationRobot+AngleErrorKinect)*tempDistanceKinect;
            yPath= yRobot+sin(orientationRobot+AngleErrorKinect)*tempDistanceKinect;
            AngleErrorFollow=PI-atan2(yPath-yRobot,(xPath-xRobot))-PI+orientationRobot;
                if(abs(AngleErrorFollow)>PI){
                    if(AngleErrorFollow<0){AngleErrorFollow=AngleErrorFollow+2*PI;}
                    else{AngleErrorFollow=AngleErrorFollow-2*PI;}
                }
                tempDistance=sqrt(pow(xPath-xRobot,2)+pow(yPath-yRobot,2));

                //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)){
                        validTrackKinect=true;
                        }
                    }
                }
*/
                if (!laser_obstacle_flag){
                    ros::Time start= ros::Time::now();                   
                    while((ros::Time::now()-start<ros::Duration(round(tempDistance/0.3))) && (!validTrackKinect) && (!laser_obstacle_flag)){
                    if (!laser_obstacle_flag)
                        {cmd_vel.linear.x = 0.3;
                        cmd_vel.angular.z=0.0;}
                    else{cmd_vel.angular.z = laser_angular_velocity;
                        cmd_vel.linear.x = laser_linear_velocity;}
                    }

                    cmd_vel.linear.x = 0.0;
                    if(!validTrackKinect){
                    if(yDirection>0){cmd_vel.angular.z=0.2;}
                    else{cmd_vel.angular.z=-0.2;}
                    }
                //Stop for loop
             //   validTrack=true;
                cmd_vel_pub.publish(cmd_vel);

    //        ROS_INFO("xLast1: %f", xLast1);
    //        ROS_INFO("yLast1: %f", yLast1);
    //        ROS_INFO("yDirection: %f", yDirection);
            }
        }


/*    else{
        xPath= xRobot+cos(orientationRobot+AngleErrorKinect)*tempDistanceKinect;
        yPath= yRobot+sin(orientationRobot+AngleErrorKinect)*tempDistanceKinect;
        AngleErrorFollow=PI-atan2(yPath-yRobot,(xPath-xRobot))-PI+orientationRobot;
            if(abs(AngleErrorFollow)>PI){
                if(AngleErrorFollow<0){AngleErrorFollow=AngleErrorFollow+2*PI;}
                else{AngleErrorFollow=AngleErrorFollow-2*PI;}
            }
//            ROS_INFO("AngleErrorFollow: %f", AngleErrorFollow);
            tempDistance=sqrt(pow(xPath-xRobot,2)+pow(yPath-yRobot,2))-count*50*0.3/1000;
            count++;
//            ROS_INFO("tempDistance: %f", tempDistance);

            if (!laser_obstacle_flag){
           // tempAngular=atan2(yLast1,xLast1);
            cmd_vel.angular.z =-AngleErrorFollow*KpAngle;
            if(AngleErrorFollow<0.5){cmd_vel.angular.z =0;}
    //       if(AngleErrorFollow<0.05&&tempDistance>0.5){
                double command_speed=0.3;
                cmd_vel.linear.x = command_speed;
          //      cmd_vel_pub.publish(cmd_vel);
            if(tempDistance<0.5){
                cmd_vel.linear.x = 0;
                if (yDirection>0){cmd_vel.angular.z=0.2;}
                else{cmd_vel.angular.z=-0.2;}
            }
//        }
            //Stop for loop
            validTrack=true;
            cmd_vel_pub.publish(cmd_vel);

//        ROS_INFO("xLast1: %f", xLast1);
//        ROS_INFO("yLast1: %f", yLast1);
//        ROS_INFO("yDirection: %f", yDirection);
    }
    }
*/

}
Ejemplo n.º 18
0
const morale_type &morale_type_data::convert_legacy( int lmt )
{
    static const std::vector<morale_type> legacy_morale_types = {{
            morale_type( "morale_null" ),
            morale_type( "morale_food_good" ),
            morale_type( "morale_food_hot" ),
            morale_type( "morale_music" ),
            morale_type( "morale_honey" ),
            morale_type( "morale_game" ),
            morale_type( "morale_marloss" ),
            morale_type( "morale_mutagen" ),
            morale_type( "morale_feeling_good" ),
            morale_type( "morale_support" ),
            morale_type( "morale_photos" ),

            morale_type( "morale_craving_nicotine" ),
            morale_type( "morale_craving_caffeine" ),
            morale_type( "morale_craving_alcohol" ),
            morale_type( "morale_craving_opiate" ),
            morale_type( "morale_craving_speed" ),
            morale_type( "morale_craving_cocaine" ),
            morale_type( "morale_craving_crack" ),
            morale_type( "morale_craving_mutagen" ),
            morale_type( "morale_craving_diazepam" ),
            morale_type( "morale_craving_marloss" ),

            morale_type( "morale_food_bad" ),
            morale_type( "morale_cannibal" ),
            morale_type( "morale_vegetarian" ),
            morale_type( "morale_meatarian" ),
            morale_type( "morale_antifruit" ),
            morale_type( "morale_lactose" ),
            morale_type( "morale_antijunk" ),
            morale_type( "morale_antiwheat" ),
            morale_type( "morale_no_digest" ),
            morale_type( "morale_wet" ),
            morale_type( "morale_dried_off" ),
            morale_type( "morale_cold" ),
            morale_type( "morale_hot" ),
            morale_type( "morale_feeling_bad" ),
            morale_type( "morale_killed_innocent" ),
            morale_type( "morale_killed_friend" ),
            morale_type( "morale_killed_monster" ),
            morale_type( "morale_mutilate_corpse" ),
            morale_type( "morale_mutagen_elf" ),
            morale_type( "morale_mutagen_chimera" ),
            morale_type( "morale_mutagen_mutation" ),

            morale_type( "morale_moodswing" ),
            morale_type( "morale_book" ),
            morale_type( "morale_comfy" ),

            morale_type( "morale_scream" ),

            morale_type( "morale_perm_masochist" ),
            morale_type( "morale_perm_hoarder" ),
            morale_type( "morale_perm_fancy" ),
            morale_type( "morale_perm_optimist" ),
            morale_type( "morale_perm_badtemper" ),
            morale_type( "morale_perm_constrained" ),
            morale_type( "morale_game_found_kitten" ),

            morale_type( "morale_haircut" ),
            morale_type( "morale_shave" ),

            morale_type( "morale_vomited" ),

            morale_type( "morale_pyromania_startfire" ),
            morale_type( "morale_pyromania_nearfire" ),
            morale_type( "morale_pyromania_nofire" ),

            morale_type( "morale_perm_filthy" ),

            morale_type( "morale_butcher" ),

            morale_type( "morale_null" )
        }
    };

    if( lmt >= 0 && ( size_t )lmt <= legacy_morale_types.size() ) {
        return legacy_morale_types[ lmt ];
    }

    debugmsg( "Requested invalid legacy morale type %d", lmt );
    return legacy_morale_types.front();
}
Ejemplo n.º 19
0
void hleEnterVblank(u64 userdata, int cyclesLate) {
	int vbCount = userdata;

	DEBUG_LOG(SCEDISPLAY, "Enter VBlank %i", vbCount);

	isVblank = 1;
	vCount++; // vCount increases at each VBLANK.
	hCountBase += hCountPerVblank; // This is the "accumulated" hcount base.
	if (hCountBase > 0x7FFFFFFF) {
		hCountBase -= 0x80000000;
	}
	frameStartTicks = CoreTiming::GetTicks();

	// Wake up threads waiting for VBlank
	u32 error;
	for (size_t i = 0; i < vblankWaitingThreads.size(); i++) {
		if (--vblankWaitingThreads[i].vcountUnblock == 0) {
			// Only wake it if it wasn't already released by someone else.
			SceUID waitID = __KernelGetWaitID(vblankWaitingThreads[i].threadID, WAITTYPE_VBLANK, error);
			if (waitID == 1) {
				__KernelResumeThreadFromWait(vblankWaitingThreads[i].threadID, 0);
			}
			vblankWaitingThreads.erase(vblankWaitingThreads.begin() + i--);
		}
	}

	// Trigger VBlank interrupt handlers.
	__TriggerInterrupt(PSP_INTR_IMMEDIATE | PSP_INTR_ONLY_IF_ENABLED | PSP_INTR_ALWAYS_RESCHED, PSP_VBLANK_INTR, PSP_INTR_SUB_ALL);

	CoreTiming::ScheduleEvent(msToCycles(vblankMs) - cyclesLate, leaveVblankEvent, vbCount + 1);

	gpuStats.numVBlanks++;

	numVBlanksSinceFlip++;

	// TODO: Should this be done here or in hleLeaveVblank?
	if (framebufIsLatched) {
		DEBUG_LOG(SCEDISPLAY, "Setting latched framebuffer %08x (prev: %08x)", latchedFramebuf.topaddr, framebuf.topaddr);
		framebuf = latchedFramebuf;
		framebufIsLatched = false;
		gpu->SetDisplayFramebuffer(framebuf.topaddr, framebuf.pspFramebufLinesize, framebuf.pspFramebufFormat);
	}
	// We flip only if the framebuffer was dirty. This eliminates flicker when using
	// non-buffered rendering. The interaction with frame skipping seems to need
	// some work.
	if (gpu->FramebufferDirty()) {
		if (g_Config.iShowFPSCounter && g_Config.iShowFPSCounter < 4) {
			CalculateFPS();
		}

		// Setting CORE_NEXTFRAME causes a swap.
		// Check first though, might've just quit / been paused.
		if (gpu->FramebufferReallyDirty()) {
			if (coreState == CORE_RUNNING) {
				coreState = CORE_NEXTFRAME;
				gpu->CopyDisplayToOutput();
				actualFlips++;
			}
		}

		gpuStats.numFlips++;

		bool throttle, skipFrame;
		// 1.001f to compensate for the classic 59.94 NTSC framerate that the PSP seems to have.
		DoFrameTiming(throttle, skipFrame, (float)numVBlanksSinceFlip * (1.001f / 60.0f));

		int maxFrameskip = 8;
		if (throttle) {
			if (g_Config.iFrameSkip == 1) {
				// 4 here means 1 drawn, 4 skipped - so 12 fps minimum.
				maxFrameskip = 4;
			} else {
				maxFrameskip = g_Config.iFrameSkip - 1;
			}
		}
		if (numSkippedFrames >= maxFrameskip) {
			skipFrame = false;
		}

		if (skipFrame) {
			gstate_c.skipDrawReason |= SKIPDRAW_SKIPFRAME;
			numSkippedFrames++;
		} else {
			gstate_c.skipDrawReason &= ~SKIPDRAW_SKIPFRAME;
			numSkippedFrames = 0;
		}

		// Returning here with coreState == CORE_NEXTFRAME causes a buffer flip to happen (next frame).
		// Right after, we regain control for a little bit in hleAfterFlip. I think that's a great
		// place to do housekeeping.
		CoreTiming::ScheduleEvent(0 - cyclesLate, afterFlipEvent, 0);
		numVBlanksSinceFlip = 0;
	}
}
Ejemplo n.º 20
0
static inline void shrinkVecToFit(std::vector<Ty_>& vec) {
	if (vec.capacity() != vec.size())
		std::vector<Ty_>(vec).swap(vec);
}
void refineGraphSE3RgbdICP(const std::vector<Ptr<RgbdFrame> >& _frames,
                           const std::vector<Mat>& poses, const std::vector<PosesLink>& posesLinks,
                           const Mat& cameraMatrix, float pointsPart,
                           std::vector<Mat>& refinedPoses, std::vector<int>& frameIndices)
{
    CV_Assert(_frames.size() == poses.size());

    // TODO: find corresp to main API?
    const int levelsCount = 3;
    vector<float> minGradientMagnitudes(levelsCount);
    minGradientMagnitudes[0] = 10;
    minGradientMagnitudes[1] = 5;
    minGradientMagnitudes[2] = 1;
    vector<int> iterCounts(levelsCount);
    iterCounts[0] = 3;
    iterCounts[1] = 4;
    iterCounts[2] = 4;

    RgbdICPOdometry odom;
    odom.set("maxPointsPart", pointsPart);
    odom.set("minGradientMagnitudes", Mat(minGradientMagnitudes));
    odom.set("iterCounts", Mat(iterCounts));
    odom.set("cameraMatrix", cameraMatrix);

    std::vector<Ptr<OdometryFrame> > frames(_frames.size());
    for(size_t i = 0; i < frames.size(); i++)
    {
        Mat gray;
        CV_Assert(_frames[i]->image.channels() == 3);
        cvtColor(_frames[i]->image, gray, CV_BGR2GRAY);
        Ptr<OdometryFrame> frame = new OdometryFrame(gray, _frames[i]->depth, _frames[i]->mask,
                                                     _frames[i]->normals, _frames[i]->ID);
        odom.prepareFrameCache(frame, OdometryFrame::CACHE_ALL);
        frames[i] = frame;
    }

    refinedPoses.resize(poses.size());
    for(size_t i = 0; i < poses.size(); i++)
        refinedPoses[i] = poses[i].clone();

    vector<Mat> pyramidCameraMatrix;
    buildPyramidCameraMatrix(cameraMatrix, iterCounts.size(), pyramidCameraMatrix);

    for(int level = iterCounts.size() - 1; level >= 0; level--)
    {
        for(int iter = 0; iter < iterCounts[level]; iter++)
        {
            G2OLinearSolver* linearSolver =  createLinearSolver(DEFAULT_LINEAR_SOLVER_TYPE);
            G2OBlockSolver* blockSolver = createBlockSolver(linearSolver);
            g2o::OptimizationAlgorithm* nonLinerSolver = createNonLinearSolver(DEFAULT_NON_LINEAR_SOLVER_TYPE, blockSolver);
            g2o::SparseOptimizer* optimizer = createOptimizer(nonLinerSolver);

            double maxTranslation = DBL_MAX;
            double maxRotation = DBL_MAX;
            double maxDepthDiff = 0.07;
            if(level == 0)
            {
                maxTranslation = 0.20;
                maxRotation = 30;
            }

            fillGraphSE3RgbdICP(optimizer, level, frames, refinedPoses, posesLinks, pyramidCameraMatrix[level], frameIndices,
                                maxTranslation, maxRotation, maxDepthDiff);

            optimizer->initializeOptimization();
            const int optIterCount = 1;
            cout << "Vertices count: " << optimizer->vertices().size() << endl;
            cout << "Edges count: " << optimizer->edges().size() << endl;
            cout << "Start optimization " << endl;
            if(optimizer->optimize(optIterCount) != optIterCount)
            {
                optimizer->clear();
                delete optimizer;
                break;
            }
            cout << "Finish optimization " << endl;

            getSE3Poses(optimizer, frameIndices, refinedPoses);

            optimizer->clear();
            delete optimizer;
        }
    }
}
Ejemplo n.º 22
0
void testMeshGridAlgorithm(MeshLib::Mesh const*const mesh,
                           std::vector<GeoLib::Point*>& pnts_for_search,
                           std::vector<size_t> &idx_found_nodes, bool contiguous)
{
    // constructing Grid
    INFO ("[MeshGridAlgorithm] constructing mesh grid object ...");

    if (contiguous) {
        std::vector<MeshLib::Node> mesh_nodes;
        size_t n_nodes(mesh->getNodes().size());
        mesh_nodes.reserve(n_nodes);
        for (size_t k(0); k<n_nodes; k++) {
            mesh_nodes.push_back(MeshLib::Node(*(mesh->getNodes()[k])));
        }
#ifndef WIN32
        BaseLib::MemWatch mem_watch;
        unsigned long mem_without_mesh (mem_watch.getVirtMemUsage());
#endif
        clock_t start_grid_construction = clock();
        GeoLib::Grid<MeshLib::Node> mesh_grid(mesh_nodes.begin(), mesh_nodes.end(), 511);
        clock_t end_grid_construction = clock();
#ifndef WIN32
        unsigned long mem_with_mesh (mem_watch.getVirtMemUsage());
#endif
        INFO("\tdone, construction time: %f seconds", (end_grid_construction-start_grid_construction)/(double)(CLOCKS_PER_SEC));
#ifndef WIN32
        INFO ("[MeshGridAlgorithm] mem for mesh grid: %i MB", (mem_with_mesh - mem_without_mesh)/(1024*1024));
#endif
        const size_t n_pnts_for_search(pnts_for_search.size());
        INFO ("[MeshGridAlgorithm] searching %d points ...", pnts_for_search.size());
        clock_t start = clock();
        for (size_t k(0); k<n_pnts_for_search; k++) {
            MeshLib::Node const* node(mesh_grid.getNearestPoint(*pnts_for_search[k]));
            idx_found_nodes.push_back(node->getID());
        }
        clock_t stop = clock();
        INFO("\tdone, search time: %f seconds", (stop-start)/(double)(CLOCKS_PER_SEC));
    } else {
#ifndef WIN32
        BaseLib::MemWatch mem_watch;
        unsigned long mem_without_mesh (mem_watch.getVirtMemUsage());
#endif
        clock_t start_grid_construction = clock();
        GeoLib::Grid<MeshLib::Node> mesh_grid(mesh->getNodes().begin(), mesh->getNodes().end(), 511);
        clock_t end_grid_construction = clock();
#ifndef WIN32
        unsigned long mem_with_mesh (mem_watch.getVirtMemUsage());
#endif
        INFO("\tdone, construction time: %f seconds", (end_grid_construction-start_grid_construction)/(double)(CLOCKS_PER_SEC));
#ifndef WIN32
        INFO ("[MeshGridAlgorithm] mem for mesh grid: %i MB", (mem_with_mesh - mem_without_mesh)/(1024*1024));
#endif
        const size_t n_pnts_for_search(pnts_for_search.size());
        INFO ("[MeshGridAlgorithm] searching %d points ...", pnts_for_search.size());
        clock_t start = clock();
        for (size_t k(0); k<n_pnts_for_search; k++) {
            MeshLib::Node const* node(mesh_grid.getNearestPoint(pnts_for_search[k]));
            idx_found_nodes.push_back(node->getID());
        }
        clock_t stop = clock();
        INFO("\tdone, search time: %f seconds", (stop-start)/(double)(CLOCKS_PER_SEC));
    }
}
Ejemplo n.º 23
0
bool cShaderBuilder::Build(const std::vector<const std::string>& i_arguments)
{
	// Get which shader program type to compile
	eShaderProgramType shaderProgramType = UNKNOWN;
	{
		if (i_arguments.size() >= 1)
		{
			const std::string& argument = i_arguments[0];
			if (argument == "vertex")
			{
				shaderProgramType = VERTEX;
				//printf("argument %s\n", argument.c_str());
			}
			else if (argument == "fragment")
			{
				shaderProgramType = FRAGMENT;
				//printf("argument %s\n", argument.c_str());
			}
			else
			{
				std::stringstream errorMessage;
				errorMessage << "\"" << argument << "\" is not a valid shader program type";
				OutputErrorMessage(errorMessage.str().c_str(), m_path_source);
				return false;
			}
		}
		else
		{
			OutputErrorMessage(
				"A Shader must be built with an argument defining which type of shader program (e.g. \"vertex\" or \"fragment\") to compile",
				m_path_source);
			return false;
		}
	}

	/*bool wereThereErrors = false;

	// Copy the source to the target
	{
		const bool dontFailIfTargetAlreadyExists = false;
		std::string errorMessage;
		if (!eae6320::CopyFile(m_path_source, m_path_target, dontFailIfTargetAlreadyExists))
		{
			wereThereErrors = true;
			std::stringstream decoratedErrorMessage;
			decoratedErrorMessage << "Windows failed to copy \"" << m_path_source << "\" to \"" << m_path_target << "\": " << errorMessage;
			eae6320::OutputErrorMessage(decoratedErrorMessage.str().c_str(), __FILE__);
		}
	}

	return !wereThereErrors;*/
	// Get the path to the shader compiler
	std::string path_fxc;
	{
		// Get the path to the DirectX SDK
		std::string path_sdk;
		{
			std::string errorMessage;
			if (!GetEnvironmentVariable("DXSDK_DIR", path_sdk, &errorMessage))
			{
				std::stringstream decoratedErrorMessage;
				decoratedErrorMessage << "Windows failed to get the path to the DirectX SDK: " << errorMessage;
				OutputErrorMessage(decoratedErrorMessage.str().c_str(), __FILE__);
				return false;
			}
		}
		path_fxc = path_sdk + "Utilities/bin/" +
#ifndef _WIN64
			"x86"
#else
			"x64"
#endif
			+ "/fxc.exe";
	}
	// Create the command to run
	std::string command;
	{
		std::stringstream commandToBuild;
		commandToBuild << "\"" << path_fxc << "\"";
		// Target profile
		switch (shaderProgramType)
		{
		case VERTEX:
			commandToBuild << " /Tvs_3_0";
			break;
		case FRAGMENT:
			commandToBuild << " /Tps_3_0";
			break;
		}
		// Entry point
		commandToBuild << " /Emain"
#ifdef _DEBUG
			// Disable optimizations so that debugging is easier
			<< " /Od"
			// Enable debugging
			<< " /Zi"
#endif
			// Target file
			<< " /Fo\"" << m_path_target << "\""
			// Don't output the logo
			<< " /nologo"
			// Source file
			<< " \"" << m_path_source << "\""
			;
		command = commandToBuild.str();
	}
	// Execute the command
	{
		DWORD exitCode;
		std::string errorMessage;
		if (ExecuteCommand(command.c_str(), &exitCode, &errorMessage))
		{
			return exitCode == EXIT_SUCCESS;
		}
		else
		{
			OutputErrorMessage(errorMessage.c_str(), m_path_source);
			return false;
		}
	}
}
Ejemplo n.º 24
0
bool CMissionItem::buildFromScript( const std::vector<std::string> & script, std::vector< std::pair< std::string, STRING_MANAGER::TParamType > > & chatParams, std::string & varName)
{
	_NoDrop = false;
	if( script.size() < 4 || script.size() > 7)
	{
		MISLOG("syntax error usage : '%s:<item_name> : <brick_craft_plan> : <req_skill> : +[ [<property>|<resist>] <value> ; | <action> ;] [ : <phrase_id>] [ : nodrop]",script[0].c_str() );
		return false;
	}
	string val;
	bool ret = true;

	// get the variable name
	varName = CMissionParser::getNoBlankString( script[1] );
	// get the sheet
	_SheetId = CSheetId( CMissionParser::getNoBlankString( script[2] ) + ".sitem" );
	if  ( _SheetId == CSheetId::Unknown )
	{
		MISLOG("Invalid sitem sheet '%s'", (CMissionParser::getNoBlankString( script[1] ) + ".sitem").c_str());
		ret = false;
	}
	_Quality = (uint16) atoi( script[3].c_str() );

	for ( uint i = 4; i < script.size(); i++)
	{
		if ( !nlstricmp( "nodrop", script[i] ) )
			_NoDrop = true;
		else
		{
			vector<string> vars;
			NLMISC::splitString( script[i], ";",vars );

			bool propFound = false;
			for ( uint i = 0; i < vars.size(); i++ )
			{	
				vector<string> args;
				CMissionParser::tokenizeString( vars[i], " \t",args );
				if ( args.size() == 2 )
				{
					// 2 params means that it is an item property
					if( !nlstricmp(args[0],"Durability" ) )
						_Params.Durability = (float)atof(args[1].c_str());
					else if( !nlstricmp(args[0],"Weight" ) )
						_Params.Weight = (float)atof(args[1].c_str());
					else if( !nlstricmp(args[0],"SapLoad" ) )
						_Params.SapLoad = (float)atof(args[1].c_str());
					else if( !nlstricmp(args[0],"Dmg" ) )
						_Params.Dmg = (float)atof(args[1].c_str());
					else if( !nlstricmp(args[0],"Speed" ) )
						_Params.Speed = (float)atof(args[1].c_str());
					else if( !nlstricmp(args[0],"Range" ) )
						_Params.Range = (float)atof(args[1].c_str());
					else if( !nlstricmp(args[0],"DodgeModifier" ) )
						_Params.DodgeModifier = (float)atof(args[1].c_str());
					else if( !nlstricmp(args[0],"ParryModifier" ) )
						_Params.ParryModifier = (float)atof(args[1].c_str());
					else if( !nlstricmp(args[0],"AdversaryDodgeModifier" ) )
						_Params.AdversaryDodgeModifier = (float)atof(args[1].c_str());
					else if( !nlstricmp(args[0],"AdversaryParryModifier" ) )
						_Params.AdversaryParryModifier = (float)atof(args[1].c_str());
					else if( !nlstricmp(args[0],"ProtectionFactor" ) )
						_Params.ProtectionFactor = (float)atof(args[1].c_str());
					else if( !nlstricmp(args[0],"MaxSlashingProtection" ) )
						_Params.MaxSlashingProtection = (float)atof(args[1].c_str());
					else if( !nlstricmp(args[0],"MaxBluntProtection" ) )
						_Params.MaxBluntProtection = (float)atof(args[1].c_str());
					else if( !nlstricmp(args[0],"MaxPiercingProtection" ) )
						_Params.MaxPiercingProtection = (float)atof(args[1].c_str());
					
					else if( !nlstricmp(args[0],"HpBuff" ) )
						_Params.HpBuff = atoi(args[1].c_str());
					else if( !nlstricmp(args[0],"SapBuff" ) )
						_Params.SapBuff = atoi(args[1].c_str());
					else if( !nlstricmp(args[0],"StaBuff" ) )
						_Params.StaBuff = atoi(args[1].c_str());
					else if( !nlstricmp(args[0],"FocusBuff" ) )
						_Params.FocusBuff = atoi(args[1].c_str());
					else if( !nlstricmp(args[0],"Color" ) )
					{
						uint8 color = (uint8)atoi(args[1].c_str());
						if( color <= 7 )
						{
							_Params.Color[color] = 1;
						}
					}
					else if( !nlstricmp(args[0],"AcidProtection") )
						_Params.AcidProtectionFactor = (float)atof(args[1].c_str());
					else if( !nlstricmp(args[0],"ColdProtection") )
						_Params.ColdProtectionFactor = (float)atof(args[1].c_str());
					else if( !nlstricmp(args[0],"FireProtection") )
						_Params.FireProtectionFactor = (float)atof(args[1].c_str());
					else if( !nlstricmp(args[0],"RotProtection") )
						_Params.RotProtectionFactor = (float)atof(args[1].c_str());
					else if( !nlstricmp(args[0],"ShockWaveProtection") )
						_Params.ShockWaveProtectionFactor = (float)atof(args[1].c_str());
					else if( !nlstricmp(args[0],"PoisonProtection") )
						_Params.PoisonProtectionFactor = (float)atof(args[1].c_str());
					else if( !nlstricmp(args[0],"ElectricityProtection") )
						_Params.ElectricityProtectionFactor = (float)atof(args[1].c_str());
					else
					{
						RESISTANCE_TYPE::TResistanceType resistance = RESISTANCE_TYPE::fromString( args[1] );
						switch( resistance )
						{
						case RESISTANCE_TYPE::Desert:
							_Params.DesertResistanceFactor = 1.0f;
							break;
						case RESISTANCE_TYPE::Forest:
							_Params.ForestResistanceFactor = 1.0f;
							break;
						case RESISTANCE_TYPE::Lacustre:
							_Params.LacustreResistanceFactor = 1.0f;
							break;
						case RESISTANCE_TYPE::Jungle:
							_Params.JungleResistanceFactor = 1.0f;
							break;
						case RESISTANCE_TYPE::PrimaryRoot:
							_Params.PrimaryRootResistanceFactor = 1.0f;
							break;
						default:
							MISLOG("Invalid param '%s'", args[0].c_str());
							ret = false;
						}
					}
					propFound = true;
				}
				else if ( args.size() > 2 )
				{
					MISLOG("Invalid property defined with more than 2 params");
					ret = false;
				}
				else if ( args.size() == 1 && i != 0 )
				{
					CSheetId enchantId = CSheetId( CMissionParser::getNoBlankString( args[0] ) + ".sphrase" );
					if (enchantId == CSheetId::Unknown)
					{
						MISLOG("Invalid enchantement '%s.sphrase'", args[0].c_str());
						ret = false;
					}
					else
					{
						_SPhraseId = CSheetId (args[0] + ".sphrase");
						if ( _SPhraseId == CSheetId::Unknown )
						{
							MISLOG("Invalid sheet '%s.sphrase'", args[0].c_str());
							ret = false;
						}
					}
				}
				else if ( !propFound )
					_PhraseId = CMissionParser::getNoBlankString( args[0] );
			}
			///\todo : actions
		}
	}
	return true;
}// CMissionItem::buildFromScript
Ejemplo n.º 25
0
///
/// groupIndividualsBasedOn: Splits individuals into three queues left, right and initial based on the consanguinous/external flags.
/// Individuals with left flag set are pushed onto the left queue in increasing order of the left flags.
/// Individuals with right flag set are  pushed onto the right queue in descending  order of the right flags.
///
void Individual::groupIndividualsBasedOn(bool consanguinousLoop,const std::vector<Individual*>& individuals,std::deque<Individual*>& initial,std::deque<Individual*>& left,std::deque<Individual*>& right,bool unique){
	
	unsigned count = individuals.size();
	for(unsigned i=0;i<count;i++){
		
		//
		// First step in the sorting process is to construct 3 queues
		// The left queue has individuals with leftLoopFlag set (front of the Q with the largest loop #)
		// The right queue has individuals with rightLoopFlag set
		// The remaining individuals are in the initial queue 
		//
		if(individuals[i]->getLeftFlag(consanguinousLoop) == 0 && individuals[i]->getRightFlag(consanguinousLoop) == 0){ 
			initial.push_back(individuals[i]);
			continue; 
		}
		
		if(individuals[i]->getLeftFlag(consanguinousLoop) > 0){
			if(!unique || (unique && individuals[i]->getLeftFlag(consanguinousLoop) >= individuals[i]->getRightFlag(consanguinousLoop))){
				// insert into the left deque
				if(left.size() > 0){
					if(left.front()->getLeftFlag(consanguinousLoop) > individuals[i]->getLeftFlag(consanguinousLoop)){
						left.push_front(individuals[i]);
					}else{
						// Make additional check for twin cases:
						if(left.back()->getLeftFlag(consanguinousLoop) > individuals[i]->getLeftFlag(consanguinousLoop) && left.back()->getTwinMarker().get() != ".")
							left.push_front(individuals[i]);
						else left.push_back(individuals[i]);
					}
				}else{
					left.push_back(individuals[i]);
				}
			}
		} 
		if(individuals[i]->getRightFlag(consanguinousLoop) > 0){
			
			if(!unique || (unique && individuals[i]->getRightFlag(consanguinousLoop) > individuals[i]->getLeftFlag(consanguinousLoop))){
				// insert into the right deque
				if(right.size() > 0){
					if(right.front()->getRightFlag(consanguinousLoop) > individuals[i]->getRightFlag(consanguinousLoop)){
						right.push_back(individuals[i]);
					}else{
						right.push_front(individuals[i]);
					}
				}else{
					right.push_back(individuals[i]);
				}
			}
		}
	}
	
	// DEBUG:
	/*std::cout << "Sorting the  Individuals based on :" << consanguinousLoop << std::endl;
	std::cout << " Initial deque " << std::endl;
	for(unsigned i=0;i<initial.size();i++){
		std::cout << initial[i]->getId() << std::endl;
	}
	std::cout << "Left deque " << std::endl;
	for(unsigned i=0;i<left.size();i++){
		std::cout << left[i]->getId() << " and " << left[i]->getLeftFlag(consanguinousLoop) << std::endl;
	}
	std::cout << "Right deque " << std::endl;
	for(unsigned i=0;i<right.size();i++){
		std::cout << right[i]->getId() << " and " << right[i]->getRightFlag(consanguinousLoop) << std::endl;
	}
	*/
	
	
}
Ejemplo n.º 26
0
    int Cornea::computeCentre(const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &led_pos, // LED locations
                              const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &glint_pos,
                              std::vector<double> &gx_guesses,
                              Eigen::Vector3d &centre,
                              double &err) {

        // initialise the cornea tracker
        create(led_pos, glint_pos);

        /*
         * Check out usage and more info about GSL:
         * http://www.csse.uwa.edu.au/programming/gsl-1.0/gsl-ref_35.html
         */

        const size_t n = 3 * pairsOfTwo(data.size());	// number of functions
        const size_t p = gx_guesses.size();				// number of parameters

        // initial guesses
        gsl_vector_const_view x = gsl_vector_const_view_array(gx_guesses.data(), p);

        gsl_multifit_function_fdf f;
        f.f			= &my_f;	// function
        f.df		= &my_df;	// derivative
        f.fdf		= &my_fdf;	// both
        f.n			= n;		// number of functions
        f.p			= p;		// number of parameters
        f.params	= this;		// additional parameter

        const gsl_multifit_fdfsolver_type *T = gsl_multifit_fdfsolver_lmsder;
        gsl_multifit_fdfsolver *solver = gsl_multifit_fdfsolver_alloc(T, n, p);
        gsl_multifit_fdfsolver_set(solver, &f, &x.vector);


        int status;
        unsigned int iter = 0;

        do {
            iter++;
            status = gsl_multifit_fdfsolver_iterate(solver);

            if(status) {
                break;
            }

            status = gsl_multifit_test_delta(solver->dx, solver->x, PRECISION, PRECISION);

        }
        while(status == GSL_CONTINUE && iter < MAX_ITER);

        if(iter == MAX_ITER) {
            printf("Cornea::computeCentre(): iter = MAX_ITER\n");
        }


        gsl_matrix *covar = gsl_matrix_alloc(p, p);
        gsl_multifit_covar(solver->J, 0.0, covar);

        // for(int row = 0; row < p; ++row) {
        //     for(int col = 0; col < p; ++col) {
        //         printf("%.2f ", covar->data[row * p + col]);
        //     }
        //     printf("\n");
        // }
        // printf("*****************************\n");

        /***********************************************************************
         * Compute the fit error
         **********************************************************************/

        err = 0;
        for(size_t i = 0; i < p; i++) {
            err += gsl_matrix_get(covar, i, i);
        }
        err = std::sqrt(err);


        Eigen::Vector3d cw(0.0, 0.0, 0.0);

        // cornea sphere radius
        const double RHO = trackerSettings.RHO;

        // remove this
        double dMaxX = -10.0;

        for(size_t i = 0; i < data.size(); ++i) {

            const DATA_FOR_CORNEA_COMPUTATION &cur_data = data[i];

            const double gx_guess = gsl_vector_get(solver->x, i);

            const double B_aux = atan2(gx_guess * tan(cur_data.alpha_aux), (cur_data.l_aux - gx_guess));

            // calculate the corneal sphere centers in the auxiliary coordinate systems
            const Eigen::Vector3d c_aux(gx_guess - RHO * sin((cur_data.alpha_aux - B_aux) / 2.),
                                        0.,
                                        gx_guess * tan(cur_data.alpha_aux) + RHO * cos((cur_data.alpha_aux - B_aux) / 2.));

            const Eigen::Vector3d tmp = cur_data.R * c_aux;

            cw(0) += tmp(0);
            cw(1) += tmp(1);
            cw(2) += tmp(2);

            if(tmp(0) > dMaxX) {
                dMaxX = tmp(0);
            }

            //        printf("%i: centre (mm): %.2f %.2f %.2f\n", (int)i, 1000.0*tmp(0), 1000.0*tmp(1), 1000.0*tmp(2));

        }

        const double nof_samples = (double)data.size();
        centre << cw(0) / nof_samples, cw(1) / nof_samples, cw(2) / nof_samples;


        // printf("Avg: %.2f %.2f %.2f\n", 1000.0*centre(0), 1000.0*centre(1), 1000.0*centre(2));
        // printf("*********************************\n");

        gsl_multifit_fdfsolver_free(solver);
        gsl_matrix_free(covar);

        return (int)iter;

    }
    /**
     * Parse client-final-message of the form:
     * c=channel-binding(base64),r=client-nonce|server-nonce,p=ClientProof
     *
     * Generate successful authentication server-final-message on the form:
     * v=ServerSignature
     *
     * or failed authentication server-final-message on the form:
     * e=message
     *
     * NOTE: we are ignoring the channel binding part of the message
    **/
    StatusWith<bool> SaslSCRAMSHA1ServerConversation::_secondStep(const std::vector<string>& input,
                                                                  std::string* outputData) {
        if (input.size() != 3) {
            return StatusWith<bool>(ErrorCodes::BadValue, mongoutils::str::stream() <<
                "Incorrect number of arguments for second SCRAM-SHA-1 client message, got " <<
                input.size() << " expected 3");
        }
        else if (!str::startsWith(input[0], "c=") || input[0].size() < 3) {
            return StatusWith<bool>(ErrorCodes::BadValue, mongoutils::str::stream() <<
                "Incorrect SCRAM-SHA-1 channel binding: " << input[0]);
        }
        else if (!str::startsWith(input[1], "r=") || input[1].size() < 6) {
            return StatusWith<bool>(ErrorCodes::BadValue, mongoutils::str::stream() <<
                "Incorrect SCRAM-SHA-1 client|server nonce: " << input[1]);
        }
        else if(!str::startsWith(input[2], "p=") || input[2].size() < 3) {
            return StatusWith<bool>(ErrorCodes::BadValue, mongoutils::str::stream() <<
                "Incorrect SCRAM-SHA-1 ClientProof: " << input[2]);
        }

        // add client-final-message-without-proof to authMessage
        _authMessage += input[0] + "," + input[1];

        // Concatenated nonce sent by client should equal the one in server-first-message
        std::string nonce = input[1].substr(2);
        if (nonce != _nonce) {
            return StatusWith<bool>(ErrorCodes::BadValue, mongoutils::str::stream() <<
                "Unmatched SCRAM-SHA-1 nonce received from client in second step, expected " <<
                _nonce << " but received " << nonce);
        }

        std::string clientProof = input[2].substr(2);
        
        // Do server side computations, compare storedKeys and generate client-final-message
        // AuthMessage     := client-first-message-bare + "," +
        //                    server-first-message + "," +
        //                    client-final-message-without-proof
        // ClientSignature := HMAC(StoredKey, AuthMessage)
        // ClientKey := ClientSignature XOR ClientProof
        // ServerSignature := HMAC(ServerKey, AuthMessage)

        unsigned int hashLen = 0;
        unsigned char clientSignature[scram::hashSize];

        std::string decodedStoredKey = base64::decode(_creds.scram.storedKey);
        // ClientSignature := HMAC(StoredKey, AuthMessage)
        fassert(18662, crypto::hmacSha1(
                            reinterpret_cast<const unsigned char*>(decodedStoredKey.c_str()),
                            scram::hashSize,
                            reinterpret_cast<const unsigned char*>(_authMessage.c_str()),
                            _authMessage.size(),
                            clientSignature,
                            &hashLen));

        fassert(18658, hashLen == scram::hashSize);

        try {
            clientProof = base64::decode(clientProof);
        }
        catch (const DBException& ex) {
            return StatusWith<bool>(ex.toStatus());
        }
        const unsigned char *decodedClientProof =
            reinterpret_cast<const unsigned char*>(clientProof.c_str());

        // ClientKey := ClientSignature XOR ClientProof
        unsigned char clientKey[scram::hashSize];
        for(size_t i=0; i<scram::hashSize; i++) {
            clientKey[i] = clientSignature[i]^decodedClientProof[i];
        }

        // StoredKey := H(ClientKey)
        unsigned char computedStoredKey[scram::hashSize];
        fassert(18659, crypto::sha1(clientKey, scram::hashSize, computedStoredKey));

        if (memcmp(decodedStoredKey.c_str(), computedStoredKey, scram::hashSize) != 0) {
            return StatusWith<bool>(ErrorCodes::AuthenticationFailed,
                mongoutils::str::stream() <<
                    "SCRAM-SHA-1 authentication failed, storedKey mismatch");
        }

        // ServerSignature := HMAC(ServerKey, AuthMessage)
        unsigned char serverSignature[scram::hashSize];
        std::string decodedServerKey = base64::decode(_creds.scram.serverKey);
        fassert(18660, crypto::hmacSha1(
                            reinterpret_cast<const unsigned char*>(decodedServerKey.c_str()),
                            scram::hashSize,
                            reinterpret_cast<const unsigned char*>(_authMessage.c_str()),
                            _authMessage.size(),
                            serverSignature,
                            &hashLen));

        fassert(18661, hashLen == scram::hashSize);

        StringBuilder sb;
        sb << "v=" << base64::encode(reinterpret_cast<char*>(serverSignature), scram::hashSize);
        *outputData = sb.str();

        return StatusWith<bool>(false);
    }
void Render()
{
	if (!g_CanDraw)
		return;

	auto width = glutGet(GLUT_WINDOW_WIDTH);
	auto height = glutGet(GLUT_WINDOW_HEIGHT);

	glClearColor(0.5f, 0.5f, 0.5f, 1.0f);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	// variables uniformes (constantes) 

	g_Camera.projectionMatrix = glm::perspectiveFov(45.f, (float)width, (float)height, 0.1f, 1000.f);
	// rotation orbitale de la camera
	float rotY = glm::radians(g_Camera.rotation.y);
	const glm::vec4 orbitDistance(0.0f, 0.0f, 200.0f, 1.0f);
	glm::vec4 position = /*glm::eulerAngleY(rotY) **/ orbitDistance;
	g_Camera.viewMatrix = glm::lookAt(glm::vec3(position), glm::vec3(0.f), glm::vec3(0.f, 1.f, 0.f));

	glBindBuffer(GL_UNIFORM_BUFFER, g_Camera.UBO);
	//glBufferData(GL_UNIFORM_BUFFER, sizeof(glm::mat4) * 2, glm::value_ptr(g_Camera.viewMatrix), GL_STREAM_DRAW);
	glBufferSubData(GL_UNIFORM_BUFFER, 0, sizeof(glm::mat4) * 2, glm::value_ptr(g_Camera.viewMatrix));

	for (uint32_t index = 0; index < g_Spheres.size(); ++index)
	{
		static const float radius = 100.0f;
		// L'illumination s'effectue dans le repere de la camera, il faut donc que les positions des lumieres
		// soient egalement exprimees dans le meme repere (view space)
		g_PointLights[index].position = g_Camera.viewMatrix * glm::vec4(g_Spheres[index].position, 1.0f);
		g_PointLights[index].position.w = radius;
	}
	glBindBuffer(GL_UNIFORM_BUFFER, PointLight::UBO);
	glBufferSubData(GL_UNIFORM_BUFFER, 0, sizeof(PointLight) * g_NumPointLights, g_PointLights);

	// rendu des murs avec illumination	

	g_GBuffer.BindBuffer();

	glBindVertexArray(g_WallMesh.VAO);
	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);

	auto program = g_GBufferShader.GetProgram();
	glUseProgram(program);
	
	auto numLightsLocation = glGetUniformLocation(program, "u_numLights");
	glUniform1i(numLightsLocation, g_NumPointLights);
	auto worldLocation = glGetUniformLocation(program, "u_worldMatrix");
	glm::mat4& transform = g_Walls.worldMatrix;
	glUniformMatrix4fv(worldLocation, 1, GL_FALSE, glm::value_ptr(transform));
	auto startIndex = 0;
	glBindTexture(GL_TEXTURE_2D, g_Walls.textures[Walls::gWallTexture]);	
	glDrawArrays(GL_TRIANGLES, startIndex, 6 * 4); startIndex += 6 * 4;	// 4 murs
	glBindTexture(GL_TEXTURE_2D, g_Walls.textures[Walls::gCeilingTexture]);	
	glDrawArrays(GL_TRIANGLES, startIndex, 6); startIndex += 6;	// plafond
	glBindTexture(GL_TEXTURE_2D, g_Walls.textures[Walls::gFloorTexture]);	
	glDrawArrays(GL_TRIANGLES, startIndex, 6); startIndex += 6;	// sol

	g_GBuffer.UnbindBuffer();

	// rendu debug

	glBindTexture(GL_TEXTURE_2D, 0);

	glBindVertexArray(g_SphereMesh.VAO);
	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, g_SphereMesh.IBO);

	program = g_AmbientShader.GetProgram();
	glUseProgram(program);

	worldLocation = glGetUniformLocation(program, "u_worldMatrix");

	for (auto index = 0; index < g_Spheres.size(); ++index) 
	{
		glm::mat4& transform = g_Spheres[index].worldMatrix;
		glUniformMatrix4fv(worldLocation, 1, GL_FALSE, glm::value_ptr(transform));
		glDrawElements(GL_TRIANGLES, g_SphereMesh.ElementCount, GL_UNSIGNED_INT, 0);
	}
	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);

	//glBindVertexArray(0);

	// dessine les tweakBar
	TwDraw();  

	glutSwapBuffers();
}
Ejemplo n.º 29
0
EdgeBasedGraphFactory::EdgeBasedGraphFactory(int nodes, std::vector<NodeBasedEdge> & inputEdges, std::vector<NodeID> & bn, std::vector<NodeID> & tl, std::vector<_Restriction> & irs, std::vector<NodeInfo> & nI, boost::property_tree::ptree speedProfile, std::string & srtm) : inputNodeInfoList(nI), numberOfTurnRestrictions(irs.size()), trafficSignalPenalty(0) {
    BOOST_FOREACH(_Restriction & restriction, irs) {
        std::pair<NodeID, NodeID> restrictionSource = std::make_pair(restriction.fromNode, restriction.viaNode);
        unsigned index;
        RestrictionMap::iterator restrIter = _restrictionMap.find(restrictionSource);
        if(restrIter == _restrictionMap.end()) {
            index = _restrictionBucketVector.size();
            _restrictionBucketVector.resize(index+1);
            _restrictionMap[restrictionSource] = index;
        } else {
            index = restrIter->second;
            //Map already contains an is_only_*-restriction
            if(_restrictionBucketVector.at(index).begin()->second)
                continue;
            else if(restriction.flags.isOnly){
                //We are going to insert an is_only_*-restriction. There can be only one.
                _restrictionBucketVector.at(index).clear();
            }
        }

        _restrictionBucketVector.at(index).push_back(std::make_pair(restriction.toNode, restriction.flags.isOnly));
    }
Ejemplo n.º 30
-16
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);
    }
}