bool MSP3D::is_in(octomap::point3d pt,std::pair<octomap::point3d,double> node){
double l=0.5*node.second;
if(fabs(pt.x()-node.first.x())<l && fabs(pt.y()-node.first.y())<l && fabs(pt.z()-node.first.z())<l){
return true;
}
return false;
}
/**
* Compute boundig box
*/
void srs_env_model::COcFilterRaycast::computeBBX(const std_msgs::Header& sensor_header, octomap::point3d& bbx_min, octomap::point3d& bbx_max)
{
std::string sensor_frame = sensor_header.frame_id;
// transform sensor FOV
geometry_msgs::PointStamped stamped_in;
geometry_msgs::PointStamped stamped_out;
stamped_in.header = sensor_header;
stamped_in.header.frame_id = sensor_frame;
// get max 3d points from camera at 0.5m and 5m.
geometry_msgs::Point p[8];
// define min/max 2d points
cv::Point2d uv[4];
uv[0].x = m_camera_stereo_offset_left;
uv[0].y = 0;
uv[1].x = m_camera_size.width + m_camera_stereo_offset_right;
uv[1].y = 0;
uv[2].x = m_camera_size.width + m_camera_stereo_offset_right;
uv[2].y = m_camera_size.height;
uv[3].x = m_camera_stereo_offset_left;
uv[3].y = m_camera_size.height;
// transform to 3d space
cv::Point3d xyz[4];
for (int i = 0; i < 4; i++) {
xyz[i] = m_camera_model.projectPixelTo3dRay(uv[i]);
cv::Point3d xyz_05 = xyz[i] * 0.5;
xyz[i] *= 5.; // 5meters
p[i].x = xyz[i].x;
p[i].y = xyz[i].y;
p[i].z = xyz[i].z;
p[i + 4].x = xyz_05.x;
p[i + 4].y = xyz_05.y;
p[i + 4].z = xyz_05.z;
}
// transform to world coodinates and find axis-aligned bbx
bbx_min.x() = bbx_min.y() = bbx_min.z() = 1e6;
bbx_max.x() = bbx_max.y() = bbx_max.z() = -1e6;
for (int i = 0; i < 8; i++) {
stamped_in.point = p[i];
m_tfListener.transformPoint(m_treeFrameId, stamped_in,
stamped_out);
p[i].x = stamped_out.point.x;
p[i].y = stamped_out.point.y;
p[i].z = stamped_out.point.z;
if (p[i].x < bbx_min.x())
bbx_min.x() = p[i].x;
if (p[i].y < bbx_min.y())
bbx_min.y() = p[i].y;
if (p[i].z < bbx_min.z())
bbx_min.z() = p[i].z;
if (p[i].x > bbx_max.x())
bbx_max.x() = p[i].x;
if (p[i].y > bbx_max.y())
bbx_max.y() = p[i].y;
if (p[i].z > bbx_max.z())
bbx_max.z() = p[i].z;
}
#ifdef SHOW_VISUALIZATION
std::cerr << "Publishing visualization marker publisher" << std::endl;
std::string fixed_frame_(m_treeFrameId);
// // visualize axis-aligned querying bbx
visualization_msgs::Marker bbx;
bbx.header.frame_id = fixed_frame_;
bbx.header.stamp = ros::Time::now();
bbx.ns = "collider";
bbx.id = 1;
bbx.action = visualization_msgs::Marker::ADD;
bbx.type = visualization_msgs::Marker::CUBE;
bbx.pose.orientation.w = 1.0;
bbx.pose.position.x = (bbx_min.x() + bbx_max.x()) / 2.;
bbx.pose.position.y = (bbx_min.y() + bbx_max.y()) / 2.;
bbx.pose.position.z = (bbx_min.z() + bbx_max.z()) / 2.;
bbx.scale.x = bbx_max.x()-bbx_min.x();
bbx.scale.y = bbx_max.y()-bbx_min.y();
bbx.scale.z = bbx_max.z()-bbx_min.z();
bbx.color.g = 1;
bbx.color.a = 0.3;
marker_pub_.publish(bbx);
// visualize sensor cone
visualization_msgs::Marker bbx_points;
bbx_points.header.frame_id = fixed_frame_;
bbx_points.header.stamp = ros::Time::now();
bbx_points.ns = "collider";
bbx_points.id = 2;
bbx_points.action = visualization_msgs::Marker::ADD;
bbx_points.type = visualization_msgs::Marker::LINE_STRIP;
bbx_points.pose.orientation.w = 1.0;
bbx_points.scale.x = 0.02;
bbx_points.scale.y = 0.02;
bbx_points.color.g = 1;
bbx_points.color.a = 0.3;
bbx_points.points.push_back(p[0]);
bbx_points.points.push_back(p[1]);
bbx_points.points.push_back(p[2]);
bbx_points.points.push_back(p[3]);
bbx_points.points.push_back(p[0]);
bbx_points.points.push_back(p[4]);
bbx_points.points.push_back(p[5]);
bbx_points.points.push_back(p[6]);
bbx_points.points.push_back(p[7]);
bbx_points.points.push_back(p[4]);
bbx_points.points.push_back(p[7]);
bbx_points.points.push_back(p[3]);
bbx_points.points.push_back(p[2]);
bbx_points.points.push_back(p[6]);
bbx_points.points.push_back(p[5]);
bbx_points.points.push_back(p[1]);
marker_pub_.publish(bbx_points);
#endif
}
/** Builds a renderizable representation of the octomap as a mrpt::opengl::COctoMapVoxels object. */
void CColouredOctoMap::getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels &gl_obj) const
{
// Go thru all voxels:
//OcTreeVolume voxel; // current voxel, possibly transformed
octomap::ColorOcTree::tree_iterator it_end = m_octomap.end_tree();
const unsigned char max_depth = 0; // all
const TColorf general_color = gl_obj.getColor();
const TColor general_color_u(general_color.R*255,general_color.G*255,general_color.B*255,general_color.A*255);
gl_obj.clear();
gl_obj.reserveGridCubes( this->calcNumNodes() );
gl_obj.resizeVoxelSets(2); // 2 sets of voxels: occupied & free
gl_obj.showVoxels(VOXEL_SET_OCCUPIED, renderingOptions.visibleOccupiedVoxels );
gl_obj.showVoxels(VOXEL_SET_FREESPACE, renderingOptions.visibleFreeVoxels );
const size_t nLeafs = this->getNumLeafNodes();
gl_obj.reserveVoxels(VOXEL_SET_OCCUPIED, nLeafs);
gl_obj.reserveVoxels(VOXEL_SET_FREESPACE, nLeafs);
double xmin, xmax, ymin, ymax, zmin, zmax;
this->getMetricMin(xmin, ymin, zmin);
this->getMetricMax(xmax, ymax, zmax);
for(octomap::ColorOcTree::tree_iterator it = m_octomap.begin_tree(max_depth);it!=it_end; ++it)
{
const octomap::point3d vx_center = it.getCoordinate();
const double vx_length = it.getSize();
const double L = 0.5*vx_length;
if (it.isLeaf())
{
// voxels for leaf nodes
const double occ = it->getOccupancy();
if ( (occ>=0.5 && renderingOptions.generateOccupiedVoxels) ||
(occ<0.5 && renderingOptions.generateFreeVoxels) )
{
mrpt::utils::TColor vx_color;
octomap::ColorOcTreeNode::Color node_color = it->getColor();
vx_color = TColor(node_color.r, node_color.g, node_color.b);
const size_t vx_set = (m_octomap.isNodeOccupied(*it)) ? VOXEL_SET_OCCUPIED:VOXEL_SET_FREESPACE;
gl_obj.push_back_Voxel(vx_set,COctoMapVoxels::TVoxel( TPoint3D(vx_center.x(),vx_center.y(),vx_center.z()) ,vx_length, vx_color) );
}
}
else if (renderingOptions.generateGridLines)
{
// Not leaf-nodes:
const mrpt::math::TPoint3D pt_min ( vx_center.x() - L, vx_center.y() - L, vx_center.z() - L);
const mrpt::math::TPoint3D pt_max ( vx_center.x() + L, vx_center.y() + L, vx_center.z() + L);
gl_obj.push_back_GridCube( COctoMapVoxels::TGridCube( pt_min, pt_max ) );
}
} // end for each voxel
// if we use transparency, sort cubes by "Z" as an approximation to far-to-near render ordering:
if (gl_obj.isCubeTransparencyEnabled())
gl_obj.sort_voxels_by_z();
// Set bounding box:
{
mrpt::math::TPoint3D bbmin, bbmax;
this->getMetricMin(bbmin.x,bbmin.y,bbmin.z);
this->getMetricMax(bbmax.x,bbmax.y,bbmax.z);
gl_obj.setBoundingBox(bbmin, bbmax);
}
}
/** Builds a renderizable representation of the octomap as a mrpt::opengl::COctoMapVoxels object. */
void COctoMap::getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels &gl_obj) const
{
// Go thru all voxels:
//OcTreeVolume voxel; // current voxel, possibly transformed
octomap::OcTree::tree_iterator it_end = m_octomap.end_tree();
const unsigned char max_depth = 0; // all
const TColorf general_color = gl_obj.getColor();
const TColor general_color_u(general_color.R*255,general_color.G*255,general_color.B*255,general_color.A*255);
gl_obj.clear();
gl_obj.reserveGridCubes( this->calcNumNodes() );
gl_obj.resizeVoxelSets(2); // 2 sets of voxels: occupied & free
gl_obj.showVoxels(VOXEL_SET_OCCUPIED, renderingOptions.visibleOccupiedVoxels );
gl_obj.showVoxels(VOXEL_SET_FREESPACE, renderingOptions.visibleFreeVoxels );
const size_t nLeafs = this->getNumLeafNodes();
gl_obj.reserveVoxels(VOXEL_SET_OCCUPIED, nLeafs);
gl_obj.reserveVoxels(VOXEL_SET_FREESPACE, nLeafs);
double xmin, xmax, ymin, ymax, zmin, zmax, inv_dz;
this->getMetricMin(xmin, ymin, zmin);
this->getMetricMax(xmax, ymax, zmax);
inv_dz = 1/(zmax-zmin + 0.01);
for(octomap::OcTree::tree_iterator it = m_octomap.begin_tree(max_depth); it!=it_end; ++it)
{
const octomap::point3d vx_center = it.getCoordinate();
const double vx_length = it.getSize();
const double L = 0.5*vx_length;
if (it.isLeaf())
{
// voxels for leaf nodes
const double occ = it->getOccupancy();
if ( (occ>=0.5 && renderingOptions.generateOccupiedVoxels) ||
(occ<0.5 && renderingOptions.generateFreeVoxels) )
{
mrpt::utils::TColor vx_color;
double coefc, coeft;
switch (gl_obj.getVisualizationMode()) {
case COctoMapVoxels::FIXED:
vx_color = general_color_u;
break;
case COctoMapVoxels::COLOR_FROM_HEIGHT:
coefc = 255*inv_dz*(vx_center.z()-zmin);
vx_color = TColor(coefc*general_color.R, coefc*general_color.G, coefc*general_color.B, 255.0*general_color.A);
break;
case COctoMapVoxels::COLOR_FROM_OCCUPANCY:
coefc = 240*(1-occ) + 15;
vx_color = TColor(coefc*general_color.R, coefc*general_color.G, coefc*general_color.B, 255.0*general_color.A);
break;
case COctoMapVoxels::TRANSPARENCY_FROM_OCCUPANCY:
coeft = 255 - 510*(1-occ);
if (coeft < 0) {
coeft = 0;
}
vx_color = TColor(255*general_color.R, 255*general_color.G, 255*general_color.B, coeft);
break;
case COctoMapVoxels::TRANS_AND_COLOR_FROM_OCCUPANCY:
coefc = 240*(1-occ) + 15;
vx_color = TColor(coefc*general_color.R, coefc*general_color.G, coefc*general_color.B, 50);
break;
case COctoMapVoxels::MIXED:
coefc = 255*inv_dz*(vx_center.z()-zmin);
coeft = 255 - 510*(1-occ);
if (coeft < 0) {
coeft = 0;
}
vx_color = TColor(coefc*general_color.R, coefc*general_color.G, coefc*general_color.B, coeft);
break;
default:
THROW_EXCEPTION("Unknown coloring scheme!")
}
const size_t vx_set = (m_octomap.isNodeOccupied(*it)) ? VOXEL_SET_OCCUPIED:VOXEL_SET_FREESPACE;
gl_obj.push_back_Voxel(vx_set,COctoMapVoxels::TVoxel( TPoint3D(vx_center.x(),vx_center.y(),vx_center.z()) ,vx_length, vx_color) );
}
}
else if (renderingOptions.generateGridLines)
bool operator() (const octomap::point3d& lhs, const octomap::point3d& rhs) const
{
return lhs.x() < rhs.x() || lhs.y() < rhs.y() || lhs.z() < rhs.z();
}
