From 54d920cc4c1742c646f4efe265d6e7a769af0428 Mon Sep 17 00:00:00 2001 From: Philipp Koch Date: Tue, 24 Oct 2017 15:08:29 +0200 Subject: [PATCH 01/18] Added a method to build a slice image from the tsd function. Added a method to buid images of all slices of a certain axis of the tsd space. --- applications/tsd_test.cpp | 6 +- obvision/reconstruct/space/TsdSpace.cpp | 158 ++++++++++++++++++++++++ obvision/reconstruct/space/TsdSpace.h | 14 +++ 3 files changed, 177 insertions(+), 1 deletion(-) diff --git a/applications/tsd_test.cpp b/applications/tsd_test.cpp index 687bd39..056104d 100644 --- a/applications/tsd_test.cpp +++ b/applications/tsd_test.cpp @@ -12,7 +12,7 @@ using namespace std; using namespace obvious; #define SENSORRAYCAST 0 - +#define SLICE_IMAGES 0 int main(void) { LOGMSG_CONF("tsd_test.log", Logger::file_off|Logger::screen_on, DBG_DEBUG, DBG_DEBUG); @@ -127,6 +127,10 @@ int main(void) Matrix P = sensor.getTransformation(); if(sensor.hasRealMeasurementRGB()) vcloud.setColors(rgb, cnt/3, 3); +#if SLICE_IMAGES + space.serializeSliceImages(Z); +#endif + Obvious3D viewer("TSD Cloud"); viewer.showSensorPose(P); diff --git a/obvision/reconstruct/space/TsdSpace.cpp b/obvision/reconstruct/space/TsdSpace.cpp index b193de0..d9c90e2 100644 --- a/obvision/reconstruct/space/TsdSpace.cpp +++ b/obvision/reconstruct/space/TsdSpace.cpp @@ -2,6 +2,7 @@ #include "obcore/base/Logger.h" #include "obcore/base/Timer.h" #include "obcore/math/mathbase.h" +#include "obcore/base/tools.h" #include "TsdSpace.h" #include "TsdSpaceBranch.h" #include "SensorProjective3D.h" @@ -874,4 +875,161 @@ TsdSpace* TsdSpace::load(const char* filename) return space; } +bool TsdSpace::sliceImage(const unsigned int idx, const EnumSpaceAxis& axis, std::vector* const rgb) +{ + rgb->clear(); + const unsigned int sizePartition = this->getPartitionSize(); + if(axis == Z) + { + const unsigned int partIdxZ = idx / sizePartition; + const unsigned int voxelIdxZ = idx - partIdxZ * sizePartition; + rgb->resize(_cellsX * _cellsY * 3, 255); + for(unsigned int i = 0; i < _cellsY; i++) + { + for(unsigned int j = 0; j < _cellsX; j++) + { + const unsigned int partIdxX = j / sizePartition; + const unsigned int partIdxY = i / sizePartition; + + TsdSpacePartition* partCur = _partitions[partIdxZ][partIdxY][partIdxX]; + + if(!partCur->isInitialized()) + continue; + + const unsigned int voxelIdxX = j - partIdxX * sizePartition; + const unsigned int voxelIdxY = i - partIdxY * sizePartition; + const double tsdCur = partCur->_space[voxelIdxZ][voxelIdxY][voxelIdxX].tsd; + const unsigned char color = static_cast(255.0 * std::abs(tsdCur)); + if(tsdCur < 0.0) //behind voxel RED + { + (*rgb)[(i * _cellsX + j) * 3 ] = color; + (*rgb)[(i * _cellsX + j) * 3 + 1] = 0; + (*rgb)[(i * _cellsX + j) * 3 + 2] = 0; + } + else //in front of voxel BLUE + { + (*rgb)[(i * _cellsX + j) * 3 ] = 0; + (*rgb)[(i * _cellsX + j) * 3 + 1] = 0; + (*rgb)[(i * _cellsX + j) * 3 + 2] = color; + } + } + } + } + else if(axis == Y) + { + const unsigned int partIdxY = idx / sizePartition; + const unsigned int voxelIdxY = idx - partIdxY * sizePartition; + rgb->resize(_cellsX * _cellsZ * 3, 255); + for(unsigned int i = 0; i < _cellsZ; i++) + { + for(unsigned int j = 0; j < _cellsX; j++) + { + const unsigned int partIdxX = j / sizePartition; + const unsigned int partIdxZ = i / sizePartition; + + TsdSpacePartition* partCur = _partitions[partIdxZ][partIdxY][partIdxX]; + + if(!partCur->isInitialized()) + continue; + + const unsigned int voxelIdxX = j - partIdxX * sizePartition; + const unsigned int voxelIdxZ = i - partIdxZ * sizePartition; + const double tsdCur = partCur->_space[voxelIdxZ][voxelIdxY][voxelIdxX].tsd; + const unsigned char color = static_cast(255.0 * std::abs(tsdCur)); + if(tsdCur < 0.0) //behind voxel RED + { + (*rgb)[(i * _cellsX + j) * 3 ] = color; + (*rgb)[(i * _cellsX + j) * 3 + 1] = 0; + (*rgb)[(i * _cellsX + j) * 3 + 2] = 0; + } + else //in front of voxel BLUE + { + (*rgb)[(i * _cellsX + j) * 3 ] = 0; + (*rgb)[(i * _cellsX + j) * 3 + 1] = 0; + (*rgb)[(i * _cellsX + j) * 3 + 2] = color; + } + } + } + } + else if(axis == X) + { + const unsigned int partIdxX = idx / sizePartition; + const unsigned int voxelIdxX = idx - partIdxX * sizePartition; + rgb->resize(_cellsY * _cellsZ * 3, 255); + for(unsigned int i = 0; i < _cellsZ; i++) + { + for(unsigned int j = 0; j < _cellsY; j++) + { + const unsigned int partIdxY = j / sizePartition; + const unsigned int partIdxZ = i / sizePartition; + + TsdSpacePartition* partCur = _partitions[partIdxZ][partIdxY][partIdxX]; + + if(!partCur->isInitialized()) + continue; + + const unsigned int voxelIdxY = j - partIdxY * sizePartition; + const unsigned int voxelIdxZ = i - partIdxZ * sizePartition; + const double tsdCur = partCur->_space[voxelIdxZ][voxelIdxY][voxelIdxX].tsd; + const unsigned char color = static_cast(255.0 * std::abs(tsdCur)); + if(tsdCur < 0.0) //behind voxel RED + { + (*rgb)[(i * _cellsY + j) * 3 ] = color; + (*rgb)[(i * _cellsY + j) * 3 + 1] = 0; + (*rgb)[(i * _cellsY + j) * 3 + 2] = 0; + } + else //in front of voxel BLUE + { + (*rgb)[(i * _cellsY + j) * 3 ] = 0; + (*rgb)[(i * _cellsY + j) * 3 + 1] = 0; + (*rgb)[(i * _cellsY + j) * 3 + 2] = color; + } + } + } + } + return true; +} + +void TsdSpace::serializeSliceImages(const EnumSpaceAxis& axis, const std::string& path) +{ + std::string storePath; + if(path.size()) + storePath = path; + else + storePath = "/tmp"; + std::vector imageBuf; + if(axis == Z) + { + for(unsigned int i = 0; i < _cellsZ; i++) + { + this->sliceImage(i, axis, &imageBuf); + std::stringstream ss; + ss << storePath << "/z_axis_" << i << ".ppm"; + serializePPM(ss.str().c_str(), imageBuf.data(), _cellsX, _cellsY); + } + } + else if(axis == Y) + { + for(unsigned int i = 0; i < _cellsY; i++) + { + this->sliceImage(i, axis, &imageBuf); + std::stringstream ss; + ss << storePath << "/y_axis_" << i << ".ppm"; + serializePPM(ss.str().c_str(), imageBuf.data(), _cellsX, _cellsZ); + } + } + else if(axis == X) + { + for(unsigned int i = 0; i < _cellsX; i++) + { + this->sliceImage(i, axis, &imageBuf); + std::stringstream ss; + ss << storePath << "/x_axis_" << i << ".ppm"; + serializePPM(ss.str().c_str(), imageBuf.data(), _cellsY, _cellsZ); + } + } + else + return; +} + } diff --git a/obvision/reconstruct/space/TsdSpace.h b/obvision/reconstruct/space/TsdSpace.h index 2559a62..76b1f58 100644 --- a/obvision/reconstruct/space/TsdSpace.h +++ b/obvision/reconstruct/space/TsdSpace.h @@ -6,6 +6,8 @@ #include "obvision/reconstruct/Sensor.h" #include "TsdSpacePartition.h" +#include + namespace obvious { @@ -27,6 +29,14 @@ enum EnumTsdSpaceInterpolate { INTERPOLATE_SUCCESS=0, INTERPOLATE_EMPTYPARTITION=2, INTERPOLATE_ISNAN=3}; +enum EnumSpaceAxis +{ + X = 0, + Y, + Z +}; + + /** * @class TsdSpace * @brief Space representing a true signed distance function @@ -219,6 +229,10 @@ enum EnumTsdSpaceInterpolate { INTERPOLATE_SUCCESS=0, */ static TsdSpace* load(const char* filename); + bool sliceImage(const unsigned int idx, const EnumSpaceAxis& axis, std::vector* const rgb); + + void serializeSliceImages(const EnumSpaceAxis& axis, const std::string& path = ""); + private: void pushRecursion(Sensor* sensor, obfloat pos[3], TsdSpaceComponent* comp, vector &partitionsToCheck); From 8cdb6559dd32534118a443c7ebcad72b55810365 Mon Sep 17 00:00:00 2001 From: Your Name Date: Mon, 23 Jul 2018 17:55:16 +0200 Subject: [PATCH 02/18] Just committed everything. not mergeable with the main fork --- Doxyfile | 0 License.txt | 0 README.md | 0 applications/CMakeLists.txt | 2 + applications/astar_test.cpp | 0 applications/icp_interactive3D.cpp | 0 applications/icp_matching2D.cpp | 0 applications/kinect.xml | 0 applications/kinect_ir.xml | 0 applications/kinect_localize.cpp | 0 applications/kinect_mesh_show.cpp | 0 applications/kinect_perspective.cpp | 0 applications/kinect_playback.cpp | 0 applications/kinect_stream_show.cpp | 0 applications/logging_example.cpp | 0 applications/lua/config.lua | 0 applications/lua/function.lua | 0 applications/lua/functionWithCallback.lua | 0 applications/lua/statePa.lua | 0 applications/lua/statePi.lua | 0 applications/lua/statePo.lua | 0 applications/lua_call_function.cpp | 0 applications/lua_callback_c.cpp | 0 applications/lua_read_config.cpp | 0 applications/lua_statemachine.cpp | 0 applications/nanoStream.cpp | 0 applications/ndt_matching2D.cpp | 0 applications/obvious3D_map.cpp | 0 applications/obvious3D_show.cpp | 0 applications/ransac_circle.cpp | 0 applications/ransac_matching2D.cpp | 0 applications/showCloud.cpp | 0 applications/statemachine_test.cpp | 0 applications/synthetic_pointcloud.cpp | 0 applications/tsd_camboardNano.cpp | 0 applications/tsd_grid_lms100.cpp | 0 applications/tsd_grid_test.cpp | 0 applications/tsd_kinect.cpp | 0 applications/tsd_raycast_visualize.cpp | 0 applications/tsd_test.cpp | 0 applications/tsd_testMineShaft.cpp | 269 ++++++ applications/tsd_xtion.cpp | 0 applications/uvccam_finddevice.cpp | 0 applications/uvccam_querycapabilities.cpp | 0 applications/uvccam_serialize.cpp | 0 applications/uvccam_stream.cpp | 0 applications/uvcvirtualcam_serialize.cpp | 0 applications/xtion.xml | 0 applications/xtionStream.cpp | 0 build/debug/CMakeLists.txt | 0 build/release/CMakeLists.txt | 6 + build/release/fixup_deb.sh.in | 0 cmake/FindEigen.cmake | 0 doxy.config | 0 obcore/Axis.h | 0 obcore/CMakeLists.txt | 0 obcore/Point3D.h | 0 obcore/base/CartesianCloud.cpp | 0 obcore/base/CartesianCloud.h | 0 obcore/base/CartesianCloudFactory.cpp | 0 obcore/base/CartesianCloudFactory.h | 0 obcore/base/Logger.cpp | 0 obcore/base/Logger.h | 0 obcore/base/Point.h | 10 + obcore/base/PointCloud.cpp | 0 obcore/base/PointCloud.h | 0 obcore/base/System.h | 0 obcore/base/System.inl | 0 obcore/base/Time.cpp | 0 obcore/base/Time.h | 0 obcore/base/Timer.cpp | 0 obcore/base/Timer.h | 0 obcore/base/tools.cpp | 0 obcore/base/tools.h | 0 obcore/base/types.h | 0 obcore/filter/BoundingBoxFilter.cpp | 0 obcore/filter/BoundingBoxFilter.h | 0 obcore/filter/CartesianFilter.cpp | 0 obcore/filter/CartesianFilter.h | 0 obcore/filter/EuclideanFilter.cpp | 0 obcore/filter/EuclideanFilter.h | 0 obcore/filter/EuclideanFilterVecD.cpp | 0 obcore/filter/EuclideanFilterVecD.h | 0 obcore/filter/Filter.h | 0 obcore/filter/FilterDistance.h | 0 obcore/filter/NormalFilter.cpp | 0 obcore/filter/NormalFilter.h | 0 obcore/grid/GradientGrid.cpp | 0 obcore/grid/GradientGrid.h | 0 obcore/grid/Grid2D.cpp | 0 obcore/grid/Grid2D.h | 0 obcore/grid/HeightGrid.cpp | 0 obcore/grid/HeightGrid.h | 0 obcore/grid/ObstacleGrid.cpp | 0 obcore/grid/ObstacleGrid.h | 0 obcore/math/IntegratorSimpson.cpp | 0 obcore/math/IntegratorSimpson.h | 0 obcore/math/PID_Controller.cpp | 0 obcore/math/PID_Controller.h | 0 obcore/math/Quaternion.cpp | 0 obcore/math/Quaternion.h | 0 obcore/math/Trajectory.cpp | 0 obcore/math/Trajectory.h | 0 obcore/math/TransformationWatchdog.cpp | 0 obcore/math/TransformationWatchdog.h | 0 obcore/math/geometry.cpp | 0 obcore/math/geometry.h | 0 obcore/math/linalg/MatrixFactory.cpp | 0 obcore/math/linalg/MatrixFactory.h | 0 obcore/math/linalg/eigen/Matrix.cpp | 0 obcore/math/linalg/eigen/Matrix.h | 0 obcore/math/linalg/eigen/Vector.cpp | 0 obcore/math/linalg/eigen/Vector.h | 0 obcore/math/linalg/gsl/Matrix.cpp | 0 obcore/math/linalg/gsl/Matrix.h | 0 obcore/math/linalg/gsl/Vector.cpp | 0 obcore/math/linalg/gsl/Vector.h | 0 obcore/math/linalg/linalg.h.in | 0 obcore/math/mathbase.h | 0 obcore/scripting/LuaScriptManager.cpp | 0 obcore/scripting/LuaScriptManager.h | 0 obcore/statemachine/Agent.cpp | 0 obcore/statemachine/Agent.h | 2 + obcore/statemachine/AgentModel.cpp | 0 obcore/statemachine/AgentModel.h | 0 obcore/statemachine/RobotModel.cpp | 0 obcore/statemachine/RobotModel.h | 0 obcore/statemachine/states/StateBase.cpp | 0 obcore/statemachine/states/StateBase.h | 0 obcore/statemachine/states/StateBaseModel.h | 0 obcore/statemachine/states/StateLua.cpp | 0 obcore/statemachine/states/StateLua.h | 0 obcore/statemachine/states/StatePing.cpp | 0 obcore/statemachine/states/StatePing.h | 0 obcore/statemachine/states/StatePong.cpp | 0 obcore/statemachine/states/StatePong.h | 0 obdevice/CMakeLists.txt | 0 obdevice/CamNano.cpp | 0 obdevice/CamNano.h | 0 obdevice/CloudFactory.cpp | 0 obdevice/CloudFactory.h | 0 obdevice/Kinect.cpp | 0 obdevice/Kinect.h | 0 obdevice/KinectPlayback.cpp | 0 obdevice/KinectPlayback.h | 0 obdevice/LaserDevice.h | 0 obdevice/OpenNiDevice.cpp | 0 obdevice/OpenNiDevice.h | 0 obdevice/ParentDevice3D.cpp | 0 obdevice/ParentDevice3D.h | 0 obdevice/PclCloudInterface.cpp | 0 obdevice/PclCloudInterface.h | 0 obdevice/SickLMS100.cpp | 0 obdevice/SickLMS100.h | 0 obdevice/UvcCam.cpp | 0 obdevice/UvcCam.h | 0 obdevice/UvcVirtualCam.cpp | 0 obdevice/UvcVirtualCam.h | 0 obgraphic/CMakeLists.txt | 0 obgraphic/CloudWidget.cpp | 0 obgraphic/CloudWidget.h | 0 obgraphic/IronPalette.h | 0 obgraphic/Obvious.h | 0 obgraphic/Obvious2D.cpp | 0 obgraphic/Obvious2D.h | 0 obgraphic/Obvious2DMap.cpp | 0 obgraphic/Obvious2DMap.h | 0 obgraphic/Obvious3D.cpp | 0 obgraphic/Obvious3D.h | 0 obgraphic/README | 0 obgraphic/VtkCloud.cpp | 0 obgraphic/VtkCloud.h | 0 obgraphic/ohm_logo.h | 0 obvision/CMakeLists.txt | 12 + obvision/README | 0 obvision/mesh/TriangleMesh.cpp | 0 obvision/mesh/TriangleMesh.h | 0 obvision/normals/NormalsEstimator.cpp | 0 obvision/normals/NormalsEstimator.h | 0 obvision/planning/AStar.cpp | 0 obvision/planning/AStar.h | 0 obvision/planning/AStarMap.cpp | 0 obvision/planning/AStarMap.h | 0 obvision/planning/AStarNode.cpp | 0 obvision/planning/AStarNode.h | 0 obvision/planning/Obstacle.cpp | 0 obvision/planning/Obstacle.h | 0 obvision/ransac/RansacPrimitives.cpp | 0 obvision/ransac/RansacPrimitives.h | 0 obvision/reconstruct/Sensor.cpp | 0 obvision/reconstruct/Sensor.h | 0 .../reconstruct/grid/RayCastAxisAligned2D.cpp | 0 .../reconstruct/grid/RayCastAxisAligned2D.h | 0 obvision/reconstruct/grid/RayCastPolar2D.cpp | 0 obvision/reconstruct/grid/RayCastPolar2D.h | 0 obvision/reconstruct/grid/SensorPolar2D.cpp | 0 obvision/reconstruct/grid/SensorPolar2D.h | 0 obvision/reconstruct/grid/TsdGrid.cpp | 0 obvision/reconstruct/grid/TsdGrid.h | 0 obvision/reconstruct/grid/TsdGridBranch.cpp | 0 obvision/reconstruct/grid/TsdGridBranch.h | 0 .../reconstruct/grid/TsdGridComponent.cpp | 0 obvision/reconstruct/grid/TsdGridComponent.h | 0 .../reconstruct/grid/TsdGridPartition.cpp | 0 obvision/reconstruct/grid/TsdGridPartition.h | 0 obvision/reconstruct/reconstruct_defs.h | 0 obvision/reconstruct/space/RayCast3D.cpp | 346 +++++++- obvision/reconstruct/space/RayCast3D.h | 6 +- .../space/RayCastAxisAligned3D.cpp | 0 .../reconstruct/space/RayCastAxisAligned3D.h | 0 .../space/SensorPolar2DWith3DPose.cpp | 138 +++ .../space/SensorPolar2DWith3DPose.h | 97 +++ obvision/reconstruct/space/SensorPolar3D.cpp | 0 obvision/reconstruct/space/SensorPolar3D.h | 0 .../reconstruct/space/SensorProjective3D.cpp | 0 .../reconstruct/space/SensorProjective3D.h | 0 obvision/reconstruct/space/TsdSpace.cpp | 809 ++++++++++++++++-- obvision/reconstruct/space/TsdSpace.h | 484 ++++++----- obvision/reconstruct/space/TsdSpaceBranch.cpp | 0 obvision/reconstruct/space/TsdSpaceBranch.h | 0 .../reconstruct/space/TsdSpaceComponent.cpp | 0 .../reconstruct/space/TsdSpaceComponent.h | 0 .../reconstruct/space/TsdSpacePartition.cpp | 2 +- .../reconstruct/space/TsdSpacePartition.h | 0 obvision/registration/Trace.cpp | 0 obvision/registration/Trace.h | 0 .../amcl/AdaptiveMonteCarloMatching.h | 0 .../icp/ClosedFormEstimator2D.cpp | 0 .../registration/icp/ClosedFormEstimator2D.h | 0 obvision/registration/icp/IRigidEstimator.h | 0 obvision/registration/icp/Icp.cpp | 0 obvision/registration/icp/Icp.h | 0 .../registration/icp/IcpMultiInitIterator.cpp | 0 .../registration/icp/IcpMultiInitIterator.h | 0 .../icp/PointToLineEstimator2D.cpp | 0 .../registration/icp/PointToLineEstimator2D.h | 0 .../icp/PointToPlaneEstimator3D.cpp | 0 .../icp/PointToPlaneEstimator3D.h | 0 .../icp/PointToPointEstimator3D.cpp | 0 .../icp/PointToPointEstimator3D.h | 0 .../icp/assign/AnnPairAssignment.cpp | 0 .../icp/assign/AnnPairAssignment.h | 0 .../icp/assign/FlannPairAssignment.cpp | 0 .../icp/assign/FlannPairAssignment.h | 0 .../icp/assign/NaboPairAssignment.cpp | 0 .../icp/assign/NaboPairAssignment.h | 0 .../icp/assign/PairAssignment.cpp | 0 .../registration/icp/assign/PairAssignment.h | 0 .../icp/assign/ProjectivePairAssignment.cpp | 0 .../icp/assign/ProjectivePairAssignment.h | 0 obvision/registration/icp/assign/assignbase.h | 0 .../icp/assign/filter/DistanceFilter.cpp | 0 .../icp/assign/filter/DistanceFilter.h | 0 .../icp/assign/filter/IPostAssignmentFilter.h | 0 .../icp/assign/filter/IPreAssignmentFilter.h | 0 .../icp/assign/filter/OcclusionFilter.cpp | 0 .../icp/assign/filter/OcclusionFilter.h | 0 .../icp/assign/filter/OutOfBoundsFilter2D.cpp | 0 .../icp/assign/filter/OutOfBoundsFilter2D.h | 0 .../icp/assign/filter/OutOfBoundsFilter3D.cpp | 0 .../icp/assign/filter/OutOfBoundsFilter3D.h | 0 .../icp/assign/filter/ProjectionFilter.cpp | 0 .../icp/assign/filter/ProjectionFilter.h | 0 .../icp/assign/filter/ReciprocalFilter.cpp | 0 .../icp/assign/filter/ReciprocalFilter.h | 0 .../assign/filter/RobotFootprintFilter.cpp | 0 .../icp/assign/filter/RobotFootprintFilter.h | 0 .../icp/assign/filter/TrimmedFilter.cpp | 0 .../icp/assign/filter/TrimmedFilter.h | 0 obvision/registration/icp/icp_def.h | 0 obvision/registration/ndt/Ndt.cpp | 0 obvision/registration/ndt/Ndt.h | 0 .../ransacMatching/PDFMatching.cpp | 0 .../registration/ransacMatching/PDFMatching.h | 0 .../ransacMatching/RandomMatching.cpp | 0 .../ransacMatching/RandomMatching.h | 0 .../ransacMatching/RandomNormalMatching.cpp | 0 .../ransacMatching/RandomNormalMatching.h | 0 .../ransacMatching/TSD_PDFMatching.cpp | 0 .../ransacMatching/TSD_PDFMatching.h | 0 .../ransacMatching/TwinPointMatching.cpp | 0 .../ransacMatching/TwinPointMatching.h | 0 test/README.md | 0 test/gtest-1.7.0.zip | Bin test/obcore/CMakeLists.txt | 0 test/obcore/base/eigen-vs-gsl.cpp | 0 test/obcore/base/pointcloud.cpp | 0 test/obcore/math/MatrixTest.cpp | 0 test/obcore/math/QuaternionTest.cpp | 0 289 files changed, 1899 insertions(+), 284 deletions(-) mode change 100644 => 100755 Doxyfile mode change 100644 => 100755 License.txt mode change 100644 => 100755 README.md mode change 100644 => 100755 applications/CMakeLists.txt mode change 100644 => 100755 applications/astar_test.cpp mode change 100644 => 100755 applications/icp_interactive3D.cpp mode change 100644 => 100755 applications/icp_matching2D.cpp mode change 100644 => 100755 applications/kinect.xml mode change 100644 => 100755 applications/kinect_ir.xml mode change 100644 => 100755 applications/kinect_localize.cpp mode change 100644 => 100755 applications/kinect_mesh_show.cpp mode change 100644 => 100755 applications/kinect_perspective.cpp mode change 100644 => 100755 applications/kinect_playback.cpp mode change 100644 => 100755 applications/kinect_stream_show.cpp mode change 100644 => 100755 applications/logging_example.cpp mode change 100644 => 100755 applications/lua/config.lua mode change 100644 => 100755 applications/lua/function.lua mode change 100644 => 100755 applications/lua/functionWithCallback.lua mode change 100644 => 100755 applications/lua/statePa.lua mode change 100644 => 100755 applications/lua/statePi.lua mode change 100644 => 100755 applications/lua/statePo.lua mode change 100644 => 100755 applications/lua_call_function.cpp mode change 100644 => 100755 applications/lua_callback_c.cpp mode change 100644 => 100755 applications/lua_read_config.cpp mode change 100644 => 100755 applications/lua_statemachine.cpp mode change 100644 => 100755 applications/nanoStream.cpp mode change 100644 => 100755 applications/ndt_matching2D.cpp mode change 100644 => 100755 applications/obvious3D_map.cpp mode change 100644 => 100755 applications/obvious3D_show.cpp mode change 100644 => 100755 applications/ransac_circle.cpp mode change 100644 => 100755 applications/ransac_matching2D.cpp mode change 100644 => 100755 applications/showCloud.cpp mode change 100644 => 100755 applications/statemachine_test.cpp mode change 100644 => 100755 applications/synthetic_pointcloud.cpp mode change 100644 => 100755 applications/tsd_camboardNano.cpp mode change 100644 => 100755 applications/tsd_grid_lms100.cpp mode change 100644 => 100755 applications/tsd_grid_test.cpp mode change 100644 => 100755 applications/tsd_kinect.cpp mode change 100644 => 100755 applications/tsd_raycast_visualize.cpp mode change 100644 => 100755 applications/tsd_test.cpp create mode 100755 applications/tsd_testMineShaft.cpp mode change 100644 => 100755 applications/tsd_xtion.cpp mode change 100644 => 100755 applications/uvccam_finddevice.cpp mode change 100644 => 100755 applications/uvccam_querycapabilities.cpp mode change 100644 => 100755 applications/uvccam_serialize.cpp mode change 100644 => 100755 applications/uvccam_stream.cpp mode change 100644 => 100755 applications/uvcvirtualcam_serialize.cpp mode change 100644 => 100755 applications/xtion.xml mode change 100644 => 100755 applications/xtionStream.cpp mode change 100644 => 100755 build/debug/CMakeLists.txt mode change 100644 => 100755 build/release/CMakeLists.txt mode change 100644 => 100755 build/release/fixup_deb.sh.in mode change 100644 => 100755 cmake/FindEigen.cmake mode change 100644 => 100755 doxy.config mode change 100644 => 100755 obcore/Axis.h mode change 100644 => 100755 obcore/CMakeLists.txt mode change 100644 => 100755 obcore/Point3D.h mode change 100644 => 100755 obcore/base/CartesianCloud.cpp mode change 100644 => 100755 obcore/base/CartesianCloud.h mode change 100644 => 100755 obcore/base/CartesianCloudFactory.cpp mode change 100644 => 100755 obcore/base/CartesianCloudFactory.h mode change 100644 => 100755 obcore/base/Logger.cpp mode change 100644 => 100755 obcore/base/Logger.h mode change 100644 => 100755 obcore/base/Point.h mode change 100644 => 100755 obcore/base/PointCloud.cpp mode change 100644 => 100755 obcore/base/PointCloud.h mode change 100644 => 100755 obcore/base/System.h mode change 100644 => 100755 obcore/base/System.inl mode change 100644 => 100755 obcore/base/Time.cpp mode change 100644 => 100755 obcore/base/Time.h mode change 100644 => 100755 obcore/base/Timer.cpp mode change 100644 => 100755 obcore/base/Timer.h mode change 100644 => 100755 obcore/base/tools.cpp mode change 100644 => 100755 obcore/base/tools.h mode change 100644 => 100755 obcore/base/types.h mode change 100644 => 100755 obcore/filter/BoundingBoxFilter.cpp mode change 100644 => 100755 obcore/filter/BoundingBoxFilter.h mode change 100644 => 100755 obcore/filter/CartesianFilter.cpp mode change 100644 => 100755 obcore/filter/CartesianFilter.h mode change 100644 => 100755 obcore/filter/EuclideanFilter.cpp mode change 100644 => 100755 obcore/filter/EuclideanFilter.h mode change 100644 => 100755 obcore/filter/EuclideanFilterVecD.cpp mode change 100644 => 100755 obcore/filter/EuclideanFilterVecD.h mode change 100644 => 100755 obcore/filter/Filter.h mode change 100644 => 100755 obcore/filter/FilterDistance.h mode change 100644 => 100755 obcore/filter/NormalFilter.cpp mode change 100644 => 100755 obcore/filter/NormalFilter.h mode change 100644 => 100755 obcore/grid/GradientGrid.cpp mode change 100644 => 100755 obcore/grid/GradientGrid.h mode change 100644 => 100755 obcore/grid/Grid2D.cpp mode change 100644 => 100755 obcore/grid/Grid2D.h mode change 100644 => 100755 obcore/grid/HeightGrid.cpp mode change 100644 => 100755 obcore/grid/HeightGrid.h mode change 100644 => 100755 obcore/grid/ObstacleGrid.cpp mode change 100644 => 100755 obcore/grid/ObstacleGrid.h mode change 100644 => 100755 obcore/math/IntegratorSimpson.cpp mode change 100644 => 100755 obcore/math/IntegratorSimpson.h mode change 100644 => 100755 obcore/math/PID_Controller.cpp mode change 100644 => 100755 obcore/math/PID_Controller.h mode change 100644 => 100755 obcore/math/Quaternion.cpp mode change 100644 => 100755 obcore/math/Quaternion.h mode change 100644 => 100755 obcore/math/Trajectory.cpp mode change 100644 => 100755 obcore/math/Trajectory.h mode change 100644 => 100755 obcore/math/TransformationWatchdog.cpp mode change 100644 => 100755 obcore/math/TransformationWatchdog.h mode change 100644 => 100755 obcore/math/geometry.cpp mode change 100644 => 100755 obcore/math/geometry.h mode change 100644 => 100755 obcore/math/linalg/MatrixFactory.cpp mode change 100644 => 100755 obcore/math/linalg/MatrixFactory.h mode change 100644 => 100755 obcore/math/linalg/eigen/Matrix.cpp mode change 100644 => 100755 obcore/math/linalg/eigen/Matrix.h mode change 100644 => 100755 obcore/math/linalg/eigen/Vector.cpp mode change 100644 => 100755 obcore/math/linalg/eigen/Vector.h mode change 100644 => 100755 obcore/math/linalg/gsl/Matrix.cpp mode change 100644 => 100755 obcore/math/linalg/gsl/Matrix.h mode change 100644 => 100755 obcore/math/linalg/gsl/Vector.cpp mode change 100644 => 100755 obcore/math/linalg/gsl/Vector.h mode change 100644 => 100755 obcore/math/linalg/linalg.h.in mode change 100644 => 100755 obcore/math/mathbase.h mode change 100644 => 100755 obcore/scripting/LuaScriptManager.cpp mode change 100644 => 100755 obcore/scripting/LuaScriptManager.h mode change 100644 => 100755 obcore/statemachine/Agent.cpp mode change 100644 => 100755 obcore/statemachine/Agent.h mode change 100644 => 100755 obcore/statemachine/AgentModel.cpp mode change 100644 => 100755 obcore/statemachine/AgentModel.h mode change 100644 => 100755 obcore/statemachine/RobotModel.cpp mode change 100644 => 100755 obcore/statemachine/RobotModel.h mode change 100644 => 100755 obcore/statemachine/states/StateBase.cpp mode change 100644 => 100755 obcore/statemachine/states/StateBase.h mode change 100644 => 100755 obcore/statemachine/states/StateBaseModel.h mode change 100644 => 100755 obcore/statemachine/states/StateLua.cpp mode change 100644 => 100755 obcore/statemachine/states/StateLua.h mode change 100644 => 100755 obcore/statemachine/states/StatePing.cpp mode change 100644 => 100755 obcore/statemachine/states/StatePing.h mode change 100644 => 100755 obcore/statemachine/states/StatePong.cpp mode change 100644 => 100755 obcore/statemachine/states/StatePong.h mode change 100644 => 100755 obdevice/CMakeLists.txt mode change 100644 => 100755 obdevice/CamNano.cpp mode change 100644 => 100755 obdevice/CamNano.h mode change 100644 => 100755 obdevice/CloudFactory.cpp mode change 100644 => 100755 obdevice/CloudFactory.h mode change 100644 => 100755 obdevice/Kinect.cpp mode change 100644 => 100755 obdevice/Kinect.h mode change 100644 => 100755 obdevice/KinectPlayback.cpp mode change 100644 => 100755 obdevice/KinectPlayback.h mode change 100644 => 100755 obdevice/LaserDevice.h mode change 100644 => 100755 obdevice/OpenNiDevice.cpp mode change 100644 => 100755 obdevice/OpenNiDevice.h mode change 100644 => 100755 obdevice/ParentDevice3D.cpp mode change 100644 => 100755 obdevice/ParentDevice3D.h mode change 100644 => 100755 obdevice/PclCloudInterface.cpp mode change 100644 => 100755 obdevice/PclCloudInterface.h mode change 100644 => 100755 obdevice/SickLMS100.cpp mode change 100644 => 100755 obdevice/SickLMS100.h mode change 100644 => 100755 obdevice/UvcCam.cpp mode change 100644 => 100755 obdevice/UvcCam.h mode change 100644 => 100755 obdevice/UvcVirtualCam.cpp mode change 100644 => 100755 obdevice/UvcVirtualCam.h mode change 100644 => 100755 obgraphic/CMakeLists.txt mode change 100644 => 100755 obgraphic/CloudWidget.cpp mode change 100644 => 100755 obgraphic/CloudWidget.h mode change 100644 => 100755 obgraphic/IronPalette.h mode change 100644 => 100755 obgraphic/Obvious.h mode change 100644 => 100755 obgraphic/Obvious2D.cpp mode change 100644 => 100755 obgraphic/Obvious2D.h mode change 100644 => 100755 obgraphic/Obvious2DMap.cpp mode change 100644 => 100755 obgraphic/Obvious2DMap.h mode change 100644 => 100755 obgraphic/Obvious3D.cpp mode change 100644 => 100755 obgraphic/Obvious3D.h mode change 100644 => 100755 obgraphic/README mode change 100644 => 100755 obgraphic/VtkCloud.cpp mode change 100644 => 100755 obgraphic/VtkCloud.h mode change 100644 => 100755 obgraphic/ohm_logo.h mode change 100644 => 100755 obvision/CMakeLists.txt mode change 100644 => 100755 obvision/README mode change 100644 => 100755 obvision/mesh/TriangleMesh.cpp mode change 100644 => 100755 obvision/mesh/TriangleMesh.h mode change 100644 => 100755 obvision/normals/NormalsEstimator.cpp mode change 100644 => 100755 obvision/normals/NormalsEstimator.h mode change 100644 => 100755 obvision/planning/AStar.cpp mode change 100644 => 100755 obvision/planning/AStar.h mode change 100644 => 100755 obvision/planning/AStarMap.cpp mode change 100644 => 100755 obvision/planning/AStarMap.h mode change 100644 => 100755 obvision/planning/AStarNode.cpp mode change 100644 => 100755 obvision/planning/AStarNode.h mode change 100644 => 100755 obvision/planning/Obstacle.cpp mode change 100644 => 100755 obvision/planning/Obstacle.h mode change 100644 => 100755 obvision/ransac/RansacPrimitives.cpp mode change 100644 => 100755 obvision/ransac/RansacPrimitives.h mode change 100644 => 100755 obvision/reconstruct/Sensor.cpp mode change 100644 => 100755 obvision/reconstruct/Sensor.h mode change 100644 => 100755 obvision/reconstruct/grid/RayCastAxisAligned2D.cpp mode change 100644 => 100755 obvision/reconstruct/grid/RayCastAxisAligned2D.h mode change 100644 => 100755 obvision/reconstruct/grid/RayCastPolar2D.cpp mode change 100644 => 100755 obvision/reconstruct/grid/RayCastPolar2D.h mode change 100644 => 100755 obvision/reconstruct/grid/SensorPolar2D.cpp mode change 100644 => 100755 obvision/reconstruct/grid/SensorPolar2D.h mode change 100644 => 100755 obvision/reconstruct/grid/TsdGrid.cpp mode change 100644 => 100755 obvision/reconstruct/grid/TsdGrid.h mode change 100644 => 100755 obvision/reconstruct/grid/TsdGridBranch.cpp mode change 100644 => 100755 obvision/reconstruct/grid/TsdGridBranch.h mode change 100644 => 100755 obvision/reconstruct/grid/TsdGridComponent.cpp mode change 100644 => 100755 obvision/reconstruct/grid/TsdGridComponent.h mode change 100644 => 100755 obvision/reconstruct/grid/TsdGridPartition.cpp mode change 100644 => 100755 obvision/reconstruct/grid/TsdGridPartition.h mode change 100644 => 100755 obvision/reconstruct/reconstruct_defs.h mode change 100644 => 100755 obvision/reconstruct/space/RayCast3D.cpp mode change 100644 => 100755 obvision/reconstruct/space/RayCast3D.h mode change 100644 => 100755 obvision/reconstruct/space/RayCastAxisAligned3D.cpp mode change 100644 => 100755 obvision/reconstruct/space/RayCastAxisAligned3D.h create mode 100755 obvision/reconstruct/space/SensorPolar2DWith3DPose.cpp create mode 100755 obvision/reconstruct/space/SensorPolar2DWith3DPose.h mode change 100644 => 100755 obvision/reconstruct/space/SensorPolar3D.cpp mode change 100644 => 100755 obvision/reconstruct/space/SensorPolar3D.h mode change 100644 => 100755 obvision/reconstruct/space/SensorProjective3D.cpp mode change 100644 => 100755 obvision/reconstruct/space/SensorProjective3D.h mode change 100644 => 100755 obvision/reconstruct/space/TsdSpace.cpp mode change 100644 => 100755 obvision/reconstruct/space/TsdSpace.h mode change 100644 => 100755 obvision/reconstruct/space/TsdSpaceBranch.cpp mode change 100644 => 100755 obvision/reconstruct/space/TsdSpaceBranch.h mode change 100644 => 100755 obvision/reconstruct/space/TsdSpaceComponent.cpp mode change 100644 => 100755 obvision/reconstruct/space/TsdSpaceComponent.h mode change 100644 => 100755 obvision/reconstruct/space/TsdSpacePartition.cpp mode change 100644 => 100755 obvision/reconstruct/space/TsdSpacePartition.h mode change 100644 => 100755 obvision/registration/Trace.cpp mode change 100644 => 100755 obvision/registration/Trace.h mode change 100644 => 100755 obvision/registration/amcl/AdaptiveMonteCarloMatching.h mode change 100644 => 100755 obvision/registration/icp/ClosedFormEstimator2D.cpp mode change 100644 => 100755 obvision/registration/icp/ClosedFormEstimator2D.h mode change 100644 => 100755 obvision/registration/icp/IRigidEstimator.h mode change 100644 => 100755 obvision/registration/icp/Icp.cpp mode change 100644 => 100755 obvision/registration/icp/Icp.h mode change 100644 => 100755 obvision/registration/icp/IcpMultiInitIterator.cpp mode change 100644 => 100755 obvision/registration/icp/IcpMultiInitIterator.h mode change 100644 => 100755 obvision/registration/icp/PointToLineEstimator2D.cpp mode change 100644 => 100755 obvision/registration/icp/PointToLineEstimator2D.h mode change 100644 => 100755 obvision/registration/icp/PointToPlaneEstimator3D.cpp mode change 100644 => 100755 obvision/registration/icp/PointToPlaneEstimator3D.h mode change 100644 => 100755 obvision/registration/icp/PointToPointEstimator3D.cpp mode change 100644 => 100755 obvision/registration/icp/PointToPointEstimator3D.h mode change 100644 => 100755 obvision/registration/icp/assign/AnnPairAssignment.cpp mode change 100644 => 100755 obvision/registration/icp/assign/AnnPairAssignment.h mode change 100644 => 100755 obvision/registration/icp/assign/FlannPairAssignment.cpp mode change 100644 => 100755 obvision/registration/icp/assign/FlannPairAssignment.h mode change 100644 => 100755 obvision/registration/icp/assign/NaboPairAssignment.cpp mode change 100644 => 100755 obvision/registration/icp/assign/NaboPairAssignment.h mode change 100644 => 100755 obvision/registration/icp/assign/PairAssignment.cpp mode change 100644 => 100755 obvision/registration/icp/assign/PairAssignment.h mode change 100644 => 100755 obvision/registration/icp/assign/ProjectivePairAssignment.cpp mode change 100644 => 100755 obvision/registration/icp/assign/ProjectivePairAssignment.h mode change 100644 => 100755 obvision/registration/icp/assign/assignbase.h mode change 100644 => 100755 obvision/registration/icp/assign/filter/DistanceFilter.cpp mode change 100644 => 100755 obvision/registration/icp/assign/filter/DistanceFilter.h mode change 100644 => 100755 obvision/registration/icp/assign/filter/IPostAssignmentFilter.h mode change 100644 => 100755 obvision/registration/icp/assign/filter/IPreAssignmentFilter.h mode change 100644 => 100755 obvision/registration/icp/assign/filter/OcclusionFilter.cpp mode change 100644 => 100755 obvision/registration/icp/assign/filter/OcclusionFilter.h mode change 100644 => 100755 obvision/registration/icp/assign/filter/OutOfBoundsFilter2D.cpp mode change 100644 => 100755 obvision/registration/icp/assign/filter/OutOfBoundsFilter2D.h mode change 100644 => 100755 obvision/registration/icp/assign/filter/OutOfBoundsFilter3D.cpp mode change 100644 => 100755 obvision/registration/icp/assign/filter/OutOfBoundsFilter3D.h mode change 100644 => 100755 obvision/registration/icp/assign/filter/ProjectionFilter.cpp mode change 100644 => 100755 obvision/registration/icp/assign/filter/ProjectionFilter.h mode change 100644 => 100755 obvision/registration/icp/assign/filter/ReciprocalFilter.cpp mode change 100644 => 100755 obvision/registration/icp/assign/filter/ReciprocalFilter.h mode change 100644 => 100755 obvision/registration/icp/assign/filter/RobotFootprintFilter.cpp mode change 100644 => 100755 obvision/registration/icp/assign/filter/RobotFootprintFilter.h mode change 100644 => 100755 obvision/registration/icp/assign/filter/TrimmedFilter.cpp mode change 100644 => 100755 obvision/registration/icp/assign/filter/TrimmedFilter.h mode change 100644 => 100755 obvision/registration/icp/icp_def.h mode change 100644 => 100755 obvision/registration/ndt/Ndt.cpp mode change 100644 => 100755 obvision/registration/ndt/Ndt.h mode change 100644 => 100755 obvision/registration/ransacMatching/PDFMatching.cpp mode change 100644 => 100755 obvision/registration/ransacMatching/PDFMatching.h mode change 100644 => 100755 obvision/registration/ransacMatching/RandomMatching.cpp mode change 100644 => 100755 obvision/registration/ransacMatching/RandomMatching.h mode change 100644 => 100755 obvision/registration/ransacMatching/RandomNormalMatching.cpp mode change 100644 => 100755 obvision/registration/ransacMatching/RandomNormalMatching.h mode change 100644 => 100755 obvision/registration/ransacMatching/TSD_PDFMatching.cpp mode change 100644 => 100755 obvision/registration/ransacMatching/TSD_PDFMatching.h mode change 100644 => 100755 obvision/registration/ransacMatching/TwinPointMatching.cpp mode change 100644 => 100755 obvision/registration/ransacMatching/TwinPointMatching.h mode change 100644 => 100755 test/README.md mode change 100644 => 100755 test/gtest-1.7.0.zip mode change 100644 => 100755 test/obcore/CMakeLists.txt mode change 100644 => 100755 test/obcore/base/eigen-vs-gsl.cpp mode change 100644 => 100755 test/obcore/base/pointcloud.cpp mode change 100644 => 100755 test/obcore/math/MatrixTest.cpp mode change 100644 => 100755 test/obcore/math/QuaternionTest.cpp diff --git a/Doxyfile b/Doxyfile old mode 100644 new mode 100755 diff --git a/License.txt b/License.txt old mode 100644 new mode 100755 diff --git a/README.md b/README.md old mode 100644 new mode 100755 diff --git a/applications/CMakeLists.txt b/applications/CMakeLists.txt old mode 100644 new mode 100755 index 0a385fb..592b76f --- a/applications/CMakeLists.txt +++ b/applications/CMakeLists.txt @@ -98,6 +98,7 @@ ADD_EXECUTABLE(logging_example logging_example.cpp) ADD_EXECUTABLE(tsd_test tsd_test.cpp) ADD_EXECUTABLE(tsd_grid_test tsd_grid_test.cpp) ADD_EXECUTABLE(tsd_kinect tsd_kinect.cpp) +ADD_EXECUTABLE(tsd_testMineShaft tsd_testMineShaft.cpp) ADD_EXECUTABLE(astar_test astar_test.cpp) ADD_EXECUTABLE(statemachine_test statemachine_test.cpp) ADD_EXECUTABLE(ransac_cirle ransac_circle.cpp) @@ -132,6 +133,7 @@ TARGET_LINK_LIBRARIES(tsd_test ${VISIONLIBS} ${GRAPHICLIBS} ${C TARGET_LINK_LIBRARIES(tsd_grid_test ${VISIONLIBS} ${GRAPHICLIBS} ${CORELIBS}) TARGET_LINK_LIBRARIES(tsd_kinect ${VISIONLIBS} ${DEVICELIBS} ${GRAPHICLIBS} ${CORELIBS} ${XML_LIBRARIES}) TARGET_LINK_LIBRARIES(tsd_raycast_visualize ${VISIONLIBS} ${GRAPHICLIBS} ${CORELIBS}) +TARGET_LINK_LIBRARIES(tsd_testMineShaft ${VISIONLIBS} ${GRAPHICLIBS} ${CORELIBS}) TARGET_LINK_LIBRARIES(showCloud ${GRAPHICLIBS} ${CORELIBS}) TARGET_LINK_LIBRARIES(astar_test ${VISIONLIBS} ${CORELIBS}) TARGET_LINK_LIBRARIES(statemachine_test ${VISIONLIBS} ${CORELIBS}) diff --git a/applications/astar_test.cpp b/applications/astar_test.cpp old mode 100644 new mode 100755 diff --git a/applications/icp_interactive3D.cpp b/applications/icp_interactive3D.cpp old mode 100644 new mode 100755 diff --git a/applications/icp_matching2D.cpp b/applications/icp_matching2D.cpp old mode 100644 new mode 100755 diff --git a/applications/kinect.xml b/applications/kinect.xml old mode 100644 new mode 100755 diff --git a/applications/kinect_ir.xml b/applications/kinect_ir.xml old mode 100644 new mode 100755 diff --git a/applications/kinect_localize.cpp b/applications/kinect_localize.cpp old mode 100644 new mode 100755 diff --git a/applications/kinect_mesh_show.cpp b/applications/kinect_mesh_show.cpp old mode 100644 new mode 100755 diff --git a/applications/kinect_perspective.cpp b/applications/kinect_perspective.cpp old mode 100644 new mode 100755 diff --git a/applications/kinect_playback.cpp b/applications/kinect_playback.cpp old mode 100644 new mode 100755 diff --git a/applications/kinect_stream_show.cpp b/applications/kinect_stream_show.cpp old mode 100644 new mode 100755 diff --git a/applications/logging_example.cpp b/applications/logging_example.cpp old mode 100644 new mode 100755 diff --git a/applications/lua/config.lua b/applications/lua/config.lua old mode 100644 new mode 100755 diff --git a/applications/lua/function.lua b/applications/lua/function.lua old mode 100644 new mode 100755 diff --git a/applications/lua/functionWithCallback.lua b/applications/lua/functionWithCallback.lua old mode 100644 new mode 100755 diff --git a/applications/lua/statePa.lua b/applications/lua/statePa.lua old mode 100644 new mode 100755 diff --git a/applications/lua/statePi.lua b/applications/lua/statePi.lua old mode 100644 new mode 100755 diff --git a/applications/lua/statePo.lua b/applications/lua/statePo.lua old mode 100644 new mode 100755 diff --git a/applications/lua_call_function.cpp b/applications/lua_call_function.cpp old mode 100644 new mode 100755 diff --git a/applications/lua_callback_c.cpp b/applications/lua_callback_c.cpp old mode 100644 new mode 100755 diff --git a/applications/lua_read_config.cpp b/applications/lua_read_config.cpp old mode 100644 new mode 100755 diff --git a/applications/lua_statemachine.cpp b/applications/lua_statemachine.cpp old mode 100644 new mode 100755 diff --git a/applications/nanoStream.cpp b/applications/nanoStream.cpp old mode 100644 new mode 100755 diff --git a/applications/ndt_matching2D.cpp b/applications/ndt_matching2D.cpp old mode 100644 new mode 100755 diff --git a/applications/obvious3D_map.cpp b/applications/obvious3D_map.cpp old mode 100644 new mode 100755 diff --git a/applications/obvious3D_show.cpp b/applications/obvious3D_show.cpp old mode 100644 new mode 100755 diff --git a/applications/ransac_circle.cpp b/applications/ransac_circle.cpp old mode 100644 new mode 100755 diff --git a/applications/ransac_matching2D.cpp b/applications/ransac_matching2D.cpp old mode 100644 new mode 100755 diff --git a/applications/showCloud.cpp b/applications/showCloud.cpp old mode 100644 new mode 100755 diff --git a/applications/statemachine_test.cpp b/applications/statemachine_test.cpp old mode 100644 new mode 100755 diff --git a/applications/synthetic_pointcloud.cpp b/applications/synthetic_pointcloud.cpp old mode 100644 new mode 100755 diff --git a/applications/tsd_camboardNano.cpp b/applications/tsd_camboardNano.cpp old mode 100644 new mode 100755 diff --git a/applications/tsd_grid_lms100.cpp b/applications/tsd_grid_lms100.cpp old mode 100644 new mode 100755 diff --git a/applications/tsd_grid_test.cpp b/applications/tsd_grid_test.cpp old mode 100644 new mode 100755 diff --git a/applications/tsd_kinect.cpp b/applications/tsd_kinect.cpp old mode 100644 new mode 100755 diff --git a/applications/tsd_raycast_visualize.cpp b/applications/tsd_raycast_visualize.cpp old mode 100644 new mode 100755 diff --git a/applications/tsd_test.cpp b/applications/tsd_test.cpp old mode 100644 new mode 100755 diff --git a/applications/tsd_testMineShaft.cpp b/applications/tsd_testMineShaft.cpp new file mode 100755 index 0000000..1ffeffe --- /dev/null +++ b/applications/tsd_testMineShaft.cpp @@ -0,0 +1,269 @@ +#include +#include "obgraphic/Obvious3D.h" +#include "obcore/base/tools.h" +#include "obcore/base/System.h" +#include "obvision/reconstruct/space/TsdSpace.h" +#include "obvision/reconstruct/space/SensorProjective3D.h" +#include "obvision/reconstruct/space/RayCast3D.h" +#include "obvision/reconstruct/space/RayCastAxisAligned3D.h" +#include "obvision/reconstruct/space/SensorPolar2DWith3DPose.h" +#include "obcore/base/Logger.h" + +#include "obcore/math/mathbase.h" + +using namespace std; +using namespace obvious; + +#define square 0 + +Obvious3D* _viewer; +SensorPolar2DWith3DPose* _sensor; +TsdSpace* _space; +VtkCloud* _vcloud; +VtkCloud* _vcloud2; +vector _coords; +vector _rgbs; + +double NOISE_RANGE = 0.015; +double WALL_DISTANCE = 1; +int WOBBLE_RANGE_DEG = 5; +double Z_STEP_SIZE = 0.01 / 4; +double ANGULAR_RES_DEG = 5; + +void generateSyntheticData(SensorPolar2DWith3DPose& sensor) +{ + + int beams = sensor.getRealMeasurementSize(); + //double angularRes = sensor.getAngularResolution(); + //double minPhi = sensor.getPhiMin(); + double maxRange = sensor.getMaximumRange(); + //double minRange = sensor.getMaximumRange(); + //double lowReflectivityRange = sensor.getLowReflectivityRange(); + + // Sample data, to be replaced with real measurements + double* data = new double[beams]; + unsigned char* rgb = new unsigned char[beams * 3]; + + double coords[beams * 3]; + unsigned char rgb2[beams * 3]; + + Matrix T = sensor.getTransformation(); + + Matrix* rays = sensor.getNormalizedRayMap(1); + + for(int i = 0; i < beams; i++) + { + double noise = -NOISE_RANGE + ((double)rand() / RAND_MAX) * NOISE_RANGE * 2; +#if square + double n[2] = + { 0, 0}; + double n2[2] = + { 0, 0}; + + //wall on x + n[0] = std::abs(WALL_DISTANCE / (*rays)(0, i)); + //wall on y + n[1] = std::abs(WALL_DISTANCE / (*rays)(1, i)); + + //take nearest wall for distance + double min = n[0] < n[1] ? n[0] : n[1]; + min += noise; + + //check maxRange + data[i] = min <= maxRange ? min : NAN; + + //wall on x + n2[0] = std::abs((WALL_DISTANCE - 0.1) / (*rays)(0, i)); + //wall on y + n2[1] = std::abs((WALL_DISTANCE - 0.1) / (*rays)(1, i)); + + //take nearest wall for distance + double min2 = n2[0] < n2[1] ? n2[0] : n2[1]; + min2 += noise; + +#else + + double n[3]; + + //wall on x + n[0] = std::abs(WALL_DISTANCE * (*rays)(0, i)); + //wall on y + n[1] = std::abs(WALL_DISTANCE * (*rays)(1, i)); + //wall on z + n[2] = std::abs(WALL_DISTANCE * (*rays)(2, i)); + + double phi = atan(n[2] / (sqrt(n[0] * n[0] + n[1] * n[1]))); + + double min = 1 / cos(phi) + noise; + double min2 = min + 0.05; +#endif + + //color it red + rgb[i * 3 + 0] = 255; + + //check maxRange + data[i] = min <= maxRange ? min : NAN; + + if(data[i] != NAN && 1) + { + _coords.push_back(T(0, 3) + min2 * (*rays)(0, i)); + _coords.push_back(T(1, 3) + min2 * (*rays)(1, i)); + _coords.push_back(T(2, 3) + min2 * (*rays)(2, i)); + _rgbs.push_back(0); + _rgbs.push_back(0); + _rgbs.push_back(255); //blue + } + } + + sensor.setRealMeasurementData(data); + sensor.setRealMeasurementRGB(rgb); + sensor.setStandardMask(); +} + +void extractEulerAngleXYZ(Matrix t, double& rotXangle, double& rotYangle, double& rotZangle) +{ + rotXangle = atan2(-t(1, 2), t(2, 2)); + double cosYangle = sqrt(pow(t(0, 0), 2) + pow(t(0, 1), 2)); + rotYangle = atan2(t(0, 2), cosYangle); + double sinXangle = sin(rotXangle); + double cosXangle = cos(rotXangle); + rotZangle = atan2(cosXangle * t(1, 0) + sinXangle * t(2, 0), cosXangle * t(1, 1) + sinXangle * t(2, 1)); +} + +void pushNewData() +{ + + Matrix currentT = _sensor->getTransformation(); + double rotXangle, rotYangle, rotZangle; + extractEulerAngleXYZ(currentT, rotXangle, rotYangle, rotZangle); + + rotYangle = rotYangle / M_PI * 180; + rotYangle *= -1; + rotYangle += -WOBBLE_RANGE_DEG + rand() % (WOBBLE_RANGE_DEG * 2 + 1); + rotYangle = rotYangle * M_PI / 180; + + rotXangle = rotXangle / M_PI * 180; + rotXangle *= -1; + rotXangle += -WOBBLE_RANGE_DEG + rand() % (WOBBLE_RANGE_DEG * 2 + 1); + rotXangle = rotXangle * M_PI / 180; + + double tf[16] = {cos(rotYangle), sin(rotYangle) * sin(rotXangle), sin(rotYangle) * cos(rotXangle), 0, 0, + cos(rotXangle), -sin(rotXangle), 0, -sin(rotYangle), cos(rotYangle) * sin(rotXangle), cos(rotYangle) + * cos(rotXangle), 0, 0, 0, 0, 1}; + + Matrix T(4, 4); + T.setData(tf); + _sensor->transform(&T); + + currentT = _sensor->getTransformation(); + currentT(2, 3) += Z_STEP_SIZE; + _sensor->setTransformation(currentT); + + generateSyntheticData(*_sensor); + _space->pushForward(_sensor); +} + +void _cbRegNewImage(void) +{ + pushNewData(); + std::cout << __PRETTY_FUNCTION__ << "get t" << std::endl; + Matrix currentT = _sensor->getTransformation(); + std::cout << __PRETTY_FUNCTION__ << " shot t" << std::endl; + _viewer->showSensorPose(currentT); + + + // _vcloud2->setCoords(_coords.data(), _coords.size() / 3, 3, NULL); + // _vcloud2->setColors(_rgbs.data(), _coords.size(), 3); +//return; + unsigned int cnt; +std::cout << __PRETTY_FUNCTION__ << "allocate stuff" << std::endl; + unsigned int cells = _space->getXDimension() * _space->getYDimension() * _space->getZDimension(); + double* coords = new double[cells * 3]; + double* normals = new double[cells * 3]; + unsigned char* rgb = new unsigned char[cells * 3]; + RayCastAxisAligned3D raycaster; + std::cout << __PRETTY_FUNCTION__ << "raycast" << std::endl; + raycaster.calcCoords(_space, coords, NULL, rgb, &cnt); + //raycaster.calcCoords(_space, coords, NULL, NULL, &cnt); + + std::cout << __PRETTY_FUNCTION__ << " show coords" << std::endl; + _vcloud->setCoords(coords, cnt / 3, 3, NULL); + //_vcloud->setColors(rgb, cnt / 3, 3); + _viewer->update(); + + // delete[] coords; + // delete[] normals; +} + +int main(void) +{ + LOGMSG_CONF("tsd_test.log", Logger::file_off | Logger::screen_on, DBG_DEBUG, DBG_DEBUG); + + obfloat voxelSize = 0.01; + _space = new TsdSpace(voxelSize, LAYOUT_8x8x8, 256, 256, 512); + _space->setMaxTruncation(3.0 * voxelSize); + +// translation of sensor + obfloat tr[3]; + _space->getCentroid(tr); + tr[2] = 0.3; + +// rotation about y-axis of sensor + double theta = 0 * M_PI / 180; + + double tf[16] = {cos(theta), 0, sin(theta), tr[0], 0, 1, 0, tr[1], -sin(theta), 0, cos(theta), tr[2], 0, 0, 0, 1}; + Matrix T(4, 4); + T.setData(tf); + +// Sensor initialization + + int beams = 360 / ANGULAR_RES_DEG; + double angularResRad = deg2rad(ANGULAR_RES_DEG); + double minPhi = deg2rad(-180.0); + double maxRange = 3.0; + double minRange = 0.3; + double lowReflectivityRange = 0.5; + + _sensor = new SensorPolar2DWith3DPose(beams, angularResRad, minPhi, maxRange, minRange, lowReflectivityRange); + + _sensor->transform(&T); + + /*unsigned char* buffer = new unsigned char[space.getXDimension()*space.getYDimension()*3]; + for(unsigned int i=0; ibuildSliceImage(i, buffer); + serializePPM(path, buffer, _space->getXDimension(), _space->getYDimension(), 0); + } + delete[] buffer;*/ + + _vcloud = new VtkCloud; + _vcloud2 = new VtkCloud; + _viewer = new Obvious3D("TSD Cloud"); + + _viewer->addAxisAlignedCube(0, _space->getMaxX(), 0, _space->getMaxY(), 0, _space->getMaxZ()); + _viewer->showAxes(true); + _viewer->addCloud(_vcloud); + _viewer->addCloud(_vcloud2); + + for(int i = 0; i < 150; ++i) + { + pushNewData(); + if(!(i % 100)) + cout << i << endl; + } + _space->serializeSliceImages(Z); + cout << "finished" << endl; + _viewer->registerKeyboardCallback("space", _cbRegNewImage, "Register new image"); + _viewer->startRendering(); + +//delete[] coords; +//delete[] normals; +//if(rgb) +// delete[] rgb; + delete _viewer; + delete _sensor; + delete _space; +} + diff --git a/applications/tsd_xtion.cpp b/applications/tsd_xtion.cpp old mode 100644 new mode 100755 diff --git a/applications/uvccam_finddevice.cpp b/applications/uvccam_finddevice.cpp old mode 100644 new mode 100755 diff --git a/applications/uvccam_querycapabilities.cpp b/applications/uvccam_querycapabilities.cpp old mode 100644 new mode 100755 diff --git a/applications/uvccam_serialize.cpp b/applications/uvccam_serialize.cpp old mode 100644 new mode 100755 diff --git a/applications/uvccam_stream.cpp b/applications/uvccam_stream.cpp old mode 100644 new mode 100755 diff --git a/applications/uvcvirtualcam_serialize.cpp b/applications/uvcvirtualcam_serialize.cpp old mode 100644 new mode 100755 diff --git a/applications/xtion.xml b/applications/xtion.xml old mode 100644 new mode 100755 diff --git a/applications/xtionStream.cpp b/applications/xtionStream.cpp old mode 100644 new mode 100755 diff --git a/build/debug/CMakeLists.txt b/build/debug/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/build/release/CMakeLists.txt b/build/release/CMakeLists.txt old mode 100644 new mode 100755 index 1923923..fdfb80d --- a/build/release/CMakeLists.txt +++ b/build/release/CMakeLists.txt @@ -1,8 +1,12 @@ cmake_minimum_required(VERSION 2.6) + + SET(CMAKE_BUILD_TYPE Release) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS} -fopenmp -Wall -O2 -DNDEBUG -pipe -march=native") + + SET(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../../cmake) if (DEFINED ENV{OBVIOUSLY_ROOT}) @@ -11,6 +15,8 @@ else() message( FATAL_ERROR "OBVIOUSLY_ROOT variable is set") endif() + + #################### # Debian Packaging # #################### diff --git a/build/release/fixup_deb.sh.in b/build/release/fixup_deb.sh.in old mode 100644 new mode 100755 diff --git a/cmake/FindEigen.cmake b/cmake/FindEigen.cmake old mode 100644 new mode 100755 diff --git a/doxy.config b/doxy.config old mode 100644 new mode 100755 diff --git a/obcore/Axis.h b/obcore/Axis.h old mode 100644 new mode 100755 diff --git a/obcore/CMakeLists.txt b/obcore/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/obcore/Point3D.h b/obcore/Point3D.h old mode 100644 new mode 100755 diff --git a/obcore/base/CartesianCloud.cpp b/obcore/base/CartesianCloud.cpp old mode 100644 new mode 100755 diff --git a/obcore/base/CartesianCloud.h b/obcore/base/CartesianCloud.h old mode 100644 new mode 100755 diff --git a/obcore/base/CartesianCloudFactory.cpp b/obcore/base/CartesianCloudFactory.cpp old mode 100644 new mode 100755 diff --git a/obcore/base/CartesianCloudFactory.h b/obcore/base/CartesianCloudFactory.h old mode 100644 new mode 100755 diff --git a/obcore/base/Logger.cpp b/obcore/base/Logger.cpp old mode 100644 new mode 100755 diff --git a/obcore/base/Logger.h b/obcore/base/Logger.h old mode 100644 new mode 100755 diff --git a/obcore/base/Point.h b/obcore/base/Point.h old mode 100644 new mode 100755 index 02797ab..77a5c14 --- a/obcore/base/Point.h +++ b/obcore/base/Point.h @@ -36,6 +36,16 @@ struct Pixel struct Point { +// Point(const Point& point): +// x(point.x), +// y(point.y), +// z(point.z){} +// void operator+=(const Point& point) +// { +// x += point.x; +// y += point.y; +// z += point.z; +// } obfloat x; obfloat y; obfloat z; diff --git a/obcore/base/PointCloud.cpp b/obcore/base/PointCloud.cpp old mode 100644 new mode 100755 diff --git a/obcore/base/PointCloud.h b/obcore/base/PointCloud.h old mode 100644 new mode 100755 diff --git a/obcore/base/System.h b/obcore/base/System.h old mode 100644 new mode 100755 diff --git a/obcore/base/System.inl b/obcore/base/System.inl old mode 100644 new mode 100755 diff --git a/obcore/base/Time.cpp b/obcore/base/Time.cpp old mode 100644 new mode 100755 diff --git a/obcore/base/Time.h b/obcore/base/Time.h old mode 100644 new mode 100755 diff --git a/obcore/base/Timer.cpp b/obcore/base/Timer.cpp old mode 100644 new mode 100755 diff --git a/obcore/base/Timer.h b/obcore/base/Timer.h old mode 100644 new mode 100755 diff --git a/obcore/base/tools.cpp b/obcore/base/tools.cpp old mode 100644 new mode 100755 diff --git a/obcore/base/tools.h b/obcore/base/tools.h old mode 100644 new mode 100755 diff --git a/obcore/base/types.h b/obcore/base/types.h old mode 100644 new mode 100755 diff --git a/obcore/filter/BoundingBoxFilter.cpp b/obcore/filter/BoundingBoxFilter.cpp old mode 100644 new mode 100755 diff --git a/obcore/filter/BoundingBoxFilter.h b/obcore/filter/BoundingBoxFilter.h old mode 100644 new mode 100755 diff --git a/obcore/filter/CartesianFilter.cpp b/obcore/filter/CartesianFilter.cpp old mode 100644 new mode 100755 diff --git a/obcore/filter/CartesianFilter.h b/obcore/filter/CartesianFilter.h old mode 100644 new mode 100755 diff --git a/obcore/filter/EuclideanFilter.cpp b/obcore/filter/EuclideanFilter.cpp old mode 100644 new mode 100755 diff --git a/obcore/filter/EuclideanFilter.h b/obcore/filter/EuclideanFilter.h old mode 100644 new mode 100755 diff --git a/obcore/filter/EuclideanFilterVecD.cpp b/obcore/filter/EuclideanFilterVecD.cpp old mode 100644 new mode 100755 diff --git a/obcore/filter/EuclideanFilterVecD.h b/obcore/filter/EuclideanFilterVecD.h old mode 100644 new mode 100755 diff --git a/obcore/filter/Filter.h b/obcore/filter/Filter.h old mode 100644 new mode 100755 diff --git a/obcore/filter/FilterDistance.h b/obcore/filter/FilterDistance.h old mode 100644 new mode 100755 diff --git a/obcore/filter/NormalFilter.cpp b/obcore/filter/NormalFilter.cpp old mode 100644 new mode 100755 diff --git a/obcore/filter/NormalFilter.h b/obcore/filter/NormalFilter.h old mode 100644 new mode 100755 diff --git a/obcore/grid/GradientGrid.cpp b/obcore/grid/GradientGrid.cpp old mode 100644 new mode 100755 diff --git a/obcore/grid/GradientGrid.h b/obcore/grid/GradientGrid.h old mode 100644 new mode 100755 diff --git a/obcore/grid/Grid2D.cpp b/obcore/grid/Grid2D.cpp old mode 100644 new mode 100755 diff --git a/obcore/grid/Grid2D.h b/obcore/grid/Grid2D.h old mode 100644 new mode 100755 diff --git a/obcore/grid/HeightGrid.cpp b/obcore/grid/HeightGrid.cpp old mode 100644 new mode 100755 diff --git a/obcore/grid/HeightGrid.h b/obcore/grid/HeightGrid.h old mode 100644 new mode 100755 diff --git a/obcore/grid/ObstacleGrid.cpp b/obcore/grid/ObstacleGrid.cpp old mode 100644 new mode 100755 diff --git a/obcore/grid/ObstacleGrid.h b/obcore/grid/ObstacleGrid.h old mode 100644 new mode 100755 diff --git a/obcore/math/IntegratorSimpson.cpp b/obcore/math/IntegratorSimpson.cpp old mode 100644 new mode 100755 diff --git a/obcore/math/IntegratorSimpson.h b/obcore/math/IntegratorSimpson.h old mode 100644 new mode 100755 diff --git a/obcore/math/PID_Controller.cpp b/obcore/math/PID_Controller.cpp old mode 100644 new mode 100755 diff --git a/obcore/math/PID_Controller.h b/obcore/math/PID_Controller.h old mode 100644 new mode 100755 diff --git a/obcore/math/Quaternion.cpp b/obcore/math/Quaternion.cpp old mode 100644 new mode 100755 diff --git a/obcore/math/Quaternion.h b/obcore/math/Quaternion.h old mode 100644 new mode 100755 diff --git a/obcore/math/Trajectory.cpp b/obcore/math/Trajectory.cpp old mode 100644 new mode 100755 diff --git a/obcore/math/Trajectory.h b/obcore/math/Trajectory.h old mode 100644 new mode 100755 diff --git a/obcore/math/TransformationWatchdog.cpp b/obcore/math/TransformationWatchdog.cpp old mode 100644 new mode 100755 diff --git a/obcore/math/TransformationWatchdog.h b/obcore/math/TransformationWatchdog.h old mode 100644 new mode 100755 diff --git a/obcore/math/geometry.cpp b/obcore/math/geometry.cpp old mode 100644 new mode 100755 diff --git a/obcore/math/geometry.h b/obcore/math/geometry.h old mode 100644 new mode 100755 diff --git a/obcore/math/linalg/MatrixFactory.cpp b/obcore/math/linalg/MatrixFactory.cpp old mode 100644 new mode 100755 diff --git a/obcore/math/linalg/MatrixFactory.h b/obcore/math/linalg/MatrixFactory.h old mode 100644 new mode 100755 diff --git a/obcore/math/linalg/eigen/Matrix.cpp b/obcore/math/linalg/eigen/Matrix.cpp old mode 100644 new mode 100755 diff --git a/obcore/math/linalg/eigen/Matrix.h b/obcore/math/linalg/eigen/Matrix.h old mode 100644 new mode 100755 diff --git a/obcore/math/linalg/eigen/Vector.cpp b/obcore/math/linalg/eigen/Vector.cpp old mode 100644 new mode 100755 diff --git a/obcore/math/linalg/eigen/Vector.h b/obcore/math/linalg/eigen/Vector.h old mode 100644 new mode 100755 diff --git a/obcore/math/linalg/gsl/Matrix.cpp b/obcore/math/linalg/gsl/Matrix.cpp old mode 100644 new mode 100755 diff --git a/obcore/math/linalg/gsl/Matrix.h b/obcore/math/linalg/gsl/Matrix.h old mode 100644 new mode 100755 diff --git a/obcore/math/linalg/gsl/Vector.cpp b/obcore/math/linalg/gsl/Vector.cpp old mode 100644 new mode 100755 diff --git a/obcore/math/linalg/gsl/Vector.h b/obcore/math/linalg/gsl/Vector.h old mode 100644 new mode 100755 diff --git a/obcore/math/linalg/linalg.h.in b/obcore/math/linalg/linalg.h.in old mode 100644 new mode 100755 diff --git a/obcore/math/mathbase.h b/obcore/math/mathbase.h old mode 100644 new mode 100755 diff --git a/obcore/scripting/LuaScriptManager.cpp b/obcore/scripting/LuaScriptManager.cpp old mode 100644 new mode 100755 diff --git a/obcore/scripting/LuaScriptManager.h b/obcore/scripting/LuaScriptManager.h old mode 100644 new mode 100755 diff --git a/obcore/statemachine/Agent.cpp b/obcore/statemachine/Agent.cpp old mode 100644 new mode 100755 diff --git a/obcore/statemachine/Agent.h b/obcore/statemachine/Agent.h old mode 100644 new mode 100755 index 370d511..177a385 --- a/obcore/statemachine/Agent.h +++ b/obcore/statemachine/Agent.h @@ -80,6 +80,8 @@ class Agent */ void deletePersistantStates(); + StateBase* getCurrentState(void){return _currentState;} + private: unsigned int _ID; diff --git a/obcore/statemachine/AgentModel.cpp b/obcore/statemachine/AgentModel.cpp old mode 100644 new mode 100755 diff --git a/obcore/statemachine/AgentModel.h b/obcore/statemachine/AgentModel.h old mode 100644 new mode 100755 diff --git a/obcore/statemachine/RobotModel.cpp b/obcore/statemachine/RobotModel.cpp old mode 100644 new mode 100755 diff --git a/obcore/statemachine/RobotModel.h b/obcore/statemachine/RobotModel.h old mode 100644 new mode 100755 diff --git a/obcore/statemachine/states/StateBase.cpp b/obcore/statemachine/states/StateBase.cpp old mode 100644 new mode 100755 diff --git a/obcore/statemachine/states/StateBase.h b/obcore/statemachine/states/StateBase.h old mode 100644 new mode 100755 diff --git a/obcore/statemachine/states/StateBaseModel.h b/obcore/statemachine/states/StateBaseModel.h old mode 100644 new mode 100755 diff --git a/obcore/statemachine/states/StateLua.cpp b/obcore/statemachine/states/StateLua.cpp old mode 100644 new mode 100755 diff --git a/obcore/statemachine/states/StateLua.h b/obcore/statemachine/states/StateLua.h old mode 100644 new mode 100755 diff --git a/obcore/statemachine/states/StatePing.cpp b/obcore/statemachine/states/StatePing.cpp old mode 100644 new mode 100755 diff --git a/obcore/statemachine/states/StatePing.h b/obcore/statemachine/states/StatePing.h old mode 100644 new mode 100755 diff --git a/obcore/statemachine/states/StatePong.cpp b/obcore/statemachine/states/StatePong.cpp old mode 100644 new mode 100755 diff --git a/obcore/statemachine/states/StatePong.h b/obcore/statemachine/states/StatePong.h old mode 100644 new mode 100755 diff --git a/obdevice/CMakeLists.txt b/obdevice/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/obdevice/CamNano.cpp b/obdevice/CamNano.cpp old mode 100644 new mode 100755 diff --git a/obdevice/CamNano.h b/obdevice/CamNano.h old mode 100644 new mode 100755 diff --git a/obdevice/CloudFactory.cpp b/obdevice/CloudFactory.cpp old mode 100644 new mode 100755 diff --git a/obdevice/CloudFactory.h b/obdevice/CloudFactory.h old mode 100644 new mode 100755 diff --git a/obdevice/Kinect.cpp b/obdevice/Kinect.cpp old mode 100644 new mode 100755 diff --git a/obdevice/Kinect.h b/obdevice/Kinect.h old mode 100644 new mode 100755 diff --git a/obdevice/KinectPlayback.cpp b/obdevice/KinectPlayback.cpp old mode 100644 new mode 100755 diff --git a/obdevice/KinectPlayback.h b/obdevice/KinectPlayback.h old mode 100644 new mode 100755 diff --git a/obdevice/LaserDevice.h b/obdevice/LaserDevice.h old mode 100644 new mode 100755 diff --git a/obdevice/OpenNiDevice.cpp b/obdevice/OpenNiDevice.cpp old mode 100644 new mode 100755 diff --git a/obdevice/OpenNiDevice.h b/obdevice/OpenNiDevice.h old mode 100644 new mode 100755 diff --git a/obdevice/ParentDevice3D.cpp b/obdevice/ParentDevice3D.cpp old mode 100644 new mode 100755 diff --git a/obdevice/ParentDevice3D.h b/obdevice/ParentDevice3D.h old mode 100644 new mode 100755 diff --git a/obdevice/PclCloudInterface.cpp b/obdevice/PclCloudInterface.cpp old mode 100644 new mode 100755 diff --git a/obdevice/PclCloudInterface.h b/obdevice/PclCloudInterface.h old mode 100644 new mode 100755 diff --git a/obdevice/SickLMS100.cpp b/obdevice/SickLMS100.cpp old mode 100644 new mode 100755 diff --git a/obdevice/SickLMS100.h b/obdevice/SickLMS100.h old mode 100644 new mode 100755 diff --git a/obdevice/UvcCam.cpp b/obdevice/UvcCam.cpp old mode 100644 new mode 100755 diff --git a/obdevice/UvcCam.h b/obdevice/UvcCam.h old mode 100644 new mode 100755 diff --git a/obdevice/UvcVirtualCam.cpp b/obdevice/UvcVirtualCam.cpp old mode 100644 new mode 100755 diff --git a/obdevice/UvcVirtualCam.h b/obdevice/UvcVirtualCam.h old mode 100644 new mode 100755 diff --git a/obgraphic/CMakeLists.txt b/obgraphic/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/obgraphic/CloudWidget.cpp b/obgraphic/CloudWidget.cpp old mode 100644 new mode 100755 diff --git a/obgraphic/CloudWidget.h b/obgraphic/CloudWidget.h old mode 100644 new mode 100755 diff --git a/obgraphic/IronPalette.h b/obgraphic/IronPalette.h old mode 100644 new mode 100755 diff --git a/obgraphic/Obvious.h b/obgraphic/Obvious.h old mode 100644 new mode 100755 diff --git a/obgraphic/Obvious2D.cpp b/obgraphic/Obvious2D.cpp old mode 100644 new mode 100755 diff --git a/obgraphic/Obvious2D.h b/obgraphic/Obvious2D.h old mode 100644 new mode 100755 diff --git a/obgraphic/Obvious2DMap.cpp b/obgraphic/Obvious2DMap.cpp old mode 100644 new mode 100755 diff --git a/obgraphic/Obvious2DMap.h b/obgraphic/Obvious2DMap.h old mode 100644 new mode 100755 diff --git a/obgraphic/Obvious3D.cpp b/obgraphic/Obvious3D.cpp old mode 100644 new mode 100755 diff --git a/obgraphic/Obvious3D.h b/obgraphic/Obvious3D.h old mode 100644 new mode 100755 diff --git a/obgraphic/README b/obgraphic/README old mode 100644 new mode 100755 diff --git a/obgraphic/VtkCloud.cpp b/obgraphic/VtkCloud.cpp old mode 100644 new mode 100755 diff --git a/obgraphic/VtkCloud.h b/obgraphic/VtkCloud.h old mode 100644 new mode 100755 diff --git a/obgraphic/ohm_logo.h b/obgraphic/ohm_logo.h old mode 100644 new mode 100755 diff --git a/obvision/CMakeLists.txt b/obvision/CMakeLists.txt old mode 100644 new mode 100755 index f4cf4db..e0d40b6 --- a/obvision/CMakeLists.txt +++ b/obvision/CMakeLists.txt @@ -8,9 +8,16 @@ PROJECT(OBVISION) SET(OBVISION_VERSION_MAJOR 0) SET(OBVISION_VERSION_MINOR 1) +add_compile_options(-std=c++11) + INCLUDE_DIRECTORIES(.. /usr/include/eigen3) +find_package(PCL 1.7 REQUIRED) +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + add_library(obvision STATIC registration/icp/assign/PairAssignment.cpp registration/icp/assign/AnnPairAssignment.cpp @@ -51,6 +58,7 @@ add_library(obvision STATIC reconstruct/space/SensorPolar3D.cpp reconstruct/space/SensorProjective3D.cpp reconstruct/space/SensorPolar3D.cpp + reconstruct/space/SensorPolar2DWith3DPose.cpp reconstruct/space/TsdSpace.cpp reconstruct/space/TsdSpaceComponent.cpp reconstruct/space/TsdSpacePartition.cpp @@ -65,6 +73,10 @@ add_library(obvision STATIC ransac/RansacPrimitives.cpp ) +target_link_libraries(obvision + ${PCL_LIBRARIES} + ) + #################### ##### Packaging #### #################### diff --git a/obvision/README b/obvision/README old mode 100644 new mode 100755 diff --git a/obvision/mesh/TriangleMesh.cpp b/obvision/mesh/TriangleMesh.cpp old mode 100644 new mode 100755 diff --git a/obvision/mesh/TriangleMesh.h b/obvision/mesh/TriangleMesh.h old mode 100644 new mode 100755 diff --git a/obvision/normals/NormalsEstimator.cpp b/obvision/normals/NormalsEstimator.cpp old mode 100644 new mode 100755 diff --git a/obvision/normals/NormalsEstimator.h b/obvision/normals/NormalsEstimator.h old mode 100644 new mode 100755 diff --git a/obvision/planning/AStar.cpp b/obvision/planning/AStar.cpp old mode 100644 new mode 100755 diff --git a/obvision/planning/AStar.h b/obvision/planning/AStar.h old mode 100644 new mode 100755 diff --git a/obvision/planning/AStarMap.cpp b/obvision/planning/AStarMap.cpp old mode 100644 new mode 100755 diff --git a/obvision/planning/AStarMap.h b/obvision/planning/AStarMap.h old mode 100644 new mode 100755 diff --git a/obvision/planning/AStarNode.cpp b/obvision/planning/AStarNode.cpp old mode 100644 new mode 100755 diff --git a/obvision/planning/AStarNode.h b/obvision/planning/AStarNode.h old mode 100644 new mode 100755 diff --git a/obvision/planning/Obstacle.cpp b/obvision/planning/Obstacle.cpp old mode 100644 new mode 100755 diff --git a/obvision/planning/Obstacle.h b/obvision/planning/Obstacle.h old mode 100644 new mode 100755 diff --git a/obvision/ransac/RansacPrimitives.cpp b/obvision/ransac/RansacPrimitives.cpp old mode 100644 new mode 100755 diff --git a/obvision/ransac/RansacPrimitives.h b/obvision/ransac/RansacPrimitives.h old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/Sensor.cpp b/obvision/reconstruct/Sensor.cpp old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/Sensor.h b/obvision/reconstruct/Sensor.h old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/grid/RayCastAxisAligned2D.cpp b/obvision/reconstruct/grid/RayCastAxisAligned2D.cpp old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/grid/RayCastAxisAligned2D.h b/obvision/reconstruct/grid/RayCastAxisAligned2D.h old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/grid/RayCastPolar2D.cpp b/obvision/reconstruct/grid/RayCastPolar2D.cpp old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/grid/RayCastPolar2D.h b/obvision/reconstruct/grid/RayCastPolar2D.h old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/grid/SensorPolar2D.cpp b/obvision/reconstruct/grid/SensorPolar2D.cpp old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/grid/SensorPolar2D.h b/obvision/reconstruct/grid/SensorPolar2D.h old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/grid/TsdGrid.cpp b/obvision/reconstruct/grid/TsdGrid.cpp old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/grid/TsdGrid.h b/obvision/reconstruct/grid/TsdGrid.h old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/grid/TsdGridBranch.cpp b/obvision/reconstruct/grid/TsdGridBranch.cpp old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/grid/TsdGridBranch.h b/obvision/reconstruct/grid/TsdGridBranch.h old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/grid/TsdGridComponent.cpp b/obvision/reconstruct/grid/TsdGridComponent.cpp old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/grid/TsdGridComponent.h b/obvision/reconstruct/grid/TsdGridComponent.h old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/grid/TsdGridPartition.cpp b/obvision/reconstruct/grid/TsdGridPartition.cpp old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/grid/TsdGridPartition.h b/obvision/reconstruct/grid/TsdGridPartition.h old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/reconstruct_defs.h b/obvision/reconstruct/reconstruct_defs.h old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/space/RayCast3D.cpp b/obvision/reconstruct/space/RayCast3D.cpp old mode 100644 new mode 100755 index 6778313..e127a97 --- a/obvision/reconstruct/space/RayCast3D.cpp +++ b/obvision/reconstruct/space/RayCast3D.cpp @@ -146,6 +146,86 @@ void RayCast3D::calcCoordsFromCurrentPose(TsdSpace* space, Sensor* sensor, doubl #endif } +void RayCast3D::callCoordsFromCurrentPose(TsdSpace* space, const Eigen::Vector3f& pos, const stdVecEig3f& rays, stdVecEig3f* const coords, Eigen::Vector3f* const normal) +{ + if(1)//space->isInsideSpace(pos)) + { + _xmin = 0.0; + _ymin = 0.0; + _zmin = 0.0; + + _xmax = 10e9; + _ymax = 10e9; + _zmax = 10e9; + } + else + { + // prevent rays to be casted parallel to a plane outside of space + // if we are outside, we might loose some reprojections + _xmin = 10e9; + _ymin = 10e9; + _zmin = 10e9; + + _xmax = 0.0; + _ymax = 0.0; + _zmax = 0.0; + } + _idxMin = 0.0; + _idxMax = space->getMaxX() / space->getVoxelSize(); + +#pragma omp parallel + { + obfloat depth = 0.0; + obfloat c[3]; + obfloat n[3]; + unsigned char color[3] = {255, 255, 255}; + double* c_tmp = new double[rays.size() * 3]; + double* n_tmp = new double[rays.size() * 3]; + unsigned char* color_tmp = new unsigned char[rays.size() * 3]; + unsigned int size_tmp = 0; + obfloat obPos[3]; + + for(unsigned int i = 0; i < 3; i++) + obPos[i] = pos(i); + +#pragma omp for schedule(dynamic) + for(unsigned int iter = 0; iter < rays.size(); iter++) + { + obfloat ray[3]; + + for(unsigned int i = 0; i < 3; i++) + ray[i] = rays[iter](i); + + // Raycast returns with coordinates in world coordinate system + if(rayCastFromSensorPose(space, obPos, ray, c, n, color, &depth)) // Ray returned with coordinates + { + for (unsigned int i = 0; i < 3; i++) + { + c_tmp[size_tmp] = c[i]; + color_tmp[size_tmp] = color[i]; + n_tmp[size_tmp++] = n[i]; + } + } +// Eigen::Vector3f coordsDummy; +// rayCastDummy(space, pos, rays[iter], &coordsDummy, 1.0); +// coordsDummyVec.push_back(coordsDummy); + } +#pragma omp critical + { + for(unsigned int i = 0; i < size_tmp; i += 3) + { + coords->push_back(Eigen::Vector3f(c_tmp[i + 0], c_tmp[i + 1], c_tmp[i + 2])); + } +// for(auto& iter : coordsDummyVec) +// coords->push_back(iter); + // memcpy(&coords[*size], c_tmp, size_tmp*sizeof(double)); + // memcpy(&normals[*size], n_tmp, size_tmp*sizeof(double)); + // if(rgb) memcpy(&rgb[*size], color_tmp, size_tmp*sizeof(unsigned char)); + // *size += size_tmp; + } + } +} + void RayCast3D::calcCoordsFromCurrentPoseMask(TsdSpace* space, Sensor* sensor, double* coords, double* normals, unsigned char* rgb, bool* mask, unsigned int* size) { Timer t; @@ -345,6 +425,12 @@ void RayCast3D::calcCoordsFromCurrentPoseMask(TsdSpace* space, Sensor* sensor, d bool RayCast3D::rayCastFromSensorPose(TsdSpace* space, obfloat pos[3], obfloat ray[3], obfloat coordinates[3], obfloat normal[3], unsigned char rgb[3], obfloat* depth) { + // std::cout << __PRETTY_FUNCTION__ << "ray: " << std::endl; + //for(unsigned int i = 0; i < 3; i++) + // { + // std::cout << pos[0] << " " << pos[1] << " " << pos[2] << std::endl; + // std::cout << ray[0] << " " << ray[1] << " " << ray[2] << std::endl; + // } obfloat position[3]; int xDim = space->getXDimension(); @@ -366,6 +452,8 @@ bool RayCast3D::rayCastFromSensorPose(TsdSpace* space, obfloat pos[3], obfloat r obfloat maxSpaceCoord = (((obfloat)xDim)-1.5)*voxelSize; // Calculate minimum number of steps to reach bounds in each dimension + // std::cout << __PRETTY_FUNCTION__ << " calc min steps" << std::endl; + // std::cout << ray[0] << " " << ray[1] << " " << ray[2] << std::endl; if(ray[0]>10e-6) { xmin = (minSpaceCoord - pos[0]) / ray[0]; @@ -398,7 +486,7 @@ bool RayCast3D::rayCastFromSensorPose(TsdSpace* space, obfloat pos[3], obfloat r zmin = (maxSpaceCoord - pos[2]) / ray[2]; zmax = (minSpaceCoord - pos[2]) / ray[2]; } - + //std::cout << __PRETTY_FUNCTION__ << "lala index stuff" << std::endl; // At least the entry bounds of each dimension needs to be crossed obfloat idxMin = max(max(xmin, ymin), zmin); idxMin = ceil(idxMin); @@ -412,25 +500,41 @@ bool RayCast3D::rayCastFromSensorPose(TsdSpace* space, obfloat pos[3], obfloat r idxMax = min(idxMax, _idxMax); if (idxMin >= idxMax) + { + std::cout << __PRETTY_FUNCTION__ << "index stuff went wrong " << idxMin << " " << idxMax << std::endl; + std::cout << xmin << " " << xmax << " " << ymin << " " << ymax << " " << zmin << " " << zmax << std::endl; + std::cout << " minspacecoord " << minSpaceCoord << " maxspacecoord " << maxSpaceCoord << std::endl; + std::cout << " min " << min(xmax, ymax) << " minimi " << min(min(xmax, ymax), zmax) << std::endl; + std::cout << " max " << max(xmin, ymin) << " maxmax " << max(max(xmin, ymin), zmin) << std::endl; return false; + } #if PRINTSTATISTICS int idxMinTmp = idxMin; #endif // Traverse partitions roughly to clip minimum index + //std::cout << __PRETTY_FUNCTION__ << " rougly" << std::endl; obfloat partitionSize = space->getPartitionSize(); for(obfloat i=idxMin; iisPartitionInitialized(position)) { break; } else idxMin = i+1.0; + //std::cout << __PRETTY_FUNCTION__ << " not" << std::endl; } // Traverse in single steps with quick test @@ -449,13 +553,13 @@ bool RayCast3D::rayCastFromSensorPose(TsdSpace* space, obfloat pos[3], obfloat r #if PRINTSTATISTICS #pragma omp critical -{ - if((int)idxMin != idxMinTmp) - _skipped += (idxMin-idxMinTmp); -} + { + if((int)idxMin != idxMinTmp) + _skipped += (idxMin-idxMinTmp); + } #endif -obfloat tsd_prev; + obfloat tsd_prev; if(space->interpolateTrilinear(position, &tsd_prev)!=INTERPOLATE_SUCCESS) tsd_prev = NAN; @@ -463,7 +567,7 @@ obfloat tsd_prev; bool found = false; double i; - + // std::cout << __PRETTY_FUNCTION__ << "real traversing" << std::endl; for(i=idxMin; iinterpolateNormal(coordinates, normal)) + { + // std::cout << __PRETTY_FUNCTION__ << " interpolating normal failed" << std::endl; + // return false; + } + + //space->interpolateTrilinearRGB(coordinates, rgb); + + return true; +} + +bool RayCast3D::rayCastDummy(TsdSpace* space, const Eigen::Vector3f& pos, const Eigen::Vector3f& ray, Eigen::Vector3f* const coords, const float length) +{ + std::cout << __PRETTY_FUNCTION__ << "ray: " << std::endl; + //for(unsigned int i = 0; i < 3; i++) + // { + // std::cout << pos[0] << " " << pos[1] << " " << pos[2] << std::endl; + // std::cout << ray[0] << " " << ray[1] << " " << ray[2] << std::endl; + // } + obfloat position[3] = {pos(0), pos(1), pos(2)}; + + int xDim = space->getXDimension(); + obfloat voxelSize = space->getVoxelSize(); + + // Interpolation weight + obfloat interp; + + obfloat xmin = _xmin; + obfloat ymin = _ymin; + obfloat zmin = _zmin; + + obfloat xmax = _xmax; + obfloat ymax = _ymax; + obfloat zmax = _zmax; + + // Leave out outmost cells in order to prevent access to invalid neighbors + obfloat minSpaceCoord = 1.5*voxelSize; + obfloat maxSpaceCoord = (((obfloat)xDim)-1.5)*voxelSize; + + // Calculate minimum number of steps to reach bounds in each dimension + std::cout << __PRETTY_FUNCTION__ << " calc min steps" << std::endl; + // std::cout << ray[0] << " " << ray[1] << " " << ray[2] << std::endl; + if(ray[0]>10e-6) + { + xmin = (minSpaceCoord - pos(0)) / ray[0]; + xmax = (maxSpaceCoord - pos(0)) / ray[0]; + } + else if(ray[0]<-10e-6) + { + xmin = (maxSpaceCoord - pos(0)) / ray[0]; + xmax = (minSpaceCoord - pos(0)) / ray[0]; + } + + if(ray[1]>10e-6) + { + ymin = (minSpaceCoord - pos(1)) / ray[1]; + ymax = (maxSpaceCoord - pos(1)) / ray[1]; + } + else if(ray[1]<-10e-6) + { + ymin = (maxSpaceCoord - pos(1)) / ray[1]; + ymax = (minSpaceCoord - pos(1)) / ray[1]; + } + + if(ray[2]>10e-6) + { + zmin = (minSpaceCoord - pos(2)) / ray[2]; + zmax = (maxSpaceCoord - pos(2)) / ray[2]; + } + else if(ray[2]<-10e-6) + { + zmin = (maxSpaceCoord - pos(2)) / ray[2]; + zmax = (minSpaceCoord - pos(2)) / ray[2]; + } + //std::cout << __PRETTY_FUNCTION__ << "lala index stuff" << std::endl; + // At least the entry bounds of each dimension needs to be crossed + obfloat idxMin = max(max(xmin, ymin), zmin); + idxMin = ceil(idxMin); + + // No exit bound must be crossed + obfloat idxMax = min(min(xmax, ymax), zmax); + idxMax = floor(idxMax); + + // clip steps to sensor modalities, i.e., the working range + idxMin = max(idxMin, _idxMin); + idxMax = min(idxMax, _idxMax); + + if (idxMin >= idxMax) + { + std::cout << __PRETTY_FUNCTION__ << "index stuff went wrong " << idxMin << " " << idxMax << std::endl; + std::cout << xmin << " " << xmax << " " << ymin << " " << ymax << " " << zmin << " " << zmax << std::endl; + std::cout << " minspacecoord " << minSpaceCoord << " maxspacecoord " << maxSpaceCoord << std::endl; + std::cout << " min " << min(xmax, ymax) << " minimi " << min(min(xmax, ymax), zmax) << std::endl; + std::cout << " max " << max(xmin, ymin) << " maxmax " << max(max(xmin, ymin), zmin) << std::endl; return false; + } + +#if PRINTSTATISTICS + int idxMinTmp = idxMin; +#endif + + // Traverse partitions roughly to clip minimum index + //std::cout << __PRETTY_FUNCTION__ << " rougly" << std::endl; +// obfloat partitionSize = space->getPartitionSize(); +// for(obfloat i=idxMin; iisPartitionInitialized(position)) +// { +// break; +// } +// else +// idxMin = i+1.0; +// //std::cout << __PRETTY_FUNCTION__ << " not" << std::endl; +// } + + // Traverse in single steps with quick test +// for(double i=idxMin; igetTsd(position, &tsd); +// if(retval==INTERPOLATE_SUCCESS && fabs(tsd)<1.0) +// break; +// else +// idxMin++; +// } + +#if PRINTSTATISTICS +#pragma omp critical + { + if((int)idxMin != idxMinTmp) + _skipped += (idxMin-idxMinTmp); + } +#endif + +// obfloat tsd_prev; +// +// if(space->interpolateTrilinear(position, &tsd_prev)!=INTERPOLATE_SUCCESS) +// tsd_prev = NAN; + + bool found = false; + + double i; + std::cout << __PRETTY_FUNCTION__ << "imin max " << idxMin << " " << idxMax << std::endl; + for(i=idxMin; iinterpolateTrilinear(position, &tsd); + // if (retval!=INTERPOLATE_SUCCESS) + // { + // tsd_prev = NAN; + // position[0] += ray[0]; + // position[1] += ray[1]; + // position[2] += ray[2]; + // continue; + // } + // + // // check sign change + // if(tsd_prev > 0 && tsd < 0) + // { + // interp = tsd_prev / (tsd_prev - tsd); + // found = true; + // + // break; + // } - space->interpolateTrilinearRGB(coordinates, rgb); + position[0] += ray[0]; + position[1] += ray[1]; + position[2] += ray[2]; + const double currentLength = std::sqrt((i * ray[0]) * (i * ray[0]) + (i * ray[1]) * (i * ray[1]) + (i * ray[2]) * (i * ray[2])); + std::cout << __PRETTY_FUNCTION__ << "currentLength " << currentLength << std::endl; + if(currentLength > length) + { + std::cout << __PRETTY_FUNCTION__ << "aaaarg > " << length << std::endl; + // abort(); + break; + } + //tsd_prev = tsd; + } + +#if PRINTSTATISTICS +#pragma omp critical + { + _traversed += i-idxMin; + } +#endif + +// if(!found) +// { +// // std::cout << __PRETTY_FUNCTION__ << " nothing found" << std::endl; +// return false; +// } + + // interpolate between voxels when sign changes + std::cout << __PRETTY_FUNCTION__ << " give out coords " << std::endl; + (*coords)[0] = position[0];// + ray[0] * (interp-1.0); + (*coords)[1] = position[1];// + ray[1] * (interp-1.0); + (*coords)[2] = position[2];// + ray[2] * (interp-1.0); + + // if(!space->interpolateNormal(coordinates, normal)) + // { + // // std::cout << __PRETTY_FUNCTION__ << " interpolating normal failed" << std::endl; + // // return false; + // } + + //space->interpolateTrilinearRGB(coordinates, rgb); return true; } diff --git a/obvision/reconstruct/space/RayCast3D.h b/obvision/reconstruct/space/RayCast3D.h old mode 100644 new mode 100755 index 2511684..498004b --- a/obvision/reconstruct/space/RayCast3D.h +++ b/obvision/reconstruct/space/RayCast3D.h @@ -8,6 +8,8 @@ namespace obvious { +typedef std::vector > stdVecEig3f; + enum AXSPARMODE { X_AXS, @@ -44,7 +46,7 @@ class RayCast3D * @param size */ virtual void calcCoordsFromCurrentPose(TsdSpace* space, Sensor* sensor, double* coords, double* normals, unsigned char* rgb, unsigned int* size); - + virtual void callCoordsFromCurrentPose(TsdSpace* space, const Eigen::Vector3f& pos, const stdVecEig3f& rays, stdVecEig3f* const coords, Eigen::Vector3f* const normal); /** * * @param space @@ -78,6 +80,8 @@ class RayCast3D bool rayCastFromSensorPose(TsdSpace* space, obfloat pos[3], obfloat ray[3], obfloat coordinates[3], obfloat normal[3], unsigned char rgb[3], obfloat* depth); + bool rayCastDummy(TsdSpace* space, const Eigen::Vector3f& pos, const Eigen::Vector3f& ray, Eigen::Vector3f* const coords, const float length = 10.0); + obfloat _xmin; obfloat _ymin; obfloat _zmin; diff --git a/obvision/reconstruct/space/RayCastAxisAligned3D.cpp b/obvision/reconstruct/space/RayCastAxisAligned3D.cpp old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/space/RayCastAxisAligned3D.h b/obvision/reconstruct/space/RayCastAxisAligned3D.h old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/space/SensorPolar2DWith3DPose.cpp b/obvision/reconstruct/space/SensorPolar2DWith3DPose.cpp new file mode 100755 index 0000000..ead001e --- /dev/null +++ b/obvision/reconstruct/space/SensorPolar2DWith3DPose.cpp @@ -0,0 +1,138 @@ +#include "SensorPolar2DWith3DPose.h" +#include "obcore/math/mathbase.h" +#include "obcore/base/Logger.h" + +#include +#include + +namespace obvious +{ + +SensorPolar2DWith3DPose::SensorPolar2DWith3DPose(unsigned int size, double angularRes, double phiMin, double maxRange, double minRange, double lowReflectivityRange) : Sensor(3, maxRange, minRange, lowReflectivityRange) +{ + _width = size; + _height = 1; + _size = size; + + _data = new double[_size]; + _mask = new bool[_size]; + for(unsigned int i=0; i<_size; i++) + _mask[i] = true; + + _angularRes = angularRes; + _phiMin = phiMin; + + // smallest angle that lies in bounds (must be negative) + _phiLowerBound = -0.5*_angularRes + _phiMin; + + // if angle is too large, it might be projected with modulo 2 PI to a valid index + // upper bound -> phiMin + (size-1 + 0.5) * resolution + _phiUpperBound = _phiMin + (((double)size)-0.5)*_angularRes; + + if(_phiMin>=180.0) + { + LOGMSG(DBG_ERROR, "Valid minimal angle < 180 degree"); + } + + _rays = new Matrix(_dim, _size); + + for(unsigned int i=0; i<_size; i++) + { + const double phi = _phiMin + ((double)i) * _angularRes; + (*_rays)(0, i) = cos(phi); + (*_rays)(1, i) = sin(phi); + (*_rays)(2, i) = 0; + } + + _raysLocal = new Matrix(_dim, size); + *_raysLocal = *_rays; +} + +SensorPolar2DWith3DPose::~SensorPolar2DWith3DPose() +{ + delete [] _data; + delete [] _mask; + + delete _rays; + delete _raysLocal; +} + +void SensorPolar2DWith3DPose::setStandardMask() +{ + resetMask(); + maskZeroDepth(); + maskInvalidDepth(); + maskDepthDiscontinuity(deg2rad(3.0)); +} + +void SensorPolar2DWith3DPose::maskDepthDiscontinuity(double thresh) +{ + int radius = 1; + double cosphi; + double sinphi; + sincos(_angularRes, &sinphi, &cosphi); + for(int i=radius; i<((int)_size)-radius; i++) + { + double betamin = M_PI; + double a = _data[i]; + if(isinf(a)) continue; + for(int j=-radius; j<=radius; j++) + { + const double b = _data[i+j]; + if(isinf(b)) continue; + // law of cosines + double c = sqrt(a*a+b*b-2*a*b*cosphi); + + if(a>b) + { + // law of sines + const double beta = asin(b/c*sinphi); + + if(beta=_phiUpperBound) return -1; + return round((phi-_phiMin) /_angularRes); +} + +void SensorPolar2DWith3DPose::backProject(Matrix* M, int* indices, Matrix* T) +{ + Timer t; + Matrix PoseInv = getTransformation(); + PoseInv.invert(); + if(T) + PoseInv *= *T; + + Matrix coords2D = Matrix::multiply(PoseInv, *M, false, true); + + const double angularResInv = 1.0 / _angularRes; + for(unsigned int i=0; igetRows(); i++) + { + const double phi = atan2(coords2D(1,i), coords2D(0,i)); + if(phi<=_phiLowerBound) indices[i] = -2; + else if(phi>=_phiUpperBound) indices[i] = -1; + else indices[i] = round((phi-_phiMin) * angularResInv); + } +} + +} diff --git a/obvision/reconstruct/space/SensorPolar2DWith3DPose.h b/obvision/reconstruct/space/SensorPolar2DWith3DPose.h new file mode 100755 index 0000000..169171a --- /dev/null +++ b/obvision/reconstruct/space/SensorPolar2DWith3DPose.h @@ -0,0 +1,97 @@ +#ifndef SENSOR_POLAR_2D_WITH_3D_POSE_H +#define SENSOR_POLAR_2D_WITH_3D_POSE_H + +#include "obvision/reconstruct/Sensor.h" + +namespace obvious +{ + +/** + * @class SensorPolar2DWith3DPose + * @brief Generic class for 2D measurement units using polar sampling + * @author Stefan May + */ +class SensorPolar2DWith3DPose : public Sensor +{ +public: + + /** + * Standard constructor + * @param[in] beams number of beams + * @param[in] angularRes angular resolution, i.e. angle between beams in rad + * @param[in] phiMin minimum angle from which beams are counted positive counter-clockwisely (rad) + * @param[in] maxRange maximum range + * @param[in] minRange minimum range + */ + SensorPolar2DWith3DPose(unsigned int beams, double angularRes, double phiMin, double maxRange=INFINITY, double minRange=0.0, double lowReflectivityRange=INFINITY); + + /** + * Destructor + */ + ~SensorPolar2DWith3DPose(); + + /** + * Set standard measurement mask (measurement data need to be set before) + * Parameter for depth discontinuity is 1/60*M_PI + */ + void setStandardMask(); + + /** + * Mask measurements with acute angles to neighbors + * @param[in] thresh threshold in rad (meaningful values < 1/36*M_PI) + */ + void maskDepthDiscontinuity(double thresh); + + /** + * Assign an arbitrary 2D coordinate to a measurement beam + * @param[in] coordinate vector + * @return beam index, negative values are invalid, -1 -> exceeded upper bound, -2 -> exceeded lower bound + */ + int backProject(double data[2]); + + /** + * Parallel version of back projection + * @param[in] M matrix of homogeneous 2D coordinates + * @param[out] indices vector of beam indices, negative values are invalid, -1 -> exceeded upper bound, -2 -> exceeded lower bound + * @param[in] T temporary transformation matrix of coordinates + */ + void backProject(Matrix* M, int* indices, Matrix* T=NULL); + + /** + * Get angular resolution + * @return angular resolution + */ + double getAngularResolution() const { return _angularRes;}; + + /** + * Get the minimum angle + * @return minimum angle + */ + double getPhiMin() const { return _phiMin; }; + + /** + * Get lower bound of field of view + * @return lower bound + */ + double getPhiLowerBound(void) const { return _phiLowerBound; }; + + /** + * Get upper bound of field of view + * @return upper bound + */ + double getPhiUpperBound(void) const { return _phiUpperBound; }; + +private: + + double _angularRes; + + double _phiMin; + + double _phiLowerBound; + + double _phiUpperBound; +}; + +} + +#endif diff --git a/obvision/reconstruct/space/SensorPolar3D.cpp b/obvision/reconstruct/space/SensorPolar3D.cpp old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/space/SensorPolar3D.h b/obvision/reconstruct/space/SensorPolar3D.h old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/space/SensorProjective3D.cpp b/obvision/reconstruct/space/SensorProjective3D.cpp old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/space/SensorProjective3D.h b/obvision/reconstruct/space/SensorProjective3D.h old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/space/TsdSpace.cpp b/obvision/reconstruct/space/TsdSpace.cpp old mode 100644 new mode 100755 index d9c90e2..2349954 --- a/obvision/reconstruct/space/TsdSpace.cpp +++ b/obvision/reconstruct/space/TsdSpace.cpp @@ -6,6 +6,11 @@ #include "TsdSpace.h" #include "TsdSpaceBranch.h" #include "SensorProjective3D.h" +#include +#include +#include +#include +#include #include #include @@ -54,12 +59,21 @@ TsdSpace::TsdSpace(const double voxelSize, const EnumTsdSpaceLayout layoutPartit _partitionsInY = _cellsY/dimPartition; _partitionsInZ = _cellsZ/dimPartition; + _lutIndex2PartitionX = new int[_cellsX]; + _lutIndex2CellX = new int[_cellsX]; + _lutIndex2PartitionY = _lutIndex2PartitionX; + _lutIndex2CellY = _lutIndex2CellX; + _lutIndex2PartitionZ = _lutIndex2PartitionX; + _lutIndex2CellZ = _lutIndex2CellX; + _lutIndex2Partition = new int[_cellsX]; _lutIndex2Cell = new int[_cellsX]; for(unsigned int i=0; i<_cellsX; i++) { _lutIndex2Partition[i] = i / dimPartition; _lutIndex2Cell[i] = i % dimPartition; + _lutIndex2PartitionX[i] = i / dimPartition; + _lutIndex2CellX[i] = i % dimPartition; } _maxTruncation = 2.0*voxelSize; @@ -101,6 +115,90 @@ TsdSpace::TsdSpace(const double voxelSize, const EnumTsdSpaceLayout layoutPartit } } +TsdSpace::TsdSpace(const double voxelSize, const EnumTsdSpaceLayout layoutPartition, const unsigned int cellsX, const unsigned int cellsY, const unsigned int cellsZ) +{ + _voxelSize = voxelSize; + _invVoxelSize = 1.0 / _voxelSize; + + _layoutPartition = layoutPartition; + + // determine number of voxels in each dimension + _cellsX = cellsX; + _cellsY = cellsY; + _cellsZ = cellsZ; + + unsigned int dimPartition = 1u << layoutPartition; + + if(dimPartition > _cellsX) + { + LOGMSG(DBG_ERROR, "Insufficient partition size : " << dimPartition << "x" << dimPartition << "x" << dimPartition << " in " << _cellsX << "x" << _cellsY << "x" << _cellsZ << " space"); + return; + } + + + if (((cellsX % dimPartition) + (cellsY % dimPartition) + (cellsZ % dimPartition)) > 0){ + LOGMSG(DBG_ERROR, "Cells must be a multiple of partition size: partition size=" << dimPartition << " cellsX= " << cellsX << " cellsY= " << cellsY << " cellsZ= " << cellsZ); + return; + } + + + _partitionsInX = _cellsX / dimPartition; + _partitionsInY = _cellsY / dimPartition; + _partitionsInZ = _cellsZ / dimPartition; + + _lutIndex2PartitionX = new int[_cellsX]; + _lutIndex2CellX = new int[_cellsX]; + _lutIndex2PartitionY = new int[_cellsY]; + _lutIndex2CellY = new int[_cellsY]; + _lutIndex2PartitionZ = new int[_cellsZ]; + _lutIndex2CellZ = new int[_cellsZ]; + + for(unsigned int i = 0; i < _cellsX; i++) + { + _lutIndex2PartitionX[i] = i / dimPartition; + _lutIndex2CellX[i] = i % dimPartition; + } + + for(unsigned int i = 0; i < _cellsY; i++) + { + _lutIndex2PartitionY[i] = i / dimPartition; + _lutIndex2CellY[i] = i % dimPartition; + } + + for(unsigned int i = 0; i < _cellsZ; i++) + { + _lutIndex2PartitionZ[i] = i / dimPartition; + _lutIndex2CellZ[i] = i % dimPartition; + } + + _maxTruncation = 2.0 * voxelSize; + + LOGMSG(DBG_DEBUG, "Dimensions are (x/y/z) (" << _cellsX << "/" << _cellsY << "/" << _cellsZ << ")"); + LOGMSG(DBG_DEBUG, "Creating TsdVoxel Space..."); + + _minX = 0.0; + _maxX = ((obfloat)_cellsX + 0.5) * _voxelSize; + _minY = 0.0; + _maxY = ((obfloat)_cellsY + 0.5) * _voxelSize; + _minZ = 0.0; + _maxZ = ((obfloat)_cellsZ + 0.5) * _voxelSize; + + LOGMSG(DBG_DEBUG, "Allocating " << _partitionsInX << "x" << _partitionsInY << "x" << _partitionsInZ << " partitions"); + LOGMSG(DBG_DEBUG, "Spanning area: " << _maxX << " " << _maxY << " " << _maxZ << endl;) + System::allocate(_partitionsInZ, _partitionsInY, _partitionsInX, _partitions); + + for(int pz = 0; pz < _partitionsInZ; pz++) + { + for(int py = 0; py < _partitionsInY; py++) + { + for(int px = 0; px < _partitionsInX; px++) + { + _partitions[pz][py][px] = new TsdSpacePartition(px * dimPartition, py * dimPartition, pz * dimPartition, dimPartition, dimPartition, dimPartition, voxelSize); + } + } + } +} + TsdSpace::~TsdSpace(void) { delete _tree; @@ -182,9 +280,11 @@ bool TsdSpace::isPartitionInitialized(obfloat coord[3]) if (coord[2] < dz) z--; - int px = _lutIndex2Partition[x]; - int py = _lutIndex2Partition[y]; - int pz = _lutIndex2Partition[z]; + //std::cout << __PRETTY_FUNCTION__ << " x y z " << x << " " << y << " " << z << std::endl; + int px = _lutIndex2PartitionX[x]; + int py = _lutIndex2PartitionY[y]; + int pz = _lutIndex2PartitionZ[z]; + //std::cout << __PRETTY_FUNCTION__ << " px py zz " << px << " " << py << " " << pz << std::endl; return _partitions[pz][py][px]->isInitialized(); } @@ -196,6 +296,17 @@ bool TsdSpace::isInsideSpace(Sensor* sensor) return (coord[0]>_minX && coord[0]<_maxX && coord[1]>_minY && coord[1]<_maxY && coord[2]>_minZ && coord[2]<_maxZ); } +bool TsdSpace::isInsideSpace(const Eigen::Vector3f& pos) +{ + // std::cout << __PRETTY_FUNCTION__ << " minX " << _minX << " " << pos(0) << " " << _maxX << std::endl; + return (pos(0)>_minX && pos(0)<_maxX && pos(1)>_minY && pos(1)<_maxY && pos(2)>_minZ && pos(2)<_maxZ); +} + + +//{ +// +//} + void TsdSpace::push(Sensor* sensor) { Timer timer; @@ -253,7 +364,7 @@ void TsdSpace::push(Sensor* sensor) unsigned char* color = NULL; if(rgb) color = &(rgb[3*index]); - if(sd >= -_maxTruncation) + if((sd > -_maxTruncation) && (sd < _maxTruncation)) { part->init(); part->addTsd((*partCoords)(c, 0), (*partCoords)(c, 1), (*partCoords)(c, 2), sd, _maxTruncation, color); @@ -283,6 +394,398 @@ void TsdSpace::push(Sensor* sensor) LOGMSG(DBG_DEBUG, "Elapsed push: " << timer.elapsed() << "s, Initialized partitions: " << TsdSpacePartition::getInitializedPartitionSize()); } +void TsdSpace::push(stdVecEig3f& points) +{ + + std::vector* > slicesZ(this->getZDimension(), NULL); + std::cout << __PRETTY_FUNCTION__ << "sort points z-wise" << std::endl; + std::sort(points.begin(), points.end(), compareZ); + obfloat nVxlSize = this->getVoxelSize(); + std::vector* subCloud; + const unsigned int nBinAngleRes = 1000; + const double binRes = (2.0 * M_PI) / static_cast(nBinAngleRes); + //static_cast(binRes); + std::vector binAngles(this->getZDimension()); + std::cout << __PRETTY_FUNCTION__ << " create sliced point clouds" << std::endl; + unsigned int i = 0; + unsigned int n = 1; //todo: why 1...should be 0. 1 times vxlsize for first depth is ok but using it as idx will go wrong + obfloat centerSpace[3] = {0.0}; + this->getCentroid(centerSpace); + + while(1) + { + subCloud = new std::vector; + nVxlSize = static_cast(n) * this->getVoxelSize(); + binAngles[n - 1].resize(nBinAngleRes, Eigen::Vector3f(NAN, NAN, NAN)); + unsigned int naaas = 0; + while(1) + { + if(points[i](2) > nVxlSize) + { + //nVxlSize += this->getVoxelSize(); + n++; + break; + } + const double angleCurrent = std::atan2(points[i](1) - centerSpace[1], points[i](0) - centerSpace[0]); + const unsigned int idxBin = static_cast(std::floor((M_PI + angleCurrent) / binRes)); + std::cout << __PRETTY_FUNCTION__ << " idxbin = " << M_PI + angleCurrent << " / " << binRes << " = " << idxBin << std::endl; + if(idxBin < nBinAngleRes) + { + if(!std::isnan(binAngles[n - 1][idxBin](0))) + { + //std::cout << __PRETTY_FUNCTION__ << " Error! Bin idx " << idxBin << " already full" << binAngles[n - 1][idxBin](0) << std::endl; + //std::cout << M_PI + std::atan2(binAngles[n - 1][idxBin](1) - centerSpace[1], binAngles[n - 1][idxBin](0) - centerSpace[0]);// << std::endl; + //std::cout << M_PI + std::atan2(binAngles[n - 1][idxBin](1) - centerSpace[1], binAngles[n - 1][idxBin](0) - centerSpace[0]) / binRes << std::endl; + // std::cout << " na aaaah " << std::endl; + naaas++; + //abort(); + } + binAngles[n - 1][idxBin] = points[i]; + } + if(naaas) + std::cout << __PRETTY_FUNCTION__ << " naas " << naaas << " of " << subCloud->size() << std::endl; + subCloud->push_back(PointWithAngle(points[i], std::atan2(points[i](1) - centerSpace[1], points[i](0) - centerSpace[0]))); + // std::cout << __PRETTY_FUNCTION__ << "i " << i << " of " << points.size() << " n " << n <= points.size()) + break; + } + + if(subCloud->size()) + { + // std::cout << __PRETTY_FUNCTION__ << " Adding slice with " << subCloud->size() << " points at pos " << n << std::endl; + slicesZ[n - 1] = subCloud; + } + if(i >= points.size()) + break; + } + + // for(auto& iter : slicesZ) + // { + // if(iter) + // std::cout << __PRETTY_FUNCTION__ << " points in slice " << iter->size() << std::endl; + // } + + std::cout << __PRETTY_FUNCTION__ << " sort angle" << std::endl; + for(auto& iter : slicesZ) + { + if(iter) + std::sort(iter->begin(), iter->end(), compareAngle); + } + + Matrix* partCoords = TsdSpacePartition::getPartitionCoords(); + Matrix* cellCoordsHom = TsdSpacePartition::getCellCoordsHom(); + + unsigned int partSize = (_partitions[0][0][0])->getSize(); + // int* idx = new int[partSize]; + unsigned int ctr = 0; + std::cout << __PRETTY_FUNCTION__ << " start pushing" << std::endl; + + + // float arsch = centerSpace[0]; + // float arscharsch = centerSpace[1]; + + Timer timer; + timer.start(); + +#pragma omp parallel + { +#pragma omp for schedule(dynamic) + + for(int pz=0; pzgetPartitionsInZ(); pz++) + { + for(int py=0; py<_partitionsInY; py++) + { + for(int px=0; px<_partitionsInX; px++) + { + TsdSpacePartition* part = _partitions[pz][py][px]; + //std::cout << __PRETTY_FUNCTION__ << " px py pz" << px << " " << py << " " << pz << std::endl; + obfloat t[3]; + part->getCellCoordsOffset(t); + const unsigned int zSlicesIdcs = t[2]/this->getVoxelSize(); + bool found = false; + for(unsigned int i = zSlicesIdcs; i < zSlicesIdcs + static_cast(part->getDepth()) / this->getVoxelSize(); i++) + { + if(slicesZ[i]) + { + found = true; + break; + } + } + if(!found) + continue; + for(unsigned int c = 0; c < partSize; c++) + { + + Eigen::Vector3f coordCell; + for(unsigned int i = 0; i < 3; i++) + coordCell(i) = ((*cellCoordsHom)(c, i) + t[i]); + Eigen::Vector3f slice0(centerSpace[0], centerSpace[1], coordCell(2)); + const double angleCell = M_PI + std::atan2(coordCell(1) - centerSpace[1], coordCell(0) - centerSpace[0]); + const unsigned int idxBeam = static_cast(std::floor(angleCell / binRes)); + if(idxBeam >= nBinAngleRes) + { + std::cout << __PRETTY_FUNCTION__ << " idx " << idxBeam << " out of range " << nBinAngleRes << " (should not happen)" << std::endl; + continue; + } + + const double hyst = 0.05; //half a degree) + obfloat beam = NAN; + const unsigned int zIdx = coordCell(2) / this->getVoxelSize(); + if(!slicesZ[zIdx]) //todo: this continue should come earlier to save time + { + // std::cout << __PRETTY_FUNCTION__ << " empty slice " << std::endl; + continue; + } + // for(unsigned int i = 0; i < slicesZ[zIdx]->size(); i++) + // { + // if(std::abs(angleCell - (*slicesZ[zIdx])[i].angle) < hyst) + // { + // Eigen::Vector3f centerVec(centerSpace[0], centerSpace[1], centerSpace[2]); + // Eigen::Vector3f beamVec = (*slicesZ[zIdx])[i].point - slice0; + // beam = beamVec.norm(); + // break; + // } + // } + // if(std::isnan(beam)) + // { + // continue; + // } + Eigen::Vector3f beamVec; + if(std::isnan(binAngles[zIdx][idxBeam](0))) //interpolate between neighbours + { + unsigned int iterPos = zIdx; + unsigned int iterNeg = zIdx; + Eigen::Vector3f posNeighbour(NAN, NAN, NAN); + Eigen::Vector3f negNeighbour(NAN, NAN, NAN); + while(1) + { + // if(std::isnan(posNeighbour(0))) + iterPos++; + // if(std::isnan(negNeighbour(0))) + iterNeg--; + if((iterPos >= nBinAngleRes) || (iterNeg >= nBinAngleRes)) + { + break; + } + if(((iterPos - zIdx) > 20) || ((zIdx - iterNeg) > 20)) + break; + if(!std::isnan(binAngles[zIdx][iterNeg](0))) + negNeighbour = binAngles[zIdx][iterNeg]; + if(!std::isnan(binAngles[zIdx][iterPos](0))) + posNeighbour = binAngles[zIdx][iterPos]; + } + if(std::isnan(binAngles[zIdx][iterNeg](0)) && !std::isnan(binAngles[zIdx][iterPos](0))) + beamVec = binAngles[zIdx][iterPos]; + else if(!std::isnan(binAngles[zIdx][iterNeg](0)) && std::isnan(binAngles[zIdx][iterPos](0))) + beamVec = binAngles[zIdx][iterNeg]; + else if(!std::isnan(binAngles[zIdx][iterNeg](0)) && !std::isnan(binAngles[zIdx][iterPos](0))) + { + if((iterPos - zIdx) > (zIdx - iterNeg)) + beamVec = binAngles[zIdx][iterNeg]; + else if((iterPos - zIdx) < (zIdx - iterNeg)) + beamVec = binAngles[zIdx][iterPos]; + else + beamVec = (binAngles[zIdx][iterPos] + binAngles[zIdx][iterPos]) / 2.0; + } + else + continue; + beamVec -= slice0; + } + else + beamVec = binAngles[zIdx][idxBeam] - slice0; + beam = beamVec.norm(); + obfloat sd = beam - (coordCell - slice0).norm(); + if((sd >= -_maxTruncation))// && (sd <= _maxTruncation)) + { + part->init(); + part->addTsd((*partCoords)(c, 0), (*partCoords)(c, 1), (*partCoords)(c, 2), sd, _maxTruncation, NULL); +#pragma omp critical + { + ctr++; + } + + } + } + } + } + } + } + std::cout << __PRETTY_FUNCTION__ << "distances pushed " << ctr << " in " << timer.elapsed() << " s " << std::endl; + propagateBorders(); +} + +void TsdSpace::push(const std::vector& data, const unsigned int width, const unsigned int height, const Eigen::Vector3f& t, const double resDepth, + const double resHor) +{ + omp_lock_t writelock; + omp_init_lock(&writelock); + Timer timer; + timer.start(); + + Matrix* partCoords = TsdSpacePartition::getPartitionCoords(); + Matrix* cellCoordsHom = TsdSpacePartition::getCellCoordsHom(); + + unsigned int partSize = (_partitions[0][0][0])->getSize(); + unsigned int ctr = 0; + +#pragma omp parallel + { +#pragma omp for schedule(dynamic) + + for(int pz=0; pzgetPartitionsInZ(); pz++) + { + for(int py=0; py<_partitionsInY; py++) + { + for(int px=0; px<_partitionsInX; px++) + { + TsdSpacePartition* part = _partitions[pz][py][px]; + //std::cout << __PRETTY_FUNCTION__ << " px py pz" << px << " " << py << " " << pz << std::endl; + obfloat partOffset[3]; + part->getCellCoordsOffset(partOffset); + + for(unsigned int c = 0; c < partSize; c++) + { + Eigen::Vector3f coordCell; + for(unsigned int i = 0; i < 3; i++) + coordCell(i) = ((*cellCoordsHom)(c, i) + partOffset[i]); + Eigen::Vector3f sliceCenter(t(0), t(1), coordCell(2)); + const double angleCell = M_PI + std::atan2(coordCell(1) - t(1), coordCell(0) - t(0)); + const unsigned int idxBeam = static_cast(std::floor(angleCell / resHor)); + + if(idxBeam >= width) + { + std::cout << __PRETTY_FUNCTION__ << " idx(h) " << idxBeam << " out of range " << width << " (should not happen)" << std::endl; + continue; + } + const unsigned int zIdx = static_cast(std::floor(coordCell(2) / resDepth)); + + if(zIdx >= height) + { + // std::cout << __PRETTY_FUNCTION__ << " idx(v) " << zIdx << " out of range " << height << std::endl; + continue; + } + + stdVecEig3f beams = data[zIdx * width + idxBeam]; //todo: inefficient. User ptr or ref instead + Eigen::Vector3f beam(0.0, 0.0, 0.0); + if(!beams.size()) + { + Eigen::Vector3f mean(0.0, 0.0, 0.0); + //stdVecEig3f subCluster; + unsigned int meanCtr = 0; + unsigned int fieldCtr = 0; + for(unsigned int i = zIdx - 5; i <= zIdx + 5; i++) + { + for(unsigned int j = idxBeam - 5; j <= idxBeam + 5; j++) + { + if((i >= height) || (j >= width)) + continue; + if(!data[i * width + j].size()) + continue; + if(data[i * width + j].size() >= 1) + { + for(auto& iter : data[i * width + j]) + { + if((coordCell - iter).norm() > 0.3) + continue; + mean += iter; + // subCluster.push_back(iter); + meanCtr++; + } + fieldCtr++; + } + else if(data[i * width + j].size() == 1) + { + mean += *data[i * width + j].begin(); + // subCluster.push_back(*data[i * width + j].begin()); + meanCtr++; + fieldCtr++; + } + + } + } + + if(fieldCtr < 1) + continue; + else + { + // pcl::PointCloud::Ptr subcloud(new pcl::PointCloud); + // subcloud->resize(subCluster.size()); + // for(unsigned int i = 0; i < subCluster.size(); i++) + // subcloud->points[i] = pcl::PointXYZ(subCluster[i](0), subCluster[i](1), subCluster[i](2)); + // + // pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + // tree->setInputCloud(subcloud); + // + // std::vector cluster_indices; + // pcl::EuclideanClusterExtraction ec; + // ec.setClusterTolerance(5.0); + // ec.setMinClusterSize (3); + // ec.setMaxClusterSize (200); + // ec.setSearchMethod(tree); + // ec.setInputCloud(subcloud); + // ec.extract(cluster_indices); + //// pcl::StatisticalOutlierRemoval sor; + //// pcl::PointCloud::Ptr input = subcloud.makeShared(); + //// sor.setInputCloud(input); + //// sor.setMeanK(5); + //// sor.setStddevMulThresh(5.0); + //// sor.filter(subcloud); + // pcl::CentroidPoint centroid; + // for(auto& iter : subcloud->points) + // { + // centroid.add(iter); + // } + // pcl::PointXYZ pclMean; + // centroid.get(pclMean); + // + // beam = Eigen::Vector3f(pclMean.x, pclMean.y, pclMean.z); + beam = mean / static_cast(meanCtr); + // std::cout << " fieldctr = " << fieldCtr << std::endl; + } + } + else if(beams.size() > 1) + { + for(auto& iter : beams) + beam += iter; + beam /= static_cast(beams.size()); + } + else + beam = *beams.begin(); + obfloat sd = (beam - sliceCenter).norm() - (coordCell - sliceCenter).norm(); + if((sd > -2.0 * _maxTruncation))// && (sd < 2.0 * _maxTruncation)) + { + part->init(); + part->addTsd((*partCoords)(c, 0), (*partCoords)(c, 1), (*partCoords)(c, 2), sd, _maxTruncation, NULL); + //#pragma omp critical + omp_set_lock(&writelock); + // { + ctr++; + // if(beams.size() > 1) + // { + // Eigen::Vector3f mean; + // for(auto& iter : beams) + // { + // mean += iter; + // std::cout << iter(0) << " " << iter(1) << " " << iter(2) << " "; + // } + // mean /= static_cast(beams.size()); + // std::cout << mean(0) << " " << mean(1) << " " << mean(2); + // std::cout << std::endl; + // std::cout << std::endl; + // } + // } + omp_unset_lock(&writelock); + } + } + } + } + } + } + omp_destroy_lock(&writelock); + std::cout << __PRETTY_FUNCTION__ << "distances pushed " << ctr << " in " << timer.elapsed() << " s " << std::endl; + propagateBorders(); +} + void TsdSpace::pushTree(Sensor* sensor) { Timer timer; @@ -370,6 +873,157 @@ void TsdSpace::pushTree(Sensor* sensor) LOGMSG(DBG_DEBUG, "Elapsed push: " << timer.elapsed() << "s, Initialized partitions: " << TsdSpacePartition::getInitializedPartitionSize()); } +void TsdSpace::pushForward(Sensor* const sensor) +{ + Timer timer; + timer.start(); + + double* data = sensor->getRealMeasurementData(); + unsigned char* rgbs = sensor->getRealMeasurementRGB(); + bool* mask = sensor->getRealMeasurementMask(); + int dim = 3; + //unsigned char* rgb = sensor->getRealMeasurementRGB(); + + obfloat tr[dim]; + sensor->getPosition(tr); + + //Matrix* partCoords = TsdSpacePartition::getPartitionCoords(); + //Matrix* cellCoordsHom = TsdSpacePartition::getCellCoordsHom(); + + int rayCount = sensor->getRealMeasurementSize(); + + Matrix* rays = sensor->getNormalizedRayMap(1); + + TsdSpacePartition* part = _partitions[0][0][0]; + unsigned int partSize[3] = {part->getWidth(),part->getHeight(),part->getDepth()}; + + + for(int zTest = 0; zTest < 1; ++zTest) + { + // 1) for each measurement in sensor, take distance and normal + for(int beam = 0; beam < rayCount; ++beam) + { + if(isinf(data[beam]) || !mask[beam]) + { + continue; + } + + unsigned char rgb[3] = {rgbs[beam * 3], rgbs[beam * 3 + 1], rgbs[beam * 3 + 2]}; + + // 2) addTsd((*partCoords)(c, 0), (*partCoords)(c, 1), (*partCoords)(c, 2), sd, _maxTruncation, color); + // to cells along ray [-truncation radius; truncation radius]: + + obfloat crd[dim]; + obfloat beamCrd[dim]; + for(int i = 0; i < dim; ++i) + { + crd[i] = (*rays)(i, beam) * data[beam] + tr[i]; + //go back -_maxTruncation on beam: + beamCrd[i] = (crd[i] + (*rays)(i, beam) * (-_maxTruncation)); + } + + //Stepsize for moving on beam + double stepSize = _voxelSize / 2; + int steps = _maxTruncation / stepSize; + + for(int i = 0; i < steps * 2; ++i) + { + // => project measurement to cell index + int vIdx[dim]; + int pIdx[dim]; + int cIdx[dim]; + + for(int j = 0; j < dim; ++j) + { + vIdx[j] = (int)(beamCrd[j] / _voxelSize + 0.5); + pIdx[j] = vIdx[j] / partSize[j]; + cIdx[j] = vIdx[j] % partSize[j]; + } + + + part = _partitions[pIdx[2]][pIdx[1]][pIdx[0]]; + + obfloat sd = data[beam] - euklideanDistance(beamCrd, tr, 3); + + part->init(); + part->addTsd(cIdx[0], cIdx[1], cIdx[2], sd, _maxTruncation, rgb); +#if PRINTSTATISTICS +#pragma omp critical + { + _distancesPushed++; + } +#endif + + for(int j = 0; j < dim; ++j) + { + beamCrd[j] += (*rays)(j, beam) * stepSize; + } + } + } + tr[2] += _voxelSize; + } + + propagateBorders(); + +#if PRINTSTATISTICS + LOGMSG(DBG_DEBUG, "Distances pushed: " << _distancesPushed); +#endif + + LOGMSG(DBG_DEBUG, "Elapsed push: " << timer.elapsed() << "s, Initialized partitions: " << TsdSpacePartition::getInitializedPartitionSize()); +} + +void TsdSpace::pushForward(const stdVecEig3f& points) +{ + std::cout << __PRETTY_FUNCTION__ << "hello" << std::endl; + TsdSpacePartition* part = _partitions[0][0][0]; + unsigned int partSize[3] = {part->getWidth(),part->getHeight(),part->getDepth()}; + std::cout << __PRETTY_FUNCTION__ << " salllutt" << std::endl; + unsigned int ctr = 0; + for(stdVecEig3f::const_iterator iter = points.begin(); iter < points.end(); iter++) + { + Eigen::Vector3f pos(0.0, 0.0, iter->z()); //(*iter)(2) + const Eigen::Vector3f org = pos; + Eigen::Vector3f dir(iter->x(), iter->y(), 0.0); + dir = (dir / dir.norm()) * this->getVoxelSize() / 2.0; + while(1) + { + std::cout << __PRETTY_FUNCTION__ << " hello again" << std::endl; + unsigned int vIdx[3]; + unsigned int pIdx[3]; + unsigned int cIdx[3]; + for(int j = 0; j < 3; ++j) + { + vIdx[j] = static_cast(std::floor(pos(j) / _voxelSize)); + pIdx[j] = vIdx[j] / partSize[j]; + cIdx[j] = vIdx[j] % partSize[j]; + std::cout << __PRETTY_FUNCTION__ << "pos v p c " << pos(j) << " " << vIdx[j] << " " << pIdx[j] << " " << cIdx[j] << std::endl; + } + pos += dir; + + part = _partitions[pIdx[2]][pIdx[1]][pIdx[0]]; + obfloat sd = iter->norm() - pos.norm(); + if((sd > 2.0 * _maxTruncation) || (sd < -2.0 * _maxTruncation)) + continue; + std::cout << __PRETTY_FUNCTION__ << " sd " << sd << std::endl; + part->init(); + part->addTsd(cIdx[0], cIdx[1], cIdx[2], sd, _maxTruncation, NULL); + ctr++; + if(sd < 2.0 * -_maxTruncation) + break; + } + + // Point vec; //start vector in zaxis + // vec.x = 0.0;//iter->x; + // vec.y = 0.0;//iter->y; + // vec.z = iter->z; + // Point dirVec = iter; + // dirVec.z = 0.0; + // dirVec = + } + std::cout << " pushed " << ctr << " points" << std::endl; + propagateBorders(); +} + void TsdSpace::pushRecursion(Sensor* sensor, obfloat pos[3], TsdSpaceComponent* comp, vector &partitionsToCheck) { if(comp->isInRange(pos, sensor, _maxTruncation)) @@ -642,18 +1296,20 @@ EnumTsdSpaceInterpolate TsdSpace::interpolateTrilinear(obfloat coord[3], obfloat int xIdx; int yIdx; int zIdx; - if(!coord2Index(coord, &xIdx, &yIdx, &zIdx, &dx, &dy, &dz)) return INTERPOLATE_INVALIDINDEX; + if(!coord2Index(coord, &xIdx, &yIdx, &zIdx, &dx, &dy, &dz)) + return INTERPOLATE_INVALIDINDEX; - int px = _lutIndex2Partition[xIdx]; - int py = _lutIndex2Partition[yIdx]; - int pz = _lutIndex2Partition[zIdx]; + int px = _lutIndex2PartitionX[xIdx]; + int py = _lutIndex2PartitionY[yIdx]; + int pz = _lutIndex2PartitionZ[zIdx]; TsdSpacePartition* part = _partitions[pz][py][px]; - if(!part->isInitialized()) return INTERPOLATE_EMPTYPARTITION; + if(!part->isInitialized()) + return INTERPOLATE_EMPTYPARTITION; - int x = _lutIndex2Cell[xIdx]; - int y = _lutIndex2Cell[yIdx]; - int z = _lutIndex2Cell[zIdx]; + int x = _lutIndex2CellX[xIdx]; + int y = _lutIndex2CellY[yIdx]; + int z = _lutIndex2CellZ[zIdx]; obfloat wx = fabs((coord[0] - dx) * _invVoxelSize); obfloat wy = fabs((coord[1] - dy) * _invVoxelSize); @@ -661,7 +1317,8 @@ EnumTsdSpaceInterpolate TsdSpace::interpolateTrilinear(obfloat coord[3], obfloat *tsd = part->interpolateTrilinear(x, y, z, wx, wy, wz); - if(isnan(*tsd)) return INTERPOLATE_ISNAN; + if(isnan(*tsd)) + return INTERPOLATE_ISNAN; return INTERPOLATE_SUCCESS; } @@ -675,22 +1332,25 @@ EnumTsdSpaceInterpolate TsdSpace::getTsd(obfloat coord[3], obfloat* tsd) int xIdx; int yIdx; int zIdx; - if(!coord2Index(coord, &xIdx, &yIdx, &zIdx, &dx, &dy, &dz)) return INTERPOLATE_INVALIDINDEX; + if(!coord2Index(coord, &xIdx, &yIdx, &zIdx, &dx, &dy, &dz)) + return INTERPOLATE_INVALIDINDEX; - int px = _lutIndex2Partition[xIdx]; - int py = _lutIndex2Partition[yIdx]; - int pz = _lutIndex2Partition[zIdx]; + int px = _lutIndex2PartitionX[xIdx]; + int py = _lutIndex2PartitionY[yIdx]; + int pz = _lutIndex2PartitionZ[zIdx]; TsdSpacePartition* part = _partitions[pz][py][px]; - if(!part->isInitialized()) return INTERPOLATE_EMPTYPARTITION; + if(!part->isInitialized()) + return INTERPOLATE_EMPTYPARTITION; - int x = _lutIndex2Cell[xIdx]; - int y = _lutIndex2Cell[yIdx]; - int z = _lutIndex2Cell[zIdx]; + int x = _lutIndex2CellX[xIdx]; + int y = _lutIndex2CellY[yIdx]; + int z = _lutIndex2CellZ[zIdx]; *tsd = (*part)(z, y, x); - if(isnan(*tsd)) return INTERPOLATE_ISNAN; + if(isnan(*tsd)) + return INTERPOLATE_ISNAN; return INTERPOLATE_SUCCESS; } @@ -704,18 +1364,20 @@ EnumTsdSpaceInterpolate TsdSpace::interpolateTrilinearRGB(obfloat coord[3], unsi int xIdx; int yIdx; int zIdx; - if(!coord2Index(coord, &xIdx, &yIdx, &zIdx, &dx, &dy, &dz)) return INTERPOLATE_INVALIDINDEX; + if(!coord2Index(coord, &xIdx, &yIdx, &zIdx, &dx, &dy, &dz)) + return INTERPOLATE_INVALIDINDEX; - int px = _lutIndex2Partition[xIdx]; - int py = _lutIndex2Partition[yIdx]; - int pz = _lutIndex2Partition[zIdx]; + int px = _lutIndex2PartitionX[xIdx]; + int py = _lutIndex2PartitionY[yIdx]; + int pz = _lutIndex2PartitionZ[zIdx]; TsdSpacePartition* part = _partitions[pz][py][px]; - if(!part->isInitialized()) return INTERPOLATE_EMPTYPARTITION; + if(!part->isInitialized()) + return INTERPOLATE_EMPTYPARTITION; - int x = _lutIndex2Cell[xIdx]; - int y = _lutIndex2Cell[yIdx]; - int z = _lutIndex2Cell[zIdx]; + int x = _lutIndex2CellX[xIdx]; + int y = _lutIndex2CellY[yIdx]; + int z = _lutIndex2CellZ[zIdx]; double wx = fabs((coord[0] - dx) * _invVoxelSize); double wy = fabs((coord[1] - dy) * _invVoxelSize); @@ -723,14 +1385,14 @@ EnumTsdSpaceInterpolate TsdSpace::interpolateTrilinearRGB(obfloat coord[3], unsi unsigned char pRGB[8][3]; - part->getRGB(z+0, y+0, x+0, pRGB[0]); - part->getRGB(z+1, y+0, x+0, pRGB[1]); - part->getRGB(z+0, y+1, x+0, pRGB[2]); - part->getRGB(z+1, y+1, x+0, pRGB[3]); - part->getRGB(z+0, y+0, x+1, pRGB[4]); - part->getRGB(z+1, y+0, x+1, pRGB[5]); - part->getRGB(z+0, y+1, x+1, pRGB[6]); - part->getRGB(z+1, y+1, x+1, pRGB[7]); + part->getRGB(z + 0, y + 0, x + 0, pRGB[0]); + part->getRGB(z + 1, y + 0, x + 0, pRGB[1]); + part->getRGB(z + 0, y + 1, x + 0, pRGB[2]); + part->getRGB(z + 1, y + 1, x + 0, pRGB[3]); + part->getRGB(z + 0, y + 0, x + 1, pRGB[4]); + part->getRGB(z + 1, y + 0, x + 1, pRGB[5]); + part->getRGB(z + 0, y + 1, x + 1, pRGB[6]); + part->getRGB(z + 1, y + 1, x + 1, pRGB[7]); double pw[8]; pw[0] = (1. - wx) * (1. - wy) * (1. - wz); @@ -742,8 +1404,8 @@ EnumTsdSpaceInterpolate TsdSpace::interpolateTrilinearRGB(obfloat coord[3], unsi pw[6] = wx * wy * (1. - wz); pw[7] = wx * wy * wz; - memset(rgb,0,3); - for(unsigned int i=0; i<8; i++) + memset(rgb, 0, 3); + for(unsigned int i = 0; i < 8; i++) { rgb[0] += pRGB[i][0] * pw[i]; rgb[1] += pRGB[i][1] * pw[i]; @@ -809,7 +1471,8 @@ void TsdSpace::serialize(const char* filename) ofstream f; f.open(filename); - f << _voxelSize << " " << (int)_layoutPartition << " " << (int)_layoutSpace << " " << _maxTruncation << endl; + //f << _voxelSize << " " << (int)_layoutPartition << " " << (int)_layoutSpace << " " << _maxTruncation << endl; + f << _voxelSize << " " << (int)_layoutPartition << " " << _cellsX << " " << " " << _cellsY << " " << _cellsZ << " " <<_maxTruncation << endl; for(int pz=0; pz<_partitionsInZ; pz++) { @@ -824,7 +1487,7 @@ void TsdSpace::serialize(const char* filename) } } } - + f << "EOF" << std::endl; LOGMSG(DBG_WARN, "Saved file: " << filename); f.close(); } @@ -844,13 +1507,18 @@ TsdSpace* TsdSpace::load(const char* filename) EnumTsdSpaceLayout layoutPartition; EnumTsdSpaceLayout layoutSpace; int lp, ls; + unsigned int cellsX = 0; + unsigned int cellsY = 0; + unsigned int cellsZ = 0; double maxTruncation; - f >> voxelSize >> lp >> ls >> maxTruncation; + //f >> voxelSize >> lp >> ls >> maxTruncation; + f >> voxelSize >> lp >> cellsX >> cellsY >> cellsZ >> maxTruncation; layoutPartition = (EnumTsdSpaceLayout)lp; layoutSpace = (EnumTsdSpaceLayout)ls; - TsdSpace* space = new TsdSpace(voxelSize, layoutPartition, layoutSpace); + // TsdSpace* space = new TsdSpace(voxelSize, layoutPartition, layoutSpace); + TsdSpace* space = new TsdSpace(voxelSize, layoutPartition, cellsX, cellsY, cellsZ); space->setMaxTruncation(maxTruncation); TsdSpacePartition**** partitions = space->getPartitions(); @@ -869,7 +1537,12 @@ TsdSpace* TsdSpace::load(const char* filename) } } } - + _minX = 0.0; + _minY = 0.0; + _minZ = 0.0; + _maxX = static_cast(_cellsX) * _voxelSize; + _maxY = static_cast(_cellsY) * _voxelSize; + _maxZ = static_cast(_cellsZ) * _voxelSize; f.close(); return space; @@ -879,6 +1552,7 @@ bool TsdSpace::sliceImage(const unsigned int idx, const EnumSpaceAxis& axis, std { rgb->clear(); const unsigned int sizePartition = this->getPartitionSize(); + bool something = false; if(axis == Z) { const unsigned int partIdxZ = idx / sizePartition; @@ -895,6 +1569,12 @@ bool TsdSpace::sliceImage(const unsigned int idx, const EnumSpaceAxis& axis, std if(!partCur->isInitialized()) continue; + if(partCur->isEmpty()) + continue; + //something = true; + else + if(!something) + something = true; const unsigned int voxelIdxX = j - partIdxX * sizePartition; const unsigned int voxelIdxY = i - partIdxY * sizePartition; @@ -906,12 +1586,14 @@ bool TsdSpace::sliceImage(const unsigned int idx, const EnumSpaceAxis& axis, std (*rgb)[(i * _cellsX + j) * 3 + 1] = 0; (*rgb)[(i * _cellsX + j) * 3 + 2] = 0; } - else //in front of voxel BLUE + else if(tsdCur > 0.0) //in front of voxel BLUE { (*rgb)[(i * _cellsX + j) * 3 ] = 0; (*rgb)[(i * _cellsX + j) * 3 + 1] = 0; (*rgb)[(i * _cellsX + j) * 3 + 2] = color; } + else + continue; } } } @@ -931,6 +1613,11 @@ bool TsdSpace::sliceImage(const unsigned int idx, const EnumSpaceAxis& axis, std if(!partCur->isInitialized()) continue; + if(partCur->isEmpty()) + continue; + else + if(!something) + something = true; const unsigned int voxelIdxX = j - partIdxX * sizePartition; const unsigned int voxelIdxZ = i - partIdxZ * sizePartition; @@ -942,12 +1629,14 @@ bool TsdSpace::sliceImage(const unsigned int idx, const EnumSpaceAxis& axis, std (*rgb)[(i * _cellsX + j) * 3 + 1] = 0; (*rgb)[(i * _cellsX + j) * 3 + 2] = 0; } - else //in front of voxel BLUE + else if(tsdCur > 0.0) //in front of voxel BLUE { (*rgb)[(i * _cellsX + j) * 3 ] = 0; (*rgb)[(i * _cellsX + j) * 3 + 1] = 0; (*rgb)[(i * _cellsX + j) * 3 + 2] = color; } + else + continue; } } } @@ -967,6 +1656,11 @@ bool TsdSpace::sliceImage(const unsigned int idx, const EnumSpaceAxis& axis, std if(!partCur->isInitialized()) continue; + if(partCur->isEmpty()) + continue; + else + if(!something) + something = true; const unsigned int voxelIdxY = j - partIdxY * sizePartition; const unsigned int voxelIdxZ = i - partIdxZ * sizePartition; @@ -978,16 +1672,18 @@ bool TsdSpace::sliceImage(const unsigned int idx, const EnumSpaceAxis& axis, std (*rgb)[(i * _cellsY + j) * 3 + 1] = 0; (*rgb)[(i * _cellsY + j) * 3 + 2] = 0; } - else //in front of voxel BLUE + else if(tsdCur > 0.0) //in front of voxel BLUE { (*rgb)[(i * _cellsY + j) * 3 ] = 0; (*rgb)[(i * _cellsY + j) * 3 + 1] = 0; (*rgb)[(i * _cellsY + j) * 3 + 2] = color; } + else + continue; } } } - return true; + return something; } void TsdSpace::serializeSliceImages(const EnumSpaceAxis& axis, const std::string& path) @@ -1002,7 +1698,8 @@ void TsdSpace::serializeSliceImages(const EnumSpaceAxis& axis, const std::string { for(unsigned int i = 0; i < _cellsZ; i++) { - this->sliceImage(i, axis, &imageBuf); + if(!this->sliceImage(i, axis, &imageBuf)) + continue; std::stringstream ss; ss << storePath << "/z_axis_" << i << ".ppm"; serializePPM(ss.str().c_str(), imageBuf.data(), _cellsX, _cellsY); @@ -1032,4 +1729,16 @@ void TsdSpace::serializeSliceImages(const EnumSpaceAxis& axis, const std::string return; } +bool compareZ(const Eigen::Vector3f& var1, const Eigen::Vector3f& var2) +{ + return var1(2) < var2(2); +} + +bool compareAngle(const PointWithAngle& var1, const PointWithAngle& var2) +{ + return var1.angle < var2.angle; +} + + + } diff --git a/obvision/reconstruct/space/TsdSpace.h b/obvision/reconstruct/space/TsdSpace.h old mode 100644 new mode 100755 index 76b1f58..14066de --- a/obvision/reconstruct/space/TsdSpace.h +++ b/obvision/reconstruct/space/TsdSpace.h @@ -2,32 +2,36 @@ #define TSDSPACE_H #include "obcore/math/linalg/linalg.h" +#include "obcore/base/Point.h" #include "obvision/reconstruct/reconstruct_defs.h" #include "obvision/reconstruct/Sensor.h" #include "TsdSpacePartition.h" #include +#include + +typedef std::vector > stdVecEig3f; namespace obvious { enum EnumTsdSpaceLayout { LAYOUT_1x1x1=0, - LAYOUT_2x2x2=1, - LAYOUT_4x4x4=2, - LAYOUT_8x8x8=3, - LAYOUT_16x16x16=4, - LAYOUT_32x32x32=5, - LAYOUT_64x64x64=6, - LAYOUT_128x128x128=7, - LAYOUT_256x256x256=8, - LAYOUT_512x512x512=9, - LAYOUT_1024x1024x1024=10}; + LAYOUT_2x2x2=1, + LAYOUT_4x4x4=2, + LAYOUT_8x8x8=3, + LAYOUT_16x16x16=4, + LAYOUT_32x32x32=5, + LAYOUT_64x64x64=6, + LAYOUT_128x128x128=7, + LAYOUT_256x256x256=8, + LAYOUT_512x512x512=9, + LAYOUT_1024x1024x1024=10}; enum EnumTsdSpaceInterpolate { INTERPOLATE_SUCCESS=0, - INTERPOLATE_INVALIDINDEX=1, - INTERPOLATE_EMPTYPARTITION=2, - INTERPOLATE_ISNAN=3}; + INTERPOLATE_INVALIDINDEX=1, + INTERPOLATE_EMPTYPARTITION=2, + INTERPOLATE_ISNAN=3}; enum EnumSpaceAxis { @@ -36,255 +40,295 @@ enum EnumSpaceAxis Z }; +struct PointWithAngle + { + PointWithAngle(const Eigen::Vector3f& point, const double angle): + point(point), + angle(angle){} + Eigen::Vector3f point; + double angle; + }; + /** * @class TsdSpace * @brief Space representing a true signed distance function * @author Philipp Koch, Stefan May */ - class TsdSpace - { - public: - - /** - * Standard constructor - * @param[in] voxelSize edge length of voxels in meters - * @param[in] layoutPartition Partition layout, i.e., voxels in partition - * @param[in] layoutSpace Space layout, i.e., partitions in space - */ - TsdSpace(const double voxelSize, const EnumTsdSpaceLayout layoutPartition, const EnumTsdSpaceLayout layoutSpace); - - /** - * Destructor - */ - virtual ~TsdSpace(); - - /** - * Reset space to initial state - */ - void reset(); - - /** - * Get number of voxels in x-direction - */ - unsigned int getXDimension() const { return _cellsX; } - - /** - * Get number of voxels in y-direction - */ - unsigned int getYDimension() const { return _cellsY; } - - /** - * Get number of voxels in z-direction - */ - unsigned int getZDimension() const { return _cellsZ; } - - /** - * Get number of partitions in x-direction - */ - int getPartitionsInX() const { return _partitionsInX; } - - /** - * Get number of partitions in y-direction - */ - int getPartitionsInY() const { return _partitionsInY; } - - /** - * Get number of partitions in z-direction - */ - int getPartitionsInZ() const { return _partitionsInZ; } - - /** - * Get edge length of voxels - */ - obfloat getVoxelSize() const { return _voxelSize; } - - /** - * Get number of cells along edge - * @return number of cells - */ - unsigned int getPartitionSize(); - - /** - * Get minimum for x-coordinate - * @return x-coordinate - */ - obfloat getMinX() const { return _minX; } - - /** - * Get maximum for x-coordinate - * @return x-coordinate - */ - obfloat getMaxX() const { return _maxX; } - - /** - * Get minimum for y-coordinate - * @return y-coordinate - */ - obfloat getMinY() const { return _minY; } - - /** - * Get maximum for y-coordinate - * @return y-coordinate - */ - obfloat getMaxY() const { return _maxY; } - - /** - * Get minimum for z-coordinate - * @return z-coordinate - */ - obfloat getMinZ() const { return _minZ; } - - /** - * Get maximum for z-coordinate - * @return z-coordinate - */ - obfloat getMaxZ() const { return _maxZ; } - - /** - * Get centroid of space - * @param[out] centroid centroid coordinates - */ - void getCentroid(obfloat centroid[3]); - - /** - * Set maximum truncation radius - * Function to set the max truncation - * @param val new truncation radius - */ - void setMaxTruncation(const obfloat val); - - /** - * Get maximum truncation radius - * @return truncation radius - */ - double getMaxTruncation() const { return _maxTruncation; } - - /** - * Get pointer to internal partition space - * @return pointer to 3D partition space - */ - TsdSpacePartition**** getPartitions() const { return _partitions; } - - /** - * Check, if partition belonging to coordinate is initialized - * @param coord query coordinate - * @return initialization state - */ - bool isPartitionInitialized(obfloat coord[3]); - - /** - * Determine whether sensor is inside space - * @param sensor - */ - bool isInsideSpace(Sensor* sensor); +class TsdSpace +{ +public: + + /** + * Standard constructor + * @param[in] voxelSize edge length of voxels in meters + * @param[in] layoutPartition Partition layout, i.e., voxels in partition + * @param[in] layoutSpace Space layout, i.e., partitions in space + */ + TsdSpace(const double voxelSize, const EnumTsdSpaceLayout layoutPartition, const EnumTsdSpaceLayout layoutSpace); + TsdSpace(const double voxelSize, const EnumTsdSpaceLayout layoutPartition, const unsigned int cellsX, const unsigned int cellsY, const unsigned int cellsZ); + + /** + * Destructor + */ + virtual ~TsdSpace(); + + /** + * Reset space to initial state + */ + void reset(); + + /** + * Get number of voxels in x-direction + */ + unsigned int getXDimension() const { return _cellsX; } + + /** + * Get number of voxels in y-direction + */ + unsigned int getYDimension() const { return _cellsY; } + + /** + * Get number of voxels in z-direction + */ + unsigned int getZDimension() const { return _cellsZ; } + + /** + * Get number of partitions in x-direction + */ + int getPartitionsInX() const { return _partitionsInX; } + + /** + * Get number of partitions in y-direction + */ + int getPartitionsInY() const { return _partitionsInY; } + + /** + * Get number of partitions in z-direction + */ + int getPartitionsInZ() const { return _partitionsInZ; } + + /** + * Get edge length of voxels + */ + obfloat getVoxelSize() const { return _voxelSize; } + + /** + * Get number of cells along edge + * @return number of cells + */ + unsigned int getPartitionSize(); + + /** + * Get minimum for x-coordinate + * @return x-coordinate + */ + obfloat getMinX() const { return _minX; } + + /** + * Get maximum for x-coordinate + * @return x-coordinate + */ + obfloat getMaxX() const { return _maxX; } + + /** + * Get minimum for y-coordinate + * @return y-coordinate + */ + obfloat getMinY() const { return _minY; } + + /** + * Get maximum for y-coordinate + * @return y-coordinate + */ + obfloat getMaxY() const { return _maxY; } + + /** + * Get minimum for z-coordinate + * @return z-coordinate + */ + obfloat getMinZ() const { return _minZ; } + + /** + * Get maximum for z-coordinate + * @return z-coordinate + */ + obfloat getMaxZ() const { return _maxZ; } + + /** + * Get centroid of space + * @param[out] centroid centroid coordinates + */ + void getCentroid(obfloat centroid[3]); + + /** + * Set maximum truncation radius + * Function to set the max truncation + * @param val new truncation radius + */ + void setMaxTruncation(const obfloat val); + + /** + * Get maximum truncation radius + * @return truncation radius + */ + double getMaxTruncation() const { return _maxTruncation; } + + /** + * Get pointer to internal partition space + * @return pointer to 3D partition space + */ + TsdSpacePartition**** getPartitions() const { return _partitions; } + + /** + * Check, if partition belonging to coordinate is initialized + * @param coord query coordinate + * @return initialization state + */ + bool isPartitionInitialized(obfloat coord[3]); + + /** + * Determine whether sensor is inside space + * @param sensor + */ + bool isInsideSpace(Sensor* sensor); + bool isInsideSpace(const Eigen::Vector3f& pos); + + /** + * Push sensor data to space + * @param[in] sensor abstract sensor instance holding current data + */ + void push(Sensor* sensor); + + void push(stdVecEig3f& points); + + void push(const std::vector& data, const unsigned int width, const unsigned int height, const Eigen::Vector3f& t, const double resDepth, + const double resHor); + + /** + * Push sensor data to space using forward raycast + * @param[in] sensor abstract sensor instance holding current data + */ + void pushForward(Sensor* const sensor); + + /** + * @brief Push pointcloud forward in without usage of a sensor. + * Every point in the vector is treated as a 2D Laser measurement from (0.0, 0.0, z) to (x, y, z) measurement. + * This method violates the sensor concept in this framework but is necessary to treat pointclouds + * from unknown poses. + */ + void pushForward(const stdVecEig3f& points); - /** - * Push sensor data to space - * @param[in] sensor abstract sensor instance holding current data - */ - void push(Sensor* sensor); + /** + * Push sensor data to space using octree insertion + * @param[in] sensor abstract sensor instance holding current data + */ + void pushTree(Sensor* sensor); - /** - * Push sensor data to space using octree insertion - * @param[in] sensor abstract sensor instance holding current data - */ - void pushTree(Sensor* sensor); + /** + * interpolate_trilineary + * Method to interpolate TSDF trilineary + * @param coord pointer to coordinates of intersection + * @param[out] tsd interpolated TSD value + */ + EnumTsdSpaceInterpolate interpolateTrilinear(obfloat coord[3], obfloat* tsd); - /** - * interpolate_trilineary - * Method to interpolate TSDF trilineary - * @param coord pointer to coordinates of intersection - * @param[out] tsd interpolated TSD value - */ - EnumTsdSpaceInterpolate interpolateTrilinear(obfloat coord[3], obfloat* tsd); + /** + * interpolate_trilineary + * Method to interpolate RGB data trilineary + * @param coord pointer to coordinates of intersection + * @param[out] rgb interpolated RGB vector + */ + EnumTsdSpaceInterpolate interpolateTrilinearRGB(obfloat coord[3], unsigned char rgb[3]); - /** - * interpolate_trilineary - * Method to interpolate RGB data trilineary - * @param coord pointer to coordinates of intersection - * @param[out] rgb interpolated RGB vector - */ - EnumTsdSpaceInterpolate interpolateTrilinearRGB(obfloat coord[3], unsigned char rgb[3]); + /** + * + * Calculates normal of crossed surface + * @param normal Variable to store the components in. Has to be allocated by calling function (3 coordinates) + */ + bool interpolateNormal(const obfloat* coord, obfloat* normal); - /** - * - * Calculates normal of crossed surface - * @param normal Variable to store the components in. Has to be allocated by calling function (3 coordinates) - */ - bool interpolateNormal(const obfloat* coord, obfloat* normal); + EnumTsdSpaceInterpolate getTsd(obfloat coord[3], obfloat* tsd); - EnumTsdSpaceInterpolate getTsd(obfloat coord[3], obfloat* tsd); + //bool buildSliceImage(const unsigned int depthIndex, unsigned char* image); + /** + * Method to store the content of the grid in a file + * @param filename + */ + void serialize(const char* filename); - //bool buildSliceImage(const unsigned int depthIndex, unsigned char* image); - /** - * Method to store the content of the grid in a file - * @param filename - */ - void serialize(const char* filename); + /** + * Method to load values out of a file into the grid + * @param filename + */ + TsdSpace* load(const char* filename); - /** - * Method to load values out of a file into the grid - * @param filename - */ - static TsdSpace* load(const char* filename); + bool sliceImage(const unsigned int idx, const EnumSpaceAxis& axis, std::vector* const rgb); - bool sliceImage(const unsigned int idx, const EnumSpaceAxis& axis, std::vector* const rgb); + void serializeSliceImages(const EnumSpaceAxis& axis, const std::string& path = ""); - void serializeSliceImages(const EnumSpaceAxis& axis, const std::string& path = ""); +private: - private: + void pushRecursion(Sensor* sensor, obfloat pos[3], TsdSpaceComponent* comp, vector &partitionsToCheck); - void pushRecursion(Sensor* sensor, obfloat pos[3], TsdSpaceComponent* comp, vector &partitionsToCheck); + void propagateBorders(); - void propagateBorders(); + void addTsdValue(const unsigned int col, const unsigned int row, const unsigned int z, double sd, unsigned char* rgb); - void addTsdValue(const unsigned int col, const unsigned int row, const unsigned int z, double sd, unsigned char* rgb); + bool coord2Index(obfloat coord[3], int* x, int* y, int* z, obfloat* dx, obfloat* dy, obfloat* dz); - bool coord2Index(obfloat coord[3], int* x, int* y, int* z, obfloat* dx, obfloat* dy, obfloat* dz); + TsdSpaceComponent* _tree; - TsdSpaceComponent* _tree; + unsigned int _cellsX; - unsigned int _cellsX; + unsigned int _cellsY; - unsigned int _cellsY; + unsigned int _cellsZ; - unsigned int _cellsZ; + obfloat _voxelSize; - obfloat _voxelSize; + obfloat _invVoxelSize; - obfloat _invVoxelSize; + obfloat _maxTruncation; - obfloat _maxTruncation; + obfloat _minX; - obfloat _minX; + obfloat _maxX; - obfloat _maxX; + obfloat _minY; - obfloat _minY; + obfloat _maxY; - obfloat _maxY; + obfloat _minZ; - obfloat _minZ; + obfloat _maxZ; - obfloat _maxZ; + TsdSpacePartition**** _partitions; - TsdSpacePartition**** _partitions; + int* _lutIndex2Partition; + int* _lutIndex2Cell; - int* _lutIndex2Partition; - int* _lutIndex2Cell; + int* _lutIndex2PartitionX; + int* _lutIndex2CellX; + int* _lutIndex2PartitionY; + int* _lutIndex2CellY; + int* _lutIndex2PartitionZ; + int* _lutIndex2CellZ; - int _partitionsInX; + int _partitionsInX; - int _partitionsInY; + int _partitionsInY; - int _partitionsInZ; + int _partitionsInZ; - EnumTsdSpaceLayout _layoutPartition; + EnumTsdSpaceLayout _layoutPartition; - EnumTsdSpaceLayout _layoutSpace; + EnumTsdSpaceLayout _layoutSpace; + +}; - }; +bool compareZ(const Eigen::Vector3f& var1, const Eigen::Vector3f& var2); +bool compareAngle(const PointWithAngle& var1, const PointWithAngle& var2); } diff --git a/obvision/reconstruct/space/TsdSpaceBranch.cpp b/obvision/reconstruct/space/TsdSpaceBranch.cpp old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/space/TsdSpaceBranch.h b/obvision/reconstruct/space/TsdSpaceBranch.h old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/space/TsdSpaceComponent.cpp b/obvision/reconstruct/space/TsdSpaceComponent.cpp old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/space/TsdSpaceComponent.h b/obvision/reconstruct/space/TsdSpaceComponent.h old mode 100644 new mode 100755 diff --git a/obvision/reconstruct/space/TsdSpacePartition.cpp b/obvision/reconstruct/space/TsdSpacePartition.cpp old mode 100644 new mode 100755 index ff4966a..52bf34e --- a/obvision/reconstruct/space/TsdSpacePartition.cpp +++ b/obvision/reconstruct/space/TsdSpacePartition.cpp @@ -265,7 +265,7 @@ void TsdSpacePartition::increaseEmptiness() if(isnan(voxel->tsd)) { - voxel->tsd = 1.0; + //voxel->tsd = 1.0; } else { diff --git a/obvision/reconstruct/space/TsdSpacePartition.h b/obvision/reconstruct/space/TsdSpacePartition.h old mode 100644 new mode 100755 diff --git a/obvision/registration/Trace.cpp b/obvision/registration/Trace.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/Trace.h b/obvision/registration/Trace.h old mode 100644 new mode 100755 diff --git a/obvision/registration/amcl/AdaptiveMonteCarloMatching.h b/obvision/registration/amcl/AdaptiveMonteCarloMatching.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/ClosedFormEstimator2D.cpp b/obvision/registration/icp/ClosedFormEstimator2D.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/ClosedFormEstimator2D.h b/obvision/registration/icp/ClosedFormEstimator2D.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/IRigidEstimator.h b/obvision/registration/icp/IRigidEstimator.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/Icp.cpp b/obvision/registration/icp/Icp.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/Icp.h b/obvision/registration/icp/Icp.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/IcpMultiInitIterator.cpp b/obvision/registration/icp/IcpMultiInitIterator.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/IcpMultiInitIterator.h b/obvision/registration/icp/IcpMultiInitIterator.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/PointToLineEstimator2D.cpp b/obvision/registration/icp/PointToLineEstimator2D.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/PointToLineEstimator2D.h b/obvision/registration/icp/PointToLineEstimator2D.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/PointToPlaneEstimator3D.cpp b/obvision/registration/icp/PointToPlaneEstimator3D.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/PointToPlaneEstimator3D.h b/obvision/registration/icp/PointToPlaneEstimator3D.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/PointToPointEstimator3D.cpp b/obvision/registration/icp/PointToPointEstimator3D.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/PointToPointEstimator3D.h b/obvision/registration/icp/PointToPointEstimator3D.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/AnnPairAssignment.cpp b/obvision/registration/icp/assign/AnnPairAssignment.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/AnnPairAssignment.h b/obvision/registration/icp/assign/AnnPairAssignment.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/FlannPairAssignment.cpp b/obvision/registration/icp/assign/FlannPairAssignment.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/FlannPairAssignment.h b/obvision/registration/icp/assign/FlannPairAssignment.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/NaboPairAssignment.cpp b/obvision/registration/icp/assign/NaboPairAssignment.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/NaboPairAssignment.h b/obvision/registration/icp/assign/NaboPairAssignment.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/PairAssignment.cpp b/obvision/registration/icp/assign/PairAssignment.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/PairAssignment.h b/obvision/registration/icp/assign/PairAssignment.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/ProjectivePairAssignment.cpp b/obvision/registration/icp/assign/ProjectivePairAssignment.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/ProjectivePairAssignment.h b/obvision/registration/icp/assign/ProjectivePairAssignment.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/assignbase.h b/obvision/registration/icp/assign/assignbase.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/filter/DistanceFilter.cpp b/obvision/registration/icp/assign/filter/DistanceFilter.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/filter/DistanceFilter.h b/obvision/registration/icp/assign/filter/DistanceFilter.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/filter/IPostAssignmentFilter.h b/obvision/registration/icp/assign/filter/IPostAssignmentFilter.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/filter/IPreAssignmentFilter.h b/obvision/registration/icp/assign/filter/IPreAssignmentFilter.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/filter/OcclusionFilter.cpp b/obvision/registration/icp/assign/filter/OcclusionFilter.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/filter/OcclusionFilter.h b/obvision/registration/icp/assign/filter/OcclusionFilter.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/filter/OutOfBoundsFilter2D.cpp b/obvision/registration/icp/assign/filter/OutOfBoundsFilter2D.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/filter/OutOfBoundsFilter2D.h b/obvision/registration/icp/assign/filter/OutOfBoundsFilter2D.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/filter/OutOfBoundsFilter3D.cpp b/obvision/registration/icp/assign/filter/OutOfBoundsFilter3D.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/filter/OutOfBoundsFilter3D.h b/obvision/registration/icp/assign/filter/OutOfBoundsFilter3D.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/filter/ProjectionFilter.cpp b/obvision/registration/icp/assign/filter/ProjectionFilter.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/filter/ProjectionFilter.h b/obvision/registration/icp/assign/filter/ProjectionFilter.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/filter/ReciprocalFilter.cpp b/obvision/registration/icp/assign/filter/ReciprocalFilter.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/filter/ReciprocalFilter.h b/obvision/registration/icp/assign/filter/ReciprocalFilter.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/filter/RobotFootprintFilter.cpp b/obvision/registration/icp/assign/filter/RobotFootprintFilter.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/filter/RobotFootprintFilter.h b/obvision/registration/icp/assign/filter/RobotFootprintFilter.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/filter/TrimmedFilter.cpp b/obvision/registration/icp/assign/filter/TrimmedFilter.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/assign/filter/TrimmedFilter.h b/obvision/registration/icp/assign/filter/TrimmedFilter.h old mode 100644 new mode 100755 diff --git a/obvision/registration/icp/icp_def.h b/obvision/registration/icp/icp_def.h old mode 100644 new mode 100755 diff --git a/obvision/registration/ndt/Ndt.cpp b/obvision/registration/ndt/Ndt.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/ndt/Ndt.h b/obvision/registration/ndt/Ndt.h old mode 100644 new mode 100755 diff --git a/obvision/registration/ransacMatching/PDFMatching.cpp b/obvision/registration/ransacMatching/PDFMatching.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/ransacMatching/PDFMatching.h b/obvision/registration/ransacMatching/PDFMatching.h old mode 100644 new mode 100755 diff --git a/obvision/registration/ransacMatching/RandomMatching.cpp b/obvision/registration/ransacMatching/RandomMatching.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/ransacMatching/RandomMatching.h b/obvision/registration/ransacMatching/RandomMatching.h old mode 100644 new mode 100755 diff --git a/obvision/registration/ransacMatching/RandomNormalMatching.cpp b/obvision/registration/ransacMatching/RandomNormalMatching.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/ransacMatching/RandomNormalMatching.h b/obvision/registration/ransacMatching/RandomNormalMatching.h old mode 100644 new mode 100755 diff --git a/obvision/registration/ransacMatching/TSD_PDFMatching.cpp b/obvision/registration/ransacMatching/TSD_PDFMatching.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/ransacMatching/TSD_PDFMatching.h b/obvision/registration/ransacMatching/TSD_PDFMatching.h old mode 100644 new mode 100755 diff --git a/obvision/registration/ransacMatching/TwinPointMatching.cpp b/obvision/registration/ransacMatching/TwinPointMatching.cpp old mode 100644 new mode 100755 diff --git a/obvision/registration/ransacMatching/TwinPointMatching.h b/obvision/registration/ransacMatching/TwinPointMatching.h old mode 100644 new mode 100755 diff --git a/test/README.md b/test/README.md old mode 100644 new mode 100755 diff --git a/test/gtest-1.7.0.zip b/test/gtest-1.7.0.zip old mode 100644 new mode 100755 diff --git a/test/obcore/CMakeLists.txt b/test/obcore/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/test/obcore/base/eigen-vs-gsl.cpp b/test/obcore/base/eigen-vs-gsl.cpp old mode 100644 new mode 100755 diff --git a/test/obcore/base/pointcloud.cpp b/test/obcore/base/pointcloud.cpp old mode 100644 new mode 100755 diff --git a/test/obcore/math/MatrixTest.cpp b/test/obcore/math/MatrixTest.cpp old mode 100644 new mode 100755 diff --git a/test/obcore/math/QuaternionTest.cpp b/test/obcore/math/QuaternionTest.cpp old mode 100644 new mode 100755 From 94ca952458ca83b31fc7540b5e42e54b6e5099f6 Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 25 Jul 2018 15:18:37 +0200 Subject: [PATCH 03/18] Added raycaster transformation to sensor->setTransform --- .gitignore | 5 +++++ obvision/reconstruct/Sensor.cpp | 3 +++ tsd.todo | 2 ++ 3 files changed, 10 insertions(+) mode change 100644 => 100755 .gitignore create mode 100644 tsd.todo diff --git a/.gitignore b/.gitignore old mode 100644 new mode 100755 index 567609b..c76faa1 --- a/.gitignore +++ b/.gitignore @@ -1 +1,6 @@ build/ +.cproject +.settings +test/ +obcore/math/linalg/linalg.h +test/gtest-1.7.0.zip test/obcore test/README.md diff --git a/obvision/reconstruct/Sensor.cpp b/obvision/reconstruct/Sensor.cpp index 6878444..2e9c329 100755 --- a/obvision/reconstruct/Sensor.cpp +++ b/obvision/reconstruct/Sensor.cpp @@ -103,6 +103,9 @@ Matrix Sensor::getTransformation() void Sensor::setTransformation(Matrix T) { + *_rays = *_raysLocal; + Matrix R(T, 0, 0, _dim, _dim); + (*_rays) = R * (*_rays); *_T = T; } diff --git a/tsd.todo b/tsd.todo new file mode 100644 index 0000000..ea0657a --- /dev/null +++ b/tsd.todo @@ -0,0 +1,2 @@ +raycasters in raca3d are not normalized from the beginning on +check the partition->increase emptyness method From cd6beca02b12b93d56604ed5173510eb99260bf0 Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 25 Jul 2018 15:20:24 +0200 Subject: [PATCH 04/18] Added to the todo --- tsd.todo | 1 + 1 file changed, 1 insertion(+) diff --git a/tsd.todo b/tsd.todo index ea0657a..ce3d88d 100644 --- a/tsd.todo +++ b/tsd.todo @@ -1,2 +1,3 @@ raycasters in raca3d are not normalized from the beginning on check the partition->increase emptyness method +sensor->set transform apply also to rays. From 964397cad0ef4fda691cfb7fd56b6f67f422cf50 Mon Sep 17 00:00:00 2001 From: Your Name Date: Sun, 12 Aug 2018 14:31:00 +0200 Subject: [PATCH 05/18] Added ray transform to set transformation method. --- obvision/reconstruct/Sensor.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/obvision/reconstruct/Sensor.cpp b/obvision/reconstruct/Sensor.cpp index 2e9c329..e737756 100755 --- a/obvision/reconstruct/Sensor.cpp +++ b/obvision/reconstruct/Sensor.cpp @@ -106,6 +106,11 @@ void Sensor::setTransformation(Matrix T) *_rays = *_raysLocal; Matrix R(T, 0, 0, _dim, _dim); (*_rays) = R * (*_rays); + for(unsigned int i=0; i<_size; i++) + { + for(unsigned int j=0; j<_dim; j++) + (*_rays)(j, i) *= _rayNorm; + } *_T = T; } From 6fa9a91eff748cfc88a26bde07d9da1b16880d33 Mon Sep 17 00:00:00 2001 From: Your Name Date: Fri, 21 Sep 2018 23:27:09 +0200 Subject: [PATCH 06/18] added inline function to return a specific tsd value to an index. --- obvision/reconstruct/space/TsdSpacePartition.h | 1 + 1 file changed, 1 insertion(+) diff --git a/obvision/reconstruct/space/TsdSpacePartition.h b/obvision/reconstruct/space/TsdSpacePartition.h index 3ce852e..1796b14 100755 --- a/obvision/reconstruct/space/TsdSpacePartition.h +++ b/obvision/reconstruct/space/TsdSpacePartition.h @@ -50,6 +50,7 @@ class TsdSpacePartition : public TsdSpaceComponent void reset(); obfloat& operator () (unsigned int z, unsigned int y, unsigned int x) const { return _space[z][y][x].tsd; } + obfloat& getTsd(unsigned int z, unsigned int y, unsigned int x) const { return _space[z][y][x].tsd; } void getRGB(unsigned int z, unsigned int y, unsigned int x, unsigned char rgb[3]); From 64caa8f55d8a4f04ca20a1e7504163274a592e9d Mon Sep 17 00:00:00 2001 From: Your Name Date: Tue, 25 Sep 2018 21:17:43 +0200 Subject: [PATCH 07/18] Changed parameter order of tsd getter. --- obvision/reconstruct/space/TsdSpacePartition.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/obvision/reconstruct/space/TsdSpacePartition.h b/obvision/reconstruct/space/TsdSpacePartition.h index 1796b14..d801bdf 100755 --- a/obvision/reconstruct/space/TsdSpacePartition.h +++ b/obvision/reconstruct/space/TsdSpacePartition.h @@ -50,7 +50,7 @@ class TsdSpacePartition : public TsdSpaceComponent void reset(); obfloat& operator () (unsigned int z, unsigned int y, unsigned int x) const { return _space[z][y][x].tsd; } - obfloat& getTsd(unsigned int z, unsigned int y, unsigned int x) const { return _space[z][y][x].tsd; } + obfloat& getTsd(unsigned int x, unsigned int y, unsigned int z) const { return _space[z][y][x].tsd; } void getRGB(unsigned int z, unsigned int y, unsigned int x, unsigned char rgb[3]); From da6c6327354a3f7c1c21e0b4d9897e249cd0793f Mon Sep 17 00:00:00 2001 From: phil Date: Thu, 13 Dec 2018 13:25:52 +0100 Subject: [PATCH 08/18] Added a method which calculates the current initialized voxels. --- obvision/reconstruct/space/TsdSpace.cpp | 18 +++++++++++++++++- obvision/reconstruct/space/TsdSpace.h | 2 ++ 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/obvision/reconstruct/space/TsdSpace.cpp b/obvision/reconstruct/space/TsdSpace.cpp index 2349954..67a4126 100755 --- a/obvision/reconstruct/space/TsdSpace.cpp +++ b/obvision/reconstruct/space/TsdSpace.cpp @@ -1739,6 +1739,22 @@ bool compareAngle(const PointWithAngle& var1, const PointWithAngle& var2) return var1.angle < var2.angle; } - +unsigned int TsdSpace::getInitializedVxls(void) +{ + unsigned int nb = 0; + for(int pz = 0; pz < this->getPartitionsInZ(); pz++) + { + for(int py = 0; py < this->getPartitionsInY(); py++) + { + for(int px = 0; px < this->getPartitionsInX(); px++) + { + obvious::TsdSpacePartition* part = this->getPartitions()[pz][py][px]; + if(part->isInitialized() && !part->isEmpty()) + nb += (this->getPartitionSize() * this->getPartitionSize() * this->getPartitionSize()); + } + } + } + return nb; +} } diff --git a/obvision/reconstruct/space/TsdSpace.h b/obvision/reconstruct/space/TsdSpace.h index 14066de..1ef81d1 100755 --- a/obvision/reconstruct/space/TsdSpace.h +++ b/obvision/reconstruct/space/TsdSpace.h @@ -267,6 +267,8 @@ class TsdSpace void serializeSliceImages(const EnumSpaceAxis& axis, const std::string& path = ""); + unsigned int getInitializedVxls(void); + private: void pushRecursion(Sensor* sensor, obfloat pos[3], TsdSpaceComponent* comp, vector &partitionsToCheck); From 6b6302375aa3c65e6dfcb0d5e35b97d15d96b03c Mon Sep 17 00:00:00 2001 From: phil Date: Fri, 21 Dec 2018 13:26:56 +0100 Subject: [PATCH 09/18] Added ostream operator to tsd space. Changed the load method to static so it doesnt need a dummy space anymore --- obvision/reconstruct/space/TsdSpace.cpp | 18 ++++++++++++------ obvision/reconstruct/space/TsdSpace.h | 4 +++- 2 files changed, 15 insertions(+), 7 deletions(-) diff --git a/obvision/reconstruct/space/TsdSpace.cpp b/obvision/reconstruct/space/TsdSpace.cpp index 67a4126..6895ada 100755 --- a/obvision/reconstruct/space/TsdSpace.cpp +++ b/obvision/reconstruct/space/TsdSpace.cpp @@ -1537,12 +1537,12 @@ TsdSpace* TsdSpace::load(const char* filename) } } } - _minX = 0.0; - _minY = 0.0; - _minZ = 0.0; - _maxX = static_cast(_cellsX) * _voxelSize; - _maxY = static_cast(_cellsY) * _voxelSize; - _maxZ = static_cast(_cellsZ) * _voxelSize; +// _minX = 0.0; +// _minY = 0.0; +// _minZ = 0.0; +// _maxX = static_cast(_cellsX) * _voxelSize; +// _maxY = static_cast(_cellsY) * _voxelSize; +// _maxZ = static_cast(_cellsZ) * _voxelSize; f.close(); return space; @@ -1757,4 +1757,10 @@ unsigned int TsdSpace::getInitializedVxls(void) return nb; } +std::ostream& operator<< (std::ostream &out, TsdSpace& space) +{ + out << "voxelsize " << space._voxelSize << " cellsX Y Z " << space._cellsX << " " << space._cellsY << " " << space._cellsZ; + return out; +} + } diff --git a/obvision/reconstruct/space/TsdSpace.h b/obvision/reconstruct/space/TsdSpace.h index 1ef81d1..96c45c7 100755 --- a/obvision/reconstruct/space/TsdSpace.h +++ b/obvision/reconstruct/space/TsdSpace.h @@ -261,7 +261,7 @@ class TsdSpace * Method to load values out of a file into the grid * @param filename */ - TsdSpace* load(const char* filename); + static TsdSpace* load(const char* filename); bool sliceImage(const unsigned int idx, const EnumSpaceAxis& axis, std::vector* const rgb); @@ -269,6 +269,8 @@ class TsdSpace unsigned int getInitializedVxls(void); + friend std::ostream& operator<< (std::ostream &out, TsdSpace& space); + private: void pushRecursion(Sensor* sensor, obfloat pos[3], TsdSpaceComponent* comp, vector &partitionsToCheck); From b14565e027f16e7c08b78a8bf8ca31f9401a4885 Mon Sep 17 00:00:00 2001 From: phil Date: Fri, 21 Dec 2018 17:09:04 +0100 Subject: [PATCH 10/18] Bugfix constuctor --- obvision/reconstruct/space/TsdSpace.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/obvision/reconstruct/space/TsdSpace.cpp b/obvision/reconstruct/space/TsdSpace.cpp index 6895ada..f4b9d9f 100755 --- a/obvision/reconstruct/space/TsdSpace.cpp +++ b/obvision/reconstruct/space/TsdSpace.cpp @@ -127,6 +127,10 @@ TsdSpace::TsdSpace(const double voxelSize, const EnumTsdSpaceLayout layoutPartit _cellsY = cellsY; _cellsZ = cellsZ; + _tree = NULL; + _lutIndex2Cell = NULL; + _lutIndex2Partition = NULL; + unsigned int dimPartition = 1u << layoutPartition; if(dimPartition > _cellsX) From c07098e2c1a6bad37ebb1024b3277915430e243e Mon Sep 17 00:00:00 2001 From: Your Name Date: Sat, 20 Apr 2019 01:19:11 +0200 Subject: [PATCH 11/18] Added subsample parameter to raycasting added graphical reconstruction of a complete tsd space. --- obvision/reconstruct/space/RayCast3D.cpp | 4 +++- obvision/reconstruct/space/RayCast3D.h | 2 +- obvision/reconstruct/space/TsdSpace.cpp | 5 +++++ obvision/reconstruct/space/TsdSpace.h | 6 ++++++ 4 files changed, 15 insertions(+), 2 deletions(-) diff --git a/obvision/reconstruct/space/RayCast3D.cpp b/obvision/reconstruct/space/RayCast3D.cpp index e127a97..f14fe31 100755 --- a/obvision/reconstruct/space/RayCast3D.cpp +++ b/obvision/reconstruct/space/RayCast3D.cpp @@ -36,7 +36,7 @@ RayCast3D::~RayCast3D() } -void RayCast3D::calcCoordsFromCurrentPose(TsdSpace* space, Sensor* sensor, double* coords, double* normals, unsigned char* rgb, unsigned int* size) +void RayCast3D::calcCoordsFromCurrentPose(TsdSpace* space, Sensor* sensor, double* coords, double* normals, unsigned char* rgb, unsigned int* size, const unsigned int subs) { Timer t; t.start(); @@ -101,6 +101,8 @@ void RayCast3D::calcCoordsFromCurrentPose(TsdSpace* space, Sensor* sensor, doubl #pragma omp for schedule(dynamic) for(unsigned int i=0; i &partitionsToCheck); From d69c19c4f5fec17792d8ee13bb75796575389988 Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 24 Apr 2019 01:33:09 +0200 Subject: [PATCH 12/18] Added a method to reconstruct the tsd space in a colored cloud --- obvision/reconstruct/space/TsdSpace.cpp | 76 +++++++++++++++++++++++-- 1 file changed, 70 insertions(+), 6 deletions(-) diff --git a/obvision/reconstruct/space/TsdSpace.cpp b/obvision/reconstruct/space/TsdSpace.cpp index ccd26e3..1d66ee5 100755 --- a/obvision/reconstruct/space/TsdSpace.cpp +++ b/obvision/reconstruct/space/TsdSpace.cpp @@ -1541,12 +1541,12 @@ TsdSpace* TsdSpace::load(const char* filename) } } } -// _minX = 0.0; -// _minY = 0.0; -// _minZ = 0.0; -// _maxX = static_cast(_cellsX) * _voxelSize; -// _maxY = static_cast(_cellsY) * _voxelSize; -// _maxZ = static_cast(_cellsZ) * _voxelSize; + // _minX = 0.0; + // _minY = 0.0; + // _minZ = 0.0; + // _maxX = static_cast(_cellsX) * _voxelSize; + // _maxY = static_cast(_cellsY) * _voxelSize; + // _maxZ = static_cast(_cellsZ) * _voxelSize; f.close(); return space; @@ -1735,7 +1735,71 @@ void TsdSpace::serializeSliceImages(const EnumSpaceAxis& axis, const std::string TsdSpace* TsdSpace::substract(TsdSpace* substractor) { + obvious::TsdSpace* diff = new obvious::TsdSpace(this->getVoxelSize(), this->getLayOutPartition(), this->getXDimension(), this->getYDimension(), this->getZDimension()); + unsigned int ctr = 0; + for(int pz = 0; pz < this->getPartitionsInZ(); pz++) + { + for(int py = 0; py < this->getPartitionsInY(); py++) + { + for(int px = 0; px < this->getPartitionsInX(); px++) + { + //std::cout << __PRETTY_FUNCTION__ << "part idx" << px << " " << py << " " << pz << std::endl; + obvious::TsdSpacePartition* part0 = this->getPartitions()[pz][py][px]; + obvious::TsdSpacePartition* part1 = substractor->getPartitions()[pz][py][px]; + obvious::TsdSpacePartition* diffPart = diff->getPartitions()[pz][py][px]; + if(0)//part0->isEmpty() && !part1->isEmpty()) //dont really know what to do in this case...is weird + { + std::cout << __PRETTY_FUNCTION__ << " huge difference..this is not supposed to happen" << std::endl; + continue; + } + else if(!part0->isInitialized())// || part0->isEmpty())// && part1->isEmpty()) + { + std::cout << __PRETTY_FUNCTION__ << "part 0 empty" << std::endl; + continue; //toDo: work!!! + } + else if(!part1->isInitialized())// || part1->isEmpty())// && !part0->isEmpty()) //dont really know what to do in this case...is weird + { + std::cout << __PRETTY_FUNCTION__ << " part1 is empty" << std::endl; + continue; + } + ctr++; + diffPart->init(); + for(unsigned int z = 0; z < part0->getDepth(); z++) + { + for(unsigned int y = 0; y < part0->getHeight(); y++) + { + for(unsigned int x = 0; x < part0->getWidth(); x++) + { + // std::cout << __PRETTY_FUNCTION__ << "vxl idx" << x << " " << y << " " << z << std::endl; +// const double tsd0 = part0->getTsd(x, y, z); +// const double tsd1 = part1->getTsd(x, y, z); + // std::cout << __PRETTY_FUNCTION__ << "got tsds " << tsd0 << " tsd1" << std::endl; + const double tsdDiff = part0->getTsd(x, y, z) - part1->getTsd(x, y, z); + // std::cout << __PRETTY_FUNCTION__ << "init diff space part" << std::endl; + + + if(std::abs(tsdDiff > 0.01)) + { + diffPart->_space[z][y][x].tsd = tsdDiff; + std::cout << __PRETTY_FUNCTION__ << tsdDiff << " = " << part0->getTsd(x, y, z) << " - " << part1->getTsd(x, y, z) <_space[z][y][x].tsd = NAN; + diffPart->_space[z][y][x].weight = MAXWEIGHT; + } + + //if(tsdDiff > 0.01) + //std::cout << __PRETTY_FUNCTION__ << "tsdDiff " << tsdDiff << std::endl; + } + } + } + } + } + } + std::cout << __PRETTY_FUNCTION__ << "found " << ctr << " non empty partitions" << std::endl; + return diff; } bool compareZ(const Eigen::Vector3f& var1, const Eigen::Vector3f& var2) From 9bc7a9874278d1b99c48172b480302bce9966826 Mon Sep 17 00:00:00 2001 From: Jasmin Dschessica Date: Thu, 20 Feb 2020 16:00:19 +0100 Subject: [PATCH 13/18] huhu. added SensorVelodyne3D sensor model for Velodyne VLP16 PUCK. to be adapted for other 3D scanners. --- obvision/CMakeLists.txt | 1 + .../reconstruct/space/SensorVelodyne3D.cpp | 281 ++++++++++++++++++ obvision/reconstruct/space/SensorVelodyne3D.h | 87 ++++++ 3 files changed, 369 insertions(+) create mode 100644 obvision/reconstruct/space/SensorVelodyne3D.cpp create mode 100644 obvision/reconstruct/space/SensorVelodyne3D.h diff --git a/obvision/CMakeLists.txt b/obvision/CMakeLists.txt index e0d40b6..c1039a9 100755 --- a/obvision/CMakeLists.txt +++ b/obvision/CMakeLists.txt @@ -58,6 +58,7 @@ add_library(obvision STATIC reconstruct/space/SensorPolar3D.cpp reconstruct/space/SensorProjective3D.cpp reconstruct/space/SensorPolar3D.cpp + reconstruct/space/SensorVelodyne3D.cpp reconstruct/space/SensorPolar2DWith3DPose.cpp reconstruct/space/TsdSpace.cpp reconstruct/space/TsdSpaceComponent.cpp diff --git a/obvision/reconstruct/space/SensorVelodyne3D.cpp b/obvision/reconstruct/space/SensorVelodyne3D.cpp new file mode 100644 index 0000000..243cce9 --- /dev/null +++ b/obvision/reconstruct/space/SensorVelodyne3D.cpp @@ -0,0 +1,281 @@ +#include "SensorVelodyne3D.h" +#include "obcore/base/System.h" +#include "obcore/math/mathbase.h" +#include + +namespace obvious +{ + +SensorVelodyne3D::SensorVelodyne3D(unsigned int raysIncl, double inclMin, double inclRes, double azimRes, double maxRange, double minRange, + double lowReflectivityRange) + : Sensor(3, maxRange, minRange, lowReflectivityRange) +{ + _azimRes = azimRes; + _inclRes = inclRes; + unsigned int raysAzim = 0; + double azimAngle = 0.0; + const double resetInclMin = inclMin; // to reset variable inclMin each time + // after exiting inner for-loop (=-15° + // here for VLP16) + + raysAzim = round(static_cast(2 * M_PI / azimRes)); + + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /// PLEASE LET SOMEONE CHECK IF THE AZIM+1 THING IS CORRECT! DO I REALLY HAVE 361 values FOR AZIMUT? yes right? + /// +1 bei allocate _indexMap und bei _width + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + // inherited from class sensor + // Size of first dimension, i.e., # of samples of a 2D scan or width of image + _width = raysAzim + 1; + // Size of second dimension, i.e., 1 for a 2D scan or height of image sensor + _height = raysIncl; + // Number of measurement samples, i.e. _width x _height + _size = _width * _height; + + _data = new double[_size]; + _mask = new bool[_size]; + for(unsigned int i = 0; i < _size; i++) + _mask[i] = true; + + // evtl mit _height u _width ersetzen + obvious::System::allocate(_width, _height, _indexMap); + + // returnRayIndex(azimAngle, inclAngle, &azimIndex, &inclIndex); + + setIndexMap(_width, _height); + + _rays = new obvious::Matrix(3, _size); + + obvious::Matrix R = obvious::Matrix(*_T, 0, 0, 3, 3); + + double inclAngle = 0.0; + double xCoord = 2.0; + double yCoord = 2.0; + double zCoord = 2.0; + returnAngles(xCoord, yCoord, zCoord, &inclAngle, &azimAngle); + + unsigned int n = 0; // counts all rays of inner loop and outer loop to store + // in matrix _rays + for(unsigned int i = 0; i < _width; i++) + { + for(unsigned int j = 0; j < _height; j++, n++) + { + obvious::Matrix calcRay(3, 1); + // to follow definition of theta (inclination angle) in spherical + // coordinate system + double thetaSphere = 0.0; + const double piHalf = deg2rad(90.0); + + if(inclMin < 0) + { + thetaSphere = piHalf + (inclMin) * (-1); + } + else + { + thetaSphere = piHalf - inclMin; + } + + calcRay(0, 0) = sin(thetaSphere) * cos(azimAngle); + calcRay(1, 0) = sin(thetaSphere) * sin(azimAngle); + calcRay(2, 0) = cos(thetaSphere); + // normalize rays + const double length = sqrt(calcRay(0, 0) * calcRay(0, 0) + calcRay(1, 0) * calcRay(1, 0) + calcRay(2, 0) * calcRay(2, 0)); + const double lengthInv = 1.0 / length; + calcRay = R * calcRay; + + // store end points of current ray in matrix* _rays inherited from class Sensor + (*_rays)(0, n) = calcRay(0, 0) * lengthInv; + (*_rays)(1, n) = calcRay(1, 0) * lengthInv; + (*_rays)(2, n) = calcRay(2, 0) * lengthInv; + + inclMin += inclRes; + } + inclMin = resetInclMin; // reset inclination angle to minimum + azimAngle += azimRes; + } + + _raysLocal = new obvious::Matrix(3, _size); + *_raysLocal = *_rays; +} + +SensorVelodyne3D::~SensorVelodyne3D() +{ + delete _rays; + delete _raysLocal; + delete[] _data; + delete[] _mask; + System::deallocate(_indexMap); +} + +// return by reference - inclAngle u azimuth in DEGREEs -- changed to RAD +void SensorVelodyne3D::returnAngles(double xCoord, double yCoord, double zCoord, double* inclAngle, double* azimAngle) +{ + // Inclination + double theta = 0.0; // careful love, this is the angle between z-axis and x-y-plane as + // defined in polar coordinates --> no distinction of + // cases for acos necessary, bec. only values from 75° - + // 105° for VLP16 + double length = sqrt(xCoord * xCoord + yCoord * yCoord + zCoord * zCoord); + double lengthInv = 1.0 / length; + + theta = acos(zCoord * lengthInv); + + if(theta > deg2rad(90.0)) // translate theta into inclination angle "aperture angle" + // from -15° to +15° + { + *inclAngle = -(theta - deg2rad(90.0)); // -15° -> 0° + } + else + { + *inclAngle = deg2rad(90.0) - theta; // 0° -> +15° + } + + // Azimuth + *azimAngle = atan2(yCoord, xCoord); + if(*azimAngle < 0) + { + *azimAngle += 2.0 * M_PI; // express angles positively in 3rd and 4th quadrant + } +} + +void SensorVelodyne3D::returnRayIndex(double azimAngle, double inclAngle, unsigned int* azimIndex, unsigned int* inclIndex) +{ + *azimIndex = round(azimAngle / _azimRes); + + // assignment will always be the same for VLP16 - inclination resolution is + // fixed 2° and nbr of rays is 16 + double mapInclination = inclAngle + deg2rad(15.0); // map inclination angles (-15° -> +15°) up to positive + // range 0° - 30° --> TO DO change so this also works for E32 + *inclIndex = round(mapInclination / _inclRes); +} + +void SensorVelodyne3D::setIndexMap(unsigned int width, unsigned int height) +{ + unsigned int column = 0; + for(unsigned int row = 0; row < width; row++) + { + for(column = 0; column < height; column++) + { + _indexMap[row][column] = row * (height) + column; + } + column = 0; // iterate over 16 vertical rays for each azimuth ray + } +} + +// todo - adapt this for E32 +unsigned int SensorVelodyne3D::lookupIndex(int indexSensormodel) +{ + unsigned int indexVelodyneROS = 0; + switch(indexSensormodel) + { + case 0: + indexVelodyneROS = 0; + break; + case 1: + indexVelodyneROS = 2; + break; + case 2: + indexVelodyneROS = 4; + break; + case 3: + indexVelodyneROS = 6; + break; + case 4: + indexVelodyneROS = 8; + break; + case 5: + indexVelodyneROS = 10; + break; + case 6: + indexVelodyneROS = 12; + break; + case 7: + indexVelodyneROS = 14; + break; + case 8: + indexVelodyneROS = 1; + break; + case 9: + indexVelodyneROS = 3; + break; + case 10: + indexVelodyneROS = 5; + break; + case 11: + indexVelodyneROS = 7; + break; + case 12: + indexVelodyneROS = 9; + break; + case 13: + indexVelodyneROS = 11; + break; + case 14: + indexVelodyneROS = 13; + break; + case 15: + indexVelodyneROS = 15; + break; + } + return indexVelodyneROS; +} + +// M sind die Koordinaten des TSD SPACES! von allen VOXELN die Mittelpunkte! +void SensorVelodyne3D::backProject(obvious::Matrix* M, int* indices, obvious::Matrix* T) +{ + obvious::Matrix PoseInv = getTransformation(); + PoseInv.invert(); + if(T) + PoseInv *= *T; + + // multiply PoseInv with M where poseInv is not transposed but M is transposed (true) + obvious::Matrix coords3D = obvious::Matrix::multiply(PoseInv, *M, false, true); + + double inclAngle = 0.0; + double azimAngle = 0.0; + unsigned int row = 0; + unsigned int column = 0; + unsigned int columnMapped = 0; + unsigned int idxCheck = 0; + + for(unsigned int i = 0; i < M->getRows(); i++) + { + double x = coords3D(0, i); + double y = coords3D(1, i); + double z = coords3D(2, i); + + returnAngles(x, y, z, &inclAngle, &azimAngle); + + // leave current loop if incl angle out of measurement area -15° --> +15.0° + if((inclAngle < deg2rad(-15.0)) || (inclAngle > deg2rad(15.0))) + { + indices[i] = -1; + continue; + } + else + { + // 1: calculate incoming azimuth = ROW index of indexMap + // 2: calculate incoming inclination = COLUMN of indexMap + returnRayIndex(azimAngle, inclAngle, &row, &column); + + // ROW CORRECTED weil row= azimindex max zb 359,9 / 0.2 = 1799 == 1800 --> index 1799 weil 0 anfängt? ist das richtig? + // 0 / 0.2 = 0 + // 0.2 / 0.2 = 1 + // muss nur beim letzten wert passieren gell? was versteh ich hier grad nicht + // warum passiert das dann bei col nicht? + // ich brauch einen index mehr gell? in allocate + + // map column from sensor model to Velodyne firing sequence (order of + // vertical rays differs between sensormodel and velodyne ros input) + columnMapped = lookupIndex(column); + + // probe: index ausrechnen + idxCheck = columnMapped + 16 * row; + + // push current value of current indexMap[row][column] into int* indices (returned by backProject to push()) + indices[i] = _indexMap[row][columnMapped]; + } + } +} +} // namespace obvious \ No newline at end of file diff --git a/obvision/reconstruct/space/SensorVelodyne3D.h b/obvision/reconstruct/space/SensorVelodyne3D.h new file mode 100644 index 0000000..37c29c0 --- /dev/null +++ b/obvision/reconstruct/space/SensorVelodyne3D.h @@ -0,0 +1,87 @@ +#ifndef OBVISION_RECONSTRUCT_SPACE_SENSORVELODYNE3D_H_ +#define OBVISION_RECONSTRUCT_SPACE_SENSORVELODYNE3D_H_ + +#include "obcore/math/linalg/eigen/Matrix.h" +#include "obvision/reconstruct/Sensor.h" + +namespace obvious +{ + +/** + * @class SensorVelodyne3D + * @brief class for velodyne 3D laser scanners (VLP16 PUCK, E32) + * @author Jasmin Ziegler + */ +class SensorVelodyne3D : public Sensor +{ +public: + /** + * Standard constructor + * @param[in] raysIncl number of inclination rays of scanning device (vertical) + * @param[in] inclMin lowest inclination angle in RAD + * @param[in] inclRes resolution of inclination rays in RAD, i.e. angle between two vertical rays + * @param[in] azimRes resolution of azimuth rays in RAD, angle between two horizontal rays in 360° plane + */ + SensorVelodyne3D(unsigned int raysIncl, double inclMin, double inclRes, double azimRes, double maxRange = INFINITY, double minRange = 0.0, + double lowReflectivityRange = INFINITY); + + /** + * Destructor + */ + virtual ~SensorVelodyne3D(); + + /** + * returns azimuth angle and inclination angle + * @param[in] xCoord x coordinate of a 3D point + * @param[in] yCoord y coordinate of a 3D point + * @param[in] zCoord z coordinate of a 3D point + * @param[out] inclAngle inclination angle in x-z-plane, 16 layers of rays from -15° to +15°, 2° resolution (VLP16 PUCK) in RAD + * @param[out] azimAngle azimuth angle in x-y-plane, 0° to 360° in RAD + */ + void returnAngles(double xCoord, double yCoord, double zCoord, double* inclAngle, double* azimAngle); + + /** + * returns ray index + * @param[in] azimAngle azimuth angle calculated in returnAngles() + * @param[in] inclAngle inclination angle calculated in returnAngles() + * @param[out] azimIndex index of azimuth ray number which is closest to 3D point to assign laser data to 3D point + * @param[out] inclIndex index of inclination ray number which is closest to 3D point to assign laser data to 3D point + * @todo remove adaptation to VLP16 PUCK - formulate generally + */ + void returnRayIndex(double azimAngle, double inclAngle, unsigned int* azimIndex, unsigned int* inclIndex); + + /** + * sets up an index map in the form of an 2D array; iterates over all azimuth values from 0° - 360° (=ROWS) and all inclination values from -15° to +15° + * (=COLUMNS) + * @param[in] width width of sensor, same as _width inherited from Sensor; in this case all azimuth rays + * @param[in] height height of sensor, same as _height inherited from Sensor; in this case all inclination rays + */ + void setIndexMap(unsigned int width, unsigned int height); + + /** + * + * @todo make this function abstract in parent class so each velodyne sensor inheriting from it has to implement it since it may differ from sensor to + * sensor + */ + unsigned int lookupIndex(int indexSensormodel); + + /** + * Project all coordinates (center of each voxel in tsd space) back to sensor index: which sensor ray comes closest to the coordinate? + * @param[in] M matrix of 3D coordinates of all voxel center points in tsd space (homogeneous) + * @param[out] indices vector of projection results (must be allocated outside) + * @param[in] T temporary transformation matrix of coordinates + */ + void backProject(obvious::Matrix* M, int* indices, obvious::Matrix* T = NULL); + +private: + double _azimRes; + double _inclRes; + int** _indexMap; + /////////////////////////////////////////// DAS HIER RAUS wird im push initialisiert + // int* _indices; + ////////////////////////////// +}; + +} /* namespace obvious */ + +#endif /* OBVISION_RECONSTRUCT_SPACE_SENSORVELODYNE3D_H_ */ From 719ff6f6cc153574472e7cb3ae544cda965b20cb Mon Sep 17 00:00:00 2001 From: Philipp Koch Date: Fri, 21 Feb 2020 14:15:28 +0100 Subject: [PATCH 14/18] Added openmp compiler flags in debug mode --- build/debug/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build/debug/CMakeLists.txt b/build/debug/CMakeLists.txt index 70d9c6a..3975c6a 100755 --- a/build/debug/CMakeLists.txt +++ b/build/debug/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.6) SET(CMAKE_BUILD_TYPE Debug) -SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -Wall -DDEBUG") +SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -Wall -DDEBUG -fopenmp") SET(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../../cmake) From 9db898aa72e33e77b73278bc5ad8d74438262bb2 Mon Sep 17 00:00:00 2001 From: Jasmin Dschessica Date: Mon, 20 Apr 2020 11:13:19 +0200 Subject: [PATCH 15/18] started working on polar sensor base class --- obvision/CMakeLists.txt | 1 + .../reconstruct/space/SensorPolar3DBase.cpp | 37 ++++++++++++++++++ .../reconstruct/space/SensorPolar3DBase.h | 39 +++++++++++++++++++ 3 files changed, 77 insertions(+) create mode 100644 obvision/reconstruct/space/SensorPolar3DBase.cpp create mode 100644 obvision/reconstruct/space/SensorPolar3DBase.h diff --git a/obvision/CMakeLists.txt b/obvision/CMakeLists.txt index c1039a9..b8285aa 100755 --- a/obvision/CMakeLists.txt +++ b/obvision/CMakeLists.txt @@ -59,6 +59,7 @@ add_library(obvision STATIC reconstruct/space/SensorProjective3D.cpp reconstruct/space/SensorPolar3D.cpp reconstruct/space/SensorVelodyne3D.cpp + reconstruct/space/SensorPolar3DBase.cpp reconstruct/space/SensorPolar2DWith3DPose.cpp reconstruct/space/TsdSpace.cpp reconstruct/space/TsdSpaceComponent.cpp diff --git a/obvision/reconstruct/space/SensorPolar3DBase.cpp b/obvision/reconstruct/space/SensorPolar3DBase.cpp new file mode 100644 index 0000000..90c1ab0 --- /dev/null +++ b/obvision/reconstruct/space/SensorPolar3DBase.cpp @@ -0,0 +1,37 @@ +#include "SensorPolar3DBase.h" + +namespace obvious +{ + +SensorPolar3DBase::SensorPolar3DBase(double inclMin, double inclMax, double inclRes, double azimMin, double azimMax, double azimRes, double maxRange = INFINITY, double minRange = 0.0, double lowReflectivityRange = INFINITY) +: Sensor(3, maxRange, minRange, lowReflectivityRange) +{ + unsigned int raysIncl = round(static_cast((abs(inclMin) + abs(inclMax)) / inclRes)); + + + //inherited from class sensor + +} + + + + + + + + + + + + + + + + + + + + + + +} // namespace obvious \ No newline at end of file diff --git a/obvision/reconstruct/space/SensorPolar3DBase.h b/obvision/reconstruct/space/SensorPolar3DBase.h new file mode 100644 index 0000000..1bd532f --- /dev/null +++ b/obvision/reconstruct/space/SensorPolar3DBase.h @@ -0,0 +1,39 @@ +#ifndef OBVISION_RECONSTRUCT_SPACE_SENSORPOLAR3DBASE_H_ +#define OBVISION_RECONSTRUCT_SPACE_SENSORPOLAR3DBASE_H_ + +#include "obcore/math/linalg/eigen/Matrix.h" +#include "obvision/reconstruct/Sensor.h" + +namespace obvious +{ +/** + * @class SensorPolar3DBase + * @brief generic sensor model class for polar sensing units (to be tested for: Velodyne VLP16 PUCK, Velodyne HDL-32E, ohm_tilt_scanner_3d, SICK LDMRS8, ohm_4d_scanner, Ouster OS0-128, & many more) + * @brief all polar sensor model classes inherit from this class & implement lookupIndex + * @author Jasmin Ziegler + */ +class SensorPolar3DBase : public Sensor +{ +public: + +/** + * Standard Constructor + * @param[in] inclMin lowest inclination angle [RAD] (vertical) + * @param[in] inclMax highest inclination angle in [RAD] (vertical) + * @param[in] inclRes vertical angular resolution [RAD] + * @param[in] azimMin lowest azimuth angle [RAD] (horizontal) + * @param[in] azimMax highest azimuth angle in [RAD] (horizontal) + * @param[in] azimRes horizontal angular resolution [RAD] + */ +SensorPolar3DBase(double inclMin, double inclMax, double inclRes, double azimMin, double azimMax, double azimRes, double maxRange = INFINITY, double minRange = 0.0, double lowReflectivityRange = INFINITY); + +/** + * Destructor + */ +virtual ~SensorPolar3DBase(); + + + +} //namespacec obvious + +#endif /* OBVISION_RECONSTRUCT_SPACE_SENSORPOLAR3DBASE_H_ */ \ No newline at end of file From 631cdc84b94ff84f90e5ce9cbbcb5bc246420246 Mon Sep 17 00:00:00 2001 From: Jasmin Dschessica Date: Mon, 20 Apr 2020 12:19:54 +0200 Subject: [PATCH 16/18] sorry phil jetzt baut es --- obvision/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/obvision/CMakeLists.txt b/obvision/CMakeLists.txt index b8285aa..5f05e44 100755 --- a/obvision/CMakeLists.txt +++ b/obvision/CMakeLists.txt @@ -59,7 +59,7 @@ add_library(obvision STATIC reconstruct/space/SensorProjective3D.cpp reconstruct/space/SensorPolar3D.cpp reconstruct/space/SensorVelodyne3D.cpp - reconstruct/space/SensorPolar3DBase.cpp + #reconstruct/space/SensorPolar3DBase.cpp reconstruct/space/SensorPolar2DWith3DPose.cpp reconstruct/space/TsdSpace.cpp reconstruct/space/TsdSpaceComponent.cpp From db69e528731cdaa53a92c54df13074226f3ec699 Mon Sep 17 00:00:00 2001 From: phil Date: Thu, 25 Jun 2020 20:46:19 +0200 Subject: [PATCH 17/18] Added c++17 to compiler options. --- build/release/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build/release/CMakeLists.txt b/build/release/CMakeLists.txt index fdfb80d..4b1cc40 100755 --- a/build/release/CMakeLists.txt +++ b/build/release/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 2.6) SET(CMAKE_BUILD_TYPE Release) -SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS} -fopenmp -Wall -O2 -DNDEBUG -pipe -march=native") +SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS} -fopenmp -Wall -O2 -DNDEBUG -pipe -std=c++17 -march=native") From 970f010d9fc14d745b4d20b406d260a56f2ed491 Mon Sep 17 00:00:00 2001 From: phil Date: Mon, 17 Jan 2022 20:37:34 +0100 Subject: [PATCH 18/18] Changed standard in obvision to c++17 --- obvision/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/obvision/CMakeLists.txt b/obvision/CMakeLists.txt index 5f05e44..cc734ed 100755 --- a/obvision/CMakeLists.txt +++ b/obvision/CMakeLists.txt @@ -8,7 +8,7 @@ PROJECT(OBVISION) SET(OBVISION_VERSION_MAJOR 0) SET(OBVISION_VERSION_MINOR 1) -add_compile_options(-std=c++11) +add_compile_options(-std=c++17) INCLUDE_DIRECTORIES(.. /usr/include/eigen3)