/** * * Funcion que retorna la altura de un arbol * **/ int alturaMaxima(Arbol *a) { if(a != NULL) { /* No Contabilizo los nodos Hoja */ if((a->der==NULL) && (a->izq==NULL)) return 0; else /* Cada vez que avanzo por un hijo aumento altura */ return 1 + max_(alturaMaxima(a->izq), alturaMaxima(a->der)); } else return 0; }
namespace ranges { inline namespace v3 { namespace detail { template<typename C1, typename C2> using monoidal_zip_cardinality = std::integral_constant<cardinality, C1::value == infinite || C2::value == infinite ? infinite : C1::value == unknown || C2::value == unknown ? unknown : C1::value >= 0 && C2::value >= 0 ? max_(C1::value, C2::value) : finite>; } // namespace detail template<typename Fun, typename R1, typename R2> struct iter_monoidal_zip_view : view_facade<iter_monoidal_zip_view<Fun, R1, R2>, detail::monoidal_zip_cardinality<range_cardinality<R1>, range_cardinality<R2>>::value> { private: friend range_access; semiregular_t<function_type<Fun>> fun_; R1 r1_; R2 r2_;
void WorldInternal::Culling(const Matrix44& cameraProjMat, bool isOpenGL) { objs.clear(); #ifdef _WIN32 if (_finite(cameraProjMat.Values[2][2]) != 0 && cameraProjMat.Values[0][0] != 0.0f && cameraProjMat.Values[1][1] != 0.0f) { #else if (isfinite(cameraProjMat.Values[2][2]) != 0 && cameraProjMat.Values[0][0] != 0.0f && cameraProjMat.Values[1][1] != 0.0f) { #endif Matrix44 cameraProjMatInv = cameraProjMat; cameraProjMatInv.SetInverted(); float maxx = 1.0f; float minx = -1.0f; float maxy = 1.0f; float miny = -1.0f; float maxz = 1.0f; float minz = 0.0f; if (isOpenGL) minz = -1.0f; Vector3DF eyebox[8]; eyebox[0 + 0] = Vector3DF(minx, miny, maxz); eyebox[1 + 0] = Vector3DF(maxx, miny, maxz); eyebox[2 + 0] = Vector3DF(minx, maxy, maxz); eyebox[3 + 0] = Vector3DF(maxx, maxy, maxz); eyebox[0 + 4] = Vector3DF(minx, miny, minz); eyebox[1 + 4] = Vector3DF(maxx, miny, minz); eyebox[2 + 4] = Vector3DF(minx, maxy, minz); eyebox[3 + 4] = Vector3DF(maxx, maxy, minz); for (int32_t i = 0; i < 8; i++) { eyebox[i] = cameraProjMatInv.Transform3D(eyebox[i]); } // 0-right 1-left 2-top 3-bottom 4-front 5-back Vector3DF facePositions[6]; facePositions[0] = eyebox[5]; facePositions[1] = eyebox[4]; facePositions[2] = eyebox[6]; facePositions[3] = eyebox[4]; facePositions[4] = eyebox[4]; facePositions[5] = eyebox[0]; Vector3DF faceDir[6]; faceDir[0] = Vector3DF::Cross(eyebox[1] - eyebox[5], eyebox[7] - eyebox[5]); faceDir[1] = Vector3DF::Cross(eyebox[6] - eyebox[4], eyebox[0] - eyebox[4]); faceDir[2] = Vector3DF::Cross(eyebox[7] - eyebox[6], eyebox[2] - eyebox[6]); faceDir[3] = Vector3DF::Cross(eyebox[0] - eyebox[4], eyebox[5] - eyebox[4]); faceDir[4] = Vector3DF::Cross(eyebox[5] - eyebox[4], eyebox[6] - eyebox[4]); faceDir[5] = Vector3DF::Cross(eyebox[2] - eyebox[0], eyebox[1] - eyebox[5]); for (int32_t i = 0; i < 6; i++) { faceDir[i].Normalize(); } for (int32_t z = 0; z < viewCullingZDiv; z++) { for (int32_t y = 0; y < viewCullingYDiv; y++) { for (int32_t x = 0; x < viewCullingXDiv; x++) { Vector3DF eyebox_[8]; float xsize = 1.0f / (float) viewCullingXDiv; float ysize = 1.0f / (float) viewCullingYDiv; float zsize = 1.0f / (float) viewCullingZDiv; for (int32_t e = 0; e < 8; e++) { float x_, y_, z_; if (e == 0){ x_ = xsize * x; y_ = ysize * y; z_ = zsize * z; } if (e == 1){ x_ = xsize * (x + 1); y_ = ysize * y; z_ = zsize * z; } if (e == 2){ x_ = xsize * x; y_ = ysize * (y + 1); z_ = zsize * z; } if (e == 3){ x_ = xsize * (x + 1); y_ = ysize * (y + 1); z_ = zsize * z; } if (e == 4){ x_ = xsize * x; y_ = ysize * y; z_ = zsize * (z + 1); } if (e == 5){ x_ = xsize * (x + 1); y_ = ysize * y; z_ = zsize * (z + 1); } if (e == 6){ x_ = xsize * x; y_ = ysize * (y + 1); z_ = zsize * (z + 1); } if (e == 7){ x_ = xsize * (x + 1); y_ = ysize * (y + 1); z_ = zsize * (z + 1); } Vector3DF yzMid[4]; yzMid[0] = eyebox[0] * x_ + eyebox[1] * (1.0f - x_); yzMid[1] = eyebox[2] * x_ + eyebox[3] * (1.0f - x_); yzMid[2] = eyebox[4] * x_ + eyebox[5] * (1.0f - x_); yzMid[3] = eyebox[6] * x_ + eyebox[7] * (1.0f - x_); Vector3DF zMid[2]; zMid[0] = yzMid[0] * y_ + yzMid[1] * (1.0f - y_); zMid[1] = yzMid[2] * y_ + yzMid[3] * (1.0f - y_); eyebox_[e] = zMid[0] * z_ + zMid[1] * (1.0f - z_); } Vector3DF max_(-FLT_MAX, -FLT_MAX, -FLT_MAX); Vector3DF min_(FLT_MAX, FLT_MAX, FLT_MAX); for (int32_t i = 0; i < 8; i++) { if (eyebox_[i].X > max_.X) max_.X = eyebox_[i].X; if (eyebox_[i].Y > max_.Y) max_.Y = eyebox_[i].Y; if (eyebox_[i].Z > max_.Z) max_.Z = eyebox_[i].Z; if (eyebox_[i].X < min_.X) min_.X = eyebox_[i].X; if (eyebox_[i].Y < min_.Y) min_.Y = eyebox_[i].Y; if (eyebox_[i].Z < min_.Z) min_.Z = eyebox_[i].Z; } /* 範囲内に含まれるグリッドを取得 */ for (size_t i = 0; i < layers.size(); i++) { layers[i]->AddGrids(max_, min_, grids); } } } } /* 外領域追加 */ grids.push_back(&outofLayers); grids.push_back(&allLayers); /* グリッドからオブジェクト取得 */ for (size_t i = 0; i < grids.size(); i++) { for (size_t j = 0; j < grids[i]->GetObjects().size(); j++) { Object* o = grids[i]->GetObjects()[j]; ObjectInternal* o_ = (ObjectInternal*) o; if ( o_->GetNextStatus().Type == OBJECT_SHAPE_TYPE_ALL || IsInView(o_->GetPosition(), o_->GetNextStatus().CalcRadius(), facePositions, faceDir)) { objs.push_back(o); } } } /* 取得したグリッドを破棄 */ for (size_t i = 0; i < grids.size(); i++) { grids[i]->IsScanned = false; } grids.clear(); } else { grids.push_back(&allLayers); /* グリッドからオブジェクト取得 */ for (size_t i = 0; i < grids.size(); i++) { for (size_t j = 0; j < grids[i]->GetObjects().size(); j++) { Object* o = grids[i]->GetObjects()[j]; ObjectInternal* o_ = (ObjectInternal*) o; if (o_->GetNextStatus().Type == OBJECT_SHAPE_TYPE_ALL) { objs.push_back(o); } } } /* 取得したグリッドを破棄 */ for (size_t i = 0; i < grids.size(); i++) { grids[i]->IsScanned = false; } grids.clear(); } } bool WorldInternal::Reassign() { /* 数が少ない */ if (outofLayers.GetObjects().size() < 10) return false; objs.clear(); for (size_t i = 0; i < layers.size(); i++) { delete layers[i]; } layers.clear(); outofLayers.GetObjects().clear(); allLayers.GetObjects().clear(); outofLayers.IsScanned = false; allLayers.IsScanned = false; for (auto& it : containedObjects) { auto o = (ObjectInternal*) (it); o->ObjectIndex = -1; } float xmin = FLT_MAX; float xmax = -FLT_MAX; float ymin = FLT_MAX; float ymax = -FLT_MAX; float zmin = FLT_MAX; float zmax = -FLT_MAX; for (auto& it : containedObjects) { ObjectInternal* o_ = (ObjectInternal*) it; if (o_->GetNextStatus().Type == OBJECT_SHAPE_TYPE_ALL) continue; if (xmin > o_->GetNextStatus().Position.X) xmin = o_->GetNextStatus().Position.X; if (xmax < o_->GetNextStatus().Position.X) xmax = o_->GetNextStatus().Position.X; if (ymin > o_->GetNextStatus().Position.Y) ymin = o_->GetNextStatus().Position.Y; if (ymax < o_->GetNextStatus().Position.Y) ymax = o_->GetNextStatus().Position.Y; if (zmin > o_->GetNextStatus().Position.Z) zmin = o_->GetNextStatus().Position.Z; if (zmax < o_->GetNextStatus().Position.Z) zmax = o_->GetNextStatus().Position.Z; } auto xlen = Max(abs(xmax), abs(xmin)) * 2.0f; auto ylen = Max(abs(ymax), abs(ymin)) * 2.0f; auto zlen = Max(abs(zmax), abs(zmin)) * 2.0f; WorldInternal(xlen, ylen, zlen, this->layerCount); for (auto& it: containedObjects) { ObjectInternal* o_ = (ObjectInternal*) (it); AddObjectInternal(o_); } return true; } void WorldInternal::Dump(const char* path, const Matrix44& cameraProjMat, bool isOpenGL) { std::ofstream ofs(path); /* カメラ情報出力 */ Matrix44 cameraProjMatInv = cameraProjMat; cameraProjMatInv.SetInverted(); float maxx = 1.0f; float minx = -1.0f; float maxy = 1.0f; float miny = -1.0f; float maxz = 1.0f; float minz = 0.0f; if (isOpenGL) minz = -1.0f; Vector3DF eyebox[8]; eyebox[0 + 0] = Vector3DF(minx, miny, maxz); eyebox[1 + 0] = Vector3DF(maxx, miny, maxz); eyebox[2 + 0] = Vector3DF(minx, maxy, maxz); eyebox[3 + 0] = Vector3DF(maxx, maxy, maxz); eyebox[0 + 4] = Vector3DF(minx, miny, minz); eyebox[1 + 4] = Vector3DF(maxx, miny, minz); eyebox[2 + 4] = Vector3DF(minx, maxy, minz); eyebox[3 + 4] = Vector3DF(maxx, maxy, minz); for (int32_t i = 0; i < 8; i++) { eyebox[i] = cameraProjMatInv.Transform3D(eyebox[i]); } ofs << viewCullingXDiv << "," << viewCullingYDiv << "," << viewCullingZDiv << std::endl; for (int32_t i = 0; i < 8; i++) { ofs << eyebox[i].X << "," << eyebox[i].Y << "," << eyebox[i].Z << std::endl; } ofs << std::endl; for (int32_t z = 0; z < viewCullingZDiv; z++) { for (int32_t y = 0; y < viewCullingYDiv; y++) { for (int32_t x = 0; x < viewCullingXDiv; x++) { Vector3DF eyebox_[8]; float xsize = 1.0f / (float) viewCullingXDiv; float ysize = 1.0f / (float) viewCullingYDiv; float zsize = 1.0f / (float) viewCullingZDiv; for (int32_t e = 0; e < 8; e++) { float x_, y_, z_; if (e == 0){ x_ = xsize * x; y_ = ysize * y; z_ = zsize * z; } if (e == 1){ x_ = xsize * (x + 1); y_ = ysize * y; z_ = zsize * z; } if (e == 2){ x_ = xsize * x; y_ = ysize * (y + 1); z_ = zsize * z; } if (e == 3){ x_ = xsize * (x + 1); y_ = ysize * (y + 1); z_ = zsize * z; } if (e == 4){ x_ = xsize * x; y_ = ysize * y; z_ = zsize * (z + 1); } if (e == 5){ x_ = xsize * (x + 1); y_ = ysize * y; z_ = zsize * (z + 1); } if (e == 6){ x_ = xsize * x; y_ = ysize * (y + 1); z_ = zsize * (z + 1); } if (e == 7){ x_ = xsize * (x + 1); y_ = ysize * (y + 1); z_ = zsize * (z + 1); } Vector3DF yzMid[4]; yzMid[0] = eyebox[0] * x_ + eyebox[1] * (1.0f - x_); yzMid[1] = eyebox[2] * x_ + eyebox[3] * (1.0f - x_); yzMid[2] = eyebox[4] * x_ + eyebox[5] * (1.0f - x_); yzMid[3] = eyebox[6] * x_ + eyebox[7] * (1.0f - x_); Vector3DF zMid[2]; zMid[0] = yzMid[0] * y_ + yzMid[1] * (1.0f - y_); zMid[1] = yzMid[2] * y_ + yzMid[3] * (1.0f - y_); eyebox_[e] = zMid[0] * z_ + zMid[1] * (1.0f - z_); } Vector3DF max_(-FLT_MAX, -FLT_MAX, -FLT_MAX); Vector3DF min_(FLT_MAX, FLT_MAX, FLT_MAX); for (int32_t i = 0; i < 8; i++) { if (eyebox_[i].X > max_.X) max_.X = eyebox_[i].X; if (eyebox_[i].Y > max_.Y) max_.Y = eyebox_[i].Y; if (eyebox_[i].Z > max_.Z) max_.Z = eyebox_[i].Z; if (eyebox_[i].X < min_.X) min_.X = eyebox_[i].X; if (eyebox_[i].Y < min_.Y) min_.Y = eyebox_[i].Y; if (eyebox_[i].Z < min_.Z) min_.Z = eyebox_[i].Z; } ofs << x << "," << y << "," << z << std::endl; for (int32_t i = 0; i < 8; i++) { ofs << eyebox_[i].X << "," << eyebox_[i].Y << "," << eyebox_[i].Z << std::endl; } ofs << max_.X << "," << max_.Y << "," << max_.Z << std::endl; ofs << min_.X << "," << min_.Y << "," << min_.Z << std::endl; ofs << std::endl; } } } ofs << std::endl; /* レイヤー情報 */ ofs << layers.size() << std::endl; for (size_t i = 0; i < layers.size(); i++) { auto& layer = layers[i]; ofs << layer->GetGridXCount() << "," << layer->GetGridYCount() << "," << layer->GetGridZCount() << "," << layer->GetOffsetX() << "," << layer->GetOffsetY() << "," << layer->GetOffsetZ() << "," << layer->GetGridSize() << std::endl; for (size_t j = 0; j < layer->GetGrids().size(); j++) { auto& grid = layer->GetGrids()[j]; if (grid.GetObjects().size() > 0) { ofs << j << "," << grid.GetObjects().size() << std::endl; } } } Culling(cameraProjMat, isOpenGL); }
float_type operator()(float_type const & x, float_type const & y) const { return max_(x, y); }
float_type operator()(float_type const & f, float_type const & limit) const { float_type zero(0); float_type neg = zero - float_type(limit); return max_(neg, min_(f, limit)); }
void visualizeOdometry_GT(node_map& nodes, FrameGenerator& frame_gen, bool estimate) { int size_px = 800; float border_m = 0.25; // size of border around bounding box cv::Mat odo_img(size_px, size_px,CV_32FC3); odo_img.setTo(cv::Scalar::all(0)); // compute the track's bounding box cv::Point2f min_(1e5,1e5); cv::Point2f max_(-1e5,-1e5); for (node_it it = nodes.begin(); it != nodes.end(); ++it) { cv::Point2f p; if (estimate){ p.x = (*it).second.opt_pose(0,3); p.y = (*it).second.opt_pose(1,3); } else p = (*it).second.gt_pos_xy; min_.x = min(min_.x,p.x); max_.x = max(max_.x,p.x); min_.y = min(min_.y,p.y); max_.y = max(max_.y,p.y); } // 0.5 Abstand um die Bounding Box double m2px_x = (2*border_m+ (max_.x-min_.x))/size_px; double m2px_y = (2*border_m+ (max_.y-min_.y))/size_px; double m2px = max(m2px_x,m2px_y); // ROS_INFO("m2px: %f", m2px); // ROS_INFO("min: %f %f", min_.x, min_.y); map<uint, cv::Point> px_pos; for (node_it it = nodes.begin(); it != nodes.end(); ++it) { cv::Point2f p; if (estimate){ p.x = (*it).second.opt_pose(0,3); p.y = (*it).second.opt_pose(1,3); } else p = (*it).second.gt_pos_xy; // printf("gt_pos: %f %f \n", p.x,p.y); px_pos[it->first] = cv::Point2i( (p.x-(min_.x-border_m))/m2px , (p.y-(min_.y-border_m))/m2px ) ; // ROS_INFO("px: %i %i",px_pos[it->first].x, px_pos[it->first].y ); // px_pos[it->first] = cv::Point2i( (border_m+(p.x-min_.x))/m2px, (border_m+(p.y-min_.y))/m2px ); } for (float x = floor(min_.x-border_m); x <= ceil(max_.x+border_m); x+=0.25) { int px_x = (x-(min_.x-border_m))/m2px; cv::line(odo_img,cv::Point2i(px_x,0),cv::Point2i(px_x,size_px), cv::Scalar(255,255,255),1); } for (float y = floor(min_.y-border_m); y <= ceil(max_.y+border_m); y+=0.25) { int px_y = (y-(min_.y-border_m))/m2px; cv::line(odo_img,cv::Point2i(0,px_y),cv::Point2i(size_px,px_y), cv::Scalar(255,255,255),1); } // show node positions for (node_it it = nodes.begin(); it != nodes.end(); ++it) { Node* current = &(*it).second; if (current->is_keyframe) cv::circle(odo_img,px_pos[current->node_id],3,cv::Scalar(255,0,0),1 ); else cv::circle(odo_img,px_pos[current->node_id],1,cv::Scalar(255,255,255),1 ); } // if (nodes.size() > 0){ // Node* last = &nodes.rbegin()->second; // for (uint i=0; i<last->tree_proposals.size(); ++i) // cv::line(odo_img,px_pos[last->node_id],px_pos[nodes[last->tree_proposals[i]].node_id],cv::Scalar::all(255),1); // } // show edges: for (node_it it = nodes.begin(); it != nodes.end(); ++it) { Node* current = &(*it).second; for (uint j=0; j<current->matches.size(); j++) { Node_match* nm = ¤t->matches.at(j); if (current->node_id < nm->other_id) continue; // ROS_INFO("edges: %i %i", current->node_id, nm->other_id); Node* neighbour = &nodes[nm->other_id]; int r,g,b; r=g=b=0; // ROS_INFO("type: %i", nm->type); switch (nm->type) { case ET_Direct_neighbour: r = b = 255; break; // yellow case ET_Tree_Proposal: r = 255; break; // red case ET_Ring: g = 255; break; case ET_Last_match: g = r = 255; break; default: r=g=b=255; break; } cv::Scalar col = cv::Scalar(b,g,r); cv::line(odo_img,px_pos[current->node_id],px_pos[neighbour->node_id],col,1); } } // if (frame_gen.node_id_running % 10 == 0){ // char buffer[300]; // sprintf(buffer, "/home/engelhar/Desktop/img/%i.png", (int)frame_gen.node_id_running);; // cv::imwrite(buffer, odo_img); // } if (estimate){ cv::namedWindow("pos_Est",1); cv::imshow("pos_Est",odo_img); } else { cv::namedWindow("odometry_GT",1); cv::imshow("odometry_GT",odo_img); } cv::waitKey(10); }