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();
}
Exemple #2
0
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;
        }
        }
    }
}