int main(int argc, char** argv) {
// default values:
string vrmlFilename = "";
string btFilename = "";
if (argc != 2 || (argc > 1 && strcmp(argv[1], "-h") == 0)){
printUsage(argv[0]);
}
btFilename = std::string(argv[1]);
vrmlFilename = btFilename + ".wrl";
cout << "\nReading OcTree file\n===========================\n";
// TODO: check if file exists and if OcTree read correctly?
OcTree* tree = new OcTree(btFilename);
cout << "\nWriting occupied volumes to VRML\n===========================\n";
std::ofstream outfile (vrmlFilename.c_str());
outfile << "#VRML V2.0 utf8\n#\n";
outfile << "# created from OctoMap file "<<btFilename<< " with bt2vrml\n";
size_t count(0);
for(OcTree::leaf_iterator it = tree->begin(), end=tree->end(); it!= end; ++it) {
if(tree->isNodeOccupied(*it)){
count++;
double size = it.getSize();
outfile << "Transform { translation "
<< it.getX() << " " << it.getY() << " " << it.getZ()
<< " \n children ["
<< " Shape { geometry Box { size "
<< size << " " << size << " " << size << "} } ]\n"
<< "}\n";
}
}
delete tree;
outfile.close();
std::cout << "Finished writing "<< count << " voxels to " << vrmlFilename << std::endl;
return 0;
}
//float xx[70000], yy[70000], zz[70000];
void findProbabilityOfCones3D(Frustum frustum[], int num_poses3D)
{
OcTree* input_tree = retrieve_octree(); //new OcTree(map)
int free = 0;
int occupied = 0;
std::cerr<<"Inside findProbabilityOfCones3D"<< std::endl;
for(OcTree::leaf_iterator it = input_tree->begin_leafs(),
end=input_tree->end_leafs(); it!= end; ++it)
{
if (input_tree->isNodeOccupied(*it))
{
double size = it.getSize();
double x = it.getX();
double y = it.getY();
double z = it.getZ();
//point3d p = it.getCoordinate();
//xx[occupied] = p.x;
//yy[occupied] = p.y;
//zz[occupied] = p.z;
occupied++;
//std::cerr<<"X: "<< x<< std::endl;
//std::cerr<<"Y: "<< y<< std::endl;
//std::cerr<<"Z: "<< z<< std::endl;
for (int i = 0; i < num_poses3D; i++)
{
if (checkInsideCone3D(x, y, z, frustum[i]))
{
frustum[i].probability += 1 + (QSR_WEIGHT * (normal_dist_2d(x, y , QSR_MEAN_1 , QSR_VAR_1 , QSR_MEAN_2 , QSR_VAR_2)));
}
}
}
else
{
free++;
}
}
std::cerr<<"occupied "<< occupied<< std::endl;
std::cerr<<"free "<< free<< std::endl;
}
int compute_value(Frustum frustum, std::vector<unsigned short int> &keys, std::vector<int> &node_values)
{
int value = 0;
int free = 0;
int occupied = 0;
int WEIGHT = 1; // compute weight from QSR model
if (input_tree == NULL)
return 0;
for(OcTree::leaf_iterator it = input_tree->begin_leafs(),
end=input_tree->end_leafs(); it!= end; ++it)
{
if (input_tree->isNodeOccupied(*it))
{
int size = (int) (it.getSize() / input_tree->getResolution());
//ROS_INFO("Size %f", size);
double x = it.getX();
double y = it.getY();
double z = it.getZ();
//std::cerr<< "xyz:" << x << "," << y << "," << z << " hash:" << hash << std::endl;
occupied++;
Vec3 point(x, y, z);
if (frustum.is_inside(point))
{
//ROS_INFO("Node inside frustum");
int node_value = WEIGHT * size;
const OcTreeKey key = it.getKey();
OcTreeKey::KeyHash computeHash;
unsigned short int hash = computeHash(key);
keys.push_back(hash);
node_values.push_back(node_value);
value += node_value;
}
}
else
{
free++;
}
}
//std::cerr<<"occupied "<< occupied<< std::endl;
//std::cerr<<"free "<< free<< std::endl;
return value;
}
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
// Usage:
// Constructors/Destructor:
// octree = octomapWrapper(resolution); // constructor: new tree with
// specified resolution
// octree = octomapWrapper(filename); // constructor: load from file
// octomapWrapper(octree); // destructor
//
// Queries:
// results = octomapWrapper(octree, 1, pts) // search
// leaf_nodes = octomapWrapper(octree, 2) // getLeafNodes
//
// Update tree:
// octomapWrapper(octree, 11, pts, occupied) // updateNote(pts, occupied).
// pts is 3-by-n, occupied is 1-by-n logical
//
// General operations:
// octomapWrapper(octree, 21, filename) // save to file
OcTree* tree = NULL;
if (nrhs == 1) {
if (mxIsNumeric(prhs[0])) { // constructor w/ resolution
if (nlhs > 0) {
double resolution = mxGetScalar(prhs[0]);
// mexPrintf("Creating octree w/ resolution %f\n", resolution);
tree = new OcTree(resolution);
plhs[0] = createDrakeMexPointer((void*)tree, "OcTree");
}
} else if (mxIsChar(prhs[0])) {
if (nlhs > 0) {
char* filename = mxArrayToString(prhs[0]);
// mexPrintf("Loading octree from %s\n", filename);
tree = new OcTree(filename);
plhs[0] = createDrakeMexPointer((void*)tree, "OcTree");
mxFree(filename);
}
} else { // destructor. note: assumes prhs[0] is a DrakeMexPointer (todo:
// could check)
// mexPrintf("Deleting octree\n");
destroyDrakeMexPointer<OcTree*>(prhs[0]);
}
return;
}
tree = (OcTree*)getDrakeMexPointer(prhs[0]);
int COMMAND = (int)mxGetScalar(prhs[1]);
switch (COMMAND) {
case 1: // search
{
mexPrintf("octree search\n");
if (mxGetM(prhs[2]) != 3)
mexErrMsgTxt("octomapWrapper: pts must be 3-by-n");
int n = mxGetN(prhs[2]);
double* pts = mxGetPrSafe(prhs[2]);
if (nlhs > 0) {
plhs[0] = mxCreateDoubleMatrix(1, n, mxREAL);
double* presults = mxGetPrSafe(plhs[0]);
for (int i = 0; i < n; i++) {
OcTreeNode* result =
tree->search(pts[3 * i], pts[3 * i + 1], pts[3 * i + 2]);
if (result == NULL)
presults[i] = -1.0;
else
presults[i] = result->getOccupancy();
}
}
} break;
case 2: // get leaf nodes
{
// mexPrintf("octree get leaf nodes\n");
int N = tree->getNumLeafNodes();
plhs[0] = mxCreateDoubleMatrix(3, N, mxREAL);
double* leaf_xyz = mxGetPrSafe(plhs[0]);
double* leaf_value = NULL, * leaf_size = NULL;
if (nlhs > 1) { // return value
plhs[1] = mxCreateDoubleMatrix(1, N, mxREAL);
leaf_value = mxGetPrSafe(plhs[1]);
}
if (nlhs > 2) { // return size
plhs[2] = mxCreateDoubleMatrix(1, N, mxREAL);
leaf_size = mxGetPrSafe(plhs[2]);
}
for (OcTree::leaf_iterator leaf = tree->begin_leafs(),
end = tree->end_leafs();
leaf != end; ++leaf) {
leaf_xyz[0] = leaf.getX();
leaf_xyz[1] = leaf.getY();
leaf_xyz[2] = leaf.getZ();
leaf_xyz += 3;
if (leaf_value) *leaf_value++ = leaf->getValue();
if (leaf_size) *leaf_size++ = leaf.getSize();
}
} break;
case 11: // add occupied pts
{
// mexPrintf("octree updateNode\n");
if (mxGetM(prhs[2]) != 3)
mexErrMsgTxt("octomapWrapper: pts must be 3-by-n");
int n = mxGetN(prhs[2]);
double* pts = mxGetPrSafe(prhs[2]);
mxLogical* occupied = mxGetLogicals(prhs[3]);
for (int i = 0; i < n; i++) {
tree->updateNode(pts[3 * i], pts[3 * i + 1], pts[3 * i + 2],
occupied[i]);
}
} break;
case 12: // insert a scan of endpoints and sensor origin
{
// pointsA should be 3xN, originA is 3x1
double* points = mxGetPrSafe(prhs[2]);
double* originA = mxGetPrSafe(prhs[3]);
int n = mxGetN(prhs[2]);
point3d origin((float)originA[0], (float)originA[1], (float)originA[2]);
Pointcloud pointCloud;
for (int i = 0; i < n; i++) {
point3d point((float)points[3 * i], (float)points[3 * i + 1],
(float)points[3 * i + 2]);
pointCloud.push_back(point);
}
tree->insertPointCloud(pointCloud, origin);
} break;
case 21: // save to file
{
char* filename = mxArrayToString(prhs[2]);
// mexPrintf("writing octree to %s\n", filename);
tree->writeBinary(filename);
mxFree(filename);
} break;
default:
mexErrMsgTxt("octomapWrapper: Unknown command");
}
}
void execute(const fremen::informationGoalConstPtr& goal, Server* as)
{
/* Octmap Estimation and Visualization */
octomap_msgs::Octomap bmap_msg;
OcTree octree (resolution);
geometry_msgs::Point initialPt, finalPt;
//Create pointcloud:
octomap::Pointcloud octoCloud;
sensor_msgs::PointCloud fremenCloud;
float x = 0*gridPtr->positionX;
float y = 0*gridPtr->positionY;
geometry_msgs::Point32 test_point;
int cnt = 0;
int cell_x, cell_y, cell_z;
cell_x = (int)(goal->x/resolution);
cell_y = (int)(goal->y/resolution);
cell_z = (int)(head_height/resolution);
for(double i = LIM_MIN_X; i < LIM_MAX_X; i+=resolution){
for(double j = LIM_MIN_Y; j < LIM_MAX_Y; j+=resolution){
for(double w = LIM_MIN_Z; w < LIM_MAX_Z; w+=resolution){
point3d ptt(x+i+resolution/2,y+j+resolution/2,w+resolution/2);
int s = goal->stamp;
if(gridPtr->retrieve(cnt, goal->stamp)>0)
{
//finalPt.z = (int)((w+resolution/2)/resolution)-cell_z;
//finalPt.y = (int)((j+resolution/2)/resolution)-cell_y;
//finalPt.x = (int)((i+resolution/2)/resolution)-cell_x;
//int cnta = ((cell_x+finalPt.x-LIM_MIN_X/resolution)*dim_y + (finalPt.y + cell_y-LIM_MIN_Y/resolution))*dim_z + (finalPt.z + cell_z-LIM_MIN_Z/resolution);
//ROS_INFO("something %d %d",cnt,cnta);
octoCloud.push_back(x+i+resolution/2,y+j+resolution/2,w+resolution/2);
octree.updateNode(ptt,true,true);
}
cnt++;
}
}
}
//Update grid
octree.updateInnerOccupancy();
//init visualization markers:
visualization_msgs::MarkerArray occupiedNodesVis;
unsigned int m_treeDepth = octree.getTreeDepth();
//each array stores all cubes of a different size, one for each depth level:
occupiedNodesVis.markers.resize(m_treeDepth + 1);
geometry_msgs::Point cubeCenter;
std_msgs::ColorRGBA m_color;
m_color.r = 0.0;
m_color.g = 0.0;
m_color.b = 1.0;
m_color.a = 0.5;
for (unsigned i = 0; i < occupiedNodesVis.markers.size(); ++i)
{
double size = octree.getNodeSize(i);
occupiedNodesVis.markers[i].header.frame_id = "/map";
occupiedNodesVis.markers[i].header.stamp = ros::Time::now();
occupiedNodesVis.markers[i].ns = "map";
occupiedNodesVis.markers[i].id = i;
occupiedNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
occupiedNodesVis.markers[i].scale.x = size;
occupiedNodesVis.markers[i].scale.y = size;
occupiedNodesVis.markers[i].scale.z = size;
occupiedNodesVis.markers[i].color = m_color;
}
ROS_INFO("s %i",cnt++);
x = gridPtr->positionX;
y = gridPtr->positionY;
for(OcTree::leaf_iterator it = octree.begin_leafs(), end = octree.end_leafs(); it != end; ++it)
{
if(it != NULL && octree.isNodeOccupied(*it))
{
unsigned idx = it.getDepth();
cubeCenter.x = x+it.getX();
cubeCenter.y = y+it.getY();
cubeCenter.z = it.getZ();
occupiedNodesVis.markers[idx].points.push_back(cubeCenter);
double minX, minY, minZ, maxX, maxY, maxZ;
octree.getMetricMin(minX, minY, minZ);
octree.getMetricMax(maxX, maxY, maxZ);
double h = (1.0 - fmin(fmax((cubeCenter.z - minZ) / (maxZ - minZ), 0.0), 1.0)) * m_colorFactor;
occupiedNodesVis.markers[idx].colors.push_back(heightMapColorA(h));
}
}
/**** Robot Head Marker ****/
//Robot Position
visualization_msgs::Marker marker_head;
marker_head.header.frame_id = "/map";
marker_head.header.stamp = ros::Time();
marker_head.ns = "my_namespace";
marker_head.id = 1;
marker_head.type = visualization_msgs::Marker::SPHERE;
marker_head.action = visualization_msgs::Marker::ADD;
marker_head.pose.position.x = goal->x;
marker_head.pose.position.y = goal->y;
marker_head.pose.position.z = head_height;
marker_head.pose.orientation.x = 0.0;
marker_head.pose.orientation.y = 0.0;
marker_head.pose.orientation.z = 0.0;
marker_head.pose.orientation.w = 1.0;
marker_head.scale.x = 0.2;
marker_head.scale.y = 0.2;
marker_head.scale.z = 0.2;
marker_head.color.a = 1.0;
marker_head.color.r = 0.0;
marker_head.color.g = 1.0;
marker_head.color.b = 0.0;
/****** Ray Traversal ******/
//ROS_INFO("Robot Position (%f,%f,%f) = (%d,%d,%d)", goal->x, goal->y, head_height, cell_x, cell_y, cell_z);
//Ray Casting (Grid Traversal - Digital Differential Analyzer)
visualization_msgs::Marker marker_rays;
marker_rays.header.frame_id = "/map";
marker_rays.header.stamp = ros::Time();
marker_rays.ns = "my_namespace";
marker_rays.id = 2;
marker_rays.type = visualization_msgs::Marker::LINE_LIST;
marker_rays.action = visualization_msgs::Marker::ADD;
marker_rays.scale.x = 0.01;
geometry_msgs::Vector3 rayDirection, deltaT, cellIndex;
std_msgs::ColorRGBA line_color;
initialPt.x = goal->x;
initialPt.y = goal->y;
initialPt.z = head_height;
line_color.a = 0.2;
line_color.r = 0.0;
line_color.g = 0.0;
line_color.b = 1.0;
float delta, H;
bool free_cell;
ROS_INFO("Performing Ray Casting...");
H = 0;
int gridsize = dim_x*dim_y*dim_z;
for(float ang_v = -0.174; ang_v < 0.174; ang_v+=angular_step){
for(float ang_h = 0; ang_h < 2*M_PI; ang_h+= angular_step){ //0 - 360 degrees
//Initial conditions:
rayDirection.z = sin(ang_v);//z
rayDirection.x = cos(ang_v) * cos(ang_h);//x
rayDirection.y = cos(ang_v) * sin(ang_h);//y
delta = fmax(fmax(fabs(rayDirection.x), fabs(rayDirection.y)), fabs(rayDirection.z));
deltaT.x = rayDirection.x/delta;
deltaT.y = rayDirection.y/delta;
deltaT.z = rayDirection.z/delta;
free_cell = true;
int max_it = RANGE_MAX/resolution * 2/sqrt(pow(deltaT.x,2) + pow(deltaT.y,2) + pow(deltaT.z,2));
// ROS_INFO("Max_it: %d %d %d", cell_x,cell_y,cell_z);
finalPt.x = 0;
finalPt.y = 0;
finalPt.z = 0;
for(int i = 0; i < max_it && free_cell; i++){
finalPt.x += deltaT.x/2;
finalPt.y += deltaT.y/2;
finalPt.z += deltaT.z/2;
//cnt?
int cnt = ((int)((cell_x+finalPt.x-LIM_MIN_X/resolution))*dim_y + (int)(finalPt.y + cell_y-LIM_MIN_Y/resolution))*dim_z + (int)(finalPt.z + cell_z-LIM_MIN_Z/resolution);//get fremen grid index!
//TODO
if (cnt < 0){
cnt = 0;
free_cell = false;
}
if (cnt > gridsize-1){
cnt = gridsize-1;
free_cell = false;
}
//ROS_INFO("CNT: %d %d", cnt,gridsize);
//ROS_INFO("DIM %d %d", dim_x, dim_z);
if(gridPtr->retrieve(cnt, goal->stamp) > 0) free_cell = false;
if(aux_entropy[cnt] ==0){
aux_entropy[cnt] = 1;
//float p = gridPtr->estimate(cnt,goal->stamp);
//h+=-p*ln(p);
H++;
}
}
marker_rays.points.push_back(initialPt);
marker_rays.colors.push_back(line_color);
finalPt.x = finalPt.x*resolution+initialPt.x;
finalPt.y = finalPt.y*resolution+initialPt.y;
finalPt.z = finalPt.z*resolution+initialPt.z;
marker_rays.points.push_back(finalPt);
marker_rays.colors.push_back(line_color);
}
}
//Entropy (text marker):
visualization_msgs::Marker marker_text;
marker_text.header.frame_id = "/map";
marker_text.header.stamp = ros::Time();
marker_text.ns = "my_namespace";
marker_text.id = 1;
marker_text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
marker_text.action = visualization_msgs::Marker::ADD;
marker_text.pose.position.x = goal->x;
marker_text.pose.position.y = goal->y;
marker_text.pose.position.z = head_height + 2;
marker_text.pose.orientation.x = 0.0;
marker_text.pose.orientation.y = 0.0;
marker_text.pose.orientation.z = 0.0;
marker_text.pose.orientation.w = 1.0;
marker_text.scale.z = 0.5;
marker_text.color.a = 1.0;
marker_text.color.r = 0.0;
marker_text.color.g = 1.0;
marker_text.color.b = 0.0;
char output[1000];
sprintf(output,"Gain: %f",H);
marker_text.text = output;
//Publish Results:
ROS_INFO("Data published!");
head_pub_ptr->publish(marker_head);
estimate_pub_ptr->publish(occupiedNodesVis);
rays_pub_ptr->publish(marker_rays);
text_pub_ptr->publish(marker_text);
as->setSucceeded();
}
