/**
*
* 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);
}
