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;
}
int main(int argc, char** argv) {
if (argc != 3)
printUsage(argv[0]);
string inputFilename = argv[1];
string outputFilename = argv[2];
// pcl point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr pclcloud( new pcl::PointCloud<pcl::PointXYZ>() );
pcl::io::loadPCDFile( inputFilename, *pclcloud );
// data conversion
Pointcloud * cloud = new Pointcloud;
for ( size_t i = 0; i < pclcloud->size(); ++ i ) {
point3d pt(pclcloud->points[i].x, pclcloud->points[i].y, pclcloud->points[i].z);
cloud->push_back( pt );
}
point3d sensor_origin(0,0,0);
OcTree* tree = new OcTree(0.1);
tree->insertPointCloud( cloud, sensor_origin );
tree->writeBinary(outputFilename);
}
OcTree* generateSphereTree(point3d origin, float radius){
OcTree* tree = new OcTree(0.05);
point3d point_on_surface = origin;
point_on_surface.x() += radius;
for (int i=0; i<360; i++) {
for (int j=0; j<360; j++) {
if (!tree->insertRay(origin, origin+point_on_surface)) {
cout << "ERROR while inserting ray from " << origin << " to " << point_on_surface << endl;
}
point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
}
point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
}
return tree;
}
void App::octreeHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const drc::map_octree_t* msg){
std::cout << "MAP_OCTREE received\n";
// TODO: Currently not handling transform, assuming identity transform
std::stringstream datastream;
datastream.write((const char*) msg->data.data(), msg->num_bytes);
tree_ = new octomap::OcTree(1); //resolution will be set by data from message
tree_->readBinary(datastream);
std::stringstream s;
s << "/tmp/map_octomap.bt" ;
printf("Saving MAP_OCTREE to: %s\n", s.str().c_str());
tree_->writeBinary(s.str().c_str());
exit(-1);
}
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);
std::list<OcTreeVolume> occupiedVoxels;
tree->getOccupied(occupiedVoxels);
delete tree;
cout << "\nWriting " << occupiedVoxels.size()<<" 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";
std::list<OcTreeVolume>::iterator it;
for (it = occupiedVoxels.begin(); it != occupiedVoxels.end(); ++it){
outfile << "Transform { translation "
<< it->first.x() << " " << it->first.y() << " " << it->first.z()
<< " \n children ["
<< " Shape { geometry Box { size "
<< it->second << " " << it->second << " " << it->second << "} } ]\n"
<< "}\n";
}
outfile.close();
std::cout << "Finished writing to " << vrmlFilename << std::endl;
}
OcTree* retrieve_octree()
{
ros::NodeHandle n;
std::string servname = "octomap_binary";
ROS_INFO("Requesting the map from %s...", n.resolveName(servname).c_str());
GetOctomap::Request req;
GetOctomap::Response resp;
while(n.ok() && !ros::service::call(servname, req, resp))
{
ROS_WARN("Request to %s failed; trying again...", n.resolveName(servname).c_str());
usleep(1000000);
}
OcTree* octree = octomap_msgs::binaryMsgToMap(resp.map);
if (octree){
ROS_INFO("Map received (%zu nodes, %f m res)", octree->size(), octree->getResolution());
return extract_supporting_planes(octree);
}
return NULL;
}
void printChanges(OcTree& tree){
unsigned int changedOccupied = 0;
unsigned int changedFree = 0;
unsigned int actualOccupied = 0;
unsigned int actualFree = 0;
unsigned int missingChanged = 0;
tree.expand();
// iterate through the changed nodes
KeyBoolMap::const_iterator it;
for (it=tree.changedKeysBegin(); it!=tree.changedKeysEnd(); it++) {
OcTreeNode* node = tree.search(it->first);
if (node != NULL) {
if (tree.isNodeOccupied(node)) {
changedOccupied += 1;
}
else {
changedFree += 1;
}
} else {
missingChanged +=1;
}
}
// iterate through the entire tree
for(OcTree::tree_iterator it=tree.begin_tree(),
end=tree.end_tree(); it!= end; ++it) {
if (it.isLeaf()) {
if (tree.isNodeOccupied(*it)) {
actualOccupied += 1;
}
else {
actualFree += 1;
}
}
}
cout << "change detection: " << changedOccupied << " occ; " << changedFree << " free; "<< missingChanged << " missing" << endl;
cout << "actual: " << actualOccupied << " occ; " << actualFree << " free; " << endl;
tree.prune();
}
int main(int argc, char** argv) {
cout << endl;
cout << "generating example map" << endl;
OcTree tree (0.1); // create empty tree with resolution 0.1
// insert some measurements of occupied cells
for (int x=-20; x<20; x++) {
for (int y=-20; y<20; y++) {
for (int z=-20; z<20; z++) {
point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
tree.updateNode(endpoint, true); // integrate 'occupied' measurement
}
}
}
// insert some measurements of free cells
for (int x=-30; x<30; x++) {
for (int y=-30; y<30; y++) {
for (int z=-30; z<30; z++) {
point3d endpoint ((float) x*0.02f-1.0f, (float) y*0.02f-1.0f, (float) z*0.02f-1.0f);
tree.updateNode(endpoint, false); // integrate 'free' measurement
}
}
}
cout << endl;
cout << "performing some queries:" << endl;
point3d query (0., 0., 0.);
OcTreeNode* result = tree.search (query);
print_query_info(query, result);
query = point3d(-1.,-1.,-1.);
result = tree.search (query);
print_query_info(query, result);
query = point3d(1.,1.,1.);
result = tree.search (query);
print_query_info(query, result);
cout << endl;
tree.writeBinary("simple_tree.bt");
cout << "wrote example file simple_tree.bt" << endl << endl;
cout << "now you can use octovis to visualize: octovis simple_tree.bt" << endl;
cout << "Hint: hit 'F'-key in viewer to see the freespace" << endl << endl;
}
// Callback function for the service 'view_eval'
bool view_eval(viper::ViewValue::Request &req,
viper::ViewValue::Response &res)
{
ROS_INFO("Received service request: view_eval (%f %f %f)", req.pose.position.x, req.pose.position.y, req.pose.position.z);
geometry_msgs::Pose camera_pose = req.pose;
//OcTree* octree = octomap_msgs::binaryMsgToMap(req.octomap);
AbstractOcTree* tree = octomap_msgs::fullMsgToMap(req.octomap);
if (!tree){
ROS_ERROR("Failed to recreate octomap");
return false;
}
OcTree* octree = dynamic_cast<OcTree*>(tree);
if (octree){
ROS_INFO("Map received (%zu nodes, %f m res)", octree->size(), octree->getResolution());
input_tree = extract_supporting_planes(octree);
} else{
ROS_ERROR("No map received!");
input_tree = NULL;
}
// Generate frustum.
Frustum f = generate_frustum(camera_pose);
int value = compute_value(f, res.keys, res.values);
ROS_INFO("VALUE: %i", value);
ROS_INFO("KEYS SIZE: %i", res.keys.size());
ROS_INFO("VALUES SIZE: %i", res.values.size());
res.value = value;
res.frustum = get_points(generate_local_frustum());
ROS_INFO("Finished service request.");
return true;
}
void generateBoxesFromOctomap(std::vector<CollisionObject*>& boxes, OcTree& tree)
{
std::vector<boost::array<FCL_REAL, 6> > boxes_ = tree.toBoxes();
for(std::size_t i = 0; i < boxes_.size(); ++i)
{
FCL_REAL x = boxes_[i][0];
FCL_REAL y = boxes_[i][1];
FCL_REAL z = boxes_[i][2];
FCL_REAL size = boxes_[i][3];
FCL_REAL cost = boxes_[i][4];
FCL_REAL threshold = boxes_[i][5];
Box* box = new Box(size, size, size);
box->cost_density = cost;
box->threshold_occupied = threshold;
CollisionObject* obj = new CollisionObject(boost::shared_ptr<CollisionGeometry>(box), Transform3f(Vec3f(x, y, z)));
boxes.push_back(obj);
}
std::cout << "boxes size: " << boxes.size() << std::endl;
}
int main(int argc, char** argv) {
//##############################################################
OcTree tree (0.05);
tree.enableChangeDetection(true);
point3d origin (0.01f, 0.01f, 0.02f);
point3d point_on_surface (4.01f,0.01f,0.01f);
tree.insertRay(origin, point_on_surface);
printChanges(tree);
tree.updateNode(point3d(2.01f, 0.01f, 0.01f), 2.0f);
printChanges(tree);
tree.updateNode(point3d(2.01f, 0.01f, 0.01f), -2.0f);
printChanges(tree);
cout << "generating spherical scan at " << origin << " ..." << endl;
for (int i=-100; i<101; i++) {
Pointcloud cloud;
for (int j=-100; j<101; j++) {
point3d rotated = point_on_surface;
rotated.rotate_IP(0, DEG2RAD(i*0.5), DEG2RAD(j*0.5));
cloud.push_back(rotated);
}
// insert in global coordinates:
tree.insertPointCloud(cloud, origin, -1);
}
printChanges(tree);
cout << "done." << endl;
return 0;
}
int main(int argc, char** argv) {
cout << endl;
cout << "generating example map" << endl;
OcTree tree (0.1); // create empty tree with resolution 0.1
// insert some measurements of occupied cells
for (int x=-20; x<20; x++) {
for (int y=-20; y<20; y++) {
for (int z=-20; z<20; z++) {
point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
tree.updateNode(endpoint, true); // integrate 'occupied' measurement
}
}
}
// insert some measurements of free cells
for (int x=-30; x<30; x++) {
for (int y=-30; y<30; y++) {
for (int z=-30; z<30; z++) {
point3d endpoint ((float) x*0.02f-1.0f, (float) y*0.02f-1.0f, (float) z*0.02f-1.0f);
tree.updateNode(endpoint, false); // integrate 'free' measurement
}
}
}
cout << endl;
cout << "performing some queries around the desired voxel:" << endl;
point3d query;
OcTreeNode* result = NULL;
for(float z = -0.6; z < -0.21; z += 0.1){
for(float y = -0.6; y < -0.21; y += 0.1){
for(float x = -0.6; x < -0.21; x += 0.1){
query = point3d(x, y, z);
result = tree.search(query);
print_query_info(query, result);
}
}
}
query = point3d(-0.5, -0.4, -0.4);
result = tree.search(query);
vector<point3d> normals;
if (tree.getNormals(query, normals)){
cout << endl;
string s_norm = (normals.size() > 1) ? " normals " : " normal ";
cout << "MC algorithm gives " << normals.size() << s_norm << "in voxel at " << query << endl;
for(unsigned i = 0; i < normals.size(); ++i)
cout << "\t" << normals[i].x() << "; " << normals[i].y() << "; " << normals[i].z() << endl;
} else{
cout << "query point unknown (no normals)\n";
}
}
int main(int argc, char** argv) {
if (argc != 2){
std::cerr << "Error: you need to specify a testfile (.bt) as argument to read" << std::endl;
return 1; // exit 1 means failure
}
std::cout << "Testing empty OcTree...\n";
//empty tree
{
OcTree emptyTree(0.999);
EXPECT_EQ(emptyTree.size(), 0);
EXPECT_TRUE(emptyTree.writeBinary("empty.bt"));
EXPECT_TRUE(emptyTree.write("empty.ot"));
OcTree emptyReadTree(0.2);
EXPECT_TRUE(emptyReadTree.readBinary("empty.bt"));
EXPECT_EQ(emptyReadTree.size(), 0);
EXPECT_TRUE(emptyTree == emptyReadTree);
AbstractOcTree* readTreeAbstract = AbstractOcTree::read("empty.ot");
EXPECT_TRUE(readTreeAbstract);
OcTree* readTreeOt = dynamic_cast<OcTree*>(readTreeAbstract);
EXPECT_TRUE(readTreeOt);
EXPECT_EQ(readTreeOt->size(), 0);
EXPECT_TRUE(emptyTree == *readTreeOt);
delete readTreeOt;
}
std::cout << "Testing reference OcTree from file ...\n";
string filename = string(argv[1]);
{
string filenameOt = "test_io_file.ot";
string filenameBtOut = "test_io_file.bt";
string filenameBtCopyOut = "test_io_file_copy.bt";
// read reference tree from input file
OcTree tree (0.1);
EXPECT_TRUE (tree.readBinary(filename));
std::cout << " Copy Constructor / assignment / ==\n";
// test copy constructor / assignment:
OcTree* treeCopy = new OcTree(tree);
EXPECT_TRUE(tree == *treeCopy);
EXPECT_TRUE(treeCopy->writeBinary(filenameBtCopyOut));
// change a tree property, trees must be different afterwards
treeCopy->setResolution(tree.getResolution()*2.0);
EXPECT_FALSE(tree == *treeCopy);
treeCopy->setResolution(tree.getResolution());
EXPECT_TRUE(tree == *treeCopy);
// flip one value, trees must be different afterwards:
point3d pt(0.5, 0.5, 0.5);
OcTreeNode* node = treeCopy->search(pt);
if (node && treeCopy->isNodeOccupied(node))
treeCopy->updateNode(pt, false);
else
treeCopy->updateNode(pt, true);
EXPECT_FALSE(tree == *treeCopy);
delete treeCopy;
std::cout << " Swap\n";
// test swap:
OcTree emptyT(tree.getResolution());
OcTree emptySw(emptyT);
OcTree otherSw(tree);
emptySw.swapContent(otherSw);
EXPECT_FALSE(emptyT == emptySw);
EXPECT_TRUE(emptySw == tree);
EXPECT_TRUE(otherSw == emptyT);
// write again to bt, read & compare
EXPECT_TRUE(tree.writeBinary(filenameBtOut));
OcTree readTreeBt(0.1);
EXPECT_TRUE(readTreeBt.readBinary(filenameBtOut));
EXPECT_TRUE(tree == readTreeBt);
std::cout <<" Write to .ot / read through AbstractOcTree\n";
// now write to .ot, read & compare
EXPECT_TRUE(tree.write(filenameOt));
AbstractOcTree* readTreeAbstract = AbstractOcTree::read(filenameOt);
EXPECT_TRUE(readTreeAbstract);
OcTree* readTreeOt = dynamic_cast<OcTree*>(readTreeAbstract);
EXPECT_TRUE(readTreeOt);
EXPECT_TRUE(tree == *readTreeOt);
// sanity test for "==": flip one node, compare again
point3d coord(0.1f, 0.1f, 0.1f);
node = readTreeOt->search(coord);
if (node && readTreeOt->isNodeOccupied(node))
readTreeOt->updateNode(coord, false);
else
readTreeOt->updateNode(coord, true);
EXPECT_FALSE(tree == *readTreeOt);
delete readTreeOt;
}
// Test for tree headers and IO factory registry (color)
{
std::cout << "Testing ColorOcTree...\n";
double res = 0.02;
std::string filenameColor = "test_io_color_file.ot";
ColorOcTree colorTree(res);
EXPECT_EQ(colorTree.getTreeType(), "ColorOcTree");
ColorOcTreeNode* colorNode = colorTree.updateNode(point3d(0.0, 0.0, 0.0), true);
ColorOcTreeNode::Color color_red(255, 0, 0);
colorNode->setColor(color_red);
colorTree.setNodeColor(0.0, 0.0, 0.0, 255, 0, 0);
colorTree.updateNode(point3d(0.1f, 0.1f, 0.1f), true);
colorTree.setNodeColor(0.1f, 0.1f, 0.1f, 0, 0, 255);
EXPECT_TRUE(colorTree.write(filenameColor));
AbstractOcTree* readTreeAbstract = AbstractOcTree::read(filenameColor);
EXPECT_TRUE(readTreeAbstract);
EXPECT_EQ(colorTree.getTreeType(), readTreeAbstract->getTreeType());
ColorOcTree* readColorTree = dynamic_cast<ColorOcTree*>(readTreeAbstract);
EXPECT_TRUE(readColorTree);
EXPECT_TRUE(colorTree == *readColorTree);
colorNode = colorTree.search(0.0, 0.0, 0.0);
EXPECT_TRUE(colorNode);
EXPECT_EQ(colorNode->getColor(), color_red);
delete readColorTree;
}
// Test for tree headers and IO factory registry (stamped)
{
std::cout << "Testing OcTreeStamped...\n";
double res = 0.05;
std::string filenameStamped = "test_io_stamped_file.ot";
OcTreeStamped stampedTree(res);
EXPECT_EQ(stampedTree.getTreeType(), "OcTreeStamped");
// TODO: add / modify some stamped nodes
//ColorOcTreeNode* colorNode = colorTree.updateNode(point3d(0.0, 0.0, 0.0), true);
//ColorOcTreeNode::Color color_red(255, 0, 0);
//colorNode->setColor(color_red);
//colorTree.setNodeColor(0.0, 0.0, 0.0, 255, 0, 0);
//colorTree.updateNode(point3d(0.1f, 0.1f, 0.1f), true);
//colorTree.setNodeColor(0.1f, 0.1f, 0.1f, 0, 0, 255);
EXPECT_TRUE(stampedTree.write(filenameStamped));
AbstractOcTree* readTreeAbstract = AbstractOcTree::read(filenameStamped);
EXPECT_TRUE(readTreeAbstract);
EXPECT_EQ(stampedTree.getTreeType(), readTreeAbstract->getTreeType());
OcTreeStamped* readStampedTree = dynamic_cast<OcTreeStamped*>(readTreeAbstract);
EXPECT_TRUE(readStampedTree);
EXPECT_TRUE(stampedTree == *readStampedTree);
//colorNode = colorTree.search(0.0, 0.0, 0.0);
//EXPECT_TRUE(colorNode);
//EXPECT_EQ(colorNode->getColor(), color_red);
delete readStampedTree;
}
std::cerr << "Test successful.\n";
return 0;
}
int main(int argc, char** argv) {
string inputFilename = "";
string outputFilename = "";
if (argc < 2 || argc > 3 || (argc > 1 && strcmp(argv[1], "-h") == 0)){
printUsage(argv[0]);
}
inputFilename = std::string(argv[1]);
if (argc == 3)
outputFilename = std::string(argv[2]);
else{
outputFilename = inputFilename + ".ot";
}
cout << "\nReading OcTree file\n===========================\n";
std::ifstream file(inputFilename.c_str(), std::ios_base::in |std::ios_base::binary);
if (!file.is_open()){
OCTOMAP_ERROR_STR("Filestream to "<< inputFilename << " not open, nothing read.");
exit(-1);
}
std::istream::pos_type streampos = file.tellg();
AbstractOcTree* tree;
// reading binary:
if (inputFilename.length() > 3 && (inputFilename.compare(inputFilename.length()-3, 3, ".bt") == 0)){
OcTree* binaryTree = new OcTree(0.1);
if (binaryTree->readBinary(file) && binaryTree->size() > 1)
tree = binaryTree;
else {
OCTOMAP_ERROR_STR("Could not detect binary OcTree format in file.");
exit(-1);
}
} else {
tree = AbstractOcTree::read(file);
if (!tree){
OCTOMAP_WARNING_STR("Could not detect OcTree in file, trying legacy formats.");
// TODO: check if .cot extension, try old format only then
// reset and try old ColorOcTree format:
file.clear(); // clear eofbit of istream
file.seekg(streampos);
ColorOcTree* colorTree = new ColorOcTree(0.1);
colorTree->readData(file);
if (colorTree->size() > 1 && file.good()){
OCTOMAP_WARNING_STR("Detected Binary ColorOcTree to convert. \nPlease check and update the new file header (resolution will likely be wrong).");
tree = colorTree;
} else{
delete colorTree;
std::cerr << "Error reading from file " << inputFilename << std::endl;
exit(-1);
}
}
}
// close filestream
file.close();
if (outputFilename.length() > 3 && (outputFilename.compare(outputFilename.length()-3, 3, ".bt") == 0)){
std::cerr << "Writing binary (BonsaiTree) file" << std::endl;
AbstractOccupancyOcTree* octree = dynamic_cast<AbstractOccupancyOcTree*>(tree);
if (octree){
if (!octree->writeBinary(outputFilename)){
std::cerr << "Error writing to " << outputFilename << std::endl;
exit(-2);
}
} else {
std::cerr << "Error: Writing to .bt is not supported for this tree type: " << tree->getTreeType() << std::endl;
exit(-2);
}
} else{
std::cerr << "Writing general OcTree file" << std::endl;
if (!tree->write(outputFilename)){
std::cerr << "Error writing to " << outputFilename << std::endl;
exit(-2);
}
}
std::cout << "Finished writing to " << outputFilename << std::endl;
exit(0);
}
int main(int argc, char** argv) {
if (argc != 2){
std::cerr << "Error: you need to specify a test as argument" << std::endl;
return 1; // exit 1 means failure
}
std::string test_name (argv[1]);
// ------------------------------------------------------------
if (test_name == "MathVector") {
// test constructors
Vector3* twos = new Vector3();
Vector3* ones = new Vector3(1,1,1);
for (int i=0;i<3;i++) {
(*twos)(i) = 2;
}
// test basic operations
Vector3 subtraction = *twos - *ones;
Vector3 addition = *twos + *ones;
Vector3 multiplication = *twos * 2.;
for (int i=0;i<3;i++) {
EXPECT_FLOAT_EQ (subtraction(i), 1.);
EXPECT_FLOAT_EQ (addition(i), 3.);
EXPECT_FLOAT_EQ (multiplication(i), 4.);
}
// copy constructor
Vector3 rotation = *ones;
// rotation
rotation.rotate_IP (M_PI, 1., 0.1);
EXPECT_FLOAT_EQ (rotation.x(), 1.2750367);
EXPECT_FLOAT_EQ (rotation.y(), (-1.1329513));
EXPECT_FLOAT_EQ (rotation.z(), 0.30116868);
// ------------------------------------------------------------
} else if (test_name == "MathPose") {
// constructors
Pose6D a (1.0f, 0.1f, 0.1f, 0.0f, 0.1f, (float) M_PI/4. );
Pose6D b;
Vector3 trans(1.0f, 0.1f, 0.1f);
Quaternion rot(0.0f, 0.1f, (float) M_PI/4.);
Pose6D c(trans, rot);
// comparator
EXPECT_TRUE ( a == c);
// toEuler
EXPECT_FLOAT_EQ (c.yaw() , M_PI/4.);
// transform
Vector3 t = c.transform (trans);
EXPECT_FLOAT_EQ (t.x() , 1.6399229);
EXPECT_FLOAT_EQ (t.y() , 0.8813442);
EXPECT_FLOAT_EQ (t.z() , 0.099667005);
// inverse transform
Pose6D c_inv = c.inv();
Vector3 t2 = c_inv.transform (t);
EXPECT_FLOAT_EQ (t2.x() , trans.x());
EXPECT_FLOAT_EQ (t2.y() , trans.y());
EXPECT_FLOAT_EQ (t2.z() , trans.z());
// ------------------------------------------------------------
} else if (test_name == "InsertRay") {
double p = 0.5;
EXPECT_FLOAT_EQ(p, probability(logodds(p)));
p = 0.1;
EXPECT_FLOAT_EQ(p, probability(logodds(p)));
p = 0.99;
EXPECT_FLOAT_EQ(p, probability(logodds(p)));
float l = 0;
EXPECT_FLOAT_EQ(l, logodds(probability(l)));
l = -4;
EXPECT_FLOAT_EQ(l, logodds(probability(l)));
l = 2;
EXPECT_FLOAT_EQ(l, logodds(probability(l)));
OcTree tree (0.05);
tree.setProbHit(0.7);
tree.setProbMiss(0.4);
point3d origin (0.01f, 0.01f, 0.02f);
point3d point_on_surface (2.01f,0.01f,0.01f);
for (int i=0; i<360; i++) {
for (int j=0; j<360; j++) {
EXPECT_TRUE (tree.insertRay(origin, origin+point_on_surface));
point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
}
point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
}
EXPECT_TRUE (tree.writeBinary("sphere_rays.bt"));
EXPECT_EQ ((int) tree.size(), 50615);
// ------------------------------------------------------------
// ray casting is now in "test_raycasting.cpp"
// ------------------------------------------------------------
// insert scan test
// insert graph node test
// write graph test
} else if (test_name == "InsertScan") {
Pointcloud* measurement = new Pointcloud();
point3d origin (0.01f, 0.01f, 0.02f);
point3d point_on_surface (2.01f, 0.01f, 0.01f);
for (int i=0; i<360; i++) {
for (int j=0; j<360; j++) {
point3d p = origin+point_on_surface;
measurement->push_back(p);
point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
}
point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
}
OcTree tree (0.05);
tree.insertPointCloud(*measurement, origin);
EXPECT_EQ (tree.size(), 53959);
ScanGraph* graph = new ScanGraph();
Pose6D node_pose (origin.x(), origin.y(), origin.z(),0.0f,0.0f,0.0f);
graph->addNode(measurement, node_pose);
EXPECT_TRUE (graph->writeBinary("test.graph"));
delete graph;
// ------------------------------------------------------------
// graph read file test
} else if (test_name == "ReadGraph") {
// not really meaningful, see better test in "test_scans.cpp"
ScanGraph graph;
EXPECT_TRUE (graph.readBinary("test.graph"));
// ------------------------------------------------------------
} else if (test_name == "StampedTree") {
OcTreeStamped stamped_tree (0.05);
// fill tree
for (int x=-20; x<20; x++)
for (int y=-20; y<20; y++)
for (int z=-20; z<20; z++) {
point3d p ((float) x*0.05f+0.01f, (float) y*0.05f+0.01f, (float) z*0.05f+0.01f);
stamped_tree.updateNode(p, true); // integrate 'occupied' measurement
}
// test if update times set
point3d query (0.1f, 0.1f, 0.1f);
OcTreeNodeStamped* result = stamped_tree.search (query);
EXPECT_TRUE (result);
unsigned int tree_time = stamped_tree.getLastUpdateTime();
unsigned int node_time = result->getTimestamp();
std::cout << "After 1st update (cube): Tree time " <<tree_time << "; node(0.1, 0.1, 0.1) time " << result->getTimestamp() << std::endl;
EXPECT_TRUE (tree_time > 0);
#ifdef _WIN32
Sleep(1000);
#else
sleep(1);
#endif
stamped_tree.integrateMissNoTime(result); // reduce occupancy, no time update
std::cout << "After 2nd update (single miss): Tree time " <<tree_time << "; node(0.1, 0.1, 0.1) time " << node_time << std::endl;
EXPECT_EQ (node_time, result->getTimestamp()); // node time updated?
point3d query2 = point3d (0.1f, 0.1f, 0.3f);
stamped_tree.updateNode(query2, true); // integrate 'occupied' measurement
OcTreeNodeStamped* result2 = stamped_tree.search (query2);
EXPECT_TRUE (result2);
result = stamped_tree.search (query);
EXPECT_TRUE (result);
std::cout << "After 3rd update (single hit at (0.1, 0.1, 0.3): Tree time " << stamped_tree.getLastUpdateTime() << "; node(0.1, 0.1, 0.1) time " << result->getTimestamp()
<< "; node(0.1, 0.1, 0.3) time " << result2->getTimestamp() << std::endl;
EXPECT_TRUE (result->getTimestamp() < result2->getTimestamp()); // result2 has been updated
EXPECT_EQ(result2->getTimestamp(), stamped_tree.getLastUpdateTime());
// ------------------------------------------------------------
} else if (test_name == "OcTreeKey") {
OcTree tree (0.05);
point3d p(0.0,0.0,0.0);
OcTreeKey key;
tree.coordToKeyChecked(p, key);
point3d p_inv = tree.keyToCoord(key);
EXPECT_FLOAT_EQ (0.025, p_inv.x());
EXPECT_FLOAT_EQ (0.025, p_inv.y());
EXPECT_FLOAT_EQ (0.025, p_inv.z());
// ------------------------------------------------------------
} else {
std::cerr << "Invalid test name specified: " << test_name << std::endl;
return 1;
}
std::cerr << "Test successful.\n";
return 0;
}
int main(int argc, char** argv) {
//##############################################################
string btFilename = "";
unsigned char maxDepth = 16;
// test timing:
timeval start;
timeval stop;
const unsigned char tree_depth(16);
const unsigned int tree_max_val(32768);
double time_it, time_depr;
if (argc <= 1|| argc >3 || strcmp(argv[1], "-h") == 0){
printUsage(argv[0]);
}
btFilename = std::string(argv[1]);
if (argc > 2){
maxDepth = (unsigned char)atoi(argv[2]);
}
maxDepth = std::min((unsigned char)16,maxDepth);
if (maxDepth== 0)
maxDepth = tree_depth;
// iterate over empty tree:
OcTree emptyTree(0.2);
EXPECT_EQ(emptyTree.size(), 0);
EXPECT_EQ(emptyTree.calcNumNodes(), 0);
size_t iteratedNodes = 0;
OcTree::tree_iterator t_it = emptyTree.begin_tree(maxDepth);
OcTree::tree_iterator t_end = emptyTree.end_tree();
EXPECT_TRUE (t_it == t_end);
for( ; t_it != t_end; ++t_it){
iteratedNodes++;
}
EXPECT_EQ(iteratedNodes, 0);
for(OcTree::leaf_iterator l_it = emptyTree.begin_leafs(maxDepth), l_end=emptyTree.end_leafs(); l_it!= l_end; ++l_it){
iteratedNodes++;
}
EXPECT_EQ(iteratedNodes, 0);
cout << "\nReading OcTree file\n===========================\n";
OcTree* tree = new OcTree(btFilename);
if (tree->size()<= 1){
std::cout << "Error reading file, exiting!\n";
return 1;
}
size_t count;
std::list<OcTreeVolume> list_depr;
std::list<OcTreeVolume> list_iterator;
/**
* get number of nodes:
*/
gettimeofday(&start, NULL); // start timer
size_t num_leafs_recurs = tree->getNumLeafNodes();
gettimeofday(&stop, NULL); // stop timer
time_depr = timediff(start, stop);
gettimeofday(&start, NULL); // start timer
size_t num_leafs_it = 0;
for(OcTree::leaf_iterator it = tree->begin(), end=tree->end(); it!= end; ++it) {
num_leafs_it++;
}
gettimeofday(&stop, NULL); // stop timer
time_it = timediff(start, stop);
std::cout << "Number of leafs: " << num_leafs_it << " / " << num_leafs_recurs << ", times: "
<<time_it << " / " << time_depr << "\n========================\n\n";
/**
* get all occupied leafs
*/
point3d tree_center;
tree_center(0) = tree_center(1) = tree_center(2)
= (float) (((double) tree_max_val) * tree->getResolution());
gettimeofday(&start, NULL); // start timer
getLeafNodesRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree, true);
gettimeofday(&stop, NULL); // stop timer
time_depr = timediff(start, stop);
gettimeofday(&start, NULL); // start timer
for(OcTree::iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it){
if(tree->isNodeOccupied(*it))
{
//count ++;
list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
}
}
gettimeofday(&stop, NULL); // stop timer
time_it = timediff(start, stop);
std::cout << "Occupied lists traversed, times: "
<<time_it << " / " << time_depr << "\n";
compareResults(list_iterator, list_depr);
std::cout << "========================\n\n";
/**
* get all free leafs
*/
list_iterator.clear();
list_depr.clear();
gettimeofday(&start, NULL); // start timer
for(OcTree::leaf_iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it) {
if(!tree->isNodeOccupied(*it))
list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
}
gettimeofday(&stop, NULL); // stop timer
time_it = timediff(start, stop);
gettimeofday(&start, NULL); // start timer
getLeafNodesRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree, false);
gettimeofday(&stop, NULL); // stop timer
time_depr = timediff(start, stop);
std::cout << "Free lists traversed, times: "
<<time_it << " / " << time_depr << "\n";
compareResults(list_iterator, list_depr);
std::cout << "========================\n\n";
/**
* get all volumes
*/
list_iterator.clear();
list_depr.clear();
gettimeofday(&start, NULL); // start timer
getVoxelsRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree->getResolution());
gettimeofday(&stop, NULL); // stop timer
time_depr = timediff(start, stop);
gettimeofday(&start, NULL); // start timers
for(OcTree::tree_iterator it = tree->begin_tree(maxDepth), end=tree->end_tree();
it!= end; ++it){
//count ++;
//std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl;
list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
}
gettimeofday(&stop, NULL); // stop timer
time_it = timediff(start, stop);
list_iterator.sort(OcTreeVolumeSortPredicate);
list_depr.sort(OcTreeVolumeSortPredicate);
std::cout << "All inner lists traversed, times: "
<<time_it << " / " << time_depr << "\n";
compareResults(list_iterator, list_depr);
std::cout << "========================\n\n";
// traverse all leaf nodes, timing:
gettimeofday(&start, NULL); // start timers
count = 0;
for(OcTree::iterator it = tree->begin(maxDepth), end=tree->end();
it!= end; ++it){
// do something:
// std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl;
count++;
}
gettimeofday(&stop, NULL); // stop timer
time_it = timediff(start, stop);
std::cout << "Time to traverse all leafs at max depth " <<(unsigned int)maxDepth <<" ("<<count<<" nodes): "<< time_it << " s\n\n";
/**
* bounding box tests
*/
//tree->expand();
// test complete tree (should be equal to no bbx)
OcTreeKey bbxMinKey, bbxMaxKey;
double temp_x,temp_y,temp_z;
tree->getMetricMin(temp_x,temp_y,temp_z);
octomap::point3d bbxMin(temp_x,temp_y,temp_z);
tree->getMetricMax(temp_x,temp_y,temp_z);
octomap::point3d bbxMax(temp_x,temp_y,temp_z);
EXPECT_TRUE(tree->coordToKeyChecked(bbxMin, bbxMinKey));
EXPECT_TRUE(tree->coordToKeyChecked(bbxMax, bbxMaxKey));
OcTree::leaf_bbx_iterator it_bbx = tree->begin_leafs_bbx(bbxMinKey,bbxMaxKey);
EXPECT_TRUE(it_bbx == tree->begin_leafs_bbx(bbxMinKey,bbxMaxKey));
OcTree::leaf_bbx_iterator end_bbx = tree->end_leafs_bbx();
EXPECT_TRUE(end_bbx == tree->end_leafs_bbx());
OcTree::leaf_iterator it = tree->begin_leafs();
EXPECT_TRUE(it == tree->begin_leafs());
OcTree::leaf_iterator end = tree->end_leafs();
EXPECT_TRUE(end == tree->end_leafs());
for( ; it!= end && it_bbx != end_bbx; ++it, ++it_bbx){
EXPECT_TRUE(it == it_bbx);
}
EXPECT_TRUE(it == end && it_bbx == end_bbx);
// now test an actual bounding box:
tree->expand(); // (currently only works properly for expanded tree (no multires)
bbxMin = point3d(-1, -1, - 1);
bbxMax = point3d(3, 2, 1);
EXPECT_TRUE(tree->coordToKeyChecked(bbxMin, bbxMinKey));
EXPECT_TRUE(tree->coordToKeyChecked(bbxMax, bbxMaxKey));
typedef unordered_ns::unordered_map<OcTreeKey, double, OcTreeKey::KeyHash> KeyVolumeMap;
KeyVolumeMap bbxVoxels;
count = 0;
for(OcTree::leaf_bbx_iterator it = tree->begin_leafs_bbx(bbxMinKey,bbxMaxKey), end=tree->end_leafs_bbx();
it!= end; ++it)
{
count++;
OcTreeKey currentKey = it.getKey();
// leaf is actually a leaf:
EXPECT_FALSE(it->hasChildren());
// leaf exists in tree:
OcTreeNode* node = tree->search(currentKey);
EXPECT_TRUE(node);
EXPECT_EQ(node, &(*it));
// all leafs are actually in the bbx:
for (unsigned i = 0; i < 3; ++i){
// if (!(currentKey[i] >= bbxMinKey[i] && currentKey[i] <= bbxMaxKey[i])){
// std::cout << "Key failed: " << i << " " << currentKey[i] << " "<< bbxMinKey[i] << " "<< bbxMaxKey[i]
// << "size: "<< it.getSize()<< std::endl;
// }
EXPECT_TRUE(currentKey[i] >= bbxMinKey[i] && currentKey[i] <= bbxMaxKey[i]);
}
bbxVoxels.insert(std::pair<OcTreeKey,double>(currentKey, it.getSize()));
}
EXPECT_EQ(bbxVoxels.size(), count);
std::cout << "Bounding box traversed ("<< count << " leaf nodes)\n\n";
// compare with manual BBX check on all leafs:
for(OcTree::leaf_iterator it = tree->begin(), end=tree->end(); it!= end; ++it) {
OcTreeKey key = it.getKey();
if ( key[0] >= bbxMinKey[0] && key[0] <= bbxMaxKey[0]
&& key[1] >= bbxMinKey[1] && key[1] <= bbxMaxKey[1]
&& key[2] >= bbxMinKey[2] && key[2] <= bbxMaxKey[2])
{
KeyVolumeMap::iterator bbxIt = bbxVoxels.find(key);
EXPECT_FALSE(bbxIt == bbxVoxels.end());
EXPECT_TRUE(key == bbxIt->first);
EXPECT_EQ(it.getSize(), bbxIt->second);
}
}
// test tree with one node:
OcTree simpleTree(0.01);
simpleTree.updateNode(point3d(10, 10, 10), 5.0f);
for(OcTree::leaf_iterator it = simpleTree.begin_leafs(maxDepth), end=simpleTree.end_leafs(); it!= end; ++it) {
std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl;
}
std::cout << "Tests successful\n";
return 0;
}
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");
}
}
int main(int argc, char** argv) {
// default values:
double res = 0.1;
if (argc < 2)
printUsage(argv[0]);
string graphFilename = std::string(argv[1]);
double maxrange = -1;
int max_scan_no = -1;
int skip_scan_eval = 5;
int arg = 1;
while (++arg < argc) {
if (! strcmp(argv[arg], "-i"))
graphFilename = std::string(argv[++arg]);
else if (! strcmp(argv[arg], "-res"))
res = atof(argv[++arg]);
else if (! strcmp(argv[arg], "-m"))
maxrange = atof(argv[++arg]);
else if (! strcmp(argv[arg], "-n"))
max_scan_no = atoi(argv[++arg]);
else {
printUsage(argv[0]);
}
}
cout << "\nReading Graph file\n===========================\n";
ScanGraph* graph = new ScanGraph();
if (!graph->readBinary(graphFilename))
exit(2);
size_t num_points_in_graph = 0;
if (max_scan_no > 0) {
num_points_in_graph = graph->getNumPoints(max_scan_no-1);
cout << "\n Data points in graph up to scan " << max_scan_no << ": " << num_points_in_graph << endl;
}
else {
num_points_in_graph = graph->getNumPoints();
cout << "\n Data points in graph: " << num_points_in_graph << endl;
}
cout << "\nCreating tree\n===========================\n";
OcTree* tree = new OcTree(res);
size_t numScans = graph->size();
unsigned int currentScan = 1;
for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {
if (currentScan % skip_scan_eval != 0){
if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush;
else cout << "("<<currentScan << "/" << numScans << ") " << flush;
tree->insertPointCloud(**scan_it, maxrange);
} else
cout << "(SKIP) " << flush;
if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no))
break;
currentScan++;
}
tree->expand();
cout << "\nEvaluating scans\n===========================\n";
currentScan = 1;
size_t num_points = 0;
size_t num_voxels_correct = 0;
size_t num_voxels_wrong = 0;
size_t num_voxels_unknown = 0;
for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {
if (currentScan % skip_scan_eval == 0){
if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush;
else cout << "("<<currentScan << "/" << numScans << ") " << flush;
pose6d frame_origin = (*scan_it)->pose;
point3d sensor_origin = frame_origin.inv().transform((*scan_it)->pose.trans());
// transform pointcloud:
Pointcloud scan (*(*scan_it)->scan);
scan.transform(frame_origin);
point3d origin = frame_origin.transform(sensor_origin);
KeySet free_cells, occupied_cells;
tree->computeUpdate(scan, origin, free_cells, occupied_cells, maxrange);
num_points += scan.size();
// count free cells
for (KeySet::iterator it = free_cells.begin(); it != free_cells.end(); ++it) {
OcTreeNode* n = tree->search(*it);
if (n){
if (tree->isNodeOccupied(n))
num_voxels_wrong++;
else
num_voxels_correct++;
} else
num_voxels_unknown++;
} // count occupied cells
for (KeySet::iterator it = occupied_cells.begin(); it != occupied_cells.end(); ++it) {
OcTreeNode* n = tree->search(*it);
if (n){
if (tree->isNodeOccupied(n))
num_voxels_correct++;
else
num_voxels_wrong++;
} else
num_voxels_unknown++;
}
}
if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no))
break;
currentScan++;
}
cout << "\nFinished evaluating " << num_points <<"/"<< num_points_in_graph << " points.\n"
<<"Voxels correct: "<<num_voxels_correct<<" #wrong: " <<num_voxels_wrong << " #unknown: " <<num_voxels_unknown
<<". % correct: "<< num_voxels_correct/double(num_voxels_correct+num_voxels_wrong)<<"\n\n";
delete graph;
delete tree;
return 0;
}
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();
}
int main(int argc, char** argv) {
// default values:
double res = 0.1;
string graphFilename = "";
string treeFilename = "";
double maxrange = -1;
int max_scan_no = -1;
bool detailedLog = false;
bool simpleUpdate = false;
bool discretize = false;
unsigned char compression = 1;
// get default sensor model values:
OcTree emptyTree(0.1);
double clampingMin = emptyTree.getClampingThresMin();
double clampingMax = emptyTree.getClampingThresMax();
double probMiss = emptyTree.getProbMiss();
double probHit = emptyTree.getProbHit();
timeval start;
timeval stop;
int arg = 0;
while (++arg < argc) {
if (! strcmp(argv[arg], "-i"))
graphFilename = std::string(argv[++arg]);
else if (!strcmp(argv[arg], "-o"))
treeFilename = std::string(argv[++arg]);
else if (! strcmp(argv[arg], "-res") && argc-arg < 2)
printUsage(argv[0]);
else if (! strcmp(argv[arg], "-res"))
res = atof(argv[++arg]);
else if (! strcmp(argv[arg], "-log"))
detailedLog = true;
else if (! strcmp(argv[arg], "-simple"))
simpleUpdate = true;
else if (! strcmp(argv[arg], "-discretize"))
discretize = true;
else if (! strcmp(argv[arg], "-compress"))
OCTOMAP_WARNING("Argument -compress no longer has an effect, incremental pruning is done during each insertion.\n");
else if (! strcmp(argv[arg], "-compressML"))
compression = 2;
else if (! strcmp(argv[arg], "-m"))
maxrange = atof(argv[++arg]);
else if (! strcmp(argv[arg], "-n"))
max_scan_no = atoi(argv[++arg]);
else if (! strcmp(argv[arg], "-clamping") && (argc-arg < 3))
printUsage(argv[0]);
else if (! strcmp(argv[arg], "-clamping")){
clampingMin = atof(argv[++arg]);
clampingMax = atof(argv[++arg]);
}
else if (! strcmp(argv[arg], "-sensor") && (argc-arg < 3))
printUsage(argv[0]);
else if (! strcmp(argv[arg], "-sensor")){
probMiss = atof(argv[++arg]);
probHit = atof(argv[++arg]);
}
else {
printUsage(argv[0]);
}
}
if (graphFilename == "" || treeFilename == "")
printUsage(argv[0]);
// verify input:
if (res <= 0.0){
OCTOMAP_ERROR("Resolution must be positive");
exit(1);
}
if (clampingMin >= clampingMax || clampingMin < 0.0 || clampingMax > 1.0){
OCTOMAP_ERROR("Error in clamping values: 0.0 <= [%f] < [%f] <= 1.0\n", clampingMin, clampingMax);
exit(1);
}
if (probMiss >= probHit || probMiss < 0.0 || probHit > 1.0){
OCTOMAP_ERROR("Error in sensor model (hit/miss prob.): 0.0 <= [%f] < [%f] <= 1.0\n", probMiss, probHit);
exit(1);
}
std::string treeFilenameOT = treeFilename + ".ot";
std::string treeFilenameMLOT = treeFilename + "_ml.ot";
cout << "\nReading Graph file\n===========================\n";
ScanGraph* graph = new ScanGraph();
if (!graph->readBinary(graphFilename))
exit(2);
unsigned int num_points_in_graph = 0;
if (max_scan_no > 0) {
num_points_in_graph = graph->getNumPoints(max_scan_no-1);
cout << "\n Data points in graph up to scan " << max_scan_no << ": " << num_points_in_graph << endl;
}
else {
num_points_in_graph = graph->getNumPoints();
cout << "\n Data points in graph: " << num_points_in_graph << endl;
}
// transform pointclouds first, so we can directly operate on them later
for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {
pose6d frame_origin = (*scan_it)->pose;
point3d sensor_origin = frame_origin.inv().transform((*scan_it)->pose.trans());
(*scan_it)->scan->transform(frame_origin);
point3d transformed_sensor_origin = frame_origin.transform(sensor_origin);
(*scan_it)->pose = pose6d(transformed_sensor_origin, octomath::Quaternion());
}
std::ofstream logfile;
if (detailedLog){
logfile.open((treeFilename+".log").c_str());
logfile << "# Memory of processing " << graphFilename << " over time\n";
logfile << "# Resolution: "<< res <<"; compression: " << int(compression) << "; scan endpoints: "<< num_points_in_graph << std::endl;
logfile << "# [scan number] [bytes octree] [bytes full 3D grid]\n";
}
cout << "\nCreating tree\n===========================\n";
OcTree* tree = new OcTree(res);
tree->setClampingThresMin(clampingMin);
tree->setClampingThresMax(clampingMax);
tree->setProbHit(probHit);
tree->setProbMiss(probMiss);
gettimeofday(&start, NULL); // start timer
unsigned int numScans = graph->size();
unsigned int currentScan = 1;
for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {
if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush;
else cout << "("<<currentScan << "/" << numScans << ") " << flush;
if (simpleUpdate)
tree->insertPointCloudRays((*scan_it)->scan, (*scan_it)->pose.trans(), maxrange);
else
tree->insertPointCloud((*scan_it)->scan, (*scan_it)->pose.trans(), maxrange, false, discretize);
if (compression == 2){
tree->toMaxLikelihood();
tree->prune();
}
if (detailedLog)
logfile << currentScan << " " << tree->memoryUsage() << " " << tree->memoryFullGrid() << "\n";
if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no))
break;
currentScan++;
}
gettimeofday(&stop, NULL); // stop timer
double time_to_insert = (stop.tv_sec - start.tv_sec) + 1.0e-6 *(stop.tv_usec - start.tv_usec);
// get rid of graph in mem before doing anything fancy with tree (=> memory)
delete graph;
if (logfile.is_open())
logfile.close();
cout << "\nDone building tree.\n\n";
cout << "time to insert scans: " << time_to_insert << " sec" << endl;
cout << "time to insert 100.000 points took: " << time_to_insert/ ((double) num_points_in_graph / 100000) << " sec (avg)" << endl << endl;
std::cout << "Pruned tree (lossless compression)\n" << "===========================\n";
outputStatistics(tree);
tree->write(treeFilenameOT);
std::cout << "Pruned max-likelihood tree (lossy compression)\n" << "===========================\n";
tree->toMaxLikelihood();
tree->prune();
outputStatistics(tree);
cout << "\nWriting tree files\n===========================\n";
tree->write(treeFilenameMLOT);
std::cout << "Full Octree (pruned) written to "<< treeFilenameOT << std::endl;
std::cout << "Full Octree (max.likelihood, pruned) written to "<< treeFilenameMLOT << std::endl;
tree->writeBinary(treeFilename);
std::cout << "Bonsai tree written to "<< treeFilename << std::endl;
cout << endl;
delete tree;
exit(0);
}
int main(int argc, char** argv) {
//##############################################################
OcTree tree (0.05);
// point3d origin (10.01, 10.01, 10.02);
point3d origin (0.01f, 0.01f, 0.02f);
point3d point_on_surface (2.01f, 0.01f, 0.01f);
cout << "generating sphere at " << origin << " ..." << endl;
for (int i=0; i<360; i++) {
for (int j=0; j<360; j++) {
if (!tree.insertRay(origin, origin+point_on_surface)) {
cout << "ERROR while inserting ray from " << origin << " to " << point_on_surface << endl;
}
point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
}
point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
}
cout << "done." << endl;
cout << "writing to sphere.bt..." << endl;
tree.writeBinary("sphere.bt");
// -----------------------------------------------
cout << "casting rays ..." << endl;
OcTree sampled_surface (0.05);
point3d direction = point3d (1.0,0.0,0.0);
point3d obstacle(0,0,0);
unsigned int hit (0);
unsigned int miss (0);
double mean_dist(0);
for (int i=0; i<360; i++) {
for (int j=0; j<360; j++) {
if (!tree.castRay(origin, direction, obstacle, true, 3.)) {
miss++;
}
else {
hit++;
mean_dist += (obstacle - origin).norm();
sampled_surface.updateNode(obstacle, true);
}
direction.rotate_IP (0,0,DEG2RAD(1.));
}
direction.rotate_IP (0,DEG2RAD(1.),0);
}
cout << "done." << endl;
mean_dist /= (double) hit;
std::cout << " hits / misses: " << hit << " / " << miss << std::endl;
std::cout << " mean obstacle dist: " << mean_dist << std::endl;
cout << "writing sampled_surface.bt" << endl;
sampled_surface.writeBinary("sampled_surface.bt");
// -----------------------------------------------
cout << "generating single rays..." << endl;
OcTree single_beams(0.03333);
int num_beams = 17;
float beamLength = 10.0f;
point3d single_origin (1.0f, 0.45f, 0.45f);
point3d single_endpoint(beamLength, 0.0f, 0.0f);
for (int i=0; i<num_beams; i++) {
if (!single_beams.insertRay(single_origin, single_origin+single_endpoint)) {
cout << "ERROR while inserting ray from " << single_origin << " to " << single_endpoint << endl;
}
single_endpoint.rotate_IP (0,0,DEG2RAD(360.0/num_beams));
}
cout << "done." << endl;
cout << "writing to beams.bt..." << endl;
single_beams.writeBinary("beams.bt");
return 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "pose_3d");
ros::NodeHandle node;
ros::Rate rate(10.0);
tf::TransformListener listener;
static tf::TransformBroadcaster br;
tf::Transform transform;
tf::StampedTransform stransform;
tf::Quaternion q;
ros::Publisher path_pub = node.advertise<nav_msgs::Path>("/youbotPath", 15);
ros::Publisher grid_pub = node.advertise<nav_msgs::OccupancyGrid>("/nav_map", 15);
ros::Publisher mark_pub = node.advertise<visualization_msgs::MarkerArray>("/NBVs", 15);
//sleep(5);
ros::Subscriber sub = node.subscribe("/octomap_binary", 15, map_cb);
ros::Subscriber flag_sub = node.subscribe("/nbv_iter", 15, flag_cb);
ros::Subscriber grid_sub = node.subscribe("/projected_map", 15, grid_cb);
point3d sensorOffset(0.1, 0.3, 0);
point3d firstPos;
/*//Create transform from sensor to robot base
transform.setOrigin( tf::Vector3(sensorOffset.x(), sensorOffset.y(), 0.0) );
q.setRPY(0, 0, 0);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base", "xtion"));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "baseGoal", "sensorGoal"));
//-----------------------*/
const clock_t start = std::clock();
point3d curPose(-1.1, -0.05, 0.0);
vector<point3d> NBVList;
vector<point3d> PoseList;
visualization_msgs::MarkerArray views;
transform.setOrigin( tf::Vector3(curPose.x(), curPose.y(), 0) );
q.setRPY(0, 0, DEG2RAD(-90));
transform.setRotation(q);
ros::Time gTime = ros::Time::now();
br.sendTransform(tf::StampedTransform(transform, gTime, "vicon", "sensorGoal"));
//Create transform from sensor to robot base
//transform.setOrigin( tf::Vector3(sensorOffset.x(), sensorOffset.y(), 0.0) );
//q.setRPY(0, 0, 0);
//transform.setRotation(q);
//br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "xtion", "base"));
//br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "sensorGoal", "baseGoal"));
//-----------------------
sleep(1);
//int x;
//cin >> x;
//moveBase(origin, yaw, &listener);
cout << range << endl;
cout << sRes << endl;
//Setup Stuff
//OcTree tree(res);
cout << "Loading Sensor Model from File" << endl;
ifstream readEm("SensorModel.bin", ios::in | ios::binary);
while (!readEm.eof())
{
int buffer;
readEm.read((char*)&buffer, sizeof(int));
if (buffer < 0) break;
optiMap.push_back(buffer);
vector<int> tempRay;
while (!readEm.eof())
{
readEm.read((char*)&buffer, sizeof(int));
if (buffer < 0) break;
tempRay.push_back(buffer);
}
optiRays.push_back(tempRay);
}
readEm.close();
cout << "Finished Loading Sensor Model" << endl;
double tempRoll, tempPitch, tempYaw;
/*try{
gTime = ros::Time::now();
listener.waitForTransform("vicon", "xtion", gTime, ros::Duration(5.0));
listener.lookupTransform("vicon", "xtion", gTime, stransform);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
point3d origin(stransform.getOrigin().x(), stransform.getOrigin().y(), stransform.getOrigin().z());
tf::Matrix3x3 rot(stransform.getRotation());
rot.getRPY(tempRoll, tempPitch, tempYaw);*/
point3d origin(0, 0, 0);
NBVList.push_back(origin);
cout << "Starting Pose: " << tempYaw << " : " << tempPitch << endl;
visualization_msgs::Marker mark;
mark.header.frame_id = "map";
mark.header.stamp = ros::Time::now();
mark.ns = "basic_shapes";
mark.id = 0;
mark.type = visualization_msgs::Marker::ARROW;
mark.action = visualization_msgs::Marker::ADD;
mark.pose.position.x = origin.x();
mark.pose.position.y = origin.y();
mark.pose.position.z = origin.z();
q.setRPY(0, 0, 0);
mark.pose.orientation.x = q.getX();
mark.pose.orientation.y = q.getY();
mark.pose.orientation.z = q.getZ();
mark.pose.orientation.w = q.getW();
mark.scale.x = 0.2;
mark.scale.y = 0.02;
mark.scale.z = 0.02;
mark.color.r = 1.0f;
mark.color.g = 0.0f;
mark.color.a = 1.0;
mark.color.b = 0.0f;
views.markers.push_back(mark);
mark_pub.publish(views);
int NBVcount = 0;
while (ros::ok())
{
cout << "NBV " << NBVcount++ << endl;
while (map2d.data.size() == 0 || !mapReceived)
{
ros::spinOnce();
}
//AbstractOcTree
OcTree* treeTemp = binaryMsgToMap(mapMsg);
//OcTree treeTemp(0.01);
treeTemp->writeBinary("firstMap.bt");
cout << "Loaded the Map" << endl;
/*try{
gTime = ros::Time::now();
listener.waitForTransform("vicon", "xtion", gTime, ros::Duration(5.0));
listener.lookupTransform("vicon", "xtion", gTime, stransform);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
point3d camPosition (stransform.getOrigin().x(), stransform.getOrigin().y(), 0);*/
origin = NBVList.back();
cout << "Map Loaded " << treeTemp->getResolution() << endl;
point3d egoPose(0, 0, 0);
point3d endPoint(-1.4, -1.5, 0);
//pObj Tuning Coefficients
double alpha = 0.5;
double beta = 2.0;
size_t pointCount = 0;
//point3d origin = curPose;
OcTree tree(res);
tree.setClampingThresMax(0.999);
tree.setClampingThresMin(0.001);
tree.setProbMiss(0.1);
tree.setProbHit(0.995);
tree.readBinary("firstMap.bt");
tree.expand();
/*for (OcTree::leaf_iterator iter = tree.begin_leafs(); iter != tree.end_leafs(); iter++)
{
if (tree.isNodeOccupied(*iter)) && (iter.getZ() < 0.03 || iter.getX() < -2 || iter.getX() > 1 || iter.getY() < -1 || iter.getY() > 1))
{
tree.setNodeValue(iter.getKey(), -lEmpty);
tree.updateNode(iter.getKey(), false);
if (tree.isNodeOccupied(*iter))
{
cout << "Errrnk" << endl;
}
}
}*/
cout << origin.x() << ", " << origin.y() << endl;
origin = tree.keyToCoord(tree.coordToKey(origin));
cout << "Origin: " << origin.x() << " : " << origin.y() << " : " << origin.z() << endl;
if (NBVcount == 1)
{
firstPos = origin;
}
//Use Second Tree to store pObj entities for NBV prediction
OcTree tree2(tree);
string fileName = "algMap" + to_string(NBVcount) + ".bt";
tree.writeBinary(fileName);
//-------------------
//Cost Function Work - 2D Map Creation, Dijkstras Path
//-------------------
double robotRad = 0.3;
nav_msgs::MapMetaData mapMeta = map2d.info;
double minX = mapMeta.origin.position.x;
double minY = mapMeta.origin.position.y;
cout << "2D Res: " << mapMeta.resolution;
double rangeX = mapMeta.width;
double rangeY = mapMeta.height;
/*double minX, minY, minZ, maxX, maxY, maxZ;
tree.getMetricMin(minX, minY, minZ);
tree.getMetricMax(maxX, maxY, maxZ);
point3d minPoint(minX, minY, minZ);
point3d maxPoint(maxX, maxY, maxZ);
cout << minX << ", " << maxX << ", " << minY << ", " << maxY << endl;
minPoint = tree.keyToCoord(tree.coordToKey(minPoint));
minX = minPoint.x();
minY = minPoint.y();
minZ = minPoint.z();
maxPoint = tree.keyToCoord(tree.coordToKey(maxPoint));
maxX = maxPoint.x();
maxY = maxPoint.y();
maxZ = maxPoint.z();
cout << minX << ", " << maxX << ", " << minY << ", " << maxY << endl;
tree.expand();
int rangeX = round((maxX - minX) * rFactor);
int rangeY = round((maxY - minY) * rFactor);*/
vector<GridNode> gridMap(rangeX * rangeY, GridNode());
for (int x = 0; x < rangeX; x++)
{
for (int y = 0; y < rangeY; y++)
{
//cout << "XY: " << x << ", " << y << endl;
gridMap[rangeX * y + x].coords = point3d(double(x) * res + minX, double(y) * res + minY, 0);
for (int i = -1; i <= 1; i += 1)
{
for (int j = -1; j <= 1; j += 1)
{
if ((i != 0 || j != 0) && x + i >= 0 && x + i < rangeX && y + j >= 0 && y + j < rangeY)
{
gridMap[rangeX * y + x].neighbors.push_back(&gridMap[rangeX * (y + j) + x + i]);
gridMap[rangeX * y + x].edges.push_back(sqrt(pow(i, 2) + pow(j, 2)) * res);
//cout << gridMap[rangeX*y + x].neighbors.size() << ", " << gridMap[rangeX*y + x].edges.size() << endl;
}
}
}
}
}
cout << "First" << endl;
cout << "Relative Sizes: " << map2d.data.size() << " : " << gridMap.size() << endl;
cout << "Dimensions: " << mapMeta.width << " by " << mapMeta.height << endl;
for (int x = 0; x < rangeX; x++)
{
for (int y = 0; y < rangeY; y++)
{
int index = rangeX * y + x;
if (map2d.data[index] > 50)
{
gridMap[index].object = true;
gridMap[index].occupied = true;
for (double offX = -robotRad; offX <= robotRad; offX += res)
{
for (double offY = -robotRad; offY <= robotRad; offY += res)
{
if (sqrt(pow(offX, 2) + pow(offY, 2)) <= robotRad)
{
int xInd = round((gridMap[index].coords.x() + offX - minX) * rFactor);
int yInd = round((gridMap[index].coords.y() + offY - minY) * rFactor);
if (xInd >= 0 && xInd < rangeX && yInd >= 0 && yInd < rangeY)
{
int offIdx = yInd * rangeX + xInd;
gridMap[offIdx].occupied = true;
}
}
}
}
}
}
}
cout << "Second" << endl;
cout << rangeX << ", " << rangeY << " : " << int((origin.x() - minX) * rFactor) << ", " << int((origin.y() - minY) * rFactor) << endl;
int origIdx = rangeX * int((origin.y() - minY) / res) + int((origin.x() - minX) / res);
//round(((origin.y() - minY) * rFactor) * rangeX + (origin.x() - minX) * rFactor);
cout << origIdx << " " << gridMap.size() << endl;
GridNode* originNode = &gridMap[origIdx];
originNode->cost = 0;
cout << "Got Here" << endl;
MinHeap heapy;
heapy.Push(originNode);
while (heapy.GetLength() > 0)
{
GridNode* minNode = heapy.Pop();
vector<float>::iterator edgeIt = minNode->edges.begin();
for (vector<GridNode*>::iterator iter = minNode->neighbors.begin(); iter != minNode->neighbors.end(); iter++, edgeIt++)
{
GridNode* nodePt = *iter;
if ((*iter)->occupied == 0 && (*iter)->cost > (minNode->cost + *edgeIt))
{
(*iter)->parent = minNode;
if ((*iter)->state == 0)
{
(*iter)->cost = minNode->cost + *edgeIt;
heapy.Push((*iter));
(*iter)->state = 1;
}
else if ((*iter)->state == 1)
{
heapy.Update((*iter)->heapIdx, minNode->cost + *edgeIt);
}
}
}
}
cout << "Dijkstra Path Complete" << endl;
nav_msgs::OccupancyGrid grid;
grid.header.stamp = ros::Time::now();
grid.header.frame_id = "map";
nav_msgs::MapMetaData metas;
metas.resolution = res;
metas.origin.position.x = minX;
metas.origin.position.y = minY;
metas.origin.position.z = 0.07;
metas.height = rangeY;
metas.width = rangeX;
grid.info = metas;
cout << gridMap.size() << endl;
for (vector<GridNode>::iterator iter = gridMap.begin(); iter != gridMap.end(); iter++)
{
if (iter->occupied)
{
grid.data.push_back(100);
}
else
{
grid.data.push_back(0);
}
}
grid_pub.publish(grid);
cout << "Map Published" << endl;
pointCount = 0;
int cubeCount = 0;
int emptyCount = 0;
/*tree.expand();
for (OcTree::leaf_iterator iter = tree.begin_leafs(); iter != tree.end_leafs(); iter++)
{
if (tree.isNodeOccupied(*iter))
{
pointCount++;
}
else
{
emptyCount++;
}
}*/
cout << cubeCount << " cubes" << endl;
cout << "Empties: " << emptyCount << endl;
//OcTreeCustom infoTree(res);
//infoTree.readBinary("kinect.bt");
//Copy all occupancy values from known nodes into pObj variables for those nodes
//infoTree.expand();
/*for (OcTreeCustom::iterator iter = infoTree.begin(); iter != infoTree.end(); iter++)
* { *
* iter->setpObj(float(iter->getOccupancy()));
}*/
int cellCount = 0;
int unkCount = 0;
//pointCount = 0;
cout << "Beginning pObj Processing" << endl;
for (OcTree::leaf_iterator iter = tree.begin_leafs(); iter != tree.end_leafs(); iter++)
{
OcTreeKey occKey = iter.getKey();
OcTreeKey unKey;
OcTreeKey freeKey;
OcTreeNode *cellNode;
OcTreeNode *unNode;
if (tree.isNodeOccupied(*iter))
{
cellCount++;
for (int i = -1; i <= 1; i++)
{
for (int j = -1; j <= 1; j++)
{
for (int k = -1; k <= 1; k++)
{
if (i != 0 || j != 0 || k != 0)
{
unKey = occKey;
unKey[0] += i;
unKey[1] += j;
unKey[2] += k;
unNode = tree.search(unKey);
if (unNode == NULL)
{
bool critical = false;
for (int u = -1; u <= 1; u++)
{
for (int v = -1; v <= 1; v++)
{
for (int w = -1; w <= 1; w++)
{
if (abs(u) + abs(v) + abs(w) == 1)
{
freeKey = unKey;
freeKey[0] += u;
freeKey[1] += v;
freeKey[2] += w;
cellNode = tree.search(freeKey);
if (cellNode != NULL && !tree.isNodeOccupied(cellNode) && tree.keyToCoord(freeKey).z() > 0.15)
{//unKey is a critical unknown cell, proceed to update pObj of unknown cells in vicinity
critical = true;
}
}
}
}
}
if (critical)
{
//cout << "Occ: " << tree.keyToCoord(occKey).x() << ", " << tree.keyToCoord(occKey).y() << ", " << tree.keyToCoord(occKey).z() << endl;
//cout << "Unk: " << tree.keyToCoord(unKey).x() << ", " << tree.keyToCoord(unKey).y() << ", " << tree.keyToCoord(unKey).z() << endl;
tree2.updateNode(tree2.coordToKey(tree.keyToCoord(unKey)), (float)log(alpha / (1 - alpha)));
//New method using custom tree
//infoTree.updateNode(tree.keyToCoord(unKey), float(0));
//infoTree.search(tree.keyToCoord(unKey))->setpObj((float)log(alpha / (1 - alpha)));
unkCount++;
for (double boxX = -5 * res; boxX <= 5 * res; boxX += res)
{
for (double boxY = -5 * res; boxY <= 5 * res; boxY += res)
{
for (double boxZ = -5 * res; boxZ <= 5 * res; boxZ += res)
{
point3d objPoint = tree.keyToCoord(unKey) + point3d(boxX, boxY, boxZ);
if (objPoint.z() > 0.05)
{
double pObj = alpha*exp(-sqrt(pow(boxX, 2) + pow(boxY, 2) + pow(boxZ, 2)) * beta);
cellNode = tree.search(objPoint);
if (cellNode == NULL)
{
if (tree2.search(objPoint) == NULL)
{
tree2.setNodeValue(objPoint, log(pObj / (1 - pObj)));
pointCount++;
}
else
{
if ((tree2.search(objPoint))->getLogOdds() < (float)log(pObj / (1 - pObj)))
{
tree2.setNodeValue(objPoint, log(pObj / (1 - pObj)));
}
//tree2.updateNode(objPoint, max((tree2.search(objPoint))->getLogOdds(), (float)log2(pObj / (1 - pObj))));
}
}
/*else
* {
* tree2.setNodeValue(objPoint, cellNode->getLogOdds());
*
* infoTree.search(objPoint)->setpObj(cellNode->getOccupancy());
}*/
}
}
}
}
}
}
}
}
}
}
}
}
/*infoTree.expand();
*
* f *or (OcTreeCustom::leaf_iterator iter = infoTree.begin_leafs(searchDepth); iter != infoTree.end_leafs(); iter++)
* {
* iter->updateOccupancyChildren();
* if (iter->getLogOdds() != 0 && abs(iter->getOccupancy() - iter->getpObj()) > 0.01)
* {
* cout << iter->getLogOdds() << ", " << iter->getOccupancy() << ", " << iter->getpObj() << endl;
* addCube(iter.getCoordinate(), res, 0, 0, 1, 0.5);
}
if (iter->getLogOdds() == 0 && iter->getpObj() > 0.001)
{
addCube(iter.getCoordinate(), sRes, 0, 1, 0, 0.5);
}
}*/
/*tree.expand();
tree2.expand();
for (OcTree::leaf_iterator iter = tree.begin_leafs(); iter != tree.end_leafs(); iter++)
{
if (tree2.search(iter.getCoordinate())->getOccupancy() != iter->getOccupancy())
{
cout << tree2.search(iter.getCoordinate())->getOccupancy() << ", " << iter->getOccupancy() << endl;
}
}*/
cout << "Nothing Different" << endl;
tree2.writeBinary("objMap.bt");
pointCount = 0;
tree2.expand();
KeyRay cellList;
point3d egoCOG(2.95, 1.95, 0);
Quaternion egoTheta(point3d(0, 0, -M_PI / 2));
endPoint = point3d(3.0, 0, 0);
OcTreeKey myCell;
OcTreeNode *cellNode;
double cellLike;
Pose6D trialPose(egoCOG, egoTheta);
endPoint = trialPose.transform(endPoint);
//Pre-Computing Table Stuff
point3d rayOrigin(0.005, 0.005, 0.005);
point3d rayInit(nRayLength * res, 0, 0);
point3d rayEnd;
/*vector<int> castMap;
* v *ector<int> logAdd;
*
* cellCount = 0;
* int traversed = 0;
*
* Pointcloud castCloud;
*
* for (double yaw = 0; yaw < 360; yaw++)
* {
* cout << yaw << endl;
* for (double pitch = -90; pitch <= 90; pitch++)
* {
* Quaternion rayTheta(point3d(0, DEG2RAD(pitch), DEG2RAD(yaw)));
* Pose6D rayPose(rayOrigin, rayTheta);
* rayEnd = rayPose.transform(rayInit);
*
* cellList.reset();
* tree.computeRayKeys(rayOrigin, rayEnd, cellList);
* //cout << "cellList Size: " << cellList.size() << endl;
* for (KeyRay::iterator iter = ++cellList.begin(); iter != cellList.end(); iter++)
* {
* traversed++;
* point3d rayCoord = tree.keyToCoord(*iter) - rayOrigin;
*
* if (rayCoord.norm() * sRes / res > 0.6)
* {
* int index = round(rayCoord.x() / res) + nRayLength + (2 * nRayLength + 1) * round(rayCoord.y() / res + nRayLength) + pow(2 * nRayLength + 1, 2) * round(rayCoord.z() / res + nRayLength);
* vector<int>::iterator findIt = find(castMap.begin(), castMap.end(), index);
* if (findIt == castMap.end())
* {
* castMap.push_back(index);
* castCloud.push_back(rayCoord + rayOrigin);
* logAdd.push_back(lFilled);
* cellCount++;
}
else
{
int foundIndex = distance(castMap.begin(), findIt);
logAdd[foundIndex] += lFilled;
}
}
}
}
}
cout << "Cells in CastMap: " << cellCount << "out of " << traversed << "cells traversed" << endl;
cout << "Min Index: " << *min_element(castMap.begin(), castMap.end()) << endl;
cout << "Max Index: " << *max_element(castMap.begin(), castMap.end()) << endl;
cout << "Max logAdd: " << *max_element(logAdd.begin(), logAdd.end()) << endl;
cout << nRayLength << endl;
cout << "Cloud Size: " << castCloud.size() << endl;
cout << "Inserting Cloud..." << endl;
tree.insertPointCloud(castCloud, rayOrigin);
tree.writeBinary("castMap.bt");
//Write pre-computed tables to files for quick retrieval
ofstream outFile("mapCast.bin", ios::out | ios::binary);
for (vector<int>::iterator iter = castMap.begin(); iter != castMap.end(); iter++)
{
outFile.write((char*)&(*iter), sizeof(int));
}
outFile.close();
outFile = ofstream("logAdd.bin", ios::out | ios::binary);
for (vector<int>::iterator iter = logAdd.begin(); iter != logAdd.end(); iter++)
{
outFile.write((char*)&(*iter), sizeof(int));
}
outFile.close();
cout << "Done Writing" << endl;
while (true)
{
}*/
//--------------------
//Create 3D Arrays of object information for quick retrieval in infoGain calcs
//--------------------
//First pass to find limits of info bounding box
double temp_x, temp_y, temp_z;
tree2.getMetricMax(temp_x, temp_y, temp_z);
//infoTree.getMetricMin(temp_x, temp_y, temp_z);
ibxMin = point3d(temp_x, temp_y, temp_z);
tree2.getMetricMin(temp_x, temp_y, temp_z);
//infoTree.getMetricMax(temp_x, temp_y, temp_z);
ibxMax = point3d(temp_x, temp_y, temp_z);
for (OcTree::iterator iter = tree2.begin_leafs(searchDepth); iter != tree2.end_leafs(); iter++)
{
if (iter->getOccupancy() > 0.001)
{
//addCube(iter.getCoordinate(), sRes, 0, 0, 1, 0.5);
ibxMin.x() = min(ibxMin.x(), iter.getCoordinate().x());
ibxMin.y() = min(ibxMin.y(), iter.getCoordinate().y());
ibxMin.z() = min(ibxMin.z(), iter.getCoordinate().z());
ibxMax.x() = max(ibxMax.x(), iter.getCoordinate().x());
ibxMax.y() = max(ibxMax.y(), iter.getCoordinate().y());
ibxMax.z() = max(ibxMax.z(), iter.getCoordinate().z());
}
}
/*for (OcTreeCustom::leaf_iterator iter = infoTree.begin_leafs(searchDepth); iter != infoTree.end_leafs(); iter++)
* { *
* iter->updateOccupancyChildren();
* if (iter->getpObj() > 0.001)
* {
* if (iter.getCoordinate().x() < ibxMin.x()) ibxMin.x() = iter.getCoordinate().x();
* if (iter.getCoordinate().y() < ibxMin.y()) ibxMin.y() = iter.getCoordinate().y();
* if (iter.getCoordinate().z() < ibxMin.z()) ibxMin.z() = iter.getCoordinate().z();
*
* if (iter.getCoordinate().x() > ibxMax.x()) ibxMax.x() = iter.getCoordinate().x();
* if (iter.getCoordinate().y() > ibxMax.y()) ibxMax.y() = iter.getCoordinate().y();
* if (iter.getCoordinate().z() > ibxMax.z()) ibxMax.z() = iter.getCoordinate().z();
}
}*/
cout << "Min Corner: " << ibxMin.x() << ", " << ibxMin.y() << ", " << ibxMin.z() << endl;
cout << "Max Corner: " << ibxMax.x() << ", " << ibxMax.y() << ", " << ibxMax.z() << endl;
cout << "S: " << sFactor << endl;
//Determine required size of infoChunk array and initialize accordingly
boxX = round((ibxMax.x() - ibxMin.x()) / sRes + 1);
boxY = round((ibxMax.y() - ibxMin.y()) / sRes + 1);
boxZ = round((ibxMax.z() - ibxMin.z()) / sRes + 1);
cout << boxX << ", " << boxY << ", " << boxZ << endl;
infoChunk = vector<vector<float> >(boxX * boxY * boxZ, { 0 , 0 });
//Second pass to populate 3D array with info values
tree2.expand();
for (OcTree::leaf_bbx_iterator iter = tree2.begin_leafs_bbx(ibxMin, ibxMax, searchDepth); iter != tree2.end_leafs_bbx(); iter++)
{
if (iter.getCoordinate().z() > 0.05 && iter->getOccupancy() > 0.001)
{
int xInd = round((iter.getCoordinate().x() - ibxMin.x()) * sFactor);
int yInd = round((iter.getCoordinate().y() - ibxMin.y()) * sFactor);
int zInd = round((iter.getCoordinate().z() - ibxMin.z()) * sFactor);
if (xInd < 0 || xInd >= boxX || yInd < 0 || yInd >= boxY || zInd < 0 || zInd >= boxZ) continue;
int index = xInd + yInd * boxX + zInd * boxX * boxY;
//addCube(iter.getCoordinate(), sRes, 0, 0, 1, 0.5);
infoChunk[index][1] = iter->getOccupancy();
infoChunk[index][0] = iter->getOccupancy();
}
}
tree.expand();
for (OcTree::leaf_bbx_iterator iter = tree.begin_leafs_bbx(ibxMin, ibxMax, searchDepth); iter != tree.end_leafs_bbx(); iter++)
{
int xInd = round((iter.getCoordinate().x() - ibxMin.x()) * sFactor);
int yInd = round((iter.getCoordinate().y() - ibxMin.y()) * sFactor);
int zInd = round((iter.getCoordinate().z() - ibxMin.z()) * sFactor);
if (xInd < 0 || xInd >= boxX || yInd < 0 || yInd >= boxY || zInd < 0 || zInd >= boxZ) continue;
int index = xInd + yInd * boxX + zInd * boxX * boxY;
if (iter.getCoordinate().z() > 0.05 && tree.isNodeOccupied(*iter))
{
double p = exp(iter->getLogOdds()) / (1 + exp(iter->getLogOdds()));
infoChunk[index][0] = (-p * log2(p) - (1 - p) * log2(1 - p)) * infoChunk[index][1];
//infoChunk[index][0] = iter->getLogOdds();
}
else
{
infoChunk[index][0] = 0;
}
}
/*for (OcTreeCustom::leaf_bbx_iterator iter = infoTree.begin_leafs_bbx(ibxMin, ibxMax, searchDepth); iter != infoTree.end_leafs_bbx(); iter++)
* { *
* iter->updateOccupancyChildren();
* if (iter->getpObj() > 0.001)
* {
* int xInd = round((iter.getCoordinate().x() - ibxMin.x()) / sRes);
* int yInd = round((iter.getCoordinate().y() - ibxMin.y()) / sRes);
* int zInd = round((iter.getCoordinate().z() - ibxMin.z()) / sRes);
*
* cout << iter->getLogOdds() << endl;
* infoChunk[xInd + yInd * boxX + zInd * boxX * boxY][0] = iter->getLogOdds();
* infoChunk[xInd + yInd * boxX + zInd * boxX * boxY][1] = iter->getpObj();
}
}*/
cout << "infoChunk populated, uploading sensor model" << endl;
cout << "Begin Test" << endl;
//Search Algorithm
vector<InfoNode> gains;
float maxHeight = 0.62;
float minHeight = 0.28;
pointCount = 0;
double maxIG = 0;
double maxCost = 0;
double maxPitch = 0;
double maxYaw = 0;
point3d bestPos(0, 0, 0);
int viewCount = 0;
int searchCount = 0;
point3d searchBoxMin = ibxMin - point3d(1.5, 1.5, 1.5);
point3d searchBoxMax = ibxMax + point3d(1.5, 1.5, 1.5);
searchBoxMin.z() = max(searchBoxMin.z(), minHeight);
searchBoxMax.z() = min(searchBoxMax.z(), maxHeight);
cout << "Search Space: " << searchBoxMin.x() << ", " << searchBoxMin.y() << ", " << searchBoxMin.z() << endl;
cout << "To: " << searchBoxMax.x() << ", " << searchBoxMax.y() << ", " << searchBoxMax.z() << endl;
//-------------
//Heuristic Search Method
//-------------
/*const clock_t searchStart = clock();
*
* i *nt neighbDist = 2;
* double neighborStepSize = 0.02;
* vector<InfoNode> neighbors(6 * neighbDist, InfoNode());
*
* tree.expand();
* double xMax, yMax, zMax, xMin, yMin, zMin;
* tree.getMetricMax(xMax, yMax, zMax);
* tree.getMetricMin(xMin, yMin, zMin);
* for (OcTree::leaf_bbx_iterator iter = tree.begin_leafs_bbx(searchBoxMin, searchBoxMax, 10); iter != tree.end_leafs_bbx(); iter++)
* {
* if (iter.getCoordinate().z() < 0 || iter.getCoordinate().x() < xMin || iter.getCoordinate().x() > xMax || iter.getCoordinate().y() < yMin || iter.getCoordinate().y() > yMax || iter.getCoordinate().z() < zMin || iter.getCoordinate().z() > zMax || tree.search(iter.getCoordinate()) == NULL || tree.isNodeOccupied(tree.search(iter.getCoordinate()))) continue;
*
* searchCount++;
}
cout << searchCount << " positions to calculate" << endl;
for (OcTree::leaf_iterator iter = tree.begin_leafs(11); iter != tree.end_leafs(); iter++)
{
if (iter.getCoordinate().z() < 0 || iter.getCoordinate().x() < xMin || iter.getCoordinate().x() > xMax || iter.getCoordinate().y() < yMin || iter.getCoordinate().y() > yMax || iter.getCoordinate().z() < zMin || iter.getCoordinate().z() > zMax || tree.search(iter.getCoordinate()) == NULL || tree.isNodeOccupied(tree.search(iter.getCoordinate()))) continue;
if (viewCount % 100 == 0)
{
cout << viewCount << endl;
}
point3d nextPos = iter.getCoordinate();
float pitch, yaw;
double infoGain = getInfoGain(&tree, &tree2, nextPos, &yaw, &pitch);
if (infoGain > 0)
{
gains.push_back(InfoNode(infoGain, 0, nextPos, yaw, pitch));
if (infoGain > maxIG)
{
maxIG = infoGain;
bestPos = nextPos;
maxYaw = yaw;
maxPitch = pitch;
}
//addCube(nextPos, 0.05, 1 - (infoGain / 40), infoGain / 40, 0, 0.9);
}
viewCount++;
}
cout << "First Pass: "******" views" << endl;
sort(gains.begin(), gains.end());
vector<InfoNode>::iterator testIt = gains.end()--;
vector<InfoNode> searchTails;
vector<InfoNode>::iterator iter = gains.end()--;
for (int i = 0; i < 5; i++)
{
cout << i << endl;
InfoNode curNode = *iter;
while (true)
{
cout << ".";
point3d curPoint = curNode.coords;
double curGain = curNode.infoGain;
vector<InfoNode>::iterator neighbIt = neighbors.begin();
for (int offX = -neighbDist; offX <= neighbDist; offX++)
{
for (int offY = -neighbDist; offY <= neighbDist; offY++)
{
for (int offZ = -neighbDist; offZ <= neighbDist; offZ++)
{
if ((offX != 0) + (offY != 0) + (offZ != 0) != 1) continue;
point3d neighbor = curPoint + point3d(offX, offY, offZ) * neighborStepSize;
neighbIt->coords = neighbor;
if (neighbor.z() < 0 || neighbor.x() < xMin || neighbor.x() > xMax || neighbor.y() < yMin || neighbor.y() > yMax || neighbor.z() < zMin || neighbor.z() > zMax || tree.search(neighbor) == NULL || tree.isNodeOccupied(tree.search(neighbor)))
{
neighbIt->infoGain = 0;
}
else
{
float pitch, yaw;
neighbIt->infoGain = getInfoGain(&tree, &tree2, neighbor, &yaw, &pitch);
viewCount++;
neighbIt->pitch = pitch;
neighbIt->yaw = yaw;
}
neighbIt++;
}
}
}
InfoNode maxNeighb = *max_element(neighbors.begin(), neighbors.end());
if (maxNeighb.infoGain <= curGain)
{
break;
}
else
{
curNode = maxNeighb;
}
}
searchTails.push_back(curNode);
iter--;
cout << endl;
}
InfoNode hNBV = *max_element(searchTails.begin(), searchTails.end());
const clock_t searchEnd = clock();
cout << "Heuristic NBV: " << hNBV.coords.x() << ", " << hNBV.coords.y() << ", " << hNBV.coords.z() << endl;
cout << "With Info Gain = " << hNBV.infoGain << endl;
cout << "In " << double(searchEnd - searchStart) / CLOCKS_PER_SEC << " seconds, for " << viewCount << "candidate viewpoints" << endl;*/
//Original Grid Search
time_t time1 = time(0);
viewCount = 0;
tree.expand();
for (OcTree::leaf_bbx_iterator iter = tree.begin_leafs_bbx(searchBoxMin, searchBoxMax, 14); iter != tree.end_leafs_bbx(); iter++)
{
if (!tree.isNodeOccupied(*iter))
{
searchCount++;
}
}
vector<double> infoMap(rangeX * rangeY, 0);
cout << searchCount << " positions to calculate" << endl;
for (OcTree::leaf_iterator iter = tree.begin_leafs(14); iter != tree.end_leafs(); iter++)
{
if (tree.search(iter.getCoordinate()) == NULL || tree.isNodeOccupied(tree.search(iter.getCoordinate())) || iter.getCoordinate().z() < minHeight || iter.getCoordinate().z() > maxHeight) continue;
if (viewCount % 100 == 0)
{
cout << viewCount << endl;
}
point3d nextPos = iter.getCoordinate();
float pitch, yaw;
double infoGain = getInfoGain(&tree, &tree2, nextPos, &yaw, &pitch);
if (infoGain > 0)
{
double moveCost = gridMap[rangeX * int((nextPos.y() - minY) / res) + int((nextPos.x() - minX) / res)].cost;
if (infoGain > infoMap[rangeX * int((nextPos.y() - minY) / res) + int((nextPos.x() - minX) / res)])
{
infoMap[rangeX * int((nextPos.y() - minY) / res) + int((nextPos.x() - minX) / res)] = infoGain;
}
gains.push_back(InfoNode(infoGain, moveCost, nextPos, yaw, pitch));
if (infoGain > maxIG)
{
maxIG = infoGain;
bestPos = nextPos;
maxYaw = yaw;
maxPitch = pitch;
}
if (moveCost > maxCost && moveCost < 600)
{
maxCost = moveCost;
}
/*infoCloud->points[pointCount].x = nextPos.x();
* infoCloud->points[pointCount].y = nextPos.y();
* infoCloud->points[pointCount].z = nextPos.z();
* infoCloud->points[pointCount].intensity = infoGain;
* pointCount++;*/
//addCube(nextPos, 0.05, 1 - (infoGain / 40), infoGain / 40, 0, 0.9);
}
viewCount++;
}
cout << "Time for " << viewCount << " views: " << (int)time(0) - time1 << endl;
cout << "The NBV is at (" << bestPos.x() << ", " << bestPos.y() << ", " << bestPos.z() << ") with an expected info gain of " << maxIG << endl;
cout << maxYaw << ", " << maxPitch << endl;
cout << "Max Cost: " << maxCost << endl;
double maxQual = 0;
double correctIG = 0;
point3d NBV = origin;
for (vector<InfoNode>::iterator iter = gains.begin(); iter != gains.end(); iter++)
{
//iter->infoGain /= maxIG;
//iter->cost /= maxCost;
if (iter->cost < 600) iter->quality = iter->infoGain - 0 * iter->cost;
else iter->quality = 0;
if (iter->quality > maxQual)
{
maxQual = iter->quality;
correctIG = iter->infoGain;
NBV = iter->coords;
maxYaw = iter->yaw;
maxPitch = iter->pitch;
}
}
cout << "Time for " << viewCount << " views: " << (int)time(0) - time1 << endl;
cout << "The corrected NBV is at (" << NBV.x() << ", " << NBV.y() << ", " << NBV.z() << ") with a quality of " << maxQual << "and IG of " << correctIG << endl;
cout << "Yaw: " << maxYaw << ", " << "Pitch: " << maxPitch << endl;
point3d oldBest = bestPos;
if (maxIG < 10)
{
cout << "Algorithm Complete" << endl;
break;
}
NBVList.push_back(NBV);
visualization_msgs::Marker mark;
mark.header.frame_id = "map";
mark.header.stamp = ros::Time::now();
mark.ns = "basic_shapes";
mark.id = NBVcount;
mark.type = visualization_msgs::Marker::ARROW;
mark.action = visualization_msgs::Marker::ADD;
mark.pose.position.x = NBV.x();
mark.pose.position.y = NBV.y();
mark.pose.position.z = NBV.z();
q.setRPY(0, DEG2RAD(-maxPitch), DEG2RAD(maxYaw));
mark.pose.orientation.x = q.getX();
mark.pose.orientation.y = q.getY();
mark.pose.orientation.z = q.getZ();
mark.pose.orientation.w = q.getW();
mark.scale.x = 0.2;
mark.scale.y = 0.02;
mark.scale.z = 0.02;
mark.color.r = 0.0f;
mark.color.g = 1.0f;
mark.color.a = 1.0;
mark.color.b = 0.0f;
views.markers.push_back(mark);
mark_pub.publish(views);
/*transform.setOrigin( tf::Vector3(NBV.x(), NBV.y(), 0) );
q.setRPY(0, 0, DEG2RAD(maxYaw));
transform.setRotation(q);
gTime = ros::Time::now();
br.sendTransform(tf::StampedTransform(transform, gTime, "vicon", "sensorGoal"));
sleep(1);
try{
listener.lookupTransform("vicon", "baseGoal",
gTime, stransform);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
point3d baseGoal(stransform.getOrigin().x(), stransform.getOrigin().y(), 0);
m = tf::Matrix3x3(stransform.getRotation());
double bRoll, bPitch, bYaw;
m.getRPY(bRoll, bPitch, bYaw);*/
point3d baseGoal = NBV;
GridNode* endNode = &gridMap[rangeX * int((baseGoal.y() - minY) / res) + int((baseGoal.x() - minX) / res)];
vector<point3d> waypoints;
while (endNode != NULL)
{
//cout << endNode->coords.x() << ", " << endNode->coords.y() << endl;
//cout << "Cost: " << endNode->cost << endl;
endNode->cost = 0;
waypoints.push_back(endNode->coords);
//cout << endNode->coords.x() << ", " << endNode->coords.y() << endl;
endNode = endNode->parent;
}
reverse(waypoints.begin(), waypoints.end());
nav_msgs::Path path;
path.header.stamp = ros::Time::now();
path.header.frame_id = "map";
int state = 0;
int oldState = -1;
for (vector<point3d>::iterator iter = waypoints.begin(); iter != waypoints.end(); iter++)
{
geometry_msgs::PoseStamped pose;
pose.header.stamp = path.header.stamp;
pose.header.frame_id = path.header.frame_id;
pose.pose.position.x = iter->x();
pose.pose.position.y = iter->y();
pose.pose.position.z = NBV.z();
pose.pose.orientation.y = maxPitch;
pose.pose.orientation.z = maxYaw;
if (path.poses.size() > 0)
{
if (iter->x() == path.poses.back().pose.position.x)
{
state = 0;
}
else if (iter->y() == path.poses.back().pose.position.y)
{
state = 1;
}
else
{
state = 2;
}
}
//cout << "State, Old State, Size: " << state << " : " << oldState << " : " << path.poses.size() << endl;
if (path.poses.size() > 1 && state == oldState)
{
path.poses.pop_back();
//cout << pose.pose.position.x << ", " << pose.pose.position.y << endl;
}
path.poses.push_back(pose);
oldState = state;
}
/*for (vector<point3d>::iterator iter = waypoints.begin(); iter != waypoints.end(); iter++)
{
geometry_msgs::PoseStamped pose;
pose.header.stamp = path.header.stamp;
pose.header.frame_id = path.header.frame_id;
pose.pose.position.x = iter->x();
pose.pose.position.y = iter->y();
pose.pose.position.z = NBV.z();
pose.pose.orientation.y = maxPitch;
pose.pose.orientation.z = maxYaw;
path.poses.push_back(pose);
//cout << path.poses.back().position.x << ", " << path.poses.back().position.y << endl;
}*/
cout << "Path Message Generated" << endl;
path_pub.publish<nav_msgs::Path>(path);
ros::spinOnce();
cout << "Path Message Published" << endl;
}
while (ros::ok())
{
mark_pub.publish(views);
rate.sleep();
ros::spinOnce();
}
return 0;
}
