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