void GraphOptimizer_G2O::genEdgeData(Eigen::Matrix4f guess, pcl::PointCloud<pcl::PointXYZRGB>::Ptr src, pcl::PointCloud<pcl::PointXYZRGB>::Ptr tgt, Eigen::Matrix4f& relPose, Eigen::Matrix<double,6,6>& infMatrix, float& photoCons)
{
pcl::PointCloud<pcl::PointXYZRGB> notUsed(640,480);
icp.setOflowStop(true);
icp.setInputSource(src);
icp.setInputTarget(tgt);
const bool LOOP=false;
icp.align(notUsed, guess,LOOP);
relPose = icp.getFinalTransformation();
photoCons = icp.getPhotoConsistency();
fillInformationMatrix(infMatrix,photoCons);
}
// Adds the row i from crossRelation to the relation naturalJoin
void addRowToNJ(_Relation* naturalJoin, _Relation* crossRelation, int z) {
vector<string> alreadyused;
for (int i = 0; i < naturalJoin->Columns.size(); ++i) {
for (int j = 0; j < crossRelation->Columns.size(); ++j) {
if (naturalJoin->Columns[i].Name == crossRelation->Columns[j].Name) {
if (notUsed(naturalJoin->Columns[i].Name, alreadyused)) {
naturalJoin->Columns[i].Rows.push_back(crossRelation->Columns[j].Rows[z]);
alreadyused.push_back(naturalJoin->Columns[i].Name);
}
break;
}
}
}
}
vector<vector<string> > findLadders(string start, string end, unordered_set<string> &dict) {
vector<vector<string> > results;
queue<vector<int> > q;
unordered_map<string, int> pos;
vector<string> words;
words.push_back(start);
buildMap(dict, pos, words);
vector<int> r;
bool reached = false;
r.push_back(0);
q.push(r);
vector<int> seperator;
seperator.push_back(-1);
q.push(seperator);
while(!q.empty()){
vector<int> cur = q.front();
q.pop();
if(cur[0] == -1){
if(reached || q.empty()){
break;
}
q.push(seperator);
continue;
}
string s = words[cur.back()];
for(int i = 0; i < s.size(); i++){
for(char c = 'a'; c <= 'z'; c++){
string tmp = s;
tmp[i] = c;
if(tmp == end){
results.push_back(genStr(cur, words, end));
reached = true;
}
else if(dict.count(tmp) > 0 && notUsed(cur, pos[tmp])){
cur.push_back(pos[tmp]);
q.push(cur);
cur.pop_back();
}
}
}
}
return results;
}
void Vehicle::generate(uint seed)
{
vectors.clear();
vectors.resize(pointsNum);
qsrand(seed);
double deltaAngle = M_PI * 2.0 / static_cast<double>(pointsNum);
unsigned char r = 128 + (qrand() % 128);
unsigned char g = 128 + (qrand() % 128);
unsigned char b = 128 + (qrand() % 128);
// Создание массива неиспользованных (для колёс) вершин.
QVector <int> notUsed(pointsNum);
for(int i = 0; i < pointsNum; ++i)
{
notUsed[i] = i;
}
wheelsNum = (maxWheelsNum > 0) ? (qrand() % (maxWheelsNum + 1)) : 0;
// Генерация векторов из точки
for(int i = 0; i < pointsNum; ++i)
{
vectors[i].angle = static_cast<double>(i) * deltaAngle + (qrand() % 1000) / 1000.0 * deltaAngle;
vectors[i].length = 0.2 + (qrand() % 1000) / 1000.0 * vectorLength;
vectors[i].r = r;
vectors[i].g = g;
vectors[i].b = b;
vectors[i].wheel = false;
vectors[i].radius = 0.3 + (qrand() % 1000) / 1000.0f * wheelRadius;
if(leftSide)
{
vectors[i].motorSpeed = -(qrand() % 30 + 10);
} else {
vectors[i].motorSpeed = (qrand() % 30 + 10);
}
}
for(int i = 0; i < wheelsNum; ++i)
{
// Выбор для колеса одной из неиспользуемых вершин.
int choosed = qrand() % notUsed.size();
int id = notUsed[choosed];
vectors[id].wheel = true;
notUsed.remove(choosed);
}
double x1, x2, y1, y2;
double r1, r2;
for(int i = 0; i < pointsNum; ++i)
{
if(vectors[i].wheel)
{
x1 = qCos(vectors[i].angle) * (vectors[i].length + springLength);
y1 = qSin(vectors[i].angle) * (vectors[i].length + springLength);
r1 = vectors[i].radius;
for(int j = i + 1; j < pointsNum; ++j)
{
x2 = qCos(vectors[j].angle) * (vectors[j].length + springLength);
y2 = qSin(vectors[j].angle) * (vectors[j].length + springLength);
r2 = vectors[j].radius;
if((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2) <= (r1 + r2) * (r1 + r2))
{
if(qrand() % 2)
{
vectors[i].wheel = false;
} else {
vectors[j].wheel = false;
}
break;
}
}
}
}
score = 0.0;
}
/* solve all symbolic references in the function: */
int solveFunctionRef(FUNCTIONPTR f)
{
notUsed(f);
return TRUE; /* nothing to do! */
}
/* solve all symbolic references in the layout: */
int solveLayoutRef(LAYOUTPTR layout)
{
notUsed(layout);
return TRUE; /* not implemented (yet)*/
}
int main (int argc, char *argv[])
{
int lower = 0; /* lower limit of log depth, if kept set to zero */
int higher = 0; /* upper limit of log depth, if kept set to zero */
/* process command line options */
int opt; /* selected option */
do
{ switch ((opt = getopt (argc, argv, "l:L:bh")))
{ case 'l': /* log depth */
if (sscanf (optarg, "%d,%d", &lower, &higher) != 2)
{ fprintf (stderr, "%s: Bad argument to l option.\n", basename (argv[0]));
printUsage (basename (argv[0]));
return EXIT_FAILURE;
}
soSetProbe (lower, higher);
break;
case 'L': /* log file */
if ((fl = fopen (optarg, "w")) == NULL)
{ fprintf (stderr, "%s: Can't open log file \"%s\".\n", basename (argv[0]), optarg);
printUsage (basename (argv[0]));
return EXIT_FAILURE;
}
soOpenProbe (fl);
break;
case 'b': /* batch mode */
batch = 1; /* set batch mode for processing: no input messages are issued */
break;
case 'h': /* help mode */
printUsage (basename (argv[0]));
return EXIT_SUCCESS;
case -1: break;
default: fprintf (stderr, "%s: Wrong option.\n", basename (argv[0]));
printUsage (basename (argv[0]));
return EXIT_FAILURE;
}
} while (opt != -1);
if ((argc - optind) != 1) /* check existence of mandatory argument: storage device name */
{ fprintf (stderr, "%s: Wrong number of mandatory arguments.\n", basename (argv[0]));
printUsage (basename (argv[0]));
return EXIT_FAILURE;
}
if (fl == NULL)
fl = stdout; /* if the switch -L was not used, set output to stdout */
else stderr = fl; /* if the switch -L was used, set stderr to log file */
/* check for storage device conformity */
char *devname; /* path to the storage device in the Linux file system */
struct stat st; /* file attributes */
devname = argv[optind];
if (stat (devname, &st) == -1) /* get file attributes */
{ printError (-errno, basename (argv[0]));
return EXIT_FAILURE;
}
if (st.st_size % BLOCK_SIZE != 0) /* check file size: the storage device must have a size in bytes
multiple of block size */
{ fprintf (stderr, "%s: Bad size of support file.\n", basename (argv[0]));
return EXIT_FAILURE;
}
/* open an unbuffered communication channel with the storage device */
int status; /* status of operation */
if ((status = soOpenBufferCache (argv[optind], UNBUF)) != 0)
{ printError (status, basename (argv[0]));
return EXIT_FAILURE;
}
/* process the command */
int cmdNumb; /* command number */
int t; /* test flag */
while (true)
{ if (batch == 0) printMenu ();
if (batch == 0) printf("\nYour command: ");
do
{ t = scanf ("%d", &cmdNumb);
scanf ("%*[^\n]");
scanf ("%*c");
} while (t != 1);
if (cmdNumb == 0) break;
if ((cmdNumb > 0) && (cmdNumb < HDL_LEN))
hdl[cmdNumb]();
else { notUsed();
if (batch != 0) break;
}
}
/* close the unbuffered communication channel with the storage device */
if ((status = soCloseBufferCache ()) != 0)
{ printError (status, basename (argv[0]));
return EXIT_FAILURE;
}
/* that's all */
if (batch == 0) printf ("Bye!\n");
return EXIT_SUCCESS;
} /* end of main */
