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);
}
discrete_domain(std::vector<std::string> Names) : Nmax(Names.size()), _names(std::move(Names)) { init_inv(); }
// 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;
}
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
}
}
}
// 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;
}
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
}
inline int32_t cksum(const std::vector<unsigned char>& v)
{
return cksum(static_cast<const void *>(v.data()), v.size());
}
//
// 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);
}
}
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);
}
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_);
}
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);
}
*/
/////////////////////////////////////
}
}
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);
}
}
*/
}
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();
}
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;
}
}
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;
}
}
}
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));
}
}
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;
}
}
}
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
///
/// 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;
}
*/
}
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 ¢re,
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();
}
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));
}
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);
}
}