void multiKinectCalibration::calibrate()
{
if (!useTFonly) {
std::cerr << "Available PCL: "<<iterationStep << " " << listOfPointCloudnames.size() << std::endl;
for (std::size_t i = 1; i < listOfPointCloudnames.size(); i++)
{
// refine the transformation with ICP
std::cerr << "Aligning kinect" << 1 << " with kinect" << i+1 << std::endl;
std::cerr << "Initial Pose est:\n" << kinect2kinectTransform[i-1].matrix() << std::endl;
pcl::IterativeClosestPoint<pcl::PointXYZRGBA, pcl::PointXYZRGBA> icp;
icp.setInputCloud(cachedPCL[i]);
icp.setInputTarget(cachedPCL[0]);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr Final(new pcl::PointCloud<pcl::PointXYZRGBA>());
std::cerr << "Starting ICP between kinect" << 1 << " with kinect" << i+1 << std::endl;
icp.align(*Final, kinect2kinectTransform[i-1].inverse().matrix().cast<float>());
std::cerr << "ICP has converged. Final transformation:\n";
std::cerr << icp.getFinalTransformation().inverse() << std::endl;
// dump the ICP transformation
kinect2kinectTransform[i - 1] = icp.getFinalTransformation().inverse().cast<double>();
std::stringstream ss;
ss << dumpLocation << "Aligned" << i << ".pcd";
pcl::PCDWriter writer;
writer.write<pcl::PointXYZRGBA> (ss.str(), *Final, true);
showPCL(Final);
}
}
else std::cerr << "using tf only";
dumpParams();
}
void traverseAstree(astree* root) {
if( flag == 0 ) {
symbol_stack.push_back(nullptr);
flag++;
}
for (size_t a = 0; a < root->children.size(); ++a) {
int sym = root->children[a]->symbol;
astree* curr = root->children[a];
switch(sym) {
case TOK_VARDECL:
case '=':
travVardecl(curr);
break;
case TOK_IF:
case TOK_WHILE:
validCompare(curr);
symbol_stack.push_back(nullptr);
blockcount++;
traverseAstree(curr->children[1]);
blockcount--;
symbol_stack.pop_back();
break;
case TOK_IFELSE:
validCompare(curr);
for(size_t i = 0; i<curr->children.size(); i++) {
symbol_stack.push_back(nullptr);
blockcount++;
traverseAstree(curr->children[i]);
blockcount--;
symbol_stack.pop_back();
}
break;
case TOK_FUNCTION:
{
root->children[a]->children[1]->block = blockcount+1;
symbol* newFunc = create_symbol(root->children[a]);
blockcount++;
newFunc->attributes.set(ATTR_function);
switch(curr->children[0]->symbol) {
case TOK_INT:
newFunc->attributes.set(ATTR_int);
break;
case TOK_STRING:
newFunc->attributes.set(ATTR_string);
break;
case TOK_CHAR:
newFunc->attributes.set(ATTR_char);
break;
case TOK_BOOL:
newFunc->attributes.set(ATTR_bool);
break;
case TOK_TYPEID:
newFunc->attributes.set(ATTR_struct);
newFunc->attributes.set(ATTR_typeid);
newFunc->type_id =*root->children[a]->children[0]->lexinfo;
break;
}
newFunc->parameters = new vector<symbol*>;
for(size_t i = 0; i<curr->children[1]->children.size(); i++) {
astree* param_node = curr->children[1]->children[i];
symbol* param = create_symbol(param_node->children[0]);
param->attributes.set(ATTR_variable);
param->attributes.set(ATTR_lval);
param->attributes.set(ATTR_param);
int paramSym = param_node->symbol;
switch(paramSym) {
case TOK_INT:
param->attributes.set(ATTR_int);
break;
case TOK_CHAR:
param->attributes.set(ATTR_char);
break;
case TOK_STRING:
param->attributes.set(ATTR_string);
break;
case TOK_BOOL:
param->attributes.set(ATTR_bool);
break;
}
newFunc->parameters->push_back(param);
}
if(symbol_stack.back() == nullptr || symbol_stack.empty()) {
symbol_table* tab = new symbol_table();
tab->insert(symbol_entry(curr->children[0]->children[0]->
lexinfo,newFunc));
symbol_stack.pop_back();
symbol_stack.push_back(tab);
} else {
symbol_stack.back()->insert(symbol_entry(curr->children[0]->
children[0]->lexinfo,newFunc));
}
dumpToFile(file_sym, newFunc, curr->children[0]->children[0]);
symbol_stack.push_back(nullptr);
if (curr->children[1]->children.size() != 0) {
dumpParams(newFunc, curr->children[1]);
}
int funcSym;
switch (curr->children[0]->symbol) {
case TOK_INT:
funcSym = TOK_INTCON;
break;
case TOK_CHAR:
funcSym = TOK_CHARCON;
break;
case TOK_STRING:
funcSym = TOK_STRINGCON;
break;
case TOK_BOOL:
funcSym = TOK_BOOL;
break;
}
traverseFunc(curr->children[2], funcSym);
blockcount--;
symbol_stack.pop_back();
break;
}
case TOK_STRUCT:
{
astree *temp = root->children[a];
symbol *newSym = create_symbol(temp->children[0]);
newSym->attributes.set(ATTR_struct);
newSym->attributes.set(ATTR_typeid);
newSym->type_id = *temp->children[0]->lexinfo;
dumpToFile(file_sym,newSym,temp->children[0]);
if(symbol_stack.back() == nullptr || symbol_stack.empty()) {
symbol_table* tab = new symbol_table();
tab->insert(symbol_entry(temp->children[0]->lexinfo,newSym));
symbol_stack.push_back(tab);
} else {
symbol_stack.back()->insert(symbol_entry(temp->children[0]->
lexinfo,newSym));
}
newSym->fields = new symbol_table();
for(size_t i = 1; i<temp->children.size(); i++) {
symbol *tempSym = create_symbol(temp->children[i]->
children[0]);
tempSym->attributes.set(ATTR_field);
switch(temp->children[i]->symbol) {
case TOK_INT:
tempSym->attributes.set(ATTR_int);
break;
case TOK_CHAR:
tempSym->attributes.set(ATTR_char);
break;
case TOK_BOOL:
tempSym->attributes.set(ATTR_bool);
break;
case TOK_STRING:
tempSym->attributes.set(ATTR_string);
break;
case TOK_TYPEID:
tempSym->attributes.set(ATTR_typeid);
tempSym->attributes.set(ATTR_struct);
break;
}
newSym->fields->insert(symbol_entry(temp->children[i]->
children[0]->lexinfo,tempSym));
}
dumpFields(newSym,temp);
break;
}
case TOK_CALL:
checkCall(curr);
break;
case TOK_PROTOTYPE:
{
symbol* newFunc = create_symbol(root->children[a]);
blockcount++;
newFunc->attributes.set(ATTR_function);
switch(curr->children[0]->symbol) {
case TOK_INT:
newFunc->attributes.set(ATTR_int);
break;
case TOK_STRING:
newFunc->attributes.set(ATTR_string);
break;
case TOK_CHAR:
newFunc->attributes.set(ATTR_char);
break;
case TOK_BOOL:
newFunc->attributes.set(ATTR_bool);
break;
case TOK_TYPEID:
newFunc->attributes.set(ATTR_struct);
newFunc->attributes.set(ATTR_typeid);
newFunc->type_id =*root->children[a]->children[0]->lexinfo;
break;
}
newFunc->parameters = new vector<symbol*>;
for(size_t i = 0; i<curr->children[1]->children.size(); i++) {
astree* param_node = curr->children[1]->children[i];
symbol* param = create_symbol(param_node->children[0]);
param->attributes.set(ATTR_variable);
param->attributes.set(ATTR_lval);
param->attributes.set(ATTR_param);
int paramSym = param_node->symbol;
switch(paramSym) {
case TOK_INT:
param->attributes.set(ATTR_int);
break;
case TOK_CHAR:
param->attributes.set(ATTR_char);
break;
case TOK_STRING:
param->attributes.set(ATTR_string);
break;
case TOK_BOOL:
param->attributes.set(ATTR_bool);
break;
}
newFunc->parameters->push_back(param);
}
if(symbol_stack.back() == nullptr || symbol_stack.empty()) {
symbol_table* tab = new symbol_table();
tab->insert(symbol_entry(curr->children[0]->children[0]->
lexinfo,newFunc));
symbol_stack.pop_back();
symbol_stack.push_back(tab);
} else {
symbol_stack.back()->insert(symbol_entry(curr->children[0]->
children[0]->lexinfo,newFunc));
}
dumpToFile(file_sym, newFunc, curr->children[0]->children[0]);
symbol_stack.push_back(nullptr);
if (curr->children[1]->children.size() != 0) {
dumpParams(newFunc, curr->children[1]);
}
int funcSym;
switch (curr->children[0]->symbol) {
case TOK_INT:
funcSym = TOK_INTCON;
break;
case TOK_CHAR:
funcSym = TOK_CHARCON;
break;
case TOK_STRING:
funcSym = TOK_STRINGCON;
break;
case TOK_BOOL:
funcSym = TOK_BOOL;
break;
}
blockcount--;
break;
}
}
}
}