diff --git a/octomap/CHANGELOG.txt b/octomap/CHANGELOG.txt index 20c86d8c..49e57e4b 100644 --- a/octomap/CHANGELOG.txt +++ b/octomap/CHANGELOG.txt @@ -1,3 +1,10 @@ +v1.6.1: 2013-06-19 +================== +- New function swapContent to exchange the contents of two octrees (issue #32) +- New normal estimation based on marching cubes (thanks to F-M. de Rainville) +- Fixed octovis edit box affecting small selection ranges +- Fixed compilation with gcc 4.8 and OSX (issues #23 #28) + v1.6.0: 2013-04-05 ================== - Speedup: Pruning is now done only on affected nodes on each call to @@ -16,6 +23,14 @@ v1.6.0: 2013-04-05 to C. Dornhege) - Sample data is now in octomap/share +v1.5.6: 2013-06-10 +================== +- Fixed compilation with OSX 10.8 (issue #23) + +v1.5.5: 2013-05-05 +================== +- Fixed compilation of external code against OctoMap with gcc4.8 (issue #28) + v1.5.4: 2013-02-27 ================== - Removed binvox binaries from sources for improved packaging compatibility diff --git a/octomap/CMakeLists.txt b/octomap/CMakeLists.txt index 37929f2b..7e7a20fd 100644 --- a/octomap/CMakeLists.txt +++ b/octomap/CMakeLists.txt @@ -6,7 +6,7 @@ ENABLE_TESTING() # version (e.g. for packaging) set(OCTOMAP_MAJOR_VERSION 1) set(OCTOMAP_MINOR_VERSION 6) -set(OCTOMAP_PATCH_VERSION 0) +set(OCTOMAP_PATCH_VERSION 1) set(OCTOMAP_VERSION ${OCTOMAP_MAJOR_VERSION}.${OCTOMAP_MINOR_VERSION}.${OCTOMAP_PATCH_VERSION}) if(COMMAND cmake_policy) cmake_policy(SET CMP0003 NEW) diff --git a/octomap/README.md b/octomap/README.md index 4862d979..3a519ef5 100644 --- a/octomap/README.md +++ b/octomap/README.md @@ -76,7 +76,7 @@ GETTING STARTED --------------- Jump right in and have a look at the example -src/octomap/simple.cpp +src/octomap/simple_example.cpp Or start the 3D viewer with `bin/octovis` diff --git a/octomap/doxygen.h b/octomap/doxygen.h index e5b515da..3519214b 100644 --- a/octomap/doxygen.h +++ b/octomap/doxygen.h @@ -80,11 +80,11 @@ You can find an overview at http://octomap.github.com/ and the code repository a

- Jump right in and have a look at the main class \ref octomap::OcTree OcTree and the examples in src/octomap/simple.cpp. + Jump right in and have a look at the main class \ref octomap::OcTree OcTree and the examples in src/octomap/simple_example.cpp. To integrate single measurements into the 3D map have a look at \ref octomap::OcTree::insertRay "OcTree::insertRay(...)", to insert full 3D scans (pointclouds) please have a look at \ref octomap::OcTree::insertPointCloud "OcTree::insertPointCloud(...)". Queries can be performed e.g. with \ref octomap::OcTree::search "OcTree::search(...)" or - \ref octomap::OcTree::castRay(...) "OcTree::castRay(...)". The preferred way to batch-access or process nodes in an Octree is with the iterators + \ref octomap::OcTree::castRay "OcTree::castRay(...)". The preferred way to batch-access or process nodes in an Octree is with the iterators \ref leaf_iterator "leaf_iterator", \ref tree_iterator "tree_iterator", or \ref leaf_bbx_iterator "leaf_bbx_iterator".

\image html uml_overview.png diff --git a/octomap/include/octomap/MCTables.h b/octomap/include/octomap/MCTables.h new file mode 100644 index 00000000..31122b41 --- /dev/null +++ b/octomap/include/octomap/MCTables.h @@ -0,0 +1,351 @@ +/* + * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees + * http://octomap.github.com/ + * + * Copyright (c) 2013, F-M. De Rainville, P. Bourke + * All rights reserved. + * License: New BSD + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of Freiburg nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef OCTOMAP_MCTABLES_H +#define OCTOMAP_MCTABLES_H + + /** + * Tables used by the Marching Cubes Algorithm + * The tables are from Paul Bourke's web page + * http://paulbourke.net/geometry/polygonise/ + * Used with permission here under BSD license. + */ + +namespace octomap { + static const int edgeTable[256]={ + 0x0 , 0x109, 0x203, 0x30a, 0x406, 0x50f, 0x605, 0x70c, + 0x80c, 0x905, 0xa0f, 0xb06, 0xc0a, 0xd03, 0xe09, 0xf00, + 0x190, 0x99 , 0x393, 0x29a, 0x596, 0x49f, 0x795, 0x69c, + 0x99c, 0x895, 0xb9f, 0xa96, 0xd9a, 0xc93, 0xf99, 0xe90, + 0x230, 0x339, 0x33 , 0x13a, 0x636, 0x73f, 0x435, 0x53c, + 0xa3c, 0xb35, 0x83f, 0x936, 0xe3a, 0xf33, 0xc39, 0xd30, + 0x3a0, 0x2a9, 0x1a3, 0xaa , 0x7a6, 0x6af, 0x5a5, 0x4ac, + 0xbac, 0xaa5, 0x9af, 0x8a6, 0xfaa, 0xea3, 0xda9, 0xca0, + 0x460, 0x569, 0x663, 0x76a, 0x66 , 0x16f, 0x265, 0x36c, + 0xc6c, 0xd65, 0xe6f, 0xf66, 0x86a, 0x963, 0xa69, 0xb60, + 0x5f0, 0x4f9, 0x7f3, 0x6fa, 0x1f6, 0xff , 0x3f5, 0x2fc, + 0xdfc, 0xcf5, 0xfff, 0xef6, 0x9fa, 0x8f3, 0xbf9, 0xaf0, + 0x650, 0x759, 0x453, 0x55a, 0x256, 0x35f, 0x55 , 0x15c, + 0xe5c, 0xf55, 0xc5f, 0xd56, 0xa5a, 0xb53, 0x859, 0x950, + 0x7c0, 0x6c9, 0x5c3, 0x4ca, 0x3c6, 0x2cf, 0x1c5, 0xcc , + 0xfcc, 0xec5, 0xdcf, 0xcc6, 0xbca, 0xac3, 0x9c9, 0x8c0, + 0x8c0, 0x9c9, 0xac3, 0xbca, 0xcc6, 0xdcf, 0xec5, 0xfcc, + 0xcc , 0x1c5, 0x2cf, 0x3c6, 0x4ca, 0x5c3, 0x6c9, 0x7c0, + 0x950, 0x859, 0xb53, 0xa5a, 0xd56, 0xc5f, 0xf55, 0xe5c, + 0x15c, 0x55 , 0x35f, 0x256, 0x55a, 0x453, 0x759, 0x650, + 0xaf0, 0xbf9, 0x8f3, 0x9fa, 0xef6, 0xfff, 0xcf5, 0xdfc, + 0x2fc, 0x3f5, 0xff , 0x1f6, 0x6fa, 0x7f3, 0x4f9, 0x5f0, + 0xb60, 0xa69, 0x963, 0x86a, 0xf66, 0xe6f, 0xd65, 0xc6c, + 0x36c, 0x265, 0x16f, 0x66 , 0x76a, 0x663, 0x569, 0x460, + 0xca0, 0xda9, 0xea3, 0xfaa, 0x8a6, 0x9af, 0xaa5, 0xbac, + 0x4ac, 0x5a5, 0x6af, 0x7a6, 0xaa , 0x1a3, 0x2a9, 0x3a0, + 0xd30, 0xc39, 0xf33, 0xe3a, 0x936, 0x83f, 0xb35, 0xa3c, + 0x53c, 0x435, 0x73f, 0x636, 0x13a, 0x33 , 0x339, 0x230, + 0xe90, 0xf99, 0xc93, 0xd9a, 0xa96, 0xb9f, 0x895, 0x99c, + 0x69c, 0x795, 0x49f, 0x596, 0x29a, 0x393, 0x99 , 0x190, + 0xf00, 0xe09, 0xd03, 0xc0a, 0xb06, 0xa0f, 0x905, 0x80c, + 0x70c, 0x605, 0x50f, 0x406, 0x30a, 0x203, 0x109, 0x0 }; + + static const int triTable[256][16] = + {{-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 1, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 8, 3, 9, 8, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 8, 3, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {9, 2, 10, 0, 2, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {2, 8, 3, 2, 10, 8, 10, 9, 8, -1, -1, -1, -1, -1, -1, -1}, + {3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 11, 2, 8, 11, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 9, 0, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 11, 2, 1, 9, 11, 9, 8, 11, -1, -1, -1, -1, -1, -1, -1}, + {3, 10, 1, 11, 10, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 10, 1, 0, 8, 10, 8, 11, 10, -1, -1, -1, -1, -1, -1, -1}, + {3, 9, 0, 3, 11, 9, 11, 10, 9, -1, -1, -1, -1, -1, -1, -1}, + {9, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {4, 3, 0, 7, 3, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 1, 9, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {4, 1, 9, 4, 7, 1, 7, 3, 1, -1, -1, -1, -1, -1, -1, -1}, + {1, 2, 10, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {3, 4, 7, 3, 0, 4, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1}, + {9, 2, 10, 9, 0, 2, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1}, + {2, 10, 9, 2, 9, 7, 2, 7, 3, 7, 9, 4, -1, -1, -1, -1}, + {8, 4, 7, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {11, 4, 7, 11, 2, 4, 2, 0, 4, -1, -1, -1, -1, -1, -1, -1}, + {9, 0, 1, 8, 4, 7, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1}, + {4, 7, 11, 9, 4, 11, 9, 11, 2, 9, 2, 1, -1, -1, -1, -1}, + {3, 10, 1, 3, 11, 10, 7, 8, 4, -1, -1, -1, -1, -1, -1, -1}, + {1, 11, 10, 1, 4, 11, 1, 0, 4, 7, 11, 4, -1, -1, -1, -1}, + {4, 7, 8, 9, 0, 11, 9, 11, 10, 11, 0, 3, -1, -1, -1, -1}, + {4, 7, 11, 4, 11, 9, 9, 11, 10, -1, -1, -1, -1, -1, -1, -1}, + {9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {9, 5, 4, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 5, 4, 1, 5, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {8, 5, 4, 8, 3, 5, 3, 1, 5, -1, -1, -1, -1, -1, -1, -1}, + {1, 2, 10, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {3, 0, 8, 1, 2, 10, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1}, + {5, 2, 10, 5, 4, 2, 4, 0, 2, -1, -1, -1, -1, -1, -1, -1}, + {2, 10, 5, 3, 2, 5, 3, 5, 4, 3, 4, 8, -1, -1, -1, -1}, + {9, 5, 4, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 11, 2, 0, 8, 11, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1}, + {0, 5, 4, 0, 1, 5, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1}, + {2, 1, 5, 2, 5, 8, 2, 8, 11, 4, 8, 5, -1, -1, -1, -1}, + {10, 3, 11, 10, 1, 3, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1}, + {4, 9, 5, 0, 8, 1, 8, 10, 1, 8, 11, 10, -1, -1, -1, -1}, + {5, 4, 0, 5, 0, 11, 5, 11, 10, 11, 0, 3, -1, -1, -1, -1}, + {5, 4, 8, 5, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1}, + {9, 7, 8, 5, 7, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {9, 3, 0, 9, 5, 3, 5, 7, 3, -1, -1, -1, -1, -1, -1, -1}, + {0, 7, 8, 0, 1, 7, 1, 5, 7, -1, -1, -1, -1, -1, -1, -1}, + {1, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {9, 7, 8, 9, 5, 7, 10, 1, 2, -1, -1, -1, -1, -1, -1, -1}, + {10, 1, 2, 9, 5, 0, 5, 3, 0, 5, 7, 3, -1, -1, -1, -1}, + {8, 0, 2, 8, 2, 5, 8, 5, 7, 10, 5, 2, -1, -1, -1, -1}, + {2, 10, 5, 2, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1}, + {7, 9, 5, 7, 8, 9, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1}, + {9, 5, 7, 9, 7, 2, 9, 2, 0, 2, 7, 11, -1, -1, -1, -1}, + {2, 3, 11, 0, 1, 8, 1, 7, 8, 1, 5, 7, -1, -1, -1, -1}, + {11, 2, 1, 11, 1, 7, 7, 1, 5, -1, -1, -1, -1, -1, -1, -1}, + {9, 5, 8, 8, 5, 7, 10, 1, 3, 10, 3, 11, -1, -1, -1, -1}, + {5, 7, 0, 5, 0, 9, 7, 11, 0, 1, 0, 10, 11, 10, 0, -1}, + {11, 10, 0, 11, 0, 3, 10, 5, 0, 8, 0, 7, 5, 7, 0, -1}, + {11, 10, 5, 7, 11, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 8, 3, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {9, 0, 1, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 8, 3, 1, 9, 8, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1}, + {1, 6, 5, 2, 6, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 6, 5, 1, 2, 6, 3, 0, 8, -1, -1, -1, -1, -1, -1, -1}, + {9, 6, 5, 9, 0, 6, 0, 2, 6, -1, -1, -1, -1, -1, -1, -1}, + {5, 9, 8, 5, 8, 2, 5, 2, 6, 3, 2, 8, -1, -1, -1, -1}, + {2, 3, 11, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {11, 0, 8, 11, 2, 0, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1}, + {0, 1, 9, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1}, + {5, 10, 6, 1, 9, 2, 9, 11, 2, 9, 8, 11, -1, -1, -1, -1}, + {6, 3, 11, 6, 5, 3, 5, 1, 3, -1, -1, -1, -1, -1, -1, -1}, + {0, 8, 11, 0, 11, 5, 0, 5, 1, 5, 11, 6, -1, -1, -1, -1}, + {3, 11, 6, 0, 3, 6, 0, 6, 5, 0, 5, 9, -1, -1, -1, -1}, + {6, 5, 9, 6, 9, 11, 11, 9, 8, -1, -1, -1, -1, -1, -1, -1}, + {5, 10, 6, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {4, 3, 0, 4, 7, 3, 6, 5, 10, -1, -1, -1, -1, -1, -1, -1}, + {1, 9, 0, 5, 10, 6, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1}, + {10, 6, 5, 1, 9, 7, 1, 7, 3, 7, 9, 4, -1, -1, -1, -1}, + {6, 1, 2, 6, 5, 1, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1}, + {1, 2, 5, 5, 2, 6, 3, 0, 4, 3, 4, 7, -1, -1, -1, -1}, + {8, 4, 7, 9, 0, 5, 0, 6, 5, 0, 2, 6, -1, -1, -1, -1}, + {7, 3, 9, 7, 9, 4, 3, 2, 9, 5, 9, 6, 2, 6, 9, -1}, + {3, 11, 2, 7, 8, 4, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1}, + {5, 10, 6, 4, 7, 2, 4, 2, 0, 2, 7, 11, -1, -1, -1, -1}, + {0, 1, 9, 4, 7, 8, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1}, + {9, 2, 1, 9, 11, 2, 9, 4, 11, 7, 11, 4, 5, 10, 6, -1}, + {8, 4, 7, 3, 11, 5, 3, 5, 1, 5, 11, 6, -1, -1, -1, -1}, + {5, 1, 11, 5, 11, 6, 1, 0, 11, 7, 11, 4, 0, 4, 11, -1}, + {0, 5, 9, 0, 6, 5, 0, 3, 6, 11, 6, 3, 8, 4, 7, -1}, + {6, 5, 9, 6, 9, 11, 4, 7, 9, 7, 11, 9, -1, -1, -1, -1}, + {10, 4, 9, 6, 4, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {4, 10, 6, 4, 9, 10, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1}, + {10, 0, 1, 10, 6, 0, 6, 4, 0, -1, -1, -1, -1, -1, -1, -1}, + {8, 3, 1, 8, 1, 6, 8, 6, 4, 6, 1, 10, -1, -1, -1, -1}, + {1, 4, 9, 1, 2, 4, 2, 6, 4, -1, -1, -1, -1, -1, -1, -1}, + {3, 0, 8, 1, 2, 9, 2, 4, 9, 2, 6, 4, -1, -1, -1, -1}, + {0, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {8, 3, 2, 8, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1}, + {10, 4, 9, 10, 6, 4, 11, 2, 3, -1, -1, -1, -1, -1, -1, -1}, + {0, 8, 2, 2, 8, 11, 4, 9, 10, 4, 10, 6, -1, -1, -1, -1}, + {3, 11, 2, 0, 1, 6, 0, 6, 4, 6, 1, 10, -1, -1, -1, -1}, + {6, 4, 1, 6, 1, 10, 4, 8, 1, 2, 1, 11, 8, 11, 1, -1}, + {9, 6, 4, 9, 3, 6, 9, 1, 3, 11, 6, 3, -1, -1, -1, -1}, + {8, 11, 1, 8, 1, 0, 11, 6, 1, 9, 1, 4, 6, 4, 1, -1}, + {3, 11, 6, 3, 6, 0, 0, 6, 4, -1, -1, -1, -1, -1, -1, -1}, + {6, 4, 8, 11, 6, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {7, 10, 6, 7, 8, 10, 8, 9, 10, -1, -1, -1, -1, -1, -1, -1}, + {0, 7, 3, 0, 10, 7, 0, 9, 10, 6, 7, 10, -1, -1, -1, -1}, + {10, 6, 7, 1, 10, 7, 1, 7, 8, 1, 8, 0, -1, -1, -1, -1}, + {10, 6, 7, 10, 7, 1, 1, 7, 3, -1, -1, -1, -1, -1, -1, -1}, + {1, 2, 6, 1, 6, 8, 1, 8, 9, 8, 6, 7, -1, -1, -1, -1}, + {2, 6, 9, 2, 9, 1, 6, 7, 9, 0, 9, 3, 7, 3, 9, -1}, + {7, 8, 0, 7, 0, 6, 6, 0, 2, -1, -1, -1, -1, -1, -1, -1}, + {7, 3, 2, 6, 7, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {2, 3, 11, 10, 6, 8, 10, 8, 9, 8, 6, 7, -1, -1, -1, -1}, + {2, 0, 7, 2, 7, 11, 0, 9, 7, 6, 7, 10, 9, 10, 7, -1}, + {1, 8, 0, 1, 7, 8, 1, 10, 7, 6, 7, 10, 2, 3, 11, -1}, + {11, 2, 1, 11, 1, 7, 10, 6, 1, 6, 7, 1, -1, -1, -1, -1}, + {8, 9, 6, 8, 6, 7, 9, 1, 6, 11, 6, 3, 1, 3, 6, -1}, + {0, 9, 1, 11, 6, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {7, 8, 0, 7, 0, 6, 3, 11, 0, 11, 6, 0, -1, -1, -1, -1}, + {7, 11, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {3, 0, 8, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 1, 9, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {8, 1, 9, 8, 3, 1, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1}, + {10, 1, 2, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 2, 10, 3, 0, 8, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1}, + {2, 9, 0, 2, 10, 9, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1}, + {6, 11, 7, 2, 10, 3, 10, 8, 3, 10, 9, 8, -1, -1, -1, -1}, + {7, 2, 3, 6, 2, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {7, 0, 8, 7, 6, 0, 6, 2, 0, -1, -1, -1, -1, -1, -1, -1}, + {2, 7, 6, 2, 3, 7, 0, 1, 9, -1, -1, -1, -1, -1, -1, -1}, + {1, 6, 2, 1, 8, 6, 1, 9, 8, 8, 7, 6, -1, -1, -1, -1}, + {10, 7, 6, 10, 1, 7, 1, 3, 7, -1, -1, -1, -1, -1, -1, -1}, + {10, 7, 6, 1, 7, 10, 1, 8, 7, 1, 0, 8, -1, -1, -1, -1}, + {0, 3, 7, 0, 7, 10, 0, 10, 9, 6, 10, 7, -1, -1, -1, -1}, + {7, 6, 10, 7, 10, 8, 8, 10, 9, -1, -1, -1, -1, -1, -1, -1}, + {6, 8, 4, 11, 8, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {3, 6, 11, 3, 0, 6, 0, 4, 6, -1, -1, -1, -1, -1, -1, -1}, + {8, 6, 11, 8, 4, 6, 9, 0, 1, -1, -1, -1, -1, -1, -1, -1}, + {9, 4, 6, 9, 6, 3, 9, 3, 1, 11, 3, 6, -1, -1, -1, -1}, + {6, 8, 4, 6, 11, 8, 2, 10, 1, -1, -1, -1, -1, -1, -1, -1}, + {1, 2, 10, 3, 0, 11, 0, 6, 11, 0, 4, 6, -1, -1, -1, -1}, + {4, 11, 8, 4, 6, 11, 0, 2, 9, 2, 10, 9, -1, -1, -1, -1}, + {10, 9, 3, 10, 3, 2, 9, 4, 3, 11, 3, 6, 4, 6, 3, -1}, + {8, 2, 3, 8, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1}, + {0, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 9, 0, 2, 3, 4, 2, 4, 6, 4, 3, 8, -1, -1, -1, -1}, + {1, 9, 4, 1, 4, 2, 2, 4, 6, -1, -1, -1, -1, -1, -1, -1}, + {8, 1, 3, 8, 6, 1, 8, 4, 6, 6, 10, 1, -1, -1, -1, -1}, + {10, 1, 0, 10, 0, 6, 6, 0, 4, -1, -1, -1, -1, -1, -1, -1}, + {4, 6, 3, 4, 3, 8, 6, 10, 3, 0, 3, 9, 10, 9, 3, -1}, + {10, 9, 4, 6, 10, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {4, 9, 5, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 8, 3, 4, 9, 5, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1}, + {5, 0, 1, 5, 4, 0, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1}, + {11, 7, 6, 8, 3, 4, 3, 5, 4, 3, 1, 5, -1, -1, -1, -1}, + {9, 5, 4, 10, 1, 2, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1}, + {6, 11, 7, 1, 2, 10, 0, 8, 3, 4, 9, 5, -1, -1, -1, -1}, + {7, 6, 11, 5, 4, 10, 4, 2, 10, 4, 0, 2, -1, -1, -1, -1}, + {3, 4, 8, 3, 5, 4, 3, 2, 5, 10, 5, 2, 11, 7, 6, -1}, + {7, 2, 3, 7, 6, 2, 5, 4, 9, -1, -1, -1, -1, -1, -1, -1}, + {9, 5, 4, 0, 8, 6, 0, 6, 2, 6, 8, 7, -1, -1, -1, -1}, + {3, 6, 2, 3, 7, 6, 1, 5, 0, 5, 4, 0, -1, -1, -1, -1}, + {6, 2, 8, 6, 8, 7, 2, 1, 8, 4, 8, 5, 1, 5, 8, -1}, + {9, 5, 4, 10, 1, 6, 1, 7, 6, 1, 3, 7, -1, -1, -1, -1}, + {1, 6, 10, 1, 7, 6, 1, 0, 7, 8, 7, 0, 9, 5, 4, -1}, + {4, 0, 10, 4, 10, 5, 0, 3, 10, 6, 10, 7, 3, 7, 10, -1}, + {7, 6, 10, 7, 10, 8, 5, 4, 10, 4, 8, 10, -1, -1, -1, -1}, + {6, 9, 5, 6, 11, 9, 11, 8, 9, -1, -1, -1, -1, -1, -1, -1}, + {3, 6, 11, 0, 6, 3, 0, 5, 6, 0, 9, 5, -1, -1, -1, -1}, + {0, 11, 8, 0, 5, 11, 0, 1, 5, 5, 6, 11, -1, -1, -1, -1}, + {6, 11, 3, 6, 3, 5, 5, 3, 1, -1, -1, -1, -1, -1, -1, -1}, + {1, 2, 10, 9, 5, 11, 9, 11, 8, 11, 5, 6, -1, -1, -1, -1}, + {0, 11, 3, 0, 6, 11, 0, 9, 6, 5, 6, 9, 1, 2, 10, -1}, + {11, 8, 5, 11, 5, 6, 8, 0, 5, 10, 5, 2, 0, 2, 5, -1}, + {6, 11, 3, 6, 3, 5, 2, 10, 3, 10, 5, 3, -1, -1, -1, -1}, + {5, 8, 9, 5, 2, 8, 5, 6, 2, 3, 8, 2, -1, -1, -1, -1}, + {9, 5, 6, 9, 6, 0, 0, 6, 2, -1, -1, -1, -1, -1, -1, -1}, + {1, 5, 8, 1, 8, 0, 5, 6, 8, 3, 8, 2, 6, 2, 8, -1}, + {1, 5, 6, 2, 1, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 3, 6, 1, 6, 10, 3, 8, 6, 5, 6, 9, 8, 9, 6, -1}, + {10, 1, 0, 10, 0, 6, 9, 5, 0, 5, 6, 0, -1, -1, -1, -1}, + {0, 3, 8, 5, 6, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {10, 5, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {11, 5, 10, 7, 5, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {11, 5, 10, 11, 7, 5, 8, 3, 0, -1, -1, -1, -1, -1, -1, -1}, + {5, 11, 7, 5, 10, 11, 1, 9, 0, -1, -1, -1, -1, -1, -1, -1}, + {10, 7, 5, 10, 11, 7, 9, 8, 1, 8, 3, 1, -1, -1, -1, -1}, + {11, 1, 2, 11, 7, 1, 7, 5, 1, -1, -1, -1, -1, -1, -1, -1}, + {0, 8, 3, 1, 2, 7, 1, 7, 5, 7, 2, 11, -1, -1, -1, -1}, + {9, 7, 5, 9, 2, 7, 9, 0, 2, 2, 11, 7, -1, -1, -1, -1}, + {7, 5, 2, 7, 2, 11, 5, 9, 2, 3, 2, 8, 9, 8, 2, -1}, + {2, 5, 10, 2, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1}, + {8, 2, 0, 8, 5, 2, 8, 7, 5, 10, 2, 5, -1, -1, -1, -1}, + {9, 0, 1, 5, 10, 3, 5, 3, 7, 3, 10, 2, -1, -1, -1, -1}, + {9, 8, 2, 9, 2, 1, 8, 7, 2, 10, 2, 5, 7, 5, 2, -1}, + {1, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 8, 7, 0, 7, 1, 1, 7, 5, -1, -1, -1, -1, -1, -1, -1}, + {9, 0, 3, 9, 3, 5, 5, 3, 7, -1, -1, -1, -1, -1, -1, -1}, + {9, 8, 7, 5, 9, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {5, 8, 4, 5, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1}, + {5, 0, 4, 5, 11, 0, 5, 10, 11, 11, 3, 0, -1, -1, -1, -1}, + {0, 1, 9, 8, 4, 10, 8, 10, 11, 10, 4, 5, -1, -1, -1, -1}, + {10, 11, 4, 10, 4, 5, 11, 3, 4, 9, 4, 1, 3, 1, 4, -1}, + {2, 5, 1, 2, 8, 5, 2, 11, 8, 4, 5, 8, -1, -1, -1, -1}, + {0, 4, 11, 0, 11, 3, 4, 5, 11, 2, 11, 1, 5, 1, 11, -1}, + {0, 2, 5, 0, 5, 9, 2, 11, 5, 4, 5, 8, 11, 8, 5, -1}, + {9, 4, 5, 2, 11, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {2, 5, 10, 3, 5, 2, 3, 4, 5, 3, 8, 4, -1, -1, -1, -1}, + {5, 10, 2, 5, 2, 4, 4, 2, 0, -1, -1, -1, -1, -1, -1, -1}, + {3, 10, 2, 3, 5, 10, 3, 8, 5, 4, 5, 8, 0, 1, 9, -1}, + {5, 10, 2, 5, 2, 4, 1, 9, 2, 9, 4, 2, -1, -1, -1, -1}, + {8, 4, 5, 8, 5, 3, 3, 5, 1, -1, -1, -1, -1, -1, -1, -1}, + {0, 4, 5, 1, 0, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {8, 4, 5, 8, 5, 3, 9, 0, 5, 0, 3, 5, -1, -1, -1, -1}, + {9, 4, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {4, 11, 7, 4, 9, 11, 9, 10, 11, -1, -1, -1, -1, -1, -1, -1}, + {0, 8, 3, 4, 9, 7, 9, 11, 7, 9, 10, 11, -1, -1, -1, -1}, + {1, 10, 11, 1, 11, 4, 1, 4, 0, 7, 4, 11, -1, -1, -1, -1}, + {3, 1, 4, 3, 4, 8, 1, 10, 4, 7, 4, 11, 10, 11, 4, -1}, + {4, 11, 7, 9, 11, 4, 9, 2, 11, 9, 1, 2, -1, -1, -1, -1}, + {9, 7, 4, 9, 11, 7, 9, 1, 11, 2, 11, 1, 0, 8, 3, -1}, + {11, 7, 4, 11, 4, 2, 2, 4, 0, -1, -1, -1, -1, -1, -1, -1}, + {11, 7, 4, 11, 4, 2, 8, 3, 4, 3, 2, 4, -1, -1, -1, -1}, + {2, 9, 10, 2, 7, 9, 2, 3, 7, 7, 4, 9, -1, -1, -1, -1}, + {9, 10, 7, 9, 7, 4, 10, 2, 7, 8, 7, 0, 2, 0, 7, -1}, + {3, 7, 10, 3, 10, 2, 7, 4, 10, 1, 10, 0, 4, 0, 10, -1}, + {1, 10, 2, 8, 7, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {4, 9, 1, 4, 1, 7, 7, 1, 3, -1, -1, -1, -1, -1, -1, -1}, + {4, 9, 1, 4, 1, 7, 0, 8, 1, 8, 7, 1, -1, -1, -1, -1}, + {4, 0, 3, 7, 4, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {4, 8, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {9, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {3, 0, 9, 3, 9, 11, 11, 9, 10, -1, -1, -1, -1, -1, -1, -1}, + {0, 1, 10, 0, 10, 8, 8, 10, 11, -1, -1, -1, -1, -1, -1, -1}, + {3, 1, 10, 11, 3, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 2, 11, 1, 11, 9, 9, 11, 8, -1, -1, -1, -1, -1, -1, -1}, + {3, 0, 9, 3, 9, 11, 1, 2, 9, 2, 11, 9, -1, -1, -1, -1}, + {0, 2, 11, 8, 0, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {3, 2, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {2, 3, 8, 2, 8, 10, 10, 8, 9, -1, -1, -1, -1, -1, -1, -1}, + {9, 10, 2, 0, 9, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {2, 3, 8, 2, 8, 10, 0, 1, 8, 1, 10, 8, -1, -1, -1, -1}, + {1, 10, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 3, 8, 9, 1, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 9, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 3, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}}; + + static const point3d vertexList[12] = + {point3d(0, 1, -1), + point3d(1, 0, -1), + point3d(0, -1, -1), + point3d(-1, 0, -1), + point3d(0, 1, 1), + point3d(1, 0, 1), + point3d(0, -1, 1), + point3d(-1, 0, 1), + point3d(-1, 1, 0), + point3d(1, 1, 0), + point3d(1, -1, 0), + point3d(-1, -1, 0)}; + + } +#endif diff --git a/octomap/include/octomap/OcTreeBaseImpl.h b/octomap/include/octomap/OcTreeBaseImpl.h index dc2f2480..3723c0eb 100644 --- a/octomap/include/octomap/OcTreeBaseImpl.h +++ b/octomap/include/octomap/OcTreeBaseImpl.h @@ -82,8 +82,20 @@ namespace octomap { OcTreeBaseImpl(double resolution); virtual ~OcTreeBaseImpl(); + /// Deep copy constructor OcTreeBaseImpl(const OcTreeBaseImpl& rhs); + + /** + * Swap contents of two octrees, i.e., only the underlying + * pointer / tree structure. You have to ensure yourself that the + * metadata (resolution etc) matches. No memory is cleared + * in this function + */ + void swapContent(OcTreeBaseImpl& rhs); + + /// Comparison between two octrees, all meta data, all + /// nodes, and the structure must be identical bool operator== (const OcTreeBaseImpl& rhs) const; std::string getTreeType() const {return "OcTreeBaseImpl";} @@ -105,19 +117,22 @@ namespace octomap { inline NODE* getRoot() const { return root; } /** - * Search node at specified depth given a 3d point (depth=0: search full tree depth) + * Search node at specified depth given a 3d point (depth=0: search full tree depth). + * You need to check if the returned node is NULL, since it can be in unknown space. * @return pointer to node if found, NULL otherwise */ NODE* search(double x, double y, double z, unsigned int depth = 0) const; /** * Search node at specified depth given a 3d point (depth=0: search full tree depth) + * You need to check if the returned node is NULL, since it can be in unknown space. * @return pointer to node if found, NULL otherwise */ NODE* search(const point3d& value, unsigned int depth = 0) const; /** * Search a node at specified depth given an addressing key (depth=0: search full tree depth) + * You need to check if the returned node is NULL, since it can be in unknown space. * @return pointer to node if found, NULL otherwise */ NODE* search(const OcTreeKey& key, unsigned int depth = 0) const; @@ -146,11 +161,12 @@ namespace octomap { /// Deletes the complete tree structure void clear(); - OcTreeBaseImpl deepCopy() const; - - - /// Lossless compression of OcTree: merge children to parent when there are - /// eight children with identical values + /** + * Lossless compression of the octree: A node will replace all of its eight + * children if they have identical values. You usually don't have to call + * prune() after a regular occupancy update, updateNode() incrementally + * prunes all affected nodes. + */ virtual void prune(); /// Expands all pruned nodes (reverse of prune()) @@ -165,7 +181,7 @@ namespace octomap { /// \return Memory usage of the complete octree in bytes (may vary between architectures) virtual size_t memoryUsage() const; - /// \return Memory usage of the a single octree node + /// \return Memory usage of a single octree node virtual inline size_t memoryUsageNode() const {return sizeof(NODE); }; /// \return Memory usage of a full grid of the same size as the OcTree in bytes (for comparison) diff --git a/octomap/include/octomap/OcTreeBaseImpl.hxx b/octomap/include/octomap/OcTreeBaseImpl.hxx index b762c94a..51eab7fa 100644 --- a/octomap/include/octomap/OcTreeBaseImpl.hxx +++ b/octomap/include/octomap/OcTreeBaseImpl.hxx @@ -113,6 +113,17 @@ namespace octomap { } + template + void OcTreeBaseImpl::swapContent(OcTreeBaseImpl& other){ + NODE* this_root = root; + root = other.root; + other.root = this_root; + + size_t this_size = this->tree_size; + this->tree_size = other.tree_size; + other.tree_size = this_size; + } + template bool OcTreeBaseImpl::operator== (const OcTreeBaseImpl& other) const{ if (tree_depth != other.tree_depth || tree_max_val != other.tree_max_val diff --git a/octomap/include/octomap/OcTreeIterator.hxx b/octomap/include/octomap/OcTreeIterator.hxx index 5a7c0920..d89d2942 100644 --- a/octomap/include/octomap/OcTreeIterator.hxx +++ b/octomap/include/octomap/OcTreeIterator.hxx @@ -130,7 +130,7 @@ return tree->keyToCoord(stack.top().key[2], stack.top().depth); } - /// @return the side if the volume occupied by the current node + /// @return the side of the volume occupied by the current node double getSize() const {return tree->getNodeSize(stack.top().depth); } /// return depth of the current node diff --git a/octomap/include/octomap/OccupancyOcTreeBase.h b/octomap/include/octomap/OccupancyOcTreeBase.h index 238daff5..5d1c41de 100644 --- a/octomap/include/octomap/OccupancyOcTreeBase.h +++ b/octomap/include/octomap/OccupancyOcTreeBase.h @@ -37,6 +37,7 @@ #include #include +#include #include "octomap_types.h" #include "octomap_utils.h" @@ -82,6 +83,8 @@ namespace octomap { * This avoids holes in the floor from mutual deletion and is more efficient than the plain * ray insertion in insertPointCloudRays(). * + * @note replaces insertScan() + * * @param scan Pointcloud (measurement endpoints), in global reference frame * @param sensor_origin measurement origin in global reference frame * @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam) @@ -98,6 +101,8 @@ namespace octomap { * This avoids holes in the floor from mutual deletion and is more efficient than the plain * ray insertion in insertPointCloudRays(). * + * @note replaces insertScan() + * * @param scan Pointcloud (measurement endpoints) relative to frame origin * @param sensor_origin origin of sensor relative to frame origin * @param frame_origin origin of reference frame, determines transform to be applied to cloud and sensor origin @@ -111,6 +116,8 @@ namespace octomap { /** * Insert a 3d scan (given as a ScanNode) into the tree, parallelized with OpenMP. * + * @note replaces insertScan + * * @param scan ScanNode contains Pointcloud data and frame/sensor origin * @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam) * @param lazy_eval whether the tree is left 'dirty' after the update (default: false). @@ -265,14 +272,25 @@ namespace octomap { * @param[in] origin starting coordinate of ray * @param[in] direction A vector pointing in the direction of the raycast. Does not need to be normalized. * @param[out] end returns the center of the cell that was hit by the ray, if successful - * @param[in] ignoreUnknownCells whether unknown cells are ignored. If false (default), the raycast aborts when an unkown cell is hit. + * @param[in] ignoreUnknownCells whether unknown cells are ignored. If false (default), the raycast aborts when an unknown cell is hit. * @param[in] maxRange Maximum range after which the raycast is aborted (<= 0: no limit, default) * @return whether or not an occupied cell was hit */ virtual bool castRay(const point3d& origin, const point3d& direction, point3d& end, bool ignoreUnknownCells=false, double maxRange=-1.0) const; - + /** + * Performs a step of the marching cubes surface reconstruction algorithm + * to retreive the normal of the triangles that fall in the cube + * formed by the voxels located at the vertex of a given voxel. + * + * @param[in] voxel for which retreive the normals + * @param[out] triangles normals + * @param[in] unknownStatus consider unknown cells as free (false) or occupied (default, true). + * @return True if the input voxel is known in the occupancy grid, and false if it is unknown. + */ + bool getNormals(const point3d& point, std::vector& normals, bool unknownStatus=true) const; + //-- set BBX limit (limits tree updates to this bounding box) /// use or ignore BBX limit (default: ignore) diff --git a/octomap/include/octomap/OccupancyOcTreeBase.hxx b/octomap/include/octomap/OccupancyOcTreeBase.hxx index adb64c6c..9e6eb10d 100644 --- a/octomap/include/octomap/OccupancyOcTreeBase.hxx +++ b/octomap/include/octomap/OccupancyOcTreeBase.hxx @@ -33,6 +33,8 @@ #include +#include + namespace octomap { template @@ -426,6 +428,79 @@ namespace octomap { } } + template + bool OccupancyOcTreeBase::getNormals(const point3d& point, std::vector& normals, + bool unknownStatus) const { + normals.clear(); + + OcTreeKey init_key; + if ( !OcTreeBaseImpl::coordToKeyChecked(point, init_key) ) { + OCTOMAP_WARNING_STR("Voxel out of bounds"); + return false; + } + + int vertex_values[8]; + + OcTreeKey current_key; + NODE* current_node; + + int x_index[4] = {-1, 1, 1, -1}; + int y_index[4] = {1, 1, -1, -1}; + int z_index[2] = {-1, 1}; + + int k = 0; + for(int j = 0; j < 2; ++j){ + for(int i = 0; i < 4; ++i){ + current_key[0] = init_key[0] + x_index[i]; + current_key[1] = init_key[1] + y_index[i]; + current_key[2] = init_key[2] + z_index[j]; + current_node = this->search(current_key); + + if(current_node){ + vertex_values[k] = this->isNodeOccupied(current_node); + + // point3d coord = this->keyToCoord(current_key); + // OCTOMAP_WARNING_STR("vertex " << k << " at " << coord << "; value " << vertex_values[k]); + }else{ + // Occupancy of unknown cells + vertex_values[k] = unknownStatus; + } + ++k; + } + } + + int cube_index = 0; + if (vertex_values[0]) cube_index |= 1; + if (vertex_values[1]) cube_index |= 2; + if (vertex_values[2]) cube_index |= 4; + if (vertex_values[3]) cube_index |= 8; + if (vertex_values[4]) cube_index |= 16; + if (vertex_values[5]) cube_index |= 32; + if (vertex_values[6]) cube_index |= 64; + if (vertex_values[7]) cube_index |= 128; + + // OCTOMAP_WARNING_STR("cubde_index: " << cube_index); + + // All vertices are occupied or free resulting in no normal + if (edgeTable[cube_index] == 0) + return true; + + // No interpolation is done yet, we use vertexList in . + for(int i = 0; triTable[cube_index][i] != -1; i += 3){ + point3d p1 = vertexList[triTable[cube_index][i ]]; + point3d p2 = vertexList[triTable[cube_index][i+1]]; + point3d p3 = vertexList[triTable[cube_index][i+2]]; + point3d v1 = p2 - p1; + point3d v2 = p3 - p1; + + // Right hand side cross product to retrieve the normal in the good + // direction (pointing to the free nodes). + normals.push_back(v1.cross(v2).normalize()); + } + + return true; + } + template bool OccupancyOcTreeBase::castRay(const point3d& origin, const point3d& directionP, point3d& end, bool ignoreUnknown, double maxRange) const { diff --git a/octomap/octomap-config.cmake.in b/octomap/octomap-config.cmake.in index 30aaf4ea..64b7d7c7 100644 --- a/octomap/octomap-config.cmake.in +++ b/octomap/octomap-config.cmake.in @@ -24,11 +24,15 @@ set(OCTOMAP_INCLUDE_DIRS @OCTOMAP_INCLUDE_DIRS@) set(OCTOMAP_LIBRARY_DIRS @OCTOMAP_LIB_DIR@) -# Our library dependencies (contains definitions for IMPORTED targets) -# include("@FOOBAR_CMAKE_DIR@/FooBarLibraryDepends.cmake") +# library ext. hardcoded to dylib on Apple +if(APPLE) + set(SO_EXT ".dylib") +else(APPLE) + set(SO_EXT ".so") +endif(APPLE) # Set library names as absolute paths: set(OCTOMAP_LIBRARIES - @OCTOMAP_LIB_DIR@/liboctomap.so - @OCTOMAP_LIB_DIR@/liboctomath.so + @OCTOMAP_LIB_DIR@/liboctomap${SO_EXT} + @OCTOMAP_LIB_DIR@/liboctomath${SO_EXT} ) \ No newline at end of file diff --git a/octomap/package.xml b/octomap/package.xml index 5875aedd..d5ee6d3d 100644 --- a/octomap/package.xml +++ b/octomap/package.xml @@ -1,15 +1,15 @@ octomap - 1.6.0 + 1.6.1 The OctoMap library implements a 3D occupancy grid mapping approach, providing data structures and mapping algorithms in C++. The map implementation is based on an octree. See - http://octomap.github.com for details. + http://octomap.github.io for details. Kai M. Wurm Armin Hornung Armin Hornung BSD - http://octomap.github.com + http://octomap.github.io https://github.com/OctoMap/octomap/issues catkin diff --git a/octomap/src/CMakeLists.txt b/octomap/src/CMakeLists.txt index 7fbec01a..be96125b 100644 --- a/octomap/src/CMakeLists.txt +++ b/octomap/src/CMakeLists.txt @@ -44,8 +44,11 @@ TARGET_LINK_LIBRARIES(eval_octree_accuracy octomap) ADD_EXECUTABLE(compare_octrees compare_octrees.cpp) TARGET_LINK_LIBRARIES(compare_octrees octomap) -ADD_EXECUTABLE(simple simple.cpp) -TARGET_LINK_LIBRARIES(simple octomap) +ADD_EXECUTABLE(simple_example simple_example.cpp) +TARGET_LINK_LIBRARIES(simple_example octomap) + +ADD_EXECUTABLE(normals_example normals_example.cpp) +TARGET_LINK_LIBRARIES(normals_example octomap) # installation: diff --git a/octomap/src/compare_octrees.cpp b/octomap/src/compare_octrees.cpp index b83aba00..40c1e5c3 100644 --- a/octomap/src/compare_octrees.cpp +++ b/octomap/src/compare_octrees.cpp @@ -37,6 +37,7 @@ #include #include #include +#include using std::cout; using std::endl; @@ -126,7 +127,7 @@ int main(int argc, char** argv) { else kld +=log(p1/p2)*p1 + log((1-p1)/(1-p2))*(1-p1); - if (isnan(kld)){ + if (std::isnan(kld)){ OCTOMAP_ERROR("KLD is nan! KLD(%f,%f)=%f; sum = %f", p1, p2, kld, kld_sum); exit(-1); } diff --git a/octomap/src/normals_example.cpp b/octomap/src/normals_example.cpp new file mode 100644 index 00000000..20eee0da --- /dev/null +++ b/octomap/src/normals_example.cpp @@ -0,0 +1,112 @@ +/* + * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees + * http://octomap.github.com/ + * + * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg + * All rights reserved. + * License: New BSD + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of Freiburg nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include +#include + +using namespace std; +using namespace octomap; + + +void print_query_info(point3d query, OcTreeNode* node) { + if (node != NULL) { + cout << "occupancy probability at " << query << ":\t " << node->getOccupancy() << endl; + } + else + cout << "occupancy probability at " << query << ":\t is unknown" << endl; +} + +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 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"; + } +} diff --git a/octomap/src/simple.cpp b/octomap/src/simple_example.cpp similarity index 100% rename from octomap/src/simple.cpp rename to octomap/src/simple_example.cpp diff --git a/octomap/src/testing/test_io.cpp b/octomap/src/testing/test_io.cpp index ac8627e7..afbbc147 100644 --- a/octomap/src/testing/test_io.cpp +++ b/octomap/src/testing/test_io.cpp @@ -68,6 +68,15 @@ int main(int argc, char** argv) { delete treeCopy; + // 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)); diff --git a/octovis/CMakeLists.txt b/octovis/CMakeLists.txt index 62222a18..7db5ee55 100644 --- a/octovis/CMakeLists.txt +++ b/octovis/CMakeLists.txt @@ -4,7 +4,7 @@ PROJECT( octovis ) # # version (e.g. for packaging) set(OCTOVIS_MAJOR_VERSION 1) set(OCTOVIS_MINOR_VERSION 6) -set(OCTOVIS_PATCH_VERSION 0) +set(OCTOVIS_PATCH_VERSION 1) set(OCTOVIS_VERSION ${OCTOVIS_MAJOR_VERSION}.${OCTOVIS_MINOR_VERSION}.${OCTOVIS_PATCH_VERSION}) # get rid of a useless warning: if(COMMAND cmake_policy) diff --git a/octovis/include/octovis/CameraFollowMode.h b/octovis/include/octovis/CameraFollowMode.h index 4a5432f4..f0637220 100644 --- a/octovis/include/octovis/CameraFollowMode.h +++ b/octovis/include/octovis/CameraFollowMode.h @@ -29,51 +29,51 @@ #include "SceneObject.h" class CameraFollowMode : public QObject { - Q_OBJECT + Q_OBJECT public: - CameraFollowMode(octomap::ScanGraph *graph = NULL); - virtual ~CameraFollowMode(); - void setScanGraph(octomap::ScanGraph *graph); + CameraFollowMode(octomap::ScanGraph *graph = NULL); + virtual ~CameraFollowMode(); + void setScanGraph(octomap::ScanGraph *graph); public slots: - void jumpToFrame(unsigned int frame); - void cameraPathStopped(int id); - void cameraPathFrameChanged(int id, int current_camera_frame); - void play(); - void pause(); - void clearCameraPath(); - void saveToCameraPath(); - void addToCameraPath(); - void removeFromCameraPath(); - void followCameraPath(); - void followRobotPath(); + void jumpToFrame(unsigned int frame); + void cameraPathStopped(int id); + void cameraPathFrameChanged(int id, int current_camera_frame); + void play(); + void pause(); + void clearCameraPath(); + void saveToCameraPath(); + void addToCameraPath(); + void removeFromCameraPath(); + void followCameraPath(); + void followRobotPath(); signals: - void changeCamPose(const octomath::Pose6D& pose); - void interpolateCamPose(const octomath::Pose6D& old_pose, const octomath::Pose6D& new_pose, double u); - void stopped(); - void frameChanged(unsigned int frame); - void deleteCameraPath(int id); - void removeFromCameraPath(int id, int frame); - void updateCameraPath(int id, int frame); - void appendToCameraPath(int id, const octomath::Pose6D& pose); - void appendCurrentToCameraPath(int id); - void addCurrentToCameraPath(int id, int frame); - void playCameraPath(int id, int start_frame); - void stopCameraPath(int id); - void jumpToCamFrame(int id, int frame); - void changeNumberOfFrames(unsigned count); - void scanGraphAvailable(bool available); + void changeCamPose(const octomath::Pose6D& pose); + void interpolateCamPose(const octomath::Pose6D& old_pose, const octomath::Pose6D& new_pose, double u); + void stopped(); + void frameChanged(unsigned int frame); + void deleteCameraPath(int id); + void removeFromCameraPath(int id, int frame); + void updateCameraPath(int id, int frame); + void appendToCameraPath(int id, const octomath::Pose6D& pose); + void appendCurrentToCameraPath(int id); + void addCurrentToCameraPath(int id, int frame); + void playCameraPath(int id, int start_frame); + void stopCameraPath(int id); + void jumpToCamFrame(int id, int frame); + void changeNumberOfFrames(unsigned count); + void scanGraphAvailable(bool available); protected: - octomap::ScanGraph *m_scan_graph; - unsigned int m_current_scan; - unsigned int m_current_cam_frame; - unsigned int m_number_cam_frames; - unsigned int m_start_frame; - bool m_followRobotTrajectory; + octomap::ScanGraph *m_scan_graph; + unsigned int m_current_scan; + unsigned int m_current_cam_frame; + unsigned int m_number_cam_frames; + unsigned int m_start_frame; + bool m_followRobotTrajectory; }; #endif /* CAMERAFOLLOWMODE_H_ */ diff --git a/octovis/octovis-config.cmake.in b/octovis/octovis-config.cmake.in index 2a78844f..083f7a05 100644 --- a/octovis/octovis-config.cmake.in +++ b/octovis/octovis-config.cmake.in @@ -9,9 +9,16 @@ set(OCTOVIS_INCLUDE_DIRS @QGLViewer_INCLUDE_DIR@ @OCTOVIS_INCLUDE_DIR@) set(OCTOVIS_LIBRARY_DIRS @QGLViewer_LIBRARY_DIR@ @OCTOVIS_LIB_DIR@) +# library ext. hardcoded to dylib on Apple +if(APPLE) + set(SO_EXT ".dylib") +else(APPLE) + set(SO_EXT ".so") +endif(APPLE) + # Set library names as absolute paths: set(OCTOVIS_LIBRARIES @QGLViewer_LIBRARIES@ @QT_LIBRARIES@ - @OCTOVIS_LIB_DIR@/liboctovis.so + @OCTOVIS_LIB_DIR@/liboctovis${SO_EXT} ) diff --git a/octovis/package.xml b/octovis/package.xml index fb549abb..84e5bdc4 100644 --- a/octovis/package.xml +++ b/octovis/package.xml @@ -1,15 +1,15 @@ octovis - 1.6.0 + 1.6.1 octovis is visualization tool for the OctoMap library based on Qt and libQGLViewer. See - http://octomap.github.com for details. + http://octomap.github.io for details. Kai M. Wurm Armin Hornung Armin Hornung GPLv2 - http://octomap.github.com + http://octomap.github.io https://github.com/OctoMap/octomap/issues diff --git a/octovis/src/CameraFollowMode.cpp b/octovis/src/CameraFollowMode.cpp index 269c4b28..4b9f3326 100644 --- a/octovis/src/CameraFollowMode.cpp +++ b/octovis/src/CameraFollowMode.cpp @@ -30,8 +30,8 @@ #define ROBOT_TRAJECTORY_ID 14 CameraFollowMode::CameraFollowMode(octomap::ScanGraph *graph) - : QObject(), m_scan_graph(graph), m_current_scan(1), m_current_cam_frame(1), m_number_cam_frames(0), - m_followRobotTrajectory(false) { +: QObject(), m_scan_graph(graph), m_current_scan(1), m_current_cam_frame(1), m_number_cam_frames(0), + m_followRobotTrajectory(false) { } CameraFollowMode::~CameraFollowMode() { @@ -39,104 +39,104 @@ CameraFollowMode::~CameraFollowMode() { void CameraFollowMode::jumpToFrame(unsigned int frame) { - if(m_followRobotTrajectory) { - if(frame >= 0 && frame <= m_scan_graph->size()) { - m_current_scan = frame; - octomath::Pose6D pose = m_scan_graph->getNodeByID(frame-1)->pose; - emit changeCamPose(pose); - } - } else { - m_current_cam_frame = frame; - emit jumpToCamFrame(CAMERA_PATH_ID, m_current_cam_frame-1); - } + if(m_followRobotTrajectory) { + if(frame <= m_scan_graph->size()) { + m_current_scan = frame; + octomath::Pose6D pose = m_scan_graph->getNodeByID(frame-1)->pose; + emit changeCamPose(pose); + } + } else { + m_current_cam_frame = frame; + emit jumpToCamFrame(CAMERA_PATH_ID, m_current_cam_frame-1); + } } void CameraFollowMode::play() { - if(m_followRobotTrajectory) { - emit deleteCameraPath(ROBOT_TRAJECTORY_ID); - //emit appendCurrentToCameraPath(ROBOT_TRAJECTORY_ID); - m_start_frame = m_current_scan; - for(unsigned int i = m_start_frame; i <= m_scan_graph->size(); i++) { - octomap::ScanNode* scanNode = m_scan_graph->getNodeByID(i-1); - if (scanNode) - emit appendToCameraPath(ROBOT_TRAJECTORY_ID, scanNode->pose); - else{ - std::cerr << "Error in " << __FILE__ << ":" << __LINE__ <<" : invalid node ID "<< i-1 << std::endl; - } - } - emit playCameraPath(ROBOT_TRAJECTORY_ID, 0); - } else { - m_start_frame = m_current_cam_frame; - emit playCameraPath(CAMERA_PATH_ID, m_start_frame-1); - } + if(m_followRobotTrajectory) { + emit deleteCameraPath(ROBOT_TRAJECTORY_ID); + //emit appendCurrentToCameraPath(ROBOT_TRAJECTORY_ID); + m_start_frame = m_current_scan; + for(unsigned int i = m_start_frame; i <= m_scan_graph->size(); i++) { + octomap::ScanNode* scanNode = m_scan_graph->getNodeByID(i-1); + if (scanNode) + emit appendToCameraPath(ROBOT_TRAJECTORY_ID, scanNode->pose); + else{ + std::cerr << "Error in " << __FILE__ << ":" << __LINE__ <<" : invalid node ID "<< i-1 << std::endl; + } + } + emit playCameraPath(ROBOT_TRAJECTORY_ID, 0); + } else { + m_start_frame = m_current_cam_frame; + emit playCameraPath(CAMERA_PATH_ID, m_start_frame-1); + } } void CameraFollowMode::pause() { - emit stopCameraPath(CAMERA_PATH_ID); - emit stopCameraPath(ROBOT_TRAJECTORY_ID); + emit stopCameraPath(CAMERA_PATH_ID); + emit stopCameraPath(ROBOT_TRAJECTORY_ID); } void CameraFollowMode::setScanGraph(octomap::ScanGraph *graph) { - m_scan_graph = graph; - emit scanGraphAvailable(true); + m_scan_graph = graph; + emit scanGraphAvailable(true); } void CameraFollowMode::cameraPathStopped(int id) { - if(id == CAMERA_PATH_ID || id == ROBOT_TRAJECTORY_ID) { - emit stopped(); - } + if(id == CAMERA_PATH_ID || id == ROBOT_TRAJECTORY_ID) { + emit stopped(); + } } void CameraFollowMode::cameraPathFrameChanged(int id, int current_camera_frame) { - if(m_followRobotTrajectory) { - m_current_scan = m_start_frame + current_camera_frame; - emit frameChanged(m_current_scan); - } else { - m_current_cam_frame = m_start_frame + current_camera_frame; - emit frameChanged(m_current_cam_frame); - } + if(m_followRobotTrajectory) { + m_current_scan = m_start_frame + current_camera_frame; + emit frameChanged(m_current_scan); + } else { + m_current_cam_frame = m_start_frame + current_camera_frame; + emit frameChanged(m_current_cam_frame); + } } void CameraFollowMode::clearCameraPath() { - emit deleteCameraPath(CAMERA_PATH_ID); - m_current_cam_frame = 1; - m_number_cam_frames = 0; - emit frameChanged(m_current_cam_frame); - emit changeNumberOfFrames(m_number_cam_frames); + emit deleteCameraPath(CAMERA_PATH_ID); + m_current_cam_frame = 1; + m_number_cam_frames = 0; + emit frameChanged(m_current_cam_frame); + emit changeNumberOfFrames(m_number_cam_frames); } void CameraFollowMode::saveToCameraPath() { - emit updateCameraPath(CAMERA_PATH_ID, m_current_cam_frame-1); + emit updateCameraPath(CAMERA_PATH_ID, m_current_cam_frame-1); } void CameraFollowMode::addToCameraPath() { - emit addCurrentToCameraPath(CAMERA_PATH_ID, m_current_cam_frame-1); - m_number_cam_frames++; - if(m_number_cam_frames == 1) m_current_cam_frame = 1; - else m_current_cam_frame++; - emit frameChanged(m_current_cam_frame); - emit changeNumberOfFrames(m_number_cam_frames); + emit addCurrentToCameraPath(CAMERA_PATH_ID, m_current_cam_frame-1); + m_number_cam_frames++; + if(m_number_cam_frames == 1) m_current_cam_frame = 1; + else m_current_cam_frame++; + emit frameChanged(m_current_cam_frame); + emit changeNumberOfFrames(m_number_cam_frames); } void CameraFollowMode::removeFromCameraPath() { - if(m_number_cam_frames>0) { - emit removeFromCameraPath(CAMERA_PATH_ID, m_current_cam_frame-1); - m_number_cam_frames--; - emit changeNumberOfFrames(m_number_cam_frames); - } + if(m_number_cam_frames>0) { + emit removeFromCameraPath(CAMERA_PATH_ID, m_current_cam_frame-1); + m_number_cam_frames--; + emit changeNumberOfFrames(m_number_cam_frames); + } } void CameraFollowMode::followCameraPath() { - m_followRobotTrajectory = false; - emit frameChanged(m_current_cam_frame); - emit changeNumberOfFrames(m_number_cam_frames); + m_followRobotTrajectory = false; + emit frameChanged(m_current_cam_frame); + emit changeNumberOfFrames(m_number_cam_frames); } void CameraFollowMode::followRobotPath() { - if(m_scan_graph) { - m_followRobotTrajectory = true; - emit frameChanged(m_current_scan); - emit changeNumberOfFrames(m_scan_graph->size()); - } + if(m_scan_graph) { + m_followRobotTrajectory = true; + emit frameChanged(m_current_scan); + emit changeNumberOfFrames(m_scan_graph->size()); + } } diff --git a/octovis/src/SelectionBox.cpp b/octovis/src/SelectionBox.cpp index 9d902f8c..4200be8b 100644 --- a/octovis/src/SelectionBox.cpp +++ b/octovis/src/SelectionBox.cpp @@ -29,262 +29,250 @@ namespace octomap{ - SelectionBox::SelectionBox() - : m_visible(false), - m_minPt(0,0,0), m_maxPt(1,1,1), - m_arrowLength(0.2) - { - - - - for (unsigned i=0; i< 3; ++i){ - m_frames.push_back(new qglviewer::ManipulatedFrame()); - } - - for (unsigned i=0; i< 3; ++i){ - m_frames.push_back(new qglviewer::ManipulatedFrame()); - } - - qglviewer::WorldConstraint* XAxis = new qglviewer::WorldConstraint(); - XAxis->setTranslationConstraint(qglviewer::AxisPlaneConstraint::AXIS, qglviewer::Vec(1.0,0.0,0.0)); - XAxis->setRotationConstraint (qglviewer::AxisPlaneConstraint::FORBIDDEN, qglviewer::Vec(0.0,0.0,0.0)); - - qglviewer::WorldConstraint* YAxis = new qglviewer::WorldConstraint(); - YAxis->setTranslationConstraint(qglviewer::AxisPlaneConstraint::AXIS, qglviewer::Vec(0.0,1.0,0.0)); - YAxis->setRotationConstraint (qglviewer::AxisPlaneConstraint::FORBIDDEN, qglviewer::Vec(0.0,0.0,0.0)); - - qglviewer::WorldConstraint* ZAxis = new qglviewer::WorldConstraint(); - ZAxis->setTranslationConstraint(qglviewer::AxisPlaneConstraint::AXIS, qglviewer::Vec(0.0,0.0,1.0)); - ZAxis->setRotationConstraint (qglviewer::AxisPlaneConstraint::FORBIDDEN, qglviewer::Vec(0.0,0.0,0.0)); - - - frame(0)->setConstraint(XAxis); - frame(1)->setConstraint(YAxis); - frame(2)->setConstraint(ZAxis); - frame(3)->setConstraint(XAxis); - frame(4)->setConstraint(YAxis); - frame(5)->setConstraint(ZAxis); +SelectionBox::SelectionBox() +: m_visible(false), + m_minPt(0,0,0), m_maxPt(1,1,1), + m_arrowLength(0.2) +{ + for (unsigned i=0; i< 3; ++i){ + m_frames.push_back(new qglviewer::ManipulatedFrame()); } - SelectionBox::~SelectionBox(){ - delete m_frames[0]; - delete m_frames[1]; + for (unsigned i=0; i< 3; ++i){ + m_frames.push_back(new qglviewer::ManipulatedFrame()); } - void SelectionBox::draw(bool withNames){ - - - // set min/max new from grabbed frame: - for (unsigned i = 0; i < m_frames.size(); ++i){ - if (frame(i)->grabsMouse()){ - qglviewer::Vec f = frame(i)->position(); - - unsigned oi = i+3; - float corr = m_arrowLength/2.0; - if (i >= 3){ - oi = i-3; - corr *= -1; - } - - qglviewer::Vec fo = frame(oi)->position(); - - unsigned ci = i%3; - m_minPt[ci] = std::min(f[ci] - corr, fo[ci] + corr); - m_maxPt[ci] = std::max(f[ci] - corr, fo[ci] + corr); - } - } - - // draw box: - - glEnable(GL_LINE_SMOOTH); - glLineWidth(2.); - glDisable(GL_LIGHTING); - glColor3f(0.9,0.0, 0.0); - glBegin(GL_LINE_LOOP); // Start drawing a line primitive - glVertex3f(m_minPt.x, m_minPt.y, m_minPt.z); - glVertex3f(m_maxPt.x, m_minPt.y, m_minPt.z); - glVertex3f(m_maxPt.x, m_maxPt.y, m_minPt.z); - glVertex3f(m_minPt.x, m_maxPt.y, m_minPt.z); - glEnd(); + qglviewer::WorldConstraint* XAxis = new qglviewer::WorldConstraint(); + XAxis->setTranslationConstraint(qglviewer::AxisPlaneConstraint::AXIS, qglviewer::Vec(1.0,0.0,0.0)); + XAxis->setRotationConstraint (qglviewer::AxisPlaneConstraint::FORBIDDEN, qglviewer::Vec(0.0,0.0,0.0)); - glBegin(GL_LINE_LOOP); - glVertex3f(m_minPt.x, m_minPt.y, m_maxPt.z); - glVertex3f(m_maxPt.x, m_minPt.y, m_maxPt.z); - glVertex3f(m_maxPt.x, m_maxPt.y, m_maxPt.z); - glVertex3f(m_minPt.x, m_maxPt.y, m_maxPt.z); -// glVertex3f(-1.0f, -1.0f, 0.0f); // The bottom left corner -// glVertex3f(-1.0f, 1.0f, 0.0f); // The top left corner -// glVertex3f(1.0f, 1.0f, 0.0f); // The top right corner -// glVertex3f(1.0f, -1.0f, 0.0f); // The bottom right corner - glEnd(); + qglviewer::WorldConstraint* YAxis = new qglviewer::WorldConstraint(); + YAxis->setTranslationConstraint(qglviewer::AxisPlaneConstraint::AXIS, qglviewer::Vec(0.0,1.0,0.0)); + YAxis->setRotationConstraint (qglviewer::AxisPlaneConstraint::FORBIDDEN, qglviewer::Vec(0.0,0.0,0.0)); - glBegin(GL_LINES); - glVertex3f(m_minPt.x, m_minPt.y, m_minPt.z); - glVertex3f(m_minPt.x, m_minPt.y, m_maxPt.z); + qglviewer::WorldConstraint* ZAxis = new qglviewer::WorldConstraint(); + ZAxis->setTranslationConstraint(qglviewer::AxisPlaneConstraint::AXIS, qglviewer::Vec(0.0,0.0,1.0)); + ZAxis->setRotationConstraint (qglviewer::AxisPlaneConstraint::FORBIDDEN, qglviewer::Vec(0.0,0.0,0.0)); - glVertex3f(m_maxPt.x, m_minPt.y, m_minPt.z); - glVertex3f(m_maxPt.x, m_minPt.y, m_maxPt.z); - glVertex3f(m_maxPt.x, m_maxPt.y, m_minPt.z); - glVertex3f(m_maxPt.x, m_maxPt.y, m_maxPt.z); + frame(0)->setConstraint(XAxis); + frame(1)->setConstraint(YAxis); + frame(2)->setConstraint(ZAxis); + frame(3)->setConstraint(XAxis); + frame(4)->setConstraint(YAxis); + frame(5)->setConstraint(ZAxis); - glVertex3f(m_minPt.x, m_maxPt.y, m_minPt.z); - glVertex3f(m_minPt.x, m_maxPt.y, m_maxPt.z); - glEnd(); - glDisable(GL_LINE_SMOOTH); - glEnable(GL_LIGHTING); +} - // correct all arrow frames: +SelectionBox::~SelectionBox(){ + delete m_frames[0]; + delete m_frames[1]; +} - for (unsigned i = 0; i < m_frames.size(); ++i){ - qglviewer::Vec pt = m_minPt; - float corr = m_arrowLength/2; - if (i/3 == 1){ - pt = m_maxPt; - corr *= -1; - } +void SelectionBox::draw(bool withNames){ - pt[i%3] += corr; - frame(i)->setTranslation(pt); + // set min/max new from grabbed frame: + for (unsigned i = 0; i < m_frames.size(); ++i){ + if (frame(i)->grabsMouse()){ + qglviewer::Vec f = frame(i)->position(); - } + unsigned oi = i+3; + float corr = m_arrowLength/2.0; + if (i >= 3){ + oi = i-3; + corr *= -1; + } - // draw spheres in their frames: -// GLUquadricObj* quadric=gluNewQuadric(); -// gluQuadricNormals(quadric, GLU_SMOOTH); -// glColor4f(1.0, 0.0, 0.0, 1.0); + qglviewer::Vec fo = frame(oi)->position(); - GLboolean lighting, colorMaterial; - glGetBooleanv(GL_LIGHTING, &lighting); - glGetBooleanv(GL_COLOR_MATERIAL, &colorMaterial); + unsigned ci = i%3; + m_minPt[ci] = std::min(f[ci] - corr, fo[ci] + corr); + m_maxPt[ci] = std::max(f[ci] - corr, fo[ci] + corr); + } + } - glDisable(GL_COLOR_MATERIAL); - for (unsigned i = 0; i < m_frames.size(); ++i){ - glPushMatrix(); - glMultMatrixd(m_frames[i]->matrix()); - if (withNames) - glPushName(i); + // draw box: + + glEnable(GL_LINE_SMOOTH); + glLineWidth(2.); + glDisable(GL_LIGHTING); + glColor3f(0.9,0.0, 0.0); + glBegin(GL_LINE_LOOP); // Start drawing a line primitive + glVertex3f(m_minPt.x, m_minPt.y, m_minPt.z); + glVertex3f(m_maxPt.x, m_minPt.y, m_minPt.z); + glVertex3f(m_maxPt.x, m_maxPt.y, m_minPt.z); + glVertex3f(m_minPt.x, m_maxPt.y, m_minPt.z); + glEnd(); + + glBegin(GL_LINE_LOOP); + glVertex3f(m_minPt.x, m_minPt.y, m_maxPt.z); + glVertex3f(m_maxPt.x, m_minPt.y, m_maxPt.z); + glVertex3f(m_maxPt.x, m_maxPt.y, m_maxPt.z); + glVertex3f(m_minPt.x, m_maxPt.y, m_maxPt.z); + // glVertex3f(-1.0f, -1.0f, 0.0f); // The bottom left corner + // glVertex3f(-1.0f, 1.0f, 0.0f); // The top left corner + // glVertex3f(1.0f, 1.0f, 0.0f); // The top right corner + // glVertex3f(1.0f, -1.0f, 0.0f); // The bottom right corner + glEnd(); + + glBegin(GL_LINES); + glVertex3f(m_minPt.x, m_minPt.y, m_minPt.z); + glVertex3f(m_minPt.x, m_minPt.y, m_maxPt.z); + + glVertex3f(m_maxPt.x, m_minPt.y, m_minPt.z); + glVertex3f(m_maxPt.x, m_minPt.y, m_maxPt.z); + + glVertex3f(m_maxPt.x, m_maxPt.y, m_minPt.z); + glVertex3f(m_maxPt.x, m_maxPt.y, m_maxPt.z); + + glVertex3f(m_minPt.x, m_maxPt.y, m_minPt.z); + glVertex3f(m_minPt.x, m_maxPt.y, m_maxPt.z); + glEnd(); + + glDisable(GL_LINE_SMOOTH); + glEnable(GL_LIGHTING); + + + // correct all arrow frames: + + for (unsigned i = 0; i < m_frames.size(); ++i){ + qglviewer::Vec pt = m_minPt; + float corr = m_arrowLength/2; + if (i/3 == 1){ + pt = m_maxPt; + corr *= -1; + } - float length = m_arrowLength; - if (frame(i)->grabsMouse()) - length *= 2; + pt[i%3] += corr; - const float radius = length/20; - - float color[4]; - if (i%3 == 0){ // x - color[0] = 1.0f; color[1] = 0.7f; color[2] = 0.7f; color[3] = 1.0f; - glPushMatrix(); - glRotatef(90.0, 0.0, 1.0, 0.0); - - } else if (i%3 == 1){ // y - color[0] = 0.7f; color[1] = 1.0f; color[2] = 0.7f; color[3] = 1.0f; - glPushMatrix(); - glRotatef(-90.0, 1.0, 0.0, 0.0); + frame(i)->setTranslation(pt); - } else { // z - glPushMatrix(); - color[0] = 0.7f; color[1] = 0.7f; color[2] = 1.0f; color[3] = 1.0f; - } - glTranslatef(0.0, 0.0, -length/2.0); - glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color); - QGLViewer::drawArrow(length, radius); - glPopMatrix(); + } + // draw spheres in their frames: + // GLUquadricObj* quadric=gluNewQuadric(); + // gluQuadricNormals(quadric, GLU_SMOOTH); + // glColor4f(1.0, 0.0, 0.0, 1.0); + + GLboolean lighting, colorMaterial; + glGetBooleanv(GL_LIGHTING, &lighting); + glGetBooleanv(GL_COLOR_MATERIAL, &colorMaterial); + + glDisable(GL_COLOR_MATERIAL); + for (unsigned i = 0; i < m_frames.size(); ++i){ + glPushMatrix(); + glMultMatrixd(m_frames[i]->matrix()); + if (withNames) + glPushName(i); + + float length = m_arrowLength; + if (frame(i)->grabsMouse()) + length *= 2; + + const float radius = length/20; + + float color[4]; + if (i%3 == 0){ // x + color[0] = 1.0f; color[1] = 0.7f; color[2] = 0.7f; color[3] = 1.0f; + glPushMatrix(); + glRotatef(90.0, 0.0, 1.0, 0.0); + + } else if (i%3 == 1){ // y + color[0] = 0.7f; color[1] = 1.0f; color[2] = 0.7f; color[3] = 1.0f; + glPushMatrix(); + glRotatef(-90.0, 1.0, 0.0, 0.0); + + } else { // z + glPushMatrix(); + color[0] = 0.7f; color[1] = 0.7f; color[2] = 1.0f; color[3] = 1.0f; + } + glTranslatef(0.0, 0.0, -length/2.0); + glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color); + QGLViewer::drawArrow(length, radius); + glPopMatrix(); - if (withNames) - glPopName(); - glPopMatrix(); - } - if (colorMaterial) - glEnable(GL_COLOR_MATERIAL); - if (!lighting) - glDisable(GL_LIGHTING); + if (withNames) + glPopName(); + glPopMatrix(); + } + if (colorMaterial) + glEnable(GL_COLOR_MATERIAL); + if (!lighting) + glDisable(GL_LIGHTING); - //gluDeleteQuadric(quadric); - } + //gluDeleteQuadric(quadric); - void SelectionBox::getBBXMin(float& x, float& y, float& z) const { - x = frame(0)->position().x; - y = frame(0)->position().y; - z = frame(0)->position().z; +} - for (unsigned i = 1; i < m_frames.size(); ++i){ - x = std::min(x,float(frame(i)->position().x)); - y = std::min(y,float(frame(i)->position().y)); - z = std::min(z,float(frame(i)->position().z)); - } - } +void SelectionBox::getBBXMin(float& x, float& y, float& z) const { + x = m_minPt.x; + y = m_minPt.y; + z = m_minPt.z; +} - void SelectionBox::getBBXMax(float& x, float& y, float& z) const { - x = frame(0)->position().x; - y = frame(0)->position().y; - z = frame(0)->position().z; +void SelectionBox::getBBXMax(float& x, float& y, float& z) const { + x = m_maxPt.x; + y = m_maxPt.y; + z = m_maxPt.z; +} - for (unsigned i = 1; i < m_frames.size(); ++i){ - x = std::max(x,float(frame(i)->position().x)); - y = std::max(y,float(frame(i)->position().y)); - z = std::max(z,float(frame(i)->position().z)); +int SelectionBox::getGrabbedFrame() const { + int frameid = -1; + for (unsigned i = 0; i < m_frames.size(); ++i){ + if (frame(i)->grabsMouse()){ + frameid = i; + break; } } - int SelectionBox::getGrabbedFrame() const { - int frameid = -1; - for (unsigned i = 0; i < m_frames.size(); ++i){ - if (frame(i)->grabsMouse()){ - frameid = i; - break; - } - } - - return frameid; - } + return frameid; +} - void SelectionBox::drawAxis(float length) const - { - const float radius = length/20; +void SelectionBox::drawAxis(float length) const +{ + const float radius = length/20; - GLboolean lighting, colorMaterial; - glGetBooleanv(GL_LIGHTING, &lighting); - glGetBooleanv(GL_COLOR_MATERIAL, &colorMaterial); + GLboolean lighting, colorMaterial; + glGetBooleanv(GL_LIGHTING, &lighting); + glGetBooleanv(GL_COLOR_MATERIAL, &colorMaterial); - glDisable(GL_COLOR_MATERIAL); + glDisable(GL_COLOR_MATERIAL); - float color[4]; - color[0] = 0.7f; color[1] = 0.7f; color[2] = 1.0f; color[3] = 1.0f; - glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color); - QGLViewer::drawArrow(length, radius); + float color[4]; + color[0] = 0.7f; color[1] = 0.7f; color[2] = 1.0f; color[3] = 1.0f; + glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color); + QGLViewer::drawArrow(length, radius); - color[0] = 1.0f; color[1] = 0.7f; color[2] = 0.7f; color[3] = 1.0f; - glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color); - glPushMatrix(); - glRotatef(90.0, 0.0, 1.0, 0.0); - QGLViewer::drawArrow(length, radius); - glPopMatrix(); + color[0] = 1.0f; color[1] = 0.7f; color[2] = 0.7f; color[3] = 1.0f; + glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color); + glPushMatrix(); + glRotatef(90.0, 0.0, 1.0, 0.0); + QGLViewer::drawArrow(length, radius); + glPopMatrix(); - color[0] = 0.7f; color[1] = 1.0f; color[2] = 0.7f; color[3] = 1.0f; - glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color); - glPushMatrix(); - glRotatef(-90.0, 1.0, 0.0, 0.0); - QGLViewer::drawArrow(length, radius); - glPopMatrix(); + color[0] = 0.7f; color[1] = 1.0f; color[2] = 0.7f; color[3] = 1.0f; + glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color); + glPushMatrix(); + glRotatef(-90.0, 1.0, 0.0, 0.0); + QGLViewer::drawArrow(length, radius); + glPopMatrix(); - if (colorMaterial) - glEnable(GL_COLOR_MATERIAL); - if (!lighting) - glDisable(GL_LIGHTING); - } + if (colorMaterial) + glEnable(GL_COLOR_MATERIAL); + if (!lighting) + glDisable(GL_LIGHTING); +} } diff --git a/octovis/src/ViewerGui.cpp b/octovis/src/ViewerGui.cpp index 8bb9ddf5..e4ffd0e2 100644 --- a/octovis/src/ViewerGui.cpp +++ b/octovis/src/ViewerGui.cpp @@ -37,443 +37,457 @@ namespace octomap{ - ViewerGui::ViewerGui(const std::string& filename, QWidget *parent) - : QMainWindow(parent), m_scanGraph(NULL), - m_trajectoryDrawer(NULL), m_pointcloudDrawer(NULL), - m_cameraFollowMode(NULL), - m_octreeResolution(0.1), m_laserMaxRange(-1.), m_occupancyThresh(0.5), - m_max_tree_depth(16), m_laserType(LASERTYPE_SICK), - m_cameraStored(false), m_filename("") { - - ui.setupUi(this); - m_glwidget = new ViewerWidget(this); - this->setCentralWidget(m_glwidget); - - // Settings panel at the right side - ViewerSettingsPanel* settingsPanel = new ViewerSettingsPanel(this); - QDockWidget* settingsDock = new QDockWidget("Octree / Scan graph settings", this); - settingsDock->setWidget(settingsPanel); - this->addDockWidget(Qt::RightDockWidgetArea, settingsDock); - ui.menuShow->addAction(settingsDock->toggleViewAction()); - - // Camera settings panel at the right side - ViewerSettingsPanelCamera* settingsCameraPanel = new ViewerSettingsPanelCamera(this); - QDockWidget *settingsCameraDock = new QDockWidget("Camera settings", this); - settingsCameraDock->setWidget(settingsCameraPanel); - this->addDockWidget(Qt::RightDockWidgetArea, settingsCameraDock); - this->tabifyDockWidget(settingsDock, settingsCameraDock); - settingsDock->raise(); - ui.menuShow->addAction(settingsCameraDock->toggleViewAction()); - - // status bar - m_mapSizeStatus = new QLabel("Map size", this); - m_mapMemoryStatus = new QLabel("Memory consumption", this); - m_mapSizeStatus->setFrameStyle(QFrame::Panel | QFrame::Sunken); - m_mapMemoryStatus->setFrameStyle(QFrame::Panel | QFrame::Sunken); - statusBar()->addPermanentWidget(m_mapSizeStatus); - statusBar()->addPermanentWidget(m_mapMemoryStatus); - - m_cameraFollowMode = new CameraFollowMode(); - - connect(this, SIGNAL(updateStatusBar(QString, int)), statusBar(), SLOT(showMessage(QString, int))); - - connect(settingsPanel, SIGNAL(treeDepthChanged(int)), this, SLOT(changeTreeDepth(int))); - connect(settingsPanel, SIGNAL(addNextScans(unsigned)), this, SLOT(addNextScans(unsigned))); - connect(settingsPanel, SIGNAL(gotoFirstScan()), this, SLOT(gotoFirstScan())); - - connect(settingsCameraPanel, SIGNAL(jumpToFrame(unsigned)), m_cameraFollowMode, SLOT(jumpToFrame(unsigned))); - connect(settingsCameraPanel, SIGNAL(play()), m_cameraFollowMode, SLOT(play())); - connect(settingsCameraPanel, SIGNAL(pause()), m_cameraFollowMode, SLOT(pause())); - connect(settingsCameraPanel, SIGNAL(clearCameraPath()), m_cameraFollowMode, SLOT(clearCameraPath())); - connect(settingsCameraPanel, SIGNAL(saveToCameraPath()), m_cameraFollowMode, SLOT(saveToCameraPath())); - connect(settingsCameraPanel, SIGNAL(removeFromCameraPath()), m_cameraFollowMode, SLOT(removeFromCameraPath())); - connect(settingsCameraPanel, SIGNAL(addToCameraPath()), m_cameraFollowMode, SLOT(addToCameraPath())); - connect(settingsCameraPanel, SIGNAL(followCameraPath()), m_cameraFollowMode, SLOT(followCameraPath())); - connect(settingsCameraPanel, SIGNAL(followRobotPath()), m_cameraFollowMode, SLOT(followRobotPath())); - - connect(m_cameraFollowMode, SIGNAL(changeNumberOfFrames(unsigned)), settingsCameraPanel, SLOT(setNumberOfFrames(unsigned))); - connect(m_cameraFollowMode, SIGNAL(frameChanged(unsigned)), settingsCameraPanel, SLOT(setCurrentFrame(unsigned))); - connect(m_cameraFollowMode, SIGNAL(stopped()), settingsCameraPanel, SLOT(setStopped())); - connect(m_cameraFollowMode, SIGNAL(scanGraphAvailable(bool)), settingsCameraPanel, SLOT(setRobotTrajectoryAvailable(bool))); - - connect(m_cameraFollowMode, SIGNAL(deleteCameraPath(int)), m_glwidget, SLOT(deleteCameraPath(int))); - connect(m_cameraFollowMode, SIGNAL(removeFromCameraPath(int,int)), m_glwidget, SLOT(removeFromCameraPath(int,int))); - connect(m_cameraFollowMode, SIGNAL(appendToCameraPath(int, const octomath::Pose6D&)), m_glwidget, SLOT(appendToCameraPath(int, const octomath::Pose6D&))); - connect(m_cameraFollowMode, SIGNAL(appendCurrentToCameraPath(int)), m_glwidget, SLOT(appendCurrentToCameraPath(int))); - connect(m_cameraFollowMode, SIGNAL(addCurrentToCameraPath(int,int)), m_glwidget, SLOT(addCurrentToCameraPath(int,int))); - connect(m_cameraFollowMode, SIGNAL(updateCameraPath(int,int)), m_glwidget, SLOT(updateCameraPath(int,int))); - connect(m_cameraFollowMode, SIGNAL(playCameraPath(int,int)), m_glwidget, SLOT(playCameraPath(int,int))); - connect(m_cameraFollowMode, SIGNAL(stopCameraPath(int)), m_glwidget, SLOT(stopCameraPath(int))); - connect(m_cameraFollowMode, SIGNAL(jumpToCamFrame(int, int)), m_glwidget, SLOT(jumpToCamFrame(int, int))); - connect(m_glwidget, SIGNAL(cameraPathStopped(int)), m_cameraFollowMode, SLOT(cameraPathStopped(int))); - connect(m_glwidget, SIGNAL(cameraPathFrameChanged(int, int)), m_cameraFollowMode, SLOT(cameraPathFrameChanged(int, int))); - - connect(this, SIGNAL(changeNumberOfScans(unsigned)), settingsPanel, SLOT(setNumberOfScans(unsigned))); - connect(this, SIGNAL(changeCurrentScan(unsigned)), settingsPanel, SLOT(setCurrentScan(unsigned))); - connect(this, SIGNAL(changeResolution(double)), settingsPanel, SLOT(setResolution(double))); - - connect(settingsCameraPanel, SIGNAL(changeCamPosition(double, double, double, double, double, double)), - m_glwidget, SLOT(setCamPosition(double, double, double, double, double, double))); - connect(m_cameraFollowMode, SIGNAL(changeCamPose(const octomath::Pose6D&)), - m_glwidget, SLOT(setCamPose(const octomath::Pose6D&))); - - connect(ui.actionReset_view, SIGNAL(triggered()), m_glwidget, SLOT(resetView())); - - if (filename != ""){ - m_filename = filename; - openFile(); - } +ViewerGui::ViewerGui(const std::string& filename, QWidget *parent) +: QMainWindow(parent), m_scanGraph(NULL), + m_trajectoryDrawer(NULL), m_pointcloudDrawer(NULL), + m_cameraFollowMode(NULL), + m_octreeResolution(0.1), m_laserMaxRange(-1.), m_occupancyThresh(0.5), + m_max_tree_depth(16), m_laserType(LASERTYPE_SICK), + m_cameraStored(false), m_filename("") { + + ui.setupUi(this); + m_glwidget = new ViewerWidget(this); + this->setCentralWidget(m_glwidget); + + // Settings panel at the right side + ViewerSettingsPanel* settingsPanel = new ViewerSettingsPanel(this); + QDockWidget* settingsDock = new QDockWidget("Octree / Scan graph settings", this); + settingsDock->setWidget(settingsPanel); + this->addDockWidget(Qt::RightDockWidgetArea, settingsDock); + ui.menuShow->addAction(settingsDock->toggleViewAction()); + + // Camera settings panel at the right side + ViewerSettingsPanelCamera* settingsCameraPanel = new ViewerSettingsPanelCamera(this); + QDockWidget *settingsCameraDock = new QDockWidget("Camera settings", this); + settingsCameraDock->setWidget(settingsCameraPanel); + this->addDockWidget(Qt::RightDockWidgetArea, settingsCameraDock); + this->tabifyDockWidget(settingsDock, settingsCameraDock); + settingsDock->raise(); + ui.menuShow->addAction(settingsCameraDock->toggleViewAction()); + + // status bar + m_mapSizeStatus = new QLabel("Map size", this); + m_mapMemoryStatus = new QLabel("Memory consumption", this); + m_mapSizeStatus->setFrameStyle(QFrame::Panel | QFrame::Sunken); + m_mapMemoryStatus->setFrameStyle(QFrame::Panel | QFrame::Sunken); + statusBar()->addPermanentWidget(m_mapSizeStatus); + statusBar()->addPermanentWidget(m_mapMemoryStatus); + + m_cameraFollowMode = new CameraFollowMode(); + + connect(this, SIGNAL(updateStatusBar(QString, int)), statusBar(), SLOT(showMessage(QString, int))); + + connect(settingsPanel, SIGNAL(treeDepthChanged(int)), this, SLOT(changeTreeDepth(int))); + connect(settingsPanel, SIGNAL(addNextScans(unsigned)), this, SLOT(addNextScans(unsigned))); + connect(settingsPanel, SIGNAL(gotoFirstScan()), this, SLOT(gotoFirstScan())); + + connect(settingsCameraPanel, SIGNAL(jumpToFrame(unsigned)), m_cameraFollowMode, SLOT(jumpToFrame(unsigned))); + connect(settingsCameraPanel, SIGNAL(play()), m_cameraFollowMode, SLOT(play())); + connect(settingsCameraPanel, SIGNAL(pause()), m_cameraFollowMode, SLOT(pause())); + connect(settingsCameraPanel, SIGNAL(clearCameraPath()), m_cameraFollowMode, SLOT(clearCameraPath())); + connect(settingsCameraPanel, SIGNAL(saveToCameraPath()), m_cameraFollowMode, SLOT(saveToCameraPath())); + connect(settingsCameraPanel, SIGNAL(removeFromCameraPath()), m_cameraFollowMode, SLOT(removeFromCameraPath())); + connect(settingsCameraPanel, SIGNAL(addToCameraPath()), m_cameraFollowMode, SLOT(addToCameraPath())); + connect(settingsCameraPanel, SIGNAL(followCameraPath()), m_cameraFollowMode, SLOT(followCameraPath())); + connect(settingsCameraPanel, SIGNAL(followRobotPath()), m_cameraFollowMode, SLOT(followRobotPath())); + + connect(m_cameraFollowMode, SIGNAL(changeNumberOfFrames(unsigned)), settingsCameraPanel, SLOT(setNumberOfFrames(unsigned))); + connect(m_cameraFollowMode, SIGNAL(frameChanged(unsigned)), settingsCameraPanel, SLOT(setCurrentFrame(unsigned))); + connect(m_cameraFollowMode, SIGNAL(stopped()), settingsCameraPanel, SLOT(setStopped())); + connect(m_cameraFollowMode, SIGNAL(scanGraphAvailable(bool)), settingsCameraPanel, SLOT(setRobotTrajectoryAvailable(bool))); + + connect(m_cameraFollowMode, SIGNAL(deleteCameraPath(int)), m_glwidget, SLOT(deleteCameraPath(int))); + connect(m_cameraFollowMode, SIGNAL(removeFromCameraPath(int,int)), m_glwidget, SLOT(removeFromCameraPath(int,int))); + connect(m_cameraFollowMode, SIGNAL(appendToCameraPath(int, const octomath::Pose6D&)), m_glwidget, SLOT(appendToCameraPath(int, const octomath::Pose6D&))); + connect(m_cameraFollowMode, SIGNAL(appendCurrentToCameraPath(int)), m_glwidget, SLOT(appendCurrentToCameraPath(int))); + connect(m_cameraFollowMode, SIGNAL(addCurrentToCameraPath(int,int)), m_glwidget, SLOT(addCurrentToCameraPath(int,int))); + connect(m_cameraFollowMode, SIGNAL(updateCameraPath(int,int)), m_glwidget, SLOT(updateCameraPath(int,int))); + connect(m_cameraFollowMode, SIGNAL(playCameraPath(int,int)), m_glwidget, SLOT(playCameraPath(int,int))); + connect(m_cameraFollowMode, SIGNAL(stopCameraPath(int)), m_glwidget, SLOT(stopCameraPath(int))); + connect(m_cameraFollowMode, SIGNAL(jumpToCamFrame(int, int)), m_glwidget, SLOT(jumpToCamFrame(int, int))); + connect(m_glwidget, SIGNAL(cameraPathStopped(int)), m_cameraFollowMode, SLOT(cameraPathStopped(int))); + connect(m_glwidget, SIGNAL(cameraPathFrameChanged(int, int)), m_cameraFollowMode, SLOT(cameraPathFrameChanged(int, int))); + + connect(this, SIGNAL(changeNumberOfScans(unsigned)), settingsPanel, SLOT(setNumberOfScans(unsigned))); + connect(this, SIGNAL(changeCurrentScan(unsigned)), settingsPanel, SLOT(setCurrentScan(unsigned))); + connect(this, SIGNAL(changeResolution(double)), settingsPanel, SLOT(setResolution(double))); + + connect(settingsCameraPanel, SIGNAL(changeCamPosition(double, double, double, double, double, double)), + m_glwidget, SLOT(setCamPosition(double, double, double, double, double, double))); + connect(m_cameraFollowMode, SIGNAL(changeCamPose(const octomath::Pose6D&)), + m_glwidget, SLOT(setCamPose(const octomath::Pose6D&))); + + connect(ui.actionReset_view, SIGNAL(triggered()), m_glwidget, SLOT(resetView())); + + if (filename != ""){ + m_filename = filename; + openFile(); } +} - ViewerGui::~ViewerGui() { - if (m_trajectoryDrawer){ - m_glwidget->removeSceneObject(m_trajectoryDrawer); - delete m_trajectoryDrawer; - m_trajectoryDrawer = NULL; - } - - if (m_pointcloudDrawer){ - m_glwidget->removeSceneObject(m_pointcloudDrawer); - delete m_pointcloudDrawer; - m_pointcloudDrawer = NULL; - } - - for (std::map::iterator it = m_octrees.begin(); it != m_octrees.end(); ++it) { - m_glwidget->removeSceneObject(it->second.octree_drawer); - delete (it->second.octree_drawer); - delete (it->second.octree); - } - m_octrees.clear(); - - if(m_cameraFollowMode) { - delete m_cameraFollowMode; - m_cameraFollowMode = NULL; - } +ViewerGui::~ViewerGui() { + if (m_trajectoryDrawer){ + m_glwidget->removeSceneObject(m_trajectoryDrawer); + delete m_trajectoryDrawer; + m_trajectoryDrawer = NULL; } - bool ViewerGui::isShown() { - return m_glwidget->isVisible(); + if (m_pointcloudDrawer){ + m_glwidget->removeSceneObject(m_pointcloudDrawer); + delete m_pointcloudDrawer; + m_pointcloudDrawer = NULL; } - void ViewerGui::showInfo(QString string, bool newline) { - std::cerr << string.toLocal8Bit().data(); - if (newline) std::cerr << std::endl; - else std::cerr << std::flush; - int duration = 0; - if (newline) - duration = 3000; - emit updateStatusBar(string, duration); + for (std::map::iterator it = m_octrees.begin(); it != m_octrees.end(); ++it) { + m_glwidget->removeSceneObject(it->second.octree_drawer); + delete (it->second.octree_drawer); + delete (it->second.octree); } + m_octrees.clear(); - bool ViewerGui::getOctreeRecord(int id, OcTreeRecord*& otr) { - std::map::iterator it = m_octrees.find(id); - if( it != m_octrees.end() ) { - otr = &(it->second); - return true; - } - else { - return false; - } + if(m_cameraFollowMode) { + delete m_cameraFollowMode; + m_cameraFollowMode = NULL; } +} - void ViewerGui::addOctree(octomap::AbstractOcTree* tree, int id, octomap::pose6d origin) { - // is id in use? - OcTreeRecord* r; - bool foundRecord = getOctreeRecord(id, r); - if (foundRecord && r->octree->getTreeType().compare(tree->getTreeType()) !=0){ - // delete old drawer, create new - delete r->octree_drawer; - if (dynamic_cast(tree)) { - r->octree_drawer = new OcTreeDrawer(); - // fprintf(stderr, "adding new OcTreeDrawer for node %d\n", id); - } - else if (dynamic_cast(tree)) { - r->octree_drawer = new ColorOcTreeDrawer(); - } else{ - OCTOMAP_ERROR("Could not create drawer for tree type %s\n", tree->getTreeType().c_str()); - } - - delete r->octree; - r->octree = tree; - r->origin = origin; +bool ViewerGui::isShown() { + return m_glwidget->isVisible(); +} - } else if (foundRecord && r->octree->getTreeType().compare(tree->getTreeType()) ==0) { - // only swap out tree +void ViewerGui::showInfo(QString string, bool newline) { + std::cerr << string.toLocal8Bit().data(); + if (newline) std::cerr << std::endl; + else std::cerr << std::flush; + int duration = 0; + if (newline) + duration = 3000; + emit updateStatusBar(string, duration); +} - delete r->octree; - r->octree = tree; - r->origin = origin; - } else { - // add new record - OcTreeRecord otr; - otr.id = id; - if (dynamic_cast(tree)) { - otr.octree_drawer = new OcTreeDrawer(); - // fprintf(stderr, "adding new OcTreeDrawer for node %d\n", id); - } - else if (dynamic_cast(tree)) { - otr.octree_drawer = new ColorOcTreeDrawer(); - } else{ - OCTOMAP_ERROR("Could not create drawer for tree type %s\n", tree->getTreeType().c_str()); - } - otr.octree = tree; - otr.origin = origin; - m_octrees[id] = otr; - m_glwidget->addSceneObject(otr.octree_drawer); - } +bool ViewerGui::getOctreeRecord(int id, OcTreeRecord*& otr) { + std::map::iterator it = m_octrees.find(id); + if( it != m_octrees.end() ) { + otr = &(it->second); + return true; } - - void ViewerGui::addOctree(octomap::AbstractOcTree* tree, int id) { - octomap::pose6d o; // initialized to (0,0,0) , (0,0,0,1) by default - addOctree(tree, id, o); + else { + return false; } +} - void ViewerGui::showOcTree() { - - // update viewer stat - double minX, minY, minZ, maxX, maxY, maxZ; - minX = minY = minZ = -10; // min bbx for drawing - maxX = maxY = maxZ = 10; // max bbx for drawing - double sizeX, sizeY, sizeZ; - sizeX = sizeY = sizeZ = 0.; - size_t memoryUsage = 0; - size_t num_nodes = 0; - size_t memorySingleNode = 0; - - - for (std::map::iterator it = m_octrees.begin(); it != m_octrees.end(); ++it) { - // get map bbx - double lminX, lminY, lminZ, lmaxX, lmaxY, lmaxZ; - it->second.octree->getMetricMin(lminX, lminY, lminZ); - it->second.octree->getMetricMax(lmaxX, lmaxY, lmaxZ); - // transform to world coords using map origin - octomap::point3d pmin(lminX, lminY, lminZ); - octomap::point3d pmax(lmaxX, lmaxY, lmaxZ); - pmin = it->second.origin.transform(pmin); - pmax = it->second.origin.transform(pmax); - lminX = pmin.x(); lminY = pmin.y(); lminZ = pmin.z(); - lmaxX = pmax.x(); lmaxY = pmax.y(); lmaxZ = pmax.z(); - // update global bbx - if (lminX < minX) minX = lminX; - if (lminY < minY) minY = lminY; - if (lminZ < minZ) minZ = lminZ; - if (lmaxX > maxX) maxX = lmaxX; - if (lmaxY > maxY) maxY = lmaxY; - if (lmaxZ > maxZ) maxZ = lmaxZ; - double lsizeX, lsizeY, lsizeZ; - // update map stats - it->second.octree->getMetricSize(lsizeX, lsizeY, lsizeZ); - if (lsizeX > sizeX) sizeX = lsizeX; - if (lsizeY > sizeY) sizeY = lsizeY; - if (lsizeZ > sizeZ) sizeZ = lsizeZ; - memoryUsage += it->second.octree->memoryUsage(); - num_nodes += it->second.octree->size(); - memorySingleNode = std::max(memorySingleNode, it->second.octree->memoryUsageNode()); - } - - m_glwidget->setSceneBoundingBox(qglviewer::Vec(minX, minY, minZ), qglviewer::Vec(maxX, maxY, maxZ)); +void ViewerGui::addOctree(octomap::AbstractOcTree* tree, int id, octomap::pose6d origin) { + // is id in use? + OcTreeRecord* r; + bool foundRecord = getOctreeRecord(id, r); + if (foundRecord && r->octree->getTreeType().compare(tree->getTreeType()) !=0){ + // delete old drawer, create new + delete r->octree_drawer; + if (dynamic_cast(tree)) { + r->octree_drawer = new OcTreeDrawer(); + // fprintf(stderr, "adding new OcTreeDrawer for node %d\n", id); + } + else if (dynamic_cast(tree)) { + r->octree_drawer = new ColorOcTreeDrawer(); + } else{ + OCTOMAP_ERROR("Could not create drawer for tree type %s\n", tree->getTreeType().c_str()); + } - //if (m_octrees.size()) { - QString size = QString("%L1 x %L2 x %L3 m^3; %L4 nodes").arg(sizeX).arg(sizeY).arg(sizeZ).arg(unsigned(num_nodes)); - QString memory = QString("Single node: %L1 B; ").arg(memorySingleNode) - + QString ("Octree: %L1 B (%L2 MB)").arg(memoryUsage).arg((double) memoryUsage/(1024.*1024.), 0, 'f', 3); - m_mapMemoryStatus->setText(memory); - m_mapSizeStatus->setText(size); - //} + delete r->octree; + r->octree = tree; + r->origin = origin; - m_glwidget->updateGL(); + } else if (foundRecord && r->octree->getTreeType().compare(tree->getTreeType()) ==0) { + // only swap out tree - // generate cubes -> display - // timeval start; - // timeval stop; - // gettimeofday(&start, NULL); // start timer - for (std::map::iterator it = m_octrees.begin(); it != m_octrees.end(); ++it) { - it->second.octree_drawer->setMax_tree_depth(m_max_tree_depth); - it->second.octree_drawer->setOcTree(*it->second.octree, it->second.origin, it->second.id); - } -// gettimeofday(&stop, NULL); // stop timer -// double time_to_generate = (stop.tv_sec - start.tv_sec) + 1.0e-6 *(stop.tv_usec - start.tv_usec); -// fprintf(stderr, "setOcTree took %f sec\n", time_to_generate); - m_glwidget->updateGL(); - } + delete r->octree; + r->octree = tree; + r->origin = origin; + } else { + // add new record + OcTreeRecord otr; + otr.id = id; + if (dynamic_cast(tree)) { + otr.octree_drawer = new OcTreeDrawer(); + // fprintf(stderr, "adding new OcTreeDrawer for node %d\n", id); + } + else if (dynamic_cast(tree)) { + otr.octree_drawer = new ColorOcTreeDrawer(); + } else{ + OCTOMAP_ERROR("Could not create drawer for tree type %s\n", tree->getTreeType().c_str()); + } + otr.octree = tree; + otr.origin = origin; + m_octrees[id] = otr; + m_glwidget->addSceneObject(otr.octree_drawer); + } +} +void ViewerGui::addOctree(octomap::AbstractOcTree* tree, int id) { + octomap::pose6d o; // initialized to (0,0,0) , (0,0,0,1) by default + addOctree(tree, id, o); +} - void ViewerGui::generateOctree() { +void ViewerGui::showOcTree() { + + // update viewer stat + double minX, minY, minZ, maxX, maxY, maxZ; + minX = minY = minZ = -10; // min bbx for drawing + maxX = maxY = maxZ = 10; // max bbx for drawing + double sizeX, sizeY, sizeZ; + sizeX = sizeY = sizeZ = 0.; + size_t memoryUsage = 0; + size_t num_nodes = 0; + size_t memorySingleNode = 0; + + + for (std::map::iterator it = m_octrees.begin(); it != m_octrees.end(); ++it) { + // get map bbx + double lminX, lminY, lminZ, lmaxX, lmaxY, lmaxZ; + it->second.octree->getMetricMin(lminX, lminY, lminZ); + it->second.octree->getMetricMax(lmaxX, lmaxY, lmaxZ); + // transform to world coords using map origin + octomap::point3d pmin(lminX, lminY, lminZ); + octomap::point3d pmax(lmaxX, lmaxY, lmaxZ); + pmin = it->second.origin.transform(pmin); + pmax = it->second.origin.transform(pmax); + lminX = pmin.x(); lminY = pmin.y(); lminZ = pmin.z(); + lmaxX = pmax.x(); lmaxY = pmax.y(); lmaxZ = pmax.z(); + // update global bbx + if (lminX < minX) minX = lminX; + if (lminY < minY) minY = lminY; + if (lminZ < minZ) minZ = lminZ; + if (lmaxX > maxX) maxX = lmaxX; + if (lmaxY > maxY) maxY = lmaxY; + if (lmaxZ > maxZ) maxZ = lmaxZ; + double lsizeX, lsizeY, lsizeZ; + // update map stats + it->second.octree->getMetricSize(lsizeX, lsizeY, lsizeZ); + if (lsizeX > sizeX) sizeX = lsizeX; + if (lsizeY > sizeY) sizeY = lsizeY; + if (lsizeZ > sizeZ) sizeZ = lsizeZ; + memoryUsage += it->second.octree->memoryUsage(); + num_nodes += it->second.octree->size(); + memorySingleNode = std::max(memorySingleNode, it->second.octree->memoryUsageNode()); + } + + m_glwidget->setSceneBoundingBox(qglviewer::Vec(minX, minY, minZ), qglviewer::Vec(maxX, maxY, maxZ)); + + //if (m_octrees.size()) { + QString size = QString("%L1 x %L2 x %L3 m^3; %L4 nodes").arg(sizeX).arg(sizeY).arg(sizeZ).arg(unsigned(num_nodes)); + QString memory = QString("Single node: %L1 B; ").arg(memorySingleNode) + + QString ("Octree: %L1 B (%L2 MB)").arg(memoryUsage).arg((double) memoryUsage/(1024.*1024.), 0, 'f', 3); + m_mapMemoryStatus->setText(memory); + m_mapSizeStatus->setText(size); + //} + + m_glwidget->updateGL(); + + // generate cubes -> display + // timeval start; + // timeval stop; + // gettimeofday(&start, NULL); // start timer + for (std::map::iterator it = m_octrees.begin(); it != m_octrees.end(); ++it) { + it->second.octree_drawer->setMax_tree_depth(m_max_tree_depth); + it->second.octree_drawer->setOcTree(*it->second.octree, it->second.origin, it->second.id); + } + // gettimeofday(&stop, NULL); // stop timer + // double time_to_generate = (stop.tv_sec - start.tv_sec) + 1.0e-6 *(stop.tv_usec - start.tv_usec); + // fprintf(stderr, "setOcTree took %f sec\n", time_to_generate); + m_glwidget->updateGL(); +} - if (m_scanGraph) { - QApplication::setOverrideCursor(Qt::WaitCursor); +void ViewerGui::generateOctree() { - showInfo("Generating OcTree... "); - std::cerr << std::endl; + if (m_scanGraph) { - //if (m_ocTree) delete m_ocTree; - OcTree* tree = new octomap::OcTree(m_octreeResolution); + QApplication::setOverrideCursor(Qt::WaitCursor); - octomap::ScanGraph::iterator it; - unsigned numScans = m_scanGraph->size(); - unsigned currentScan = 1; - for (it = m_scanGraph->begin(); it != m_nextScanToAdd; it++) { - tree->insertPointCloud(**it, m_laserMaxRange); - fprintf(stderr, "generateOctree:: inserting scan node with %d points, origin: %.2f ,%.2f , %.2f.\n", - (unsigned int) (*it)->scan->size(), (*it)->pose.x(), (*it)->pose.y(), (*it)->pose.z() ); + showInfo("Generating OcTree... "); + std::cerr << std::endl; - std::cout << " S ("<addOctree(tree, DEFAULT_OCTREE_ID); - this->showOcTree(); + octomap::ScanGraph::iterator it; + unsigned numScans = m_scanGraph->size(); + unsigned currentScan = 1; + for (it = m_scanGraph->begin(); it != m_nextScanToAdd; it++) { + tree->insertPointCloud(**it, m_laserMaxRange); + fprintf(stderr, "generateOctree:: inserting scan node with %d points, origin: %.2f ,%.2f , %.2f.\n", + (unsigned int) (*it)->scan->size(), (*it)->pose.x(), (*it)->pose.y(), (*it)->pose.z() ); - showInfo("Done.", true); - QApplication::restoreOverrideCursor(); - } - else { - std::cerr << "generateOctree called but no ScanGraph present!\n"; + std::cout << " S ("<addOctree(tree, DEFAULT_OCTREE_ID); + this->showOcTree(); + + showInfo("Done.", true); + QApplication::restoreOverrideCursor(); + } + else { + std::cerr << "generateOctree called but no ScanGraph present!\n"; } - // == incremental graph generation ======================= +} - void ViewerGui::gotoFirstScan(){ - if (m_scanGraph){ - showInfo("Inserting first scan node into tree... ", true); - QApplication::setOverrideCursor(Qt::WaitCursor); +// == incremental graph generation ======================= - m_nextScanToAdd = m_scanGraph->begin(); - - // if (m_ocTree) delete m_ocTree; - // m_ocTree = new octomap::OcTree(m_octreeResolution); - OcTree* tree = new octomap::OcTree(m_octreeResolution); - this->addOctree(tree, DEFAULT_OCTREE_ID); +void ViewerGui::gotoFirstScan(){ + if (m_scanGraph){ + showInfo("Inserting first scan node into tree... ", true); + QApplication::setOverrideCursor(Qt::WaitCursor); - addNextScan(); + m_nextScanToAdd = m_scanGraph->begin(); - QApplication::restoreOverrideCursor(); - showOcTree(); - } + // if (m_ocTree) delete m_ocTree; + // m_ocTree = new octomap::OcTree(m_octreeResolution); + OcTree* tree = new octomap::OcTree(m_octreeResolution); + this->addOctree(tree, DEFAULT_OCTREE_ID); + + addNextScan(); + + QApplication::restoreOverrideCursor(); + showOcTree(); } +} - void ViewerGui::addNextScan(){ +void ViewerGui::addNextScan(){ - if (m_scanGraph){ - showInfo("Inserting next scan node into tree... ", true); + if (m_scanGraph){ + showInfo("Inserting next scan node into tree... ", true); - QApplication::setOverrideCursor(Qt::WaitCursor); - if (m_nextScanToAdd != m_scanGraph->end()){ - OcTreeRecord* r; - if (!getOctreeRecord(DEFAULT_OCTREE_ID, r)) { - fprintf(stderr, "ERROR: OctreeRecord for id %d not found!\n", DEFAULT_OCTREE_ID); - return; - } - // not used with ColorOcTrees, omitting casts - ((OcTree*) r->octree)->insertPointCloud(**m_nextScanToAdd, m_laserMaxRange); - m_nextScanToAdd++; + QApplication::setOverrideCursor(Qt::WaitCursor); + if (m_nextScanToAdd != m_scanGraph->end()){ + OcTreeRecord* r; + if (!getOctreeRecord(DEFAULT_OCTREE_ID, r)) { + fprintf(stderr, "ERROR: OctreeRecord for id %d not found!\n", DEFAULT_OCTREE_ID); + return; } + // not used with ColorOcTrees, omitting casts + ((OcTree*) r->octree)->insertPointCloud(**m_nextScanToAdd, m_laserMaxRange); + m_nextScanToAdd++; + } - QApplication::restoreOverrideCursor(); - showOcTree(); + QApplication::restoreOverrideCursor(); + showOcTree(); - } } +} - void ViewerGui::addNextScans(unsigned scans){ - for (unsigned i = 0; i < scans; ++i){ - addNextScan(); - } +void ViewerGui::addNextScans(unsigned scans){ + for (unsigned i = 0; i < scans; ++i){ + addNextScan(); } +} - // == file I/O =========================================== +// == file I/O =========================================== - void ViewerGui::openFile(){ - if (!m_filename.empty()){ - m_glwidget->clearAll(); +void ViewerGui::openFile(){ + if (!m_filename.empty()){ + m_glwidget->clearAll(); - QString temp = QString(m_filename.c_str()); - QFileInfo fileinfo(temp); - if (fileinfo.suffix() == "graph"){ - openGraph(); - }else if (fileinfo.suffix() == "bt"){ - openTree(); - } - else if (fileinfo.suffix() == "ot") - { - openOcTree(); - } - else if (fileinfo.suffix() == "hot"){ - openMapCollection(); - } - else if (fileinfo.suffix() == "dat"){ - openPointcloud(); - } - else { - QMessageBox::warning(this, "Unknown file", "Cannot open file, unknown extension: "+fileinfo.suffix(), QMessageBox::Ok); - } + QString temp = QString(m_filename.c_str()); + QFileInfo fileinfo(temp); + if (fileinfo.suffix() == "graph"){ + openGraph(); + }else if (fileinfo.suffix() == "bt"){ + openTree(); + } + else if (fileinfo.suffix() == "ot") + { + openOcTree(); + } + else if (fileinfo.suffix() == "hot"){ + openMapCollection(); + } + else if (fileinfo.suffix() == "dat"){ + openPointcloud(); + } + else { + QMessageBox::warning(this, "Unknown file", "Cannot open file, unknown extension: "+fileinfo.suffix(), QMessageBox::Ok); } } +} - void ViewerGui::openGraph(bool completeGraph){ +void ViewerGui::openGraph(bool completeGraph){ - QApplication::setOverrideCursor(Qt::WaitCursor); - showInfo("Loading scan graph from file " + QString(m_filename.c_str()) ); + QApplication::setOverrideCursor(Qt::WaitCursor); + showInfo("Loading scan graph from file " + QString(m_filename.c_str()) ); - if (m_scanGraph) delete m_scanGraph; - m_scanGraph = new octomap::ScanGraph(); - m_scanGraph->readBinary(m_filename); + if (m_scanGraph) delete m_scanGraph; + m_scanGraph = new octomap::ScanGraph(); + m_scanGraph->readBinary(m_filename); - loadGraph(completeGraph); - } + loadGraph(completeGraph); +} - void ViewerGui::openPointcloud(){ - - QApplication::setOverrideCursor(Qt::WaitCursor); - showInfo("Loading ASCII pointcloud from file "+QString(m_filename.c_str()) + "..."); +void ViewerGui::openPointcloud(){ - if (m_scanGraph) delete m_scanGraph; - m_scanGraph = new octomap::ScanGraph(); + QApplication::setOverrideCursor(Qt::WaitCursor); + showInfo("Loading ASCII pointcloud from file "+QString(m_filename.c_str()) + "..."); + if (m_scanGraph) delete m_scanGraph; + m_scanGraph = new octomap::ScanGraph(); - // read pointcloud from file - std::ifstream s(m_filename.c_str()); - Pointcloud* pc = new Pointcloud(); - if (!s) { - std::cout <<"ERROR: could not read " << m_filename << std::endl; - return; - } + // read pointcloud from file + std::ifstream s(m_filename.c_str()); + Pointcloud* pc = new Pointcloud(); - pc->read(s); + if (!s) { + std::cout <<"ERROR: could not read " << m_filename << std::endl; + return; + } - pose6d laser_pose(0,0,0,0,0,0); - m_scanGraph->addNode(pc, laser_pose); + pc->read(s); - loadGraph(true); - showInfo("Done.", true); - } + pose6d laser_pose(0,0,0,0,0,0); + m_scanGraph->addNode(pc, laser_pose); + loadGraph(true); + showInfo("Done.", true); +} - void ViewerGui::setOcTreeUISwitches() { - ui.actionPointcloud->setChecked(false); - ui.actionPointcloud->setEnabled(false); - ui.actionOctree_cells->setChecked(true); - ui.actionOctree_cells->setEnabled(true); - ui.actionFree->setChecked(false); - ui.actionFree->setEnabled(true); - ui.actionOctree_structure->setEnabled(true); - ui.actionOctree_structure->setChecked(false); - ui.actionTrajectory->setEnabled(false); - ui.actionConvert_ml_tree->setEnabled(true); - ui.actionReload_Octree->setEnabled(true); - ui.actionSettings->setEnabled(false); - } - void ViewerGui::openTree(){ - OcTree* tree = new octomap::OcTree(m_filename); +void ViewerGui::setOcTreeUISwitches() { + ui.actionPointcloud->setChecked(false); + ui.actionPointcloud->setEnabled(false); + ui.actionOctree_cells->setChecked(true); + ui.actionOctree_cells->setEnabled(true); + ui.actionFree->setChecked(false); + ui.actionFree->setEnabled(true); + ui.actionOctree_structure->setEnabled(true); + ui.actionOctree_structure->setChecked(false); + ui.actionTrajectory->setEnabled(false); + ui.actionConvert_ml_tree->setEnabled(true); + ui.actionReload_Octree->setEnabled(true); + ui.actionSettings->setEnabled(false); +} + +void ViewerGui::openTree(){ + OcTree* tree = new octomap::OcTree(m_filename); + this->addOctree(tree, DEFAULT_OCTREE_ID); + + m_octreeResolution = tree->getResolution(); + emit changeResolution(m_octreeResolution); + + setOcTreeUISwitches(); + showOcTree(); + m_glwidget->resetView(); +} + +void ViewerGui::openOcTree(){ + AbstractOcTree* tree = AbstractOcTree::read(m_filename); + + if (tree){ this->addOctree(tree, DEFAULT_OCTREE_ID); m_octreeResolution = tree->getResolution(); @@ -482,736 +496,722 @@ namespace octomap{ setOcTreeUISwitches(); showOcTree(); m_glwidget->resetView(); + + if (tree->getTreeType() == "ColorOcTree"){ + // map color and height map share the same color array and QAction + ui.actionHeight_map->setText ("Map color"); // rename QAction in Menu + this->on_actionHeight_map_toggled(true); // enable color view + ui.actionHeight_map->setChecked(true); + } + } + else { + QMessageBox::warning(this, "File error", "Cannot open OcTree file", QMessageBox::Ok); } +} - void ViewerGui::openOcTree(){ - AbstractOcTree* tree = AbstractOcTree::read(m_filename); - if (tree){ - this->addOctree(tree, DEFAULT_OCTREE_ID); +// EXPERIMENTAL +void ViewerGui::openMapCollection() { - m_octreeResolution = tree->getResolution(); - emit changeResolution(m_octreeResolution); + OCTOMAP_DEBUG("Opening hierarchy from %s...\n", m_filename.c_str()); - setOcTreeUISwitches(); - showOcTree(); - m_glwidget->resetView(); + std::ifstream infile(m_filename.c_str(), std::ios_base::in |std::ios_base::binary); + if (!infile.is_open()) { + QMessageBox::warning(this, "File error", "Cannot open OcTree file", QMessageBox::Ok); + return; + } + infile.close(); - if (tree->getTreeType() == "ColorOcTree"){ - // map color and height map share the same color array and QAction - ui.actionHeight_map->setText ("Map color"); // rename QAction in Menu - this->on_actionHeight_map_toggled(true); // enable color view - ui.actionHeight_map->setChecked(true); - } - } + MapCollection > collection(m_filename); + int i=0; + for (MapCollection >::iterator it = collection.begin(); + it != collection.end(); ++it) { + OCTOMAP_DEBUG("Adding hierarchy node %s\n", (*it)->getId().c_str()); + OcTree* tree = (*it)->getMap(); + if (!tree) + OCTOMAP_ERROR("Error while reading node %s\n", (*it)->getId().c_str()); else { - QMessageBox::warning(this, "File error", "Cannot open OcTree file", QMessageBox::Ok); + OCTOMAP_DEBUG("Read tree with %zu tree nodes\n", tree->size()); } - } - + pose6d origin = (*it)->getOrigin(); + this->addOctree(tree, i, origin); + ++i; + } + setOcTreeUISwitches(); + showOcTree(); + m_glwidget->resetView(); + OCTOMAP_DEBUG("done\n"); +} - // EXPERIMENTAL - void ViewerGui::openMapCollection() { +void ViewerGui::loadGraph(bool completeGraph) { + + ui.actionSettings->setEnabled(true); + ui.actionPointcloud->setEnabled(true); + ui.actionPointcloud->setChecked(false); + ui.actionTrajectory->setEnabled(true); + ui.actionOctree_cells->setEnabled(true); + ui.actionOctree_cells->setChecked(true); + ui.actionOctree_structure->setEnabled(true); + ui.actionOctree_structure->setChecked(false); + ui.actionFree->setChecked(false); + ui.actionFree->setEnabled(true); + ui.actionReload_Octree->setEnabled(true); + ui.actionConvert_ml_tree->setEnabled(true); + + unsigned graphSize = m_scanGraph->size(); + unsigned currentScan; + + if (completeGraph){ + fprintf(stderr, "loadGraph:: generating octree from complete graph.\n" ); + m_nextScanToAdd = m_scanGraph->end(); + generateOctree(); + currentScan = graphSize; + } + else{ + m_nextScanToAdd = m_scanGraph->begin(); + + //if (m_ocTree) delete m_ocTree; + //m_ocTree = new octomap::OcTree(m_octreeResolution); + OcTree* tree = new octomap::OcTree(m_octreeResolution); + this->addOctree(tree, DEFAULT_OCTREE_ID); - OCTOMAP_DEBUG("Opening hierarchy from %s...\n", m_filename.c_str()); + addNextScan(); - std::ifstream infile(m_filename.c_str(), std::ios_base::in |std::ios_base::binary); - if (!infile.is_open()) { - QMessageBox::warning(this, "File error", "Cannot open OcTree file", QMessageBox::Ok); - return; - } - infile.close(); - - MapCollection > collection(m_filename); - int i=0; - for (MapCollection >::iterator it = collection.begin(); - it != collection.end(); ++it) { - OCTOMAP_DEBUG("Adding hierarchy node %s\n", (*it)->getId().c_str()); - OcTree* tree = (*it)->getMap(); - if (!tree) - OCTOMAP_ERROR("Error while reading node %s\n", (*it)->getId().c_str()); - else { - OCTOMAP_DEBUG("Read tree with %zu tree nodes\n", tree->size()); - } - pose6d origin = (*it)->getOrigin(); - this->addOctree(tree, i, origin); - ++i; - } - setOcTreeUISwitches(); - showOcTree(); - m_glwidget->resetView(); - OCTOMAP_DEBUG("done\n"); + currentScan = 1; } - void ViewerGui::loadGraph(bool completeGraph) { - - ui.actionSettings->setEnabled(true); - ui.actionPointcloud->setEnabled(true); - ui.actionPointcloud->setChecked(false); - ui.actionTrajectory->setEnabled(true); - ui.actionOctree_cells->setEnabled(true); - ui.actionOctree_cells->setChecked(true); - ui.actionOctree_structure->setEnabled(true); - ui.actionOctree_structure->setChecked(false); - ui.actionFree->setChecked(false); - ui.actionFree->setEnabled(true); - ui.actionReload_Octree->setEnabled(true); - ui.actionConvert_ml_tree->setEnabled(true); - - unsigned graphSize = m_scanGraph->size(); - unsigned currentScan; - - if (completeGraph){ - fprintf(stderr, "loadGraph:: generating octree from complete graph.\n" ); - m_nextScanToAdd = m_scanGraph->end(); - generateOctree(); - currentScan = graphSize; - } - else{ - m_nextScanToAdd = m_scanGraph->begin(); - - //if (m_ocTree) delete m_ocTree; - //m_ocTree = new octomap::OcTree(m_octreeResolution); - OcTree* tree = new octomap::OcTree(m_octreeResolution); - this->addOctree(tree, DEFAULT_OCTREE_ID); - - addNextScan(); - - currentScan = 1; - } - - m_glwidget->resetView(); - QApplication::restoreOverrideCursor(); + m_glwidget->resetView(); + QApplication::restoreOverrideCursor(); - emit changeNumberOfScans(graphSize); - emit changeCurrentScan(currentScan); - showInfo("Done (" +QString::number(currentScan)+ " of "+ QString::number(graphSize)+" nodes)", true); + emit changeNumberOfScans(graphSize); + emit changeCurrentScan(currentScan); + showInfo("Done (" +QString::number(currentScan)+ " of "+ QString::number(graphSize)+" nodes)", true); - if (!m_trajectoryDrawer){ - m_trajectoryDrawer = new TrajectoryDrawer(); - } - m_trajectoryDrawer->setScanGraph(*m_scanGraph); + if (!m_trajectoryDrawer){ + m_trajectoryDrawer = new TrajectoryDrawer(); + } + m_trajectoryDrawer->setScanGraph(*m_scanGraph); - if (!m_pointcloudDrawer){ - m_pointcloudDrawer = new PointcloudDrawer(); - } - m_pointcloudDrawer->setScanGraph(*m_scanGraph); + if (!m_pointcloudDrawer){ + m_pointcloudDrawer = new PointcloudDrawer(); + } + m_pointcloudDrawer->setScanGraph(*m_scanGraph); - m_cameraFollowMode->setScanGraph(m_scanGraph); + m_cameraFollowMode->setScanGraph(m_scanGraph); - if (ui.actionTrajectory->isChecked()) - m_glwidget->addSceneObject(m_trajectoryDrawer); + if (ui.actionTrajectory->isChecked()) + m_glwidget->addSceneObject(m_trajectoryDrawer); - if (ui.actionPointcloud->isChecked()) - m_glwidget->addSceneObject(m_pointcloudDrawer); - } + if (ui.actionPointcloud->isChecked()) + m_glwidget->addSceneObject(m_pointcloudDrawer); +} - void ViewerGui::changeTreeDepth(int depth){ - // range check: - if (depth < 1 || depth > 16) - return; +void ViewerGui::changeTreeDepth(int depth){ + // range check: + if (depth < 1 || depth > 16) + return; - m_max_tree_depth = unsigned(depth); + m_max_tree_depth = unsigned(depth); - if (m_octrees.size() > 0) - showOcTree(); - } + if (m_octrees.size() > 0) + showOcTree(); +} - void ViewerGui::on_actionExit_triggered(){ - this->close(); - } +void ViewerGui::on_actionExit_triggered(){ + this->close(); +} - void ViewerGui::on_actionHelp_triggered(){ - m_glwidget->help(); - } +void ViewerGui::on_actionHelp_triggered(){ + m_glwidget->help(); +} - void ViewerGui::on_actionSettings_triggered(){ +void ViewerGui::on_actionSettings_triggered(){ - ViewerSettings dialog(this); - dialog.setResolution(m_octreeResolution); - dialog.setLaserType(m_laserType); - dialog.setMaxRange(m_laserMaxRange); + ViewerSettings dialog(this); + dialog.setResolution(m_octreeResolution); + dialog.setLaserType(m_laserType); + dialog.setMaxRange(m_laserMaxRange); - if (dialog.exec()){ + if (dialog.exec()){ - double oldResolution = m_octreeResolution; - double oldLaserMaxRange = m_laserMaxRange; - double oldType = m_laserType; + double oldResolution = m_octreeResolution; + double oldLaserMaxRange = m_laserMaxRange; + double oldType = m_laserType; - m_octreeResolution = dialog.getResolution(); - m_laserType = dialog.getLaserType(); - m_laserMaxRange = dialog.getMaxRange(); - - // apply new settings - bool resolutionChanged = (std::abs(oldResolution - m_octreeResolution) > 1e-5); - bool maxRangeChanged = (std::abs(oldLaserMaxRange - m_laserMaxRange) > 1e-5); + m_octreeResolution = dialog.getResolution(); + m_laserType = dialog.getLaserType(); + m_laserMaxRange = dialog.getMaxRange(); - if (resolutionChanged) - emit changeResolution(m_octreeResolution); + // apply new settings + bool resolutionChanged = (std::abs(oldResolution - m_octreeResolution) > 1e-5); + bool maxRangeChanged = (std::abs(oldLaserMaxRange - m_laserMaxRange) > 1e-5); - if (oldType != m_laserType){ // parameters changed, reload file: - openFile(); - } else if (resolutionChanged || maxRangeChanged){ - generateOctree(); - } + if (resolutionChanged) + emit changeResolution(m_octreeResolution); + if (oldType != m_laserType){ // parameters changed, reload file: + openFile(); + } else if (resolutionChanged || maxRangeChanged){ + generateOctree(); } + } +} - void ViewerGui::on_actionOpen_file_triggered(){ - QString filename = QFileDialog::getOpenFileName(this, - tr("Open data file"), "", - "All supported files (*.graph *.bt *.ot *.dat);;OcTree file (*.ot);;Bonsai tree file (*.bt);;Binary scan graph (*.graph);;Pointcloud (*.dat);;All files (*)"); - if (!filename.isEmpty()){ +void ViewerGui::on_actionOpen_file_triggered(){ + QString filename = QFileDialog::getOpenFileName(this, + tr("Open data file"), "", + "All supported files (*.graph *.bt *.ot *.dat);;OcTree file (*.ot);;Bonsai tree file (*.bt);;Binary scan graph (*.graph);;Pointcloud (*.dat);;All files (*)"); + if (!filename.isEmpty()){ #ifdef _WIN32 - m_filename = std::string(filename.toLocal8Bit().data()); + m_filename = std::string(filename.toLocal8Bit().data()); #else - m_filename = filename.toStdString(); + m_filename = filename.toStdString(); #endif - openFile(); - } + openFile(); } +} - void ViewerGui::on_actionOpen_graph_incremental_triggered(){ - QString filename = QFileDialog::getOpenFileName(this, - tr("Open graph file incrementally (at start)"), "", - "Binary scan graph (*.graph)"); - if (!filename.isEmpty()){ - m_glwidget->clearAll(); +void ViewerGui::on_actionOpen_graph_incremental_triggered(){ + QString filename = QFileDialog::getOpenFileName(this, + tr("Open graph file incrementally (at start)"), "", + "Binary scan graph (*.graph)"); + if (!filename.isEmpty()){ + m_glwidget->clearAll(); #ifdef _WIN32 - m_filename = std::string(filename.toLocal8Bit().data()); + m_filename = std::string(filename.toLocal8Bit().data()); #else - m_filename = filename.toStdString(); + m_filename = filename.toStdString(); #endif - openGraph(false); - } + openGraph(false); } +} - void ViewerGui::on_actionSave_file_triggered(){ +void ViewerGui::on_actionSave_file_triggered(){ - OcTreeRecord* r; - if (!getOctreeRecord(DEFAULT_OCTREE_ID, r)) { - fprintf(stderr, "ERROR: OctreeRecord for id %d not found!\n", DEFAULT_OCTREE_ID); - QMessageBox::warning(this, tr("3D Mapping Viewer"), - "Error: No OcTree present.", - QMessageBox::Ok); - return; - } + OcTreeRecord* r; + if (!getOctreeRecord(DEFAULT_OCTREE_ID, r)) { + fprintf(stderr, "ERROR: OctreeRecord for id %d not found!\n", DEFAULT_OCTREE_ID); + QMessageBox::warning(this, tr("3D Mapping Viewer"), + "Error: No OcTree present.", + QMessageBox::Ok); + return; + } - QString filename = QFileDialog::getSaveFileName(this, tr("Save octree file"), - "", tr("Full OcTree (*.ot);;Bonsai Tree file (*.bt);;")); + QString filename = QFileDialog::getSaveFileName(this, tr("Save octree file"), + "", tr("Full OcTree (*.ot);;Bonsai Tree file (*.bt);;")); - if (filename != ""){ - QApplication::setOverrideCursor(Qt::WaitCursor); - showInfo("Writing file... ", false); - - QFileInfo fileinfo(filename); - std::string std_filename; + if (filename != ""){ + QApplication::setOverrideCursor(Qt::WaitCursor); + showInfo("Writing file... ", false); + + QFileInfo fileinfo(filename); + std::string std_filename; #ifdef _WIN32 - std_filename = filename.toLocal8Bit().data(); + std_filename = filename.toLocal8Bit().data(); #else - std_filename = filename.toStdString(); + std_filename = filename.toStdString(); #endif - AbstractOcTree* t = r->octree; + AbstractOcTree* t = r->octree; - if (fileinfo.suffix() == "bt") { - AbstractOccupancyOcTree* ot = dynamic_cast (t); - if (ot) - ot->writeBinaryConst(std_filename); - else{ - QMessageBox::warning(this, "Unknown tree type", - "Could not convert to occupancy tree for writing .bt file", - QMessageBox::Ok); - } - } - else if (fileinfo.suffix() == "ot"){ - r->octree->write(std_filename); - } - else { - QMessageBox::warning(this, "Unknown file", - "Cannot write file, unknown extension: "+fileinfo.suffix(), + if (fileinfo.suffix() == "bt") { + AbstractOccupancyOcTree* ot = dynamic_cast (t); + if (ot) + ot->writeBinaryConst(std_filename); + else{ + QMessageBox::warning(this, "Unknown tree type", + "Could not convert to occupancy tree for writing .bt file", QMessageBox::Ok); } - - QApplication::restoreOverrideCursor(); - showInfo("Done.", true); } - } - - void ViewerGui::on_actionClear_nodes_in_selection_triggered(){ - point3d min, max; - m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z()); - m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z()); + else if (fileinfo.suffix() == "ot"){ + r->octree->write(std_filename); + } + else { + QMessageBox::warning(this, "Unknown file", + "Cannot write file, unknown extension: "+fileinfo.suffix(), + QMessageBox::Ok); + } - updateNodesInBBX(min, max, false); + QApplication::restoreOverrideCursor(); + showInfo("Done.", true); } +} - void ViewerGui::on_actionClear_selection_triggered(){ - point3d min, max; - m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z()); - m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z()); +void ViewerGui::on_actionClear_nodes_in_selection_triggered(){ + point3d min, max; + m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z()); + m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z()); - setNodesInBBX(min, max, false); - } + updateNodesInBBX(min, max, false); +} - void ViewerGui::on_actionClear_unknown_in_selection_triggered() - { - point3d min, max; - m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z()); - m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z()); +void ViewerGui::on_actionClear_selection_triggered(){ + point3d min, max; + m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z()); + m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z()); - setNonNodesInBBX(min, max, false); - } + setNodesInBBX(min, max, false); +} - void ViewerGui::on_actionFill_unknown_in_selection_triggered() - { - point3d min, max; - m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z()); - m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z()); +void ViewerGui::on_actionClear_unknown_in_selection_triggered() +{ + point3d min, max; + m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z()); + m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z()); - setNonNodesInBBX(min, max, true); - } + setNonNodesInBBX(min, max, false); +} - void ViewerGui::on_actionFill_selection_triggered(){ - point3d min, max; - m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z()); - m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z()); +void ViewerGui::on_actionFill_unknown_in_selection_triggered() +{ + point3d min, max; + m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z()); + m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z()); - setNodesInBBX(min, max, true); - } + setNonNodesInBBX(min, max, true); +} - void ViewerGui::on_actionFill_nodes_in_selection_triggered(){ - point3d min, max; - m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z()); - m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z()); +void ViewerGui::on_actionFill_selection_triggered(){ + point3d min, max; + m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z()); + m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z()); - updateNodesInBBX(min, max, true); - } + setNodesInBBX(min, max, true); +} - void ViewerGui::on_actionDelete_nodes_in_selection_triggered(){ - point3d min, max; - m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z()); - m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z()); +void ViewerGui::on_actionFill_nodes_in_selection_triggered(){ + point3d min, max; + m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z()); + m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z()); - for (std::map::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) { - OcTree* octree = dynamic_cast(t_it->second.octree); + updateNodesInBBX(min, max, true); +} - if (octree){ - for(OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(min,max), - end=octree->end_leafs_bbx(); it!= end; ++it){ - octree->deleteNode(it.getKey(), it.getDepth()); - } - } else{ - QMessageBox::warning(this, "Not implemented", "Functionality not yet implemented for this octree type", - QMessageBox::Ok); +void ViewerGui::on_actionDelete_nodes_in_selection_triggered(){ + point3d min, max; + m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z()); + m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z()); + for (std::map::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) { + OcTree* octree = dynamic_cast(t_it->second.octree); + + if (octree){ + for(OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(min,max), + end=octree->end_leafs_bbx(); it!= end; ++it){ + octree->deleteNode(it.getKey(), it.getDepth()); } - } + } else{ + QMessageBox::warning(this, "Not implemented", "Functionality not yet implemented for this octree type", + QMessageBox::Ok); - showOcTree(); + } } - void ViewerGui::on_actionDelete_nodes_outside_of_selection_triggered(){ - point3d min, max; - m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z()); - m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z()); + showOcTree(); +} - for (std::map::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) { - OcTree* octree = dynamic_cast(t_it->second.octree); +void ViewerGui::on_actionDelete_nodes_outside_of_selection_triggered(){ + point3d min, max; + m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z()); + m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z()); - if (octree){ - octomap::OcTreeKey minKey, maxKey; + for (std::map::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) { + OcTree* octree = dynamic_cast(t_it->second.octree); - if (!octree->coordToKeyChecked(min, minKey) || !octree->coordToKeyChecked(max, maxKey)){ - return; - } + if (octree){ + octomap::OcTreeKey minKey, maxKey; - for(OcTree::leaf_iterator it = octree->begin_leafs(), - end=octree->end_leafs(); it!= end; ++it){ - // check if outside of bbx: - OcTreeKey k = it.getKey(); - if (k[0] < minKey[0] || k[1] < minKey[1] || k[2] < minKey[2] - || k[0] > maxKey[0] || k[1] > maxKey[1] || k[2] > maxKey[2]) - { - octree->deleteNode(k, it.getDepth()); - } - } - } else - QMessageBox::warning(this, "Not implemented", "Functionality not yet implemented for this octree type", - QMessageBox::Ok); - } + if (!octree->coordToKeyChecked(min, minKey) || !octree->coordToKeyChecked(max, maxKey)){ + return; + } - showOcTree(); + for(OcTree::leaf_iterator it = octree->begin_leafs(), + end=octree->end_leafs(); it!= end; ++it){ + // check if outside of bbx: + OcTreeKey k = it.getKey(); + if (k[0] < minKey[0] || k[1] < minKey[1] || k[2] < minKey[2] + || k[0] > maxKey[0] || k[1] > maxKey[1] || k[2] > maxKey[2]) + { + octree->deleteNode(k, it.getDepth()); + } + } + } else + QMessageBox::warning(this, "Not implemented", "Functionality not yet implemented for this octree type", + QMessageBox::Ok); } - void ViewerGui::updateNodesInBBX(const point3d& min, const point3d& max, bool occupied){ - for (std::map::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) { - OcTree* octree = dynamic_cast(t_it->second.octree); + showOcTree(); +} - if (octree){ - float logodds; - if (occupied) - logodds = octree->getClampingThresMaxLog(); - else - logodds = octree->getClampingThresMinLog(); +void ViewerGui::updateNodesInBBX(const point3d& min, const point3d& max, bool occupied){ + for (std::map::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) { + OcTree* octree = dynamic_cast(t_it->second.octree); - for(OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(min,max), - end=octree->end_leafs_bbx(); it!= end; ++it) - { - // directly set values of leafs: - it->setLogOdds(logodds); - } + if (octree){ + float logodds; + if (occupied) + logodds = octree->getClampingThresMaxLog(); + else + logodds = octree->getClampingThresMinLog(); - // update inner nodes to make tree consistent: - octree->updateInnerOccupancy(); + for(OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(min,max), + end=octree->end_leafs_bbx(); it!= end; ++it) + { + // directly set values of leafs: + it->setLogOdds(logodds); + } - } else - QMessageBox::warning(this, "Not implemented", "Functionality not yet implemented for this octree type", - QMessageBox::Ok); + // update inner nodes to make tree consistent: + octree->updateInnerOccupancy(); - } + } else + QMessageBox::warning(this, "Not implemented", "Functionality not yet implemented for this octree type", + QMessageBox::Ok); - showOcTree(); } + showOcTree(); +} - void ViewerGui::setNodesInBBX(const point3d& min, const point3d& max, bool occupied){ - for (std::map::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) { - OcTree* octree = dynamic_cast(t_it->second.octree); - - if (octree){ - float logodds = octree->getClampingThresMaxLog() - octree->getClampingThresMinLog(); - if (!occupied) - logodds *= -1; - OcTreeKey minKey(0,0,0); - OcTreeKey maxKey(0,0,0); - octree->coordToKeyChecked(min, minKey); - octree->coordToKeyChecked(max, maxKey); - OcTreeKey k; - for (k[0] = minKey[0]; k[0] < maxKey[0]; ++k[0]){ - for (k[1] = minKey[1]; k[1] < maxKey[1]; ++k[1]){ - for (k[2] = minKey[2]; k[2] < maxKey[2]; ++k[2]){ - octree->updateNode(k, logodds); - } +void ViewerGui::setNodesInBBX(const point3d& min, const point3d& max, bool occupied){ + for (std::map::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) { + OcTree* octree = dynamic_cast(t_it->second.octree); + + if (octree){ + float logodds = octree->getClampingThresMaxLog() - octree->getClampingThresMinLog(); + if (!occupied) + logodds *= -1; + + OcTreeKey minKey(0,0,0); + OcTreeKey maxKey(0,0,0); + octree->coordToKeyChecked(min, minKey); + octree->coordToKeyChecked(max, maxKey); + OcTreeKey k; + for (k[0] = minKey[0]; k[0] < maxKey[0]; ++k[0]){ + for (k[1] = minKey[1]; k[1] < maxKey[1]; ++k[1]){ + for (k[2] = minKey[2]; k[2] < maxKey[2]; ++k[2]){ + octree->updateNode(k, logodds); } } } - } - showOcTree(); } - void ViewerGui::setNonNodesInBBX(const point3d& min, const point3d& max, bool occupied) { - for (std::map::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) { - OcTree* octree = dynamic_cast(t_it->second.octree); - - if (octree){ - float logodds = octree->getClampingThresMaxLog() - octree->getClampingThresMinLog(); - if (!occupied) - logodds *= -1; - - OcTreeKey minKey(0,0,0); - OcTreeKey maxKey(0,0,0); - octree->coordToKeyChecked(min, minKey); - octree->coordToKeyChecked(max, maxKey); - OcTreeKey k; - for (k[0] = minKey[0]; k[0] < maxKey[0]; ++k[0]){ - for (k[1] = minKey[1]; k[1] < maxKey[1]; ++k[1]){ - for (k[2] = minKey[2]; k[2] < maxKey[2]; ++k[2]){ - OcTreeNode* n = octree->search(k); - if(!n) - octree->updateNode(k, logodds); - } + showOcTree(); +} + +void ViewerGui::setNonNodesInBBX(const point3d& min, const point3d& max, bool occupied) { + for (std::map::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) { + OcTree* octree = dynamic_cast(t_it->second.octree); + + if (octree){ + float logodds = octree->getClampingThresMaxLog() - octree->getClampingThresMinLog(); + if (!occupied) + logodds *= -1; + + OcTreeKey minKey(0,0,0); + OcTreeKey maxKey(0,0,0); + octree->coordToKeyChecked(min, minKey); + octree->coordToKeyChecked(max, maxKey); + OcTreeKey k; + for (k[0] = minKey[0]; k[0] < maxKey[0]; ++k[0]){ + for (k[1] = minKey[1]; k[1] < maxKey[1]; ++k[1]){ + for (k[2] = minKey[2]; k[2] < maxKey[2]; ++k[2]){ + OcTreeNode* n = octree->search(k); + if(!n) + octree->updateNode(k, logodds); } } } - } - showOcTree(); } - void ViewerGui::on_actionExport_view_triggered(){ - m_glwidget->openSnapshotFormatDialog(); - m_glwidget->saveSnapshot(false); - } + showOcTree(); +} - void ViewerGui::on_actionExport_sequence_triggered(bool checked){ - if(checked) { - if(m_glwidget->openSnapshotFormatDialog()) { - m_glwidget->saveSnapshot(false); - m_glwidget->setSnapshotCounter(0); - connect(m_glwidget, SIGNAL(drawFinished(bool)), m_glwidget, SLOT(saveSnapshot(bool))); - } else { - ui.actionExport_sequence->setChecked(false); - } +void ViewerGui::on_actionExport_view_triggered(){ + m_glwidget->openSnapshotFormatDialog(); + m_glwidget->saveSnapshot(false); +} + +void ViewerGui::on_actionExport_sequence_triggered(bool checked){ + if(checked) { + if(m_glwidget->openSnapshotFormatDialog()) { + m_glwidget->saveSnapshot(false); + m_glwidget->setSnapshotCounter(0); + connect(m_glwidget, SIGNAL(drawFinished(bool)), m_glwidget, SLOT(saveSnapshot(bool))); } else { - disconnect(m_glwidget, SIGNAL(drawFinished(bool)), m_glwidget, SLOT(saveSnapshot(bool))); + ui.actionExport_sequence->setChecked(false); } + } else { + disconnect(m_glwidget, SIGNAL(drawFinished(bool)), m_glwidget, SLOT(saveSnapshot(bool))); } +} - void ViewerGui::on_actionPrintout_mode_toggled(bool checked){ - if (checked) { - ui.actionHeight_map->setChecked(false); - ui.actionSemanticColoring->setChecked(false); - } - - m_glwidget->enablePrintoutMode(checked); +void ViewerGui::on_actionPrintout_mode_toggled(bool checked){ + if (checked) { + ui.actionHeight_map->setChecked(false); + ui.actionSemanticColoring->setChecked(false); } - void ViewerGui::on_actionSelection_box_toggled(bool checked){ + m_glwidget->enablePrintoutMode(checked); +} + +void ViewerGui::on_actionSelection_box_toggled(bool checked){ - ui.menuDelete_nodes->setEnabled(checked); - ui.menuFill_selection->setEnabled(checked); - ui.menuChange_nodes_in_selection->setEnabled(checked); + ui.menuDelete_nodes->setEnabled(checked); + ui.menuFill_selection->setEnabled(checked); + ui.menuChange_nodes_in_selection->setEnabled(checked); - m_glwidget->enableSelectionBox(checked); + m_glwidget->enableSelectionBox(checked); - m_glwidget->updateGL(); + m_glwidget->updateGL(); +} + +void ViewerGui::on_actionHeight_map_toggled(bool checked){ + if (checked) { + ui.actionPrintout_mode->setChecked(false); + ui.actionSemanticColoring->setChecked(false); } + m_glwidget->enableHeightColorMode(checked); +} - void ViewerGui::on_actionHeight_map_toggled(bool checked){ - if (checked) { - ui.actionPrintout_mode->setChecked(false); - ui.actionSemanticColoring->setChecked(false); - } - m_glwidget->enableHeightColorMode(checked); +void ViewerGui::on_actionSemanticColoring_toggled(bool checked) { + if (checked) { + ui.actionHeight_map->setChecked(false); + ui.actionPrintout_mode->setChecked(false); } - void ViewerGui::on_actionSemanticColoring_toggled(bool checked) { - if (checked) { - ui.actionHeight_map->setChecked(false); - ui.actionPrintout_mode->setChecked(false); - } + m_glwidget->enableSemanticColoring(checked); +} - m_glwidget->enableSemanticColoring(checked); - } +void ViewerGui::on_actionStore_camera_triggered(){ + m_glwidget->camera()->deletePath(0); + m_glwidget->camera()->addKeyFrameToPath(0); + m_cameraStored = true; + ui.actionRestore_camera->setEnabled(true); +} - void ViewerGui::on_actionStore_camera_triggered(){ - m_glwidget->camera()->deletePath(0); - m_glwidget->camera()->addKeyFrameToPath(0); - m_cameraStored = true; - ui.actionRestore_camera->setEnabled(true); +void ViewerGui::on_actionRestore_camera_triggered(){ + if (m_cameraStored){ + m_glwidget->camera()->playPath(0); } +} - void ViewerGui::on_actionRestore_camera_triggered(){ - if (m_cameraStored){ - m_glwidget->camera()->playPath(0); - } +void ViewerGui::on_actionPointcloud_toggled(bool checked){ + if (m_pointcloudDrawer){ + if (checked) + m_glwidget->addSceneObject(m_pointcloudDrawer); + else + m_glwidget->removeSceneObject(m_pointcloudDrawer); } +} - void ViewerGui::on_actionPointcloud_toggled(bool checked){ - if (m_pointcloudDrawer){ - if (checked) - m_glwidget->addSceneObject(m_pointcloudDrawer); - else - m_glwidget->removeSceneObject(m_pointcloudDrawer); - } +void ViewerGui::on_actionTrajectory_toggled(bool checked){ + if (m_trajectoryDrawer){ + if (checked) + m_glwidget->addSceneObject(m_trajectoryDrawer); + else + m_glwidget->removeSceneObject(m_trajectoryDrawer); } +} - void ViewerGui::on_actionTrajectory_toggled(bool checked){ - if (m_trajectoryDrawer){ - if (checked) - m_glwidget->addSceneObject(m_trajectoryDrawer); - else - m_glwidget->removeSceneObject(m_trajectoryDrawer); - } +void ViewerGui::on_actionAxes_toggled(bool checked){ + for (std::map::iterator it = m_octrees.begin(); + it != m_octrees.end(); ++it) { + it->second.octree_drawer->enableAxes(checked); } + m_glwidget->updateGL(); +} - void ViewerGui::on_actionAxes_toggled(bool checked){ - for (std::map::iterator it = m_octrees.begin(); - it != m_octrees.end(); ++it) { - it->second.octree_drawer->enableAxes(checked); - } +void ViewerGui::on_actionHideBackground_toggled(bool checked) { + OcTreeRecord* r; + if (getOctreeRecord(DEFAULT_OCTREE_ID, r)) { + if (checked) m_glwidget->removeSceneObject(r->octree_drawer); + else m_glwidget->addSceneObject(r->octree_drawer); m_glwidget->updateGL(); } +} - void ViewerGui::on_actionHideBackground_toggled(bool checked) { - OcTreeRecord* r; - if (getOctreeRecord(DEFAULT_OCTREE_ID, r)) { - if (checked) m_glwidget->removeSceneObject(r->octree_drawer); - else m_glwidget->addSceneObject(r->octree_drawer); - m_glwidget->updateGL(); - } - } - - void ViewerGui::on_actionClear_triggered() { - for (std::map::iterator it = m_octrees.begin(); - it != m_octrees.end(); ++it) { - m_glwidget->removeSceneObject(it->second.octree_drawer); - delete (it->second.octree_drawer); - delete (it->second.octree); - } - m_octrees.clear(); - showOcTree(); +void ViewerGui::on_actionClear_triggered() { + for (std::map::iterator it = m_octrees.begin(); + it != m_octrees.end(); ++it) { + m_glwidget->removeSceneObject(it->second.octree_drawer); + delete (it->second.octree_drawer); + delete (it->second.octree); } + m_octrees.clear(); + showOcTree(); +} - void ViewerGui::on_actionTest_triggered(){ +void ViewerGui::on_actionTest_triggered(){ - } +} - void ViewerGui::on_actionReload_Octree_triggered(){ - if (m_scanGraph) { - generateOctree(); - } else { - openFile(); - } +void ViewerGui::on_actionReload_Octree_triggered(){ + if (m_scanGraph) { + generateOctree(); + } else { + openFile(); } +} - void ViewerGui::on_actionConvert_ml_tree_triggered(){ - QApplication::setOverrideCursor(Qt::WaitCursor); - if (m_octrees.size()) { - showInfo("Converting OcTree to maximum Likelihood map... "); - for (std::map::iterator it = m_octrees.begin(); - it != m_octrees.end(); ++it) { - AbstractOcTree* t = it->second.octree; - if (dynamic_cast(t)) { - ((OcTree*) t)->toMaxLikelihood(); - } - else if (dynamic_cast(t)) { - ((ColorOcTree*) t)->toMaxLikelihood(); - } +void ViewerGui::on_actionConvert_ml_tree_triggered(){ + QApplication::setOverrideCursor(Qt::WaitCursor); + if (m_octrees.size()) { + showInfo("Converting OcTree to maximum Likelihood map... "); + for (std::map::iterator it = m_octrees.begin(); + it != m_octrees.end(); ++it) { + AbstractOcTree* t = it->second.octree; + if (dynamic_cast(t)) { + ((OcTree*) t)->toMaxLikelihood(); + } + else if (dynamic_cast(t)) { + ((ColorOcTree*) t)->toMaxLikelihood(); } - showInfo("Done.", true); - showOcTree(); - QApplication::restoreOverrideCursor(); } + showInfo("Done.", true); + showOcTree(); + QApplication::restoreOverrideCursor(); } +} - void ViewerGui::on_actionPrune_tree_triggered(){ - QApplication::setOverrideCursor(Qt::WaitCursor); - if (m_octrees.size()) { - showInfo("Pruning OcTree... "); - for (std::map::iterator it = m_octrees.begin(); - it != m_octrees.end(); ++it) { - it->second.octree->prune(); - } - showOcTree(); - showInfo("Done.", true); +void ViewerGui::on_actionPrune_tree_triggered(){ + QApplication::setOverrideCursor(Qt::WaitCursor); + if (m_octrees.size()) { + showInfo("Pruning OcTree... "); + for (std::map::iterator it = m_octrees.begin(); + it != m_octrees.end(); ++it) { + it->second.octree->prune(); } - QApplication::restoreOverrideCursor(); + showOcTree(); + showInfo("Done.", true); } + QApplication::restoreOverrideCursor(); +} - void ViewerGui::on_actionExpand_tree_triggered(){ +void ViewerGui::on_actionExpand_tree_triggered(){ - QApplication::setOverrideCursor(Qt::WaitCursor); + QApplication::setOverrideCursor(Qt::WaitCursor); - // if (m_ocTree) { - if (m_octrees.size()) { - showInfo("Expanding OcTree... "); - for (std::map::iterator it = m_octrees.begin(); - it != m_octrees.end(); ++it) { - it->second.octree->expand(); - } - showOcTree(); - - showInfo("Done.", true); - } - QApplication::restoreOverrideCursor(); + // if (m_ocTree) { + if (m_octrees.size()) { + showInfo("Expanding OcTree... "); + for (std::map::iterator it = m_octrees.begin(); + it != m_octrees.end(); ++it) { + it->second.octree->expand(); + } + showOcTree(); + + showInfo("Done.", true); } + QApplication::restoreOverrideCursor(); +} - void ViewerGui::on_actionOctree_cells_toggled(bool enabled) { - for (std::map::iterator it = m_octrees.begin(); - it != m_octrees.end(); ++it) { - it->second.octree_drawer->enableOcTreeCells(enabled); - } - m_glwidget->updateGL(); +void ViewerGui::on_actionOctree_cells_toggled(bool enabled) { + for (std::map::iterator it = m_octrees.begin(); + it != m_octrees.end(); ++it) { + it->second.octree_drawer->enableOcTreeCells(enabled); } + m_glwidget->updateGL(); +} - void ViewerGui::on_actionOctree_structure_toggled(bool enabled) { - for (std::map::iterator it = m_octrees.begin(); - it != m_octrees.end(); ++it) { - it->second.octree_drawer->enableOcTree(enabled); - } - m_glwidget->updateGL(); +void ViewerGui::on_actionOctree_structure_toggled(bool enabled) { + for (std::map::iterator it = m_octrees.begin(); + it != m_octrees.end(); ++it) { + it->second.octree_drawer->enableOcTree(enabled); } + m_glwidget->updateGL(); +} - void ViewerGui::on_actionFree_toggled(bool enabled) { - for (std::map::iterator it = m_octrees.begin(); - it != m_octrees.end(); ++it) { - it->second.octree_drawer->enableFreespace(enabled); - } - m_glwidget->updateGL(); - +void ViewerGui::on_actionFree_toggled(bool enabled) { + for (std::map::iterator it = m_octrees.begin(); + it != m_octrees.end(); ++it) { + it->second.octree_drawer->enableFreespace(enabled); } + m_glwidget->updateGL(); - void ViewerGui::on_actionSelected_toggled(bool enabled) { - // if(m_octreeDrawer) { +} + +void ViewerGui::on_actionSelected_toggled(bool enabled) { + // if(m_octreeDrawer) { // m_octreeDrawer->enableSelection(enabled); - // // just for testing, you should set the selection somewhere else and only enable it here: - // if (enabled){ - // std::list selection; - // std::pair volume(octomath::Vector3(0.0, 0.0, 0.0), 0.2); - // selection.push_back(volume); - // m_octreeDrawer->setOcTreeSelection(selection); - - // } else{ - // m_octreeDrawer->clearOcTreeSelection(); - // } - // m_glwidget->updateGL(); - // } - } + // // just for testing, you should set the selection somewhere else and only enable it here: + // if (enabled){ + // std::list selection; + // std::pair volume(octomath::Vector3(0.0, 0.0, 0.0), 0.2); + // selection.push_back(volume); + // m_octreeDrawer->setOcTreeSelection(selection); + + // } else{ + // m_octreeDrawer->clearOcTreeSelection(); + // } + // m_glwidget->updateGL(); + // } +} - void ViewerGui::on_action_bg_black_triggered() { - m_glwidget->setBackgroundColor( QColor(0,0,0) ); - m_glwidget->qglClearColor( m_glwidget->backgroundColor() ); - } +void ViewerGui::on_action_bg_black_triggered() { + m_glwidget->setBackgroundColor( QColor(0,0,0) ); + m_glwidget->qglClearColor( m_glwidget->backgroundColor() ); +} - void ViewerGui::on_action_bg_white_triggered() { - m_glwidget->setBackgroundColor( QColor(255,255,255) ); - m_glwidget->qglClearColor( m_glwidget->backgroundColor() ); - } +void ViewerGui::on_action_bg_white_triggered() { + m_glwidget->setBackgroundColor( QColor(255,255,255) ); + m_glwidget->qglClearColor( m_glwidget->backgroundColor() ); +} - void ViewerGui::on_action_bg_gray_triggered() { - m_glwidget->setBackgroundColor( QColor(117,117,117) ); - m_glwidget->qglClearColor( m_glwidget->backgroundColor() ); - } +void ViewerGui::on_action_bg_gray_triggered() { + m_glwidget->setBackgroundColor( QColor(117,117,117) ); + m_glwidget->qglClearColor( m_glwidget->backgroundColor() ); +} - void ViewerGui::on_savecampose_triggered() { - QString filename = QFileDialog::getSaveFileName(this, "Save Viewer State", "camera.xml", "Camera/State file (*.xml)"); - if (!filename.isEmpty()) { - saveCameraPosition(filename.toAscii().constData()); - } +void ViewerGui::on_savecampose_triggered() { + QString filename = QFileDialog::getSaveFileName(this, "Save Viewer State", "camera.xml", "Camera/State file (*.xml)"); + if (!filename.isEmpty()) { + saveCameraPosition(filename.toAscii().constData()); } +} - void ViewerGui::on_loadcampose_triggered() { - QString filename = QFileDialog::getOpenFileName(this, "Load Viewer State", "camera.xml", "Camera/State file (*.xml)"); - if (!filename.isEmpty()) { - loadCameraPosition(filename.toAscii().constData()); - } +void ViewerGui::on_loadcampose_triggered() { + QString filename = QFileDialog::getOpenFileName(this, "Load Viewer State", "camera.xml", "Camera/State file (*.xml)"); + if (!filename.isEmpty()) { + loadCameraPosition(filename.toAscii().constData()); } +} - void ViewerGui::saveCameraPosition(const char* filename) const { - // HACK get non-const pointer to myself - ViewerWidget* aux = const_cast( m_glwidget); - aux->setStateFileName(QString(filename)); - aux->saveStateToFile(); - aux->setStateFileName(QString::null); - } +void ViewerGui::saveCameraPosition(const char* filename) const { + // HACK get non-const pointer to myself + ViewerWidget* aux = const_cast( m_glwidget); + aux->setStateFileName(QString(filename)); + aux->saveStateToFile(); + aux->setStateFileName(QString::null); +} - void ViewerGui::loadCameraPosition(const char* filename) { - m_glwidget->setStateFileName(QString(filename)); - m_glwidget->restoreStateFromFile(); - m_glwidget->setStateFileName(QString::null); - } +void ViewerGui::loadCameraPosition(const char* filename) { + m_glwidget->setStateFileName(QString(filename)); + m_glwidget->restoreStateFromFile(); + m_glwidget->setStateFileName(QString::null); +} } diff --git a/octovis/src/ViewerSettingsPanelCamera.cpp b/octovis/src/ViewerSettingsPanelCamera.cpp index 5fa08a90..ea47ad05 100644 --- a/octovis/src/ViewerSettingsPanelCamera.cpp +++ b/octovis/src/ViewerSettingsPanelCamera.cpp @@ -27,18 +27,18 @@ #include ViewerSettingsPanelCamera::ViewerSettingsPanelCamera(QWidget *parent) - : QWidget(parent), m_currentFrame(1), m_numberFrames(0), m_robotTrajectoryAvailable(false) +: QWidget(parent), m_currentFrame(1), m_numberFrames(0), m_robotTrajectoryAvailable(false) { - ui.setupUi(this); - connect(ui.posX, SIGNAL(valueChanged(double)), this, SLOT(positionEditDone(double))); - connect(ui.posY, SIGNAL(valueChanged(double)), this, SLOT(positionEditDone(double))); - connect(ui.posZ, SIGNAL(valueChanged(double)), this, SLOT(positionEditDone(double))); - connect(ui.lookX, SIGNAL(valueChanged(double)), this, SLOT(positionEditDone(double))); - connect(ui.lookY, SIGNAL(valueChanged(double)), this, SLOT(positionEditDone(double))); - connect(ui.lookZ, SIGNAL(valueChanged(double)), this, SLOT(positionEditDone(double))); - - ui.followTrajectoryButton->setEnabled(m_robotTrajectoryAvailable); - dataChanged(); + ui.setupUi(this); + connect(ui.posX, SIGNAL(valueChanged(double)), this, SLOT(positionEditDone(double))); + connect(ui.posY, SIGNAL(valueChanged(double)), this, SLOT(positionEditDone(double))); + connect(ui.posZ, SIGNAL(valueChanged(double)), this, SLOT(positionEditDone(double))); + connect(ui.lookX, SIGNAL(valueChanged(double)), this, SLOT(positionEditDone(double))); + connect(ui.lookY, SIGNAL(valueChanged(double)), this, SLOT(positionEditDone(double))); + connect(ui.lookZ, SIGNAL(valueChanged(double)), this, SLOT(positionEditDone(double))); + + ui.followTrajectoryButton->setEnabled(m_robotTrajectoryAvailable); + dataChanged(); } ViewerSettingsPanelCamera::~ViewerSettingsPanelCamera() @@ -47,7 +47,7 @@ ViewerSettingsPanelCamera::~ViewerSettingsPanelCamera() } QSize ViewerSettingsPanelCamera::sizeHint() const { - return QSize(250, 180); + return QSize(250, 180); } void ViewerSettingsPanelCamera::positionEditDone(double){ @@ -65,81 +65,81 @@ void ViewerSettingsPanelCamera::setCurrentFrame(unsigned frame){ } void ViewerSettingsPanelCamera::setRobotTrajectoryAvailable(bool available) { - m_robotTrajectoryAvailable = available; - if(!available) ui.followTrajectoryButton->setChecked(false); - ui.followTrajectoryButton->setEnabled(available); + m_robotTrajectoryAvailable = available; + if(!available) ui.followTrajectoryButton->setChecked(false); + ui.followTrajectoryButton->setEnabled(available); } void ViewerSettingsPanelCamera::gotoFrame(unsigned int frame) { - if(frame > 0 && frame <= m_numberFrames) { - m_currentFrame = frame; - emit jumpToFrame(m_currentFrame); - dataChanged(); - } + if(frame > 0 && frame <= m_numberFrames) { + m_currentFrame = frame; + emit jumpToFrame(m_currentFrame); + dataChanged(); + } } void ViewerSettingsPanelCamera::on_nextScanButton_clicked(){ - gotoFrame(m_currentFrame + 1); + gotoFrame(m_currentFrame + 1); } void ViewerSettingsPanelCamera::on_previousScanButton_clicked(){ - gotoFrame(m_currentFrame - 1); + gotoFrame(m_currentFrame - 1); } void ViewerSettingsPanelCamera::on_firstScanButton_clicked(){ - gotoFrame(1); + gotoFrame(1); } void ViewerSettingsPanelCamera::on_lastScanButton_clicked(){ - gotoFrame(m_numberFrames); + gotoFrame(m_numberFrames); } void ViewerSettingsPanelCamera::on_followCameraPathButton_clicked(){ - emit followCameraPath(); + emit followCameraPath(); } void ViewerSettingsPanelCamera::on_followTrajectoryButton_clicked(){ - emit followRobotPath(); + emit followRobotPath(); } void ViewerSettingsPanelCamera::on_cameraPathAdd_clicked(){ - emit addToCameraPath(); + emit addToCameraPath(); } void ViewerSettingsPanelCamera::on_cameraPathRemove_clicked(){ - emit removeFromCameraPath(); + emit removeFromCameraPath(); } void ViewerSettingsPanelCamera::on_cameraPathClear_clicked(){ - emit clearCameraPath(); + emit clearCameraPath(); } void ViewerSettingsPanelCamera::on_cameraPathSave_clicked(){ - emit saveToCameraPath(); + emit saveToCameraPath(); } void ViewerSettingsPanelCamera::on_playScanButton_clicked(){ - if(ui.playScanButton->isChecked()) { - ui.scanProgressSlider->setEnabled(false); - ui.followGroupBox->setEnabled(false); - emit play(); - } else { - ui.scanProgressSlider->setEnabled(true); - ui.followGroupBox->setEnabled(true); - emit pause(); - } - dataChanged(); + if(ui.playScanButton->isChecked()) { + ui.scanProgressSlider->setEnabled(false); + ui.followGroupBox->setEnabled(false); + emit play(); + } else { + ui.scanProgressSlider->setEnabled(true); + ui.followGroupBox->setEnabled(true); + emit pause(); + } + dataChanged(); } void ViewerSettingsPanelCamera::on_scanProgressSlider_sliderMoved(int value) { - gotoFrame(value); + gotoFrame(value); } void ViewerSettingsPanelCamera::setStopped(){ - ui.followGroupBox->setEnabled(true); - ui.scanProgressSlider->setEnabled(true); - ui.playScanButton->setChecked(false); - dataChanged(); + ui.followGroupBox->setEnabled(true); + ui.scanProgressSlider->setEnabled(true); + ui.playScanButton->setChecked(false); + dataChanged(); } @@ -151,52 +151,52 @@ void ViewerSettingsPanelCamera::dataChanged(){ ui.scanProgressSlider->setMinimum(1); if(ui.playScanButton->isChecked()) { - ui.firstScanButton->setEnabled(false); - ui.nextScanButton->setEnabled(false); - ui.previousScanButton->setEnabled(false); - ui.lastScanButton->setEnabled(false); + ui.firstScanButton->setEnabled(false); + ui.nextScanButton->setEnabled(false); + ui.previousScanButton->setEnabled(false); + ui.lastScanButton->setEnabled(false); } else { - if (m_currentFrame >= max){ - ui.nextScanButton->setEnabled(false); - ui.playScanButton->setEnabled(false); - ui.lastScanButton->setEnabled(false); - } else { - ui.nextScanButton->setEnabled(true); - ui.playScanButton->setEnabled(true); - ui.lastScanButton->setEnabled(true); - } - - if (m_currentFrame < 2){ - ui.firstScanButton->setEnabled(cur > 0); - ui.previousScanButton->setEnabled(false); - } else{ - ui.firstScanButton->setEnabled(true); - ui.previousScanButton->setEnabled(true); - } - - if (max > 1) { - ui.playScanButton->setEnabled(true); - } else { - ui.playScanButton->setEnabled(false); - } + if (m_currentFrame >= max){ + ui.nextScanButton->setEnabled(false); + ui.playScanButton->setEnabled(false); + ui.lastScanButton->setEnabled(false); + } else { + ui.nextScanButton->setEnabled(true); + ui.playScanButton->setEnabled(true); + ui.lastScanButton->setEnabled(true); + } + + if (m_currentFrame < 2){ + ui.firstScanButton->setEnabled(cur > 0); + ui.previousScanButton->setEnabled(false); + } else{ + ui.firstScanButton->setEnabled(true); + ui.previousScanButton->setEnabled(true); + } + + if (max > 1) { + ui.playScanButton->setEnabled(true); + } else { + ui.playScanButton->setEnabled(false); + } } if(followRobotTrajectory() || ui.playScanButton->isChecked()) { - ui.cameraPathAdd->setEnabled(false); - ui.cameraPathRemove->setEnabled(false); - ui.cameraPathSave->setEnabled(false); - ui.cameraPathClear->setEnabled(false); + ui.cameraPathAdd->setEnabled(false); + ui.cameraPathRemove->setEnabled(false); + ui.cameraPathSave->setEnabled(false); + ui.cameraPathClear->setEnabled(false); } else { - ui.cameraPathAdd->setEnabled(true); - ui.cameraPathRemove->setEnabled(m_numberFrames > 0); - ui.cameraPathSave->setEnabled(m_numberFrames > 0); - ui.cameraPathClear->setEnabled(m_numberFrames > 0); + ui.cameraPathAdd->setEnabled(true); + ui.cameraPathRemove->setEnabled(m_numberFrames > 0); + ui.cameraPathSave->setEnabled(m_numberFrames > 0); + ui.cameraPathClear->setEnabled(m_numberFrames > 0); } if(max > 0 && !ui.playScanButton->isChecked()) { - ui.scanProgressSlider->setEnabled(true); + ui.scanProgressSlider->setEnabled(true); } else { - ui.scanProgressSlider->setEnabled(false); + ui.scanProgressSlider->setEnabled(false); } ui.scanProgressSlider->setValue(cur); @@ -208,7 +208,7 @@ void ViewerSettingsPanelCamera::dataChanged(){ } bool ViewerSettingsPanelCamera::followRobotTrajectory(){ - return ui.followTrajectoryButton->isChecked(); + return ui.followTrajectoryButton->isChecked(); } diff --git a/octovis/src/ViewerWidget.cpp b/octovis/src/ViewerWidget.cpp index 3e7403eb..d72227ad 100644 --- a/octovis/src/ViewerWidget.cpp +++ b/octovis/src/ViewerWidget.cpp @@ -33,54 +33,54 @@ using namespace std; namespace octomap { - ViewerWidget::ViewerWidget(QWidget* parent) : - QGLViewer(parent), m_zMin(0.0),m_zMax(1.0) { - - m_printoutMode = false; - m_heightColorMode = false; - m_semantic_coloring = false; - m_drawSelectionBox = false; - } +ViewerWidget::ViewerWidget(QWidget* parent) : + QGLViewer(parent), m_zMin(0.0),m_zMax(1.0) { - void ViewerWidget::init() { + m_printoutMode = false; + m_heightColorMode = false; + m_semantic_coloring = false; + m_drawSelectionBox = false; +} -// setHandlerKeyboardModifiers(QGLViewer::CAMERA, Qt::AltModifier); -// setHandlerKeyboardModifiers(QGLViewer::FRAME, Qt::NoModifier); -// setHandlerKeyboardModifiers(QGLViewer::CAMERA, Qt::ControlModifier); - setMouseTracking(true); +void ViewerWidget::init() { - // Restore previous viewer state. - restoreStateFromFile(); + // setHandlerKeyboardModifiers(QGLViewer::CAMERA, Qt::AltModifier); + // setHandlerKeyboardModifiers(QGLViewer::FRAME, Qt::NoModifier); + // setHandlerKeyboardModifiers(QGLViewer::CAMERA, Qt::ControlModifier); + setMouseTracking(true); - // Make camera the default manipulated frame. - setManipulatedFrame( camera()->frame() ); - // invert mousewheel (more like Blender) - camera()->frame()->setWheelSensitivity(-1.0); + // Restore previous viewer state. + restoreStateFromFile(); + // Make camera the default manipulated frame. + setManipulatedFrame( camera()->frame() ); + // invert mousewheel (more like Blender) + camera()->frame()->setWheelSensitivity(-1.0); - // Light initialization: - glEnable(GL_LIGHT0); - float pos[4] = {-1.0, 1.0, 1.0, 0.0}; - // Directional light - glLightfv(GL_LIGHT0, GL_POSITION, pos); + // Light initialization: + glEnable(GL_LIGHT0); - // background color defaults to white - this->setBackgroundColor( QColor(255,255,255) ); - this->qglClearColor( this->backgroundColor() ); - } + float pos[4] = {-1.0, 1.0, 1.0, 0.0}; + // Directional light + glLightfv(GL_LIGHT0, GL_POSITION, pos); + + // background color defaults to white + this->setBackgroundColor( QColor(255,255,255) ); + this->qglClearColor( this->backgroundColor() ); +} - void ViewerWidget::resetView(){ - this->camera()->setOrientation((float) -M_PI_2, (float) M_PI_2); - this->showEntireScene(); - updateGL(); - } +void ViewerWidget::resetView(){ + this->camera()->setOrientation((float) -M_PI_2, (float) M_PI_2); + this->showEntireScene(); + updateGL(); +} - QString ViewerWidget::helpString() const{ - QString help = "

Octomap 3D viewer

"; +QString ViewerWidget::helpString() const{ + QString help = "

Octomap 3D viewer

"; - help +="The Octomap library implements a 3D occupancy grid mapping approach. " + help +="The Octomap library implements a 3D occupancy grid mapping approach. " "It provides data structures and mapping algorithms. The map is implemented " "using an octree. 3D maps can be viewed an built using this 3D viewer." "

" @@ -90,309 +90,309 @@ namespace octomap { "

" "Please refer to the \"Keyboard\" and \"Mouse\" tabs for instructions on " "how to use the viewer."; - return help; - } - - void ViewerWidget::enableHeightColorMode (bool enabled) { - m_heightColorMode = enabled; - for(std::vector::iterator it = m_sceneObjects.begin(); it != m_sceneObjects.end(); it++) { - (*it)->enableHeightColorMode(enabled); - } - updateGL(); - } - - void ViewerWidget::enablePrintoutMode(bool enabled) { - m_printoutMode = enabled; - for(std::vector::iterator it = m_sceneObjects.begin(); it != m_sceneObjects.end(); it++) { - (*it)->enablePrintoutMode(enabled); - } - updateGL(); - } - - void ViewerWidget::enableSemanticColoring (bool enabled) { - m_semantic_coloring = enabled; - for(std::vector::iterator it = m_sceneObjects.begin(); it != m_sceneObjects.end(); it++) { - (*it)->enableSemanticColoring(enabled); - } - updateGL(); - } - - void ViewerWidget::enableSelectionBox(bool enabled) { - m_drawSelectionBox = enabled; - updateGL(); - } - - - qglviewer::Quaternion ViewerWidget::poseToQGLQuaternion(const octomath::Pose6D& pose) { - // copying octomap::Quaternion parameters to qglviewer::Quaternion does not work (reason unknown) - // octomath::Quaternion quaternion = pose.rot().normalized(); - // return qglviewer::Quaternion(quaternion.x(), quaternion.y(), quaternion.z(), quaternion.u()); + return help; +} - // Compute viewing direction and use code from libqglviewer's "look at" function - octomath::Vector3 dir = pose.rot().rotate(octomath::Vector3(1.,0.,0.)); - qglviewer::Vec direction(dir.x(), dir.y(), dir.z()); - //qglviewer::Vec xAxis = direction ^ camera()->upVector(); - // useing 0, 0, 1 as upvector instead: - qglviewer::Vec xAxis = direction ^ qglviewer::Vec(0.0, 0.0, 1.0); - - qglviewer::Quaternion q; - q.setFromRotatedBasis(xAxis, xAxis^direction, -direction); - return q; +void ViewerWidget::enableHeightColorMode (bool enabled) { + m_heightColorMode = enabled; + for(std::vector::iterator it = m_sceneObjects.begin(); it != m_sceneObjects.end(); it++) { + (*it)->enableHeightColorMode(enabled); } + updateGL(); +} - void ViewerWidget::setCamPosition(double x, double y, double z, double lookX, double lookY, double lookZ){ - this->camera()->setOrientation(-M_PI/2., M_PI/2.); - camera()->setPosition(qglviewer::Vec(x, y, z)); - camera()->lookAt(qglviewer::Vec(lookX, lookY, lookZ)); - camera()->setUpVector(qglviewer::Vec(0.0, 0.0, 1.0)); - updateGL(); +void ViewerWidget::enablePrintoutMode(bool enabled) { + m_printoutMode = enabled; + for(std::vector::iterator it = m_sceneObjects.begin(); it != m_sceneObjects.end(); it++) { + (*it)->enablePrintoutMode(enabled); } + updateGL(); +} - void ViewerWidget::setCamPose(const octomath::Pose6D& pose){ - octomath::Pose6D ahead = pose * octomath::Pose6D(octomath::Vector3(1,0,0), octomath::Quaternion()); - setCamPosition(pose.x(), pose.y(), pose.z(), ahead.x(), ahead.y(), ahead.z()); +void ViewerWidget::enableSemanticColoring (bool enabled) { + m_semantic_coloring = enabled; + for(std::vector::iterator it = m_sceneObjects.begin(); it != m_sceneObjects.end(); it++) { + (*it)->enableSemanticColoring(enabled); } - - void ViewerWidget::jumpToCamFrame(int id, int frame) { - qglviewer::KeyFrameInterpolator *kfi = camera()->keyFrameInterpolator(id); - if(kfi && frame >= 0 && frame < kfi->numberOfKeyFrames()) { - camera()->setPosition(kfi->keyFrame(frame).position()); - camera()->setOrientation(kfi->keyFrame(frame).orientation()); - } else { - std::cerr << "Error: Could not jump to frame " << frame << " of " << kfi->numberOfKeyFrames() << std::endl; - } - updateGL(); + updateGL(); +} + +void ViewerWidget::enableSelectionBox(bool enabled) { + m_drawSelectionBox = enabled; + updateGL(); +} + + +qglviewer::Quaternion ViewerWidget::poseToQGLQuaternion(const octomath::Pose6D& pose) { + // copying octomap::Quaternion parameters to qglviewer::Quaternion does not work (reason unknown) + // octomath::Quaternion quaternion = pose.rot().normalized(); + // return qglviewer::Quaternion(quaternion.x(), quaternion.y(), quaternion.z(), quaternion.u()); + + // Compute viewing direction and use code from libqglviewer's "look at" function + octomath::Vector3 dir = pose.rot().rotate(octomath::Vector3(1.,0.,0.)); + qglviewer::Vec direction(dir.x(), dir.y(), dir.z()); + //qglviewer::Vec xAxis = direction ^ camera()->upVector(); + // useing 0, 0, 1 as upvector instead: + qglviewer::Vec xAxis = direction ^ qglviewer::Vec(0.0, 0.0, 1.0); + + qglviewer::Quaternion q; + q.setFromRotatedBasis(xAxis, xAxis^direction, -direction); + return q; +} + +void ViewerWidget::setCamPosition(double x, double y, double z, double lookX, double lookY, double lookZ){ + this->camera()->setOrientation(-M_PI/2., M_PI/2.); + camera()->setPosition(qglviewer::Vec(x, y, z)); + camera()->lookAt(qglviewer::Vec(lookX, lookY, lookZ)); + camera()->setUpVector(qglviewer::Vec(0.0, 0.0, 1.0)); + updateGL(); +} + +void ViewerWidget::setCamPose(const octomath::Pose6D& pose){ + octomath::Pose6D ahead = pose * octomath::Pose6D(octomath::Vector3(1,0,0), octomath::Quaternion()); + setCamPosition(pose.x(), pose.y(), pose.z(), ahead.x(), ahead.y(), ahead.z()); +} + +void ViewerWidget::jumpToCamFrame(int id, int frame) { + qglviewer::KeyFrameInterpolator *kfi = camera()->keyFrameInterpolator(id); + if(kfi && frame >= 0 && frame < kfi->numberOfKeyFrames()) { + camera()->setPosition(kfi->keyFrame(frame).position()); + camera()->setOrientation(kfi->keyFrame(frame).orientation()); + } else { + std::cerr << "Error: Could not jump to frame " << frame << " of " << kfi->numberOfKeyFrames() << std::endl; } - - void ViewerWidget::deleteCameraPath(int id) { - if(camera()->keyFrameInterpolator(id)) { - disconnect(camera()->keyFrameInterpolator(id), SIGNAL(interpolated()), this, SLOT(updateGL())); - disconnect(camera()->keyFrameInterpolator(id), SIGNAL(interpolated()), this, SLOT(cameraPathInterpolated())); - disconnect(camera()->keyFrameInterpolator(id), SIGNAL(endReached()), this, SLOT(cameraPathFinished())); - camera()->deletePath(id); - } + updateGL(); +} + +void ViewerWidget::deleteCameraPath(int id) { + if(camera()->keyFrameInterpolator(id)) { + disconnect(camera()->keyFrameInterpolator(id), SIGNAL(interpolated()), this, SLOT(updateGL())); + disconnect(camera()->keyFrameInterpolator(id), SIGNAL(interpolated()), this, SLOT(cameraPathInterpolated())); + disconnect(camera()->keyFrameInterpolator(id), SIGNAL(endReached()), this, SLOT(cameraPathFinished())); + camera()->deletePath(id); } - - void ViewerWidget::appendToCameraPath(int id, const octomath::Pose6D& pose) { - qglviewer::Vec position(pose.trans().x(), pose.trans().y(), pose.trans().z()); - qglviewer::Quaternion quaternion = poseToQGLQuaternion(pose); - qglviewer::Frame frame(position, quaternion); - if(!camera()->keyFrameInterpolator(id)) { - camera()->setKeyFrameInterpolator(id, new qglviewer::KeyFrameInterpolator(camera()->frame())); - } - camera()->keyFrameInterpolator(id)->addKeyFrame(frame); +} + +void ViewerWidget::appendToCameraPath(int id, const octomath::Pose6D& pose) { + qglviewer::Vec position(pose.trans().x(), pose.trans().y(), pose.trans().z()); + qglviewer::Quaternion quaternion = poseToQGLQuaternion(pose); + qglviewer::Frame frame(position, quaternion); + if(!camera()->keyFrameInterpolator(id)) { + camera()->setKeyFrameInterpolator(id, new qglviewer::KeyFrameInterpolator(camera()->frame())); } - - void ViewerWidget::removeFromCameraPath(int id, int frame) { - qglviewer::KeyFrameInterpolator *old_kfi = camera()->keyFrameInterpolator(id); - if(old_kfi) { - qglviewer::KeyFrameInterpolator *new_kfi = new qglviewer::KeyFrameInterpolator(camera()->frame()); - for(int i = 0; i < old_kfi->numberOfKeyFrames(); i++) { - if(i != frame) { - new_kfi->addKeyFrame(old_kfi->keyFrame(i)); - } + camera()->keyFrameInterpolator(id)->addKeyFrame(frame); +} + +void ViewerWidget::removeFromCameraPath(int id, int frame) { + qglviewer::KeyFrameInterpolator *old_kfi = camera()->keyFrameInterpolator(id); + if(old_kfi) { + qglviewer::KeyFrameInterpolator *new_kfi = new qglviewer::KeyFrameInterpolator(camera()->frame()); + for(int i = 0; i < old_kfi->numberOfKeyFrames(); i++) { + if(i != frame) { + new_kfi->addKeyFrame(old_kfi->keyFrame(i)); } - deleteCameraPath(id); - camera()->setKeyFrameInterpolator(id, new_kfi); } + deleteCameraPath(id); + camera()->setKeyFrameInterpolator(id, new_kfi); } - - void ViewerWidget::updateCameraPath(int id, int frame) { - qglviewer::KeyFrameInterpolator *old_kfi = camera()->keyFrameInterpolator(id); - if(old_kfi) { - qglviewer::KeyFrameInterpolator *new_kfi = new qglviewer::KeyFrameInterpolator(camera()->frame()); - for(int i = 0; i < old_kfi->numberOfKeyFrames(); i++) { - if(i != frame) { - new_kfi->addKeyFrame(old_kfi->keyFrame(i)); - } else { - new_kfi->addKeyFrame(*(camera()->frame())); - } +} + +void ViewerWidget::updateCameraPath(int id, int frame) { + qglviewer::KeyFrameInterpolator *old_kfi = camera()->keyFrameInterpolator(id); + if(old_kfi) { + qglviewer::KeyFrameInterpolator *new_kfi = new qglviewer::KeyFrameInterpolator(camera()->frame()); + for(int i = 0; i < old_kfi->numberOfKeyFrames(); i++) { + if(i != frame) { + new_kfi->addKeyFrame(old_kfi->keyFrame(i)); + } else { + new_kfi->addKeyFrame(*(camera()->frame())); } - deleteCameraPath(id); - camera()->setKeyFrameInterpolator(id, new_kfi); } + deleteCameraPath(id); + camera()->setKeyFrameInterpolator(id, new_kfi); } - - void ViewerWidget::appendCurrentToCameraPath(int id) { - int frame = 0; - if(camera()->keyFrameInterpolator(id)) frame = camera()->keyFrameInterpolator(id)->numberOfKeyFrames(); - addCurrentToCameraPath(id, frame); - } - - void ViewerWidget::addCurrentToCameraPath(int id, int frame) { - qglviewer::KeyFrameInterpolator *old_kfi = camera()->keyFrameInterpolator(id); - if(!old_kfi || frame >= old_kfi->numberOfKeyFrames()) { - camera()->addKeyFrameToPath(id); - } else { - qglviewer::KeyFrameInterpolator *new_kfi = new qglviewer::KeyFrameInterpolator(camera()->frame()); - for(int i = 0; i < old_kfi->numberOfKeyFrames(); i++) { - new_kfi->addKeyFrame(old_kfi->keyFrame(i)); - if(i == frame) { - new_kfi->addKeyFrame(camera()->frame()); - } +} + +void ViewerWidget::appendCurrentToCameraPath(int id) { + int frame = 0; + if(camera()->keyFrameInterpolator(id)) frame = camera()->keyFrameInterpolator(id)->numberOfKeyFrames(); + addCurrentToCameraPath(id, frame); +} + +void ViewerWidget::addCurrentToCameraPath(int id, int frame) { + qglviewer::KeyFrameInterpolator *old_kfi = camera()->keyFrameInterpolator(id); + if(!old_kfi || frame >= old_kfi->numberOfKeyFrames()) { + camera()->addKeyFrameToPath(id); + } else { + qglviewer::KeyFrameInterpolator *new_kfi = new qglviewer::KeyFrameInterpolator(camera()->frame()); + for(int i = 0; i < old_kfi->numberOfKeyFrames(); i++) { + new_kfi->addKeyFrame(old_kfi->keyFrame(i)); + if(i == frame) { + new_kfi->addKeyFrame(camera()->frame()); } - deleteCameraPath(id); - camera()->setKeyFrameInterpolator(id, new_kfi); } + deleteCameraPath(id); + camera()->setKeyFrameInterpolator(id, new_kfi); } - - void ViewerWidget::playCameraPath(int id, int start_frame) { - qglviewer::KeyFrameInterpolator *kfi = camera()->keyFrameInterpolator(id); - if(kfi && !kfi->interpolationIsStarted() && start_frame >= 0 && start_frame < kfi->numberOfKeyFrames()) { - m_current_camera_path = id; - m_current_camera_frame = start_frame; - kfi->setInterpolationTime(kfi->keyFrameTime(start_frame)); - std::cout << "Playing path of length " << kfi->numberOfKeyFrames() << ", start time " << kfi->keyFrameTime(start_frame) << std::endl; - connect(kfi, SIGNAL(interpolated()), this, SLOT(updateGL())); - connect(kfi, SIGNAL(interpolated()), this, SLOT(cameraPathInterpolated())); - connect(kfi, SIGNAL(endReached()), this, SLOT(cameraPathFinished())); - kfi->startInterpolation(); - } +} + +void ViewerWidget::playCameraPath(int id, int start_frame) { + qglviewer::KeyFrameInterpolator *kfi = camera()->keyFrameInterpolator(id); + if(kfi && !kfi->interpolationIsStarted() && start_frame >= 0 && start_frame < kfi->numberOfKeyFrames()) { + m_current_camera_path = id; + m_current_camera_frame = start_frame; + kfi->setInterpolationTime(kfi->keyFrameTime(start_frame)); + std::cout << "Playing path of length " << kfi->numberOfKeyFrames() << ", start time " << kfi->keyFrameTime(start_frame) << std::endl; + connect(kfi, SIGNAL(interpolated()), this, SLOT(updateGL())); + connect(kfi, SIGNAL(interpolated()), this, SLOT(cameraPathInterpolated())); + connect(kfi, SIGNAL(endReached()), this, SLOT(cameraPathFinished())); + kfi->startInterpolation(); } - - void ViewerWidget::stopCameraPath(int id) { - if(camera()->keyFrameInterpolator(id) && camera()->keyFrameInterpolator(id)->interpolationIsStarted()) { - disconnect(camera()->keyFrameInterpolator(id), SIGNAL(interpolated()), this, SLOT(updateGL())); - disconnect(camera()->keyFrameInterpolator(id), SIGNAL(interpolated()), this, SLOT(cameraPathInterpolated())); - disconnect(camera()->keyFrameInterpolator(id), SIGNAL(endReached()), this, SLOT(cameraPathFinished())); - camera()->keyFrameInterpolator(id)->stopInterpolation(); - } +} + +void ViewerWidget::stopCameraPath(int id) { + if(camera()->keyFrameInterpolator(id) && camera()->keyFrameInterpolator(id)->interpolationIsStarted()) { + disconnect(camera()->keyFrameInterpolator(id), SIGNAL(interpolated()), this, SLOT(updateGL())); + disconnect(camera()->keyFrameInterpolator(id), SIGNAL(interpolated()), this, SLOT(cameraPathInterpolated())); + disconnect(camera()->keyFrameInterpolator(id), SIGNAL(endReached()), this, SLOT(cameraPathFinished())); + camera()->keyFrameInterpolator(id)->stopInterpolation(); } - - void ViewerWidget::cameraPathFinished() { - if(camera()->keyFrameInterpolator(m_current_camera_path)) { - disconnect(camera()->keyFrameInterpolator(m_current_camera_path), SIGNAL(interpolated()), this, SLOT(updateGL())); - disconnect(camera()->keyFrameInterpolator(m_current_camera_path), SIGNAL(interpolated()), this, SLOT(cameraPathInterpolated())); - disconnect(camera()->keyFrameInterpolator(m_current_camera_path), SIGNAL(endReached()), this, SLOT(cameraPathFinished())); - emit cameraPathStopped(m_current_camera_path); - } +} + +void ViewerWidget::cameraPathFinished() { + if(camera()->keyFrameInterpolator(m_current_camera_path)) { + disconnect(camera()->keyFrameInterpolator(m_current_camera_path), SIGNAL(interpolated()), this, SLOT(updateGL())); + disconnect(camera()->keyFrameInterpolator(m_current_camera_path), SIGNAL(interpolated()), this, SLOT(cameraPathInterpolated())); + disconnect(camera()->keyFrameInterpolator(m_current_camera_path), SIGNAL(endReached()), this, SLOT(cameraPathFinished())); + emit cameraPathStopped(m_current_camera_path); } - - void ViewerWidget::cameraPathInterpolated() { - qglviewer::KeyFrameInterpolator *kfi = camera()->keyFrameInterpolator(m_current_camera_path); - if(kfi) { - int current_frame = m_current_camera_frame; - for(int i = m_current_camera_frame + 1; i < kfi->numberOfKeyFrames(); i++) { - if(kfi->keyFrameTime(current_frame) <= kfi->interpolationTime()) current_frame = i; - else break; - } - if(current_frame != m_current_camera_frame) { - m_current_camera_frame = current_frame; - emit cameraPathFrameChanged(m_current_camera_path, m_current_camera_frame); - } +} + +void ViewerWidget::cameraPathInterpolated() { + qglviewer::KeyFrameInterpolator *kfi = camera()->keyFrameInterpolator(m_current_camera_path); + if(kfi) { + int current_frame = m_current_camera_frame; + for(int i = m_current_camera_frame + 1; i < kfi->numberOfKeyFrames(); i++) { + if(kfi->keyFrameTime(current_frame) <= kfi->interpolationTime()) current_frame = i; + else break; } - } - - void ViewerWidget::setSceneBoundingBox(const qglviewer::Vec& min, const qglviewer::Vec& max){ - m_zMin = min[2]; - m_zMax = max[2]; - QGLViewer::setSceneBoundingBox(min, max); - } - - void ViewerWidget::addSceneObject(SceneObject* obj){ - assert (obj); - m_sceneObjects.push_back(obj); - updateGL(); - } - - void ViewerWidget::removeSceneObject(SceneObject* obj){ - assert(obj); - for(std::vector::iterator it = m_sceneObjects.begin(); - it != m_sceneObjects.end();){ - if (*it == obj) - it = m_sceneObjects.erase(it); - else - ++it; + if(current_frame != m_current_camera_frame) { + m_current_camera_frame = current_frame; + emit cameraPathFrameChanged(m_current_camera_path, m_current_camera_frame); } - updateGL(); } - - void ViewerWidget::clearAll(){ - // clear drawable objects: - for(std::vector::iterator it = m_sceneObjects.begin(); it != m_sceneObjects.end(); it++){ - (*it)->clear(); - } +} + +void ViewerWidget::setSceneBoundingBox(const qglviewer::Vec& min, const qglviewer::Vec& max){ + m_zMin = min[2]; + m_zMax = max[2]; + QGLViewer::setSceneBoundingBox(min, max); +} + +void ViewerWidget::addSceneObject(SceneObject* obj){ + assert (obj); + m_sceneObjects.push_back(obj); + updateGL(); +} + +void ViewerWidget::removeSceneObject(SceneObject* obj){ + assert(obj); + for(std::vector::iterator it = m_sceneObjects.begin(); + it != m_sceneObjects.end();){ + if (*it == obj) + it = m_sceneObjects.erase(it); + else + ++it; } + updateGL(); +} - void ViewerWidget::draw(){ +void ViewerWidget::clearAll(){ + // clear drawable objects: + for(std::vector::iterator it = m_sceneObjects.begin(); it != m_sceneObjects.end(); it++){ + (*it)->clear(); + } +} - // debugging: draw light in scene - //drawLight(GL_LIGHT0); +void ViewerWidget::draw(){ - glEnable(GL_BLEND); - glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + // debugging: draw light in scene + //drawLight(GL_LIGHT0); - glEnable(GL_LIGHTING); - if (m_printoutMode){ - glCullFace(GL_BACK); - } + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); - // draw drawable objects: - for(std::vector::iterator it = m_sceneObjects.begin(); - it != m_sceneObjects.end(); ++it){ - (*it)->draw(); - } + glEnable(GL_LIGHTING); + if (m_printoutMode){ + glCullFace(GL_BACK); + } - if (m_drawSelectionBox){ - m_selectionBox.draw(); + // draw drawable objects: + for(std::vector::iterator it = m_sceneObjects.begin(); + it != m_sceneObjects.end(); ++it){ + (*it)->draw(); + } - if (m_selectionBox.getGrabbedFrame() >= 0){ - setMouseBinding(Qt::LeftButton, FRAME, TRANSLATE); - } else { - setMouseBinding(Qt::LeftButton, FRAME, ROTATE); - } + if (m_drawSelectionBox){ + m_selectionBox.draw(); + if (m_selectionBox.getGrabbedFrame() >= 0){ + setMouseBinding(Qt::LeftButton, FRAME, TRANSLATE); + } else { + setMouseBinding(Qt::LeftButton, FRAME, ROTATE); } } - void ViewerWidget::drawWithNames(){ +} - if (m_drawSelectionBox) - m_selectionBox.draw(true); - } +void ViewerWidget::drawWithNames(){ - void ViewerWidget::postDraw(){ + if (m_drawSelectionBox) + m_selectionBox.draw(true); +} - // Reset model view matrix to world coordinates origin - glMatrixMode(GL_MODELVIEW); - glPushMatrix(); - camera()->loadModelViewMatrix(); - // TODO restore model loadProjectionMatrixStereo +void ViewerWidget::postDraw(){ - // Save OpenGL state - glPushAttrib(GL_ALL_ATTRIB_BITS); + // Reset model view matrix to world coordinates origin + glMatrixMode(GL_MODELVIEW); + glPushMatrix(); + camera()->loadModelViewMatrix(); + // TODO restore model loadProjectionMatrixStereo - glDisable(GL_COLOR_MATERIAL); - qglColor(foregroundColor()); + // Save OpenGL state + glPushAttrib(GL_ALL_ATTRIB_BITS); - if (gridIsDrawn()){ - glLineWidth(1.0); - drawGrid(5.0, 10); - } - if (axisIsDrawn()){ - glLineWidth(2.0); - drawAxis(1.0); - } + glDisable(GL_COLOR_MATERIAL); + qglColor(foregroundColor()); + + if (gridIsDrawn()){ + glLineWidth(1.0); + drawGrid(5.0, 10); + } + if (axisIsDrawn()){ + glLineWidth(2.0); + drawAxis(1.0); + } - // Restore GL state - glPopAttrib(); - glPopMatrix(); + // Restore GL state + glPopAttrib(); + glPopMatrix(); - m_drawAxis = axisIsDrawn(); - m_drawGrid = gridIsDrawn(); - setAxisIsDrawn(false); - setGridIsDrawn(false); - QGLViewer::postDraw(); + m_drawAxis = axisIsDrawn(); + m_drawGrid = gridIsDrawn(); + setAxisIsDrawn(false); + setGridIsDrawn(false); + QGLViewer::postDraw(); - setAxisIsDrawn(m_drawAxis); - setGridIsDrawn(m_drawGrid); - } + setAxisIsDrawn(m_drawAxis); + setGridIsDrawn(m_drawGrid); +} - void ViewerWidget::postSelection(const QPoint&) - { +void ViewerWidget::postSelection(const QPoint&) +{ - } +}